本文将详细介绍一个最近的一个研究，围绕多拖轮协同作业任务，构建基于三自由度水动力学的强化学习仿真与控制框架。

在拖轮智能体设计方面，控制输入定义为舵角与螺旋桨转速的二维连续动作空间，状态变量则涵盖大地坐标系下的位置、航向角以及船体坐标系下的纵向、横向和首摇速度。智能体通过高频仿真步长更新状态。

动力学建模采用了经典的水面船舶三自由度Fossen模型，通过欧拉法进行离散化。该模型系统地计算了拖轮的质量矩阵、附加质量矩阵、随速度变化的科里奥利矩阵，以及由正面和侧面水下投影面积估算的非线性二次阻力矩阵。螺旋桨推力则基于转速、直径和特定推力系数进行换算，从而建立了较符合物理实际的控制量到受力与运动状态的映射。下面详细介绍各个模块设计细节：

多个拖船智能体设计：

动作空间设计

仿真步长（取0.1s, 10Hz）

拖轮的动作定义（连续动作，2 维）

每个时间步，拖轮智能体输出一个二维连续动作： $A_{t}=[ \delta_{t} , n_{t} ]$

其中：

$\delta_{t}$ ：表示舵角，范围为[-35, 35], 单位角度
$n_t$ ：表示螺旋桨转速，范围为[0, 265]，单位rpm

拖轮的状态变量

\mathbf{x}_{k}=\left[ \begin{array} {c} {x_{k}} \\ {y_{k}} \\ {\psi_{k}} \\ {u_{k}} \\ {v_{k}} \\ {r_{k}} \\ \end{array} \right] \! , \quad\mathbf{u}_{k}=\left[ \begin{array} {c} {\delta_{k}} \\ {n_{k}} \\ \end{array} \right]

$x_k, y_k$ ：表示船舶在大地坐标系下的位置（单位是m）

$\psi_{k}$ ：表示船舶的航向角，单位是弧度

$u_{k}$ ：表示船体坐标系下的纵向速度（Surge velocity，船头方向）。

$v_{k}$ ：表示船体坐标系下的横向速度（Sway velocity，右舷方向）。

$r_{k}$ ：表示船体坐标系下的首摇角速度（Yaw rate，转向快慢）。

$\delta_{k}$ ：舵角（Rudder angle）。控制方向。

$n_{k}$ ：螺旋桨转速（Propeller revolution speed，通常单位：rpm 或 r/s）。控制前进动力。

拖轮的运动学模型 [针对后3个公式查找论文]

模型采用了欧拉法（Euler Method）进行离散化:

\begin{array} {r l} {x_{k+1}} & {=x_{k}+\Delta t \cdot( u_{k} {\cos} \psi_{k}-v_{k} {\sin} \psi_{k} )} \\ {y_{k+1}} & {=y_{k}+\Delta t \cdot( u_{k} {\sin} \psi_{k}+v_{k} {\cos} \psi_{k} )} \\ {\psi_{k+1}} & {=\psi_{k}+\Delta t \cdot r_{k}} \\ {u_{k+1}} & {=u_{k}+\Delta t \cdot(-X_{u} u_{k}+X_{u | u |} \, | u_{k} \, | \, u_{k}+X_{n} n_{k}^{2} )} \\ {v_{k+1}} & {=v_{k}+\Delta t \cdot(-Y_{v} v_{k}-Y_{r} r_{k}+Y_{g} \delta_{k} )} \\ {r_{k+1}} & {=r_{k}+\Delta t \cdot(-N_{v} v_{k}-N_{r} r_{k}+N_{g} \delta_{k} )} \end{array}

前两个公式的含义：船体坐标系下的纵向速度和横向速度，通过旋转矩阵（由航向角决定）转换到大地坐标系，从而更新船的 $x,y$ 坐标。 • 这里利用了三角函数变换，将“随船动的速度”分解为“向东”和“向北”的速度。
第三个公式的含义：下一时刻的航向角 = 当前航向角 + 时间步长 x 角速度
第四个公式的含义：

智能体观测空间：

障碍物/他船

不要直接喂全局AIS列表，做成固定长度：

最近 N 个目标，每个目标含：
- 相对位置 (\Delta x,\Delta y)
- 相对速度
- CPA / TCPA
- 航向差
- 目标类别（船/浮标/码头设施）

周围环境感知向量：

从智能体中心向8个方向均匀射出100m长度的射线，如果射线碰撞到岸线，则计算射线长度，否则改射线长度为100m，将8个射线的长度编码成一个观测向量。

奖励函数设计

0.基本定义：

定义第i艘拖轮在t时刻的位置、航向、速度为：

p_t^i \in \mathbb{R}^2,\qquad \psi_t^i,\qquad v_t^i

目标船的位置、航向、速度分别为：

p_t^{ship},\qquad \psi_t^{ship},\qquad v_t^{ship}

第i艘拖船在目标船坐标系下的位置、航向偏差为：

p_*^i(t),\qquad \Delta\psi_*^i(t)

其中， $p_*^i$ 为期望拖船在目标船舶的位置，例如在船舶中心的左侧30m，前方80m；

$\Delta\psi_*^i(t)$ 为期望拖船在目标船的角度，例如在船舶中心的60°处。

定义截断算子：

\mathrm{clip}_{[a,b]}(x)=\min\{b,\max(a,x)\}

1.全局奖励项：

R_t^{\mathrm{term}}=\begin{cases}C_{\mathrm{succ}}, & \text{任务成功} \\-C_{\mathrm{col}}, & \text{发生碰撞} \\-C_{\mathrm{out}}, & \text{越界} \\-C_{\mathrm{timeout}}, & \text{超时} \\0, & \text{其他情况}\end{cases}

其数值分别为500，600，600，300

2.误差项：

2.1位置误差：

e_{p,t}^i= R(\psi_t^{ship})^\top\left(p_t^i-p_t^{ship}\right)-p_*^i(t)

$\R(\psi_t^{ship})^\top$ 为转置矩阵，其目的是为了让大地坐标系下的拖船位置转换成目标船坐标系下

2.2航向误差：

e_{\psi,t}^i = \mathrm{wrap}\!\left( \psi_t^i-\psi_t^{ship}-\Delta\psi_*^i(t) \right)

warp 是角度归一化函数，把任意角度差映射到一个标准范围内 $(- \pi , \pi]$

2.3速度误差：

e_{v,t}^i = R(\psi_t^{\mathrm{ship}})^\top \left(v_t^i-v_t^{\mathrm{ship}}\right)-v_*^i(t)

通常来说，应保持拖船速度和目标船的速度一致，因此， $v_*^i(t)=0$

3.误差归一化

3.1位置误差：

\bar e_{p,t}^i=\operatorname{clip}_{[0,1]}\left(\frac{\|e_{p,t}^i\|}{D_{\mathrm{ref}}}\right)

$D_{\mathrm{ref}}$ 为位置误差归一化尺度，用于控制位置误差在奖励中的敏感范围，其取值与目标船尺寸、作业距离、机动空间大小有关，目标船设置为300m，参数在此取240m

3.2角度误差：

\bar e_{\psi,t}^i=\operatorname{clip}_{[0,1]}\left(\frac{|e_{\psi,t}^i|}{\Psi_{\mathrm{ref}}}\right)

$\Psi_{\mathrm{ref}}$ 为航向误差归一化尺度，其取值与接近目标时的姿态偏差范围有关，在此取90度，即最大允许拖船垂直与目标船航向，如更加严格，可以取更小的数值

3.3相对速度归一化

\bar e_{v,t}^i=\operatorname{clip}_{[0,1]}\left(\frac{\|e_{v,t}^i\|}{V_{\mathrm{ref}}}\right)

$V_{\mathrm{ref}}$ 为相对速度误差归一化尺度，用于刻画拖船与目标船之间速度不匹配的容忍范围，与目标船的速度有关数据中目标船的速度范围集中在6-12节，因此这个参数为0.4倍的最大速度，在此取2.5m/s

第i艘船的基础奖励可以简单的设置为：

\begin{equation}r_i(t)=-w_p\bar e_{p,t}^i+w_{prog}\operatorname{clip}_{[-1,1]}\left(\frac{\left\|e_{p,t-1}^{i}\right\|-\left\|e_{p,t}^{i}\right\|}{d_{step}}\right)\end{equation}

其中，第一项表示误差惩罚，第二项表示接近奖励，其中，dstep=5m

在此处训练完成后，可以加入航向和航速的奖励函数：

\begin{equation}r_i(t)=-w_p\bar e_{p,t}^i+w_{prog}\operatorname{clip}_{[-1,1]}\left(\frac{\left\|e_{p,t-1}^{i}\right\|-\left\|e_{p,t}^{i}\right\|}{d_{step}}\right)-w_{\psi}\bar e_{\psi,t}^i-w_v\bar e_{v,t}^i\end{equation}

\begin{equation} w_p = 1.0, \qquad w_{prog}=1.0, \qquad R_{success}=100, \qquad R_{fail}=50 \end{equation}

4.近场激活函数

\sigma_t^i=\exp\left(-\left(\frac{\|e_{p,t}^i\|}{r_{\mathrm{cap}}}\right)^2\right)

$r_{\mathrm{cap}}$ 为近场激活半径，控制航向误差项和相对速度误差项何时开始显著起作用。其决定拖船何时从接近阶段到精细对接阶段，此处取70m

5.单船任务奖励

J_t^i=w_p \bar e_{p,t}^i+\sigma_t^i\left(w_\psi \bar e_{\psi,t}^i+w_v \bar e_{v,t}^i\right)

$w_p, w_{\psi}, w_v$ 分别为位置误差、航向误差和相对速度误差的权重。其取值主要体现任务偏好，这里取1，0.5，0.7

任务奖励如下：

R_t^{\mathrm{task}}=\frac{1}{N}\sum_{i=1}^{N}\operatorname{clip}_{[-1,1]}\left(J_{t-1}^i - J_t^i\right)

6.安全裕度;

安全裕度主要包括风险项：拖船之间的风险，拖船与动态障碍物的碰撞，拖船与静态障碍物的碰撞，拖船与边界、岸壁等的接近。

6.1拖船之间

定义拖船之间的距离为：

\begin{equation}d_{ij,t}^{tt} = \left\| p_t^i - p_t^j \right\|\end{equation}

其中最小距离为：

\begin{equation}d_{tt,t}^i = \min_{j\neq i} d_{ij,t}^{tt}\end{equation}

拖船之间的风险量化为：

\begin{equation}q_{tt,t}^i =\operatorname{clip}_{[0,1]}\left(\frac{d_{tt}^{safe}-d_{tt,t}^i}{d_{tt}^{safe}}\right)\end{equation}

${d_{tt}^{safe}}$ 设定为50m

6.2环境交互

构建一个以拖船中心为椭圆中心、长轴沿航向方向的对称椭圆，其长轴为四倍船长，短轴为1.6倍船长，拖船和动态障碍物均使用这个船舶领域。

并定义拖船到某一障碍物k的相对角度为：

\begin{equation}\beta_{ik,t}=\operatorname{wrap}\!\left(\operatorname{atan2}(y_{k,t}-y_t^i,\ x_{k,t}-x_t^i)-\psi_t^i\right)\end{equation}

则拖船中心到船舶域边界某一角度的距离为：

\begin{equation} D_i(\beta_{ik,t}) = \frac{a_i b_i} {\sqrt{\,b_i^2\cos^2\beta_{ik,t}+a_i^2\sin^2\beta_{ik,t}\,}} \end{equation}

6.2.1动态障碍物

对第k个障碍物，其某一方向上到领域边界的距离为：

\begin{equation} D_k^{\mathrm{dyn}}(\beta_{ki,t}) = \frac{a_k b_k}{\sqrt{b_k^2\cos^2\beta_{ki,t}+a_k^2\sin^2\beta_{ki,t}}} \end{equation}

则两者领域叠加后的距离为：

\begin{equation} d_{ik,t}^{\mathrm{dyn,clr}} = d_{ik,t}^{\mathrm{cen}} - D(\beta_{ik,t}) - D_k^{\mathrm{dyn}}(\beta_{ki,t}) \end{equation}

取所有障碍物中最危险者：

\begin{equation} d_{dyn,t}^i = \min_{k\in\mathcal O_t^{\mathrm{dyn}}} d_{ik,t}^{\mathrm{dyn,clr}} \end{equation}

风险归一化：

\begin{equation} q_{dyn,t}^i = \operatorname{clip}_{[0,1]} \left( \frac{d_{dyn}^{\mathrm{safe}}-d_{dyn,t}^i}{d_{dyn}^{\mathrm{safe}}} \right) \end{equation}

其中安全距离取20m

6.2.2静态障碍物

静态障碍物的领域设置为圆形，

\begin{equation}d_{ik,t}^{\mathrm{sta,clr}}=d\!\left(p_t^i,\mathcal O_k^{\mathrm{sta}}\right)-D(\beta_{ik,t})\end{equation}\\ \begin{equation}d_{sta,t}^i=\min_{k\in\mathcal O_t^{\mathrm{sta}}}d_{ik,t}^{\mathrm{sta,clr}}\end{equation}\\ \begin{equation}q_{sta,t}^i=\operatorname{clip}_{[0,1]}\left(\frac{d_{sta}^{\mathrm{safe}}-d_{sta,t}^i}{d_{sta}^{\mathrm{safe}}}\right)\end{equation}\\

类似的， $d\!\left(p_t^i,\mathcal O_k^{\mathrm{sta}}\right)$ 为拖船中心到静态障碍物边界的最短距离

在此 $d_{sta}^{\mathrm{safe}}=15m$

6.3安全函数

\begin{equation}R_t^{\mathrm{safe}}=-\frac{1}{N}\sum_{i=1}^{N}\left(w_{tt}(q_{tt,t}^i)^2+w_{sta}(q_{sta,t}^i)^2+w_{dyn}(q_{dyn,t}^i)^2\right)\end{equation}

w_{tt}=1.5,\qquad w_{sta}=1.0,\qquad w_{dyn}=1.3

7. 控制平滑：

R_t^{\mathrm{ctrl}} = -\frac{1}{N}\sum_{i=1}^{N} \left[ \left(\frac{\Delta n_{L,t}^i}{\Delta n_{\mathrm{ref}}}\right)^2+ \left(\frac{\Delta n_{R,t}^i}{\Delta n_{\mathrm{ref}}}\right)^2+ \left(\frac{\Delta \delta_{L,t}^i}{\Delta \delta_{\mathrm{ref}}}\right)^2+ \left(\frac{\Delta \delta_{R,t}^i}{\Delta \delta_{\mathrm{ref}}}\right)^2 \right]

其中， $\Delta\delta_{\mathrm{ref}}$ 为5度（如果还是一直调整，可设置更小数值）

$\Delta n_{\mathrm{ref}}$ 为0.1倍的最大转速

8.时间惩罚：

R_t^{\mathrm{step}}=-c_{\mathrm{step}}

c取0.002，如果存在拖延情况，加大取值

9.最终奖励

R_t=R_t^{\mathrm{term}}+\lambda_{\mathrm{task}} R_t^{\mathrm{task}}+\lambda_{\mathrm{safe}} R_t^{\mathrm{safe}}+\lambda_{\mathrm{ctrl}} R_t^{\mathrm{ctrl}}+R_t^{\mathrm{step}}

$\lambda_{\mathrm{task}} , \lambda_{\mathrm{safe}} ,\lambda_{\mathrm{ctrl}}$ 分别为任务推进项、安全项和控制平滑项的权重，取1.0， 2.0，0.02

10.成功判据：

\|e_{p,t}^i\|\le \varepsilon_p,\qquad |e_{\psi,t}^i|\le \varepsilon_\psi,\qquad \|e_{v,t}^i\|\le \varepsilon_v,\qquad m_t^i\ge 0,\quad \forall i=1,\dots,N

其中参数取值分别为10m， 20度， 0.3m/s ，连续20个时间步

3️⃣被牵引船舶运动设计：

从AIS数据集中提取油轮和货轮的数据进行插值，仿真的时候随机抽取一条，提供一个ship.csv 文件，csv列为： [mmsi, longitude, latitude, speed, heading, timestamp, shiptype]障碍物设计：

这里会提供很多个油轮或者货轮的数据，具体而言其长度应满足200m以上，国籍为外国

base_interp_1s.zip

4️⃣运动障碍物设计：

1. 障碍物船舶类型

中型船舶（货船、客船）

尺寸：长度 50-100m，宽度 10-20m
速度范围：4-7.5 m/s
运动特点：按航道规则航行，转向速率 1-2度/秒

大型船舶（油轮、集装箱船）

尺寸：长度 150-300m，宽度 30-50m
速度范围：5-10 m/s
运动特点：惯性大，转向速率 0.5-1度/秒，需要大转弯半径

2. 运动模式设计

模式1：航路点跟随 (Waypoint Following)

适用场景：船舶按预定航线航行（如进出港航线）
航路点定义：一系列经纬度坐标点 [(lon1,lat1), (lon2,lat2), ...]
到达判定：当船舶距离当前目标航点 < 50m 时，切换到下一个航点
转向控制：平滑转向，避免急转弯

模式2：横穿航道 (Channel Crossing)

适用场景：船舶从航道一侧横穿到另一侧（如渔船、小型货船）
起点和终点：在警戒区两侧边界附近随机生成
横穿角度：尽量垂直于航道方向（减少在航道内停留时间）
避让规则：必须避让主航道内的大型船舶，优先级最低

3. 碰撞避让规则(查一下论文，有没有相关的规范或者规定)

安全距离定义：

中型船舶：100m
大型船舶：200m
拖轮作业区：300m（其他船舶必须避让）

避让优先级（从高到低）：

1. 拖轮作业（最高优先级，其他船舶必须避让）
1. 主航道内的大型船舶
1. 主航道内的中型船舶
1. 横穿航道的船舶（最低优先级）

4. 数据结构设计

obstacles.csv 文件格式：

[id, type,init_time,init_lon, init_lat, init_heading, init_speed, length, width, behavior_pattern, waypoints]

字段说明：

id: 障碍物唯一标识
type: 类型 (2=中型, 3=大型)
init_time:仿真系统中初始化的时间点
init_lon/lat: 初始经纬度
init_heading: 初始航向 (0-360度)
init_speed: 初始速度 (m/s)
length/width: 船体尺寸 (m)
behavior_pattern: 行为模式 (1=航路点跟随, 2=横穿航道)
waypoints: 航路点序列，JSON格式字符串，如 "[(lon1,lat1),(lon2,lat2)]" 或 "null"

5. 实现要点

**碰撞检测：**计算 CPA (Closest Point of Approach) 和 TCPA (Time to CPA)，当 CPA < 安全距离且 TCPA < 阈值时触发避让
**运动更新：**每个时间步长更新位置、航向和速度，考虑转向速率限制
**边界处理：**船舶离开地图范围后移除，可选择在边界动态生成新障碍物
**性能优化：**使用空间分区（如网格）加速碰撞检测，只检测相邻区域的船舶

论文动力学建模：

\tau=\left[ \begin{matrix} {{\cos( \delta_{l} )}} & {{\cos( \delta_{r} )}} \\ {{\sin( \delta_{l} )}} & {{\sin( \delta_{r} )}} \\ {{l_{x} \sin( \delta_{l} )+l_{y} \cos( \delta_{l} )}} & {{l_{x} \sin( \delta_{r} )+l_{y} \cos( \delta_{r} )}} \end{matrix} \right] \! \! T

其中 $\mathbf{T} = [T_l,T_r]^{T}$ , 表示调节左右推进器的转速 $n_l$ 和 $n_r$ 产生的推进力。公式中的 $\delta_{l}$ 和 $\delta_{r}$ 表示左右推进器的旋转角度。 $l_x$ 和 $l_y$ 表示左右推进器距离船舶两个对称轴 $x_b$ 和 $y_b$ 的距离.

矩阵的计算结果是 $3 \times 1$ 的向量，分别代表 [纵向力, 横向力, 首摇力矩]。

控制量： $[n_l,n_r,\delta_{l},\delta_{r}]$

根据船厂提供的数据, $l_x$ 的值为12m. $l_y$ 估计为2.5m

转速与推力的关系为：

T_{l}=K \cdot n_{l} \cdot\left| n_{l} \right|

公式中出现绝对值是为了处理“倒船的情况”，K表示推力系数（由水密度，螺旋桨形状决定）

⚠️： $n_l$ 的单位是rps

K的计算公式如下：

K=K_{T 0} \cdot\rho\cdot D^{4}

$K_{T0}$ 表示系泊工况推力系数，如果没有手册，常规导管螺旋桨的取值在0.3～0.5

$\rho$ 表示水密度，海水的密度为 $1025kg/m^3$

D表示螺旋桨直径，根据手册可知为：2.4m

参考代码：

python
def calculate_thrust(n_rpm, diameter_m, kt_fwd, kt_bwd=None, rho=1025.0):
    """
    计算螺旋桨产生的推力 (Thrust)。
    基于公式: T = K_T * rho * D^4 * n * |n|

    参数:
        n_rpm      (float): 螺旋桨转速，单位：转/分 (RPM)。正数代表正转，负数代表反转。
        diameter_m (float): 螺旋桨直径，单位：米 (m)。
        kt_fwd     (float): 前进(正转)时的推力系数 (无量纲，通常在 0.3~0.5 之间)。
        kt_bwd     (float, optional): 倒车(反转)时的推力系数。如果不传，默认取前进系数的 60%。
        rho        (float, optional): 流体密度，单位：kg/m^3。默认为海水密度 1025.0。

    返回:
        float: 推力，单位：牛顿 (N)。正值表示推力向前，负值表示拉力向后。
    """
    
    # 1. 如果未提供倒车系数，按经验值设定 (通常倒车效率低)
    if kt_bwd is None:
        kt_bwd = kt_fwd * 0.6
        
    # 2. 单位转换: RPM -> RPS (转/秒)
    # 标准流体力学公式中使用的是 RPS
    n_rps = n_rpm / 60.0
    
    # 3. 根据转速方向选择对应的推力系数
    # abs(n_rps)用于计算幅值，符号由 n_rps 决定
    current_kt = kt_fwd if n_rpm >= 0 else kt_bwd
    
    # 4. 计算推力
    # T = K_T * rho * D^4 * n^2
    # 使用 n * |n| 来保留推力的方向性
    thrust = current_kt * rho * (diameter_m ** 4) * n_rps * abs(n_rps)
    
    return thrust

有了推力后继续看整体的动力学模型，又叫**Fossen 模型，这段公式描述的是一个船舶在水平面上的 3 自由度（3-DOF）**运动：纵荡 (Surge, 前后)、**横荡 (Sway, 左右)**和 首摇 (Yaw, 转向)。

先看运动学部分：

\dot{\eta}=R ( \psi) \nu

其中 $\eta=[ x , y , \psi]^{T}$ ， $x$ 和 $y$ 表示船的绝对位置， $\psi$ 表示船的航向角（比如正北是 0 度）。 $\nu=[ u , v , r ]^{T}$ ， $u$ 表示船头朝前的速度， $v$ 表示船侧向漂移的速度， $r$ 表示船转头的角速度。

R表示一个旋转矩阵函数

展开写如下：

\left[ \begin{matrix} {\dot{x}} \\ {\dot{y}} \\ {\dot{\psi}} \\ \end{matrix} \right]=\left[ \begin{matrix} {\operatorname{c o s} ( \psi)} & {-\operatorname{s i n} ( \psi)} & {0} \\ {\operatorname{s i n} ( \psi)} & {\operatorname{c o s} ( \psi)} & {0} \\ {0} & {0} & {1} \\ \end{matrix} \right] \left[ \begin{matrix} {u} \\ {v} \\ {r} \\ \end{matrix} \right]

公式含义：负责把船自身的速度转换成地图上的位移速度。

再看动力学方程（受力分析）：

本质上与 $ma =F$ 一样：

M \dot{\nu}+C ( \nu) \nu+D ( \nu) \nu=\tau+d

⚠️ 3dof仿真的质量是指排水量699 吨

计算质量矩阵 $M$

M=M_{R B}+M_{A}

假设重心 (CG) 位于船体几何中心附近,为了简化，我们暂时忽略重心偏移带来的耦合项。

计算转动惯量： $I_{z}=m \times( k_{z z} L )^{2}$ , $k_{zz}$ 是回转半径，对于拖轮这种宽船体，取值为0.25.

I_{z}=6 9 9 , 0 0 0 \times( 0 . 2 5 \times3 6 )^{2}=6 9 9 , 0 0 0 \times8 1 \approx{\bf5 . 6 6 \times1 0^{7} \; k g \cdot m^{2}}

再计算附加质量矩阵 $M_A$ ,拖轮水下形状复杂，但可用工程经验系数估算：

纵向附加质量 $X_{\dot{u}}$ ,通常为船体质量的5%~10%, $X_{\dot{u}} \approx-0 . 0 5 \times m=-3 4 , 9 5 0 \mathrm{~ k g}$
横向附加质量 $Y_{\dot{v}}$ , 横向阻力大，为船体质量的50%～80%, $Y_{\dot{v}} \approx-0 . 5 \times m=-3 4 9 , 5 0 0 \mathrm{~ k g}$
首摇附加转动惯量 $N_{\dot{r}}$ :通常为刚体转动惯量的40%~60%, $N_{\dot{r}} \approx-0 . 5 \times I_{z}=-2 . 8 3 \times1 0^{7} \: \mathrm{k g} \cdot\mathrm{m}^{2}$

根据以上的结果带入公式可得M矩阵的值：

M=\left[ \begin{matrix} {m-X_{\hat{u}}} & {0} & {0} \\ {0} & {m-Y_{\hat{v}}} & {0} \\ {0} & {0} & {I_{z}-N_{\hat{r}}} \\ \end{matrix} \right]=\left[ \begin{matrix} {7 . 3 4 \times1 0^{5}} & {0} & {0} \\ {0} & {1 . 0 5 \times1 0^{6}} & {0} \\ {0} & {0} & {8 . 4 9 \times1 0^{7}} \\ \end{matrix} \right]

（单位： $kg,kg,kg \cdot m^2$ ）

计算科里奥利矩阵 $C ( \nu)$

这个矩阵不需要新的参数，它是 M 矩阵和当前速度 $\nu=[ u , v , r ]^{T}$ 的函数。

令： $M_{1 1}=7 . 3 4 \times1 0^{5} , M_{2 2}=1 . 0 5 \times1 0^{6}$ ,带入公式可得：

C(\nu)=\left[ \begin{matrix} {0} & {0} & {-M_{2 2} v} \\ {0} & {0} & {M_{1 1} u} \\ {M_{2 2} v} & {-M_{1 1} u} & {0} \\ \end{matrix} \right]=\left[ \begin{matrix} {0} & {0} & {-1 . 0 5 \times1 0^{6} \cdot v} \\ {0} & {0} & {7 . 3 4 \times1 0^{5} \cdot u} \\ {1 . 0 5 \times1 0^{6} \cdot v} & {-7 . 3 4 \times1 0^{5} \cdot u} & {0} \\ \end{matrix} \right]

注：科里奥利矩阵的数值随速度变化

计算计算阻力矩阵 $D(\nu)$

阻力分为线性阻力（低速时主导，如靠泊微操）和非线性阻力（高速时主导）。我们主要计算非线性部分，因为它是主要的能量消耗项。

D_{N L}=\mathrm{d i a g} ( X_{\vert u \vert u} \vert u \vert, Y_{\vert v \vert v} \vert v \vert, N_{\vert r \vert r} \vert r \vert)

在计算之前需要计算吃水T :

已知船长 (L): 36 m，船宽 (B): 11 m，质量 (m): 699 t=699,000 kg， 海水密度 (ρ): $1025 kg/m^3$

排水量公式:

\boldsymbol{\Delta}=\boldsymbol{L} \times\boldsymbol{B} \times\boldsymbol{T} \times\boldsymbol{C}_{b} \times\boldsymbol{\rho}

带入可得： $6 9 9 , 0 0 0=3 6 \times1 1 \times T \times0 . 5 8 \times1 0 2 5$

解得：

T \approx3 . 0 \; \mathrm{m}

A. 纵向二次阻力系数 $X_{| u | u}$

基于正面投影面积估算。

正面水下投影面积 $A_{F} \approx B \times T=1 1 \times3 . 0=3 3 \; \mathrm{m}^{2}$
纵向阻力系数 $C_{Dx}$ , 一般船型取0.05～0.1
计算可得： $X_{| u | u} \approx-\frac{1} {2} \rho A_{F} C_{D x}=-0 . 5 \times1 0 2 5 \times3 3 \times0 . 1 \approx-\mathbf{1 , 7 0 0} \mathrm{~ k g / m}$

⚠️：如果这艘拖船很钝（方头），纵向阻力系数可能会更大

B. 横向二次阻力系数 $Y_{| v | v}$ 这是拖轮最重要的参数，因为侧向漂移阻力巨大。

侧面水下投影面积 $A_{L} \approx L \times T=3 6 \times3 . 0=1 0 8 \; \mathrm{m}^{2}$
横向阻力系数 $C_{Dy}$ ,近似看作平板，取0.9
代入可得： $Y_{| v | v} \approx-\frac{1} {2} \rho A_{L} C_{D y}=-0 . 5 \times1 0 2 5 \times1 0 8 \times0 . 9 \approx-\bf5 0 , 0 0 0 \ k g / m$

C. 首摇二次阻力系数 $N_{| r | r}$

这是一个力矩阻力，通常通过对横向阻力沿船长积分得到。

经验公式: $N_{| r | r} \approx\frac{1} {1 0} \times\lvert Y_{| v | v} \rvert\times L^{2}$
代入计算可得： $N_{| r | r} \approx0 . 1 \times5 0 , 0 0 0 \times3 6^{2} \approx-{\bf6} , {\bf5 0 0} , {\bf0 0 0} \, \, \mathrm{k g} \cdot{\bf m}^{\bf2}$

D. 线性阻力系数（用于防止零速度除零错误）

通常取非线性系数的 1/10 或更小，主要用于数学稳定性。

$X_{u} \approx-5 0 0$
$Y_{v} \approx-5 0 0 0$
$N_{r} \approx-1 , 0 0 0 , 0 0 0$

$D(\nu)$ 矩阵实现:

D ( \nu)=\left[ \begin{matrix} {5 0 0+1 7 0 0 | u |} & {0} & {0} \\ {0} & {5 0 0 0+5 0 0 0 0 | v |} & {0} \\ {0} & {0} & {1 0^{6}+6 . 5 \times1 0^{6} | r |} \\ \end{matrix} \right]

(注意：公式中通常带负号，这里写的是绝对值，放在方程中时，正值即代表耗散)

物理引擎构建：

1.输入：当前的螺旋桨推力 (τ) 和环境干扰 (d)。

2.计算加速度：根据动力学方程，算出 $\dot{\nu}$ ：

\dot{\nu}=M^{-1} ( \tau+d-C ( \nu) \nu-D ( \nu) \nu)

3.引擎更通过时间步积分得到速度： $\nu_{n e w}=\nu_{o l d}+\dot{\nu} \cdot\Delta t$ 4.坐标变换：利用航向角 $\psi$ 和旋转矩阵，将 $\nu$ 转换为 $\dot{\eta}$

5.引擎更通过时间步积分得到位姿： $\eta_{n e w}=\eta_{o l d}+\dot{\eta} \cdot\Delta t$

⚠️： $M^{-1}$ 有解析解，只需要计算一次即可。

接下来的工作

1.场景数据加入

1.加入目标船舶
2.加入干扰船舶，数量依次增加，航道内为追越对遇，警戒区为交叉

2.整合场景数据跟船舶动力学

1.船舶动力学验证，包括回环验证这z字验证
场景数据与船舶动力学整合

3.打通godot强化学习框架

网址：godot_rl_agents/README.md 主页 ·埃德比奇/godot_rl_agents ·GitHub 已实现参考案例

4.强化学习框架

1.总体框架为集中训练分布式执行（CTDE）

5.论文章节安排

1.引言

2.动力学（或者是基础这一块的描述）

3.强化学习框架介绍

4.场景搭建与数据处理

5.仿真实验

6.结论

目录