多机编队——（6）解决机器人跟踪过程中mpc控制转圈问题

---

问题描述

机器人再跟踪轨迹过程中，有时会突然出现原地转圈跟踪的现象，如视频所示：

---

一、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问题后，很简单我做了一个角度转换，当前角度与之前的角度差值过大，处理下即可。之后进行了多次测试机器人不再出现转头的问题。

---