问题描述
机器人再跟踪轨迹过程中，有时会突然出现原地转圈跟踪的现象，如视频所示：
机器人原地转圈问题
一、MPC控制
先简单梳理下，之前写的mpc跟踪代码：
简单来说，fast_planner生成期望轨迹点，odom里程计得到小车当前实时位置，知道期望轨迹和当前位置就可以计算误差转换为最小值问题通过mpc控制器控制机器人进行跟踪。
这是加了终端惩罚的目标函数，约束条件为：
（1）动态约束：
（2）边界条件：
（2.1）初始状态约束和控制变量的界限：
这个mpc控制器很简单，并没有在其中加入避障等约束，就是用来跟踪生成的轨迹，通过规划避障，只要规划的轨迹够快跟踪足够精确，就不会发生碰撞。
尝试在MPC中加入转角约束来约束机器人转向，使每个时刻的theta变化不超过180度，即每个相邻时间步i和i+1机器人在角度方向的变化必须小于等于180度。
但加入角度约束后，仍出现转圈的现象，我就把相邻转角的差值打印出来看约束是否奏效。得到的图像如下：
发现规律有，一旦发生转圈问题后，大概隔7秒发生一次，相邻角度差值在350~400之间。思索了一下，把fast_planner轨迹生成时间间隔打印了下大概也相隔7秒左右。
所以不同于传统的轨迹跟踪(已知一条确定的轨迹)、通过控制器计算u控制机器人进行跟踪，fast_planner生成的是一条动态变化的轨迹。每次轨迹变化的过程中都会重新通过mpc计算线速度和角速度，所以mpc的约束只在局部有效，并且因为mpc计算的速度设置的很快(0.1秒)，所以加的角度约束在两段轨迹间是无效的。
我打印了各时刻角度值，出现角度跳变的情况都是在两次mpc计算之间。顺其自然的想到把前一段轨迹的机器人角度值保存起来，在下次计算时当作约束，使段与段之前的角度变化也小于180度。这样做了，但运行一段时间后找不到解。思考了一阵但并没用想到好的思路和解决办法，所以没有继续用mpc加约束条件来解决转向问题。
二、传入角度处理
我把传给mpc控制器的fast_planner期望角度和odom实际角度数据收集起来绘制出图。因为之前fast_planner角度以及经过处理了是平滑的，但发现Odom的角度是有跳变的，根据图像很容易判断出，在接近180度时机器人的角度由+180跳变为-180，这下很容易理解为什么机器人转圈了。
Odom传给mpc控制器的角度有问题，odom只是返回机器人当前的位姿信息，在角度超过180后直接记为负值，这样角度差相差360度，mpc控制器以为机器人转圈了就控制机器人转圈跟踪，实际上却并没有变化这么大。
查找到odom问题后，很简单我做了一个角度转换，当前角度与之前的角度差值过大，处理下即可。之后进行了多次测试机器人不再出现转头的问题。
