The Shortest Path for Mobile Robots in Obstacle Avoidance

Abstract

An obstacle avoidance strategy for mobile robot is presented based on obstacle position and size information. Furthermore, rational robot paths in obstacle avoidance are discussed in detail, which leads to the final important result of optimal path for mobile robot to avoid an obstacle. Simple as the results are, they have been shown by simulations te be effective and convenient in implementation. A good application prospect of the result can thus be expected.

Content

《1 引言》

1 引言

移动机器人正在成为应用最为广泛的机器人, 它们已经从单纯工业应用发展到执行太空任务、探险、搜索与援救、勘测与绘图、深海打捞等领域 ^[1,2,3,4]。除此之外, 移动机器人也已经在博物馆、医院以及娱乐场所 ^[5,6,7]广泛应用, 它们正在各个领域成为人类的得力助手, 甚至从事人类不能或不便从事的工作。移动机器人应用领域的主要难题包括环境识别、障碍躲避、路径选择、机器人定位、任务规划以及动作执行系统的控制与诊断等。目前, 已有的障碍躲避方法中关于机器人路径规划问题的研究多集中在如何合理规划或选择路径 ^[6,7,8,9];Dubins ^[10]和Reeds ^[11]等人最早开始研究最佳路径问题;Reister ^[12]用数学分析的方法研究了加速度有界条件下的最优路径, 其计算过程非常复杂, 不便于实现;Devin ^[13]采用几何学原理讨论了速度有界而加速度和转矩无界时机器人的最优路径问题, 该方法较为直观, 但对于路径的实现问题未进行讨论, 而且其中只有部分结论在特定条件下成立。到目前为止, 差分驱动机器人最优路径的确定和实现这一重要问题还没有成熟的理论体系。

由于路径规划问题是机器人合理高效地执行任务的前提, 如何选择一条合理甚至最佳的路径是非常重要的研究课题。为此, 针对以左右两个驱动轮为基础的移动机器人, 提出一类合理的弧形路径, 并找到了实现障碍躲避的一条最佳路径, 解决了确定性环境中路径规划领域的一个难题, 具有一定的理论意义。实验表明, 采用该方法进行路径规划后在机器人路径的合理性、执行任务的准确性和工作效率等方面得到了大幅度的改善, 同时也在很大程度上降低了能量损耗。

《2 最佳路径》

2 最佳路径

在障碍躲避问题中, 人们总是希望机器人在绕过障碍物的过程中, 在保持平均速度不变的情况下, 用最短的时间、最少的能量和最短的路径实现障碍的躲避。

定义1 (合理路径) 如果在障碍躲避过程中, 移动机器人以允许的平均线速度运动, 且它与障碍物间的距离保持单调减小, 则称此时机器人经过的路径为合理路径。

定理1 对于由左右两个轮驱动的移动机器人, 假设机器人在绕过障碍的过程中其平均标量线速度保持恒定, 则它绕开障碍物所经过的任何圆弧曲线路径中, 沿 $\bar{B Ρ_{1}}$ $\bar{B Ρ_{1}}$ 方向互相叠加的扇形上的圆弧路径是合理路径 (如图1a所示) , 且这些圆弧的弦长总和为 $\bar{B Ρ_{1}}$ $\bar{B Ρ_{1}}$ 。

证明假设机器人路径为任意连接的曲线路线 (如图1b所示) , 且不沿 $\bar{B Ρ_{1}}$ $\bar{B Ρ_{1}}$ 方向叠加。假设该路线总长度为 $l (α, \bar{B Ρ_{1}})$ $l (α, \bar{B Ρ_{1}})$ , 显然该路线必然可用n条线段串联起来 (图1b中所示为n=2的情况) , 于是必然有

$l (α, \bar{B Ρ_{1}}) \geq \underset{n 条线段}{\bar{B Ρ} + \dots + \bar{Ρ Ρ_{1}}} 。 (1)$ $l (α, \bar{B Ρ_{1}}) \geq \underset{n 条线段}{\bar{B Ρ} + \dots + \bar{Ρ Ρ_{1}}} 。 (1)$

《图1》

图1机器人路径

Fig.1 Robot paths

另外, 由于必然存在若干个叠加圆弧以B和P₁为起点和终点, 且它们的弦串接成 $\bar{B Ρ_{1}}$ $\bar{B Ρ_{1}}$ , 而其弧长总和小于上述n条线段长度之和。同时, 只要适当选择这样的叠加圆弧, 就能使机器人在沿此路径躲避障碍时保证其与障碍间的距离单调减小。因此这样的弧形路径是任意选择路径中的合理路径, 即定理1成立。

定理1表明, 机器人要绕开障碍物需要经过的最短路径必然为以当前位置为起点、倾斜角度为α的直线路径, 且路径长度的下界为 $\bar{B Ρ_{1}}$ $\bar{B Ρ_{1}}$ 。于是, 可直接得到如下结论:

定理2 对于由左右两个轮驱动的移动机器人, 假设机器人在绕过障碍的过程中其平均标量线速度保持恒定, 且机器人与障碍物间的距离保持单调减小, 则机器人绕开障碍物所需经过的路径长度的下界只与机器人与障碍物间的距离和倾斜角有关。

定义2 (等价路径) 在障碍躲避过程中, 在平均线速度相同的情况下, 移动机器人从同一初始位置和初始行进方向, 沿不同的路径运动到同一终止位置和相同的行进方向, 如果所经过的路径长度相同, 则称这样的路径互为等价路径 (如图2所示) 。

《图2》

图2等价的扇形叠加路径

Fig.2 Equivalent arc paths

定理3 对于由左右两个轮驱动的移动机器人, 假设机器人在绕过障碍的过程中其平均标量线速度保持恒定, 则它以相互叠加的合理圆弧路径绕开障碍物时 (见图1a和图2) , 这些路径必为等价路径, 且路径长度与所选路径和圆弧数量无关, 恒为 $\frac{π - 2 α}{2 \cos α} \bar{B Ρ_{1}}$ $\frac{π - 2 α}{2 \cos α} \bar{B Ρ_{1}}$ 。

证明对于图2所示的两种合理路径, 其路径长度均为:

$l = (\frac{\bar{Ο Η}}{2 \cos α} + \frac{\bar{Ο^{'} Η}}{2 \cos α}) θ = \frac{(π - 2 α)}{2 \cos α} \bar{B Ρ_{1}} 。 (2)$ $l = (\frac{\bar{Ο Η}}{2 \cos α} + \frac{\bar{Ο^{'} Η}}{2 \cos α}) θ = \frac{(π - 2 α)}{2 \cos α} \bar{B Ρ_{1}} 。 (2)$

可见任意两个大小不等的扇形叠加成的合理路径等价于两个相同扇形叠加成的合理路径。另外, 用类似的方法可以证明, 任意奇数条弧形叠加成的合理路径 (如图1a底层的3个弧形路径) 总等价于某单个弧形合理路径 (如图2a底层的一个弧形路径) , 也就是图1a (代表n条弧形叠加成的合理路径) 、图2a以及图2b中的路径均等价。所以, 对于由任意n条半径 (指机器人驱动轮轴中点的转弯半径) 分别为 $R_{i} (R_{i} \geq \bar{A B} ‚ i = 1, 2, \dots, n)$ $R_{i} (R_{i} \geq \bar{A B} ‚ i = 1, 2, \dots, n)$ 的扇形叠加而成的合理路径, 机器人要在定理要求的条件下完成障碍躲避, n就必然为偶数, 而且该路径一定等价于由n条半径同为R_i=R的圆弧叠加成的合理路径, 且路径总长度可通过如下过程来计算:

$\begin{array}{l} l = n R θ, θ + 2 α = π, (3) \\ R = \frac{1}{2} (\frac{1}{n} \bar{B Ρ_{1}}) / \cos α, (4) \end{array}$ $\begin{array}{l} l = n R θ, θ + 2 α = π, (3) \\ R = \frac{1}{2} (\frac{1}{n} \bar{B Ρ_{1}}) / \cos α, (4) \end{array}$

也就是

$l = n R θ = \frac{π - 2 α}{2 \cos α} \bar{B Ρ_{1}} 。 (5)$ $l = n R θ = \frac{π - 2 α}{2 \cos α} \bar{B Ρ_{1}} 。 (5)$

因此, 定理3的结论成立。

推论1 所有等价的叠加圆弧路径构成机器人障碍躲避的一个合理路径族, 在路径规划过程中选择该路径族中的任何一条路径所做的决策在路径长度和能量意义下是等价的。

如果在所有可能的路径中存在一条最短的路径, 则每次发现障碍物后, 机器人就可在条件允许的情况下选择该最短路径执行障碍躲避任务, 从而最大限度地节省时间和能量。根据前面的定理2可知, 机器人如果可沿直线行走, 则线段 $\bar{B Ρ_{1}}$ $\bar{B Ρ_{1}}$ 便是最短路径。但是, 由于机器人为左右轮驱动, 所以它必须至少通过某适当长度的弧形路径由当前行进方向旋转一定角度后, 才可沿直线路径完成障碍躲避。于是, 有如下结论:

定理4 对于由左右两个轮驱动的移动机器人, 假设机器人在绕过障碍的过程中其平均标量线速度保持恒定, 则它绕开障碍物的最短路径为图3所示路径, 即以轮A为圆心、以车身宽为半径旋转一定角度, 然后沿直线绕开障碍, 最后再将车身行进转到与初始方向相同的方向。此时, 最短路径长度为

$\begin{array}{l} l_{\min} = (\bar{B Ρ_{1}^{2}} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2} + D [π - α - \\ \arccos \frac{D}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}} - \\ \arcsin \frac{D s i n α}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}}] ‚ \end{array}$ $\begin{array}{l} l_{\min} = (\bar{B Ρ_{1}^{2}} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2} + D [π - α - \\ \arccos \frac{D}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}} - \\ \arcsin \frac{D s i n α}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}}] ‚ \end{array}$

其中 $ϕ = ∠ B A C = ∠ A^{'} Ρ_{1} C^{'} ‚ α = ∠ A B Ρ_{1} ‚ D = \bar{A B}$ $ϕ = ∠ B A C = ∠ A^{'} Ρ_{1} C^{'} ‚ α = ∠ A B Ρ_{1} ‚ D = \bar{A B}$ 。

《图3》

图3最佳路径

Fig.3 The optimal path

证明由图3可知总路径长度为

$l = 2 ϕ \frac{D}{2} + \bar{C Ρ_{1}} ‚ (6)$ $l = 2 ϕ \frac{D}{2} + \bar{C Ρ_{1}} ‚ (6)$

其中

$\begin{array}{l} \bar{C Ρ_{1}^{2}} = \bar{A Ρ_{1}^{2}} - \bar{A C^{2}} = \bar{A Ρ_{1}^{2}} - D^{2} ‚ (7) \\ \bar{A Ρ_{1}^{2}} = \bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α ‚ (8) \end{array}$ $\begin{array}{l} \bar{C Ρ_{1}^{2}} = \bar{A Ρ_{1}^{2}} - \bar{A C^{2}} = \bar{A Ρ_{1}^{2}} - D^{2} ‚ (7) \\ \bar{A Ρ_{1}^{2}} = \bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α ‚ (8) \end{array}$

另外, 根据图3中的几何关系, 利用三角学中的正弦定理和余弦定理可以得到:

$\begin{array}{l} ϕ = π - α - \arccos \frac{D}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}} - \\ \arcsin \frac{D s i n α}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}} 。 (9) \end{array}$ $\begin{array}{l} ϕ = π - α - \arccos \frac{D}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}} - \\ \arcsin \frac{D s i n α}{(\bar{B Ρ_{1}^{2}} + D^{2} - 2 \bar{B Ρ_{1}} D c o s α)^{1 / 2}} 。 (9) \end{array}$

于是, 通过对上述关系进行整理即可得到定理4中的结论, 定理4得证。

注1:上述结论虽然是针对立方形障碍物而获得的, 但它们不受障碍物外形的限制。只要机器人与障碍物的相对位置满足上述结论的条件, 通过确定机器人与障碍物之间的切线和切点即可得出同样的相关结论, 包括最优路径结论。

《3 障碍躲避及最佳路径的实现》

3 障碍躲避及最佳路径的实现

下面介绍机器人障碍躲避和最佳路径的编程和实现问题。在仿真实验中, 所设计的机器人障碍躲避系统包括负责障碍物距离检测的两个固定超声波传感器 (由软件模拟) 和一个用来确定障碍物大小和方位的旋转超声波传感器 (由软件模拟) 。需要指出的是, 下面提到传感器和障碍物时均表示虚拟传感器和虚拟障碍物。在障碍检测时, 传感器向外界发射声波, 同时不断检测反射波信号, 然后根据声波的速度和飞行时间计算出机器人距障碍物的距离。传感器每次发射数量一定的脉冲信号, 然后切换到接收模式, 它既接收自身信号的反射波也接收其他传感器信号的反射波。控制系统通过分析所接收的反射信号的脉冲数量, 推知发送信号的传感器, 并根据两个固定传感器及其所接收的信号的几何关系, 进而确定出障碍物的距离、方位和大小。具体原理如图4所示, 由图4可知反射点位于以距离测量值S₁为半径、以传感器A为圆心的圆的左前半部任一点处。同时, 传感器A发出的信号经该反射点反射后到达传感器B后, 系统得到另一个距离测量值S₁+S₂。由于两个传感器间的距离 (即车身宽度) 为D, 所以反射点一定在以传感器A和B为焦点、且满足c=D/2和2a=S₁+S₂的椭圆上。因此有如下椭圆和圆:

$\begin{array}{l} \frac{x^{2}}{a^{2}} + \frac{y^{2}}{b^{2}} = 1 ‚ (10) \\ (x + c)^{2} + y^{2} = r^{2}, (11) \end{array}$ $\begin{array}{l} \frac{x^{2}}{a^{2}} + \frac{y^{2}}{b^{2}} = 1 ‚ (10) \\ (x + c)^{2} + y^{2} = r^{2}, (11) \end{array}$

其中r=S₁, 2a=S₁ + S₂, 2c=D以及b= (a²-c²) ^1/2。上面的圆和椭圆的交点便是障碍物上的反射点。

《图4》

图4障碍物位置检测

Fig.4 Obstacle position detection

得到反射点位置坐标后, 采用如下的距离函数对障碍物 (包括围墙等) 的距离进行综合

$D (Ρ_{Ο}) = \min_{i} d_{i} (Ρ_{Ο}) ‚ (12)$ $D (Ρ_{Ο}) = \min_{i} d_{i} (Ρ_{Ο}) ‚ (12)$

其中d_i (P_O) 代表超声波传感器测量的障碍物P_O的距离信息, 可根据反射信号和飞行时间原理来计算。

关于障碍物的大小, 仿真实验中依据图5所示原理, 根据旋转传感器O的旋转角以及障碍物距离信息来确定障碍物的大小。图中 $\bar{Ο Ρ_{1}}$ $\bar{Ο Ρ_{1}}$ 和β₁分别表示传感器最先检测到障碍物时的障碍物距离和传感器角度, $\bar{Ο Ρ_{2}}$ $\bar{Ο Ρ_{2}}$ 和β₂分别为传感器扫描离开障碍物前的障碍物距离和传感器角度, 于是由图5可知障碍物大小为

$\bar{Ρ_{1} Ρ_{2}} = | \bar{Ο Ρ_{2}} \cos β_{2} - \bar{Ο Ρ_{1}} \cos β_{1} | 。 (13)$ $\bar{Ρ_{1} Ρ_{2}} = | \bar{Ο Ρ_{2}} \cos β_{2} - \bar{Ο Ρ_{1}} \cos β_{1} | 。 (13)$

《图5》

图5障碍物大小检测

Fig.5 Obstacle size detection

计算出障碍物的位置和大小之后, 就可以确定机器人躲避障碍物时的运动方向。在图5的基础上, 如果β₁和β₂都为锐角, 说明障碍物在机器人右前方。此时, 如果 $\bar{Ο Ρ_{2}} \cos β_{2} > D / 2$ $\bar{Ο Ρ_{2}} \cos β_{2} > D / 2$ , 则机器人可以沿直线直接通过而不须转弯;如果 $\bar{Ο Ρ_{2}} \cos β_{2} \leq D / 2$ $\bar{Ο Ρ_{2}} \cos β_{2} \leq D / 2$ , 则机器人需要向左转。

如果β₁和β₂都为钝角, 说明障碍物在机器人左前方。此时, 如果 $\bar{Ο Ρ_{1}} \cos β_{1}$ $\bar{Ο Ρ_{1}} \cos β_{1}$ >D/2, 则机器人可沿直线直接通过而不须转弯;如果 $\bar{Ο Ρ_{1}} \cos β_{1}$ $\bar{Ο Ρ_{1}} \cos β_{1}$ ≤ D/2, 则机器人需要向右转。

如果β₁为锐角, β₂为钝角, 说明障碍物在机器人前方。此时, 机器人是选择左转、右转还是直行, 要取决于 $\bar{Ο Ρ_{2}} \cos β_{2}}$ $\bar{Ο Ρ_{2}} \cos β_{2}}$ 和 $\bar{Ο Ρ_{1}} \cos β_{1}$ $\bar{Ο Ρ_{1}} \cos β_{1}$ , 如果前者较大就向右转, 如果后者较大就向左转, 如果两者相当则机器人可沿直线直接通过而不需转弯。

关于机器人执行转弯命令时的速度计算问题是编程的一个难点。考虑到所提出的最佳路径需要机器人左右轮有一个轮子停止的情况, 不便于实现。所以, 在仿真实验中, 最佳路径以及相应的速度计算是用接近于最佳路径的一个合理的弧形路径近似实现的。关于路径确定后机器人速度的计算, 以图6所示情形为例来讲述, 其他情况与此类似。

《图6》

图6转弯速度的计算

Fig.6 Turning speed calculation

由图6有如下方程:

$\begin{array}{l} θ R_{1} = v_{1} t ‚ (14) \\ \frac{R_{r} + R_{1}}{2} θ = v t ‚ (15) \\ v = \frac{v_{r} + v_{1}}{2} ‚ (16) \\ 2 R_{r} \sin \frac{θ}{2} = \bar{B Ρ_{2}} ‚ (17) \\ θ + 2 α = π ‚ (18) \\ \bar{B Ρ_{2}} \sin α = \bar{Ο Ρ_{2}} \sin β ‚ (19) \\ R_{r} - R_{1} = 2 c ‚ (20) \end{array}$ $\begin{array}{l} θ R_{1} = v_{1} t ‚ (14) \\ \frac{R_{r} + R_{1}}{2} θ = v t ‚ (15) \\ v = \frac{v_{r} + v_{1}}{2} ‚ (16) \\ 2 R_{r} \sin \frac{θ}{2} = \bar{B Ρ_{2}} ‚ (17) \\ θ + 2 α = π ‚ (18) \\ \bar{B Ρ_{2}} \sin α = \bar{Ο Ρ_{2}} \sin β ‚ (19) \\ R_{r} - R_{1} = 2 c ‚ (20) \end{array}$

其中v_r, v₁和v分别表示机器人右轮速度、左轮速度和平均速度;R₁和R_r分别表示左右轮转弯半径, θ为机器人的转弯角度, t为机器人所经历的时间。根据式 (14) 有

$2 c θ = (v_{r} - v_{1}) t 。 (21)$ $2 c θ = (v_{r} - v_{1}) t 。 (21)$

另外, 由式 (16) 和式 (21) 可得

$c θ = (v_{r} - v) t 。 (22)$ $c θ = (v_{r} - v) t 。 (22)$

同样, 由式 (15) 和式 (20) 有

$v t = (R_{r} - c) θ 。 (23)$ $v t = (R_{r} - c) θ 。 (23)$

然后, 根据式 (22) 和式 (23) 可以得到

$\begin{array}{l} v_{r} = \frac{R_{r} v}{R_{r} - c} ‚ (24) \\ v_{1} = \frac{(R_{r} - 2 c) v}{R_{r} - c} ‚ (25) \end{array}$ $\begin{array}{l} v_{r} = \frac{R_{r} v}{R_{r} - c} ‚ (24) \\ v_{1} = \frac{(R_{r} - 2 c) v}{R_{r} - c} ‚ (25) \end{array}$

其中R_r可由式 (17) 、式 (18) 和式 (19) 得到, v为恒值。另外, 根据式 (18) 和式 (19) 可以得到θ, 并进而得到机器人转弯所需的时间

$t = \frac{R_{r} - c}{v} θ 。 (26)$ $t = \frac{R_{r} - c}{v} θ 。 (26)$

《4 仿真结果》

4 仿真结果

通过软件编程对机器人障碍躲避和路径规划问题进行了多次仿真实验, 部分实验结果见图7和图8。图7对采用该最佳路径前后机器人在障碍躲避过程中的路径进行了比较。可以看出, 采用该最佳路径之前 (图7a所示) , 机器人虽然可以完成障碍躲避任务, 但所经过的路径不太合理。图7b所示为机器人采用该最佳路径执行任务的情况, 显然此时路径非常合理, 而且大大缩短。

《图7》

图7障碍躲避中的路径比较

Fig.7 Comparison of obstacle avoidance paths

《图8》

图8复杂环境下的障碍躲避和路径规划

Fig.8 Obstacle avoidance and path planning in complex environments

图8所示为机器人在较为复杂的环境下执行任务时的仿真结果。图8a和图8b为机器人从不同初始位置出发完成目标搜索和障碍躲避的情况, 图8c和

图8d 为环境变化后机器人完成目标搜索和障碍躲避的情况。从图8中可以看出, 机器人选择该最佳路径后, 能够非常有效的完成障碍躲避和目标搜索任务, 大大缩短了执行任务的时间和路径, 并因而降低了能量损耗。

《5 结语》

5 结语

针对带有左右两个驱动轮的移动机器人, 提出一类合理的路径, 并通过证明指出在给定环境条件下实现障碍躲避的一条最佳路径。该方法成功地解决了一直困扰着研究人员的机器人转弯和障碍躲避过程中的路径选择问题, 在很大程度上提高了机器人执行这类任务的效率, 降低了能量损耗, 并为机器人路径规划提供了理论基础, 具有一定的理论意义。同时, 仿真实验表明, 该方法便于实现, 具有较高的实用价值。当然, 由于机器人所处环境的复杂性和驱动特性的不同, 按照该方法确定机器人路径和左右轮速度, 并不能够保证机器人完全像期望的那样沿一条合理或最佳的路径完成障碍躲避。而是在行进过程中可能偏离选定的行进路径, 或者即使能够按照期望的路径行进, 也可能因为环境中随时出现的变化因素而无法完成任务。为此, 就必须随时确定机器人当前的运行位置和方向, 并根据需要对机器人运行方向和速度做出调整。另外, 文章只给出单机器人作业时的最佳路径及其实现方法, 而对于多机器人协作执行任务的情况未加讨论。所以, 多机器人协作中的最佳路径问题以及运行过程中机器人的动态定位问题是笔者今后的两个主要研究课题。

Show More