现代控制理论（2）—— LQR

Linear Quadratic Regulator（线性二次型调节器）

Linear：系统需要能够写成 \(\dot{x} = Ax + Bu\) ，云台在转动的时候虽然有sin但是在小范围的时候可以视为线性的。

Quadratic：这是一个能量的概念，在数学上使用误差的平方，电流的平方量化指标。（二次可微，连续，凸）

量化代价函数

代价函数：\(J = \int^{\infty}_{0} (x^{T} Q x + u^{T} R u) dt\)

其中 \(x^TQx\) 为当前状态向量距离平衡点的欧氏距离的平方。

其中的 \(Q\) 矩阵（通常是一个对角矩阵）：

\[Q = \begin{bmatrix} q_1 & 0 \\ 0 & q_2 \end{bmatrix} \]

\(x^T Q x = q_1 x_1^2 + q_2 x_2^2\)。(如果 \(q1\) 比较大, \(q2\) 比较小就会导致 \(x_1\) 迅速增大 )

控制项：\(u^T R u\)

其中 \(u\) 为控制输入，\(R\) 为控制约束。

\(R\) 越大越吝啬能量，\(R\) 很小的时候不惜代价加压回正。

为什么需要二次型？

1. 凸函数有全局最优解，LQR 算出来的反馈矩阵 \(K\) 为唯一最优的。

2. 可导：平方项求导之后为线性，使得最终的控制律 \(u = -Kx\) 也是线性的。

Yaw-Pitch 两轴云台建模

1. 我们需要描述状态变量有两个轴的角度 (\(\theta\)) 和角速度 \((\dot{\theta})\)

   \[x = \begin{bmatrix}\theta_{y} & \dot{\theta_{y}} & \theta_p & \dot{\theta_p} \end{bmatrix} \]

2. 建模过程中我们需要辨识：每个轴的转动惯量 \(J\) 和等效阻尼系数 \(b\)。

   系统辨识法：

   ​ 电机转动动力学方程：\(J \cdot \dot{\omega} + b \cdot \omega = K_t \cdot i\)

   1. 固定云台： 确保一个轴动的时候，另一个轴锁定（或不受干扰）。

   2. 施加阶跃： 在 \(t=0\) 时刻，给电机输出一个恒定的 PWM 值（比如 \(20\%\) 占空比），模拟一个恒定的电压/力矩输入。

   3. 高速采样： 以尽可能高的频率（如 \(1\text{ms}\) 一次）通过串口（UART）发送当前时间 \(t\) 和角速度 \(\omega\)。

   4. 停止： 等速度稳定后停止。

   5. 最小二乘法拟合

      由于存在噪声，可以利用 Python 的 scipy.optimize.curve_fit 拟合上述指数方程，得到更准的 \(\tau\) 和 \(\Omega_{ss}\)。

   注意其中的pitch轴需要做重力补偿，慢慢增加电流直到 \(\tau_g\)，此时pitch轴刚好能平衡在水平位置不掉下来，测 \(J\) 和 \(b\) 的时的电流需要减去 \(\tau_g\) 。