DWA路径规划算法
来源丨古月居1.DWA路径规划基本原理动态窗口法主要是在速度(v,w)空间中采样多组速度，并模拟机器人在这些速度下一定时间(sim_period)内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该算法突出点在于动态窗口这个名词，它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围内。2.DWA路径规划流程3....
·
来源丨古月居
1.DWA路径规划基本原理
动态窗口法主要是在速度(v,w)空间中采样多组速度，并模拟机器人在这些速度下一定时间(sim_period)内的轨迹。
在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。
该算法突出点在于动态窗口这个名词，它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围内。
2.DWA路径规划流程
3.栅格地图上绘制XY图像
3.1 栅格地图和XY坐标系关系
可以参考：
https://blog.csdn.net/qq_42727752/article/details/117339892
主要区别在于：
DWA算法是采用XY的坐标值来表示机器人位置和地图特征信息，其实用行列表示是一样的，只是 求解轨迹时，DWA算法路线很细致随机，用行列表示无法表示如[3.21,4.56]这种位置，因此需要进行行列值向坐标值的相互转换
MATLAB的plot函数和scatter函数等，绘制图像都是针对XY坐标系的
在栅格地图上的坐标系X对应于列col，而坐标Y对于与行row
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 == 2
coord(1,:) = sub(2,:);
coord(2,:) = sub(1,:);
end
if w == 2
coord(:,1) = sub(:,2);
coord(:,2) = sub(:,1);
end
end3.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 == 2
sub(1,:) = coord(2,:);
sub(2,:) = coord(1,:);
end
if w == 2
sub(:,1) = coord(:,2);
sub(:,2) = coord(:,1);
end
end4.DWA路径规划MATLAB代码
tips：本例只有动态障碍物，本例对于sub和coord的转换难理解可以见最后的纯坐标系代码
DWA算法存在陷入局部最优的死循环（可以参考其他文献）
4.1 MATLAB效果示例
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.5
break;
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_rangeMax
for 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:periodTime
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];
% 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态
% 该模型公式参考模型1，其中dt即△t，利用微分思想即dt是一个很小的数这里均取0.1
rob_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>disttmp
dist=disttmp;
end
end
% 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径
if dist>=2*obstacleR
dist=2*obstacleR;
end
%% 2.5 速度评价函数
% 评价当前轨迹的速度值大小。速度越大，评分越高
velocity = temp_v;
%% 2.6 利用制动距离限定速度是在有效的
% 制动距离的计算，保证所选的速度和加速度不会发生碰撞，参考了博客
stopDist = temp_v*temp_v/(2*maxVelAcc);
% 将有效的速度和角速度存入评价总的评价函数
if dist>stopDist
evalDB=[evalDB;[temp_v temp_w heading dist velocity 0]];
end
end
end
%% 3、对当前状态所有假设的速度加速度组合的评价函数正则化，选取合适的加速度和速度作为下一状态
% 如果评价函数为空则使得机器人停止，即evalDB全0
if isempty(evalDB)
evalDB = [0 0 0 0 0 0];
end
% 将评价函数进行正则化
if sum(evalDB(:,3))~=0
evalDB(:,3)=evalDB(:,3)/sum(evalDB(:,3));
end
if sum(evalDB(:,4))~=0
evalDB(:,4)=evalDB(:,4)/sum(evalDB(:,4));
end
if sum(evalDB(:,5))~=0
evalDB(:,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) == 0
movpos = [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) < cols
if dy_obsCoord(i,2) + movpos(temp_pos,2) > 0 && dy_obsCoord(i,2) + movpos(temp_pos,2) < rows
dy_obsCoord(i,1) = dy_obsCoord(i,1) + movpos(temp_pos,1);
dy_obsCoord(i,2) = dy_obsCoord(i,2) + movpos(temp_pos,2);
end
end
end
end
step = step + 1;
end5. DWA算法Python代码
5.1 实现效果
与MATLAB不同，Python代码是绕完所有静态障碍物
5.2 自定义的基本函数PathPlanning.py
参考（1.4.2节）：
https://blog.csdn.net/qq_42727752/article/details/117339892
代码复制重命名即可
5.3 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]] = 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/2
robotV = 0
robotW = 0
obstacleR = 0.6
dt = 0.1
maxVel = 1.0
maxRot = 20.0/180*np.pi
maxVelAcc = 0.2
maxRotAcc = 50.0/180*np.pi
alpha = 0.05
beta = 0.2
gama = 0.1
periodTime = 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.1
rob_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*180
heading = 180 - evalTheta
# 2.4 计算不同条件下的空隙评价函数
# ①空隙评价函数代表机器人在“当前轨迹上”与最近的障碍物之间的距离
# ②如果在这条轨迹上没有障碍物，那就将其设定一个常数
# ③障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重
dist = np.inf
for 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全0
if 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_v
for 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.4 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.5
break;
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_rangeMax
for 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:periodTime
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];
% 求解矩阵5*5*5*1+5*2*2*1 = 5*1向量表征机器人假设的状态
% 该模型公式参考模型1，其中dt即△t，利用微分思想即dt是一个很小的数这里均取0.1
rob_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>disttmp
dist=disttmp;
end
end
% 限定距离评价函数不能太大,同时对于没有障碍物的距离函数设置为2倍包容半径
if dist>=2*obstacleR
dist=2*obstacleR;
end
%% 2.5 速度评价函数
% 评价当前轨迹的速度值大小。速度越大，评分越高
velocity = temp_v;
%% 2.6 利用制动距离限定速度是在有效的
% 制动距离的计算，保证所选的速度和加速度不会发生碰撞，参考了博客
stopDist = temp_v*temp_v/(2*maxVelAcc);
% 将有效的速度和角速度存入评价总的评价函数
if dist>stopDist
evalDB=[evalDB;[temp_v temp_w heading dist velocity 0]];
end
end
end
%% 3、对当前状态所有假设的速度加速度组合的评价函数正则化，选取合适的加速度和速度作为下一状态
% 如果评价函数为空则使得机器人停止，即evalDB全0
if isempty(evalDB)
evalDB = [0 0 0 0 0 0];
end
% 将评价函数进行正则化
if sum(evalDB(:,3))~=0
evalDB(:,3)=evalDB(:,3)/sum(evalDB(:,3));
end
if sum(evalDB(:,4))~=0
evalDB(:,4)=evalDB(:,4)/sum(evalDB(:,4));
end
if sum(evalDB(:,5))~=0
evalDB(:,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) < 10
if obstacle(i,2) + movpos(temp_pos,2) > 0 && obstacle(i,2) + movpos(temp_pos,2) < 10
obstacle(i,1) = obstacle(i,1) + movpos(temp_pos,1);
obstacle(i,2) = obstacle(i,2) + movpos(temp_pos,2);
end
end
end
end本文仅做学术分享，如有侵权，请联系删文。
3D视觉工坊精品课程官网：3dcver.com
2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
重磅！3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。
一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、答疑解惑、助你高效解决问题
觉得有用，麻烦给个赞和在看~
为开发者提供学习成长、分享交流、生态实践、资源工具等服务，帮助开发者快速成长。
更多推荐
20
1- 0
已为社区贡献13条内容
所有评论(0)