面向阿克曼移动机器人（自行车模型）的LQR（线性二次型调节器）路径跟踪方法

        线性二次调节器（Linear Quadratic Regulator，LQR）是针对线性系统的最优控制方法。LQR 方法标准的求解体系是在考虑到损耗尽可能小的情况下, 以尽量小的代价平衡其他状态分量。一般情况下，线性系统在LQR 控制方法中用状态空间方程描述，性能能指标函数由二次型函数描述。

LQR 方法存在以下优点：

1. 最小能量消耗和最高路径跟踪精度。
2. 求解时能够考虑多状态情况。
3. 鲁棒性较强。

缺点：

1. 控制效果和模型精确程度有很大相关性。
2. 实时计算状态反馈矩阵和控制增益。

一、系统模型

1.1 车辆模型

        一般来说阿克曼移动机器人可以简化为自行车模型，是一个非线性时变系统，工程上一般通过在平衡点附近差分线性化转化为线性系统来分析和控制，具体就不推导了，我直接给出模型。

1.2 线性系统状态反馈控制示意图

状态反馈是线性能控线性系统镇定的一个有效方法，主要是通过极点配置方法寻找一组非正的闭环极点使得闭环系统大范围渐进稳定。

 A，B，C分别代表系统矩阵、输入矩阵和输出矩阵，K是待设计的状态反馈增益。

二、控制器设计

2.1 代价函数泛函设计

最优控制里，代价函数一般设计为控制性能和控制代价的范数加权和，形式如下

其中，期望和实际的误差系统定义为

2.2 最优状态反馈控制律推导

当想要状态与期望状态之间的误差越差越小，同时控制消耗更少的能量。求解极小值点时，新定义拉格朗日函数如下 

在拉格朗日函数基础上对各个优化变量的一阶导为零 ，得

 当时候，

推导LQR控制律时候，设 ,求偏导后可得