CN111257853A - 一种基于imu预积分的自动驾驶***激光雷达在线标定方法 - Google Patents

Abstract

本发明公开了一种基于IMU预积分的自动驾驶***激光雷达外参估计方法，其目的是：基于自动驾驶***原有车载传感器，提供一种端到端的激光雷达外参在线标定方法，避免复杂的数学模型推导和优化，利用深度卷积神经网络的数据分析能力处理激光雷达的点云数据和车辆状态轨迹数据，在线实时估计出激光雷达的外参误差，从而实现对激光雷达外参的实时修正，提升自动驾驶***环境感知功能的准确性和稳定性，保证自动驾驶***的行车安全。

Description

一种基于IMU预积分的自动驾驶***激光雷达在线标定方法

技术领域

本发明涉及一种面向自动驾驶***的激光雷达外参估计方法，特别是关于一种基于IMU预积分的自动驾驶***激光雷达在线标定方法。

背景技术

自动驾驶***可以解放驾驶员的双手，给乘客带来安全、高效、舒适的乘坐新体验，是汽车和交通运输行业的新增长点，受到了世界各国的重视。世界各大汽车厂商，如戴姆勒集团、大众集团、通用集团等均已推出自动驾驶***的量产计划，互联网公司，如谷歌、优步、百度也陆续成立了自己的自动驾驶事业部，推出自动驾驶研发平台和共享出行计划。

为准确感知环境信息，自动驾驶***目前最常用的传感器之一是激光雷达，如谷歌的自动驾驶出租车、百度的自动驾驶出租车和优步的共享出行出租车，均使用激光雷达作为环境感知的主要手段。激光雷达获取点云数据并识别障碍物后，需要通过外参将其转换到车辆坐标系下，才能为决策***所用。但不幸的是，在车辆行驶过程中，会因颠簸、载荷不均等因素造成激光雷达外参的暂时性变动。同时，激光雷达设备的安装支架也会在使用过程中逐步产生形变，导致其外参永久性变动。这些因素都会导致激光雷达所识别的障碍物无法正确转换到车辆坐标系下，从而影响自动驾驶***的正常决策，甚至引起决策误判，威胁车辆行驶安全。

目前激光雷达外参估计方面的研究多针对静态场景，即，***使用前事先在特定的场地，使用特殊的设备采集特殊工况下的数据，然后离线计算或迭代优化出外参参数。这种方式有如下缺陷：1)需要实现布置特定的场景，使用区域受限；2)需要特殊设备，增加额外成本；3)需要采集特殊工况下的数据，灵活性受限；4)只能离线进行，不能在线实时地纠正激光雷达的外参误差。

激光雷达外参的在线实时估计能保证自动驾驶***环境感知功能的正常、准确工作，但在线估计功能既不能使用特殊工具，也不能依赖特定场景，给算法的处理带来挑战，静态外参估计的算法不再适用。近年来兴起的深度卷积神经网络技术具有强非线性拟合能力和学习能力，易于从大数据中自动分离出关键特征，从而实现端到端的模式识别。将深度卷积神经网络技术与自动驾驶***激光雷达的外参估计相结合，有望在不额外增加设备的情况下实现激光雷达外参的在线实时估计。但相关工作目前仍为空白。

发明内容

针对现有技术存在的不足，本发明的目的在于提供一种基于IMU预积分的自动驾驶***激光雷达外参估计方法，利用深度卷积神经网络的数据分析能力处理激光雷达的点云数据和IMU数据，在线实时估计出激光雷达的外参误差，从而实现对激光雷达外参的实时修正，保证自动驾驶***的正常工作。

为实现上述目的，本发明提供了如下技术方案：一种基于IMU预积分的自动驾驶***激光雷达在线标定方法，包括如下步骤：

步骤1，采集并预处理传感器数据，所涉及的传感器包括激光雷达和IMU，将传感器数据转换为算法常用格式；

步骤2，进行预积分与状态轨迹计算，即对IMU数据流进行滑动窗口预积分，并求取车辆状态轨迹；

步骤3，将激光点云数据和车辆状态轨迹分别转为深度图，然后将二者融合为混合深度图；

步骤4，缓存历史混合深度图，将若干帧混合深度图融合为大深度图，输入给深度卷积神经网络，通过深度卷积神经网络计算出激光雷达外参误差。

作为本发明的进一步改进，所述步骤1中的IMU设备包括加速度计和角速度计两部分，二者分别测量车辆的三轴加速度和三轴角速度后输出。

作为本发明的进一步改进，所述步骤2中的预积分的计算方式为：以激光雷达数据到来的时刻k为边界，将k到k+1时刻的IMU数据预先积分并存储起来，具体公司如下：

式中，

表示t时刻由IMU测量得到的三轴加速度向量，

表示由IMU测量的四元数形式三轴角速度向量，

表示从t_k时刻到t时刻范围内车辆的空间角度变化，Δv_k，k+1为车辆速度的预积分值，Δp_k，k+1为车辆位置的预积分值，Δq_k，k+1为车辆角度的预积分值。

作为本发明的进一步改进，所述步骤2中的车辆状态轨迹由以下步骤计算得出：

步骤21，取k时刻的车辆位姿增广为下式所示：

于是整个数据流中的车辆状态轨迹可由如下变量表示，其中N表示激光雷达数据的总帧数：

步骤22，使用列文伯格-马夸尔特方法从IMU数据流中优化得到最优的车辆状态轨迹，其中定义的代价函数如下：

其中：

注意，上式中，g为重力向量，Δt＝t_k-t_k-1。

作为本发明的进一步改进，所述步骤3中将激光点云数据和车辆状态轨迹分别转为深度图，然后将二者融合为混合深度图的具体步骤如下：

步骤31，将激光点云转换为深度图，以便与卷积神经网络结合；

步骤32，将车辆位姿数据转换为深度图，以便与激光点云数据和卷积神经网络结合；

步骤33，将激光深度图与车辆位姿深度图结合为分层深度图，即混合深度图，以便卷积神经网络处理；

步骤34，将多帧混合深度图拼接为一幅大深度图，以便融合历史信息，获得对激光雷达外参的准确估计。

本发明的有益效果，1)不需要人为推导激光雷达的外参偏差变化数学模型，易于工程实用；2)进行外参误差估计所需的第三方设备为IMU，该设备在自动驾驶***中是必备的，所以本发明未增加额外设备，易于应用在现有自动驾驶***中；3)本发明实现了激光雷达外参的实时在线标定，能为自动驾驶***提供实时的外参修正，从而提升了自动驾驶***的环境感知精度和可靠性，进而提升了自动驾驶***的安全性；4)本发明中使用的第三方设备为IMU，该设备在汽车行业已经成熟，成本低廉，所以本发明的方案是低成本解决方案。5) 使用范围广，不局限于地形开阔地区，在地下停车场等封闭区域也能使用。

附图说明

图1是本发明的硬件拓扑示意图；

图2是本发明的大数据引擎架构示意图；

图3是本发明的预积分模型示意图；

图4是本发明的模型训练架构示意图；

图5是本发明的深度卷积神经网络模型示意图；

图6是本发明的模型部署应用架构示意图。

具体实施方式

下面将结合附图所给出的实施例对本发明做进一步的详述。

如图1所示，本发明所涉及的自动驾驶相关硬件包括激光雷达、IMU(InertialMeasurement Unit，惯性测量单元)和计算单元。其中激光雷达在自动驾驶***中用于障碍物检测，他会实时向自动驾驶***反馈所检测到的点云信息，自动驾驶的障碍物检测算法基于点云信息分析障碍物的类别和位置。IMU设备通常包含加速度计和角速度计两部分，二者分别测量车辆的三轴(x、y和z轴，分别指向车辆前进方向、车辆左侧和天空)加速度和三轴角速度(分别绕x、y和 z轴的角速度)。计算单元中运行着自动驾驶***的核心算法，进行障碍物检测、可通行区域识别、决策规划，以及车辆动力学控制。激光雷达是本发明的研究对象，本发明所计算的外参误差即为激光雷达的位姿标定偏差。IMU为本发明的算法提供辅助信息，本发明通过神经网络计算激光雷达外参误差的本质是：连续多帧激光点云变化所对应的车辆轨迹应与IMU惯性数据积分得到的位置变化轨迹一致。激光点云变化所对应的车辆轨迹具有强非线性，不易通过数据建模获得解析解，本发明通过深度卷积神经网络的学习能力，学习这种强非线性关系，从而避免复杂的数学建模，获得可靠的激光雷达外参误差。计算单元在本发明中有2个主要作用：1)在搭建大数据引擎时，采集和记录数据；2)在模型部署应用时，运行深度网络模型，实时估计激光雷达外参误差，供自动驾驶算法实用。注意，在实际实用过程中，自动驾驶算法也运行在计算单元中。

如图2所示，本发明的大数据引擎主要包含：车载部分和服务器部分。深度卷积神经网络中每个神经元的参数需要经过大数据的训练才能收敛到有效值，故而需要搭建大数据引擎，用于采集、处理和保存必要训练数据，以供深度卷积神经网络的训练使用。

大数据引擎的车载部分包括：传感器、预处理和数据存储。其中，传感器主要包括：1)激光雷达，即为本发明的研究对象，提供自动驾驶车辆周围的环境点云数据；2)IMU，提供车辆三轴加速度和三轴角速度信息。预处理部分主要进行：1)激光雷达的点云数据解析，即将激光雷达的原始数据转换为算法所需要的常用格式，以激光点的空间三维坐标为表征；2)IMU的数据解析，将IMU 的原始数据转换为算法所需要的常用格式，即车辆坐标系下以“m/s²”为单位的加速度值和以“rad/s”为单位的角速度值。存储部分即，将对齐后的激光点云数据与IMU数据按时间顺序存储在计算单元的硬盘中。注意，大数据引擎的车载部分不需要进行数据同步处理，只是因为IMU的数据更新频率远超激光雷达，如果在此处进行数据同步将会浪费大量IMU数据。

大数据引擎的服务器部分主要包括：数据解析、人为加偏部分和存储三部分。数据解析部分主要进行：1)从所存储的数据中解析出激光点云，这部分工作量不大，因为大数据引擎的车载部分在存储数据时已经使用了常用的激光点云格式，此处只是简单读取和加载；2)从所存储的数据中解析出IMU数据，并进行预积分，推导车辆状态轨迹(该名词将在图3中进行解释)。人为加偏的目的是，利用现有数据生成更多有效数据，并通过特定的数据加偏内容，增强深度卷积神经网络的鲁棒性。加偏的内容主要包括：1)模拟车速增高，利用现有数据进行数值模拟，生成高速行驶工况的数据；2)模拟车辆颠簸，利用现有数据，对点云和IMU数据得到的车辆状态轨迹共同施加预定好的短时间大俯仰和侧倾角度变换，模拟车辆的震动工况；3)模拟遮挡工况，人为滤除多个角度的激光点云数据，使得深度卷积神经网络能应对点云稀少的工况；4)模拟不同外参偏差值，通过人为加入预定好的外参偏差，获得带外参偏差的数据。存储部分，即将前述新生成的数据按时间顺序进行存储，以备后续训练使用。

大数据引擎的车载部分在自动驾驶车辆中运行，采集自动驾驶***的目标工作环境数据。每次数据采集前，先通过高精度设备对激光雷达的外参进行静态标定，获得准确外参值，作为模型训练时的参考。采集过程中，尽量保持车辆不发生猛烈颠簸，这样做是为了避免设备的外参因车身的剧烈运动而发生改变。采集完成后，将车载部分的数据拷贝到服务器中，以进行后续处理。

大数据引擎的服务器部分运行在后台服务器中，根据车载采集的数据，经过预积分和车辆状态轨迹计算，并进行人为加偏后生成更多有效数据，以支持深度卷积神经网络的大数据训练。其中，人为加偏的步骤可以：1)增加数据的工况种类，数据在实车采集时，只有低速、平稳行驶工况，通过人为加偏可以获得高速、剧烈驾驶工况下的数据；2)增加数据量，深度卷积神经网络容易过拟合，只有数据量足够大时，深度卷积神经网络才能在保证精度的基础上提升泛化能力，避免过拟合。

注意，在数据采集和存储过程中，也会记录好前述利用高精度设备所测量的激光雷达外参标定值，作为模型训练时的参考值。

图3中所示为本发明的预积分模型。图中的紫色点表示IMU数据，即三轴加速度和三轴角速度，图中的立方体(标有

和

字样，其中

)表示激光雷达数据到来时刻的车辆位姿，即车辆的空间位置和车辆的空间角度，分别用位置向量

和四元数向量

表示。其中的下标“b”表示车辆，k则表示时刻的序号。为简洁起见，下文中将使用符号p_k和q_k表示上述二者。注意，四元数向量的角度表示方法与欧拉角 (即常用的横滚角、俯仰角和横摆角)的表示方式等效，本发明用四元数的表示方式是为了数值计算方便。IMU的数据更新频率比激光雷达快很多，且并不是每个激光雷达数据到来的时刻都恰巧有IMU数据到来，二者是异步关系。但我们可以利用线性插值近似求得激光雷达数据到来时刻的IMU数据近似值，因此我们可以认为每个激光雷达数据到来时刻都恰好有一个IMU数据与之匹配。

为便于深度卷积神经网络处理，保证最终的外参估计效果，需要将原始IMU 数据转换为车辆状态轨迹。车辆状态轨迹指：由若干车辆位姿组成的序列。注意，本实施例中车辆状态轨迹由激光雷达数据到来时刻的车辆位姿组成，不是其他时刻的车辆位姿序列。为将原始IMU数据转换为车辆状态轨迹，本实施例中使用IMU数据流优化的方法。

为节省计算量，避免优化过程中反复积分IMU数据，本实施例中以激光雷达数据到来的时刻k为边界，将k到k+1时刻的IMU数据预先积分并存储起来，如下式所示。

上式中，

表示t时刻由IMU测量得到的三轴加速度向量，

表示由IMU 测量的四元数形式三轴角速度向量，

表示从t_k时刻到t时刻范围内车辆的空间角度变化，Δv_k，k+1为车辆速度的预积分值，Δp_k，k+1为车辆位置的预积分值，Δq_k，k+1为车辆角度的预积分值。

为优化得出车辆状态轨迹，取k时刻的车辆位姿增广为下式所示。即，增加了速度项。后续使用神经网络时不需要速度项，在后续步骤中直接忽略速度项即可。

于是整个数据流中的车辆状态轨迹可由如下变量表示，其中N表示激光雷达数据的总帧数。

本实施例中使用列文伯格-马夸尔特方法从IMU数据流中优化得到最优的车辆状态轨迹，为此需要定义代价函数，如下式所示。

其中：

注意，上式中，g为重力向量，Δt＝t_k-t_k-1。

基于上述代价函数，以及状态向量X，即可使用列文伯格-马夸尔特方法迭代优化出整个车辆状态轨迹。迭代优化过程中，需要求r_k关于x_k的雅可比矩阵，本实施例中使用数值求导法，为加快计算速度，也可进行必要数学推导，给出雅可比矩阵的近似理论公式，然后在使用列文伯格-马夸尔特方法的优化过程中，用近似理论公式计算雅可比矩阵。

为权衡优化精度和计算量之间的矛盾，本实施例中引入滑动窗口策略，即，每一轮迭代优化只涉及部分时刻的车辆位姿，而不是考虑所有时刻的车辆位姿。假设所有数据的总帧数为N，滑动窗口的窗口大小为M，则第一轮迭代优化只针对第1～M时刻的车辆位姿，第二轮迭代优化只针对第2～(M+1)时刻的车辆位姿，第i轮迭代优化只针对第i～(M+i-1)时刻的车辆位姿，如此反复，依次遍历整个数据流，总共进行

轮迭代优化。其中符号

表示不小于实数a 的最小整数。

如图4所示，本发明的模型训练利用了深度卷积神经网络的反向修正机制。首先从存储器中读取大数据，将其解析为算法常用的格式，即激光点的空间三维坐标形式和车辆的空间三维6自由度位姿形式。然后将激光点云和车辆位姿数据输入给深度卷积神经网络模型，深度卷积神经网络模型会计算出外参的偏差值。将深度卷积神经网络计算的外参偏差值与存储器中存储的外参标准值进行比较，即可得到深度卷积神经网络的预测残差。利用预测残差向深度卷积神经网络模型的神经元参数求导，即可得到神经元参数的反向修正值。最后利用所获得的反向修正值对深度卷积神经网络的神经元参数进行修正，修正后即可进入下一轮迭代，如此往复，即可不断优化神经元参数，使得深度卷积神经网络具备对激光雷达外参误差的预测能力。

图5中所示为本实施例的深度卷积神经网络模型具体结构，主要包括：数据流输入、全卷积层、池化层、金字塔池化层和全连接层。图5中以16线Velodyne 激光雷达的数据处理为例，每帧激光点云包含16线数据，每线数据包含上千个数据点。本发明中首先将每帧点云数据转换为深度图，像素为1800×16×1，即，宽度×高度×灰度。同时，为将车辆状态轨迹数据也作为神经网络的输入，先从车辆状态轨迹中提取出车辆位姿数据，然后把车辆位姿数据转化为1800×16×1 的深度图，该深度图的像素值即为车辆的位姿数值。把点云深度图与车辆位姿深度图叠加在一起，形成1800×16×2的混合深度图。只有多帧图像运动才能产生车辆运动的路径，才能估计激光雷达的外参，因此本发明中缓存9帧历史混合深度图，与当前帧深度图共同拼接为一幅大深度图，作为深度卷积神经网络的输入，其像素为1800×160×2。如图5中所示，在输入大深度图以后，深度卷积神经网络依次经过全卷积、池化、金字塔池化和全连接层后得到预测的激光雷达外参，该外参以6维李代数的形式描述。在与数据库中所存储的精确外参做差后得到外参误差，外参误差将用于计算神经元的修正项，然后通过修正项的反向传播来调整神经元的参数值。

图6中所示为本发明的深度神经模型部署应用***架构，主要包括：传感器和软件2部分。在训练完成后，深度卷积神经网络将部署在实车的自动驾驶***中，用于实时计算激光雷达的外参偏差。部署架构的传感器部分包括：1) 激光雷达，即为所求外参偏差的主体；2)IMU，提供辅助信息，即车辆的三轴加速度和三轴角速度信息，以便深度卷积神经网络能估计出准确外参。部署架构的软件部分包括：预处理、构建深度图和模型3部分。预处理部分主要进行：1)激光雷达的点云数据解析，即将激光雷达的原始数据转换为算法所需要的常用格式，以激光点的空间三维坐标为表征；2)IMU的数据解析，将IMU的原始数据转换为算法所需要的常用格式，即车辆坐标系下以“m/s²”为单位的加速度值和以“rad/s”为单位的角速度值。构建深度图部分主要进行：1)将激光点云数据转换为激光深度图；2)对IMU数据进行预积分，缓存窗口宽度为M的预积分数据，通过迭代优化得到窗口中的车辆状态轨迹，并将车辆状态轨迹转换为车辆位姿深度图。模型部分主要进行：1)拼接激光深度图与车辆位姿深度图，形成混合深度图；2)缓存9帧历史混合深度图，并与当前帧混合深度图共同拼接为大深度图；3)将大深度图输入给深度卷积神经网络，得到估计的激光雷达外参；4)将所估计的外参与原始静态标定外参值做差得到外参误差。

图6中所示的过程在线运行，为自动驾驶***提供实时的激光雷达外参误差，以便提升自动驾驶***的环境感知能力。这里需要注意，在线部署运行过程中，基于预积分的车辆状态轨迹计算不需要覆盖整个历史数据，只需要覆盖滑动窗口内的数据即可，这点与大数据引擎构建不同。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于IMU预积分的自动驾驶***激光雷达在线标定方法，其特征在于：包括如下步骤：

步骤1，采集并预处理传感器数据，所涉及的传感器包括激光雷达和IMU，将传感器数据转换为算法常用格式；

步骤2，进行预积分与状态轨迹计算，即对IMU数据流进行滑动窗口预积分，并求取车辆状态轨迹；

步骤3，将激光点云数据和车辆状态轨迹分别转为深度图，然后将二者融合为混合深度图；

步骤4，缓存历史混合深度图，将若干帧混合深度图融合为大深度图，输入给深度卷积神经网络，通过深度卷积神经网络计算出激光雷达外参误差。

2.根据权利要求1所述的基于IMU预积分的自动驾驶***激光雷达在线标定方法，其特征在于：所述步骤1中的IMU设备包括加速度计和角速度计两部分，二者分别测量车辆的三轴加速度和三轴角速度后输出。

3.根据权利要求2所述的基于IMU预积分的自动驾驶***激光雷达在线标定方法，其特征在于：所述步骤2中的预积分的计算方式为：以激光雷达数据到来的时刻k为边界，将k到k+1时刻的IMU数据预先积分并存储起来，具体公司如下：

式中，

表示t时刻由IMU测量得到的三轴加速度向量，

表示由IMU测量的四元数形式三轴角速度向量，

表示从t_k时刻到t时刻范围内车辆的空间角度变化，Δv_k，k+1为车辆速度的预积分值，Δp_k，k+1为车辆位置的预积分值，Δq_k，k+1为车辆角度的预积分值。

4.根据权利要求3所述的基于IMU预积分的自动驾驶***激光雷达在线标定方法，其特征在于：所述步骤2中的车辆状态轨迹由以下步骤计算得出：

步骤21，取k时刻的车辆位姿增广为下式所示：

于是整个数据流中的车辆状态轨迹可由如下变量表示，其中N表示激光雷达数据的总帧数：

步骤22，使用列文伯格-马夸尔特方法从IMU数据流中优化得到最优的车辆状态轨迹，其中定义的代价函数如下：

其中：

注意，上式中，g为重力向量，Δt＝t_k-t_k-1。

5.根据权利要求4所述的基于IMU预积分的自动驾驶***激光雷达在线标定方法，其特征在于：所述步骤3中将激光点云数据和车辆状态轨迹分别转为深度图，然后将二者融合为混合深度图的具体步骤如下：

步骤31，将激光点云转换为深度图，以便与卷积神经网络结合；

步骤32，将车辆位姿数据转换为深度图，以便与激光点云数据和卷积神经网络结合；

步骤33，将激光深度图与车辆位姿深度图结合为分层深度图，即混合深度图，以便卷积神经网络处理；

步骤34，将多帧混合深度图拼接为一幅大深度图，以便融合历史信息，获得对激光雷达外参的准确估计。

Images