ros局部路径规划-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

 

 

 

posted on 2018-02-28 17:30  kuangxionghui  阅读(25778)  评论(0编辑  收藏  举报