路径规划局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）

Posted 2023-04-11 CHH3213

文章目录

• 参考资料
• 1. DWA算法原理
• • 1.1 简介
  • 1.2 算法原理
  • • 1. 速度采样
    • 2. 轨迹预测（轨迹推算）
    • 3. 轨迹评价
• 2. Python实现
• • 2.1 参数配置
  • 2.2 机器人运动学模型
  • 2.3 DWA算法类实现
  • 2.4 画图
  • 2.5 主函数
• 3. c++实现
• 4. 总结

参考资料

• The Dynamic Window Approach to Collision Avoidance
• 基于室内多障碍物复杂环境的路径规划方法研究
• 基于改进A*算法与改进DWA算法的无人驾驶汽车路径规划研究
• 室内移动机器人路径规划方法研究
• 机器人局部避障的动态窗口法

1. DWA算法原理

1.1 简介

• 动态窗口算法(Dynamic Window Approaches, DWA) 是基于预测控制理论的一种次优方法，因其在未知环境下能够安全、有效的避开障碍物, 同时具有计算量小, 反应迅速、可操作性强等特点。

• DWA算法属于局部路径规划算法。

• DWA算法的核心思想是根据移动机器人当前的位置状态和速度状态在速度空间 $(v,\omega )$ 中确定一个满足移动机器人硬件约束的采样速度空间，然后计算移动机器人在这些速度情况下移动一定时间内的轨迹， 并通过评价函数对该轨迹进行评价，最后选出评价最优的轨迹所对应的速度来作为移动机器人运动速度， 如此循环直至移动机器人到达目标点。

• 对于无人驾驶汽车而言，情况类似，将车辆的位置变化转化为线速度和角速度控制，避障问题转变成空间中的运动约束问题，这样可以通过运动约束条件选择局部最优的路径。

1.2 算法原理

• DWA算法的思路是：先通过机器人数学模型采集机器人速度样本，并预测模拟出在样本速度下一段时间内生成的运动轨迹， 并对这些运动轨迹进行标准评价， 选择出一组最优轨迹，机器人按照最优轨迹运动。机器人的运动姿态和方向是由机器人当前的线速度及角速度 (转向速度) 共同决定的。

• DWA算法主要包括3个步骤：速度采样、轨迹预测（推算）、轨迹评价。

1. 速度采样

由于移动机器人硬件、结构和环境等限制条件，移动机器人的速度采样空间 ${\mathbf{V}}_{\mathrm{s}}$ 中 $(v,\omega )$ 有一定的范围限制。该限制主要分为三大类：

• 速度边界限制

  根据移动机器人的硬件条件和环境限制, 移动机器人的速度存在的边界限制, 此时可采样的速度空间 ${\mathbf{V}}_m$ 为
  $${\mathbf{V}}_m=(v,\omega )\mid v\in \left[v_{\text{min }},v_{\text{max }}\right],\omega \in \left[{\omega }_{\text{min }},{\omega }_{\text{max }}\right]\text{\hspace{1em}(1)}$$
  式中 $v_{\text{min }}、v_{\text{max }}$ 分别为移动机器人最小线速度和最大线速度, ${\omega }_{\text{min }}、{\omega }_{\max }$ 分别 为移动机器人最小角速度和最大角速度。

• 加速度限制

  由于移动机器人的驱动电机的限制， 故移动机器人的线加速度和角加速度均存在边界限制，假设最大加速度和减速度大小一样，故考虑加速度时可采样的速度空间 ${\mathbf{V}}_d$ 为
  $${\mathbf{V}}_d=(v,\omega )\mid \begin{array}{l}v\in \left[v_c-a_{v\max }\cdot \Delta t,v_c+a_{v\max }\bullet \Delta t\right]\\ \omega \in \left[{\omega }_c-a_{w\max }\bullet \Delta t,{\omega }_c+a_{w\max }\bullet \Delta t\right]\end{array}\text{\hspace{1em}(2)}$$
  式中 $v_c、{\omega }_c$ 分别为移动机器人当前时刻的线速度和角速度，$a_{v\max }、a_{w\text{ max }}$ 分别为 移动机器人最大线加速度和最大角加速度。

• 环境障碍物限制

  局部规划还需要有动态实时的避障功能。考虑移动机器人的周围的障碍物因素，某一时刻移动机器人不与周围障碍物发生碰撞的可约束条件为
  $${\mathbf{V}}_a=\left\{(v,\omega )\mid \begin{array}{l}v\in \left[v_{\min },\sqrt{2\cdot dist(v,\omega )\cdot a_{v\max }}\right]\\ \omega \in \left[{\omega }_{\min },\sqrt{2\cdot dist(v,\omega )\cdot a_{\omega \max }}\right]\end{array}\right\}\text{\hspace{1em}(3)}$$

  式中$\mathrm{dist}(v,\omega )$表示当前速度下对应模拟轨迹与障碍物之间的最近距离。 在无障碍物的情况下 $\mathrm{dist}(v,\omega )$ 会是一个很大的常数值。当机器人运行采样速度在公式 (3) 范围时, 能够以最大减速度的约束实现安全减速直至避开障碍物。

  注意: 这个限制条件在采样初期是得不到的，需要我们先使用${\mathbf{V}}_m\cap {\mathbf{V}}_d$的速度组合采样模拟出轨迹后, 计算当前速度下对应模拟轨迹与障碍物之间的最近距离, 然后看当前采样的这对速度能否在碰到障碍物之前停下来， 如果能够停下来, 那这对速度就是可接收的。如果不能停下来, 这对速度就得抛弃掉。

  我在代码中并没有采用这种做法，而是直接计算机器人当前位置（并不是模拟轨迹）与障碍物的最近距离来得到 V a \\mathbfV_a V

  matlab基于蚁群算法的二维路径规划算法matlab优化算法二十二

  基于蚁群算法的二维路径规划算法

  路径规划算法

  路径规划算法是指在有障碍物的工作环境中寻找一条从起点到终点的、无碰撞地绕过所有障碍物的运动路径。路径规划算法较多，大体上可分为全局路径规划算法和局部路径规划算法两类。其中，全局路径规划方法包括位形空间法、广义锥方法、顶点图像法、栅格划归法；局部路径规划算法主要有人工势场法等。

  MAKLINK图论理论

  MAKLINK图论可以建立二维路径规划的空间模型， MAKLINK图论通过生成大量的MAKLINK线构造二维路径规划可行空间， MAKLINK线定义为两个障碍物之间不与障碍物相交的顶点之间的连线，以及障碍物顶点与边界相交的连线。典型 MAKLINE图形如图所示。
  
  在 MAKLINK图上存在l条自由连接线，连接线的中点依次为v,v，…，v，连接所有MAKLINK线的中点加上始点S和终点T构成用于初始路径规划的无向网络图

  蚁群算法

  蚁群算法是由 dorigo.M等人在20世纪90年代初提出的一种新型进化算法，它来源于对蚂蚁搜索问题的研究。人们在观察蚂蚁搜索食物时发现，蚂蚁在寻找食物时，总在走过的路径上释放一种称为信息素的分泌物，信息素能够保留一段时间，使得在一定范围内的其他蚂蚁能够觉察到该信息素的存在。后继蚂蚁在选择路径时，会选择信息素浓度较高的路径，并且在经过时留下自己的信息素这样该路径的信息素会不断增强，蚂蚁选择的概率也在不断增大蚁群算法最优路径寻找如图。
  
  图表达了蚂蚁在觅食过程中的三个过程，其中点A是蚂蚁蚁巢，点D是食物所在地，四边形 EBFCE表示在蚁巢和食物之间的障碍物。蚂蚁如果想从蚁巢点A达到点D，只能经过路径BFC或者路径BEC，假定从蚁巢中出来若干只蚂蚁去食物所在地D搬运食物，每只蚂蚁经过后留下的信息素为1，信息素保留的时间为1.一开始，路径BFC和BEC上都没有信息素，在点A的蚂蚁可以随机选择路径，蚂蚁以相同的概率选择路径BFC或BEC，如图（b）所示。由于BFC路径长度只是BEC路径长度的一半，所以在一段时间内经过BFC到达点D的蚂蚁数量是经过BEC到达点D数量的两倍，在路径BFC上积累的信息素的浓度就是在路径BEC上积累的信息素浓度的两倍。这样在蚂蚁选择路径的时候，选择路径BFC的概率大于选择路径BEC的概率，随着时间的推移，蚂蚁将以越来越大的概率选择路径BFC，最终会完全选择路径BFC作为从蚁巢出发到食物源的路径，如图（c）所示。

  dijkstra算法

  dijkstra算法是典型的单源最短路径算法，用于计算非负权值图中一个节点到其他所有节点的最短路径，其基本思想是把带权图中所有节点分为两组，第1组包括已确定最短路径的节点，第2组为未确定最短路径的节点。按最短路径长度递增的顺序逐个把第2组的节点加入第1组中，直到从源点出发可到达的所有节点都包含在第1组中。
  dijkstra算法流程如下：
  （1）初始化存放未确定最短路径的节点集合V和已确定最短路径的节点集合S，利用带权图的邻接矩阵arcs初始化原点到其他节点最短路径长度D，如果源点到其他节点有连接弧，对应的值为连接弧的权值，否则对应的值取为极大值
  （2）选择D中的最小值D【i】,D【i】是源点到点i的最短路径长度，把点i从集合V中取出并放入集合S中。
  （3）根据节点i修改更新数组D中源点到集合V中的节点k对应的路径长度值
  （4）重复步骤（2）与步骤（3）的操作，直至找出源点到所有节点的最短路径为止。
  采用蚁群算法在200×200的二维空间中寻找一条从起点S到终点T的最优路径，该二维空间中存在4个障碍物，障碍物1的4个顶点的坐标分别为（40140；60160；
  100140；60120），障碍物2的4个顶点分为别（5030；3040；8080；10040），障碍物3的4个顶点分别为（120160；140100；180170；165180），障碍物4的3个顶点分别为（12040；17040；14080），其中点S为起点，起点坐标为（20,180）；点T为终点，终点坐标为（160,90）。二维规划空间如图所示。
  
  

  dijkstra算法

  采用 dijkstra算法规划初始路径，其算法思想是先计算点点之间的距离，然后依次计算各点到出发点的最短距离，程序如下：

  function path = DijkstraPlan(position,sign)
  %% 基于Dijkstra算法的路径规划算法
  %position    input     %节点位置
  %sign        input     %节点间是否可达
   
  %path        output    %规划路径
   
  %% 计算路径距离
  cost = ones(size(sign))*10000;
  [n,m] = size(sign);
  for i = 1:n
      for j = 1:m
          if sign(i,j) == 1
              cost(i,j) = sqrt(sum((position(i,:)-position(j,:)).^2));
          end
      end
  end
   
  %% 路径开始点
  dist = cost(1,:);             %节点间路径长度           
  s = zeros(size(dist));        %节点经过标志
  s(1) = 1;dist(1) = 0;
  path = zeros(size(dist));     %依次经过的节点
  path(1,:) = 1;
   
  %% 循环寻找路径点
  for num = 2:n   
      
      % 选择路径长度最小点
      mindist = 10000;
      for i = 1:length(dist)
          if s(i) == 0
              if dist(i)< mindist
                  mindist = dist(i);
                  u = i;
              end
          end
      end
      
      % 更新点点间路径
      s(u) = 1;
      for w = 1:length(dist)
          if s(i) == 0
              if dist(u)+cost(u,w) < dist(w)
                  dist(w) = dist(u)+cost(u,w);
                  path(w) = u;
              end
          end
      end
  end
  
  

  主程序

  %% 清空环境
  clc;clear
  
  %% 障碍物数据
  position = load('barrier.txt');
  plot([0,200],[0,200],'.');
  hold on
  B = load('barrier.txt');
  xlabel('km','fontsize',12)
  ylabel('km','fontsize',12)
  title('二维规划空间','fontsize',12)
  %% 描述起点和终点
  S = [20,180];
  T = [160,90];
  plot([S(1),T(1)],[S(2),T(2)],'.');
  
  % 图形标注
  text(S(1)+2,S(2),'S');
  text(T(1)+2,T(2),'T');
   
  %% 描绘障碍物图形
  fill(position(1:4,1),position(1:4,2),[0,0,0]);
  fill(position(5:8,1),position(5:8,2),[0,0,0]);
  fill(position(9:12,1),position(9:12,2),[0,0,0]);
  fill(position(13:15,1),position(13:15,2),[0,0,0]);
  
  % 下载链路端点数据
  L = load('lines.txt');
   
  %% 描绘线及中点
  v = zeros(size(L));
  for i=1:20
      plot([position(L(i,1),1),position(L(i,2),1)],[position(L(i,1),2)...
          ,position(L(i,2),2)],'color','black','LineStyle','--');
      v(i,:) = (position(L(i,1),:)+position(L(i,2),:))/2;
      plot(v(i,1),v(i,2),'*');
      text(v(i,1)+2,v(i,2),strcat('v',num2str(i)));
  end
   
  %% 描绘可行路径
  sign = load('matrix.txt');
  [n,m]=size(sign);
   
  for i=1:n
      
      if i == 1
          for k=1:m-1
              if sign(i,k) == 1
                  plot([S(1),v(k-1,1)],[S(2),v(k-1,2)],'color',...
                      'black','Linewidth',2,'LineStyle','-');
              end
          end
          continue;
      end
      
      for j=2:i
          if i == m
              if sign(i,j) == 1
                  plot([T(1),v(j-1,1)],[T(2),v(j-1,2)],'color',...
                      'black','Linewidth',2,'LineStyle','-');
              end
          else
              if sign(i,j) == 1
                  plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],...
                      'color','black','Linewidth',2,'LineStyle','-');
              end
          end
      end
  end
  path = DijkstraPlan(position,sign);
  j = path(22);
  plot([T(1),v(j-1,1)],[T(2),v(j-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
  i = path(22);
  j = path(i);
  count = 0;
  while true
      plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
      count = count + 1;
      i = j;
      j = path(i);
      if i == 1 || j==1
          break;
      end
  end
  plot([S(1),v(i-1,1)],[S(2),v(i-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
  
  
  count = count+3;
  pathtemp(count) = 22;
  j = 22;
  for i=2:count
      pathtemp(count-i+1) = path(j);
      j = path(j);
  end
  path = pathtemp;
  path = [1     9     8     7    13    14    12    22];
  
  %% 蚁群算法参数初始化
  pathCount = length(path)-2;          %经过线段数量
  pheCacuPara=2;                       %信息素计算参数
  pheThres = 0.8;                      %信息素选择阈值
  pheUpPara=[0.1 0.0003];              %信息素更新参数
  qfz= zeros(pathCount,10);            %启发值
  
  phePara = ones(pathCount,10)*pheUpPara(2);         %信息素
  qfzPara1 = ones(10,1)*0.5;           %启发信息参数
  qfzPara2 = 1.1;                      %启发信息参数
  m=10;                                %种群数量
  NC=500;                              %循环次数
  pathk = zeros(pathCount,m);          %搜索结果记录
  shortestpath = zeros(1,NC);          %进化过程记录
   
  %% 初始最短路径
  dijpathlen = 0;
  vv = zeros(22,2);
  vv(1,:) = S;
  vv(22,:) = T;
  vv(2:21,:) = v;
  for i=1:pathCount-1
  dijpathlen = dijpathlen + sqrt((vv(path(i),1)-vv(path(i+1),1))^2+(vv(path(i),2)-vv(path(i+1),2))^2);
  end
  LL = dijpathlen;
   
  %% 经过的链接线
  lines = zeros(pathCount,4);
  for i = 1:pathCount
      lines(i,1:2) = B(L(path(i+1)-1,1),:);
      lines(i,3:4) = B(L(path(i+1)-1,2),:);
  end
   
  %% 循环搜索
  for num = 1:NC
      
      %% 蚂蚁迭代寻优一次
      for i=1:pathCount
          for k=1:m
              q = rand();
              qfz(i,:) = (qfzPara2-abs((1:10)'/10-qfzPara1))/qfzPara2; %启发信息
              if q<=pheThres%选择信息素最大值
                  arg = phePara(i,:).*(qfz(i,:).^pheCacuPara);
                  j = find(arg == max(arg));
                  pathk(i,k) = j(1);
              else  % 轮盘赌选择
                  arg = phePara(i,:).*(qfz(i,:).^pheCacuPara)算法学习之DWA局部路径规划算法
  
   路径规划基于改进动态窗口法DWA实现机器人动态避障matlab源码含 GUI
   路径规划基于改进动态窗口DWA算法机器人静态避障matlab源码
   路径规划基于matlab DWA动态避障路径规划含Matlab源码 2356期
   路径规划基于matlab DWA算法机器人局部避障路径规划含Matlab源码 890期
   路径规划基于matlab GUI改进的DWA算法机器人动态避障路径规划含Matlab源码 1271期