企业官网建设流程全解析

简介：面向无人机三维同步定位与建图（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

这个写法有两点值得说明。第一，雅可比用有限差分而不是手推解析式：状态只有 15+3m 维，每次预测多算十几次运动模型，在 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] u

u 是质量归一化的推力加速度指令，单位 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 位置项取 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; % 质量归一化推力加速度(世界系) end

a_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 估计频率与控制频率解耦：零阶保持与运动学外推

两类模块的典型频率如下，架构上先按这个差距设计：

最简单可靠的做法是零阶保持：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; % 丢弃并计数，方便日志里查规律 end

11.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 联调就从"凭感觉调参"变成了可度量的对比实验。

本文还有配套的精品资源，点击获取