一种改进APF-RRT算法的机器人避障路径规划方法技术

本发明专利技术公开了一种改进APF

【技术实现步骤摘要】
一种改进APF
‑
RRT算法的机器人避障路径规划方法


[0001]本专利技术涉及机器人避障路径规划
，尤其涉及一种改进APF
‑
RRT算法的机器人避障路径规划方法。

技术介绍

[0002]机器人在自动化和智能化发展中扮演着及其重要的角色，随着工作环境越来越复杂,对其工作精度及效率的要求也在不断提高。机器人的避障路径规划是机器人在工业应用过程中的一个重要问题，即已知起始位置和目标位置，在工作空间中规划出一条不与机器人本体及障碍物和周边环境发生碰撞的连续路径。传统路径规划算法主要有Dijkstra算法、A*算法、人工势场法(Artificial potential field method，APF)等，这些算法在移动机器人避障路径规划中具有较好的表现，但在六自由度机器人避障路径规划中，由于空间维数高，严重降低了传统路径规划算法的成功率和运行效率。针对这些问题，目前在六自由度机器人避障路径规划中常用的方法有：RRT算法(快速扩展随机树)、RRT
‑
Connect算法(双向快速扩展随机树)、RRT*算法(快速渐进最优)等。传统快速随机树算法在不同维度的机器人路径规划中具有较好的效果，通过不停采样后，整个状态空间都会被充分探索，最终生成避障路径，但该算法的采样方式是完全随机的，通过这种方法扩展搜索树新节点后，算法的效率和路径质量并不能受到保障。传统RRT
‑
Connect算法分别在初始位置和目标位置构建搜索树，利用两个搜索树交互的扩展来搜索工作路径，与传统RRT算法相比，加快了算法速度，但RRT
‑
Connect算法的探索方向依旧是随机的，且路径质量也没有明显得到改善。RRT*算法通过重选父节点和重新布线的方式对路径质量进行优化，与前两种算法比较，RRT*算法可以获取较优的路径，但计算量会随之增大，算法运行时间较长。因此，基于RRT改进的算法仍然存在快速性和最优性难以平衡，运行内存占用大等问题。

技术实现思路

[0003]针对现有技术存在的缺陷和不足，本专利技术的目的在于提供一种改进APF
‑
RRT算法的机器人避障路径规划方法，与传统APF算法和传统RRT算法相比，该方案将障碍物数目参数、算法切换策略、双边最近节点选择策略、节点两次拓展策略、圆锥体子集约束采样和目标引导结合的混合采样策略、去除冗余节点策略、3次非均匀有理B样条(NURBS)曲线平滑策略进行有机融合，不仅可以提高算法运行效率，缩短路径长度，而且还能让机器人可以平稳的运行。
[0004]本专利技术提出了一种改进APF
‑
RRT算法的机器人避障路径规划方法，具体包括以下步骤：
[0005]步骤1：获取工作空间和障碍物环境信息，设置机器人在关节空间中的起始位置q
init
、目标位置q
goal
、步长L、路径节点集S；
[0006]步骤2：计算当前节点与障碍物之间的距离；如果当前节点与最近的障碍物之间的距离大于步长的两倍，则执行步骤3；如果当前节点与最近的障碍物之间的距离小于步长的
两倍，则执行步骤4；
[0007]步骤3：引入障碍物数目作为势场参数，采用修改后的人工势场函数使q
goal
对当前节点q
nearest
产生吸引力，使障碍物对q
nearest
生成排斥力，q
nearest
按照给定的步长L沿着两个力的合力方向生成新的节点q
new
；
[0008]改进的人工势场包括引力场函数U
att
(p)如式1所示、斥力场函数U
rep
(p)如式2所示，则合力势场U
sum
函数如式3所示：
[0009][0010][0011]U
att
＝U
att
(q)+ΣU
rep
(q)
ꢀꢀꢀ
(3)
[0012]其中，ε为引力势场的系数，σ为斥力势场的系数，d(q,q
goal
)表示当前位置与目标位置的欧氏距离,d(q,q
obs
)表示当前位置与障碍物中心位置的欧氏距离，d0为障碍物影响半径，n为障碍物数目；引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式(4)和(5)，合力F
sum
如式(6)所示，斥力的分力F
rep1
和F
rep2
如式(7)和(8)所示：
[0013][0014][0015]F
sum
＝F
att
(q)+ΣF
rep
(q)
ꢀꢀꢀ
(6)
[0016][0017][0018]其中，表示d(q,q
goal
)的导数，F
rep1
方向由障碍物指向当前位置，F
rep2
方向由当前位置指向目标点；当目标点附近存在障碍物时，机器人受到的斥力较大，引力较小，存在无法到达目标点的现象，通过改进的斥力场函数，机器人在靠近障碍物时，受到的斥力随着距离越近变得越小，避免产生目标不可达问题；
[0019]使用改进后的APF算法在目标点和障碍物的联合作用下进行搜索和前进，(1)计算路径点受到的吸引力和排斥力，根据合力方向生成距离当前节点步长L的新节点(起始位置q
init
为初始节点)；(2)确定新节点所受的吸引力和排斥力是否相等且方向相反，如果相等且方向相反，证明新节点陷入局部最小值，执行步骤4，使用改进后的RRT算法生成下一节点，直到有节点跳出局部最小值，将该节点添加到路径节点集S中，并在该节点处重新执行步骤3进行路径搜索；如果吸引力和排斥力不是相等且方向相反，则将新节点保存到路径节点集S中，继续执行步骤；(3)计算节点与最近障碍物的距离，如果节点与最近障碍物之间的距离小于步长的两倍，则跳转到步骤4，否则，继续执行步骤。(4)判断当前节点与目标位置q
goal
距离是否小于步长L，如果节点与目标位置q
goal
之间的距离小于步长L，则将目标位置
q
goal
添加到路径节点集S中，并结束算法，保存路径节点集S；否则，重新执行步骤3，继续路径搜索；
[0020]步骤4：使用改进的RRT算法进行路径搜索；(1)初始化随机搜索树，获取起始位置q
init
、目标位置q
goal
、步长L、路径节点集S，起始位置q
init
作为随机搜索树的根节点，此时树中只有一个节点q
init
；(2)利用均匀采样法，在区间[0，1]中随机生成概率P
rand
，如果随机概率P
rand
小于目标偏置率P(P＝0.5)，将采样点q
rand
选择为目标位置q本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种改进APF
‑
RRT算法的机器人避障路径规划方法，其特征在于：在已知起始位置，目标位置和障碍物环境信息的条件下，利用改进APF
‑
RRT算法，引入障碍物数目参数和算法切换策略避免传统APF算法无法到达目标点和陷入局部最小值，引入双边最近节点选择策略、节点两次拓展策略、圆锥体子集约束采样和目标引导结合的混合采样策略，加快改进RRT算法的收敛效率，采用去除冗余点和3次非均匀有理B样条(NURBS)曲线平滑策略对避障路径进行裁剪和平滑，减少路径长度，满足机器人运动要求。2.根据权利要求1所述的一种改进APF
‑
RRT算法的机器人避障路径规划方法，其特征在于：包括以下步骤：步骤1：获取工作空间和障碍物环境信息，设置机器人在关节空间中的起始位置q
init
、目标位置q
goal
、步长L、路径节点集S；步骤2：计算当前节点与障碍物之间的距离；如果当前节点与最近的障碍物之间的距离大于步长的两倍，则执行步骤3；如果当前节点与最近的障碍物之间的距离小于步长的两倍，则执行步骤4；步骤3：引入障碍物数目作为势场参数，采用修改后的人工势场函数使q
goal
对当前节点q
nearest
产生吸引力，使障碍物对q
nearest
生成排斥力，q
nearest
按照给定的步长L沿着两个力的合力方向生成新的节点q
new
；使用改进后的APF算法在目标点和障碍物的联合作用下进行搜索和前进，(1)计算路径点受到的吸引力和排斥力，根据合力方向生成距离当前节点步长L的新节点(起始位置q
init
为初始节点)；(2)确定新节点所受的吸引力和排斥力是否相等且方向相反，如果相等且方向相反，证明新节点陷入局部最小值，执行步骤4，使用改进后的RRT算法生成下一节点，直到有节点跳出局部最小值，将该节点添加到路径节点集S中，并在该节点处重新执行步骤3进行路径搜索；如果吸引力和排斥力不是相等且方向相反，则将新节点保存到路径节点集S中，继续执行步骤；(3)计算节点与最近障碍物的距离，如果节点与最近障碍物之间的距离小于步长的两倍，则跳转到步骤4，否则，继续执行步骤。(4)判断当前节点与目标位置q
goal
距离是否小于步长L，如果节点与目标位置q
goal
之间的距离小于步长L，则将目标位置q
goal
添加到路径节点集S中，并结束算法，保存路径节点集S；否则，重新执行步骤3，继续路径搜索；步骤4：使用改进的RRT算法进行路径搜索；(1)初始化随机搜索树，获取起始位置q
init
、目标位置q
goal
、步长L、路径节点集S，起始位置q
init
作为随机搜索树的根节点，此时树中只有一个节点q
init
；(2)利用均匀采样法，在区间[0，1]中随机生成概率P
rand
，如果随机概率P
rand
小于目标偏置率P(P＝0.5)，将采样点q
rand
选择为目标位置q
goal
；如果大于等于目标偏置率P，则在工作空间中的随机生成采样点q
rand
；(3)使用双边最近邻节点选择策略选取最近邻节点，在节点q
nearest
指向采样点q
rand
的方向进行步长L展开，得到一个新的节点q
new
，进行碰撞检测，判断新节点与最近邻节点的连线是否与障碍物碰撞，如果发生碰撞，舍弃该新节点q
new
，重新执行(2)；如果没有碰撞，则将新节点q
new
添加到随机搜索树中，并将新节点q
new
的父节点分配给最近邻节点q
nearest
；在生成的新节点q
new1
上，利用节点二次扩展策略生成新节点q
new2
，继续执行(4)，否则，舍弃新节点q
new2
，继续执行(4)；(4)当随机搜索树中的节点与目标位置q
goal
的距离小于步长L时，结束算法，将目标位置q
goal
添加到随机搜索树中并将随机搜索树中的节点保存到路径节点集S中；当随机搜索树中的节点与最近障碍物的距离大于步长的两倍时，跳转到步骤3，执行改进后的APF算法，否则，跳转到(2)，继续进行路径搜索；
步骤5：改进APF
‑
RRT算法执行完毕后，将路径节点集S中的节点进行连接，去除冗余节点，得到处理后的路径S
new
；步骤6：使用3次NURBS曲线对路径S
new
进行平滑，最终得到机器人的避障路径。3.根据权利要求1所述的一种改进APF
‑
RRT算法的机器人避障路径规划方法，其特征在于：引入障碍物数目作为势场参数，采用修改后的人工势场函数使q
goal
对当前节点q
nearest
产生吸引力，使障碍物对q
nearest
生成排斥力，q
nearest
按照给定的步长L沿着两个力的合力方向生成新的节点q
new
；改进的人工势场包括引力场函数U
att
(p)如式1所示、斥力场函数U
rep
(p)如式2所示，则合力势场U
sum
函数如式3所示：函数如式3所示：U

【专利技术属性】
技术研发人员：唐新星，刘博聪，项天野，陈国梁，裴洪鑫，曹尧，
申请(专利权)人：长春工业大学，
类型：发明
国别省市：