企业官网建设流程全解析

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，因为它里面的坐标是整数类型，精度更高。整体的会话流程分四步：

1. 发送MISSION_COUNT，告诉飞控接下来要上传多少条航点。
2. 飞控会回一个MISSION_REQUEST，里面带一个seq序号，表示“把第0条发给我”。
3. 你根据序号回一个MISSION_ITEM_INT，把航点类型、坐标、参数填进去。
4. 所有航点发完后，飞控返回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 实机调试动作清单

一套完整的实机姿态调参流程，我会按这个顺序做：

1. 解锁后先低转速悬停，观察飞机的漂移方向和漂移速度，这个阶段不要碰任何参数，只记录现象。
2. 在风很小的天气，把飞机切换到Loiter模式（如果是不带GPS的室内机，就用Stabilize），原地悬停30秒。
3. 用手轻轻推一下机头，看它回正的速度和回正时是否有超调。如果超调超过两次，说明阻尼不够，把ATC_RAT_PIT_D加大。
4. 快速打满副翼杆让飞机横滚45度，快速回中，观察机翼回平过程中有没有“过头”现象。这个动作对新手来说一定要在足够高的高度做，否则容易打地。

位置控制的调参一般放在姿态调好之后。先在Loiter模式下悬停，观察飞机是不是会绕着悬停点画圈。画圈就说明位置环阻尼不够，或者GPS数据有延迟。这里有一个很容易被忽略的坑——Loiter模式下位置环依赖GPS速度，而GPS速度更新频率只有5Hz到10Hz。ArduPilot内部有速度估计补偿机制，但如果GPS的HDOP值偏大，速度估计噪声就会放大，位置环会莫名抖起来。这种情况下我宁可先把PSC_ACCZ_P之类的高频项降下来，也不能单纯加D。

5.3 常见问题速查表

6. 实操工具链与源码二次开发心得

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的代码，很多函数名和注释一下子就会“有了灵魂”。