简介面向无人机三维同步定位与建图3D SLAM及轨迹跟踪控制方向的 Matlab 综合代码包适合高校计算机、电子信息工程、数学等专业学生用于课程设计、期末大作业或毕业设计。压缩包内共 51 个文件包含 46 个 .m 源程序、3 个 .mat 数据文件、1 个 .md 说明文档和 1 个 .avi 演示视频总大小约 13.82MB可直接运行并支持参数化修改与注释对照。代码以扩展卡尔曼滤波实现未知环境下的无人机 3D SLAM同时采用线性二次型调节器LQR完成姿态与轨迹跟踪控制覆盖仿真参数设置、观测模型、状态预测、更新及绘图等完整流程附带蒙特卡洛测试数据和操作演示便于理解算法原理与调试验证。目前已有 118 人学习下载适合需要快速上手 EKF-SLAM 与 LQR 控制结合的读者作为可运行模板并能在此基础上开展改进与拓展研究。1. 为什么 EKF 的 UAV 3D SLAM 与 LQR 轨迹跟踪要放进同一个 MATLAB 工程EKF 的 UAV 3D SLAM 解决我在地图哪里LQR 解决下一步往哪飞、给多大油门单独跑都成熟合进一个工程后难点全在接口EKF 以 10~30 Hz 输出带协方差的后验估计LQR 需要 100 Hz 以上的平滑状态做误差反馈SLAM 把地标建在世界系控制器却要在机体系解算期望姿态。频率差和坐标差处理不好单跑正常的模块合起来就会出现低频抖动或轨迹恒定偏角。下面按典型 MATLAB 仿真包的写法展开状态向量与雅可比怎么组织、lqr() 权重怎么设、EKF 输出怎么接进控制器最后给三个能直接复现的调参验证检查点适合正在做无人机导航与控制联调的工程师也适合准备把二维 EKF SLAM 扩成三维的读者。这类代码包通常把 ekfPredict、ekfUpdate、lqrGain、trajectory 拆成独立函数文件主脚本只负责调度后面的写法也按这个结构来。2. EKF 做 UAV 3D SLAM状态向量、IMU 预测与观测更新2.1 状态向量结构15 3m 的组成与选取理由3D EKF SLAM 的状态向量是无人机自身状态和地标位置的并集。自身部分取 15 维等于位置 3 维、速度 3 维、欧拉角 3 维、加速度计偏置 3 维、陀螺仪偏置 3 维每个地标再贡献 3 维位置。写成列向量是x [p_n, p_e, p_d, v_n, v_e, v_d, φ, θ, ψ, b_a(3), b_g(3), ℓ_1(3), ..., ℓ_m(3)]ᵀ前 15 维是飞行器状态后面的 3m 维是地图。为什么教学习惯用 15 维而不是带四元数的 16 维欧拉角的运动模型和观测模型雅可比都是直接三角函数代码可读性好调试时打印状态一眼能看懂代价是俯仰接近 ±90° 时欧拉角微分矩阵奇异。无人机常规飞行不碰这个边界先用欧拉角把三维 SLAM 跑通再换误差状态四元数是更稳的路线。偏置两项是三维 EKF 最容易忽略但必须加的。加速度计偏置不估计速度误差随时间近似线性增长位置误差则二次增长室内长航时场景几分钟就能漂出几米。陀螺仪偏置影响姿态姿态又通过旋转矩阵放大到位置预测上。所以两个偏置都进状态、不进观测靠 IMU 预测和地标观测的互补性把它们估出来。2.2 IMU 驱动的运动模型与预测方程预测用惯性测量推进加速度计给出机体系比力陀螺仪给出机体角速度各减偏置后推进状态。位置和速度用加速度做二阶积分姿态走欧拉角微分方程。可直接抄的预测函数如下function [x, P] ekfPredict(x, P, imu, dt) % imu [ax ay az wx wy wz]机体坐标系的比力和角速度 p x(1:3); v x(4:6); eul x(7:9); ba x(10:12); bg x(13:15); Rbw eul2rotm(eul.); % 机体系 - 世界系(NED) a_w Rbw * (imu(1:3) - ba) [0;0;9.81]; omg imu(4:6) - bg; phi eul(1); th eul(2); E [1, sin(phi)*tan(th), cos(phi)*tan(th); 0, cos(phi), -sin(phi); 0, sin(phi)/cos(th), cos(phi)/cos(th)]; % 欧拉角微分矩阵 f0 x; f0(1:3) p v*dt 0.5*a_w*dt^2; f0(4:6) v a_w*dt; f0(7:9) eul E*omg*dt; % 偏置按随机游走保持不动 n numel(x); Fx zeros(n, n); dx 1e-6; for j 1:n xp x; xp(j) xp(j) dx; Fx(:,j) (motionStep(xp, imu, dt) - f0) / dx; % 有限差分雅可比 end F eye(n) Fx*dt; P F * P * F. getProcessNoise(imu, dt); % 过程噪声协方差 x f0; end这个写法有两点值得说明。第一雅可比用有限差分而不是手推解析式状态只有 153m 维每次预测多算十几次运动模型在 MATLAB 仿真里耗时可忽略却避免了大矩阵里推错一个偏导后找不到 bug 的困境真机移植时再换回解析雅可比。第二motionStep 的实现必须和 f0 完全同构否则差分出来的 Fx 是错的。motionStep 就是把 f0 那三段状态更新单独提成一个函数输入状态和 imu 与 dt返回推进后的完整状态向量。getProcessNoise的常见做法是按位置-速度-欧拉角-两个偏置分块对角位置块给零速度块按加速度计噪声给(0.1~0.5)^2*eye(3)欧拉角块由陀螺仪白噪声积分等效偏置块给很小的随机游走。若仿真里位置协方差收敛得过快先检查 Q 是否明显小于传感器真实噪声这是 EKF 过度自信最常见的原因。提示eul2rotm 在 Robotics System Toolbox 等工具箱里如果机器上没有手写R Rz(psi)*Ry(theta)*Rx(phi)三行乘法也一样关键是方向约定前后一致。2.3 距离-方位观测模型与 EKF 更新三维 SLAM 的观测模型按传感器分两类激光或深度相机给距离加方位距离-方位-俯仰单目相机给纯方位。教学工程最常见的是前一种测量向量z [r; az; el]更新函数写成下面这样function [x, P] ekfUpdate(x, P, z, idLm, Rm) % z [range; azimuth; elevation]Rm 是 3x3 观测噪声协方差 n numel(x); lmIdx 15 3*(idLm-1) (1:3); % 第 idLm 个地标在状态里的位置 lm x(lmIdx); del lm - x(1:3); % 地标相对无人机的位置差 rng norm(del); rho sqrt(del(1)^2 del(2)^2); az atan2(del(2), del(1)); el atan2(del(3), rho); % 观测 h 对相对位置向量 del 的雅可比3x3 Jd [del. / rng; [-del(2), del(1), 0] / (rho^2); [-del(3)*del(1), -del(3)*del(2), rho^2] / (rng^2 * rho)]; H zeros(3, n); H(:, 1:3) -Jd; % 对无人机位置的偏导 H(:, lmIdx) Jd; % 对地标位置的偏导 h [rng; az; el]; inov z - h; inov(2:3) wrapToPi(inov(2:3)); % 角度差折叠到 [-pi, pi] S H * P * H. Rm; K P * H. / S; % 右除避免显式求逆 x x K * inov; IKH eye(n) - K*H; P IKH * P * IKH. K * Rm * K.; % Joseph 形式数值更稳 end更新函数里最容易错两处。一是角度新息必须 wrapToPi否则地标从 179° 转到 -179° 时会产生巨大的虚假新息把状态直接打飞二是地标落在无人机正上方时 rho 接近零方位角的雅可比会爆炸代码里要加一个最小阈值判断。一个测量周期内有多条观测时逐条调用 ekfUpdate、每次用更新后的 P比堆成大矩阵一次性更新更稳。数据关联在典型教学包里被简化测量自带地标 ID省掉最近邻匹配。真机上数据关联比滤波本身更容易炸把它改成先门控再更新的做法见第 4 章。2.4 主循环骨架预测、更新与地标管理的调度串起来的主循环不长关键是节奏IMU 预测可以在高频跑观测更新只在有测量的周期执行。常见做法是外层按控制周期推进里面按需做 EKF 步骤for k 1 : N imu imuBuf(:, k); [x, P] ekfPredict(x, P, imu, dt); for j 1 : numel(meas{k}) m meas{k}(j); [x, P] ekfUpdate(x, P, m.z, m.id, Rm); end estHist(k,:) x(1:6).; % 记录位置速度估计供对比与画图 end第一次见到某个地标时常见做法是在状态末尾追加 3 维并给很大的初始协方差比如 100 m² 量级表示见过但不知道在哪。这个追加逻辑和 2.2 节的有限差分雅可比一起工作时要注意雅可比维度同步增长否则 Fx 的列数对不上——维度不同步是这个联合仿真里最常见的运行时错误。拿到这类 ekf 算法源码第一步也不是跑主脚本而是确认状态向量维度注释和每个函数的输入输出签名。3. LQR 无人机控制线性化模型、黎卡提方程与轨迹跟踪权重3.1 LQR 与 EKF 的搭配逻辑LQR 是状态反馈控制里和 EKF 最同构的一种EKF 用协方差表达对状态的信任度LQR 用权重矩阵 Q、R 表达对误差和控制代价的权衡两者都是先建模、再解代数问题、最后给常增益。这个特点让联调舒服EKF 收敛后协方差进入稳态LQR 增益是常数整套系统没有随时间切换的调度逻辑行为可预测。经典结论里单输入 LQR 在输入断开点有至少 60° 的相位裕度和无穷大增益裕度这是 PID 很难保证的。多输入情况不能直接套这个结论但设计直觉保留LQR 天然比单纯极点配置对模型误差更宽容正好匹配 EKF 输出里既有估计噪声又有估计时延的情况。用过 lqr 平衡车的话类比很直接Q 是把车扶正有多重要R 是电机出力有多贵在无人机上 Q 的位置项就是跟踪轨迹偏差的代价。3.2 悬停点线性化位置环退化成双积分器四旋翼完整模型是 12 维的但做轨迹跟踪时位置环和姿态环可以分层内环把期望姿态跟踪到接近理想外环看到的位置动力学就只剩双积分关系。误差状态取e [p - p_ref; v - v_ref]线性化模型是d/dt [dp; dv] [0 I; 0 0] [dp; dv] [0; I] uu 是质量归一化的推力加速度指令单位 m/s²。这个降维不是偷懒内环带宽如果是位置环的 5~10 倍内外环耦合就落在参数不确定性里LQR 的裕度足以吸收。反过来直接对 12 维模型做全状态 LQR增益矩阵是 4×12调参维度暴增教学工程很少这么干。3.3 用 lqr() 解黎卡提方程代码与 Q/R 参数表MATLAB 里求解增益就三行A [zeros(3,3), eye(3); zeros(3,3), zeros(3,3)]; B [zeros(3,3); eye(3)]; Q diag([8 8 8 2 2 2]); % 位置误差权重 8速度误差权重 2 R 0.8 * eye(3); % 加速度指令代价 [K, S, e] lqr(A, B, Q, R);位置环三轴对称3×6 的 K 实际是两个标量增益乘单位阵K ≈ [3.16·I₃, 2.97·I₃]。对应闭环极点在 -1.49 ± 0.98j阻尼比约 0.83自然频率约 1.78 rad/s对 10~30 Hz 的 EKF 更新率来说控制带宽只占很小一段频谱上不会和估计噪声撞车。现在用 codex 这类 AI 工具可以一行生成 lqr() 调用但 Q/R 为什么这么取、带宽为什么不能更高仍然需要人来回答。Q/R 的调整规律用一张表说清楚参数作用调大后果调小后果Q 位置块(1:3)轨迹跟踪紧度位置误差变小、带宽变高易激发结构共振弯道处轨迹明显外切Q 速度块(4:6)误差阻尼响应变钝、收敛变慢超调增大容易和位置项互相牵制R(标量·I)加速度指令代价指令平滑、抗噪好、跟踪迟钝指令毛刺大、逼近执行器饱和经验起点是 Q 位置项取 5~10、速度项取位置项的 1/4 到 1/2、R 取 0.5~1之后按上表后果单边调整。注意 lqr 在 Control System Toolbox 里不在优化工具箱只有基础 MATLAB 的环境可以换成 care() 解连续代数黎卡提方程效果等价。3.4 前馈 反馈的轨迹跟踪期望姿态解算有了 K 之后控制律是标准前馈加反馈。重力项在这个式子里不出现因为误差动力学里它被消掉了但它会在后面的姿态解算里回来function [a_cmd, e] lqrTrack(t, x, traj, K) [p_ref, v_ref, a_ref] traj(t); % 轨迹生成器位置/速度/加速度 e [x(1:3) - p_ref; x(4:6) - v_ref]; a_cmd a_ref - K * e; % 质量归一化推力加速度(世界系) enda_cmd 往下游走一步就是期望姿态和油门总加速度 a_total a_cmd g推力方向取 a_total 的方向油门 T m·‖a_total‖。小角度下期望姿态是a_total a_cmd [0;0;9.81]; th_ref a_total(1) / 9.81; % 俯仰 ph_ref -a_total(2) / 9.81; % 滚转符号与欧拉角定义强相关这里符号是无数人踩过的坑。不同仿真包对欧拉角和机体轴定义不同期望滚转角到底是 a_total(2)/g 还是其相反数取决于 NED 与机体轴朝向。最稳的验证办法是给 x 方向加速度阶跃看俯仰角方向是否符合你的定义符号错一个轨迹跟踪会发散去。注意航向角不为零的轨迹8 字、螺旋线里先把 a_total(1:2) 按 -ψ_ref 旋转到轨迹航向系再算 th_ref 与 ph_ref否则转弯时会一直缺一个横滚分量。4. EKF 输出接入 LQR坐标系对齐、频率匹配与协方差门控4.1 坐标系对齐NED 约定与 z 轴符号坑EKF 建图和控制器轨迹都写在 NED 世界系地标是绝对坐标这本身没有歧义。歧义出在机体系到世界系的旋转方向以及不同模块对z 轴向下为正还是向上为正的默认值。把 SLAM 估计位置直接接进 LQR 之前第一个验证动作是让无人机悬停 10 秒比较估计位置、真值位置和控制器看到的位置三者是否一致。不一致时先查 eul2rotm 的转置再查观测模型里 el 角的正负。常见错误是某根轴被当成 ENU 处理结果高度环误差出现恒定偏置LQR 会一直给一个错误的预偏来抵消一个不存在的重力项。这个 bug 在单模块仿真里发现不了因为 EKF 和 LQR 各自闭合串起来才表现为轨迹高度始终低 0.3 米这类恒定偏移。4.2 估计频率与控制频率解耦零阶保持与运动学外推两类模块的典型频率如下架构上先按这个差距设计环节典型频率说明IMU 预测100~400 Hz预测步轻量可随控制节拍跑观测更新10~30 Hz受视觉或雷达帧率限制LQR 位置环100~200 Hz位置环带宽约 2~5 rad/s足够姿态内环200~500 Hz与位置环拉开 10 倍以上最简单可靠的做法是零阶保持EKF 每更新一次就把 x 和 P 写进共享变量控制循环读最新值不做插值。控制频率是估计频率的 5 倍以上时零阶保持引入的延迟只有 1~2 个控制周期落在前面算的相位裕度里。追求更高跟踪精度时可以在两次 EKF 更新之间用运动模型外推一针function xc extrapolate(x, imu, dtCtl) % 轻量外推只推进位置速度姿态不更新协方差 [xc, ~] motionStep(x, imu, dtCtl); end外推的代价是协方差不增长控制器把外推值当成确定量相当于隐式地给系统加了噪声。所以外推窗口要短超过 20 ms 或两个控制周期时宁可用旧值也不要继续推。提示控制周期固定 5 ms 时连续 LQR 增益直接用于离散系统误差可忽略控制周期超过 20 ms 就需要先用 c2d(A,B,dt) 转离散模型再用 lqrd 解离散增益。4.3 协方差 P 与创新门控让异常观测不进闭环EKF 协方差在闭环里最值得用的地方是新息门控更新前用 S HPH R 算马氏距离超标的观测直接丢弃。这是把 2.3 节更新函数改成工程版本的关键一步S H * P * H. Rm; g2 inov. / S * inov; if g2 11.34 % 3 自由度卡方分布 99% 分位数 [x, P] ekfUpdateCore(x, P, z, idLm, Rm); % 复用 2.3 节更新 else nReject nReject 1; % 丢弃并计数方便日志里查规律 end11.34 是 chi2inv(0.99, 3) 的值写死可以省掉统计工具箱依赖。门控防止错误关联的观测把状态拉偏这在 EKF 里比在粒子滤波里更致命因为卡尔曼增益会把错误新息按协方差比例永久写进状态。被丢弃观测的 idLm 如果连续出现通常意味着该地标已经移动或数据关联表过期了。另外协方差更新里的矩阵求逆在真机移植到 stm32 这类 MCU 时要换成 Cholesky 分解求解K P*H. / S避免显式 inv() 的数值问题和耗时。4.4 串起整个 MATLAB 仿真主循环把所有模块合成可运行的闭环仿真主循环骨架如下dtCtl 0.005; dtUpd 0.05; % 控制 200 Hz更新 20 Hz x zeros(15,1); P blkdiag(1e-6*eye(9), 1e-4*eye(6)); % 姿态和偏置给宽松初值 tNextUpd 0; for k 1 : round(Tsim / dtCtl) t (k-1) * dtCtl; % 1) 控制用共享的 EKF 估计 [a_cmd, ~] lqrTrack(t, xEst, traj, K); % 2) 仿真无人机真实动力学 传感器采样(真值加噪声) xTrue uavDynamics(xTrue, a_cmd, dtCtl); imu sampleImu(xTrue, t); % 3) EKF 预测(高频)与观测更新(低频) [xEst, P] ekfPredict(xEst, P, imu, dtCtl); if t tNextUpd meas sampleMeas(xTrue, landmarks, t); for j 1 : numel(meas) [xEst, P] gatedUpdate(xEst, P, meas(j), Rm); % 带门控的更新 end tNextUpd tNextUpd dtUpd; end end注意第 2 步用真值推进第 1 步的控制量来自 EKF 估计这才是工程的实际形态。很多初版联调代码图省事让控制器直接用真值做反馈结果 EKF 完全被架空这点在下一章的验证方法里专门处理。gatedUpdate 的内部就是先算 h、H、inov、S门控通过才执行更新复用 2.3 节的函数体。5. 调参验证EKF-LQR 闭环的三个检查点与一个对比技巧5.1 检查点一EKF 发散性检验同一套参数换随机种子重跑 20 次统计位置 RMSE 的均值与标准差。EKF 对地标初始方差和过程噪声很敏感单次仿真通过不代表收敛for r 1 : 20 rng(r); err runClosedLoop(r); % 同轨迹、同控制器、不同噪声序列 rmse(r,:) sqrt(mean(err.^2, 1)); end fprintf(RMSE mean%.3f std%.3f\n, mean(rmse(:)), std(rmse(:)));标准差超过均值一半时先调过程噪声 Q再看地标初始协方差不要动 LQR。发散仿真要在轨迹跑完之前停机否则发散后的数值会污染统计量。5.2 检查点二Q/R 联动与带宽验证固定一条带急弯的闭合轨迹只缩放 Q 的位置块记录峰值位置误差与控制加速度峰值。一组示意数据Q_pos 从 0.5 倍放到 4 倍峰值误差从 0.42 m 压到 0.14 m控制加速度峰值从 2.8 涨到 7.9 m/s²。这个交换关系就是选型依据执行器有饱和限制时最优 Q_pos 是误差刚进入允许阈值的最小值而不是误差最小的值。先做一次位置阶跃看超调与调节时间再决定往哪个方向动权重比直接扫参更有直觉。5.3 检查点三延迟裕度测试与估计-真值双闭环对比给 EKF 输出人为叠加零阶保持延迟从 0 加到 80 ms观察跟踪误差何时开始振荡。对 200 Hz 控制器经验上 30~50 ms 是常见裕度边界超过后位置环噪声明显放大。最后用双闭环对比技巧收尾同一轨迹分别用真值和 EKF 估计做反馈两者 RMSE 之差就是估计误差对跟踪性能的真实代价errTrue runClosedLoop(feedback, true); errEst runClosedLoop(feedback, ekf); cost sqrt(mean(errEst.^2,1)) - sqrt(mean(errTrue.^2,1));差值大说明估计是瓶颈优先压观测噪声 Rm 或提高观测频率差值小才值得去扩 LQR 带宽。把这三组曲线和门控丢弃计数日志放在一起EKF-LQR 联调就从凭感觉调参变成了可度量的对比实验。本文还有配套的精品资源点击获取
EKF 3D SLAM与LQR轨迹跟踪的MATLAB联合仿真工程实践
简介面向无人机三维同步定位与建图3D SLAM及轨迹跟踪控制方向的 Matlab 综合代码包适合高校计算机、电子信息工程、数学等专业学生用于课程设计、期末大作业或毕业设计。压缩包内共 51 个文件包含 46 个 .m 源程序、3 个 .mat 数据文件、1 个 .md 说明文档和 1 个 .avi 演示视频总大小约 13.82MB可直接运行并支持参数化修改与注释对照。代码以扩展卡尔曼滤波实现未知环境下的无人机 3D SLAM同时采用线性二次型调节器LQR完成姿态与轨迹跟踪控制覆盖仿真参数设置、观测模型、状态预测、更新及绘图等完整流程附带蒙特卡洛测试数据和操作演示便于理解算法原理与调试验证。目前已有 118 人学习下载适合需要快速上手 EKF-SLAM 与 LQR 控制结合的读者作为可运行模板并能在此基础上开展改进与拓展研究。1. 为什么 EKF 的 UAV 3D SLAM 与 LQR 轨迹跟踪要放进同一个 MATLAB 工程EKF 的 UAV 3D SLAM 解决我在地图哪里LQR 解决下一步往哪飞、给多大油门单独跑都成熟合进一个工程后难点全在接口EKF 以 10~30 Hz 输出带协方差的后验估计LQR 需要 100 Hz 以上的平滑状态做误差反馈SLAM 把地标建在世界系控制器却要在机体系解算期望姿态。频率差和坐标差处理不好单跑正常的模块合起来就会出现低频抖动或轨迹恒定偏角。下面按典型 MATLAB 仿真包的写法展开状态向量与雅可比怎么组织、lqr() 权重怎么设、EKF 输出怎么接进控制器最后给三个能直接复现的调参验证检查点适合正在做无人机导航与控制联调的工程师也适合准备把二维 EKF SLAM 扩成三维的读者。这类代码包通常把 ekfPredict、ekfUpdate、lqrGain、trajectory 拆成独立函数文件主脚本只负责调度后面的写法也按这个结构来。2. EKF 做 UAV 3D SLAM状态向量、IMU 预测与观测更新2.1 状态向量结构15 3m 的组成与选取理由3D EKF SLAM 的状态向量是无人机自身状态和地标位置的并集。自身部分取 15 维等于位置 3 维、速度 3 维、欧拉角 3 维、加速度计偏置 3 维、陀螺仪偏置 3 维每个地标再贡献 3 维位置。写成列向量是x [p_n, p_e, p_d, v_n, v_e, v_d, φ, θ, ψ, b_a(3), b_g(3), ℓ_1(3), ..., ℓ_m(3)]ᵀ前 15 维是飞行器状态后面的 3m 维是地图。为什么教学习惯用 15 维而不是带四元数的 16 维欧拉角的运动模型和观测模型雅可比都是直接三角函数代码可读性好调试时打印状态一眼能看懂代价是俯仰接近 ±90° 时欧拉角微分矩阵奇异。无人机常规飞行不碰这个边界先用欧拉角把三维 SLAM 跑通再换误差状态四元数是更稳的路线。偏置两项是三维 EKF 最容易忽略但必须加的。加速度计偏置不估计速度误差随时间近似线性增长位置误差则二次增长室内长航时场景几分钟就能漂出几米。陀螺仪偏置影响姿态姿态又通过旋转矩阵放大到位置预测上。所以两个偏置都进状态、不进观测靠 IMU 预测和地标观测的互补性把它们估出来。2.2 IMU 驱动的运动模型与预测方程预测用惯性测量推进加速度计给出机体系比力陀螺仪给出机体角速度各减偏置后推进状态。位置和速度用加速度做二阶积分姿态走欧拉角微分方程。可直接抄的预测函数如下function [x, P] ekfPredict(x, P, imu, dt) % imu [ax ay az wx wy wz]机体坐标系的比力和角速度 p x(1:3); v x(4:6); eul x(7:9); ba x(10:12); bg x(13:15); Rbw eul2rotm(eul.); % 机体系 - 世界系(NED) a_w Rbw * (imu(1:3) - ba) [0;0;9.81]; omg imu(4:6) - bg; phi eul(1); th eul(2); E [1, sin(phi)*tan(th), cos(phi)*tan(th); 0, cos(phi), -sin(phi); 0, sin(phi)/cos(th), cos(phi)/cos(th)]; % 欧拉角微分矩阵 f0 x; f0(1:3) p v*dt 0.5*a_w*dt^2; f0(4:6) v a_w*dt; f0(7:9) eul E*omg*dt; % 偏置按随机游走保持不动 n numel(x); Fx zeros(n, n); dx 1e-6; for j 1:n xp x; xp(j) xp(j) dx; Fx(:,j) (motionStep(xp, imu, dt) - f0) / dx; % 有限差分雅可比 end F eye(n) Fx*dt; P F * P * F. getProcessNoise(imu, dt); % 过程噪声协方差 x f0; end这个写法有两点值得说明。第一雅可比用有限差分而不是手推解析式状态只有 153m 维每次预测多算十几次运动模型在 MATLAB 仿真里耗时可忽略却避免了大矩阵里推错一个偏导后找不到 bug 的困境真机移植时再换回解析雅可比。第二motionStep 的实现必须和 f0 完全同构否则差分出来的 Fx 是错的。motionStep 就是把 f0 那三段状态更新单独提成一个函数输入状态和 imu 与 dt返回推进后的完整状态向量。getProcessNoise的常见做法是按位置-速度-欧拉角-两个偏置分块对角位置块给零速度块按加速度计噪声给(0.1~0.5)^2*eye(3)欧拉角块由陀螺仪白噪声积分等效偏置块给很小的随机游走。若仿真里位置协方差收敛得过快先检查 Q 是否明显小于传感器真实噪声这是 EKF 过度自信最常见的原因。提示eul2rotm 在 Robotics System Toolbox 等工具箱里如果机器上没有手写R Rz(psi)*Ry(theta)*Rx(phi)三行乘法也一样关键是方向约定前后一致。2.3 距离-方位观测模型与 EKF 更新三维 SLAM 的观测模型按传感器分两类激光或深度相机给距离加方位距离-方位-俯仰单目相机给纯方位。教学工程最常见的是前一种测量向量z [r; az; el]更新函数写成下面这样function [x, P] ekfUpdate(x, P, z, idLm, Rm) % z [range; azimuth; elevation]Rm 是 3x3 观测噪声协方差 n numel(x); lmIdx 15 3*(idLm-1) (1:3); % 第 idLm 个地标在状态里的位置 lm x(lmIdx); del lm - x(1:3); % 地标相对无人机的位置差 rng norm(del); rho sqrt(del(1)^2 del(2)^2); az atan2(del(2), del(1)); el atan2(del(3), rho); % 观测 h 对相对位置向量 del 的雅可比3x3 Jd [del. / rng; [-del(2), del(1), 0] / (rho^2); [-del(3)*del(1), -del(3)*del(2), rho^2] / (rng^2 * rho)]; H zeros(3, n); H(:, 1:3) -Jd; % 对无人机位置的偏导 H(:, lmIdx) Jd; % 对地标位置的偏导 h [rng; az; el]; inov z - h; inov(2:3) wrapToPi(inov(2:3)); % 角度差折叠到 [-pi, pi] S H * P * H. Rm; K P * H. / S; % 右除避免显式求逆 x x K * inov; IKH eye(n) - K*H; P IKH * P * IKH. K * Rm * K.; % Joseph 形式数值更稳 end更新函数里最容易错两处。一是角度新息必须 wrapToPi否则地标从 179° 转到 -179° 时会产生巨大的虚假新息把状态直接打飞二是地标落在无人机正上方时 rho 接近零方位角的雅可比会爆炸代码里要加一个最小阈值判断。一个测量周期内有多条观测时逐条调用 ekfUpdate、每次用更新后的 P比堆成大矩阵一次性更新更稳。数据关联在典型教学包里被简化测量自带地标 ID省掉最近邻匹配。真机上数据关联比滤波本身更容易炸把它改成先门控再更新的做法见第 4 章。2.4 主循环骨架预测、更新与地标管理的调度串起来的主循环不长关键是节奏IMU 预测可以在高频跑观测更新只在有测量的周期执行。常见做法是外层按控制周期推进里面按需做 EKF 步骤for k 1 : N imu imuBuf(:, k); [x, P] ekfPredict(x, P, imu, dt); for j 1 : numel(meas{k}) m meas{k}(j); [x, P] ekfUpdate(x, P, m.z, m.id, Rm); end estHist(k,:) x(1:6).; % 记录位置速度估计供对比与画图 end第一次见到某个地标时常见做法是在状态末尾追加 3 维并给很大的初始协方差比如 100 m² 量级表示见过但不知道在哪。这个追加逻辑和 2.2 节的有限差分雅可比一起工作时要注意雅可比维度同步增长否则 Fx 的列数对不上——维度不同步是这个联合仿真里最常见的运行时错误。拿到这类 ekf 算法源码第一步也不是跑主脚本而是确认状态向量维度注释和每个函数的输入输出签名。3. LQR 无人机控制线性化模型、黎卡提方程与轨迹跟踪权重3.1 LQR 与 EKF 的搭配逻辑LQR 是状态反馈控制里和 EKF 最同构的一种EKF 用协方差表达对状态的信任度LQR 用权重矩阵 Q、R 表达对误差和控制代价的权衡两者都是先建模、再解代数问题、最后给常增益。这个特点让联调舒服EKF 收敛后协方差进入稳态LQR 增益是常数整套系统没有随时间切换的调度逻辑行为可预测。经典结论里单输入 LQR 在输入断开点有至少 60° 的相位裕度和无穷大增益裕度这是 PID 很难保证的。多输入情况不能直接套这个结论但设计直觉保留LQR 天然比单纯极点配置对模型误差更宽容正好匹配 EKF 输出里既有估计噪声又有估计时延的情况。用过 lqr 平衡车的话类比很直接Q 是把车扶正有多重要R 是电机出力有多贵在无人机上 Q 的位置项就是跟踪轨迹偏差的代价。3.2 悬停点线性化位置环退化成双积分器四旋翼完整模型是 12 维的但做轨迹跟踪时位置环和姿态环可以分层内环把期望姿态跟踪到接近理想外环看到的位置动力学就只剩双积分关系。误差状态取e [p - p_ref; v - v_ref]线性化模型是d/dt [dp; dv] [0 I; 0 0] [dp; dv] [0; I] uu 是质量归一化的推力加速度指令单位 m/s²。这个降维不是偷懒内环带宽如果是位置环的 5~10 倍内外环耦合就落在参数不确定性里LQR 的裕度足以吸收。反过来直接对 12 维模型做全状态 LQR增益矩阵是 4×12调参维度暴增教学工程很少这么干。3.3 用 lqr() 解黎卡提方程代码与 Q/R 参数表MATLAB 里求解增益就三行A [zeros(3,3), eye(3); zeros(3,3), zeros(3,3)]; B [zeros(3,3); eye(3)]; Q diag([8 8 8 2 2 2]); % 位置误差权重 8速度误差权重 2 R 0.8 * eye(3); % 加速度指令代价 [K, S, e] lqr(A, B, Q, R);位置环三轴对称3×6 的 K 实际是两个标量增益乘单位阵K ≈ [3.16·I₃, 2.97·I₃]。对应闭环极点在 -1.49 ± 0.98j阻尼比约 0.83自然频率约 1.78 rad/s对 10~30 Hz 的 EKF 更新率来说控制带宽只占很小一段频谱上不会和估计噪声撞车。现在用 codex 这类 AI 工具可以一行生成 lqr() 调用但 Q/R 为什么这么取、带宽为什么不能更高仍然需要人来回答。Q/R 的调整规律用一张表说清楚参数作用调大后果调小后果Q 位置块(1:3)轨迹跟踪紧度位置误差变小、带宽变高易激发结构共振弯道处轨迹明显外切Q 速度块(4:6)误差阻尼响应变钝、收敛变慢超调增大容易和位置项互相牵制R(标量·I)加速度指令代价指令平滑、抗噪好、跟踪迟钝指令毛刺大、逼近执行器饱和经验起点是 Q 位置项取 5~10、速度项取位置项的 1/4 到 1/2、R 取 0.5~1之后按上表后果单边调整。注意 lqr 在 Control System Toolbox 里不在优化工具箱只有基础 MATLAB 的环境可以换成 care() 解连续代数黎卡提方程效果等价。3.4 前馈 反馈的轨迹跟踪期望姿态解算有了 K 之后控制律是标准前馈加反馈。重力项在这个式子里不出现因为误差动力学里它被消掉了但它会在后面的姿态解算里回来function [a_cmd, e] lqrTrack(t, x, traj, K) [p_ref, v_ref, a_ref] traj(t); % 轨迹生成器位置/速度/加速度 e [x(1:3) - p_ref; x(4:6) - v_ref]; a_cmd a_ref - K * e; % 质量归一化推力加速度(世界系) enda_cmd 往下游走一步就是期望姿态和油门总加速度 a_total a_cmd g推力方向取 a_total 的方向油门 T m·‖a_total‖。小角度下期望姿态是a_total a_cmd [0;0;9.81]; th_ref a_total(1) / 9.81; % 俯仰 ph_ref -a_total(2) / 9.81; % 滚转符号与欧拉角定义强相关这里符号是无数人踩过的坑。不同仿真包对欧拉角和机体轴定义不同期望滚转角到底是 a_total(2)/g 还是其相反数取决于 NED 与机体轴朝向。最稳的验证办法是给 x 方向加速度阶跃看俯仰角方向是否符合你的定义符号错一个轨迹跟踪会发散去。注意航向角不为零的轨迹8 字、螺旋线里先把 a_total(1:2) 按 -ψ_ref 旋转到轨迹航向系再算 th_ref 与 ph_ref否则转弯时会一直缺一个横滚分量。4. EKF 输出接入 LQR坐标系对齐、频率匹配与协方差门控4.1 坐标系对齐NED 约定与 z 轴符号坑EKF 建图和控制器轨迹都写在 NED 世界系地标是绝对坐标这本身没有歧义。歧义出在机体系到世界系的旋转方向以及不同模块对z 轴向下为正还是向上为正的默认值。把 SLAM 估计位置直接接进 LQR 之前第一个验证动作是让无人机悬停 10 秒比较估计位置、真值位置和控制器看到的位置三者是否一致。不一致时先查 eul2rotm 的转置再查观测模型里 el 角的正负。常见错误是某根轴被当成 ENU 处理结果高度环误差出现恒定偏置LQR 会一直给一个错误的预偏来抵消一个不存在的重力项。这个 bug 在单模块仿真里发现不了因为 EKF 和 LQR 各自闭合串起来才表现为轨迹高度始终低 0.3 米这类恒定偏移。4.2 估计频率与控制频率解耦零阶保持与运动学外推两类模块的典型频率如下架构上先按这个差距设计环节典型频率说明IMU 预测100~400 Hz预测步轻量可随控制节拍跑观测更新10~30 Hz受视觉或雷达帧率限制LQR 位置环100~200 Hz位置环带宽约 2~5 rad/s足够姿态内环200~500 Hz与位置环拉开 10 倍以上最简单可靠的做法是零阶保持EKF 每更新一次就把 x 和 P 写进共享变量控制循环读最新值不做插值。控制频率是估计频率的 5 倍以上时零阶保持引入的延迟只有 1~2 个控制周期落在前面算的相位裕度里。追求更高跟踪精度时可以在两次 EKF 更新之间用运动模型外推一针function xc extrapolate(x, imu, dtCtl) % 轻量外推只推进位置速度姿态不更新协方差 [xc, ~] motionStep(x, imu, dtCtl); end外推的代价是协方差不增长控制器把外推值当成确定量相当于隐式地给系统加了噪声。所以外推窗口要短超过 20 ms 或两个控制周期时宁可用旧值也不要继续推。提示控制周期固定 5 ms 时连续 LQR 增益直接用于离散系统误差可忽略控制周期超过 20 ms 就需要先用 c2d(A,B,dt) 转离散模型再用 lqrd 解离散增益。4.3 协方差 P 与创新门控让异常观测不进闭环EKF 协方差在闭环里最值得用的地方是新息门控更新前用 S HPH R 算马氏距离超标的观测直接丢弃。这是把 2.3 节更新函数改成工程版本的关键一步S H * P * H. Rm; g2 inov. / S * inov; if g2 11.34 % 3 自由度卡方分布 99% 分位数 [x, P] ekfUpdateCore(x, P, z, idLm, Rm); % 复用 2.3 节更新 else nReject nReject 1; % 丢弃并计数方便日志里查规律 end11.34 是 chi2inv(0.99, 3) 的值写死可以省掉统计工具箱依赖。门控防止错误关联的观测把状态拉偏这在 EKF 里比在粒子滤波里更致命因为卡尔曼增益会把错误新息按协方差比例永久写进状态。被丢弃观测的 idLm 如果连续出现通常意味着该地标已经移动或数据关联表过期了。另外协方差更新里的矩阵求逆在真机移植到 stm32 这类 MCU 时要换成 Cholesky 分解求解K P*H. / S避免显式 inv() 的数值问题和耗时。4.4 串起整个 MATLAB 仿真主循环把所有模块合成可运行的闭环仿真主循环骨架如下dtCtl 0.005; dtUpd 0.05; % 控制 200 Hz更新 20 Hz x zeros(15,1); P blkdiag(1e-6*eye(9), 1e-4*eye(6)); % 姿态和偏置给宽松初值 tNextUpd 0; for k 1 : round(Tsim / dtCtl) t (k-1) * dtCtl; % 1) 控制用共享的 EKF 估计 [a_cmd, ~] lqrTrack(t, xEst, traj, K); % 2) 仿真无人机真实动力学 传感器采样(真值加噪声) xTrue uavDynamics(xTrue, a_cmd, dtCtl); imu sampleImu(xTrue, t); % 3) EKF 预测(高频)与观测更新(低频) [xEst, P] ekfPredict(xEst, P, imu, dtCtl); if t tNextUpd meas sampleMeas(xTrue, landmarks, t); for j 1 : numel(meas) [xEst, P] gatedUpdate(xEst, P, meas(j), Rm); % 带门控的更新 end tNextUpd tNextUpd dtUpd; end end注意第 2 步用真值推进第 1 步的控制量来自 EKF 估计这才是工程的实际形态。很多初版联调代码图省事让控制器直接用真值做反馈结果 EKF 完全被架空这点在下一章的验证方法里专门处理。gatedUpdate 的内部就是先算 h、H、inov、S门控通过才执行更新复用 2.3 节的函数体。5. 调参验证EKF-LQR 闭环的三个检查点与一个对比技巧5.1 检查点一EKF 发散性检验同一套参数换随机种子重跑 20 次统计位置 RMSE 的均值与标准差。EKF 对地标初始方差和过程噪声很敏感单次仿真通过不代表收敛for r 1 : 20 rng(r); err runClosedLoop(r); % 同轨迹、同控制器、不同噪声序列 rmse(r,:) sqrt(mean(err.^2, 1)); end fprintf(RMSE mean%.3f std%.3f\n, mean(rmse(:)), std(rmse(:)));标准差超过均值一半时先调过程噪声 Q再看地标初始协方差不要动 LQR。发散仿真要在轨迹跑完之前停机否则发散后的数值会污染统计量。5.2 检查点二Q/R 联动与带宽验证固定一条带急弯的闭合轨迹只缩放 Q 的位置块记录峰值位置误差与控制加速度峰值。一组示意数据Q_pos 从 0.5 倍放到 4 倍峰值误差从 0.42 m 压到 0.14 m控制加速度峰值从 2.8 涨到 7.9 m/s²。这个交换关系就是选型依据执行器有饱和限制时最优 Q_pos 是误差刚进入允许阈值的最小值而不是误差最小的值。先做一次位置阶跃看超调与调节时间再决定往哪个方向动权重比直接扫参更有直觉。5.3 检查点三延迟裕度测试与估计-真值双闭环对比给 EKF 输出人为叠加零阶保持延迟从 0 加到 80 ms观察跟踪误差何时开始振荡。对 200 Hz 控制器经验上 30~50 ms 是常见裕度边界超过后位置环噪声明显放大。最后用双闭环对比技巧收尾同一轨迹分别用真值和 EKF 估计做反馈两者 RMSE 之差就是估计误差对跟踪性能的真实代价errTrue runClosedLoop(feedback, true); errEst runClosedLoop(feedback, ekf); cost sqrt(mean(errEst.^2,1)) - sqrt(mean(errTrue.^2,1));差值大说明估计是瓶颈优先压观测噪声 Rm 或提高观测频率差值小才值得去扩 LQR 带宽。把这三组曲线和门控丢弃计数日志放在一起EKF-LQR 联调就从凭感觉调参变成了可度量的对比实验。本文还有配套的精品资源点击获取
