基于粒子群优化算法的移动机器人全局路径规划-附代码

基于粒子群优化算法的移动机器人全局路径规划-附代码

摘要：本文主要介绍一种基于粒子群优化算法的移动机器人全局路径规划算法。

1.问题描述与建模

对于移动机器人, 路径规划就是寻找其在环境中移动时所必须经过的点的集合. 如图1 所示, 在全局坐标系O-XY 中, S 为机器人的出发点, G 为终点.图中黑色实心填充的物体表示障碍. 机器人的路径规划即为寻找一个点的集合
P = { S , p 1 , p 2 , . . , P m , G } (1) P = \{S,p_1,p_2,..,Pm,G\} \tag{1} P={S,p1​,p2​,..,Pm,G}(1)
其中$(p_1,p_2,..,p_m)$为全局地图中一个点的序列,即规划目标. 对点$p_j$的要求是: $p_j$为非障碍点, $p_j$与相邻点的连线上不存在障碍点.

图1.路径产生过程

在全局地图中建立一个新的坐标系, 以 SG 作为 X ′轴, 垂直于 X′且经过 S 点的直线作为 Y′轴,X′, Y′轴的方向如图 1所示. 对应的坐标变换为：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{cc}cos\alpha & -sin\alpha \\ sin\alpha & cos\alpha \end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}xs\\ ys\end{array}\right]$$
其中: $(x,y),(x\prime ,y\prime )$ 分别为地图中某一点在不同坐标系 $O-XY$ 和 $S-X\prime Y\prime$下的坐标, $A$为坐标轴$X$ 与$X\prime$的夹角, $(xS,yS)$ 为 $S$ 点在坐标系 $O-XY$ 下的坐标。

将线段$SG$进行$(m+1)$ 等分, 在每一个等分点作垂线, 得到平行直线族$(l_1,l_2,\cdots ,l_m)$ , 它们与路径$P$的交点即为目标点序列$(p_1,p_2,\cdots ,p_m)$ . 定义 $S$ 为$p_0$ , $G$为 $p_{m+1}$ , 路径 $P$ 的长度 $LP$ 为:
$$LP=Ls{p}_1+\sum _{j=1}^{m-1}L{p}_j{p}_{j+1}+L{p}_{m}G=\sum _{j=0}^{m}L{p}_j{p}_{j+1}\text{\hspace{1em}(2)}$$
其中$L{p}_j{p}_{j+1}$ 表示点$p_j$ 与点$p_{j+1}$间的距离. 最终优化算法的目标函数即为求一条不存在障碍物的路径使得LP的值最小。

2.基于粒子群算法的路径规划

粒子群算法原理这里不在阐述，网上资料很多。粒子群的适应度函数为：
f i t n e s s = a r g m i n { L P } (2) fitness = argmin\{LP\} \tag{2} fitness=argmin{LP}(2)
约束条件为，路径不能经过障碍物。

3. 实验结果

粒子群相关参数设定如下：

%% 粒子群参数设定
%速度范围设定
Vmax = 2;
Vmin = - 2;
dim = lineNum-1;%变量维度，段数-1
fobj = @(x) fun(x,PointStart,PointEnd,ks,bs);%适应度函数
lb = 1; %变量下边界
ub = boundary-1;%变量上边界
SearchAgents_no = 30;%种群数量
Max_iteration = 1000;%最大迭代次数

设置不同的分段数如5，10，15。如结果如下图所示：


粒子算法的收敛曲线如下图所示：

4.参考文献

[1]孙波,陈卫东,席裕庚.基于粒子群优化算法的移动机器人全局路径规划[J].控制与决策,2005(09):1052-1055+1060.

5.Matlab代码

基于粒子群优化算法的移动机器人全局路径规划
个人资料介绍