我正在尝试为以摄像头为主要传感器的机器人实施“信仰空间”计划。与SLAM相似，该机器人具有3D点的地图，并且通过在每个步骤与环境进行2D-3D匹配来进行本地化。出于这个问题的目的，我假设地图没有变化。

作为信念空间规划的一部分，我想为机器人规划从头到尾的路径，但要始终确保其定位精度最大化。因此，我将不得不采样机器人的可能状态，而无需实际移动到那里，并且如果机器人处于这些状态，则机器人将进行观察，这些观察（如果我错了，请纠正我）一起构成了机器人的“信念” ，随后在这些位置编码其定位不确定性。然后我的计划者将尝试连接那些给我带来最小不确定性（协方差）的节点。

由于我对基于摄像头的机器人的定位不确定性完全取决于诸如从给定位置可见多少个特征点，机器人的航向角等问题：我需要估算某个样本上的定位有多“糟糕”确定我是否应该丢弃它。为了到达那里，我如何为此定义测量模型，它是相机的测量模型，还是与机器人位置有关的东西？如何预先“猜测”我的测量值，以及如何通过这些猜测的测量值计算机器人的协方差？

编辑：对我来说，主要参考是快速探索随机信仰树的想法，它是“ 信仰路线图 ”方法的扩展。另一篇相关论文将RRBT用于受限计划。在本文中，类似于传统RRT一样对状态进行采样，以顶点表示为图形，但是当要连接顶点时，算法会将信念从当前顶点传播到新顶点（第V部分的PROPAGATE函数1） ，这就是我遇到的问题：我不完全了解如何在不实际遍历和获取新度量的情况下沿边缘传播信念，从而从定位获得新的协方差。该RRBT纸 说“协方差预测和成本期望方程是在PROPAGATE函数中实现的”：但是，如果仅使用预测，它怎么知道，例如，未来位置是否有足够的功能可以提高/降低定位精度？

使用仅方位定位可以对摄像机的信息进行建模，并以零噪声模拟测量（例如，无需创新）。

由于各种原因，从理论上讲，这实际上是估算路径信息的合理方法。

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

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

在我上次回顾此文献时，这是一种相当容易理解的方法，但是请不要相信我（回顾自己！）。至少这应该形成易于模拟的基准方法。让我们利用文学的力量。您可能会仔细阅读本文中的设置和方程式。

总结

1. $x$$\Sigma$
2. $\Sigma_i$
3. 假设轨迹的“成本”是向目标进展和协方差的倒数的某些凸组合（例如，信息矩阵）

一些微妙之处

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

您将必须导出线性化系统，但可以从上面的论文中借用它。确保不要理会模拟测量（例如，如果仅使用“模拟测量”进行EKF更新，则假定测量是真实的且没有噪声。

如果我们应用伍德伯里矩阵身份

$n$

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

$H$$H$$nx2$$n$$n$$2$$R$$n\times n$$\sigma I_{n\times n}$$\sigma$

测量方程是什么？它的

$t$$r$

展开递归。我将进行如下操作：

1. 编写路径搜索算法，在不考虑不确定性的情况下找到自己的方式。
2. $H$
3. $trace(H^TRH)$
4. 请注意，结果与轨迹的FIM匹配（练习留给读者），并且您已经正确且以理论上合理的方式确定了最有用的轨迹。