CN101837591B - 基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法，主要解决现有技术中存在的规划路径平滑性差且收敛速度缓慢的问题。其规划步骤是：对移动机器人运动环境进行建模；借助三次Ferguson样条连接描述移动机器人路径；由Ferguson样条的端点与相应的切向量构造优化粒子；由最短性惩罚函数和安全性惩罚函数定义需要求解路径的适应度函数；利用基于双群协同竞争粒子群算法对适应度函数进行全局优化；根据优化结果输出由Ferguson样条描述的机器人路径。本发明综合考虑了机器人路径的最短性和安全性，规划的路径平滑，易于移动机器人的运动控制，符合人工规划的意图，可用于各类移动机器人的自主导航。

Description

基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法

技术领域

本发明属于机器人技术领域，特别是一种涉及移动机器人的路径规划方法，可用于各类移动机器人的自主导航。

背景技术

路径规划问题是移动机器人导航的关键技术之一，主要任务是在有障碍物的环境中寻找一条从起点S到目标点G之间可行的、无碰撞的最优或者接近最优的路径。现有的移动机器人路径规划方法主要包括：栅格法、可视图法、人工势场法以及各种智能规划算法等。栅格法求得的路径在一定条件下可以得到最优解，但是栅格大小的选择会影响到解的质量，搜索空间较大时需要的存储空间也较大。可视图法需要不断重构可视图，搜索效率比较低。另一方面，栅格法和可视图法规划的路径一般为折线，不利于机器人的运动控制。人工势场法结构简单，易于实现，规划的路径比较平滑、安全，得到了广泛的应用，但是也有较大缺陷：如局部极值点、目标不可达、障碍物前振荡、规划路径非全局最优等。近年来，神经网络、蚁群算法、遗传算法和免疫算法等多种智能算法被应用到路径规划领域。这类智能路径规划方法存在以下不足：(1)这类算法一般采用栅格或链接图建模，然后利用优化算法对路径进行优化，这就继承了栅格法和链接图法的缺点，规划的路径一般为折线，不利于机器人运动控制，往往需要进行路径平滑；(2)运算时间普遍过长，很难达到实时路径规划的要求；(3)既有的智能算法仍无法避免早熟收敛问题。

发明内容

本发明的目的在于克服现有技术中存在的不足，提出一种基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法，以快速有效地实现障碍环境下机器人平滑无碰路径规划。

实现本发明目的的技术方案是：针对路径规划问题的特点和采用的智能优化算法的特点，借助三次Ferguson样条连接描述路径，将移动机器人路径规划问题转化为三次Ferguson样条曲线的参数优化问题，借助基于双群协同竞争粒子群算法实现最有路径规划，以满足机器人路径规划的需求。具体步骤包括如下：

(1)根据机器人自身携带的摄像头、声纳、激光测距仪传感器获取的信息对移动机器人运动环境进行建模；

(2)借助Ferguson样条连接描述移动机器人路径，该样条的二维空间表示式为：

r(t)＝(x(t)，y(t))＝P₀f₁(t)+P₁f₂(t)+P′₀f₃(t)+P′₁f₄(t)＝a₀+a₁t+a₂t²+a₃t³

其中，

$\left\{\begin{array}{c}{a}_0=2P_0-2P_1+P_0^{\′}+P_1^{\′}\\ a_1=-3P_0+3P_1-2P_0^{\′}-P_1^{\′}\\ a_2=P_0^{\′}\\ a_3=P_0\end{array}\right.$

每个样条由端点P₀，P₁与切向量P′₀，P′₁′所决定；

(3)每两个相邻的Ferguson样条共用一个端点与相应的切向量，构造由n个Ferguson样条连接而成的二维路径轨线优化粒子P＝{P_0xP_0yP′_0xP′_0yP_1xP_1yP′_1xP′_1yP_2xP_2yP′_2xP′_2y…P_nxP_nyP′_nxP′_ny}，P_mx，m＝0，1，…，n代表第m个端点的横坐标，P_my，m＝0，1，…，n代表第m个端点的纵坐标，P′_mx，m＝0，1，…，n代表第m个端点切向量的横坐标，P′_my，m＝0，1，…，n代表第m个端点切向量的纵坐标；

(4)定义适应度函数：

f＝f₁+αf₂

其中，α为权值系数，用来调整最短性和安全性在路径规划中所占的权重；

f₁为最短性惩罚函数，定义为：

$f_1=\frac{l}{l_{\min }},$ l_min为当前点到目标点的欧氏距离，l是轨线的长度；

f₂为安全性惩罚函数，定义为：

$f_2=\left\{\begin{array}{cc}1,& d_{\min }>D_{\mathrm{safe}}\\ \exp (\frac{D_{\mathrm{safe}}+1}{d_{\min }+1}-1),& 0<d_{\min }\&le;D_{\mathrm{safe}}\end{array}\right.$

式中，D_safe为反映障碍物影响的常数，d_min为轨线与所有障碍物之间的最小欧氏距离；

(5)对适应度函数进行如下全局优化：

(5.1)初始化两个种群，设定两个种群A和B的参数，并为两个种群中的每个粒子随机赋予初始位置和初始速度；

(5.2)计算每个粒子对应的样条的轨线长度l，并根据步骤(1)所建立模型计算轨线与障碍物之间的最小欧氏距离d_min，并根据公式9)计算两个种群中每个粒子的适应度函数；

(5.3)确定当前代k种群A每个粒子个人历史最优位置p_i ^A(k)，i＝1，…，N_A和种群A迄今为止所经历的最优位置p_g ^A(k)，其中N_A为种群A的粒子数；确定当前代k种群B每个粒子个人历史最优位置p_j ^B(k)，j＝1，…，N_B和种群B迄今为止所经历的最优位置p_g ^B(k)，其中N_B为种群B的粒子数；

(5.4)更新每个粒子的速度和位置；

(5.5)更新整个种群的全局最优位置；

$p_g\left(k\right)=\arg \left\{\min \left(f\right[p_g^A\left(k\right)],f[p_g^B\left(k\right)\left]\right)\right\}$

(5.6)检验终止条件，如果当前的进化次数达到预设的最大进化代数或优化结果达到预设误差，则寻优结束，输出最优解及最优值，否则将返回(5.2)继续进行搜索；

(6)根据优化结果输出由Ferguson样条描述的机器人路径。

本发明具有如下优点：

①由于采用三次Ferguson样条连接描述机器人路径，规划的路径具有连续的一阶导数，利于机器人的运动控制；

②由于采用最短性惩罚函数和安全性惩罚函数定义需要求解路径的目标函数，在保证安全性的前提下可以实现最短路径规划；

③由于采用权重系数调整最短性和安全性在路径规划中所占的权重，可以根据不同的需求方便地调整适应度函数；

④由于采用基于双群协同竞争粒子群算法进行路径优化，因而具有快速收敛和全局寻优特性。

附图说明

图1为本发明基于双群协同竞争粒子群和Ferguson样条机器人路径规划方法的流程图；

图2为本发明对路径全局优化的流程图；

图3为本发明进行路径全局优化的粒子结构图；

图4为本发明进行路径规划粒子群初始化后的仿真图；

图5为本发明进行路径规划粒子群进化100代后的仿真图；

图6为本发明进行路径规划粒子群进化200代后的仿真图；

图7为本发明进行路径规划粒子群进化300代后的仿真图；

图8为本发明进行路径规划粒子群进化400代后的仿真图；

图9为本发明进行路径规划粒子群进化500代后的仿真图；

图10为本发明进行路径规划的收敛曲线。

具体实施方式

以下参照附图对本发明作进一步详细的描述。

参照图1，本发明进行路径规划的步骤如下：

步骤1，对机器人运动环境进行建模。

在某一室内，把机器人简化为一个点，场地长度范围[-1000，1000]，宽度范围[-1000，1000]；给出机器人当前坐标及朝向角和目标坐标及朝向角，即当前坐标为[-880 -880]，当前朝向角为[600 400]，目标坐标为[300 300]，目标朝向角为[-300100]。

步骤2，利用Ferguson样条描述移动机器人路径。

二维空间中的Ferguson样条表示为：

r(t)＝(x(t)，y(t))＝P₀f₁(t)+P₁f₂(t)+P′₀f₃(t)+P′₁f₄(t)＝a₀+a₁t+a₂t²+a₃t³  1)

其中，f₁(t)，f₂(t)，f₃(t)，f₄(t)为Ferguson多项式，是四个三次混合函数，其定义如下：

$\left\{\begin{array}{c}{f}_1\left(t\right)=2t^3-3t^2+1\\ f_2\left(t\right)=-2t^3+3t^2\\ f_3\left(t\right)=t{(t-1)}^2\\ f_4\left(t\right)=t^2(t-1)\end{array}\right.---2)$

结合公式1)和2)，易得

$\left\{\begin{array}{c}{a}_0=2P_0-2P_1+P_0^{\&prime;}+P_1^{\&prime;}\\ a_1=-3P_0+3P_1-2P_0^{\&prime;}-P_1^{\&prime;}\\ a_2=P_0^{\&prime;}\\ a_3=P_0\end{array}\right.---3)$

该样条由端点P₀，P₁与切向量P′₀，P′₁所决定；

设另一条Ferguson样条表示如下：

$\stackrel{\&OverBar;}{r}\left(t\right)={\stackrel{\&OverBar;}{P}}_0{f}_1\left(t\right)+{\stackrel{\&OverBar;}{P}}_1{f}_2\left(t\right)+{\stackrel{\&OverBar;}{P}}_0^{\&prime;}{f}_3\left(t\right)+{\stackrel{\&OverBar;}{P}}_1^{\&prime;}{f}_4\left(t\right)---4)$

该样条由端点与切向量

决定；

将满足

条件的两个Ferguson样条r(t)与

连接到一起，描述一条机器人移动轨线。

步骤3，由Ferguson样条的端点与相应的切向量构造优化粒子。

每两个相邻的样条共用一个端点与相应的切向量，构造出的优化粒子P＝{P_0xP_0yP′_0xP′_0yP_1xP_1yP′_1xP′_1y P_2xP_2yP′_2x P′_2y…P_nx P_ny P′_nx P′_ny}，P_mx，m＝0，1，…，n代表第m个端点的横坐标，P_my，m＝0，1，…，n代表第m个端点的纵坐标，P′_mx，m＝0，1，…，n代表第m个端点切向量的横坐标，P′_my，m＝0，1，…，n代表第m个端点切向量的纵坐标，即一条二维路径轨线由4(n+1)个Ferguson样条连接而成，如图3所示。

步骤4：定义适应度函数。

(4.1)定义最短性惩罚函数：

$f_1=\frac{l}{l_{\min }}---5)$

其中，l_min为当前点到目标点的欧氏距离，l是轨线的长度，由下式计算：

$l={\&Integral;}_0^1\sqrt{{\left(x^{\&prime;}\left(t\right)\right)}^2+{\left(y^{\&prime;}\left(t\right)\right)}^2}\mathrm{dt};---6)$

(4.2)定义安全性惩罚函数：

$f_2=\left\{\begin{array}{cc}1,& d_{\min }>D_{\mathrm{safe}}\\ \exp (\frac{D_{\mathrm{safe}}+1}{d_{\min }+1}-1),& 0<d_{\min }\&le;D_{\mathrm{safe}}\end{array}\right.---7)$

D_safe为反映障碍物影响的常数，d_min为轨线与障碍物之间的最小距离，由下式计算：

$d_{\min }=\underset{o\&Element;O}{\min }\underset{t\&Element;\left[\mathrm{0,1}\right]}{\min }\sqrt{{(x\left(t\right)-O_x)}^2+{(y\left(t\right)-O_y)}^2}---8)$

式中，x(t)，y(t)由公式1)确定，O为机器人空间的所有障碍物的集合，O_x为障碍物的横坐标，O_y为障碍物的纵坐标；

(4.3)由最短性惩罚函数和安全性惩罚函数定义适应度函数：

f＝f₁+αf₂    9)

其中，α为权值系数，用来调整最短性和安全性在路径规划中所占的权重。

步骤5：对适应度函数进行全局优化，得到最优的移动机器人路径。

参照图2，本步骤的具体实施如下：

(5.1)初始化两个种群：设定LVPSO两个种群的参数及障碍物影响参数，最大进化代数为500，两个子群人口均为20，种群A的收缩因子χ^A与种群B收缩因子χ^B相同，即χ^A＝χ^B＝0.729，种群A的加速因子

种群B的加速因子

障碍物影响常数D_safe＝120，最短性和安全性权重系数α＝1；为两个种群中的每个粒子随机赋予初始位置和初始速度；

(5.2)评价粒子：根据公式6)计算每个粒子对应的样条的轨线长度l，利用公式8)计算每个粒子对应样条上所有点距离最近的障碍物的距离d_min，并根据公式9)计算两个种群中每个粒子的适应度函数；

(5.3)更新两个种群的粒子个人最优位置和全局最优位置：确定当前代k种群A每个粒子个人历史最优位置p_i ^A(k)，i＝1，…，N_A和种群A迄今为止所经历的最优位置p_g ^A(k)，其中N_A为种群A的粒子数；确定当前代k种群B每个粒子个人历史最优位置p_j ^B(k)，j＝1，…，N_B和种群B迄今为止所经历的最优位置p_g ^B(k)，其中N_B为种群B的粒子数；

(5.4)更新粒子：对种群A和种群B均采用带收缩因子的粒子群优化算法，分别引入种群A和种群B的群间协作项c₃ ^Ar_3i ^A(p_g(k)-x_i ^A(k))和c₃ ^Br_3i ^B(p_g(k)-x_i ^B(k))，按如下公式对种群A和种群B的速度和位置进行更新：

$v_i^A(k+1)={\mathrm{\&chi;}}^A(v_i^A\left(k\right)+c_1^A{r}_{1i}^A(p_i^A\left(k\right)-x_i^A\left(k\right)))$

$+c_2^A{r}_{2i}^A(p_g^A\left(k\right)-x_i^A\left(k\right))+c_3^A{r}_{3i}^A(p_g\left(k\right)-x_i^A\left(k\right))---10)$

$x_i^A(k+1)=x_i^A\left(k\right)+v_i^A(k+1)---11)$

$v_j^B(k+1)={\mathrm{\&chi;}}^B(v_j^B\left(k\right)+c_1^B{r}_{1j}^B(p_j^B\left(k\right)-x_j^B\left(k\right))---12)$

$+c_2^B{r}_{2j}^B(p_g^B\left(k\right)-x_j^B\left(k\right))+c_3^B{r}_{3j}^B(p_g\left(k\right)-x_j^B\left(k\right))$

$x_j^B(k+1)=x_j^B\left(k\right)+v_j^B(k+1)---13)$

其中，i为种群A粒子序号，i∈(1，2，…，N_A)；j为种群B粒子序号，j∈(1，2，…，N_B)；v_i ^A为种群A中第i个粒子的速度；v_j ^B为种群B中第j个粒子的速度；x_i ^A为种群A中第i个粒子的位置；x_j ^B为种群B中第j个粒子的位置；χ^A为种群A的收缩因子，χ^B为种群B的收缩因子；c₁ ^A、c₂ ^A、c₂ ^A为种群A的加速因子；c₁ ^B、c₂ ^B、c₃ ^B为种群B的加速因子；r₁、r₂和r₃是区间[0，1]之间的随机数；p_i ^A为种群A中第i个粒子的个人历史最优位置；p_j ^B为种群B中第j个粒子的个人历史最优位置；v_i ^A(k+1)为种群A更新后的粒子速度，x_i ^A(k+1)为种群A更新后的粒子位置；v_j ^B(k+1)为种群B更新后的粒子速度，x_j ^B(k+1)为种群B更新后的粒子位置；

(5.5)将整个种群的全局最优位置更新为：

$p_g\left(t\right)=\arg \left\{\min \left(f\right[p_g^A\left(t\right)],f[p_g^B\left(t\right)\left]\right)\right\};---14)$

(5.6)检验终止条件：如果当前的进化次数达到预设的最大进化代数或优化结果达到预设误差，则寻优结束，输出最优解及最优值，否则将返回(5.2)继续进行搜索。

步骤6：根据优化结果输出由Ferguson样条描述的机器人路径。

本发明的效果可通过以下仿真进一步说明：

图4-图9所示为本发明进行路径规划的实例。其中图4所示为本发明进行路径规划粒子群初始化后的仿真图，图5所示为本发明进行路径规划粒子群进化100代后的仿真图，图6所示为本发明进行路径规划粒子群进化200代后的仿真图，图7所示为本发明进行路径规划粒子群进化300代后的仿真图，图8所示为本发明进行路径规划粒子群进化400代后的仿真图，图9所示为本发明进行路径规划粒子群进化500代后的仿真图。从图4-图9可以看出，本发明由于采用Ferguson样条表示路径，并采用基于双群协同竞争的粒子群算法进行全局优化，规划路径具有很好的平滑性、安全性和最短性。

图10所示为本发明进行路径规划的收敛曲线。从图10可以看出，本发明由于采用双群协同竞争粒子群优化方法，进化200代后可快速收敛到全局最优路径。

Claims (3)

1.一种基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法，包括以下步骤：

(1)根据机器人自身携带的摄像头、声纳、激光测距仪传感器获取的信息对移动机器人运动环境进行建模；

(2)借助Ferguson样条连接描述移动机器人路径，该样条的二维空间表示式为：

r(t)＝(x(t)，y(t))＝P₀f₁(t)+P₁f₂(t)+P′₀f₃(t)+P′₁f₄(t)＝a₀+a₁t+a₂t²+a₃t³    1)

其中，

$\left\{\begin{array}{c}{a}_0=2P_0-2P_1+P_0^{\&prime;}+P_1^{\&prime;}\\ a_1=-3P_0+3P_1-2P_0^{\&prime;}-P_1^{\&prime;}\\ a_2=P_0^{\&prime;}\\ a_3=P_0\end{array}\right.---2)$

每个样条由端点P₀，P₁与切向量P′₀，P′₁所决定；

(3)每两个相邻的Ferguson样条共用一个端点与相应的切向量，构造由n个Ferguson样条连接而成的二维路径轨线优化粒子P＝{P_0xP_0yP′_0xP′_0yP_1xP_1yP_1xP′_1yP_2xP_2yP′_2xP′_2y…P_nxP_nyP′_nxP′_ny}，P_mx，m＝0，1，…，n代表第m个端点的横坐标，P_my，m＝0，1，…，n代表第m个端点的纵坐标，P′_mx，m＝0，1，…，n代表第m个端点切向量的横坐标，P′_my，m＝0，1，…，n代表第m个端点切向量的纵坐标；

(4)定义适应度函数：

f＝f₁+αf₂    9)

其中，α为权值系数，用来调整最短性和安全性在路径规划中所占的权重；

f₁为最短性惩罚函数，定义为：

l_min为当前点到目标点的欧氏距离，l是轨线的长度；

f₂为安全性惩罚函数，定义为：

$f_2=\left\{\begin{array}{cc}1,& d_{\min }>D_{\mathrm{safe}}\\ \exp (\frac{D_{\mathrm{safe}}+1}{d_{\min }+1}-1),& 0<d_{\min }\&le;D_{\mathrm{safe}}\end{array}\right.---7)$

式中，D_safe为反映障碍物影响的常数，d_min为轨线与所有障碍物之间的最小欧氏距离；

(5)对适应度函数进行如下全局优化：

(5.1)初始化两个种群，设定两个种群A和B的参数，并为两个种群中的每个粒子随机赋予初始位置和初始速度；

(5.2)计算每个粒子对应的样条的轨线长度l，并根据步骤(1)所建立模型计算轨线与障碍物之间的最小欧氏距离d_min，并根据公式9)计算两个种群中每个粒子的适应度函数；

(5.3)确定当前代k种群A每个粒子个人历史最优位置p_i ^A(k)，i＝1，…，N_A和种群A迄今为止所经历的最优位置p_g ^A(k)，其中N_A为种群A的粒子数；确定当前代k种群B每个粒子个人历史最优位置p_j ^B(k)，j＝1，…，N_B和种群B迄今为止所经历的最优位置p_g ^B(k)，其中N_B为种群B的粒子数；

(5.4)更新每个粒子的速度和位置；

(5.5)更新整个种群的全局最优位置；

$p_g\left(k\right)=\arg \left\{\min \left(f\right[p_g^A\left(k\right)],f[p_g^B\left(k\right)\left]\right)\right\}---14)$

(5.6)检验终止条件，如果当前的进化次数达到预设的最大进化代数或优化结果达到预设误差，则寻优结束，输出最优解及最优值，否则将返回(5.2)继续进行搜索；

(6)根据优化结果输出由Ferguson样条描述的机器人路径。

2.根据权利要求1所述的基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法，其中步骤(3)所述的优化粒子P，它的参数数量为4(n+1)个，n为用于描述路径的Ferguson样条的数量。

3.根据权利要求1所述的基于双群协同竞争粒子群和Ferguson样条的机器人路径规划方法，其中步骤(5.4)所述的更新粒子速度和位置，引入种群A和种群B的群间协作项c₃ ^Ar_3i ^A(p_g(k)-x_i ^A(k))和c₃ ^Br_3i ^B(p_g(k)-x_i ^B(k))，具体公式如下：

$v_i^A(k+1)={\mathrm{\&chi;}}^A(v_i^A\left(k\right)+c_1^A{r}_{1i}^A(p_i^A\left(k\right)-x_i^A\left(k\right))$

$+c_2^A{r}_{2i}^A(p_g^A\left(k\right)-x_i^A\left(k\right))+c_3^A{r}_{3i}^A(p_g\left(k\right)-x_i^A\left(k\right)))---10)$

$x_i^A(k+1)=x_i^A\left(k\right)+v_i^A(k+1)---11)$

$v_j^B(k+1)={\mathrm{\&chi;}}^B(v_j^B\left(k\right)+c_1^B{r}_{1j}^B(p_j^B\left(k\right)-x_j^B\left(k\right))$

$+c_2^B{r}_{2j}^B(p_g^B\left(k\right)-x_j^B\left(k\right))+c_3^B{r}_{3j}^B(p_g\left(k\right)-x_j^B\left(k\right)))---12)$

$x_j^B(k+1)=x_j^B\left(k\right)+v_j^B(k+1)---13)$

其中，i为种群A粒子序号，i∈(1，2，…，N_A)；j为种群B粒子序号，j∈(1，2，…，N_B)；v_i ^A为种群A中第i个粒子的速度；v_j ^B为种群B中第j个粒子的速度；x_i ^A为种群A中第i个粒子的位置；x_j ^B为种群B中第j个粒子的位置；种群A和种群B的收缩因子χ^A＝χ^B＝0.729；种群A的加速因子

种群B的加速因子

r₁、r₂和r₃是区间[0，1]之间的随机数；p_i ^A为种群A中第i个粒子的个人历史最优位置；p_j ^B为种群B中第j个粒子的个人历史最优位置；v_i ^A(k+1)为种群A更新后的粒子速度，x_i ^A(k+1)为种群A更新后的粒子位置；v_j ^B(k+1)为种群B更新后的粒子速度，x_j ^B(k+1)为种群B更新后的粒子位置。

Images