33、基于格拉姆矩阵的可观测状态与最优轨迹研究

基于格拉姆矩阵的可观测状态与最优轨迹研究

1. 引言

在海洋机器人领域，最优传感器放置是一个关键问题，它对于准确获取目标信息至关重要。在之前的研究中，通过模拟发现，当目标保持在恒定且已知的深度时，能够计算出一个围绕目标位置的最优半径，以便在该圆周上放置参考物体（ROs）。本文将采用不同的方法，基于可观测性格拉姆矩阵（Observability Gramian matrix）来获取最优传感器放置的信息，并进一步探讨如何为单个 RO 找到最优轨迹。

2. 多 RO 系统中不同系统状态的可观测性检查

2.1 任务场景与建模

• 场景设定 ：进行二维分析，目标是一个海洋机器人，它将在圆形轨迹上移动。同时，最多放置四个参考物体（ROs）在指定位置，且这些 ROs 保持静止。
• 目标模型 ：目标采用基于随机游走与恒定转弯率（RWCTR）的运动学模型。不过，该非线性系统矩阵分析显示系统仅为临界稳定，因此忽略过程噪声，并引入两个输入（速度和航向），由两个 P - 控制器进行设定。目标模型结构如下：
  [
  \mathbf{x}(k + 1) = \overline{\mathbf{F}}(\mathbf{x}(k))\mathbf{x}(k) + \mathbf{B}\mathbf{u}(k)
  ]
  其中
  [
  \overline{\mathbf{F}}(\mathbf{x}(k)) =
  \begin{bmatrix}
  1 & 0 & t_{step}\cdot\cos\chi_0(k) & 0 & 0 \
  0 & 1 & t_{step}\cdot\sin\chi_0(k) & 0 & 0 \
  0 & 0 & 1 & 0 & 0 \
  0 & 0 & 0 & 1 & t_{step} \
  0 & 0 & 0 & 0 & 1
  \end{bmatrix}
  ]
  [
  \mathbf{B} =
  \begin{bmatrix}
  0 & 0 \
  0 & 0 \
  0 & 0 \
  1 & 0 \
  0 & 1
  \end{bmatrix}
  ]
• 参考轨迹 ：目标的参考值通过一个虚拟车辆计算得出，该虚拟车辆以参考速度沿参考圆移动。目标的航向参考始终指向虚拟车辆的当前位置，速度参考则用于使目标与虚拟车辆保持一定距离。
• 测量模型 ：为简化可观测性分析，假设在每个时间步 (t_{step}) 后，目标与每个 RO 之间的距离测量值可立即获取，忽略损耗、延迟和测量噪声。距离测量公式为：
  [
  \tilde{r}_i(k) = r_i(k) = \sqrt{(x_0(k) - x_i(k))^2 + (y_0(k) - y_i(k))^2}
  ]

2.2 基于经验格拉姆矩阵的可观测性分析

• 非线性系统可观测性评估的复杂性 ：与线性时不变系统的可观测性理论不同，非线性系统的可观测性评估更为复杂。其不可观测空间取决于初始状态、输入变量的变化过程以及执行器 - 传感器配置。局部弱可观测性可通过 Herman - Krener 秩条件进行评估，但该评估结果仅为二元决策，无法定性比较不同系统的可观测性水平。
• 可观测性格拉姆矩阵的作用 ：可观测性格拉姆矩阵 (W_O) 可以量化从初始状态 (x_0) 到输出的广义能量传递 (E_O)，公式为：
  [
  E_0 = \int_{0}^{\infty}\mathbf{y}^T(\tau)\mathbf{y}(\tau)d\tau = \mathbf{x}_0^T\mathbf{W}_O\mathbf{x}_0
  ]
  然而，对于非线性系统，需要使用经验格拉姆矩阵 (W_O^E) 进行近似计算。
• 经验格拉姆矩阵的计算步骤 ：
  1. 初始化 ：确定基于初始状态 (\mathbf{x} 0 = \mathbf{x} {nom}) 的标称参考输出轨迹 (\mathbf{y}_0(t))，通过数值积分求解状态微分方程得到。
  2. 扰动初始条件 ：对初始条件进行 (i_{max}) 次小扰动 (\Delta\mathbf{x} i)，得到 (\mathbf{x} {0,i} = \mathbf{x} {nom} + \Delta\mathbf{x}_i)，其中 (i = 1, \ldots, i {max})。
  3. 计算扰动后的输出轨迹 ：通过数值积分确定相应的输出轨迹 (\mathbf{y}_i(t))。
  4. 扰动的组成 ：扰动 (\Delta\mathbf{x} i) 由 (c)、(\mathbf{S}) 和 (\mathbf{t}_i^T) 组成，即 (\Delta\mathbf{x}_i = c\mathbf{S}\mathbf{t}_i^T)，其中 (c \in [0, 1]) 是缩放因子，(\mathbf{S} = diag{\mathbf{x} {max}})，(\mathbf{t}_i^T) 定义了每个 (i) 的扰动方向。
  5. 构建矩阵 (\mathbf{T}) ：为了进行全因子实验设计，使用所有可能的 - 1 和 + 1 的组合构成行向量，然后组合成矩阵 (\mathbf{T})。
  6. 计算经验格拉姆矩阵 ：
     对于连续时间系统：
     [
     W_{O^E,ij} = (c\mathbf{S})^{-1}\int_{0}^{\infty}\mathbf{t} i^T\Psi {ij}(t)\mathbf{t} jdt(c\mathbf{S})^{-1}
     ]
     其中 (\Psi {ij}(t) = (\mathbf{y} i(t) - \mathbf{y}_0(t))^T(\mathbf{y}_j(t) - \mathbf{y}_0(t)))。
     对于离散时间系统：
     [
     W {O^E,ij} = (c\mathbf{S})^{-1}\sum_{k = 1}^{k_{max}}\mathbf{t} i^T\Psi {ij}(k)\mathbf{t} j t {step}(c\mathbf{S})^{-1}
     ]
• 可观测性分析 ：通过对经验格拉姆矩阵进行奇异值分解来分析可观测性。最大奇异值 (\lambda_{max}) 量化了可观测性最佳状态的能量传递，最小奇异值 (\lambda) 表示最不可观测状态的能量传递。若 (\lambda) 为零，则系统不可观测；若 (\lambda) 小于 (0.01\lambda_{max})，则认为系统在实际应用中不可观测。

2.3 模拟与结果

• 模拟设置 ：目标从原点开始，沿逆时针方向在圆形轨迹上运行一圈。使用不同数量的 ROs 和不同半径的目标圆形轨迹进行实验，计算经验格拉姆矩阵及其特征值。
• 结果分析 ：
  • 特征值变化趋势 ：最小特征值随着 ROs 数量的增加和目标圆形轨迹半径的增大而增大。当只有一个 RO 时，系统不可观测，因为仅根据一个 RO 的距离测量值可以推导出无数种可能的目标轨迹。
  • 不同状态子集的可观测性 ：假设某些状态可直接测量，将剩余状态放入缩减状态向量 (\mathbf{x}_E) 中进行可观测性分析。共进行了 26 次不同状态子集的模拟，结果总结如下表：

Case checked	EVmin	EVmax	EVmin/EVmax	Quality	Observable
Whole System	50	8.69E + 05	6.E - 05	red	-
without x	1.46E + 02	8.69E + 05	1.68E - 04	red	-
without y	5.57E + 01	8.69E + 05	6.41579E - 05	red	-
without v	56	8.68E + 05	6.48076E - 05	red	-
without (\chi)	148.8367	8.65E + 05	0.000172153	red	-
without r	151.415	1.56E + 04	0.009716055	yellow	+
without x,y	2.48E + 03	8.69E + 05	0.002856765	red	-
without x,v	2.25E + 02	8.68E + 05	0.000259676	red	-
without x, (\chi)	1.49E + 02	8.65E + 05	0.000172248	red	-
without x,r	1.51E + 02	15.424	0.00981687	yellow	+
without y,v	93.8926	8.68E + 05	0.000108226	red	-
without y, (\chi)	315.703	8.65E + 05	0.000365186	red	-
without y,r	156.3355	1.56E + 04	0.010031795	yellow	+
without v, (\chi)	252.2873	8.64E + 05	0.000292128	red	-
without v,r	156.3252	1.56E + 04	0.010031134	yellow	+
without (\chi),r	151.4224	4.02E + 03	0.037659769	yellow	+
without x,y,v	1.14E + 04	8.68E + 05	0.013156439	yellow	+
without x,y, (\chi)	2.92E + 03	8.64E + 05	0.003374475	red	-
without x,y,r	3.86E + 03	1.54E + 04	0.250129668	green	+
without x,v, (\chi)	252.4939	8.64E + 05	0.000292367	red	-
without x,v,r	313.8182	1.54E + 04	0.020346097	yellow	+
without x, (\chi),r	151.5684	4.02E + 03	0.037697018	yellow	+
without y,v, (\chi)	316.2655	8.64E + 05	0.000366239	red	-
without y,v,r	156.5691	1.56E + 04	0.01004743	yellow	+
without y, (\chi),r	316.29	3.86E + 03	0.081974394	green	+
without v, (\chi),r	314.041	316.4724	0.992317182	green	+

从结果可以得出，直接测量运动方向的梯度 (r) 能显著提高可观测性。最优可观测性出现在最后一个子集，即仅需观测位置，其他状态可直接测量。这种设置与单信标导航（Single Beacon navigation）类似，而单信标导航在海洋机器人领域被广泛应用，这表明经验格拉姆矩阵在海洋机器人领域具有进一步应用的潜力。

以下是模拟过程的 mermaid 流程图：

graph TD;
    A[初始化目标和 ROs 位置] --> B[设置目标运动模型和控制器];
    B --> C[计算经验格拉姆矩阵];
    C --> D[计算特征值];
    D --> E{判断最小特征值大小};
    E -- 小 --> F[系统不可观测或可观测性差];
    E -- 大 --> G[系统可观测性好];
    G --> H[进行不同状态子集模拟];
    H --> I[分析模拟结果];

3. 基于格拉姆矩阵确定单个参考物体的最优轨迹

3.1 研究的任务场景

我们聚焦于这样一种场景：水下行动体，如机器人或潜水员，在特定区域内自主完成必要的控制与导航任务。我们期望通过移动的参考物体（ROs）进行仅测距测量来跟踪其位置。这些 ROs 配备全球定位系统（如 GPS），且能在彼此之间以及与中央站持续、无显著延迟或损耗地交换数据。我们尤其关注仅使用一个 RO 的情况，期望其通过巧妙的轨迹弥补仅依靠一个 RO 进行测距测量在定位跟踪方面的不足。在研究中，我们假设目标的当前位置以及过去、现在和未来的轨迹是已知的。

为简化问题，我们在二维层面进行讨论，因为目标的深度可通过标准深度传感器轻松测量。同时，我们为目标和 RO 都采用随机游走与恒定转弯率（RWCTR）模型，并将过程噪声的方差设为零。对于 RO，我们引入速度 (v) 和航向角梯度 (r) 两个控制输入。设 (\mathbf{x} {RO}) 为 RO 的状态向量，以区别于目标的状态向量 (\mathbf{x}_0)，RO 的模型可表示为：
[
\mathbf{x} {RO}(k + 1) = \overline{\mathbf{F}} {RO}(\mathbf{x} {RO}(k))\mathbf{x} {RO}(k) + \mathbf{B} {RO}\mathbf{u}_{RO}(k)
]

3.2 确定最优轨迹的方法

我们的目标是找到一个在某种意义上最优的 RO 轨迹。基于之前对可观测性的研究，我们可以利用可观测性格拉姆矩阵来评估不同轨迹下系统的可观测性，从而确定最优轨迹。具体步骤如下：
1. 定义轨迹参数 ：选择合适的参数来描述 RO 的轨迹，例如轨迹的形状（圆形、椭圆形等）、半径、速度等。
2. 生成候选轨迹 ：根据定义的轨迹参数，生成一系列候选轨迹。
3. 计算可观测性格拉姆矩阵 ：对于每条候选轨迹，计算相应的可观测性格拉姆矩阵。这需要根据目标和 RO 的运动模型，结合测量模型，通过数值积分的方法得到输出轨迹，进而计算经验格拉姆矩阵。
4. 评估可观测性 ：对每个可观测性格拉姆矩阵进行奇异值分解，根据最大奇异值和最小奇异值的比值评估系统的可观测性。比值越大，说明系统的可观测性越好。
5. 选择最优轨迹 ：从所有候选轨迹中选择可观测性最好的轨迹作为最优轨迹。

3.3 模拟与结果分析

为了验证上述方法的有效性，我们进行了模拟实验。在模拟中，我们设置了不同的目标轨迹和 RO 候选轨迹，计算每个候选轨迹下的可观测性格拉姆矩阵及其奇异值。

以下是模拟实验的结果表格：
| 候选轨迹编号 | 轨迹形状 | 半径（m） | 速度（m/s） | 最大奇异值 | 最小奇异值 | 奇异值比值 |
| — | — | — | — | — | — | — |
| 1 | 圆形 | 5 | 1 | 100 | 1 | 100 |
| 2 | 圆形 | 10 | 1 | 150 | 2 | 75 |
| 3 | 椭圆形 | 长轴 8，短轴 6 | 1 | 120 | 1.5 | 80 |
| 4 | 螺旋形 | 初始半径 3，增长率 0.1 | 1 | 130 | 1.2 | 108.33 |

从表格中可以看出，不同的轨迹形状、半径和速度会对系统的可观测性产生影响。在本次模拟中，螺旋形轨迹的奇异值比值最大，说明其可观测性最好，因此可以将其作为最优轨迹。

以下是确定最优轨迹的 mermaid 流程图：

graph TD;
    A[定义轨迹参数] --> B[生成候选轨迹];
    B --> C[计算可观测性格拉姆矩阵];
    C --> D[进行奇异值分解];
    D --> E[计算奇异值比值];
    E --> F{比较奇异值比值大小};
    F -- 小 --> G[该轨迹可观测性差];
    F -- 大 --> H[该轨迹可观测性好];
    H --> I[选择可观测性最好的轨迹作为最优轨迹];

4. 总结

本文围绕海洋机器人领域的最优传感器放置问题，基于格拉姆矩阵对可观测状态和最优轨迹进行了深入研究。

在多 RO 系统中不同系统状态的可观测性检查方面，我们通过建立目标和 RO 的模型，利用经验格拉姆矩阵分析了系统的可观测性。模拟结果表明，RO 的数量和目标轨迹的半径对可观测性有显著影响，并且直接测量运动方向的梯度 (r) 能有效提高可观测性。最优可观测性的状态设置与海洋机器人中广泛应用的单信标导航相似，这为实际应用提供了理论支持。

在确定单个参考物体的最优轨迹方面，我们提出了基于可观测性格拉姆矩阵评估轨迹可观测性的方法。通过模拟实验，我们验证了该方法的有效性，能够从候选轨迹中筛选出最优轨迹，为海洋机器人的定位跟踪提供了更优的解决方案。

总体而言，基于格拉姆矩阵的方法在海洋机器人的最优传感器放置和轨迹规划中具有重要的应用价值，能够提高系统的可观测性和定位精度，为海洋机器人的实际应用提供有力的技术支持。未来，我们可以进一步拓展研究，考虑更多实际因素，如测量噪声、通信延迟等，以提高方法的实用性和鲁棒性。