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

Abstract

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

Description

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

技术领域

本发明涉及移动机器人技术领域，尤其是一种静态、动态障碍物并存，且障碍物分布具有多分辨率特点的复杂情况下的移动机器人混合路径规划方法。

背景技术

路径规划技术一直是智能移动机器人研究的核心内容之一，其要求机器人根据预先获知或自身传感器对环境的感知信息，自行规划出一条从起点到终点，满足一定准则(路径长度最短等)的无碰安全路径。高效、可靠的路径规划技术是机器人完成具体任务的前提和基本要求。

根据环境信息获取程度不同，路径规划方法可分为依靠先验已知信息的全局规划方法和依靠局部感知信息的局部规划方法。在一些大规模地图范围的路径规划任务中，障碍物环境往往具有多分辨率特点：即环境中障碍物分布疏密程度差异大、存在一些预知的典型障碍物，如通过卫星地图等获知的固定建筑等，局部还存在未探明的小的静态或动态障碍物。此时传统的全局规划算法，如自由空间法等根据地图几何特征搜索路径的方法，以及粒子群算法、启发式A*等智能算法，无法处理未知的静态或动态障碍物，而局部规划算法，如人工势场法、滚动窗口法等，由于缺乏已知全局信息的引导，往往计算复杂或易陷入局部死循环。因而，采用全局路径规划和实时局部避障相结合的混合路径规划方法是满足移动机器人高效、可靠、实时需求的。

环境建模是机器人进行全局路径规划的前提，其与路径规划的算法效率紧密相关。常用的环境建模方法包括栅格法、链接图法、等分坐标变换法等，然而这些方法在针对多分辨率的障碍物环境中缺乏灵活适应性：栅格法需要根据最小的障碍物确定栅格的划分，造成极大的计算资源浪费；链接图法根据障碍物顶点生成的网状链接图需要经过复杂处理才能定义种群的搜索空间；等分坐标变换法以等分始末点连线的垂线确定种群的搜索空间时，很难兼顾路径解最优性与计算资源的平衡，在障碍物稀疏处搜索空间太大浪费计算资源，障碍物密集处离散化的搜索空间太小难以规划出最优路径。

粒子群算法是解决机器人全局路径规划最常用的算法之一，它是由Kennedy和Eberhart两位博士根据自然界鸟群觅食的行为特点首次在1995年的国际会议“IEEEInterationalConferenceonNeuralNetwork”中的“ParticleSwarmOptimization”(粒子群优化)一文中提出的一种智能搜索算法。粒子群中的每个粒子代表一个路径解，粒子的维数代表路径点的个数。粒子在搜索空间内展开随机搜索，计算每个粒子所代表路径解的评价指标(如路径长度等)，选出粒子群中指标最优(长度最短、安全系数最高等)的粒子来更新每个粒子的历史最优路径解和粒子群的全局最优路径解，以此引导当前粒子快速收敛到满足路径指标要求的最优路径解。其具有结构简单、参数易调节的优点，但其仍存在容易过早收敛到非最优路径解的不足。

根据实时获取的感知信息进行局部动态避障是移动机器人路径规划的难点，要求算法具有计算效率高、求解稳定的优点。人工势场法由Khatib于1986年首次在国际期刊“TheInternationalofRoboticsResearch”中第五卷第一期的“Real-timeobstacleavoidanceformanipulatorsandmobilerobots”(移动机器人和控制器的实时避障)一文中提出以解决机器人路径规划和控制问题，其将机器人在环境中的运动视为一种虚拟人工受力场中的运动。目标对机器人产生引力，障碍物对其产生斥力，通过引力和斥力的合力来改变机器人的运动方向和速度，驱使其躲避障碍物朝向目标点位置移动。其与滚动窗口法、行为法等相比算法简单、计算量小、产生的路径平滑，但仍存在由于对避障距离缺乏预估计，使机器人在无需避障时远离障碍导致路径长度过大，以及在特殊情况下陷入局部死循环的问题。

总体而言，现有技术在处理具有多分辨率障碍物的动态环境中的路径规划中还存在以下缺陷：(1)初始规划存在盲目性。在具有未知障碍物的动态环境中，只依靠实时获取的环境信息进行规划，缺乏全局信息引导，初始规划存在盲目性。(2)全局路径规划的环境建模方法缺乏灵活适应性。已有的环境建模方法不能根据障碍物分布灵活确定路径搜索空间的大小，导致计算资源浪费。(3)对未知的静态或动态障碍物的实时避障能力差。对于动态不确定运动的障碍物，由于对避障距离及角度缺乏预估可能导致机器人过度远离障碍物，导致路径长度过大。对一些含有陷阱的特殊环境，可能使程序陷入死循环。

发明内容

本发明要解决的技术问题是提供一种多分辨率障碍物的动态环境下移动机器人混合路径规划方法，将全局路径规划和局部动态避障相结合，解决现有方法存在的初始规划存在盲目性、环境建模缺乏灵活适应性、对未知动态或静态障碍物实时避障能力差的问题。

本发明解决以上技术问题采用如下技术方案：

第一步：采用自适应非均匀极径的极坐标建模方法对移动机器人运动空间进行环境建模。

自适应非均匀极径的极坐标建模方法能够根据障碍物的疏密程度灵活设定全局路径规划方法的路径搜索空间，同时相对传统的笛卡儿坐标建模，极坐标系允许通过路径回退逃离某些凹形障碍物，具体步骤为：

1.1在机器人运动空间内建立笛卡尔坐标系oxy，将机器人和预先探明的障碍物信息在笛卡尔坐标系oxy上标注，包括机器人起始位置S、目标点位置G以及各障碍物的顶点位置，从键盘接收已知的障碍物总数目ob_num。

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对所得的极径序列作以下处理：设置相邻的初始全局路径点之间允许的最大差值d_max和最小差值d_min。d_max根据L和最少需要的全局路径点个数来定，d_max为L/(q+1)，q为最少需要的全局路径点个数，通常设2≤q≤5。d_min根据L和最多需要的全局路径点个数来定，d_min为L/(p+1)，p为最多需要的全局路径点个数，通常设20≤p≤50。若极径序列中相邻两极径的差值大于D_max，则在极径序列中这两相邻极径之间插入两相邻极径的平均值；若极径序列中两相邻极径的差值小于D_min，则在极径序列中删除两相邻极径，在序列中插入两相邻极径的平均值，得到新的极径序列D＝[d₁,d₂,...,d_n]，n为最终确定的极径个数。

第二步：采用重力粒子群搜索方法规划出从起点到终点的初始全局路径，机器人沿此初始全局路径边行走边进行局部动态避障。

相比孙波等2005年在“控制与决策”期刊第20卷第9期中“基于粒子群优化算法的移动机器人全局路径规划”一文求解路径规划的粒子群算法，重力粒子群搜索算法采用碰撞约束和路径长度优化目标相区分的支配关系评价路径的优劣，能够快速在可行解范围内展开搜索，同时采用重力吸引的思想充分利用计算的路径评价值的定量信息，提高粒子群算法收敛速度和搜索精度。重力粒子群搜索方法步骤如下：

2.1初始化重力粒子群算法参数：

2.1.1设置重力粒子群算法的参数：包括粒子群的粒子数目N，一般取值为30到200之间的整数。最大迭代代数Iter，一般取500到1000之间的整数。学习因子c1、c2，通常均设置为2。加速度系数c3，通常设为1。初始化当前代数t为0，惯性因子w为0.9。

2.1.2初始化粒子群中的每个粒子。粒子群表示粒子1,2,3,...,N的集合。每个粒子表示一个全局路径点序列，由全局路径点的极角序列和极径序列组成。粒子的维数，即所表示的初始全局路径点的个数初始化为n。粒子k(k＝1,2,…,N)的极径序列编码为D_k＝[d^k ₁,...,d^k _i...,d^k _n]，d^k _i表示粒子k中第i个全局路径点的极径，取值范围为(0,L)。D₁,D₂,...,D_N均初始化为D。粒子k的极角序列编码为X_k＝[θ^k ₁,...,θ^k _i...,θ^k _n]。θ^k _i表示粒子k中第i个全局路径点的极角，取值范围为X₁，X₂，...，X_N中的每个元素均初始化为0。粒子1,2,3,...,N的个体历史最优极角序列pbest₁,pbest₂,...,pbest_N初始化为各自的极角序列X₁,X₂,...,X_N，个体历史最优的路径长度f_length(pbest₁),f_length(pbest₂),...,f_length(pbest_N)均初始化为+∞，个体历史最优的路径碰撞约束co(pbest₁),co(pbest₂),...,co(pbest_N)均初始化1，粒子群的全局最优极角序列gbest初始化为pbest₁，gbest的路径长度f_length(gbest)初始化为+∞，gbest的碰撞约束co(gbest)初始化为1。粒子k的速度序列编码为V_k＝[v^k ₁,..,v^k _i,..,v^k _n]。v^k _i表示粒子k的第i个全局路径点的极角的变化速度，取值范围为对V₁,V₂,...,V_N进行初始化，方法是：

2.1.2.1初始化k＝1，i＝1。

2.1.2.2设置v^k _i为区间内的随机数。

2.1.2.3令i＝i+1，若i<＝n，返回2.1.2.2，否则执行步骤2.1.2.4。

2.1.2.4令k＝k+1，i＝1，若k<＝N，返回2.1.2.2，否则执行步骤2.2。

2.2计算粒子1,2,3,...,N所表示的路径的评价值，更新pbest₁,pbest₂,...,pbest_N和gbest。

路径的评价值采用碰撞约束和路径长度优化目标相区分的支配关系来计算，能够快速引导粒子在可行解范围内展开搜索。具体步骤为：

2.2.1初始化k＝1。

2.2.2计算粒子k的X_k的路径长度f_length(X_k)：

$f_{length}\left(X_k\right)=\underset{i=0}{\overset{n}{\&Sigma;}}\sqrt{{\left({d^k}_{i+1}{{\mathrm{cos\&theta;}}^k}_{i+1}-{d^k}_i{{\mathrm{cos\&theta;}}^k}_i\right)}^2+{\left({d^k}_{i+1}{{\mathrm{sin\&theta;}}^k}_{i+1}-{d^k}_i{{\mathrm{sin\&theta;}}^k}_i\right)}^2}---\left(1\right)$

由于所有粒子的极径序列相同，因此f_length(X_k)完全由X_k决定。

2.2.3计算粒子k的X_k的碰撞约束程度co(X_k)：

$co\left(X_k\right)=\frac{1}{{\mathrm{ob}}_{num}}\underset{j=1}{\overset{{\mathrm{ob}}_{num}}{\&Sigma;}}{\mathrm{co}}_j\left(X_k\right)---\left(2\right)$

其中

2.2.4计算粒子k的X_k和pbest_k之间的支配关系，计算方法为：

如果满足下列两个条件之一，则认为X_k支配pbest_k。

a)co(X_k)＜co(pbest_k)

b)co(X_k)＝co(pbest_k)andf_length(X_k)＜f_length(pbest_k)

若X_k支配pbest_k，表示X_k所表示的路径优于pbest_k所表示的路径，则令pbest_k＝X_k，f_length(pbest_k)＝f_length(X_k)，co(pbest_k)＝co(X_k)，执行步骤2.2.5。若X_k不支配pbest_k，则执行步骤2.2.6。

2.2.5计算粒子k的pbest_k和粒子群的gbest之间的支配关系。计算方法为：

如果满足下列两个条件之一，则认为pbest_k支配gbest。

a)co(pbest_k)＜co(gbest)

b)co(pbest_k)＝co(gbest)andf_length(pbest_k)＜f_length(gbest)；

若pbest_k不支配gbest，则直接执行步骤2.2.6；否则令gbest＝pbest_k，f_length(gbest)＝f_length(pbest_k)，co(gbest)＝co(pbest_k)，执行步骤2.2.6。

2.2.6令k＝k+1，若k<＝N，返回2.2.2，否则表明这一代所有粒子以及个体历史最优、粒子群的全局最优的评价值计算完成，执行步骤2.3。

2.3计算粒子群中每个粒子受到其他粒子的引力重力加速度。

传统粒子群算法由于对自身经验的记忆和社会信息的共享运用，因此收敛快，但计算的评价信息只是用于定性比较粒子的优劣，还存在早熟精度低的缺点。采用重力吸引的思想充分利用计算的路径评价值的定量信息，精确量化粒子受到其他粒子的作用力，能够提高搜索的精度和速度。具体步骤为：

2.3.1初始化k＝1。

2.3.2计算归一化的粒子k的惯性质量M_k：

$M_k=\frac{m_k}{\underset{j=1}{\overset{N}{\&Sigma;}}{m}_j}---\left(3\right)$

其中粒子k的惯性质量

$m_k=\frac{f_{length}\left(X_k\right)-worst}{best-worst},best=\underset{k\&Element;\{1,2,\mathrm{...}N\}}{min}{f}_{length}\left(X_k\right),worst=\underset{k\&Element;\{1,2,...N\}}{max}{f}_{length}\left(X_k\right)$

best和worst分别表示粒子群所有粒子所表示的最短路径长度和最长路径长度。粒子表示的路径长度越短，则惯性质量越大，表示它越接近最短路径解。那么该粒子对其他粒子有更大的吸引力，并且自身移动速度也就越慢，在最短路径解附近进行精确搜索。

2.3.3计算粒子k受到粒子群中其他粒子的吸引力大小FP_k：

${\mathrm{FP}}_k=\underset{j=1,j\&NotEqual;k}{\overset{N}{\&Sigma;}}rand*{\mathrm{FP}}_{kj},{\mathrm{FP}}_{kj}=G\left(t\right)M_k{M}_j{n}_{kj}---\left(4\right)$

其中FP_kj表示粒子k受到粒子j的吸引力向量。重力因子t为当前代数，一般设参数G₀＝100，α＝20。n_kj表示粒子j对粒子k的作用力方向的单位向量。若M_k＜M_j，则粒子j对粒子k产生吸引力，n_kj由粒子k位置指向粒子j位置，反之粒子j对粒子k产生排斥力，n_kj由粒子j位置指向粒子k位置。FP_k表示除自身外所有粒子对粒子k产生的合力向量，rand是(0,1]之间的随机数。

2.3.4计算粒子k受其他粒子作用力产生的加速度

2.3.5令k＝k+1，若k<＝N，返回2.3.2，否则表明计算完粒子群所有粒子的重力加速度，执行步骤2.4。

2.4更新粒子群中每个粒子的位置和速度。具体步骤为：

2.4.1初始化k＝1。

2.4.2更新粒子k的位置和速度：

$\begin{array}{c}{V}_k=V_k*w+c1*r1*\left({\mathrm{pbest}}_k-X_k\right)+c2*r2*\left(gbest-X_k\right)+c3*a_k\\ X_k=X_k+V_k\end{array}---\left(5\right)$

其中r1、r2为[0,1)之间的随机数，W_max＝0.9，W_min＝0.4。

随着迭代更新，粒子k的所代表的路径会逐渐向最优路径调整直至收敛。

2.4.3令k＝k+1，若k<＝N，返回2.4.1，否则执行步骤2.5。

2.5令t＝t+1，若t<＝Iter，则执行步骤2.2；否则输出全局最优极角序列gbest，作为初始全局路径，执行第三步。

第三步：根据初始全局路径，从当前位置出发，设置下一个初始全局路径点为子目标点，采用改进的人工势场法进行局部动态避障，到达子目标点。具体步骤为：

3.1初始化参数。若机器人当前位置不是初始位置S，则执行3.2，否则初始化以下参数：机器人的质量为m，行走一个步长的时间为T，机器人传感器局部感知范围为ρ₀，机器人允许的最大速度大小为v_max。设置目标点对机器人的引力系数α_p，一般设为50到200之间任意实数。障碍物对机器人的斥力系数ε，一般设为500到2000之间任意实数。机器人与障碍物之间最小安全距离为λ，其为机器人的膨化半径、包络障碍物的最小外接圆半径和机器人与障碍物之间允许的最小安全距离的总和，一般设为0.5m到1.0m之间的实数。初始化机器人速度V为0。

3.2机器人采用传感器感知到的障碍物信息更新障碍物及机器人相对于障碍物的位置和速度信息。

3.3计算机器人受到当前子目标点的虚拟引力和感知到障碍物的虚拟排斥力，根据虚拟引力和虚拟排斥力的虚拟合力计算赋给机器人的加速度和速度。具体步骤为：

3.3.1计算当前时刻要到达的子目标点对机器人产生的虚拟引力：

$F_{att}=\left\{\begin{array}{cc}{\mathrm{\&alpha;}}_p{\mathrm{\&rho;}}_g{n}_{RG}& {\mathrm{\&rho;}}_g<{\mathrm{\&rho;}}_0\\ {\mathrm{\&alpha;}}_p{\mathrm{\&rho;}}_0{n}_{RG}& {\mathrm{\&rho;}}_g\&GreaterEqual;{\mathrm{\&rho;}}_0\end{array}\right.---\left(6\right)$

其中，F_att表示机器人受到子目标点的虚拟引力，ρ_g为机器人与子目标点的相对距离，n_RG表示由机器人质心指向子目标点的单位向量，α_p为引力系数，ρ₀为机器人感知范围。虚拟引力驱使机器人向子目标方向运动，若目标在机器人感知范围内，虚拟引力大小与机器人与目标的相对距离成正比，到达目标处为0。

3.3.2计算当前时刻机器人感知到的障碍物对机器人的虚拟排斥力：

$F_{rep1}=-\mathrm{\&epsiv;}\left(\right(\frac{1}{\mathrm{\&rho;}-{\mathrm{\&rho;}}_m}-\frac{1}{{\mathrm{\&rho;}}_0})e^{{\mathrm{\&beta;}}_m-\mathrm{\&beta;}}(\frac{{\mathrm{\&rho;}}_m}{{(\mathrm{\&rho;}-\mathrm{\&lambda;})}^2}+\frac{sin\mathrm{\&beta;}}{\left|\right|RV\left|\right|})+\frac{1}{{(\mathrm{\&rho;}-{\mathrm{\&rho;}}_m)}^2}\left(e^{{\mathrm{\&beta;}}_m-\mathrm{\&beta;}}-1\right))\frac{{{\mathrm{\&rho;}}_g}^2}{2}{n}_{RO}---\left(8\right)$

$F_{rep2}=-\mathrm{\&epsiv;}(\frac{1}{\mathrm{\&rho;}-{\mathrm{\&rho;}}_m}-\frac{1}{{\mathrm{\&rho;}}_0})e^{{\mathrm{\&beta;}}_m-\mathrm{\&beta;}}\frac{{{\mathrm{\&rho;}}_g}^2}{2}\frac{cos\mathrm{\&beta;}}{\left|\right|RV\left|\right|}{n}_{Ro\&perp;}---\left(9\right)$

$F_{rep3}=\mathrm{\&epsiv;}(e^{({\mathrm{\&beta;}}_m-\mathrm{\&beta;})}-1)(\frac{1}{\mathrm{\&rho;}-{\mathrm{\&rho;}}_m}-\frac{1}{{\mathrm{\&rho;}}_0}){\mathrm{\&rho;}}_g{n}_{RG}---\left(10\right)$

其中公式(7)中的F_rep(RΡ,RV)综合表示障碍物对机器人的排斥力，其与机器人与障碍物的相对位置RΡ和相对速度RV有关。ρ表示障碍物相对机器人的距离，ρ_m＝λ+T·RV·n_RO，表示综合考虑最小安全距离λ和时间T内障碍物相对机器人运动距离T·RV·n_RO的避障最小安全距离。n_RO表示从机器人指向障碍物的方向的单位向量。β为障碍物相对机器人运动方向与n_RO方向的夹角，β_m为根据RΡ和RV预测的机器人能够安全躲避障碍物的运动方向与机器人到障碍物连线方向的最小夹角，称为安全避碰角，取值为当|β|＝β_m时，机器人与障碍物擦肩而过，而|β|＞β_m时，按照当前的趋势机器人不会与障碍物发生碰撞。条件|β|＜β_m表示排斥力只有在预测的不安全夹角内才起作用，0＜ρ-ρ_m＜ρ₀表示排斥力只有在感知范围内才起作用。F_rep1表示沿障碍物中心指向机器人方向的排斥力分力，F_rep2表示机器人侧向绕行障碍物的分力，F_rep3表示指向目标点的分力。n_RO⊥表示垂直于机器人与障碍物的连线方向的单位向量，n_RG表示从机器人指向目标点的单位向量。

由公式(8)可以看到，分力F_rep1使机器人远离障碍物，大小与相对距离大小ρ成反比，与相对速度大小||RV||成正比，即碰撞趋势越明显该方向阻碍作用越大；公式(9)表明分力F_rep2给予机器人侧向绕行障碍物的动力，大小与机器人相对障碍物运动速度RV在n_RO方向的分量成正比，与相对速度RV与n_RO夹角成反比，表明绕行越容易该力越小，不仅避免过度偏离优化路径，还能很大程度消除传统人工势场法局部极小的产生。公式(10)中分力F_rep3沿机器人指向目标方向，可将其与引力合并。

3.3.3计算机器人受到的目标位置虚拟引力和感知范围内的障碍物虚拟斥力的合力F_all，更新需要赋给机器人的速度V：

F_all＝F_att+F_rep(11)

V＝V+T·F_all/m(12)

若公式(12)所计算的速度大小||V||大于v_max，则将V替换为V/||V||·v_max。

3.4移动机器人按照更新后的速度V行走一个步长时间T，若到达子目标点位置则执行第四步，否则继续执行3.2。

第四步：判断移动机器人到达的子目标点是否为目标点位置G，是则输出最终的全局无碰路径，整个算法结束，否则执行第三步。

采用本发明可以达到以下技术效果：

1.本发明第二步采用重力粒子群搜索方法规划出从起点到终点的初始全局路径，再基于该初始全局路径，在第三步采用改进的人工势场法进行局部动态避障。这种混合的双层规划结构，能够有效利用预先获知的环境信息为机器人提供一个全局导向，改善复杂动态环境中初始规划的盲目性。

2.第一步提出的自适应非均匀极径的极坐标建模方法，能够根据环境的障碍物分布灵活设置搜索空间，降低了粒子群算法的计算代价，提高了算法的灵活性。同时极坐标建模允许通过路径回退逃离某些凹形障碍物。

3.第二步提出的重力粒子群全局搜索算法在2.2中路径的评价值采用碰撞约束和路径长度优化目标相区分的支配关系来计算，能够快速引导粒子在可行解范围内展开搜索。2.3中采用重力吸引的思想充分利用计算的路径评价值的定量信息，精确量化粒子受到其他粒子的作用力，能够提高搜索的精度和速度。

4.第三步所提出的改进人工势场法能够针对实时感知到的静态、动态障碍物进行动态避障，同时在构造的虚拟斥力中通过预估安全避障角，防止过度远离障碍物导致路径长度过大。虚拟斥力中的侧向分力能很大程度消除传统动态避障中的程序陷入死循环问题。

附图说明

图1是本发明总体流程图。

图2是本发明第一步采用自适应非均匀半径方法建立的极坐标环境图。

图3是本发明第二步采用重力粒子群算法进行全局路径规划的流程图。

图4是本发明2.3步计算粒子群中每个粒子受到其他粒子的引力重力加速度的流程图。

图5是本发明第三步采用改进的人工势场法进行局部动态避障的流程图。

图6是本发明3.3.2步机器人对感知障碍物产生虚拟排斥力的实时避障的环境图。

具体实施方式

图1是本发明总体流程图。本发明包括以下四步：

第一步：采用自适应非均匀极径的极坐标建模方法对移动机器人运动空间进行环境建模。

第二步：采用重力粒子群搜索方法规划出从起点到终点的初始全局路径，机器人沿此初始全局路径边行走边进行局部动态避障。

第三步：根据初始全局路径，从当前位置出发，设置下一个初始全局路径点为子目标点，采用改进的人工势场法进行局部动态避障，到达子目标点。

第四步：判断移动机器人到达的子目标点是否为目标点位置G，是则输出最终的全局无碰路径，整个算法结束，否则执行第三步。

图2是本发明第一步采用自适应非均匀半径方法建立的极坐标环境图。

如图2所示，oxy为机器人运动空间的笛卡尔坐标系。S为极坐标的极点，SG为极坐标的极轴，p₁为极坐标中一点，p₁到S的距离为p₁的极径，Sp₁与极轴的夹角θ为p₁的极角，极角的正方向为逆时针方向。环境中预知的障碍物数目ob_num为5，障碍物o₁的顶点分别为o₁₁、o₁₂、o₁₃、o₁₄，o₂的顶点分别为o₂₁、o₂₂、o₂₃，其他障碍物编码依次类推。sr₁、sr₂、sr₃、sr₄、sr₅为按照角度增量δ＝30所引出的极角分别为0,±30,±60度的m条向量，m为5。Sp₁、Sp₂、Sp₃、Sp₄、Sp₅、Sp₆为按照1.3步操作最终所确定的由小到大排列的n条极径，n为6。则所得极径序列为D＝[sp₁,sp₂,sp₃,sp₄,sp₅,sp₆]。

图3是本发明第二步采用重力粒子群算法进行全局路径规划的流程图。包括以下五步：

2.1初始化重力粒子群算法参数。

2.2计算粒子1,2,3,...,N所表示的路径的评价值，更新pbest₁,pbest₂,...,pbest_N和gbest。

2.3计算粒子群中每个粒子受到其他粒子的引力重力加速度。

2.4更新粒子群中每个粒子的位置和速度。

2.5令t＝t+1，若t<＝Iter，则执行步骤2.2；否则输出全局最优极角序列gbest，作为初始全局路径，执行第三步。

图4是本发明2.3步计算粒子群中每个粒子受到其他粒子的引力重力加速度的流程图。包括以下五步：

2.3.1初始化k＝1。

2.3.2计算归一化的粒子k的惯性质量。

2.3.3计算粒子k受到粒子群中其他粒子的吸引力大小。

2.3.4计算粒子k受其他粒子作用力产生的加速度。

2.3.5令k＝k+1，若k<＝N，返回2.3.2，否则表明计算完粒子群所有粒子的重力加速度，执行步骤2.4。

图5是本发明第三步采用改进的人工势场法进行局部动态避障的流程图。包括以下四步：

3.1初始化参数。

3.2机器人采用传感器感知到的障碍物信息更新障碍物及机器人相对于障碍物的位置和速度信息。

3.3计算机器人受到当前子目标点的虚拟引力和感知到障碍物的虚拟排斥力，根据虚拟引力和虚拟排斥力的虚拟合力计算赋给机器人的加速度和速度。

3.4移动机器人按照更新后的速度V行走一个步长时间T，若到达子目标点位置则第四步，否则继续执行3.2。

图6是本发明3.3.2步机器人对感知障碍物产生虚拟排斥力的实时避障的环境图。

如图6所示，R表示移动机器人，obs表示实时感知到的某一动态障碍物，RV表示机器人相对于障碍物的速度。ρ表示障碍物中心相对机器人中心的距离，λ表示允许的障碍物与机器人之间的最小安全距离，ρ_m＝λ+T·RV·n_RO，表示综合考虑最小安全距离λ和时间T内障碍物相对机器人运动距离T·RV·n_RO的避障最小安全距离。n_RO表示从机器人指向障碍物的方向的单位向量。β为机器人相对障碍物运动方向与n_RO方向的夹角，β_m为根据RΡ和RV预测的机器人能够安全躲避障碍物的运动方向与机器人到障碍物连线方向的最小夹角，称为安全避碰角，取值为表示垂直于机器人与障碍物的连线方向的单位向量。

Claims (7)

1.一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于包括以下步骤：

第一步：采用自适应非均匀极径的极坐标建模方法对移动机器人运动空间进行环境建模，方法是:

1.1.在机器人运动空间内建立笛卡尔坐标系oxy，将机器人和预先探明的障碍物信息在笛卡尔坐标系oxy上标注，包括机器人起始位置S、目标点位置G以及各障碍物的顶点位置，从键盘接收已知的障碍物总数ob_num；

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.对所得的极径序列作以下处理：设置相邻的初始全局路径点之间允许的最大差值d_max和最小差值d_min，d_max根据L和最少需要的全局路径点个数来定，d_max为L/(q+1)，q为最少需要的全局路径点个数；d_min根据L和最多需要的全局路径点个数来定，d_min为L/(p+1)，p为最多需要的全局路径点个数；若极径序列中相邻两极径的差值大于D_max，则在极径序列中这两相邻极径之间插入两相邻极径的平均值；若极径序列中两相邻极径的差值小于D_min，则在极径序列中删除两相邻极径，在序列中插入两相邻极径的平均值，得到新的极径序列D＝[d₁,d₂,...,d_n]，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的极径序列编码为D_k＝[d^k ₁,...,d^k _i...,d^k _n]，k＝1,2,…,N，d^k _i表示粒子k中第i个全局路径点的极径，取值范围为(0,L)；D₁,D₂,...,D_N均初始化为D；粒子k的极角序列编码为X_k＝[θ^k ₁,...,θ^k _i,...,θ^k _n]，θ^k _i表示粒子k中第i个全局路径点的极角，取值范围为X₁,X₂,...,X_N中的每个元素均初始化为0；粒子1,2,3,...,N的个体历史最优极角序列pbest₁,pbest₂,...,pbest_N初始化为各自的极角序列X₁,X₂,...,X_N，个体历史最优的路径长度f_length(pbest₁),f_length(pbest₂),...,f_length(pbest_N)均初始化为+∞，个体历史最优的路径碰撞约束co(pbest₁),co(pbest₂),...,co(pbest_N)均初始化1，粒子群的全局最优极角序列gbest初始化为pbest₁，gbest的路径长度f_length(gbest)初始化为+∞，gbest的碰撞约束co(gbest)初始化为1；粒子k的速度序列编码为V_k＝[v^k ₁,..,v^k _i,..,v^k _n]，v^k _i表示粒子k的第i个全局路径点的极角的变化速度，取值范围为对V₁,V₂,...,V_N进行初始化；

2.2.计算粒子1,2,3,...,N所表示的路径的评价值，更新pbest₁,pbest₂,...,pbest_N和gbest：

2.2.1.初始化k＝1；

2.2.2.计算粒子k的X_k的路径长度f_length(X_k)：

$f_{length}\left(X_k\right)=\underset{i=0}{\overset{n}{\&Sigma;}}\sqrt{{({d^k}_{i+1}{{\mathrm{cos\&theta;}}^k}_{i+1}-{d^k}_i{{\mathrm{cos\&theta;}}^k}_i)}^2+{({d^k}_{i+1}{{\mathrm{sin\&theta;}}^k}_{i+1}-{d^k}_i{{\mathrm{sin\&theta;}}^k}_i)}^2}---\left(1\right)$

2.2.3.计算粒子k的X_k的碰撞约束程度co(X_k)：

$co\left(X_k\right)=\frac{1}{{\mathrm{ob}}_{num}}\underset{j=1}{\overset{{\mathrm{ob}}_{num}}{\&Sigma;}}{\mathrm{co}}_j\left(X_k\right)---\left(2\right)$

其中

2.2.4.计算粒子k的X_k和pbest_k之间的支配关系，计算方法为：

如果满足下列两个条件之一，则X_k支配pbest_k：

a)co(X_k)＜co(pbest_k)

b)co(X_k)＝co(pbest_k)andf_length(X_k)＜f_length(pbest_k)

若X_k支配pbest_k，表示X_k所表示的路径优于pbest_k所表示的路径，则令pbest_k＝X_k，f_length(pbest_k)＝f_length(X_k)，co(pbest_k)＝co(X_k)，执行步骤2.2.5；若X_k不支配pbest_k，则执行步骤2.2.6；

2.2.5.计算粒子k的pbest_k和粒子群的gbest之间的支配关系，计算方法为：

如果满足下列两个条件之一，则pbest_k支配gbest：

a)co(pbest_k)＜co(gbest)

b)co(pbest_k)＝co(gbest)andf_length(pbest_k)＜f_length(gbest)；

若pbest_k不支配gbest,则直接执行步骤2.2.6；否则令gbest＝pbest_k，

f_length(gbest)＝f_length(pbest_k)，co(gbest)＝co(pbest_k)，执行步骤2.2.6；

2.2.6.令k＝k+1，若k<＝N，返回2.2.2，否则表明这一代所有粒子以及个体历史最优、粒子群的全局最优的评价值计算完成，执行步骤2.3；

2.3.计算粒子群中每个粒子受到其他粒子的作用力产生的加速度，得到粒子1,2,...,k,...,N受其他粒子作用力产生的加速度a₁,a₂,...,a_k,...,a_N；

2.4.更新粒子群中每个粒子的位置和速度，方法为：

2.4.1.初始化k＝1；

2.4.2.更新粒子k的位置和速度：

$\begin{array}{c}{V}_k=V_k*w+c1*r1*\left({\mathrm{pbest}}_k-X_k\right)+c2*r2\left(gbest-X_k\right)+c3*a_k\\ X_k=X_k+V_k\end{array}---\left(5\right)$

其中r1、r2为[0,1)之间的随机数，W_max＝0.9，W_min＝0.4；

2.4.3令k＝k+1，若k<＝N，返回2.4.1，否则执行步骤2.5；

2.5.令t＝t+1，若t<＝Iter，则执行步骤2.2；否则输出全局最优极角序列gbest，作为初始全局路径，执行第三步；

第三步：根据初始全局路径，从当前位置出发，设置下一个初始全局路径点为子目标点，采用改进的人工势场法进行局部动态避障，到达子目标点，具体步骤为：

3.1.初始化参数：若机器人当前位置不是初始位置S，则执行3.2，否则初始化以下参数：机器人的质量为m，行走一个步长的时间为T，机器人传感器局部感知范围为ρ₀，机器人允许的最大速度大小为v_max；设置目标点对机器人的引力系数α_p，α_p为正实数；障碍物对机器人的斥力系数ε，ε为正实数；机器人与障碍物之间最小安全距离为λ，其为机器人的膨化半径、包络障碍物的最小外接圆半径和机器人与障碍物之间允许的最小安全距离的总和，λ为正实数；初始化机器人速度V为0；

3.2.机器人采用传感器感知到的障碍物信息更新障碍物及机器人相对于障碍物的位置和速度信息；

3.3.计算机器人受到当前子目标点的虚拟引力和感知到障碍物的虚拟排斥力，根据虚拟引力和虚拟排斥力的虚拟合力计算赋给机器人的加速度和速度，具体步骤为：

3.3.1.计算当前时刻要到达的子目标点对机器人产生的虚拟引力：

$F_{att}=\left\{\begin{array}{cc}{\mathrm{\&alpha;}}_p{\mathrm{\&rho;}}_g{n}_{RG}& {\mathrm{\&rho;}}_g<{\mathrm{\&rho;}}_0\\ {\mathrm{\&alpha;}}_p{\mathrm{\&rho;}}_0{n}_{RG}& {\mathrm{\&rho;}}_g\&GreaterEqual;{\mathrm{\&rho;}}_0\end{array}\right.---\left(6\right)$

其中，F_att表示机器人受到子目标点的虚拟引力，ρ_g为机器人与子目标点的相对距离，n_RG表示由机器人质心指向子目标点的单位向量，α_p为引力系数，ρ₀为机器人感知范围；虚拟引力驱使机器人向子目标方向运动，若目标在机器人感知范围内，虚拟引力大小与机器人与目标的相对距离成正比，到达目标处为0；

3.3.2.计算当前时刻机器人感知到的障碍物对机器人的虚拟排斥力：

$F_{rep1}=-\mathrm{\&epsiv;}\left(\right(\frac{1}{\mathrm{\&rho;}-{\mathrm{\&rho;}}_m}-\frac{1}{{\mathrm{\&rho;}}_0}\left)e^{{\mathrm{\&beta;}}_m-\mathrm{\&beta;}}\right(\frac{{\mathrm{\&rho;}}_m}{{(\mathrm{\&rho;}-\mathrm{\&lambda;})}^2}+\frac{sin\mathrm{\&beta;}}{\left|\right|RV\left|\right|})+\frac{1}{{(\mathrm{\&rho;}-{\mathrm{\&rho;}}_m)}^2}(e^{{\mathrm{\&beta;}}_m-\mathrm{\&beta;}}-1\left)\right)\frac{{{\mathrm{\&rho;}}_g}^2}{2}{n}_{RO}---\left(8\right)$

$F_{rep2}=-\mathrm{\&epsiv;}(\frac{1}{\mathrm{\&rho;}-{\mathrm{\&rho;}}_m}-\frac{1}{{\mathrm{\&rho;}}_0})e^{{\mathrm{\&beta;}}_m-\mathrm{\&beta;}}\frac{{{\mathrm{\&rho;}}_g}^2}{2}\frac{cos\mathrm{\&beta;}}{\left|\right|RV\left|\right|}{n}_{RO\&perp;}---\left(9\right)$

$F_{rep3}=\mathrm{\&epsiv;}(e^{({\mathrm{\&beta;}}_m-\mathrm{\&beta;})}-1)(\frac{1}{\mathrm{\&rho;}-{\mathrm{\&rho;}}_m}-\frac{1}{{\mathrm{\&rho;}}_0}){\mathrm{\&rho;}}_g{n}_{RG}---\left(10\right)$

其中公式(7)中的F_rep(RΡ,RV)综合表示障碍物对机器人的排斥力，其与机器人与障碍物的相对位置RΡ和相对速度RV有关；ρ表示障碍物相对机器人的距离，ρ_m＝λ+T·RV·n_RO，表示综合考虑最小安全距离λ和时间T内障碍物相对机器人运动距离T·RV·n_RO的避障最小安全距离；n_RO表示从机器人指向障碍物的方向的单位向量；β为障碍物相对机器人运动方向与n_RO方向的夹角，β_m为根据RΡ和RV预测的机器人能够安全躲避障碍物的运动方向与机器人到障碍物连线方向的最小夹角，称为安全避碰角，取值为当|β|＝β_m时，机器人与障碍物擦肩而过，而|β|＞β_m时，按照当前的趋势机器人不会与障碍物发生碰撞；条件|β|＜β_m表示排斥力只有在预测的不安全夹角内才起作用，0＜ρ-ρ_m＜ρ₀表示排斥力只有在感知范围内才起作用；F_rep1表示沿障碍物中心指向机器人方向的排斥力分力，F_rep2表示机器人侧向绕行障碍物的分力，F_rep3表示指向目标点的分力；n_RO⊥表示垂直于机器人与障碍物的连线方向的单位向量，n_RG表示从机器人指向目标点的单位向量；

3.3.3.计算机器人受到的目标位置虚拟引力和感知范围内的障碍物虚拟斥力的合力，更新需要赋给机器人的速度：

F_all＝F_att+F_rep(11)

V＝V+T·F_all/m(12)

若公式(12)所计算的速度大小||V||大于v_max，则将V替换为V/||V||·v_max；

3.4.移动机器人按照更新后的速度V行走一个步长时间T，若到达子目标点位置则执行第四步，否则继续执行3.2；

第四步：判断移动机器人到达的子目标点是否为目标点位置G，是则输出最终的全局无碰路径，结束；否则执行第三步。

2.如权利要求1所述的一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于所述最少需要的全局路径点个数q满足2≤q≤5；最多需要的全局路径点个数p满足20≤p≤50。

3.如权利要求1所述的一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于所述粒子群的粒子数目N取值为30到200之间的整数；最大迭代代数Iter取500到1000之间的整数；学习因子c1、c2均设置为2；加速度系数c3设为1。

4.如权利要求1所述的一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于2.1.2步所述对V₁,V₂,...,V_N进行初始化的方法是：

2.1.2.1.初始化k＝1，i＝1；

2.1.2.2.设置v^k _i为区间内的随机数；

2.1.2.3.令i＝i+1，若i<＝n，返回2.1.2.2，否则执行步骤2.1.2.4；

2.1.2.4.令k＝k+1，i＝1，若k<＝N，返回2.1.2.2，否则初始化完毕。

5.如权利要求1所述的一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于2.3步计算粒子群中每个粒子受到其他粒子的引力重力加速度的方法为：

2.3.1.初始化k＝1；

2.3.2.计算归一化的粒子k的惯性质量M_k：

$M_k=\frac{m_k}{\underset{j=1}{\overset{N}{\&Sigma;}}{m}_j}---\left(3\right)$

其中粒子k的惯性质量

$m_k=\frac{f_{length}\left(X_k\right)-worst}{best-worst},best=\underset{k\&Element;\{1,2,...N\}}{min}{f}_{length}\left(X_k\right),worst=\underset{k\&Element;\{1,2,...N\}}{max}{f}_{length}\left(X_k\right)$

best和worst分别表示粒子群所有粒子所表示的最短路径长度和最长路径长度；

2.3.3.计算粒子k受到粒子群中其他粒子的吸引力大小FP_k：

${\mathrm{FP}}_k=\underset{j=1,j\&NotEqual;k}{\overset{N}{\&Sigma;}}rand*{\mathrm{FP}}_{kj},{\mathrm{FP}}_{kj}=G\left(t\right)M_k{M}_j{n}_{kj}---\left(4\right)$

其中FP_kj表示粒子k受到粒子j的吸引力向量，重力因子t为当前代数，参数G₀、α为正整数，n_kj表示粒子j对粒子k的作用力方向的单位向量，若M_k＜M_j，则粒子j对粒子k产生吸引力，n_kj由粒子k位置指向粒子j位置，反之粒子j对粒子k产生排斥力，n_kj由粒子j位置指向粒子k位置；FP_k表示除自身外所有粒子对粒子k产生的合力向量，rand是(0,1]之间的随机数；

2.3.4.计算粒子k受其他粒子作用力产生的加速度

2.3.5.令k＝k+1，若k<＝N，返回2.3.2，否则表明计算完粒子群所有粒子的重力加速度。

6.如权利要求5所述的一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于参数G₀＝100，α＝20。

7.如权利要求1所述的一种多分辨率障碍物环境下移动机器人混合路径规划方法，其特征在于目标点对机器人的引力系数α_p设为50到200之间任意实数，障碍物对机器人的斥力系数ε设为500到2000之间任意实数，机器人与障碍物之间最小安全距离为λ设为0.5m到1.0m之间的实数。