标签：RobotPoint 10 避障 angle target robot matlab obs 势场
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.
部分理论引用网络文献，若有侵权联系博主删除。
标签：RobotPoint,10,避障,angle,target,robot,matlab,obs,势场 来源： https://blog.csdn.net/qq_59747472/article/details/122021314
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。