01.2｜从牛顿—欧拉到海洋六自由度：每一项从哪里来

这一课的输入是姿态、机体对地速度、海流和推进器合力；输出是机体速度导数与导航位置导数。它解释为什么水下机器人不能只写“力等于质量乘加速度”，也解释为什么高阶控制器之前必须先辨清惯性、阻尼和恢复力。先建立无流刚体模型，再把水作用力搬到等式左边，最后单独处理海流；这样每一步都有清楚的物理来源。

符号	含义与维度	单位／坐标约定
$\eta=[(p^n)^T,\phi,\theta,\psi]^T$	$6\times1$ 位姿	前3项m，后3项rad；位置 $n$
$\nu=[(v^b)^T,(\omega^b)^T]^T$	$6\times1$ 对地广义速度	$\mathrm{m/s}$ 与 $\mathrm{rad/s}$ ； $b$
$m,I_G,r_G,r_B$	质量、重心惯量、重心和浮心杆臂	kg、 $\mathrm{kg\,m^2}$ 、m、m；杆臂均 $b$
$M_{RB},M_A,C,D,g(\eta)$	刚体／附加惯性、科氏、阻尼、恢复广义力	$6\times6$ 或 $6\times1$ ；平移行N，转动行N·m
$W,\mathcal B,\tau$	重量、浮力大小、外部控制广义力	N、N、[N,N,N,N·m,N·m,N·m]
$v_c^n,\nu_r$	导航流速、机体相对水速度	$\mathrm{m/s}$ ；流速 $n$ ，相对广义速度 $b$

第一步：为什么机体方程需要叉乘项

任一向量在惯性系中的导数，等于其机体系分量导数加上坐标轴旋转带来的角速度叉乘。先把参考点放在重心，质量恒定、机体刚性、惯性张量在 $b$ 中恒定。对线动量和角动量分别应用这个输运定理，就得到牛顿和欧拉方程。特别注意： $v$ 上面的点是“机体分量的导数”，并不单独等于导航加速度旋转回来。

\begin{aligned}\frac{d}{dt}(R_b^na^b)&=R_b^n(\dot a^b+\omega^b\times a^b),\\F^b&=m\dot v^b+m\omega^b\times v^b,\\N_G^b&=I_G\dot\omega^b+\omega^b\times(I_G\omega^b).\end{aligned}

(1.8)

式 $(1.8)$ 先区分“分量变了”与“向量变了”

$a^b$ 是任意物理向量的机体分量， $\dot a^b$ 表示这些数字的变化；乘积求导的另一项 $\dot R a^b$ 描述机体基轴自身在转动。把 $\dot R=R[\omega^b]_\times$ 代入后，才能将共同的 $R$ 提出来。用于线动量时取 $a^b=mv^b$ ，用于角动量时取 $a^b=I_G\omega^b$ 。 $F^b$ 是合外力， $N_G^b$ 是关于重心的合外力矩；二者不能在参考点不同的情况下混用。

例如机体前速 $u=1\ \mathrm{m/s}$ 、偏航角速度 $r_r=0.2\ \mathrm{rad/s}$ ，若机体速度分量恒定， $\dot v^b=0$ ，导航中却有侧向加速度 $0.2\ \mathrm{m/s^2}$ 。质量二十千克时必须有侧向合力四牛顿，正是 $m\omega\times v$ 。这解释了为什么匀速转弯仍需要力，不能把“机体速度读数不变”当成“没有加速度”。

若机体系原点 $O$ 不在重心，重心速度是 $O$ 点速度加角速度叉乘重心杆臂。把它代入动能，展开平方项：平移平方给左上块，交叉项给两个互为转置的非对角块，杆臂平方给平行轴项。这比逐项记住六行方程更可靠。角度以弧度计，矩阵不同块的单位不同，不能笼统把整个六阶矩阵都叫“kg”。

\begin{aligned}v_G^b&=v^b-[r_G^b]_\times\omega^b,\\\mathcal T&=\tfrac12m(v_G^b)^Tv_G^b+\tfrac12(\omega^b)^TI_G\omega^b=\tfrac12\nu^TM_{RB}\nu,\\I_O&=I_G-m[r_G^b]_\times^2,\\M_{RB}&=\begin{bmatrix}mI_3&-m[r_G^b]_\times\\m[r_G^b]_\times&I_O\end{bmatrix}.\end{aligned}

(1.9)

从式 $(1.9)$ 的平方项读出每一块惯性

记 $S_G=[r_G^b]_\times$ ，则 $v_G=v-S_G\omega$ 。展开平移动能得 $\tfrac12mv^Tv-mv^TS_G\omega+\tfrac12m\omega^TS_G^TS_G\omega$ 。因为 $S_G^T=-S_G$ ，最后一项变成 $-\tfrac12m\omega^TS_G^2\omega$ 。加上重心转动动能，右下块就是 $I_G-mS_G^2$ ；中间交叉项必须由两个对称块共同产生，所以左下块等于右上块转置。

若 $r_G=(0.1,0,0)^T\ \mathrm m$ 、质量二十千克，绕右轴和下轴的惯量各增加 $0.2\ \mathrm{kg\,m^2}$ ，绕前轴不变。若取 $v=(0,0.1,0)^T$ 、 $\omega=(0,0,1)^T$ ，重心侧速为 $0.2\ \mathrm{m/s}$ ，不能只把原点动能与绕重心转动动能简单相加而漏掉交叉项。代码应先固定惯性参考点，再组装矩阵。

这里惯性矩阵四块的单位分别为kg、kg·m、kg·m、 $\mathrm{kg\,m^2}$ 。将广义动量分成线动量 $a$ 和角动量 $b$ ，可构造一个通用反对称科氏矩阵。其上半部乘速度给角速度叉线动量，下半部给线速度叉线动量加角速度叉角动量。取重心为原点时，线速度叉自身的项为零，就退回刚才的牛顿—欧拉方程。MSS的m2c采用这条动量构造路线。TB01-S2

\begin{aligned}\begin{bmatrix}a\\b\end{bmatrix}&=M\nu,\qquad C_M(\nu)=\begin{bmatrix}0&-[a]_\times\\-[a]_\times&-[b]_\times\end{bmatrix},\\C_M(\nu)\nu&=\begin{bmatrix}\omega^b\times a\\v^b\times a+\omega^b\times b\end{bmatrix},\\C_M^T&=-C_M\quad\Longrightarrow\quad \nu^TC_M\nu=0.\end{aligned}

(1.10)

式 $(1.10)$ 为何是科氏项，而不是额外阻力

这里暂用 $a$ 、 $b$ 表示广义动量的前三项和后三项，分别是线动量与关于所选参考点的角动量，不是加速度和坐标标签。矩阵第一行块乘法得到 $-[a]_\times\omega=\omega\times a$ ，第二行块乘法得到 $-[a]_\times v-[b]_\times\omega=v\times a+\omega\times b$ 。后两种写法相等是叉乘反交换律，不是近似。

对于重心原点、对角惯量 $(I_x,I_y,I_z)$ ，滚转科氏项就是 $(I_z-I_y)q_rr_r$ 。若惯量依次为 $(1,2,3)\ \mathrm{kg\,m^2}$ ， $q_r=0.2$ 、 $r_r=0.3\ \mathrm{rad/s}$ ，该项为 $0.06\ \mathrm{N\,m}$ 。它把旋转能量在轴间重新分配，但总功率为零。数值仿真可逐步记录 $\nu^TC\nu$ ，它应只剩浮点舍入量级；这比单看某条轨迹更直接地检验叉乘符号。

第二步：把水的惯性、耗散与浮力加入

加速艇体也要加速周围流体，因此流体反作用包含与加速度相反的项。把它移到左边，就得到附加惯性。附加质量不是凭空增加机器人称重质量，而是流场动能对广义速度的二次型系数；它也有平移—转动耦合。阻尼则把机械能耗散到尾流与黏性摩擦，低速常用线性项，较高速加入速度绝对值乘速度的二次项。以下是对角教学模型，复杂艇体可以使用对称的耦合模型。

\begin{aligned}\tau_{hydro}&=-M_A\dot\nu-C_A(\nu)\nu-D(\nu)\nu,\\M&=M_{RB}+M_A,\quad C=C_{RB}+C_A,\\D(\nu)&=\operatorname{diag}(d_{1i}+d_{2i}|\nu_i|),\quad d_{1i},d_{2i}\geq0,\\M\dot\nu+C(\nu)\nu+D(\nu)\nu+g(\eta)&=\tau,\\\dot\eta&=\begin{bmatrix}R_b^n&0\\0&T(\phi,\theta)\end{bmatrix}\nu.\end{aligned}

(1.11)

式 $(1.11)$ 是一组方程，不是把五种量相加的缩写

第一行先给水对艇体的反作用，移到牛顿方程左边时所有负号才变成正号。 $M_A$ 是附加惯性， $C_A$ 是与它配套的动量输运，不能只增加一个“质量”而仍保留旧的科氏项。 $D\nu$ 是阻力的反向量，物理阻力本身是 $-D\nu$ 。 $g$ 同样是被移到左边的静水外力，而不是重力加速度。

最后一行回答的是“速度怎样改变坐标”，不是“力怎样改变速度”。 $J=\operatorname{diag}(R,T)$ 的左上块是正交旋转，右下块是非正交的欧拉速率变换。位置以米、角度以弧度表示，因而 $J$ 混合了两种运动学关系。实现中宜分别更新位置与四元数，不必把欧拉角接近奇异点的 $T$ 硬塞进积分器。动力学输出 $\dot\nu$ 与运动学输出 $\dot\eta$ 都应使用同一时刻状态。

直线平移轴的线性阻尼系数单位是 $\mathrm{N\,s/m}$ ，二次系数是 $\mathrm{N\,s^2/m^2}$ ；旋转轴则是 $\mathrm{N\,m\,s/rad}$ 与 $\mathrm{N\,m\,s^2/rad^2}$ 。恢复项先从真正的外力算起：重量在NED中向下，浮力向上，二者分别作用于重心和浮心。由于将它们搬到了方程左边， $g$ 必须取这些广义外力的负号。这个几何写法对任意姿态有效，也能避免背展开公式时弄错符号。TB01-S3

\begin{aligned}e_3&=[0,0,1]^T,\quad f_W^b=(R_b^n)^TW e_3,\quad f_B^b=-(R_b^n)^T\mathcal B e_3,\\g(\eta)&=-\begin{bmatrix}f_W^b+f_B^b\\r_G^b\times f_W^b+r_B^b\times f_B^b\end{bmatrix}.\end{aligned}

(1.12)

式 $(1.12)$ 怎样展开出恢复力，而不丢失正负号

$e_3$ 是导航向下单位向量， $W=mg_0$ 、 $\mathcal B=\rho g_0\nabla$ 分别是重量和浮力的正大小；这里 $g_0$ 是重力加速度， $\nabla$ 是排水体积，均不要与恢复向量 $g(\eta)$ 混淆。将两个力各自转回机体，分别与重心、浮心杆臂叉乘，再求和。最后外层负号来自移项。若把所有力都假设作用在原点，就会错误地消掉静态姿态稳定性。

水平中性浮力时，平移恢复项为零；浮心在重心上方时，横滚和俯仰仍具有回复力矩，偏航通常没有。这说明“六自由度全驱动”并不意味着六个方向都有相同的自然稳定性。深度变化若导致体积或水密度变化，应让浮力大小随环境改变；当前常参数模型则把它们固定在工作深度附近。

手算恢复力：水平时 $W=100\ \mathrm N$ 、浮力102 N，净外力向上2 N，因而 $g$ 的第三项为正2 N。若又令 $O$ 在重心、浮心位于上方0.05 m，横滚10度，则浮力矩绕前轴约为−0.886 N·m， $g$ 的横滚项为正0.886 N·m，确实把正横滚拉回水平。重量与浮力相等只消除了净竖直力；浮心和重心分离仍可以产生恢复力矩。

第三步：海流只对水动力使用相对速度

假设海流在局部导航系中均匀、恒定且无旋。机体转弯时，即使导航流速不变，机体坐标中的流速分量仍会变化。对旋转后的流速求导，就得到负角速度叉流速。刚体惯性仍与对地加速有关，附加惯性和阻尼则与相对水流有关。先保留下面这条一般结构，不要不加条件把方程中每一个 $\nu$ 都替换成相对速度。

\begin{aligned}v_c^b&=(R_b^n)^Tv_c^n,\quad\nu_c=[(v_c^b)^T,0,0,0]^T,\quad\nu_r=\nu-\nu_c,\\\dot v_c^b&=-[\omega^b]_\times v_c^b,\quad\dot\nu_r=\dot\nu-\dot\nu_c,\\M_{RB}\dot\nu+C_{RB}(\nu)\nu+M_A\dot\nu_r+C_A(\nu_r)\nu_r+D(\nu_r)\nu_r+g&=\tau,\\M\dot\nu&=\tau-C_{RB}(\nu)\nu-C_A(\nu_r)\nu_r-D(\nu_r)\nu_r-g+M_A\dot\nu_c.\end{aligned}

(1.13)

式 $(1.13)$ 每一次替换对应哪个物理对象

$\nu_c$ 的后三项为零，表达无旋流本身没有角速度；这并不意味着艇体角速度为零。由 $\nu_r=\nu-\nu_c$ 求导，得到加速度差；把这一差代入附加惯性项后，未知的 $\dot\nu$ 分别被 $M_{RB}$ 与 $M_A$ 乘，合起来成为 $M$ 。右端的 $+M_A\dot\nu_c$ 不是经验扰动补偿，而是严格移项所得。

设机头向北、海流在导航北向为 $0.3\ \mathrm{m/s}$ ，艇体以 $0.2\ \mathrm{rad/s}$ 正偏航，瞬时机体流速导数是 $(0,-0.06,0)^T\ \mathrm{m/s^2}$ 。下一拍船首向右转，固定的北向海流在机体看来转向左侧，符合这个负号。代码若每拍重新用姿态计算流速，不一定要积分这条导数，但计算相对加速度时仍必须包含它。

最后一行只是代入相对加速度并收集未知的对地加速度，所以适合直接实现。以绝对 $\nu$ 计算运动学，以 $\nu_r$ 计算水动力，就能模拟“对地悬停但仍需顶流”。例如前向流0.3 $\mathrm{m/s}$ 、艇体对地速度零，线性阻尼8 $\mathrm{N\,s/m}$ 、二次阻尼12 $\mathrm{N\,s^2/m^2}$ ，对应 $[D(\nu_r)\nu_r]_1=-3.48\ \mathrm N$ ；稳态控制力必须为−3.48 N，向后顶住前向水流。水流沿正方向推艇，所以维持位置需要反方向推力。

能量检验、最小积分器与代码边界

先看无海流情形。动能对时间求导时，恒定对称 $M$ 使两项合并；科氏项因反对称性不做功。恢复力若来自势能，其功率可以与势能导数合并。因此无控制输入时，总能量只会被阻尼消耗。这个性质是检查符号的工具：如果自由衰减仿真越跑越快，先查阻尼号和积分步长，而不是立刻调整PID。

\begin{aligned}\dot{\mathcal T}&=\nu^TM\dot\nu=\nu^T\tau-\nu^TD\nu-\nu^Tg,\\\dot U&=\nu^Tg,\qquad\frac{d}{dt}(\mathcal T+U)=\nu^T\tau-\nu^TD\nu.\end{aligned}

(1.14)

式 $(1.14)$ 到底证明了什么

$\mathcal T$ 是动能、 $U$ 是势能，单位都为焦耳； $\nu^T\tau$ 是执行器对机器人做功的功率，平移部分是力乘速度，旋转部分是力矩乘角速度。 $\nu^TD\nu$ 非负，说明阻尼消耗能量。证明中去掉科氏项依赖 $C$ 的零功率结构，合并惯性导数依赖 $M$ 常值且对称，不能对一个随意拟合、非对称的矩阵直接套结论。

当无输入时，结论是总机械能不增加，不自动等于所有位置都收敛到原点。水平平移和偏航可能没有恢复势能，最终静止的位置由初始条件决定。若有持续海流，流体可向艇体输入能量，不能仍把无流被动性式当成对地能量一定下降的证明。控制设计中的耗散项和扰动输入应在选定的速度变量下重新配对。

参考实现：C++17，先独立手写再展开

// 独立教学实现：动量构造验证 + 单轴水动力积分，C++17
#include <array>
#include <cmath>
#include <cassert>
#include <iostream>
using V=std::array<double,3>;
V cross(V a,V b){return {a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0]};}
double dot(V a,V b){return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];}
V add(V a,V b){for(int i=0;i<3;++i)a[i]+=b[i];return a;}
V scale(V a,double k){for(double &x:a)x*=k;return a;}
int main(){
    double m=20; V rg{.1,0,0},v{1,0,0},w{0,0,.2};
    V IG{1,2,3}; // 重心对角惯量kg m^2
    V a=scale(add(v,cross(w,rg)),m); // 线动量
    V Iw{IG[0]*w[0],IG[1]*w[1],IG[2]*w[2]};
    Iw=add(Iw,scale(cross(rg,cross(rg,w)),-m)); // I_O * omega
    V b=add(scale(cross(rg,v),m),Iw);
    V CF=cross(w,a), CN=add(cross(v,a),cross(w,b));
    assert(std::abs(CF[0]+.08)<1e-12 && std::abs(CF[1]-4)<1e-12);
    assert(std::abs(dot(v,CF)+dot(w,CN))<1e-12);
    // 静水前向加速：有效质量30kg，阻尼8和12，控制力10N
    auto drag=[](double u){return 8*u+12*std::abs(u)*u;};
    double u=.5,dt=.01,acc=(10-drag(u))/30;
    assert(std::abs(acc-.1)<1e-12);
    assert(std::abs(u+dt*acc-.501)<1e-12);
    // 无输入自由衰减；每一拍能量不增加
    for(int k=0;k<1000;++k){
        double E=.5*30*u*u;
        u+=dt*(-drag(u)/30);
        assert(.5*30*u*u<=E+1e-14);
    }
    // 对地静止、海流+0.3m/s：所需稳态推力是-3.48N
    assert(std::abs(drag(-.3)+3.48)<1e-12);
    std::cout<<CF[0]<<" "<<CF[1]<<" "<<acc<<"\n";
}

ArduSub对应关系：libraries/AC_AttitudeControl 的姿态／角速度控制消化力矩类需求，libraries/AC_PosControl 的位置速度控制产生加速度类目标，libraries/AP_Motors/AP_Motors6DOF.cpp 负责执行器混控。阅读时不要把“控制器没显式积分六自由度模型”理解成物理不存在：反馈增益、经验效能、前馈和饱和共同近似处理这些动力学。完整 $M$ 、 $C$ 、 $D$ 模型适合离线仿真和模型控制；项目的 $2\times2$ 水平效能矩阵是另一个对象。

练习：有效质量30 kg时，撤去上例10 N推力，初始加速度应为 $-7/30\approx-0.233333\ \mathrm{m/s^2}$ 。手写无输入积分器，同时记录动能；足够小的步长下动能应下降。再将流速设为0.3 $\mathrm{m/s}$ ，以−3.48 N保持对地零速。最后自行填写对角六自由度 $M$ 并按动量法构造 $C$ ，任选 $\nu$ 检查 $\nu^TC(\nu)\nu\approx0$ ；答案要点是检查代数性质而不是逐个猜科氏系数。

TB01-S2 · Thor I. Fossen：MSS m2c.m

精读动量分块与反对称构造；注意实现将 $M$ 对称化，动量变量不是加速度。

TB01-S3 · Thor I. Fossen：Marine Craft Model

先读模型总式与Properties，再读Equations of Relative Motion；本课通过动能、输运定理和力臂重新推导。

把刚体科氏矩阵真正展开：两种表示为何都成立

前面的动量构造已经能计算科氏项；现在把它展开到仅由质量、重心与速度组成，才能与Fossen的模型逐项对应。记 $v=\nu_1$ 、 $\omega=\nu_2$ 、 $S_G=[r_G^b]_\times$ ，则线动量是 $a=mv-mS_G\omega=m(v+\omega\times r_G)$ ，角动量是 $b=mS_Gv+I_O\omega$ 。代入式 $(1.10)$ ，利用叉乘矩阵对向量的线性性，得到：

C_{RB}(\nu)= \begin{bmatrix} 0&-m[v]_\times-m[\omega\times r_G]_\times\\ -m[v]_\times-m[\omega\times r_G]_\times&m[v\times r_G]_\times-[I_O\omega]_\times \end{bmatrix}.

这里每个块都是三乘三。右下块中出现正的 $m[v\times r_G]_\times$ ，是因为 $-[m(r_G\times v)]_\times=+[m(v\times r_G)]_\times$ 。矩阵本身依赖平移速度，但物理只要求乘积 $C_{RB}\nu$ 正确；一个矩阵加上任意满足 $\Delta C(\nu)\nu=0$ 的项，仍表达同一动力学。这给了我们选择更方便表示的自由。

另一种表示只依赖角速度。把偏心刚体的牛顿—欧拉方程按 $v$ 和 $\omega$ 收集，得到下式。它与上一矩阵逐个元素不必相等，但乘同一个 $\nu$ 得到的广义力相等。

C_{RB}^{\omega}(\omega)= \begin{bmatrix} m[\omega]_\times&-m[\omega]_\times S_G\\ mS_G[\omega]_\times&-[I_O\omega]_\times \end{bmatrix}.

要验证第一行，只需把乘积写成 $m\omega\times v-m\omega\times(r_G\times\omega)$ ，它就是重心线动量的旋转输运。下半部可用三重叉乘恒等式 $a\times(b\times c)=b(a^Tc)-c(a^Tb)$ 与前一个表示对照。左下块恰好是右上块的负转置，因此它仍反对称。重心在原点时非对角块消失，上半部退成 $m[\omega]_\times$ ，这就是最直接的机体系牛顿方程。

数值检查可以选 $m=20\ \mathrm{kg}$ 、 $r_G=(0.1,0,0)^T\ \mathrm m$ 、 $v=(1,0,0)^T\ \mathrm{m/s}$ 、 $\omega=(0,0,0.2)^T\ \mathrm{rad/s}$ 。第一行的平移科氏力是 $(-0.08,4,0)^T\ \mathrm N$ ：侧向四牛顿来自转弯，前向负零点零八牛顿来自偏心重心的向心运动。这个例子能把偏心项与普通转弯项分开看清。

附加质量不是六个独立的“多加几千克”

以线性化的流体反作用为起点，定义 $X_{\dot u}=\partial X_{hydro}/\partial\dot u$ 等水动力导数。字母 $X,Y,Z$ 代表三个力分量， $K,M,N$ 代表三个力矩分量；这里力矩字母 $M$ 与惯性矩阵 $M$ 只在上下文中区分，因此编程时宜使用不同变量名。定义中的水动力是等式右边的实际外力，所以移项后的附加惯性带一个整体负号：

M_A=-\begin{bmatrix} X_{\dot u}&X_{\dot v}&X_{\dot w}&X_{\dot p_r}&X_{\dot q_r}&X_{\dot r_r}\\ Y_{\dot u}&Y_{\dot v}&Y_{\dot w}&Y_{\dot p_r}&Y_{\dot q_r}&Y_{\dot r_r}\\ Z_{\dot u}&Z_{\dot v}&Z_{\dot w}&Z_{\dot p_r}&Z_{\dot q_r}&Z_{\dot r_r}\\ K_{\dot u}&K_{\dot v}&K_{\dot w}&K_{\dot p_r}&K_{\dot q_r}&K_{\dot r_r}\\ M_{\dot u}&M_{\dot v}&M_{\dot w}&M_{\dot p_r}&M_{\dot q_r}&M_{\dot r_r}\\ N_{\dot u}&N_{\dot v}&N_{\dot w}&N_{\dot p_r}&N_{\dot q_r}&N_{\dot r_r} \end{bmatrix} =\begin{bmatrix}A_{11}&A_{12}\\A_{12}^T&A_{22}\end{bmatrix}.

$a_{ij}$ 在这里不另作标量记号； $A_{11}$ 是平移附加惯性块， $A_{22}$ 是转动附加惯性块， $A_{12}$ 是平移—转动耦合块。对称性意味着下左块是上右块的转置，而不是任意情况下两个块逐元素相同。举例说 $X_{\dot u}=-10\ \mathrm{kg}$ ，移到左边得到前向附加质量十千克；加上二十千克刚体质量，有效质量为三十千克。若力对角加速度有导数，其单位为牛顿除以弧度每二次方秒，等效为质量乘长度，正是耦合块应有的量纲。

流体动能为 $\tfrac12\nu_r^TM_A\nu_r$ 。对广义速度求偏导得到流体动量，将线动量和角动量分开，并应用同样的旋转输运结构，就得到附加科氏项，而不是再独立猜一套系数：

\begin{aligned} a_A&=A_{11}v_r+A_{12}\omega,\\ b_A&=A_{12}^Tv_r+A_{22}\omega,\\ C_A(\nu_r)&=\begin{bmatrix}0&-[a_A]_\times\\-[a_A]_\times&-[b_A]_\times\end{bmatrix}. \end{aligned}

$a_A$ 单位为线动量， $b_A$ 单位为角动量；它们不是加速度。若前向附加质量十千克、相对前速一米每秒、偏航角速度零点二弧度每秒，则仅由前向流体动量产生的侧向项为两牛顿。将这两牛顿漏掉却保留十千克附加质量，会破坏原先动能模型所要求的耦合关系。程序中可复用一个 coriolisFromMomentum 函数，分别传入刚体动量与流体动量。

从水动力导数到耗散：非对角阻尼也有物理要求

线性阻尼矩阵可以含轴间耦合。令 $K_v$ 表示实际水动力对广义速度的一阶导数矩阵，每行对应一个力或力矩分量，每列对应一个速度分量，那么线性化外力是 $K_v\nu_r$ ，移项后阻尼为 $D_L=-K_v$ 。本课对角二次模型只是下面一般结构的一个特例：

D(\nu_r)=D_L+D_N(\nu_r),\qquad \nu_r^TD(\nu_r)\nu_r =\nu_r^T\frac{D(\nu_r)+D(\nu_r)^T}{2}\nu_r\geq0.

等号成立是因为反对称部分不做功；不等号是耗散模型应满足的条件。因而不必要求每一个非对角元素为正，真正需要的是对称部分半正定。若采用 $D_{ii}=d_{1i}+d_{2i}|\nu_{r,i}|$ 且系数非负，则每轴功率为 $d_{1i}\nu_{r,i}^2+d_{2i}|\nu_{r,i}|^3$ ，天然非负。辨识带耦合阻尼时，单看对角系数正值仍不够，要检查整个对称部分。

完整六项恢复向量：把叉乘真正乘出来

先从旋转矩阵第三行读出导航下轴在机体中的表示： $h=(R_b^n)^Te_3=(-\sin\theta,\cos\theta\sin\phi,\cos\theta\cos\phi)^T$ 。重力与浮力合力是 $(W-\mathcal B)h$ 。令 $A_x=x_GW-x_B\mathcal B$ 、 $A_y=y_GW-y_B\mathcal B$ 、 $A_z=z_GW-z_B\mathcal B$ ，三者单位都是牛顿米。它们是“作用位置乘力”的差，不是新的经验参数。恢复力矩等于 $-(A\times h)$ ，其中 $A=(A_x,A_y,A_z)^T$ 。逐行展开便得到：

g(\eta)=\begin{bmatrix} (W-\mathcal B)\sin\theta\\ -(W-\mathcal B)\cos\theta\sin\phi\\ -(W-\mathcal B)\cos\theta\cos\phi\\ -A_y\cos\theta\cos\phi+A_z\cos\theta\sin\phi\\ A_z\sin\theta+A_x\cos\theta\cos\phi\\ -A_x\cos\theta\sin\phi-A_y\sin\theta \end{bmatrix}.

前三行由负的合力直接得到；第四行来自 $-(A_yh_z-A_zh_y)$ ；第五行来自 $-(A_zh_x-A_xh_z)$ ；第六行来自 $-(A_xh_y-A_yh_x)$ 。这样每个负号都能追溯到移项和叉乘。小姿态、重心原点、浮心在上方距离 $h_B>0$ 时， $A_z=\mathcal B h_B$ ，横滚恢复项近似 $\mathcal B h_B\phi$ ，俯仰恢复项近似 $\mathcal B h_B\theta$ ；它们在动力学左侧为正，所以实际力矩抵抗正倾角。

这里讨论完全浸没机器人。水面船舶还存在水线面积造成的深沉回复和稳心高度造成的横摇回复，不能把其水线面积刚度机械移植到完全浸没的艇体。若艇体正在出入水面，排水体积与浮心会随姿态和位置变化，应改用相应的静水几何模型。

恒定无旋流下，怎样严谨得到更紧凑的相对模型

先坚持式 $(1.13)$ 的绝对刚体、相对水动力形式。采用只依赖角速度的 $C_{RB}^{\omega}$ ，再利用导航恒定流满足 $\dot v_c^b=-\omega\times v_c^b$ ，可以直接算出刚体中与流相关的两项相互抵消：

\begin{aligned} M_{RB}\dot\nu_c+C_{RB}^{\omega}(\omega)\nu_c &=\begin{bmatrix}m\dot v_c^b\\mS_G\dot v_c^b\end{bmatrix} +\begin{bmatrix}m[\omega]_\times v_c^b\\mS_G[\omega]_\times v_c^b\end{bmatrix}=0,\\ M\dot\nu_r+\bigl(C_{RB}^{\omega}(\omega)+C_A(\nu_r)\bigr)\nu_r+D(\nu_r)\nu_r+g(\eta)&=\tau. \end{aligned}

第一行上半部分抵消的是平移动量，下半部分抵消的是偏心参考点对应的角动量；因此该关系不仅限于重心原点。第二行通过 $\nu=\nu_r+\nu_c$ 展开刚体方程，并删去第一行的零项得到。流必须在导航系中恒定且空间均匀；如果海流有局部加速度或剪切，应回到更一般模型，不能静默丢掉这些项。角速度不变换成“相对海流角速度”，因为这里无旋流没有独立转动。

如果数值状态仍存对地速度，最后还要把相对加速度加回坐标旋转造成的流速导数。下面的 $C_r$ 是上一式括号内的组合， $\tau_{env}$ 表示另外建模的风浪等广义外力，单位与 $\tau$ 相同：

\dot\nu= \begin{bmatrix}-[\omega]_\times v_c^b\\0_{3\times1}\end{bmatrix} +M^{-1}\bigl(\tau+\tau_{env}-C_r\nu_r-D\nu_r-g\bigr).

逆矩阵只用于书面表达；实现时解线性方程 $M x=\mathrm{rhs}$ 得到相对加速度，再加流速导数。导航位置导数仍由对地速度 $R_b^nv^b$ 产生。若误用相对速度积分位置，一艘完全随水漂流、相对水速零的艇会被仿真成对地静止。

势能梯度为什么要用逆转置，而不是随便乘一个姿态矩阵

恢复项是机体系的广义力，势能梯度则对位置和欧拉角求导，两者不在同一坐标表示中。由虚功相等， $g^T\nu=(\nabla_\eta U)^T\dot\eta$ ，再用 $\dot\eta=J\nu$ ，可得 $J^T\nabla_\eta U=g$ 。在欧拉参数化非奇异处，才可以写成：

\nabla_\eta U=J^{-T}g,\qquad U(\eta)-U(\eta_0)=\int_{\eta_0}^{\eta}(J(\xi)^{-T}g(\xi))^T\,d\xi.

这个推导说明了积分里的坐标变换为何是逆转置。对保守的重浮力模型，积分与路径无关；导航向下位置为 $z$ 时，固定净重产生的势能是 $-(W-\mathcal B)z$ 。小横滚、浮心在重心上方时，姿态势能近似 $\tfrac12\mathcal B h_B\phi^2$ 。二者求导再乘 $J^T$ ，分别恢复垂直和横滚的广义恢复项。被动性证明因此不只是“科氏不做功”，还要求势能与恢复力用同一套坐标变换。

从模型到一拍C++计算的顺序

建议按数据依赖实现：先由四元数得到 $R$ ；由导航海流求机体海流；计算相对速度；计算惯性、两类动量和阻尼；由重心浮心计算恢复向量；组装右端并线性求解；最后用对地速度更新位置、用机体角速度更新四元数。若使用固定步长欧拉法，先用小步长观察自由衰减，再与半步长比较，区分模型不稳定与离散误差。不要在同一拍中有的项使用更新前速度、有的项使用更新后速度，除非明确采用了对应的积分格式。

原始资料的阅读应回到Thor I. Fossen：Marine Craft Model的Kinematics、Kinetics、Properties与Relative Motion四部分；本节采用输运定理、动能与虚功重新推导各项，符号和实现约定以本教材为准。结合MSS m2c.m，重点看动量块如何形成叉乘矩阵，而不是把整页矩阵原样搬进程序。