LQR记录

LQR最优设计是指设计出的状态反馈控制器K要使二次型目标函数J取最小值，而K由权矩阵Q与R唯一决定，故此Q,R的选择 尤为重要。

Apollo 使用 LQR 进行横向控制。横向控制包含四个组件：

横向误差   

横向误差的变化率  

朝向误差   

朝向误差的变化率   

上面四个组件的集合为X，这个集合X捕获车辆的状态。除了状态之外，该车有三个控制输入：转向，加速 和制动。这三个控制为集合U

 

离散线性系统

                                     (1)

LQR的目标就是找到一组控制量 使足够小，即系统达到稳定状态；

足够小，即花费较小的控制代价。

为了达到上述效果，定义代价函数：

                      (2)

其中x为状态量，u为控制量，Q为状态权重矩阵，R为控制权重矩阵，为最终状态权重矩阵，N为到达最终状态的控制序列数。

根据代价函数，再定义表示为从t时刻的状态z开始，通过最优控制序列，获得的到结束后的最小代价。

                          (3)

根据公式3可得，当t=N时有：

                                                                    （4）

即只和最终状态有关。可以想到如果t时刻获取最优控制，上一时刻t-1也应该是最优控制，这样倒推上一状态的控制来获取控制序列。

顺着这个思路，上一时刻与当前时刻的关系有：

                           (5)

其中公式6是当前时刻的代价，公式7是当前时刻到下一时刻到最终时刻的代价。

                  （6）

                    （7）

至此，求解最优化控制序列转化为动态规划问题。

因为当前状态z与优化命题无关，公式5可以改写为：

（8）

根据公式4形式，假设t时刻的代价有如下表达形式：

                      （9）

把改写为与t时刻同样的形式，代入公式8后，有：

（10）

这样对于无约束凸优化问题，只需令其导数等于0即可求得最优解。对公式10中求最小控制量的矩阵求导得：

                                         （11）

由公式11求最优控制有：

                               （12）

将该控制量带入公式8，有：

                      （13）

          （14）

                                                                        (15)

公式15中为：

        （16）

当t=N时，有,而

                                                    (17)

在公式15中P的推导表达式可以得到，任意时刻t有表达式

                                                (18)

自此，构造完满足凸优化条件的求解方程。

同时，当t相对时域N很小时，P趋于稳定不变，有：

          (19)

公式19被称为代数黎卡提方程，结合公式12，有：

                            (20)

其中，                                       (21)

原控制系统框图可以表示为：

其中K即为通过迭代求解黎卡提方程得到的等效闭环反馈矩阵。

至此，LQR算法求解过程可以总结为：

1.  令P等于最终状态权重矩阵；

2.迭代黎卡提方程求出新的P；

3.  当两次P的差值足够小时，计算反馈矩阵K；

4.   根据反馈阵K获取最优控制量u；

采用《Apollo控制算法之汽车动力学模型》一文中的模型，代入AD、BD、到以上算法对应各项，因模型中的CD项为常量，在LQR求解完成后加入即可获得最终所需的系统控制量。