1. 控制链路整体架构从导航指令到舵机指令的完整闭环写ArduPilot的姿态和位置控制绕不开一个问题这架飞机是怎么知道“该往哪飞”并且“确实飞过去的”大多数刚接触ArduPilot源码的人第一反应都是去看main函数或者载具的主循环但说实话那里面一堆调度器和条件编译宏很容易把人绕晕。我的建议是从控制链路的中间切开先理解ArduPilot把整个飞行控制分成了几层然后逐层往下追代码。ArduPilot的Copter飞控代码里控制逻辑大致分成四层导航层Navigation、位置控制层Position Control、姿态控制层Attitude Control和角速度控制层Rate Control。很多入门资料把位置控制和姿态控制分开讲这本身没错但在实际代码里它俩是串在一个200Hz到400Hz的控制周期里的。最关键的一件事是姿态控制永远跑在比位置控制更高的频率上。Copter默认的调度器里位置控制跑10Hz甚至更低的外部循环具体要看版本和模式姿态控制跑100Hz到200Hz最内层的角速度PID则是直接用定时器中断驱动。这样设计的原因很朴素——外环一旦有个相位滞后内环得靠更高的带宽先稳住飞机本身的姿态。位置控制输出的不是“位置”而是一个期望加速度再通过机头朝向和重力补偿转换成期望姿态角。姿态控制接收这个期望姿态角之后内部其实又分了两步先把期望姿态角转成期望角速度这里用了姿态误差的比例反馈再把期望角速度交给内环PID去输出力矩。所以ArduPilot里的姿态控制本质上是一个P比例加PID的级联结构。这点理解了后面看AC_AttitudeControl和AC_PosControl代码的时候就不会乱。2. 姿态控制AC_AttitudeControl的误差结算与角速度生成逻辑2.1 四元数姿态误差不是简单做减法第一次打开libraries/AC_AttitudeControl/AC_AttitudeControl.cpp的人大概率会去找“期望姿态减去当前姿态”这种代码但ArduPilot里你找不到这么直白的减法。原因在于欧拉角在接近垂直爬升或倒飞时会出现万向锁而且姿态的加减法并不满足刚体转动的合成规则。ArduPilot的姿态误差全部采用四元数来结算。核心函数是attitude_error_q它的计算逻辑是// 错误姿态 当前姿态的逆 与 期望姿态 做四元数乘法 float attitude_error_q[4]; quat_expected.inverse(q_attitude_quat, temp); quat_expected. multiply(temp, attitude_error_q);这个四元数乘积的结果代表的是“从当前姿态转到期望姿态需要施加的旋转”。ArduPilot随后会把四元数的虚部提取出来转换成机体坐标系下的姿态误差向量。这里要特别注意坐标系的转换如果搞反了飞机的修正方向会完全反掉试飞的时候基本就是翻车现场。ArduPilot在代码注释里写得很清楚姿态误差必须在机体坐标系中表达这样后面的比例控制器才能直接乘以一个对角矩阵得到期望角速度。2.2 期望角速度P控制器在姿态环的作用ArduPilot的姿态环输出是期望角速度不是直接输出力矩。这一步可能让很多人困惑为什么姿态误差不做PID只做P因为这个P是带宽的决定因素而角速度内环的PID才是消除稳态误差和阻尼振荡的主力。你可以把姿态环类比成“你看着仪表盘决定往左打多少方向舵”把角速度环类比成“你根据飞机的转动快慢来回修正方向舵”。如果两层都做PI很容易出现带宽耦合调参时互相打架这也是很多二次开发者在改代码时容易踩的坑。在attitude_controller_run_quat函数里你可以看到以下几行关键代码// 期望角速度 姿态误差 * 比例增益 _target_ang_vel.x 2.0f * _p_angle_roll.kP() * attitude_error_q[1]; _target_ang_vel.y 2.0f * _p_angle_pitch.kP() * attitude_error_q[2]; _target_ang_vel.z 2.0f * _p_angle_yaw.kP() * attitude_error_q[3];这里乘的2.0就是上面说的“姿态误差虚部与旋转角度”之间的近似关系。如果换用轴角表示法也能推出一套等价公式但四元数的最大优势是避开了欧拉角的各种奇异点而且在数值稳定性上更适合嵌入式环境。2.3 角速度内环力矩输出与油门前馈角速度控制是在AC_AttitudeControl::rate_controller_run里完成的。这里才是三个PID真正工作的地方。Copter默认用的PID控制器结构是P项对应刚度I项负责消除稳态误差比如重心偏移或螺旋桨拉力不一致造成的持续偏转D项提供阻尼另外还有一个前馈项把期望角速度的变化率直接前馈到输出用来补偿大机动时的滞后。姿态控制输出的是三个轴的力矩指令roll, pitch, yaw但真正发送给电调的是四个电机的油门值。这个映射关系在AC_AttitudeControl::get_rate_roll_pid之后通过output_motor_boost和电机混控矩阵完成。代码里你会看到经典的“X型四旋翼混控矩阵”不同机架类型X、、V型等对应不同的正负号组合。改机架类型时如果只改参数不改矩阵或者反过来飞机必然无法稳定飞行。我刚才提到“油门前馈”这在ArduPilot里是一个常被忽略的细节。大油门拉升时旋翼转速升高反扭矩也会变化这会让航向轴出现扰动。ArduPilot在throttle_feedforward里加了一个跟油门杆量相关的偏航前馈补偿这部分在代码里很容易被当作普通参数忽略但实际调悬停偏航振荡时非常关键。我自己的经验是如果四旋翼在快速垂直上升时偏航会自己转30度以上多半是这一步没做对。3. 位置控制AC_PosControl与Copter的惯性系修正3.1 位置环速度环以及加速前馈的三角关系如果姿态控制是整个飞控的“肌肉”那位置控制就是“小脑”。ArduPilot的位置控制核心代码在libraries/AC_AttitudeControl/AC_PosControl.cpp里这名字有些人会觉得奇怪——位置控制为啥放在姿态控制的库文件目录下面其实这是个历史遗留的组织方式逻辑上它和姿态控制是并列的。位置控制的核心思路是串级P控制器位置误差先乘一个比例系数生成期望速度期望速度再去跟当前速度做误差乘一个比例系数生成期望加速度。但ArduPilot在速度环里不是一个单纯的P它加了一个加速度前馈目标点移动时位置环直接把这个移动加速度前馈过去不需要等速度误差积出来再去追。这点在跟踪快速移动目标或者执行自动航线时特别明显没有前馈的话飞机会像在追自己的影子一样一直慢半拍。代码里的关键实现位于update_pos_controller你仔细看会读到类似这样的结构// 速度误差 期望速度 - 当前速度 Vector3F vel_error _target_vel - _pos.velocity; // 期望加速度 速度误差 * 速度环KP 加速度前馈 _target_accel vel_error * _p_vel_xy.kP() _accel_feedforward _vel_to_accel_ff;3.2 坐标系转换NEU与NED的恩怨位置控制里最容易翻车的是坐标系符号问题。ArduPilot把位置环放在惯性坐标系北东地NED中计算所以位置误差是大地坐标系里的南北和东西方向。但加速度要作用到飞机上需要先把惯性系下的期望加速度转到机体坐标系。这个转换用到了姿态矩阵DCM或者四元数旋转矩阵。在悬停时还好机体坐标系和惯性坐标系的夹角很小但在大风或者大坡度机动时重力补偿尤其重要。ArduPilot位置控制输出的总加速度向量里有一个部分是重力分量这个分量由姿态矩阵的第三列前馈进去保证即使机体倾斜推力方向能正确平衡重力。如果这个前馈做错了就会出现“飞机明明给了很大的滚转角但还是往一边飘”的怪现象。3.3 垂直方向高度控制其实是一个串级加前馈的混合系统垂直通道Z轴虽然也属于位置控制但ArduPilot单独给Z轴写了很多特殊逻辑。因为垂直方向受重力影响最大而且多旋翼的垂直推力响应和水平推力响应完全不对称。Z轴位置环的期望加速度在update_pos_controller之后还会经过一个重力补偿和油门曲线转换// 期望油门 垂直加速度期望 / 最大加速度斜率 悬停油门 重力补偿项 _throttle_in (accel_z_target GRAVITY_MSS) / _hover_throttle_scale _hover_throttle;这里那个_hover_throttle太关键了。它不是常数而是会随着电池电压、载重和空气密度变化。ArduPilot用了一个低通滤波的节流阀积分器throttle integrator来持续估计当前载重下的悬停油门。如果电量的电压变化剧烈或者挂载了云台和相机这个积分器能不能快速收敛直接决定了高度控制的品质。4. MAVLink航点信息注入的实际操作通过外部Dart程序下发航点给ArduPilot很多做地面站或者自主巡检二次开发的人都会遇到一个需求在电脑上跑一个Dart程序把一串航点发给飞控。这个流程看起来简单但里面有几个特别容易踩的坑。我先讲一个完整的实际操作流程再讲最关键的三个细节。首先是MAVLink航点的标准下发流程。ArduPilot支持两种航点消息格式老版MISSION_ITEM和带int的MISSION_ITEM_INT。现在官方强烈推荐用MISSION_ITEM_INT因为它里面的坐标是整数类型精度更高。整体的会话流程分四步发送MISSION_COUNT告诉飞控接下来要上传多少条航点。飞控会回一个MISSION_REQUEST里面带一个seq序号表示“把第0条发给我”。你根据序号回一个MISSION_ITEM_INT把航点类型、坐标、参数填进去。所有航点发完后飞控返回MISSION_ACK此时再发送MAV_CMD_NAV_GUIDED命令通过SET_MODE把飞控切到Guided模式飞机就会按刚才传的航点飞。在Dart里实现时很多人会直接按照mavlink库生成的类去发消息但有个细节经常被忽略MISSION_ITEM_INT里的frame字段一定要设置成MAV_FRAME_GLOBAL_RELATIVE_ALT_INT相对于起飞点高度如果你用成绝对高度飞机飞到海拔比你起飞点高几百米的地方就非常危险。第二个坑是消息发送时序。飞控一次只请求一条航点你要严格按照MISSION_REQUEST的序号回复。如果你用多线程或者异步队列一股脑把航点全发出去飞控会发现序号对不上直接返回MISSION_ERROR。我见过很多Dart写的二次开发demo就死在这里解决方法是写一个同步的请求-响应循环收到一条MISSION_REQUEST就回一条MISSION_ITEM_INT直到收到MISSION_ACK。第三个坑是坐标系方向。ArduPilot用的是NED坐标系高度是相对高度向上为正其实是NEU习惯但很多地面站程序习惯用海拔高度。发航点时如果你收到的是WGS84海拔一定要先换算成相对起飞点的高度。这个换算可以用飞控返回的HOME_POSITION里的alt作为基准。实际项目中我建议先写一个命令行工具打印飞控回传的航点确认信息和当前GPS坐标先在地面验证流程正确再上真机。直接上机的后果不是炸机就是飞丢不值得。5. 参数整定与经验调试从软件到真机的完整调参路径5.1 先认识参数组成员ArduPilot的姿态和位置控制参数都在几个固定的前缀下ATC_开头姿态控制器参数如ATC_RAT_RLL_P对应横滚角速度PID的P。ANGLE_开头姿态角度环参数如ANGLE_RLL_P、ANGLE_PIT_P。PSC_开头位置控制器的速度与加速度参数如PSC_VELXY_P、PSC_ACCZ_P。WPNAV_开头自动模式下的速度和加速度限制如WPNAV_SPEED、WPNAV_ACCEL。很多人会把姿态角P调得特别大希望能让飞机“反应快”。但ArduPilot这个级联结构里角度环P增益的上限受到角速度环带宽的限制。角度环P调太大角速度环跟不上就会形成“指挥乱、执行乱”的振荡。我的日常工作习惯是先用默认参数然后把ATC_RAT_RLL_P一点点往上加直到悬停时出现轻微的高频抖动再往回调20%。这个方法比看着公式算理论值接地气得多。5.2 实机调试动作清单一套完整的实机姿态调参流程我会按这个顺序做解锁后先低转速悬停观察飞机的漂移方向和漂移速度这个阶段不要碰任何参数只记录现象。在风很小的天气把飞机切换到Loiter模式如果是不带GPS的室内机就用Stabilize原地悬停30秒。用手轻轻推一下机头看它回正的速度和回正时是否有超调。如果超调超过两次说明阻尼不够把ATC_RAT_PIT_D加大。快速打满副翼杆让飞机横滚45度快速回中观察机翼回平过程中有没有“过头”现象。这个动作对新手来说一定要在足够高的高度做否则容易打地。位置控制的调参一般放在姿态调好之后。先在Loiter模式下悬停观察飞机是不是会绕着悬停点画圈。画圈就说明位置环阻尼不够或者GPS数据有延迟。这里有一个很容易被忽略的坑——Loiter模式下位置环依赖GPS速度而GPS速度更新频率只有5Hz到10Hz。ArduPilot内部有速度估计补偿机制但如果GPS的HDOP值偏大速度估计噪声就会放大位置环会莫名抖起来。这种情况下我宁可先把PSC_ACCZ_P之类的高频项降下来也不能单纯加D。5.3 常见问题速查表现象可能原因优先检查项悬停时机身持续往一个方向漂配平不准或者重心偏检查机架重心、水平校准、螺旋桨动平衡快速打杆后飞机振荡超过3次才稳定角速度环D过小ATC_RAT_RLL_D、ATC_RAT_PIT_D高空自稳没问题近地突然抖地面效应导致油门曲线变化减少试飞高度或调低悬停油门估计值Loiter模式下飞机画圈位置环P过大或GPS速度噪声大PSC_VELXY_P检查GPS精度自动航点飞行时高度先掉再补高度前馈与油门匹配不良WPNAV_SPEED_DN、WPNAV_ACCEL_Z偏航到目标角度后出现余振偏航角速度环D不足ATC_RAT_YAW_D降落时螺旋桨转速突然剧烈波动垂直位置环振荡耦合PSC_ACCZ_P、PSC_ACCZ_D6. 实操工具链与源码二次开发心得6.1 ArduPilot用到哪些管理工具真正折腾ArduPilot代码的人肯定离不开这几类工具。第一类是地面站软件Mission Planner是功能最全的Windows下老鸟用得最多适合调参和刷固件QGroundControl界面现代一点但有些ArduPilot特有的参数项展示不全更适合飞PX4的人。第二类是命令行工具MAVProxy这个我能吹半天因为它是个极简的Python命令行地面站不需要图形界面SSH到树莓派上就能用而且它的wp命令可以批量上传航点wp list wp clear wp upload 航点文件.waypoints第三类是日志分析工具ArduPilot的二进制日志用Mission Planner自带的Log Viewer可以看曲线图但我觉得对于代码解析来说更重要的是一套能快速解析.bin日志的库——我常用则是用Python的pymavlink配合pymavlink.dialects.v20.ardupilotmega来做离线分析在飞完一个架次之后批量检查姿态误差和角速度输出是否饱和。6.2 源码二次开发的几个落点如果你想在姿态控制里加入自己的想法我建议不要一上来就改主循环。ArduPilot的架构里给你预留了几个合理的插入点在AC_AttitudeControl类里加一个自定义的辅助控制器比如加一个主动抗风补偿或者相机增稳叠加项。在rate_controller_run的PID输出之后、混控矩阵之前插入一个力矩限幅或者轴间耦合补偿。在AC_PosControl::update_pos_controller之前注入自定义的位置前馈向量。这在做视觉引导降落时非常有用——视觉识别出来的目标偏移先转成期望加速度再注入位置环飞机会表现得很顺滑。我实际做视觉引导降落时就是在update_pos_controller之前加了一个set_pos_target_velocity调用把视觉解算出的速度直接叠到目标速度上。这样做的好处是避开了位置误差累积而且不需要破坏原有控制结构。代码改动量很小但效果立竿见影。这里有一个值得提的细节——ArduPilot的shrink_velocity函数会导致期望加速度被限制在一个半圆弧内这个限制是防止期望加速度跳跃过大。做视觉引导时如果视觉目标移动太快这个圆弧限速反而会成为瓶颈我当时的解法是把WPNAV_ACCEL临时调大让圆弧半径变大。这个思路在追船降落或者动态平台降落时很管用。6.3 反汇编代码解析在ArduPilot调试中的定位网络热词里出现“反汇编代码解析”这个说法我觉得得说清楚一个概念。ArduPilot默认的固件是你自己从源码编译出来的大多数情况下根本用不到反汇编。除非你在排查某些只有release版才出现的诡异bug比如编译器优化导致某段代码行为不一致这时候才需要objdump看汇编。我见过一个案例GCC在-O2优化下把某个浮点比较的NaN分支给优化掉了导致姿态误差在某几个特殊角度变成无穷大。这种问题不反汇编看根本找不到原因。所以我给二开开发者的建议是日常调参用源码和日志就够了了解反汇编工具存在即可但不要轻易把时间耗在汇编阅读上。7. 写在后面控制系统的“手感”要从代码和日志里同时建立姿态和位置控制这条链路表面上看起来是一堆PID、四元数和滤波器但真正把它调好之后你会发现它其实很像一个“手感活”。我见过有些人调参完全是碰运气参数越改越乱最后退回来重来。我的习惯是一天只改一组参数飞完一个架次导出日志用曲线看姿态响应有没有超调、角速度输出有没有饱和然后再决定下一步动哪里。最后还是分享一个我自己专用的调试小技巧在Mission Planner的Telemetry曲线里同时监控ATT.DesRoll、ATT.Roll和RCOU.Ch2或者对应的电机输出曲线。当飞机做定高航线时这三条曲线应该呈现出“目标角度先动、实际角度跟随、电机输出同步变化”的传递关系。如果目标角度还没变电机输出先剧烈变了那就是位置环输出里噪声太大检查一下加速度计滤波和GPS速度估计。如果目标角度变了但实际角度慢半拍那是姿态环带宽不够需要补角度环P。这两条经验比任何调参教程都实用。如果你正准备入坑ArduPilot的源码我的建议是别盯着某个控制算法看太久先从这条链路的角度去理解“位置环给姿态环下指令、姿态环给电机下指令、电机改变机体的力和力矩、机体反过来影响位置和姿态”这个闭环。把这环想通了你再看ArduPilot的代码很多函数名和注释一下子就会“有了灵魂”。