机器人控制算法——移动机器人横向控制最优控制LQR算法

1.Introduction
LQR (外文名linear quadratic regulator)即线性二次型调节器，LQR可得到状态线性反馈的最优控制规律，易于构成闭环最优控制。LQR最优控制利用廉价成本可以使原系统达到较好的性能指标(事实也可以对不稳定的系统进行整定) ，而且方法简单便于实现 ，同时利用 Matlab 强大的功能体系容易对系统实现仿真。
之前已经在自动驾驶汽车上工程落地，参见自动驾驶——最优控制算法(LQR）工程化总结。
与上次不同的是，此次被控对象是移动机器人，两轮差速模型，差速模型原理参见另一篇机器人控制算法——两轮差速驱动运动模型
2. Implment LQR Algorithm for Differential Drive Motion

车辆运动学模型是根据车辆的几何关系建立的，不考虑影响车辆运动的力。以车辆后轴中点作为研究对象，在惯性坐标系OXY下，车辆运动状态可用车辆坐标(x,y)和车身航向角。v为车辆纵向行驶速度，如下图所示。

则车辆运动学模型为：