| 局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！） | 您所在的位置：网站首页 › 话题讲解 › 局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！） |
局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！）
|
转载请注明出处，谢谢 理论基础前面学习的全局路径规划方法，Dijkstra、Best-First-Search、A*算法都属于状态采样（State Sampling）方法，而 DWA 局部路径规划则属于典型的动作采样（action sampling）方法 DWA 算法（Dynamic Window Approach）的原理主要是以一定的分辨率在速度空间 (v, w) 中采样多组速度，并模拟出这些速度在一定时间的运动轨迹，得到可行轨迹后通过评价函数对轨迹进行评分，评分后选取最优轨迹对应的 (v, w) 驱动机器人运动 速度空间速度空间 (v, w) 即机器人的速度范围，机器人的速度受到各种因素的限制 移动机器人受自身最大速度最小速度的限制Vs 为机器人能够到达的所有矢量速度的集合；机器人受到最大最小线速度和角速度影响
由于加速度有一个范围限制，所以最大加速度或最大减速度一定时间内能达到的速度 ，才会被保留，表达式如下：
为了能在碰到障碍物前停下来，在最大减速度的条件下，速度满足以下条件：
其中dist(v,w)为（v,w）对应的轨迹上离障碍物最近的距离 在上述 3 个约束条件下，速度空间 (v, w) 会有一定的范围，注意速度空间不是固定不变的，而是时变的，t 时刻的速度空间与 t + dt 时刻的速度空间是不同的，故将其称为动态窗口 速度采样得到 t 时刻的速度空间 (v, w) 后，以一定的分辨率对速度 v 和角速度 w 进行采样 设 t 时刻小车 x 轴线速度取值范围 5~20m/s,为简化数学模型，设 y轴线速度为0m/s，小车的角速度取值范围 0.1~1rad/s,小车的线速度的速度分辨率为0.1m/s，角速度的速度分辨率为0.05rad/s 根据离散的方法，离散 x 轴线速度和角速度如下
之所以要分析机器人的运动学模型，是因为要根据采样的速度 (v, w) 模拟机器人的运动轨迹
DWA 算法适用于差速运动模型和全向运动模型，并不适用于阿克曼模型 差速运动模型机器人只能向前运动或者旋转，图中有世界坐标系和机器人坐标系，下式中 v(t) 指机器人坐标系中 x 方向的速度
在全向移动模型中，多了机器人坐标系下 y 方向的速度
令 Vy(t) = 0，则退化为差速运动模型，因此 ROS 自带的局部路径规划器使用上式计算 轨迹预测在确定机器人的采样速度和运动模型后，可以对机器人的状态进行预测和更新 机器人下一时刻位移距离
机器人下一时刻坐标的变化
采样时刻机器人坐标与坐标变化求和，得到下一时刻机器人坐标
设 ∆t 为 0.1s，实际上预测机器人前向几秒内的所有状态，假设前向预测时间为 3s，则会预测 t ~ t + 3 s内的所有状态，即 t ~ t + ∆t ~ t + 2*∆t ~ …. ~ t + 3，相当于会预测出 t 时刻位置前 30 个点的位置，如下图所示
对速度空间进行采样后，根据机器人运动学模型能够预测出多条轨迹，需要对这些轨迹进行评价，选取最优的轨迹，机器人根据最优轨迹对应的速度进行运动 DWA 算法对轨迹的评价函数一般如下所示
很好理解： 1、希望我的前进方向对准终点 2、希望不发生任何碰撞 3、希望速度尽量快 4、除此之外，还要保证最短刹车距离是安全的 原文中提到方位角评价函数对轨迹规划的影响较大，太大易陷入局部最优解，太小能更好地避障，但路径有点长，效率有限低 当然也可以对评价函数进行优化，添加更多的评价函数指标 注意需要对评价函数的结果进行归一化处理，将每一项除以每一项的总和，如下式
这样可以避免不同类别得分基数相差太大造成的结果误差 至此，DWA 算法的完整流程
程序的思维导图如下，这里只讲解重要的部分，绘图及障碍物设置部分暂时略过
这里的 for 循环有次数限制，最多循环 5000 次 for i = 1:5000 % DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹 [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);%算出下发速度u/当前速度u x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度 abc = abc+1; % 历史轨迹的保存 result.x = [result.x; x']; %最新结果 以行的形式 添加到result.x，保存的是所有状态参数值，包括坐标xy、朝向、线速度、角速度，其实应该是只取坐标就OK % 是否到达目的地 if norm(x(POSE_X:POSE_Y)-goal') stopDist && Flag == 0 % 如果可能撞到最近的障碍物 则舍弃此路径 （到最近障碍物的距离 大于 刹车距离 才取用） evalDB = [evalDB;[vt ot heading dist vel]]; trajDB = [trajDB;traj]; % 每5行 一条轨迹 disp(evalDB); disp(trajDB); end end endEvaluation() 这个函数也很重要，它肩负了较多的职责，包括 对速度空间进行离散化采样 for vt = Vr(1):model(5):Vr(2) %根据速度分辨率遍历所有可用速度： 最小速度和最大速度 之间 速度分辨率 递增 for ot=Vr(3):model(6):Vr(4) %根据角度分辨率遍历所有可用角速度： 最小角速度和最大角速度 之间 角度分辨率 递增 进行轨迹推测，预测出当前时刻到预测时刻之间的轨迹调用 GenerateTrajectory() 函数进行轨迹预测 %% 单条轨迹生成、轨迹推演函数 % 输入参数： 当前状态、vt当前速度、ot角速度、evaldt 前向模拟时间、机器人模型参数（没用到） % 返回参数; % x : 机器人模拟时间内向前运动 预测的终点位姿(状态); % traj: 当前时刻 到 预测时刻之间 过程中的位姿记录（状态记录） 当前模拟的轨迹 % 轨迹点的个数为 evaldt / dt + 1 = 3.0 / 0.1 + 1 = 31 % function [x,traj] = GenerateTrajectory(x,vt,ot,evaldt) global dt; time = 0; u = [vt;ot];% 输入值 traj = x; % 机器人轨迹 while time theta targetTheta = goalTheta-theta;% [deg] else targetTheta = theta-goalTheta;% [deg] end heading = 180 - targetTheta;调用 CalcDistEval() 计算障碍物距离评价函数 %% 障碍物距离评价函数 （机器人在当前轨迹上与最近的障碍物之间的距离，如果没有障碍物则设定一个常数） % 输入参数：位姿、所有障碍物位置、障碍物半径 % 输出参数：当前预测的轨迹终点的位姿距离所有障碍物中最近的障碍物的距离 如果大于设定的最大值则等于最大值 % 距离障碍物距离越近分数越低 function [dist,Flag] = CalcDistEval(x,ob,R) dist=100; for io = 1:length(ob(:,1)) disttmp = norm(ob(io,:)-x(1:2)')-R; %到第io个障碍物的距离 - 障碍物半径 ！！！有可能出现负值吗 if disttmp disttmp % 大于最小值 则选择最小值 dist = disttmp; end end % 障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重 if dist >= 3*R %最大分数限制 dist = 3*R; end调用 abs() 计算速度评价函数 vel = abs(vt); % 速度得分 速度越快分越高调用 CalcBreakingDist() 函数计算制动距离，并比较制动距离与最近障碍物距离，判断是否要舍弃该路径，这里相当于考虑了速度空间中的 Va %% 计算制动距离 %根据运动学模型计算制动距离, 也可以考虑成走一段段圆弧的累积 简化可以当一段段小直线的累积 function stopDist = CalcBreakingDist(vel,model) global dt; MD_ACC = 3;% stopDist=0; while vel>0 %给定加速度的条件下 速度减到0所走的距离 stopDist = stopDist + vel*dt;% 制动距离的计算 vel = vel - model(MD_ACC)*dt;% end NormalizeEval() 函数计算得到各轨迹的评价函数后，调用 NormalizeEval() 函数对各评价函数正则化 %% 归一化处理 % 每一条轨迹的单项得分除以本项所有分数和 function EvalDB=NormalizeEval(EvalDB) % 评价函数正则化 if sum(EvalDB(:,3))~= 0 % disp(EvalDB(:,3)); % disp(sum(EvalDB(:,3))); 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然后就选取评分最高的路径，并返回该路径对应的控制量 % 最终评价函数的计算 feval=[]; for id=1:length(evalDB(:,1)) feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %根据评价函数参数 前三个参数分配的权重 计算每一组可用的路径参数信息的得分 end evalDB = [evalDB feval]; % 最后一组；加最后一列，每一组速度的最终得分 [maxv,ind] = max(feval);% 选取评分最高的参数 对应分数返回给 maxv 对应下标返回给 ind u = evalDB(ind,1:2)';% 返回最优参数的速度、角速度获得最优控制量后，在主循环中更新机器人为下一时刻状态，并将历史轨迹保存，同时根据当前点与目标点间的欧氏距离判断是否到达目标点 x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度 abc = abc+1; % 历史轨迹的保存 result.x = [result.x; x']; %最新结果 以行的形式 添加到result.x，保存的是所有状态参数值，包括坐标xy、朝向、线速度、角速度，其实应该是只取坐标就OK % 是否到达目的地 if norm(x(POSE_X:POSE_Y)-goal')> dwa_V_1_0 Dynamic Window Approach sample program start!! ==========Arrive Goal!!========== 历时 46.357604 秒。 323将航向得分比重改为 0.04，结果如下
将航向得分比重改为 0.08，结果如下
可以看到当航向得分比重较小时规划的路径更安全，但相对长度也更长；当航向得分比重较大时规划的路径更短，但靠近障碍物时会“犹豫”，有一定撞到障碍物的风险 参考ROS 2D导航原理系列（六）|局部路径规划-DWA算法 DWA Planner | Husky Robot | Motion Planning for Robots DWA算法总结 DWA动态窗口法的原理及应用:The Dynamic Window Approach to Collision Avoidance The Dynamic Window Approach to Collision Avoidance 《The_dynamic_window_approach_to_collision_avoidance》 《基于动态窗口法和改进A*算法的多车路径规划》 《面向车场日巡检机器人的路径规划研究_李清天》 《移动机器人的SLAM与自主路径规划研究》 |
【本文地址】