23、服务机器人场景分析与主动感知技术

服务机器人场景分析与主动感知技术

1. 合成传感器模型对比

对于不同宽度的两个物体，每个物体由两个 3D 特征点表示，且它们与立体相机系统的距离相同。通过绘制 p(z|ω) 作为 ω 在距离与垂直旋转空间中的函数，可以对比它们的合成传感器模型结果。结果显示，较小物体的旋转分量姿态估计具有明显更高的不确定性。

2. p(ω|z) 的高斯近似

2.1 重要性采样问题

测量模型可用于在重要性采样步骤中为给定粒子集找到权重。但当找到大量兴趣点时，测量模型的锐度很高，会导致粒子滤波器的收敛速度下降。常用的解决方法是使用混合提议，即从测量中抽取样本并根据状态转移模型进行加权。

2.2 高斯近似方法

由于目前没有直接从对应给定测量 z 的状态 ω 分布中采样的方法，因此可以使用重要性采样从测量模型中生成样本。这里使用从测量及其分布推导而来的高斯近似。

姿态估计 ω 基于实际测量向量 ˆz，ω 通过最小化模型点的加权反投影误差之和来确定：
- 反投影误差：$d_n = sV_n (ω) - z_{c(n)}^n$
- 误差函数：$f_{err}(z,ω) = \sum_{n=1}^{N} d_n^T \cdot (\Sigma_n^{ges})^{-1} \cdot d_n$

当有足够的点对应时，误差函数会达到局部最小值，即误差函数关于姿态 ω 的梯度 g 为 0：
$g(z,ω) = \frac{\partial f_{err}}{\partial ω} (z,ω) = 0$

这个方程隐式定义了非线性函数 f(z) = ω，用于为给定