CN113485356A

CN113485356A - 一种机器人快速运动规划方法

Info

Publication number: CN113485356A
Application number: CN202110851774.5A
Authority: CN
Inventors: 朱战霞; 仲剑飞; 李洁
Original assignee: Northwestern Polytechnical University
Current assignee: Northwestern Polytechnical University
Priority date: 2021-07-27
Filing date: 2021-07-27
Publication date: 2021-10-08
Anticipated expiration: 2041-07-27
Also published as: CN113485356B

Abstract

本发明提出一种机器人快速运动规划方法，以采样生成新路径点过程中的碰撞检测结果为基准，动态调整目标偏置概率，根据动态调整的目标偏置概率引导采样点生成，进行机器人运动路径规划。本发明方法可以提高在不同障碍物分布情况下目标偏置概率的合理性，并在提高规划速度的同时进行轨迹优化。

Description

一种机器人快速运动规划方法

技术领域

本发明涉及机器人运动规划领域，具体涉及一种机器人快速运动规划方法。

背景技术

针对机器人运动规划问题提出和使用的算法主要可以分为基于几何构造的空间搜索法、基于轨迹优化的运动规划算法及基于采样的运动规划算法等三类。在处理高维机器人运动规划问题时，基于采样的运动规划较为高效。其基本思想是确定一个能充分表示自由空间连通性的有限避障位形集合，用于解决含有障碍物环境中运动规划问题的路径图。随机采样规划中目前最常用的两种算法是先学习再查询的概率路线图(ProbabilisticRoadmap Method,PRM)和一边学习一边查询的快速扩展随机树(Rapidly-exploringRandom Tree,RRT)。刘华军等在移动机器人运动规划研究综述中对PRM和RRT进行比较，相比与PRM，RRT算法充分考虑了机器人客观存在的约束(如非完整约束、运动动力学约束等)，得到的轨迹也更为合理。Steven M.LaValle在Randomized Kinodynamic Planning[J].(The International Journal of Robotics Research,2001,20(5):378-400)一文中引入目标偏置思想，提出了基于目标偏置的RRT算法(GOAL_BIASED_RRT,GB-RRT)，通过向目标引导的方式提高了基本RRT算法的搜索效率。但是固定的目标偏置概率使得路径在障碍物稀疏的构型空间和障碍物稠密的构型空间中扩张时是同样的目标偏置概率，当障碍物在空间中不是均匀分布时，固定值的目标偏置概率并不能很好的处理快速性。同时，固定值的目标偏置概率也不能优化轨迹或路径。Lavalle S M,Kuffner J J.在Rapidly-exploringRandom Trees:Progress and prospects[C].(4th International Workshop on theAlgorithmic Foundations of Robotics,2001:293-308)一文提出了稀疏稳定型的RRT算法(STABLE-SPARSE-RRT,SST)，这是一种渐近近似最优的规划方法。该算法没有考虑目标偏置的作用，仍然面向所有构型空间采样，与实际中从初始路径尽可能的直达目标路径不符，同时该算法需要消耗大量的时间进行迭代才能得到最有轨迹。

发明内容

本发明的目的在于针对上述现有技术中的问题，提出一种机器人快速运动规划方法，提高在不同障碍物分布情况下目标偏置概率的合理性，并在提高规划速度的同时进行轨迹优化。

本发明是通过以下技术方案来实现：

一种机器人快速运动规划方法，以采样生成新路径点过程中的碰撞检测结果为基准，动态调整目标偏置概率，根据动态调整的目标偏置概率引导采样点生成，进行机器人运动路径规划。

优选的，包括：

步骤1，初始化机器人运动状态和障碍物运动状态；

步骤2，定义碰撞检测数组CollisionResult，用来记录当前状态前n次的碰撞检测结果；该数组有n个元素，每个元素是0或1；0代表碰撞检测结果为无碰撞，1代表碰撞检测结果为有碰撞；初始化CollisionResult矩阵为全零矩阵；

步骤3，根据碰撞检测数组CollisionResult，计算目标偏置概率p_goal；

步骤4，通过随机数产生概率p，若p<p_goal则取目标构型x_goal为采样点s_sample，否则随机采样；

步骤5，根据采样结果计算出笛卡尔空间中机器人各关节的位置及末端执行器的位置x_nearest；根据位置x_nearest得到障碍物碰撞检测状态；

步骤6，根据位置x_nearest通过蒙特卡洛进行节点扩展，结合正运动学得到机器人各关节的位置以及末端执行器的位置x_new，然后根据位置x_new和障碍物碰撞检测状态进行碰撞检测；

步骤7，若发生碰撞则更新碰撞检测数组并跳转至步骤8；

若未发生碰撞，则更新碰撞检测数组和机器人运动状态，结合附近最优算法和稀疏算法更新路径图，并跳转至步骤8；

步骤8，判断是否已到达目标或超过迭代次数，若否，则返回步骤3；若是，则输出路径图，运动规划结束。

进一步的，步骤3中，计算目标偏置概率p_goal，公式如下：

进一步的，步骤2中，n为10。

优选的，步骤5中，根据附近最优算法和正运动学计算出笛卡尔空间中机器人各关节的位置及末端执行器的位置x_nearest。

优选的，步骤7中，若发生碰撞，则碰撞检测数组更新如下：

CollsionResult＝[CollsionResult(2,n),1]。

优选的，步骤7中，若未发生碰撞，则碰撞检测数组更新如下：

CollsionResult＝[CollsionResult(2,n),0]。

优选的，步骤7中，结合附近最优算法和稀疏算法更新路径图，并跳转至步骤8，具体为：以位置x_new为中心，以稀疏半径δ_s为半径，在监视集S中寻找最近的监视状态s_new，x_peer是监视集S中代表监视状态s_new在状态空间中的路径点，计算x_new和x_peer的代价函数，分别为cost(x_new)和cost(x_peer)；若cost(x_new)≥cost(x_peer)，则进行步骤8；否则将x_nearest到x_new的路径添加至已有路径图，并更新活跃路径点集V_active和非活跃路径点集V_inactive，进行路径稀疏，再进行步骤8。

进一步的，步骤7中，若监视集S中不存在位置x_new附近的监视状态s_new，则将x_new添加到监视集S中，并视x_new为s_new。

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

本发明基于碰撞反馈的动态目标偏置概率的DP-SST规划算法，通过动态调整目标偏置概率，使路径在障碍物稀疏的环境中更加偏向于目标点扩展，在稠密障碍物的环境中更加偏向于全局扩展，并结合附近最优算法实现在提高规划速度的同时也能进行轨迹优化。本发明可以在障碍物分布不均的情况中快速实现运动规划，同时进行路径优化。另外也可以针对不同障碍物分布环境寻找一个合理的目标偏置概率，为其他固定目标偏置概率的运动规划方法提供参考。

附图说明

图1本发明运动规划方法(DP-SST)的整体流程图；

图2一般障碍物环境下DP-SST运动规划示意图；

图3一般障碍物环境下DP-SST运动规划过程中价值函数的变化；

图4一般障碍物环境下DP-SST运动规划过程中目标偏置概率p_goal的变化；

图5一般障碍物环境下其他运动规划算法运动规划示意图；(a)GB-RRT运动规划示意图；(b)SST运动规划示意图；

图6特殊障碍物环境下DP-SST运动规划示意图；

图7特殊障碍物环境下DP-SST运动规划过程中价值函数的变化；

图8特殊障碍物环境下DP-SST运动规划过程中目标偏置概率p_goal的变化；

图9特殊障碍物环境下其他运动规划算法运动规划示意图；(a)GB-RRT运动规划示意图；(b)SST运动规划示意图；

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

本发明提出了基于碰撞检测反馈的动态目标偏置概率的SST算法(DYNAMIC-PROBABILITY-SST,DP-SST)。该算法以采样生成新路径点过程中的碰撞检测结果为基准，动态调整目标偏置概率。DP-SST定义了一个表示碰撞检测结果动态变化的碰撞检测数组CollisionResult，来记录当前状态前n次的碰撞检测结果。以n＝10为例，该数组有10个元素，每个元素是0或1。0代表碰撞检测为无碰撞，1代表碰撞检测有碰撞。例如

CollisionResult＝[0,0,0,1,0,0,1,0,0,0]

该数组表示该状态下第4次、第7次的碰撞检测结果为发生了碰撞，其余未发生碰撞。因为过于靠前的碰撞检测结果对当前状态路径图的扩展影响不是很大，因此只对当前状态前10次的碰撞检测结果进行考虑。目标偏置概率计算规则如下

在后续扩展中，碰撞检测数组依次记录前十次的碰撞检测结果，更新数据。若碰撞检测数组CollisionResult的元素全为0，说明当前状态下，路径图所接触到的范围没有障碍物，此时目标偏置概率p_goal＝1,使得目标点变成采样点，加速路径图向目标点扩展。若碰撞检测数组CollisionResult的元素全为1，说明当前状态下，路径图所接触到的范围障碍物稠密，此时目标偏置概率p_goal＝0，驱使采样点在全局中进行采样，加速路径图向全局扩展。检测到碰撞的次数越多，目标偏置概率越低，反之目标偏置概率越高。通过设置该目标偏置概率，使路径在障碍物稀疏的环境中更加偏向于目标点扩展，在稠密障碍物的环境中更加偏向于全局扩展，并结合附近最优算法实现在提高规划速度的同时也能进行轨迹优化。

本发明运动规划方法整体流程图如图1所示，具体包括如下步骤：

步骤1，初始化机器人运动状态，包括基座的位置、姿态、关节角的初始构型、末端执行器的初始位置x₀和目标位置x_goal，给出算法所需参数包括查询半径δ_v、稀疏半径δ_s以及通过逆运动学得到关节空间的目标构型。同时对障碍物运动状态进行初始化。

步骤2，初始化CollisionResult矩阵为全零矩阵。

步骤3，计算目标偏置概率p_goal，公式如下：

步骤4，通过随机数产生概率p。若p<p_goal则取目标构型x_goal为采样点s_sample，反之则随机采样。

步骤5，根据采样结果，并根据附近最优算法和正运动学计算出笛卡尔空间中机器人各关节的位置及末端执行器的位置x_nearest。并根据位置x_nearest得到障碍物碰撞检测状态。即，在以采样点s_sample为中心，步骤1中给定的查询半径范围内通过附近最优算法(BestNear)求解代价最小质量最优的路径点x_nearest。

步骤6，根据位置x_nearest通过蒙特卡洛(MonteCarlo-Prop)进行节点扩展，结合正运动学得到机器人各关节的位置以及末端执行器的位置x_new，然后根据位置x_new和障碍物碰撞检测状态进行碰撞检测。

步骤7，若发生碰撞则碰撞检测数组更新如下

CollsionResult＝[CollsionResult(2,n),1]

更新完成之后判断是否到达目标或超过迭代次数，即进行步骤8。

若未发生碰撞检测，则

(1)碰撞检测数组更新如下

CollsionResult＝[CollsionResult(2,n),0]

(2)更新机器人运动状态，包括其基座、关节角、末端执行器的运动信息，并对监视集S进行更新。

(3)以位置x_new为中心，以稀疏半径δ_s为半径，在监视集S中寻找最近的监视状态s_new。若监视集S中不存在位置x_new附近的监视状态s_new，则将x_new添加到监视集S中，并视x_new为s_new。x_peer是监视集S中代表监视状态s_new在状态空间中的路径点。计算x_new和x_peer的代价函数，分别为cost(x_new)和cost(x_peer)，比较两节点路径质量。

若cost(x_new)≥cost(x_peer)，则进行步骤8，即判断是否到达目标或超过迭代次数，否则将x_nearest到x_new的路径添加至已有路径图，并更新活跃路径点集V_active和非活跃路径点集V_inactive，后进行步骤8。

步骤8，判断是否已到达目标或超过迭代次数，若否，则返回步骤3；若是，则输出路径，运动规划结束。

实施例

以平面质点在障碍物环境下的运动规划为例进行数值仿真，并与通用的p_goal为0.5的GB-RRT算法和SST算法进行比较，同时考虑到含有目标偏置的规划算法可能会陷入局部障碍物包裹的“目标陷阱”中，难以或不能自主跳出“目标陷阱”，在一般障碍物分布和存在“目标陷阱”的特殊障碍物分布两种仿真环境下进行仿真验证。

设置两种环境下路径点的评价指标均为

cost(x_i)＝k_localgcost₁(x_i)+k_goalgcost₂(x_i)

其中，

为该路径点已经走过的长度，x_j为已有路径中的节点，x_{j_parent}为其父节点，cost₂(x_i)＝||x_i-x_goal||为该路径点距离目标点的长度。||g||为两节点间欧氏距离，即

如果只设置cost₁(x_i)为评价指标，则路径总是寻找最小的点，出现往回走，“回溯”的现象。如果只设置cost₂(x_i)为评价指标，可能会选择距离目标点近，但已走过的路径更曲折的点，并不能衡量路径的质量。因此通过比例系数k_local∈[0,1]和k_goal∈[0,1]来调整二者的比例，权衡路径点已走过的路径和目标点对路径点的影响，用cost(x_i)来表征当前点对应的路径质量。

为比较三种运动规划算法的优劣，设评估算法规划效率与轨迹优化效果的计算方法为

其中n₁、n₂、n₃为三种运动规划算法的迭代次数，可反应运动规划的规划效率，J_i1数值越小表明算法规划效率相对越高。L₁、L₂、L₃为三种运动规划算法的优化指标结果，即规划得到的路径长度，可以反映轨迹优化效果，J_i2数值越小表明算法轨迹优化相对越好。为了总体衡量算法的规划效率与轨迹优化效果，设算法的总体评估指标计算方法为：

J_i越小表明该算法越能在提高规划效率的同时进行轨迹优化，效果更优，其值不大于1。

1.一般障碍物环境下的运动规划

在MATLAB仿真环境下，设置地图大小为[800,680]，初始点为[50,50]，目标点为[700,600]，二者之间直线距离为851，并在二者之间布置了障碍物。评价指标按实际长度计算，取k_local和k_goal均为1，仿真结果如图2、图3和图4所示。

仿真结果表明，规划得到的路径完全避开了障碍物，没有发生任何碰撞。同时为了确保路径最短，尽量沿直线行走，长度为1281。且随着运动规划的进行，评价函数cost₂(x_i)整体呈减少趋势，也就是说路径一直在朝着目标点扩展。cost(x_i)一直在增加，符合总路径增加的实际情况。同时由图4可得，在该障碍物环境下，若采用固定目标偏置概率，则推荐p_goal为0.3～0.4。

同样障碍物环境下，GB-RRT和SST仿真结果如图5所示。表1为一般障碍物环境下三种规划算法仿真结果，由表1可得，与GB-RRT算法相比，虽然DP-SST算法迭代次数多，速度慢，但其规划得到的路径更短、更优。与SST相比，虽然DP-SST算法所得路径长度较长，但其迭代次数远远小于SST算法的迭代次数。对迭代次数及路径长度进行综合考虑，DP-SST算法更优，与总体评估指标显示结果一致。也从侧面说明了DP-SST算法不仅提高了规划效率，也进行了轨迹优化，解决了基于采样的运动规划算法中规划效率与轨迹优化相冲突的问题。

表1一般障碍物环境下DP-SST算法与GB-RRT、SST算法的结果比较

2.特殊障碍物环境下的运动规划

特殊障碍物环境是指“目标陷阱”型障碍物环境。在MATLAB仿真环境下，设置地图大小为[800,680]，初始点[50,50]，目标点为[550,600]，二者之间直线距离为743，并在二者之间布置了“目标陷阱”型障碍物。评价指标按实际长度计算，取k_local和k_goal均为1，仿真结果如图6、图7和图8所示。

仿真结果表明，图中有一条路径进入“目标陷阱”，并径直朝着目标前进，但在碰到障碍物后迅速开始全局扩展，最后到达目标点。最终路径长度为1167。且随着运动规划的进行，评价函数cost₂(x_i)整体呈减少趋势，也就是说路径一直在朝着目标点扩展。cost(x_i)一直在增加，符合实际情况。由图8可得，在该障碍物环境下，若采用固定目标偏置概率，则推荐p_goal为0.3～0.4。

同样障碍物环境下，GB-RRT和SST仿真结果如图9所示。表2为特殊障碍物环境下三种规划算法仿真结果，由表可得，在“目标陷阱”型障碍物分布的环境下，DP-SST算法与GB-RRT算法相比，无论是迭代次数还是路径长度，DP-SST算法均优于GB-RRT算法。虽然SST算法所得路径长度相比DP-SST较短，但其迭代次数远远大于DP-SST算法的迭代次数。对迭代次数及路径长度进行综合考虑，DP-SST算法更优，与总体评估指标显示结果一致。

表2特殊障碍物环境下DP-SST算法与GB-RRT、SST算法的结果比较

Claims

1.一种机器人快速运动规划方法，其特征在于，以采样生成新路径点过程中的碰撞检测结果为基准，动态调整目标偏置概率，根据动态调整的目标偏置概率引导采样点生成，以进行机器人运动路径规划。

2.根据权利要求1所述的机器人快速运动规划方法，其特征在于，包括：

步骤1，初始化机器人运动状态和障碍物运动状态；

步骤7，若发生碰撞则更新碰撞检测数组并跳转至步骤8；

3.根据权利要求2所述的机器人快速运动规划方法，其特征在于，步骤3中，计算目标偏置概率p_goal，公式如下：

4.根据权利要求2所述的机器人快速运动规划方法，其特征在于，步骤2中，n为10。

5.根据权利要求1所述的机器人快速运动规划方法，其特征在于，步骤5中，根据附近最优算法和正运动学计算出笛卡尔空间中机器人各关节的位置及末端执行器的位置x_nearest。

6.根据权利要求1所述的机器人快速运动规划方法，其特征在于，步骤7中，若发生碰撞，则碰撞检测数组更新如下：

CollsionResult＝[CollsionResult(2,n),1]。

7.根据权利要求1所述的机器人快速运动规划方法，其特征在于，步骤7中，若未发生碰撞，则碰撞检测数组更新如下：

CollsionResult＝[CollsionResult(2,n),0]。

8.根据权利要求1所述的机器人快速运动规划方法，其特征在于，步骤7中，结合附近最优算法和稀疏算法更新路径图，并跳转至步骤8，具体为：以位置x_new为中心，以稀疏半径δ_s为半径，在监视集S中寻找最近的监视状态s_new，x_peer是监视集S中代表监视状态s_new在状态空间中的路径点，计算x_new和x_peer的代价函数，分别为cost(x_new)和cost(x_peer)；若cost(x_new)≥cost(x_peer)，则进行步骤8；否则将x_nearest到x_new的路径添加至已有路径图，并更新活跃路径点集V_active和非活跃路径点集V_inactive，进行路径稀疏，再进行步骤8。

9.根据权利要求8所述的机器人快速运动规划方法，其特征在于，步骤7中，若监视集S中不存在位置x_new附近的监视状态s_new，则将x_new添加到监视集S中，并视x_new为s_new。