DWA路径规划算法-程序员宅基地

技术标签：算法 python java 机器学习人工智能

来源丨古月居

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


end

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 == 2
        sub(1,:) = coord(2,:);
        sub(2,:) = coord(1,:);
    end
    
    if w == 2
        sub(:,1) = coord(:,2);
        sub(:,2) = coord(:,1);
    end
end

4.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;
end

5. 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

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
 4.国内首个面向工业级实战的点云处理课程
 5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
 6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
 7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
 8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

15.国内首个3D缺陷检测教程：理论、源码与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~

本文链接：https://blog.csdn.net/Yong_Qi2015/article/details/125383210

原作者删帖不实内容删帖广告或垃圾文章投诉

智能推荐

hive学习笔记之四：分区表-程序员宅基地

文章浏览阅读373次，点赞4次，收藏4次。笔者已经把面试题和答案整理成了面试专题文档《一线大厂Java面试题解析+核心总结学习笔记+最新讲解视频+实战项目源码》点击传送门即可获取！链图片转存中…(img-4xkqGpj1-1712104936689)][外链图片转存中…(img-6VtfOlP7-1712104936689)][外链图片转存中…(img-WoiUrEg2-1712104936690)][外链图片转存中…(img-W72fxn2J-1712104936690)]

数字图像处理使用计算机对,数字图像处理的应用现状及发展方向-程序员宅基地

文章浏览阅读5.6k次。随着计算机技术的不断发展，数字图像处理技术的应用也愈发广阔。图像是人们接受、表达、传递信息的快捷方式，通过计算机程序的运用，将图像用数字的形式表现出来，可以更加全面的控制图像处理，在全球各个尖端领域都有发展的前景。【关键词】数字图像处理计算机技术1 数字图像处理技术的优点数字图像处理是指利用数字硬件与计算机，将图像信息转换为电子信号，进行相应的数学运算，用以提高图像的实用性。数字图像处理精确度..._谈谈你对数字图像处理课程的理解,及其数字图像处理用途和使用场合的认识

stl map遍历和删除的方法_vs2008 stl map查询equal_range(),并删除-程序员宅基地

文章浏览阅读6.1k次。for(；iter!=mapStudent.end()；){ if((iter->second)>=aa) { //满足删除条件，删除当前结点，并指向下面一个结点 mapStudent.erase(iter++)； } else { //_vs2008 stl map查询equal_range(),并删除

rank函数python_Python pandas.DataFrame.rank函数方法的使用-程序员宅基地

文章浏览阅读1.4k次。DataFrame.rank(self: ~FrameOrSeries,axis=0,method: str = 'average',numeric_only: Union[bool,NoneType] = None,na_option: str = 'keep',ascending: bool = True,pct: bool = False)→ ~FrameOrSeries计算..._python dataframe rank

彻底扒光通过智能路由器拆解看其本质-程序员宅基地

文章浏览阅读1.7k次。可以看到很多联发科的MT芯片摘自：https://net.zol.com.cn/531/5312999.html彻底扒光通过智能路由器拆解看其本质2015-07-23 00:40:00[中关村在线原创] 作者：陈赫|责编：白宁收藏文章分享到评论(24)关注智能路由器拆解的朋友们注意啦！我们已经将这五款产品彻底扒开，将主板的真容展现在了大家的眼前。网友们可以看见这些智能路由器主板的做工和用料，我们还为网友们展示了主要的电子元器件，供大家品评观赏。..._路由器拆解

Java--深入JDK和hotspot底层源码剖析Thread的run()、start()方法执行过程_jdk的源码hotspot跟jdk是分开的-程序员宅基地

文章浏览阅读2.1k次，点赞101次，收藏78次。【学习背景】今天主要是来了解Java线程Thread中的run()、start()两个方法的执行有哪些区别，会给出一个简单的测试代码样例，快速理解两者的区别，再从源码层面去追溯start()底层是如何最终调用Thread#run()方法的，个人觉得这样的学习不论对面试，还是实际编程来说都是比较有帮助的。进入正文~学习目录一、代码测试二、源码分析2.1 run()方法2.2 start()方法三、使用总结一、代码测试执行Thread的run()、start()方法的测试代码如下：public_jdk的源码hotspot跟jdk是分开的

随便推点

SM4国密算法原理及python代码实现_根据sm4_s计算sm4_sbox_t-程序员宅基地

文章浏览阅读6.6k次，点赞13次，收藏54次。SM4.0（原名SMS4.0）是中华人民共和国政府采用的一种分组密码标准，由国家密码管理局于2012年3月21日发布。相关标准为“GM/T 0002-2012《SM4分组密码算法》（原SMS4分组密码算法）”。在商用密码体系中，SM4主要用于数据加密，其算法公开，分组长度与密钥长度均为128bit，加密算法与密钥扩展算法都采用32轮非线性迭代结构，S盒为固定的8比特输入8比特输出。_根据sm4_s计算sm4_sbox_t

DMA映射 dma_addr_t-程序员宅基地

文章浏览阅读617次。DMA映射一个DMA映射是要分配的DMA缓冲区与为该缓冲区生成的、设备可访问地址的组合。DMA映射建立了一个新的结构类型——dma_addr_t来表示总线地址。dma_addr_t类型的变量对驱动程序是不透明的，唯一允许的操作是将它们传递给DMA支持例程以及设备本身。根据DMA缓冲区期望保留的时间长短，PCI代码有两种DMA映射: 1) 一致性映射； 2) 流式DMA映射(推荐)。..._dma_addr_t

XSS跨站脚本攻击漏洞_小明是公司的开发工程师,发现公司网站存在xss漏洞,通过修改javascript代码进-程序员宅基地

文章浏览阅读1k次。XSS（跨站脚本攻击）是一种常见的网络安全漏洞，它允许攻击者在网站中植入恶意的脚本代码，当其他用户访问该网站时，这些脚本代码会在用户的浏览器中执行。这可能会导致严重的安全后果，比如窃取用户的敏感信息，欺骗用户，或者在用户的浏览器中执行恶意操作。XSS漏洞通常出现在网站中输入数据未经过滤或者不当过滤的情况下，攻击者可以通过向网站发送带有恶意脚本的数据，使得脚本在用户的浏览器中执行。在反射型XSS中，攻击者向网站发送一个带有恶意脚本的URL，当其他用户点击该URL时，恶意脚本就会在用户的浏览器中执行。_小明是公司的开发工程师,发现公司网站存在xss漏洞,通过修改javascript代码进

软件体系结构_采用结构化技术开发的软件是否具有体系结构?-程序员宅基地

文章浏览阅读2k次。软件体系结构是具有一定形式的结构化元素，即构件的集合，包括处理构件、数据构件和连接构件。处理构件负责对数据进行加工，数据构件是被加工的信息，连接构件把体系结构的不同部分组组合连接起来。这一定义注重区分处理构件、数据构件和连接构件，这一方法在其他的定义和方法中基本上得到保持。一、软件体系结构的定义　　虽然软件体系结构已经在软件工程领域中有着广泛的应用，但迄今为止还没有一个被大家所公认的_采用结构化技术开发的软件是否具有体系结构?

springbbot运行无法编译成功，找不到jar包报错：Error:(3, 46) java: 程序包org.springframework.context.annotation不存在-程序员宅基地

文章浏览阅读1k次，点赞2次，收藏2次。文章目录问题描述：解决方案：问题描述：提示：idea springbbot运行无法编译成功，找不到jar包报错E:\ideaProject\demokkkk\src\main\java\com\example\demo\config\WebSocketConfig.javaError:(3, 46) java: 程序包org.springframework.context.annotation不存在Error:(4, 46) java: 程序包org.springframework.conte_error:(3, 46) java: 程序包org.springframework.context.annotation不存在

react常见面试题_recate面试-程序员宅基地

文章浏览阅读6.4k次，点赞6次，收藏36次。1、redux中间件中间件提供第三方插件的模式，自定义拦截 action -> reducer 的过程。变为 action -> middlewares -> reducer 。这种机制可以让我们改变数据流，实现如异步 action ，action 过滤，日志输出，异常报告等功能。常见的中间件：redux-logger：提供日志输出redux-thunk：处理异步操作..._recate面试