对局部避障的动态窗口法(dynamic window approach)的理解

首飞Kevin

算法原理

对算法原理的学习主要参考下面两个链接的文章：
机器人局部避障的动态窗口法(dynamic window approach) 讲解了DWA的算法原理
一个DWA学习专题基于原理讲解了代码是如何实现的

对主要工程文件的分析

costmap_model.cpp中CostmapModel类的footprintCost方法用于获取机器人外轮廓是否与障碍物重叠的cost值，cost为负数表明有异常。

    // returns:
    //  -1 if footprint covers at least a lethal obstacle cell, or
    //  -2 if footprint covers at least a no-information cell, or
    //  -3 if footprint is [partially] outside of the map, or
    //  a positive value for traversable space

footprint_helper.cpp中FootprintHelper的getFootprintCells方法返回footprint在costmap中的所有cell坐标。

latched_stop_rotate_controller.cpp中的LatchedStopRotateController类提供判断机器人是否已经到达目标点的函数，以及到达目标点的旋转操作，保证角度误差在设定的范围内。

local_planner_util.cpp中LocalPlannerUtil类提供getLocalPlan函数来根据当前机器人位置截取一段全局规划路径。

map_cell.cpp中定义了map中cell的数据结构。

map_grid_cost_function.cpp中的MapGridCostFunction继承了TrajectoryCostFunction虚基类，并实现了基于路径上栅格cost值给局部路径评分的函数scoreTrajectory。在该文件中, 因目的不同（为让轨迹尽可能接近全局路径，为让机器人朝着局部目标前进，还有为让机器人的头保持指向局部目标），最终执行的代价函数因配置不同而不同。

map_grid.cpp中定义了地图的数据结构。

obstacle_cost_function.cpp中的ObstacleCostFunction类TrajectoryCostFunction虚基类，并实现了基于footprint轮廓范围内最大栅格cost值给局部路径评分的函数scoreTrajectory。

odometry_helper_ros.cpp中OdometryHelperRos类用于获取里程计得到的速度。

oscillation_cost_function.cpp中的OscillationCostFunction类TrajectoryCostFunction虚基类，并实现了基于是否出现震荡给局部路径评分的函数scoreTrajectory。当机器人从标记震荡的位置处离开一定距离就会复位震荡标志位。

prefer_forward_cost_function.cpp中的PreferForwardCostFunction类TrajectoryCostFunction虚基类，并实现了基于是否尽量直行评分的函数scoreTrajectory。

simple_scored_sampling_planner.cpp中实现的findBestTrajectory函数调用了各评分函数选出了评分最小的局部轨迹。

trajectory.cpp中定义了局部轨迹的数据结构。其中xv_,xy_,thetav_存储该轨迹采样的速度。

dwa_planner_ros.cpp中的computeVelocityCommands函数就是move_base调用来生成最终执行速度的函数。

simple_trajectory_generator.cpp中SimpleTrajectoryGenerator类的initialise函数根据加速度参数采样了所有可行的速度。generateTrajectory函数根据采样的速度和当前机器人位置生成采样的路径。

twirling_cost_function.cpp中的TwirlingCostFunction类TrajectoryCostFunction虚基类，并实现了基于旋转速度评分的函数scoreTrajectory。

注意：可以自己编写继承于TrajectoryCostFunction虚基类的cost_function文件来基于不同功能给局部仿真路径评分，从而选出适合场景的轨迹。输入轨迹到评分函数scoreTrajectory(Trajectory &traj)中, 该函数会输出轨迹评价分数。如果输出负分数意味着轨迹无效；如果输出正直，对于代价函数来说值越小越好。每个代价函数有一个比例因子，与其它代价函数比较时候，可以通过调节比例因子调节代价函数的影响程度。

参考：

http://wiki.ros.org/dwa_local_planner
python实现的DWA仿真代码
 dwa_planner vs. base_local_planner这里描述了一些dwa_planner 和 base_local_planne的差异