05.1｜选一艘真实开源八推 ROV：从 SDF 到状态空间

先修：矩阵与秩、状态空间、Taylor 线性化。

零基础桥梁：模型、状态、测量分别回答什么问题

模型是对现实保留哪些规律的明确选择，不是逼真的三维外观。SDF 是描述仿真世界、刚体连接、参数和插件的文件格式；link 是刚体部件，joint 是连接，plugin 是向仿真添加力或传感器等行为的程序。看见八个桨叶网格，只证明画面里有八个形状；本章还要检查八条施力路径，才知道它们真能推动模型。

状态保存预测未来所需的记忆；输入是能够施加的控制；输出／测量是实际能读到的量。两艘艇当前都在原点，一艘静止、一艘正以 $1$ m/s 向前：即使未来都不施力，下一秒位置也不同，所以只用位置当状态不够。对于简化单轴模型，状态可选 $x=[p,v]^T$，位置 $p$ 用 m、速度 $v$ 用 m/s；并不要求两个分量单位相同。测量可能只有 $y=p$，它不是完整状态，速度要借助一段位置历史与模型估计。

状态空间的目的是把“记忆怎样更新”和“传感器看见什么”分开写：$\dot x=Ax+Bu$ 描述变化率，$y=Cx$ 描述理想测量。$A$ 表示状态之间的影响，$B$ 表示输入影响哪些变化率，$C$ 表示测到哪些状态组合。最简单的无阻力轴，输入 $u$ 取加速度时，$\dot p=v$、$\dot v=u$；若 $p=2$ m、$v=0.3$ m/s、$u=0$，经过 $0.1$ s 位置变为 $2.03$ m，速度不变。若输入改用牛顿，必须通过质量换成加速度，不能继续沿用同一个 $B$。

可控制问输入能改变哪些运动，可观测问测量历史能区分哪些状态。八推冗余并不能让未知位置自动可观测；增加传感器也不能让坏掉的推进器恢复施力。模型课先确定状态和输入，下一课再用测量矩阵检查可观测性。

为什么先悬停，再线性化

工作点是要在其附近研究的状态和输入；平衡点要求各变化率为零；线性化是在该点用局部斜率近似非线性关系。悬停不是“把推进器设零”，而是合力和力矩先平衡。若净向上浮力为 $2$ N，就需先用合计向下 $2$ N 抵消；线性控制输入表示在这个基准上的增减，不能漏掉基准推力。

斜率为何只局部有效？设一个教学阻力大小关系为 $F_D=10|v|v$，$v$ 单位 m/s，系数单位为 N·s²/m²。在 $v_0=0.2$ m/s 附近，阻力为 $0.4$ N、斜率为 $4$ N·s/m；速度增加 $0.01$ m/s 时，线性预测阻力增加 $0.04$ N，精确增加为 $0.041$ N，差异很小。在零速点斜率却是零，不能因为非零速度有阻力，就在零速线性模型中凭空写入常数阻尼。后文会区分 SDF 的严格线性化与教学设计假设。

基础在线练习： 二维分配 先练矩阵把输入映成合力及限幅；它不替代本章六维八推秩检验。坐标变换 先练同一物理向量如何换分量。

本章要交付什么

学习路径是一条可检验的链：模型文件 → 力学方程 → 工作点 → 状态空间 → 可观测性 → 观测器 → 状态反馈 → 稳定性证明 → C++ 数值实验 → Gazebo／SITL 接口实验。先回答每一层为什么存在，再写下一层代码。

本章选 BlueROV2 Heavy 的八推进器模型作几何与软件接线基线，不把原有实艇的 ArduSub 4.1.1 升级当作前提。新建学习工作区，仿真固件、参数、端口与实艇完全分开。第一轮只做悬停附近低速实验，不同时解决视觉、航迹规划、强流、推进器故障与非线性全局稳定性。

1. 先验证“八推六自由度”，不要从项目标题猜

选用 clydemcqueen/bluerov2_gz，锁定源码提交 4451a5d1b15d204581d43ef7d93df4a0508eddbb。检查的核心不是截图，而是：

• models/bluerov2_heavy/model.sdf：八个推进器 link、joint、Thruster 插件和八个 ArduPilot channel。
• configs.yaml：几何与惯性参数。
• scripts/generate_model.py：参数怎样写入模型。

这个模型的 base_link 质量为 $13\,\mathrm{kg}$；另有八个各 $0.002\,\mathrm{kg}$ 的推进器 link，组装质量为 $13.016\,\mathrm{kg}$。教学刚体近似忽略这些小 link 的质量与惯量；质心处惯量对角项为 $(0.26,0.23,0.37)\,\mathrm{kg\,m^2}$。几何原点下质心为 $(0,0,0.011)\,\mathrm m$，浮心为 $(0,0,0.06)\,\mathrm m$。这些是该仿真模型的参数，不是所有 Heavy 实艇的辨识结果。模型的附加质量项和线性阻力项设为零；二次平动阻力系数绝对值为 $(58.42,55.137,124.818)$，转动项为 $(4,4,4)$。SDF 采用负的稳定导数；写成阻力放在方程左边时采用正的耗散系数。注意不能把符号直接抄反。

坐标是模型的一部分

本课推导先使用 Gazebo 机体 FLU：前、左、上；世界使用 ENU：东、北、上。这样几何直接来自 SDF。需要送入 ArduPilot 时才转换到 FRD／NED，不能只改字段名。

$$p_N=T_{NE}p_E,\qquad T_{NE}=\begin{bmatrix}0&1&0\\1&0&0\\0&0&-1\end{bmatrix},\qquad v_{FRD}=T_{BF}v_{FLU},\qquad T_{BF}=\operatorname{diag}(1,-1,-1).$$

$p_E,p_N$ 是同一个位置向量的 ENU、NED 分量；$v$ 是同一个机体速度的两套分量。两个 $T$ 都是旋转矩阵。力、力矩、角速度也须做相同的相应旋转；姿态矩阵则必须同时转换世界端和机体端。上游 SDF 已为 ArduPilot 插件指定这些转换；外部 ROS 节点不要再重复一次。

2. 从八股力推导六维输入矩阵

令第 $i$ 个推进器相对机体原点的位置为 $r_i\in\mathbb R^3$，其正推力单位方向为 $q_i\in\mathbb R^3$，有符号推力为 $f_i$，单位 N。它产生的力是 $q_i f_i$，力矩是杠杆与力的叉积。

$$\tau= B_f f,\qquad B_f=\begin{bmatrix}q_1&\cdots&q_8\\r_1\times q_1&\cdots&r_8\times q_8\end{bmatrix}\in\mathbb R^{6\times8},\qquad f=[f_1,\ldots,f_8]^T.$$

$\tau=[F_x,F_y,F_z,M_x,M_y,M_z]^T$ 的前三项单位 N，后三项 N·m。矩阵上半部分无量纲，下半部分单位 m。八个输入不是八个自由度：艇的刚体运动只有六维，多出的输入使分配有冗余。

方向来自 joint axis 与 link pose，不来自桨叶外观。 此模型每个 joint 的轴为局部 $(0,0,-1)$。令 $R_i$ 是由该 link 的 roll、pitch、yaw 得到的旋转，先计算 $q_i=R_i(0,0,-1)^T$。把源码中三位小数角度理想化为精确的 $\pi/2$、$\pi/4$，得到下表。$c=1/\sqrt2$。

编号	$r_i$，m	$q_i$，FLU
1	$(0.14,-0.092,0)$	$(-c,-c,0)$
2	$(0.14,0.092,0)$	$(-c,c,0)$
3	$(-0.14,-0.092,0)$	$(c,-c,0)$
4	$(-0.14,0.092,0)$	$(c,c,0)$
5	$(0.118,-0.215,0.064)$	$(0,0,-1)$
6	$(0.118,0.215,0.064)$	$(0,0,-1)$
7	$(-0.118,-0.215,0.064)$	$(0,0,-1)$
8	$(-0.118,0.215,0.064)$	$(0,0,-1)$

例如第 5 列力矩为 $(0.215,0.118,0)^T f_5$。一个向下推的右前推进器同时产生 roll 与 pitch 力矩，这正是“四个竖推”可以独立组合升沉、横滚、俯仰的原因。水平四推则组合纵荡、横荡、偏航。

$$f^\star=B_f^T(B_fB_f^T)^{-1}\tau_d.$$

这里 $\tau_d$ 是期望六维力／力矩，$f^\star$ 是无约束最小二范数解；成立条件是 $\operatorname{rank} B_f=6$。本章 C++ 程序对理想几何实际算出秩 6，并检验 $B_f B_f^+=I_6$，最大误差约 $2.22\times10^{-16}$。直接裁剪 $f^\star$ 会破坏力矩平衡；下一步应解带推进器上下限的最小二乘，并记录残差 $B_f f-\tau_d$。这不是“秩满所以任何力都能实现”：有限推力仍限定了可行力集合。

3. 为什么状态选位姿和速度，而不是把所有量塞进向量

状态是“给定现在与未来输入，就足够预测未来”的最小记忆。刚体受到力以后先改变速度，速度再改变位置，因此只保存位置不足以预测。选择 $\eta=[p^T,\Theta^T]^T\in\mathbb R^6$ 和 $\nu=[v^T,\omega^T]^T\in\mathbb R^6$；$\Theta$ 是局部欧拉角，$\omega$ 是机体角速度，两者一般不相等。

$$\dot\eta=J(\eta)\nu,\qquad M\dot\nu+C(\nu)\nu+D(\nu_r)\nu_r+g(\eta)=B_f f+\tau_{ext}.$$

这是海洋刚体的常用结构：Fossen 的原始模型说明。$M$ 是刚体与附加质量组成的 $6\times6$ 惯性矩阵；$C$ 描述旋转坐标与耦合惯性；$D$ 是相对水体运动的阻力；$g$ 是重力浮力恢复项；$\nu_r$ 是相对水速度；$\tau_{ext}$ 是未显式建模的外力／力矩。上述紧凑写法在静水下使用；本章所选 SDF 的附加质量为零时也可按相对流速计算阻力。若一般模型含附加质量且有海流，应区分 $M_{RB}\dot\nu+C_{RB}(\nu)\nu$ 与 $M_A\dot\nu_r+C_A(\nu_r)\nu_r$，不能把两种速度混写成同一个项。若已用相对流速算了海流阻力，就不要把同一海流效果再重复加入 $\tau_{ext}$。

原点不在质心时，刚体惯性不能只写成对角块：

$$M_{RB}=\begin{bmatrix}mI_3&-mS(r_g)\\mS(r_g)&I_G-mS(r_g)^2\end{bmatrix},\qquad S(r)a=r\times a.$$

$r_g$ 是原点到质心的向量，$I_G$ 是质心惯量；$S(r)$ 是叉积的反对称矩阵。下右块是平行轴定理，上下非对角块是平移与转动的耦合。也可把所有推进器位置、浮心位置都平移到质心重新建模，这时 $r_g=0$，但不能只移动惯性、不移动力臂。

4. 在悬停点把非线性模型拆成可设计的线性模型

先求平衡：$\nu_0=0$，并选 $f_0$ 满足 $B_f f_0=g(\eta_0)-\tau_{ext,0}$。正浮力艇悬停时通常并非零推力。定义偏差 $\delta\eta=\eta-\eta_0$、$\delta\nu=\nu-\nu_0$、$\delta f=f-f_0$，才谈“小信号”。

$$x=\begin{bmatrix}\delta\eta\\\delta\nu\end{bmatrix},\qquad \dot x=Ax+B\delta f+E\delta\tau_{ext},$$ $$A=\begin{bmatrix}0&J_0\\-M^{-1}G_0&-M^{-1}D_0\end{bmatrix},\quad B=\begin{bmatrix}0\\M^{-1}B_f\end{bmatrix},\quad E=\begin{bmatrix}0\\M^{-1}\end{bmatrix}.$$

$x$ 有 12 个分量；$J_0=J(\eta_0)$ 是运动学变换；$G_0$ 是恢复力对位姿的雅可比；$D_0$ 是速度阻力的局部雅可比。这里只写静水静止平衡的形式；非零航速、旋转或海流下还应保留相应雅可比项。在水平零航向且一致坐标下 $J_0=I_6$。不要因为角度小就把有明显浮心偏置的 roll、pitch 恢复项全部删除。

特别重要：$F_D=c_D|v|v$ 在 $v=0$ 的导数是零。因此这个上游纯二次阻力模型的严格零速线性化没有线性阻尼。为控制器设置 $a=0.4\,\mathrm{s^{-1}}$ 是下一课的教学假设，不是从该 SDF 推出来的数字。实际设计可在指定非零相对流速线性化，或通过受控小幅试验辨识等效阻尼，并标明工作范围。

参数辨识的第一轮实验

先单轴施加小推力，记录加速度、速度与稳态速度；重复正负方向，逐步拟合 $F=m_{eff}\dot v+d_1v+d_2|v|v$。$m_{eff}$ 是有效质量，$d_1,d_2$ 是阻力系数。若只有速度测量，不应直接用噪声差分计算 $\dot v$；使用滤波回归或积分形式。训练区间和验证区间分开，报告未用于拟合的轨迹误差。否则“在仿真里拟合很好”只说明重现了同一组数据。

小结与练习

1. 把原点从模型原点移到质心后，重算第 5 推进器力臂。为什么 $F_x,F_y,F_z$ 不变而力矩表示会变？
2. 只保留两个竖推时，重新计算输入矩阵秩；说明不能独立控制哪些轴。
3. 设置 $d_1=0$ 后，再使用“模型里有 $0.4v$ 阻尼”的控制器，会产生什么未建模项？下一课会把它与外扰一起看作总扰动，而不是宣称观测器能识别每种物理原因。

下一课：从测量选择到可证明的抗扰反馈。