6、海洋机器人导航数据结构与控制概念解析

海洋机器人导航数据结构与控制概念解析

1. 导航数据结构基础

在海洋机器人导航中，为了便于描述和处理数据，我们会对导航数据进行合理的结构化。首先，我们可以将位姿坐标组合在位姿向量 $\eta_i$ 中，它能进一步分解为位置向量 $\mathbf{p}$ 和方向/姿态向量 $\boldsymbol{\Theta}$，具体表示如下：
$\eta_i = \begin{bmatrix} \mathbf{p} \ \boldsymbol{\Theta} \end{bmatrix}; \quad \mathbf{p} = \begin{bmatrix} x \ y \ z \end{bmatrix}; \quad \boldsymbol{\Theta} = \begin{bmatrix} \phi \ \theta \ \psi \end{bmatrix}$

这里涉及到几个重要的参考框架，如地球中心固定（ECEF）框架 $X_EY_EZ_E$、大地坐标系 $\lambda, \varphi, h$ 以及局部东北天（NED）框架 $X_NY_NZ_N$。

2. 速度和力/力矩描述的固定坐标系

在海洋导航里，我们不仅关注机器人的位姿估计，还关心其速度，也就是位姿坐标的一阶导数。不过，位姿和速度可能会在不同的坐标系中表示，因此需要进行坐标转换。

通常，我们会将速度、力和力矩等数据转换到以机器人重心（CG）为原点，坐标轴沿机器人惯性主轴方向的固定坐标系中。我们用下标 0 来表示坐标轴，$X_0$ 是从船尾到船头的纵轴，$Y_0$ 是指向右舷的横轴，$Z_0$ 是向下的法轴。使用上标 $b$ 表示固定坐标系（也称为 b - 框架），可以引入速度向