基于多雷达点云线特征的自适应在线估计标定系统及方法与流程

1.本技术涉及传感器、参数估计和自动驾驶感知技术领域，尤其涉及一种基于多雷达点云线特征的自适应在线估计标定系统及方法。


背景技术：

2.无人机装箱卡车体型巨大，包括车头、拖挂和箱体三部分组成，针对集装箱卡车的感知系统需要综合考虑车身总体范围内的视野尽可能大和死角尽可能小，从而保障自动化作业过程中的安全性。通常情况下需要利用多颗激光雷达来实现对车身周边的全覆盖感知，特别是针对拖挂的箱体位置检测；因此，绝大多数无人集装箱卡车装配了多颗激光雷达。
3.将雷达标定到以车体为中心的车体坐标系，并与其它传感器协同是车辆装配前的必要步骤之一，通常情况下可通过眼手标定估计每个雷达的轨迹，然后进行轨迹之间的配准获得外参；或基于各雷达的重叠视场进行基于标定参考物的关键点同名点检测，进行两两雷达之间的标定。上述方法主要是基于离线标定设计，需要在车辆出厂前完成。为了保障无人集卡的感知视野，通常的雷达安装方案兼顾车身两侧和车顶，传感器之间间距较大。而无人集卡车头由多块金属拼接，属于非刚体结构；通过支架装配的多个雷达之间也非刚体连接，进一步使得雷达之间的稳定性降低。当车辆行驶过程发生震动，所标定的外参无法保障雷达点云能够投影到统一的坐标系，不同雷达点云形成的重影，从而影响后续的目标感知和slam定位等下游任务。
4.此外，无人集卡作业过程需要对箱体，箱体质量大、冲击性强，起放震动对车身零部件影响大。在车辆长时间作业后传感器容易出现轻微的安装偏移，从而影响外参数据，导致投影错误。如当不同雷达投影存在误差，地面车道线信息将出现多条、而路面以上的动态目标则出现重影，对应的检测结果则产生偏差。如果不及时发现，也将影响后续感知任务的作业。而针对传感器长时间工作偏移带来的误差也需要通过停止作业，进行离线标定完成，从而影响总体工作效率。


技术实现要素：

5.为了解决上述技术问题，本技术实施例提供一种基于多雷达点云线特征的自适应在线估计标定系统及方法，用于对无人集装箱卡车车身安装的多雷达进行在线外参估计和自动标定，从而消除或降低由于传感器安装偏移导致的外参误差，保障融合点云能够投影到统一的车体坐标，为后续感知任务提供精准的原始输出信息。
6.本技术实施例第一方面提供了一种基于多雷达点云线特征的自适应在线估计标定系统，包括：
7.外参标定模块，获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷点点云进行外参估计；
8.位姿估计模块，针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车
身状态和间隔时间，估计车身的最新运动状态；
9.特征抽取模块，对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点；
10.位姿估计模块，基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息。
11.进一步地，所述外参标定模块中，将基于初始点云和初始获得的外参矩阵作为计算种子，通过手工测量所安装的多颗雷达到车体中心的距离偏移和旋转偏移，采用ndt或icp算法对雷达点云进行外参估计。
12.进一步地，所述位姿估计模块采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，获得车辆运动位姿信息变化后调整所获得的雷达点云在车体坐标系的位姿。
13.进一步地，所述特征抽取模块包括：
14.点云分割模块：基于点云进行体素化分割，将整个场景内所选取的区域内的点云进行均匀划分；
15.平面获取模块：基于每个体素内的点云进行随机采用一直化的迭代平面抽取算法，获取本体素内的所有平面以及每个三维空间的平面公式；
16.特征抽取模块：针对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在预设范围内的平面对，并求取对应的相交线；将相交线附近预设范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准使用。
17.进一步地，所述平面获取模块中对每个平面的点数量进行过滤，对于所包含的点数量不超过预设下限值数量的平面进行剔除。
18.进一步地，所述位姿估计模块；基于线特征的slam建图与里程估计模块，包括：
19.建图模块，构建一个基于上一帧里程信息所在位置的局部地图，该局部地图的范围小于全局地图范围、大于单帧点云数据；
20.配对模块，获取基于点云线特征抽取模块在当前局部地图中的线特征以及基于法向排序和编号的标签，将线特征的点使用近邻搜索寻找局部地图中的对应特征点，基于法向约束进行线特征的配对。
21.估算模块，基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点采用icp算法获得多组估算结果，并剔除偏差较大的外参估计结果，基于剩余配准外参数据求取均值作为最终结果；
22.更新模块，将当前帧的最终结果投影到全局地图中，更新全局地图和估算里程计信息；利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
23.本技术实施例第二方面提供了一种基于多雷达点云线特征的自适应在线估计标定方法，包括：
24.s100:获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷点点云进行外参估计；
25.s200：针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和
间隔时间，估计车身的最新运动状态；
26.s300：对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点；
27.s400：基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息。
28.进一步地，所述步骤s200采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，获得车辆运动位姿信息变化后调整所获得的雷达点云在车体坐标系的位姿。
29.进一步地，所述步骤s300包括：
30.301)基于点云进行体素化分割，将整个场景内所选取的区域内的点云进行均匀划分；
31.302)基于每个体素内的点云进行随机采用一直化的迭代平面抽取算法，获取本体素内的所有平面以及每个三维空间的平面公式；
32.303)针对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在预设范围内的平面对，并求取对应的相交线；将相交线附近预设范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准使用。
33.进一步地，所述步骤s400包括：
34.401)构建一个基于上一帧里程信息所在位置的局部地图，该局部地图的范围小于全局地图范围、大于单帧点云数据；
35.402)获取基于点云线特征抽取模块在当前局部地图中的线特征以及基于法向排序和编号的标签，将线特征的点使用近邻搜索寻找局部地图中的对应特征点，基于法向约束进行线特征的配对。
36.403)基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点采用i cp算法获得多组估算结果，并剔除偏差较大的外参估计结果，基于剩余配准外参数据求取均值作为最终结果；
37.404)将当前帧的最终结果投影到全局地图中，更新全局地图和估算里程计信息；利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
38.本发明的优点在于：
39.(1)针对多雷达环境观测系统可能导致噪声分布存在差异的情况，使用自适应卡尔曼滤波算法来估计车体运动信息，能够进一步提升卡尔曼滤波对噪声的估计精度，同时提供更加稳定的车体运动位姿信息；
40.(2)基于港口无人集卡所在的场景，提出了基于体素分割的并行平面拟合，以及基于平面相交的点云线特征抽取算法，能够稳定的描述港口场景的点云特征，同时考虑了算法的并行化，提升算法效率；
41.(3)基于点云线特征slam建图与定位算法能够基于数据量较少的点云线特征进行全局建图与里程估计，同时，基于法向约束的配准策略和全局最优化方法进一步提升帧间点云的配准效率和精度；
42.(4)所设计的在线配准系统无需特殊的参考目标，即可通过一定时间的在线运行
和slam算法完成对雷达外参的在线标定；同时，算法可与slam建图与定位系统无缝衔接，提供车辆的实时定位信息。
附图说明
43.为了更清楚地说明本技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
44.图1为多雷达车体安装示意图；
45.图2为系统全局流程图；
46.图3为点云线特征抽取示意图；
47.图4为三个雷达点云单独显示示意图；
48.图5为手工测量三个雷达外参偏移的示意图；
49.图6为直接使用icp进行粗外参估计投影结果示意图；
50.图7为随着算法迭代估计外参的点云误差变化示意图；
51.图8为最终配准结果示意图。
具体实施方式
52.为使得本技术的申请目的、特征、优点能够更加的明显和易懂，下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本技术一部分实施例，而非全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本技术保护的范围。
53.下面结合附图和具体实施例，进一步阐明本发明。
54.在本技术的描述中，需要理解的是，术语“上”、“下”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本技术和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本技术的限制。
55.针对现有技术的问题，本技术提出的一种基于多雷达点云线特征的自适应在线估计标定系统及方法，用于对无人集装箱卡车车身安装的多雷达进行在线外参估计和自动标定，从而消除或降低由于传感器安装偏移导致的外参误差，保障融合点云能够投影到统一的车体坐标，为后续感知任务提供精准的原始输出信息。同时，针对无人集卡所在的港口场景存在大量集装箱体，设计了基于点云体素分割和平面拟合的线特征抽取算法，以及对应的基于点云线特征进行同步定位与建图的slam算法用于估计车辆里程变化。最终，利用slam技术获取准确的里程变化信息，并反向优化雷达外参信息和更新自适应卡尔曼滤波器增益(也就是噪声估计)，从而实现调控闭环。
56.本发明提出的在线标定系统具体包括：
57.外参标定模块，获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷点点云进行外参估计。
58.具体地：外参标定模块通过手工测量所安装的多颗雷达到车体中心的距离偏移t和旋转偏移r，然后基于初始点云和手工测量的外参矩阵t
ei
＝[r，t]作为计算种子，其中i代表对应的雷达编号，使用ndt或icp算法对雷达点云进行外参估计。但由于雷达安装为了保障车辆周围的视场(field of view，fov)的覆盖，因此，雷达之间的重叠fov通常较小，仅能获得相对准确的粗外参信息。
[0059]
位姿估计模块，针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态。
[0060]
具体地：位姿估计模块针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态。由于所能够观测的车身运动主要来自安装在车体的多个雷达，而不同的雷达由于安装位置不同，运动过程中的fov存在一定的差异。在进行后续建图和里程估计时，存在的噪声分布各不相同。传统的卡尔曼滤波和扩展卡尔曼滤波要求目标测量系统时线性的，且状态空间模型已知，噪声分布服从高斯模型钱噪声的协方差已知，在引入多雷达观测系统的情况下难以完全满足上述要求。因此，采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，从而提高对车体运动变化的推算精度，并基于所获得的车辆运动位姿tm信息变化进一步调整所获得的雷达点云在车体坐标系的位姿。
[0061]
特征抽取模块，对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点。
[0062]
具体地：特征抽取模块针对港口集装箱码头存在大量箱体和人工建筑，激光雷达点云具有大量显著的线特征。首先，基于点云进行体素化分割，将整个场景内感兴趣区域(regionof interest，roi)内的点云进行均匀划分，方便并行化加速。然后基于每个体素内的点云进行随机采用一直化random sample consensus(ransac)的迭代平面抽取算法，获取本体素内的所有平面(且仅考虑点数量大于一定阈值的平面)，以及每个三维空间的平面公式ax by cz＝d，其中xyz代表轴，abcd均为系数。然后对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在一定范围内的平面对，并求取对应的相交线。最终，将相交线附近一定范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准。
[0063]
位姿估计模块，基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息。
[0064]
具体地：基于线特征的slam建图与里程估计模块，由于港口场景存在大量人工建筑和箱体，因此丰富的线特征不会导致特征丢失，且相对稳定；同时，基于线特征抽取的点能够降低计算成本。首先，建图过程维护一个基于上一帧里程信息所在位置的局部地图，该地图要远小于全局地图范围，但大于单帧点云数据。基于相同的线特征抽取流程，获得当前局部地图中的线特征，以及基于法向排序和编号的标签。由于车辆行驶过程种场景中的大部分物体不会发生改变，基于当前雷达帧的线特征点使用近邻搜索寻找局部地图中的对应特征点，进一步过滤配准点候选集合的规模，并基于法向约束进行线特征的配对。基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点进行icp算法。获得多组估算结果，并剔除偏差较大的外参估计结果，基于剩余配准外参数据求取均值作为最终结果。并
将当前帧按估算结果投影到全局地图中，更新全局地图和估算里程计信息。
[0065]
最后，利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
[0066]
相应地，本发明提供的在线标定系统，包括以下步骤：
[0067]
步骤100)安装雷达到车体，并通过手工或cad测量安装雷达到车体中心，一般指惯导安装位置的外参信息t
ei
＝[x，y，z，roll，pitch，yaw]；其中，xyz为第i个雷达到惯导中心(车体中心)在三维空间的偏移量，roll，pitch，yaw分别为雷达安装到车体坐标系三个坐标轴的偏差角。上述值可仅精确到分米和10度左右偏差。然后，采集雷达点云信息，首先将顶部雷达采用手眼标定方法标定到车体坐标系；接着，使用ndt或icp将左右两个雷达标定到中间雷达上，标定时使用初始外参信息作为种子，并得到优化后的、各雷达到车体中心的外参t
ei
。当不存在惯导时，则以车顶雷达为车体坐标进行外参估计。
[0068]
步骤200)启动车辆在厂区或测试区行驶，并采集雷达点云信息(j时刻i雷达的点云)。
[0069]
将每帧点云基于自身外参投影到车体中心，获得车体坐标系下的粗雷达并基于该观测值和相邻时间，使用自适应卡尔曼滤波算法(adaptive kalman filter，akf)估计车体的运动位姿变化并估计噪声的协方差。其中，v
point
为直接观测信息，为基于后续slam里程估计得到的间接信息(实际值，需等待步骤400输出)，基于akf估计最新为估计结果(当前步骤可获取)。并基于实际值预测误差噪声、更新增益方程和计算残差。
[0070]
步骤300)基于当前帧点云(不考虑来自哪个雷达)在roi范围内(如roi＝[-30，70，-40，40，-5，5])进行体素空间分割，如20m，则获得以20米为边长的立方体点云空间。然后基于ransac进行平面拟合，获取每个体素内的平面，并基于平面夹角约束获得满足条件的相交直线，直线距离0.3m范围内的点，以及直线法向。得到当前帧的点云线特征。
[0071]
步骤400)基于当前车辆所在估计位置，在slam所构建的全局点云地图中获取局部子图，选取范围为当前位置的前后100m，左右100m范围。采用步骤3中相同的方法获取局部子图的点云线特征集合。基于局部子图的点云线特征和当前雷达的点云线特征进行全局配准，遍历当前帧的点云线特征基于flann算法获取子图特征中距离较近的线特征点，见，并基于法向进行过滤，构建当前点云帧到局部地图的特征点集对获得潜在匹配的线特征点，采用icp算法进行配准。
[0072][0073]
对于每组线特征，可以获得一组估计配准外参t
x
，接着采用模糊估计的方法(如上式所示)，利用ransac剔除所有t
x
中的离群值，然后基于均值估计最终的外参矩阵tf＝mean(ransac(t1，t2，...，t
x
))。最终，基于tf将当前帧点云投影到全局地图，更新全局地图信息，并估算里程变化。并将该结果返回给步骤200)，用于更新自适应卡尔曼滤波器的因子估计，以及修正对应雷达的外参。
[0074]
作为一个具体的实施例，本发明主要的四个阶段工作流如图2所示，下面结合一个实施例具体说明。
[0075]
步骤100)针对如图1所示的集卡车头支架安装了顶雷达、左雷达和右雷达，而由于安装位置和角度不同，各雷达fov仅有部分重叠，三个雷达所获取的点云数据如图4所示。通过手工测量，将雷达标定到车头后轮中心位置，部分测量偏移结果如图5所示。首先基于icp算法和手工测量值进行估算，得到一组雷达初始外参t
ei
＝[x，y，z，roll，pitch，yaw]，其投影结果如图6所示。
[0076]
步骤200)驾驶车辆在线特征丰富的场景行驶，并启动自适应卡尔曼滤波算法估计车辆运动状态。考虑雷达未进行同步，系统收到的最近一帧点云为t时刻i号雷达的点云，则投影到车体坐标下的点云为基于上一帧t-1时刻和当前帧t时刻的点云slam里程估计结果推算车体中心的运动位姿变化，包括平移t＝[x，y，z]
t
和旋转r＝[roll，pitch，yaw]
t
共计6个自由度在se(3)李群代数下的变化估计车体运动的位姿变化用于计算最新的点云
[0077]
步骤300)针对当前点云帧基于roi过滤，并对roi范围内的点云进行体素分割，接着基于ransac对体素内点云进行平面拟合，获得满足平面内点大于200的若干点云平面aix biy ciz di＝0，以及平面法向normi＝[a/d，b/d，c/d]。基于平面相交公式(如下)(排除法向夹角不在[30，150]度的平面对)，求取相交直线，以及到直线距离小于0.2m的点作为线特征集合线特征抽取效果如3所示。
[0078][0079]
步骤400)，基于当前步骤200的车体粗略位姿，获取slam局部地图，若初始情况，则原地停留20秒积累点云。采用步骤300相同算法获取局部地图的线特征集合基于步骤3所得的线特征集合基于flann在中进行近邻搜索，并通过法向约束进行线特征集配
对，够成多组特征集对针对没对数据采用icp进行配置估计，其中来自局部地图的点作为基底(base)，求取将当前帧点云投影到基底的旋转矩阵t
x
。
[0080]
对于每组线特征，可以获得一组估计配准外参t
x
，接着采用模糊估计的方法，利用ransac剔除所有t
x
中的离群值，然后基于均值估计最终的外参矩阵tf＝mean(ransac(t1，t2，...，t
x
))。最终，基于tf将当前帧点云投影到全局地图，更新全局地图信息，并估算里程变化。并将该结果返回给步骤200，用于更新自适应卡尔曼滤波器的因子估计，以及修正对应雷达的外参。
[0081]
通过多轮次迭代，每个雷达基于外参投影到车体的平均旋转误差(root mean square error，rmse)如图7所示会逐渐收敛。最终，基于所估计的外参投影得到的结果如图8所示，能够消除对环境感知的重影。
[0082]
以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护。