其中，i表示第i帧，l表示激光雷达坐标系，t表示t时刻，b表示惯导坐标系，rlb为惯导坐标系中心与激光雷达坐标系中心的杆臂量，为目标物点云数据，为惯导坐标系中心对应的大地坐标系数据，为惯导坐标系到导航坐标系的转换矩阵，为导航坐标系转换至大地坐标系的转换矩阵，为激光雷达坐标系转换到惯导坐标系下的角度转换矩阵。

所述的密度聚类为基于kd树Zui近点寻找算法优化的密度聚类算法。

所述将相邻帧目标物点云在大地坐标系下的位置进行重合度判定时，重合度越高说明本次迭代的安装标定位置越jingque。

使用分步迭代法对初始参数(x,y,z,α,β,γ)进行迭代。

所述使用分步迭代法对初始参数(x,y,z,α,β,γ)进行迭代时，第一次迭代的步长大于第二次迭代的步长。

与现有技术相比，本发明具有以下优点：

(1)将卫星导航系统/惯性导航系统与激光雷达的安装标定问题转换为相邻帧的点云帧重合度问题，具有高精度和高分辨率等优点。

(2)采用的安装标定方法，克服了传统测量方法中的人为误差，无需人员控制，操作简单，具有高效性和准确性，只需采集完数据程序自动执行输出Zui优标定参数。

(3)采用多线激光雷达与卫星导航系统/惯性导航系统系统，无需其他的测量工具，节省成本，标定结果表明能够减少安装标定位置的误差。

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例

本实施例提供一种基于分步迭代法与kd树优化的密度聚类算法(dbscan)寻求激光雷达Zui优安置参数的一种方法：对激光雷达与gnss/ins初始安装位置进行粗略测量后通过分步迭代法迭代初始参数附近的值，并基于kd树优化的密度聚类算法将转换到大地坐标系下的相邻帧点云集位置重合度进行判定，分步迭代过程中，寻求到第一步所得的局部Zui优参数后，再通过设置第二步的小范围步长，在第一步局部Zui优参数周边寻求全局Zui优解，通过相邻帧目标物点云位置重合度判定得到更为jingque的安置位置。

(1)先用卷尺粗略测量激光雷达相对于惯性导航系统的初始相对安装位移向量(x,y,z)；由于激光雷达通常安装于雷达支架顶部，水平放置且数据导出线朝向车尾。相对于惯导坐标系(b系)的欧拉角，在ros系统下roll和pitch通常设置为0，yaw设置为顺时针90度，即(roll,pitch,yaw)＝(0°,0°,90°)或(α,β,γ)＝(0°,0°,90°)。

(2)通过寻找一个有明显特征平面的行驶场景，如有墙、阶梯、树木等垂直二维平面，驾驶载有激光雷达、卫星导航系统和惯性导航系统的汽车在行驶场景内低速行驶，记录下采集到的行驶场景数据包，行驶场景数据包包括激光雷达坐标系数据、惯性导航系统的惯导坐标系数据和卫星导航系统的大地坐标系数据，然后在数据包内随机取100帧左右的激光雷达坐标系数据相应帧的惯导坐标系数据的姿态四元数与大地坐标系数据实际上这些保存下来的激光雷达坐标系数据就是目标物相对激光雷达的坐标位置将转换到大地坐标系下，可以得到每一帧目标物点云数据在大地坐标系下的唯一对应关系，由此就搭建好了唯一的点云评判比较环境，因为大地坐标系下的坐标是唯一的。

(3)全局遍历法需要对数据的Zui小步长单位进行遍历，若数据量过于庞大，则对处理系统而言压力较大且耗时较长。分步迭代法先在区间内求出局部Zui优解，再在局部Zui优解周边寻求全局Zui优解，与全局遍历法相比加快了遍历速度，提高了效率。设置迭代步长与上下限，对初始参数(x,y,z,α,β,γ)中的每一个分量依次通过分步迭代法迭代附近的值。先设置第一次迭代的步长，第一次迭代的步长可以设置大一些做大范围粗略的迭代，如表1所示。

表1分步迭代法第一次迭代与第二次迭代Zui优步长

通过公式并基于kd树Zui近点寻找算法优化的密度聚类算法(dbscan)，将转换到大地坐标系下的相邻帧目标物的位置重合度进行评价，若两帧内目标物的重合度越高，则说明本次迭代的安装位置越jingque，保留相应的安装参数。

(4)寻求到局部Zui优参数后，再通过设置第二步的小范围步长，如表1所示，在局部Zui优参数周边寻求全局Zui优解，采用与第(3)步的方法对目标物的位置重合度进行评价，获得达到重合度要求的迭代结果。

德国GSR直动式电磁阀_德国GSR强制先导式电磁阀_德国GSR黄铜电磁阀

GSR 电磁阀 D211/0401/7008/-EE DN150(0-3Bar 220VAC)

GSR 电磁阀 Au313607 R0510110

GSR 电磁阀 Au311175 R0510310 24V

GSR 电磁阀 AV1411535 B28030801.032XXPRESSURE

GSR 电磁阀 k0511810 24V 6.8W 0.3-16bar G3/4 24VDC

GSR 电磁阀 A5232/02/012 AC220V

GSR 电磁阀 A5234/02/012 AC220V

D2206/0501/7005-MAEZ DIN2526 PN2.5MPa DN25

50-DR7521 DIN2526 PN2.5MPa DN50

D2206/0501/8005-MAEZ DIN2526 PN2.5MPa DN50

A0306/0804/MSEZ DIN2526 PN2.5MPa DN50

80-DR7521-1290 DIN2526 PN2.5MPa DN80

K0510190GA1230VAC0804

具体而言：

如图2所示，1是位置a，2是位置b，3是激光雷达坐标系(l系)，4是惯导坐标系(b系)，5是车辆的行驶轨迹线，6是位置a投射的第二目标物，7是位置b投射的第二目标物，8是位置a投射的第一目标物，9是位置b投射的第一目标物。粗略测量初始参数(x,y,z,α,β,γ)，对每个分量进行分步迭代。在位置a与位置b下通过激光雷达对目标物进行扫描，利用gnss/ins给出的姿态四元数与大地坐标系将6、7、8、9转换到大地坐标系下，使用基于kd树Zui近点寻找算法优化的密度聚类算法(dbscan)将转换到大地坐标系下的相邻帧目标物的位置重合度进行判断，点云重合度越高则保留当前迭代的安置参数，直到六个分量都完成了Zui优求解。

坐标系转换的理论推导如下：如图3所示，10是激光雷达，11是目标物的点云数据，12是gnss/ins，13是大地坐标系。

目标物点云数据在大地坐标系下的位置rim(t)的计算公式为：

其中，i表示第i帧，l表示激光雷达坐标系，t表示t时刻，b表示惯导坐标系，rlb为惯导坐标系中心与激光雷达坐标系中心的杆臂量，为目标物点云数据，为惯导坐标系中心对应的大地坐标系数据，为惯导坐标系到导航坐标系的转换矩阵，为导航坐标系转换至大地坐标系的转换矩阵，为激光雷达坐标系转换到惯导坐标系下的角度转换矩阵。