激光SLAM导航系列(五)局部路径规划

局部路径规划
局部路径规划简介
机器人在获得目的地信息后，首先经过全局路径规划规划出一条大致可行的路线，然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略，ROS中主要是使用了DWA算法。在ROS中每当move_base处于规划状态就调用DWA算法计算出一条最佳的速度指令，发送给机器人运动底盘执行。

DWA算法
DWA算法全称为dynamic window approach，其原理主要是在速度空间（v,w）中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹打分，最优的速度被选择出来发送给下位机。
1.确定机器人模型
在动态窗口算法中，要模拟机器人的轨迹，需要知道机器人的运动模型。假设两轮移动机器人的轨迹是一段一段的圆弧或者直线（旋转速度为0时），一对（vt，wt）就代表一个圆弧轨迹,具体推导如下:
假设不是全向运动机器人，它做圆弧运动的半径为：

当旋转速度w不等于0时，机器人坐标为：


2.速度采样
机器人的轨迹运动模型有了，根据速度就可以推算出轨迹。因此只需采样很多速度，推算轨迹，然后评价这些轨迹好不好就行了。
（一）移动机器人受自身最大速度最小速度的限制

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

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

3.评价函数
当采样完速度后，就对每个速度所形成的每条轨迹进行评价，采用的评价函数如下：

局部路径规划程序架构
1.程序模块流程

（1）初始化：为DWA算法做准备，加载参数和实例化对象等
（2）采样速度样本：计算出需要评价的速度样本
（3）样本评分：对计算出的速度样本进行逐一评分，记录下评价最高的样本
（4）发布plan：发布得到的局部路径规划策略
2.DWA算法程序分析
（1）初始化：
在move_base节点中，通过类加载模块载入了BaseLocalPlanner（局部路径规划）的子类DWAPlannerROS的实例tc_，并调用其初始化函数，获取了一些初始状态信息比如机器人当前位置等，并创建了真正实现DWA算法的DWAPlanner类的实例dp_，最后设置了动态参数配置服务。dp_的构造函数做了一系列参数获取的操作，最重要的是将几种cost计算方法的实例加入一个名为critics的vector容器里。
（2）采样速度样本：
当move_base调用tc_的computeVelocityCommands方法后，tc_会调用dwaComputeVelocityCommands方法，并在其中调用dp_的findBestPath方法。findBestPath方法里调用SimpleTrajectoryGenerator类的实例generator_的initialise函数，这个函数就是主要负责速度采样的。
每个维度速度需要采样的养本数存放在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_的容器里。至此，速度采样就完成了。
（3）样本评分
速度采样完成后，逐一循环对样本空间内的样本进行评分。对每一组速度调用scoreTrajectory函数计算其评分，而scoreTrajectory函数则对这一组速度调用所有critics容器里的costfunction计算每个cost从而累加算出总的cost。在计算过程中，一旦累加的cost大于当前最小的cost则抛弃这组速度。
之前说到的几种cost成本函数为下列所示：
ObstacleCostFunction
这个成本函数基于感知障碍物来评估轨迹。它或者由于轨迹通过障碍物而返回负值，或者0。
MapGridCostFunction
这个成本函数类基于轨迹离全局路径或者接近目标点有多近来评估轨迹。这个尝试利用距离预计算地图有相同距离的路径或者目标点的所有的规划，来优惠计算速度。
在 dwa_local_planner中，代价函数因为不同的目的，被多次实例化。保持轨迹接近于路径，使机器人朝局部目标前进，并且使机器人的前段点指向局部目标。代价函数是一个启发，可以带来坏的结果或者不合适的参数的失败。
OscillationCostFunction
震荡发生在X,Y,theta维度上，正/负值被连续的选择。为了阻止震荡，当机器人在任何方向移动时，与下一个循环相反的方向被标记为无效，直到机器人已经从所设置标记的位置移动而并且超过一定的距离。这个成本函数类帮助减少某些震荡，虽然这可以有效的阻止这些震荡，如果使用不合适的参数，但是有可能阻止良好的解。
PreferForwardCostFunction
考虑到好的激光扫描范围只在机器人的前面，这个成本函数类被设计在像PR2一样的机器人上。成本函数更喜欢正面向前运动，惩罚背面运用及扫射动作。在其他机器人上或者其他领域，这可能是非常不可取的行为。
（4）发布plan
通过上述几种评分机制，选取最优的一组速度样本，传递给move_base，并发布相应的local plan。move_base如果收到了可用的速度则发布给底盘，否则发布0速度，且如果寻找最优速度的时间超过了限制就会执行障碍物清理模式，state_会变为CLEARING。

部分参考自 http://blog.csdn.net/heyijia0327/article/details/44983551