如何排除基于轮子的机器人在前一个姿态之后的姿态

Question

我目前正在研究基于轮子的机器人姿态的编码传感器融合，来自GPS，激光雷达，视觉和车辆测量。它的模型是使用EKF的基本运动学，并且不区分传感器，即数据是基于时间戳输入的。

由于以下问题，我很难融合这些传感器；有时，当最新传入的数据来自不同于先前状态的传感器时，机器人的最新姿态会落后于先前的姿态。因此，数据融合不会因此变得如此平滑和曲折。

我希望丢弃绘制在先前数据之后/向后的数据，并且即使传感器提供的数据在时间戳t和时间戳t+1之间发生变化，也要采用总是位于先前状态之前/之前的数据。由于数据帧是全局帧，因此不可能依靠其x坐标的负值来实现这一点。

如果你对此有一些想法，请让我知道。非常感谢你提前这么做。最好的

Answer 1

初步警告

在为您的问题提出可能的解决方案之前，请允许我在这里提出一个警告:在丢弃基于您当前估计的数据时要小心，因为您永远不会知道上一个度量是“拉回姿势”还是前一个度量是错误的，并导致您的估计向前移动了太多。

可行的解决方案

在类卡尔曼滤波器中，假设观测值提供关于状态向量变量的独立、不相关的信息。假设这些观测值具有作为零均值高斯变量分布的随机误差。然而，现实生活更难：

有时，测量会受到“偏差”(固定项，类似于具有非零均值的高斯误差)的影响。例如，众所周知，对流层扰动会在GPS定位中引入随时间缓慢漂移的位置误差。如果你用几个传感器观测相同的变量，比如GPS和激光雷达的位置，但它们有不同的偏差，你的估计就会来回跳跃。伸缩问题也有类似的效果。

我会假设这就是你问题的根源。如果没有，请细化您的问题。

您如何缓解此问题？我看到了几种替代方案：

在状态向量中

• Introduce一个偏差/比例校正项，以补偿传感器偏差/漂移。这是惯性传感器融合(陀螺/加速度计)的EKF中非常常见的技巧，当对感觉输入进行一些预处理以纠正已知问题时，EKF可以很好地工作。这可能很难调整滤波器，以估计状态向量和传感器参数在同一time.
• Change如何解释观测。例如，使用连续位置观测值之间的差异，这样就可以创建一个假的里程表传感器。这极大地减少了输出的漂移。不是丢弃观察，而是集成它们并在内部保留“跳跃”状态向量，但平滑输出向量以消除跳跃。这是在一些无人机自动驾驶仪中完成的，因为这样的跳跃会影响PID controllers.

的性能

最后，最明显和最简单的方法:基于一些统计测试的丢弃观察值。残差的卡方检验可以用来确定观测值是否离预期值太远，必须丢弃。不过，要注意这个选项:观察拒绝方案必须使用状态向量重新初始化逻辑来完成，以便以稳定的行为重新输出。

几乎所有这些解决方案都需要知道每个观察值的来源，因此您将无法再模糊地对待它们。