内容列表
5.1 DWA路径规划基本原理
动态窗口法主要是在速度(v,w)空间中采样多组速度，并模拟机器人在这些速度下一定时间(sim_period)内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。
该算法突出点在于动态窗口这个名词，它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围内。
参考内容
博客-机器人局部避障的动态窗口法(dynamic window approach)
5.2 DWA路径规划流程
5.3 栅格地图上绘制XY图像
5.3.1 栅格地图和XY坐标系关系
可以参考3 移动机器人路径规划（1- 栅格地图绘制），主要区别在于：
DWA算法是采用XY的坐标值来表示机器人位置和地图特征信息，其实用行列表示是一样的，只是 求解轨迹时，DWA算法路线很细致随机，用行列表示无法表示如[3.21,4.56]这种位置，因此需要进行行列值向坐标值的相互转换
MATLAB的plot函数和scatter函数等，绘制图像都是针对XY坐标系的
在栅格地图上的坐标系X对应于列col，而坐标Y对于与行row
5.3.2 栅格行列位置转坐标系函数sub2coord.m
function coord = sub2coord(sub)%SUB2COORD 将行列式下标装换为坐标格式，此时的坐标格式和原本认知坐标方向也不一致（如下所示）% 1 2 3 4 5 6 7 .... X坐标% 1|——————————>% 2|% 3|% 4|% 5|% Y坐标\/
[l,w] = size(sub);% 长度l=2表示sub为2*n矩阵if l == 2coord(1,:) = sub(2,:);coord(2,:) = sub(1,:);endif w == 2coord(:,1) = sub(:,2);coord(:,2) = sub(:,1);end
end
5.3.3 栅格坐标系位置转行列位置函数coord2sub.m
function sub = coord2sub(coord)%COORD2SUB 将坐标转换为矩阵行列格式，坐标格式为下图所示% 1 2 3 4 5 6 7 .... X坐标% 1|——————————>% 2|% 3|% 4|% 5|% Y坐标\/
[l,w] = size(coord);% 长度l=2表示sub为2*n矩阵if l == 2sub(1,:) = coord(2,:);sub(2,:) = coord(1,:);endif w == 2sub(:,1) = coord(:,2);sub(:,2) = coord(:,1);endend
5.4 DWA路径规划MATLAB代码
本例只有动态障碍物
本例对于sub和coord的转换难理解可以见最后的纯坐标系代码
DWA算法存在陷入局部最优的死循环（可以参考其他文献）
5.4.1 MATLAB效果示例
5.4.2 主函数：DWA_sub.m
clear;close all;rows = 15; cols = 20; % 地图行列尺寸startSub = [15,2]; % 起点行列位置goalSub = [2,20]; % 终点行列位置dy_obsSub = [4,4; 13,5; 9,14; 2,18; 12,16]; % 动态障碍物行列step = 0; % 动态障碍物更新运动的频率
% 设置地图属性field = ones(rows, cols);field(startSub(1),startSub(2)) = 4;field(goalSub(1),goalSub(2)) = 5;dy_obsR = dy_obsSub(:,1);dy_obsC = dy_obsSub(:,2);dy_obsIndex = sub2ind([rows,cols],dy_obsR,dy_obsC);field(dy_obsIndex) = 3;% 颜色表征矩阵cmap = [1 1 1; ... % 1-白色-空地0 0 0; ... % 2-黑色-静态障碍1 0 0; ... % 3-红色-动态障碍1 1 0;... % 4-黄色-起始点1 0 1;... % 5-品红-目标点0 1 0; ... % 6-绿色-到目标点的规划路径0 1 1]; % 7-青色-动态规划的路径colormap(cmap);image(1.5,1.5,field);
% 设置栅格属性grid on;hold on;set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);set(gca,'xtick',1:cols+1,'ytick',1:rows+1);set(gca, 'XAxisLocation','top')axis image;
% 机器人现在状态% 对于DWA算法，结算结果建立在XY坐标系上，先将数据转成XY格式% 坐标系只影响位置，对于机器人其他运动学不影响robotXY = sub2coord(startSub);goalCoord = sub2coord(goalSub);dy_obsCoord = sub2coord(dy_obsSub);
robotT = pi/2; % 机器人当前方向角度(0->2pi)robotV = 0; % 机器人当前速度robotW = 0; % 机器人当前角速度obstacleR=0.6; % 冲突判定用的障碍物半径dt = 0.1; % 时间[s]
% 机器人运动学模型maxVel = 1.0; % 机器人最大速度m/smaxRot = 20.0/180*pi; % 机器人最大角速度rad/smaxVelAcc = 0.2; % 机器人最大加速度m/ssmaxRotAcc = 50.0/180*pi; % 机器人最大角加速度rad/ss
% 评价系数alpha = 0.05; % 方位角评价系数αbeta = 0.2; % 空隙评价系数βgama = 0.1; % 速度评价系数γperiodTime = 3.0; % 预测处理时间,也就是绿色搜寻轨迹的长度
path = []; % 记录移动路径
%% 开始DWA算法求解while true% 是否到达目的地，到达目的地则跳出循环if norm(robotXY-goalCoord) < 0.5break;end%% 1、求速度区间==============================================vel_rangeMin = max(0, robotV-maxVelAcc*dt);vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt);rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt);rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt);% 存放当前机器人状态的各个线速度角速度下的评价函数的值% evalDB格式为n*6，其中6列为下一状态速度、角速度、方向角函数、距离函数、速度函数、评价函数值evalDB = [];%% 2、计算不同线速度角速度下的评价函数，目的取出最合适的线速度和角速度******************for temp_v = vel_rangeMin : 0.01 : vel_rangeMaxfor temp_w = rot_rangeMin : pi/180 : rot_rangeMax%% 2.1 最关键内容，不同线速度角速度都是建立在机器人最初始状态下的rob_perState = [robotXY(1),robotXY(2),robotT,robotV,robotW]';%% 2.2 求出在sim_period时间后机器人的状态for time = dt:dt:periodTimematE = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0 0;0 0 0 0 0];matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];% 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态% 该模型公式参考模型1，其中dt即△t，利用微分思想即dt是一个很小的数这里均取0.1rob_perState = matE*rob_perState+matV*[temp_v;temp_w];end%% 2.3 计算不同条件下的方位角评价函数,此时机器人状态是在预测时间后的假设状态% ①方向角评价函数是用来评价机器人在当前设定的采样速度下，% ②达到模拟轨迹末端时的朝向和目标之间的角度差距goalTheta=atan2(goalCoord(2)-rob_perState(2),goalCoord(1)-rob_perState(1));% 目标点的方位的角度evalTheta = abs(rob_perState(3)-goalTheta)/pi*180;heading = 180 - evalTheta;%% 2.4 计算不同条件下的空隙评价函数% ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离% ②如果在这条轨迹上没有障碍物，那就将其设定一个常数% ③障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重dist = inf;for i=1:length(dy_obsCoord(:,1))% 利用二范数求距离disttmp=norm(dy_obsCoord(i,:)-rob_perState(1:2)')-obstacleR;% 保证dist是最近的障碍物距离if dist>disttmpdist=disttmp;endend% 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径if dist>=2*obstacleRdist=2*obstacleR;end
%% 2.5 速度评价函数% 评价当前轨迹的速度值大小。速度越大，评分越高velocity = temp_v;
%% 2.6 利用制动距离限定速度是在有效的% 制动距离的计算，保证所选的速度和加速度不会发生碰撞，参考了博客stopDist = temp_v*temp_v/(2*maxVelAcc);% 将有效的速度和角速度存入评价总的评价函数if dist>stopDistevalDB=[evalDB;[temp_v temp_w heading dist velocity 0]];endendend%% 3、对当前状态所有假设的速度加速度组合的评价函数正则化，选取合适的加速度和速度作为下一状态% 如果评价函数为空则使得机器人停止，即evalDB全0if isempty(evalDB)evalDB = [0 0 0 0 0 0];end% 将评价函数进行正则化if sum(evalDB(:,3))~=0evalDB(:,3)=evalDB(:,3)/sum(evalDB(:,3));endif sum(evalDB(:,4))~=0evalDB(:,4)=evalDB(:,4)/sum(evalDB(:,4));endif sum(evalDB(:,5))~=0evalDB(:,5)=evalDB(:,5)/sum(evalDB(:,5));end% 最终评价函数的计算for i=1:length(evalDB(:,1))evalDB(i,6)=alpha*evalDB(i,3)+beta*evalDB(i,4)+gama*evalDB(i,5);end[~,ind]=max(evalDB(:,6)); % 选取出最优评价函数的索引nextVR=evalDB(ind,1:2)'; % 机器人下一速度和角速度即为该评价函数对应的速度和角速度%% 4、选择好角速度线速度更新机器人下一状态matE = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0 0;0 0 0 0 0];matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];robot_NextState = matE*[robotXY(1),robotXY(2),robotT,robotV,robotW]'+matV*nextVR;% 更新状态开启下一轮DWA算法求解robotXY(1) = robot_NextState(1); robotXY(2) = robot_NextState(2);robotT = robot_NextState(3); robotV = robot_NextState(4);robotW = robot_NextState(5);% 将路径存放到路径矩阵，主要是为了绘图path = [path;[robotXY(1),robotXY(2)]];%% 5、绘制图像field(dy_obsIndex) = 1;dy_obsSub = coord2sub(dy_obsCoord);dy_obsR = dy_obsSub(:,1);dy_obsC = dy_obsSub(:,2);dy_obsIndex = sub2ind([rows,cols],dy_obsR,dy_obsC);field(dy_obsIndex) = 3;image(1.5,1.5,field);scatter(robotXY(1)+0.5,robotXY(2)+0.5,'r','LineWidth',1.5); % 绘制机器人，红色圆圈表示plot(path(:,1)+0.5,path(:,2)+0.5,'-b'); % 绘制路径drawnow;%% 6、将障碍物位置更新实现障碍物也在移动，对于静态障碍物删除从此往下代码if mod(step,20) == 0movpos = [0,1; 0,-1; -1,0; 1,0]; % 对应上下左右四个方向for i = 1:length(dy_obsCoord(:,1))temp_obs = dy_obsCoord(i,:);temp_pos = randi(4);% 移动范围限制在地图区间里if dy_obsCoord(i,1) + movpos(temp_pos,1) > 0 && dy_obsCoord(i,1) + movpos(temp_pos,1) < colsif dy_obsCoord(i,2) + movpos(temp_pos,2) > 0 && dy_obsCoord(i,2) + movpos(temp_pos,2) < rowsdy_obsCoord(i,1) = dy_obsCoord(i,1) + movpos(temp_pos,1);dy_obsCoord(i,2) = dy_obsCoord(i,2) + movpos(temp_pos,2);endendendendstep = step + 1;end
5.4.3 函数代码：sub2coord.m和coord2sub.m
见本文的5.3.2和5.3.3
5.5 DWA算法Python代码
5.5.1 实现效果
与MATLAB不同，Python代码是绕完所有静态障碍物
5.5.2 自定义的基本函数PathPlanning.py
见3 移动机器人路径规划（1- 栅格地图绘制）的1.4.2节，代码复制重命名即可
5.5.2 DWA_py.py
import numpy as npimport copyimport matplotlib.pyplot as pltimport PathPlanning
'''# 设置地图信息，其中rows和cols都是从行列的个数，而非最大行列下标值# 相比于MATLAB位置从1-->rows，Python的地图信息会相对于数值少1即是0-->rows-1# 绘制栅格地图的python和MATLAB的坐标系是一样的需要理解转化关系'''rows = 15cols = 20startSub = [14, 1]goalSub = [1, 3]dy_obsSub = [[3, 3], [12, 4], [8, 13], [1, 17], [11, 15]]
field = np.ones([rows, cols])field[startSub[0], startSub[1]] = 4field[goalSub[0], goalSub[1]] = 5for i in range(len(dy_obsSub)):field[dy_obsSub[i][0], dy_obsSub[i][1]] = 3
robotXY = PathPlanning.sub2coord(startSub)goalCoord = PathPlanning.sub2coord(goalSub)
dy_obsCoord = []for i in range(len(dy_obsSub)):dy_obsCoord.append(PathPlanning.sub2coord(dy_obsSub[i]))
'''# 机器人现在状态# 对于DWA算法，结算结果建立在XY坐标系上，先将数据转成XY格式# 坐标系只影响位置，对于机器人其他运动学不影响'''robotT = np.pi/2robotV = 0robotW = 0obstacleR = 0.6dt = 0.1
maxVel = 1.0maxRot = 20.0/180*np.pimaxVelAcc = 0.2maxRotAcc = 50.0/180*np.pi
alpha = 0.05beta = 0.2gama = 0.1periodTime = 3.0
path = []pathx = []pathy = []PathPlanning.drawmap(field)
'''# 代码注释同MATLAB代码是一致的'''while True:# 是否到达目的地，到达目的地则跳出循环dat = np.sqrt((robotXY[0]-goalCoord[0])*(robotXY[0]-goalCoord[0])+(robotXY[1]-goalCoord[1])*(robotXY[1]-goalCoord[1]))if dat < 0.5:break
# 1、求速度区间==============================================vel_rangeMin = max(0, robotV-maxVelAcc*dt)vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt)rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt)rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt)
# 存放当前机器人状态的各个线速度角速度下的评价函数的值# evalDB格式为n*6，其中6列为下一状态速度、角速度、方向角函数、距离函数、速度函数、评价函数值evalDB = []
## 2、计算不同线速度角速度下的评价函数，目的取出最合适的线速度和角速度******************for temp_v in np.arange(vel_rangeMin,vel_rangeMax,0.01):for temp_w in np.arange(rot_rangeMin,rot_rangeMax,np.pi/180):# 2.1 最关键内容，不同线速度角速度都是建立在机器人最初始状态下的rob_perState = [robotXY[0],robotXY[1],robotT,robotV,robotW]
# 2.2 求出在sim_period时间后机器人的状态for time in np.arange(dt,periodTime,dt):matE = np.array([[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]])matV = np.array([[dt*np.cos(rob_perState[2]),0],[dt*np.sin(rob_perState[2]),0],[0,dt],[1,0],[0,1]])# 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态# 该模型公式参考模型1，其中dt即△t，利用微分思想即dt是一个很小的数这里均取0.1rob_perState = np.dot(matE,rob_perState) + np.dot(matV,np.array([temp_v,temp_w]))# 2.3 计算不同条件下的方位角评价函数,此时机器人状态是在预测时间后的假设状态# ①方向角评价函数是用来评价机器人在当前设定的采样速度下，# ②达到模拟轨迹末端时的朝向和目标之间的角度差距goalTheta = np.arctan2(goalCoord[1]-rob_perState[1],goalCoord[0]-rob_perState[0])evalTheta = np.abs(rob_perState[2]-goalTheta)/np.pi*180heading = 180 - evalTheta
# 2.4 计算不同条件下的空隙评价函数# ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离# ②如果在这条轨迹上没有障碍物，那就将其设定一个常数# ③障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重dist = np.inffor i in range(len(dy_obsCoord)):disttmp = np.sqrt((dy_obsCoord[i][0]-rob_perState[0])*(dy_obsCoord[i][0]-rob_perState[0])+ (dy_obsCoord[i][1]-rob_perState[1])*(dy_obsCoord[i][1]-rob_perState[1])) - obstacleR# 保证dist是最近的障碍物距离if dist > disttmp:dist = disttmp
# 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径if dist > 2*obstacleR:dist = 2*obstacleR
# 2.5 速度评价函数# 评价当前轨迹的速度值大小。速度越大，评分越高velocity = temp_v
# 2.6 利用制动距离限定速度是在有效的# 制动距离的计算，保证所选的速度和加速度不会发生碰撞，参考了博客stopDist = temp_v*temp_v/(2*maxVelAcc)
# 将有效的速度和角速度存入评价总的评价函数if dist>stopDist:evalDB.append([temp_v,temp_w,heading,dist,velocity,0])
# 3、对当前状态所有假设的速度加速度组合的评价函数正则化，选取合适的加速度和速度作为下一状态# 如果评价函数为空则使得机器人停止，即evalDB全0if len(evalDB) == 0:evalDB = [[0,0,0,0,0,0],[0,0,0,0,0,0]]
# 4、将评价函数进行正则化evalDB = np.array(evalDB)
sum_h = sum(evalDB[:,2])sum_d = sum(evalDB[:,3])sum_v = sum(evalDB[:,4])
if sum_h != 0:for i in range(len(evalDB)):evalDB[i][2] = evalDB[i][2]/sum_h
if sum_d != 0:for i in range(len(evalDB)):evalDB[i][3] = evalDB[i][3]/sum_d
if sum_v != 0:for i in range(len(evalDB)):evalDB[i][4] = evalDB[i][4]/sum_vfor i in range(len(evalDB)):evalDB[i][5] = alpha*evalDB[i][2]+beta*evalDB[i][3]+gama*evalDB[i][4]
idx = np.argmax(evalDB[:,5])nextVR = evalDB[idx,0:2]
# 5、选择好角速度线速度更新机器人下一状态matE = np.array([[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0],[0,0,0,0,0],[0,0,0,0,0]])matV = np.array([[dt*np.cos(rob_perState[2]),0],[dt*np.sin(rob_perState[2]),0],[0,dt],[1,0],[0,1]])robot_NextState = np.dot(matE,[robotXY[0],robotXY[1],robotT,robotV,robotW]) + np.dot(matV,np.array(nextVR))
robotXY[0] = robot_NextState[0]robotXY[1] = robot_NextState[1]robotT = robot_NextState[2]robotV = robot_NextState[3]robotW = robot_NextState[4]
path.append([robotXY[0],robotXY[1]])pathx.append(robotXY[0]+0.5)pathy.append(robotXY[1]+0.5)
plt.plot(pathx,pathy,'b')plt.pause(0.01)
5.6 DWA代码纯坐标系版
clear;% 机器人现在状态robotX = 1; % 机器人当前X位置robotY = 1; % 机器人当前Y位置robotT = pi/2; % 机器人当前方向角度(0->2pi)robotV = 0; % 机器人当前速度robotW = 0; % 机器人当前角速度
% 地图信息goal=[9,9]; % 目标点位置 [x(m),y(m)]obstacle=[2,2;4,4;6,6;8,8]; % 障碍物位置列表 [x(m) y(m)]obstacleR=0.6; % 冲突判定用的障碍物半径dt = 0.1; % 时间[s]
% 机器人运动学模型maxVel = 1.0; % 机器人最大速度m/smaxRot = 20.0/180*pi; % 机器人最大角速度rad/smaxVelAcc = 0.2; % 机器人最大加速度m/ssmaxRotAcc = 50.0/180*pi; % 机器人最大角加速度rad/ss
% 评价系数alpha = 0.05; % 方位角评价系数αbeta = 0.2; % 空隙评价系数βgama = 0.1; % 速度评价系数γperiodTime = 3.0; % 预测处理时间,也就是绿色搜寻轨迹的长度
area=[0 10 0 10]; % 模拟区域范围 [xmin xmax ymin ymax]path = []; % 记录移动路径
%% 开始DWA算法求解while true% 是否到达目的地，到达目的地则跳出循环if norm([robotX,robotY]-goal') < 0.5break;end%% 1、求速度区间==============================================vel_rangeMin = max(0, robotV-maxVelAcc*dt);vel_rangeMax = min(maxVel, robotV+maxVelAcc*dt);rot_rangeMin = max(-maxRot, robotW-maxRotAcc*dt);rot_rangeMax = min(maxRot, robotW+maxRotAcc*dt);% 存放当前机器人状态的各个线速度角速度下的评价函数的值% evalDB格式为n*6，其中6列为下一状态速度、角速度、方向角函数、距离函数、速度函数、评价函数值evalDB = [];%% 2、计算不同线速度角速度下的评价函数，目的取出最合适的线速度和角速度******************for temp_v = vel_rangeMin : 0.01 : vel_rangeMaxfor temp_w = rot_rangeMin : pi/180 : rot_rangeMax%% 2.1 最关键内容，不同线速度角速度都是建立在机器人最初始状态下的rob_perState = [robotX,robotY,robotT,robotV,robotW]';%% 2.2 求出在sim_period时间后机器人的状态for time = dt:dt:periodTimematE = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0 0;0 0 0 0 0];matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];% 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态% 该模型公式参考模型1，其中dt即△t，利用微分思想即dt是一个很小的数这里均取0.1rob_perState = matE*rob_perState+matV*[temp_v;temp_w];end%% 2.3 计算不同条件下的方位角评价函数,此时机器人状态是在预测时间后的假设状态% ①方向角评价函数是用来评价机器人在当前设定的采样速度下，% ②达到模拟轨迹末端时的朝向和目标之间的角度差距goalTheta=atan2(goal(2)-rob_perState(2),goal(1)-rob_perState(1));% 目标点的方位的角度evalTheta = abs(rob_perState(3)-goalTheta)/pi*180;heading = 180 - evalTheta;%% 2.4 计算不同条件下的空隙评价函数% ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离% ②如果在这条轨迹上没有障碍物，那就将其设定一个常数% ③障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重dist = inf;for i=1:length(obstacle(:,1))% 利用二范数求距离disttmp=norm(obstacle(i,:)-rob_perState(1:2)')-obstacleR;% 保证dist是最近的障碍物距离if dist>disttmpdist=disttmp;endend% 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径if dist>=2*obstacleRdist=2*obstacleR;end
%% 2.5 速度评价函数% 评价当前轨迹的速度值大小。速度越大，评分越高velocity = temp_v;
%% 2.6 利用制动距离限定速度是在有效的% 制动距离的计算，保证所选的速度和加速度不会发生碰撞，参考了博客stopDist = temp_v*temp_v/(2*maxVelAcc);% 将有效的速度和角速度存入评价总的评价函数if dist>stopDistevalDB=[evalDB;[temp_v temp_w heading dist velocity 0]];endendend%% 3、对当前状态所有假设的速度加速度组合的评价函数正则化，选取合适的加速度和速度作为下一状态% 如果评价函数为空则使得机器人停止，即evalDB全0if isempty(evalDB)evalDB = [0 0 0 0 0 0];end% 将评价函数进行正则化if sum(evalDB(:,3))~=0evalDB(:,3)=evalDB(:,3)/sum(evalDB(:,3));endif sum(evalDB(:,4))~=0evalDB(:,4)=evalDB(:,4)/sum(evalDB(:,4));endif sum(evalDB(:,5))~=0evalDB(:,5)=evalDB(:,5)/sum(evalDB(:,5));end% 最终评价函数的计算for i=1:length(evalDB(:,1))evalDB(i,6)=alpha*evalDB(i,3)+beta*evalDB(i,4)+gama*evalDB(i,5);end[~,ind]=max(evalDB(:,6)); % 选取出最优评价函数的索引nextVR=evalDB(ind,1:2)'; % 机器人下一速度和角速度即为该评价函数对应的速度和角速度%% 4、选择好角速度线速度更新机器人下一状态matE = [1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0 0;0 0 0 0 0];matV = [dt*cos(rob_perState(3)) 0;dt*sin(rob_perState(3)) 0;0 dt;1 0;0 1];robot_NextState = matE*[robotX,robotY,robotT,robotV,robotW]'+matV*nextVR;% 更新状态开启下一轮DWA算法求解robotX = robot_NextState(1); robotY = robot_NextState(2);robotT = robot_NextState(3); robotV = robot_NextState(4);robotW = robot_NextState(5);% 将路径存放到路径矩阵，主要是为了绘图path = [path;[robotX,robotY]];%% 5、绘制图像hold off;scatter(robotX,robotY,'r','LineWidth',1.5);hold on; % 绘制机器人，红色圆圈表示plot(goal(1),goal(2),'*r');hold on; % 绘制地图终点scatter(obstacle(:,1),obstacle(:,2),200,'k');hold on; % 绘制障碍物plot(path(:,1),path(:,2),'-b');hold on; % 绘制路径axis(area);grid on;drawnow;%% 6、将障碍物位置更新实现障碍物也在移动，对于静态障碍物删除从此往下代码movpos = [0,0.2; 0,-0.2; -0.2,0; 0.2,0]; % 对应上下左右四个方向for i = 1:length(obstacle(:,1))temp_obs = obstacle(i,:);temp_pos = randi(4);% 移动范围限制在地图区间里if obstacle(i,1) + movpos(temp_pos,1) > 0 && obstacle(i,1) + movpos(temp_pos,1) < 10if obstacle(i,2) + movpos(temp_pos,2) > 0 && obstacle(i,2) + movpos(temp_pos,2) < 10obstacle(i,1) = obstacle(i,1) + movpos(temp_pos,1);obstacle(i,2) = obstacle(i,2) + movpos(temp_pos,2);endendendend
评论（1）
您还未登录，请登录后发表或查看评论