CN100570523C - 一种基于障碍物运动预测的移动机器人避障方法 - Google Patents

Abstract

本发明公开了一种基于障碍物运动预测的移动机器人避障方法，包括以下步骤：在机器人移动过程当中，将环绕于机器人周围的空间以机器人为中心划分为3个区域，由外到内分别为：路径规划区域、冲突避免区域以及紧急逃逸区域；机器人根据障碍物当前的运动状态，判断障碍物当前所处的区域，并预测障碍物下一时间的运动状态；根据障碍所处的区域，机器人实行不同的避障策略。本发明方法采用CV、CA和CS模型描述动态障碍物运动，减少了对动态障碍物运动状态的限制。使用IMM算法预测障碍物的运动状态，得到精确的预测结果，使得移动机器人可以在小范围进行动态避障，而不用大范围偏离原规划路径。

Description

一种基于障碍物运动预测的移动机器人避障方法

技术领域

本发明涉及智能机器人技术领域，具体地说涉及一种基于障碍物运动预测的移动机器人避障方法。

背景技术

所谓的机器人避障就是机器人导航过程中避开周围的障碍物。随着机器人技术的纵深发展及广泛应用，对机器人的智能性提出了更高的要求。机器人必须能够依赖外部传感器，感知自身的运动状态和周围环境信息，进行逻辑推理，除了要避开静态障碍物之外，还得避开动态障碍物以及与其它机器人的碰撞。

目前国内外众多学者对动态避障问题展开了深入研究。Khatib提出了人工势场法(Khatib O.Real time obstacle avoidance for manipulators andmobile robots[J].The International of Robotics Research，1986，5(1)：90-98.)，人工势场法的基本思想是将移动机器人在环境中的运动视为一种虚拟人工受力场中的运动。障碍物对移动机器人产生斥力，目标点对移动机器人产生引力，引力和斥力在移动机器人周围形成势场，机器人在势场中受到引力和斥力的合力作用，合力使得机器人绕过障碍物。该方法结构简单，便于实时控制移动机器人，在实时避障领域得到了广泛应用。其不足在于存在局部最优解，容易产生死锁现象，因而可能使移动机器人在到达目标点之前就停留在局部最优点。

除了人工势场法以外，还有通过估计障碍物运动状态进行避障的方法，如利用概率法或扩展卡尔曼滤波(EKF)法估计障碍物的运动状态，这些方法有一定的局限性。首先其对障碍物的运动状态是有限制的，需要事先知道障碍物运动的状态方程；其次是这些方法不能精确地预测障碍物运动加速度，不便于避障的路径规划。

目前众多动态避障方法都只是针对机器人与其它运动物体要发生碰撞时，采取的避让措施，而没有主动的碰撞预防。任何碰撞策略或算法，都有其适用范围，而且难免会出现避障失败，目前很少有避障方法对避障失败的后处理策略作进一步说明。

发明内容

本发明提供了一种对障碍物运动状态没有限制并能主动预防碰撞的基于障碍物运动预测的移动机器人避障方法。

一种基于障碍物运动预测的移动机器人避障方法，包括以下步骤：

(1)在机器人移动过程当中，将环绕于机器人周围的空间以机器人为中心划分为3个区域，由外到内分别为：路径规划区域(Path PlanningArea，PPA)、冲突避免区域(Collision Avoidance Area，CAA)以及紧急逃逸区域(Urgent Escape Area，UEA)；

(2)机器人根据障碍物当前的位置，判断障碍物当前所处的区域，并预测障碍物下一时间的运动状态；

机器人通过传感器感知周围空间障碍物的位置，障碍物的位置是随时间连续变化的，但机器人对障碍物位置的判断以及运动状态的预测不是连续的，而是会间隔一时间段进行判断和预测，该时间段相对于障碍物的运动时间是很小的，可以将该时间段近似为一个时间点，也就是说下一时间的运动状态近似为下一时间段的运动状态，障碍物的下一时间运动状态是指机器人进行下次预测时的运动状态。障碍物运动状态是指障碍物的位置、速度和加速度的大小。

根据障碍物当前位置预测障碍物下一时间的运动状态方法有很多，优选为以下方式：

a.使用常速(Constant Velocity，CV)，常加速(Constant Acceleration，CA)和当前统计(Current Statistical，CS)模型描述障碍物运动状态，分别得到障碍物的状态方程；

b.采用交互多模型(Interacting Multiple Model，IMM)算法对三个状态方程描述的障碍物运动状态进行估计并融合三个估计结果，预测障碍物下一时间的运动状态。

当所有障碍物处于路径规划区域时，机器人根据预测的障碍物下一时间的运动状态进行路径规划。路径规划的方法有很多，最传统的是采用人工势场法，其引力势函数U_rt(q)可表示为：

$U_{\mathrm{rt}}\left(q\right)=\frac{1}{2}{K}_t{|q-q_t|}^2$

斥力势函数U_re(q)为：

$U_{\mathrm{re}}\left(q\right)=\frac{1}{2}{K}_r{[\frac{1}{\mathrm{\ρ}(q,q_o)}-\frac{1}{{\mathrm{\ρ}}_0}]}^2,$ ρ≤ρ₀，U_re(q)＝0，ρ＞ρ₀

以上两公式中的q、q_t和q_o分别为机器人、目标和障碍物位置向量。K_t与K_r分别为引力势场常数与斥力势场常数。ρ(q，q_o)＝|q-q_o|，ρ₀是障碍物的斥力影响距离，超过该距离则此障碍物对移动机器人影响为0。

人工势场法简单，计算量少，速度快。但也有其明显的缺点，在势函数中，只将个运动物体的位置坐标作为参考依据，即为零阶方法，而忽略了各运动物体的速度及加速度等机动性能之间的相互关系导致局部最小问题的概率增大。且在实际使用中，还存在躲避不及而相互碰撞，以及原本不会发生碰撞而机器人根据势场法确作出了无谓的避让运动等问题。

本发明的路径规划方法对传统的人工势场法进行了改进，在势场函数中引入了预测的障碍物下一时段的速度和加速度，构建了引力势场函数U_rt(q)与斥力势场函数U_re(q)分别如下：

$U_{\mathrm{rt}}\left(q\right)=\frac{1}{2}{K}_t{|q-q_t|}^2+\frac{1}{2}{K}_{\mathrm{rv}}{\left|v_r\right|}^2+\frac{1}{2}{K}_{\mathrm{ra}}{\left|a_r\right|}^2$

$U_{\mathrm{re}}\left(q\right)=\frac{1}{2}{K}_r{[\frac{1}{\mathrm{\ρ}(q,q_o)}-\frac{1}{{\mathrm{\ρ}}_0}]}^2+\frac{1}{2}{K}_{\mathrm{ev}}{|v_e-v_r|}^2+\frac{1}{2}{K}_{\mathrm{ea}}{|a_e-a_r|}^2$

以上两公式中的K_rv，K_ra分别是速度与加速度的引力势场常数。K_ev，K_ea分别是速度与加速度的斥力场常数。

b.机器人通过使用上述改进的势场函数的人工势场法来规划路径。

当有障碍物进入冲突避免区域时，机器人停止路径规划，根据预测的该障碍物下一时间的运动状态，通过变速或绕行避开该障碍物；

当避障失败，障碍物从冲突避免区域进入紧急逃逸区域时，机器人计算逃逸的最佳分离角，以最大速度躲避障碍物。

所述的分离角为机器人紧急逃逸的方向与障碍物运动方向的夹角。

本发明方法采用CV、CA和CS模型描述动态障碍物运动，减少了对动态障碍物运动状态的限制。使用IMM算法预测障碍物的运动状态，得到精确的预测结果，使得移动机器人可以在小范围进行动态避障，而不用大范围偏离原规划路径。

附图说明

图1为本发明机器人周围空间区域划分示意图；

图2为本发明避障方法的冲突避免的变速避障方式示意图；

图3为本发明避障方法的冲突避免的绕行避障方式示意图；

图4为本发明避障方法的紧急逃逸避障模式示意图。

具体实施方式

本发明组件了一个使用一个Pioneer 3-AT与三个AmigoBot小机器人的实施平台。其中Pioneer 3-AT作为移动机器人(以下简称机器人)，三个AmigoBot小机器人模拟环境中的动态障碍物(以下简称障碍物)。Pioneer 3-AT上装备的SICK激光扫描仪，BumbleBee2双目视觉传感器，用来感知环境信息及障碍物信息。将移动机器人与动态障碍物置于空旷的房间内，该机器人将从起点出发达到目的地。

(1)如图1所示，在移动过程当中，机器人将环绕于机器人周围的空间以机器人为中心划分为3个区域，由外到内分别为：路径规划区域(Path Planning Area，PPA)、冲突避免区域(Collision Avoidance Area，CAA)以及紧急逃逸区域(Urgent Escape Area，UEA)；

(2)机器人根据障碍物当前的位置，判断障碍物当前所处的区域，并预测障碍物下一时间的运动状态；

根据障碍物当前位置预测障碍物下一时间的运动状态方法如下：

a.使用常速(Constant Velocity，CV)，常加速(Constant Acceleration，CA)和当前统计(Current Statistical，CS)模型描述障碍物运动状态，分别得到障碍物的状态方程；

b.采用IMM算法对三个状态方程描述的障碍物运动状态进行估计并融合三个估计结果，预测障碍物下一时间的运动状态，具体如下：

IMM算法首先采用卡尔曼滤波器分别对三个模型描述的障碍物运动状态进行估计，卡尔曼滤波器将观测到的实际运动状态信息(障碍物当前位置)作为滤波器样本输入，根据状态更新方程获得运动状态的估计值；然后计算模型混合概率，按照该混合概率将卡尔曼滤波器估计的三个模型的运动状态进行融合，得到最终的障碍物运动状态估计值，即预测出障碍物下一时间的运动状态。

当所有障碍物处于路径规划区域时，机器人根据预测的障碍物下一时段的运动状态进行路径规划，上述路径规划方法对传统的人工势场法进行了改进，在势场函数中引入了预测的障碍物下一时段的速度和加速度，构建了引力势场函数U_rt(q)与斥力势场函数U_re(q)分别如下：

$U_{\mathrm{rt}}\left(q\right)=\frac{1}{2}{K}_t{|q-q_t|}^2+\frac{1}{2}{K}_{\mathrm{rv}}{\left|v_r\right|}^2+\frac{1}{2}{K}_{\mathrm{ra}}{\left|a_r\right|}^2$

$U_{\mathrm{re}}\left(q\right)=\frac{1}{2}{K}_r{[\frac{1}{\mathrm{\ρ}(q,q_o)}-\frac{1}{{\mathrm{\ρ}}_0}]}^2+\frac{1}{2}{K}_{\mathrm{ev}}{|v_e-v_r|}^2+\frac{1}{2}{K}_{\mathrm{ea}}{|a_e-a_r|}^2$

以上两公式中的K_rv，K_ra分别是速度与加速度的引力势场常数。K_ev，K_ea分别是速度与加速度的斥力场常数。

b.机器人通过使用上述改进的势场函数的人工势场法来规划路径。

上述规划好的路径并不是起点到目的地的路径，它仅仅是两次预测之间的移动路径规划，将所有这样的路径规划连起来就是完整的一条路径。

当有障碍物进入冲突避免区域时，机器人停止路径规划，根据预测的该障碍物下一时间的运动状态，通过变速或绕行避开该障碍物；

如图2所示，机器人采用变速方式进行避障，机器人从起始点A以速度v_R移动至目的地B，动态障碍物C沿着跟AB交叉的方向以某一速度及加速度运动，当障碍物C进入冲突避免区域时，机器人采用变速策略进行避障，具体如下：

机器人停止该时刻的路径规划，继续按原来的运动方向进行移动，在障碍物到达碰撞点D前，机器人有两种选择：其一是减速到达E点前的某点，在障碍物C经过D点前慢慢前行，等待障碍物通过D后，再加速继续原方向前进。其二是加速通过D点到达F点，再按照原方向前行，使用加速或减速视具体应用情形而定。

如图3所示，机器人跟动态障碍物运动在近似同一条直线上，通过变速策略显然解决不了碰撞问题。机器人主动绕行，避开障碍物前进方向，在避障成功后，再返回避让该障碍物之前规划好的路径当中。

当避障失败，障碍物从冲突避免区域进入紧急逃逸区域时，机器人计算逃逸的最佳分离角，以最大速度躲避障碍物。

分离角为机器人逃逸方向与障碍物运动方向的夹角，最佳分离角通过如下方式计算得到：

首先，建立以启动紧急逃逸模式时的障碍物质心为原点，障碍物的运动方向为x轴正方向的直角坐标系。

其次，以下列不等式组求解能够逃逸成功的分离角范围：

a.移动机器人与动态障碍物之间不发生碰撞，需满足如下不等式：

$\sqrt{{(x_R+v_{\mathrm{RMAX}}t\cos \mathrm{\θ}-v_{O}t)}^2+{(y_R+v_{\mathrm{RMAX}}t\sin \mathrm{\θ})}^2}\≥(R_O+R_R)$

b.其次，动态障碍物已运动到机器人的冲突避免区域或机器人紧急逃逸成功，分别需满足如下两个不等式：

$\sqrt{{(x_R+v_{\mathrm{RMAX}}t\cos \mathrm{\θ}-v_{O}t)}^2+{(y_R+v_{\mathrm{RMAX}}t\sin \mathrm{\θ})}^2}\≥D_{F3}$ (障碍物运动到冲突避免区域)

y_R+v_RMAXtsinθ≥(R_O+R_R)    (机器人紧急逃逸成功)

(x_R，y_R)是机器人当前位置坐标，(x_R+v_RMAXtcosθ，y_R+v_RMAXtsinθ)在避障过程中机器人的坐标位置可表示为随时间t变化的线性方程，其中v_RMAX为逃逸的最大速度，θ为分离角，障碍物的移动时刻的坐标位置可也可表示为随时间t变化(v_Ot，0)。

最后，在求得能逃逸成功的分离角范围之后，然后根据机器人的运动方向，选择一个与机器人运动方向偏离最小的分离角，该分离角就是逃逸成功的最佳分离角。

Claims (3)

1、一种基于障碍物运动预测的移动机器人避障方法，包括以下步骤：

(1)在机器人移动过程当中，将环绕于机器人周围的空间以机器人为中心划分为3个区域，由外到内分别为：路径规划区域、冲突避免区域以及紧急逃逸区域；

(2)机器人根据障碍物当前的位置，判断障碍物当前所处的区域，并预测障碍物下一时间的运动状态；

当所有障碍物处于路径规划区域时，机器人根据预测的障碍物下一时间的运动状态进行路径规划；

当有障碍物进入冲突避免区域时，机器人停止路径规划，根据预测的该障碍物下一时间的运动状态，通过变速或绕行避开该障碍物；

当避障失败，障碍物从冲突避免区域进入紧急逃逸区域时，机器人计算能够逃逸成功的分离角范围，根据机器人的运动方向，选择一个与机器人运动方向偏离最小的分离角作为最佳分离角，以最大速度躲避障碍物。

2、如权利要求1所述的移动机器人避障方法，其特征在于，所述的路径规划采用如下方法：

a.将预测的障碍物下一时段的运动状态中的速度和加速度引入传统的势力场函数，得到如下所示改进的引力势场函数U_rt(q)与斥力势场函数U_re(q)分别如下：

$U_{\mathrm{rt}}\left(q\right)=\frac{1}{2}{K}_t{|q-q_t|}^2+\frac{1}{2}{K}_{\mathrm{rv}}{\left|v_r\right|}^2+\frac{1}{2}{K}_{\mathrm{ra}}{\left|a_r\right|}^2$

$U_{\mathrm{re}}\left(q\right)=\frac{1}{2}{K}_r{[\frac{1}{\mathrm{\ρ}(q,q_o)}-\frac{1}{{\mathrm{\ρ}}_0}]}^2+\frac{1}{2}{K}_{\mathrm{ev}}{|v_e-v_r|}^2+\frac{1}{2}{K}_{\mathrm{ea}}{|a_e-a_r|}^2,$ ρ(q，q_o)＝|q-q_o|

q、q_t和q_o分别为机器人、目标和障碍物位置向量，K_t与K_r分别为引力势场常数与斥力势场常数，ρ₀是障碍物的斥力影响距离，超过该距离则此障碍物对移动机器人影响为0，K_rv、K_ra分别是速度与加速度的引力势场常数，K_ev、K_ea分别是速度与加速度的斥力场常数；

b.机器人通过使用上述改进的势场函数的人工势场法来规划路径。

3、如权利要求1所述的移动机器人避障方法，其特征在于，所述的预测障碍物下一时间的运动状态的方法如下：

a.使用常速、常加速和当前统计模型描述障碍物运动状态，分别得到障碍物的状态方程；

b.采用交互多模型算法对三个状态方程描述的障碍物运动状态进行估计并融合三个估计结果，预测障碍物下一时间的运动状态。

Images