1 简介移动机器人的路径规划是移动机器人研究领域中的一个热点问题。2 部分代码Xo[0,0];%起点位置k10;%计算引力需要的增益系数K0;%初始化m1;%计算斥力的增益系数自己设定d2;%障碍影响距离当障碍和车的距离大于这个距离时斥力为0即不受该障碍的影响,自己设定n10;%障碍个数l0.5;%步长J200;%循环迭代次数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_robotXo;%X_robot是机器人的定位坐标,将车的起始坐标Xo赋给X_robt%***************初始化结束开始主体循环******************/for j1:JRobotPoint(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_xjx_atx_re;F_yjy_aty_re;%计算合力与x轴夹角if F_xj0angle_F(j)atan(F_yj/F_xj);elseangle_F(j)piatan(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_robotXnext;%判断是否到达目标点,距离在一个步长之内认为到达目标点r_targetsqrt((X_robot(1)-X_target(1))^2(X_robot(2)-X_target(2))^2);%考虑靠近目标点的情况尽量直线运动防止反复来回震荡if(r_targetlr_target2*l)[d_obs_line] distance_fuzzyControl( X_robot,X_target,X_obs,n);if( d_obs_line0.5)%far,不会影响机器人的直线运动Xnext(1) X_robot(1)l*cos(angle_at);Xnext(2) X_robot(2)l*sin(angle_at);X_robotXnext;endend%判断是否到达目标点,距离在一个步长之内认为到达目标点if(r_targetl)Kj;%记录迭代到多少次到达目标。break;end%如果不符合if的条件重新返回循环继续执行。end%大循环结束%***********************************画出规划路径*************************xRobotPoint(:,1);yRobotPoint(:,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.部分理论引用网络文献若有侵权联系博主删除。
