1. 控制链路全景位置环、速度环、姿态环如何层层收紧我早期读ArduPilot代码时有个很深的困惑明明飞控代码里到处是PID、limit、angle这些字眼但真要顺着一条控制链路追下去往往在一堆类之间跳来跳去最后被坐标变换绕晕。直到我把整个控制栈按“外环到内环”的顺序理清楚才发现ArduPilot的姿态和位置控制本质上就是一套标准的串级控制结构只是它用C类把这套结构拆得很细。ArduPilot以ArduCopter为例的控制器整体是四层嵌套位置控制层输入是期望位置与当前位置的误差输出是期望速度速度控制层输入是期望速度与当前速度的误差输出是期望加速度姿态控制层角度环输入是期望姿态角与当前姿态角的误差输出是期望角速度角速度控制层角速度环输入是期望角速度与当前角速度的误差输出是期望力矩最终换算成每个电机的转速指令。把这四层展开就是一套“位置误差→速度误差→倾角误差→角速度误差→力矩”的逐级传递链。外环每级只做一件事把当前级别的误差变成下一级的期望输入。这样设计的好处很明显每一层可以独立调参、独立限幅、独立应对传感器噪声不至于一个环节改动就牵连全局。1.1 位置控制与姿态控制的分工很多人以为“位置控制”就是GPS坐标一比对直接把电机转速改了。实际上ArduPilot把位置控制彻底限制在水平面内它只负责回答一个问题飞机当前离期望位置差多远应该以多大的水平速度飞过去。这个速度指令并不会直接作用到电机上而是先换算成期望的横滚角和俯仰角交给姿态控制器去执行。姿态控制器才是真正和“手感”相关的核心。它接收期望角度后先在角度环算出期望角速度再在角速度环算出扭矩指令。用一句话概括位置控制负责“去哪儿”姿态控制负责“怎么稳定地过去”。这种分工在代码里也分得非常清楚——AC_PosControl类管位置和速度AC_AttitudeControl类管角度和角速度二者之间通过“期望倾角”这个接口衔接。我在读代码时一度以为AC_PosControl会直接调用姿态控制类实际上它的输出只是给AC_AttitudeControl设置了三个期望角横滚、俯仰、偏航真正的姿态闭环是在另一个模块里完成的。这个解耦设计非常典型也值得做上层应用开发的同行参考模块间不要互相知道对方的内部实现只约定好“我给你的数据是什么、你给我的数据是什么”。1.2 控制代码的执行节奏ArduPilot的调度器Scheduler把不同任务按优先级和频率拆开执行。以ArduCopter为例姿态角速度环在1kHz的fast_loop里运行姿态角度环大约在400Hz左右运行而位置环和速度环的运行频率要低得多通常会落在10Hz到100Hz之间具体取决于飞行模式和目标平台。这个频率差异是有讲究的。角速度环直接对抗风扰和电机响应延迟必须跑得足够快才能稳住飞机位置环处理的是GPS或光流这类相对“慢”的传感器输出跑太快不仅没意义还会把噪声放大。频率分层意味着各层的参数天然解耦也是串级控制在工程上最优雅的一面。2. 姿态误差是怎么算出来的四元数旋转与轴角误差姿态控制的第一步是把“当前姿态”和“期望姿态”之间的差距算出来。这个看似简单的问题在工程实现里有一个容易踩的坑直接用欧拉角相减会带来大姿态角下的奇异性。这里必须说明ArduPilot的姿态控制器内部很少直接使用欧拉角做误差运算。它拿到的“当前姿态”是来自姿态解算模块的四元数或旋转矩阵期望姿态经过earth_to_body这类变换之后也转成四元数。两者之间的误差通过四元数乘法得到然后转换成“旋转向量”的形式来用。2.1 为什么不用欧拉角直接做减法举一个实际场景飞机在做大机动时俯仰角接近±90度欧拉角会出现方向锁Gimbal Lock问题。此时横滚和偏航的旋转方向变得部分重叠单纯用欧拉角相减算出来的误差会变得不连续姿态控制器会突然给出一个完全错误的方向指令。四元数没有这个问题因为它是四维单位向量天然在球面上连续旋转不存在某个“奇异点”。从代码实现上看ArduPilot的姿态解算部分AP_AHRS、AP_InertialNav输出的也是四元数而不是欧拉角。控制器内部拿到四元数后先计算误差四元数// 简化示意真实代码分散在多个helper函数中 Quaternion q_current ahrs.get_quaternion(); Quaternion q_error q_current.inverse() * q_target;这里的q_error就代表了“从当前姿态转到期望姿态所需经历的旋转”。注意顺序ArduPilot的旋转约定是“先转期望坐标系、再乘误差”如果顺序写反结果就是完全相反的旋转方向这是自己做姿态算法时常犯的错误。2.2 从旋转向量到轴角控制量有了误差四元数之后控制器一般把它拆成轴角形式旋转轴 旋转角度。ArduPilot的AC_AttitudeControl里会调用类似quat_to_axis_angle或直接提取向量部分的操作把四元数拆成三个旋转向量分量。这三个分量直接对应机体坐标系下的误差角乘上比例系数后就是期望角速度。这个过程的本质是误差四元数里的“虚部向量”在小角度下近似等于角度误差本身。小角度假设是姿态控制器能稳定工作的前提之一也是为什么姿态控制在悬停、慢速飞行时表现特别好、而特技模式要靠额外前馈和调参才能压住误差的原因。另外很多做二次开发的人会在这里犯迷糊为什么角度环输出不是直接的PWM而是一个角速度答案很简单——因为内环角速度控制器才是真正输出力矩的地方。外环如果直接输出力矩内环就没有存在的意义了抗风性也会差很多。2.3 期望姿态的来源期望姿态在ArduPilot里有几个来源理解它们的区别能帮你更快定位代码逻辑定高/悬停模式期望横滚俯仰角直接由摇杆位置映射偏航角是闭环Loiter/PosHold模式期望倾角来自位置控制器的输出偏航角可以闭环Auto模式期望航向来自航点路径规划横滚俯仰同样来自位置控制器。也就是说姿态控制器的输入并不总是“用户摇杆对应的角度”在很多自动化模式里它其实是位置控制链路的最终产物。沿着这个思路往下读代码就不会再被“为什么摇杆没动、角度却在变”这类问题搞晕。3. 串级姿态控制的逐行拆解从角度误差到电机指令姿态控制是ArduPilot里最值得精读的一段代码因为它包含了串级控制的全部精华外环P控制、内环PID控制、前馈、限幅、积分抗饱和。我最初读的时候在AC_AttitudeControl.cpp里看到一堆_pid_ang_*和_pid_rate_*变量一度以为每个轴用两套独立的PID后来才明白它们是串级关系。3.1 角度环一个推导出来的期望角速度角度环的本质是“姿态误差乘以一个比例系数得到期望角速度”。在ArduPilot的input_euler_angle_roll_pitch_yaw和attitude_loop路径里逻辑大致是计算当前姿态四元数与期望姿态四元数的误差把误差转成旋转向量得到三个轴的角度误差乘上角度环P增益如ATC_ANG_RLL_P得到期望角速度对期望角速度做限幅防止内环收到不可能完成或过于激进的目标。这一步的物理意义很直观误差越大就希望飞机转得越快。但角速度不能无限大否则电机饱和、姿态震荡。限幅值通常和机的最大倾斜角、最大角速度参数相关。值得注意ArduPilot的角度环并不只是P控制在部分版本里它会加入前馈项——把用户摇杆的角速度需求直接叠加到期望角速度里。这样做的好处是响应更快代价是更容易激发高频震荡。所以新版代码里前馈强度通常会伴随低通滤波器一起调。3.2 角速度环真正的PID输出角速度环在rate_controller_run中执行它做的事情是// 简化示意 float rate_error desired_rate - current_rate; float output pid.update(rate_error, dt);这里的pid是ArduPilot的AC_PID类包含P、I、D三项。P项提供主要控制力I项负责消除稳态误差D项提供阻尼以抑制超调。输出被解释成期望角加速度再乘以转动惯量矩阵得到期望力矩最终映射为电机转速差异。角速度环的采样频率一般是1kHz这意味着PID计算和滤波都在1kHz下运行。角速度信号来自陀螺仪通常经过一个低通滤波器如ATC_RAT_RLL_FLTT、ATC_RAT_RLL_FLTE来抑制高频噪声。D项对噪声非常敏感这就是为什么ArduPilot的D项往往需要配合滤波器系数一起调而不是单独调大。3.3 为什么输出前一定要限幅和抗饱和角速度环的PID输出不是随便给的它必须落在电机的物理能力范围内。ArduPilot在输出力矩之前会经过多个限幅环节角度环限幅、角速度环积分限幅、总力矩限幅以及“油门优先”逻辑。最关键的限幅是积分限幅。PID在误差较大时I项会快速累积如果误差持续存在积分项可能冲到几十倍于输出上限的数值等误差反转后积分项还要“吐”很久才能恢复正常这个现象叫积分饱和。ArduPilot的每个轴PID里都有_integrator以及对应的最大值限幅并且只在特定条件如电机未饱和、无人机处于可控状态下才允许积分累积。实际调参时我也踩过这个坑把ATC_RAT_RLL_I调大后飞机确实能压住稳态误差但风一吹就来回震荡而且幅度越来越大。后来发现是I项积得太多、饱和后“追尾”导致的。把积分限幅降下来再把ATC_RAT_RLL_FILT稍微调大一些问题立刻缓解。这段经历让我养成了一个习惯——调PID先看积分限幅再看比例和微分顺序不能反。3.4 前馈与角速度期望的协调ArduPilot在姿态控制里加入了前馈通道尤其是在偏航轴Yaw和摇杆操控上前馈的作用非常明显。它的逻辑是用户摇杆输入本身包含一个期望的转动速度不经过角度环直接叠加到角速度期望里。前馈强度由ATC_RATE_FF_ENABLE和相关参数控制。如果前馈太强飞机会显得“发贼”杆量稍微一动姿态快速响应但容易过冲如果太弱飞机会显得“肉”转向和倾转都慢半拍。这里没有万能参数我通常的做法是先关闭前馈把角度环和角速度环调稳再逐步增加前馈每次加0.05直到出现轻微高频抖动再回退0.02。这样的“后调前馈”顺序能有效避免同时调多个环节带来的“串扰”。新手容易上来就把所有参数往大了加结果飞机抖得像筛子然后根本不知道是哪个参数引起的。4. 位置控制的漫长链路从水平位移到期望倾角位置控制是很多做航线飞行、精准起降的开发者最关心的部分但它也是最容易被误解的一层。我在接触ArduPilot之前一直以为位置控制就是“位置误差乘比例系数得到电机指令”读源码才发现它实际上是一个独立于姿态控制的完整控制子系统而且还分成了水平通道和垂直通道。4.1 坐标系约定NED下的位置环位置控制器运行的坐标系是NED北东地也就是惯性系。当前位置来自EKF融合后的估计值期望位置来自航点或用户目标。误差计算方式很简单Vector3f pos_error desired_pos - current_pos;但这个误差值不能直接用来控制原因很简单一架四轴飞行器没法直接输一个“往北的力”它只能通过倾斜机身来产生水平加速度。所以位置误差必须先转换成期望速度再转换成期望加速度最终换成期望倾角。4.2 水平位置环从位置误差到期望速度水平位置环负责X和Y两个方向。ArduPilot的AC_PosControl::update_xy_controller会经历这些步骤用位置误差计算期望速度通常是个比例控制器也可加积分和微分对期望速度做限幅限制飞行器最大水平速度用速度误差计算期望加速度速度环通常是PID对期望加速度做水平和垂直方向的限幅把期望加速度换算成期望横滚角和俯仰角把换算后的期望角度交给姿态控制器。速度环的代价在于它需要可靠的线速度估计。在GPS信号良好的情况下EKF输出的水平速度比较干净但在室内或无GPS环境下只能靠光流或视觉里程计一旦速度估计有延迟位置环就会表现得很奇怪——飞机会在某个方向上来回“找位置”却始终稳不住。4.3 期望加速度怎么变成期望倾角这是位置控制里最值得理解的转换。四轴飞行器的水平加速度来源于机身倾斜后升力的水平分量。假设当前总拉力产生的加速度为a_total那么期望的水平加速度a_x和期望的俯仰角θ之间满足tan(θ) ≈ a_x / a_totalArduPilot内部并不会直接算这个三角关系它调用类似accel_to_lean_angles的函数把期望加速度转成横滚、俯仰角度。这个换算过程有三个限制条件倾角上限期望角度不能超过ANGLE_MAX否则飞机会倾斜到危险角度加速度上限水平加速度不能超过当前总加速度的地面投影范围否则会对高度造成明显扰动协调转弯限制偏航旋转时横滚角需要和向心加速度匹配以保证转弯不掉高。限制条件在代码里体现为limit_lean_angles和limit_vector两个函数。它们不仅要限制大小还负责向量方向投影——比如你同时期望向北加速、向东加速合成后的期望倾角方向是倾斜机身时所在的方向这个方向需要用四元数或旋转矩阵换算到机体坐标系。我一开始忽略了这些限幅只盯着P和I参数调位置控制总是不收敛。后来才发现问题不在参数而是期望加速度被限幅压得死死的位置误差再大也只能以最大速度飞行自然达不到“快速到位”的效果。理解了限幅逻辑之后你才会意识到位置控制的外环参数重要但限幅和路径规划策略同样重要。4.4 垂直方向高度控制是另一套逻辑垂直方向和水平方向在ArduPilot里是分开控制的它的控制链路是高度误差→期望垂直速度垂直速度误差→期望垂直加速度期望垂直加速度→期望总拉力油门再叠加姿态角变化带来的升力损失补偿。高度控制最麻烦的地方在于对油门基准的依赖。ArduPilot在悬停时需要知道“大约多少油门能稳住当前高度”这个基准由THR_HOVER参数或自动学习机制提供。如果这个基准偏差太大垂直速度环就要花很大力气去补偿表现出来就是飞机在定高模式下持续上下浮动。在代码里AC_PosControl::update_z_controller会从当前高度和垂直速度估计值出发计算期望垂直加速度再结合运动学方程推出期望油门。高度环使用的Z轴位置通常来自气压计、GPS海拔或者激光测距传感器EKF的融合质量直接决定了高度控制的稳定性。我自己试过在室内用超声波代替气压计高度控制的死区明显小了很多这再次说明了“传感器质量决定控制上限”这个道理。5. 一条航点任务在代码中的完整流转前面把每一层拆开讲了这一节把它们串起来看一个典型Auto模式下航点任务从“规划”到“电机转速”到底是怎么流转的。这也是我在源码阅读中最有收获的一步——不看整体流转单独读某个类永远是“只见树木不见森林”。5.1 航点导航生成期望位置航点任务从AC_WPNav类开始。它根据当前航点、下一个航点、飞机当前位置和预设的航线速度生成一个平滑的期望位置。这个期望位置不是跳变的而是沿着航线方向以一定速度移动并根据比例计算当前位置与期望位置之间的偏差把它输出给位置控制器。ArduPilot在这里引入了“路径跟踪”的概念而不是简单的“点对点飞行”。看AC_WPNav::update_wpnav的逻辑你会发现它一直在做向量投影和边界检查确保飞机能沿着两个航点之间的直线飞而不是像老式飞控那样“飞向目标点但可能绕远路”。// 简化示意 Vector3f target_vel wpnav-get_target_velocity(); Vector3f target_pos wpnav-get_target_position(); pos_control-set_pos_target(target_pos); pos_control-set_vel_target(target_vel);5.2 位置控制器输出期望姿态角AC_PosControl拿到期望位置和期望速度后经过前面说的位置环、速度环、限幅和倾角换算最终调用set_lean_angles之类的接口把期望横滚角和俯仰角写入姿态控制器。偏航角的目标则单独由航点任务或任务脚本指定。从这一刻开始位置控制的使命已经完成剩下的“稳住姿态”完全交给了AC_AttitudeControl。5.3 姿态控制器到电机指令AC_AttitudeControl在attitude_loop里计算期望角速度在rate_controller_run里计算期望力矩然后把力矩指令传给AP_Motors类。AP_Motors负责把三轴期望力矩和总拉力指令按电机的几何布局映射成各个电机的PWM值。到这里一条完整的控制链路就闭合了。这个小节的目的是提醒所有读代码的朋友如果你想给ArduPilot增加一个自定义控制器必须想清楚自己插在哪一层。如果想改飞行路径算法关注AC_WPNav如果想改位置响应特性关注AC_PosControl如果想改飞行手感或抗风性关注AC_AttitudeControl如果想适配新机型布局关注AP_Motors。不要试图在每一层都塞进自己的逻辑ArduPilot的分层结构已经很成熟正确做法是“在哪一层做你需要的事就只改那一层”。6. 参数调优与常见故障的排查思路代码读完还是要落地到实际调参。ArduPilot的调参网站有大量说明文档但我觉得最有价值的经验往往来自“飞机表现异常时你按什么顺序去定位问题”。这一节把姿态和位置控制相关参数整理成一张实用表再分享几个我踩过的坑。6.1 姿态控制相关参数速查ArduPilot的参数名很规律ATC_ANG_*是角度环ATC_RAT_*是角速度环:2后缀代表轴编号1为横滚2为俯仰4为偏航。常用参数整理如下参数名作用常见范围Copter备注ANGLE_MAX最大期望倾角3000~45000.01度限制位置控制最大倾角ATC_ANG_RLL_P / PIT_P角度环P增益4.0~8.0响应速度过大引起震荡ATC_RAT_RLL_P角速度环P增益0.1~0.3横滚主控强度ATC_RAT_RLL_I角速度环I增益0.1~0.5消除稳态误差ATC_RAT_RLL_D角速度环D增益0.001~0.01阻尼过大会放大噪声ATC_RAT_RLL_FLTT / FLTE角速度D项滤波器频率10~80Hz越低越平滑但延迟越大ATC_THR_MIX_MIN / MAN油门与姿态混控权重0.1~0.5大油门时保持姿态可控性调参顺序永远是先保证姿态角速度环稳定再调角度环最后调位置环。你要是上来就调PSC_VELXY_P飞机可能连基本的稳定姿态都做不到后面的位置控制自然无从谈起。6.2 位置控制相关参数速查位置控制参数集中在PSC_前缀下这里列出常用的几个参数名作用常见范围备注PSC_VELXY_P水平速度环P1.0~4.0位置控制主增益PSC_VELXY_I水平速度环I0.1~1.0消除稳态侧风漂移PSC_VELXY_D水平速度环D0.0~0.5抑制速度震荡容易引入噪声PSC_ACCZ_P垂直加速度环P0.2~0.8高度控制主增益PSC_ACCZ_I垂直加速度环I0.1~0.5高度稳态误差PSC_VELZ_P垂直速度环P2.0~8.0高度响应速度PSC_ACCZ_FLTT / FLTE加速度噪声滤波5~30Hz对高度噪声敏感时使用如果飞机在Loiter模式下来回晃大概率是PSC_VELXY_P偏大如果飞机在某个方向上持续漂移先怀疑速度环积分强度再怀疑GPS定向是否正确如果飞机在定高模式下高度缓慢下滑优先排查油门悬停基线而不是一顿乱调PSC_ACCZ。6.3 三个典型故障的排查记录我在实际使用ArduPilot的过程中遇到过三个非常有代表性的问题排查过程比最终结果更有价值。第一个是“姿态高频抖动”。现象悬停时机身以大约20Hz的频率轻微发抖声音发闷。我先调低了角速度环D增益抖动从20Hz降到10Hz左右说明D项是主要来源但P增益仍然偏高继续调低P增益后抖动消失。结论是高频抖动优先查角速度环P和D不要动角度环和位置环。第二个是“Loiter模式下来回追位置”。现象飞机每次到达目标点附近都会冲过头然后反向再冲形成明显的波浪轨迹。一开始我把PSC_VELXY_P调低结果航线变“肉”了但追位置现象依旧。后来发现是速度环积分项累积过快在误差减小时积分还在推着飞机往前冲。降低PSC_VELXY_I并把速度误差死区调大后问题解决。这个案例说明串联控制里的“慢环节”出问题经常被误认为是“快环节”参数不对定位时要看现象变化趋势而不是只调参数数量。第三个是“大机动后掉高明显”。现象飞完一个大半径转弯后飞机高度掉了快1米。我把高度环参数调了一遍都没用最后才发现是期望倾角限幅过大转弯时总拉力水平分量占用太多垂直升力不足而高度环的反应速度跟不上。解决方法是把最大倾角从4500降到3500同时打开“转弯时自动补偿油门”的相关选项低版本ArduPilot通过ATC_THR_MIX_MAN参数影响这一行为新版用ANGLE_MAX和混控权重配合控制。6.4 关于日志判读的一个小建议ArduPilot的.bin日志里包含RATE、ATT、CTUN、NTUN等组数据很多人打开日志后不知道从哪里看起。我的经验是先看阶跃响应再看悬停稳定性。给飞机一个固定的期望角度变化观察角度曲线是否快速地、无超调地到达目标然后悬停一段时间观察角度误差的均值和方差。这样可以快速判断“姿态层是否过关”再往下判断位置层才有意义。最近我在读ArduPilot 4.4版本代码时发现姿态控制的代码结构已经做了不少重构很多函数被拆成了更细的helper方法但整体分层思想和参数体系并没有本质变化。如果你正在读的版本与我文中的函数名有出入不妨先找到AC_AttitudeControl和AC_PosControl这两个核心类再顺着update_*和rate_controller_run这些约定俗成的函数名往下追架构上的认识不会过时。