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

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

时间：2024-07-01 23:58:46

文章目录

基于粒子群优化算法的移动机器人全局路径规划-附代码1.问题描述与建模2.基于粒子群算法的路径规划3. 实验结果4.参考文献5.Matlab代码

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

1.问题描述与建模

对于移动机器人, 路径规划就是寻找其在环境中移动时所必须经过的点的集合. 如图1 所示, 在全局坐标系O-XY 中, S 为机器人的出发点, G 为终点.图中黑色实心填充的物体表示障碍. 机器人的路径规划即为寻找一个点的集合

P={S,p1,p2,..,Pm,G}(1)P = \{S,p_1,p_2,..,Pm,G\} \tag{1} P={S,p1,p2,..,Pm,G}(1)

其中(p1,p2,..,pm)(p_1,p_2,..,p_m)(p1,p2,..,pm)为全局地图中一个点的序列,即规划目标. 对点pjp_jpj的要求是: pjp_jpj为非障碍点, pjp_jpj与相邻点的连线上不存在障碍点.

图1.路径产生过程

在全局地图中建立一个新的坐标系, 以 SG 作为 X ′轴, 垂直于 X′且经过 S 点的直线作为 Y′轴,X′, Y′轴的方向如图 1所示. 对应的坐标变换为：

[x′y′]=[cosα−sinαsinαcosα][xy]+[xsys]\left[\begin{matrix}x'\\ y'\end{matrix}\right] = \left[\begin{matrix}cos\alpha&-sin\alpha\\ sin\alpha&cos\alpha\end{matrix}\right]\left[\begin{matrix}x\\ y\end{matrix}\right] + \left[\begin{matrix}xs\\ ys\end{matrix}\right] [x′y′]=[cosαsinα−sinαcosα][xy]+[xsys]

其中: (x,y),(x′,y′)(x, y) , ( x′, y′)(x,y),(x′,y′) 分别为地图中某一点在不同坐标系 O−XYO-X YO−XY 和 S−X′Y′S-X ′Y′S−X′Y′下的坐标, AAA为坐标轴XXX 与X′X′X′的夹角, (xS,yS)( x S , y S )(xS,yS) 为 SSS 点在坐标系 O−XYO-X YO−XY 下的坐标。

将线段SGSGSG进行(m+1)( m + 1)(m+1) 等分, 在每一个等分点作垂线, 得到平行直线族(l1,l2,⋯,lm)( l_1 , l_2 , ⋯, l_m )(l1,l2,⋯,lm) , 它们与路径PPP的交点即为目标点序列(p1,p2,⋯,pm)( p_1 , p_2 , ⋯, p_m )(p1,p2,⋯,pm) . 定义 SSS 为p0p_0p0 , GGG为 pm+1p_{m+ 1}pm+1 , 路径 PPP 的长度 LPLPLP 为:

LP=Lsp1+∑j=1m−1Lpjpj+1+LpmG=∑j=0mLpjpj+1(2)LP=Lsp_1+\sum_{j=1}^{m-1}Lp_jp_{j+1}+Lp_mG=\sum_{j=0}^mLp_jp_{j+1}\tag{2} LP=Lsp1+j=1∑m−1Lpjpj+1+LpmG=j=0∑mLpjpj+1(2)

其中Lpjpj+1Lp_jp_{j+1}Lpjpj+1 表示点pjp_jpj 与点pj+1p_{j+1}pj+1间的距离. 最终优化算法的目标函数即为求一条不存在障碍物的路径使得LP的值最小。

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

粒子群算法原理这里不在阐述，网上资料很多。粒子群的适应度函数为：

fitness=argmin{LP}(2)fitness = argmin\{LP\} \tag{2} fitness=argmin{LP}(2)

约束条件为，路径不能经过障碍物。

3. 实验结果

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

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

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

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