简介面向无人直升机控制算法学习与开发者这份基于MATLAB的源码实现了LQG控制策略融合LQR最优控制与Kalman滤波用于解决复杂环境下自主飞行的状态估计与鲁棒控制问题。包内仅1个m文件大小1KB虽然精简却覆盖系统建模、状态空间线性化、LQR控制器设计、Kalman滤波器参数设置以及闭环实时更新等关键环节能直观呈现俯仰、滚转、偏航与高度等多自由度耦合模型的简化实现过程。已有276人学习浏览。资源还点明MPI在大型矩阵运算中的并行加速思路有助于理解算法从单机仿真到分布式计算扩展的路径。适合正在研究无人机飞控、最优控制或状态估计的工程师参考通过研读与调试该代码可快速掌握理论公式向MATLAB代码转化的方法也能为课程设计、毕业设计或飞行控制课题提供起点。1. 无人直升机控制为什么首选 LQG测不准和控不稳是同一个问题悬停状态的无人直升机有两件事让经典频域法很难受。第一件是开环不稳定由于桨盘有效上反和旋翼摆振的耦合滚转与俯仰通道里各有一对接近虚轴的弱不稳定模态偏航又是一个积分环节姿态角不会自己回到零。第二件是测量带噪声姿态角来自加速度计与陀螺的融合角速率来自陀螺垂向速度来自气压计每一项都混着随机误差和延迟。LQG 控制恰好把这两件事放进同一个框架——用 LQR 设计状态反馈把弱不稳定极点拉进左半平面用卡尔曼滤波器从带噪声的观测里重建状态分离定理保证估计器与反馈律可以分开设计、拼起来闭环仍然稳定。对手里只有一套 IMU、气压计和悬停线性模型的飞控工程师来说LQG 控制是能直接落地、而不是停留在仿真里的起点。2. 从悬停线性模型到状态空间无人直升机 LQG 的地基LQG 的所有结论都建立在 A、B、C 三个矩阵上。模型辨识得准不准直接决定 LQR 的极点敢放在哪里、卡尔曼滤波的增益敢不敢给大。所以第一步不是调 Q 和 R而是把无人直升机的悬停运动整理成一份规范的线性时不变状态空间模型。2.1 悬停线性化的适用前提和模型假设无人直升机大速度前飞时机身入流、平尾尾流和旋翼摆振都会随空速明显变化气动导数不再是常数模型强非线性。但大多数作业——吊装、巡检、起降——集中在悬停和 5 m/s 以内的低速段。工程上的常见做法是选悬停作为配平点对六自由度刚体方程做小扰动线性化得到一组常系数导数再为不同空速点分别辨识模型做增益调度。LQG 本身要求对象是 LTI这决定了它天然绑定悬停和低速场景。线性化过程有三个默认假设欧拉角小角度所以姿态运动可以近似成对 φ、θ 独立积分机体角速率近似等于欧拉角速率不做姿态变换矩阵的耦合忽略旋翼摆振自由度把周期变距对机体的力矩效果直接折进 B 矩阵。第三点是小型无人直升机最常见的简化代价是当控制带宽逼近旋翼转速的 1/3 时摆振模态会被激励起来悬停模型在这个频段失效。后面做鲁棒性检查本质上是给这种简化兜底。2.2 状态、输入、测量矩阵的约定状态选机体系下的三个线速度、三个角速率和三个欧拉角顺序固定下来A 矩阵的耦合关系就很容易对照物理过程检查。状态含义单位说明u, v, w前向、侧向、垂向速度m/s机体坐标轴系p, q, r滚转、俯仰、偏航角速率rad/s陀螺直接测量的量φ, θ, ψ滚转、俯仰、偏航角rad姿态融合输出输入选四个总距 δ_col、纵向周期变距 δ_lon、横向周期变距 δ_lat、尾桨距 δ_ped单位统一成归一化杆量0 到 1 对应满行程。这样控制增益的数值可以直接和舵机行程联系起来而不是抽象的弧度。测量取七路p、q、r、φ、θ、ψ、w。前六路来自 IMU 和姿态融合w 来自气压计加 GPS 垂向速度融合对应 C 矩阵是 7×9。2.3 用 Python 组装悬停模型并查看开环特征值下面这组导数是 5kg 级小型无人直升机悬停点的典型取值带零下标的系数表示配平点附近的稳定性导数变化量。具体机型的数值要靠频率扫描加辨识得到这里先给出能跑通全流程的一组底数。import numpy as np # 状态顺序: u v w p q r phi theta psi A np.array([ [-0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -9.81, 0.0], [ 0.0, -0.04, 0.0, 0.0, 0.0, 0.0, 9.81, 0.0, 0.0], [ 0.0, 0.0, -0.30, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [ 0.0, 1.0, 0.0, -8.6, 0.0, 0.0, 0.0, 0.0, 0.0], [ 0.8, 0.0, 0.0, 0.0, -8.6, 0.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 0.0, 0.0, -0.55, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], ]) B np.array([ [ 0.0, -9.81, 0.0, 0.0], [ 0.0, 0.0, 9.81, 0.0], [-32.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, 62.0, 0.0], [ 0.0, 62.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 11.0], [ 0.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 0.0], ]) eig np.linalg.eigvals(A) print(np.sort_complex(eig).round(3))运行后可以看到-8.6 是旋翼的滚转和俯仰阻尼导数-0.55 是偏航阻尼-9.81 是小扰动下重力对速度方程的贡献M_u0.8 和 L_v1.0 是桨盘有效上反带来的俯仰-速度、滚转-侧向速度耦合正是无人直升机悬停弱不稳定的根源。计算机会给出三组值得注意的极点纵向上一对实部为正的复数极点约 0.03±0.95j横向上一个约 1.0 的实数发散极点外加偏航和滚转角的积分极点。开环不稳定但又不是发散得特别快这种形态最适合用增稳控制去收拾。2.4 可观性校核别在滤波前丢状态C 矩阵定义完以后先用秩条件确认九个状态都能从这七路测量里恢复出来。C np.zeros((7, 9)) C[0, 3] 1.0 # 测 p C[1, 4] 1.0 # 测 q C[2, 5] 1.0 # 测 r C[3, 6] 1.0 # 测 phi C[4, 7] 1.0 # 测 theta C[5, 8] 1.0 # 测 psi C[6, 2] 1.0 # 测 w O np.vstack([C np.linalg.matrix_power(A, k) for k in range(9)]) print(可观性矩阵秩:, np.linalg.matrix_rank(O)) # 必须是 9如果删掉 w 这一路测量秩立刻掉到 8卡尔曼滤波器的 Riccati 方程不会收敛后面的 LQG 全是假象。这里还有一个值得注意的细节u 和 v 的可观性完全来自 M_u、L_v 这两个耦合项而不是直接测量。也就是说估计器重建前向和侧向速度是靠模型传播和姿态测量的高阶信息噪声会偏大。这是这种低成本测量配置的固有代价后面调 Qw 时要把这个因素考虑进去。3. LQR 与卡尔曼滤波分开设计分离定理和 Q、R、Qw、Rv 的参数初值LQG 的工程魅力在于把两个子问题拆开先确定性地设计 LQR再随机性地设计卡尔曼滤波器两个设计彼此独立拼起来仍然最优。但拆开的前提是两边的权重都选得对这一章把四个矩阵的物理含义和初值取值讲清楚。3.1 LQR 的代价函数与 Bryson 参数初值LQR 求解的是无限时域的线性二次最优问题J ∫₀^∞ (xᵀQx uᵀRu)dt。Q 惩罚状态偏差R 惩罚操纵量。解代数 Riccati 方程得到矩阵 P反馈增益 K R⁻¹BᵀP。注意代价函数里没有噪声的位置LQR 是确定性设计——这是分离定理的第一半。权重初值不要凭感觉填。工程上最常用的是 Bryson 规则Q_ii 1/(允许状态偏差最大值)²R_jj 1/(允许操纵量最大值)²。对无人直升机悬停姿态要求在 0.1 rad 内拉回角速率允许 0.3 rad/s速度允许 2 m/s各通道杆量允许 10% 行程于是得到下面这组初值。通道允许偏差权重初值u, v2.0 m/s0.25w1.0 m/s1.0p, q, r0.3 rad/s11.1φ, θ0.1 rad100ψ0.1 rad100四个输入0.1 杆量100这组初值的含义是姿态用 0.1 rad 的尺度衡量而速度用 2 m/s 的尺度衡量姿态权重比速度权重大了两个数量级符合悬停增稳的优先级。实际整定时先固定 Q 不动 R把 R 逐步减小直到某一路杆量在初始扰动下接近饱和的临界值再回头微调 Q 里的姿态项。3.2 卡尔曼滤波器的 G、Qw、Rv噪声从哪来滤波器针对的是随机模型 ẋ Ax Bu Gw测量 y Cx v。Qw E[wwᵀ] 描述模型在哪些方向不可信Rv E[vvᵀ] 描述传感器噪声水平。Qw 的物理来源很具体悬停模型忽略了摆振、来流扰动和参数辨识误差这些误差主要落在角加速度和线加速度通道上Rv 的来源则是传感器数据手册或者 Allan 方差曲线上的白噪声段数值不需要猜。两种 G 的取法各有场景。取 G B相当于把不确定度归到等效输入扰动物理上对应桨尖涡和阵风改变桨盘受力的过程Qw 的维度是输入数取 G I相当于在每个状态导数上直接叠加噪声实现最省事权重也最好解释。工程上我一般先用 G I 跑通闭环再切到 G B 对照估计效果。下面这组 Qw、Rv 是按 G I 配置的Qw np.diag([0.25, 0.25, 0.6, 2.0, 2.0, 0.8, 0.01, 0.01, 0.001]) Rv np.diag([2.25e-4, 2.25e-4, 2.25e-4, 2.25e-4, 2.25e-4, 4.0e-4, 2.5e-3])Rv 对角线对应陀螺 0.015 rad/s 的噪声标准差、姿态角 0.015 到 0.02 rad、垂向速度 0.05 m/s都是消费级 IMU 实测水平。Qw 里 p、q 通道给了 2.0因为摆振模型误差主要作用在角加速度上φ、θ 通道给 0.01姿态运动学本身是精确积分不需要额外噪声。常见错误是把 Qw 整体调大让估计“跟得快”结果 P_f 对角线虚低估计器自以为自己很准闭环却抖得厉害。3.3 分离定理把控制极点与估计极点拼起来把控制器 u -Kx̂ 和观测器 x̂̇ Ax̂ Bu L(y - Cx̂) 代入真实对象定义估计误差 e x - x̂可以得到一组块三角形式的闭环方程ẋ (A - BK)x BKe Gwė (A - LC)e Gw - Lv矩阵是上三角形的所以闭环特征值集合正好是 A-BK 的特征值加上 A-LC 的特征值。这说明“分开设计”不是一个近似技巧而是精确结论分离定理保证了两套极点互不干扰。工程上的直接用法是先用 A-BK 的极点定控制带宽再要求 A-LC 的极点比它快 3 到 5 倍让估计误差在控制起作用之前就衰减完。提示分离定理成立的前提是模型精确、噪声高斯白且统计特性已知。模型有误差时 LQG 没有天然鲁棒性保证这在第 5 章专门处理。3.4 用 python-control 计算 K 和 Lpython-control 的 lqr 和 lqe 直接解连续时间 Riccati 方程签名和 MATLAB 保持一致适合快速迭代。import control as ct Q np.diag([0.25, 0.25, 1.0, 11.1, 11.1, 11.1, 100.0, 100.0, 100.0]) R np.diag([100.0, 100.0, 100.0, 100.0]) K, P_lqr, _ ct.lqr(A, B, Q, R) G np.eye(9) # 过程噪声直接加在状态导数上 L, P_f, _ ct.lqe(A, G, C, Qw, Rv) print(控制极点:, np.linalg.eigvals(A - B K).round(3)) print(估计极点:, np.linalg.eigvals(A - L C).round(3))ct.lqr 内部求解 AᵀP PA - PBR⁻¹BᵀP Q 0返回的 K 是 4×9 的状态反馈矩阵ct.lqe 求解的是滤波形式 A P P Aᵀ - P Cᵀ Rv⁻¹ C P G Qw Gᵀ 0返回的 L 是 9×7 的卡尔曼增益P_f 就是稳态估计协方差。注意 lqe 第三个参数传 C 矩阵本身不要传转置这是最常见的调用错误。算完之后检查两类极点控制极点实部大概在 -2 到 -6 rad/s估计极点实部在 -10 附近两组极点至少隔开 3 倍。如果间距不够回路带宽会被估计器拖累闭环动态会出现低频振荡。4. 无人直升机 LQG 闭环仿真代码、调参表和验收指标K 和 L 算完只完成了一半工作。真正决定能不能上机的是闭环仿真里控制律和观测器是不是按真实飞控的逻辑在跑。这一章给出完整可运行的仿真以及第一轮调试的验收指标。4.1 闭环结构控制器、观测器和误差方程仿真里同时存在两套系统。真实对象是 ẋ Ax Bu Gw测量是 y Cx v这是被控世界的模型飞控里实际运行的是 u -Kx̂ 和 x̂̇ Ax̂ Bu L(y - Cx̂)这是决策世界的模型。真实世界只有仿真里有飞行中永远拿不到 x 本身只能拿到 x̂。因此闭环仿真必须同时积分 x 和 x̂控制律只用 x̂。只积分 x、然后把 x 直接送进控制器验证出来的“稳定”是 LQG 仿真里最常见的假象——分离定理在仿真里成立在真实飞行里却不成立因为真实飞行里没有人把 x 递给你。4.2 完整仿真代码Euler-Maruyama 离散化连续时间模型做离散仿真时状态噪声项要用随机积分的规则处理高斯白噪声的增量方差是 dt所以噪声项乘的是 sqrt(dt) 而不是 dt。逐点手动积分的写法如下。from numpy.random import default_rng rng default_rng(20250420) def simulate_lqg(A, B, C, K, L, Qw, Rv, x0, xh0, T12.0, dt0.005): n A.shape[0]; nu B.shape[1]; ny C.shape[0] steps int(T / dt) t np.arange(steps 1) * dt x np.zeros((n, steps 1)) # 真实状态 xh np.zeros((n, steps 1)) # 滤波器估计 u np.zeros((nu, steps 1)) x[:, 0] x0 xh[:, 0] xh0 # 故意给偏检验估计收敛 Lw np.linalg.cholesky(Qw) # 状态噪声平方根 Lv np.linalg.cholesky(Rv) # 测量噪声平方根 for k in range(steps): y C x[:, k] Lv rng.standard_normal(ny) # 测量 u[:, k] -K xh[:, k] # 只用估计状态 x[:, k1] (x[:, k] dt * (A x[:, k] B u[:, k]) np.sqrt(dt) * Lw rng.standard_normal(n)) xh[:, k1] (xh[:, k] dt * (A xh[:, k] B u[:, k] L (y - C xh[:, k]))) return t, x, xh, u x0 np.array([0, 0, 0, 0, 0, 0, 0.10, -0.08, 0.06]) xh0 x0 * 0.5 # 初始估计只有一半幅值 t, x, xh, u simulate_lqg(A, B, C, K, L, Qw, Rv, x0, xh0)代码逻辑分四步先生成测量再算控制量然后推进真实状态最后推进估计状态。Lw 和 Lv 用 Cholesky 分解得到保证噪声协方差精确等于 Qw 和 Rv。初始估计故意给成真值的一半目的是检验卡尔曼滤波器在收敛阶段的真实行为——如果初始估计恰好给准了收敛阶段的问题全都会被掩盖。dt 取 0.005 秒对应 200 Hz 的 IMU 采样率与主流飞控一致如果 L 的特征值实部超过 1/dt 的十分之一离散化会吃掉估计带宽仿真里会出现高频抖振。4.3 第一轮仿真要盯哪几个量姿态通道看曲线形态φ 和 θ 应该在 2 到 3 秒内回到 ±0.5° 以内超调不超过初始扰动的一半。估计通道看误差收敛x̂ 与 x 的差在 1 秒内进入稳态稳态波动幅度应该和 P_f 对角元的平方根在同一个量级这是滤波器整定是否正确的最直观判据。控制通道看杆量四路 u 在收敛阶段不应持续打满如果任意一路贴近限幅说明 R 权重太小或姿态权重过大回 3.1 调整。提示仿真里把输入限幅加上再跑一遍比如 np.clip(u, -0.3, 0.3)。LQG 本身没有抗饱和机制限幅后的非线性会打破分离定理的假设这一步不做试飞时的积分饱和迟早找上门。4.4 Q、R、Qw、Rv 的调参方向表四个矩阵的调整方向整理成一张表每个参数至少调 2 到 3 倍才会看到明显变化一次只动一个。参数作用调大的效果调小的效果R控制能量代价增益低、响应慢、杆量平缓响应快、接近饱和Q 姿态项姿态误差代价姿态收敛快姿态松、速度被动拖带Qw模型信任度估计跟得快、输出噪声大估计平滑、滞后明显Rv传感器信任度估计更平滑、滞后估计跟得快、噪声大口诀是先调 R 后调 Q再调 Qw 后调 Rv。前一组决定控制带宽后一组决定估计带宽两组之间靠 3.3 的分离定理保持至少 3 倍间隔。仿真里如果姿态响应慢但杆量还很平稳优先减小 R如果姿态收敛快但杆量抖得像噪声先看估计器输出的高频成分而不是急着加滤波器。5. LQG 的丢增益、LTR 恢复和估计器一致性校验5.1 LQG 没有天然增益裕度LTR 恢复 LQR 回路形状LQR 有一项被反复引用的好性质单回路增益摄动 ±6 dB、相位裕度 60°。但把卡尔曼滤波器接上去组成 LQG 之后这套保证就没了。Doyle 在 1978 年给出的反例表明LQG 的稳定裕度可以任意小甚至可以破坏闭环稳定性。原因是 LQR 的相位裕度基于全状态反馈而卡尔曼滤波器的引入改变了回路传递函数在中断点看到的形状。工程上的标准补救是 LQG/LTR 回路传递恢复把 Qw 取成 q²BBᵀ 加一个小对角项q 从 0.1 逐步增大到 10³重构 L 并检查输出端断开的开环传函是否逼近纯 LQR 的开环传函。def controller_tf(A, B, C, K, L, s): n len(A) Ac A - L C - B K # 观测器与反馈的组合矩阵 return -K np.linalg.solve(s * np.eye(n) - Ac, L) # y 到 u def loop_at_output(A, B, C, K, L, s): G C np.linalg.solve(s * np.eye(n) - A, B) # 对象传函 return G controller_tf(A, B, C, K, L, s) # 输出端开环在闭环带宽附近的频率点比如 s 2j比较 LQG 与纯 LQR 的开环奇异值q 足够大时两者会重合。经验上 q 取 10 到 100 就能恢复 80% 的回路形状q 再继续增大测量噪声会被滤波器放大试飞时反而抖。LTR 的代价是估计器不再是最小方差意义上的最优换来的却是实打实的鲁棒性储备。5.2 用 Riccati 解 P_f 做估计器一致性校验卡尔曼滤波器整定得对不对有一种地面就能做的免拆机校验跑 40 次蒙特卡洛仿真只换随机种子统计稳态段估计误差的样本协方差与 P_f 对角线逐项比较。errs [] for i in range(40): t, x, xh, u simulate_lqg(A, B, C, K, L, Qw, Rv, x0, xh0, T8.0, dt0.005) errs.append((x - xh)[:, 400:]) # 跳过前 2 秒暂态 err np.hstack(errs) ratio np.diag(np.cov(err))[:3] / np.diag(P_f)[:3] print(ratio.round(2)) # u/v/w 三个通道的比值比值落在 0.7 到 1.5 之间说明 Qw 和 Rv 的取值与仿真里的真实噪声统计匹配比值远大于 1.5说明 Qw 给小了或者噪声不是高斯白重点检查桨尖涡、振动这些有色噪声源比值远小于 0.7说明模型某些通道比预期还准可以适当减小 Qw 让估计更平滑。这个校验在试飞前是免费的却能把 Qw 和 Rv 装反、矩阵维度搭错这类低级问题全部拦下来。本文还有配套的精品资源点击获取