自动驾驶传感器融合算法 - 自动驾驶汽车中的激光雷达和雷达

自动驾驶传感器融合算法-自动驾驶汽车中的激光雷达和雷达

在本文中，我们将深入探讨L1DAR和RADAR之间的传感器融

合。这两种传感器在自动驾驶和许多其他机器人应用中被广泛使用。

我们将首先简要介绍这两种传感器，然后介绍我们使用的融合算法及

其特性。

顺便问一下，什么是传感器融合？在感知模块中，即观察环境（道

路线、交通标志、建筑物、行人等），我们需要使用多个传感器。这

可以增加冗余度、确定性，或者利用多个传感器的优势并创建多个用

例。这创建了一个我们称之为传感器融合的领域。

O

PERCEPTIONUOCAUZATIONPLANNIMGCONTROL

例如，使用相机可以让我们看到交通信号灯的颜色。它是分类、

车道线检测等的完美工具……使用LiDAR非常适合SLAM（同步定位

和地图构建）和深度估计一一即估计任何物体的精确距离。最后，

RADAR具有一种可以测量物体速度并给你开超速罚单的特定技大！

在本文中,我们将学习融合LiDAR和RADAR,从而利用LiDAR

技术估计距离并以3D方式观察世界，并利用RADAR估计速度的

能力。

用于LiDAR雷达融合的传感器数据

在考虑任何传感器融合任务之前，我们必须做两件事:

1.选择多个传感器进行合并一一并定义一个明确

的目标。

2.研究两个传感器来确定“如何”将它们融合。

让我们花一点时间来回顾一下不同的传感器..・

雷达（无线电探测和测距）

雷达发射无线电波来探测几米（约150米）范围内的物体。多

年来，雷达一直安装在我们的汽车上，用于探测盲区中的车辆并避免

碰撞。

与其他传感器计算两次测量之间的位置差异不同，雷达利用多普

勒效应，通过测量车辆向我们移动或远离时下一波频率的变化。这称

为径向速度。

雷达可以直接估算速度。

因此，它们在移动物体上的表现比在静态物体上的表现更好。

它的分辨率较低，可以知道被检测物体的位置和速度。另一方

面，它很难确定被检测的物体是什么。

LiDAR（光检测和测距）

LiDAR使用红外传感器来确定与物体的距离。旋转系统可以发送

波并测量波返回所需的时间。这使得可以生成传感器周围环境的点

云。它每秒可以生成大约200万个点。点云具有不同的3D形状,

因此可以借助Lidar对物体进行分类。

它具有良好的范围（10。至300米），并且可以准确估计周

围物体的位置。但是它的尺寸很笨重，有些人可能会把它看作拐

杖。更不用说，它的价格（1,000-50,000美元）多年来一直很高，

而且仍然远远高于相机（500美元）或雷达（300美元）或雷达

（300美元）的平均价格。

传感器的优点和缺点

以下是每个传感器的优缺点：摄像头、激光雷达和雷达

Strengths&WeaknessesofSensors

★

SPATIALRESOLUTION★★★★★★

★

NOISE★★★★★★★★

★★★

vaocin(STIMATIOM★★★★★★★★★

DISTANCEESTIMATION★★★

★★★★★★

★★

CLASSIHCATION★★★★二★★★

AUWUTHER★★★★★★★

SIZE★★★★★★

现在我们对雷达和激光雷达有了很好的了解。让我们看看如何将

它们融合！

使用哪种类型的数据融合?

卡尔曼滤波器是数据融合中最流行的算法之一。它由鲁道夫•卡

尔曼于1960年发明，现在用于我们的手机或卫星的导航和跟踪c该

滤波器最著名的用途是在阿波罗11号任务中，该任务将机组人员送

回月球。

在本文中，您将了解卡尔曼滤波器的工作原理，以及如何使用卡

尔曼滤波器合并来自两个传感器的数据。使用的示例是IJDAR和

RADAR,但实际上我们可以用任何传感器来实现这一点，正如您将看

到的。

何时使用卡尔曼滤波器？

卡尔曼滤波器可用于数据融合，以估计动态系统（随时间发展）

的现在（过滤）、过去（平滑）或未来（预测）的状态。

自动驾驶汽车中嵌入的传感器发出的测量结果有时不完整且有

噪声。传感器的不准确性（也称为噪声）是一个非常重要的问题，可

以通过卡尔曼滤波器来处理。让我告诉你如何解决。

卡尔曼滤波器用于估计系统的状态，表示为xo该状态向量由位

置p和速度V组戌。

在每个估计中，我们都关联一个不确定性度量P。使用不确定性

非常好，因为我们可以考虑到LiDAR在我们的测量中比RADAR更准

确的事实！

通过进行数据融合，我们考虑传感器噪声和输出。

举个例子，看一下这两个LiDAR融合在一起观察行人的情况。

□DAR1UOAR1

X=10;uncertainty0.SX=10.8;uncertainty«0.5

9151010510111U

1OJ】L3

Thepedestrianisbetween10.3and10.5

两个传感器都同样确定，但我们可以看到，如果它们不确定的话,

情况会发生怎样的变化。

与只有一个传感器的情况相比，考虑多个传感器及其不确定性可

以帮助我们更好地了解行人的位置。

现在，让我们看看如何在雷达/激光雷达融合中实际使用卡尔曼

滤波器。首先，我们需要了解传感器融合算法（如卡尔曼滤波器）表

示估计值的方式：使用高斯函数C

高斯

状态和不确定性用高斯表示。

Meanp

Wvtr

高斯是一个连续函数，其面积为lo正如您在这里看到的，高斯

是一个以我们所谓的平均值为中心的公式，协方差有助于理解我们有

多“确定”。

我们用均值U表示状态，用方差。2表示不确定性。方差越

大，不确定性越大。

I;I/1\

\raotf/I\Nifa

I!Vansce/\V)r・nct一ma

IH二-、/•二、

S*TwilMediumHigh

unce^ir^yuxetWpuncertainly

高斯可以估计系统状态和不确定性的概率。

我们采用的是正态分布的概率。卡尔曼滤波器是单峰的：这意味

着我们每次都有一个峰值来估计系统的状态。

UnlmodalBimodalMultimodal

换句话说：障碍物并不是在90%距离10米处和70%距离8

米处；而是在98%距离9.7米处或没有障碍物处。

卡尔曼滤波器是连续的单峰函数。

到目前为止，我们已经了解了卡尔曼滤波器如何在表示过程中发

挥作用，但在传感器融合过程中却没有发挥作用。现在，让我们通过

理解一个关键概念来考虑传感器融合：贝叶斯滤波。

贝叶斯过滤

卡尔曼滤波器是贝叶斯滤波器的一种实现，即反复在预测和更新

之间交替。

0

预测：我们利用估计的状态来预测未来的状态和不确定性。

更新：我们使用传感器的观测来修正预测并获得更准确的估计。

传感器融合的具体实现如下：传感器数据到达；我们更新正在跟

踪的行人的估计位置，并预测下一个行人的位置。接下来，新的传感

器数据到达，我们更新行人的位置，评估预测的准确程度，并据此预

测下一个行人的位置。

要对卡尔曼滤波器进行估计，只需要当前观测值和先前预测值。

不需要测量历史。这不是机器学习，而是人工智能。因此，此工具很

轻便，并且会随着时间的推移而改进。

让我们回顾一下，分析一下迄今为止所学到的一切：

1.我们正在进行LiDAR和RADAR之间的传感器融合

2.但只有它们各自的输出（后期融合）

3.为此，我们使用卡尔曼滤波器

4.哪些是单峰的，并使用高斯来表示状态和不确定性

5.并使用预测/更新循环（也称为贝叶斯过滤）进行工作。

现在，让我们看看它的内部工作原理C

卡尔曼滤波器背后的数学

卡尔曼滤波器背后的数学原理是由矩阵的加法和乘法组成的。我

们有两个阶段：预测和更新。

在本节中，我将概述卡尔曼滤波器背后的公式。如果您想进一步

了解并真正掌握这些内容，请参加我的课程学习卡尔曼滤波器：默默

为未来提供动力的隐藏算法。在那里，您将了解卡尔曼滤波器的工作

原理，了解数学原理，并从头开始为不同的用例编写自己的卡尔曼滤

波器。

预测公式

我们的预测包括根据时间t时的先前状态x和P估计时间

t+1时的状态X'和不确定性P\

x=Fx+u

P'=FPFT+Q

因此，我们只有两个公式来估计X和Po其他值是:

F：从t到t+1的转移矩阵

v：噪声

Q：包含噪声的协方差矩阵

如果我们简化事情一一我们可以说我们的新位置x'等于前一

个位置X,乘以矩阵Fo这个矩阵F称为我们的运动模型。

这是一个关于它的外观和使用方法的简短示例。

X,二产•X

X、=[o4

父，联心*火6&辿-N

*

X

如您所见，F只是一个矩阵，描述我们如何从步骤t移动到步

骤t+lo

这里，我们有：

位置（t+1）=位置（t）+速度（t）*时间

速度（t+1）=速度（t）

换句话说，我们考虑恒定速度。

我们的预测方程旨在转换两个时间帧之间的运动。我们可以让F

矩阵实现不同的运动模型，例如恒定转弯速率、恒定速度、恒定加速

这就是我们预测下一个位置的方法-F是用于平移运动的矩阵。

现在，让我们看看更新公式。

更新公式

在更新步骤中，我们要调整我们的位置，并纠正我们下一步的预

测方式。

y=Z—Hx

S=HPfHT+R

K=PHiST

x=x7+Ky

P={I-KH）P,

让我们看一看它是如何工作的一一我们有时间t+1的X,（预测

状态）和P'（预测协方差）C

现在，我们收到了新的传感器数据并可以确认我们是否接近。

y是实际测量值与我们的预测之间的差异一一这是预测误差！

其他矩阵用于考虑传感器噪声（R）、估计系统误差（S）和卡尔

曼增益（K）一一最后一个值介于0和1之间，有助于决定我们是

否应该更信任预测或测量。

所有这些都导致计算一个新的x和一个新的P。

更新阶段可以估计出比测量和预测更接近现实的x和Po

经过几个循环后，卡尔曼滤波器将会收敛，做出越来越准确的预

测。

在回顾并了解全局之前，让我们先考虑以下有关贝叶斯概率的内

容。

先验/后验和贝叶斯过滤

该图显示了贝叶斯过滤器内部的最终数学°

likelihoodprior

P(B⑷*P(4)

P(川B)~~w~~

posterior

normalizer

我们想要估计的就是后验概率。从数学上讲，它等于先验概率（预

测）乘以似然概率（测量）。

将其翻译成高斯如下：

如你所见，更新（后验）始终是最佳估计。其次是测量和预测,

这自然是三者中最不确定的。

此时，您可能会想“好吧.....这很酷，但是Jeremy,您并没

有谈论激光雷达和雷达.....您只是以非常笼统的方式谈论了传感

器融合。”

你说得对，那我们就进入正题吧。

雷达/激光雷达传感器融合流程

流程如下：每次传感器到来时，我们都会运行一个新的预测+更

新循环。

每次预测都会降低确定性，每次更新都会增加确定性c但最终，

两者结合可以提供更多数据，并获得更好的总体结果。

需要理解的是：我们始终使用相同的状态一一我们的状态(位置、

速度)估计。

在雷达LiDAR融合中使用卡尔曼滤波器

使用LiDAR时，我们有“笛卡尔”线性值。我们的数学公式全

部用y=ax+b类型的线性函数实现。

另一方面，当我们使用雷达时，数据不是线性的。该传感器通过

三个指标来观察世界：

(rho)；到被跟踪物体的距离。

6(phi)：x轴和物体之间的角度。

(rhodot)：□的变化，导致径向速度。

由于包含了角度6,这三个值使得我们的测量结果呈非线性。

意思是说，当使用雷达时，我刚才谈到的一切都是错误的，无法

起作用。

Measurementsarenon-¥

linear•z

非线性卡尔曼滤波器

这里的一切都将我们引向这一点一一世界是非线性的。并非所有

事物都完美地沿直线移动c传感器的工作方式并不相同c因此，我们

必须使用两种特定类型的卡尔曼滤波器：

扩展卡尔曼滤波器

无迹卡尔曼滤波器

这些过滤器将首先“线性化”非线性数据，以便它可以在经典过

滤器中工作。在扩展版本中，我们使用雅可比矩阵和泰勒级数对数据

进行线性化。在无迹版本中，我们使用Sigma点预测。

最终传感器融合流程

考虑到这一点，这是我们最终的传感器融合流程。

INmAUSADON1rMEASUREMENT

晦DICTION

,UDAWRADAR

LiDAR会定期更新，而RADAR则需要进行线性化处理。这个循

环不断重复。我们永远不会将数据融合在一起，而总是一个接一个地

进行。

结果

以下视频展示了扩展卡尔曼滤波器和无迹卡尔曼滤波器的结果。

我们可以特别看到我们的传感器（黑色原点）、雷达数据（红色）、

激光雷达数据（蓝色）和我们的预测（绿色）。每次我们从我们的一

个传感器接收到一些数据时，我们都会实现一个预测。

2086Zeomcu

TimoStop441

RMSE

*00591

Y:0.0934

VX:0.3517

VY:0.2321