一种路侧激光雷达点云坐标动态修正方法

1.本发明涉及数据处理技术领域，尤其是涉及一种路侧激光雷达点云坐标动态修正方法。


背景技术：

2.自动驾驶技术是当前交通领域发展的重点项目之一，国内外的政府部门和各大相关软硬件厂商均在逐步加大力度，提供一套行之有效、安全可靠的自动驾驶解决方案。车路协同技术是利用先进的无线通信技术，在大范围时空中实时采集路网中的动态交通信息，并在多源交通数据融合的基础上，实现车
‑
车、车
‑
路之间实时动态感知，结合车辆端主动安全控制和道路端协同管理，实现更加高效、安全、绿色的智能交通系统。
3.在车路协同系统中，利用路侧基础设施的布设优势可以为自动驾驶车辆提供更丰富、更全面的感知信息。智能车辆上装载的传感器通常受到前后车辆的遮挡等影响而具有局限性。路侧感知设备能够探测到传感器周边交通参与者的行为特征和道路异常事件信息，譬如车辆的超速、变道、逆行等异常行为，交通事故，道路施工区，路面抛洒物、积水、结冰等信息，并将信息通过数据传输设备发送临近车辆，以补充车辆的传感器探测视野盲区，帮助智能车做出更加精准的驾驶决策，提高运行的安全效率。
4.激光雷达技术是一种数据格式较简单且对提取目标物体的空间坐标十分友好的技术手段，已经逐步成为诸多自动驾驶车辆常备的一套感知设备。使用路侧激光雷达对路段进行高频快速扫描，能够建立道路动态场景完整点云数据，并将全体交通参与者的数据纳入v2x数据网络之中，以解决智能车的非视距信息感知。使用路侧布设激光雷达设备以辅助自动驾驶车辆的方案虽仍处于研究阶段，但其具备感知范围更广、数据覆盖更全面以及缩减自动驾驶车辆硬件需求等诸多好处使其具备充足的研究潜力。
5.在实际使用过程中，路侧激光雷达方案还存在着一些问题，其中之一便是交通环境中因大风、重载车辆经过等状况引发的不规律振动所导致的雷达硬件设备晃动。激光雷达传感器不可预测的晃动使得探测数据相对于先前标定传感器外参产生偏差，造成晃动期间雷达定位坐标产生漂移现象，某些条件下甚至可能存在误差达米级以上的情况。不同采样帧之间数据的漂移会对后续进行交通参与者的追踪以及行为特征、轨迹特征分析增加较大的难度，同时也会造成识别出的目标对象轨迹偏离真实轨迹，对后续路侧设施与智能车辆信息传递以及车辆驾驶决策产生较大的影响，甚至造成不必要的交通事故损失。因此，需要在激光雷达工作时动态地修正当前雷达探测点云数据的偏差，以增加对象追踪的准确性以及数据的可用性。
6.中国的专利cn202010382518.1提出了一种基于激光雷达反射强度点特征的相机
‑
激光雷达相对外参标定方法。其目的是利用激光雷达的反射强度差异，采集标定版上的特征点信息，通过迭代最近点算法进行相机
‑
激光雷达相对外部参数标定，并求得变换矩阵，进而统一传感器检测数据坐标。该专利利用标定版上的不同特征点的激光雷达反射强度差异来进行传感器外参标定，能够在不对相机及激光雷达做旋转平移操作的状态下进行外参
标定，但是对于传感器外参的标定是静态的，没有考虑激光雷达传感器自身的振动对检测结果的影响。


技术实现要素：

7.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种考虑激光雷达传感器自身的振动对检测结果影响的路侧激光雷达点云坐标动态修正方法。
8.本发明的目的可以通过以下技术方案来实现：
9.一种路侧激光雷达点云坐标动态修正方法，所述的路侧激光雷达通过立杆安装于路侧，包括以下步骤：
10.s1：在路面设置控制点，作为固定特征点；
11.s2：获取路侧激光雷达的初始修正参数；
12.s3：获取路侧激光雷达安装立杆的杆端偏转角、固定特征点的真实坐标以及路侧激光雷达采集的交通场景点云数据，并进行预处理；
13.s4：根据点云数据、杆端偏转角和偏转修正公式，对路侧激光雷达点云数据进行偏转修正；
14.s5：根据固定特征点的真实坐标，确立固定特征点的真实坐标与点云数据坐标之间的映射关系，对路侧激光雷达点云数据进行二次坐标修正。
15.进一步地，所述的控制点为反射强度强于周边环境的特征点，包括交通标志、交通标线或人为增设标志点。如果检测范围内没有合适的特征物形成传感器容易识别的控制点，可以人为新增的控制点位，新增的所有控制点应当满足其反射强度均明显强于周边环境，并保证新增的控制点，为不影响交通参与者的正常道路使用。设立多个控制点时，点位与点位之间需要保持一定的间隔，以减小传感器探测与识别控制点时所产生的误差。
16.进一步地，所述的杆端偏转角具体包括：路侧激光雷达安装立杆的杆端x轴和y轴的偏转角，所述的x轴和y轴为对应路侧激光雷达点云坐标的x轴和y轴方向。
17.进一步地，每个路侧激光雷达的扫描区域内至少包括4个不共线的控制点，且相邻路侧激光雷达的扫描区域内至少包括1个共用的控制点。
18.更进一步地，所述的杆端偏转角通过两个倾角传感器采集得到，所述的两个倾角传感器均安装于路侧激光雷达安装立杆上，设置于路侧激光雷达旁侧，且两个倾角传感器保持在同一水平面内并互相垂直，可以使两个倾角传感器所感知的偏移量特征具有正交性。
19.优选地，所述的固定特征点的真实坐标通过rtk设备采集获得。
20.进一步地，步骤s4具体包括：
21.s41：若点云数据未经激光雷达输出处理的原始点云数据，则执行步骤s43，否则执行步骤s42；
22.s42：记录路侧激光雷达自身位置在点云数据中的坐标，通过平移使点云数据以路侧激光雷达自身为原点，并执行步骤s43；
23.s43：利用偏转修正公式，对路侧激光雷达点云数据进行偏转修正。
24.更进一步地，所述的偏转修正公式为：
[0025][0026]
其中，x1、y1、z1表示路侧激光雷达偏转修正前的点云坐标，x0、y0、z0表示路侧激光雷达偏转修正后的点云坐标，
ɑ
和β分别表示路侧激光雷达绕y轴和绕x轴的旋转角，角度方向以顺时针为正，t为旋转修正矩阵，θ表示旋转后的z轴与原z轴之间的夹角，(n
x
,n
y
,n
z
)表示单位化的旋转轴向量。
[0027]
进一步地，步骤s5具体包括：
[0028]
s51：从点云数据中筛选出固定特征点所在的点云部分；
[0029]
s52：根据固定特征点的真实坐标，在点云数据中搜索该坐标周围设定范围内的疑似区域；
[0030]
s53：根据固定特征点的真实坐标，确立固定特征点的真实坐标与其在点云数据中的坐标之间的关系，获取二者的偏移特征并计算点云数据坐标与真实坐标之间的映射关系；
[0031]
s54：构建点云数据坐标与真实坐标之间的仿射变换函数；
[0032]
s55：通过仿射变换函数修正路侧激光雷达检测帧中的所有点云坐标。
[0033]
更进一步地，步骤s54中，构建仿射变换函数时，在保证邻接疑似区域包含共用固定特征点的前提下，随机选取4个固定特征点，构建的仿射变换函数表达式为：
[0034][0035]
其中，a、b、c分别表示仿射变换参数，x0、y0、z0表示路侧激光雷达偏转修正后的点云坐标，x0、y0、z0表示对应固定特征点的真实坐标。
[0036]
与现有技术相比，本发明具有以下优点：
[0037]
1)本发明的路测激光雷达点云坐标动态修正方法面向真实的交通环境场景，重点解决因大风、重载车辆经过等情景下，道路或传感器安装支架的不规律振动所导致的雷达硬件设备晃动而造成的晃动期间雷达对视野范围内，交通参与者的检测点云定位坐标发生漂移的现象，实用性高，在修正过程中考虑激光雷达传感器自身的振动，能够有效提高后续检测的准确性；
[0038]
2)本发明所需要的系统安装步骤和布设方案简单灵活，后端数据处理过程快速，数据计算成本较小，可以满足路测激光雷达传感器采集数据时实时动态修正的需求；
[0039]
3)本发明中步骤s3和步骤s4对应两套设备系统，步骤s3对应倾角传感器，步骤s4对应路面控制点，两套设备系统之间不是强关联状态，，即步骤s4中进行的二次修正，可以在步骤s3经过偏转修正的基础上，也可以不以此为基础，若某一套系统功能失效不影响另一套系统的正常使用，提高系统修正精度的同时增强系统鲁棒性，避免部分系统失效导致系统整体瘫痪。
附图说明
[0040]
图1为本发明方法流程示意图；
[0041]
图2为路侧激光雷达和倾角传感器的安装示意图；
[0042]
图3为路侧激光雷达和倾角传感器的安装侧视图；
[0043]
图4为路侧激光雷达和倾角传感器的安装俯视图；
[0044]
图5为控制点选取方法示意图。
[0045]
其中，1、立柱，2、倾角传感器，3、路侧激光雷达，4、支撑脚架，5、几何中心点，6、端点。
具体实施方式
[0046]
下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
[0047]
实施例1
[0048]
本发明提供一种路侧激光雷达点云坐标动态修正方法，所述的路侧激光雷达通过立杆安装于路侧，包括以下步骤：
[0049]
s1：在路面设置控制点，作为固定特征点，其中，控制点为反射强度强于周边环境的特征点，包括交通标志、交通标线或人为增设标志点，同时，每个路侧激光雷达的扫描区域内至少包括4个不共线的控制点，且相邻路侧激光雷达的扫描区域内至少包括1个共用的控制点。。
[0050]
s2：获取路侧激光雷达的初始修正参数。
[0051]
s3：获取路侧激光雷达安装立杆的杆端偏转角、固定特征点的真实坐标以及路侧激光雷达采集的交通场景点云数据，并进行预处理。
[0052]
固定特征点的真实坐标通过rtk设备采集获得。杆端偏转角具体包括：路侧激光雷达安装立杆的杆端x轴和y轴的偏转角，其中，x轴和y轴为对应路侧激光雷达点云坐标的x轴和y轴方向。杆端偏转角通过两个倾角传感器采集得到，两个倾角传感器均安装于路侧激光雷达安装立杆上，设置于路侧激光雷达旁侧，且两个倾角传感器保持在同一水平面内并互相垂直。
[0053]
s4：根据点云数据、杆端偏转角和偏转修正公式，对路侧激光雷达点云数据进行偏转修正，具体包括以下步骤：
[0054]
s41：若点云数据未经激光雷达输出处理的原始点云数据，则执行步骤s43，否则执
行步骤s42；
[0055]
s42：记录路侧激光雷达自身位置在点云数据中的坐标，通过平移使点云数据以路侧激光雷达自身为原点，并执行步骤s43；
[0056]
s43：利用偏转修正公式，对路侧激光雷达点云数据进行偏转修正。
[0057]
偏转修正公式的表达式为：
[0058][0059]
其中，x1、y1、z1表示路侧激光雷达偏转修正前的点云坐标，x0、y0、z0表示路侧激光雷达偏转修正后的点云坐标，
ɑ
和β分别表示路侧激光雷达绕y轴和绕x轴的旋转角，角度方向以顺时针为正，t为旋转修正矩阵，θ表示旋转后的z轴与原z轴之间的夹角，(n
x
,n
y
,n
z
)表示单位化的旋转轴向量。
[0060]
s5：基于偏转修正的结果，根据固定特征点的真实坐标，确立固定特征点的真实坐标与点云数据坐标之间的映射关系，对路侧激光雷达点云数据进行二次坐标修正，具体包括以下步骤：
[0061]
s51：从点云数据中筛选出固定特征点所在的点云部分；
[0062]
s52：根据固定特征点的真实坐标，在点云数据中搜索该坐标周围设定范围内的疑似区域；
[0063]
s53：根据固定特征点的真实坐标，确立固定特征点的真实坐标与其在点云数据中的坐标之间的关系，获取二者的偏移特征并计算点云数据坐标与真实坐标之间的映射关系；
[0064]
s54：构建点云数据坐标与真实坐标之间的仿射变换函数；
[0065]
s55：通过仿射变换函数修正路侧激光雷达检测帧中的所有点云坐标。
[0066]
构建仿射变换函数时，在保证邻接疑似区域包含共用固定特征点的前提下，随机选取4个固定特征点，构建的仿射变换函数表达式为：
[0067][0068]
其中，a、b、c分别表示仿射变换参数，x0、y0、z0表示路侧激光雷达偏转修正后的点
云坐标，x0、y0、z0表示对应固定特征点的真实坐标。
[0069]
如图1所示，本实施例给出具体实施过程包括以下步骤：
[0070]
(1)设备布设
[0071]
在数据采集前，需先布设所需的器材设备，修正设备分为两个部分，分别为安装于路侧激光雷达附近的两个倾角传感器和布置于路侧激光雷达检测范围内的路面上的高识别特性定位控制点。
[0072]
在已安装路侧激光雷达传感器基础上，首先需要在传感器安装杆上靠近激光雷达的位置安装两个灵敏的倾角传感器，用于感知两个方向的雷达传感器偏移角度。其次需要在路面上设置控制点，设置控制点的目的是在数据修正时作为传感器检测场景中的固定不动的特征点，通过对激光雷达当前检测帧中静止的控制点坐标数据与真实的控制点坐标数据进行比较，计算出当前数据与安装时所标定的传感器外参的偏移特征与偏移量。
[0073]
如图2
‑
图4所示，倾角传感器2和路侧激光雷达3均通过支撑脚架4安装于路侧的立柱1上，将两个倾角传感器2放置于路侧激光雷达3安装位置附近，一般要求倾角传感器2灵敏轴位与路侧激光雷达3圆心的间距不大于10cm。两个倾角传感器2的灵敏轴应当相互垂直，且安装位置应尽量位于同一水平面内，要求两条灵敏轴应当与激光雷达自身的x、y轴相对应，且灵敏轴初始状态下应当保持水平。
[0074]
实际安装过程中，一般先保障路侧激光雷达3安装时初始状态的z轴与重力方向相同，可采用水平仪进行操作；再安装倾角传感器2，同样可通过水平仪校准，安装完成后需利用人工标志物验证三者之间的垂直关系。
[0075]
在实际道路场景之中，对控制点的选取有一定要求。要求控制点在点云数据中能够被清晰标识。由于道路中的交通标识线具有比较明显的光线反射能力，一般情况下可采用特征点明显的交通标志、交通标线。如果检测范围内没有合适的特征物形成传感器容易识别的控制点，可以人为新增的控制点位，新增的所有控制点应当满足其反射强度均明显强于周边环境，并保证新增的控制点，为不影响交通参与者的正常道路使用。设立多个控制点时，点位与点位之间需要保持一定的间隔，以减小传感器探测与识别控制点时所产生的误差。
[0076]
实际操作过程中，路面定位控制点的选取一般有两种做法，一是选择已有的路面标志标线，如车道分界线、导向车道线、导向箭头等，二是对于检测范围内没有合适特征物形成传感器高识别特性控制点的情境下，人工建立新增的路面控制点位。对于选择已有路面标志标线的方案，应当选择磨损程度较轻且图案较为简单的部分，控制点需选择系统易识别的点位，如端点6或角点等，或备选方案是选取某独立标识的几何中心点5，或选择标识上离激光雷达最远或最近的唯一点，便于系统在点云数据中提取控制点坐标，在后续特征点识别时减小误差，其中几何中心点5和端点6的选取如图5所示。
[0077]
对于端点6，在点云数据中筛选出目标区域后，直接提取最远点或最近点即可；对于几何中心点5，求目标区域中所有点的x坐标均值和y坐标均值即可得到近似的几何中心坐标，虽然可能因点云分布不均导致位置偏移，但该偏移对于自动驾驶场景处于可接受误差范围内。需要注意的是，上述提取方法的前提为，控制点区域轮廓应当被较为准确的提取，要求边缘区域也应当被准确提出，点云数据面积与实际控制区面积之差应小于8％，以保证对场景内控制点的识别精度。对于新建路面控制点位，可在不影响交通的区域人工设
立t型或十字型标识，涂层应当选择反射强度较高的材料，使得侧重点与周围环境形成明显差异，以确保在点云数据中可清晰分辨，具体处理方式同上。控制点的布设间隔与激光雷达设备的性能有关，经实验测定，一般选取4至5米左右。
[0078]
(2)数据采集及预处理
[0079]
设备布设完成后，需要确定激光雷达传感器的初始修正参数。确定初始的修正参数通过数据预采集的方式进行。在实施过程中可以通过人工或机械方法模拟环境振动场景并采集数据，注意振动幅度不宜过大。后端采集的数据包括路侧激光雷达获取的交通场景内的点云坐标数据，两个倾角传感器采集的杆端x轴和y轴的偏转角和利用rtk设备采集的路面标识点真实精确坐标，上述所有数据需进行时间同步以保证数据修正过程中的准确性。
[0080]
实际使用过程中，由于所选取的控制点自身不会移动，因此正常情况下，控制点的gps精确定位坐标只需在系统投入使用前测量一次即可作为定值输入系统，但倾角数据为动态变化数据，因此必须保障二者之间的时间同步关系。另外，系统运行中，出于运行效率的考虑，倾角数据和控制点点云坐标的传输频率可以不一致，一般要求倾角数据必须保持与激光雷达同步传输，即每帧点云数据都需有匹配的倾角数据；但通过点云数据提取的控制点坐标可允许低频率传输，即多帧数据共有一套控制点坐标，在振动发生频率低的地区，预设值可以设置为每1小时传输一次，频率较高的地区预设值可设置为每5分钟传输一次。
[0081]
(3)偏转修正
[0082]
由前期实验表明，交通环境中的振动对杆端造成的位移可视为小形变，不足以影响服务于自动驾驶的感知结果的精确度，然而相比之下靠近激光雷达传感器的杆端角度偏转对于探测远距离目标的点云数据产生较大影响，这是因为在激光雷达发射端振动造成的小角度偏转，会随着激光束行程距离的增加而放大，当激光束探测到较远距离的目标时，点云数据的偏移足以影响对物体轨迹的追踪与判断，因而安装杆端由于振动造成的角度偏转是特别需要被修正的部分。忽略杆端位移，视为杆端的激光雷达设备在振动下仅发生角度偏转，利用点云数据中的扫描角信息、倾角传感器数据和偏转修正公式进行角度修正。
[0083]
如果实际投入使用中激光雷达输出的点云坐标经过了处理，即原心不再是激光雷达设备自身，则需先记录激光雷达自身位置在点云数据中的坐标，建立平移关系式，使数据以激光雷达自身为原心。若可获取点云原始数据亦可替代上述步骤。已知目标点在偏转后的雷达坐标系中的坐标以及偏转后的坐标系与原坐标系之间的夹角
ɑ
、β，两个夹角均由倾角传感器获取，通过构建旋转修正矩阵获取原坐标系在偏转后坐标系中的向量，最后求取目标点对该向量的截距即为理论上的真实坐标。利用如下公式修正激光雷达偏转造成的定位误差：
[0084][0085]
其中，x1、y1、z1表示路侧激光雷达偏转修正前的点云坐标，x0、y0、z0表示路侧激光雷达偏转修正后的点云坐标，
ɑ
和β分别表示路侧激光雷达绕y轴和绕x轴的旋转角，角度方向以顺时针为正，t为旋转修正矩阵，θ表示旋转后的z轴与原z轴之间的夹角，(n
x
,n
y
,n
z
)表示单位化的旋转轴向量。所有点云数据通过代入上述公式进行修正即可得到消除坐标系偏转导致的定位误差，对于经过平移处理后的数据还需进行逆平移处理使之坐标表示与输出要求一致。由测试结果表明，上述方法对15米以外的点云坐标修正效果较为明显，修正的误差占总误差的80％以上，最大修正距离在1米以上。
[0086]
(4)坐标修正
[0087]
经过角度修正后的点云数据仍然可能存在定位误差，可利用已知点位的控制点对所有坐标进行二次修正，以获取点云的准确位置数据。在设备布设阶段，为了使控制点能够被清晰明显地识别出来，所选取的控制点的反射强度与周围道路反射强度有明显的差异。因此在当前点云数据中，利用点云数据与周围数据反射强度差异值，可以筛选出疑似为控制点所在区域的点云部分，再通过已知控制点的精确定位坐标，在点云数据中搜索其坐标周围一定范围内的疑似区域，该范围通常选为控制点间距的1/2(2米左右)为搜索半径，一般在前期选点的人为控制下只会在搜索范围内匹配出一个疑似区域。系统预先记录每个控制点对应控制区域的相对位置，如最远最近端点或几何中心等，通过简单计算即可快速地在疑似区域中提取出控制点的位置坐标，再与其真实坐标建立映射关系。根据设置的控制点的准确位置坐标确立点云数据中控制点的坐标之间的偏移特征计算点云数据与真实坐标之间的映射矩阵参数。
[0088]
考虑到应用环境中一般为等间距布设于路侧的多台激光雷达设备组成的点云感知网络，每台激光雷达的扫描范围通常存在重合区域用于点云拼接。因此，为求解下述方程，需在每台激光雷达的点云扫描区域内随机选择4个控制点位，但应当保证邻接区域存在共用点位。将4个控制点坐标代入以下公式：
[0089]
[0090]
其中，a、b、c分别表示仿射变换参数，x0、y0、z0表示路侧激光雷达偏转修正后的点云坐标，x0、y0、z0表示。rtk采集的对应固定特征点的真实坐标。
[0091]
将激光雷达检测的控制点点云坐标与控制点的真实坐标数据带入到改公式中，可以求解出公式中的所有参数，并以此可构建完整的仿射变换函数。最后通过仿射变换函数即可修正当前激光雷达检测帧中所有点云坐标。在大风影响或载重车辆经过导致路面振动的场景下，对激光雷达检测的过程中的点云数据坐标动态计算仿射变换矩阵的参数，并实时构建仿射变换函数，动态修正点云坐标数据以得到准确的检测结果。
[0092]
由测试结果表明，上述修正方法主要对控制区域附近的点云效果明显，越靠近控制区域边缘的点云的修正效果越小，在第一次修正的基础上基本能修正90％的数据误差，平均的点云误差大小基本控制在5cm以下，几乎不影响自动驾驶功能。
[0093]
实施例2
[0094]
本实施例中，步骤(4)进行第二次坐标修正时，可以不引用步骤(3)中偏转修正后的结果，其余与实施例1相同，即在倾角传感器失效的情况下，仅通过步骤(4)的二次修正，仍能实现点云坐标修正功能，经试验测定，该情况下不同区域的误差修正情况不一，平均误差修正率在70％以上。
[0095]
本专利中各名词解释如下：
[0096]
点云数据：point cloud data，是扫描资料以点的形式记录，每一个点包含有三维坐标，和反射强度信息(intensity)。
[0097]
坐标修正：设立设计路面控制点，对雷达硬件设备晃动造成的检测点云定位坐标发生漂移的现象进行坐标的变换，以还原真实的点云坐标。
[0098]
倾角传感器：运用惯性原理的一种加速度传感器，用于系统的水平角度变化测量。
[0099]
控制点：在传感器检测范围内的选定点，利用已知的控制点精确数据对点云数据进行坐标修正。
[0100]
rtk：real
‑
time kinematic，实时动态。载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。
[0101]
仿射变换：在几何中，一个向量空间进行一次线性变换并接上一个平移，变换为另一个向量空间。
[0102]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。