最近，作者参加了关于RMUS 高校 SimReal挑战赛，首次接触到了机器人导航领域，这里记录一下这段时间的收货。sim2real的全称是simulation to reality，是强化学习的一个分支，同时也属于transfer learning的一种。主要解决的问题是机器人领域中，直接让机器人或者机械臂在仿真中对于物理环境存在误差，如何将仿真上取得的成果应用到实际中的问题。
机器人导航的路径规划问题主要分为全局路径规划和局部路径规划，这两者是根据对环境信息获取程度划分的。
全局路径规划和局部路径规划并没有本质的区别，很多适用于全局路径规划的方法经过改进也可以用于局部路径规划，而适用于局部路径规划的方法同样经过改进后也可适用于全局路径规划。两者协同工作，机器人可更好的规划从起始点到终点的行走路径。
本文主要针对局部路径规划作介绍。
机器人在获得目的地信息后，首先经过全局路径规划规划出一条大致可行的路线，然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略。常用的局部路径规划算法有动态窗口法(DWA)、时间弹性带(TEB)和模型预测控制(MPC)。
动态窗口法在一定程度上采用了粒子滤波的思想，在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。
时间弹性带(TEB)简而言之，就是连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time。
t i m e + e l a s t i c b a n d = t i m e d e l a t i c s b a n d time + elastic band = timed elatics band time+elasticband=timedelaticsband
其目标函数的定义：
虽然待优化的橡皮筋有不少状态点与时间段，目标函数也好像很多。但是，每个目标函数只与橡皮筋中的某几个状态有关，而非整条橡皮筋。将它描述成图，然后用图优化。
MPC其实是一种基于对受控对象进行预测的控制方法。MPC最大的特点在于，相对于LQR控制而言，MPC可以考虑空间状态变量的各种约束，而LQR，PID等控制只能够考虑输入输出变量的各种约束。
MPC的作用机理可以表述为：在每一个采样时刻，根据当前的测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素用于被控对象；在下一个采样时刻，用新的测量值作为此时预测系统未来动态的初试条件，刷新优化问题求解。应用于机器人的典型的模型预测控制方法：
m i n C F ( x ( t f ) ) + f t = t 0 t f C R ( x , u ) d t x ˙ = f ( x , u ) g ( x , u ) < 0 h ( x , u ) = 0 minC_F(x(t_f))+f_{t=t_0}^{t_f}C_R(x, u)dt\\ \dot{x}=f(x, u)\\ g(x, u)<0\\ h(x,u)=0\\ minCF(x(tf))+ft=t0tfCR(x,u)dtx˙=f(x,u)g(x,u)<0h(x,u)=0
m i n C F ( x ( t f ) ) + f t = t 0 t f C R ( x , u ) d t minC_F(x(t_f))+f_{t=t_0}^{t_f}C_R(x, u)dt\\ minCF(x(tf))+ft=t0tfCR(x,u)dt
传统MPC控制框图为
这里以mpc_local_planner为例，首先，在本地终端中键入
sudo apt install ros-melodic-mpc-local-planner
由于apt安装有可能导致我们运行失败，上面的指令只是用来安装依赖。同时，克隆远程仓库mpc_local_planner到ros的工作空间中，执行catkin_make编译。对应ros navigation中的move_base.launch，修改其中的局部路径规划器类型
<?xml version="1.0"?>
<launch>
<!-- 运行move_base节点 -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<!--move_base参数配置http://wiki.ros.org/move_base -->
<param name="base_global_planner" value="global_planner/GlobalPlanner" /><!-- 选择全局规划器类型 -->
<rosparam file="$(find mpc_navigation)/config/base_global_planner_params.yaml" command="load" />
<rosparam file="$(find mpc_navigation)/config/mpc_local_planner_params_minimum_time.yaml" command="load" />
<param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />
<rosparam file="$(find mpc_navigation)/config/move_base_params.yaml" command="load" /><!-- 其它参数 -->
<param name="controller_frequency" value="1" /><!-- 选择全局规划器类型 -->
<!-- 全局代价地图和局部代价地图参数配置http://wiki.ros.org/costmap_2d -->
<rosparam file="$(find mpc_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find mpc_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find mpc_navigation)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_navigation)/config/global_costmap_params.yaml" command="load" />
</node>
</launch>
对应mpc_local_planner的参数文件可以在克隆仓库中的mpc_local_planner_examples中获得，将cfg/diff_drive中的参数yaml文件复制到对应的config文件夹中即可，base_ local_planner参数设置为mpc_local_planner中定义的类型MpcLocalPlannerROS。
mpc_local_planner提供了两个参数文件，一个是以最短时间为目的，另一个是二次规划模型，可以根据需求自行选择。
其他的局部规划器同样是修改base_local_planner的类型和提供相应的功能包以及yaml参数文件实现。
喜欢的话可以关注一下我的公众号技术开发小圈，尤其是对深度学习以及计算机视觉有兴趣的朋友，我会把相关的源码以及更多资料发在上面，希望可以帮助到新入门的大家！
