传统模型预测控制轨迹跟踪——波浪形轨迹（功能包已经更新）

时隔一个多月，终于有时间把这个功能包框架给升级一下了。

前文传统模型预测控制轨迹跟踪——圆形轨迹中的功能包更新如下：

mpc_simulation_ROS_and_experiment，使用方式见 README.md。

更新的功能

1.路径发送的时候顺便把规划的速度和角速度也发送出去了

2.在mpc_client.cpp中将速度、角速度传给mpc_server.py中去作为期望值参与优化计算

3.更新了一条波浪形路径来验证跟踪算法

4.更新了实车中实验的程序以及操作手册（见path_track/readme.txt）

需要说明的内容

1.仅考虑了小车的x方向速度和角速度，当然y方向的速度已经在path_track/path_point.srv预留了接口，大家可以加入y方向速度试一下，似乎还有一些问题。

2.由于预测模型的设计中，状态量为位姿误差，控制量为速度增量，所以对于给出的路径一定是需要路径点的速度信息，这个路径设计可以在send_point.launch中设置，也可以直接改cpp文件。

3.仿真或者实车中跑mpc时，小车位姿距离路径起始点别超过1m，否则仿真效果会变差，这可能与控制器中的一些参数有关，如预测时域等。

4.不能跑整个圆形了，可能与姿态角误差有关系，从前半圆过渡到后半圆的时候角度误差没有给出合适的计算方式，还没有去处理。

仿真结果

波浪形曲线，红色为小车轨迹，绿色为规划轨迹。

最后一点的尾巴没合上是因为优化器里面设置了三级减速，如有需要可以把减速判断程序注释掉。

具体跟踪情况如下：

mpc_track_wave_trajectory

 实验结果

跟踪直线轨迹

具体跟踪情况如下：

mpc_track_line_experiment