CN114161416B - 基于势函数的机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种基于势函数的机器人路径规划方法，其特征在于:首先采用基于势函数的随机采样方式；其次基于概率，采用与随机采样点欧式距离最近和代价函数值最小的方式选择最近邻节点，并在新节点拓展上采用两次拓展的方法加快算法的搜索效率，第一次拓展采用目标偏向的拓展策略、第二次拓展采用矩形区域随机采样的策略；然后对新节点做重新选择父节点操作和对路径做重新布线操作，得到一条从起始点到目标点的无碰撞的路径，利用删除冗余节点和最大曲率约束对生成的路径进行去除冗余节点和最大曲率约束操作，提高路径的平滑度。本发明具有能够有效地减少了搜索时间和路径的长度，同时平均采样节点数也有了明显的减少的特点。

Description

基于势函数的机器人路径规划方法

技术领域

本发明涉及机器人运动规划领域，具体涉及一种基于势函数的机器人路径规划方法。

背景技术

机械臂作为工业机器人的运动稳定性是机器人研究的关键工作，因此找到一条高效且无碰撞的路径对机器人的运动就显得尤为重要了。现有技术中，运动规划RRT算法在状态空间中随机采样，不需要对状态空间进行预处理，其随机性就证明了RRT算法本身具有较强的搜索能力，且算法是概率完备的，许多学者对RRT算法进行了深入的研究和应用。Jr等人提出了建立双向快速探索随机树(RRT-connect)算法，两树分别根植于开始状态和目标状态，两树分别向周围空间进行探索，加快了搜索效率。KaramanS等人引入了新的算法RRT*算法，该算法是渐近最优的，算法引入重新选择父节点和重新布线操作，使解的代价收敛至最优，降低了搜索的成本，且提高了算法的效率。KimD等人提出了一种基于RRT*的采样启发式算法，局部地利用不同同伦类中的解使收敛速度更快，进而达到优化算法的目的。NasirJ等人提出了RRT*-Smart，采用了RRT*的路径优化和智能采样方法，达到了加快收敛速度的目的。Adiyatov O等人提出了RRT*FN算法，当树中的节点数增加到预定设置时，则随机删除一个低成本的弱节点。Gammell J D等人提出了新算法Informed-rrt*，在椭球体中进行采样，该方法提高了收敛速度和路径的质量。Qureshi A H等人在RRT*的基础上将人工势场法的思想加入到RRT*算法中得到P-RRT*算法，该算法使收敛速度大大提高且减少了内存的占用。Jeong I B等人提出了Q-RRT*算法，其在RRT*的基础上又考虑了新节点的父节点的父节点，扩大了父顶点的范围，还将此方法用到了重新布线的操作中，进一步提高了算法的搜索效率和路径的质量。Li Y等人提出了一种将P_RRT*和Q_RRT*算法相结合的PQ-RRT*算法，充分结合两个算法的优势，进一步提高了算法的收敛速率和效率。总之，目前面对复杂的障碍物环境改进的RRT算法仍然存在效率低、收敛至最优解的速度慢、占用计算内存较大等问题；对于机器人的运动路径，存在较多转折点，未对规划出来的路径作出很好的平滑处理，严重影响了机器人的使用寿命。

发明内容

本发明的目的在于克服上述缺点而提出了一种能够有效地减少了搜索时间和路径的长度，同时平均采样节点数也有了明显的减少的基于势函数的机器人路径规划方法。

本发明的一种基于势函数的机器人路径规划方法，包括以下步骤：

步骤1：初始化随机树中的各个参数，包括插入起始点、目标点、障碍物、步长、目标偏向步长、障碍物；

步骤2：利用P_RRT*算法中势函数随机采样点选择方法RGD(q_rand)进行采样，获得随机采样点q_prand；

步骤3：获取服从均匀分布的随机数概率rand，进行最近邻节点q_nearest选取：如果随机数概率rand小于最近邻节点概率p_nearest＝0.5，则最近邻节点是与随机采样点q_prand欧式距离最近的树中的节点；如果随机概率rand大于最近邻节点概率p_nearest＝0.5，则采用公式(1)计算代价函数C(q)，选择树中代价函数C(q)最小的点作为最近邻节点；

所述公式(1)为：

其中||q_end-q||为目标点q_end与当前节点q之间的欧式距离，w_d为距离比例系数，w_c为混合比例系数，k表示当前节点附近圆内障碍物的个数，最近邻节点则选择树中最小的代价函数C(q)；

步骤4：利用得到的最近邻节点q_nearest进行拓展，第一步拓展利用目标偏向策略，通过公式(2)得到新节点q_1new，并判断新节点q_1new是否与障碍物碰撞，如果碰撞了则返回步骤2；否则，执行步骤5；

所述公式(2)为：

其中kp表示引力系数，ρ为随机树的生长步长，q_prand表示随机采样点，q_nearest表示最近邻节点，||q_end-q_nearest||表示目标点q_end到最近邻节点q_nearest的欧氏距离，||q_prand-q_nearest||表示随机采样点q_prand到最近邻节点q_nearest的欧式距离；

步骤5：在拓展的新节点q_1new上，通过公式(3)进行第二步拓展，即采用矩形区域内随机选取点作为第二次拓展的得到新节点q_2new，并判断新节点q_2new与障碍物是否碰撞或者新节点q_2new与新节点q_1new之间的连线与障碍物是否碰撞，如果碰撞则执行步骤5重新进行新节点q_2new的随机采样；否则，执行步骤6；

所述公式(3)为：

[q_2new(x)，q_2new(y)，q_2new(z)]＝[q_lnew(x)，q_1new(y)，q_1new(z)]+r*[rand(q_1new(x)→q_prand(x))，rand(q_1new(y)→q_prand(y))，rand(q_1new(z)→q_prand(z))]

其中q_2new(*)、q_1new(*)和q_prand(*)分别表示q_2new、q_1new、q_prand的坐标值；r表示拓展比例系数，根据实验分析取r＝0.1；rand(*)为选取的随机数值；并判断q_2new与q_1new之间的连线是否与障碍物碰撞，如果碰撞则放弃q_2new的拓展，重新进行q_2new的采样，若没有碰撞则将q_2new加入到随机树中，并令其父节点赋予q_1new；

步骤6：以新节点q_2new为中心、半径r₁作邻近圆R₁，为新节点q_2new在邻近圆R₁内进行重新选择父节点的操作，找到欧氏距离代价值最小，且无碰撞的新节点q_2new的父节点；

步骤7：以新节点q_2new为中心、以半径r₂作邻近圆R₂，以新节点q_2new作为父节点在邻近圆R₂里进行重新布线操作，找到邻近圆R₂里的点以新节点q_2new作为其父节点的欧式距离代价值小于其本身欧氏距离代价值且无碰撞的节点做重新布线操作，找到路径最优；

步骤8：检查新节点q_2new与目标点之间的欧式距离是否小于一倍的步长ρ，如果是，则连接新节点q_2new和目标点，得到一条从起始点到目标点的无碰撞的路径，算法结束；否则，返回执行步骤2。

上述的基于势函数的机器人路径规划方法，其中：所述步骤2中利用P_RRT*算法中势函数随机采样点选择方法RGD(q_rand)，其具体步骤为：在选择随机节点q_rand的操作中引入了人工势场法的思想，目标点产生吸引力

定义一个潜在的随机样本点q_prand，q_prand是q_rand沿着吸引势场梯度减小方向的点，即偏向于目标方向的点，以一个小的步长λ向下坡进行运动得到的，将q_prand代替q_rand作为随机采样点，当靠近目标点的时候引力的势场梯度会随之减小拓展过程中实时判断与最近障碍物之间的欧式距离，如果小于障碍物空间距离

则立即终止程序，返回q_prand，否则进行下一次的随机样本的有限次数k的拓展，其中参数λ、k、d_obs的取值需要调整。

上述的基于势函数的机器人路径规划方法，其中：所述参数λ、k、d_obs的取值为：λ＝0.02，k＝80，d*_obs＝0.1。

上述的基于势函数的机器人路径规划方法，其中：所述步骤6中为新节点q_2new在邻近圆内R₁进行重新选择父节点的操作，其具体过程为：首先计算出以最近邻节点q_nearest为父节点的代价值Cost(q_2new)，在状态空间Q选择潜在的父节点与新节点q_2new进行连接，然后判断以此潜在的父节点作为新节点q_2new的父节点的欧式距离代价值Cost(Q(i))是否小于代价值Cost(q_2new)，如果大于代价值Cost(q_2new)则放弃此潜在的父节点，进行下一个潜在父节点的计算，如果小于Cost(q_2new)，且潜在的父节点与新节点q_2new之间的连线未与障碍物碰撞，则进行新节点q_2new的父节点的重新选择并连线，如果与障碍物碰撞了则新节点q_2new的父节点不变仍是最近邻节点q_nearest，依次比较，直到所有的潜在父节点完成比较。

上述的基于势函数的机器人路径规划方法，其中：所述步骤7中以新节点q_2new作为父节点在邻近圆R₂里进行重新布线操作，其具体过程为：新节点q_2new作为邻近圆R₂内所有潜在子节点的父节点，如果以新节点q_2new为潜在子节点的父节点的欧氏距离代价值Cost(q_{potential_child})比潜在子节点原始的欧式距离代价值小，且未与障碍物碰撞，则放弃潜在子节点原始的父节点将新节点q_2new作为其父节点进行重新布线，否则保持不变。

上述的基于势函数的机器人路径规划方法，其中：所述步骤8中从起始点到目标点的无碰撞的路径，进行删除冗余节点的操作，具体操作过程：从起始点开始，放弃第二个路径点，连接第三个路径点，如果此路径未与障碍物碰撞则证明第二个节点是冗余节点则删除第二个节点，再将起始节点连接第四个节点判断是否碰撞，如果没有碰撞则删除第三个节点，按照此方法进行判断，如果碰撞了则保留该节点，并以该节点作为起始点进行上述判断，直至到达目标点，即可得到一条从起始点到目标点的去除冗余节点的路径。

上述的基于势函数的机器人路径规划方法，其中：所述从起始点到目标点的去除冗余节点的路径，采用一种最大曲率约束的方法进行路径的平滑处理，即在得到的去除冗余点的路径中，从起始点开始依次计算路径之间的夹角的余弦值，如果大于规定的最大转角的余弦值0.707，则在此夹角的顶点为标准在其左右两侧分别加上原始未去除冗余节点的此顶点的子节点和父节点，即左侧加入父节点，右侧加入子节点。

本发明与现有技术的相比，具有明显的有益效果，由以上方案可知，所述首先利用P_RRT*算法中基于势函数的思想进行随机采样点的选取；然后利用距离随机采样点欧式距离最近或代价函数最小进行最近邻节点的选择，引入最近邻节点的选择策略，以一定的概率采用传统的欧氏距离最近和一定的概率选择代价函数C(q)最小的方法交替进行最近邻节点的选择，使得算法更有效地避开了障碍物；利用目标偏向的思想进行第一个新节点的扩展，再使用矩形区域随机采样的方法进行第二个新节点的扩展，引入新节点的两次拓展策略，在传统的新节点采用目标偏向策略拓展的基础上，进行新节点的第二次拓展，更有效地提高了算法的效率；然后利用RRT*算法中的重新选择父节点和重新布线的方法对新节点进行处理，得到一条最优的路径；最后对于生成的路径存在较多转折点且路径的曲率较大的现象分别采用去除冗余节点操作和最大曲率约束操作，对重新选择父节点和重新布线操作后的路径进行去除冗余节点操作和最大曲率约束操作，去除路径中冗余的节点，最大曲率约束在曲率较大的转角处通过向转折点左右两侧分别加入原始路径上的点使得路径更加平滑了；对去除冗余点和最大曲率约束操作后的路径进行贝赛尔曲线拟合操作，得到了一条更加符合机器人运动的光滑路径。总之，本发明具有更高的搜索效率和更高的收敛性，有效地减少了搜索时间和路径的长度，同时平均采样节点数也有了明显的减少，降低了电脑的内存消耗。

以下通过具体实施方式，进一步说明本发明的有益效果。

附图说明

图1为本发明的流程示意图；

图2为本发明的新节点第一步拓展思维图；

图3为本发明的新节点第二步拓展思维图；

图4为本发明的选择父节点操作的思维图；

图5为本发明的布线操作思维图；

图6为本发明与不同算法在地图1中生成路径对比图。

具体实施方式

以下结合附图及较佳实施例，对依据本发明提出的一种基于势函数的机器人路径规划方法的具体实施方式、特征及其功效，详细说明如后。

参见图1，本发明的一种基于势函数的机器人路径规划方法，该方法包括以下步骤：

步骤1：初始化随机树中的各个参数，包括插入起始点、目标点、障碍物、步长、目标偏向步长、障碍物；

步骤2：利用P_RRT*算法中势函数随机采样点选择方法RGD(q_rand)进行采样，获得随机采样点q_prand；

所述利用P_RRT*算法中势函数随机采样点选择方法RGD(q_rand)，其具体步骤为：在选择随机节点q_rand的操作中引入了人工势场法的思想，目标点产生吸引力

定义一个潜在的随机样本点q_prand，q_prand是q_rand沿着吸引势场梯度减小方向的点，即偏向于目标方向的点，以一个小的步长λ向下坡进行运动得到的，将q_prand代替q_rand作为随机采样点，当靠近目标点的时候引力的势场梯度会随之减小拓展过程中实时判断与最近障碍物之间的欧式距离，如果小于障碍物空间距离

则立即终止程序，返回q_prand，否则进行下一次的随机样本的有限次数k的拓展，其中参数λ、k、d_obs的取值需要调整，本发明忽略了参数的调整，取λ＝0.02，k＝80，d*_obs＝0.1。

步骤3：获取服从均匀分布的随机数概率rand，进行最近邻节点q_nearest选取：如果随机数概率rand小于最近邻节点概率p_nearest＝0.5，则最近邻节点是与随机采样点q_prand欧式距离最近的树中的节点；如果随机概率rand大于最近邻节点概率p_nearest＝0.5，则采用公式(1)计算代价函数C(q)，选择树中代价函数C(q)最小的点作为最近邻节点；

所述公式(1)为：

其中||q_end-q||为目标点q_end与当前节点q之间的欧式距离，w_d为距离比例系数，w_c为混合比例系数，k表示当前节点附近圆内障碍物的个数，最近邻节点则选择树中最小的代价函数C(q)；

步骤4：利用得到的最近邻节点q_nearest进行拓展，第一步拓展利用目标偏向策略(如图2)，通过公式(2)得到新节点q_1new，并判断新节点q_1new是否与障碍物碰撞，如果碰撞了则返回步骤2；否则，执行步骤5；

所述公式(2)为：

其中kp表示引力系数，ρ为随机树的生长步长，q_prand表示随机采样点，q_nearest表示最近邻节点，||q_end-q_nearest||表示目标点q_end到最近邻节点q_nearest的欧氏距离，||q_prand-q_nearest||表示随机采样点q_prand到最近邻节点q_nearest的欧式距离；

步骤5：在拓展的新节点q_1new上，通过公式(3)进行第二步拓展(如图3)，即采用矩形区域内随机选取点作为第二次拓展的得到新节点q_2new，并判断新节点q_2new与障碍物是否碰撞或者新节点q_2new与新节点q_1new之间的连线与障碍物是否碰撞，如果碰撞则执行步骤5重新进行新节点q_2new的随机采样；否则，执行步骤6；

所述公式(3)为：

[q_2new(x)，q_2new(y)，q_2new(z)]＝[q_lnew(x)，q_lnew(y)，q_lnew(z)]+r*[rand(q_1new(x)→q_prand(x))，rand(q_1new(y)→q_prand(y))，rand(q_1new(z)→q_prand(z))]

其中q_2new(*)、q_1new(*)和q_prand(*)分别表示q_2new、q_1new、q_prand的坐标值；r表示拓展比例系数，根据实验分析取r＝0.1；rand(*)为选取的随机数值；并判断q_2new与q_1new之间的连线是否与障碍物碰撞，如果碰撞则放弃q_2new的拓展，重新进行q_2new的采样，若没有碰撞则将q_2new加入到随机树中，并令其父节点赋予q_1new；

步骤6：以新节点q_2new为中心、半径r₁作邻近圆R₁，为新节点q_2new在邻近圆R₁内进行重新选择父节点的操作(如图4)，找到欧氏距离代价值最小，且无碰撞的新节点q_2new的父节点；

所述为新节点q_2new在邻近圆内R₁进行重新选择父节点的操作，其具体过程为：首先计算出以最近邻节点q_nearest为父节点的代价值Cost(q_2new)，在状态空间Q选择潜在的父节点与新节点q_2new进行连接，然后判断以此潜在的父节点作为新节点q_2new的父节点的欧式距离代价值Cost(Q(i))是否小于代价值Cost(q_2new)，如果大于代价值Cost(q_2new)则放弃此潜在的父节点，进行下一个潜在父节点的计算，如果小于Cost(q_2new)，且潜在的父节点与新节点q_2new之间的连线未与障碍物碰撞，则进行新节点q_2new的父节点的重新选择并连线，如果与障碍物碰撞了则新节点q_2new的父节点不变仍是最近邻节点q_nearest，依次比较，直到所有的潜在父节点完成比较。

步骤7：以新节点q_2new为中心、以半径r₂作邻近圆R₂，以新节点q_2new作为父节点在邻近圆R₂里进行重新布线操作(如图5)，找到邻近圆R₂里的点以新节点q_2new作为其父节点的欧式距离代价值小于其本身欧氏距离代价值且无碰撞的节点做重新布线操作，找到路径最优；

所述以新节点q_2new作为父节点在邻近圆R₂里进行重新布线操作，其具体过程为：新节点q_2new作为邻近圆R₂内所有潜在子节点的父节点，如果以新节点q_2new为潜在子节点的父节点的欧氏距离代价值Cost(q_{potential_child})比潜在子节点原始的欧式距离代价值小，且未与障碍物碰撞，则放弃潜在子节点原始的父节点将新节点q_2new作为其父节点进行重新布线，否则保持不变；

步骤8：检查新节点q_2new与目标点之间的欧式距离是否小于一倍的步长ρ，如果是，则连接新节点q_2new和目标点，得到一条从起始点到目标点的无碰撞的路径，算法结束；否则，返回执行步骤2。

所述从起始点到目标点的无碰撞的路径，进行删除冗余节点的操作，具体操作过程：从起始点开始，放弃第二个路径点，连接第三个路径点，如果此路径未与障碍物碰撞则证明第二个节点是冗余节点则删除第二个节点，再将起始节点连接第四个节点判断是否碰撞，如果没有碰撞则删除第三个节点，按照此方法进行判断，如果碰撞了则保留该节点，并以该节点作为起始点进行上述判断，直至到达目标点，即可得到一条从起始点到目标点的去除冗余节点的路径。

所述从起始点到目标点的去除冗余节点的路径，采用一种最大曲率约束的方法进行路径的平滑处理，即在得到的去除冗余点的路径中，从起始点开始依次计算路径之间的夹角的余弦值，如果大于0.707，则在此夹角的顶点为标准在其左右两侧分别加上原始未去除冗余节点的此顶点的子节点和父节点，即左侧加入父节点，右侧加入子节点。

其中：

1、路径规划问题的定义

路径规划需要解决的三个问题，令

表示所描述问题的状态空间，其中R为设定的集合，r(i)_i∈n表示n到R的映射，即i∈n映射为r(i)∈R，

表示障碍空间，Q_free＝Q\Q_obs表示无障碍空间。Q_init和Q_end分别是起始状态和终止状态，他们都在无障碍空间中，设置障碍物空间距离

即将障碍物模型简化为球体，以球体的半径向外扩展的距离，连续函数α：[0，1]→Q_free为可行的无碰撞路径。

路径规划的问题在于找到一条无碰撞的路径，即α：[0，1]→Q_free，从起始点α(0)∈Q_init到达终止点α(1)∈Q_end和路径上的点α(τ)∈Q_free，对所有的τ∈[0，1]，则被称为可行路径。

问题1：给定三元组{Q_init，Q_obs，Q_ena}，如果找到一条路径则证明存在可行路径，如果找不到路径则证明不存在。

问题2：在所有的路径集合∑中，∑_feasible表示所有的可行路径，C(·)表示以欧式距离作为衡量的代价函数，则给定的三元组{Q_init，Q_obs，Q_end}，若有C(α)＝min{c(α)：α∈∑_feasible}，则存在最优路径，如果没有则最优路径规划失败。

问题3：t∈T，表示算法找到一条无碰撞路径所需的时间，则要求在最短的时间t∈T内找到最优路径解。

2、基本的P_RRT＊算法

经典RRT算法是一种多维空间中有效率的规划方法。以一个初始节点作为根节点(q_init)，然后在状态空间里随机选择一个随机采样点(q_rand)，根据Get_Nearest函数从随机树中挑选一个离q_rand欧氏距离最近的一个节点(q_nearest)。然后从q_nearest以一个步长(ρ)的距离向q_rand扩展，生成新节点q_new，并判断q_new和障碍物是否发生碰撞，如果发生碰撞则删除q_new，并重新进行随机点的采样，如果没有碰撞则将q_new加入到随机树中，并将q_new的父节点赋予给q_nearest。进行下一次随机采样，重复上述步骤，当q_new与目标点q_end之间的距离小于步长ρ时，连接q_new和目标点则得到一条从起始点到终止点的一条无碰撞路径。

RRT*算法在经典RRT算法的基础上进行了改进，主要针对最近邻区域内的父节点的重新选择(Reselect_Parent_Node)和最近邻区域内的节点的重新布线(Rewire_Node)进行了改进。RRT算法中将q_new的父节点赋予给q_nearest，而RRT*算法对新节点的父节点的选择做了改进，具体步骤是以q_new为中心，以r₁为半径的圆内，找到树中q_new的相邻的所有潜在父节点的集合(Q(potential_parent))，找到在状态空间Q(potential_parent)中是否有节点比q_nearest更适合做q_new的父节点，即以q_new到q_init的欧氏距离为代价函数进行评估，该方法首先计算出以q_nearest为父节点的Cost(q_new)，在状态空间Q(potential_parent)选择潜在的父节点与新节点进行连接，然后判断以此潜在的父节点作为新节点的父节点的欧式距离代价值Cost(Q(i))是否小于Cost(q_new)，如果大于Cost(q_new)则放弃此潜在的父节点，进行下一个潜在父节点的计算，如果小于Cost(q_new)，且潜在的父节点与新节点之间的连线未与障碍物碰撞，则进行q_new的父节点的重新选择并连线，如果与障碍物碰撞了则新节点的父节点不变仍是q_nearest，依次比较，直到所有的潜在父节点完成比较，如图4所示。RRT*算法还采用了最近邻区域内节点的重新布线，其思想是将q_new作为以r₂为半径的邻近圆内所有潜在子节点Q(potential_child)的父节点，如果以q_new为潜在子节点的父节点的欧氏距离代价值Cost(q_{potential_child})比潜在子节点原始的欧式距离代价值小，且未与障碍物碰撞，则放弃潜在子节点原始的父节点将新节点作为其父节点进行重新布线，否则保持不变，如图5所示。欧式距离代价值为此点到根节点的路径的欧式距离和。

P_RRT*算法是在RRT*的基础上采用势函数对随机采样点q_rand的选择进行了改进，定义为势函数随机采样点选择方法RGD(q_rand)。RGD(q_rand)的具体步骤是在选择随机节点q_rand的操作中引入了人工势场法的思想，APG表示吸引力梯度，目标点产生吸引力

定义一个潜在的随机样本点q_prand，q_prand是q_rand沿着吸引势场梯度减小方向的点，即偏向于目标方向的点，以一个小的步长λ向下坡进行运动得到的。将q_prand代替q_rand作为随机采样点，当靠近目标点的时候引力的势场梯度会随之减小。拓展过程中实时判断与最近障碍物之间的欧式距离，如果小于障碍物空间距离

则立即终止程序，返回q_prand，否则进行下一次的随机样本的有限次数k的拓展，其中参数λ和k和d_obs的取值需要调整，本发明忽略了参数的调整，取λ＝0.02，k＝80，d*_obs＝0.1。

3、改进P_RRT＊算法

基本的P_RRT*算法对状态空间中随机节点的选取进行了改进，使其更偏向于目标点的位置，减少了树中无效节点的的生成概率，同时节省了计算空间，减少了内存的使用。但是其成功搜索到路径的效率和搜索的收敛率还是很低，为了提高其搜索效率，达到更高的收敛率，本发明对P_RRT*算法中最近邻节点的选择和新节点的拓展进行了进一步的改进。

3.1最近邻节点的选择

本发明对P_RRT*算法中最近邻节点的选择做了改进

(Nearest_Neighbor(q_prand，q_end))，传统的最近邻节点的选择是选择与随机采样点欧氏距离最近的节点作为最近邻节点q_nearest，本发明在树中寻找代价函数C(q)最小的值作为最近邻节点q_nearest，C(q)定义为：

其中||q_end-q||为目标点q_end与当前节点q之间的欧式距离，w_d为距离比例系数，w_c为混合比例系数，k表示当前节点附近圆内障碍物的个数，为了提高避障能力此圆的半径选RRT*算法中两倍的r₁作为半径，最近邻节点则选择树中最小的C(q)。

令rand为区间[0-1]之间服从均匀分布的随机数，为了提高算法的避障能力，根据实验分析取p_nearest＝0.5，p_nearest表示选择树中与随机采样点欧式距离最小的节点作为最近邻节点的概率，即如果rand＜p_nearest，则选择传统的与随机采样点q_prand欧式距离最小的节点作为最近邻节点q_nearest，如果rand＞p_nearest，则选择树中代价函数C(q)最小的点作为最近邻节点q_nearest。

3.2新节点的拓展

传统的P_RRT*算法首先基于势函数进行随机点的采样，然后找到离随机采样点欧式距离最近的节点q_nearest，并沿着随机点的方向进行拓展得到q_new，传统的算法都是只进行一次新节点的拓展，效率不高，本发明对P_RRT*算法中的新节点的拓展进行了改进，采用将新节点拓展两次的策略，加快收敛速度，提高了算法的效率，具体步骤是：

第一步的新节点(q_lnew)拓展采用目标偏向的拓展方法，即在拓展的步长上引入人工势场法中的引力思想，使其朝向目标点生长，如图2所示，q_lnew定义为：

其中k_p表示引力系数，ρ为随机树的生长步长，q_prand表示随机采样点，q_nearest表示最近邻节点，||q_end-q_nearest||表示目标点q_end到最近邻节点q_nearest的欧氏距离，||q_prand-q_nearest||表示随机采样点q_prand到最近邻节点q_nearest的欧式距离。

如图3所示，第二步的新节点(q_2new)拓展采用q_1new到q_prand矩形区域随机采样的策略进行拓展(New_Node_2(q_1new，q_prand))，由P_RRT*算法中利用人工势场法对随机采样点的操作可知，由q_1new到q_prand构成一个矩形区域，即以第一次拓展的新节点q_1new到q_rand的最后一个拓展点q_prand所构成的的矩形区域内随机选取点作为第二次拓展的新节点q_2new。令_rand(*)为随机选取*区间中的随机数值，则该新节点q_2new可以表示为：

式中q_2new(*)、q_lnew(*)和q_prand(*)分别表示q_2new、q_lnew、q_prand的坐标值，r表示拓展比例系数，根据实验分析取r＝0.1。并判断q_2new与q_lnew之间的连线是否与障碍物碰撞，如果碰撞则放弃q_2new的拓展，重新进行q_2new的采样，若没有碰撞则将q_2new加入到随机树中，并令其父节点赋予q_lnew。

4、冗余节点的删除和最大曲率约束

4.1冗余节点删除操作

由于算法进行随机拓展，则势必会造成路径的曲折，包含了许多不必要的转折点，对于机器人来说，多余的转折点会造成不必要的能量损耗，而且会造成机器人的磨损，降低机器人的使用寿命。因此针对多余的转折点本发明采用去除冗余点操作，删除冗余节点的操作是对随机树生成的从起始点到终止点的无碰撞路径进行的，具体操作是从起始点开始，连接后续路径点，放弃第二个路径点，连接第三个路径点，如果此路径未与障碍物碰撞则证明第二个节点是冗余节点则删除第二个节点，再将起始节点连接第四个节点判断是否碰撞，如果没有碰撞则删除第三个节点，按照此方法进行判断，如果碰撞了则保留该节点，并以该节点作为起始点进行上述判断，直至到达目标点。由此可得到一条从起始点到终止点的去除冗余节点的路径。

4.2最大曲率约束

在上述提到的删除冗余节点操作中，得到了一条从起始点到终止点的一条去除冗余节点后的路径。得到的路径可能存在转角过大的情况，导致机器人在运行的过程中产生冲击损伤，严重影响机器人的使用寿命，不适合机器人在实际中的应用。由此采用一种最大曲率约束的方法进行路径的平滑处理，根据数据分析规定最大的转角的余弦值为0.707，在得到的去除冗余点的路径中从起始点开始依次计算路径之间的夹角的余弦值，如果大于0.707，则在此夹角的顶点为标准在其左右两侧分别加上原始未去除冗余节点的此顶点的子节点和父节点，即左侧加入父节点，右侧加入子节点。此步骤为下文的贝赛尔曲线拟合做准备。

5、贝赛尔曲线

经过冗余节点的删除和最大曲率约束操作的路径已经是一条较为平滑的路径，但在实际应用中，还需尽量减少转折点的冲击损伤，因此本发明采用了贝赛尔曲线对路径做了最后的平滑处理，使得路径更加平滑，更适合机器人在实际场景中的应用。采用n阶贝塞尔曲线，包含n+1个节点，公式C(u)如下所示：

式中p_i为n+1个空间的点，参数为u的权重系数B_n，i(u)代表伯恩斯坦基函数，该式的计算方式如下：

生成的最终曲线由n+1个节点组成，这些节点被称作控制点，当u＝0和u＝1时，分别位于起点和终点。

实验与分析

对本发明的改进的P_RRT*算法进行了实验验证，将改进的P_RRT*算法与现有的RRT、RRT*和P_RRT*算法在相同的三维环境下进行了比较，对改进算法的优越性、有效性和可靠性进行了验证。本实验在实验室惠普电脑Intel(R)Core(TM)i5-6500 CPU@3.20GHz3.19GHz 4GB内存的Win10环境下进行的python和ROS仿真实验，设置初始节点坐标为[0，0，0]，终止节点坐标为[8，10，10]，随机拓展步长ρ＝1.0，λ＝0.02，k＝80，d*_obs＝0.1，邻近圆半径r等参数都相同，其中用于重新选择父节点操作的邻近圆半径r₁＝2，用于重新布线操作的邻近圆半径r₂＝1，然后规划出一条从起始点到终止点的高效且无碰撞路径。同时本发明采用尤傲公司的UR5机器人进行了仿真实验，用于仿真实验验证。其中搜索时间大于100s表示搜索未成功，反之则成功，本发明主要在平均搜索时间、平均采样节点数、平均路径长度、搜索成功率等指标与RRT、RRT*和P_RRT*算法进行对比实验分析。

python实验分析

实验1相同障碍物个数下的算法对比实验

为了验证本发明算法的优越性，在障碍物个数相同且随机分布的情况下与RRT、RRT*、P_RRT*算法进行比较，每组实验进行200次，由于算法的随机性，取平均值作为比较，如表1所示。

根据表1数据所示，经典RRT算法平均搜索时间、平均采样节点数相比较其他三种算法都很大，路径的平均长度也最长，搜索成功率为70％；RRT*算法搜索时间次之，相比较RRT算法其平均搜索时间和平均采样节点数都有了明显的减小；而P_RRT*算法相比较RRT、RRT*算法的平均搜索时间也有了较明显的变化，但是其平均采样节点数比RRT*算法多，这就势必导致算法占用内存较大、消耗了更多的计算空间；改进的P_RRT*算法相比较其余三种算法在平均搜索时间、平均采样节点数上都有了很明显的减少，且其平均路径长度也是最短的，有效的证明了本发明改进算法的优越性。

表1相同障碍物个数下的算法比较

实验2不同障碍物个数下的算法对比实验

为了验证本发明改进算法的有效性，将改进算法在障碍物个数不同且随机分布的条件下与RRT、RRT*和P_RRT*算法进行对比实验分析，每组实验进行200次，取结果的平均值，如表2所示。

根据表2数据可知，经典RRT算法在障碍物较小的情况下搜索时间相对较小，当障碍物较多的时候其平均搜索时间、平均采样节点数都增长的很快，且搜索成功率也有了很明显的下降；RRT*算法在障碍物逐渐增加的情况下，平均搜索时间有了较明显的增加，且搜索成功率有所降低，平均采样节点数也较大；P_RRT*算法相比较RRT、RRT*算法在平均搜索时间、平均路径长度上虽然有了明显的提高，但是其在障碍物逐渐增加的情况下，平均采样节点数也随之大幅提高，收敛性降低且消耗大量内存空间；改进的P_RRT*算法在障碍物逐渐增多的情况下，仍然可以保持高效的搜索效率，平均搜索时间小且平稳，平均采样节点数也有了明显的降低，算法的收敛速率更快，同时其平均路径长度相比较RRT、RRT*和P_RRT*三种算法有了很大的减少，表现出了优异的效果。该实验验证了本发明改进算法的有效性。

表2不同障碍物个数下算法的比较

实验3两种环境地图下的算法对比实验

为了验证本发明改进算法的可靠性，对两种环境地图进行各个算法的对比实验，分别与RRT、RRT*和P_RRT*算法进行实验分析，

(1)地图1

在起始点和目标点等参数都相同的情况下与RRT、RRT*、P_RRT*算法进行对比实验，每组实验进行200次，取结果的平均值，如表3所示，不同算法生成的路径图如图6(a)～(d)所示。

表3地图1下的算法比较

(2)地图2

在起始点和目标点等参数都相同的情况下与RRT、RRT*、P_RRT*算法进行对比实验，每组实验进行200次，取结果的平均值，如表4所示。

表4地图2下的算法比较

根据表3、4数据可知，在地图1、2有较多障碍物且环境复杂的情形下，RRT算法的平均搜索时间非常大，分别为76.38s和83.89s，RRT*算法为52.99s和62.72s，P_RRT*算法为29.2s和46.36s，改进的P_RRT*算法最小为4.51s和8.3s，RRT算法的规划效率、搜索成功率很低，且平均采样节点数很高，导致其占用内存也较大；RRT*算法相比较RRT算法平均搜索时间、平均采样节点数有所减少，但与P_RRT*和改进的P_RRT*算法相比还是很高，且搜索成功率也较低；P_RRT*算法在平均搜索时间、平均路径长度上相比较于RRT、RRT*算法都有了改进，但其平均采样节点数还是很大，P_RRT*算法相比较于本发明的改进的P_RRT*算法仍然存在效率低、内存占用大、路径长且曲折的缺点；本发明改进P_RRT*算法的平均搜索时间、平均采样节点数都较小，在复杂的多障碍物环境下表现出了更加突出的效果，搜索的路径更短且更加光滑，通过两种不同环境地图的实验对比分析证明了本发明改进算法的可靠性。

ROS仿真实验分析

理论结合实际，为了验证本文算法的可行性，本节利用尤傲公司的UR5机器人在ROS中进行仿真分析，UR5机器人是一种六自由度的铰接机器人。首先在MoveIt中构建环境场景，利用ROS中rviz工具进行可视化演示，加载机器人模型，对移动机器人的停靠误差利用误差变换矩阵进行停靠误差的补偿，设置障碍物、起始点位姿、目标点位姿其中起始点、目标点位姿均是误差补偿以后的位姿。将改进的P_RRT*算法添加到OMPL库中，并对里面对应的ymal文件进行修改，采用Moveit里的KDL求解器进行各个关节角度的求解，最终得到一条符合要求的无碰撞路径，机器人的初始位姿和目标位姿的各个关节角度值如表5所示。为了验证改进算法的优越性，分别与RRT、RRT*和P_RRT*算法进行仿真实验对比分析，在上述相同障碍物的条件下，每个算法进行20次仿真实验，然后取结果的平均值，如表6所示，据表数据可知改进的P_RRT*算法在仿真实验中仍然具有很高的运动规划效率，验证了本文改进算法的优越性。

表5机器人起始、目标点各关节的角度值(弧度)

表6相同障碍物环境下的算法比较

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (6)

1.一种基于势函数的机器人路径规划方法，其特征在于：该方法包括以下步骤:

步骤1:初始化随机树中的各个参数，包括插入起始点、目标点、障碍物、步长、目标偏向步长、障碍物；

步骤2:利用双向快速探索随机树算法P_RRT*中势函数随机采样点选择方法RGD(q_rand)进行采样，获得随机采样点q_prand；

所述利用双向快速探索随机树算法P_RRT*中势函数随机采样点选择方法RGD(q_rand)进行采样，其具体步骤为:在选择随机节点q_rand的操作中引入了人工势场法的思想，目标点产生吸引力

定义一个潜在的随机样本点q_prand，q_prand是q_rand沿着吸引势场梯度减小方向的点，即偏向于目标方向的点，以一个小的步长λ向下坡进行运动得到的，将q_prand代替q_rand作为随机采样点，当靠近目标点的时候引力的势场梯度会随之减小拓展过程中实时判断与最近障碍物之间的欧式距离，如果小于障碍物空间距离

则立即终止程序，返回q_prand，否则进行下一次的随机样本的有限次数k的拓展，其中参数λ、k、d_obs的取值需要调整；

步骤3:获取服从均匀分布的随机数概率rand，进行最近邻节点q_nearest选取：如果随机数概率rand小于最近邻节点概率p_nearest，则最近邻节点是与随机采样点q_prand欧式距离最近的树中的节点；如果随机概率rand大于最近邻节点概率p_nearest，则采用公式(1)计算代价函数C(q)，选择树中代价函数C(q)最小的点作为最近邻节点；

所述公式(1)为：

其中||q_end-q||为目标点q_end与当前节点q之间的欧式距离，w_d为距离比例系数，w_c为混合比例系数，k表示当以前节点为中心、以半径r作的圆内障碍物的个数，最近邻节点则选择树中最小的代价函数C(q)；

步骤4:利用得到的最近邻节点q_nearest进行拓展，第一步拓展利用目标偏向策略，通过公式(2)得到新节点q_1new，并判断新节点q_1new是否与障碍物碰撞，如果碰撞了则返回步骤2；否则，执行步骤5；

所述公式(2)为：

其中k_p表示引力系数，ρ表示随机树的生长步长，q_prand表示随机采样点，q_nearest表示最近邻节点，||q_end-q_nearest||表示目标点q_end到最近邻节点q_nearest的欧氏距离，||q_prand-q_nearest||表示随机采样点q_prand到最近邻节点q_nearest的欧式距离；

步骤5：在拓展的新节点q_1new上，通过公式(3)进行第二步拓展，即采用由q_1new到q_prand构成一个矩形区域内随机选取点作为第二次拓展的得到新节点q_2new，并判断新节点q_2new与障碍物是否碰撞或者新节点q_2new与新节点q_1new之间的连线与障碍物是否碰撞，如果碰撞则执行步骤5重新进行新节点q_2new的随机采样；否则，执行步骤6；

所述公式(3)为：

[q_2new(x),q_2new(y),q_2new(z)]＝[q_1new(x),q_1new(y),q_1new(z)]+r*[rand(q_1new(x)→q_prand(x)),rand(q_1new(y)→q_prand(y)),rand(q_1new(z)→q_prand(z))]

其中q_2new(*)、q_1new(*)和q_prand(*)分别表示q_2new、q_1new、q_prand的坐标值；r表示拓展比例系数，取r＝0.1；rand(*)为选取的随机数值；并判断q_2new与q_1new之间的连线是否与障碍物碰撞，如果碰撞则放弃q_2new的拓展，重新进行q_2new的采样，若没有碰撞则将q_2new加入到随机树中，并将父节点赋予q_1new；

步骤6:以新节点q_2new为中心、半径r₁作邻近圆R₁，为新节点q_2new在邻近圆R₁内进行重新选择父节点的操作，找到欧氏距离代价值最小，且无碰撞的新节点q_2new的父节点；

所述为新节点q_2new在邻近圆内R₁进行重新选择父节点的操作，其具体过程为：首先计算出以最近邻节点q_nearest为父节点的代价值Cost(q_2new)，在状态空间Q选择潜在的父节点与新节点q_2new进行连接，然后判断以此潜在的父节点作为新节点q_2new的父节点的欧式距离代价值Cost(Q(i))是否小于代价值Cost(q_2new)，如果大于代价值Cost(q_2new)则放弃此潜在的父节点，进行下一个潜在父节点的计算，如果小于Cost(q_2new)，且潜在的父节点与新节点q_2new之间的连线未与障碍物碰撞，则进行新节点q_2new的父节点的重新选择并连线，如果与障碍物碰撞了则新节点q_2new的父节点不变仍是最近邻节点q_nearest，依次比较，直到所有的潜在父节点完成比较；

步骤7:以新节点q_2new为中心、以半径r₂作邻近圆R₂，以新节点q_2new作为父节点在邻近圆R₂里进行重新布线操作，找到邻近圆R₂里的点以新节点q_2new作为其父节点的欧式距离代价值小于其本身欧氏距离代价值且无碰撞的节点做重新布线操作，找到路径最优；

所述以新节点q_2new作为父节点在邻近圆R₂里进行重新布线操作，其具体过程为:新节点q_2new作为邻近圆R₂内所有潜在子节点的父节点，如果以新节点q_2new为潜在子节点的父节点的欧氏距离代价值Cost(q_{potential_child})比潜在子节点原始的欧式距离代价值小，且未与障碍物碰撞，则放弃潜在子节点原始的父节点将新节点q_2new作为其父节点进行重新布线，否则保持不变；

步骤8:检查新节点q_2new与目标点之间的欧式距离是否小于一倍的步长ρ，如果是，则连接新节点q_2new和目标点，得到一条从起始点到目标点的无碰撞的路径，算法结束；否则，返回执行步骤2；

所述从起始点到目标点的无碰撞的路径，进行删除冗余节点的操作，具体操作过程：从起始点开始，放弃第二个路径点，连接第三个路径点，如果此路径未与障碍物碰撞则证明第二个节点是冗余节点则删除第二个节点，再将起始节点连接第四个节点判断是否碰撞，如果没有碰撞则删除第三个节点，按照此方法进行判断，如果碰撞了则保留该节点，并以该节点作为起始点进行上述判断，直至到达目标点，即可得到一条从起始点到目标点的去除冗余节点的路径。

2.如权利要求1所述的基于势函数的机器人路径规划方法，其特征在于：所述步骤2中，参数λ、k、d_obs的取值为：λ＝0.02，k＝80，d*_obs＝0.1。

3.如权利要求1所述的基于势函数的机器人路径规划方法，其特征在于：所述步骤3中的最近邻节点概率p_nearest＝0.5。

4.如权利要求1至3中任一项所述的基于势函数的机器人路径规划方法，其特征在于：所述步骤8中，从起始点到目标点的去除冗余节点的路径，采用一种最大曲率约束的方法进行路径的平滑处理，即在得到的去除冗余点的路径中，从起始点开始依次计算路径之间的夹角的余弦值，如果大于规定的最大转角的余弦值，则在此夹角的顶点为标准在其左右两侧分别加上原始未去除冗余节点的此顶点的子节点和父节点，即左侧加入父节点，右侧加入子节点。

5.如权利要求4所述的基于势函数的机器人路径规划方法，其特征在于：所述规定的最大转角的余弦值为0.707。

6.如权利要求4所述的基于势函数的机器人路径规划方法，其特征在于：所述从起始点到目标点的去除冗余节点的路径，采用最大曲率约束的方法进行路径的平滑处理后，采用贝赛尔曲线进行进一步的平滑处理。

Images