DWA局部路径规划

DWA算法的介绍

ros中使用的DWA算法主要是通过获取机器人现在的速度,和设置的最大加速度,获取机器人现在可以达到的速度集合.计算这个速度集合中所有的速度的代价,选择代价最小的速度发送给机器人.
这样可以把DWA的软件实现主要看作两个目标
1. 获取速度集合
2.计算速度集合中各个速度的代价

1.获取速度集合

在DWA代码中使用SimpleTrajectoryGenerator类来获取机器人的速度集合.
计算速度集合需要传入机器人的当前位姿,速度,设定的参数(最大速度,最小速度,速度分辨率等),通过现在速度和设定的加速的判断机器人现在可以达到的最大速度和最小速度(通过机器人现在速度加减机器人加速度,再与最大速度比较最后得到一个机器人当前可以到达的最大速度和最小速度),再通过设定的机器人速度分辨率完成一个机器人最大速度到最小速度的全排列(x轴 y轴 角速度是分开的)最后速度集合的大小

 vector.size = vx_samples*vy_samples*vth_samples

2.计算速度集合中各个速度的代价计算速度集合中各个速度的代价

在遍历速度集合的时候,会调用

bool generateTrajectory(Eigen::Vector3f pos,
Eigen::Vector3f vel,Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj);

计算得到在这个速度下运行的轨迹,最后计算的轨迹会保存在traj中.traj中保存着机器人的目标速度,该速度下的轨迹,机器人的当前位置等.使用这些数据计算以速度移动的代价

1. map_grid_cost_function:
   地图映射代价,这个计算函数可以计算两种代价,
   1与全局路径的差异(与全局路径越相似代价越小)
   2与局部地图上的目标点的远近(全局路径与局部地图上的交点为局部地图上的目标点,离目标点越近代价越小)
   两种代价计算都是实现计算好局部地图上所有点的代价,直接通过路径上的点在代价地图上映射得到的代价计算的.减少代价的重复计算
2. Obstacle_Cost_Function
3. Oscillation_Cost_Function

---

下次写完