简介针对无人机三维同步定位与建图3D SLAM及航迹跟踪控制需求这份Matlab代码资源集成了扩展卡尔曼滤波EKF实现未知与已知环境的地图构建并采用线性二次型调节器LQR控制无人机沿目标轨迹稳定飞行适合计算机、电子信息工程、数学等专业学生用于课程设计、期末大作业和毕业设计。压缩包共51个文件以46个M脚本程序为主体覆盖状态预测与更新、地标检测、姿态与轨迹控制等核心函数另含3个MAT数据文件、1个MD说明文档和1个AVI演示视频整体约13.82MB代码采用参数化编程注释明细支持Matlab 2014/2019a/2021a附赠案例数据可直接运行。包内同时提供已知与未知环境下的SLAM、目标轨迹生成、LQR姿态与轨迹控制、3D动画绘制等模块MD文档帮助理解算法流程AVI展示地标检测的实时效果便于对照仿真进行排错与分析。目前已有118人学习适合作为无人机自主导航方向的项目参考或算法入门模板有利于快速复现典型UAV导航方案并理解EKF与LQR结合的实现细节。1. 当 GPS 失效时EKF 与 LQR 就是无人机最后的驾驶逻辑无人机在室内、桥底或树冠下飞行时GPS 信号要么没有、要么漂移成一条弧线。这时候要让飞机既不撞墙、又能沿着一条预设轨迹飞完靠的是两套算法的配合EKF扩展卡尔曼滤波负责回答“我现在在哪、周围有什么”LQR线性二次型调节器负责回答“下一步该给多大的油门和舵量”。标题里那份 MATLAB 代码就是把这两套东西接到同一个循环里做 3D SLAM 和轨迹跟踪。对刚接触无人机自主飞行的开发者来说难点不在看懂 EKF 或 LQR 的数学而在搞清楚状态向量怎么定、观测怎么对齐、控制周期和滤波周期怎么匹配。这篇文章就把这条链路摊开给出可以直接跑的 MATLAB 骨架。2. EKF 如何撑起 UAV 的 3D SLAM 状态估计2.1 状态向量的定义位置、姿态、速度与路标点3D SLAM 里 EKF 的状态向量不是只装无人机自身位姿还要把地图里的路标坐标一起扩进去。常见做法是把状态分成两部分无人机自身的 13 维状态——三维位置、四元数姿态、三维速度、陀螺零偏——加上 N 个路标的三维坐标。这里姿态选四元数而不是欧拉角是为了避开万向锁EKF 预测更新时四元数归一化那一步不能省否则协方差矩阵迭代几次就发散了。% 状态向量结构示意9 维自身状态 3 维路标单个 % x [px py pz vx vy vz qw qx qy qz bgx bgy bgz] % 实际实现中四元数放 7~10 位路标坐标追加在末尾 n_state 13; % 位置3 速度3 四元数4 陀螺零偏3 n_landmark 3; % 单个路标的 x y z x zeros(n_state n_landmark, 1); P eye(n_state n_landmark) * 0.1; % 初始协方差这段代码定义的是状态向量的骨架。初始化时位置和速度可以从起飞点给零值四元数给单位四元数[1 0 0 0]协方差矩阵的对角元表示对初始状态的信任程度——给 0.1 说明认为起飞点漂移在 0.3 米以内。路标初始不确定度可以给大一些因为第一次观测到它时距离往往有误差。2.2 运动模型与观测模型的线性化EKF 的“E”就体现在这里系统状态转移和观测模型都不是线性的所以要用雅可比矩阵代替线性卡尔曼里的状态转移矩阵。IMU 积分出来的位置和速度是状态转移的核心它的噪声来源是加速度计和陀螺仪的零偏稳定性激光雷达或视觉特征点提供的观测是位置修正的核心噪声来源是测距精度和特征匹配误差。两个模型分开写的好处是可以单独标定。IMU 的噪声参数在静止状态下就能测出来路标的观测噪声则需要看传感器手册或做一次静态重复观测实验。很多初学 EKF SLAM 的人把过程噪声设得太小滤波器对观测的信任度过高结果就是轨迹出现跳变反过来过程噪声太大滤波结果就和纯 IMU 积分一样漂。参数典型初值调整方向过程噪声加速度0.1 m/s²轨迹抖动调大响应迟钝调小过程噪声陀螺0.01 rad/s姿态发散时检查这里观测噪声路标距离0.05 m传感器精度的一半左右观测噪声路标角度0.01 rad视觉特征提取误差约 0.5 像素时取此量级2.3 MATLAB 里 EKF 预测与更新的一轮完整实现function [x, P] ekf_slam_predict(x, P, imu_data, dt) % 提取当前状态 p x(1:3); v x(4:6); q x(7:10); bg x(11:13); % 四元数归一化防止数值误差累积 q q / norm(q); % 由 IMU 线加速度和角速度计算状态转移 R quat2rotm(q); acc imu_data.acc - bg(1:3); p_new p v * dt 0.5 * (R * acc) * dt^2; v_new v (R * acc) * dt; % 陀螺仪角速度 - 四元数微分 omega imu_data.gyro - bg; q_new quat_multiply(q, [1; 0.5 * omega * dt]); q_new q_new / norm(q_new); x(1:3) p_new; x(4:6) v_new; x(7:10) q_new; % F 矩阵状态转移对状态求偏导这里用数值差分近似 F eye(length(x)) jacobian_motion(x, imu_data, dt); Q diag([0.1*ones(3,1); 0.01*ones(3,1); 0.001*ones(4,1)]); P F * P * F Q; end这段代码体现了预测步的两个关键点四元数必须每步归一化陀螺零偏要在角速度里减去再积分否则姿态会持续旋转漂移。jacobian_motion函数里用的是数值差分精度低于解析雅可比但胜在调状态向量维度时不用重推公式。实际跑仿真时如果发现协方差矩阵不对称了多半是这里数值差分步长取得不合适。更新步的逻辑是把观测到的路标与状态里已有的路标做数据关联然后计算卡尔曼增益并更新状态。数据关联是 SLAM 里最容易出 bug 的地方——匹配错了路标更新步会把状态往错误方向拉且协方差还会同步缩小让滤波器对错误估计越来越自信。一个可靠的兜底策略是观测量残差的马氏距离超过 3 就认为是新路标宁肯多建几个点也不强行匹配。function [x, P] ekf_slam_update(x, P, z, landmark_id, R_obs) % z 是观测到的路标位置 idx 13 (landmark_id-1)*3 1 : 13 landmark_id*3; z_pred x(idx); H zeros(3, length(x)); H(:, idx) eye(3); % 观测就是路标位置H 矩阵只在这一块非零 S H * P * H R_obs; K P * H / S; innovation z - z_pred; mahal innovation / S * innovation; if mahal 9 % 3 马氏距离阈值 x x K * innovation; P (eye(length(x)) - K * H) * P; end end当观测方程写成“路标坐标直接等于当前位置”H 矩阵会非常稀疏。MATLAB 里用稀疏矩阵存 P 可以省不少内存路标超过几百个时尤其明显。检查更新步写没写对的土办法是把观测噪声 R 调到极大比如 1e6更新后的状态几乎不变说明滤波器的信任逻辑是对的。3. 基于 LQR 的轨迹跟踪控制器设计3.1 无人机线性化模型与 LQR 代价函数EKF 输出的是无人机在世界系下的位置和姿态但控制器真正操作的是油门和欧拉角指令。要把 LQR 用上去得先把无人机模型在悬停点附近线性化。悬停时速度为零、姿态角为零、油门等于重力把非线性方程在这一点展开得到一个 12 维的线性系统——位置、速度、姿态角、角速度各三项。里程计或 VIO 系统给出的定位频率一般不够直接做姿态环所以实际工程里 LQR 常用在外环位置环内环姿态环留给 PID 或串级控制去做。% 悬停点线性化后的状态空间模型位置环 % 状态 [px py pz vx vy vz]输入 [roll_cmd pitch_cmd thrust_cmd] A [zeros(3) eye(3); zeros(3) zeros(3)]; B [zeros(3); diag([g g 1])]; % g 是重力加速度z 通道油门增益近似取 1 C eye(6); D zeros(6, 3); sys ss(A, B, C, D);sys是 MATLAB 控制工具箱里的状态空间对象把模型写成这种形式后线性化误差会被内环控制器兜住。位置环如果只处理平移roll 和 pitch 通道的增益就是重力加速度这是无人机在悬停附近的近似模型大机动飞行时误差会变大。对轨迹跟踪来说这条假设基本够用因为绝大多数轨迹规划的加速度都不超过 1 m/s²。3.2 离散 LQR 求解与增益矩阵计算LQR 的核心是找一个反馈增益 K让状态误差e x - x_ref按代价函数最小的方式衰减。代价函数里两个权重矩阵 Q 和 RQ 惩罚状态误差R 惩罚控制量。MATLAB 里直接用dlqr或lqr就能解出来不需要手写黎卡提方程迭代。% 连续系统离散化采样时间取控制器周期 dt_ctrl 0.05; sys_d c2d(sys, dt_ctrl, zoh); Ad sys_d.A; Bd sys_d.B; % 权重矩阵位置误差权重远大于速度误差 Q diag([10 10 10 1 1 1]); R diag([0.1 0.1 0.1]); [K, S, e] dlqr(Ad, Bd, Q, R);代码里Q的位置项设为 10速度项设为 1代表允许的速度误差是位置误差的约三倍这在轨迹跟踪里通常合理——位置误差影响的是是否撞上障碍物速度误差只影响到达时间。R设成 0.1 是个经验起点它表示你愿意用多大的控制能量去换跟踪精度。R 太小时控制量可能超出电机饱和限制太大时轨迹会成为一条懒洋洋的大弧线。提示dlqr求解前务必确认(Ad, Bd)是可控的。无人机模型可控性一般没问题但如果你给B矩阵的某一行赋值全零比如限制 yaw 通道求解器会报错此时需要检查模型降维是否把必要通道也砍掉了。3.3 参考输入前馈与误差反馈的结合纯反馈 LQR 的跟踪效果在匀速段还行但在加速度大的拐点会出现滞后。解决办法是在反馈基础上加一个前馈项期望状态对应的控制力加上 LQR 反馈修正。对位置环来说前馈就是质量乘参考加速度在三个轴上的投影。轨迹规划器输出位置和速度前馈项里要用到的加速度可以由轨迹方程解析求导避免对位置做数值差分。% 参考轨迹某个采样点的期望状态与控制前馈 x_ref [ref_pos; ref_vel]; u_ff m * ref_acc; % 质量已知时用 m未知时可省去增益合并进 R % 反馈控制量 u_fb -K * (x_current - x_ref); u_total u_ff u_fb; % 转换到机体坐标系把世界系下期望加速度旋转到机体系 R_body quat2rotm(q_current); u_body R_body * u_total;这里u_total还要经过坐标变换才是机体坐标下的油门和姿态指令。很多调试时出现的“轨迹跟踪越来越偏”其实不是 LQR 没调好而是前馈项没加或者坐标变换方向搞反了——旋转矩阵的转置方向错一个飞机就会朝相反方向加速位置误差越拉越大。验证坐标变换对错的方法是把参考加速度设为 0、只保留反馈如果轨迹跟踪依然发散那问题必然出在反馈通道而非前馈。4. 在 MATLAB 里把 EKF 与 LQR 接成联合仿真4.1 主循环时序滤波频率与控制频率的错配实战中 EKF 的预测频率由 IMU 决定常见是 100Hz 到 200HzLQR 控制环跑在 20Hz 到 50Hz观测更新则看传感器激光雷达 10Hz、视觉 30Hz。三个频率不匹配是仿真最容易翻车的地方。解决方案是主循环按最高频率跑控制指令只在整周期沿触发EKF 更新则放到独立的判断分支里。MATLAB 里用if mod(t, dt_ctrl) dt_sim这类判断来控制不同模块的执行时段。dt_sim 0.005; % 仿真步长 5ms dt_imu 0.01; % IMU 10ms dt_ctrl 0.05; % 控制环 50ms dt_obs 0.1; % 观测 100ms t 0; x_state x_init; P_state P_init; x_true x_init; % 真实状态由仿真模型生成 while t t_end % IMU 数据生成与 EKF 预测 imu_data generate_imu(x_true, t, dt_imu); [x_state, P_state] ekf_slam_predict(x_state, P_state, imu_data, dt_imu); % 观测更新 if mod(t, dt_obs) dt_sim z generate_observation(x_true, landmarks, t); [x_state, P_state] ekf_slam_update(x_state, P_state, z, landmark_id, R_obs); end % LQR 控制更新 if mod(t, dt_ctrl) dt_sim [ref_pos, ref_vel, ref_acc] sample_trajectory(t); u_total lqr_controller(x_state, ref_pos, ref_vel, ref_acc); end % 施加控制到仿真模型推进真实状态 x_true simulate_drone(x_true, u_total, dt_sim); t t dt_sim; % 记录数据用于事后画图 log(t) struct(est, x_state, true, x_true, u, u_total); end这个主循环把三个模块的时序分得很清楚时间靠mod判断来控制。控制环读取的是滤波后的状态估计而不是真实状态——这一点和纯控制仿真不一样。如果你发现控制环用的是x_true跑出来的跟踪效果很好、换到 EKF 输出就抖不要先怀疑 LQR 参数先用这个仿真环境把滤波器噪声调到合理范围再比。4.2 轨迹生成与跟踪精度的记录方式轨迹跟踪仿真里参考轨迹的生成要满足两个约束位置连续、加速度有界。最简单的做法是生成一个八字形或螺旋上升的轨迹让三个通道都有持续激励。MATLAB 里可以用参数方程直接计算例如 x Asin(wt), y Acos(2wt) 的利萨茹曲线好处是位置、速度、加速度都可以解析求出来不需要额外插值。function [pos, vel, acc] sample_trajectory(t) A 2.0; w 0.3; pos [A * sin(w*t); A * sin(2*w*t); 0.5 * sin(0.2*w*t)]; vel [A * w * cos(w*t); 2*A * w * cos(2*w*t); 0.1 * cos(0.2*w*t)]; acc [-A * w^2 * sin(w*t); -4*A * w^2 * sin(2*w*t); -0.02 * sin(0.2*w*t)]; endvel 那行里第二个通道的2*A*w*cos(2*w*t)是 y 位置对时间求导的结果sin 求导变 cos系数乘以 2w。这种解析求导的方式比数值差分干净得多保存轨迹数据时顺带把参考速度和加速度也存下来事后分析控制能量时直接用。4.3 观测噪声设置与 EKF 实际位姿输入的关系仿真里的实际传感器噪声和 EKF 内部假设的噪声不一定一致。常见做法是传感器层用较大的噪声标准差生成观测EKF 的测量噪声矩阵 R 里填一个偏乐观的小值。这会让滤波器更信任观测但如果传感器实际噪声比假设的大滤波结果反而会跳得比真实轨迹还凶。联合仿真里最好把这两层噪声分开调先固定 EKF 的 R 不变量改传感器噪声观察估计曲线是否出现毛刺再固定传感器噪声微调 EKF 的假设噪声。上讲实践里最容易忽略的是 EKF 估计出来的协方差矩阵是否真的传给了 LQR——有的简化实现直接用点估计做控制协方差只在画误差条的时候用一下。但对于 3D SLAM 来说协方差里面保留了地图和自身位姿的相关性信息这些信息在后续回环检测和路径重规划时能派上用场留好数据结构比省这几行代码有价值。5. 参数整定与验证让联合仿真结果可信的两个技巧5.1 调参原则先验开环再闭环系统跑起来第一个要看的是 EKF 的估计轨迹和真实轨迹是否贴合。把 LQR 的输出暂时设成零给无人机一个固定的开环激励比如推杆 2 秒、横滚 1 秒观察滤波位置和真实位置的重合程度。开环激励的好处是控制量是已知的位置偏差要么来自模型参数质量、惯性、要么来自观测噪声设置和控制器无关排错范围被大幅缩小。此时如果 EKF 和真实轨迹偏差超过 0.5 米后面闭环调试会寸步难行。5.2 三个收敛判断信号调好参数后不要只看位置误差曲线还需要同时看三个信号确认系统真实工作正常。一是 EKF 的协方差矩阵对角线元素是否单调收敛到固定量级——协方差不降说明观测没起作用协方差无穷大说明雅可比矩阵求错了。二是控制输入的频谱里是否出现和结构频率重合的峰值LQR 理论上不会产生特定频段的振荡但如果 R 设得太小控制器会在每一拍都用尽全力修正产生类似抖振的现象。三是 LQR 闭环系统矩阵的特征值是否都在单位圆内MATLAB 里用eig(Ad - Bd*K)直接算特征值模长大于 1 说明仿真已经不稳定了。5.3 验证指标与保存现场最终判定用两个指标就够了轨迹跟踪位置误差的 RMSE均方根误差控制在 0.1 米量级EKF 估计位置和真实位置的 RMSE 控制在 0.05 米量级。跑完一次仿真后把状态轨迹、误差曲线、协方差对角线和控制输入全部存成.mat文件调参后回放数据时直接对比。推荐把 Q 的每个对角元单独记在一个结构体里用命名如Q.position 10; Q.velocity 1比裸矩阵可读性好很多——一周后回来看自己的代码能省不少回忆成本。本文还有配套的精品资源点击获取