飞控算法从入门到精通 061LQR线性二次型调节器控制基础从一次炸机事故说起去年夏天我在调试一架四旋翼的悬停控制时遇到了一个诡异的问题。PID参数已经调得相当“丝滑”——阶跃响应超调不到5%稳态误差在0.1度以内。但当我给飞机加上一个200克的负载模拟挂载摄像头飞机开始出现低频振荡大约2Hz左右幅度逐渐增大最后直接翻了过去。事后分析日志发现问题出在PID的鲁棒性上。PID本质上是一个“被动”的控制器——它只根据当前误差调整输出不会主动考虑系统的动态特性和能量消耗。当系统参数发生变化比如转动惯量增加PID的增益裕度就不够了。这就是LQR登场的地方。LQR不是PID的替代品而是从另一个维度思考控制问题在满足系统动态约束的前提下寻找一个使某个性能指标最小的控制律。翻译成人话就是既要让状态误差小又要让控制量别太大还要考虑整个时间历程上的累积效果。LQR问题的数学表述先别急着看公式我们从一个实际飞控问题出发。四旋翼的滚转通道简化模型可以写成x_dot A*x B*u其中状态向量x [phi, phi_dot]^T滚转角、角速度控制输入u是期望的滚转力矩或者更底层是电机差速指令。对于我的那架飞机A矩阵和B矩阵大致是A [0, 1; 0, -0.5] // 第二行是阻尼项来自空气阻力和电机响应 B [0; 2.3] // 控制增益单位是 rad/s^2 per N·mLQR的目标是找到一个状态反馈增益K使得控制律 u -K*x 能够最小化以下代价函数J ∫(x^T * Q * x u^T * R * u) dt这里Q矩阵惩罚状态误差R矩阵惩罚控制量。Q和R的选择直接决定了控制器的“性格”——Q给得大系统响应快但控制量猛R给得大控制平缓但响应慢。权重矩阵的物理意义很多初学者在这里会犯一个错误把Q和R当成纯数学参数来调。实际上它们有明确的物理含义。Q矩阵的对角元素对应每个状态的“代价”。对于滚转通道如果Q(1,1)100意味着每1度的滚转角偏差带来的代价是100Q(2,2)10意味着每1 rad/s的角速度偏差代价是10。这两个值的比值决定了位置误差和速度误差之间的权衡。R矩阵更关键——它直接对应执行器的“成本”。R0.01意味着控制量u每变化1单位代价是0.01。这个值太小控制器会疯狂输出电机可能饱和甚至烧毁太大控制器就“舍不得”用力响应慢得像老牛拉车。我个人的经验是先根据执行器极限确定R的下界。比如我的电机最大能输出0.5 N·m的力矩那么R至少应该让控制量u在0.5以内时代价可控。然后根据期望的响应时间调整Q。求解Riccati方程LQR的核心是求解代数Riccati方程A^T * P P * A - P * B * R^(-1) * B^T * P Q 0这个方程的解P是一个对称正定矩阵然后最优增益K R^(-1) * B^T * P。实际工程中没人手算这个——MATLAB的lqr()函数或者Python的control.lqr()都能直接求解。但有个坑数值稳定性问题。当系统矩阵A的特征值接近虚轴比如飞控中的积分环节Riccati方程可能病态。我遇到过几次求解出来的K矩阵元素数量级差10^6导致控制器输出直接溢出。解决办法对状态变量做归一化。把角度从弧度换成度角速度从rad/s换成deg/s让所有状态量在同一个数量级比如0.1100之间。这样Q矩阵的对角元素可以设成110之间的值R设成0.1~1求解过程稳定得多。从LQR到实际飞控代码理论讲完上代码。这是一个四旋翼滚转通道的LQR控制器实现片段// LQR状态反馈控制函数// 输入当前滚转角phi(rad)角速度phi_dot(rad/s)// 输出期望滚转力矩(N·m)// 注意这里假设状态已经归一化到[-1,1]区间floatlqr_roll_control(floatphi,floatphi_dot){// 状态向量 [phi; phi_dot]floatx[2]{phi,phi_dot};// LQR增益矩阵 K [k1, k2]// 这里踩过坑K矩阵是从MATLAB离线算好写死的// 别在飞控里实时求解Riccati方程计算量太大staticconstfloatK[2]{2.5f,1.2f};// 对应Qdiag(10,1), R0.1// 计算控制量 u -K*xfloatu-(K[0]*x[0]K[1]*x[1]);// 限幅保护别这样写死限幅值应该根据电机能力动态调整// 这里用宏定义方便调试#defineMAX_TORQUE0.5f// 最大力矩 N·mif(uMAX_TORQUE)uMAX_TORQUE;if(u-MAX_TORQUE)u-MAX_TORQUE;returnu;}这段代码看起来简单但实际部署时有个关键问题状态估计的延迟。LQR假设状态是实时可得的但飞控中角度来自IMU的加速度计/陀螺仪融合角速度来自陀螺仪两者都有延迟。如果直接用原始传感器数据控制效果会打折扣。我的做法是在LQR前面加一个状态观测器比如卡尔曼滤波器对状态做一步预测补偿传感器延迟。具体实现后面章节会详细讲这里先记住LQR的性能上限取决于状态估计的精度和实时性。与PID的对比实验回到开头的炸机事故。我在同一架飞机上分别用PID和LQR做了对比测试PID参数P4.5, I0.8, D0.3经过Ziegler-Nichols整定LQR参数Qdiag(10,1), R0.1, K[2.5, 1.2]测试条件空载 - 加载200g - 卸载PID的表现空载时响应很好加载后出现2Hz振荡卸载后振荡持续了3秒才衰减。原因PID的积分项在负载变化后需要重新积分动态过程中增益裕度下降。LQR的表现加载瞬间有约5度的超调但0.5秒内恢复没有振荡。卸载时同样快速收敛。原因LQR直接基于状态反馈不依赖积分项对参数变化有更好的鲁棒性。但LQR也有弱点稳态误差。因为LQR本质上是比例控制状态反馈没有积分项所以当存在常值扰动比如重心偏移时会有稳态误差。解决办法在状态向量中加入误差积分项变成LQI线性二次型积分控制或者外环用PI内环用LQR。工程中的几个坑Q和R的调参不是线性的。Q增大一倍响应速度不会快一倍而是可能引起振荡。建议用MATLAB的lqr函数先仿真观察闭环极点位置再微调。执行器饱和是LQR的致命伤。当控制量达到限幅值LQR的线性假设失效系统可能失稳。必须加抗饱和anti-windup机制比如条件积分或者动态限幅。离散化问题。飞控是离散系统采样频率通常200-500Hz。直接用连续LQR增益会有误差。正确做法先离散化系统矩阵零阶保持器法再求解离散Riccati方程。MATLAB的dlqr()函数就是干这个的。状态不可测。飞控中有些状态比如角加速度无法直接测量。要么用观测器估计要么在LQR设计中忽略这些状态但会降低性能。我的建议至少保证可测状态的数量不少于控制量的数量否则LQR可能无解。个人经验总结LQR不是万能的但它提供了一个比PID更系统的设计框架。对于飞控这种多变量、强耦合的系统LQR的优势在于可以显式地权衡状态误差和控制能量增益矩阵有明确的物理意义状态反馈容易扩展到多输入多输出MIMO系统但LQR也有适用条件系统必须是线性的或者在工作点附近线性化模型参数要准确。对于四旋翼这种非线性系统LQR通常用于姿态控制的内环角速度环外环位置环还是用PID或者更复杂的非线性控制。最后给个实用建议不要试图用LQR替代PID而是让它们互补。我现在的飞控架构是内环角速度用LQR快速、鲁棒外环角度用PI消除稳态误差位置环用PID简单可靠。这样既发挥了LQR的动态性能又保留了PID的工程便利性。下一章我们会讨论LQR的进阶版本——LQI带积分项的线性二次型控制以及如何在飞控中实现状态观测器。到时候会用一个完整的四旋翼仿真案例来演示。
061、LQR（线性二次型调节器）控制基础
飞控算法从入门到精通 061LQR线性二次型调节器控制基础从一次炸机事故说起去年夏天我在调试一架四旋翼的悬停控制时遇到了一个诡异的问题。PID参数已经调得相当“丝滑”——阶跃响应超调不到5%稳态误差在0.1度以内。但当我给飞机加上一个200克的负载模拟挂载摄像头飞机开始出现低频振荡大约2Hz左右幅度逐渐增大最后直接翻了过去。事后分析日志发现问题出在PID的鲁棒性上。PID本质上是一个“被动”的控制器——它只根据当前误差调整输出不会主动考虑系统的动态特性和能量消耗。当系统参数发生变化比如转动惯量增加PID的增益裕度就不够了。这就是LQR登场的地方。LQR不是PID的替代品而是从另一个维度思考控制问题在满足系统动态约束的前提下寻找一个使某个性能指标最小的控制律。翻译成人话就是既要让状态误差小又要让控制量别太大还要考虑整个时间历程上的累积效果。LQR问题的数学表述先别急着看公式我们从一个实际飞控问题出发。四旋翼的滚转通道简化模型可以写成x_dot A*x B*u其中状态向量x [phi, phi_dot]^T滚转角、角速度控制输入u是期望的滚转力矩或者更底层是电机差速指令。对于我的那架飞机A矩阵和B矩阵大致是A [0, 1; 0, -0.5] // 第二行是阻尼项来自空气阻力和电机响应 B [0; 2.3] // 控制增益单位是 rad/s^2 per N·mLQR的目标是找到一个状态反馈增益K使得控制律 u -K*x 能够最小化以下代价函数J ∫(x^T * Q * x u^T * R * u) dt这里Q矩阵惩罚状态误差R矩阵惩罚控制量。Q和R的选择直接决定了控制器的“性格”——Q给得大系统响应快但控制量猛R给得大控制平缓但响应慢。权重矩阵的物理意义很多初学者在这里会犯一个错误把Q和R当成纯数学参数来调。实际上它们有明确的物理含义。Q矩阵的对角元素对应每个状态的“代价”。对于滚转通道如果Q(1,1)100意味着每1度的滚转角偏差带来的代价是100Q(2,2)10意味着每1 rad/s的角速度偏差代价是10。这两个值的比值决定了位置误差和速度误差之间的权衡。R矩阵更关键——它直接对应执行器的“成本”。R0.01意味着控制量u每变化1单位代价是0.01。这个值太小控制器会疯狂输出电机可能饱和甚至烧毁太大控制器就“舍不得”用力响应慢得像老牛拉车。我个人的经验是先根据执行器极限确定R的下界。比如我的电机最大能输出0.5 N·m的力矩那么R至少应该让控制量u在0.5以内时代价可控。然后根据期望的响应时间调整Q。求解Riccati方程LQR的核心是求解代数Riccati方程A^T * P P * A - P * B * R^(-1) * B^T * P Q 0这个方程的解P是一个对称正定矩阵然后最优增益K R^(-1) * B^T * P。实际工程中没人手算这个——MATLAB的lqr()函数或者Python的control.lqr()都能直接求解。但有个坑数值稳定性问题。当系统矩阵A的特征值接近虚轴比如飞控中的积分环节Riccati方程可能病态。我遇到过几次求解出来的K矩阵元素数量级差10^6导致控制器输出直接溢出。解决办法对状态变量做归一化。把角度从弧度换成度角速度从rad/s换成deg/s让所有状态量在同一个数量级比如0.1100之间。这样Q矩阵的对角元素可以设成110之间的值R设成0.1~1求解过程稳定得多。从LQR到实际飞控代码理论讲完上代码。这是一个四旋翼滚转通道的LQR控制器实现片段// LQR状态反馈控制函数// 输入当前滚转角phi(rad)角速度phi_dot(rad/s)// 输出期望滚转力矩(N·m)// 注意这里假设状态已经归一化到[-1,1]区间floatlqr_roll_control(floatphi,floatphi_dot){// 状态向量 [phi; phi_dot]floatx[2]{phi,phi_dot};// LQR增益矩阵 K [k1, k2]// 这里踩过坑K矩阵是从MATLAB离线算好写死的// 别在飞控里实时求解Riccati方程计算量太大staticconstfloatK[2]{2.5f,1.2f};// 对应Qdiag(10,1), R0.1// 计算控制量 u -K*xfloatu-(K[0]*x[0]K[1]*x[1]);// 限幅保护别这样写死限幅值应该根据电机能力动态调整// 这里用宏定义方便调试#defineMAX_TORQUE0.5f// 最大力矩 N·mif(uMAX_TORQUE)uMAX_TORQUE;if(u-MAX_TORQUE)u-MAX_TORQUE;returnu;}这段代码看起来简单但实际部署时有个关键问题状态估计的延迟。LQR假设状态是实时可得的但飞控中角度来自IMU的加速度计/陀螺仪融合角速度来自陀螺仪两者都有延迟。如果直接用原始传感器数据控制效果会打折扣。我的做法是在LQR前面加一个状态观测器比如卡尔曼滤波器对状态做一步预测补偿传感器延迟。具体实现后面章节会详细讲这里先记住LQR的性能上限取决于状态估计的精度和实时性。与PID的对比实验回到开头的炸机事故。我在同一架飞机上分别用PID和LQR做了对比测试PID参数P4.5, I0.8, D0.3经过Ziegler-Nichols整定LQR参数Qdiag(10,1), R0.1, K[2.5, 1.2]测试条件空载 - 加载200g - 卸载PID的表现空载时响应很好加载后出现2Hz振荡卸载后振荡持续了3秒才衰减。原因PID的积分项在负载变化后需要重新积分动态过程中增益裕度下降。LQR的表现加载瞬间有约5度的超调但0.5秒内恢复没有振荡。卸载时同样快速收敛。原因LQR直接基于状态反馈不依赖积分项对参数变化有更好的鲁棒性。但LQR也有弱点稳态误差。因为LQR本质上是比例控制状态反馈没有积分项所以当存在常值扰动比如重心偏移时会有稳态误差。解决办法在状态向量中加入误差积分项变成LQI线性二次型积分控制或者外环用PI内环用LQR。工程中的几个坑Q和R的调参不是线性的。Q增大一倍响应速度不会快一倍而是可能引起振荡。建议用MATLAB的lqr函数先仿真观察闭环极点位置再微调。执行器饱和是LQR的致命伤。当控制量达到限幅值LQR的线性假设失效系统可能失稳。必须加抗饱和anti-windup机制比如条件积分或者动态限幅。离散化问题。飞控是离散系统采样频率通常200-500Hz。直接用连续LQR增益会有误差。正确做法先离散化系统矩阵零阶保持器法再求解离散Riccati方程。MATLAB的dlqr()函数就是干这个的。状态不可测。飞控中有些状态比如角加速度无法直接测量。要么用观测器估计要么在LQR设计中忽略这些状态但会降低性能。我的建议至少保证可测状态的数量不少于控制量的数量否则LQR可能无解。个人经验总结LQR不是万能的但它提供了一个比PID更系统的设计框架。对于飞控这种多变量、强耦合的系统LQR的优势在于可以显式地权衡状态误差和控制能量增益矩阵有明确的物理意义状态反馈容易扩展到多输入多输出MIMO系统但LQR也有适用条件系统必须是线性的或者在工作点附近线性化模型参数要准确。对于四旋翼这种非线性系统LQR通常用于姿态控制的内环角速度环外环位置环还是用PID或者更复杂的非线性控制。最后给个实用建议不要试图用LQR替代PID而是让它们互补。我现在的飞控架构是内环角速度用LQR快速、鲁棒外环角度用PI消除稳态误差位置环用PID简单可靠。这样既发挥了LQR的动态性能又保留了PID的工程便利性。下一章我们会讨论LQR的进阶版本——LQI带积分项的线性二次型控制以及如何在飞控中实现状态观测器。到时候会用一个完整的四旋翼仿真案例来演示。
