计算配备视觉传感器的机器人的概率置信度的最佳方法是什么？

#1 楼

使用纯方位角定位来建模相机的信息量，并模拟零噪声的测量（例如，无需创新）。

出于多种原因，这实际上是估计路径信息量的合理方法。

有许多“免费的”信息量度指标，例如Fisher信息矩阵。您只需要机器人的位置和地图中地标的位置，即可确定通过测量地标位置获得多少有关机器人位置的信息。 （反之亦然，测量的创新既适用于目标也适用于机器人（这是SLAM吗？），所以相同的度量标准对这两者都适用。）

我将从轴承传感器入手，因为这是视觉传感器的良好且广为接受的模型。通过假设世界上定位特征的几个像素误差来找出轴承测量结果中的“噪音”。假设系统状态为机器人的位置加上其不确定性，然后采样路径（如您所建议）。从采样路径中的每个位置，我将使用FIM重新计算预测的不确定性。这并不难做到，只要假设测量中没有错误即可（即，不会对机器人的信念进行“创新”，但是您仍然会遇到不确定性下降的情况，不确定性下降的原因是机器人位置估计的协方差不断缩小。我会不会更新地标的位置或不确定性，只是为了简化问题。

这是我上次回顾此文献时记得的一种相当容易理解的方法，但请不要相信我的话。为此（请自己复习！）至少应该形成一个易于模拟的基准方法，让我们利用文献的力量。您可以仔细阅读本文中的内容和公式。


形成状态向量$ x $，该状态向量是机器人的位置（如果需要的话，还可以定向），以及这些位置元素$ \ Sigma $中的不确定性。
使用RRT，离散图计划器（dijkstra / A *）或网格搜索（可能是A *），但是在采样轨迹的每个“步骤”中，都使用标准EKF更新公式重新计算$ \ Sigma_i $。
让轨迹的“成本”是实现目标的进展和协方差的倒数的凸组合（例如信息矩阵）

一些细微之处

>使用有意义的最小状态向量。如果可以假定机器人可以独立于运动而指向摄像机，或者具有多个摄像机，则可以忽略方向而仅跟踪位置。我将仅在2D位置进行操作。

您将不得不推导线性化系统，但可以从上面的论文中借鉴一下。确保不要打扰模拟测量（例如，如果仅使用“模拟测量”进行EKF更新，则假定测量是真实的且没有噪声。）

至关重要的是，这简化了规划问题，因为状态除了不确定性和计划运动外不会改变。请检查此处的EKF方程，请注意，与计算新协方差相关的两个方程是
$$ P_ {i | i-1} = F ^ T_iP_ {i-1 | i-1} F_i + Q $$
和$$ P = P-PH ^ T（HPH ^ T + R）^ {-1} HP $$ ... （我将卡尔曼增益扩展到了最后一个方程式）。

如果我们应用伍德伯里矩阵恒等式

，我们将第二个简化为$$ P ^ {-1} = P ^ {-1} + H ^ TR ^ {-1} H $$。

我们完成了。您对轨迹的$ n $步长有了很好的信息量度，如

$$ I = \ sum_ {i = 1} ^ n H_i ^ TR ^ {-1} H_i $$

其中$ R $是轨迹中每个时间步的测量噪声（例如，测量到每个界标的方位误差）。由于我们正在查看信息（协方差的倒数），因此我们希望最大化$ \ sum_ {i = 1} ^ n H_i ^ TR ^ {-1} H_i $的跟踪，行列式或其他东西

那么，$ H $是多少？好吧，这是关于机器人位置的测量函数的雅可比行列式。也就是说，$ H $的大小为$ nx2 $，因为您具有$ n $个界标（$ n $个测量函数）和$ 2 $个机器人的状态元素：x，y。 $ R $的大小为$ n \ timesn $，在这里我将其设置为$ \ sigma I_ {n \ timesn} $，并假装了$ \ sigma $的值，（实际不确定性无关紧要，都是相同的常数。

测量方程是什么？

$$ \ tan ^ {-1} \ frac {y_t-y_r} {x_t-x_r} $$

目标$ t $和机器人位置$ r $。


递归递归，我将按照以下步骤操作：


编写一种无需考虑不确定性即可找到其路径的路径搜索算法。
导出$ H $，或在任何方位角定位文件中进行查找。
确定路径的“优度”要在搜索算法中使用，请为路径中的每个姿势添加$ trace（H ^ TRH）$。
请注意结果与轨迹的FIM匹配（练习留给读者），并且您已经正确并以理论上合理的方式确定了最有用的轨迹。