自动驾驶中的传感器融合算法：第一部分-卡尔曼滤波器和扩展卡尔曼滤波器

资源分类

2019-09-24 |

102 |

原标题：自动驾驶中的传感器融合算法：第一部分-卡尔曼滤波器和扩展卡尔曼滤波器来源：AI研习社 [ 发布者：AiChinaTech ]

原文链接：https://www.yanxishe.com/TextTranslation/1136

发起：程炜校对：Pita 审核：Pita

参与翻译（1人）：不到7不改名

英文原文：Sensor Fusion Algorithms For Autonomous Driving: Part 1 — The Kalman filter and Extended Kalman Filter

介绍：

追踪静止和移动的目标是自动驾驶技术领域最为需要的核心技术之一。来源于多种传感器的信号，包括摄像头，雷达，以及激光雷达（基于脉冲激光的测距设备）等传感器组合的组合体来估计位置，速度，轨迹以及目标的种类，例如其他车辆和行人。详情请见：Link（原文中的链接是无效的因此我将原作者的文章连接替换了）

你可能会问——为什么我们需要这么多的传感器？

这是因为每种传感器提供了追踪物体所需要的不同精度和类型的信息，尤其是在不同天气条件下。比如，以激光雷达为基础的传感器能很好地解决位置的问题，但是在糟糕的天气条件下其精度和性能都会有很大程度的下降。另一方面，雷达的空间解决方案也相对便宜很多，与此同时，在恶劣的天气条件下也能提供更准确的数据。

同样的，不像激光雷达传感器，雷达可以提供目标的速度和方位。雷达数据也是计算密集型的，因为一束激光发射非常多包含每个独立的激光点的范围的数据，它使得你必须理解你的算法。

组合来自不同传感器信息的技术称之为传感器融合技术。之所以较早的讨论这个，是因为应用在传感器融合之上的算法必须处理短暂的，充满杂讯的输入，生成可靠的运动状态估计的概率。

该文章展示了在位置的追踪和估计中最通用的算法，卡尔曼滤波器的变种——‘扩展卡尔曼滤波器’。在进一步的文章中，我们会兼顾到其他技术比如无损卡尔曼滤波器和粒子滤波器。

1.使用激光雷达数据的基础卡尔曼滤波器:

卡尔曼滤波器的历史已经超过半个世纪,但是对于输入数据的噪声信息和状态估计的平滑来说仍然是最有效的传感器融合算法之一.它假定位置参数是符合高斯分布的,即完全可以被均值和协方差参数化:X∼N(μ, σ²)

当传感器的信息流开始的时候,卡尔曼滤波器使用一系列的状态信息来预测和衡量更新步骤去更新被追踪目标的信心值(概率).预测和更细心的步骤如下图所示:

状态预测:

我们会使用一个简化的线性状态空间模型(Model Link)去阐述滤波器的工作方式. 一个系统在t时刻的线性状态可以从t-1时刻根据以下等式被估计:

衡量更新:

卡尔曼滤波器的下一部分则是去使用实测参数z去更新预测状态'x',通过缩放因子(通常称之为卡尔曼增益)成比例的计算估计值和测量值之间的误差.

你可以在以下链接中找到衡量更新等式的求导:

Derivation Link

举个例子

理论结束！让我们尝试用一些代码去表示一些卡尔曼滤波器的基础过程。

对此，我们模拟一个目标，它的状态被四维向量x=[px,py,vx,vy]所描述。

在这个例子中测量传感器是返回位置数据但是没有速度信息的激光传感器。为了观测到速度信息我们需要使用雷达传感器数据。在接下来的章节中，当我们讨论扩展卡尔曼滤波的时候我们将会涉及到这些。

让我们以一些假设开始：

伪代码：

基础版本的卡尔曼滤波器代码步骤列在了下面。你可以找到一个基础的例子：code link

"""prediction step"""

def predict(x, P):
x = (F * x) + u
P = F * P * F.transpose() #Acceleration noise Q is assumed to be zero
return x, P

"""measurement update step"""
def update(x, P,z):
# measurement update
Z = matrix([z])
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
return x, P

最后的迭代步骤通过测量和应用预测以及滤波器的更新步骤如下所示：

plot_position_variance(x,P,edgecolor='r') #plot initial position and covariance in red
for z in measurements:
x,P = predict(x, P)
x,P = update(x, P,z)
plot_position_variance(x,P,edgecolor='b') #plot updates in blue
print(x)
print(P)