一种多分辨率障碍物环境下移动机器人混合路径规划方法技术

本发明专利技术公开了一种多分辨率障碍物环境下移动机器人混合路径规划方法，目的是解决现有方法初始规划存在盲目性、环境建模缺乏灵活性、实时避障能力差的问题。技术方案是采用自适应非均匀极径的极坐标建模方法对移动机器人运动空间进行环境建模；采用重力粒子群搜索方法规划出从起点到终点的初始全局路径；根据初始全局路径，采用改进的人工势场法，通过预估最小安全距离和安全避碰角，进行局部动态避障，依次到达各初始全局路径点；到达规划终点则输出最终的全局无碰路径。采用本发明专利技术能有效改善初始全局规划的盲目性和环境建模灵活性，对动态未知障碍物的实时避障能力强，方法的速度快、精度高、适应性强。

A hybrid path planning method for mobile robot with multi resolution obstacles

The invention discloses a multi resolution obstacle environment mobile robot path planning method is designed to solve the existing mixed method of initial planning blindness, environment modeling and lack of flexibility, real-time obstacle avoidance capability is poor. The technical scheme is based on adaptive non uniform polar polar coordinate modeling method for environment modeling of mobile robot motion space; using gravity particle swarm search method planning from the starting point to the initial global path end point; according to the initial global path, using the improved artificial potential field method, the estimated minimum safe distance and collision angle of local dynamic obstacle avoidance, in order to reach the initial global path planning; reach the end point is the final output of the global path. By adopting the method, the invention can effectively improve the blindness of the initial global planning and the flexibility of the environmental modeling, and has the advantages of strong real-time obstacle avoidance capability for the dynamic unknown obstacle, high speed, high precision and strong adaptability of the method.

【技术实现步骤摘要】

本专利技术涉及移动机器人
，尤其是一种静态、动态障碍物并存，且障碍物分布具有多分辨率特点的复杂情况下的移动机器人混合路径规划方法。
技术介绍
路径规划技术一直是智能移动机器人研究的核心内容之一，其要求机器人根据预先获知或自身传感器对环境的感知信息，自行规划出一条从起点到终点，满足一定准则(路径长度最短等)的无碰安全路径。高效、可靠的路径规划技术是机器人完成具体任务的前提和基本要求。根据环境信息获取程度不同，路径规划方法可分为依靠先验已知信息的全局规划方法和依靠局部感知信息的局部规划方法。在一些大规模地图范围的路径规划任务中，障碍物环境往往具有多分辨率特点：即环境中障碍物分布疏密程度差异大、存在一些预知的典型障碍物，如通过卫星地图等获知的固定建筑等，局部还存在未探明的小的静态或动态障碍物。此时传统的全局规划算法，如自由空间法等根据地图几何特征搜索路径的方法，以及粒子群算法、启发式A*等智能算法，无法处理未知的静态或动态障碍物，而局部规划算法，如人工势场法、滚动窗口法等，由于缺乏已知全局信息的引导，往往计算复杂或易陷入局部死循环。因而，采用全局路径规划和实时局部避障相结合的混合路径规划方法是满足移动机器人高效、可靠、实时需求的。环境建模是机器人进行全局路径规划的前提，其与路径规划的算法效率紧密相关。常用的环境建模方法包括栅格法、链接图法、等分坐标变换法等，然而这些方法在针对多分辨率的障碍物环境中缺乏灵活适应性：栅格法需要根据最小的障碍物确定栅格的划分，造成极大的计算资源浪费；链接图法根据障碍物顶点生成的网状链接图需要经过复杂处理才能定义种群的搜索空间；等分坐标变换法以等分始末点连线的垂线确定种群的搜索空间时，很难兼顾路径解最优性与计算资源的平衡，在障碍物稀疏处搜索空间太大浪费计算资源，障碍物密集处离散化的搜索空间太小难以规划出最优路径。粒子群算法是解决机器人全局路径规划最常用的算法之一，它是由Kennedy和Eberhart两位博士根据自然界鸟群觅食的行为特点首次在1995年的国际会议“IEEEInterationalConferenceonNeuralNetwork”中的“ParticleSwarmOptimization”(粒子群优化)一文中提出的一种智能搜索算法。粒子群中的每个粒子代表一个路径解，粒子的维数代表路径点的个数。粒子在搜索空间内展开随机搜索，计算每个粒子所代表路径解的评价指标(如路径长度等)，选出粒子群中指标最优(长度最短、安全系数最高等)的粒子来更新每个粒子的历史最优路径解和粒子群的全局最优路径解，以此引导当前粒子快速收敛到满足路径指标要求的最优路径解。其具有结构简单、参数易调节的优点，但其仍存在容易过早收敛到非最优路径解的不足。根据实时获取的感知信息进行局部动态避障是移动机器人路径规划的难点，要求算法具有计算效率高、求解稳定的优点。人工势场法由Khatib于1986年首次在国际期刊“TheInternationalofRoboticsResearch”中第五卷第一期的“Real-timeobstacleavoidanceformanipulatorsandmobilerobots”(移动机器人和控制器的实时避障)一文中提出以解决机器人路径规划和控制问题，其将机器人在环境中的运动视为一种虚拟人工受力场中的运动。目标对机器人产生引力，障碍物对其产生斥力，通过引力和斥力的合力来改变机器人的运动方向和速度，驱使其躲避障碍物朝向目标点位置移动。其与滚动窗口法、行为法等相比算法简单、计算量小、产生的路径平滑，但仍存在由于对避障距离缺乏预估计，使机器人在无需避障时远离障碍导致路径长度过大，以及在特殊情况下陷入局部死循环的问题。总体而言，现有技术在处理具有多分辨率障碍物的动态环境中的路径规划中还存在以下缺陷：(1)初始规划存在盲目性。在具有未知障碍物的动态环境中，只依靠实时获取的环境信息进行规划，缺乏全局信息引导，初始规划存在盲目性。(2)全局路径规划的环境建模方法缺乏灵活适应性。已有的环境建模方法不能根据障碍物分布灵活确定路径搜索空间的大小，导致计算资源浪费。(3)对未知的静态或动态障碍物的实时避障能力差。对于动态不确定运动的障碍物，由于对避障距离及角度缺乏预估可能导致机器人过度远离障碍物，导致路径长度过大。对一些含有陷阱的特殊环境，可能使程序陷入死循环。
技术实现思路
本专利技术要解决的技术问题是提供一种多分辨率障碍物的动态环境下移动机器人混合路径规划方法，将全局路径规划和局部动态避障相结合，解决现有方法存在的初始规划存在盲目性、环境建模缺乏灵活适应性、对未知动态或静态障碍物实时避障能力差的问题。本专利技术解决以上技术问题采用如下技术方案：第一步：采用自适应非均匀极径的极坐标建模方法对移动机器人运动空间进行环境建模。自适应非均匀极径的极坐标建模方法能够根据障碍物的疏密程度灵活设定全局路径规划方法的路径搜索空间，同时相对传统的笛卡儿坐标建模，极坐标系允许通过路径回退逃离某些凹形障碍物，具体步骤为：1.1在机器人运动空间内建立笛卡尔坐标系oxy，将机器人和预先探明的障碍物信息在笛卡尔坐标系oxy上标注，包括机器人起始位置S、目标点位置G以及各障碍物的顶点位置，从键盘接收已知的障碍物总数目obnum。1.2以S为极点，SG射线为极轴，逆时针为正方向，建立机器人运动空间的极坐标系。将1.1中输入的机器人和预先探明的障碍物信息转化为极径和极角表示的极坐标信息。1.3确定初始全局路径点的个数和搜索空间：1.3.1确定初始全局路径点的极径序列：如图2所示，以SG的距离L为半径，在极轴两侧作四分之一圆。设置角度增量为δ(δ∈[5，90]且90能被δ整除)。从极点S出发，依次引出极角为mδ(m＝-(90-δ)/δ,-(90-2δ)/δ,...,-1,0,1,...,(90-2δ)/δ,(90-δ)/δ)、极径(即始末点总距离)为L的1+180/δ条向量，分别计算每条向量与每个障碍物各边的交点，将所有向量与障碍物的交点的极径按照从小到大的顺序排列，得到一个极径序列。1.3.2对所得的极径序列作以下处理：设置相邻的初始全局路径点之间允许的最大差值dmax和最小差值dmin。dmax根据L和最少需要的全局路径点个数来定，dmax为L/(q+1)，q为最少需要的全局路径点个数，通常设2≤q≤5。dmin根据L和最多需要的全局路径点个数来定，dmin为L/(p+1)，p为最多需要的全局路径点本文档来自技高网...

【技术保护点】
一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于包括以下步骤：第一步：采用自适应非均匀极径的极坐标建模方法对移动机器人运动空间进行环境建模，方法是:1.1.在机器人运动空间内建立笛卡尔坐标系oxy，将机器人和预先探明的障碍物信息在笛卡尔坐标系oxy上标注，包括机器人起始位置S、目标点位置G以及各障碍物的顶点位置，从键盘接收已知的障碍物总数obnum；1.2.以S为极点，SG射线为极轴，逆时针为正方向，建立机器人运动空间的极坐标系，将1.1中输入的机器人和预先探明的障碍物信息转化为极径和极角表示的极坐标信息；1.3.确定初始全局路径点的个数和搜索空间：1.3.1.确定初始全局路径点的极径序列：以SG的距离L为半径，在极轴两侧作四分之一圆；设置角度增量为δ，δ∈[5，90]且90能被δ整除，从极点S出发，依次引出极角为mδ，m＝‑(90‑δ)/δ,‑(90‑2δ)/δ,...,‑1,0,1,...,(90‑2δ)/δ,(90‑δ)/δ、极径即始末点总距离为L的1+180/δ条向量，分别计算每条向量与每个障碍物各边的交点，将所有向量与障碍物的交点的极径按照从小到大的顺序排列，得到一个极径序列；1.3.2.对所得的极径序列作以下处理：设置相邻的初始全局路径点之间允许的最大差值dmax和最小差值dmin，dmax根据L和最少需要的全局路径点个数来定，dmax为L/(q+1)，q为最少需要的全局路径点个数；dmin根据L和最多需要的全局路径点个数来定，dmin为L/(p+1)，p为最多需要的全局路径点个数；若极径序列中相邻两极径的差值大于Dmax，则在极径序列中这两相邻极径之间插入两相邻极径的平均值；若极径序列中两相邻极径的差值小于Dmin，则在极径序列中删除两相邻极径，在序列中插入两相邻极径的平均值，得到新的极径序列D＝[d1,d2,...,dn]，n为最终确定的极径个数；第二步：采用重力粒子群搜索方法规划出从起点到终点的初始全局路径，机器人沿此初始全局路径边行走边进行局部动态避障，方法是：2.1.初始化重力粒子群算法参数：2.1.1.设置重力粒子群算法的参数：包括粒子群的粒子数目N，最大迭代代数Iter，学习因子c1、c2，加速度系数c3，N、Iter、c1、c2、c3均为正整数，初始化当前代数t为0，惯性因子w为0.9；2.1.2.初始化粒子群中的每个粒子，粒子群表示粒子1,2,3,...,N的集合，每个粒子表示一个全局路径点序列，由全局路径点的极角序列和极径序列组成；粒子的维数，即所表示的初始全局路径点的个数初始化为n；粒子k的极径序列编码为Dk＝[dk1,...,dki...,dkn]，k＝1,2,…,N，dki表示粒子k中第i个全局路径点的极径，取值范围为(0,L)；D1,D2,...,DN均初始化为D；粒子k的极角序列编码为Xk＝[θk1,...,θki,...,θkn]，θki表示粒子k中第i个全局路径点的极角，取值范围为X1,X2,...,XN中的每个元素均初始化为0；粒子1,2,3,...,N的个体历史最优极角序列pbest1,pbest2,...,pbestN初始化为各自的极角序列X1,X2,...,XN，个体历史最优的路径长度flength(pbest1),flength(pbest2),...,flength(pbestN)均初始化为+∞，个体历史最优的路径碰撞约束co(pbest1),co(pbest2),...,co(pbestN)均初始化1，粒子群的全局最优极角序列gbest初始化为pbest1，gbest的路径长度flength(gbest)初始化为+∞，gbest的碰撞约束co(gbest)初始化为1；粒子k的速度序列编码为Vk＝[vk1,..,vki,..,vkn]，vki表示粒子k的第i个全局路径点的极角的变化速度，取值范围为对V1,V2,...,VN进行初始化；2.2.计算粒子1,2,3,...,N所表示的路径的评价值，更新pbest1,pbest2,...,pbestN和gbest：2.2.1.初始化k＝1；2.2.2.计算粒子k的Xk的路径长度flength(Xk)：flength(Xk)=Σi=0n(dki+1cosθki+1-dkicosθki)2+(dki+1sinθki+1-dkisinθki)2---(1)]]>2.2.3.计算粒子k的Xk的碰撞约束程度co(Xk)：co(Xk)=1obnumΣj=1obnumcoj(Xk)---(2)]]>其中2.2.4.计算粒子k的Xk和pbestk之间的支配关系，计算方法为：如果满足下列两个条件之一，则Xk支配pbestk：a)co(Xk)＜co(pbestk...

【技术特征摘要】
1.一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于包括以下
步骤：
第一步：采用自适应非均匀极径的极坐标建模方法对移动机器人运动空间进行环境建
模，方法是:
1.1.在机器人运动空间内建立笛卡尔坐标系oxy，将机器人和预先探明的障碍物信息
在笛卡尔坐标系oxy上标注，包括机器人起始位置S、目标点位置G以及各障碍物的
顶点位置，从键盘接收已知的障碍物总数obnum；
1.2.以S为极点，SG射线为极轴，逆时针为正方向，建立机器人运动空间的极坐标
系，将1.1中输入的机器人和预先探明的障碍物信息转化为极径和极角表示的极坐标
信息；
1.3.确定初始全局路径点的个数和搜索空间：
1.3.1.确定初始全局路径点的极径序列：以SG的距离L为半径，在极轴两侧作四
分之一圆；设置角度增量为δ，δ∈[5，90]且90能被δ整除，从极点S出发，依次
引出极角为mδ，m＝-(90-δ)/δ,-(90-2δ)/δ,...,-1,0,1,...,(90-2δ)/δ,(90-δ)/δ、极径即始末点
总距离为L的1+180/δ条向量，分别计算每条向量与每个障碍物各边的交点，将所
有向量与障碍物的交点的极径按照从小到大的顺序排列，得到一个极径序列；
1.3.2.对所得的极径序列作以下处理：设置相邻的初始全局路径点之间允许的最
大差值dmax和最小差值dmin，dmax根据L和最少需要的全局路径点个数来定，dmax为
L/(q+1)，q为最少需要的全局路径点个数；dmin根据L和最多需要的全局路径点
个数来定，dmin为L/(p+1)，p为最多需要的全局路径点个数；若极径序列中相邻
两极径的差值大于Dmax，则在极径序列中这两相邻极径之间插入两相邻极径的平均
值；若极径序列中两相邻极径的差值小于Dmin，则在极径序列中删除两相邻极径，
在序列中插入两相邻极径的平均值，得到新的极径序列D＝[d1,d2,...,dn]，n为最终确
定的极径个数；
第二步：采用重力粒子群搜索方法规划出从起点到终点的初始全局路径，机器人沿此
初始全局路径边行走边进行局部动态避障，方法是：
2.1.初始化重力粒子群算法参数：
2.1.1.设置重力粒子群算法的参数：包括粒子群的粒子数目N，最大迭代代数Iter，
学习因子c1、c2，加速度系数c3，N、Iter、c1、c2、c3均为正整数，初始化当前
代数t为0，惯性因子w为0.9；
2.1.2.初始化粒子群中的每个粒子，粒子群表示粒子1,2,3,...,N的集合，每个粒子表
示一个全局路径点序列，由全局路径点的极角序列和极径序列组成；粒子的维数，
即所表示的初始全局路径点的个数初始化为n；粒子k的极径序列编码为
Dk＝[dk1,...,dki...,dkn]，k＝1,2,…,N，dki表示粒子k中第i个全局路径点的极径，取值
范围为(0,L)；D1,D2,...,DN均初始化为D；粒子k的极角序列编码为
Xk＝[θk1,...,θki,...,θkn]，θki表示粒子k中第i个全局路径点的极角，取值范围为
X1,X2,...,XN中的每个元素均初始化为0；粒子1,2,3,...,N的个体历史最优极
角序列pbest1,pbest2,...,pbestN初始化为各自的极角序列X1,X2,...,XN，个体历史最优的
路径长度flength(pbest1),flength(pbest2),...,flength(pbestN)均初始化为+∞，个体历史最优的路径
碰撞约束co(pbest1),co(pbest2),...,co(pbestN)均初始化1，粒子群的全局最优极角序列
gbest初始化为pbest1，gbest的路径长度flength(gbest)初始化为+∞，gbest的碰撞约束
co(gbest)初始化为1；粒子k的速度序列编码为Vk＝[vk1,..,vki,..,vkn]，vki表示粒子k的
第i个全局路径点的极角的变化速度，取值范围为对V1,V2,...,VN进行初始
化；
2.2.计算粒子1,2,3,...,N所表示的路径的评价值，更新pbest1,pbest2,...,pbestN和gbest：
2.2.1.初始化k＝1；
2.2.2.计算粒子k的Xk的路径长度flength(Xk)：
flength(Xk)=Σi=0n(dki+1cosθki+1-dkicosθki)2+(dki+1sinθki+1-dkisinθki)2---(1)]]>2.2.3.计算粒子k的Xk的碰撞约束程度co(Xk)：
co(Xk)=1obnumΣj=1obnumcoj(Xk)---(2)]]>其中2.2.4.计算粒子k的Xk和pbestk之间的支配关系，计算方法为：
如果满足下列两个条件之一，则Xk支配pbestk：
a)co(Xk)＜co(pbestk)
b)co(Xk)＝co(pbestk)andflength(Xk)＜flength(pbestk)
若Xk支配pbestk，表示Xk所表示的路径优于pbestk所表示的路径，则令
pbestk＝Xk，flength(pbestk)＝flength(Xk)，co(pbestk)＝co(Xk)，执行步骤2.2.5；若Xk不
支配pbestk，则执行步骤2.2.6；
2.2.5.计算粒子k的pbestk和粒子群的gbest之间的支配关系，计算方法为：
如果满足下列两个条件之一，则pbestk支配gbest：
a)co(pbestk)＜co(gbest)
b)co(pbestk)＝co(gbest)andflength(pbestk)＜flength(gbest)；
若pbestk不支配gbest,则直接执行步骤2.2.6；否则令gbest＝pbestk，
flength(gbest)＝flength(pbestk)，co(gbest)＝co(pbestk)，执行步骤2.2.6；
2.2.6.令k＝k+1，若k<＝N，返回2.2.2，否则表明这一代所有粒子以及个体历史最
优、粒子群的全局最优的评价值计算完成，执行步骤2.3；
2.3.计算粒子群中每个粒子受到其他粒子的作用力产生的加速度，得到粒子
1,2,...,k,...,N受其他粒子作用力产生的加速度a1,a2,...,ak,...,aN；
2.4.更新...

【专利技术属性】
技术研发人员：尹全军，彭勇，秦龙，焦鹏，张琪，杨伟龙，孙林，
申请(专利权)人：中国人民解放军国防科学技术大学，
类型：发明
国别省市：湖南;43