DWA算法在局部路径规划中的应用

ROS的路径规划器分为全局路径和局部路径规划，其中局部路径规划器使用的最广的为dwa，个人理解为：

首先全局路径规划会生成一条大致的全局路径，局部路径规划器会把全局路径给分段，然后根据分段的全局路径的坐标，进行局部重新规划，例如：

全局规划后有一组目标点数组【1,2,3,4,5，......】，局部是根据当前速度和时间、约束条件，随机模拟出50条轨迹，选出最佳一条轨迹到达1号目标点，随后重复2,3...，

一直到达到目的地为止。具体的步骤如下：

一、路径存储

1、全局路径存储

在ros move base控制循环中会在规划出新的路径时，将新的全局路径利用setPlan传给DWAPlannerROS，直接保存为global_plan_。

此时的路径时相对于全局地图的全局坐标系的（通常为"map")。

2、局部路径存储

在计算速度前，会先将全局路径映射到局部地图坐标系下（通常为“odom”），在getLocalPlan中，先将较长的全局路径映射并截断到局部地图内（即坐标系转换为局部地图，

且范围完全在局部地图内，超出地图的则抛弃，局部地图为costmap），局部地图使用的是rolling_window。然后，裁减全局路径和局部路径（与机器人当前位置距离超过1m的旧的路径会被裁减掉）。

有了局部的目标点，就可以做规划了。

二、机器人运动模型

运动模型推到是为了进行轨迹推演，具体可参考：http://blog.csdn.net/heyijia0327/article/details/44983551

三、速度采样

机器人的轨迹运动模型有了，根据速度就可以推算出轨迹。因此只需采样很多速度，推算轨迹，然后评价这些轨迹好不好就行了。但是，速度（Vx,Vw）理论上有无穷多组，假定是在两轮差速轮上，

因此根据机器人自身的条件和环境的限制，速度采样范围需要做一定的限制：

1、移动机器人受自身最大速度最小速度的限制

2、移动机器人受电机性能的影响:由于电机力矩有限,存在最大的加減速限制,因此移动机器人軌迹前向模拟的周期sim_period内,存在一个动态窗口,在该窗口内的速度是机器人能够实际达到的速度:

3、基于移动机器人安全的考虑:为了能够在碰到障碍物前停下来, 因此在最大减速度条件下, 速度有一个范围:

在获取空间边界时，根据use_dwa参数，采用两套策略。

如果use_dwa==false，首先用当前位置与目标位置的距离处理仿真时间sim_time（模拟仿真时间内匀减速到0，刚好到达目标点的情景）得到max_vel_x和max_vel_y边界，然后基于acc_lim * sim_time得到一种边界（上边界），还有设置的速度极限参数（max_vel_xxx，min_vel_xxx）作为一种边界，然后选取三种边界中空间较小的边界。这种策略，能够获得较大的采样空间（因为用了sim_time）。

如果use_dwa==true，则直接用acc_lim * sim_period得到边界，然后还有设置的速度极限参数作为边界，然后选取两种边界中空间较小的边界。

得到速度空间边界后，根据x，y，theta三个采样个数进行插补，进而组合出整个速度采样空间。

每个维度速度需要采样的养本数存放在vsamples_这个结构体内，vsamples_[0]是x方向样本数，vsamples_[1]是y方向样本数，vsamples_[2]是z方向样本数。首先计算各个方向的最大速度和最小速度，DWA算法只在第一步进行采样，所以最大速度为:
Max_vel=max(max_vel,vel+acc_lim*sim_period)
最小速度为:
Min_vel=min(min_vel,vel-acc_lim*sim_period)
其中max_vel，min_vel为人为设定的最大和最小速度，vel是当前速度，acc_lim是人为设定的最大加速度，sim_period是第一步的模拟时间，由人为设定的局部路径规划频率决定，默认为0.05。
当计算出各个维度的最大最小速度后，就创建三个VelocityIterator类的对象，并传入最大最小速度和样本数目，此对象的构造函数会生成同样数目的速度样本并放入samples_这个容器内。具体做法是先计算步长step_size：
step_size=(max-min)/(nums_samples-1)
max为最大速度，min为最小速度，nums_samples为样本数目。从最小速度每次多累加一次step_size即为一个速度样本，直到达到最大速度。将每个维度的速度样本取得后，再全部循环每个样本组里选择一个组合放入结构体vel_sample，最后将这些vel_sample放入sample_params_的容器里。至此，速度采样就完成了。

四、评价函数

根据采样的速度模拟推演出相对应的轨迹，然后对每一条轨迹进行评价，找出最优的一条轨迹进行局部导航。如何找出最优的轨迹？这里DWA使用的是一个打分系统，

原始的论文只有3个评分项：

最后对每一个项做归一化处理，目的是进行平滑。

评价函数的意义就是：找出一条最优路径，使得机器人在局部导航过程中，避开障碍物，以最快的速度到达目标点。

通过上述几种评分机制，选取最优的一组速度样本，传递给move_base，并发布相应的local plan。move_base如果收到了可用的速度则发布给底盘，否则发布0速度，且如果寻找最优速度的时间超过了限制就会执行障碍物清理模式，state_会变为CLEARING。

五、停止判断

对于DWAPlannerROS的停止处理逻辑，由LatchedStopRotateController类完成，主要包括了停止判断、加速停止、旋转至目标朝向三个大的部分。

在到达目标位置的前提下，还要判断机器人朝向是否满足目标朝向yaw_goal_tolerance的需求，如果也满足，则判断当前速度是否满足停止条件，即x和y的速度小于trans_stopped_velocity，theta速度小于rot_stopped_velocity。

只有到达位置、达到朝向、速度满足停止三个条件都满足的情况下，才算机器人到达了目标位姿，isGoalReached函数才会返回true。

参考：

https://www.cnblogs.com/sakabatou/p/8297479.html

http://blog.csdn.net/heyijia0327/article/details/44983551

分类: ROS and Slam

<div id="blog_post_info">

好文要顶关注我收藏该文

kuangxinghui
关注 - 2
粉丝 - 3

+加关注

<div class="clear"></div>
<div id="post_next_prev"><a href="https://www.cnblogs.com/kuangxionghui/p/8423507.html" class="p_n_p_prefix">« </a> 上一篇：    <a href="https://www.cnblogs.com/kuangxionghui/p/8423507.html" title="发布于 2018-02-06 18:11">ROS机器人路径规划介绍--全局规划</a>
<br>
<a href="https://www.cnblogs.com/kuangxionghui/p/8494566.html" class="p_n_p_prefix">» </a> 下一篇：    <a href="https://www.cnblogs.com/kuangxionghui/p/8494566.html" title="发布于 2018-03-02 18:13">几种常见建图算法简介</a>

DWA算法在局部路径规划中的应用相关推荐

dwa算法实现局部路径规划——基于python
dwa算法简介动态窗口法(dynamic window approach, dwa),用于实现机器人的局部路径规划实现原理: 在速度空间(v,w)中不断采样,模拟机器人在采样得到的速度下的运行轨迹 ...
arcgis做dijkstra_改进的Dijkstra算法在GIS路径规划中的应用
) ")$""!%$"* 收稿日期: %""*$!"$!) 作者简介: 李宁宁 ( !)')$ ) , 女, 吉林人, 北京理工大 ...
局部路径规划算法-DWA动态窗口法
引言本文主要是转载学习的内容,为了记录传统算法的学习,方便复习,对比看看传统算法和新的算法有哪些相似的地方. DWA算法,在仿真上效果好些,在扫地机器人等室内机器人方面可能效果好,但是在自动驾驶还是 ...
【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）
文章目录参考资料 1. DWA算法原理 1.1 简介 1.2 算法原理 1. 速度采样 2. 轨迹预测(轨迹推算) 3. 轨迹评价 2. Python实现 2.1 参数配置 2.2 机器人运动学模型 ...
局部路径规划算法——实现DWA（dynamic window approach）控制空间采样
DWA算法是局部路径规划算法,在全局路径规划算法完成后,DWA算法能够根据当前小车(机器人)位置.障碍物.终点的位置进行控制空间(速度.角速度)的采用,从而完成局部路径规划. DWA算法流程: 初始化 ...
智能仿生算法在机器人路径规划的应用综述
智能仿生算法在机器人路径规划的应用综述 1 绪论 1.1 机器人路径规划方法路径规划是移动机器人导航中最重要的技术之一. 对于移动机器人而言,路径规划就是解决特定工作环境和任务要求下的路径优选问题, ...
DWA局部路径规划算法论文阅读：The Dynamic Window Approach to Collision Avoidance。
DWA(动态窗口)算法是用于局部路径规划的算法,已经在ROS中实现,在move_base堆栈中:http://wiki.ros.org/dwa_local_planner DWA算法第一次提出应该是1 ...
【路径规划】基于matlab DWA算法机器人局部避障路径规划【含Matlab源码 890期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[路径规划]基于matlab DWA算法机器人局部避障路径规划[含Matlab源码 890期] 获取代码方式2: 通过订阅紫极神光博客付费 ...
ROS常用局部路径规划算法比较
本博文主要讨论ROS导航包中集成的局部路径规划算法,DWA.TEB.MPC等算法在使用过程中的各自的优缺点.以下均为自己在使用过程中总结的经验及查阅资料得来,如有理解不到位的地方,还希望在评论区多多讨 ...

DWA算法在局部路径规划中的应用

DWA算法在局部路径规划中的应用相关推荐

最新文章

热门文章