matlab代码动态窗口法

close all; clear all; MAX0 = [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] ; MAX=rot90(MAX0,3); %%%设置0,1摆放的图像与存入的数组不一样，需要先逆时针旋转90*3=270度给数组，最后输出来的图像就是自己编排的图像 % imshow( MAX) MAX_X=size(MAX,2); %%% 获取列数，即x轴长度 MAX_Y=size(MAX,1); %%% 获取行数，即y轴长度 MAX_VAL=10; %%% 返回由数字组成的字符表达式的数字值，就是函数用于将数值字符串转换为数值 x_val = 1; y_val = 1; axis([1 MAX_X+1, 1 MAX_Y+1]) %%% 设置x，y轴上下限 set(gca,'xtick',1:1:MAX_X+1,'ytick',1:1:MAX_Y+1,'GridLineStyle','-',... 'xGrid','on','yGrid','on') grid on; %%% 在画图的时候添加网格线 hold on; %%% 当前轴及图像保持而不被刷新，准备接受此后将绘制的图形，多图共存 n=0;%Number of Obstacles %%% 障碍的数量 k=1; %%%% 将所有障碍物放在关闭列表中；障碍点的值为1;并且显示障碍点 CLOSED=[0 0]; for j=1:MAX_X for i=1:MAX_Y if (MAX(i,j)==1) %%plot(i+.5,j+.5,'ks','MarkerFaceColor','b'); 原来是红点圆表示 fill([i,i+1,i+1,i],[j,j,j+1,j+1],'k'); %%%改成 用黑方块来表示障碍物 CLOSED(k,1)=i; %%% 将障碍点保存到CLOSE数组中 CLOSED(k,2)=j; k=k+1; end end end Area_MAX(1,1)=MAX_X; Area_MAX(1,2)=MAX_Y; Obs_Closed=CLOSED; Num_obs=size(CLOSED,1); %%%存储障碍物的数量 ********************************************************************* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 2 设置 起始点、目标点 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% 选择目标位置 pause(0.3); %%% 程序暂停0.3秒 h=msgbox('请使用鼠标左键选择目标'); %%% 显示提示语 uiwait(h,5); %%% 程序暂停 if ishandle(h) == 1 %%% ishandle(H) 将返回一个元素为 1 的数组；否则，将返回 0。 delete(h); end xlabel('请使用鼠标左键选择目标','Color','black'); %%% 显示图x坐标下面的提示语 but=0; while (but ~= 1) %Repeat until the Left button is not clicked %%% 重复，直到没有单击“向左”按钮 [xval,yval,but]=ginput(1); %%% ginput提供了一个十字光标使我们能更精确的选择我们所需要的位置，并返回坐标值。 end B(1)=xval; B(2)=yval; xval=floor(xval); %%% floor（）取不大于传入值的最大整数，向下取整 yval=floor(yval); xTarget=xval;%X Coordinate of the Target %%% 目标的坐标 yTarget=yval;%Y Coordinate of the Target Target(1,1)=xTarget; Target(1,2)=yTarget; MAP(xval,yval) = -1 ; %%% 目标坐标点位置的值设为-1 plot(xval+.5,yval+.5,'go'); %%% 目标点颜色b 蓝色 g 绿色 k 黑色 w白色 r 红色 y黄色 m紫红色 c蓝绿色 % text(xval+1,yval+.5,'Target') %%% text(x,y,'string')在二维图形中指定的位置(x,y)上显示字符串string %%% 选择起始位置 h=msgbox('请使用鼠标左键选择车辆初始位置'); %%%原文 Please Select the Vehicle initial position using the Left Mouse button uiwait(h,5); if ishandle(h) == 1 delete(h); end xlabel('请选择车辆初始位置 ','Color','black'); %%% 原文 Please Select the Vehicle initial position but=0; while (but ~= 1) %Repeat until the Left button is not clicked %%%重复，直到没有单击“向左”按钮 [xval,yval,but]=ginput(1); A(1)=xval; A(2)=yval; xval=floor(xval); yval=floor(yval); end xStart=xval;%Starting Position yStart=yval;%Starting Position Start(1,1)=xStart; Start(1,2)=yStart; MAP(xval,yval)=2; %%% 起始点位置的值设置为1；目标点为0，障碍点为-1，其余空白点为2 plot(xval+.5,yval+.5,'b^'); xlabel('起始点位置标记为 △ ，目标点位置标记为 o ','Color','black'); hold on; %% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] % x=[0 0 pi/2 0 0]'; % 5x1矩阵 列矩阵 位置 0，0 航向 pi/2 ,速度、角速度均为0 x = [xStart yStart pi/10 0 0]'; goal = [xTarget,yTarget]; % 目标点位置 [x(m),y(m)] % 下标宏定义 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] POSE_X = 1; %坐标 X POSE_Y = 2; %坐标 Y YAW_ANGLE = 3; %机器人航向角 V_SPD = 4; %机器人速度 W_ANGLE_SPD = 5; %机器人角速度 obstacleR = 0.4;% 冲突判定用的障碍物半径 global dt; dt = 0.1;% 时间[s] % 机器人运动学模型参数 % 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], % 速度分辨率[m/s],转速分辨率[rad/s]] Kinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)]; %定义Kinematic的下标含义 MD_MAX_V = 1;% 最高速度m/s] MD_MAX_W = 2;% 最高旋转速度[rad/s] MD_ACC = 3;% 加速度[m/ss] MD_VW = 4;% 旋转加速度[rad/ss] MD_V_RESOLUTION = 5;% 速度分辨率[m/s] MD_W_RESOLUTION = 6;% 转速分辨率[rad/s]] % 评价函数参数 [heading,dist,velocity,predictDT] % 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间 evalParam = [0.05, 0.2 ,0.1, 3.0]; area = [-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax] % 模拟实验的结果 result.x=[]; %累积存储走过的轨迹点的状态值 tic; % 估算程序运行时间开始 %% Main loop 循环运行 5000次 指导达到目的地 或者 5000次运行结束 for i = 1:5000 [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,Obs_Closed,obstacleR); x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度 % 历史轨迹的保存 result.x = [result.x; x']; %最新结果 以列的形式 添加到result.x % 是否到达目的地 if norm(x(POSE_X:POSE_Y)-goal')<0.5 % norm函数来求得坐标上的两个点之间的距离 disp('Arrive Goal!!');break; end %====Animation==== hold off; % 关闭图形保持功能。 新图出现时，取消原图的显示。 ArrowLength = 0.3; % 箭头长度 % 机器人 % quiver(x,y,u,v) 在 x 和 y 中每个对应元素对组所指定的坐标处将向量绘制为箭头 quiver(x(POSE_X)+.5, x(POSE_Y)+.5, ArrowLength*cos(x(YAW_ANGLE))+.5, ArrowLength*sin(x(YAW_ANGLE))+.5, 'ok'); % 绘制机器人当前位置的航向箭头 hold on; %启动图形保持功能，当前坐标轴和图形都将保持，从此绘制的图形都将添加在这个图形的基础上，并自动调整坐标轴的范围 set(gca,'xtick',1:1:MAX_X+1,'ytick',1:1:MAX_Y+1,'GridLineStyle','-',... 'xGrid','on','yGrid','on') plot(result.x(:,POSE_X)+.5,result.x(:,POSE_Y)+.5,'-b');hold on; % 绘制走过的所有位置 plot(goal(1)+.5,goal(2)+.5,'*r');hold on; %%绘制目标位置 DrawObstacle_plot(Obs_Closed,obstacleR); % 绘制所有障碍物位置 % 探索轨迹 画出待评价的轨迹 if ~isempty(traj) %%轨迹非空 for it=1:length(traj(:,1))/5