参考

Optimization Based Control 从基础到复杂地介绍最优控制理论

麻省理工大学机器人算法第八章 LQR 大概说了 lqr 的推导过程，主页有更多算法介绍

wiki LQR 控制器 LQR 多种公式说明

Formulas for discrete time LQR, LQG, LEQG and minimax LQG optimal control problems 离散 LQR 问题论文

黎卡提方程解释

LQR 理论

LQR 实现

公式推导部分理解有限，这里不折腾了，直接进入应用环节。

LQR 二次线性调节器 Linear Quadratic Regulators

wiki 中将 LQR 控制器根据应用分类出四种公式：

• Finite-horizon, continuous-time，有限视野，连续时域

• Infinite-horizon, continuous-time，无限视野，连续时域

• Finite-horizon, discrete-time，有限视野，离散时域

• Infinite-horizon, discrete-time，无限视野，离散时域

在这里，有限视野的公式中会考虑控制的终点状态，而无限视野的公式不考虑控制的终点状态，可以说在无限视野中，LQR 调节是永不停息的。

在实际应用控制中，都是以固定的频率控制系统，所以这里只考虑离散时域的控制公式。另外，在无限视野中，随着控制调节次数增加（时间流逝），系统会逐渐稳定。系统稳定后虽然不再需要控制调节，但是算法仍会继续工作。下面以无限视野，离散时域的公式作说明。

LQR 用线性方程描述系统，用代价函数表示控制的优劣，其中有些代价项的计算可能得到负值，所以会对代价项进行平方操作以保证代价值为正，且存在最小值。方程描述如下：

公式 (1) 中， 是下个时刻的系统状态，一般来说系统状态描述为误差值。例如猎豹捕食追赶羚羊，猎豹和羚羊之间的位置误差，也就是欧氏距离作为系统状态，该系统运行到稳定就是猎豹追赶到羚羊，它们间位置误差为 0。

公式 (2) 是代价评估， 表示状态代价, 表示控制代价， 表示？？？代价。Q 是半正定加权矩阵，R 是正定加权矩阵，N 是 ？？？，需要调参设置。

公式 (3) 是使得系统代价 J 最小的控制序列定义。其中控制增益 K 由公式 (4) 给出。

P 根据黎卡提方程求解：

注意黎卡提方程求解是一个逆向求解，通常令终点，然后迭代 n 次后得到  值。

翻了一些资料没有找到代价公式中 的解释，该公式来自论文《Formulas for discrete time LQR, LQG, LEQG and minimax LQG optimal control problems》，但论文也只是直接给出公式：

从公式上可以理解是“控制转换到状态的代价？”。对比其他文章资料，此时将 N 设置为零矩阵即可。

高度跟踪

先测试简单的一维高度跟踪例子。

一个飘浮器 P 每个时刻会在 [-0.25，0.25] 米范围内飘动，一个跟踪器 G 通过的控制跟踪飘浮器。系统状态描述为跟踪器与飘浮器的高度差：

其中 ，。

仿真步骤：

1. 随机初始化飘浮器 P 和跟踪器 G 的高度；

2. while (1)：

3. 更新飘浮器 P 高度；

4. 逆向迭代求解矩阵 P，然后计算增益 K，最后计算得到控制 u；

5. 更新跟踪器 G 高度；

6. 计算控制消耗；

测试，绿线表示飘浮器 P 高度，红线表示跟踪器 G 高度。

当权重矩阵 Q=1，R=1 时候，跟踪效果如下：

当权重矩阵 Q=60，R=1 时候，跟踪效果如下：

当权重矩阵 Q=130，R=1 时候，跟踪效果如下：

当权重矩阵 Q=130，R=30 时候，跟踪效果如下：

从上面测试可以看出 Q 矩阵越大，说明系统状态越重要，LQR 控制会尽量输出大控制来收敛系统误差。而 R 矩阵越大，说明控制幅度不能太大，LQR 控制会尽量降低控制幅度。

尽管公式 (3) 计算控制增益 K，但是在该公式中仅仅包含系统代价中状态代价和控制代价的权重，而不关心具体的代价形式，最终的系统稳态调节效果依赖权重矩阵 Q 和 R 的调参。