机器人学中不确定性传播的再思考

最新推荐文章于 2025-11-24 17:27:50 发布

原创最新推荐文章于 2025-11-24 17:27:50 发布 · 343 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#不确定性传播 #无迹变换 #卡尔曼滤波 #粒子滤波 #高斯混合模型

部署运行你感兴趣的模型镜像

重新审视机器人学中的不确定性传播

示意图0

摘要

本文重新审视了机器人系统中不确定性传播的问题。标准方法采用卡尔曼滤波，包括扩展型（线性化）（EKF）或基于无迹变换（UKF）的卡尔曼滤波器，来处理机器人系统的状态估计。这些动态滤波器在预测‐校正循环中运行，其中当前状态根据先验状态进行预测，然后利用计算值的校正权重结合当前观测进行校正。这些方法提供了状态估计的均值和不确定性（协方差）。替代的状态估计技术包括基于蒙特卡洛的粒子滤波器。系统状态估计通常与其他变换组合使用，以在世界坐标系中定位物体。通常这些内部变换被视为已知标称量，其不确定性被忽略。本文重新探讨了误差处理问题，并比较了多种表示方法，以突出当前方法的局限性。

一、引言

在现实世界中运行是困难的，不存在完美的机器人系统和完美的传感器；因此系统必须适当地处理误差和不确定性。因此，定位或状态估计问题是机器人学中的基本挑战之一，这一点已被广泛认可[1],[2]。这可以是相对于已知地图或全球定位系统（GPS）的定位，也可以是相对于未知环境的定位，而在未知环境中需要同时进行定位与建图[3],[4]。

传感器测量提供了有关系统状态的有价值的信息，但所有传感器都包含一定程度的不确定性或噪声。此外，传感器仅能提供过去某一时刻的信息；当传感器信息被收集并传输到传感器融合算法时，这些信息已经过时。因此，噪声和延迟必然导致系统状态的不确定性。根据这种不确定性的大小，可能导致自动驾驶车辆无法完成其任务执行。本文综述了处理不确定性的当前方法，指出了这些当前方法的缺点，并提出了为应对这些挑战而进行的正在进行的研究。

A. 系统设置

示意图1

机器人本体参考坐标系相对于世界坐标系的位置为(xb, yb)，旋转角度为 θ ；设wqb=[xb, yb, θ]表示机器人相对于世界坐标系的位姿，其中 w表示相关的世界坐标参考系。设bqs=[xs, ys, ψ]表示传感器参考坐标系相对于机器人本体坐标系的位姿。最后，传感器将报告检测到的物体相对于传感器坐标系的距离和方位角(r, φ)；在齐次笛卡尔传感器坐标中，该物体位于点

$$
s p=
\begin{bmatrix}
r \cdot \cos(\varphi) \
r \cdot \sin(\varphi) \
1
\end{bmatrix}.
(1)
$$

机器人需要报告物体在世界坐标系 wp 中的位置，并给出适当的位置不确定性，其中 wqb 、 bqs 和 sp 均存在不确定性。为此，需应用一系列坐标变换。

为了实现这一目标，我们将考虑一个简化的2D系统，该系统由一个机器人本体和一个相对于系统参考原点存在偏移的单个测距‐测向传感器组成。本体位姿的估计具有不确定性，传感器坐标系相对于本体的位置同样存在不确定性或噪声。

使用标准表示法[1]，令

$$
wTb=
\begin{bmatrix}
\cos(\theta) & -\sin(\theta) & x_b \
\sin(\theta) & \cos(\theta) & y_b \
0 & 0 & 1
\end{bmatrix}
(2)
$$

$$
bTs=
\begin{bmatrix}
\cos(\psi) & -\sin(\psi) & x_s \
\sin(\psi) & \cos(\psi) & y_s \
0 & 0 & 1
\end{bmatrix}
(3)
$$

物体在世界坐标系中由以下给出

$$
wp= wTb \cdot bTs \cdot sp
(4)
$$

根据标准的矩阵乘法规则，可以从传感器读数到世界坐标系中的点，针对每个坐标确定整体的多变量映射

$$
wp=
\begin{bmatrix}
f_x(\mathbf{u}) \
f_y(\mathbf{u}) \
1
\end{bmatrix}
=
\begin{bmatrix}
f_x(x_b, y_b, \theta, x_s, y_s, \psi, r, \varphi) \
f_y(x_b, y_b, \theta, x_s, y_s, \psi, r, \varphi) \
1
\end{bmatrix},
(5)
$$

其中 $\mathbf{u}=[x_b, y_b, \theta, x_s, y_s, \psi, r, \varphi]$。该映射中的每个因变量都具有不确定性，其均值为

$$
\bar{\mathbf{u}}=[\bar{x}_b \bar{y}_b \bar{\theta} \bar{x}_s \bar{y}_s \bar{\psi} \bar{r} \bar{\varphi}]
(6)
$$

和独立的标准差 $\sigma_{u_i}$。

这些值及其不确定性需要通过先前定义的变换进行传播，以确定世界坐标系中该点的整体位置和不确定性。请参考图2中的结果，该图显示了从独立正态分布 $\mathbf{u} \sim N(\bar{u} i, \sigma {u_i})$ 中抽取的两百万次蒙特卡洛随机采样 $\mathbf{u}$ 的结果。

$$
\bar{\mathbf{u}}=[6.5\ 7.2\ 0.660\ 0.1\ 0.1\ 0.174\ 3.0\ -0.433],\ \sigma_{\mathbf{u}}=[0.1\ 0.2\ 0.174\ 0.1\ 0.1\ 0.174\ 0.1\ 0.174,]
$$

其中不确定性被夸大以展示由不确定性引起的扩散。

B. 论文结构

本文讨论了多种形式化表示不确定性的技术。我们首先概述标准统计术语和不确定性传播技术，并在第二节中回顾当前最先进的状态估计方法。在第三节中，我们展示了系统的仿真结果，突出了当前方法的局限性，并为我们正在探索的新方法提供了思路。第四节总结了结果，并阐述了对目标定位领域 ongoing 工作的影响。

II. 相关工作

如前所述，状态估计是机器人技术中的一个基本挑战 [1],[2]。传感器不确定性问题促使人们需要进行信息融合，即结合来自不同传感器的信息以及已知的物理模型，以在自动驾驶车辆运行期间实现对状态的整体更好逼近。这个问题已被深入研究，并且有许多标准实现在开源ROS生态系统[3],[5]中广泛可用。

在本节中，我们将概述其中的一些技术。在描述这些技术之前，我们首先回顾一些将在本文中使用的常见统计术语的定义。

A. 标量变量的统计回顾

不确定性或误差传播是指在应用数学运算时合并不确定值的方法[6]。由于大多数物理量无法直接测量，因此需要通过关联它们的公式，将可测量物理量的不确定性传播到目标物理量。传播不确定性取决于这些因素以正确表达不确定性：数值、运算和相关性。这些因素描述了数值的不确定程度、如何进行组合，以及根据变量之间的相互关系，不确定性是否需要减小或扩大。通过综合这些属性，可以准确地表达任意数学运算过程中数值的不确定性。

首先，我们定义本文中使用标准统计术语[1],[2]。我们从标量随机变量 $u \in U$ 的定义开始，然后推广到更高维度。均值 $\bar{u}$ 定义为在 $u$ 上的概率分布下所给定的期望或最可能的值。对于连续分布$p(u)$，关于 $u$ 的Expectation，记作 $E[u]$，由以下给出

$$
\bar{u}= E[u]= \int_U u \cdot p(u).
(7)
$$

对于有限分布，积分被替换为求和。标量随机变量的方差，记为 $\sigma^2$，是

$$
\sigma^2 = E[(u - \bar{u})^2]= \int_U (u - \bar{u})^2 \cdot p(u).
(8)
$$

对于未知的母体分布，这些术语是根据从未知分布 $N$ 中抽取的 $N$-样本定义的。

对于单个变量 $u$，均值$(\bar{u})$、标准差$(\sigma)$和标准误差$(\alpha)$的方程为：

$$
\bar{u}= \frac{1}{N} \sum_{i=1}^{N} u_i,
(9)
$$

$$
\sigma_u= \sqrt{ \frac{1}{N - 1} \sum_{i=1}^{N} (u_i - \bar{u})^2 },
(10)
$$

$$
\alpha_u= \frac{\sigma_u}{\sqrt{N}},
(11)
$$

其中$u_i$表示第$i$个样本。

均值被理解为基于多次测量（样本）得到的测量值的最佳估计值。一组观测值的标准差 $\sigma_u$由均值得出，表示各点与均值之间的平均偏离程度。标准差被理解为各点相对于均值的离散程度或正常偏差。方差 $\sigma^2$是另一个常用指标，定义为标准差的平方。

标准误差 $\alpha_u$由标准差导出，表示测量中均值围绕未知真值的不确定性。标准误差常被称为均值的标准差[6]，意味着它代表了与母体分布真均值可能的偏差。

需要注意的是，$N$这一假设在滤波中的适用性并不如通过采样计算最终测量值那样直接。其影响在于，标准误差不会被明确指出，而是给出标准差。标准误差在后续关于不确定性传播的讨论中有用，但对于后面的技术，将重点关注标准差，因为这是通常报告的内容。本文将任何不确定的标量随机变量 $u$的均值记为 $\bar{u}$，标准差记为$\sigma_u$，方差记为 $\sigma^2_u$，标准误差记为 $\alpha_u$。

B. 不确定性传播

考虑因变量 $z$ 的情况，其中 $z= f(u)$，$u$ 为独立标量变量。鉴于 $u$ 是一个被测量的不确定的随机变量，其对应的函数计算值 $z$ 同样具有不确定性。对 $z$ 的均值和不确定性的标准函数计算为：[6],

$$
\alpha_z= |f(\bar{u}+ \alpha_u)− f(\bar{u})|
(12)
$$

如果假设误差是对称的，则此方程成立。(12)展示了不确定性传播的函数方法。该方法指出，为了确定输出变量的总误差，将输入均值与通过函数传播的均值 ±的标准误差之间的差异，将得到输出变量[6]的结果不确定性。

我们隐式地考虑了 $z= f(u;\Theta)= f_\Theta(u)= f(u)$的情形，其中$\Theta$是一组已知的非随机参数集，其不确定性极小。本文重点关注与系统状态或不确定性参数相关的项。

基于微积分的方法对不确定性传播[6]使用泰勒级数近似，

$$
z= f(u)= f(\bar{u})+ \frac{df(u)}{du} \alpha_u+ \sum_{i=2}^{\infty} \left( \frac{1}{i!} \frac{\partial^i f(u)}{du^i}(\alpha_u)^i \right),
(13)
$$

其中$\frac{\partial^i f(u)}{du^i}$是 $i$th 阶导数，$(\alpha_u)^i$中的 $i$ 是指数。在假设不确定性较小，因而高阶项可忽略的情况下，这给出

$$
z \approx f(\bar{u})+ \frac{df(u)}{du} \alpha_u.
(14)
$$

使用上述方差的定义，计算

$$
\sigma^2_z= \frac{1}{N - 1} \sum_{i=1}^{N} (z_i - \bar{z})^2.
(15)
$$

为了通过具有多个变量的函数传播不确定性，必须追踪每个不确定值的影响，并将其组合以获得所需量的总体不确定估计。作为一个简单的具体示例，考虑一个具有两个独立变量的标量输出，即 $z=f(a, b)$。假设 $a$和 $b$的值是通过测量得到的，其均值分别为 $\bar{a}$和 $\bar{b}$，标准不确定度分别为 $\alpha_a$和 $\alpha_b$。我们可以通过在所述函数中使用 $a$ 和 $b$ 的均值来计算最可能的值 $\bar{z}$，得到方程 $\bar{z}= f(\bar{a}, \bar{b})$。

为了计算该值的不确定性，必须将各个不确定性分别通过函数进行传播，然后利用期望均值与计算值之间的差异来确定总体不确定性[6]。然而，在实际系统中，变量通常是相关的；因此，我们必须考虑不确定性中的协方差[7]。

两个值的协方差公式为，

$$
\sigma_{ab}= \frac{1}{N - 1} \sum_{i=1}^{N} (a_i - \bar{a})(b_i - \bar{b}).
(16)
$$

为了利用协方差因子计算因变量的不确定性，设第$i$个样本 $z_i= f(a_i, b_i)$，并使用(14)式中的近似方法，通过以下方程定义 $z_i$的一阶近似，

$$
z_i \approx f(\bar{a},\bar{b})+(a_i - \bar{a}) \frac{\partial z}{\partial a} +(b_i - \bar{b}) \frac{\partial z}{\partial b}
(17)
$$

我们将(17)代入(15)得到

$$
\sigma^2_z \approx \frac{1}{N - 1} \sum_{i=1}^{N} \left((a_i - \bar{a}) \frac{\partial z}{\partial a} +(b_i -\bar{b}) \frac{\partial z}{\partial b} \right)^2
(18)
$$

$$
\sigma^2_z \approx \left( \frac{\partial z}{\partial a} \right)^2 \sigma^2_a + \left( \frac{\partial z}{\partial b} \right)^2 \sigma^2_b + 2 \frac{\partial z}{\partial a} \frac{\partial z}{\partial b} \sigma_{ab}.
(19)
$$

其中$\sigma_{ab}$在(16)中给出。根据这一阶近似，我们得到了由下式定义的总不确定性的approximation

$$
\alpha_z = \frac{\sigma_z}{\sqrt{N}}.
(20)
$$

需要明确的是，(19)是基于一阶泰勒级数的近似，忽略了高阶项。

这些项通常表示为协方差矩阵$\Sigma$，我们可以用向量‐矩阵表示法将(19)重写为：

$$
\sigma^2_z \approx \begin{bmatrix} \frac{\partial z}{\partial a} & \frac{\partial z}{\partial b} \end{bmatrix} \begin{bmatrix} \sigma^2_x & \sigma_{yx} \ \sigma_{xy} & \sigma^2_y \end{bmatrix} \begin{bmatrix} \frac{\partial z}{\partial a} \ \frac{\partial z}{\partial b} \end{bmatrix} = \nabla z \cdot \Sigma \cdot \nabla^T z,
(21)
$$

其中 $\nabla z$是单值函数 $z$的梯度。

C. 多变量函数的扩展

以上述讨论为基础，我们现在考虑大多数感兴趣的系统依赖于多个状态变量，并使用具有多输出的函数[1],[2]。

设 $\mathbf{u} = [u_1, u_2,\cdots, u_N]^T$ 为具有分量的多变量量；即具有一个$n$维状态空间，其标量分量为 $u_i$。令 $\bar{\mathbf{u}}={\bar{u}_1, \bar{u}_2,\cdots, \bar{u}_N}$ 表示该多变量量的均值，扩展上述定义，我们有

$$
\bar{\mathbf{u}}= E[\mathbf{u}]= \int_{\mathbb{R}^N} \mathbf{u} \cdot p_{\mathbb{R}^N}(\mathbf{u}).
(22)
$$

协方差矩阵扩展了方差的概念：

$$
\Sigma= E[(\mathbf{u} - \bar{\mathbf{u}})^T \cdot (\mathbf{u} - \bar{\mathbf{u}})],
(23)
$$

其中 $\Sigma$ 是一个 $N \times N$ 矩阵。

协方差矩阵的对角项就是如(8)式所定义的方差，而非对角项被称为 $u_i$和 $u_j$的协方差，其定义为描述两个变量相互之间变化关系的值[7]。这意味着，如果正在测量 $\mathbf{u}$ 的两个不确定分量 $u_i$和 $u_j$，且当 $u_i$增大时$u_j$也随之增大，则协方差值 $\sigma^2_{ij}$将为正。

我们还考虑多变量函数 $f: \mathbb{R}^N \rightarrow \mathbb{R}^M$ 使得 $\mathbf{z}(\mathbf{u})= f(\mathbf{u})$ 是一个具有 $\mathbf{z}(\mathbf{u})=[z_1(\mathbf{u}) \cdots, z_M(\mathbf{u})]^T$ 的多变量函数。

这些概念通过雅可比矩阵扩展到高维系统的输出。设 $F$为 $f$的雅可比矩阵，使得 $F_{ij} = \frac{\partial f_i}{\partial q_j}$。此处双下标表示矩阵的$i$th行和$j$th列。

使用一阶近似方法的协方差映射由下式给出

$$
\Sigma_z= F\Sigma_uF^T.
(24)
$$

D. 卡尔曼滤波

卡尔曼滤波器将随时间从传感器获得的信息与已知的运动学模型相结合，以试图在系统运行期间提高位姿估计的精度[1],[2],[8]。

卡尔曼滤波器算法使用一系列随时间获取的测量值、预定义的动力学状态转移模型以及将状态映射到预测测量的传感器测量模型。给定上一时间步的状态估计，系统使用动力学模型预测当前状态，并将预测测量与当前传感器测量进行比较。然后根据状态估计的不确定性以及传感器测量相关的不确定性对状态进行“校正”。该基本算法的推导假设动力学模型在状态变量，且测量值和模型中的不确定性服从均值为零的高斯分布。

卡尔曼滤波器不仅跟踪状态估计的均值，还跟踪协方差数据[1],[2]。给定一个线性状态转移矩阵 $F$和测量矩阵 $H$

$$
\mathbf{q}_{n+1|n}= F(\mathbf{q}_n)\cdot \mathbf{q} + \mathbf{v},
(25)
$$

$$
\mathbf{y} {n+1}= H(\mathbf{q} {n+1|n})\cdot \mathbf{q}_{n+1}+ \mathbf{w},
(26)
$$

噪声协方差为 $V$和 $W$时，协方差传递为

$$
\Sigma_{n+1|n}= F_n\Sigma_nF^T_n+ V_n,
(27)
$$

其中$\mathbf{q} {n+1|n}$是基于先验估计的预测状态，$\Sigma {n+1|n}$是在 $n+1$th时刻根据$n$th时刻信息得到的状态变量的预测协方差矩阵。(27)中的预测协方差与(24)一致。卡尔曼滤波器随后计算新息，即实际测量与预测测量之间的差异。

$$
\nu= y_{n+1} - H(\mathbf{q} {n+1|n})\cdot \mathbf{q} {n+1|n}.
(28)
$$

卡尔曼滤波器根据校正预测状态

$$
\mathbf{q} {n+1}= \mathbf{q} {n+1|n+1}= \mathbf{q}_{n+1|n}+ R\cdot \nu
$$

$$
\Sigma_{n+1|n+1}=\Sigma_{n+1|n} - R\cdot H(\mathbf{q} {n+1})\cdot\Sigma {n+1|n}
$$

where

$$
S= H(\mathbf{q} {n+1})\cdot\Sigma {n+1|n} \cdot H(\mathbf{q} {n+1})^T+ W {n+1}
$$

$$
R=\Sigma_{n+1|n} \cdot H(\mathbf{q}_{n+1})^T \cdot S^{-1}
$$

对于具有零均值高斯分布噪声且协方差为 $V$和 $W$的线性转移矩阵 $F$和 $H$，卡尔曼滤波器在贝叶斯意义下提供最优估计[1],[2]。

对于一般的非线性系统，扩展卡尔曼滤波器（EKF）使用一般的非线性状态转移来预测下一个状态，然后利用线性化的 $F$和 $H$雅可比矩阵进行协方差和状态校正更新[1],[2]。多年来，EKF一直是机器人领域的主力状态估计技术；然而，这种线性化仅在小不确定性且函数在相关时间步长内接近线性时才准确[2],[9],[10]。如 [9],[10]所述并在第三节中演示的那样，EKF在非线性系统中会低估不确定性。

无迹变换被开发为一种在存在显著非线性[9]–[11]的情况下仍能正确保持均值和协方差数据的方法。与使用线性化的 $F$和 $H$矩阵来传播不确定性不同，无迹变换采用有限数量的加权sigma点，这些点通过完整的非线性 $f()$和$h()$方程进行映射。这些加权sigma点的确定方式使其能够保持系统的均值和协方差；这“基于一种直觉，即近似概率分布，而非近似非线性函数或变换”（引自[10]）。

该方法已应用于基于无迹卡尔曼滤波器（UKF）的状态估计问题，其中状态预测和校正方程使用无迹变换来计算期望均值和协方差。对于非线性系统，无迹卡尔曼滤波器（UKF）的性能已被多次证明优于扩展卡尔曼滤波器（EKF）[9]–[11]。尽管有这些改进，无迹卡尔曼滤波器（UKF）在表示完整分布时仍主要关注二阶统计量。我们将在第三节中展示无迹变换的优势与局限性。

E. 粒子滤波器

粒子滤波器提供了一种通过大量样本[2]来近似系统未知分布的技术。这些滤波器扩展了蒙特卡洛过程的思想，即使用一组随机样本或粒子，将其输入非线性变换，并利用新的粒子状态集合来预测新的分布。粒子滤波器采用基于观测值的似然性对粒子进行重采样的进化式的适者生存模型。因此，粒子的分布在证据基础上发生变化，类似于卡尔曼滤波器的正确阶段。

粒子滤波器特别适用于近似复杂的不规则分布，包括具有多峰分布的状态[2]。这与依赖于单峰分布的卡尔曼滤波器形成对比。

粒子滤波器的缺点是可能需要大量粒子才能较好地近似分布[2]。使用大量粒子会带来沉重的计算负担，可能导致粒子滤波器在嵌入式系统中难以用于在线使用。这种随机采样与无迹卡尔曼滤波器（UKF）中通过无迹变换所使用的有限数量的确定性样本形成对比。因此，对于单峰分布更倾向于使用UKF，而对于未知或多重模态分布则更倾向于采用粒子滤波方法。一些方法将简单的基于扩展卡尔曼滤波器（EKF）的系统与基于粒子的技术相结合，以处理非线性和多峰分布[2]。

III. 实验验证

在本节中，我们讨论基于第一节中定义的变换进行的仿真结果，这些变换用于在给定不确定状态 $\mathbf{u}$ 的情况下计算$w\mathbf{p}(\mathbf{u})$。我们展示了使用第二节中描述的多种方法进行误差传播的结果，并讨论其影响。

对于本文，我们假设 $\mathbf{q}_b$及其相关不确定性 $\Sigma_b$由一个独立于检测到的障碍物位置的状态估计系统（例如卡尔曼滤波器）确定。同样，传感器在机体坐标系中的位姿 $\mathbf{q}_s$及其不确定性 $\Sigma_s$与 $\mathbf{q}_b ,\Sigma_b$相互独立，这种情况适用于机器人定位基于不同传感器的情形。同样，距离和方位角 $(r, \varphi)$取决于传感器特性，并且与相对于机器人机体的位置无关。尽管当传感器被用于机器人定位时，这些独立性假设可能不成立，但在此项研究中仍具有实用性。

在本次讨论中，不确定性主要集中在(5)式的角度项上，以突出非线性的影响。仿真将状态向量的均值和不确定性初始化为 $N(\bar{u} i, \sigma {u_i})$

$$
\bar{\mathbf{u}}=[6.5\ 7.2\ 0.660\ 0.1\ 0.1\ 0.174\ 3.0\ -0.433],\ \sigma_{\mathbf{u}}=[0.002\ 0.002\ 0.087\ 0.001\ 0.001\ 0.044\ 0.001\ 0.087],
$$

其中 $\Sigma_u=$为对角矩阵 $\sigma_{\mathbf{u}}$。

A. 蒙特卡洛结果

该仿真使用了从独立正态分布中抽取的两百万次蒙特卡洛随机采样 $\mathbf{u}$ （$\bar{u} i, \sigma {u_i}$），通过大量样本对未知分布进行合理近似。这些样本的均值和协方差已计算得出。

观察图3，可以看出由于式(5)中的非线性特性，高斯输入的组合导致输出样本呈现非高斯分布，并将不确定样本限制在一条紧密弧线上。注意，图3中间小图中显示的样本均值位于该弧线内侧，不在可能结果的弧线上。这相对于通过映射输入均值得到的标称值产生了系统性偏差。蒙特卡洛样本及其计算值的均值和协方差被用作“真实值”，以与其他本文所述方法进行比较。

我们首先比较基于(5)中完整映射的不确定性预测。

示意图2

B. 计算（一阶雅可比）结果

与蒙特卡洛结果进行比较的第一种方法是使用基于一阶雅可比的误差传播方法，结合(5)式中的 $\bar{\mathbf{u}}$、$\Sigma_u$ 以及多变量雅可比矩阵 $F$，求解(24)中所述的$w\bar{\mathbf{p}}=w\mathbf{p}(\bar{\mathbf{u}})$和$\Sigma_p = F\Sigma F^T$。

这种线性近似方法忽略了泰勒级数近似中的高阶项（参见标量情况下的(13)），与蒙特卡洛方法相比，低估了不确定性，如图3中所示的细蓝色椭圆所示。然而需要注意的是，雅可比方法的均值与标称期望值一致，并位于蒙特卡洛样本的弧线上。表 I 给出了该方法及其他所有方法的数值比较结果。

C. 无迹变换结果

无迹变换还被用来展示一种利用更接近粒子滤波方法的误差传播潜在方法。该方法使用围绕系统均值预先确定的西格玛点，以确定变换对均值和不确定性[2],[9],[10]的影响。这些点记为 $\chi_i$，定义如下：

$$
\alpha= 1, \kappa= 1, \beta= 2
(29)
$$

$$
\lambda= \alpha^2 (n+ \kappa)- n
(30)
$$

$$
\chi_0 = \bar{\mathbf{u}}
(31)
$$

$$
\chi_i = \bar{\mathbf{u}}\pm \sqrt{(n+ \lambda)\Sigma} \text{ for } i= 1,…, 2n
(32)
$$

$n$参数指的是变换输入的维度数，对于公式(5)而言为8，因此产生17个西格玛点。$\alpha$、$\kappa$和 $\beta$的值是超参数，旨在帮助捕捉变换的非线性特性。在本文中，我们使用了$\alpha= 1$、$\kappa= 1$和 $\beta= 2$，这在底层源分布为高斯分布[2]时效果最佳。

这些点根据此模式被赋予权重，

$$
w_0^m= \frac{\lambda}{n+ \lambda}
(33)
$$

$$
w_0^c= \frac{\lambda}{n+ \lambda}+(1 - \alpha+ \beta)
(34)
$$

$$
w_i^m= w_i^c= \frac{1}{2(n+ \lambda)} \text{ for } i= 1,…, 2n
(35)
$$

使用(5)对西格玛点进行变换，并利用其结果通过与每个西格玛点相关的权重以及公式37& 38[2]中所示的关系来近似均值和协方差矩阵。

$$
Y_i=w\mathbf{p}(\chi_i)
(36)
$$

$$
\bar{\mathbf{p}}= \sum_{i=0}^{2n} w_i^m Y_i
(37)
$$

$$
\Sigma= \sum_{i=0}^{2n} w_i^c(Y_i - \bar{\mathbf{p}})(Y_i - \bar{\mathbf{p}})^T
(38)
$$

与基于雅可比的变换相比，无迹变换的西格玛点遵循非线性映射，并且与图3所示的蒙特卡洛预测相吻合。与低估不确定性的雅可比映射不同，无迹变换和蒙特卡洛结果均高估了不确定性，因为二者均未考虑非线性约束及由此引起的预测偏度，忽略了高阶项的影响。因此，尽管西格玛点能够匹配非线性映射，协方差矩阵表示却未能反映这种偏度。计算值的均值相对于期望均值也发生了偏移，这在物体定位中引入了系统偏差，类似于蒙特卡洛方法的情况。

仿真的结果证实了我们的预期发现，并与先前研究[9]–[11]一致。对于弱非线性情况，一阶误差传播能够合理地近似误差椭圆；然而，在存在显著非线性时，由于忽略了高阶项，其局限性变得明显。无迹变换方法展示了忽略这些高阶项的影响；然而，通过将西格玛点缩减为协方差矩阵表示，非线性约束丢失，不确定性被高估，且均值相对于标称值发生偏移。

D. 分段映射结果

之前的示例使用了（5）中的完整映射，将状态参数与物体在世界坐标系中的位置关联起来。尽管该完整映射是变量的正确组合，但许多实际应用采用分段的独立变换来转换状态参数。

示意图3

一个参考另一个。我们尚未在以往文献中看到对无迹变换的分段应用进行讨论，因此比较了多种可能的方法。

1) 分段雅可比 ：雅可比方法已更新，采用独立变换而非整体完整映射。此更新遵循一个迭代过程，其中一个变换的传播结果会被再次用于下一个变换中。该更新的结果与整体映射雅可比方法保持一致，如图4所示，其中浅蓝色细置信椭圆低估了蒙特卡洛不确定性，同时与标称均值相符。因此，无论好坏，分段雅可比的表现与完整雅可比一致。

2) 分段无迹变换 ：我们考虑两种以分段方式处理无迹变换的方法。第一种方法为每一分段变换重新实现无迹变换过程，其中西格玛点的相对数量根据变换的维度生成。在为一个变换生成西格玛点后，这些点通过该变换进行传播，然后重新计算均值和协方差。前一个变换输出的最终均值和协方差将用于增广下一个变换的均值和方差，如此重复，直到所有变换完成。从(4)式的右侧开始，分别具有2、3和3个状态，经过增广状态后最终得到2、5和5， resulting in 5+ 11+ 11= 27 total sigma points。

如图4所示，该方法导致蒙特卡洛不确定性被低估。通过在每次变换结束时重新计算均值和不确定性，由于高阶项引起的相关性在下一次变换中丢失且未得到正确表示。由此生成的西格玛点与约束弧段的匹配程度也较差，因此，尽管整体均值与完整的无迹变换和蒙特卡洛结果吻合较好，但不确定性被低估了。同样，均值显示出相对于标称值的系统偏差。这种估计置信度的增加可能导致比使用完整无迹变换时略为更大的偏差估计。

出于这个原因，我们考虑采用一种不同的方法来应用无迹变换，以避免这种信息损失。在通过极坐标到笛卡尔坐标映射生成前五个西格玛点（2 2+1）后，我们为传感器到本体映射生成七个（2 3+ 1）西格玛点。我们将均值西格玛点（例如 +1）配对，然后对剩余的西格玛点取笛卡尔积，生成25个（= 1+ 6 4）新的西格玛点，用于在传感器到本体变换中传播。同样地，我们为本体到世界变换生成额外的七个西格玛点，配对均值，并对剩余的西格玛点取笛卡尔积，生成145个（= 1+ 6 24）西格玛点，用于在最终变换中传播。如图4所示，该方法使西格玛点与蒙特卡洛弧线吻合，并略微高估了不确定性，同时再次匹配了均值（存在系统性偏差）。

这种笛卡尔积方法的明显缺点是，随着变换数量的增加，西格玛点的数量呈指数增长。虽然在二维的实际系统中这一缺点尚可接受，但对于一个在6自由度工作空间中的完整7自由度机械臂而言，该方法将需要超过3500万个点（=(2 ∗6)^7 +1）。未来工作将致力于更智能地配对分段西格玛点，以限制数量的增长，同时保持适当的统计特性。