Movebase之FTC局部规划器详解

FTC原理介绍

FTC(Follow the Carrot)跟随萝卜算法是Movebase中局部规划器的一种，给定一个全局规划路径和代价地图之后，该算法会产生一个速度控制指令驱动机器人运动，是一种行之有效同时原理简单、便于操作的局部规划算法，该算法在高精度SLAM及导航控制中可以表现出不错的效果。

原理部分详解及FTC的安装与使用请参考FTC wiki及ROS及SLAM进阶教程（四）Movebase使用小结，本文配图来自wiki官方

FTC会尽可能地贴合全局规划路径进行局部路径规划，它基于全局路径计算出的速度会使机器人到达全局路径中的一个中间点，从而进行下一步采样。

运动控制

在全局规划给定后，FTC会根据两个参数（最大速度max_vel，仿真时间sim_time）计算出机器人在sim_time时间内能到达的最远距离，在最远距离的范围内取全局规划路径中距离机器人最远的点作为这一时刻的目标点，在设定目标点之后，FTC规划过程分为三步：

1. 向目标点进行旋转
   机器人计算出目标点与当前位置连线与机器人朝向的夹角，从而进行旋转，旋转的速度与夹角的大小有关，直到夹角的大小小于旋转容忍度时才会停止旋转。由于在夹角越小时机器人旋转速度会越慢，因此一般不会过度旋转，并且旋转得非常平稳，产生的平移位移非常小。
2. 驶向目标点
   在判断旋转到位后，机器人开始向目标点运动，同理机器人计算自己距离目标点的距离和偏离角度，其运动速度的大小受到该距离的远近影响，当距离目标点越近时速度越小，同时偏离角度会给机器人一个旋转方向的一个很小的速度使机器人实现自我校正，当偏离角度大于某个值时会回到第一阶段。这种速度控制在相对不够平滑