CN103336526A - 基于协同进化粒子群滚动优化的机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种可以使机器人在具有静态障碍物且障碍物位置未知的工作环境下实现自主导航，采用多子群协同进化粒子群算法进行实时滚动优化避障处理的机器人路径规划方法。本发明具有以下有益效果：可以实现机器人在具有静态障碍物且障碍物位置未知的工作环境下的自主导航；本发明在多子群协同进化中引入群体质心策略，提高了种群的搜索能力；相对于现有的采用标准粒子群算法(PSO)和协同进化粒子群算法(CPSO)的路径规划方法，本发明采用了优胜劣汰策略，在进化收敛速度与精度上更具优越性；本发明具有能够在复杂障碍物环境中实时规划出有效的避障路径，具有鲁棒性好、求解效率高等优点。

Description

基于协同进化粒子群滚动优化的机器人路径规划方法

技术领域

本发明属于机器人路径实时规划技术领域，具体涉及一种基于协同进化粒子群滚动优化的机器人路径规划方法。

背景技术

路径规划是指按距离、耗时或安全性等性能要求，搜索机器人从起始点到目标点间的一条安全无障碍路径，它是机器人领域的重要课题。根据对环境知识的了解，可将路径规划分为已知环境的路径规划和未知环境下的路径规划两种，前者也被称为全局路径规划；后者则要求机器人利用自身的传感器探测周围有限范围内的信息，不断地进行动态的路径规划，也被称为在线路径规划或局部路径规划。由于需要实现在线避障处理，因此在线路径规划要比全局路径规划复杂得多。

当前，局部路径规划方法主要有：人工势场法、滚动窗口规划法与智能算法规划法等。人工势场法算法结构简单但存在着局部极值，机器人会陷入局部极值而徘徊不前；采用滚动窗口规划方法中存在死锁现像。近年来，随着对仿生智能研究的深入，许多智能优化算法被逐渐用于机器人路径规划中，如遗传算法、蚁群算法与粒子群算法。

发明内容

为了克服现有技术中存在的缺陷，提供一种可以使机器人在具有静态障碍物且障碍物位置未知的工作环境下实现自主导航，采用多子群协同进化粒子群算法进行实时滚动优化避障处理的机器人路径规划方法。本发明具体的技术方案如下：

基于协同进化粒子群滚动优化的机器人路径规划方法，用于对一机器人的行走路径进行规划，使机器人避开工作环境中的若干静止障碍物，包括如下步骤：

步骤S1，对工作环境进行建模；

步骤S2，检测机器人当前点和目标点之间的距离，若距离小于预设的距离误差，则认为到达目标点，输出各次规划的终点集，获得规划路线；否则转入步骤S3；

步骤S3，根据机器人当前点和目标点获取当前最佳运动方向角度；

步骤S4，根据预设的机器人视野角度，对机器人的转向域进行划分；根据当前最佳运动方向角度确定当前机器人的运动朝向所在的转向域；

步骤S5，若当前机器人的视野域内无障碍物，以当前点为极点建立局部极坐标系，根据预设的视野半径和当前最佳运动方向角度获得本次规划的局部终点，并将局部终点存入终点集，将局部终点作为下一次规划的当前点，转入步骤S2；若当前机器人的视野域内有障碍物，转入步骤S6；

步骤S6，基于协同进化粒子群算法采用迭代运算的方式进行避障处理；步骤S6进一步包括：

步骤S61，将视野半径等分为若干个分段，同时确定若干个分段运动方向角度，分段运动方向角度小于视野角度；设置若干粒子，将每个粒子编码为由若干分段运动方向角度组成的序列，将若干粒子组成一原始种群，并对原始种群进行初始化，得到一初始种群；若初始化成功，将初始种群分为若干个子群，转入步骤S62；若初始化失败，转入步骤S7；

步骤S62，计算各子群中各粒子的适应值，并对各子群中的粒子按适应值进行排序，再按预设的基因比例选出各子群中适应值较小的粒子，组成一基因库；

步骤S63，根据各子群中各粒子的适应值，获得各子群的最优解，并根据最优解求得一种群质心点；

步骤S64，更新各子群中粒子的速度与位置；若更新成功，则转入步骤S65，若更新失败，转入步骤S7；

步骤S65，更新各子群中粒子的适应值、粒子的个体最优解与子群的全局最优解；

步骤S66，判断是否达到预设的进化周期，若未达到进化周期则直接转入步骤S67；否则更新子群和基因库，然后更新种群质心点，再转入步骤S67；

步骤S67，判断是否达到预设的最大迭代次数，若已达到最大迭代次数，则终止迭代，获得本次规划的局部终点，并将局部终点加入终点集中，将局部终点作为下一次规划的当前点，转入步骤S2；若未达到最大迭代次数，则转入步骤S64继续迭代；

步骤S7，进行避障失败处理，将机器人转入下一转向域，返回步骤S61进行避障；若机器人在所有转向域均避障失败，则无法实现避障，失败跳出。

作为优化方案，步骤S1的工作环境建模具体包括如下步骤：

步骤S11，忽略工作环境中的高度信息，建立二维坐标系；

步骤S12，设机器人的视野域为(r,θ)，其中r为视野半径，θ为视野角度；获取机器人的出发点S与目标点G的坐标；用凸多边形描述障碍物，障碍物的坐标未知；

步骤S13，根据机器人的尺寸对障碍物进行膨化，并将机器人作为一质点处理；

步骤S14，设置当前滚动规划次数t为1，设置机器人的当前点P^t为出发点S，并将当前点P^t存入终点集{P}。

作为优化方案，步骤S3具体为：

当前最佳运动方向角度

是指当前运动朝向与二维坐标系x轴的夹角，当前运动朝向为当前点P^t到目标点G的直线方向；当前最佳运动方向角度

的计算公式如式（1）所示：

  (1)

其中，t为当前滚动规划次数，若

则令

G_x为所述目标点G在x轴的坐标值，G_y为所述目标点G在y轴的坐标值，P_x ^t为所述当前点P^t在x轴的坐标值，P_y ^t为所述当前点P^t在y轴的坐标值。

作为优化方案，步骤S4具体为：

根据视野角度θ，将机器人的转向域划分为K＝2π/θ个，根据当前最佳运动方向角度确定机器人当前运动朝向所在的转向域k，当前运动朝向所在的转向域k的计算公式如式（2）所示：

  (2)

其中，若k＝0，则令k＝K，其中ceil()为向上取整函数，mod()为取余函数。

作为优化方案，步骤S5具体为：

局部终点P^t'的计算公式如式（3）所示：

  (3)

其中，P^t'_x为所述局部终点P^t'在x轴的坐标值，P^t'_y为所述局部终点P^t'在y轴的坐标值，P_x ^t为所述当前点P^t在x轴的坐标值，P_y ^t为所述当前点P^t在y轴的坐标值；将所述局部终点P^t'存入所述终点集{P}后，令t＝t+1。

作为优化方案，步骤S61进一步包括如下步骤：

步骤S611，将视野半径r等分为m段，得到一分段半径λ，随机确定m个分段运动方向角度

其中，i＝1,2,...m，获得若干分段路径dist_i，由若干分段路径dist_i连接组成机器人在视野半径r范围内避开障碍物需经过的路径总长dist_t；

步骤S612，设置若干粒子用于进行粒子群算法的迭代运算，将粒子编码为由若干分段运动方向角度组成的序列，设若干粒子的维度为m，根据当前运动朝向所在的转向域k，定义若干粒子的范围为[(k-1)·θ,k·θ]，其中，k＝1,2,...K,K＝2π/θ；

步骤S613，确定路径总长dist_t，路径总长dist_t的计算公式如式（4）所示：

${\mathrm{dist}}_t=\mathrm{\λ}+\underset{i-2}{\overset{m}{\mathrm{\Σ}}}{\mathrm{dist}}_i=r/m+\underset{i=2}{\overset{m}{\mathrm{\Σ}}}\sqrt{{\left((i-1)\mathrm{\λ}\right)}^2+{\left(\mathrm{i\λ}\right)}^2+2\mathrm{i\λ}(i-1)\mathrm{\λ}\cos (x_i-x_{i-1})}$   (4)

其中，i=2,3,...,m；λ＝r/m，即视野半径r作m等分后的分段长度；x_i为编码后粒子的第i维；

获取局部终点P^t'到目标点G的直线距离

直线距离的计算公式如式（5）所示：

${\mathrm{dist}}_{P^{t\′}G}=\sqrt{{({P^{t\′}}_x-G_x)}^2+{\left({P^{t\′}}_y{-G}_y\right)}^2}$   (5)

根据路径总长dist_t和直线距离

设定一适应值函数，适应值函数如式（6）所示：

$\mathrm{Fit}={\mathrm{\ω}}_1{\mathrm{dist}}_t+{\mathrm{\ω}}_2{\mathrm{dist}}_{P^{t\′}G}$   (6)

其中的ω₁与ω₂为权重系数；

步骤S614，将若干粒子组成一原始种群，对原始种群进行初始化；初始化要求从本次规划的当前点P^t开始，以分段路径dist_i为视野半径的视野域内不存在障碍物，否则初始化失败；

步骤S615，若初始化成功，将初始种群划分为s个子群，根据当前运动朝向所在的转向域k，定义若干粒子的范围[x_min,x_max]为[(k-1)·θ,k·θ]；设置粒子的最大速度为粒子范围的0.1倍。

作为优化方案，步骤S614中的初始化具体为：

设定初始维度为m、初始规模为N，进行初始化前，设置原始种群的种群规模远大于初始规模N；设置粒子最大初始化次数p_max和维度最大初始化次数d_max；若原始种群中某一粒子的某一维在作d_max次初始化后仍未成功，则重新初始化当前维度；若该粒子经p_max次初始化仍失败，则认为当前粒子初始化失败；

丢弃初始化失败的粒子，将成功初始化的粒子组成初始种群；若初始种群的规模达到初始规模N，则初始化成功；否则，初始化失败。

作为优化方案，步骤S63中种群质心点P_c的定义如式（7）所示：

$P_c(R+1)=\frac{1}{s}\underset{1}{\overset{s}{\mathrm{\Σ}}}{p}_{\mathrm{gd}}^i\left(R\right)i=\mathrm{1,2},...,s$   (7)

其中，R为进化周期，

为第i个子群的全局最优解。

作为优化方案，步骤S64具体为：

根据式（8）和式（9）更新各子群中粒子的速度与位置：

${v_i}^j(\mathrm{iter}+1)=\mathrm{\ω}\×{v_i}^j\left(\mathrm{iter}\right)+c_1{r}_1\×({P_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_2{r}_2\×({g_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_3{r}_3({P_c}^R-{x_i}^j\left(\mathrm{iter}\right))$

(8)

$x_i^j(\mathrm{iter}+1)=x_i^j\left(\mathrm{iter}\right)+v_i^j(\mathrm{iter}+1)$   (9)

其中iter为种群进化代数，j为子群标记，i为子群内粒子标记，

为第j个子群中第i个粒子的速度；P_i ^j为第j个子群中第i个粒子的历史最优解；

为当前粒子所在子群的最优解；c₁和c₂为学习因子；c₃为交流因子；r₁、r₂、r₃为[0,1]间的随机数；ω为惯性权重，惯性权重ω采用线性递减策略，如式（10）所示：

ω(iter)=ω_max-iter×(ω_max-ω_min)/iter_max    (10)

对粒子的速度与位置边界处理包括：若子群j中某一粒子i的d维速度 $v_{i,j}^d>v_{\max },$ 则令 $v_{i,j}^d=v_{\max };$ 若 $v_{i,j}^d<v_{\min },$ 则令 $v_{i,j}^d=v_{\min };$ 若 $x_{i,j}^d>x_{\max },$ 则令 $x_{i,j}^d=x_{\max };$ 若 $x_{i,j}^d<x_{\min },$ 则 $x_{i,j}^d=x_{\min };$

对粒子位置更新后的各维要求包括：从当前点P^t开始，以分段路径dist_i为视野半径的视野域内不存在障碍物，若存在障碍物则当前维度更新失败；若对某一粒子的某一维作d_max次更新仍未成功，则该粒子当前维度更新失败，若对某一粒子作p_max次更新仍未成功，则该粒子更新失败；若所有子群均更新失败，则终止迭代并转入步骤S7。

作为优化方案，步骤S66具体为：

判断是否达到预设的进化周期R，若达到则首先根据预设的死亡比例，从基因库中随机选择粒子替代各子群中最劣的粒子；再根据适应值，若各子群中某一粒子的适应值小于基因库中某一粒子的适应值，则用各子群中的该粒子替换基因库中的相应粒子；最后更新种群质心P_c。

作为优化方案，步骤S7中将机器人转入下一转向域的方法为：若所述当前最佳运动方向角度

本次滚动规划的局部终点P^t'位于当前点P^t的y轴增向，则对各转向域作逆时针旋转测试，否则对各转向域作顺时针旋转测试。

与现有技术相比，本发明具有以下有益效果：

（1）本发明可以实现机器人在具有静态障碍物且障碍物位置未知的工作环境下的自主导航；

（2）本发明在多子群协同进化中引入群体质心策略，提高了种群的搜索能力；

（3）相对于现有的采用标准粒子群算法(PSO)和协同进化粒子群算法(CPSO)的路径规划方法，本发明采用了优胜劣汰策略，在进化收敛速度与精度上更具优越性；

（4）本发明具有能够在复杂障碍物环境中实时规划出有效的避障路径，具有鲁棒性好、求解效率高等优点。

附图说明

图1为本发明总体流程图；

图2为本发明步骤S6的流程图；

图3为本发明步骤S1中工作环境建模的示意图；

图4为本发明步骤S6中分段避障的示意图；

图5为本发明步骤S63中计算分段路径总长的示意图；

图6为本发明步骤S7中转向域测试的示意图；

图7为本发明实施例1典型障碍物环境中的规划路径结果；

图8为采用三种不同算法进行路径规划的对比图；

图9为本发明实施例1狭缝环境中的规划路径结果；

图10为本发明实施例1长条形障碍物环境中的规划路径结果；

图11为本发明实施例1凹形障碍物环境中的规划路径结果。

具体实施方式

下面结合附图以实施例的方式详细描述本发明。

实施例1：

如图1所示，基于协同进化粒子群滚动优化的机器人路径规划方法，用于对一机器人的行走路径进行规划，使机器人避开工作环境中的若干静止障碍物，包括如下步骤：

步骤S1，对工作环境进行建模，如图3所示，具体包括如下步骤：

步骤S11，忽略工作环境中的高度信息，建立二维坐标系。此步骤忽略了机器人与障碍物的高度信息，将机器人的工作环境作描述为一个二维有界的环境。

步骤S12，设机器人的视野域为(r,θ)，其中r为视野半径，θ为视野角度；获取机器人的出发点S与目标点G的坐标；在二维坐标系中用凸多边形描述障碍物，障碍物的坐标未知。

步骤S13，根据机器人的尺寸对障碍物进行膨化，并将机器人作为一质点处理。在本发明的路径规划中忽略机器人本身的形状和结构，将其看作一个质点进行处理。

步骤S14，设置当前滚动规划次数t为1，设置机器人的当前点P^t为出发点S，并将当前点P^t存入终点集{P}。

步骤S2，检测机器人当前点P^t和目标点G之间的距离，若距离小于预设的距离误差ξ，则认为到达目标点，输出各次规划的终点集{P}，获得规划路线；否则转入步骤S3；

步骤S3，根据机器人当前点P^t和目标点G获取当前最佳运动方向角度当前最佳运动方向角度

是指当前运动朝向与二维坐标系x轴的夹角，当前运动朝向为当前点P^t到目标点G的直线方向；当前最佳运动方向角度

的计算公式如式（1）所示：

  (1)

其中，t为当前滚动规划次数，若

则令

G_x为所述目标点G在x轴的坐标值，G_y为所述目标点G在y轴的坐标值，P_x ^t为所述当前点P^t在x轴的坐标值，P_y ^t为所述当前点P^t在y轴的坐标值。

步骤S4，根据视野角度θ，将机器人的转向域划分为K＝2π/θ个，根据当前最佳运动方向角度

确定机器人当前运动朝向所在的转向域k，当前运动朝向所在的转向域k的计算公式如式（2）所示：

  (2)

其中，若k＝0，则令k＝K，其中ceil()为向上取整函数，mod()为取余函数。

步骤S5，若当前机器人的视野域内无障碍物，以当前点为极点建立局部极坐标系，根据预设的视野半径r和当前最佳运动方向角度

获得本次规划的局部终点P^t'，局部终点P^t'的计算公式如式（3）所示：

  (3)

其中，P^t'_x为所述局部终点P^t'在x轴的坐标值，P^t'_y为所述局部终点P^t'在y轴的坐标值，P_x ^t为所述当前点P^t在x轴的坐标值，P_y ^t为所述当前点P^t在y轴的坐标值；

将局部终点P^t'存入终点集{P}，令t＝t+1，并将局部终点P^t'作为下一次规划的当前点P^t，转入步骤S2；若当前机器人的视野域内有障碍物，转入步骤S6。

如图2所示，步骤S6，基于协同进化粒子群算法采用迭代运算的方式进行避障处理；具体包括如下步骤：

步骤S61，种群初始化步骤，具体包括如下步骤：

步骤S611，将视野半径r等分为m段，得到一分段半径λ，同时随机确定m个分段运动方向角度

，其中，i＝1,2,...m，分段运动方向角度

小于机器人的视野角度θ；本步骤是尝试以较小的分段与较小的角度试图“绕”过障碍物，如图4所示，根据分段半径λ和分段运动方向角度

可在机器人的视野域内获得m个分段点；相邻两个分段点之间的距离为分段路径dist_i，从而获得m个分段路径dist_i，由m个分段路径dist_i连接组成机器人在视野半径r范围内避开障碍物需经过的路径总长dist_t。

步骤S612，设置若干粒子用于进行粒子群算法的迭代运算，将粒子编码为由若干分段运动方向角度组成的序列，设若干粒子的维度为m，根据当前运动朝向所在的转向域k，定义若干粒子的范围为[(k-1)·θ,k·θ]，(k＝1,2,...K,K＝2π/θ)。

步骤S613，如图5所示，根据余弦定理可以确定路径总长dist_t，路径总长dist_t的计算公式如式（4）所示：

${\mathrm{dist}}_t=\mathrm{\&lambda;}+\underset{i-2}{\overset{m}{\mathrm{\&Sigma;}}}{\mathrm{dist}}_i=r/m+\underset{i=2}{\overset{m}{\mathrm{\&Sigma;}}}\sqrt{{\left((i-1)\mathrm{\&lambda;}\right)}^2+{\left(\mathrm{i\&lambda;}\right)}^2+2\mathrm{i\&lambda;}(i-1)\mathrm{\&lambda;}\cos (x_i-x_{i-1})}$   (4)

其中，i＝2,3,...,m；λ＝r/m，即视野半径r作m等分后的分段半径；第一个分段路径dist₁＝λ，x_i为编码后粒子的第i维；

获取局部终点P^t'到目标点G的直线距离直线距离

的计算公式如式（5）所示：

${\mathrm{dist}}_{P^{t\&prime;}G}=\sqrt{{({P^{t\&prime;}}_x-G_x)}^2+{\left({P^{t\&prime;}}_y{-G}_y\right)}^2}$   (5)

根据路径总长dist_t和直线距离设定一适应值函数，适应值函数如式（6）所示：

$\mathrm{Fit}={\mathrm{\&omega;}}_1{\mathrm{dist}}_t+{\mathrm{\&omega;}}_2{\mathrm{dist}}_{P^{t\&prime;}G}$   (6)

其中的ω₁与ω₂为权重系数。

步骤S614，将若干粒子组成一原始种群，对原始种群进行初始化；初始化要求从本次规划的当前点P^t开始，以分段路径dist_i为视野半径的视野域内不存在障碍物，否则初始化失败；步骤S614进一步包括：

设定初始维度为m、初始规模为N，进行初始化前，设置原始种群的种群规模远大于初始规模N；设置粒子最大初始化次数p_max和维度最大初始化次数d_max；若原始种群中某一粒子的某一维在作d_max次初始化后仍未成功，则重新初始化当前维度；若该粒子经p_max次初始化仍失败，则认为当前粒子初始化失败；

丢弃初始化失败的粒子，将成功初始化的粒子组成初始种群；若初始种群的规模达到初始规模N，则初始化成功，转入下一步；否则，初始化失败，转入步骤S7。

步骤S615，若初始化成功，将初始种群划分为s个子群，根据当前运动朝向所在的转向域k，定义若干粒子的范围[x_min,x_max]为[(k-1)·θ,k·θ]；设置粒子的最大速度为粒子范围的0.1倍。

步骤S62，计算各子群中各粒子的适应值，并对各子群中的粒子按适应值进行排序，再按预设的基因比例选出各子群中适应值较高的粒子，组成一基因库。本步骤采用了“优胜劣汰”策略，按一定的基因比例选择各子群中最优的“精英”粒子创建基因库，该基因库中的粒子均为适应值较小的“精英”粒子。

步骤S63，根据各子群中各粒子的适应值，获得各子群的最优解并根据最优解求得一种群质心点P_c；种群质心点P_c的定义如式（7）所示：

$P_c(R+1)=\frac{1}{s}\underset{1}{\overset{s}{\mathrm{\&Sigma;}}}{p}_{\mathrm{gd}}^i\left(R\right)i=\mathrm{1,2},...,s$   (7)

其中，R为进化周期，

为第i个子群的全局最优解。种群质心点P_c即各子群最优解的平均值，代表了整个种群的理想中心，它随进化周期而更新，无速度分量。种群质心点为各子群的全局最优解的均值，是一种理想的质心，并非一个实际的粒子，引入质心的理由是，粒子在解空间中飞翔时将追随个体历史最优、子群全局最优与种群理想最优，由于比标准粒子群进化公式中多了一项种群理想最优，所以理论上粒子飞行时将方向性更明确些，能够降低粒子飞行的随意性，各子群间也有了一定的“交流”，因此能够显著改善种群的搜索能力。

步骤S64，从此步骤开始进行滚动迭代运算，根据式（8）和式（9）更新各子群中粒子的速度与位置：

${v_i}^j(\mathrm{iter}+1)=\mathrm{\&omega;}\&times;{v_i}^j\left(\mathrm{iter}\right)+c_1{r}_1\&times;({P_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_2{r}_2\&times;({g_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_3{r}_3({P_c}^R-{x_i}^j\left(\mathrm{iter}\right))$

(8)

$x_i^j(\mathrm{iter}+1)=x_i^j\left(\mathrm{iter}\right)+v_i^j(\mathrm{iter}+1)$   (9)

其中iter为种群进化代数，j为子群标记，i为子群内粒子标记，

为第j个子群中第i个粒子的速度；P_i ^j为第j个子群中第i个粒子的历史最优解；

为当前粒子所在子群的最优解；c₁和c₂为学习因子；c₃为交流因子；r₁、r₂、r₃为[0,1]间的随机数；ω为惯性权重，惯性权重ω采用线性递减策略，如式（10）所示：

ω(iter)=ω_max-iter×(ω_max-ω_min)/iter_max    (10)

对粒子的速度与位置边界处理包括：若子群j中某一粒子i的d维速度 $v_{i,j}^d>v_{\max },$ 则令 $v_{i,j}^d=v_{\max };$ 若 $v_{i,j}^d<v_{\min },$ 则令 $v_{i,j}^d=v_{\min };$ 若 $x_{i,j}^d>x_{\max },$ 则令 $x_{i,j}^d=x_{\max };$ 若 $x_{i,j}^d<x_{\min },$ 则 $x_{i,j}^d=x_{\min };$

对粒子位置更新后的各维要求包括：从当前点P^t开始，以分段路径dist_i为视野半径的视野域内不存在障碍物，若存在障碍物则当前维度更新失败；若对某一粒子的某一维作d_max次更新仍未成功，则该粒子当前维度更新失败，若对某一粒子作p_max次更新仍未成功，则该粒子更新失败；若所有子群均更新失败，则终止迭代并转入步骤S7；若更新成功则转入步骤S65。

步骤S65，更新各子群中粒子的适应值、粒子的个体最优解与子群的全局最优解。

步骤S66，判断是否达到预设的进化周期R，若未达到进化周期R则直接转入步骤S67；若达到进化周期R则首先根据预设的死亡比例，从基因库中随机选择粒子替代各子群中最劣的粒子；若各子群中某一粒子的适应值小于基因库中某一粒子的适应值，则用各子群中的该粒子替换基因库中的相应粒子；然后更新种群质心P_c；最后转入步骤S67。本步骤采用了“优胜劣汰”策略，粒子的适应值越小，则该粒子越优秀。先根据死亡比例淘汰各子群中一部分适应值较高的粒子，从基因库中选择粒子进行替换；再淘汰基因库中适应值较高的粒子，从各子群中选取优秀的粒子进行替换；从而使基因库和各子群中的粒子都保持较小的适应值，使进化的收敛速度更快，精度更高。

步骤S67，判断是否达到预设的最大迭代次数iter_max，若已达到最大迭代次数iter_max，则终止迭代，根据所有子群的全局最优解与视野半径r获得本次规划的局部终点P^t'，并将局部终点P^t'加入终点集{P}中，令t＝t+1，将局部终点P^t'作为下一次规划的当前点P^t，转入步骤S2；若未达到最大迭代次数iter_max，则转入步骤S64继续迭代；

步骤S7，进行避障失败处理，将机器人转入下一转向域，转入下一转向域的方法为：若当前最佳运动方向角度

本次滚动规划的局部终点P^t'位于当前点P^t的y轴增向，则对各转向域作逆时针旋转测试，即k＝mod(k+i,K)，i=1,2,...K-1；否则对各转向域作顺时针旋转测试。顺时针旋转时，对k～1转向域按k＝mod(k-i,K)变换转向域,其中i＝0，1,2,...k-1；对K～k转向域按k＝mod(K-i,K)变换转向域，其中i＝0，1,2,...K-k-1，如式（11）所示：

转入下一转向域后，返回步骤S61进行避障，如图6所示；若机器人在所有转向域均避障失败，则无法实现避障，失败跳出。

在本实施例中，选择以下参数进行进一步说明。

(1)粒子群参数：取初始规模N=20，子群数s=4、子群规模为5，粒子维度m＝10，学习与交流因子c₁＝c₂＝c₃＝2，惯性权重ω从0.9线性下降至0.4，适应值函数中权重系数ω₁=ω₂=1，最大迭代次数iter_max＝100；种群进化周期R＝4，粒子最大初始化次数p_max和维度最大初始化次数d_max为p_max＝d_max＝20，基因比例为0.2，死亡比例为0.2。

(2)机器人工作参数：视野半径r＝5，视野角度为θ＝π/3，转向域为K＝2π/θ＝6个，距离误差ξ＝2。

创建大小为100×100的典型障碍物工作环境，其中存在着若干个静态障碍物，设置起点为S(10,10)，目标点为G(90,90)，规划出的路径如图7所示。

***参数相同，选择临近障碍物的点(45,29)，使用标准粒子群算法(PSO)、协同进化粒子群算法(CPSO)与本发明采用“优胜劣汰”策略的多子群协同进化粒子群算法(MCPSO)分别进行避障测试，三种算法的适应值变化曲线如图8所示，由图可知MCPSO算法在进化收敛速度与精度上更具优越性。

在特殊的障碍物环境下，如图9中的狭缝环境，图10中的巨形长条型障碍物环境及图11中的凹型障碍物环境，采用本方案也可以有效地实施在线路径规划，规划出的路径见图7、图8与图9。

以上公开的仅为本申请的几个具体实施例，但本申请并非局限于此任何本领域的技术人员能思之的变化，都应落在本申请的保护范围内。

Claims (11)

1.基于协同进化粒子群滚动优化的机器人路径规划方法，用于对一机器人的行走路径进行规划，使机器人避开工作环境中的若干静止障碍物，其特征在于，包括如下步骤：

步骤S1，对所述工作环境进行建模；

步骤S2，检测所述机器人当前点和目标点之间的距离，若所述距离小于预设的距离误差，则认为到达目标点，输出各次规划的终点集，获得规划路线；否则转入步骤S3；

步骤S3，根据所述机器人当前点和目标点获取当前最佳运动方向角度；

步骤S4，根据预设的机器人视野角度，对机器人的转向域进行划分；根据所述当前最佳运动方向角度确定当前所述机器人的运动朝向所在的转向域；

步骤S5，若当前所述机器人的视野域内无障碍物，以所述当前点为极点建立局部极坐标系，根据预设的视野半径和所述当前最佳运动方向角度获得本次规划的局部终点，并将所述局部终点存入所述终点集，将所述局部终点作为下一次规划的当前点，转入步骤S2；若当前所述机器人的视野域内有障碍物，转入步骤S6；

步骤S6，基于协同进化粒子群算法采用迭代运算的方式进行避障处理；所述步骤S6进一步包括：

步骤S61，将所述视野半径等分为若干个分段，同时确定若干个分段运动方向角度，所述分段运动方向角度小于所述视野角度；设置若干粒子，将每个所述粒子编码为由若干所述分段运动方向角度组成的序列，将若干所述粒子组成一原始种群，并对所述原始种群进行初始化，得到一初始种群；若初始化成功，将所述初始种群分为若干个子群，转入步骤S62；若初始化失败，转入步骤S7；

步骤S62，计算各所述子群中各粒子的适应值，并对各所述子群中的粒子按适应值进行排序，再按预设的基因比例选出各所述子群中适应值较小的粒子，组成一基因库；

步骤S63，根据各所述子群中各粒子的适应值，获得各所述子群的最优解，并根据所述最优解求得一种群质心点；

步骤S64，更新各所述子群中粒子的速度与位置；若更新成功，则转入步骤S65，若更新失败，转入步骤S7；

步骤S65，更新各所述子群中粒子的适应值、粒子的个体最优解与子群的全局最优解；

步骤S66，判断是否达到预设的进化周期，若未达到所述进化周期则直接转入步骤S67；否则更新所述子群和所述基因库，然后更新所述种群质心点，再转入步骤S67；

步骤S67，判断是否达到预设的最大迭代次数，若已达到所述最大迭代次数，则终止迭代，获得本次规划的局部终点，并将所述局部终点加入所述终点集中，将所述局部终点作为下一次规划的当前点，转入步骤S2；若未达到所述最大迭代次数，则转入步骤S64继续迭代；

步骤S7，进行避障失败处理，将所述机器人转入下一转向域，返回步骤S61进行避障；若所述机器人在所有转向域均避障失败，则无法实现避障，失败跳出。

2.根据权利要求1所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S1的所述工作环境建模具体包括如下步骤：

步骤S11，忽略所述工作环境中的高度信息，建立二维坐标系；

步骤S12，设所述机器人的视野域为(r,θ)，其中r为视野半径，θ为视野角度；获取所述机器人的出发点S与目标点G的坐标；用凸多边形描述所述障碍物，所述障碍物的坐标未知；

步骤S13，根据所述机器人的尺寸对所述障碍物进行膨化，并将所述机器人作为一质点处理；

步骤S14，设置当前滚动规划次数t为1，设置所述机器人的当前点P^t为所述出发点S，并将所述当前点P^t存入终点集{P}。

3.根据权利要求2所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S3具体为：

所述当前最佳运动方向角度是指当前运动朝向与所述二维坐标系x轴的夹角，所述当前运动朝向为所述当前点P^t到所述目标点G的直线方向；所述当前最佳运动方向角度

的计算公式如式（1）所示：

  (1)

其中，t为当前滚动规划次数，若

则令

G_x为所述目标点G在x轴的坐标值，G_y为所述目标点G在y轴的坐标值，

为所述当前点P^t在x轴的坐标值，

为所述当前点P^t在y轴的坐标值。

4.根据权利要求3所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S4具体为：

根据所述视野角度θ，将所述机器人的转向域划分为K＝2π/θ个，根据所述当前最佳运动方向角度

确定所述机器人当前运动朝向所在的转向域k，所述当前运动朝向所在的转向域k的计算公式如式（2）所示：

  (2)

其中，若k＝0，则令k＝K，其中ceil()为向上取整函数，mod()为取余函数。

5.根据权利要求4所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S5具体为：

所述局部终点P^t'的计算公式如式（3）所示：

  (3)

其中，P^t'_x为所述局部终点P^t'在x轴的坐标值，P^t'_y为所述局部终点P^t'在y轴的坐标值，P_x ^t为所述当前点P^t在x轴的坐标值，P_y ^t为所述当前点P^t在y轴的坐标值；将所述局部终点P^t'存入所述终点集{P}后，令t＝t+1。

6.根据权利要求5所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S61进一步包括如下步骤：

步骤S611，将所述视野半径r等分为m段，得到一分段半径λ，随机确定m个所述分段运动方向角度

其中，

i＝1,2,...m，获得若干分段路径dist_i，由所述若干分段路径dist_i连接组成所述机器人在所述视野半径r范围内避开所述障碍物需经过的路径总长dist_t；

步骤S612，设置若干粒子用于进行粒子群算法的迭代运算，将所述粒子编码为由若干所述分段运动方向角度

组成的序列，设所述若干粒子的维度为m，根据所述当前运动朝向所在的转向域k，定义所述若干粒子的范围为[(k-1)·θ,k·θ]，其中，k＝1,2,...K,K＝2π/θ；

步骤S613，确定所述路径总长dist_t，所述路径总长dist_t的计算公式如式（4）所示：

${\mathrm{dist}}_t=\mathrm{\&lambda;}+\underset{i-2}{\overset{m}{\mathrm{\&Sigma;}}}{\mathrm{dist}}_i=r/m+\underset{i=2}{\overset{m}{\mathrm{\&Sigma;}}}\sqrt{{\left((i-1)\mathrm{\&lambda;}\right)}^2+{\left(\mathrm{i\&lambda;}\right)}^2+2\mathrm{i\&lambda;}(i-1)\mathrm{\&lambda;}\cos (x_i-x_{i-1})}$   (4)

其中，i＝2,3,...,m；λ＝r/m，即视野半径r作m等分后的分段长度；x_i为编码后粒子的第i维；

获取所述局部终点P^t'到所述目标点G的直线距离

所述直线距离

的计算公式如式（5）所示：

${\mathrm{dist}}_{P^{t\&prime;}G}=\sqrt{{({P^{t\&prime;}}_x-G_x)}^2+{\left({P^{t\&prime;}}_y{-G}_y\right)}^2}$   (5)

根据所述路径总长dist_t和所述直线距离

设定一适应值函数，所述适应值函数如式（6）所示：

$\mathrm{Fit}={\mathrm{\&omega;}}_1{\mathrm{dist}}_t+{\mathrm{\&omega;}}_2{\mathrm{dist}}_{P^{t\&prime;}G}$   (6)

其中的ω₁与ω₂为权重系数；

步骤S614，将所述若干粒子组成一原始种群，对原始种群进行初始化；所述初始化要求从本次规划的当前点P^t开始，以所述分段路径dist_i为视野半径的视野域内不存在障碍物，否则初始化失败；

步骤S615，若初始化成功，将所述初始种群划分为s个子群，根据所述当前运动朝向所在的转向域k，定义所述若干粒子的范围[x_min,x_max]为[(k-1)·θ,k·θ]；设置粒子的最大速度为粒子范围的0.1倍。

7.根据权利要求6所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S614中的初始化具体为：

设定初始维度为m、初始规模为N，进行初始化前，设置所述原始种群的种群规模远大于初始规模N；设置粒子最大初始化次数p_max和维度最大初始化次数d_max；若所述原始种群中某一粒子的某一维在作d_max次初始化后仍未成功，则重新初始化当前维度；若该粒子经p_max次初始化仍失败，则认为当前粒子初始化失败；

丢弃初始化失败的粒子，将成功初始化的粒子组成所述初始种群；若所述初始种群的规模达到初始规模N，则初始化成功；否则，初始化失败。

8.根据权利要求7所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S63中所述种群质心点P_c的定义如式（7）所示：

$P_c(R+1)=\frac{1}{s}\underset{1}{\overset{s}{\mathrm{\&Sigma;}}}{p}_{\mathrm{gd}}^i\left(R\right)i=\mathrm{1,2},...,s$   (7)

其中，R为进化周期，

为第i个子群的全局最优解。

9.根据权利要求8所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S64具体为：

根据式（8）和式（9）更新各所述子群中粒子的速度与位置：

${v_i}^j(\mathrm{iter}+1)=\mathrm{\&omega;}\&times;{v_i}^j\left(\mathrm{iter}\right)+c_1{r}_1\&times;({P_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_2{r}_2\&times;({g_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_3{r}_3({P_c}^R-{x_i}^j\left(\mathrm{iter}\right))$

(8)

$x_i^j(\mathrm{iter}+1)=x_i^j\left(\mathrm{iter}\right)+v_i^j(\mathrm{iter}+1)$   (9)

其中iter为种群进化代数，j为子群标记，i为子群内粒子标记，

为第j个子群中第i个粒子的速度；P_i ^j为第j个子群中第i个粒子的历史最优解；

为当前粒子所在子群的最优解；c₁和c₂为学习因子；c₃为交流因子；r₁、r₂、r₃为[0,1]间的随机数；ω为惯性权重，所述惯性权重ω采用线性递减策略，如式（10）所示：

ω(iter)=ω_max-iter×(ω_max-ω_min)/iter_max    (10)

对粒子的速度与位置边界处理包括：若子群j中某一粒子i的d维速度 $v_{i,j}^d>v_{\max },$ 则令 $v_{i,j}^d=v_{\max };$ 若 $v_{i,j}^d<v_{\min },$ 则令 $v_{i,j}^d>v_{\min },$ 若 $x_{i,j}^d>x_{\max },$ 则令 $x_{i,j}^d=x_{\max };$ 若 $x_{i,j}^d<x_{\min },$ 则 $x_{i,j}^d=x_{\min };$

对粒子位置更新后的各维要求包括：从所述当前点P^t开始，以所述分段路径dist_i为视野半径的视野域内不存在障碍物，若存在障碍物则当前维度更新失败；若对某一粒子的某一维作d_max次更新仍未成功，则该粒子当前维度更新失败，若对某一粒子作p_max次更新仍未成功，则该粒子更新失败；若所有子群均更新失败，则终止迭代并转入步骤S7。

10.根据权利要求9所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S66具体为：

判断是否达到预设的进化周期R，若达到则首先根据预设的死亡比例，从所述基因库中随机选择粒子替代各所述子群中最劣的粒子；若各所述子群中某一粒子的适应值小于所述基因库中某一粒子的适应值，则用各所述子群中的该粒子替换所述基因库中的相应粒子；最后更新种群质心P_c。

11.根据权利要求10所述的基于协同进化粒子群滚动优化的机器人路径规划方法，其特征在于，所述步骤S7中将所述机器人转入下一转向域的方法为：若所述当前最佳运动方向角度本次滚动规划的局部终点P^t'位于当前点P^t的y轴增向，则对各转向域作逆时针旋转测试，否则对各转向域作顺时针旋转测试。

Images