01｜架构与坐标：一条指令怎样变成运动

四层闭环，而不是一个大 PID

导航估计回答“现在在哪里、怎样运动”；制导回答“下一步往哪走”；控制回答“需要多大的力和力矩”；分配回答“每台推进器出多少力”。位置参考的单位是 m，速度参考是 m/s，广义力的平移分量是 N、转动分量是 N·m。归一化摇杆或 PWM 又是执行接口，不能与真实推力直接画等号。

对 ArduSub 类架构，先按这些职责理解飞控、伴随计算机、传感器驱动和任务进程：高层任务可以较慢，姿态和速度反馈要及时，驱动负责把字节变成带坐标与时间的测量。具体部署方式可变化，但物理量之间的关系不变。

旋转不是换变量名

采用导航系 n=NED（北、东、下），机体系 b=FRD（前、右、下）。R 把机体系向量旋转到导航系，因此逆变换是 Rᵀ。位置积分使用导航系速度；推进器分配通常在机体系工作。

$${\text{ṗ}}^{\text{n}}\text{= R}{\text{v}}^{\text{b}}\text{，}{\text{v}}^{\text{b}}\text{=}{\text{R}}^{\text{T}}{\text{v}}^{\text{n}}\text{，}{\text{R}}^{\text{T}}\text{R = I}$$

只看水平面，R 的前两维是 [[cosψ,−sinψ],[sinψ,cosψ]]。当 ψ=90°，机体正前方指向东。希望向北以 0.2 m/s 运动时，机体系速度应为 [0,−0.2]，也就是向左侧移，而不是前进。

ENU 变 NED 需要交换前两个轴并反转第三轴：T=[[0,1,0],[1,0,0],[0,0,−1]]。协方差同样要转：P′=TPTᵀ，因为不确定度椭球也属于坐标系。

杆臂：转动会产生线速度

传感器不在机体参考原点时，其读数包含旋转造成的运动。令 r 为原点指向传感器的机体系位置，角速度为 ω，则传感器点速度为：

$${{\text{v}}_{\text{sensor}}}^{\text{b}}\text{=}{{\text{v}}_{\text{origin}}}^{\text{b}}\text{+}{\text{ω}}^{\text{b}}\text{×}{\text{r}}^{\text{b}}$$

算例：DVL 在原点前方 0.2 m，机器人以 0.5 rad/s 正偏航。即使原点不平移，DVL 点也有右向 0.1 m/s 的速度。把它误当原点速度，会让控制器在原地转艏时莫名侧移。杆臂项不是滤波噪声，而是确定的刚体运动学。

实现顺序与小实验

先写旋转与逆旋转，再写叉乘杆臂，再加入安装旋转。用航向 0°、90°、180° 的三个手算例检查方向，最后随机生成姿态做往返变换。之后模拟“原地转艏、偏心 DVL”，分别补偿与不补偿杆臂，观察虚假位置漂移。完整三维姿态用四元数或旋转矩阵传播，欧拉角留给人阅读；陀螺测得的 p、q、r 一般不等于欧拉角的逐项导数。