1. LQR和LQG到底在解决什么问题1.1 从“比例控制不够用”说起LQR到底在做什么在我接触到的不少入门者眼里LQR线性二次型调节器被包装得神乎其神好像只要把矩阵一摆无人机就能稳稳悬停、机械臂就能精准走轨迹。但实际上LQR解决的是这样一个问题对于线性系统如何设计一个状态反馈控制器让系统状态尽快收敛到零同时又不过度消耗控制能量。它的核心是那个二次型性能指标J ∫(x^T Q x u^T R u) dt一句话解释就是整个运行过程中状态偏差的累计“惩罚”和控制器输出的累计“代价”要一起最小化。你在Q和R矩阵里填的每个数字本质上是告诉控制器“哪些偏差不能忍”“哪些执行器动作别太猛”。我常说LQR是“把工程直觉量化”的算法。你用PID时调Kp、Ki、Kd是在猜“这个偏差该掰回多少”而用LQR时你是在系统性地定义“偏差多大会让我难受控制量多大会让我心疼”剩下的交给Riccati方程去解。区别不是算法本身多高级而是你把经验沉淀成了目标函数这在实际项目里意味着可移植性更强、调试边界更清晰。1.2 LQG当你的状态“看不全”也“测不准”时现实中几乎没有哪个项目能直接拿到全部状态量的完美测量值。无人机上你可能只有IMU的加速度和角速度、光流或GPS的位置但速度、姿态角、角加速度这些往往需要估计机械臂上你大概率只有电机侧编码器的角度而连杆侧的真实角度、末端速度、关节摩擦力矩基本都靠猜。这时候LQR直接用的“全状态反馈”就成了空谈。LQG线性二次型高斯控制给出的答案是先用卡尔曼滤波做状态估计再把估计值当作真值送给LQR控制器。这里面最优雅的是一个叫“分离原理”的结论——在满足线性、高斯噪声、二次代价这几个前提时最优控制器可以拆成“最优状态估计器”和“最优状态反馈”两部分独立设计两者组合起来依然是全局最优。但请注意分离原理在理论上很漂亮真机上却很“脆”。模型误差、非高斯噪声、参数不确定性都会破坏它的前提。我见过太多团队把LQG当“万能药”结果卡尔曼滤波估计出来的状态本身就滞后LQR还死命往参考点赶整个系统就进入振荡失稳的死循环。所以理解LQG的关键在于它不是一个“加强版LQR”而是一个“在信息受限条件下如何妥协”的框架妥协得好不好全看你对自己系统的噪声特性了解多深。1.3 为什么无人机和机械臂是这两兄弟的最佳试验场无人机和机械臂作为控制算法的落地点有几个共同的显著特征第一它们的动力学模型相对清晰牛顿-欧拉或拉格朗日方程都能导出比较准确的微分方程这为线性化和模型辨识提供了基础第二它们都是欠驱动或全驱动系统的典型代表无人机是典型的欠驱动系统4个输入6个自由度机械臂则是全驱动但强耦合、强非线性的系统第三它们对实时性和鲁棒性的要求很极致飞控和机械臂控制器的控制周期通常在1kHz以上算法必须简单、可预测、算得完。正是这些特征让LQR/LQG这种“在线性框架内最优”的方法既有发挥空间又容易暴露出所有基于线性化设计的通病。如果你已经在这两个领域摸爬滚打一阵子了下面这些实战细节大概正是你需要的。2. 无人机项目里的LQR落地建模、调参与避坑2.1 从牛顿-欧拉到状态空间悬停点附近的线性化用LQR做无人机控制第一步就是把非线性的刚体动力学方程在某个工作点线性化。绝大多数人选择在悬停点附近展开速度为零、姿态角为零、油门处于悬停油门附近。此时可以把无人机模型简化为两个解耦的子系统水平通道x-y和垂直通道z以及三个姿态角通道。以四旋翼为例常用的小角度线性化模型是x g·θy -g·φz (T - mg)/mθ τ_θ / I_xxφ τ_φ / I_yyψ τ_ψ / I_zz把状态向量取为 [x, x, y, y, z, z, φ, φ, θ, θ, ψ, ψ]输入为 [T - mg, τ_φ, τ_θ, τ_ψ]这就得到了一个标准的12维状态空间方程。不过我得提醒这个模型里隐含了几个容易忽略的假设小角度假设欧拉角接近零、陀螺力矩忽略、旋翼空气动力简化为线性。我在实际项目里习惯把模型再降维。因为水平位置通道本质上是通过倾斜姿态产生水平加速度所以更常见的做法是设计内外环内环用LQR控制姿态角速率外环用串级PID或另一个LQR控制位置。姿态环的线性化模型在悬停点附近足够准确位置环的动态相对更慢这样分层设计也能降低整定难度。2.2 Q和R矩阵怎么选我在调参过程中总结的三步法关于LQR的Q和R整定网上能搜到的说法大多是“凭经验调”这句话没错但对工程新人几乎等于没说。我自己的方法是先定优先级、再算数量级、最后微调迭代。第一步明确这个通道最重要的指标是什么。对无人机姿态环来说通常姿态角偏差比角速度偏差更“贵”所以Q矩阵中角度对应的对角元素比角速度对应的元素大一到两个数量级。对位置环位置误差比速度误差更需要注意但也要防止位置权重过大导致系统阻尼变差。比如我做某个测绘无人机时水平位置误差容忍度是10cm那在位置环的Q里x和y的权重就取得比较狠。第二步算数量级而不是瞎填。把R取成单位阵I然后观察单位阶跃响应里的控制量幅度再反过来调整Q。很多教材推荐Bryson方法Q对角线取1/期望最大状态偏差^2R对角线取1/期望最大控制量^2。这个方法简单实用。比如姿态角期望偏差不超过0.1rad那对应Q元素约为100最大控制力矩是0.5Nm那R对应元素约为4。第三步是迭代微调。仿真里把Q放大导致控制变猛、超调增加通常说明控制权重相对偏低系统收敛太慢则说明对应状态权重偏低。在实际飞控里我会把Q和R初始值代入真机测试记录姿态响应和控制量曲线观察有没有振荡和饱和抖动再做小幅调整一次只调一组参数不要全局乱动。2.3 无人机LQR的典型坑执行器饱和、时延与风扰第一个大坑是执行器饱和。LQR在推导时假设控制量是无限幅的但实际电机的转速、力矩都有上限。当任务要求大幅度机动时LQR设计出来的控制指令可能远超电机能输出的范围饱和之后系统表现为响应迟钝、甚至振荡。我的处理方式是在仿真阶段就把控制量限幅器加进去再看闭环响应是否还能接受同时在Q/R整定时把R适当增大别让控制律一上来就“猛打”。第二个坑是延时。飞控系统里从传感器采样到PWM输出的链路存在不可忽视的时间延迟。我实测过一些开源飞控IMU数据到姿态通道的延迟在2到8毫秒PWM输出到电机响应又有几十毫秒。LQR的相位裕度在这种延迟下会被吃掉很多系统表现为高频抖动。不少项目直接忽略建模延迟导致仿真很好、真机就炸。解决思路有两个一是在模型里加入一阶惯性延迟环节再设计控制器二是用Smith预估器补偿已知延迟。第三个坑是风扰。无人机在室外飞行时风对系统的扰动往往不是线性小扰动而是持续的、带梯度的干扰。普通LQR没有积分作用所以对常值风的响应会存在稳态误差。我常用的补救手段是给位置外环增加积分项或者在LQR基础上再串一个小的积分补偿器——本质上就是LQI线性二次型积分控制把误差积分扩展成新的状态强迫系统消除稳态偏差。3. 机械臂项目里的LQG实践估计与控制协同设计3.1 机械臂的LQG架构为什么要先分清“电机侧”和“连杆侧”机械臂和无人机最大的不同在于关节驱动链更长常见的谐波减速器、行星减速器带来了弹性变形和背隙所以电机端的编码器读到的是电机转角而连杆端的实际转角总有一个偏差。这个偏差在高速运动和高负载时特别明显也是很多“机械臂偏差”抱怨的根本原因。控制器的状态量必须建立在“连杆侧”而不是“电机侧”否则位置精度根本做不上去。用LQG来解决这个问题有两种使用方式。其一将连杆侧的关节角、角速度作为状态电机侧编码器作为量测用卡尔曼滤波估计出连杆侧状态后再送给LQR。其二将关节柔性也纳入模型建立双惯量系统模型状态空间阶数翻倍用LQG同时估计电机侧、连杆侧的位置和速度。第一种方式简单够用适合做定位精度要求不高的场合第二种方式更彻底适合做高速高精度轨迹跟踪但模型辨识工作量会大很多。我在一个6轴机械臂的视觉抓取项目里用的是第二种方式。谐波减速器的刚度参数从厂商给的资料里只能拿到标称值真正工作时的柔性与负载有关所以我花了大量时间做频率响应测试来辨识刚度再把辨识结果代入模型。这个环节别偷懒模型不准卡尔曼滤波和LQR的设计再漂亮都是空中楼阁。3.2 卡尔曼滤波调参Q_noise和R_noise的工程直觉LQG的另一个大头是卡尔曼滤波的噪声协方差矩阵设计。教科书里Q_w是过程噪声方差R_v是测量噪声方差但在工程上可观测的实验数据是编码器读数、电流指令、加速度计信号要把这些转化为合理的噪声方差必须对系统有感觉。我的经验是R_v直接按传感器数据手册和实测标准差来填。以17bit多圈绝对值编码器为例分辨率约0.0027度实测静态重复精度在0.01度左右那R_v的初值就可以填(0.01*π/180)^2量级大约是3e-8。Q_w则反推先观察卡尔曼滤波对阶跃信号和斜坡信号的跟随速度如果估计值滞后明显说明Q_w相对R_v取得太小如果估计值抖动剧烈则说明Q_w偏大。我通常把Q_w初始设为预测模型的单位噪声强度再通过对比估算速度与实际速度的残差来调整。还有一个很容易踩的坑卡尔曼滤波对噪声分布有高斯假设但机械臂运动中的摩擦扰动静摩擦、Stribeck效应和柔性振动常常是非高斯的。结果就是滤波在低速换向时会出现明显的估计尖峰。处理办法是别追求“纯卡尔曼”在低速段适当降低过程噪声值以增加滤波平滑度或者在估计残差增大时动态调低权重也就是工程里常说的自适应噪声调节。3.3 机械臂LQG的典型坑重力补偿、摩擦和前馈很多人在机械臂上把LQR的参考点设置为期望关节角误以为LQR是直接把这些关节角“锁死”在目标位置。但LQR本质上是调节器它只能处理围绕原点的偏差调节。如果目标位置距离原点很远或者期望轨迹一直在变化纯LQR控制会出现大的跟踪误差。更关键的是机械臂在非零姿态下始终受重力矩影响LQR在没有对重力进行前馈补偿时会给控制器强加一个负担导致静态误差明显。我目前的推荐架构是重力前馈计算期望关节角对应的重力矩 前馈速度/加速度项 基于LQR的反馈项。前馈消除了大部分非线性LQR只需要去“兜底”剩余的偏差和扰动。这样一个组合在带负载的UR类型机械臂上做轨迹跟踪精度能比纯LQR提升至少一个数量级。摩擦也是一个绕不开的坎。LQR模型里没有摩擦项而实际机械臂关节的库仑摩擦可能占电机额定力矩的2%到5%。低速时摩擦会让关节“粘住”表现为跟踪死区和极限环振荡。我在模型里增加一个简单的库仑摩擦补偿项τ_f f_c * sign(q_dot)补偿量由离线测试获取。这个补偿放在前馈通道里不会影响LQR的稳定性但能显著改善低速性能。4. 实用工具箱从模型辨识到仿真验证的完整链路4.1 模型线性化和辨识M系列工具与频域测试对无人机我习惯直接用PX4和ArduPilot的仿真模型做线性化它们提供了标准quadrotor模型用MATLAB或者python-control里的linearize工具能快速得到悬停点附近的A、B矩阵。这些模型参数和真机差距不会特别大但别忘了把电机响应时间常数加入进去我之前就吃过亏仿真模型没考虑电机延迟真机LQR参数直接照着上机结果天差地别。对机械臂推荐用系统辨识的方法。力位模式下的扫频测试最常用给关节叠加一个Chirp正弦速度指令记录电流和实际角速度通过傅里叶变换得到频响再从频响里拟合惯量、阻尼和刚度。我用这类方法在Dynamixel和合作厂商的谐波关节上都做过辨识效果还行。开源方面可以考虑用MATLAB System Identification Toolbox或者Python的sippy库做子空间辨识。4.2 仿真环境推荐别省这一步无论无人机还是机械臂我都强烈建议先做数字仿真再上真机。无人机这块的标配是Gazebo配合PX4的sitl仿真可以比较真实地模拟风场和传感器噪声LQR控制器写成ROS节点后直接和PX4的mavros接口对接。我在仿真里先跑出姿态环和位置环的Q/R初值再迁移到真机上时只需要小幅微调大幅缩短了调参时间。机械臂更推荐用MuJoCo或PyBullet做控制算法验证。如果你用的是ROSUR5、Panda这类机械臂都有现成的URDF和Gazebo模型配合MoveIt能快速搭建一个带视觉反馈的仿真环境。之前有段时间我在研究基于Webots的多机械臂智能分拣系统LQG的估计和控制在仿真里改动Python脚本就能重跑比在真机上磨半天要高效得多。4.3 开源参考实现站在前人的肩膀上LQR和卡尔曼滤波的教科书实现很多但真正能“抄作业”的工程示例反而不容易找。无人机方面我在PX4官方仓库里找到过LQR姿态控制的旧版本代码虽然现在被INDI等替代了但作为学习参考还是挺直观的。GitHub上有一些独立飞控项目比如基于STM32的开源飞控实现了姿态LQR代码量不大适合移植学习。机械臂方面建议先看ROS上的ros_control框架理解Controller接口再搜索cartesian trajectory controller之类带有前馈的控制器实现。Python的控制库python-control也提供了完善的lqr函数适合快速验证和批量扫参。如果要做LQGpython-control里有kalman和lqe函数搭配scipy.integrate做闭环仿真能在一个小时内把整个LQG闭环跑通。5. 常见问题与排查技巧实录5.1 无人机LQR高频问题速查我把这些年现场调试遇到的问题汇总成了一张速查表按“现象-原因-解法”整理方便排查。现象常见原因排查方法与解法悬停频繁振荡姿态角高频抖动LQR增益过大或延时补偿不足降低Q或增大R记录PWM实际输出看是否饱和加入延时模型后重新整定大风下位置漂移严重LQR无积分作用常值风扰有静差在外环位置通道增加积分项或改用LQI姿态响应慢恢复不到水平Q过小或R过大增大姿态角对应Q保持角速度Q不变逐次微调从仿真参数直接上真机效果差异大真机模型误差、电气延迟、振动环境真机上至少预留30%的稳定裕度重新做频响辨识低电量油门变化时姿态不稳悬停油门变化导致模型工作点偏移做油门自适应补偿或考虑增益调度5.2 机械臂LQG高频问题速查机械臂上的问题往往更琐碎但也更有代表性。现象常见原因排查方法与解法低速运动时关节出现抖动或爬行库仑摩擦未被补偿LQG估计残差尖峰加入摩擦前馈检查卡尔曼滤波的R_v是否过小停止在目标位置后缓慢漂移重力前馈不准确或柔性关节回弹重新辨识重心和负载质量检查谐波减速器回差高速轨迹末端出现明显超调轨迹规划加速度前馈缺失增加速度/加速度前馈LQR反馈只负责残余误差卡尔曼滤波估计速度噪声大过程噪声Q_w设置过大降低Q_w或采用自适应噪声调节手眼标定后抓取偏差依旧控制模型和视觉坐标未对齐先单独验证运动学精度再做手眼标定避免误差叠加5.3 我的排查方法论总结如果你想少走点弯路我总结的排查步骤是先开环测再闭环调。开环测试解决模型和硬件的问题闭环调试专注控制器和估计器的参数校准。遇到任何“抖动”“漂移”“发散”的诡异现象先记录控制周期内每个环节的时间戳和数据曲线把问题定位在传感器、估计器、控制器还是执行器再去动参数。别一上来就调Q和R那只会让问题越调越浑。另外分享一个实用技巧在机械臂的LQG调试过程中把卡尔曼滤波的估计状态和电机编码器实测值同时画在时间轴上比较。如果两者出现持续的偏差趋势说明模型参数或传感器标定有问题如果估计值比实测值平滑很多但滞后明显则是过程噪声和测量噪声的比值不合理。这个图几乎能秒杀一半的机械臂控制疑难杂症。6. LQR/LQG之外什么时候该放弃这两兄弟6.1 认清LQR/LQG的适用边界我不劝所有人都在项目里硬用LQR/LQG。如果你的系统工作范围很大、经常跑出线性化区域或者环境扰动剧烈且模型很不准那LQR/LQG很可能不是最佳选择。一个典型的例子是特技飞行无人机需要频繁做倒飞、翻滚等大姿态机动这时候基于悬停点线性化的LQR几乎帮不上忙非线性控制如反步法、滑模控制、MPC反而更合适。机械臂上也有类似情况。如果你只做点到点定位对轨迹过程轨迹形状不敏感那即使是简单的PID加重力前馈也能应付LQG的优势在于处理复杂轨迹跟踪和带有噪声的传感场景。如果非要LQR/LQG不可也要考虑它和模型预测控制MPC的边界MPC能显式处理约束和非线性但计算量更大LQR/LQG胜在算得快、在线运算稳定、实现成本低。6.2 增益调度与非线性扩展让线性方法“再跑一会儿”既然LQR只能基于固定工作点设计那遇到大范围工况时最简单的扩展就是增益调度Gain Scheduling 。具体做法是把工作空间离散成若干个区域在每个区域分别做线性化和LQR设计存下一组K矩阵运行时根据当前状态插值选取。我在六轴机械臂的多姿态抓取任务里用过类似思路不同末端姿态对应不同重力矩和惯量把关节空间划分成多个区域分别设计LQR整体跟踪精度比单点设计有明显改善。但增益调度也不是免费的午餐。它引入了额外的切换逻辑如果切换不及时甚至会导致系统不稳定。我的建议是先尽量把一个工作点上的LQR/LQG做到最优再考虑扩展工作范围如果工作范围实在太大不如直接上更通用的控制方法别用线性方法的“补丁”硬撑。6.3 开源社区与生态支持最后说下生态支持。PX4和ArduPilot的内部控制环主要是串级PID和INDILQR并没有成为默认配置但在学术研究和教育项目里LQR仍然被作为经典线性控制方法广泛教学和验证。ROS 2的control框架下编写自定义的LQR控制器并不难官方template中有ControlToolbox社区也有一些基于Eigen的LQR实现。机械臂方面MoveIt的默认运动规划器和ros_control中的joint_position_controller主要还是PID风格但这不妨碍你写一个自定义的LQR Controller插件。我在Gazebo仿真里调试Panda机械臂时自己实现了LQR前馈控制的插件整个代码量不到三百行效果就比默认PID好很多。有兴趣的话完全可以照着我上面说的方法从仿真开始慢慢验证再迁移到真机。说到底LQR和LQG不只是一个“放之四海而皆准”的工具它们是一套帮助你系统性思考控制的框架。在实际项目里真正帮你解决问题的不是公式本身而是你对模型的敬畏、对调参方法的理解以及对每一类系统特性的尊重。