★ FEATURED ARTICLE 简介一份面向四旋翼无人机研究与个人学习的 Matlab 代码资料包围绕动力学建模、控制策略与路径规划三大主题涵盖姿态与位置控制、轨迹生成及搜索算法等仿真实现适合需要结合代码理解无人机系统设计的初学者、学生或研究者。资源压缩包共 56 个文件其中 45 个 .m 脚本为主体代码9 个 .zbak 为早期备份版本另含 1 个 zip 知识拓展包和 1 个 md 说明文档整体大小仅 1.61MB易于下载并按需查阅。代码包内含多版仿真主程序、势场法、样条轨迹生成、NLGL 制导与路径规划模块并附 README 文档.zbak 备份文件可对照代码演进过程理解不同实现细节。目前已有 89 人学习浏览代码支持按模块独立运行与整机仿真适合作为四旋翼入门与进阶的个人学习参考资料。1. 四旋翼无人机的动力学、控制与路径规划这套 Matlab 代码包到底值不值得花时间拆做无人机仿真的人应该都有这种体会论文里到处是漂亮的公式和曲线但真正落到 Matlab 代码上能跑通、能改参、能复现出完整飞行逻辑的公开资源其实很少。这套代码包是我见过少数把“动力学建模—姿态控制—路径规划”串成完整链路的东西。它不是那种讲概念的 PPT也不是单点演示的控制 demo而是从电机动力学开始一路做到 B 样条轨迹优化和人工势场避障的整套工程实现。适合三类人正在做无人机课程设计或毕设的学生、刚接触四旋翼控制想找个能跑的仿真底子的工程师以及想验证自己控制算法和路径规划思路的研究者。你会看到 PID 控制器怎么调参、NLGL 制导律怎么写、势场法避障怎么和轨迹生成器对接还有 B 样条曲线怎么做平滑。整份代码全部是 .m 脚本打开就能一步步跟读下文我按使用顺序拆给你看。2. 先把骨架搭明白从动力学建模到代码文件架构2.1 为什么要先看 quad_simulation2v5.m而不是直接看控制一个新手拿到这个压缩包最容易犯的错是看到一堆名字就开始乱点比如先打开 optimizeSplineTrajectory.m 研究 B 样条。我建议你第一个打开的文件是 quad_simulation2v5.m因为这个文件是整个仿真主循环的代表作。在四旋翼仿真这件事上主循环顺序往往是先根据当前状态量算力和力矩再更新角加速度和线加速度最后积分得到下一帧状态。quad_simulation2v5.m 里就完整写了这条路。% quad_simulation2v5.m 核心主循环节选逻辑 for t dt:dt:T_total % 1. 由当前姿态和目标姿态计算控制力矩 [torque, thrust] attitudeController(attitude, attitude_des, omega, gains); % 2. 根据力矩反解四个电机的转速指令 omega_cmd mixControl(torque, thrust, motor_params); % 3. 考虑电机动态响应一阶惯性环节 omega_actual motorDynamics(omega_cmd, omega_prev, motor_time_constant, dt); % 4. 由实际转速合成力和力矩 [F, M] sumOfForces(omega_actual, motor_params); % 5. 刚体动力学更新角加速度 线加速度 omega_dot (M - cross(omega, I*omega)) / I; accel F / mass - [0;0;g]; % 6. 积分更新状态 omega omega omega_dot * dt; attitude updateAttitude(attitude, omega, dt); position position velocity * dt; end代码逻辑不复杂但这一步是整个仿真能不能跑得“像真机”的分水岭。很多简化的仿真只做运动学直接把期望速度积分成位置等于跳过了四旋翼“不能平移只能倾斜”的特性。quad_simulation2v5.m 里至少保留了角速度交叉项 cross(omega, I*omega)这个是刚体动力学里的陀螺力矩项如果你把它去掉仿真在快速旋转时会明显“飘”这就是为什么有人跑出的响应曲线看着很怪但找不到原因。参数方面mass 整机的质量I 是三轴转动惯量矩阵g 重力加速度dt 建议从 0.005 秒开始设数值稳定性比较好。2.2 代码文件的功能分区一团散落的文件名其实是有组织的这个包里文件名看着多而乱但按功能分实际上就四个区。第一个区是“仿真主循环”代表是 quad_simulation.m、quad_simulation2.m 一直到 v5 这些版本v2v5 是迭代版本功能上大同小异但修正了不少 bug比如 v5 修了姿态更新时的旋转矩阵归一化问题。第二个区是“控制相关”包括 quad_simulation_pid.m、attitudeChange.m、orientationController.m、MotorControlSimulation.m。第三个区是“路径规划与轨迹生成”包括 potential_field.m、genPathGradient.m、bSplineTrajectory.m、controlPointOptimization.m、bezierBNT.m。第四个区是“制导与跟踪测试”包括 NLGL.m、NLGL_testing.m、PN_testing.m、tracking_algo_test.m。我拿到这类资源的第一件事从来不是读代码而是先建立一个“哪些文件是入口、哪些是被调用”的依赖图。startup.m 就是入口初始化脚本里面一般设置路径和全局参数quad_simulation2v5.m 是主仿真testing 后缀的脚本是验证模块用的比如你想单独看 NLGL 制导的效果就直接跑 NLGL_testing.m不需要走全仿真这样调试效率高很多。README.md 是索引文档写清了每个文件的作用和调用顺序我建议你把它当作路线图而不是收藏夹里吃灰的说明。2.3 动力学建模里最容易出错的两个物理量这份代码对动力学建模的处理有个地方值得单独说sumOfForces.m 把每个电机的推力方向和机体坐标系绑死了。四旋翼的升力方向永远沿着机体 z 轴而扭矩由电机转速差产生。新手常犯的错是在世界坐标系下直接加力结果无人机在倾斜时受力方向完全错乱。正确做法是先在机体坐标系算总推力和力矩再用旋转矩阵转到世界坐标系。代码里 sumOfForces.m 的输入是四个电机的实际转速输出是 [F; M]F 是标量总推力沿机体 z 方向M 是三轴力矩。如果你想加入气动阻力一般做法是在这个文件里追加一个 -k_d * velocity 项。另一个物理量是电机响应延迟。真正四旋翼的电机不是“给指令立刻到转速”的而是有几十到几百毫秒的时间常数。包里 motorDynamics.m 就是处理这个的它用一阶惯性环节模拟电机延迟omega_actual 的变化是 omega_cmd 和 omega_prev 的加权平均。time_constant 越大电机响应越慢飞起来越“肉”。我见过有人在仿真里完全不建模这个延迟结果控制参数搬到真机上就剧烈振荡原因就是真机有延迟而仿真没有。这份代码至少帮你把这个关键环节补上了。3. 把控制串起来PID 调参与姿态控制的代码实现3.1 attitudeChange.m 和 orientationController.m 分别负责什么四旋翼控制通常分成内外环内环控制姿态角速度外环控制姿态角或位置。attitudeChange.m 在生产代码里一般负责的是“根据当前姿态和目标姿态计算误差姿态”也就是从旋转矩阵或四元数里提取出需要纠正的角度差orientationController.m 则是把角度差映射成力矩指令。两者合起来才是一个完整的姿态控制器。这里有个常见做法值得留意如果直接用欧拉角的差值做 PID大角度机动时会出现万向节锁问题表现是角度接近 90 度时控制量突变。包里用的是旋转矩阵或四元数路径误差姿态的计算方式是先求当前姿态到目标姿态的相对旋转矩阵再把相对旋转矩阵转成轴角轴就是旋转轴角就是误差角度。这样在数学上永远不会出现欧拉角的奇异性。我在自己的项目里强烈建议你在仿真阶段就统一用这个方案否则后面做全姿态机动测试时会被欧拉角折腾到怀疑人生。3.2 从代码看 PID 参数怎么设先角速度环再角度环quad_simulation_pid.m 是一个典型的串级 PID 实现。所谓串级就是外环角度环的输出作为内环角速度环的期望输入。这个结构的物理意义是角度误差告诉无人机“你要往哪个方向转一点”角速度环再告诉电机“你要转多快”。只做单环角度 PID 的问题是如果角速度太快角度会超调太慢又跟不上指令。串级结构天然缓解了这个问题。% quad_simulation_pid.m 串级PID姿态控制核心片段 % 外环角度环输出期望角速度 omega_des Kp_angle * angle_error; % P控制足够了通常不加微分 % 内环角速度环输出期望力矩 torque Kp_omega * (omega_des - omega_current) Kd_omega * (-omega_dot); % 推力环高度保持输出总升力 thrust Kp_alt * (alt_des - alt_current) Kd_alt * (-vel_z) hover_thrust;代码逻辑很直白角度环输出期望角速度角速度环输出力矩。需要注意的是角度环我一般只用 P因为加了 D 会放大噪声角速度环加 D 才有实际意义它相当于阻尼项能抑制振荡。调参顺序永远是先内环后外环。先把角速度环的 Kp_omega 从小往大加直到角速度响应快且没有嗡嗡声的振荡然后再加角度环的 Kp_angle。反过来调的人通常会在内环还没稳时就被外环参数带偏最后整个姿态都在抖。hover_thrust 是悬停油门补偿大约等于 mass * g / 4四个电机但还与力臂长度有关具体看代码里的力臂参数。3.3 姿态环调参的实操建议与验证方法跑 orientationController.m 或者 quad_simulation_pid.m 的时候我建议你做一个固定动作给一个 30 度的俯仰阶跃指令然后盯三个东西超调量、调节时间、稳态误差。超调量控制在 5% 以内调节时间在 1 到 2 秒之间稳态误差趋近于零这个仿真就算立住了。如果超调大优先加角速度环的 Kd_omega而不是减角度环的 Kp_angle——加阻尼比降增益更符合物理直觉。如果响应慢优先加角速度环的 Kp_omega。如果始终有稳态误差检查是不是 hover_thrust 掐得太准了实际仿真里加一点小偏置很正常。4. 路径规划的两个层次先全局寻径再局部避障4.1 potential_field.m 人工势场数学简单但陷阱不少人工势场法的思想就是把目标点设计成引力场把障碍物设计成斥力场无人机在梯度下降方向上运动。potential_field.m 里的实现基本就是计算当前位置的合力归一化后按步长移动一步然后循环直到到达目标点。单独跑这个文件是没问题的但如果你把势场法当成全局路径规划来用一定会遇到两个经典问题局部极小值和目标点附近的振荡。局部极小值的表现是无人机停在一个不是目标点的地方不动了因为那点的引力和斥力正好抵消。目标点附近的振荡是因为随着距离减小引力趋于无穷而斥力方向变化太快。% potential_field.m 核心力场计算节选 % 引力正比于距离指向目标点 F_att K_att * (target_pos - current_pos); % 斥力当距离小于安全距离时生效 dist_obs norm(obs_pos - current_pos); if dist_obs d_safe F_rep K_rep * (1/dist_obs - 1/d_safe) * (1/dist_obs^2) ... * (current_pos - obs_pos) / dist_obs; else F_rep 0; end % 合力方向作为运动方向 F_total F_att F_rep; direction F_total / norm(F_total); current_pos current_pos direction * step_size;这段代码里的参数只有在配合调参时才有意义。K_att 是引力增益K_rep 是斥力增益d_safe 是障碍物安全距离step_size 是每步移动距离。我建议你跑的时候把 K_rep 设成 K_att 的 3 到 5 倍d_safe 设成无人机半径的 1.5 到 2 倍step_size 不要超过 d_safe 的十分之一否则会把障碍物边缘“锯齿化”。局部极小值没有通用的数学解法实践中我会加随机扰动或者改用下一节要讲的梯度路径生成。4.2 genPathGradient.m 梯度路径生成把“直飞”变成“能走的路”genPathGradient.m 做的事情是在栅格地图上计算每个格子的势场值然后从起点沿梯度下降方向走直到终点。它和 potential_field.m 的区别是potential_field.m 是连续空间的实时受力导航genPathGradient.m 是离散网格上的一次性离线规划。你可以在路径规划阶段调用它生成一条全局大致路径然后交给低层控制器去跟踪。这里有个现实问题栅格法的路径是走格子的会有锯齿和直角弯直接给无人机当轨迹会飞出各种急转。包里把 genPathGradient.m 生成的粗路径喂给 bSplineTrajectory.m用 B 样条做平滑。我在自己的开发流程里一直是这套配合梯度寻的是路B 样条修的是形。如果你只做寻径不做平滑仿真里能看到的就是无人机在拐角处抖动控制环一直在追一个不连续的指令。4.3 bSplineTrajectory.m 与 controlPointOptimization.m把折线变成可飞的曲线B 样条的核心思想是路径由控制点定义样条曲线经过或逼近这些控制点且高阶连续。bSplineTrajectory.m 就是根据控制点生成样条曲线的求值代码controlPointOptimization.m 则是用优化方法调整控制点的位置让曲线满足动力学约束比如最大加速度限制。为什么需要优化而不是随便给控制点因为样条本身只保证数学平滑不保证物理可飞。如果控制点距离太近样条会拐得很急对应到物理上就是需要很大的向心加速度。controlPointOptimization.m 的做法通常是把控制点的位置作为优化变量把“轨迹平滑度”或“总加速度能量”作为目标函数把“经过关键航点”作为约束用 fmincon 或 quadprog 做求解。这个文件里的优化逻辑很可能就是二次规划因为轨迹平滑优化是典型的 QP 问题。在实际使用中我的建议是先随便给控制点跑一遍 bSplineTrajectory.m观察二维或三维轨迹有没有多余的波浪如果有就增加控制点的间距或者调高优化里平滑项的权重。不要一上来就调控制器参数因为轨迹本身不平滑控制器再努力也没办法让无人机飞出一条它追不上的曲线。轨迹生成是“给控制器喂什么”的问题控制器只能保证“喂什么追什么”。5. 避坑指南这套仿真代码跑起来最容易翻车的五个地方5.1 现象startup.m 报错说找不到函数或路径未定义原因这个包是压缩文件直接解压后文件散落在各个子文件夹里而 Matlab 默认路径不含这些子文件夹。解决先解压到固定目录然后在 Matlab 里 cd 到主目录运行 addpath(genpath(pwd)) 把当前目录及所有子目录加入搜索路径。之后再跑 startup.m 就不会报 Undefined function 了。有的 .zbak 后缀文件是备份文件如果你误把 .zbak 当成 .m 运行Matlab 会直接报错忽略它们即可真正的代码都在同名或相近名的 .m 文件里。5.2 现象仿真波形在起始几秒内出现剧烈尖峰原因初始姿态和目标姿态差得太大姿态控制器的输出直接饱和电机转速指令被上限截断后波形自然会有毛刺。解决把初始姿态设置成和目标姿态接近的值比如水平悬停时初始欧拉角全为零或者给姿态误差做一个限幅比如单轴误差超过 45 度时先按 45 度处理。这个方法不改变最终收敛结果但能避免开始阶段的数值冲击。5.3 现象无人机绕着目标点转圈、永远飞不到终点原因这个是势场法路径规划的经典问题。目标点附近引力很大斥力方向和引力方向高度不共线导致合力方向的切向分量持续存在无人机就像被弹弓打偏一样绕圈。解决把斥力的作用范围稍微调小让无人机在接近目标时不再被远处障碍物影响可以给斥力加一个方向修正项让斥力只在障碍物正对无人机的方向上起作用或者像前面说的别用势场法做末端逼近切换到直线制导或比例导引段。5.4 现象换了一台电脑跑同样代码收敛速度明显不同原因仿真里凡是写死 dt 的地方不同 CPU 性能下积分数值误差不同。代码里所有 dt 是硬编码的 0.01 秒有的版本可能在主循环里用了固定步长而没检查步长和系统动力学时间常数的匹配度。解决确认主循环的 dt 是否小于电机时间常数的十分之一。如果电机时间常数是 0.05 秒dt 至少要用 0.005 秒或更小。计算量大的路径规划测试可以单独跑不关联仿真循环的脚本比如 bSplineTrajectory.m 本身不依赖 dt快慢不会影响结果。5.5 现象路径规划导出的轨迹在跟踪时误差越来越大原因轨迹跟踪误差的累积通常不是因为控制器问题而是参考轨迹本身没满足无人机的微分约束。比如 B 样条曲线在数学上平滑但它某些位置曲率半径太小无人机飞不过去或需要很大的倾斜角才能跟上。解决用 controlPointOptimization.m 重新优化控制点并在优化约束中加一条曲率限制条件让样条任意位置的曲率半径大于无人机最小转弯半径。最小转弯半径可以从最大倾斜角和速度估算公式是 R_min v^2 / (g * tan(phi_max))phi_max 取 30 度到 40 度之间比较保守。6. 把仿真玩出深度从姿态阶跃到完整任务的验证链路很多人满足于“代码能跑、波形能出”但实际项目里更关键的是你改了一个参数怎么确定性能是变好了还是变坏了我自己的习惯是建一套标准验证链路每次改动后强制走一遍从姿态响应到路径跟踪都看到结果再结束。第一步是姿态阶跃测试。给俯仰角一个 30 度阶跃指令记录超调量、调节时间和稳态误差并且记录角速度环的响应曲线。如果超调反复出现我会同时看 Kd_omega 和电机时间常数因为它们共同决定了阻尼比。第二步是做位置跟踪测试。给定一个直线航点序列让无人机依次飞出五段直线观察每段切换时的位置误差峰值和恢复时间。第三步是完整任务测试。路径规划模块生成一条包含转弯和避障的轨迹无人机从起点起飞依次路过全部航点然后降落。全程记录电机转速曲线检查有没有转速到顶的饱和段饱和段的出现说明控制器在“硬掰”一个它掰不过来的轨迹请求。NLGL_testing.m 是一个被低估的验证工具。NLGL 制导律的思想是在参考轨迹上选一个前方点look-ahead point无人机朝这个点飞而不是朝最近的点飞。好处是轨迹跟踪有前瞻性不会在急弯处抖振。你可以在 NLGL_testing.m 里改 look-ahead 距离参数对比不同取值下的跟踪误差。经验值是look-ahead 距离设成无人机速度的 1 到 2 倍速度 3m/s 时取 5 到 6 米比较合适。设太小会来回追、设太大则在转弯时切弯内侧。从那以后我每次做轨迹跟踪都会先跑一遍 NLGL 参数扫描宁可多花十分钟扫参数也不要在主仿真里折腾一晚上还说不清误差从哪里来。这套代码里还有一个值得单独玩的模块是 PN_testing.m比例导引。它常用于追踪动态目标核心是视线角速度乘以导航常数 N 得到加速度指令。你可以把追踪场景设计成一个目标在平面内做蛇形机动然后对比比例导引和单纯直线追踪的脱靶量差异。仿真阶段把这项加进去你会发现路径规划模块的“智能”其实有很大一部分是控制层给的。最后给你一个收尾习惯每次跑完仿真导出转速、姿态角、位置误差三条曲线存成一个固定格式的图时间戳命名。改参数后先对比新图与旧图再去找代码原因。这个习惯帮我躲过了不少“改 A 参数结果 B 文件报错”的连锁翻车。希望帮到你。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
