自动驾驶合集29

Taobao 开发板商城 whaosoft aiot http://143ai.com  天皓智联  自动驾驶合集

 # 路径规划概述

1 决策控制与运动规划概述

目前决策控制方法可以分为三类：sequential planning、behavior-aware planning、和end-to-end planning。

• sequential planning：最传统的方法，感知、决策与控制三个部分层次较为清晰；

• behavior-aware planning：相比第一种亮点在于引入人机共驾、车路协同以及车辆对外部动态环境的风险预估；

• end-to-end planning：DL、DRL技术，借助大量的数据训练，获得从图像等感知信息到方向盘转角等车辆控制输入的关系，属于时下最热门的方法之一。

本文将对sequential planning进行介绍，按照整个决策控制顺序讲述自动驾驶汽车的感知控制过程，最后会简要总结一下前文所提到的待解决的问题。

Control architecture for automated vehicles

2 路径规划概述

sequential planning 的过程简要概括为路径规划->决策过程->车辆控制，本文讲述的路径规划属于第一步与第三步。

https://mediatum.ub.tum.de/doc/1523134/74110395763.pdf

在无人车的运动轨迹生成问题上，有直接轨迹生成法与路径-速度分解法两种，相比第一种，路径-速度难度更小，因此更加常用。

2.1 路径规划的类型

路径规划可分为四大类：以PRM、RRT为代表的基于采样的算法、以为A* 、D* 代表的基于搜索的算法、以β样条曲线为代表的基于插值拟合的轨迹生成算法，和以MPC为代表的用于局部路径规划的最优控制算法。本小节将按照上述顺序逐一讲解：

A Review of Motion Planning Techniquesfor Automated Vehicles

2.2 路径规划算法优缺点

3 路径规划方法

3.1 基于采样的算法

3.1.1 基本算法PRM与RRT

(1) PRM

PRM算法（Probabilistic Road Map）。PRM主要包含了两个步骤，一是学习阶段，二是查询阶段。

第一步，学习阶段：对状态空间内的安全区域均匀随机采样ｎ个点，并删除采样落在障碍物上的点，接着对相邻的点进行连接并做碰撞检测，剔除不是collision-free的连线，最终得到一个连通图。

第二步，查询阶段：对于给定的一对初始和目标状态，利用上一步构建好的采样节点及连续，运用图搜索的方法（Dijkstra或者A*）来找出一条可行路径。

完成PRM构建后，就可以用于解决不同初始、目标状态的运动规划问题，但是这个特性对于无人车运动规划而言是不必要的。另外PRM要求对状态之间作精确连接，这对于存在复杂微分约束的运动规划问题是十分困难的。

(2) RRT

RRT (Rapidly-exploring Random Tree)算法。RRT其实代表了一系列基于随机生长树思想的算法，是目前机器人领域运用最广泛、优化变种最多的一类算法.

① 树初始化：初始化树的结点集和边集，结点集只包含初始状态，边集为空；

② 树的生长：对状态空间随机采样，当采样点落在状态空间安全区域时，选择当前树中离采样点最近的结点，将其向采样点扩展连接；若生成的轨迹不与障碍物发生碰撞，则将该轨迹加入树的边集，该轨迹的终点加入到树的结点集

③ 重复步骤②，直至扩展到目标状态集中，相比PRM的无向图而言，RRT构建的是初始状态作为根结点、目标状态作为叶结点的树结构，对于不同的初始和目标状态，需要构建不同的树。

RRT不要求状态之间的精确连接，更适合解决像无人车运动规划这样的运动动力学问题。

3.1.2 采样法的问题与解决办法

求解效率与是否最优解。PRM与RRT拥有概率完备性的原因在于其几乎会遍历构型空间中所有位置。

(1) 求解效率

在提升求解效率方面，优化RRT的核心思想在于引导树向空旷区域，即尽量远离障碍物，避免对于障碍物处的节点的重复检查，以此提升效率。主要解决方法：

① 均匀采样

标准RRT算法对状态空间均匀随机采样，当前树中结点获得扩展的概率与其Voronoi区域面积成正比，所以树会向着状态空间的空旷区域生长，均匀充满状态空间的自由区域。

RRT-connect算法同时构建两棵分别起始于初始状态和目标状态的树，当两棵树生长到一起时则找到可行解。Go-biaing在随机采样序列中以一定比例插入目标状态，引导树向目标状态扩展，加快求解速度，提高求解质量。

heuristic RRT使用启发式函数增加扩展代价低的结点被采样的概率，计算树中每个结点的代价，但是在复杂环境中，代价函数的定义困难的，为解决这一问题，f-biased采样方法先将状态空间离散化为网格，再使用Dijkstra算法计算每个网格上的代价，这个网格所在区域的点的代价值都等于该值，以此构建启发式函数。

② 优化距离度量

距离用来度量两个构形之间路径的代价，辅助生成启发式代价函数，引导树的走向。但在考虑障碍物的情况下，距离计算的难度高，运动规划中距离的定义采用类似欧氏距离的定义。RG-RRT （rechability guided RRT）可以消除不准确的距离对RRT探索能力的影响，它需要计算树中结点的能达集，当采样点到结点的距离大于到该结点能达集的距离时，该节点才有可能被选中进行扩展。

③ 降低碰撞检查次数

碰撞检查采样法的效率瓶颈之一，通常的做法是对路径等距离离散化，再对每个点处的构形作碰撞检查。resolution complete RRT通过降低靠近障碍物的结点获得扩展的概率，它对输入空间离散化，对于某个结点输入只使用一次；若某个输入对应的轨迹与障碍物碰撞，则对该节点加上一个惩罚值，该惩罚值越高，该节点获得扩展的概率越小。dynamic domain RRT与adaptive dynamic domain RRT限制采样区域在当前树所在的局部空间，以防止靠近障碍物的结点反复扩展失败，提高算法效率。

④ 提升实时性

Anytime RRT先快速构建一个 RRT，获得一个可行解并记录其代价，之后继续采样，但仅将有利于降低可行解代价的结点插入树中，从而逐渐获得较优的可行解。Replanning将整个规划任务分解为若干等时间的子任务序列，在执行当前任务的同时规划下一个任务。

(2) 在解决最优性问题上主要有以下方法：

RGG算法（random geometric graph）：根据随机几何图理论对标准PRM和RRT 改进的具有渐近最优性质的PRM、RRG和RRT 算法，在状态空间中随机采样ｎ个点，并将距离小于r(n)的点连起来，就构成了RGG。

RRT* 算法：在RRG基础上引入“重新连接”步骤，检查新插入结点作为其临近点的父结点是否会使其临近点的代价降低，若降低，则去掉临近点原来的父子关系，将当前插入点作为其父结点，这就是RRT*算法。

LBT-RRT算法：大量的结点连接和局部调整使得PRM和RRT的效率十分低下。LBT-RRT算法将RRG和RRT* 算法结合起来，在获得渐进最优性的前提下，获取更高的效率。

3.2 基于搜索的算法

基本思想是将状态空间通过确定的方式离散成一个图，然后利用各种启发式搜索算法搜索可行解甚至是最优解，该种类别算法比较成熟。

基于搜索的算法的基础是状态格子，状态格子是状态空间离散化，由状态结点和从该结点出发到达相邻结点的运动基元组成，一个状态结点可以通过其运动基元变换到另一个状态结点。状态格子就将原来连续的状态空间转化为一个搜索图，运动规划问题就变成了在图中搜索出一系列将初始状态变换到目标状态运动基元，构建起状态格子后就可以使用图搜索算法来搜索最优轨迹。

3.2.1 基础算法Dijkstra、A*的构建

Dijkstra算法遍历整个构型空间，找出每两个格子之间的距离，最后选择出发点到目标点的最短路径，其广度优先的性质导致效率很低，在该算法的基础上加入启发式函数，即所搜索结点到目标节点的距离，并以此为基础再次进行搜索可避免全局搜索带来的效率低下，这即为A*算法，如下图所示，红色为搜索区域。

图6：A*与Dijkstra算法效果对比图

3.2.2 搜索法的问题与建议

与基于采样的算法相同，这种类别的算法也需要做效率与最优性的优化。

在提升效率上面，A* 本身属于静态规划的算法，针对A* 算法的延申有weighted A* 通过增加启发式函数的权重进一步引导搜索方向向这目标节点进行，搜索速度很快，但是容易陷入局部极小值，无法保证全局最优解。

对于运动的车辆来说，使用A* 的衍生算法D（dynamic A）可大幅度提升效率。同样以动态规划为基础的还有LPA，该算法可以处理状态格子的运动基元的代价是时变的情况，当环境发生变化时可以通过对较少数目节点的重新搜索规划出新的最优路径。在LPA 的基础上开发出D*-Lite可以获得与D*同样的结果，但是效率更高。

在进行最优化解的探寻时，ARA* 是在Weighted A* 基础上发展出的具有Anytime性质的搜索算法，它通过多次调用Weighted A* 算法且每次调用就缩小启发式函数的权重，这样算法可以快速求出可行解，通过引入集合INCONS使得每次循环可以继续使用上一次循环的信息，对路径做出优化，逐渐逼近最优解。

在兼顾算法效率与最优性的问题上，Sandin aine等提出了MHA* 算法，引入多个启发式函数，保证其中有一个启发式函数在单独使用时可以找到最优解，从而通过协调不同启发式函数生成的路径代价，可以兼顾算法的效率和最优性。DMHA在MHA的基础上在线实时生成合适的启发式函数，从而避免局部最小值问题。

3.3 基于插值拟合的算法

基于插值拟合的算法可被定义为：根据已知的一系列用于描述道路图的点集，通过使用数据插值与曲线拟合的方式创造出智能车将行驶的路径，该路径可提供较好的连续性、较高的可导性。具体的方法如下：

Dubins曲线和Reeds and Sheep(RS)曲线是连接构形空间中任意两点的最短路径，分别对应无倒车和有倒车的情况。它们都是由最大曲率圆弧和直线组成的，在圆弧和直线连接处存在曲率不连续，实际车辆按照这样曲线行驶时必须在曲率不连续处停车调整方向轮才能继续行驶。

多项式插值曲线是最常用的一种方法，它可以通过满足结点的要求来设定多项式系数，并且获得较好的连续可导性，四阶多项式常用于纵向约束控制，五阶多项式常用于横向约束控制，三阶多项式也被用于超车轨迹中。

样条曲线具有封闭的表达式，容易保证曲率连续性。β样条曲线可以实现曲率连续性，三次Bezier曲线可以保证曲率的连续性和有界性，并且计算量相对较小。η^3曲线[43]是一种七次样条曲线，它有着很好的性质：曲率连续性和曲率导数的连续性，这对于高速行驶车辆是很有意义的。

3.4 基于最优控制的算法

将基于最优控制的算法归在路径规划中，主要是因为其中的MPC可以进行局部的路径规划以进行避障，除此之外，MPC主要的作用是进行轨迹跟踪，其所考虑的问题除了必要的动力学、运动学约束以外，未来还应考虑舒适性、感知信息的不确定性、车间通信的不确定性，并且在局部轨迹规划时还可以将驾驶员纳入控制闭环。对于以上所提到的不确定性问题与将驾驶员纳入控制闭环将在第四节讨论。关于MPC的学习，主要从优化理论与工程实践两个方面入手。对于前者，推荐Dimitri P. Bertsekas的Convex Optimization Algorithms，James B. Rawlings的Model Predictive Control：Theory, Computation, and Design。中文领域刘浩洋老师的最优化书个人觉得相对清晰易懂。对于后者，首先龚建伟老师的那本无人驾驶MPC书强推了，老版书里的demo有问题，不过都在新版里解决了。

MPC所使用的预测模型有很多种：诸如卷积神经网络、模糊控制、状态空间等等，其中用的最多的为状态空间法。MPC可简要表述为：满足必要的动力学、运动学等等约束的情况下，通过数值手段求解模型的最优解，该最优解即为状态方程的控制量，如方向盘转角等等，并将控制量作用在车模上以获得要求的状态量，如速度、加速度、坐标等等。

通过上述描述可知，MPC的关键在于模型的建立与模型的求解，如何等效简化模型的建立以及提升求解的效率是重中之重。在不同的控制输入下车辆会走不同的轨迹，每一条轨迹都与之对应一个目标函数值，无人驾驶车辆会通过求解算法找出最小目标函数值对应的控制量，并将其作用在车上，如下图所示：

为了降低建模难度，也有使用人工势能场模型进行建模，人工势能场的基本思想类似于电场，道路上的障碍物类似于电场中与场源相异电荷极性的电荷。障碍物（动态、静态）处的势能更高，无人车将向低势能位置前进。      whaosoft aiot http://143ai.com

4 开源项目

推荐一个开源项目CppRobotics：

• Path Planning

• Dijkstra

• A Star

• RRT

• Dynamic Window Approach

• Model Predictive Trajectory Generator

• Cubic Spline Planner

• State Lattice Planner

• Frenet Frame Trajectory

5 学习方法

入门新领域的学习脉络是：工程、理论以及视野三驾马车齐头并进，以路径规划为例：

5.1 工程

指的是了解各路径规划算法内容，一边从广度上了解各算法内容，一边从深度上深入学习各算法细节。关于路径规划领域的算法，当前没见全面的教程，但是龚建伟的NMPC运动规划可以参考。

5.2 理论

指的是了解支撑这些算法运算数学原理以及这些算法产生的原因（数学视角）。

• 构建目标函数与约束条件同时求极值来得到最优控制量（路径），属于最优化理论；

• 在求解最优控制量时大家常见的牛顿法、最速下降法等等这些数值求解方法，本质来自于数值求解代数等式方程，属于数值分析；

• 求解过程中所见到的导数雅可比矩阵、判断条件中的向量范数等等，本质就是把一维数值求解放到了高维，属于矩阵理论。

5.3 视野

指的是了解路径规划在科研以及企业的主要应用，手段分别为科研文献以及成果报告等等。

6 小结

本文介绍了当前路径规划的梗概，了解目前路径规划有那些方法。内容很繁杂，很难在没有实际应用导向的情况下下短期全部学通，只能在需要的时候再重点学习。

# 自动驾驶场景中的长尾问题~解决

自动驾驶长尾问题是指自动驾驶汽车中的边缘情况，即发生概率较低的可能场景。感知的长尾问题是当前限制单车智能自动驾驶车辆运行设计域的主要原因之一。自动驾驶的底层架构和大部分技术问题已经被解决，剩下的5%的长尾问题，逐渐成了制约自动驾驶发展的关键。这些问题包括各种零碎的场景、极端的情况和无法预测的人类行为。

自动驾驶中的边缘场景

长尾”是指自动驾驶汽车 (AV) 中的边缘情况，边缘情况是发生概率较低的可能场景。这些罕见的事件因为出现率较低且比较特殊，因此在数据集中经常被遗漏。虽然人类天生擅长处理边缘情况，但人工智能却不是这样。可能引起边缘场景的因素有：带有突起的卡车或者异形车辆、车辆急转弯、在拥挤的人群中行驶、乱穿马路的行人、极端天气或极差光照条件、打伞的人，人在车后搬箱子、树倒在路中央等等。

例子：

1. 放透明薄膜在车前，透明物体是否可以被识别，车辆是否会减速

2. 激光雷达公司Aeye就做了一次挑战，自动驾驶如何处理一个漂浮在路中央的气球。L4级无人驾驶汽车往往偏向避免碰撞，在这种情况下,它们会采取规避动作或者踩刹车，来避免不必要的事故。而气球是个软性的物体，可以直接无障碍的通过。

解决长尾问题的方法

合成数据是个大概念，而感知数据(nerf, camera/sensor sim)只是其中一个比较出圈的分支。在业界，合成数据在longtail behavior sim早已成为标准答案。合成数据，或者说sparse signal upsampling是解决长尾问题的第一性解法之一。长尾能力是模型泛化能力与数据内含信息量的乘积。

特斯拉解决方案：

用合成数据（synthetic data）生成边缘场景来扩充数据集
数据引擎的原理：首先，检测现有模型中的不准确之处，随后将此类案例添加到其单元测试中。它还收集更多类似案例的数据来重新训练模型。这种迭代方法允许它捕获尽可能多的边缘情况。制作边缘案例的主要挑战是收集和标注边缘情况的成本比较高，再一个就是收集行为有可能非常危险甚至无法实现。

NVIDIA解决方案：

NVIDIA 最近提出了一种名为“模仿训练”的战略方法（下图）。在这种方法中，真实世界中的系统故障案例在模拟环境中被重现，然后将它们用作自动驾驶汽车的训练数据。重复此循环，直到模型的性能收敛。

以下真实场景中由于卡车高度过高(上)、车辆凸出部分遮挡后车(下)导致模型输出时车框丢失，成为边缘场景，过NVIDIA改进后的模型可以在此边缘情况下生成正确的边界框：

一些思考：

Q：合成数据是否有价值？

A: 这里的价值分为两种 ， 第一种是测试有效性， 即在生成的场景中测试 是否能发现探测算法中的一些漏洞， 第二种是训练有效性， 即生成的场景用于算法的训练是否也能够有效提升性能。

Q: 如何使用虚拟数据提升性能？虚拟数据真的有必要添加到训练集中去吗？添加进去了是否会产生性能回退？

A: 这些问题都难以回答， 于是产生了很多不一样的提高训练精度的方案:

• 混合训练:在真实数据中添加不同比例的虚拟数据， 以求性能提升，

• Transfer Learning:使用真实数据预训练好的模型，然后Freeze 某些layer， 再添加混合数据进行训练。

• Imitation Learning:针对性设计一些模型失误的场景， 并由此产生一些数据，进而逐步提升模型的性能， 这一点也是非常自然的。在实际的数据采集和模型训练中， 也是针对性采集一些补充数据， 进而提升性能。

一些扩展：

为了彻底评估 AI 系统的稳健性，单元测试必须包括一般情况和边缘情况。然而，某些边缘案例可能无法从现有的真实世界数据集中获得。为此，人工智能从业者可以使用合成数据进行测试。

一个例子是ParallelEye-CS，这是一种用于测试自动驾驶汽车视觉智能的合成数据集。与使用真实世界数据相比，创建合成数据的好处是可以对每个图像的场景进行多维度控制。

合成数据将作为生产 AV 模型中边缘情况的可行解决方案。它用边缘案例补充现实世界的数据集，确保 AV 即使在异常事件下也能保持稳健。它也比真实世界的数据更具可扩展性，更不容易出错，并且更便宜。

# GRIL-Calib

 摘要：随着IMU‑LiDAR融合系统重要性的增加，无目标IMU‑LiDAR外在校准方法越来越受到关注。现有的校准方法需要在所有轴上完全运动的情况下推导校准参数。当IMU和LiDAR安装在地面机器人上且其运动仅限于平面运动时，现有的校准方法可能会表现出性能下降，为了解决这个问题，我们提出GRIL‑Calib：一种新颖的无目标地面机器人IMU‑LiDAR校准方法。我们提出的方法利用地面信息来补偿不受限制的全运动的缺乏。首先，我们提出使用地平面残差来提高校准精度的激光雷达里程计（LO）。其次，我们提出了地平面运动（GPM）约束并将其纳入校准优化中，从而能够确定完整的6‑DoF外在参数，包括理论上不可观测的方向。最后，与基线方法不同，我们没有将校准看作连续的两次优化，而是看作单个优化（SO）问题，同时求解所有校准参数并提高准确性。我们通过将GRIL‑Calib应用到三个公共现实数据集，并将其性能与现有最先进方法在准确性和鲁棒性方面的性能进行比较来验证GRIL‑Calib。代码可在https://github.com/Taeyoung96/GRIL获取。

创新点导读：

本文基于FAST-LIO和LI-Init的IESKF算法改进了两点：

（1）地面约束引入到雷达里程计BA优化降低z轴漂移；

（2）地面约束引入到外参标定BA优化提升平面运动的可观测性。

第一点地面约束提升里程计精度是典型做法，主要介绍下第二点地面约束提升外参估计精度。现有无目标约束算法通过运动约束建立激光雷达运动和IMU运动（轨迹/速度加速度等）之间的约束方程进行优化求解，要求六自由度运动，但是小车只有平面运动三自由度运动（水平两维度平移和一个航向角旋转），为了弥补缺失的三自由度（垂直方向平移、横滚和倾斜角旋转），引入地面约束，具体如下图所示。

（1）引入重力方向向量，弥补横滚和倾斜角自由度缺失，具体表现为通过IMU相对于地平面的姿态和雷达相对于地面姿态构建外参旋转约束方程；

（2）引入重力方向的垂直测距值，弥补垂直方向平移，具体表现为引入L相对于G的高度dL和IMU相对与地面的高度计算垂直方向dI的平移约束方程。dL可以通过地面检测计算得到，dI手动测量（会引入误差，但IMU到地面距离实在是算不出来....）

（大拇指往上滑，点击最上方的卡片关注我，整个操作只会花你 1.328 秒，然后带走未来、所有、免费的干货，万一有内容对您有帮助呢～）

一、简介

LiDAR比其他传感器获取更准确的3D信息，广泛适用于各种机器人应用，例如自我运动估计[1]、定位[2]和地图[3]系统。然而，激光雷达的缺点是当其应用于快速运动时，容易出现运动失真。最近，为了克服这个缺点，LiDAR已与IMU（惯性测量单元）[4]、[5]、[6]结合使用。IMU可以高频测量加速度和角速度，可有效补偿LiDAR的运动失真。精确的外参标定是IMU‑LiDAR融合系统的基本要求。外在标定是指在IMU和LiDAR之间建立空间变换的过程，从而将两个传感器的测量结果统一到一个坐标系[7]。

针对IMU和LiDAR之间的外参标定进行了大量的研究，可分为两个方向：基于目标的方法和无目标的方法。基于目标的方法使用已知的预定义对象（例如圆柱体[8]）来估计IMU和LiDAR之间的空间变换。基于目标的方法相对准确，但缺点是需要预先确定目标。另一方面，无目标方法不使用任何预定义的物体进行校准，由于易于使用和可重复性而受到广泛关注。相反，无目标方法需要两个传感器的综合激励运动来激活所有自由度(DoF)。对于具有集成IMU和LiDAR传感器的手持设备来说，实现这种运动非常简单，因为它们可以轻松执行全方位的运动。然而，对于地面机器人来说是一个重大挑战，因为它们的运动仅限于平面运动。

此外，从理论角度来看，缺乏全运动会使Fisher信息矩阵（FIM）的秩小于其维数，而退化或秩不足的FIM将导致估计的校准参数具有较高的不确定性或不成为独特的。为了解决这个问题，一些论文进行了可观测性分析[9]、[10]、[11]，但该分析仅识别不可观测的方向，并没有直接估计外参参数。

在本文中，我们提出了GRIL‑Calib：新型无目标地面机器人IMU‑LiDAR外在标定方法。为了解决上述退化问题，我们利用地平面运动（GPM）约束。GPM提供有关机器人运动的几何信息，即使某些外部参数不可观测，也允许完整的6‑DoF外部参数。地面机器人IMU和LiDAR之间的外在标定问题如图1所示。LiDAR里程计（LO）[12]或视觉惯性里程计（VIO）[13]也提出了与地面相关的类似约束。然而，我们提出了一个地平面运动（GPM)约束，专门针对IMU和LiDAR的外部标定。有关机器人运动的几何信息，即使某些外部参数不可观测，也允许完整的6‑DoF外部参数。

提出了一种新颖的地面机器人IMU‑LiDAR融合系统的外在标定方法，因此不需要满足完全激励运动。

（１)为了提高标定性能，我们提出了一种利用地平面观测的新型激光雷达里程计（LO）。地平面残差被合并到迭代误差状态卡尔曼滤波器(IESKF)中，以减少绕z轴的漂移。

（２）提出了地平面运动（GPM）约束，并将该约束应用于标定的非线性优化。GPM使我们能够解决包括不可观测方向在内的参数，并提高标定精度。

（３）在标定的非线性优化中，我们将平移和旋转参数的标定同时制定为单一优化（SO）问题求解参数。然而，基线方法单独且顺序地标定两个参数，从而降低了标定精度。

（４）在M2DGR[14]、Hilti[15]和S3E[16]数据集上验证了GRIL‑Calib，与最先进的无目标方法相比，它取得了优异的结果。

本文的其余部分组织如下：第二部分对最新IMU‑LiDAR标定方法进行了全面回顾。在第三节中，我们介绍了GRIL‑Calib的系统概述，并在第四节和第五节中详细描述了每个关键部分。第六节描述了在各种真实数据集上进行的实验以及结果分析。最后，第七节提供结论并讨论未来的工作。

二、相关工作

在早期的IMU-LiDAR标定研究中，需要使用任意目标进行标定[8]，[17]。然而，基于目标的方法并不实用，因此最近更多地研究了无需目标的标定方法。Subodh等人[18]提出了一种基于扩展卡尔曼滤波器（EKF）的基于运动的标定方法。此外，Lv等人[19]利用基于B样条的连续时间轨迹形式进行了研究。此外，OA-LICalib [11]扩展了他们以前的工作[19]，提出了两个观测性感知模块，旨在解决退化情况。不幸的是，所提出的观测性感知模块仅设计用于拒绝不必要的运动。FAST-LIO2 [4]是一种基于迭代误差状态卡尔曼滤波器（IESKF）的准确的LiDAR-惯性测距（LIO）算法，其中包括外部标定参数。由于强非线性，LIO系统的性能高度依赖于准确的初始状态。为了获得准确的初始状态估计，Zhu等人[20]提出了一种基于优化的算法，该算法对齐角速度和加速度。现有方法[18]，[11]，[20]需要传感器的所有轴线上的足够运动才能获得准确的结果。当应用于可以自由在各个方向移动的手持设备时，这些方法是实用的。然而，当应用于地面机器人时，现有方法会遇到挑战，因为运动仅限于平面运动。相反，我们的方法利用地面观测，地面机器人可以轻松观察到这些观测，以获取准确的6自由度外部参数，即使在所有轴线上没有足够的运动的情况下也是如此。最近，提出了一种基于扩展卡尔曼滤波器（EKF）的IMU-LiDAR标定算法，利用平面运动中的GNSS观测模型[7]。然而，它需要额外的GNSS传感器，而我们提出的算法可以在没有额外传感器的情况下标定IMU-LiDAR。

三、系统概述

GRIL-Calib的整体系统包括两个主要阶段：预处理和优化，如图2所示。我们的框架受到[20]的启发，第一阶段是预处理阶段，它生成了对接下来的优化阶段至关重要的额外信息。具体而言，预处理阶段执行以下关键任务：

• 通过地面分割进行地面观测，从LiDAR数据中提取地面点，从而便于估计LiDAR传感器相对于地面的相对方向。

• LiDAR测距（LO），利用提取的地面平面并整合提出的地面平面残差，增强LO性能。

• IMU处理，涉及IMU测量的预处理以及使用Madgwick滤波器[21]计算IMU相对于地面的相对方向。

• 数据同步，旨在使用交叉相关方法确定IMU和LiDAR数据之间的近似时间偏移。值得注意的是，此过程与[20]中提出的方法相一致。

第二阶段是优化阶段，旨在利用预处理阶段获取的数据优化外部标定参数。应用三种类型的约束：基于角速度和线性加速度的统一时间-空间约束，以及提出的地面平面运动（GPM）约束。GPM约束确保机器人的运动保持在地面上，并被整合到目标函数中以实现精确的外部标定，同时限制平面运动。与[20]中每种目标函数分别和顺序地优化不同，我们通过为每个目标分配不同的权重值，同时优化所有三种类型的约束。这种方法相比于[20]中的方法可以提高性能。

四、预处理

在预处理阶段，我们获取了一些参数和传感器测量数据，这些将在随后的优化阶段中使用。
A. 通过LiDAR地面分割进行地面观测

B. 利用地平面残差的LiDAR测距
我们使用迭代误差状态卡尔曼滤波器（IESKF）来获取LiDAR测距（LO），其过程类似于[20]。然而，我们通过利用地面机器人被限制在二维平面上移动的假设来提高LO的性能。当我们将LiDAR的状态向量   定义为
 和其运动如下：

将上两个零约束合并你，定义地平面测量模型为：

一阶近似后得到：

C.IMU处理和激光里程计同步

IMU测量模型为：

需要注意的是IMU测量频率远大于雷达里程计，我们分别为IMU和雷达里程计使用两个时间索引i和k，时间戳示意图如图3所示。

五、通过流形优化估计外参

A.时空约束

联合优化文献[20]中的两约束，第一个约束使用IMU和LO的角速度测量构建：

通过补偿时间偏差并假设IMU角加速度为常量，得到：

第二个约束使用IMU和LO的线加速度测量构建：

可以得到约束：

B.地面运动约束（GPM）

写成残差形式：

本文Gril-Calib通过地平面运动约束实现在平面运动下在不客观方向下的外参标定。

C.单优化过程

通过最小化目标函数同时优化三个残差，残差下标表示马氏距离协方差矩阵，系数为柯西核函数。

六、实验

A. 实验设置

B. M2DGR上的外部校准性能

表I总结了 M2DGR 数据集上的校准结果。M2DGR 使用 Velodyne VLP-32C 作为 LiDAR，使用 Handsfree A9 作为 IMU。为了保持一致的比较，所有算法的初始旋转和平移参数都设置为相同。初始旋转参数设置为(−5.0,−5.0, 5.0)°。初始平移参数偏离基于 RMSE 的地面实况 0.402 米，大小为 (0.6, 0.45, 0.6)m。我们将所有剩余的状态向量 tf , bg, ba 初始化为零。数据是从室内（Hall 04 和 Lift 04）和室外（Gate 01 、Rotation 02 和 Street 08 ）收集的，部分可视化如图 5 所 示。表 I总结了 M2DGR 上的校准结果。该表显示，本文 Gril-Calib 明显优于其他竞争方法，特别是与基线 [20]的 LI-Init相比，旋转的 RMSE 降低了 75 %，平移的 RMSE 降低了79 %。

每种标定方法的外部参数如图6所示。图6a 和图6b分别是旋转矩阵和平移矩阵的结果。在这两个图 中，虚线表示外在参数的基本事实。因此，越接近虚线， 校准方法越好。图中没有显示之外ICL ，因为它的误差明显大于其他方法。

C. 其他的外在校准性能

S3E 和 HILTI 数据用作附加数据集，这两个数据集都是在室内捕获的。S3E 数据集使用 Velodyne VLP-16 和 Xsens MTi 30 IMU，而HILTI 数据集使用 Ouster OS0-64 LiDAR 和 AlphaSense IMU。其结果分别总结在表 II和表 III，我们的算法优于其他竞争方法，它显示了本文 Gril-Calib 在各种机器人平台上的可靠准确性。我们还观察到，表 III 中基线 3 序列中的内存计算问题，OA-LICalib 在校准中失败。可能是由于 OA-LICalib 使用 surfel 映射进行校准，计算成本随着 surfel 的数量而迅速增加。相比之下，我们的算法运行流畅，没有任何内存问题。

D. 对初始猜测的鲁棒性

外部校准通常需要初始参数值，并且他显着影响标定结果 。我们改变初始参数进行多次实验，测试不同标定方法对初始参数的鲁棒性。使用M2DGR 中的Gate 01 序列。结果如图8所示。图 8a显示了在 固定初始平移外参但更改旋转外参 时校准误差的变化。图 8b是 固定初始旋转外参但更改平移外参时的结果。有趣的是，OA-LICalib 在前三个数据集中表现出相对较好的性能，但随着初始猜测的增加，其准确性迅速下降。相反，LI-Init 表现出 与最初猜测相反的强大性能，但由于平面运动有限，不如本文 Gril-Calib 准确。

E. 应用于 IMU-LiDAR 融合系统

我们通过将各算法的结果应用于 IMU-LiDAR 融合系统并评估相应的里程计精度来比较竞争的外部校准方法。在实验中，我们利用 Faster-LIO 作为 IMU-LiDAR 融 合系统。使用 hall 02 序列（该序列未在第VI-B小节中用过）。在图7 中描述了不同校准方 法的绝对位姿误差（APE）。可以看出， Gril-Calib 获得的里程计优于其他方法。特别是， Gril-Calib 比　LI-Init 提高了 8.6 %，比 OA-LICalib 提高了 9.6 %。同时该实验表明，IMU-LiDAR 融合SLAM时标定的重要性。

F. 消融研究

进行三个关键模块的消融实验，将结果与基线 [20] 的结果进行比较，如表IV所示。在表格第一行，“GPM”、“SO”和“LO w/ GP”表示我们论文中所做的三项贡献：“使用 GPM 约束的外参标定”、“单次优化标定”和“地平面残差的LO”。表中显示，有SO 比无SO平移外参的 RMSE 降低 34 %，表明旋转外参和平移外参是高度相关的，忽略相关性的分阶段优化会导致精度下降。如图 9 所示，有地面约束的LO比没有地面约束的LO，z 轴漂移有效减少 ，这种漂移的降低导致了更稳定和准确的测量，从而提高了我们校准方法的整体精度。

七、结论

在本文中，我们提出了一种针对地面机器人的新型无目标 IMU-LiDAR 外部标定框架GRIL-Calib。为了解决机器人在平面运动时计算准确的6-DoF 外参可观性差的问题，提出了地面观测的地面运动 （GPM）约束。将校准问题表述为单一优化（SO）问题，保留多个残差的相关性。此外，我们提出了地平面残差函数来减少 LiDAR 里程计 的 z 轴漂移，提升LO的精度，从而间接提高校准的准确性。最后，我们将 GRIL-Calib 应用于三个公共数据集并改变一些初始校准参数来验证算法性能。我们承认，我们的算法的性能可能会受到不平坦地面的影响，未来的工作应侧重于此类地形的校准。

# 搭建仿真环境

网友提问:

“我也了解自驾厂很多都用C++来写代码，我也学习了C++。但如果用C++的话，我应该怎么搭建去搭建仿真的环境呢？比如说要用到哪些软件？(我在一些评论区有看到Linux和ros，但不是很了解)”      --普拉斯兔同学

仿真环境的搭建

1.自己搭建仿真环境

可视化主要就是绘制障碍物，绘制机器人本体，绘制轨迹等。上边的三个方法都可以方便的绘制各种图形和各种颜色。

当然，搭建仿真环境的目的，只是为了验证自己的代码，比如我是想开发一个规划算法，那对我来说，核心开发内容是规划模块，剩余其他各个模块，都是辅助模块，即能够提供给规划模块相应的仿真输入即可，比如下图所示的架构图：

仿真环境主要是两个功能，一是提供相应的输入，二是将结果可视化。可视化我常用的有三种方式：

• opencv

https://github.com/onlytailei/CppRobotics

• python

https://github.com/AtsushiSakai/PythonRobotics

 

• matplotlibcpp

https://github.com/alexliniger/MPCC

2.ros仿真工具

ROS中存在webots、gazebo、stage三种仿真环境。我自己用过的是gazebo和stage。

• gazebo：提供一个仿真的物理世界(3D)

• stage：提供一个简化的仿真物理世界(2D)

• ros navigation导航包：提供感知和导航算法

• turtlebot: 提供一个仿真的机器人模型(包含传感器，执行器)

这四个加起来，就可以跑仿真了。

具体如何仿真可以参考：

• https://www.ncnynl.com/category/ros1-easy3/

• https://www.bilibili.com/video/BV1Ci4y1L7ZZ/?spm_id_from=333.337.search-card.all.click&vd_source=c22e088abc8e5f41edd4d4ebc99ecc48

3.自动驾驶仿真平台

仿真平台的作用是提供模拟的世界和传感器，自动驾驶算法软件则需要如apollo和autoware提供。

CARLA: 开源的自动驾驶仿真平台，提供了丰富的环境和场景来测试自动驾驶系统。CARLA设计了多种传感器模拟和详细的环境设置，支持自定义场景和传感器配置。

LGSVL Simulator: 由LG电子的先进研究院开发的一个高性能仿真平台，支持多种传感器模型和动态环境条件。LGSVL Simulator与多个自动驾驶开发框架（如Apollo和Autoware）集成，支持自动驾驶系统的端到端测试。