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

[[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]]

#1 楼

两件事情。


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

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


包括您所在州的陀螺仪和IMU偏差，否则您的估算值将
发散）通常用于此问题
实现可以从头开始，通常不需要
“查找”。
大多数本地化和SLAM问题都存在实现，因此不要做比您需要做的更多的工作。请参阅：机器人操作系统ROS




现在，在您的系统环境中解释EKF。
我们有一个IMU +陀螺仪， GPS和
测距法。所讨论的机器人是如上所述的差动驱动器。
过滤任务是获取机器人的当前姿态估计
$ {\ hat {x} _t} $，控制输入$ u_t $以及每个传感器的测量值$ z_t $，并在下一个时间步
$ \ hat {x} _ {t + 1} $产生估算值。我们将IMU测量值称为$ I_t $，GPS为$ G_t $，测距法为$ O_t $。

我假设我们有兴趣将机器人姿态估算为
$ x_t = {x，y，\ dot {x}，\ dot {y}，\ theta，\ dot {\ theta}} $。
IMU和陀螺仪的问题是漂移。加速度中有一个非平稳的
偏差，您必须在EKF中解决。
（通常）通过将偏差置于估算状态来完成。这使您可以直接估算每个时间步的偏差。
$ x_t = {x，y，\ dot {x}，\ dot {y}，\ theta，\ dot {\ theta} ，b} $，对于一个
偏置$ b $的向量。表示脚踏板以较小的时间增量行进的距离
$ I_t $ =三个方向测量值$ {\ alpha，\ beta，\ theta} $和三个加速测量值ddot {x}，\ ddot {y}，\ ddot {z}} $。
$ G_t $ =机器人在全局框架中的位置，
$ ^ Gx_t，^ Gy_t $。

通常，控制输入的结果（所需的速度为每个
踩脚都很难映射到输出（
机器人的姿势变化）。代替$ u $，通常将测距法用作控件的“结果”（请参见测距法，测程问题）。当您不在几乎没有摩擦的表面上时，这种假设会很好地发挥作用。如我们所见，IMU和GPS可以
帮助纠正滑移。

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

因此，我们现在可以预测$ \ hat {x} _ {t + 1} = f（\ hat {x} _t，O_t）$。这是
传播或预测步骤。您可以通过简单地
传播来操作机器人。如果$ O_t $值是完全准确的，您将
永远不会有一个与您的
真实状态不完全相同的估算$ \ hat {x} $。在实践中这从来没有发生过。因此，要传播不确定性，必须使用EKF方程
（在高斯噪声下以封闭形式传播不确定性
），粒子滤波器（使用基于采样的方法） ）*，UKF（使用不确定性的逐点近似）或许多其他变体中的一种。

对于EKF，我们进行如下操作。假设$ P_t $是机器人状态的协方差矩阵。我们使用泰勒级数展开来线性化函数$ f $
，以获得线性系统。使用卡尔曼滤波器可以轻松地求解线性系统。假设在时间$ t $处估计的
协方差为$ P_t $，并且在里程表中假设的噪声的
协方差为矩阵$ U_t $
（通常是对角$ 2 \ times2 $矩阵，例如$ .1 \ times I_ {2 \ times 2} $）。
对于函数$ 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 $$

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

首先，使用每个传感器来估算某个部分状态，如GPS，IMU的某些功能$ h_g（）$和$ h_i（）$。形成残差或创新，即残差或创新值与预测值和测量值之差。
然后，以所有传感器的协方差矩阵$ R $的形式估算每个传感器估算值的精度（在这种情况下为$ R_g $，$ R_i $）。最后，找到$ h $的Jacobian并更新状态，如下所示：

对于每个传感器$ s $，其状态估计为$ z_s $（以下是维基百科的条目）

$$ 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 ^ TS ^ {-1} _s $$
$$ \ hat {x} _ {t + 1} = \ hat {x} _ {t + 1}-K * v $$
$$ P_ {t + 1} =（IK * H_s）* P_ {t + 1} $$

GPS，测量值$ z_g = h_g（）$可能只是从纬度和经度到机器人的本地框架的转换，因此雅可比定律$ H_g $几乎是Identity。在大多数情况下，$ R_g $由GPS单元直接报告。

对于IMU + Gyro，函数$ z_i = h_i（）$是加速度和加法偏差项。处理IMU的一种方法是
对加速度进行数值积分，以找到所需位置的位置和速度估计值。如果您的IMU在每个加速度估算值中都有一个较小的
附加噪声项$ p_i $，则您必须对该噪声进行积分以找到位置估算的精度。那么协方差$ R_i $是所有
小附加噪声项$ p_i $的积分。由于我的专业知识，合并用于
偏见的更新更加困难。但是，由于您对平面运动感兴趣，因此可以简化此问题。
您必须为此阅读文学作品。

一些非常规的参考文献：


改进EKF的视觉惯性的精度
里程表
基于可观性的多机器人一致EKF估计器
协作
本地化具有未知故障偏差的GPS松耦合系统

该领域已经足够成熟，谷歌（学者）可能会找到
您一个可行的实施方案。如果您打算在这方面做很多工作，我建议您拿一本扎实的教科书。可能
类似Google Car的S. Thrun的S. Thrun的Probablistic Robotics。 > *
机器人操作系统（ROS）中有几种基于PF的估计器。但是，这些已针对室内使用进行了优化。粒子过滤器处理
多模态PDF，这些PDF可能是由于基于地图的本地化（在这扇门或那扇门附近）造成的。我相信大多数户外
实现（尤其是那些可以使用GPS的实现，至少
间歇性地）使用扩展卡尔曼滤波器（EKF）。我已经成功地将扩展卡尔曼滤波器用于带差动驱动的室外地面流动车。

评论

---

$ \ begingroup $
（1）我看不到与粒子过滤器的“明显”连接。 （2）如果还有其他问题/话题讨论与我的问题类似的话题，请显示指向它们的链接。 （3）我了解EKF的原理，并且一定会切换到使用EKF ...如果我实际上首先知道状态转换（这是我的问题的很大一部分）。 （4）用相机和激光雷达改善状态估计的想法抽象起来很酷，但这超出了我的需求范围。不过，感谢您的参考。
$ \ endgroup $
–罗布兹
2012年11月15日下午6:24

---

$ \ begingroup $
粒子滤波器是一种非线性估计器。我将尽快更新链接/参考。 IMU，陀螺仪和里程表的状态转换已在文献中广泛涉及（包括参考文献1）。同样，我将很快更新一些参考。
$ \ endgroup $
–乔什·范德·胡克（Josh Vander Hook）
2012年11月15日13:58

---

$ \ begingroup $
@Robz大规模编辑OP。由于不确定回复评论的标准做法，因此我在帖子中添加了尽可能多的信息。
$ \ endgroup $
–乔什·范德·胡克（Josh Vander Hook）
2012年11月18日，0：50

---

#2 楼

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


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

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

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