30、协作导航方法解析与仿真验证

协作导航方法解析与仿真验证

在协作导航领域，准确的位置和速度估计对于多机器人团队的协同作业至关重要。本文将详细介绍协作导航中的相关方法，包括线性和非线性模型的构建、卡尔曼滤波器的应用以及仿真验证。

1. 协作导航基础与估计稳定性

在车辆 0 处，成功通信完成后的时间步 $t_1^R$ 可获取测量数据，这些数据与通信开始前时间步 $t_1^T$ 车辆 1 的位置和速度相关。车辆 0 处的滤波器 1 虽无法确切知晓通信起始时间，但可通过典型通信过程长度 $S_{ac}$ 进行估计：
$ \hat{t} 1^T = t_1^R - S {ac} $

滤波器先恢复到 $\hat{t}_1^T$ 之后的状态，基于接收到的测量值进行后验估计，接着在 $\hat{t}_1^T$ 和 $t_1^R$ 之间进行多次先验估计，之后持续进行实时先验估计，直至有新的测量数据。

由于系统本身存在不稳定行为，估计稳定性十分关键。可区分两种情况：
- 情况 1：连续通信正常时，卡尔曼滤波器可进行先验和后验估计。因系统线性且时不变，估计误差在无限时间内收敛到零，估计稳定。
- 情况 2：无通信连接时，只能进行先验估计，估计误差可能持续增长。

当情况 1 和情况 2 的时间比满足 $t_1/t_2 > e$ 时，估计稳定，阈值 $e$ 取决于系统动态、滤波器设计和任务场景。

2. 线性系统模型与先验估计

对于线性系统模型，引入状态向量 $\mathbf{x} {01}$ 作为时间步 $k$ 的函数：
$ \mathbf{x} {01}(k) = [x_{01,0}(k) \quad y_{01,0}(k) \quad v_{i,x,1}(k) \quad v_{i,y,1}(k) \quad c_{i,x}(k) \quad c_{i,y}(k)]^T $

状态差分方程如下：
$
\begin{cases}
x_{01,0}(k + 1) = x_{1,0}(k) + t_{step}(v_{x,1}(k) - v_{x,0}(k)) \
y_{01,0}(k + 1) = y_{1,0}(k) + t_{step}(v_{y,1}(k) - v_{y,0}(k)) \
v_{i,x,1}(k + 1) = v_{x,1}(k) + w_{v,x}(k) \
v_{i,y,1}(k + 1) = v_{y,1}(k) + w_{v,y}(k) \
c_{i,x}(k + 1) = c_{x}(k) \
c_{i,y}(k + 1) = c_{y}(k)
\end{cases}
$

这些方程代表的物理量如下：
- 前两个状态表示车辆 1 相对于车辆 0 的相对位置的 $x$ 和 $y$ 坐标。
- 第三和第四个状态表示车辆 1 惯性速度的 $x$ 和 $y$ 分量。
- 最后两个状态表示惯性系统中海流的 $x$ 和 $y$ 分量。

车辆 0 的惯性速度向量 $\mathbf{v} 0(k) = [v {x,0}(k) \quad v_{y,0}(k)]^T$ 表示其对地速度，作为系统输入。由于车辆 0 未配备 DVL 系统，无法直接测量对地速度，可通过估计其水中航速 $s_{u,0}$ 和从 AHRS 接收的航向角 $\psi_2$ 计算水中速度向量 $\mathbf{v} {w,0}$，结合海流速度 $\mathbf{c}(k)$ 可得：
$ \mathbf{v}_0(k) = \mathbf{v} {w,0}(k) + \mathbf{c}(k) $

$ \mathbf{v} 0(k) = \begin{bmatrix} v {x,0}(k) \ v_{y,0}(k) \end{bmatrix} = \begin{bmatrix} s_{u,0}(k) \cdot \cos(\psi_0(k)) + c_{x}(k) \ s_{u,0}(k) \cdot \sin(\psi_0(k)) + c_{y}(k) \end{bmatrix} $

$ \chi_0(k) = \text{atan2}(s_{u,0}(k) \cdot \sin(\psi_0(k)) + c_{x}(k), s_{u,0}(k) \cdot \cos(\psi_0(k)) + c_{y}(k)) $

将上述关系代入状态方程，可得线性系统方程的矩阵表示：
$ \mathbf{x} {01}(k + 1) = \mathbf{A} {01} \mathbf{x} {01}(k) + \mathbf{B} {01} \mathbf{u} {01}(k) + \mathbf{L} {01} \mathbf{w}(k) $

其中：
$ \mathbf{A} {01} =
\begin{bmatrix}
1 & 0 & t {step} & 0 & -t_{step} & 0 \
0 & 1 & 0 & t_{step} & 0 & -t_{step} \
0 & 0 & 1 & 0 & 0 & 0 \
0 & 0 & 0 & 1 & 0 & 0 \
0 & 0 & 0 & 0 & 1 & 0 \
0 & 0 & 0 & 0 & 0 & 1
\end{bmatrix}
$

$ \mathbf{B} {01} =
\begin{bmatrix}
-t {step} & 0 \
0 & -t_{step} \
0 & 0 \
0 & 0 \
0 & 0 \
0 & 0
\end{bmatrix}
$

$ \mathbf{u} {01}(k) = s {u,0} \cdot \begin{bmatrix} \cos(\psi_0(k)) \ \sin(\psi_0(k)) \end{bmatrix} $

$ \mathbf{L}_{01} =
\begin{bmatrix}
0 & 0 \
0 & 0 \
0 & 0 \
0 & 0 \
1 & 0 \
0 & 1
\end{bmatrix}
$

$ \mathbf{w}(k) = \begin{bmatrix} w_{v,x}(k) \ w_{v,y}(k) \end{bmatrix} $

滤波器 1 的先验估计可按相关流程进行，协方差矩阵方程需调整为：
$ \mathbf{P} {01}^-(k + 1) = \mathbf{A} {01} \mathbf{P} {01}^+(k) \mathbf{A} {01}^T + \mathbf{L} {01} \mathbf{Q} {01} \mathbf{L}_{01}^T $

其中 $ \mathbf{Q}_{01} = \begin{bmatrix} \sigma_v^2 & 0 \ 0 & \sigma_v^2 \end{bmatrix} $

3. 测量模型与后验估计

成功通信时，滤波器 1 可获取车辆 1 的惯性速度（滤波器 0 的估计值），可能还有 USBL 测量值。滤波器 3 情况略有不同，车辆 3 无法测量对地速度，通过声学通道传输其水中航速 $s_{u,2}$ 和航向角 $\psi_2$，滤波器 3 接收后根据相关方程计算车辆 2 对地速度的伪测量值。

成功进行 USBL 测量时，滤波器可获取距离 $\hat{r} 1^{3D}$、方位角 $\hat{\alpha} {01}$ 等。为使用线性卡尔曼滤波器，需计算线性伪测量值。首先计算车辆 0 在 $t_1^R$ 时刻相对于 $\hat{t} 1^T$ 时刻的位置：
$
\begin{bmatrix}
\hat{x} {0,\hat{t} 1^T}^0(t_1^R) \
\hat{y} {0,\hat{t} 1^T}^0(t_1^R)
\end{bmatrix}
=
\begin{bmatrix}
\sum {i = \hat{t} 1^T + t {step}}^{t_1^R} [t_{step}(s_{u,0}(i) \cdot \cos(\psi_0(i)) + c_{x}(i))] \
\sum_{i = \hat{t} 1^T + t {step}}^{t_1^R} [t_{step}(s_{u,0}(i) \cdot \sin(\psi_0(i)) + c_{y}(i))]
\end{bmatrix}
$

车辆 1 相对于车辆 0 的水平距离为：
$
\begin{cases}
r_1(t_j^R) = \sqrt{\hat{r} 1^{3D2}(t_j^R) - \hat{z}_0^2(t_j^R)} \
r_2(t_j^R) = \sqrt{\hat{r}_2^{3D2}(t_j^R) - (\hat{z} {02}(t_j^R) - \hat{z}_0(t_j^R))^2}
\end{cases}
$

车辆 1 相对位置的伪测量值为：
$
\begin{bmatrix}
\hat{x} {1,0}(t_j^R) \
\hat{y} {1,0}(t_j^R)
\end{bmatrix}
=
\begin{bmatrix}
\hat{x} {2,\hat{t}_1^T}^2(t_1^R) + r_1(t_j^R) \cdot \cos(\alpha {01}(t_j^R) + \psi_0(t_j^R)) \
\hat{y} {2,\hat{t}_1^T}^2(t_1^R) + r_1(t_j^R) \cdot \sin(\alpha {01}(t_j^R) + \psi_0(t_j^R))
\end{bmatrix}
$

测量向量 $\mathbf{y}$ 包含四个参数（USBL 测量成功时）或仅包含惯性速度（USBL 测量失败时）。协方差矩阵 $\mathbf{R} {01}$ 维度为 4x4 或 2x2，输出矩阵 $\mathbf{C} {01}$ 根据是否有 USBL 测量值设计：
$
\mathbf{C} {01} =
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 & 0 \
0 & 1 & 0 & 0 & 0 & 0 \
0 & 0 & 1 & 0 & 0 & 0 \
0 & 0 & 0 & 1 & 0 & 0
\end{bmatrix}
\quad \text{或} \quad
\mathbf{C} {01} =
\begin{bmatrix}
0 & 0 & 1 & 0 & 0 & 0 \
0 & 0 & 0 & 1 & 0 & 0
\end{bmatrix}
$

后验估计可根据卡尔曼滤波器算法进行，滤波器 3 和 4 与滤波器 1 算法基本相同，滤波器 4 无需进行后验估计的预处理。

4. 仿真验证

在 MATLAB 中创建了仿真环境，可对车辆、通信、测量和不同滤波器进行完整仿真。在相关项目中，还在特定仿真框架内进行了仿真测试，开发了基于 ROS 的中间件系统用于协调控制和 HIL 仿真。

将 MATLAB 软件导出为 C++ 代码并放入 ROS 包装器中，与其他实际海试中使用的软件组件一起进行仿真。仿真结果表明，导航解决方案能以较高精度为控制算法提供合理数据，任务可成功执行。

滤波器 1 的位置估计误差在笛卡尔 $x$ 和 $y$ 方向以及总水平位置估计误差的仿真结果显示，总估计误差均值为 1.1 m，方差为 0.07 m。

5. 线性滤波器与无迹卡尔曼滤波器仿真结果对比

为了更直观地比较线性卡尔曼滤波器和无迹卡尔曼滤波器（UKF）的性能，进行了多次独立的仿真运行。以下是滤波器 1 在不同滤波器架构下的误差均值和标准差对比：
| 滤波器类型 | 运行次数 | 位置误差均值 [m] | 位置误差标准差 [m] | 速度误差均值 [m/s] | 速度误差标准差 [m/s] |
| — | — | — | — | — | — |
| 线性卡尔曼滤波器 | 1 | 0.84 | 0.58 | 0.040 | 0.026 |
| 线性卡尔曼滤波器 | 2 | 0.65 | 0.35 | 0.038 | 0.025 |
| 线性卡尔曼滤波器 | 3 | 0.91 | 0.52 | 0.039 | 0.025 |
| 线性卡尔曼滤波器 | 4 | 0.72 | 0.40 | 0.037 | 0.025 |
| 线性卡尔曼滤波器 | 5 | 0.71 | 0.43 | 0.037 | 0.025 |
| 线性卡尔曼滤波器 | 平均 | 0.77 | 0.46 | 0.038 | 0.025 |
| 无迹卡尔曼滤波器 | 1 | 0.43 | 0.28 | 0.022 | 0.018 |
| 无迹卡尔曼滤波器 | 2 | 0.51 | 0.33 | 0.025 | 0.017 |
| 无迹卡尔曼滤波器 | 3 | 0.47 | 0.26 | 0.026 | 0.019 |
| 无迹卡尔曼滤波器 | 4 | 0.48 | 0.29 | 0.026 | 0.018 |
| 无迹卡尔曼滤波器 | 5 | 0.40 | 0.26 | 0.021 | 0.015 |
| 无迹卡尔曼滤波器 | 平均 | 0.46 | 0.28 | 0.024 | 0.017 |

从上述表格可以看出，无迹卡尔曼滤波器在相对水平位置估计和惯性速度估计方面的误差均值和标准差普遍低于线性卡尔曼滤波器，说明无迹卡尔曼滤波器具有更好的估计性能。

6. 总结

本文详细介绍了协作导航中的线性和非线性方法，包括目标模型、测量模型的构建，以及卡尔曼滤波器和无迹卡尔曼滤波器的应用。通过仿真验证，展示了不同方法的性能特点。线性方法在一定程度上解决了导航问题，但存在局限性；无迹卡尔曼滤波器在处理非线性问题时表现更优，能提供更准确的估计结果。在实际应用中，可根据具体任务需求和系统特点选择合适的导航方法。

协作导航方法解析与仿真验证

7. 非线性方法：无迹卡尔曼滤波器的应用

虽然线性卡尔曼滤波器在协作导航中取得了一定的成果，但为了追求更优的性能，研究了无迹卡尔曼滤波器（UKF）的应用。UKF 能够处理非线性系统，避免了线性化带来的误差。

7.1 非线性目标模型

使用无迹卡尔曼滤波器时，采用了新的车辆和测量模型，这些模型具有非线性特性。对于滤波器 1，状态向量定义如下：
$
\mathbf{x} {01}(k) =
\begin{bmatrix}
x {01,0}(k) \
y_{01,0}(k) \
v_{1,0}(k) \
\chi_{i,1,0}(k) \
v_1(k) \
\chi_{i,1}(k) \
r_1(k) \
c_{i,x}(k) \
c_{i,y}(k)
\end{bmatrix}
$

状态差分方程为：
$
\begin{cases}
x_{1,0}(k + 1) = x_{1,0}(k) + t_{step}(v_{1,0}(k) \cdot \cos(\chi_{1,0}(k))) \
y_{1,0}(k + 1) = y_{1,0}(k) + t_{step}(v_{1,0}(k) \cdot \sin(\chi_{1,0}(k))) \
v_{1,0}(k + 1) = \sqrt{(v_1(k) \cdot \cos(\chi_1(k)) - u_{2,x}(k) - c_x(k))^2 + (v_1(k) \cdot \sin(\chi_1(k)) - u_{2,y}(k) - c_y(k))^2} \
\chi_{1,0}(k + 1) = \text{atan2}(v_1(k) \cdot \sin(\chi_1(k)) - u_{2,y}(k) - c_y(k), v_1(k) \cdot \cos(\chi_1(k)) - u_{2,x}(k) - c_x(k)) \
v_1(k + 1) = v_1(k) + w_v(k) \
\chi_1(k + 1) = \chi_1(k) + t_{step} \cdot r_1(k) + w_{\chi}(k) \
r_1(k + 1) = r_1(k) + w_r(k) \
c_x(k + 1) = c_x(k) \
c_y(k + 1) = c_y(k)
\end{cases}
$

其中，$w_v$、$w_{\chi}$ 和 $w_r$ 为高斯分布的零均值白噪声，用于模拟 SSV 的未知运动。输入值 $u_{0,x}(k)$ 和 $u_{0,y}(k)$ 表示车辆 0 的水中速度分量：
$
\mathbf{u} {01}(k) =
\begin{bmatrix}
u {0,x}(k) \
u_{0,y}(k)
\end{bmatrix}
= s_{u,0}(k)
\begin{bmatrix}
\cos(\psi_0(k)) \
\sin(\psi_0(k))
\end{bmatrix}
$

7.2 非线性测量模型

每次成功通信后，可获取车辆 1 惯性速度的测量值 $v’ {x,1}$ 和 $v’ {y,1}$。若 USBL 测量成功，还可获取 $\alpha’ {01}$ 和 $r’_1^{3D}$ 的测量值。测量向量定义如下：
$
\mathbf{y} {21}(t_j^R) =
\begin{bmatrix}
v_{x,1}(t_j^R) \
v_{y,1}(t_j^R) \
r_1(t_j^R) \
\alpha_{01}(t_j^R)
\end{bmatrix}^T
=
\begin{bmatrix}
v_1(t_j^R) \cdot \cos(t_j^R) \
v_1(t_j^R) \cdot \sin(t_j^R) \
\sqrt{(x_{1,0}(t_j^R) - \hat{x} {0,\hat{t}_1^T}^0(t_1^R))^2 + (y {1,0}(t_j^R) - \hat{y} {0,\hat{t}_1^T}^0(t_1^R))^2} \
\text{atan2}(y {1,0}(t_j^R) - \hat{y} {0,\hat{t}_1^T}^0(t_1^R), x {1,0}(t_j^R) - \hat{x}_{0,\hat{t}_1^T}^0(t_1^R)) - \psi_0(t_j^R)
\end{bmatrix}
$

若没有 USBL 测量值，$\mathbf{y} {21}$ 仅包含前两个元素。协方差矩阵 $\mathbf{R} {01}$ 的维度为 4x4 或 2x2。

使用无迹卡尔曼滤波器时，设置参数 $\alpha = 0.001$，$\beta = 2$，$\kappa = 3$。

7.3 仿真验证

在 MATLAB 仿真中，假设车辆按预定轨迹以 0.5 m/s 的恒定速度行驶，以解耦导航滤波器测试和控制算法。仿真场景用于测试线性卡尔曼滤波器和无迹卡尔曼滤波器。仿真环境的参数设置如下：
- 声学通信成功率为 90%。
- 成功通信时接收到 USBL 测量值的概率为 88.9%。
- 测量值叠加零均值白噪声，标准差分别为：速度 0.1 m/s，航向角 2°，USBL 距离 0.2 m、方位角 3°，RTK GPS 位置 0.01 m、速度 0.01 m/s，深度测量 0.01 m。

线性滤波器的测量噪声协方差矩阵 $\mathbf{R}$ 设置为 $\text{diag}(0.05 m^2, 0.05 m^2, 0.01 m^2/s^2, 0.01 m^2/s^2)$，过程噪声协方差矩阵 $\mathbf{Q}$ 设置为 $\text{diag}(2.5 \times 10^{-7} m^2/s^2, 2.5 \times 10^{-7} m^2/s^2)$。UKF 的 $\mathbf{Q}$ 设置为 $\text{diag}(2.5 \times 10^{-7} m^2/s^2, 6.25 \times 10^{-6} rad, 1.44 \times 10^{-6} rad/s)$，$\mathbf{R}$ 设置为 $\text{diag}(0.05 m^2/s^2, 0.05 m^2/s^2, 0.25 m^2, 0.2 rad)$。

进行了五次独立仿真运行，比较滤波器 1 对车辆 1 相对水平位置和惯性速度估计的误差均值和标准差，结果如下表所示：
| 滤波器类型 | 运行次数 | 位置误差均值 [m] | 位置误差标准差 [m] | 速度误差均值 [m/s] | 速度误差标准差 [m/s] |
| — | — | — | — | — | — |
| 线性卡尔曼滤波器 | 1 | 0.84 | 0.58 | 0.040 | 0.026 |
| 线性卡尔曼滤波器 | 2 | 0.65 | 0.35 | 0.038 | 0.025 |
| 线性卡尔曼滤波器 | 3 | 0.91 | 0.52 | 0.039 | 0.025 |
| 线性卡尔曼滤波器 | 4 | 0.72 | 0.40 | 0.037 | 0.025 |
| 线性卡尔曼滤波器 | 5 | 0.71 | 0.43 | 0.037 | 0.025 |
| 线性卡尔曼滤波器 | 平均 | 0.77 | 0.46 | 0.038 | 0.025 |
| 无迹卡尔曼滤波器 | 1 | 0.43 | 0.28 | 0.022 | 0.018 |
| 无迹卡尔曼滤波器 | 2 | 0.51 | 0.33 | 0.025 | 0.017 |
| 无迹卡尔曼滤波器 | 3 | 0.47 | 0.26 | 0.026 | 0.019 |
| 无迹卡尔曼滤波器 | 4 | 0.48 | 0.29 | 0.026 | 0.018 |
| 无迹卡尔曼滤波器 | 5 | 0.40 | 0.26 | 0.021 | 0.015 |
| 无迹卡尔曼滤波器 | 平均 | 0.46 | 0.28 | 0.024 | 0.017 |

从表格中可以看出，无迹卡尔曼滤波器在位置和速度估计方面的误差均值和标准差均小于线性卡尔曼滤波器，表明其具有更好的性能。

8. 方法对比与选择建议

通过对线性卡尔曼滤波器和无迹卡尔曼滤波器的仿真验证，可以总结出以下对比结果：
- 线性卡尔曼滤波器 ：
- 优点 ：实现简单，计算效率高。
- 缺点 ：需要对系统进行线性化，可能引入误差，在处理非线性系统时性能受限。
- 无迹卡尔曼滤波器 ：
- 优点 ：能够处理非线性系统，无需线性化，估计精度高。
- 缺点 ：计算复杂度相对较高。

在实际应用中，可根据以下因素选择合适的导航方法：
- 系统特性 ：如果系统接近线性，线性卡尔曼滤波器可能是一个不错的选择；如果系统具有明显的非线性特性，无迹卡尔曼滤波器更适合。
- 计算资源 ：如果计算资源有限，线性卡尔曼滤波器可能更易于实现；如果计算资源充足，可以考虑使用无迹卡尔曼滤波器以获得更高的估计精度。
- 任务要求 ：对于对估计精度要求不高的任务，线性卡尔曼滤波器可能足够；对于对精度要求较高的任务，无迹卡尔曼滤波器更能满足需求。

9. 未来展望

协作导航技术在海洋机器人团队中具有重要的应用前景，但仍有许多方面值得进一步研究和改进：
- 多传感器融合 ：结合更多类型的传感器，如视觉传感器、激光雷达等，以提高导航的准确性和可靠性。
- 实时性优化 ：进一步优化滤波器算法，减少计算时间，提高导航系统的实时性。
- 复杂环境适应性 ：研究在复杂海洋环境（如强海流、多障碍物等）下的导航方法，提高系统的适应性和鲁棒性。

通过不断的研究和创新，协作导航技术将在海洋探索、资源开发等领域发挥更大的作用。

10. 总结

本文全面介绍了协作导航中的线性和非线性方法，包括目标模型、测量模型的构建，以及卡尔曼滤波器和无迹卡尔曼滤波器的应用。通过详细的仿真验证，对比了线性卡尔曼滤波器和无迹卡尔曼滤波器的性能。结果表明，无迹卡尔曼滤波器在处理非线性系统时具有明显优势，能够提供更准确的估计结果。在实际应用中，应根据系统特性、计算资源和任务要求选择合适的导航方法。未来，协作导航技术有望在多传感器融合、实时性优化和复杂环境适应性等方面取得进一步的突破。