LQR算法核心思想

本章以倒立摆为解决目的

什么是线性二次型控制器（LQR）

开环系统

即状态变量的倒数 = 系统的状态空间矩阵A * 系统状态变量x

A状态矩阵：描述系统本身物理特性的一个矩阵，它是由系统本身的机械结构、物理结构决定的，无法改变。

系统状态变量x：用四个变量描述了倒立摆整个系统的运动状态

闭环系统：

B状态矩阵也是由系统本身的物理特性决定的。

u就是反馈项，u = - kx

由于这个式子

整个闭环算法中最重要的就是求出最佳的K，来使得系统稳定。

如何求最优K是核心问题，LQR实际上提供了求解最优K的方法

通过代价函数来求K

可以看到里面还有Q和R，这两个与系统的收敛有关。

Q是系统状态变量的权重，也叫权重矩阵

根据这个来看，比如Q11就是倒立摆角度的权重，Q22就是角速度的权重，Q33就是飞轮角度的权重、Q44就是飞轮角速度的权重。

如果希望道理摆角度收敛的更快（更快稳定），可以给Q11的权重增加

R的话，可以理解为决定系统输出量大小的项。一般而言R都设为1,不去改变它。

结合这两个式子

R越大，u的输出越小；R越小，u的输出越大。

一句话，要想求出K，就需要A矩阵、B矩阵、Q矩阵、R矩阵（一般为1）

其实只需要三个

A矩阵和B矩阵得通过建模得到！！！

物理建模 --- 建立动力学模型（为了A矩阵和B矩阵）

这个是动量轮动力摆系统，可以用牛顿第二定律来分析力，但是我们用更常用的求法

利用拉格朗日方程：

因为拉格朗日方程不需要列出系统具体的力是怎样平衡的

它只需要列出系统的动能减去势能得到算子，然后争对这个算子对系统不同的广义坐标求偏导，最后得到系统的广义力

通过图可分析：

这个θ是摆杆的摆角

这个φ是动量轮自身的转动角度

有两个广义坐标就可以列出两个拉格朗日方程：

这个是θ的导数，偏导。同理φ。

我们开始详细讲

MATLAB

求A、B矩阵

可以看到Q阵把第三项的权重设小

最后求出K，

由于是通过电压来控制电机的力矩，电压总不可能去到300多，所以应该适当缩放

从结果也能看出第三项小到可以忽略不计。 

 

看最核心的代码：