CN109990787B - 一种机器人在复杂场景中规避动态障碍物的方法 - Google Patents

Abstract

本发明涉及一种机器人在复杂场景中规避动态障碍物的方法。该方法可以在机器人存在有限感知范围，且动态障碍物速度大于机器人速度的条件下，产生有效的轨迹；同时在动态障碍物速度不会突变的条件下，该方法产生的轨迹依然可以规避障碍物，而不需要重复计算新路径。该方法基于快速随机树方法生成随机树来寻找可行路径，在快速随机树生长的过程中，利用速度障碍法来进行树节点的生长和选取，可以将避障问题在速度域与位置域同时考虑，且速度障碍生成速度很快，具有高效性；同时，采用考虑到最大机器人速度的速度障碍法(MVO)，可以规避相对高速的动态障碍物；另外，速度障碍物在多机避碰上具有优势，该专利中的方法同样可以适用于多动态障碍物避碰。

Description

一种机器人在复杂场景中规避动态障碍物的方法

技术领域

本发明涉及机器人技术领域，更具体地，涉及一种机器人在复杂场景中规避动态障碍物的方法。

背景技术

随着相关科学技术的发展，机器人渐渐的在各个领域内得到了较为广泛的应用。同时，任务场景的愈加复杂使得机器人在复杂场景中无碰撞运动的需求愈加强烈。例如在仓储、码头装运等等领域中，机器人需要首先根据自身的任务路径行进，同时需要应对随时出现的动态障碍物。所以，机器人在复杂场景中规避动态障碍物具有巨大的研究价值。在复杂场景中，可能存在静态与动态障碍物，机器人如何在这样的环境中运动，如何产生无碰撞的路径，是该问题研究的关键点。

一般的步骤可以概括为，在场景中开始运动之前，机器人首先根据已知的地图(包含静态障碍物)产生一条全局无碰撞路径。在进行路径跟随的过程中，探测到动态障碍物时，进行临时规避。当障碍物规避之后，继续沿着全局路径行驶，直到到达终点。过程包括了全局路径生成算法、路径优化算法、路径跟随算法以及局部路径规划算法。

国内外各个高校以及研究所对机器人在复杂场景中规避动态障碍物的方法也进行了大量的研究。快速搜索随机树方法(rrt)就是一种传统的结局路径规划的算法。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。之后各种改进算法将rrt向各个方向改进。Rrt*加入启发式方程使得得到的结果更加趋于距离最优；connected rrt通过生成两棵树来加快搜索速度；rt-rrt将空间分成栅格，提高了搜索的效率使其效率更快，从而可以用于规避动态障碍物。在规避动态障碍物的情况下，rrt类算法往往效果不佳，一方面rrt中随机树的生成效率不高，另一方面是动态障碍物不断运动，需要不断的生成树和搜索树。

在另一方面，速度障碍法(VO)在多机动态避碰上有着优秀的效果，可以在多障碍物多机器人的场景中，使机器人进行避碰运动。但是此类方法适合机器人在局部范围内的速度域上进行避碰，没法产生一系列无碰撞路径。

发明内容

本发明为克服上述现有技术所述的至少一种缺陷，提供一种机器人在复杂场景中规避动态障碍物的方法，可以适用于不同模型的机器人任务中。

为解决上述技术问题，本发明采用的技术方案是：一种机器人在复杂场景中规避动态障碍物的方法，包括以下步骤：

S1.全局在运动的开始，机器人已知包含静态障碍物信息的场景地图以及目标点的位置，使用informed rrt*算法进行全局的路径规划，得到一条在限定计算时间内最优的全局路径；

S2.将上一步产生的一系列的全局路径点传入路径优化器，路径优化器使用多项式插值的方法进行路径优化，得到一条加加速度最小的路径，同时，该路径在位置、速度、加速度上都是平滑的；

S3.机器人使用模型预测法进行路径跟随，将步骤S2得到的一系列平滑路径点作为模型预测法的位置约束，将机器人模型以及状态约束作为模型预测法的等式、不等式约束；通过求解模型预测模型得到一系列未来的状态值和控制量，选取第一个控制量作为机器人的控制量传入控制系统；

S4.机器人在跟随路径的过程中，感知系统一直在检测动态障碍物；当有动态障碍物进入探测范围内时，使用RRT-VO算法进行路径规划，产生一条可以规避障碍物的新轨迹；

S5.重复步骤S2至S4，直到到达终点。

本发明提供的方法用于机器人在一个存在静态障碍物以及动态障碍物的环境中，进行路径规划规避障碍物的方法。可以在机器人存在有限感知范围，且动态障碍物速度大于机器人速度的条件下，产生有效的轨迹；同时在动态障碍物速度不会突变(变化在一定范围内)的条件下，该方法产生的轨迹依然可以规避障碍物，而不需要重复计算新路径。该专利提出的路径规划方法基于快速随机树方法生成随机树来寻找可行路径，在快速随机树生长的过程中，利用速度障碍法来进行树节点的生长和选取，可以将避障问题在速度域与位置域同时考虑，且速度障碍生成速度很快，具有高效性；同时，采用考虑到最大机器人速度的速度障碍法(MVO)，可以规避相对高速的动态障碍物；另外，速度障碍物在多机避碰上具有优势，该专利中的方法同样可以适用于多动态障碍物避碰。

进一步的，所述的RRT-VO算法具体包括以下步骤：

S41.通过Sample()在地图中进行采样，得到一个随机节点Node_rand；

S42.使用Nearest()函数寻找到在树种最近的节点Node_nearest；

S43.将Node_rand和Node_nearest传入树生长的函数Extend()，构建速度障碍空间：

定义机器人A与障碍物O的半径分别为r_A、r_O，位置为p_O|A＝p_O-p_A；定义在位置域下碰撞区域为：

D(p_O-p_A,r_A+r_O)＝{q|q-(p_O-p_A)＜r_A+r_O}

定义τ为预先定义的时间窗；

D(p,r)＝{q|||q-p||＜r}，代表以P为中心r为半径的圆域；

根据速度障碍法的定义，得到速度障碍法表示为：

考虑机器人最大速度的速度障碍法(MVO)，得到一个增补的速度障碍区域：

两个速度障碍区域取并集即刻得到新的速度障碍区域；

此时，判断以速度为当前速度大小，方向为

的速度是否在速度障碍区域内，如果在，则抛弃Node_rand；如果不在，则生长一条新的树枝，长度为τv，起点为Node_nearest，方向为

得到一个新的节点Node_new，将Node_new加入已经存在的搜索树中，相连接的节点为Node_nearest；

S44.不断重复步骤S41至S43，直到Node_new到达目标点附近，再全局路径结合，得到一条修正后的全局轨迹。

本发明提出的算法RRT-VO，综合快速随机搜索树和速度障碍法，具有二者的优点，可以产生局部无碰撞轨迹，同时，可以应用在多个动态障碍物同时存在的情况下。在障碍物的速度不会剧烈变化的情况下，该算法产生的路径一直有效，不需要反复计算。

与现有技术相比，有益效果是：本发明提出的一种机器人在复杂场景中规避动态障碍物的方法，本发明提出的RRT-VO算法具有计算量小，实时性高，可以应用于多动态障碍物等复杂场景中，不需要多次重复计算等等优势。同时，在路径跟随步骤中，采用模型预测法进行控制，可以结合不同的机器人模型，可以施加不同的状态约束，使得整体的方案可以得到广泛的应用。在全局轨迹生成中，使用informed rrt*算法以及多项式差值优化，使得得到的全局轨迹更加平滑。

附图说明

图1是本发明实施例中一种机器人在复杂场景中规避动态障碍物的方法的系统流程示意图。

图2是本发明实施例中全局路径生成与优化步骤的效果图。

图3是本发明实施例中局部无碰撞路径生成与优化步骤的效果图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

如图1所示，一种机器人在复杂场景中规避动态障碍物的方法，包括以下步骤：

步骤1.全局在运动的开始，机器人已知包含静态障碍物信息的场景地图以及目标点的位置，使用informed rrt*算法进行全局的路径规划，得到一条在限定计算时间内最优的全局路径；该方法通过不断缩小采样的范围，从而提高得到更优路径的概率。根据RRT*首先找到路径后的一个Cbest，在根据Cbest构建一个椭圆进行采样，当Cbest不断减小，椭圆的范围也不断减小，最终得到最优轨迹。

步骤2.将上一步产生的一系列的全局路径点传入路径优化器，路径优化器使用多项式插值的方法进行路径优化，得到一条加加速度最小的路径，同时，该路径在位置、速度、加速度上都是平滑的；具体的：

将产生的一系列的全局路径点传入路径优化器，路径优化器使用polynomialinterpolation(多项式差值)的方法进行路径优化。在离散的路径点中，每两个点可以组成一个细分路径，可以用多项式来表示：

p(t)＝p₀+p₁t+p₂t²+…+p_ntⁿ；

其中p₁,p₂,…,p_n为每一段轨迹的多项式系数，这是一个关于时间t的方程；该方法可以针对机器人的速度约束、加速度约束进行路径优化，得到一条加加速度(jerk)最小的路径：

minjerk:minf(p)＝min(p⁽³⁾(t))²

然后将问题转化成一个二次规划问题：

其中Q是根据使加加速度最小的原则得到的系数矩阵，p为参数向量；

通过解这个二次规划问题，得到路径在位置、速度、加速度上都是平滑的；

经过步骤1和步骤2可以得到如图2所示的轨迹。

步骤3.机器人使用模型预测法进行路径跟随，将步骤S2得到的一系列平滑路径点作为模型预测法的位置约束，将机器人模型以及状态约束作为模型预测法的等式、不等式约束；通过求解模型预测模型得到一系列未来的状态值和控制量，选取第一个控制量作为机器人的控制量传入控制系统；

目标函数可以构造为以下形式，其中z_k为机器人状态变量；

k为预测步长，ω₁，ω₂和ω₃为权重系数；

为k个位置点与参考轨迹的偏差；

为机器人的朝向角度与目标轨迹的偏差；‖u_k-u_d‖²可以使机器人以一个特定的速度范围内运动；

和u_d分别为参考路径，参考朝向和控制量的期望值。

同时，可以将机器人模型约束以及状态约束加入方程中：

其中等式约束为零点约束z_t和机器人的模型，不等式可以为机器人的非完整性约束等等约束。通过求解这个模型预测问题可以得到一系列未来的状态值和控制量。选取第一个控制量作为机器人的控制量传入控制系统。

步骤4.机器人在跟随路径的过程中，感知系统一直在检测动态障碍物；当有动态障碍物进入探测范围内时，使用RRT-VO算法进行路径规划，产生一条可以规避障碍物的新轨迹；此时，在局部生成一个随机搜索树，找到一条新的无碰撞轨迹，使用RRT-VO算法；RRT-VO算法具体包括以下步骤：

S41.通过Sample()在地图中进行采样，得到一个随机节点Node_rand；

S42.使用Nearest()函数寻找到树中最近的节点Node_nearest；

S43.将Node_rand和Node_nearest传入树生长的函数Extend()，构建速度障碍空间：

定义机器人A与障碍物O的半径分别为r_A、r_O，位置为p_O|A＝p_O-p_A；定义在位置域下碰撞区域为：

D(p_O-p_A,r_A+r_O)＝{q|q-(p_O-p_A)＜r_A+r_O}

定义τ为预先定义的时间窗；

D(p,r)＝{q|||q-p||＜r}，代表以P为中心r为半径的圆域；

根据速度障碍法的定义，得到速度障碍法表示为：

考虑机器人最大速度的速度障碍法(MVO)，得到一个增补的速度障碍区域：

两个速度障碍区域取并集即刻得到新的速度障碍区域；

此时，判断以速度为当前速度大小，方向为

的速度是否在速度障碍区域内，如果在，则抛弃Node_rand；如果不在，则生长一条新的树枝，长度为τv，起点为Node_nearest，方向为

得到一个新的节点Node_new，将Node_new加入已经存在的搜索树中，相连接的节点为Node_nearest；

S44.不断重复步骤S41至S43，直到Node_new到达目标点附近，再全局路径结合，得到一条修正后的全局轨迹，如图3所示。

步骤5.重复步骤S2至S4，直到到达终点。

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

Claims (1)

1.一种机器人在复杂场景中规避动态障碍物的方法，其特征在于，包括以下步骤：

S1.全局在运动的开始，机器人已知包含静态障碍物信息的场景地图以及目标点的位置，使用informed rrt*算法进行全局的路径规划，得到一条在限定计算时间内最优的全局路径；

S2.将上一步产生的一系列的全局路径点传入路径优化器，路径优化器使用多项式插值的方法进行路径优化，得到一条加加速度最小的路径，同时，该路径在位置、速度、加速度上都是平滑的；

S3.机器人使用模型预测法进行路径跟随，将步骤S2得到的一系列平滑路径点作为模型预测法的位置约束，将机器人模型以及状态约束作为模型预测法的等式、不等式约束；通过求解模型预测模型得到一系列未来的状态值和控制量，选取第一个控制量作为机器人的控制量传入控制系统；

S4.机器人在跟随路径的过程中，感知系统一直在检测动态障碍物；当有动态障碍物进入探测范围内时，使用RRT-VO算法进行路径规划，产生一条可以规避障碍物的新轨迹；RRT-VO算法具体包括以下步骤：

S41.通过Sample()在地图中进行采样，得到一个随机节点Node_rand；

S42.使用Nearest()函数寻找到树中最近的节点Node_nearest；

S43.将Node_rand和Node_nearest传入树生长的函数Extend()，用于构建速度障碍空间：

定义机器人A与障碍物O的半径分别为r_A、r_O，位置为P_O|A＝P_O-P_A；定义在位置域下碰撞区域为：

D(p_O-p_A,r_A+r_O)＝{q|q-(p_O-p_A)<r_A+r_O}

定义τ为预先定义的时间窗；

D(p,r)＝{q|||q-p||<r}，代表以P为中心r为半径的圆域；

根据速度障碍法的定义，得到速度障碍法表示为：

考虑机器人最大速度的速度障碍法(MVO)，得到一个增补的速度障碍区域：

两个速度障碍区域取并集即刻得到新的速度障碍区域；

此时，判断以速度为当前速度大小，方向为

的速度是否在速度障碍区域内，如果在，则抛弃Node_rand；如果不在，则生长一条新的树枝，长度为τv，起点为Node_nearest，方向为

得到一个新的节点Node_new，将Node_new加入已经存在的搜索树中，相连接的节点为Node_nearest；

S44.不断重复步骤S41至S43，直到Node_new到达目标点附近，再全局路径结合，得到一条修正后的全局轨迹；

S5.重复步骤S2至S4，直到到达终点。

Images