学习笔记 搜索笔记

原课号：06.4

水动力学到控制律：六自由度模型、推进分配与抗扰设计

从刚体动能和相对水速建立水下模型，推导恢复力与推进器分配，再用一个带阻力、偏置扰动的悬停轴走通辨识、状态空间、极点配置、观测器、LQR和稳定性验证。

建议先读：从牛顿—欧拉到海洋六自由度 · 从一轴动力学推到可运行的离散 PID

本页目录

1. 同样是机器人，为什么水下模型多了附加质量

本课先建立六自由度结构，再在一个轴上完整设计控制，最后回到多轴限制。我们不把水下机器人直接当成“机械臂去掉关节”：它与周围流体交换动量，推进器产生的力还需要通过布局映射到六轴。

采用导航系 NED（北、东、下）和机体系 FRD（前、右、下），RR 将机体系向量变成导航系向量。定义

η=[x,y,z,ϕ,θ,ψ]T,ν=[u,v,w,p,q,r]T,τ=[X,Y,Z,K,M,N]T.\eta=[x,y,z,\phi,\theta,\psi]^T,\quad \nu=[u,v,w,p,q,r]^T,\quad \tau=[X,Y,Z,K,M,N]^T.

η\eta 前三项用 m、后三项用 rad；ν\nu 前三项是机体系线速度 m/s，后三项是机体系角速度 rad/s。这里 qq 是俯仰角速度符号，不是上一课的关节向量，也不是四元数。τ\tau 前三项用 N、后三项用 N·m。

运动学为 η˙=J(η)ν\dot\eta=J(\eta)\nu。注意机体系角速度不等于 Euler 角导数；大姿态仿真宜用四元数更新，具体坐标关系回看第零章。

2. 先从刚体动能得到质量矩阵

设机体系原点为 OO，质心相对它的位置为 rGr_G。原点速度为 vOv_O，角速度为 ω\omega，则刚体上质心的速度为

vG=vO+ω×rG=vO−S(rG)ω.v_G=v_O+\omega\times r_G=v_O-S(r_G)\omega.

S(a)b=a×bS(a)b=a\times b 是叉乘矩阵。刚体动能为

TRB=12m∥vO−S(rG)ω∥2+12ωTIGω.T_{RB}=\frac12m\|v_O-S(r_G)\omega\|^2+\frac12\omega^TI_G\omega.

展开并整理成 12νTMRBν\tfrac12\nu^TM_{RB}\nu：

MRB=[mI3−mS(rG)mS(rG)IG−mS(rG)2].M_{RB}=\begin{bmatrix} mI_3&-mS(r_G)\\mS(r_G)&I_G-mS(r_G)^2 \end{bmatrix}.

因为 ST=−SS^T=-S，整个矩阵对称。右下角正是平行轴定理的结果 IO=IG+m(∥rG∥2I−rGrGT)I_O=I_G+m(\|r_G\|^2I-r_Gr_G^T)。若原点在质心，交叉块消失；若 CAD 惯量已经相对原点给出，再加平行轴项就会重复。

艇加速时必须同时推动部分周围水体。低频理想化模型可把相应流体动能写成 TA=12νrTMAνrT_A=\tfrac12\nu_r^TM_A\nu_r，其中 MAM_A 为附加质量矩阵，νr\nu_r 是相对水的速度。它不是艇真的变重了，而是“加速同样快需要更多力”的惯性效应。不同方向附加质量不同，也可能耦合；在常用理想模型中 MAM_A 对称且半正定，组合 MRB+MAM_{RB}+M_A 应正定。

3. 为什么会有 Coriolis 项，又为什么它不做净功

机体系本身在转动，因此动量分量的变化不只是普通时间导数。设某个常数对称惯性矩阵为 MM，动量为

[h1h2]=Mν,\begin{bmatrix}h_1\\h_2\end{bmatrix}=M\nu,

h1h_1 为线动量，h2h_2 为角动量。一种保持能量结构的速度耦合表示是

C(ν)=[0−S(h1)−S(h1)−S(h2)].C(\nu)=\begin{bmatrix}0&-S(h_1)\\-S(h_1)&-S(h_2)\end{bmatrix}.

它满足 CT=−CC^T=-C，因此 νTCν=0\nu^TC\nu=0。例如质心原点、M=diag⁡(mI,IG)M=\operatorname{diag}(mI,I_G) 时，第一块 −S(mv)ω=mω×v-S(mv)\omega=m\omega\times v，正是转动坐标描述平动产生的项。刚体和附加质量可分别构造相应耦合项，但含海流时必须注意它们使用的速度不同。

这并不意味着耦合项“不重要”。它不改变总动能，却会改变各轴的速度和所需力矩；忽略它可能导致转弯时显著误差。

4. 阻力和恢复力的符号从物理得到

单轴线性加二次阻力写成

FD=−d1vr−d2∣vr∣vr,d1≥0,d2≥0.F_D=-d_1v_r-d_2|v_r|v_r,\quad d_1\ge0,\quad d_2\ge0.

FDvr=−d1vr2−d2∣vr∣3≤0F_Dv_r=-d_1v_r^2-d_2|v_r|^3\le0，说明阻力相对水耗能。d1d_1 单位 N·s/m，d2d_2 单位 N·s²/m²。若误写成 −d2vr2-d_2v_r^2，倒退时力仍向后，反而越阻越快。

六轴常用 D(νr)νrD(\nu_r)\nu_r 表示阻力的负号搬到方程左侧后的量。对角模型只是起点；非对称艇可能需要交叉项，仍应检查耗散条件。

重力大小 W=mg0W=mg_0，浮力大小 Bf=ρg0VB_f=\rho g_0\mathcal V；V\mathcal V 为排水体积，ρ\rho 为水密度。NED 中竖直向下单位向量 e3=[0,0,1]Te_3=[0,0,1]^T。机体系实际静水力与力矩为

fhb=RT(W−Bf)e3,f_h^b=R^T(W-B_f)e_3, μhb=rG×(RTWe3)+rB×(−RTBfe3).\mu_h^b=r_G\times(R^TW e_3)+r_B\times(-R^TB_f e_3).

这里 rBr_B 是浮心相对原点的位置。把实际静水作用搬到方程左侧，定义 g(η)=−[fhb;μhb]g(\eta)=-[f_h^b;\mu_h^b]，便不必背一个很长的三角函数表，也不会把浮力方向写反。中性浮力 W=BfW=B_f 不代表没有恢复力矩：质心与浮心分离仍可产生姿态恢复作用。

5. 海流绝不能只在最后随便加一个常数力

设环境流在导航系中为均匀常向量 vcnv_c^n，无旋转流分量，则机体系流速为 vcb=RTvcnv_c^b=R^Tv_c^n。即使世界中的流恒定，艇转向时机体系分量仍变化：

v˙cb=−ω×vcb.\dot v_c^b=-\omega\times v_c^b.

定义 νc=[vcb;0]\nu_c=[v_c^b;0]，νr=ν−νc\nu_r=\nu-\nu_c。一个常用六自由度结构是

MRBν˙+CRB(ν)ν+MAν˙r+CA(νr)νr+D(νr)νr+g(η)=τ+τext.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(\eta) =\tau+\tau_{\rm ext}.

刚体项描述艇相对惯性空间的运动，流体项依赖相对水运动。不能把所有 ν\nu 无条件换成 νr\nu_r，也不能忘掉 ν˙c\dot\nu_c。 对均匀无旋流和合适的 Coriolis 参数化可进一步化简，但需先验证假设。Fossen 的官方海洋载体模型给出了相应完整结构与相对运动条件。

用于固定航向、低速单轴悬停时，可以把部分未建模流作用集总成缓变外力。这是控制设计近似，不意味着海流物理上总是常力。

6. 八推进器的模型必须包含“力能否实现”

第 ii 个推进器安装位置为 rir_i（m），正推方向单位向量为 did_i，其推力 fif_i 用 N。它产生的广义力为

τi=[diri×di]fi.\tau_i=\begin{bmatrix}d_i\\r_i\times d_i\end{bmatrix}f_i.

将八列并排构成 BT∈R6×8B_T\in\mathbb R^{6\times8}（与推进器分配课的 BτB_\tau 是同类几何映射，数值仍由实际布局决定），则 τ=BTf\tau=B_Tf。前面三行是方向余弦，后面三行是力臂；两部分元素的量纲不同。

检查 rank⁡(BT)=6\operatorname{rank}(B_T)=6 才能说明线性代数意义上六轴都有施力方向。还必须检查推力上下限、正反推不对称、变化率和供电约束。即使满秩，给定大力矩也可能无解。

若实际输入是归一化指令 ai∈[−1,1]a_i\in[-1,1]，还需标定 fi=γi(ai)f_i=\gamma_i(a_i)，它可能有死区、正反向不同曲线；动态过程可近似 Tif˙i+fi=γi(ai)T_i\dot f_i+f_i=\gamma_i(a_i)。控制器请求的是 τ\tau，混控接收的是 aia_i，中间至少隔着分配和执行器两层。

针对水平 22.5∘22.5^\circ 布局，应从实际 ri,dir_i,d_i 构造列，而不是将所有经验系数称为惯量。项目中的经验水平映射 GxyG_{xy}、物理惯量 MM、几何分配 BTB_T 是三个不同对象，不能混用。

7. 如何从模型得到一个能设计的悬停轴

以固定姿态附近的一个水平轴为例。把线性阻力和等效惯量保留，把缓变未建模作用写成外力 dd：

p˙=v,mev˙=−bv+u+d.\dot p=v,\qquad m_e\dot v=-b v+u+d.

这里 uu 是实际轴向力，非杆量；mem_e 含刚体和附加质量，b≥0b\ge0。若外扰平衡值为 d0d_0，平衡输入应为 u0=−d0u_0=-d_0。悬停并不保证零推力。

在 x=[p,v]Tx=[p,v]^T 上，

A=[010−b/me],B=[01/me].A=\begin{bmatrix}0&1\\0&-b/m_e\end{bmatrix},\quad B=\begin{bmatrix}0\\1/m_e\end{bmatrix}.

这个模型已经线性；若保留二次阻力 d2∣v∣vd_2|v|v，在工作速度 v0v_0 附近的斜率是 2d2∣v0∣2d_2|v_0|。零速时纯二次阻力的一阶斜率为零，不能凭直觉在零速模型里填一个任意线性阻尼。

用真实数字做一次极点配置

取教学参数 me=20m_e=20 kg、b=10b=10 N·s/m。位置目标固定为 pdp_d，控制为

u=−kp(p−pd)−kvv−d^.u=-k_p(p-p_d)-k_vv-\hat d.

其中 uu 是轴向力。选目标自然频率 ωn=1\omega_n=1 rad/s、阻尼比 ζ=0.8\zeta=0.8。先令扰动补偿准确，则闭环误差满足

mee¨+(b+kv)e˙+kpe=0.m_e\ddot e+(b+k_v)\dot e+k_pe=0.

与 e¨+2ζωne˙+ωn2e=0\ddot e+2\zeta\omega_n\dot e+\omega_n^2e=0 比较，得到

kp=meωn2=20 N/m,kv=2meζωn−b=22 N s/m.k_p=m_e\omega_n^2=20\ \mathrm{N/m},\quad k_v=2m_e\zeta\omega_n-b=22\ \mathrm{N\,s/m}.

闭环极点为 −0.8±0.6j-0.8\pm0.6j s⁻¹。增益来自目标响应和物理参数，而不是两次试车碰运气。若不补偿恒定 d=4d=4 N，稳态位置误差为 e∞=d/kp=0.2e_\infty=d/k_p=0.2 m；提高比例会减小它，却增加力需求，不能等同于估计并消除扰动。

8. 为什么要设计偏置观测器

把 dd 作为缓变状态，先用 d˙=0\dot d=0 的教学模型；测量选速度 y=v+ny=v+n。对 z=[v,d]Tz=[v,d]^T，

z˙=Aoz+Bou,Ao=[−b/me1/me00],Bo=[1/me0],Co=[1,0].\dot z=A_oz+B_ou,\quad A_o=\begin{bmatrix}-b/m_e&1/m_e\\0&0\end{bmatrix},\quad B_o=\begin{bmatrix}1/m_e\\0\end{bmatrix},\quad C_o=[1,0].

观测器为 z^˙=Aoz^+Bou+L(y−Coz^)\dot{\hat z}=A_o\hat z+B_ou+L(y-C_o\hat z)。为什么这一结构合理？前半预测力能造成多大加速度，后半用实测速度与预测速度的差修正；若速度持续比预测快，说明存在遗漏的正向作用。

取 L=[lv,ld]TL=[l_v,l_d]^T。观测误差矩阵特征多项式为

s2+(b/me+lv)s+ld/me.s^2+(b/m_e+l_v)s+l_d/m_e.

若希望两个观测误差极点都在 −λo-\lambda_o，系数比较得

lv=2λo−b/me,ld=meλo2.l_v=2\lambda_o-b/m_e,\qquad l_d=m_e\lambda_o^2.

令 λo=3\lambda_o=3 s⁻¹，得到 lv=5.5l_v=5.5 s⁻¹、ld=180l_d=180 N/m。第二个增益乘速度残差后产生 N/s 的扰动力变化率。较快观测器不一定更好：噪声、延迟、未建模执行器会限制带宽。还要用实际实现的力做输入，而非未实现的饱和前指令，否则观测器会把执行器饱和误认成环境扰动。

可观测性矩阵为 [Co;CoAo]=[[1,0],[−b/me,1/me]][C_o;C_oA_o]=[[1,0],[-b/m_e,1/m_e]]，有限正 mem_e 时满秩，因此此理想单轴模型中速度历史能区分 v,dv,d。它不意味着仅靠 DVL 就能确定全局绝对位置，也不意味着所有六轴环境状态都可辨。

9. LQR 与手工极点配置是什么关系

极点配置先指定误差收敛速度；LQR 则先说明“状态偏差和控制努力如何权衡”。本节为简洁，将偏差状态 x~=[p−pd,v]T\tilde x=[p-p_d,v]^T 和偏差输入 u~=u−u0\tilde u=u-u_0 临时记作 x,ux,u；若已知恒定外扰 d0d_0，则 u0=−d0u_0=-d_0，最终实际力必须加回 u0u_0。下面的代价不是把非零目标位置和必要的平衡推力都惩罚到零。在线性局部偏差模型中定义

J=∫0∞(xTQcx+uTRcu) dt,Qc⪰0, Rc≻0.J=\int_0^\infty(x^TQ_cx+u^TR_cu)\,dt,\quad Q_c\succeq0,\ R_c\succ0.

这里 Qc,RcQ_c,R_c 是控制代价权重，不是卡尔曼滤波的过程/测量噪声协方差。位置和速度量纲不同，应按允许尺度归一化；例如 Qc=diag⁡(1/pscale2,1/vscale2)Q_c=\operatorname{diag}(1/p_{\rm scale}^2,1/v_{\rm scale}^2)，Rc=1/uscale2R_c=1/u_{\rm scale}^2，只是一个可解释的起点。

设最优剩余代价为 V=xTPxV=x^TPx，其中 P=PTP=P^T。对线性系统 x˙=Ax+Bu\dot x=Ax+Bu，连续时间 Bellman 最优性条件写为

0=min⁡u[xTQcx+uTRcu+2xTP(Ax+Bu)].0=\min_u\left[x^TQ_cx+u^TR_cu+2x^TP(Ax+Bu)\right].

这里 2Px2Px 是 VV 对状态的梯度，最后一项是沿系统运动的代价变化率。固定当前 xx，只对输入 uu 求导：

2Rcu+2BTPx=0,u∗=−Rc−1BTPx.2R_cu+2B^TPx=0,\qquad u^*=-R_c^{-1}B^TPx.

由于 Rc≻0R_c\succ0，这一步关于 uu 的二次型严格凸，驻点是最小值。代回后，控制相关两项合成 −xTPBRc−1BTPx-x^TPBR_c^{-1}B^TPx；又因 xTPAxx^TPAx 是标量，等于其转置 xTATPxx^TA^TPx，可把全部项收成 xx 的二次型。对所有状态成立，才得到

ATP+PA−PBRc−1BTP+Qc=0.A^TP+PA-PBR_c^{-1}B^TP+Q_c=0.

在可稳定、适当可检测等条件下，取稳定化解得到 K=Rc−1BTPK=R_c^{-1}B^TP。随后还要检验力限幅、采样周期和模型误差。LQR 不是自动处理所有约束的算法；约束直接参与优化时要进入 MPC 等框架。公式背景可对照 MIT Underactuated Robotics 的 LQR 章节。

10. 稳定性、抗扰与可实现性要分开验收

对理想 PD 单轴、d^=d\hat d=d，取

V=12mev2+12kpe2,V˙=−(b+kv)v2≤0.V=\frac12m_ev^2+\frac12k_pe^2,\qquad \dot V=-(b+k_v)v^2\le0.

最大不变集中 v=0v=0 且 v˙=−kpe/me=0\dot v=-k_pe/m_e=0，因此 e=0e=0。若 d~=d−d^\tilde d=d-\hat d 非零，则

V˙=−(b+kv)v2+vd~.\dot V=-(b+k_v)v^2+v\tilde d.

后者可能瞬间为正，不能继续用原来的证明声称“任意扰动下都收敛到零”。对有界扰动估计误差可进一步推导有界性；线性观测器与状态反馈在精确连续模型下可使用分离原理分析，但饱和、非线性、延迟会破坏直接套用的条件。

如果可用推力只有 33 N，而恒定外扰为 44 N，任何上述控制器都不能实现精确静止。此时需要报告不可实现、降低任务要求或改变构型，而不是继续增加增益。

11. C++ 实验：误差从哪里来，补偿是否有效

完成以下三种实验：无扰动 PD；44 N 扰动无补偿；相同扰动加偏置观测器。比较后两者稳态位置，不要只看短暂峰值。下面代码采用小步长显式 Euler，只是教学离散化；工程中应检验采样稳定性和积分精度。

展开可独立编译的 C++17 实验与断言 
#include <cmath>
#include <cassert>
#include <iostream>
struct Result { double p,v,dhat; };
Result simulate(double disturbance,bool compensate) {
    constexpr double dt=0.001,m=20,b=10,kp=20,kv=22;
    double p=0.7,v=0,vhat=0,dhat=0;
    for(int k=0;k<30000;++k) {
        const double u=-kp*p-kv*v-(compensate?dhat:0.0);
        const double innovation=v-vhat;
        const double vhat_dot=(-b*vhat+u+dhat)/m+5.5*innovation;
        const double dhat_dot=180.0*innovation;
        const double a=(-b*v+u+disturbance)/m;
        p+=dt*v; v+=dt*a;
        vhat+=dt*vhat_dot; dhat+=dt*dhat_dot;
    }
    return {p,v,dhat};
}
int main() {
    auto no_dist=simulate(0,false), pd=simulate(4,false), dob=simulate(4,true);
    assert(std::abs(no_dist.p)<1e-6);
    assert(std::abs(pd.p-0.2)<1e-6);
    assert(std::abs(dob.p)<1e-6 && std::abs(dob.dhat-4)<1e-6);
    std::cout<<no_dist.p<<" "<<pd.p<<" "<<dob.p<<" "<<dob.dhat<<"\n";
}

代码可读取真值速度，只是为了隔离观测器模型原理；接入真实传感器时替换为有时间戳的测量，并加入噪声、缺测和执行器限制。不要把这里的理想结果当成实机指标。接着复用第五章观测器实验与仿真步骤加入多种工况。

12. 一次完整设计实际要交付什么

  • 画出控制边界：传感器、估计器、控制器、分配器、执行器、物理对象。
  • 固定坐标、输入单位、正方向和采样时刻。
  • 从质量/几何与实验得到第一版模型，保留参数不确定范围。
  • 选择工作点并求平衡输入，再线性化；明确局部有效范围。
  • 检查可控性/可稳定性、可观测性/可检测性；选择真的可估计的状态。
  • 推导反馈和观测器，并写清稳定性证明的假设。
  • 用独立测试数据检查模型残差；在仿真中逐项加入参数误差、噪声、延迟、失效和限幅。
  • 最后才将控制输出接入实际接口，核对分配后是否真正产生需求广义力。

本课六自由度结构参考 Fossen 作者模型，具体单轴参数、控制/观测器设计与 C++ 实验为独立教学构造。它的价值是让你能解释“为什么这样选模型、为什么这样设计”，而不是把一个控制器名字放进简历。

仅保存在当前浏览器，可在学习记录页导出。