CN105223956A - 一种全向移动机器人的动态避障方法 - Google Patents

Abstract

本发明提供的动态避障方法针对互惠式速度避障算法的缺陷，设置了避障责任系数，并针对不同的障碍物引入风险因子，以改进互惠式速度避障算法中由于测量误差导致的避障不及时与多余避障的现象，并使规划的避障轨迹更为平滑。在没有外部定位系统的未知动态环境中，采用车载激光测距仪检测障碍物并估计动态障碍物的运动速度，并基于所提出的避障方案实现全向移动机器人自主躲避运动区间内的动态及静态障碍物。

Description

一种全向移动机器人的动态避障方法

技术领域

本发明涉及机器人领域，更具体地，涉及一种全向移动机器人的动态避障方法。

背景技术

机器人能实现任意方向的运动，可广泛应用于军事、工业、家用服务等领域。移动机器人在未知环境中如何有效避开障碍物是机器人实现其自主移动的关键问题之一。在有运动障碍物环境下的避障问题称为动态避障。与传统静态环境下避障不同，由于障碍物的速度与位置有可能实时变化，未知环境中的动态避障含有更多不确定因素，这对动态避障算法的实时性与传感器精度都提出了更高的要求。目前对机器人避障的研究多集中在静态环境中的避障，或把运动障碍物在某个时刻考虑为是静止的，这不利于避障的实时性要求，尤其在遇到复杂环境中速度变化较快的障碍物时，采用这些方案易使机器人出现避障失败或多余避障的现象。

移动机器人的避障研究主要是通过设计障碍物检测策略与避障控制算法，根据车载传感器反馈信息实时调整机器人的速度大小与方向进行避障规划，从而使机器人在行进过程中不与动态及静态障碍物发生任何的碰撞。人工势场法在移动机器人自主避障中得到广泛应用，人工势场法把目标看作低势能点，目标对机器人产生了吸引力，环境中障碍物为高势能点，对机器人产生斥力，机器人在引力和斥力的共同作用下运动以躲避障碍物，并到达目标位置。由于人工势场法仅考虑了移动机器人与障碍物的位置信息，当遇到复杂环境下速度变化较快的障碍物时，人工势场法易出现振荡、多余避碰或避障失败等现象。不同于人工势场法仅考虑位置信息，速度障碍法将碰撞规避问题转化成规避速度障碍的问题。当存在多个障碍物时，计算传感范围内所有障碍物对它的障碍区，然后根据速度障碍区所在的空间位置关系，计算与速度障碍区均不相交的速度，以实现自主避障。当未知环境中存在多个动态障碍时，传统的速度障碍法所规划的避障路径易出现振荡的现象，并且所规划的机器人速度较慢。D.B.J.Van等人提出了互惠式速度避障算法，该算法考虑了多机器人之间的速度信息，利用外部定位系统提供的多个机器人的精确定位信息，在多机器人移动的动态场景中表现出优秀的相互避障性能。互惠式速度避障算法依赖所获信息的对称性，例如在考虑两个机器人之间的避碰时，单个机器人各自承担一半的避障任务。另一方面，算法假设移动机器人能即刻达到任意所需的速度。在实际应用中，由于存在检测与估计误差以及环境变化过快等原因，所获得的单个机器人的信息并不完全对称，而且由于惯性机器人也难以即刻达到所需的速度。在此情况下互惠式速度避障算法易出现避障不及时与多余避障的现象。

将运动区间内的动态障碍物作为移动机器人，因此，机器人在未知动态环境中的自主避障可类比为多个移动机器人之间的避碰。本方案基于速度避障策略设计动态避障算法，考虑互惠式速度避障算法的优点并针对其局限性，本发明设计改进的互惠式速度避障算法，设置避障责任系数，并针对不同的障碍物引入风险因子，以改进互惠式速度障碍算法中由于测量误差导致的避障不及时与多余避障的现象，并使规划的避障轨迹更为平滑。在没有外部定位系统的未知动态环境中，采用车载激光测距仪检测障碍物并估计动态障碍物的运动速度，并基于所提出的避障方案实现机器人自主躲避运动区间内的动态及静态障碍物。

发明内容

本发明为解决以上现有技术的缺陷，提供了一种全向移动机器人的动态避障方法，该方法能够确保移动机器人动态避障的实时性与运动轨迹的平滑性，为未知动态环境中全向移动机器人自主躲避动态及静态障碍物提供一种高适用性、有一定抗干扰能力的方案。

为实现以上发明目的，采用的技术方案是：

一种全向移动机器人的动态避障方法，包括以下步骤：

S1.对机器人周围障碍物的位置进行检测，并根据机器人与障碍物相对位置的变化计算二者的相对速度，然后在获知机器人的速度的基础上，计算障碍物的速度；

S2.设机器人i、障碍物j的半径分别为r_i与r_j，速度为v_i与v_j，位置为p_i与p_j，则障碍物j对机器人i产生的速度障碍空间记为VO_i|j ^τ，它表示导致机器人i与障碍物j在时间τ内相碰撞的相对速度的集合：

${{\mathrm{VO}}_{i|j}}^{\mathrm{\τ}}=\{v_i-v_j|\∃t\∈\[0,\mathrm{\τ}\],t*(v_i-v_j)\∈D(p_j-p_i,r_i+r_j)\},$

其中D(p_j-p_i,r_i+r_j)＝{q|||q-(p_j-p_i)||＜r_i+r_j}表示以(p_j-p_i)为中心，(r_i+r_j)为半径的圆域；

S3.计算机器人i和障碍物j的避障责任系数a_i、a_j，其具体步骤如下：

S31.初始时刻，设每个机器人或障碍物初始速度的大小相同，按照各自从起点到目标点的距离从大到小进行排序以分配责任系数，其中，距离目标点最远的机器人或障碍物的责任系数为1，距离目标点最近的机器人或障碍物的责任系数为n_r，n_r为机器人和障碍物的数量；

S32.在行进过程中，把机器人或障碍物到各自目标点的距离除以机器人或障碍物自身的速度大小作为需耗费的时间值，然后根据该时间值按从大到小的顺序重新排序，以此更新每个机器人的责任系数，时间值最大的机器人的责任系数为1，时间值最小的机器人的责任系数为n_r；

S4.计算障碍物j对机器人i的风险因子，具体如下：

(1)机器人i与障碍物j的相对速度在速度障碍空间的外部时，采用机器人i和障碍物j之间的距离来表示障碍物j对机器人i的风险因子x_i|j：

$x_{i|j}=\left\{\begin{array}{cc}0,& \left|\right|p_i,p_j\left|\right|\&GreaterEqual;d_{\max },\\ 100*\frac{d_{\max }-\left|\right|p_i,p_j\left|\right|}{d_{\max }-d_{\min }},& d_{\min }<\left|\right|p_i,p_j\left|\right|<d_{\max },\\ 100,& \left|\right|p_i,p_j\left|\right|\&le;d_{\min }.\end{array}\right.$

其中||p_i,p_j||表示机器人i和障碍物j之间的相对距离，d_max、d_min分别表示相对距离的最大阈值和最小阈值；

(2)机器人i与障碍物j的相对速度在速度障碍空间的内部时，计算两者的碰撞时间t_i|j，并采用碰撞时间t_i|j和相对距离来确定风险因子的大小：

$x_{i|j}=\left\{\begin{array}{cc}0,& \left|\right|p_i,p_j\left|\right|>d_{\max }and{t}_{i|j}\&GreaterEqual;t_{\max },\\ 100*\frac{t_{\max }-t_{i|j}}{t_{\max }-t_{\min }},& \left|\right|p_i,p_j\left|\right|>d_{\min }and{t}_{\min }<t_{i|j}<t_{\max },\\ 100,& \left|\right|p_i,p_j\left|\right|\&le;d_{\min }or{t}_{i|j}\&le;t_{\min }.\end{array}\right.;$

其中t_max、t_min表示碰撞时间的两个阈值；

S4.根据计算得到的风险因子x_i|j来确定机器人i与障碍物j的相对速度的改变量Δv_ij：

${\mathrm{\&Delta;v}}_{ij}=\left\{\begin{array}{cc}0,& \begin{array}{cc}if& x_{i|j}=0\end{array},\\ \frac{x_{i|j}}{50}u,& 0<x_{i|j}<100,\\ 5u,& \begin{array}{cc}if& x_{i|j}=100\end{array}.\end{array}\right.$

其中u为机器人i和障碍物j的相对速度(v_i-v_j)到VO_i|j ^τ边界长度最小的向量，即： $u=\left(\underset{v\&Element;\&part;{{\mathrm{VO}}_{i|j}}^{\mathrm{\&tau;}}}{\arg \min }\right||v-(v_i-v_j\left)\right|\left|\right)-(v_i-v_j)$

S5.求取机器人i、障碍物j的速度改变量，表示如下

$\left\{\begin{array}{c}\mathrm{\&Delta;}{v}_i=\frac{1}{M+2}\mathrm{\&Delta;}{v}_{ij},\\ \mathrm{\&Delta;}{v}_j=\frac{M+1}{M+2}\mathrm{\&Delta;}{v}_{ij}.\end{array}\right.$

其中a_j-a_i＝M；

S6.求取机器人i和障碍物j在时间τ内不会相撞的速度集合ORCA_i|j ^τ，ORCA_i|j ^τ是一个以v_i+Δv_i为起始点，以n为方向的半平面：

ORCA_i|j ^τ＝{v|(v-(v_i+Δv_i))·n≥0}

若n_r＞2，可通过下式构造ORCA_i ^τ集合：

${{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}=D(0,{v_i}^{\max })\underset{j\&NotEqual;i}{I}{{\mathrm{ORCA}}_{i|j}}^{\mathrm{\&tau;}};$

D(0,v_i ^max)代表机器人的速度限制；

S7.根据构造的ORCA_i ^τ，求解机器人i的更新速度v_i ^adjusted：

${v_i}^{adjusted}=\underset{v\&Element;{{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}}{\mathrm{argmin}}\left|\right|v-{v_i}^{pref}\left|\right|$

更新速度v_i ^adjusted为集合ORCA_i ^τ中最接近期待速度v_i ^pref的值。

优选地，所述步骤S1中，使用激光测距仪对机器人与障碍物的距离进行检测，然后根据检测的数据确定障碍物相对机器人的位置，其具体过程如下：

S11.建立扫描窗对激光测距仪回传的扫描数据进行处理，扫描窗的半径设为d_setmax，m为数据点的序列号，d_m为第m个数据距离激光测距仪的距离值，并且d_m≠d_setmax；

S12.设相邻两个数据点对应的距离的差值为|d_m+1-d_m|，根据|d_m+1-d_m|使用S121的方法判断数据点对应的障碍物检测情况；

S13.假设第j个障碍物表示为其中 分别表示第j个障碍物在扫描窗内的起始点、结束点、中间点的序列号；d_j为中间点与激光测距仪的距离，把d_j近似为障碍物j到机器人i几何中心的相对距离，由此可估计障碍物j相对机器人i的位置；

所述步骤S121的操作过程具体如下：

A.判断d_m是否与d_setmax相等，若否，执行步骤B，否则执行步骤D；

B.判断d_m+1是否与d_setmax相等，若是，则表示检测不到障碍物，否则执行步骤C；

C.判断|d_m-d_m+1|是否大于设定的阈值d_hold，若是则表示检测到了两个障碍物，若否则表示这两个数据点检测到的障碍物为同一障碍物；

D.判断d_m+1是否与d_setmax相等，若是，则表示检测不到障碍物，否则执行步骤F；

F.判断d_m+k是否与d_setmax相等，若是则执行步骤G，若否则令k＝k+1，然后执行步骤F；

G.判断k是否大于设定的阈值n_hold，若是则表示检测到了新的障碍物，若否则表示检测不到障碍物。

其中，n_hold为设置的有效障碍物数据点数的阈值，采用动态阈值法设置该阈值，以此减少避障过程中由于外部干扰产生的障碍物误判：障碍物距离机器人越近，n_hold的值设置得越大；障碍物距离机器人越远，n_hold的值设置得越小。

优选地，步骤S1中，计算机器人与障碍物相对速度的具体过程如下：

设在t时刻检测到扫描窗内第j个障碍物的信息表示为分别表示第j个障碍物t时刻在扫描窗内的起始点、结束点、中间点的序列号，d_j,t表示t时刻障碍物j到机器人i的相对距离；而在t+T时刻，检测到的第j个障碍物的信息表示为则可求取T时间内障碍物j相对机器人i沿X轴和Y轴的位置变化量Δx和Δy：

$\left\{\begin{array}{c}\mathrm{\&Delta;}x=d_{j,t+T}*\cos (\mathrm{\&pi;}*N_{j,t+T}^{middle}/628)-d_{j,t}*\cos (\mathrm{\&pi;}*N_{j,t}^{middle}/628),\\ \mathrm{\&Delta;}y=d_{j,t+T}*\sin (\mathrm{\&pi;}*N_{j,t+T}^{middle}/628)-d_{j,t}*\sin (\mathrm{\&pi;}*N_{j,t}^{middle}/628).\end{array}\right.$

根据(Δx,Δy)可计算障碍物j与机器人的相对速度v_OtoR大小：

$V_{{\mathrm{OtoR}}_x}=\frac{\mathrm{\&Delta;}x}{T},V_{{\mathrm{OtoR}}_y}=\frac{\mathrm{\&Delta;}y}{T}$

相对速度v_OtoR的方向为：

$\mathrm{\&alpha;}=\left\{\begin{array}{cc}\arctan \frac{V_{{\mathrm{OtoR}}_y}}{V_{{\mathrm{OtoR}}_x}},& V_{{\mathrm{OtoR}}_x}>0,\\ \mathrm{\&pi;}+\arctan \frac{V_{{\mathrm{OtoR}}_y}}{V_{{\mathrm{OtoR}}_x}},& V_{{\mathrm{OtoR}}_x}<0.\end{array}\right.$

与现有技术相比，本发明的有益效果是：

本发明提供的动态避障方法针对互惠式速度避障算法的缺陷，设置了避障责任系数，并针对不同的障碍物引入风险因子，以改进互惠式速度避障算法中由于测量误差导致的避障不及时与多余避障的现象，并使规划的避障轨迹更为平滑。在没有外部定位系统的未知动态环境中，采用车载激光测距仪检测障碍物并估计动态障碍物的运动速度，并基于所提出的避障方案实现全向移动机器人自主躲避运动区间内的动态及静态障碍物。

附图说明

图1为检测障碍物的示意图。

图2判断数据点对应的障碍物检测情况的方法流程图。

图3为计算机器人与障碍物相对速度的原理图。

图4为某时刻机器人i和障碍物j的位置分布图。

图5为速度障碍VO_i|j ^τ和集合ORCA_i|j ^τ的几何表示。

图6为碰撞时间t_i|j的求解过程示意图。

图7为动态避障方法的流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

以下结合附图和实施例对本发明做进一步的阐述。

实施例1

在对本发明提供的动态避障方法进行阐述之前，首先对现有技术提供的互惠式速度避障算法进行介绍，其具体技术方案如下：

互惠式速度避障算法基于机器人之间位置信息和相对速度求取机器人的最优避障速度集合ORCA_i ^τ，然后根据此集合规划机器人的新速度，以保障一定时间内机器人不与其它机器人碰撞。考虑两个机器人i与j，假设两者的半径分别为r_i与r_j，位置为p_i与p_j，速度为v_i与v_j，如图4所示。将机器人j对机器人i产生的速度障碍记为VO_i|j ^τ，它表示导致机器人i与j在时间τ内相碰撞的相对速度的集合：

${{\mathrm{VO}}_{i|j}}^{\mathrm{\&tau;}}=\{v_i-v_j|\&Exists;t\&Element;\&lsqb;0,\mathrm{\&tau;}\&rsqb;,t*(v_i-v_j)\&Element;D(p_j-p_i,r_i+r_j)\},$

其中D(p,r)＝{q|||q-p||＜r}代表以p为中心r为半径的圆域。集合VO_i|j ^τ的几何表示如图5左上方所示。若机器人i和j的相对速度不在该速度集合内，则机器人i与机器人j在时间τ内不会相撞。令u为机器人i和j的相对速度(v_i-v_j)到VO_i|j ^τ边界长度最小的向量，即：

$u=\left(\underset{v\&Element;\&part;{{\mathrm{VO}}_{i|j}}^{\mathrm{\&tau;}}}{\arg \min }\right||v-(v_i-v_j\left)\right|\left|\right)-(v_i-v_j),$

改变u即可确保机器人i与机器人j不发生相撞。假设机器人i与机器人j分别承担一半的碰撞规避职责，故机器人i与机器人j的速度最少需改变u/2。设n为边界上的点(v_i-v_j+u)上的单位外法向量。因此，保证机器人i和j在时间τ内不会相撞的i的速度集合ORCA_i|j ^τ是一个以v_i+u/2为起始点，以n为方向的半平面：

${{\mathrm{ORCA}}_{i|j}}^{\mathrm{\&tau;}}=\left\{v\right|(v-(v_i+\frac{1}{2}u\left)\right)\&CenterDot;n\&GreaterEqual;0\}$

根据对称性，用同样的方法亦能得到ORCA_j|i ^τ。对于机器人数目为n_r＞2的场景，可以通过下式构造出ORCA_i ^τ集合：

${{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}=D(0,{v_i}^{\max })\underset{j\&NotEqual;i}{I}{{\mathrm{ORCA}}_{i|j}}^{\mathrm{\&tau;}}---\left(1\right)$

至此，保证在时间τ内不与其它物体相撞的机器人i的最优避障速度集合ORCA_i ^τ构造完成。根据构造的ORCA_i ^τ集合，通过求解下式可得到机器人i的更新速度v_i ^adjusted：

${v_i}^{adjusted}=\underset{v\&Element;{{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}}{\arg \min }\left|\right|v-{v_i}^{pref}\left|\right|---\left(2\right)$

其中更新速度v_i ^adjusted为集合ORCA_i ^τ中最接近期待速度v_i ^pref的值。由于集合ORCA_i ^τ是一个广义上的凸集，可通过线性规划的方法求解(1)、(2)等式。

然而，在动态避障过程中机器人的速度实时变化，可能出现部分机器人的运动速度变得很小而相距目标点又较远的情况，由此这些机器人将会在到达目标点的耗时长度这一指标上落后于其它机器人；此外，在行进过程中可能出现移动速度慢的机器人长时间停留在一个区域而使其它机器人的移动通道被挡的情况，以上因素都将造成多机器人系统到达目标点的时间变长。

因此，本发明为每个机器人设立避障责任系数，各机器人根据责任系数决定各自避障时的任务承担量，以节省多机器人系统整体路径规划的时间。

同时，当机器人的运动区域内存在多个动态障碍物时，由于各障碍物的位置与速度位置信息各不同，因而碰撞的风险也不同。由于存在检测误差与环境信息变化过快等原因，如果机器人在避障时不考虑碰撞风险的因素，可能会出现与风险较大的障碍物相碰撞，而与无碰撞风险的障碍物进行多余避障的情况。因此，本方案引入各障碍物相对机器人的风险因子，根据风险因子合理地规划机器人的实时运动速度，以改善机器人避障不及时及多余避障现象，使规划的避障轨迹更为平滑。并分别考虑机器人i与障碍物j的相对速度在速度障碍空间外部与内部两种不同的情形以分析和确定风险因子。

在以上的基础上，本实施例对本发明提供的动态避障算法的具体方案如下：

S1.对机器人周围障碍物的位置进行检测，并根据机器人与障碍物相对位置的变化计算二者的相对速度，然后在获知机器人的速度的基础上，计算障碍物的速度；

S2.设机器人i、障碍物j的半径分别为r_i与r_j，速度为v_i与v_j，位置为p_i与p_j，则障碍物j对机器人i产生的速度障碍空间记为VO_i|j ^τ，它表示导致机器人i与障碍物j在时间τ内相碰撞的相对速度的集合：

${{\mathrm{VO}}_{i|j}}^{\mathrm{\&tau;}}=\{v_i-v_j|\&Exists;t\&Element;\&lsqb;0,\mathrm{\&tau;}\&rsqb;,t*(v_i-v_j)\&Element;D(p_j-p_i,r_i+r_j)\},$

其中D(p_j-p_i,r_i+r_j)＝{q|||q-(p_j-p_i)||＜r_i+r_j}表示以(p_j-p_i)为中心，(r_i+r_j)为半径的圆域；

S3.计算机器人i和障碍物j的避障责任系数a_i、a_j，其具体步骤如下：

S31.初始时刻，设每个机器人或障碍物初始速度的大小相同，按照各自从起点到目标点的距离从大到小进行排序以分配责任系数，其中，距离目标点最远的机器人或障碍物的责任系数为1，距离目标点最近的机器人或障碍物的责任系数为n_r，n_r为机器人和障碍物的数量；

S32.在行进过程中，把机器人或障碍物到各自目标点的距离除以机器人或障碍物自身的速度大小作为需耗费的时间值，然后根据该时间值按从大到小的顺序重新排序，以此更新每个机器人的责任系数，时间值最大的机器人的责任系数为1，时间值最小的机器人的责任系数为n_r；

S4.计算障碍物j对机器人i的风险因子，具体如下：

(1)机器人i与障碍物j的相对速度在速度障碍空间的外部时，采用机器人i和障碍物j之间的距离来表示障碍物j对机器人i的风险因子x_i|j：

$x_{i|j}=\left\{\begin{array}{cc}0,& \left|\right|p_i,p_j\left|\right|\&GreaterEqual;d_{\max },\\ 100*\frac{d_{\max }-\left|\right|p_i,p_j\left|\right|}{d_{\max }-d_{\min }},& d_{\min }<\left|\right|p_i,p_j\left|\right|<d_{\max },\\ 100,& \left|\right|p_i,p_j\left|\right|\&le;d_{\min }.\end{array}\right.$

其中||p_i,p_j||表示机器人i和障碍物j之间的相对距离，d_max、d_min分别表示相对距离的最大阈值和最小阈值；

(2)机器人i与障碍物j的相对速度在速度障碍空间的内部时，计算两者的碰撞时间t_i|j，并采用碰撞时间t_i|j和相对距离来确定风险因子的大小：

$x_{i|j}=\left\{\begin{array}{cc}0,& \left|\right|p_i,p_j\left|\right|>d_{\max }and{t}_{i|j}\&GreaterEqual;t_{\max },\\ 100*\frac{t_{\max }-t_{i|j}}{t_{\max }-t_{\min }},& \left|\right|p_i,p_j\left|\right|>d_{\min }and{t}_{\min }<t_{i|j}<t_{\max },\\ 100,& \left|\right|p_i,p_j\left|\right|\&le;d_{\min }or{t}_{i|j}\&le;t_{\min }.\end{array}\right.;$

其中t_max、t_min表示碰撞时间的两个阈值；

图6所示为t_i|j的求解，其中，v_ij表示机器人i与障碍物j的相对速度；p_i与p_j分别表示机器人i与障碍物j的位置，||p_i,p_j||表示机器人i和障碍物j之间的相对距离；p_x为碰撞点；r_i+r_j表示机器人i与障碍物j的半径之和。在三角形p_ip_jp_x内，由向量v_ij与向量可求出α的角度值，则由余弦定理求出线段p_ip_x的长度。将碰撞时间t_i|j考虑为线段p_ip_x的长度与相对速度v_ij的比值，则t_i|j可计算求得：

t_i|j＝||p_i,p_x||/|v_ij|

S4.根据计算得到的风险因子x_i|j来确定机器人i与障碍物j的相对速度的改变量Δv_ij：

${\mathrm{\&Delta;v}}_{ij}=\left\{\begin{array}{cc}0,& \begin{array}{cc}if& x_{i|j}=0,\end{array}\\ \frac{x_{i|j}}{50}u,& 0<x_{i|j}<100,\\ 5u,& \begin{array}{cc}if& x_{i|j}=100.\end{array}\end{array}\right.$

其中u为机器人i和障碍物j的相对速度(v_i-v_j)到VO_i|j ^τ边界长度最小的向量，即： $u=\left(\underset{v\&Element;\&part;{{\mathrm{VO}}_{i|j}}^{\mathrm{\&tau;}}}{\arg \min }\right||v-(v_i-v_j\left)\right|\left|\right)-(v_i-v_j)$

S5.求取机器人i、障碍物j的速度改变量，表示如下

$\left\{\begin{array}{c}\mathrm{\&Delta;}{v}_i=\frac{1}{M+2}\mathrm{\&Delta;}{v}_{ij},\\ \mathrm{\&Delta;}{v}_j=\frac{M+1}{M+2}\mathrm{\&Delta;}{v}_{ij}.\end{array}\right.$

其中a_j-a_i＝M；

S6.求取机器人i和障碍物j在时间τ内不会相撞的速度集合ORCA_i|j ^τ，ORCA_i|j ^τ是一个以v_i+Δv_i为起始点，以n为方向的半平面：

ORCA_i|j ^τ＝{v|(v-(v_i+Δv_i))·n≥0}

若n_r＞2，可通过下式构造ORCA_i ^τ集合：

${{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}=D(0,{v_i}^{\max })\underset{j\&NotEqual;i}{I}{{\mathrm{ORCA}}_{i|j}}^{\mathrm{\&tau;}};$

D(0,v_i ^max)代表机器人的速度限制；

S7.根据构造的ORCA_i ^τ，求解机器人i的更新速度v_i ^adjusted：

${v_i}^{adjusted}=\underset{v\&Element;{{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}}{\mathrm{argmin}}\left|\right|v-{v_i}^{pref}\left|\right|$

更新速度v_i ^adjusted为集合ORCA_i ^τ中最接近期待速度v_i ^pref的值。

在具体的实施过程中，使用激光测距仪对机器人与障碍物的距离进行检测，然后根据检测的数据确定障碍物相对机器人的位置，如图1所示，其具体过程如下：

S11.建立扫描窗对激光测距仪回传的扫描数据进行处理，扫描窗的半径设为d_setmax，m为数据点的序列号，d_m为第m个数据距离激光测距仪的距离值，并且d_m≠d_setmax；

S12.设相邻两个数据点对应的距离的差值为|d_m+1-d_m|，根据|d_m+1-d_m|使用S121的方法判断数据点对应的障碍物检测情况；

S13.假设第j个障碍物表示为其中 分别表示第j个障碍物在扫描窗内的起始点、结束点、中间点的序列号；d_j为中间点与激光测距仪的距离，把d_j近似为障碍物j到机器人i几何中心的相对距离，由此可估计障碍物j相对机器人i的位置；

如图2所示，所述步骤S121的操作过程具体如下：

A.判断d_m是否与d_setmax相等，若否，执行步骤B，否则执行步骤D；

B.判断d_m+1是否与d_setmax相等，若是，则表示检测不到障碍物，否则执行步骤C；

C.判断|d_m-d_m+1|是否大于设定的阈值d_hold，若是则表示检测到了两个障碍物，若否则表示这两个数据点检测到的障碍物为同一障碍物；

D.判断d_m+1是否与d_setmax相等，若是，则表示检测不到障碍物，否则执行步骤F；

F.判断d_m+k是否与d_setmax相等，若是则执行步骤G，若否则令k＝k+1，然后执行步骤F；

G.判断k是否大于设定的阈值n_hold，若是则表示检测到了新的障碍物，若否则表示检测不到障碍物。

其中，n_hold为设置的有效障碍物数据点数的阈值，采用动态阈值法设置该阈值，以此减少避障过程中由于外部干扰产生的障碍物误判：障碍物距离机器人越近，n_hold的值设置得越大；障碍物距离机器人越远，n_hold的值设置得越小。

其中，如图2所示，计算机器人与障碍物相对速度的具体过程如下：

设在t时刻检测到扫描窗内第j个障碍物的信息表示为

分别表示第j个障碍物t时刻在扫描窗内的起始点、结束点、中间点的序列号，d_j,t表示t时刻障碍物j到机器人i的相对距离；而在t+T时刻，检测到的第j个障碍物的信息表示为则可求取T时间内障碍物j相对机器人i沿X轴和Y轴的位置变化量Δx和Δy：

$\left\{\begin{array}{c}\mathrm{\&Delta;}x=d_{j,t+T}*\cos (\mathrm{\&pi;}*N_{j,t+T}^{middle}/628)-d_{j,t}*\cos (\mathrm{\&pi;}*N_{j,t}^{middle}/628),\\ \mathrm{\&Delta;}y=d_{j,t+T}*\sin (\mathrm{\&pi;}*N_{j,t+T}^{middle}/628)-d_{j,t}*\sin (\mathrm{\&pi;}*N_{j,t}^{middle}/628).\end{array}\right.$

根据(Δx,Δy)可计算障碍物j与机器人的相对速度v_OtoR大小：

$V_{{\mathrm{OtoR}}_x}=\frac{\mathrm{\&Delta;}x}{T},V_{{\mathrm{OtoR}}_y}=\frac{\mathrm{\&Delta;}y}{T}$

相对速度v_OtoR的方向为：

$\mathrm{\&alpha;}=\left\{\begin{array}{cc}\arctan \frac{V_{{\mathrm{OtoR}}_y}}{V_{{\mathrm{OtoR}}_x}},& V_{{\mathrm{OtoR}}_x}>0,\\ \mathrm{\&pi;}+\arctan \frac{V_{{\mathrm{OtoR}}_y}}{V_{{\mathrm{OtoR}}_x}},& V_{{\mathrm{OtoR}}_x}<0.\end{array}\right..$

动态避障算法的流程如图7所示，基于检测与估计的障碍物信息根据动态避障算法计算机器人躲避障碍的实时速度，移动机器人采用此更新速度运动一个控制周期，在行进过程中按照该动态避障算法流程循环，直至到达目标点。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (4)

1.一种全向移动机器人的动态避障方法，其特征在于：包括以下步骤：

S1.对机器人周围障碍物的位置进行检测，并根据机器人与障碍物相对位置的变化计算二者的相对速度，然后在获知机器人的速度的基础上，计算障碍物的速度；

S2.设机器人i、障碍物j的半径分别为r_i与r_j，速度为v_i与v_j，位置为p_i与p_j，则障碍物j对机器人i产生的速度障碍空间记为VO_i|j ^τ，它表示导致机器人i与障碍物j在时间τ内相碰撞的相对速度的集合：

其中D(p_j-p_i,r_i+r_j)＝{q|||q-(p_j-p_i)||＜r_i+r_j}表示以(p_j-p_i)为中心，(r_i+r_j)为半径的圆域；

S3.计算机器人i和障碍物j的避障责任系数a_i、a_j，其具体步骤如下：

S31.初始时刻，设每个机器人或障碍物初始速度的大小相同，按照各自从起点到目标点的距离从大到小进行排序以分配责任系数，其中，距离目标点最远的机器人或障碍物的责任系数为1，距离目标点最近的机器人或障碍物的责任系数为n_r，n_r为机器人和障碍物的数量；

S32.在行进过程中，把机器人或障碍物到各自目标点的距离除以机器人或障碍物自身的速度大小作为需耗费的时间值，然后根据该时间值按从大到小的顺序重新排序，以此更新每个机器人的责任系数，时间值最大的机器人的责任系数为1，时间值最小的机器人的责任系数为n_r；

S4.计算障碍物j对机器人i的风险因子，具体如下：

(1)机器人i与障碍物j的相对速度在速度障碍空间的外部时，采用机器人i和障碍物j之间的距离来表示障碍物j对机器人i的风险因子x_i|j：

$x_{i|j}=\left\{\begin{array}{cc}0,& \left|\right|p_i,p_j\left|\right|\&GreaterEqual;d_{\max },\\ 100*\frac{d_{\max }-\left|\right|p_i,p_j\left|\right|}{d_{\max }-d_{\min }},& d_{\min }<\left|\right|p_i,p_j\left|\right|<d_{\max },\\ 100,& \left|\right|p_i,p_j\left|\right|\&le;d_{\min }.\end{array}\right.$

其中||p_i,p_j||表示机器人i和障碍物j之间的相对距离，d_max、d_min分别表示相对距离的最大阈值和最小阈值；

(2)机器人i与障碍物j的相对速度在速度障碍空间的内部时，计算两者的碰撞时间t_i|j，并采用碰撞时间t_i|j和相对距离来确定风险因子的大小：

$x_{i|j}=\left\{\begin{array}{cc}0,& \left|\right|p_i,p_j\left|\right|>d_{\max }and{t}_{i|j}\&GreaterEqual;t_{\max },\\ 100*\frac{t_{\max }-t_{i|j}}{t_{\max }-t_{\min }},& \left|\right|p_i,p_j\left|\right|>d_{\min }and{t}_{\min }<t_{i|j}<t_{\max },\\ 100,& \left|\right|p_i,p_j\left|\right|\&le;d_{\min }or{t}_{i|j}\&le;t_{\min }.\end{array}\right.;$

其中t_max、t_min表示碰撞时间的两个阈值；

S4.根据计算得到的风险因子x_i|j来确定机器人i与障碍物j的相对速度的改变量Δv_ij：

${\mathrm{\&Delta;v}}_{ij}=\left\{\begin{array}{cc}0,& if{x}_{i|j}=0,\\ \frac{x_{i|j}}{50}u,& 0<x_{i|j}<100,\\ 5u,& if{x}_{i|j}=100.\end{array}\right.$

其中u为机器人i和障碍物j的相对速度(v_i-v_j)到VO_i|j ^τ边界长度最小的向量，即： $u=\left(\underset{v\&Element;\&part;{{\mathrm{VO}}_{i|j}}^{\mathrm{\&tau;}}}{\arg \min }\right||v-(v_i-v_j)\left|\right|)-(v_i-v_j)$

S5.求取机器人i、障碍物j的速度改变量，表示如下

$\left\{\begin{array}{c}{\mathrm{\&Delta;v}}_i=\frac{1}{M+2}{\mathrm{\&Delta;v}}_{ij},\\ {\mathrm{\&Delta;v}}_j=\frac{M+1}{M+2}{\mathrm{\&Delta;v}}_{ij}.\end{array}\right.$

其中a_j-a_i＝M；

S6.求取机器人i和障碍物j在时间τ内不会相撞的速度集合ORCA_i|j ^τ，ORCA_i|j ^τ是一个以v_i+Δv_i为起始点，以n为方向的半平面：

ORCA_i|j ^τ＝{v|(v-(v_i+Δv_i))·n≥0}

若n_r＞2，可通过下式构造ORCA_i ^τ集合：

${{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}=D(0,{v_i}^{\max })\underset{j\&NotEqual;i}{I}{{\mathrm{ORCA}}_{i|j}}^{\mathrm{\&tau;}};$

D(0,v_i ^max)代表机器人的速度限制；

S7.根据构造的ORCA_i ^τ，求解机器人i的更新速度v_i ^adjusted：

${v_i}^{adjusted}=\underset{v\&Element;{{\mathrm{ORCA}}_i}^{\mathrm{\&tau;}}}{\arg min}\left|\right|v-{v_i}^{pref}\left|\right|$

更新速度v_i ^adjusted为集合ORCA_i ^τ中最接近期待速度v_i ^pref的值。

2.根据权利要求1所述的全向移动机器人的动态避障方法，其特征在于：所述步骤S1中，使用激光测距仪对机器人与障碍物的距离进行检测，然后根据检测的数据确定障碍物相对机器人的位置，其具体过程如下：

S11.建立扫描窗对激光测距仪回传的扫描数据进行处理，扫描窗的半径设为d_setmax，m为数据点的序列号，d_m为第m个数据距离激光测距仪的距离值，并且d_m≠d_setmax；

S12.设相邻两个数据点对应的距离的差值为|d_m+1-d_m|，根据|d_m+1-d_m|使用S121的方法判断数据点对应的障碍物检测情况；

S13.假设第j个障碍物表示为其中 分别表示第j个障碍物在扫描窗内的起始点、结束点、中间点的序列号；d_j为中间点与激光测距仪的距离，把d_j近似为障碍物j到机器人i几何中心的相对距离，由此可估计障碍物j相对机器人i的位置；

所述步骤S121的操作过程具体如下：

A.判断d_m是否与d_setmax相等，若否，执行步骤B，否则执行步骤D；

B.判断d_m+1是否与d_setmax相等，若是，则表示检测不到障碍物，否则执行步骤C；

C.判断|d_m-d_m+1|是否大于设定的阈值d_hold，若是则表示检测到了两个障碍物，若否则表示这两个数据点检测到的障碍物为同一障碍物；

D.判断d_m+1是否与d_setmax相等，若是，则表示检测不到障碍物，否则执行步骤F；

F.判断d_m+k是否与d_setmax相等，若是则执行步骤G，若否则令k＝k+1，然后执行步骤F；

G.判断k是否大于设定的阈值n_hold，若是则表示检测到了新的障碍物，若否则表示检测不到障碍物。

3.根据权利要求2所述的全向移动机器人的动态避障方法，其特征在于：所述阈值n_hold为设置的有效障碍物数据点数的阈值，采用动态阈值法设置该阈值，以此减少避障过程中由于外部干扰产生的障碍物误判：障碍物距离机器人越近，n_hold的值设置得越大；障碍物距离机器人越远，n_hold的值设置得越小。

4.根据权利要求2所述的全向移动机器人的动态避障方法，其特征在于：步骤S1中，计算机器人与障碍物相对速度的具体过程如下：

设在t时刻检测到扫描窗内第j个障碍物的信息表示为 分别表示第j个障碍物t时刻在扫描窗内的起始点、结束点、中间点的序列号，d_j,t表示t时刻障碍物j到机器人i的相对距离；而在t+T时刻，检测到的第j个障碍物的信息表示为则可求取T时间内障碍物j相对机器人i沿X轴和Y轴的位置变化量Δx和Δy：

$\left\{\begin{array}{c}\mathrm{\&Delta;}x=d_{j,t+T}*\cos \left(\mathrm{\&pi;}*N_{j,t+T}^{middle}/628\right)-d_{j,t}*\cos \left(\mathrm{\&pi;}*N_{j,t}^{middle}/628\right),\\ \mathrm{\&Delta;}y=d_{j,t+T}*\sin \left(\mathrm{\&pi;}*N_{j,t+T}^{middle}/628\right)-d_{j,t}*\sin \left(\mathrm{\&pi;}*N_{j,t}^{middle}/628\right).\end{array}\right.$

根据(Δx,Δy)可计算障碍物j与机器人的相对速度v_OtoR大小：

$V_{{\mathrm{OtoR}}_x}=\frac{\mathrm{\&Delta;}x}{T},V_{{\mathrm{OtoR}}_y}=\frac{\mathrm{\&Delta;}y}{T}$

相对速度v_OtoR的方向为：

$\mathrm{\&alpha;}=\left\{\begin{array}{cc}\arctan \frac{V_{{\mathrm{OtoR}}_y}}{V_{{\mathrm{OtoR}}_x}},& V_{{\mathrm{OtoR}}_x}>0,\\ \mathrm{\&pi;}+\arctan \frac{V_{{\mathrm{OtoR}}_y}}{V_{{\mathrm{OtoR}}_x}},& V_{{\mathrm{OtoR}}_x}<0.\end{array}\right..$