如何融合传感器的线性和角度数据

我和我的团队正在建立一个室外机器人，该机器人配备编码器，商业级IMU和GPS传感器。机器人具有基本的油箱驱动器，因此编码器可充分提供左右车轮的滴答声。IMU以x，y和z给出滚动，俯仰，偏航和线性加速度。稍后我们可以添加其他IMU，这些IMU可以提供冗余，但也可以额外提供滚动，俯仰和偏航的角速率。GPS发布全局的x，y和z坐标。

知道机器人的xy位置和航向将对机器人进行定位和绘制导航环境很有帮助。机器人的速度还可用于做出平稳的运动决策。这是一个基于地面的机器人，因此我们不太在乎z轴。该机器人还具有激光雷达传感器和摄像头-因此侧倾和俯仰将有助于转换激光雷达和摄像头数据以获得更好的方向。

我试图找出如何将所有这些数字融合在一起，从而最佳地利用所有传感器的准确性。现在，我们使用卡尔曼滤波器[x, x-vel, x-accel, y, y-vel, y-accel]通过简单的转换矩阵生成的估算值：

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

滤波器仅根据IMU提供的加速度估算状态。（IMU并不是最好的质量；在大约30秒内，它将显示机器人（静止时）从其初始位置漂移了20米。）我想知道如何从IMU以及可能的侧倾，俯仰和偏航率，来自车轮的编码器数据以及GPS数据，以改善状态估计。

通过一点数学，我们可以使用两个编码器在机器人上生成x，y和航向信息，以及线速度和角速度。编码器非常精确，但在室外场所可能会打滑。

在我看来，这里有两组独立的数据，很难融合：

1. x，x-vel，x-accel，y，y-vel，y-accel的估计
2. 侧倾，俯仰，偏航和侧倾，俯仰和偏航率的估计

即使这两套设备之间存在交叉，但我仍难以确定如何将它们组合在一起。例如，如果机器人以恒定速度行驶，则由其x-vel和y-vel确定的机器人方向将与其偏航相同。虽然，如果机器人处于静止状态，则无法通过x和y速度准确确定偏航。而且，由编码器提供的数据，转换为角速度，可能是偏航率的更新……但是如何才能最终获得更好的位置估计呢？

将所有12个数字放入同一个过滤器是否有意义，或者通常将它们分开存放？是否已经有成熟的方法来处理此类问题？

两件事情。

1. 如果您打算进行映射，则需要完善的同时定位和映射（SLAM）算法。请参阅：同时定位和映射（SLAM）：第一部分基本算法。在SLAM中，估计机器人状态只是问题的一半。如何做到这一点比这里可以回答的问题要大得多。

2. 关于本地化（估计机器人的状态），这不是卡尔曼滤波器的工作。从过渡 到X （吨+ 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$由于角加速度和速度，它不是线性函数。因此，您需要为该任务考虑非线性估计量。是的，有标准的方法可以做到这一点。是的，它们可以在文献中找到。是的，通常所有输入都放在同一过滤器中。机器人的位置，速度，方向和角速度用作输出。是的，我将在这里简要介绍他们的共同主题。主要要点是

   1. 包括您所在州的陀螺仪和IMU偏差，否则您的估计会有所不同
   2. 一个扩展卡尔曼滤波器（EKF）通常用于这个问题
   3. 实施可以从头开始，通常不需要“查找”。
   4. 大多数本地化和SLAM问题都存在实现方法，因此不必做过多的工作。请参阅：机器人操作系统ROS

现在，在您的系统环境中解释EKF。我们有一个IMU +陀螺仪，GPS和里程表。如上所述，该机器人是差动驱动器。过滤任务是，以所述机器人的当前姿态估计 X吨，控制输入ü 吨，并从各传感器，测量Ž 吨，和在下一时间步骤产生所述估计 X吨+ 1。我们将IMU测量值称为I t，将GPS 称为G t，将里程表称为O t。${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

我假定我们感兴趣的是估计机器人姿态作为 。IMU和陀螺仪的问题在于漂移。加速度中存在非平稳偏差，您必须在EKF中解决。（通常）通过将偏差置于估算状态来完成此操作。这使您可以直接估计每个时间步的偏差。 X 吨 = X ，ÿ ，˙ X，˙ ÿ，θ ，˙ θ，$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$，对于偏差向量。$b$

我假设：

1. =两次距离测量值，代表胎面以很小的时间增量行进的距离$O_t$
2. =三个取向测量值 α ，β ，θ和三个accelleration测量 ¨ X，¨ ÿ，¨ ž。$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. =机器人在全局框架中 的位置 G x t ，G y t。$G_t$$^Gx_t,^Gy_t$

通常，控制输入的结果（每个踏面的期望速度）很难映射到输出（机器人姿势的变化）。代替，通常是将里程表用作控件的“结果” （请参见Thrun，里程表问题）。当您不在几乎没有摩擦的表面上时，此假设非常适用。如我们所见，IMU和GPS可以帮助纠正滑移。$u$

因此，第一个任务是预测从当前状态的下一个状态 。如果是差动驱动机器人，则可以直接从文献中获得此预测（请参见《轮式移动机器人的运动学》或任何现代机器人学教科书中更为简洁的介绍），或从头得出，如下所示：里程表问题。$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

这样，我们现在可以预测X吨+ 1 = ˚F （X吨，ö 吨）。这是传播或预测步骤。您可以通过简单地传播来操作机器人。如果值Ø 牛逼是完全准确的，你将永远不会有一个估计X不完全等于你的真实状态。在实践中，这永远不会发生。$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

这仅给出先前估计的预测值，而不能告诉我们估计的精度如何随时间降低。因此，要传播不确定性，必须使用EKF方程（在高斯噪声假设下以封闭形式传播不确定性），粒子滤波器（使用基于采样的方法）*，UKF（使用逐点方法）不确定性的近似值），或许多其他变体之一。

对于EKF，我们进行如下操作。令为机器人状态的协方差矩阵。我们 使用泰勒级数展开来线性化函数f以获得线性系统。使用卡尔曼滤波器可以轻松地求解线性系统。假设在时间t处估计的协方差为P t，并且在里程表中假定的噪声协方差作为矩阵U t给出 （通常是对角2 × 2矩阵，如.1 × I 2 × 2）。在函数f的情况下，我们获得了雅可比行列式$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ 和˚Fü=∂$F_x=\frac{\partial f}{\partial x}$，然后传播不确定性为$F_u=\frac{\partial f}{\partial u}$

$$P_{t+1}=F_x*P_t*F_x^T + F_u*U_t*F_u^T$$

现在我们可以传播估计值和不确定性。请注意，不确定性将随时间单调增加。这是预期的。为了解决这个问题，通常要做的是使用和G t更新预测状态。这被称为过滤过程的测量步骤，因为传感器提供了对机器人状态的间接测量。$I_t$$G_t$

$h_g()$$h_i()$$R$$R_g$$R_i$$h$

$s$$z_s$

小号小号 = ħ 小号 * P 吨+

$$v_s=z_s- h_s(\hat{x}_{t+1})$$ $$S_s = H_s*P_{t+1}*H_s^T + R_s$$ $$K = P_{t+1}*H_s^T S^{-1}_s$$ $$\hat{x}_{t+1} = \hat{x}_{t+1} - K*v$$ $$P_{t+1} = (I-K*H_s)*P_{t+1}$$

$z_g=h_g()$$H_g$$R_g$

$z_i=h_i()$$p_i$$R_i$$p_i$。出于偏见而合并更新更加困难，而且这是我的专长。但是，由于您对平面运动感兴趣，因此可以简化此问题。您必须为此寻找文学。

一些我的头上参考：

1. 提高基于EKF的视觉惯性里程表的准确性

2. 基于可观察性的一致多机器人联合EKF估计器

3. 具有未知故障偏置的INS-GPS松耦合系统的自适应两阶段EKF

这个领域已经足够成熟，谷歌（学者）可能会找到一个可行的实现。如果您打算在这方面做很多工作，建议您阅读一本扎实的教科书。也许类似Google Car 的S. Thrun的Probablistic Robotics。（我发现它对于那些深夜的实现是有用的参考）。

* 机器人操作系统（ROS）中有几种基于PF的估算器 。但是，这些已针对室内使用进行了优化。粒子过滤器处理多模态PDF，这些PDF可能是由于基于地图的本地化（我在这扇门或那扇门附近）引起的。我相信大多数户外设备（尤其是至少可以间歇使用GPS的设备）都使用扩展卡尔曼滤波器（EKF）。我已经成功地将扩展卡尔曼滤波器用于带差动驱动的室外地面流动站。

在大多数情况下，您可以大大简化问题：

• 许多“商业级” IMus（例如Xsens）的加速度计都非常嘈杂。甚至不必费心将它们融合起来以提高速度，里程表已经好了几个数量级。IMU将提供的唯一可用数据是俯仰和横滚，以及在某种程度上的航向（请参阅下一点）
• 从IMU出发并不是值得信赖的。它使用测力计，并且会在铁磁质量附近显示巨大的漂移（在我们的情况下，超过2m可达25度），例如在建筑物墙壁中可以看到的漂移。解决此问题的方法是使用IMU航向，但估计航向偏差。
• 如果您在户外，请不要忘记在10度倾斜上行驶10m与在平坦地形上行驶10m不会导致相同的X和Y变化。这通常是通过估算Z来解决的，但是我想可以用不同的方式估算它。
• GPS也是一个说谎的母狗，通常在高多径环境中使用。再加上低等级（甚至在某些情况下是高等级）的GPS，倾向于报告非常错误的标准偏差。我们使用了一些简单的卡方检验来检查是否应集成特定的GPS测量值（即检查它是否与当前滤波器估计值匹配到特定点），这给了我们不错的结果。

对我们而言，“典型”解决方案是使用里程计+ IMU获得自我运动估计，然后使用GPS校正X，Y，Z和航向偏差。

这是我们广泛使用的EKF实现。如果您需要估计IMU的方向（即，如果它还没有内置过滤器），则还可以使用以下两个过滤器之一：UKF和EKF。