本发明涉及自动驾驶及机器人，具体涉及一种面向矿区的多lidar与radar联合标定方法、装置。
背景技术：
1、随着智慧矿山的兴起和传感器技术的快速发展，自动驾驶在矿山场景下落地，露天矿无人化运输已经成为矿区发展的重要一环。由于对机器越来越高的自动化需求，多传感器的联合使用和多模态数据融合已经十分普遍，可以借此获得全方位信息更好判断周围的环境，使各类传感器取长补短、扬长避短，获得更高精度的信息，最终使得信息既多元又精准。激光雷达(lidar)以及毫米波雷达(radar)作为用于自动驾驶领域的两大重要传感器，常配合使用用于感知融合，一方面能够通过激光雷达的点云数据获得车辆周围环境的精确信息，另一方面通过毫米波雷达输出的障碍物点获得其位置及运动等信息，同时毫米波雷达具有较大的探测范围和抗干扰能力。因此，将多个激光雷达和毫米波雷达构成的系统统一于同一主坐标系下，即进行精确且高效的标定，是车辆能够拥有高精度感知系统的前提和基础。当前的激光雷达-毫米波雷达联合标定技术基本上是针对小型轿车或智能机器人，包括基于标志物的标定方法以及基于环境特征的标定方法。
技术实现思路
1、鉴于上述问题，本发明提供了一种面向矿区的多lidar与radar联合标定方法、装置，解决了现有技术中激光雷达和毫米波雷达标定速度慢、精度差的技术问题。
2、第一方面，本发明提供了一种面向矿区的多lidar与radar联合标定方法，包括：
3、步骤s1、确定车辆传感器标定坐标转换关系，包括：确定主坐标系，确定车辆传感器与主坐标系之间的转换关系；所述车辆传感器包括安装于车辆上的多个毫米波雷达、安装于车辆上的多个激光雷达；所述转换关系为位姿变换矩阵；
4、步骤s2、获取多个毫米波雷达之间的坐标系转换关系，包括：获取毫米波雷达测量的多个角反射器的位置信息，基于所述毫米波雷达测量的多个角反射器的位置信息，计算多个毫米波雷达之间的位姿变换矩阵；
5、步骤s3、获取毫米波雷达与激光雷达的坐标系转换关系；包括：获取激光雷达测量的多个角反射器的位置信息；对所述毫米波雷达测量的多个角反射器的位置信息进行筛选，获取筛选后的毫米波雷达测量的多个角反射器的位置信息；基于激光雷达测量的多个角反射器的位置信息和筛选后的毫米波雷达测量的多个角反射器的位置信息，计算毫米波雷达与激光雷达之间的位姿变换矩阵。
6、优选地，在步骤s1中：所述多个激光雷达中包括一前向激光雷达；所述确定主坐标系包括：将前向激光雷达设置为主激光雷达，将前向激光雷达的坐标系设定为主坐标系；所述确定车辆传感器与主坐标系之间的转换关系包括：构建多个毫米波雷达到主坐标系的转换关系；构建主坐标系到世界坐标系的转换关系。
7、优选地，所述多个毫米波雷达包括：多个前毫米波雷达；
8、所述除前向激光雷达以外的其他所述车辆传感器到主坐标系的转换关系包括：所述多个前毫米波雷达与主激光雷达的转换关系。
9、优选地，在步骤s2中：所述获取毫米波雷达测量的多个角反射器的位置信息包括：获取多个毫米波雷达测量得到的多个角反射器的二维位置坐标，基于所述二维位置坐标计算多个毫米波雷达之间的旋转角和平移量；基于所述多个毫米波雷达之间的旋转角和平移量计算多个毫米波雷达之间的位姿变换矩阵。
10、优选地，所述多个前毫米波雷达为左、中、右毫米波雷达；
11、所述基于所述二维位置坐标计算多个毫米波雷达之间的旋转角θ1和平移量(a1,b1)的计算方式为：
12、
13、
14、
15、其中，(xl1,yl1)表示左毫米波雷达观测到第1个角反射器的二维位置坐标，(xm1,ym1)表示中毫米波雷达观测到第1个角反射器的二维位置坐标，(xl2,yl2)表示左毫米波雷达观测到第2个角反射器的二维位置坐标，(xm2,ym2)表示中毫米波雷达观测到第2个角反射器的二维位置坐标；
16、通过以上公式进行非线性优化计算得到结果(θ1,a1,b1)。
17、优选地，在步骤s3中，所述获取激光雷达测量的标志物的位置信息包括
18、获取主激光雷达测量的三个角反射器点云；对所述角反射器点云进行聚类，获取多个点云聚类；排除所述多个点云聚类中与激光雷达距离大于4米的聚类；提取地面点云，获取地面的法向量方向上的高度，基于所述高度去除各点云聚类中和地面距离为所述高度的点；获取三个角反射器点云所对应的最终点云聚类；计算各最终点云聚类的中心点的三维位置坐标，将所述各点云聚类的中心点的所述三维位置坐标转换为二维位置坐标，作为激光雷达测量的角反射器点二维位置坐标，将所述角反射器点二维位置坐标作为激光雷达测量的多个角反射器的位置信息。
19、优选地，所述对所述毫米波雷达测量的标志物的位置信息进行筛选，获取筛选后的毫米波雷达测量的标志物的位置信息包括：
20、获取左、中、右毫米波雷达在坐标区间内测量的角反射器坐标点；基于所述坐标区间内的左、中、右毫米波雷达测量得到的所有角反射器坐标点的数量、被左、中、右毫米波雷达中两个或三个毫米波雷达同时检测到的角反射器坐标点的数量、仅被所述左、中、右毫米波雷达中一个毫米波雷达检测到的角反射器坐标点的数量进行筛选，获取筛选后的所述角反射器的二维位置坐标作为毫米波雷达测量的多个角反射器的位置信息。
21、优选地，基于所述坐标区间内的左、中、右毫米波雷达测量得到的所有角反射器坐标点的数量、被左、中、右毫米波雷达中两个或三个毫米波雷达同时检测到的角反射器坐标点的数量、仅被所述左、中、右毫米波雷达中一个毫米波雷达检测到的角反射器坐标点的数量进行筛选具体包括：
22、取左、中、右毫米波雷达在坐标x(-5，5)，y(0，5)区间内测量得到的所有角反射器坐标点的集合设定为集合a；将被左、中、右毫米波雷达中两个或三个毫米波雷达同时检测到的点的集合设定为集合b；将仅被所述左、中、右毫米波雷达中一个毫米波雷达检测到的点的集合设定为集合c；基于集合a、集合b和集合c中的点的数量进行筛选。
23、优选地，步骤s3中，基于激光雷达测量的多个角反射器的位置信息和筛选后的毫米波雷达测量的多个角反射器的位置信息，计算毫米波雷达与激光雷达之间的位姿变换矩阵具体包括：
24、获取激光雷达测量得到的两个或三个角反射器的二维位置坐标、毫米波雷达测量得到的两个或三个角反射器的二维位置坐标；对所述两种二维位置坐标进行匹配对应，当匹配的坐标点数量至少为2时，毫米波雷达与激光雷达之间的旋转角θ2和平移量(a2,b2)的计算方式为：
25、
26、
27、
28、其中，(xm1,ym1)表示三个毫米波雷达同时观测到的第1个角反射器的二维位置坐标，(xl1,yl1)为主激光雷达观测到的第1个角反射器的二维位置坐标，(xm2,ym2)表示三个毫米波雷达同时观测到的第2个角反射器的二维位置坐标，(xl2,yl2)为主激光雷达观测到的第2个角反射器的二维位置坐标；
29、通过以上公式进行非线性优化计算得到结果(θ2,a2,b2)；基于所述毫米波雷达与激光雷达之间的旋转角和平移量计算毫米波雷达与激光雷达之间的位姿变换矩阵。
30、第二方面，本技术实施例提供了一种面向矿区的多lidar与radar联合标定装置，包括：
31、确定单元，用于确定车辆传感器标定坐标转换关系，包括：确定主坐标系，确定车辆传感器与主坐标系之间的转换关系；所述车辆传感器包括安装于车辆上的多个毫米波雷达、安装于车辆上的多个激光雷达；所述转换关系为位姿变换矩阵；
32、第一获取单元，用于获取多个毫米波雷达之间的坐标系转换关系，包括：获取毫米波雷达测量的多个角反射器的位置信息，基于所述毫米波雷达测量的多个角反射器的位置信息，计算多个毫米波雷达之间的位姿变换矩阵；
33、第二获取单元，用于获取毫米波雷达与激光雷达的坐标系转换关系；包括：获取激光雷达测量的多个角反射器的位置信息；对所述毫米波雷达测量的多个角反射器的位置信息进行筛选，获取筛选后的毫米波雷达测量的多个角反射器的位置信息；基于激光雷达测量的多个角反射器的位置信息和筛选后的毫米波雷达测量的多个角反射器的位置信息，计算毫米波雷达与激光雷达之间的位姿变换矩阵。
34、与现有技术相比，本发明至少具有如下有益效果：
35、1.本发明依托于矿区实际环境，设计开发出一套能够标定多个激光雷达和毫米波雷达的完整方案，使流程规范化，能够提升自动驾驶矿车标定的安全性与有效性，并且能够提升多个激光雷达和毫米波雷达的标定效率。
36、2.本发明包括可适用于矿区环境的标定算法，能够解决矿区场景下针对矿车标定时所面临的问题。同时具有一定的纠错和优化能力，在标定前进行场景的初步判断和传感器数据的初步检查，能够提升多个激光雷达和毫米波雷达标定的精度。
1.一种面向矿区的多lidar与radar联合标定方法，包括以下步骤：
2.如权利要求1所述的面向矿区的多lidar与radar联合标定方法，其特征在于，在步骤s1中：
3.如权利要求2所述的面向矿区的多lidar与radar联合标定方法，其特征在于，所述多个毫米波雷达包括：多个前毫米波雷达；
4.如权利要求3所述的面向矿区的多lidar与radar联合标定方法，其特征在于，在步骤s2中：
5.如权利要求4所述的面向矿区的多lidar与radar联合标定方法，其特征在于：
6.如权利要求3所述的面向矿区的多lidar与radar联合标定方法，其特征在于，在步骤s3中，所述获取激光雷达测量的多个角反射器的位置信息包括：
7.如权利要求6所述的面向矿区的多lidar与radar联合标定方法，其特征在于，所述对所述毫米波雷达测量的多个角反射器的位置信息进行筛选，获取筛选后的毫米波雷达测量的多个角反射器的位置信息包括：
8.如权利要求7所述的面向矿区的多lidar与radar联合标定方法，其特征在于，基于所述坐标区间内的左、中、右毫米波雷达测量得到的所有角反射器坐标点的数量、被左、中、右毫米波雷达中两个或三个毫米波雷达同时检测到的角反射器坐标点的数量、仅被所述左、中、右毫米波雷达中一个毫米波雷达检测到的角反射器坐标点的数量进行筛选具体包括：
9.如权利要求8所述的面向矿区的多lidar与radar联合标定方法，其特征在于，步骤s3中，基于激光雷达测量的多个角反射器的位置信息和筛选后的毫米波雷达测量的多个角反射器的位置信息，计算毫米波雷达与激光雷达之间的位姿变换矩阵具体包括：
10.一种面向矿区的多lidar与radar联合标定装置：包括以下单元：
