之前的轨迹跟踪的方法有些问题，最近换了LQR，具体的实现太细的理论也有人写过了，去看一下LQR、欧拉法、黎卡提相关的知识就好，区别是修改了一下模型，网上的大部分都是阿克曼模型的，我们是双轮差速的，然后重写了控制的类，主要调用部分自己的代码不方便给出来还用的网上的。
公式推导的话可以看一下这张图
以下是仿真视频,绿色线是给定轨迹，红色点是控制机器人走的位姿
lqr轨迹跟踪模拟
以下是实际上车跑的视频
实车跑轨迹跟踪
最后给一下代码仓库
https://github.com/jiujiaowoxiaoyangba/lqr_track
最后如果嫌弃走起来在拐角处线速度有停顿可以把
U LQRController::LimitSpeed(U control_value) 删了，这块是怕实际车反馈、控制达不到加的