1 简介
移动机器人的路径规划是移动机器人研究领域中的一个热点问题。
2 部分代码
Xo=[0,0];%起点位置
k=10;%计算引力需要的增益系数
K=0;%初始化
m=1;%计算斥力的增益系数，自己设定
d=2;%障碍影响距离，当障碍和车的距离大于这个距离时，斥力为0，即不受该障碍的影响,自己设定
n=10;%障碍个数
l=0.5;%步长
J=200;%循环迭代次数
X_target=[10,10];
X_obs=[1 1.5;3 3;4 4.5;3 6;6 2.5;5.5 7;8 8.5;9,9.5;10 5;7 6];%这个向量是n*2维，障碍的位置
X_robot=Xo;%X_robot是机器人的定位坐标,将车的起始坐标Xo赋给X_robt
%***************初始化结束，开始主体循环******************/
for j=1:J
RobotPoint(j,1)=X_robot(1);%赋初值,RobotPoint存放机器人走过的每个点的坐标，刚开始先将起点放进该向量
RobotPoint(j,2)=X_robot(2);
%调用计算角度模块
[angle_at,angle_re]=compute_angle(X_robot,X_target,X_obs,n);%angle_at,angle_re是计算出来的机器人和目标/障碍之间的与X轴之间的夹角，统一规定角度为逆时针方向
%调用计算引力模块,x_at/y_at是引力在x/y轴的分量之和
[x_at,y_at]=compute_attraction(X_robot,X_target,k,angle_at);%机器人坐标，目标点，增益常数，角度
%调用斥力模块
[x_re,y_re]=compute_repultion(X_robot,X_target,X_obs,m,angle_re,n,d);%输入参数为当前坐标，Xsum是障碍物的坐标向量，增益常数,障碍物方向的角度,障碍物个数,影响阈值
%计算合力在x,y轴的分量
F_xj=x_at+x_re;
F_yj=y_at+y_re;
%计算合力与x轴夹角
if F_xj>0
angle_F(j)=atan(F_yj/F_xj);
else
angle_F(j)=pi+atan(F_yj/F_xj);
end
%下一步行进方向
Xnext(1)= X_robot(1)+l*cos(angle_F(j));
Xnext(2)= X_robot(2)+l*sin(angle_F(j));
%保存机器人的每一个位置
X_robot=Xnext;
%判断是否到达目标点,距离在一个步长之内认为到达目标点
r_target=sqrt((X_robot(1)-X_target(1))^2+(X_robot(2)-X_target(2))^2);
%考虑靠近目标点的情况，尽量直线运动，防止反复来回震荡
if(r_target>l&&r_target<2*l)
[d_obs_line] = distance_fuzzyControl( X_robot,X_target,X_obs,n);
if( d_obs_line>=0.5)%far,不会影响机器人的直线运动
Xnext(1)= X_robot(1)+l*cos(angle_at);
Xnext(2)= X_robot(2)+l*sin(angle_at);
X_robot=Xnext;
end
end
%判断是否到达目标点,距离在一个步长之内认为到达目标点
if(r_target<=l)
K=j;%记录迭代到多少次，到达目标。
break;
end%如果不符合if的条件，重新返回循环，继续执行。
end%大循环结束
%***********************************画出规划路径*************************
x=RobotPoint(:,1);
y=RobotPoint(:,2);
x_obs=[1,3,4,3,6,5.5,8,9,10,7];%障碍的x坐标
y_obs=[1.5,3,4.5,6,2.5,7,8.5,9.5,5,6];%障碍的y坐标
plot(x_obs,y_obs,'o',10,10,'v',0,0,'ms',x,y,'r')%把路径点，障碍，起点，目标分别用不同的标记画出
%plot(RobotPoint(:,1),RobotPoint(:,2));
clear;
3 仿真结果
4 参考文献
[1]杨一波, 王朝立. 基于改进的人工势场法的机器人避障控制及其MATLAB实现[J]. 上海理工大学学报, 2013, 35(5):5.
部分理论引用网络文献，若有侵权联系博主删除。