ROS中的局部导航算法

ROS机器人导航时，首先在代价地图上使用全局规划算法规划出一条路径，然后调用局部规划算法及代价地图的信息规划出具体的行动指令。其中，局部路径规划算法在ros中主要是DWA算法（Dynamic Window Approach），基本操作是在速度空间(v,w)中采样多组速度，并模拟一定时间内的运动轨迹，再通过一个函数（Gerkey的论文《planning and control in unstrutured Terrain》中的评价函数）评价这些轨迹的分数，选择其中最优轨迹的速度，发送给下位机。
ROS中机器人运动模型，使用的是简单的模型。机器人在x轴上的轨迹为X=X0+VΔtcos(θt)-VyΔtsin(θt)，y轴上的轨迹为Y=Y0+VΔtsin(θt)+VyΔtcos(θt)，旋转角度的轨迹为θt=θ0+wΔt，其中，v为机器人的运动速度，θt为旋转角度，ros中的相关代码在simple_trajectory_generator.cpp的256行。
速度采样要考虑三个方面：1.机器人自身的最大速度与最小速度；2.机器人电机性能的影响，所能实现的实际速度区间；3.机器人检测到障碍物之后，能成功刹车的速度区间。在速度采样之后，通过评价函数对每一个速度进行评价，选择最优的速度发送给下位机。
ROS中的局部路径规划程序：
1.初始化：在move_base节点中，通过类加载模块载入了BaseLocalPlanner的子类DWAPlannerROS的实例tc_，并调用初始化函数，获取一些初始状态信息，并创建DWAPlanner类的实例dp_，最后设置类动态参数配置服务。dp_的构造函数做了一些列参数获取的操作，最重要的是将几种代价算法的实例加入critics的vector容器里。
2.采样速度：当move_base调用tc_的computeVelocityCommands方法后，tc_会调用dwaComputeVelocityCommands方法，并在其中调用dp_的findBestPath方法。findBestPath方法里调用SimpleTrajectoryGenerator类的实例generator_的initialise函数，这个函数就是负责速度采样。x、y、z三个维度速度需要采样的养本数存放在vsamples_这个结构体内，vsamples_[0]是x方向样本数，vsamples_[1]是y方向样本数，vsamples_[2]是z方向样本数。首先计算各个方向的最大速度和最小速度，DWA算法只在第一步进行采样，所以最大速度为:Max_vel=max(max_vel,vel+acc_limsim_period)，
最小速度为:Min_vel=min(min_vel,vel-acc_limsim_period)，
其中max_vel，min_vel为人为设定的最大和最小速度，vel是当前速度，acc_lim是人为设定的最大加速度，sim_period是第一步的模拟时间，由人为设定的局部路径规划频率决定，默认为0.05。
当计算出各个维度的最大最小速度后，就创建三个VelocityIterator类的对象，并传入最大最小速度和样本数目，此对象的构造函数会生成同样数目的速度样本并放入samples_这个容器内。具体做法是先计算步长step_size=(max-min)/(nums_samples-1)，max为最大速度，min为最小速度，nums_samples为样本数目。从最小速度每次多累加一次step_size即为一个速度样本，直到达到最大速度。将每个维度的速度样本取得后，再全部循环每个样本组里选择一个组合放入结构体vel_sample，最后将这些vel_sample放入sample_params_的容器里。至此，速度采样就完成了。
3.评价函数评分：速度采样完成后，采用scoreTrajectory函数对样本空间中的样本进行评分。scoreTrajectory函数对样本速度调用critics容器中的代价函数计算每个代价，并将其累加，得到总的代价。并且，计算过程中，一旦正在累加的代价和大于目前最小的代价和，则放弃这组样本速度。代价函数共有4个，分别为ObstacleCostFunction（感知障碍物来评价）、MapGridCostFuntion（轨迹离全局路径或者接近目标点有多近来评价）、OscillationCostFunction（震荡的大小来评价）、PreferForwardCostFunction（轨迹是否向前来评价）。
4.发送：将最优的一组样本速度发送给move_base，并发布相应的局部规划。move_base收到可用的速度，将该速度发送给地盘。如果move_base收到不可用，则发送0速度给地盘。且如果寻找最优速度的时间超过了限制就会执行障碍物清理模式，state_会变为CLEARING。

参考：https://blog.csdn.net/lqygame/article/details/72861439
：https://blog.csdn.net/heyijia0327/article/details/44983551