源码主线 03｜从厘米位置误差到八推进器命令

控制链最容易被算法名字遮住的，是单位和中间量。项目的 PosHold 不是“位置误差直接乘一个PID”：它先生成速度目标，再生成加速度目标，随后做姿态旋转、水平效能补偿和多轴混控。每走过一个函数，都应能说清输入是什么、输出是什么。

先把接口单位摊开

边界	坐标／单位	代码定位
EKF内部导航量	导航位置m、速度m/s	AP_NavEKF3
惯导反馈给位置控制	NEU，cm、cm/s	AP_InertialNav_NavEKF::update
位置／速度串级	位置差→速度目标→加速度目标	update_xy_controller_poshold_impl
执行器效能映射	机体系加速度→归一化forward/lateral	accel_target_to_xy_thrust
推进器输出	混控、限幅、死区、硬件报文	AP_Motors6DOF

NEU与NED只在垂直正方向不同；m变cm还要乘100。若EKF位置NED为(1.2,−0.4,2.0)m，则反馈NEU为(120,−40,−200)cm。目标向上不能在这些接口中一律使用相同正号。量纲错误常产生100倍增益，坐标错误则会把负反馈变成正反馈。

位置 P 与速度 PID 各负责什么

$$\text{v* = K\_p^p(p*−p)+v\_ff； a* = PID\_v(v*−v)+K\_ff^v v*+a\_ff}$$

位置P把“还有多远”变成“应以多快接近”；速度PID把“速度差多少”变成加速度需求。速度前馈让移动目标不必先产生位置误差才开始运动，加速度前馈则直接告诉内环参考正在怎样变化。AC_PID_Basic::update_all包含误差低通、导数低通、条件积分和前馈。项目给出的速度环默认P=3、I=0.3、D=0、FF=0.1、IMAX=30，应连同输入单位理解。

教学算例：北向差20 cm，外环增益取0.5 s⁻¹、前馈速度5 cm/s，则速度目标15 cm/s；当前8 cm/s，速度误差7 cm/s。只取P=3 s⁻¹这一项，加速度需求为21 cm/s²。这里外环0.5是演示值，目的在于逐层算单位，而非推荐实艇调参。

2×2矩阵为什么比两个独立比例系数更有用

$$\text{a\_b = R\_bnᵀ[a\_N,a\_E,0]ᵀ； u\_xy = M⁻¹[a\_bx,a\_by]ᵀ}$$

$$\text{M =}\left[\begin{array}{cc}\text{5.33}& \text{−0.83}\\ \text{1.39}& \text{3.21}\end{array}\right]\text{； det(M) = 18.263}$$

M近似表达归一化水平命令到机体水平加速度的映射。第一列告诉你只给forward时，前向与侧向分别怎样响应；第二列同理。非对角元素意味着侧推会串到前向，前推也会串到侧向。补偿耦合应通过矩阵求逆，而不是单独把两个轴调得更硬。进入这个物理映射前，将cm/s²换成m/s²。

$${\text{M}}^{\text{−1}}\text{=}\frac{\text{1}}{\text{18.263}}\left[\begin{array}{cc}\text{3.21}& \text{0.83}\\ \text{−1.39}& \text{5.33}\end{array}\right]$$

希望机体系加速度为(1.0,0.5)m/s²，计算得到forward≈0.1985、lateral≈0.0698。代回：5.33×0.1985−0.83×0.0698≈1.0，1.39×0.1985+3.21×0.0698≈0.5。若错误地分别除对角项，便会留下交叉轴误差。矩阵可以用一组低幅命令与测得加速度做最小二乘辨识；需要同时激励两个轴才能区分四个系数。

姿态旋转、局部限幅与最终混控

航向90°且水平时，北向加速度会转换为机体左向，而不是前向。完整姿态旋转还会产生body-Z分量。水平映射取body-X/Y，深度通道也有自己的三维投影，因此“完整R”和“完整六轴分配”不能画等号；大姿态下要看各通道共同能提供的力。

公共缩放保持方向：若逆映射得(1.2,0.6)，同除1.2得到(1.0,0.5)，比逐项裁成(1.0,0.6)更能保留需求方向。局部可达加速度可用于限制积分增长；随后姿态、深度、偏航还会争用同一批推进器，最终饱和是另一层约束。抗饱和真正需要知道的是“要求了多少”和“实际能实现多少”的差。

AP_Motors6DOF的前四台承担水平／偏航混控，后四台承担垂直／横滚／俯仰组合；随后有归一化、反向、裁剪、PWM映射与非零死区补偿，项目CAN输出使用0x181。混控系数是控制接口约定，物理力分配τ=Bf还要用推进器方向、力臂和推力曲线来建立。

为什么换模式不能只复制一组增益

PosHold消费位置与速度反馈做水平串级；GUIDED非零XY目标走另一套速度闭环与增益调整；CIRCLE的水平输入直接形成归一化平移命令。相同数字0.2，在速度接口里可表示0.2 m/s，在归一化接口里则是20%命令量。读模式实现的第一步不是找PID，而是确定它控制的物理量。

练习：把上面的矩阵算例分别放到航向0°和90°，计算相同北向任务需要的机体系命令；再逐渐增加需求直到公共缩放发生，计算M u_actual与原目标之差。这份残差就是把“执行器能力”反馈到控制器和上层制导的起点。