CN114397896B - 一种改进粒子群算法的动态路径规划方法 - Google Patents

Abstract

本发明公开了一种改进粒子群算法的动态路径规划方法，其特征在于：包括以下步骤：步骤一、采用改进粒子群算法生成全局路径，将全局路径划分为由{S₁G₁,S₂G₂,…S_nG_n}组成的n段局部路径；步骤二、对局部路径S₁G₁利用动态窗口法DWA进行路径规划，包括搭建机器人运动学模型、搭建机器人速度模型、构建动态窗口法DWA的目标函数，根据目标函数选择最优的轨迹，记录机器人到达位置G₁的航向角

步骤三、对局部路径S₂G₂利用动态窗口法DWA进行局部路径规划，并继承上一次运动规划的航向角；步骤四、重复利用动态窗口法DWA对路径S₃G₃,…S_nG_n依次进行局部路径规划；步骤五、输出完整的最终路径。本发明具有能缩短规划路径长度，提高规划路径平滑性、实时性等的特点。

Description

一种改进粒子群算法的动态路径规划方法

技术领域

本发明涉及机器人运动规划领域，具体涉及一种改进粒子群算法的动态路径规划方法。

背景技术

移动机器人的动态路径规划是指在动态环境中规划出一条满足某种条件(通常是指最优的)的抵达目标点的无碰撞路径。可以应用于智能仓储领域，减少人力物力，提高整体运输效率。现有技术，包括环境已知的全局路径规划和环境未知的局部路径规划。全局规划算法主要针对静态已知环境，提前进行地图构建，可以保证机器人以较短路径达到目标位置。常用的全局路径规划算法有智能仿生算法(遗传算法、蚁群算法、粒子群算法、蜂群算法(ABC)等)、图搜索类算法(A*算法等)。局部路径规划是指机器人利用携带传感器采集的局部环境信息进行路径规划，具有实时性和灵活性，可用于未知环境，缺点是缺少全局性，宜陷入局部最优，主要包括动态窗口算法(DWA)、人工势场法等。全局路径规划和局部路径规划侧重点不同，前者进行实时避障，后者则统筹全局，旨在获得全局最优路径。想要获得效果显著的移动机器人的动态路径规划，需要将二者结合起来。粒子群算法(PSO)因其结构简单，易于实现，广泛应用于全局路径规划中，但现有的PSO算法精度低、转折角多，生成路径不平滑、实时路径差；而动态窗口法是局部路径规划算法，实时性强，但是易陷入局部最优。

发明内容

本发明的目的在于克服上述缺点而提出了一种能够缩短路径长度，提高平滑性、实时性的改进粒子群算法的动态路径规划方法。

本发明的一种改进粒子群算法的动态路径规划方法，其中：该方法包括以下步骤:

步骤一、采用改进粒子群算法生成全局路径，提取全局路径生成的关键节点{P₀,P₁,…P_n+1}作为局部目标点，即每一次局部路径规划的起点为S{S₁,S₂,…S_n}＝{P₀,P₁,…P_n}；每一次局部路径规划的局部目标点为G{G₁,G₂,…G_n}＝{P₁,P₂,…P_n+1}，将路径划分成由{S₁G₁,S₂G₂,…S_nG_n}组成的n段局部路径；

所述改进粒子群算法提取全局路径的具体步骤包括：

(1)输入起点、终点、全局环境信息；建立初始种群，维度为D，粒子数为N，初始化位置X，速度V；

(2)利用适应度函数Fitness计算个体最优位置P_i ^t和全局最优位置G^t；

所述适应度函数Fitness，包括路径长度函数、安全程度函数；

所述路径长度函数f₁：用来计算移动机器人从起始点S和目标点G之间的路径长度，公式为：

其中，(x_i,y_i)和(x_i+1,y_i+1)分别代表起始点和目标点之间的路径节点P_i和P_i+1的位置坐标；

所述安全程度函数f₂：移动机器人的路径规划在考虑从起始点S到目标点G的路径长度的同时，要保证生成路径不会与障碍物发生碰撞，将环境模型中的障碍物用圆形表示，m为障碍物数量，对于不规则障碍物，采用圆形近似处理；将障碍物半径设定为安全阈值R＝{rad₀,rad₁,…,rad_m}，为获得无碰撞路径，需保证路径节点以及路径节点连线与障碍物的距离dis都大于安全阈值；路径节点为k个，_ψ为危险性因子，对于穿过障碍物的节点判定为危险，给予惩罚，δ为权重系数，则安全程度函数即惩罚函数为：

则适应度函数Fitness为：Fitness＝f₁+f₂；

(3)对粒子位置X、粒子速度V进行更新，生成优化的种群；

(4)基于粒子群算法PSO生成的种群作为差分进化算法DE的初始种群，进行变异、交叉、选择的操作；

(5)根据步骤(4)得到差分进化算法DE下的最优位置与粒子群算法PSO生成的全局最优位置G^t进行比较，选择最小的进行更新；

(6)输出最优全局路径Path＝{P₀,P₁,...,P_n,P_n+1}；

步骤二、对局部路径S₁G₁利用动态窗口法DWA进行路径规划：

(1)初始化速度空间，航向角θ₁ ^S，建立由线速度和角速度构成的二维速度空间(v,ω)；

(2)搭建机器人运动学模型：移动机器人的运动状态由速度空间(v,ω)进行描述；

(3)搭建机器人速度模型：二维速度空间中存在无穷组，需根据机器人实际情况进行约束，获得可行的速度范围，包括速度约束、动力学约束、制动距离约束；

(4)构建动态窗口法DWA的目标函数：

目标函数用来对在约束下生成的速度空间对应的轨迹进行评估，主要由与目标位置代价函数Head(v,ω)、与障碍物距离代价函数Obs(v,ω)、速度代价函数Vel(v,ω)组成，故目标函数即总代价函数Cost(v,ω)表达式如下：

Cost(v,ω)＝αHead(v,ω)+βObs(v,ω)+γVel(v,ω)

式中，Head(v,ω)表示移动机器人与目标位置连线与当前位置航向之间夹角；Obs(v,ω)表示与障碍物最近距离；Vel(v,ω)表示当前模拟的速度代价；其中α，β，γ为加权系数；

(5)根据目标函数选择最优的轨迹，通过对应的二维速度空间(v,ω)驱动移动机器人进行路径规划；记录机器人到达位置G₁的航向角θ₁ ^G；

步骤三、对局部路径S₂G₂利用动态窗口法DWA进行局部路径规划，令

即对第二段路径进行局部路径规划时初始航向角继承上一阶段的航向角，以保证生成路径的平滑性；

步骤四、重复利用动态窗口法DWA对路径S₃G₃,…S_nG_n，依次进行局部路径规划；

步骤五、记录每一段局部路径规划路线{S₁G₁,S₂G₂,…S_nG_n}，最后输出完整的最终路径。

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(3)中的对粒子位置X、粒子速度V进行更新公式为：

式中，V_i ^t+1为第t+1次迭代时第i个粒子的速度；

为第t+1次迭代时第i个粒子的速度位置；P_i ^t为第t次迭代时第i个粒子的个体最优位置；G^t为第t次迭代时的全局最优位置；N为种群的粒子数；w称为惯性权重；r₁和r₂为[0,1]范围内随机数；c₁和c₂为大于或等于零的数值，其中，c₁为认知因子，控制个体粒子局部范围内的变化，c₂为社会因子，控制粒子群中的最优粒子对整个区域的影响，两个学习因子协同运作，促进粒子不断向优化方向靠拢。

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(4)中的变异操作：差分进化算法DE基于父代向量x_i(t),i＝1,2,…,n，随机选择个体进行差分变异，变异策略表示为：

h_i(t)＝x_p1(t)+F·(x_p2(t)-x_p3(t))

其中，h_i(t)为生成的变异向量；x_p1(t),x_p2(t),x_p3(t)为种群在第t代中编号为p1,p2,p3的解向量；F为缩放因子，取值范围为(0,1.2]。

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(4)中的交叉操作：父代向量x_i(t),i＝1,2,...,n以交叉概率P_c和变异向量h_i(t)进行杂交，产生新的个体向量，成为试验向量v_i,j(t)，试验向量中的第j，j＝1,2,…,D，维根据交叉概率P_c在父代和变异向量中进行选择，公式如下：

式中：rand为[0,1]范围内的均匀随机数；Pc为交叉概率因子，P_c∈[0,1]；j_rand为[0,1]范围内的随机正整数，使得至少存在一个分量为变异向量产出，进而保证产生新向量。

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(4)中的选择操作：经过交叉变异操作后生成的向量v_i(t+1)与父代向量x_i(t)进行比较，保留适应度值Fitness(*)较好的向量，公式如下：

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤二中搭建机器人运动学模型，所述移动机器人的运动状态由速度空间(v,ω)进行描述，假设时间间隔Δt很小，机器人在Δt内即(t-t+1)内为匀速直线运动，则运动学模型如下：

其中，x_t，x_t+1分别表示移动机器人在t，t+1时刻的x坐标，y_t，y_t+1分别表示移动机器人在t，t+1时刻y坐标，θ_t，θ_t+1表示移动机器人在t，t+1时刻处航向角，v表示线速度，ω表示角速度。

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤二中搭建机器人速度模型的速度约束：移动机器人速度受机器人本身能到达的或者是安全性因素下的最大最小速度制约，其速度约束空间用V_m表示：

V_m＝{(v,ω)|v∈[V_min,V_max],ω∈[ω_min,ω_max]}

式中，V_min,V_max,ω_min,ω_max表示移动机器人的最小，最大线速度和最小，最大角速度。

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤二中搭建机器人速度模型的动力学约束：根据应用的移动机器人的电机性能不同，加减速度不同，故需要施加动力学性能约束，即：

V_d＝{(v,ω)v∈[v_c-v_bΔt,v_c+v_aΔt],ω∈[ω_c-ω_bΔt,ω_c+ω_aΔt]}

式中，V_d表示动力学约束空间，v_c,ω_c表示移动机器人当前线速度，角速度；v_a,ω_a表示最大加速度；v_b,ω_b表示最大减速度。

上述的改进粒子群算法的动态路径规划方法，其中：所述步骤二中搭建机器人速度模型的制动距离约束：为保证安全性，基于最大减速度要求，移动机器人在运动过程中需要保证在撞到障碍物前停止，即速度减为0，V_a表示制动距离约束空间，即：

V_a＝{(v,ω)|v≤(2Dis(v,ω)v_b)^1/2,ω≤(2Dis(v,ω)ω_b)^1/2}

式中，Dis(v,ω)表示二维速度空间(v,ω)对应的轨迹与障碍物之间的最小距离。

本发明与现有技术的相比，具有明显的有益效果，由以上方案可知，所述改进粒子群算法首先利用差分进化算法改进粒子群算法寻优机制，对粒子群算法进行优化，构造新的适应度函数，实现对全局的路径规划；然后，根据生成的路径，提取关键节点作为动态窗口法的局部目标点，采用动态窗口法进行局部路径规划，使起始航向角继承上一次到达局部目标点的航向角，生成从起始点到目标点的最短且平滑的路径。总之，本发明具有能缩短规划路径长度，提高规划路径平滑性、实时性等的特点。

以下通过具体实施方式，进一步说明本发明的有益效果。

附图说明

图1为本发明的流程示意图；

图2为本发明在场景2下的路径规划图。

具体实施方式

以下结合附图及较佳实施例，对依据本发明提出的一种改进粒子群算法的动态路径规划的具体实施方式、特征及其功效，详细说明如后。

参见图1，本发明的一种改进粒子群算法的动态路径规划方法，其中：该方法包括以下步骤:

步骤一、采用改进粒子群算法生成全局路径，提取全局路径生成的关键中节点{P₀,P₁,…P_n+1}作为局部目标点，即每一次局部路径规划的起点为S{S₁,S₂,…S_n}＝{P₀,P₁,…P_n}；每一次局部路径规划的局部目标点为G{G₁,G₂,…G_n}＝{P₁,P₂,…P_n+1}，将路径划分成由{S₁G₁,S₂G₂,…S_nG_n}组成的n段局部路径；

所述改进粒子群算法提取全局路径的具体步骤包括：

(1)输入起点、终点、全局环境信息；建立初始种群，维度为D，粒子数为N，初始化位置X，速度V；

(2)利用适应度函数Fitness计算个体最优位置P_i ^t和全局最优位置G^t；

所述适应度函数Fitness，包括路径长度函数、安全程度函数；

所述路径长度函数f₁：用来计算移动机器人从起始点S和目标点G之间的路径长度，公式为：

其中，(x_i,y_i)和(x_i+1,y_i+1)分别代表起始点和目标点之间的路径节点P_i和P_i+1的位置坐标；

所述安全程度函数f₂：移动机器人的路径规划在考虑从起始点S到目标点G的路径长度的同时，要保证生成路径不会与障碍物发生碰撞，将环境模型中的障碍物用圆形表示，m为障碍物数量，对于不规则障碍物，采用圆形近似处理；将障碍物半径设定为安全阈值记为R＝{rad₀,rad₁,…,rad_m}，为获得无碰撞路径，需保证路径节点以及路径节点连线与障碍物的距离dis都大于安全阈值；路径节点为k个，ψ为危险性因子，对于穿过障碍物的节点判定为危险，给予惩罚，δ为权重系数，则安全程度函数即惩罚函数为：：

则用于路径规划的适应度函数Fitness为：Fitness＝f₁+f₂；

(3)对粒子位置X、粒子速度V进行更新，生成优化的种群；

所述对粒子位置X、粒子速度V进行更新公式为：

式中，V_i ^t+1为第t+1次迭代时第i个粒子的速度；

为第t+1次迭代时第i个粒子的速度位置；P_i ^t为第t次迭代时第i个粒子的个体最优位置；G^t为第t次迭代时的全局最优位置；D为目标搜索空间维度；N为种群的粒子数；w称为惯性权重；r₁和r₂为[0,1]范围内随机数；c₁和c₂为大于或等于零的数值，其中，c₁为认知因子，控制个体粒子局部范围内的变化，c₂为社会因子，控制粒子群中的最优粒子对整个区域的影响，两个学习因子协同运作，促进粒子不断向优化方向靠拢；

(4)基于粒子群算法PSO生成的种群作为差分进化算法DE的初始种群，进行变异、交叉、选择的操作；

所述变异操作：差分进化算法DE基于父代向量x_i(t),i＝1,2,…,n，随机选择个体进行差分变异，变异策略表示为：

h_i(t)＝x_p1(t)+F·(x_p2(t)-x_p3(t))

其中，h_i(t)为生成的变异向量；x_p1(t),x_p2(t),x_p3(t)为种群在第t代中编号为p1,p2,p3的解向量；F为缩放因子，取值范围为(0,1.2]；

所述交叉操作：父代向量x_i(t),i＝1,2,...,n以交叉概率P_c和变异向量h_i(t)进行杂交，产生新的个体向量，成为试验向量v_i,j(t)，试验向量中的第j，j＝1,2,…,D，维根据交叉概率P_c在父代和变异向量中进行选择，公式如下：

式中：rand为[0,1]范围内的均匀随机数；Pc为交叉概率因子，P_c∈[0,1]；j_rand为[0,1]范围内的随机正整数，使得至少存在一个分量为变异向量产出，进而保证产生新向量；

所述选择操作：经过交叉变异操作后生成的向量v_i(t+1)与父代向量x_i(t)进行比较，保留适应度值Fitness(*)较好的向量，公式如下：

(5)根据步骤(4)得到差分进化算法DE下的最优位置与粒子群算法PSO生成的全局最优位置G^t进行比较，选择最小的进行更新；

(6)输出最优全局路径Path＝{P₀,P₁,...,P_n,P_n+1}；

步骤二、对局部路径S₁G₁利用动态窗口法DWA进行路径规划

(1)初始化速度空间，航向角θ₁ ^S，建立由线速度和角速度构成的二维速度空间(v,ω)；

(2)搭建机器人运动学模型：移动机器人的运动状态由速度空间(v,ω)进行描述；假设时间间隔Δt很小，机器人在Δt内即(t-t+1)内为匀速直线运动，则运动学模型如下：

其中，x_t，x_t+1分别表示移动机器人在t，t+1时刻的x坐标，y_t，y_t+1分别表示移动机器人在t，t+1时刻y坐标，θ_t，θ_t+1表示移动机器人在t，t+1时刻处航向角，v表示线速度，ω表示角速度；

(3)搭建机器人速度模型：二维速度空间中存在无穷组，需根据机器人实际情况进行约束，获得可行的速度范围，包括速度约束、动力学约束、制动距离约束；

所述速度约束：移动机器人速度受机器人本身能到达的或者是安全性因素下的最大最小速度制约，其速度约束空间用_Vm表示：

V_m＝{(v,ω)|v∈[V_min,V_max],ω∈[ω_min,ω_max]}

式中，V_min,V_max,ω_min,ω_max表示移动机器人的最小，最大线速度和最小，最大角速度；

所述动力学约束：根据应用的移动机器人的电机性能不同，加减速度不同，故需要施加动力学性能约束，即：

V_d＝{(v,ω)|v∈[v_c-v_bΔt,v_c+v_aΔt],ω∈[ω_c-ω_bΔt,ω_c+ω_aΔt]}

式中，V_d表示动力学约束空间，v_c,ω_c表示移动机器人当前线速度，角速度；v_a,ω_a表示最大加速度；v_b,ω_b表示最大减速度；

所述制动距离约束：为保证安全性，基于最大减速度要求，移动机器人在运动过程中需要保证在撞到障碍物前停止即速度减为0，V_a表示制动距离约束空间，即：

V_a＝{(v,ω)|v≤(2Dis(v,ω)v_b)^1/2,ω≤(2Dis(v,ω)ω_b)^1/2}

式中，Dis(v,ω)表示二维速度空间(v,ω)对应的轨迹与障碍物之间的最小距离；

(4)构建动态窗口法DWA的目标函数：

目标函数用来对在约束下生成的速度空间对应的轨迹进行评估，主要由与目标位置代价函数Head(v,ω)、与障碍物距离代价函数Obs(v,ω)、速度代价函数Vel(v,ω)组成，故目标函数即总代价函数Cost(v,ω)表达式如下：

Cost(v,ω)＝αHead(v,ω)+βObs(v,ω)+γVel(v,ω)

式中，Head(v,ω)表示移动机器人与目标位置连线与当前位置航向之间夹角；Obs(v,ω)表示与障碍物最近距离；Vel(v,ω)表示当前模拟的速度代价；其中α，β，γ为加权系数；

(5)根据目标函数选择最优的轨迹，通过对应的二维速度空间(v,ω)驱动移动机器人进行路径规划；记录机器人到达位置G₁的航向角θ₁ ^G；

步骤三、对局部路径S₂G₂利用动态窗口法进行局部路径规划，令

即对第二段路径进行局部路径规划时初始航向角继承上一阶段的航向角，以保证生成路径的平滑性；

步骤四、重复利用动态窗口法DWA对路径S₃G₃,…S_nG_n依次进行局部路径规划；

步骤五、记录每一段局部路径规划路线{S₁G₁,S₂G₂,…S_nG_n}，最后输出完整的最终路径。

其中：

1传统的PSO算法

Kennedy于1995年提出PSO算法，通过模仿鸟群觅食行为来搜索目标函数的全局最优值。PSO算法的路径规划问题本质上是优化问题。将每个粒子视为优化问题的一个解。对粒子赋予两个属性：位置和速度，在搜索空间里面控制粒子的运动，使得粒子向最优解方向移动。

设定目标搜索空间维度为D、种群的粒子数为N，则粒子i的位置是X_i＝(x_i1,x_i2,…,x_iD)，i＝1,2,…,N；粒子i的速度是V_i＝(v_i1,v_i2,…,v_iD)，i＝1,2,…,N；粒子i所经历的个体最优位置是P_i＝(p_i1,p_i2,…,p_iD)；对整个粒子群进行判断后得到的全局最优位置G＝(g₁,g₂,…,g_D)是。粒子速度和位置更新公式如下：

式中，w称为惯性权重，使算法在迭代过程中对先前迭代的惯性速度进行自适应调整。即随着迭代次数的增大，w随之减小，使得在迭代初期，有较大的搜索速度，提高算法搜索能力，保证算法效率；在迭代后期，搜索速度下降，提高搜索精度，具体公式如下：

其中w_max和w_min为分别为w的最大值最小值，t为迭代次数，T为设定的最大迭代次数。r₁和r₂为[0,1]范围内随机数，目的是增加算法搜索范围；c₁和c₂为大于或等于零的数值，其中，c₁为认知因子，控制个体粒子局部范围内的变化，c₂为社会因子，控制粒子群中的最优粒子对整个区域的影响，两个学习因子协同运作，促进粒子不断向优化方向靠拢。P_i ^t为第t次迭代时第i个粒子的个体最优位置；G^t为第t次迭代时的全局最优位置；将算法的适应度函数设为Fitness，则个体最优位置和全局最优位置的表达式如下：

2算法改进

2.1改进的PSO-DE算法

面向传统PSO算法因为结构本身缺陷而产生的易早熟，收敛速度慢等不足，本发明利用结构简单，寻优能力强的差分进化(DE)算法，来促进PSO算法更好的寻找全局最优位置，提出一种改进的PSO-DE优化算法。

2.1.1适应度函数设计

针对优化算法设计应用于路径规划中的适应度函数，基于实际需求，评价指标主要包括安全程度、路径长度两个因素。设置起始点Start坐标为P₀，目标点Goal坐标为P_n+1，生成路径由Path＝{P₀,P₁,...,P_n,P_n+1}表示。

1.路径长度：

路径长度函数f₁用来计算移动机器人从起始点Start和目标点Goal之间的路径长度，可用式(6)来表示：

其中，(x_i,y_i)和(x_i+1,y_i+1)分别代表路径点P_i和P_i+1的位置坐标；

2.惩罚函数：

移动机器人的路径规划在考虑从Start到Goal的路径长度的同时，要保证生成路径不会与障碍物发生碰撞。路径与障碍物相交次数越多，生成路径的危险程度越大。

将环境模型中的障碍物用圆形表示，m为障碍物数量。对于不规则障碍物，采用圆形近似处理。将障碍物半径设定为安全阈值记为R＝{rad₀,rad₁,…,rad_m}，为获得无碰撞路径，需保证路径节点以及路径节点连线与障碍物的距离dis都大于安全阈值。路径节点为k个，ψ为危险性因子，对于穿过障碍物的节点判定为危险，给予惩罚，δ为权重系数，则惩罚函数为：

综上所述，应用于路径规划的适应度函数表达式为公式(7)：

Fitness＝f₁+f₂ (7)

2.1.2改进的PSO-DE寻优机制

改进的PSO-DE寻优机制实现流程如下：

(1)建立初始种群，维度为D，粒子数为N，初始化位置X，速度V；

(2)利用Fitness计算P_i ^t和G^t；

(3)根据表达式(1)(2)对X、V进行更新，生成优化的种群；

(4)基于PSO算法生成的种群作为DE算法的初始种群，进行“变异”，“交叉”，“选择”三个操作；

(5)DE算法的变异操作：

DE算法基于父代个体x_i(t),i＝1,2,…,n，随机选择个体进行差分变异，变异策略表示为“DE/子代生成方式/进行差分的组数/交叉方式”，本发明选择DE/rand/1的策略：

h_i(t)＝x_p1(t)+F·(x_p2(t)-x_p3(t)) (8)

其中，h_i(t)为生成的变异向量；x_p1(t),x_p2(t),x_p3(t)为种群在第t代中编号为p1,p2,p3的解向量，编号随机选取且与i两两互异；F为缩放因子，取值范围一般控制在(0,1.2]；

(6)DE算法的交叉操作：

父代向量x_i(t),i＝1,2,…,n以交叉概率P_c和变异向量h_i(t)进行杂交，产生新的个体向量，成为试验向量v_i,j(t)，试验向量中的第j(j＝1,2,…,D)维根据P_c在父代和变异向量中进行选择，公式如下：

式中：rand为[0,1]范围内的均匀随机数；Pc为交叉概率因子，P_c∈[0,1]；j_rand为[0,1]范围内的随机正整数，使得至少存在一个分量为变异向量产出，进而保证产生新向量；

(7)DE算法的选择操作：

经过交叉变异操作后生成的向量v_i(t+1)与父代向量x_i(t)进行比较，保留适应度值(Fitness(*))较好的向量，公式如下：

(8)根据5)，6)，7)得到DE算法下的最优位置与PSO算法生成的G^t进行比较，选择最小的进行更新；

(9)输出全局最优路径Path＝{P₀,P₁,...,P_n,P_n+1}。

2.2基于PSO-DE和动态窗口法的融合算法

改进的PSO-DE算法优化了全局路径规划的能力，但是仅针对静态环境，且路径存在尖点。动态窗口法是经典的局部路径规划算法，实用性强，但是易陷入局部最优，不具备全局性。本发明在对PSO算法进行优化后，进一步采用动态窗口实现动态环境的实时避障，同时提高生成路径的平滑性，满足机器人的动力学性能，提出基于改进粒子群算法和动态窗口法的融合算法。

该融合算法的基本原理：利用PSO-DE算法生成全局路径，提取关键节点作为动态窗口法的局部目标点，其中，影响生成路径平滑度的关键因子为航向角，本发明在每次利用动态窗口法进行局部路径规划时令起始航向角继承上一次到达局部目标点的航向角，进而保证路径的平滑性。

3实验分析

本发明的实验环境为运行内存8GB的64位WIN10操作系统，实验平台是集成的开发环境Anaconda3为保持良好的对比效果，在实验过程中，移动机器人的最大线速度、最小线速度、最大角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预估时间等参数依次设定为：1.4m/s、0m/s(设置为不倒车)、40°/s、0.2m/s2、40°/s2、0.01m/s、0.1°/s、0.1s、3s。路径规划的起点与终点保持一致，分别为(0,0)、(10,10)。

3.1全局路径规划

本实验在场景1下进行，将算法与传统的PSO算法，ABC算法的路径规划结果进行对比分析，场景1为方形和圆形障碍物混合地图，共有10个障碍物，x和y的域值为[0,10]。由于障碍物比较密集，选择路径节点数为3。利用经典测试函数对算法进行训练，确定改进的PSO-DE算法可以在较小的种群下寻找到最优结果，故而将所有算法种群大小设置为15，最大迭代次数设为100。

表1列出了20次运行结果的平均值(Mean)，最佳适应度值(Best)，最差适应度值(Worst)以及标准差(Std)，可以看出，PSO-DE算法相较于传统的启发式算法具有更好的路径寻优能力，生成的路径长度最短，且标准差值Std较小，具有较好的稳定性。

表1场景1下各算法进行路径规划结果

Table 1 Path planning results based on different algorithms inenvironment 1.

3.2局部路径规划

本实验在场景2下对改进的PSO-DE和动态窗口法融合的算法进行试验验证并与传统的动态窗口法进行比较，场景2为动静障碍物混合地图，共有8个静态圆形障碍物和一个匀速向左运动的方形障碍物，x和y的域值为[0,10]，Start为(0,0)，Goal终点为(10,10)。

图2可以看出融合算法平滑性好，且路径更短，将路径从17.786缩短到14.691。融合算法改善了全局路径规划算法平滑性差，实时性不好的问题，同时改善了局部路径规划算法易陷入局部最优的缺陷，生成的路径符合机器人的运动学模型。

总之，针对传统PSO算法存在的精度低、转折角多，只适用于静态环境，和传统动态窗口法效率低、易陷入局部最优等问题，本发明先通过改进的PSO-DE算法进行全局路径规划，生成关键中节点，再进行局部规划，可以成功进行动态障碍物的规避以及生成符合机器人动力学模型的最短平滑路径。

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (9)

1.一种改进粒子群算法的动态路径规划方法，其特征在于：该方法包括以下步骤：

步骤一、采用改进粒子群算法生成全局路径，提取全局路径生成的关键节点{P₀,P₁,…P_n+1}作为局部目标点，即每一次局部路径规划的起点为S{S₁,S₂,…S_n}＝{P₀,P₁,…P_n}；每一次局部路径规划的局部目标点为G{G₁,G₂,…G_n}＝{P₁,P₂,…P_n+1}，将路径划分成由{S₁G₁,S₂G₂,…S_nG_n}组成的n段局部路径；

所述改进粒子群算法提取全局路径的具体步骤包括：

(1)输入起点、终点、全局环境信息；建立初始种群，维度为D，粒子数为N，初始化位置X，速度V；

(2)利用适应度函数Fitness计算个体最优位置P_i ^t和全局最优位置G^t；

所述适应度函数Fitness，包括路径长度函数、安全程度函数；

所述路径长度函数f₁：用来计算移动机器人从起始点S和目标点G之间的路径长度，公式为：

其中，(x_i,y_i)和(x_i+1,y_i+1)分别代表起始点和目标点之间的路径节点P_i和P_i+1的位置坐标；

所述安全程度函数f₂：移动机器人的路径规划在考虑从起始点S到目标点G的路径长度的同时，要保证生成路径不会与障碍物发生碰撞，将环境模型中的障碍物用圆形表示，m为障碍物数量，对于不规则障碍物，采用圆形近似处理；将障碍物半径设定为安全阈值R＝{rad₀,rad₁,…,rad_m}，为获得无碰撞路径，需保证路径节点以及路径节点连线与障碍物的距离dis都大于安全阈值；路径节点为k个，ψ为危险性因子，对于穿过障碍物的节点判定为危险，给予惩罚，δ为权重系数，则安全程度函数即惩罚函数为：

if dis_k≤rad_k:rad＝0；else:ψ＝1,

则适应度函数Fitness为：Fitness＝f₁+f₂；

(3)对粒子位置X、粒子速度V进行更新，生成优化的种群；

(4)基于粒子群算法PSO生成的种群作为差分进化算法DE的初始种群，进行变异、交叉、选择的操作；

(5)根据步骤(4)得到差分进化算法DE下的最优位置与粒子群算法PSO生成的全局最优位置G^t进行比较，选择最小的进行更新；

(6)输出最优全局路径Path＝{P₀,P₁,...,P_n,P_n+1}；

步骤二、对局部路径S₁G₁利用动态窗口法DWA进行路径规划：

(1)初始化速度空间，航向角θ₁ ^S，建立由线速度和角速度构成的二维速度空间(v,ω)；

(2)搭建机器人运动学模型：移动机器人的运动状态由速度空间(v,ω)进行描述；

(3)搭建机器人速度模型：二维速度空间中存在无穷组，需根据机器人实际情况进行约束，获得可行的速度范围，包括速度约束、动力学约束、制动距离约束；

(4)构建动态窗口法DWA的目标函数：

目标函数用来对在约束下生成的速度空间对应的轨迹进行评估，主要由与目标位置代价函数Head(v,ω)、与障碍物距离代价函数Obs(v,ω)、速度代价函数Vel(v,ω)组成，故目标函数即总代价函数Cost(v,ω)表达式如下：

Cost(v,ω)＝αHead(v,ω)+βObs(v,ω)+γVel(v,ω)

式中，Head(v,ω)表示移动机器人与目标位置连线与当前位置航向之间夹角；Obs(v,ω)表示与障碍物最近距离；Vel(v,ω)表示当前模拟的速度代价；其中α，β，γ为加权系数；

(5)根据目标函数选择最优的轨迹，通过对应的二维速度空间(v,ω)驱动移动机器人进行路径规划；记录机器人到达位置G₁的航向角θ₁ ^G；

步骤三、对局部路径S₂G₂利用动态窗口法DWA进行局部路径规划，令

即对第二段路径进行局部路径规划时初始航向角继承上一阶段的航向角，以保证生成路径的平滑性；

步骤四、重复利用所述动态窗口法DWA对路径S₃G₃,…S_nG_n依次进行局部路径规划；即每次利用所述动态窗口法进行局部路径规划时令起始航向角继承上一次到达局部目标点的航向角；

步骤五、记录每一段局部路径规划路线{S₁G₁,S₂G₂,…S_nG_n}，最后输出完整的最终路径。

2.如权利要求1所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(3)中的对粒子位置X、粒子速度V进行更新公式为：

式中，V_i ^t+1为第t+1次迭代时第i个粒子的速度；

为第t+1次迭代时第i个粒子的速度位置；P_i ^t为第t次迭代时第i个粒子的个体最优位置；G^t为第t次迭代时的全局最优位置；N为种群的粒子数；w称为惯性权重；r₁和r₂为[0,1]范围内随机数；c₁和c₂为大于或等于零的数值，其中，c₁为认知因子，控制个体粒子局部范围内的变化，c₂为社会因子，控制粒子群中的最优粒子对整个区域的影响，两个学习因子协同运作，促进粒子不断向优化方向靠拢。

3.如权利要求1所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(4)中的变异操作：差分进化算法DE基于父代向量x_i(t),i＝1,2,…,n，随机选择个体进行差分变异，变异策略表示为：

h_i(t)＝x_p1(t)+F·(x_p2(t)-x_p3(t))

其中，h_i(t)为生成的变异向量；x_p1(t),x_p2(t),x_p3(t)为种群在第t代中编号为p1,p2,p3的解向量；F为缩放因子，取值范围为(0,1.2]。

4.如权利要求2所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(4)中的交叉操作：父代向量x_i(t),i＝1,2,...,n以交叉概率P_c和变异向量h_i(t)进行杂交，产生新的个体向量，成为试验向量v_i,j(t)，试验向量中的第j，j＝1,2,…,D，维根据交叉概率P_c在父代和变异向量中进行选择，公式如下：

式中：rand为[0,1]范围内的均匀随机数；Pc为交叉概率因子，P_c∈[0,1]；j_rand为[0,1]范围内的随机正整数，使得至少存在一个分量为变异向量产出，进而保证产生新向量。

5.如权利要求3所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤一中的改进粒子群算法提取全局路径的具体步骤(4)中的选择操作：经过交叉变异操作后生成的向量v_i(t+1)与父代向量x_i(t)进行比较，保留适应度值Fitness(*)较好的向量，公式如下：

6.如权利要求1所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤二中搭建机器人运动学模型，所述移动机器人的运动状态由速度空间(v,ω)进行描述，假设时间间隔Δt很小，机器人在Δt内即(t-t+1)内为匀速直线运动，则运动学模型如下：

其中，x_t，x_t+1分别表示移动机器人在t，t+1时刻的x坐标，y_t，y_t+1分别表示移动机器人在t，t+1时刻y坐标，θ_t，θ_t+1表示移动机器人在t，t+1时刻处航向角，v表示线速度，ω表示角速度。

7.如权利要求1所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤二中搭建机器人速度模型的速度约束：移动机器人速度受机器人本身能到达的或者是安全性因素下的最大最小速度制约，其速度约束空间用V_m表示：

V_m＝{(v,ω)|v∈[V_min,V_max],ω∈[ω_min,ω_max]}

式中，V_min,V_max,ω_min,ω_max表示移动机器人的最小，最大线速度和最小，最大角速度。

8.如权利要求6所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤二中搭建机器人速度模型的动力学约束：根据应用的移动机器人的电机性能不同，加减速度不同，故需要施加动力学性能约束，即：

V_d＝{(v,ω)|v∈[v_c-v_bΔt,v_c+v_aΔt],ω∈[ω_c-ω_bΔt,ω_c+ω_aΔt]}

式中，V_d表示动力学约束空间，v_c,ω_c表示移动机器人当前线速度，角速度；v_a,ω_a表示最大加速度；v_b,ω_b表示最大减速度。

9.如权利要求7所述的改进粒子群算法的动态路径规划方法，其特征在于：所述步骤二中搭建机器人速度模型的制动距离约束：为保证安全性，基于最大减速度要求，移动机器人在运动过程中需要保证在撞到障碍物前停止即速度减为0，V_a表示制动距离约束空间，即：

V_a＝{(v,ω)|v≤(2Dis(v,ω)v_b)^1/2,ω≤(2Dis(v,ω)ω_b)^1/2}

式中，Dis(v,ω)表示二维速度空间(v,ω)对应的轨迹与障碍物之间的最小距离。

Images