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. 驶向目标点
   在判断旋转到位后，机器人开始向目标点运动，同理机器人计算自己距离目标点的距离和偏离角度，其运动速度的大小受到该距离的远近影响，当距离目标点越近时速度越小，同时偏离角度会给机器人一个旋转方向的一个很小的速度使机器人实现自我校正，当偏离角度大于某个值时会回到第一阶段。这种速度控制在相对不够平滑，可以修改为机器人的速度大小只与当前位置距离全局路径终点的距离有关，这样经过中间点时就不会减速。
3. 到达目标点后向全局规划给定的最终位姿旋转。
   在机器人与目标位置的距离小于误差容忍度时，机器人便会停止运动进入第三阶段的旋转，该旋转的目的是为了使机器人朝向全局规划中给定的方向，其旋转原理与过程1类似。

在实际的算法中，这三个过程不是并列的，而是有优先级的，即机器人先判断当前位置与目标位置的连线与当前朝向的角度是否大于角度误差容忍度，如果大于则会停下进行旋转，小于时才会继续运动；运动过程中会判断机器人当前位置距离终点的距离是否小于位移误差容忍度，如果小于则会停止运动进行第三步的位姿调整，否则会继续运动。
这种优先级保障了机器人不会在丢失定位或者偏差过大的情况下依然会向错误的方向移动，也不会在距离目标点很近的时候才会调整自己的位置，而会保证机器人始终以比较精确的方向在向目标点运动。

速度控制

在上节中已经提及机器人运动角速度和速度的大小影响因素，下面详细介绍其速度控制原理。

如上图所示为FTC速度计算的过程，给定机器人的参数sim_time=1s, max_x_vel=2m/s, max_rotation_vel=50degree/s，绿色线条为全局路径。
在上图上半部分中机器人计算出在sim_time时间内自己最远能到达的距离2m，在全局路径中逐个排查2m范围内的所有离散点，之后机器人再次计算出sim_time时间内自己最大能旋转的角度，在2m范围内逐个排查偏差角度最大的点，根据两次排查选择可以到达得最远的点（上部分第三张图）作为目标点，其角度偏差为10°，距离机器人2m，因此计算得到旋转速度10rad/s，平移速度2m/s，在与max_x_vel和max_rotation_vel比较后发现没有超过最大速度，于是采用计算结果作为速度大小，如果超过最大速度则使用最大速度；上图下半部分与上半部分不同之处在于，其全局路径的弧度非常大，但是计算过程相同，机器人先找到2m范围内的最远的点（下半部分第一张图），但是发现其偏差角度大于50°，于是在这个点与机器人之间再寻找满足偏差角度和最远距离的点，经过排查最终发现全局路径上与机器人偏差50°方向距离1.8m处的点满足条件，将其作为目标点进行平移速度和旋转速度的计算得到旋转速度50grad/s，平移速度1.8m/s。
机器人的速度是实时更新的，由于默认速度控制过程不够平滑，因为机器人每到一个中间点都会减速，因此将距离的判断修改为局路径终点的距离，这样使机器人在中途运动时始终以较平稳且快的速度运动，只有在距离终点的距离小于最大距离sim_time*max_x_vel时才采用上述的计算方法。
在机器人快要到达终点时，为了避免机器人速度过快驶过终点，FTC提供一个线性减速机制，每次计算速度时都对机器人的平移速度作减速处理，直到机器人静止，这样可以保证机器人平稳减速。
由其运动原理和速度计算原理图可以发现，该局部规划方法只适用于差分驱动的机器人，同时没有后退机制，对于我们来说这个不足恰巧变成了优点，因为FTC是只针对差分驱动和圆形机器人设计的局部规划方法。

避障原理

FTC提供一种基于代价地图的避障算法，它需要实时更新全局代价地图，当机器人的局部代价地图中存在障碍物时，其默认的处理方法是使机器人由平移运动的第二阶段进入第一阶段，即旋转状态，使用减速机制缓慢减速，保持低速并尝试重新规划路径，同时会与上一时刻的代价值进行比较，如果判断这一时刻代价值大于上一时刻的代价值，则判断为机器人可能会在之后发生碰撞，并返回false给move_base使机器人静止。这里我们可以修改为在局部控制中使机器人直接停止运动，直到障碍物消失，同时去除return false，使机器人保持当前状态而避免进入旋转恢复。
这个方法存在的问题是它由于依赖局部代价地图进行判断，存在延迟或者更新错误的情况，实际测试机器人的避障判断效果并不好，在机器人错误判断障碍物时会存在时停时走的情况发生，如果机器人后方有动态障碍物进入其局部代价地图范围内，机器人同样会判断为进入避障状态，因此建议去掉该避障功能以及代价地图的使用。

FTC源码简介

FTC代码结构简单，主要有核心代码部分和参数配置文件，其中核心代码又分为FTC规划（ftc_planner.cpp），全局路径转换（transform_global_plan.cpp）和加入代价地图（join_costmap.cpp）三部分，参数配置文件FTCPlanner.cfg中包含了所有代码中要使用的参数及其默认值。
这里主要介绍主函数ftc_planner.cpp中相关函数的功能

函数名称	实现功能
setplan	设置目标点函数是在move_base中将全局规划的路径传递给局部规划，并起到初始化局部规划的部分参数的作用
computeVelocity	在setPlan成功后，move_base会开始启动局部规划器进行速度控制，由第一章原理部分可知，计算速度指令分为三个阶段，其有前后优先级的关系，结构清晰
driveToward	平移速度计算，默认的速度控制不够平滑
calculteAngle	计算机器人与目标点的角度偏差
rotateToOrientation	旋转速度计算，默认的速度控制反应缓慢
checkMaxDist	计算机器人在sim_time时间内以max_vel_x可到达的最远点max_point，该变量在速度控制等函数中有非常关键的作用
checkMaxAngle	计算机器人与全局规划的点中相差角度最大的点，与checkMaxDist配合使用，具体参考速度控制原理章节
checkCollision	避障检测机制，该函数在速度控制函数中被调用，建议注释掉

FTC规划使用评价

FTC虽然不如DWA常用，但依然是非常有效的局部规划器，根据前述分析我们可以看出，在固定路径规划中FTC主要有如下优势：

1. 快速驾驶：机器人尽可能达到最大速度行驶；
2. 参数很少：只需设置11个参数，重要的参数只有几个，方便调试；
3. 很少有剧烈转换的情况，控制平稳，能够一次性控制到位，同时对AMCL撒点带来的畸变影响不大；
4. 小房间情况：机器人紧密跟随全局路径，不像dwa_local_planner那样存在围绕目标点循环的问题，所以它可以在小房间中无冲突行驶；
5. 代码结构简单，处理时间短，同时留给我们的开发空间更大，可以添加我们想要的功能。

同时有以下不足：

1. 仅适用于差分驱动的机器人；
2. 仅有向前走，没有后退机制，需自己修改；
3. 理论上其避障机制可以判别机器人身后的障碍物，但实际应用效果一般，可以去掉。

对于FTC规划器我们可以充分利用其优势，实现更好效果的固定路径规划，同时对其避障方法进行修改或者替换，以实现更好的效果，大家可以在目前的调试基础上添加更加完善的功能。

博主有两年多ROS的使用经验，目前仍在不停研究中。本系列ROS及SLAM进阶教程将涵盖ROS的进阶功能使用、机器人SLAM及导航的设计及研究等领域，持续不断更新中。如果大家有相关问题或发现作者漏洞欢迎私戳，同时欢迎关注收藏。
同时欢迎关注博主Git：
https://github.com/redglassli