3 移动机器人路径规划

5.1 DWA路径规划基本原理
5.2 DWA路径规划流程
5.3 栅格地图上绘制XY图像
- 5.3.1 栅格地图和XY坐标系关系
- 5.3.2 栅格行列位置转坐标系函数sub2coord.m
- 5.3.3 栅格坐标系位置转行列位置函数coord2sub.m
5.4 DWA路径规划MATLAB代码
- 5.4.1 MATLAB效果示例
- 5.4.2 主函数：DWA_sub.m
- 5.4.3 函数代码：sub2coord.m和coord2sub.m
5.5 DWA算法Python代码
- 5.5.1 实现效果
- 5.5.2 自定义的基本函数PathPlanning.py
- 5.5.2 DWA_py.py
5.6 DWA代码纯坐标系版

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);endend

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);end
end

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/s
maxRot = 20.0/180*pi;               % 机器人最大角速度rad/s
maxVelAcc = 0.2;                    % 机器人最大加速度m/ss
maxRotAcc = 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 np
import copy
import matplotlib.pyplot as plt
import PathPlanning'''
# 设置地图信息，其中rows和cols都是从行列的个数，而非最大行列下标值
# 相比于MATLAB位置从1-->rows，Python的地图信息会相对于数值少1即是0-->rows-1
# 绘制栅格地图的python和MATLAB的坐标系是一样的需要理解转化关系
'''
rows = 15
cols = 20
startSub = [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]] = 4
field[goalSub[0], goalSub[1]] = 5
for i in range(len(dy_obsSub)):field[dy_obsSub[i][0], dy_obsSub[i][1]] = 3robotXY = 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/2
robotV = 0
robotW = 0
obstacleR = 0.6
dt = 0.1maxVel = 1.0
maxRot = 20.0/180*np.pi
maxVelAcc = 0.2
maxRotAcc = 50.0/180*np.pialpha = 0.05
beta = 0.2
gama = 0.1
periodTime = 3.0path = []
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_hif sum_d != 0:for i in range(len(evalDB)):evalDB[i][3] = evalDB[i][3]/sum_dif 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/s
maxRot = 20.0/180*pi;               % 机器人最大角速度rad/s
maxVelAcc = 0.2;                    % 机器人最大加速度m/ss
maxRotAcc = 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

3 移动机器人路径规划（5- DWA路径规划算法）相关推荐

ros局部路径规划器dwa
ROS的路径规划器分为全局路径和局部路径规划,其中局部路径规划器使用的最广的为dwa,个人理解为: 首先全局路径规划会生成一条大致的全局路径,局部路径规划器会把全局路径给分段,然后根据分段的全局路径的 ...
【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）
文章目录参考资料 1. DWA算法原理 1.1 简介 1.2 算法原理 1. 速度采样 2. 轨迹预测(轨迹推算) 3. 轨迹评价 2. Python实现 2.1 参数配置 2.2 机器人运动学模型 ...
【ROS-Navigation】—— DWA路径规划算法解析
文章目录前言 1. 涉及的核心配置文件与启动文件 1.1 demo01_gazebo.launch 1.2 nav06_path.launch 1.3 nav04_amcl.launch 1.4 n ...
移动机器人技术（1）-- 路径规划 Path Planning 总结
路径规划算法总结目录 1 自主机器人近距离操作运动规划体系 ········1.1 单个自主机器人的规划体系 ········1.2 多自主机器人协同规划体系 2 路径规划研究 ········2. ...
a算法和a*算法的区别_机器人路径规划算法，全局路径规划与局部路径规划究竟有哪些区别？...
若步智能移动这一简单动作,对于人类来说相当容易,但对机器人而言就变得极为复杂,说到机器人移动就不得不提到路径规划,路径规划是移动机器人导航最基本的环节,指的是 ...
【RRT三维路径规划】基于matlab RRT算法无人机三维路径规划【含Matlab源码 155期】
一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab RRT算法无人机三维 ...
【RRT三维路径规划】基于matlab RRT算法无人机三维路径规划【含Matlab源码 1363期】
一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab RRT算法无人机三维 ...
【路径规划】基于matlab灰狼算法机器人栅格地图最短路径规划【含Matlab源码 2334期】
⛄一.灰狼算法的厂房巡检机器人路径规划简介 0 引言近年来,我国各行各业的不断发展使相关工作流程得到了完善,其中巡检岗位是一个不可或缺的职位,尤其是在电厂.燃气厂房和煤矿等危险领域中的工作,更不能缺 ...
autoware使用rosbag数据生成路径点并进行路径规划（七）
autoware使用rosbag数据生成路径点并进行路径规划(七) 第一步启动autoware和播放数据包 $ cd ~/autoware.ai $ source install/setup.bash ...

3 移动机器人路径规划（5- DWA路径规划算法）