CN102129249A - 一种危险源环境下的机器人全局路径规划方法 - Google Patents

Abstract

本发明公布了一种危险源环境下的机器人全局路径规划方法，旨在提供一种可以保障机器人在危险源环境下高效快速完成任务的全局路径规划方法，步骤如下：(1)探测并确定机器人的工作环境信息，包括机器人的起始点和目标点、障碍物的位置和形状，以及危险源的位置；(2)对机器人工作环境进行建模；(3)定义路径的长度和危险程度为评价路径优劣的两个性能指标，即路径规划问题的两个目标函数；(4)利用改进的多目标粒子群优化算法对步骤3所确定的两个目标函数进行全局优化，得到一组Pareto最优路径集；(5)采用一个模糊隶属度函数模拟决策者对任务的偏好，并从Pareto最优路径集中选出一个满意的折中解作为机器人移动的最终路径。

Description

一种危险源环境下的机器人全局路径规划方法

技术领域

本发明涉及一种机器人全局路径规划方法，尤其是危险源环境下的机器人全局路径规划方法。

背景技术

机器人路径规划的任务就是寻找一条从起始点到目标点的、满足一定性能指标(如路径长度、危险程度或行走时间等)的无碰撞路径。一个好的路径规划方案是保障机器人高效完成任务的前提。

根据机器人对所处环境的掌握状况，已有规划方法可分为基于模型的全局路径规划方法(又称静态路径规划)和基于检测信息的局部路径规划方法(又称在线路径规划)。局部路径规划方法具有很好的实时性，但是无法保证机器人最终到达目标点。

研究可以保证机器人到达目标点的全局路径规划方法，已有技术包括2006年1月出版的《中国工程科学》第1期杂志“移动机器人运动规划研究综述”一文提到的可视图法、拓扑法和栅格法等传统方法，以及中国专利说明书CN1883887(2006年12月27日公布)公开的基于虚拟场景的机器人避障路径规划方法和中国专利说明书CN101769754A(2010年7月7日公布)公开的基于类三维地图的移动机器人全局路径规划方法等新型方法。最近，又出现了利用粒子群来处理传统路径规划问题的方法，包括中国专利说明书CN101604166(2009年12月16日公布)公开的基于粒子群优化算法的移动机器人路径规划方法，以及中国专利说明书CN101387888(2009年3月18日公布)公开的基于二进制量子粒子群算法的移动机器人路径规划方法。上述全局路径规划问题具有如下特点：机器人的工作环境中只包含障碍物，并且仅利用路径长度作为评价路径的单一性能指标。

本发明考虑危险源环境下的机器人全局路径规划方法。在机器人执行某些任务时，经常需要避开一些危及自身安全的物体(称为危险源)，如机器人侦查中的敌人、机器人火灾营救中的燃烧物等。与常说的障碍物不同，这些危险源具有一定的辐射区域。机器人可以穿越它的辐射区域，但是要承担一定的风险。一般距离危险源越近，机器人承担的风险越大。该类问题称为危险源环境下的机器人全局路径规划问题，特点如下：同时包含障碍物和需要机器人回避的危险源，并具有两个相互矛盾的路径评价指标，即路径的长度和危险程度。

传统全局规划方法在处理该类全局路径规划问题时，存在如下缺点：一方面，由于危险源和障碍物之间有着本质的区别，传统处理障碍物的方法无法用于处理危险源；另一方面，传统方法大都利用路径长度作为评价路径的单一性能指标，未考虑危险源对路径的影响，即路径的危险程度这一性能指标。经查相关文献，目前还不存在一个适于危险源环境的机器人全局路径规划方法。

发明内容

本发明所要解决的技术问题：克服现有技术的不足，基于多目标粒子群优化算法提出一种适于危险源环境的高效机器人全局路径规划方法，使机器人及时找到满意的最佳路径。

本发明的技术解决方案：一种基于多目标粒子群优化的移动机器人全局路径规划方法，其特征在于步骤如下：

步骤1：探测并确定机器人的工作环境信息，包括机器人的起始点和目标点、障碍物的位置和形状，以及危险源的位置。

步骤2：对机器人工作环境进行建模

将起始点和目标点之间的线段进行(n+1)等分，并在每个等分点处作垂线，得到平行直线族{l₁，l₂，…，l_n}。标记一条路径与该直线族的交点序列为(ph₁，ph₂，…，ph_n)，并利用该交点序列表示路径的相应坐标。注意：等分参数n根据机器人自身尺寸和障碍物的大小确定。

步骤3：确定用于评价路径的两个性能指标，即规划问题的两个目标函数，路径的长度和危险程度。

(3.1)标记机器人的起始点和目标点分别为ph₀和ph_n+1，由

计算路径PH＝(ph₁，ph₂，…，ph_n)的长度，其中d(ph_j，ph_j+1)表示点ph_j与点ph_j+1间的距离。

(3.2)设危险源DS_i(i＝1，2，…，m)的位置为定义隶属函数

${\mathrm{dan}}_i\left(\mathrm{PH}\right)=\left\{\begin{array}{cc}1,& d\left({\mathrm{PH},\mathrm{DS}}_i\right)\≤d_{\min }^i,\\ \frac{d_{\max }^i-d\left({\mathrm{PH},\mathrm{DS}}_i\right)}{d_{\max }^i-d_{\min }^i},& d_{\max }^i>d(\mathrm{PH},{\mathrm{DS}}_i)>d_{\min }^i,\\ 0,& d\left({\mathrm{PH},\mathrm{DS}}_i\right)\≥d_{\max }^i,\end{array}\right.---\left(1\right)$

计算路径PH相对危险源DS_i的危险程度。其中，d(PH，DS_i)为路径PH与危险源DS_i间的最短距离，便于描述，简称d(PH，DS_i)为路径PH与DS_i间距离；为危险源DS_i的最大辐射半径，d(PH，DS_i)大于这个值时，相对危险源DS_i路径PH是绝对安全的，即危险程度为0；

为危险源DS_i的绝对辐射半径，d(PH，DS_i)小于这个值时，相对危险源DS_i路径PH是绝对危险的，即危险程度为1。

对于危险源个数大于1的情况，利用模糊和计算路径PH相对全部危险源的危险程度。

步骤4：利用改进的多目标粒子群优化算法对步骤(3)所确定的两个目标函数进行全局优化，以得到一组Pareto最优路径，具体方法如下：

(4.1)初始化粒子群，可行储备集和非可行储备，以及粒子的个体最优点。

(4.2)采用步骤3中方法计算每个粒子的适应值，采用路径与障碍物的碰撞次数评价每个粒子违背约束(与障碍物碰撞)的程度。

(4.3)更新每个粒子的个体最优点。对于一个粒子，设置粒子当前时刻所得最好位置作为它的新个体最优点。

(4.4)更新算法的可行储备集和非可行储备集。约束违背程度为0且适应值最优的粒子存入可行储备集；约束违背程度大于0但适应值较好的粒子存入非可行储备集。当可行储备集中元素大于其固有容量N_a时，保留拥挤度最大(所在区域解的分布最稀疏)的N_a个元素；同理，当非可行储备集中元素大于其固有容量N_a′时，保留拥挤度最大的N_a′个元素。

(4.5)从非可行储备集和可行储备集中选择粒子的全局最优点。对于一个粒子

如果生成的随机数r∈[0，1]小于选择概率p_s，则采用规模为2的锦标赛选择方法，从非可行储备集中选择拥挤距离值较大的元素作为它的全局最优点；否则，采用规模为2的锦标赛选择方法，从可行储备集中选择拥挤距离值较大的元素作为它的全局最优点。其中，选择概率为

$p_s=0.5+0.3(1-\frac{2t}{T_{\max }}),---\left(2\right)$

T_max为算法终止代数。

(4.6)基于有限重复采样和一致变异更新每个粒子的位置。首先由

$\left\{\begin{array}{c}{v}_{i,j}(t+1)={\mathrm{wv}}_{i,j}\left(t\right)+c_1{r}_1(p_{i,j}\left(t\right)-x_{i,j}\left(t\right))+c_2{r}_2(g_j\left(t\right)-x_{i,j}\left(t\right))\\ x_{i,j}(t+1)=x_{i,j}\left(t\right)+v_{i,j}(t+1)\end{array}\right.---\left(3\right)$

更新x_i，j的位置。如果由x_i，j-1和x_i，j确定的新生路段与障碍物相碰撞，继续采用上式对x_i，j值进行重新采样，并对更新后的x_i，j值进行一致变异。直到生成一条可行路段或采样次数大于阈值it_max。上式中，

和

分别为当前粒子的个体最优点和全局最优点；c₁和c₂为粒子群算法的学习因子，w为惯性权重，r₁和r₂为[0，1]间的随机数。

(4.7)判断是否满足终止准则。若满足，则终止算法，并输出可行储备集中的Pareto最优解；否则，返回步骤(4.2)。

步骤5：采用一个模糊隶属度函数模拟决策者对任务的偏好，并从Pareto最优解集中选出一个满意度最大的解作为机器人移动的最终路径。

本发明与现有技术相比的优点在于：针对传统全局路径规划方法难于解决的危险源环境下的机器人路径规划问题，本发明给出了一种快速有效的全局路径规划方法，进一步拓宽了机器人的应用范围；另一方面，非可行解、有限重复采样和一致变异等新颖算子的相互协作，不仅改善了所提方法的鲁棒性，而且显著提高了算法对规划问题的处理速度。

附图说明

图1含有危险源的机器人工作环境的建模

图2多目标粒子群优化算法的流程图

图3粒子全局最优点的更新流程图

图4粒子位置的更新流程图

图5针对演示实例本发明方法所得Pareto最优路径集合及其折中路径。

图中标号说明：

图1和图5中ST为机器人的起始点，TA为目标点，多边形物体表示障碍物，星号表示危险源；

图5中虚线为所得Pareto最优路径，实线为机器人最终选择的折中路径。

具体实施方式

下面结合具体附图和实例对本发明的实施方式进行详细说明。

步骤1：探测并确定机器人的工作环境信息。

根据执行任务的要求确定机器人的起始点和目标点的坐标；利用感知系统探测的结果确定起始点和目标点之间的障碍物，并把他们处理为二维空间中的多边形；利用感知系统探测的结果确定危险源的位置。

步骤2：根据上述环境信息，对机器人工作环境进行建模。该步骤的最终目的是将机器人路径表示为用有限个点表示的的序列，以便执行后续优化过程。

本发明采用2005年9月出版的《控制与决策》第9期杂志中“基于粒子群优化算法的移动机器人全局路径规划”公开的方法构建环境模型。该方法具有操作简单且不受障碍物形状影响的优点。如图1所示，在全局坐标系O-XY中，当起始点和目标点所在直线与X轴相交时，进行如下坐标变换：以ST和TA间连线作为X′轴，以垂直于X′且经过ST点的直线为Y′轴。对应的坐标变化公式为，

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\φ}& -\sin \mathrm{\φ}\\ \sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]\·\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}{x}_{\mathrm{ST}}\\ y_{\mathrm{ST}}\end{array}\right]---\left(4\right)$

其中，(x，y)和(x′，y′)分别为同一点在不同坐标系O-XY和ST-X’Y’下的坐标值，φ为坐标轴X与X’之间夹角，(x_ST，y_ST)为坐标系O-XY中ST点的坐标值。

在新坐标系中，将线段ST-TA进行(n+1)等分，并在每个等分点处作垂线，则得到平行直线族{l₁，l₂，…，l_n}。该直线族与任一路径的交点将对应一个点序列(ph₁，ph₂，…，ph_n)。由此，机器人路径规划可进一步表示为寻找如下点集合

PH＝{ST，ph₁，ph₂，…，ph_n，TA}。

对点ph_i的要求是：ph_i为非障碍点，ph_i与相邻点的连线上不存在障碍点。

步骤3：确定用于评价路径性能的两个目标函数，路径的长度和危险程度。

在含有危险源的工作环境中评价一条路径，既要考虑路径的长度，又要考虑路径的安全程度。

(3.1)路径长度的计算

定义ST为ph₀，TA为ph_n+1，路径PH＝(ph₁，ph₂，…，ph_n)的长度可近似为

$L\left(\mathrm{PH}\right)=\underset{j=0}{\overset{n}{\mathrm{\Σ}}}d({\mathrm{ph}}_j,{\mathrm{ph}}_{j+1}),$

其中，d(ph_j，ph_j+1)表示点

与点间的距离。在ST-X′Y′坐标系下，上式可具体为

$L\left(\mathrm{PH}\right)=\underset{j=0}{\overset{n}{\mathrm{\Σ}}}\sqrt{{(x_{{\mathrm{ph}}_j}^{\′}-x_{{\mathrm{ph}}_{j+1}}^{\′})}^2+{(y_{{\mathrm{ph}}_j}^{\′}-y_{{\mathrm{ph}}_{j+1}}^{\′})}^2}$

$=\underset{j=0}{\overset{n}{\mathrm{\Σ}}}\sqrt{{\left(\frac{x_{{\mathrm{ph}}_0}^{\′},x_{{\mathrm{ph}}_{n+1}}^{\′}}{n+1}\right)}^2+{(y_{{\mathrm{ph}}_j}^{\′}-y_{{\mathrm{ph}}_{j+1}}^{\′})}^2}.---\left(5\right)$

(3.2)路径危险程度的计算

设危险源DS_i(i＝1，2，…，m)的位置为

利用隶属函数(1)

${\mathrm{dan}}_i\left(\mathrm{PH}\right)=\left\{\begin{array}{cc}1,& d\left({\mathrm{PH},\mathrm{DS}}_i\right)\≤d_{\min }^i,\\ \frac{d_{\max }^i-d\left({\mathrm{PH},\mathrm{DS}}_i\right)}{d_{\max }^i-d_{\min }^i},& d_{\max }^i>d(\mathrm{PH},{\mathrm{DS}}_i)>d_{\min }^i,\\ 0,& d\left({\mathrm{PH},\mathrm{DS}}_i\right)\≥d_{\max }^i,\end{array}\right.---\left(6\right)$

计算路径PH相对危险源DS_i的危险程度。

对于危险源个数大于1的情况，利用模糊和计算路径PH相对全部危险源的危险程度。例如，当危险源数目为2时，路径PH的危险程度为

$\mathrm{dan}\left(\mathrm{PH}\right)={\mathrm{dan}}_1\⊕{\mathrm{dan}}_2={\mathrm{dan}}_1+{\mathrm{dan}}_2-{\mathrm{dan}}_1{\mathrm{dan}}_2.$

当危险源数目为3时，路径PH的危险程度为

$\mathrm{dan}\left(\mathrm{PH}\right)=({\mathrm{dan}}_1\⊕{\mathrm{dan}}_2)\⊕{\mathrm{dan}}_3=({\mathrm{dan}}_1+{\mathrm{dan}}_2-{\mathrm{dan}}_1{\mathrm{dan}}_2)\⊕{\mathrm{dan}}_3.$

依次类推，可以求得路径PH相对于全部危险源的危险程度。

由此，危险源下机器人路径规划问题描述为如下两目标优化问题：寻找路径

使得

①ph_i∈F，i＝1，2，…，n，其中F表示机器人的有效活动范围；

②与No_ob个障碍物不相碰，OB_j表示第j个障碍物，

③且同时最小化目标向量函数

$\stackrel{\→}{f}\left(\mathrm{PH}\right)=(f_1,f_2)=(L\left(\mathrm{PH}\right),\mathrm{dan}\left(\mathrm{PH}\right)).$

通常，性能指标L(PH)和dan(PH)是相互矛盾的。一条长度较短的路径可能意味着要承受较大的危险；同样，一条安全的路径可能意味着机器人要绕行很长的路程。因此，上述问题不具有使两个性能指标同时最优的唯一解，其最优解为一个可能包含多个元素的Pareto解集。下面就利用多目标粒子群优化算法求得这个最优解集。

步骤4：对步骤(3)所确定的危险源环境下机器人路径规划问题实施多目标粒子群优化算法。

首先，对上述规划问题的决策变量进行编码，以明确粒子所表达的物理意义。路径PH由点集{ph₁，ph₂，…，ph_n}完全决定，点集中任意点ph_i(i＝1，2，…，n)的横坐标值由直线族{l₁，l₂，…，l_n}决定，而直线族{l₁，l₂，…，l_n}又是环境建模时事先确定的。因此，环境建模后，决定{ph₁，ph₂，…，ph_n}取值的只有它们的纵坐标值，即本发明选取作为决策变量，并以决策变量每个分量的输出作为一个编码元素，多个编码元素组成一个粒子。一个粒子代表决策变量的一组取值，一组取值决定了一条机器人全局路径。

接着，设定多目标粒子群算法的详细参数，即更新公式(3)中学习因子和惯性权值、算法终止条件、可行储备集和非可行储备集的容量、重复采样上限it_max。群体规模一般为20～50，可行储备集和非可行储备集容量一般为5～20；选择终止代数T_max作为算法的终止条件，一般取T_max＝100×No_ob。

基于上述准备，然后启动改进的多目标粒子群优化算法。结合图2，下面详细说明多目标粒子群优化算法的具体实施步骤：

(4.1)初始化规粒子群，在给定可行区域中为粒子群中每个粒子随机指定一个初始位置；设置每个粒子个体最优点的初始值为其自身，其中，

为粒子群中第i个粒子；初始化可行储备集和非可行储备集为空集。

(4.2)用步骤3中方法计算每个粒子的适应值，采用路径与障碍物的碰撞次数评价每个粒子违背约束的程度。以粒子

为例，它的约束违背程度为，

$\mathrm{cv}\left({\stackrel{\→}{x}}_i\right)=\frac{1}{\mathrm{No}\_\mathrm{ob}}\underset{j=1}{\overset{\mathrm{No}\_\mathrm{ob}}{\mathrm{\Σ}}}{\mathrm{cv}}_j\left({\stackrel{\→}{x}}_i\right)$

(4.3)更新每个粒子的个体最优点。

定义一种约束支配关系来比较粒子之间的优劣。对于给定粒子

和

如果它们满足如下条件之一：

① $\mathrm{cv}\left({\stackrel{\&RightArrow;}{x}}_1\right)<\mathrm{cv}\left({\stackrel{\&RightArrow;}{x}}_2\right);$

② $\mathrm{cv}\left({\stackrel{\&RightArrow;}{x}}_1\right)=\mathrm{cv}\left({\stackrel{\&RightArrow;}{x}}_2\right),$ $\mathrm{dan}\left({\stackrel{\&RightArrow;}{x}}_1\right)<\mathrm{dan}\left({\stackrel{\&RightArrow;}{x}}_2\right)$ 且 $L\left({\stackrel{\&RightArrow;}{x}}_1\right)\&le;L\left({\stackrel{\&RightArrow;}{x}}_2\right)$

③ $\mathrm{cv}\left({\stackrel{\&RightArrow;}{x}}_1\right)=\mathrm{cv}\left({\stackrel{\&RightArrow;}{x}}_2\right),$ $\mathrm{dan}\left({\stackrel{\&RightArrow;}{x}}_1\right)\&le;\mathrm{dan}\left({\stackrel{\&RightArrow;}{x}}_2\right)$ 且 $L\left({\stackrel{\&RightArrow;}{x}}_1\right)<L\left({\stackrel{\&RightArrow;}{x}}_2\right).$

则称

含约束支配

粒子个体最优点是粒子从初始到目前迭代次数所得到的最好位置，它可以看作是粒子的记忆。采用含约束支配关系更新粒子的个体最优点。设粒子

的个体最优点为第t+1代新生粒子为

具体更新方法如下：当含约束支配

时，粒子个体最优点取当两者相互不支配时，从

和

中随机选择一个作为

否则，

取

(4.4)更新算法的可行储备集和非可行储备集。

在每次迭代时，为更新算法两个储备集，首先把新生粒子划分为可行解和非可行解两类。基于上述划分结果，更新可行储备集和非可行储备集。

a.可行储备集的更新。首先，合并可行储备集中已有元素和粒子群中新生可行解，组成一个新种群；接着，利用Pareto支配关系，选出该种群中互不支配的元素，并将这些元素保存到可行储备集中。如果可行储备集中元素的数目超过其固有容量N_a，则利用期刊2002年4月出版的《IEEE Transactions on Evolutionary Computations》第2期杂志中“A fast and elitistmultiobjective genetic algorithm：NSGA-II”一文公开的拥挤距离方法，计算每个元素的拥挤距离值，并保留分布最稀疏，即具有较大拥挤距离值的N_a个元素。

b.非可行储备集的更新。采用与步骤(a)中类似的方法更新非可行储备集。不同之处在于：在更新非可行储备集时，除路径长度和危险程度外，还利用约束违背程度作为问题的第3性能指标，用来比较不同解间的Pareto支配关系；而在更新可行储备集时，只考虑了路径长度和危险程度。

(4.5)从非可行储备集和可行储备集中，为每个粒子选择其全局最优点。

以粒子

为例，结合图3详细说明粒子全局最优点的选择过程。在算法迭代过程中，首先，判断非可行储备集n_Ar和可行储备集Ar是否为空集。如果非可行储备集为空集(|n_Ar|＝0)，则从可行储备集中选择粒子的全局最优点；如果可行储备集为空集(|Ar|＝0)，则从非可行储备集中选择粒子的全局最优点。如果非可行储备集和可行储备集皆非空集，那么，产生一个随机数r∈[0，1]，并执行下述操作：如果概率值Ps(0≤Ps≤1)大于r，就从非可行储备集中选择该粒子的全局最优点，否则，从可行储备集中选择粒子全局最优点。以此类推，直到为所有粒子选出全局最优点为止。

(4.6)基于有限重复采样和一致变异的更新每个粒子的位置。

以粒子群中第i个粒子

为例，结合图4详细说明粒子位置更新过程的实施方法。设该粒子的个体最优点为

全局最优点为

那么，

位置的更新过程如下：

a.初始化j＝1，iter＝0；

b.利用式(3)更新x_i，j的位置，且iter＝ter+1；

c.如果由x_i，j-1和x_i，j确定的局部路段

与障碍物相碰撞(即Y(x_i，j-1，x_i，j)＝1)，并且iter≤it_max，执行如下操作，

(c1)更新变异概率p_m，p_m＝(iter/it_max)²；

(c2)生成随机数r∈[0，1]，如果p_m≤r，返回步骤b；否则，对x_i，j进行如下变异：

$\left\{\begin{array}{c}\mathrm{range}=[\mathrm{up}\_\lim \left(j\right)-\mathrm{low}\_\lim \left(j\right)]*p_m\\ x_{i,j}=x_{i,j}+N\left(\mathrm{0,1}\right)*\mathrm{range}\end{array}\right.---\left(8\right)$

(c3)如果x_i，j＞up_lim(j)，取x_i，j＝up_lim(j)；

如果x_i，j＜low_lim(j)，取x_i，j＝low_lim(j)；

(c4)如果局部路段

仍于障碍物相碰，则返回步骤b。

d.j＝j+1。如果j＞n，则终止更新过程，输出结果；否则，重置iter＝0，并返回步骤b。

其中，it_max值越大，生成可行路段的概率越大，但是，算法用于判断路径可行性的计算花费越大；up_lim(j)和low_lim(j)分别为决策变量第j维分量的的上下界。

(4.7)判断算法是否达到终止代数T_max。若达到，则终止算法，并输出可行储备集A_fea中的Pareto最优解；否则，返回步骤(4.2)。

步骤5：采用一个模糊隶属度函数模拟决策者对任务的偏好，并从Pareto最优解集中选出一个满意度最大的解作为机器人移动的最终路径。

以可行储备集中最优解

为例，定义它的满意程度为

${\mathrm{\&mu;}}^k=\underset{i=1}{\overset{2}{\mathrm{\&Sigma;}}}{w}_i\&times;{\mathrm{\&mu;}}_i^k$

${\mathrm{\&mu;}}_i^k\left({\stackrel{\&RightArrow;}{x}}_k\right)=\left\{\begin{array}{cc}1,& f_i\left({\stackrel{\&RightArrow;}{x}}_k\right)\&le;f_i^{\min }\\ \frac{f_i^{\max }-f_i\left({\stackrel{\&RightArrow;}{x}}_k\right)}{f_i^{\max }-f_i^{\min }},& f_i^{\min }<f_i\left({\stackrel{\&RightArrow;}{x}}_k\right)<f_i^{\max }\\ 0,& f_i\left({\stackrel{\&RightArrow;}{x}}_k\right)\&GreaterEqual;f_i^{\max }\end{array}\right.---\left(9\right)$

其中，μ^k为反映决策者偏好的模糊隶属度函数；模糊隶属度函数模拟决策者对任务的偏好

和

分别是关于第i个目标函数的最大值和最小值，w_i(i＝1，2)用来反映决策者对路径长度和安全程度的偏重；|A_fea|为可行储备集中元素的数目。

可行储备集A_fea中具有最大μ值的元素即为所求折中解。

以战场中侦查机器人的路径规划问题为例进一步说明本发明的具体实现过程。

假设机器人正在50×50的正方形环境中执行侦察任务。该环境包括5个静态障碍物和1个哨兵。要求机器人从起始点ST安全快速的到达目标点TA。本发明全局路径规划方法的具体实现如下：

步骤1：探测并确定机器人的工作环境信息。

确定机器人起始点为(6.8，30.4)，目标点为(46.5，28.8)。1个危险源(哨兵)的位置分别为(21.0，21.8)，5个规范化后的障碍物的顶点坐标(由左到右)分别为，其中OB₁和OB₅分别为U型和V型陷阱障碍物。图5展示了该实例所对应环境图形。

${\mathrm{OB}}_1={\left[\begin{array}{cccccccc}13.6& 21.6& 21.6& 13.5& 13.6& 20.1& 20.1& 13.5\\ 40.0& 40.0& 25.1& 25.2& 27.4& 27.3& 38.2& 38.0\end{array}\right]}^{\&prime;},$

${\mathrm{OB}}_2=\left[\begin{array}{cc}26.5& 48.1\\ 27.0& 35.1\\ 28.9& 35.1\\ 29.1& 48.2\end{array}\right],$ ${\mathrm{OB}}_3=\left[\begin{array}{cc}26.8& 32.6\\ 26.3& 21.7\\ 28.7& 21.6\\ 29.3& 32.4\end{array}\right],$ ${\mathrm{OB}}_4=\left[\begin{array}{cc}26.1& 17.8\\ 25.5& 6.95\\ 28.8& 6.76\\ 28.7& 17.5\end{array}\right],$

${\mathrm{OB}}_5={\left[\begin{array}{cccccc}47.9& 38.7& 38.3& 40.0& 40.2& 48.0\\ 37.2& 37.1& 24.9& 25.0& 35.2& 35.2\end{array}\right]}^{\&prime;}.$

步骤2：根据上述环境信息，对机器人工作环境进行建模。

将线段ST-TA 10等分，确定直线族{l₁，l₂，…，l₁₀}，并利用下述序列表示相应路径的具体坐标：

PH＝{ST，ph₁，ph₂，…，ph₁₀，TA}。

步骤3：确定用于评价路径的两个性能指标，路径的长度和危险程度。

设定每个危险源的最大作用半径d(DS)_max＝10，最小作用半径d(DS)_min＝2，那么，路径的长度和危险程度的计算公式如下：

$\left\{\begin{array}{c}L\left(\mathrm{PH}\right)=\underset{j=0}{\overset{10}{\mathrm{\&Sigma;}}}\sqrt{{\left(\frac{d({\mathrm{ph}}_0,{\mathrm{ph}}_{n+1})}{11}\right)}^2+{({y^{\&prime;}}_{{\mathrm{ph}}_j}-{y^{\&prime;}}_{{\mathrm{ph}}_{j+1}})}^2}=\underset{j=0}{\overset{10}{\mathrm{\&Sigma;}}}\sqrt{13.3+{({y^{\&prime;}}_{{\mathrm{ph}}_j}-{y^{\&prime;}}_{{\mathrm{ph}}_{j+1}})}^2}\\ \stackrel{\&OverBar;}{\mathrm{dan}}\left(\mathrm{PH}\right)=({\mathrm{dan}}_1+{\mathrm{dan}}_2-{\mathrm{dan}}_1{\mathrm{dan}}_2)\&CirclePlus;{\mathrm{dan}}_3\end{array}\right..$

其中，

${\mathrm{dan}}_i\left(\mathrm{PH}\right)=\left\{\begin{array}{cc}1,& d(\mathrm{PH},{\mathrm{DS}}_i)\&le;2,\\ \frac{10-d\left(D{S}_i\right)}{10-2},& 10>d(\mathrm{PH},{\mathrm{DS}}_i)>2,\\ 0,& d(\mathrm{PH},{\mathrm{DS}}_i)\&GreaterEqual;10.\end{array}\right.i=\mathrm{1,2,3}.$

步骤4：对步骤(3)所确定的危险源不确定下机器人路径规划问题实施多目标粒子群优化算法。

a.对问题的决策变量进行编码。取序列{ph₁，ph₂，…，ph₁₀}的纵坐标值

作为决策变量，并以决策变量每个分量的输出作为一个编码元素，多个编码元素组成一个粒子。

b.设定多目标粒子群算法的详细参数。更新公式(3)中学习因子c₁＝c₂＝2，惯性权值w＝0.6；粒子群规模N_s＝50，可行储备集和非可行储备集容量N_a＝N′_a＝5，重复采样上限it_max＝10，算法终止代数T_max＝100×No_ob。

c.基于上述准备，执行多目标粒子群优化算法。

所得Pareto最优解集如下表：

步骤5：从Pareto最优解集中选出一个最满意的折中解作为机器人移动的最终路径。

假设决策者对路径长度和危险程度的偏重为w₁＝w₂＝0.5。首先，相对每一个目标函数，通过比较可行储备集A_fea中所有元素，求得

$\left\{\begin{array}{c}{f}_1^{\max }=52.8\\ f_1^{\min }=43.6\end{array}\right.,$ $\left\{\begin{array}{c}{f}_1^{\max }=0.94\\ f_1^{\min }=0\end{array}\right..$

进而，由

${\mathrm{\&mu;}}^k=\underset{i=1}{\overset{2}{\mathrm{\&Sigma;}}}{w}_i\&times;{\mathrm{\&mu;}}_i^k$

${\mathrm{\&mu;}}_1^k=\left\{\begin{array}{cc}1,& f_1\left({\stackrel{\&RightArrow;}{x}}_k\right)\&le;43.6\\ \frac{52.8-f_1\left({\stackrel{\&RightArrow;}{x}}_k\right)}{52.8-43.6}& 43.6<f_1\left({\stackrel{\&RightArrow;}{x}}_k\right)<52.8.\\ 0,& f_1\left({\stackrel{\&RightArrow;}{x}}_k\right)\&GreaterEqual;52.8\end{array}\right.$

${\mathrm{\&mu;}}_2^k=\left\{\begin{array}{cc}1,& f_1\left({\stackrel{\&RightArrow;}{x}}_k\right)\&le;0\\ \frac{0.95-f_2\left({\stackrel{\&RightArrow;}{x}}_k\right)}{0.95-0},& 0<f_1\left({\stackrel{\&RightArrow;}{x}}_k\right)<0.95.\\ 0,& f_1\left({\stackrel{\&RightArrow;}{x}}_k\right)\&GreaterEqual;0.95\end{array}\right.$

计算出储备集A_fea中所有元素的满意度依次为：

μ¹＝0.5，μ²＝0.5，μ³＝0.6，μ⁴＝0.43，μ⁵＝0.46

比较发现解

对应的满意度最大，因此机器人所选最终路径为{(6.8，30.4)，(10.8，27.0)(14.7，24.4)，(18.7，25.0)，(22.7，25.0)，(26.6，21.4)，(30.6，21.7).(34.6，23.0)，(38.5，24.3)，(42.5，26.2)，(46.5，28.8)}

图5展示了本发明处理实例所得Pareto最优路径集合以及所选折中路径。

Claims (5)

1.一种危险源环境下的机器人全局路径规划方法，其特征在于步骤如下：

步骤1：探测并确定机器人的工作环境信息，包括机器人的起始点和目标点、障碍物的位置和形状，以及危险源的位置；

步骤2：对机器人工作环境进行建模，利用点序列(ph₁，ph₂，…，ph_n)表示路径的相应坐标；

步骤3：确定用于评价路径的两个性能指标，即规划问题的两个目标函数，路径的长度和危险程度；

(3.1)标记机器人的起始点和目标点分别为ph₀和ph_n+1，由

计算路径PH＝(ph₁，ph₂，…，ph_n)的长度，其中d(ph_j，ph_j+1)表示点ph_j与点ph_j+1之间的距离；

(3.2)设危险源DS_i(i＝1，2，…，m)的位置为

定义隶属函数

${\mathrm{dan}}_i\left(\mathrm{PH}\right)=\left\{\begin{array}{cc}1,& d\left({\mathrm{PH},\mathrm{DS}}_i\right)\&le;d_{\min }^i,\\ \frac{d_{\max }^i-d\left({\mathrm{PH},\mathrm{DS}}_i\right)}{d_{\max }^i-d_{\min }^i},& d_{\max }^i>d(\mathrm{PH},{\mathrm{DS}}_i)>d_{\min }^i,\\ 0,& d\left({\mathrm{PH},\mathrm{DS}}_i\right)\&GreaterEqual;d_{\max }^i,\end{array}\right.$

计算路径PH相对危险源DS_i的危险程度；其中，d(PH，DS_i)为路径PH与危险源DS_i间的最短距离；

为危险源DS_i的最大辐射半径，d(PH，DS_i)大于这个值时，相对危险源DS_i路径PH是绝对安全的，即危险程度为0；

为危险源DS_i的绝对辐射半径，d(PH，DS_i)小于这个值时，相对危险源DS_i路径PH是绝对危险的，即危险程度为1；

对于危险源个数大于1的情况，利用模糊和计算路径PH相对全部危险源的危险程度；

步骤4：利用改进的多目标粒子群优化算法对步骤(3)所确定的两个目标函数进行全局优化，以得到一组Pareto最优路径，具体方法如下：

(4.1)初始化粒子群，可行储备集和非可行储备，以及粒子的个体最优点；

(4.2)采用步骤3中方法计算每个粒子的适应值，采用路径与障碍物的碰撞次数评价每个粒子违背约束(与障碍物碰撞)的程度；

(4.3)更新每个粒子的个体最优点。对于一个粒子，设置粒子当前时刻所得最好位置作为它的个体最优点；

(4.4)更新算法的可行储备集和非可行储备集；约束违背程度为0且适应值最优的粒子存入可行储备集；约束违背程度大于0但适应值较好的粒子存入非可行储备集。当可行储备集中元素大于其固有容量N_a时，保留拥挤度最大(所在区域解的分布最稀疏)的N_a个元素；同理，当非可行储备集中元素大于其固有容量N_a′时，保留拥挤度最大的N_a′个元素；

(4.5)从非可行储备集和可行储备集中选择粒子的全局最优点；对于一个粒子

如果生成的随机数r∈[0，1]小于选择概率p_s，则采用规模为2的锦标赛选择方法，从非可行储备集中选择拥挤距离值较大的元素作为它的全局最优点；否则，采用规模为2的锦标赛选择方法，从可行储备集中选择拥挤距离值较大的元素作为它的全局最优点，其中选择概率为

$p_s=0.5+0.3(1-\frac{2t}{T_{\max }}),$

T_max为算法终止代数；

(4.6)采用有限重复采样和一致变异更新每个粒子的位置，首先，由

$\left\{\begin{array}{c}{v}_{i,j}(t+1)={\mathrm{wv}}_{i,j}\left(t\right)+c_1{r}_1(p_{i,j}\left(t\right)-x_{i,j}\left(t\right))+c_2{r}_2(g_j\left(t\right)-x_{i,j}\left(t\right))\\ x_{i,j}(t+1)=x_{i,j}\left(t\right)+v_{i,j}(t+1)\end{array}\right.---\left(3\right)$

更新x_i，j的位置；如果由x_i，j-1和x_i，j确定的新生路段与障碍物相碰撞，继续采用上式对x_i，j值进行重新采样，并对更新后的x_i，j值进行一致变异，直到生成一条可行路段或采样次数大于阈值it_max；式中，

和

分别为当前粒子的个体最优点和全局最优点；c₁和c₂为粒子群算法的学习因子，w为惯性权重，r₁和r₂为[0，1]间的随机数；

(4.7)判断是否满足终止准则；若满足，则终止算法，并输出可行储备集中的Pareto最优解；否则，返回步骤(4.2)；

步骤5：采用一个模糊隶属度函数模拟决策者对任务的偏好，并从Pareto最优解集中选出一个满意度最大的解作为机器人移动的最终路径。

2.根据权利要求1所述的危险源环境下的机器人全局路径规划方法，其特征在于：步骤2中，危险源环境下的机器人路径规划问题描述为如下优化问题：寻找路径

使得

①ph_i∈F，i＝1，2，…，n，其中F表示机器人的有效活动范围；

②与No_ob个障碍物不相碰，OB_j表示第j个障碍物，

③且同时最小化目标向量函数

3.根据权利要求1所述的危险源环境下的机器人全局路径规划方法，其特征在于：步骤(4.3)中粒子的约束违背程度的计算公式为：

$\mathrm{cv}\left({\stackrel{\&RightArrow;}{x}}_i\right)=\frac{1}{\mathrm{No}\_\mathrm{ob}}\underset{j=1}{\overset{\mathrm{No}\_\mathrm{ob}}{\mathrm{\&Sigma;}}}{\mathrm{cv}}_j\left({\stackrel{\&RightArrow;}{x}}_i\right)$

4.根据权利要求1所述的危险源环境下的机器人全局路径规划方法，其特征在于：步骤(4.4)在更新非可行储备集时，除路径长度和危险程度外，还利用约束违背程度

作为问题的第3性能指标，用来比较不同解间的Pareto支配关系。

5.根据权利要求1所述的危险源环境下的机器人全局路径规划方法，其特征在于：步骤(4.6)中的一致变异同时改变粒子的变异概率和变异范围，具体方法如下：

a.更新变异概率，p_m＝(iter/it_max)²；

b.生成随机数r∈[0，1]，如果p_m＞r，则对x_i，j进行如下操作：

$\left\{\begin{array}{c}\mathrm{range}=[\mathrm{up}\_\lim \left(j\right)-\mathrm{low}\_\lim \left(j\right)]*p_m\\ x_{i,j}=x_{i,j}+N\left(\mathrm{0,1}\right)*\mathrm{range}\end{array}\right.$

c.如果x_i，j＞up_lim(j)，取x_i，j＝up_lim(j)；

如果x_i，j＜low_lim(j)，取x_i，j＝low_lim(j)；

其中，iter为采样次数，it_max为采样次数的最大阈值；up_lim(j)和low_lim(j)分别为决策变量第j维分量的的上下界。

Images