CN113359097B - 一种毫米波雷达和相机联合标定的方法 - Google Patents

Abstract

本发明涉及一种毫米波雷达和相机联合标定的方法。具体步骤：①放置角反射器并采集数据；②计算毫米波雷达到车辆坐标系的外参矩阵；③计算相机到车辆坐标系的外参矩阵；④计算相机到毫米波雷达的外参矩阵。本发明利用同一角反射器在不同毫米波雷达空间直角坐标系下的坐标，经坐标转换到WGS84空间直角坐标系，坐标应重合。通过观测车辆运动到不同位置时，同一角反射器在不同毫米波雷达空间直角坐标系下的坐标，通过坐标转换将其转换到WGS84空间直角坐标系下。优化同一角反射器在WGS84空间直角坐标系下的欧式距离平方和，得毫米波雷达到车辆坐标系的外参参数。根据相同原理，可得相机到车辆坐标系的外参参数。通过矩阵相乘，得毫米波雷达到相机的外参参数。

Description

一种毫米波雷达和相机联合标定的方法

技术领域

本发明涉及传感器融合标定技术领域，具体的说是一种毫米波雷达和相机联合标定的方法。

背景技术

毫米波雷达因受光照、天气影响小，监测距离长而被广泛应用于自动驾驶领域。但是毫米波雷达定位精度低，不能精确表达检测物的纹理信息。同毫米波雷达相比，多目相机可以精确检测物体的纹理，并且通过视查得到物体的位置信息，但是相机受光照、天气影响较大。因此，目前的自动驾驶感知系统，通常依赖毫米波雷达、相机、激光雷达等多传感器耦合，得到多种传感器检测的数据，从而提高目标定位及识别的精度，并且提高自动驾驶系统的安全冗余以及稳定性。

[CN111077506A]提出了一种对毫米波雷达进行标定的方法、装置及系统。其根据毫米波雷达获取的特征点数据，以及毫米波雷达与激光雷达之间的外参参数，将毫米波雷达数据投影到激光雷达坐标系下，然后根据激光雷达获取的相应的特征点数据对毫米波雷达进行标定。但是该方法要求毫米波雷达与激光雷达之间外参参数已知，而通常情况下，毫米波雷达与激光雷达之间外参参数未知，需要额外进行标定。

[CN110390697A]提出了一种基于LM算法的毫米波雷达与摄像头联合标定方法，该方法首先人工测量毫米波雷达与摄像头的相对位置关系，构建坐标转移矩阵，然后使用毫米波雷达和摄像头对多个标定物体进行连续观测，分别获得不同标定物体在像素平面坐标系下的像素坐标均值以及雷达点云坐标系的三维坐标均值。然后根据LM算法对坐标转移矩阵进行迭代优化，从而得到优化后的外参矩阵。但是该方法需要预先测量毫米波雷达与摄像头的相对位置关系，初始的坐标转移矩阵对毫米波雷达与摄像头联合标定的精度和速度都有影响，人工测量毫米波雷达与摄像头的相对位置会引入测量误差。

[CN112162252A]提出了一种毫米波雷达与可见光传感器的数据标定方法。该方法将毫米波雷达和可见光传感器同时观测多个金属标定物，然后通过线性最小二乘法计算毫米波雷达的扫描平面坐标系与可见光传感器的平面坐标系之间转换的单应性矩阵，根据求得的单应性矩阵利用单应性变换，将毫米波雷达目标投射到可见光传感器的图像上，通过最近邻算法对毫米波雷达和可见光传感器目标进行数据关联，从而得到毫米波雷达的扫描平面坐标系与可见光传感器图像的平面坐标系之间转换的单应性矩阵，但是根据单应性矩阵并不能直接求得毫米波雷达与可见光传感器之间的外参参数。

[CN111383285A]提出了一种基于毫米波雷达与摄像机传感器融合标定方法及系统。该方法在不同位置放置角反射器，然后使用毫米波雷达和摄像机采集不同位置的角反射器的图像和数据，然后手动提取角反射器中心的图像坐标，并计算图像坐标和相应毫米波雷达坐标之间的单应性矩阵，然后根据单应性矩阵计算每个角反射器中心的重投影误差，手动去除重投影误差大的点，重新计算单应性矩阵，得到优化的结果。该方法使用手动提取角反射器中心坐标，得到的角反射器中心坐标存在人工误差，并且手动剔除重投影误差大的点，增加了标定的工作量。

发明内容

本发明的目的是提供一种毫米波雷达和相机联合标定的方法。该方法利用同一角反射器在不同毫米波雷达空间直角坐标系下的坐标，经过坐标转换到WGS84空间直角坐标系，坐标应该重合。通过观测车辆运动到不同位置时，同一角反射器在不同毫米波雷达空间直角坐标系下的坐标，通过坐标转换将其转换到WGS84空间直角坐标系下。然后通过优化同一角反射器在WGS84空间直角坐标系下的欧式距离的平方和，得到毫米波雷达到车辆坐标系的外参参数。根据相同的原理，将观测得到的同一角反射器在不同相机空间直角坐标系下的坐标，转换到WGS84空间直角坐标系下，优化同一角反射器的WGS84空间直角坐标之间的欧式距离的平方和，可以得到相机到车辆坐标系的外参矩阵。根据矩阵相乘，可以得到毫米波雷达与相机之间的外参矩阵。

本发明的目的是这样实现的：一种毫米波雷达和相机联合标定方法，具体的步骤如下：

步骤1、沿着车辆行驶的道路放置角反射器并使用毫米波雷达和相机采集角反射器的数据；

步骤2、计算毫米波雷达到车辆坐标系的外参矩阵；

步骤3、计算相机到车辆坐标系的外参矩阵；

步骤4、计算相机到毫米波雷达的外参矩阵。

本发明的优点和技术效果是：

1、传统方法标定毫米波雷达及相机需要精确设置角反射器的位置或者需要精确测量毫米波雷达的安装角度，操作复杂；本发明不需要精确测量角反射器的位置，并且对角反射器放置位置无要求，通过观测车辆运动到不同位置时，同一角反射器在不同的毫米波雷达空间直角坐标系下的坐标，根据坐标转换，将同一角反射器在不同毫米波雷达空间直角坐标系的坐标转换到WGS84空间直角坐标系下。

2、毫米波雷达空间直角坐标系是以毫米波雷达重心为坐标原点，X轴指向车前进的方向，Z轴垂直于车辆指向车顶，Y轴分别与Z轴和X轴垂直构成的右手坐标系。毫米波雷达空间直角坐标系是局部坐标系，不同毫米波雷达构成不同的毫米波雷达空间直角坐标系，相同的毫米波雷达在不同位置构成不同的毫米波雷达空间直角坐标系。WGS84空间直角坐标系是以地球质心为坐标原点，地球质心到国际时间服务机构1984.0定义的协议地球极(Conventional Terrestrial Pole，CTP)的方向为Z轴方向，X轴方向指向BIH 1984.0的零子午面和CTP赤道的交点，Y轴分别与Z轴和X轴垂直构成的右手坐标系。由于角反射器在标定过程中静止，即同一角反射器的几何中心在标定过程中其WGS84空间直角坐标系下的坐标固定不变。因此同一角反射器的几何中心在不同毫米波雷达空间直角坐标系的坐标经过转换到WGS84空间直角坐标系下后，坐标应该相等。通过优化坐标转换后的同一角反射器的WGS84空间直角坐标之间的欧式距离的平方和，得到毫米波雷达到车辆坐标系的外参矩阵。车辆坐标系是以车辆重心为原点，Y轴指向于车辆前进的方向；X轴垂直于车辆前进的方向，指向右；Z轴垂直于车辆指向车顶。根据相同的原理，将观测得到的同一角反射器的几何中心在不同相机空间直角坐标系下的坐标，转换到WGS84空间直角坐标系下，优化同一角反射器几何中心的WGS84空间直角坐标之间的欧式距离的平方和，可以得到相机到车辆坐标系的外参矩阵。相机空间直角坐标系是以相机重心为坐标原点，Y轴垂直与车辆，指向车底；X轴垂直于车辆行进的方向，指向右；Z轴指向车辆前进的方向。根据矩阵之间的相乘计算，可以得到毫米波雷达与相机之间的外参矩阵。同传统标定毫米波雷达及相机之间外参参数的方法比较，本发明更加简便，不需要人工测量，易于操作，适用于各种标定场景。

3、操作简单，对角反射器放置没有要求，不需要人工测量，易于应用推广。

4、由于毫米波雷达与相机扫描频率不同，因此某一时刻毫米波雷达和相机可能不能同时扫到同一角反射器，因此分别标定毫米波雷达和相机，计算毫米波雷达到车辆坐标系的外参参数以及相机到车辆坐标系的外参参数，通过矩阵转换得到毫米波雷达与相机之间的外参参数；由于是分别标定毫米波雷达和相机，不要求毫米波雷达和相机同时观测同一特征点，可以应用与不同场景。

5、由于毫米波雷达扫描距离长达300米，而相机的拍摄范围较近，并且距离越远，相机拍摄的物体的精度越低，因此引入权重，通过降低远距离角反射器引起的标定误差的权重，提高毫米波雷达和相机之间的外参参数的精度。

附图说明

图1是本发明毫米波雷达与相机标定系统示意图。

图2是本发明毫米波雷达与相机数据采集流程图。

图3是本发明毫米波雷达与相机标定流程图。

图4是本发明计算毫米波雷达到车辆坐标系的外参矩阵的流程示意图。

图5是本发明优化外参矩阵原理示意图.

图6是本发明计算相机到车辆坐标系的外参矩阵流程示意图。

具体实施方式

一种毫米波雷达和相机联合标定系统，该标定系统包括车辆与角反射器两部分，所述车辆上安装多个毫米波雷达传感器及相机传感器。

本发明方案采用将毫米波雷达传感器及相机传感器分别标定到车辆坐标系下，由于车辆坐标系只与车辆硬件系统以及车行进的方向有关，与传感器的类型、传感器数目以及安装位置无关，因此本发明方案同样适用与多个毫米波雷达以及多个相机之间的标定。

如图1所示：本发明方案提供的实施例为单毫米波雷达与单相机之间的标定，其中S1为相机，S2为毫米波雷达。A～E为角反射器。角反射器是一种金属材质的雷达波反射器。当毫米波雷达扫描到角反射器后，会产生很强的回波信号，因此角反射器常用于毫米波雷达的标定。

如图3所示：一种毫米波雷达和相机联合标定方法，具体的步骤如下：

步骤1、放置角反射器并采集数据

如图2所示：车辆沿着道路行驶，车辆上安装多个传感器，其中传感器包括毫米波雷达、相机、GPS、IMU等；角反射器A～E依次放置在车辆前方的道路两侧，要求角反射器之间距离不宜太近。车辆低速匀速在道路上行驶(例如10km/h)，依次通过角反射器A～E,打开毫米波雷达及相机采集角反射器的数据。

步骤2、计算毫米波雷达到车辆坐标系的外参矩阵(由附图4所示：)

2.1由于毫米波雷达扫描得到的点较稀疏，毫米波雷达扫描得到的一帧数据可能无法分辨物体，因此采用多帧数据进行融合处理；由于扫描的同时，车辆在向前低速运动，如果融合的帧数过多，可能出现同一物体在数据中出现在多个位置的现象；考虑到这种情况，我们利用三维坐标叠加的方法对每5帧毫米波雷达采集数据进行融合；毫米波雷达的扫描频率一般在10帧/s，在采集5帧毫米波雷达数据的时间中，车辆大概行进1.4m，可以忽略车辆行进带来的影响；融合后毫米波雷达数据采集的时间记为融合数据中第一帧数据的采集时间，假设t0为数据采集的起始时间，t0～t0+0.5s(不包含t0+0.5s时刻)采集的5帧毫米波雷达采集数据融合后，数据的采集时间记为t0,t0+0.5s～t0+1s(不包含t0+1s时刻)采集的5帧毫米波雷达采集数据融合后，数据的采集时间记为t0+0.5；对于最后几帧数据，如果帧数不满5帧，则剔除数据；每5帧融合后的毫米波雷达数据称为一切片毫米波雷达数据。

2.2对融合后的毫米波雷达扫描数据，采用DBScan(Density-Based SpatialClustering of Applications with Noise)算法对数据进行聚类；采用DBScan算法是因为其不需要预先设定聚类的数目，且算法简单、易操作。

2.3由于角反射器在毫米波雷达扫描结果中呈高反射，因此毫米波雷达数据中表示角反射器的点反射强度值较大；根据每个聚类的反射强度值I_G，剔除反射强度不符合条件的聚类；数学方程如下所示：

公式(1)中：I_G表示每个聚类的反射强度，I_i表示该聚类中点i的反射强度，n表示聚类中点的个数。I_T表示聚类反射强度的阈值。

2.4采集5帧毫米波雷达数据的时间中，车辆大概行进1.4m，为了保证角反射器在毫米波雷达空间直角坐标系中的位置精度，计算每个聚类的几何中心，用聚类的几何中心表示该聚类。数学方程如下所示：

公式(2)中：

表示t时刻聚类G的几何中心在毫米波雷达空间直角坐标系下三维坐标，(x_i，y_i，z_i)表示聚类中点i在毫米波雷达空间直角坐标系下的三维坐标。毫米波雷达空间直角坐标系是以毫米波雷达重心为原点，X轴指向车辆行驶方向，Y轴垂直于车辆，指向左，Z轴垂直于车辆指向上，X轴与Y轴、Z轴共同组成了右手坐标系；毫米波雷达空间直角坐标系为局部坐标系，不同毫米波雷达构成不同的毫米波雷达空间直角坐标系，相同的毫米波雷达在不同的位置构成不同的毫米波雷达空间直角坐标系。

2.5根据GPS及IMU观测数据得到t0时刻车辆的WGS84空间直角坐标(X_t0，Y_t0，Z_t0)以及车辆的姿态角(α_t0，β_t0，γ_t0)。由于GPS及IMU的扫描频率远大于毫米波雷达的扫描频率，如果GPS或IMU在t0时刻没有采样，可以通过线性插值得到t0时刻GPS及IMU观测数据。假设GPS的采样时刻分别为t0-a与t0+b(a，b均大于0)，则(X_t0，Y_t0，Z_t0)的计算公式为：

公式(3)中：(X_t0-a，Y_t0-a，Z_t0-a)表示GPS在t0-a时刻采样车辆的WGS84空间直角坐标，(X_t0+b，Y_t0+b，Z_t0+b)表示GPS在t0+b时刻采样车辆的WGS84空间直角坐标。

求解t0时刻的姿态角(α_t0，β_t0，γ_t0)比较复杂，由于直接对姿态角进行线性插值会产生死锁，因此需要先将姿态角转换为四元数，通过四元数插值得到t0时刻的四元数，最后将四元数转换为姿态角。假设IMU的采样时刻分别为t0-c与t0+d，(c，d均大于0)则(α_t0，β_t0，γ_t0)的计算公式为：

公式(4)中：(α_t0-c，β_t0-c，γ_t0-c)，(α_t0+d，β_t0+d，γ_t0+d)分别是t0-c与t0+d时刻的姿态角，q_t0-c，q_t0，q_t0+d分别为t0-c，t0，t0+d时刻的四元数，Quanternion为姿态角转换为四元数的函数，slerp为四元数插值函数，θ为q_t0-c到q_t0+d的旋转角度，可通过四元数运算求得，Euler为四元数转换为姿态角的函数。

2.6对经过步骤2.1数据融合后的每份切片毫米波雷达数据，重复步骤2.2-2.4，得到每份切片毫米波雷达数据中的角反射器的几何中心在毫米波雷达空间直角坐标系下的三维坐标。

2.7由于毫米波雷达的最大测距距离可达300多米，因此同一个角反射器可能出现在多份切片毫米波雷达数据中，因此采用匈牙利算法对步骤2.6得到的聚类进行追踪，得到同一个角反射器的几何中心在每份切片毫米波雷达数据中的不同位置。

2.8假设角反射器A的几何中心在WGS84空间直角坐标为(X_A，Y_A，Z_A)，根据t0时刻车辆的WGS84空间直角坐标(X_t0，Y_t0，Z_t0)以及车辆的姿态角(α_t0，β_t0，γ_t0)，可以计算t0时刻角反射器A的几何中心在车辆坐标系下的坐标为

数学公式如下：

公式(5)中：

为WGS84空间直角坐标系转换到车辆坐标系下的转换矩阵，可以通过(X_t0，Y_t0，Z_t0)以及(α_t0，β_t0，γ_t0)获得；X_WGS84为某点在WGS84空间直角坐标系下的坐标，X_C表示该点在车辆坐标系下的坐标。

根据车辆坐标系到毫米波雷达的转换矩阵

可以得到角反射器A的几何中心在毫米波雷达空间直角坐标系下的坐标。即满足数学公式：

公式(6)中：x为角反射器A的几何中心在毫米波雷达空间直角坐标系下的三维坐标

如图5所示：车辆在t0、t1时刻的WGS84坐标分别为(X_t0，Y_t0，Z_t0)以及(X_t1，Y_t1，Z_t1)；t0时刻的车辆坐标系为O-X_{c_t0}Y_{c_t0}Z_{c_t0}，毫米波雷达空间直角坐标系为O-X_{MR_t0}Y_{MR_t0}Z_{MR_t0}，t1时刻的车辆坐标系为O-X_{c_t1}Y_{c_t1}Z_{c_t1}，毫米波雷达空间直角坐标系为O-X_{MR_t1}Y_{MR_t1}Z_{MR_t1}；注意，为了清晰展示车辆坐标系以及毫米波雷达空间直角坐标系，图5对毫米波雷达空间直角坐标系以及车辆坐标系作平移；实际状况下，车辆坐标系的原点在车辆重心，毫米波雷达空间直角坐标系的原点在毫米波雷达重心；角反射器的几何中心在毫米波雷达空间直角坐标系下的坐标x通过毫米波雷达空间直角坐标系到车辆坐标系之间的转换矩阵

可以转换到车辆坐标系下，得到角反射器的几何中心在车辆坐标系下的坐标，再通过车辆坐标系到WGS84空间直角坐标系下的转换矩阵

可以求得角反射器的几何中心在WGS84空间直角坐标系下的坐标x_WGS84；由于角反射器静止不动，不同时刻下同一个角反射器的几何中心在WGS84空间直角坐标系下的坐标相同，即t0时刻角反射器A的几何中心在毫米波雷达空间直角坐标系下的坐标x_t0经过坐标系转换，得到的角反射器A的几何中心在WGS84空间直角坐标系下的坐标，应该等于t1时刻角反射器A的几何中心在毫米波雷达空间直角坐标系下的坐标x_t1经过坐标系转换，得到的角反射器A的几何中心在WGS84空间直角坐标系下的坐标；计算不同时刻同一个角反射器的几何中心在不同毫米波雷达空间直角坐标系的坐标，经过坐标系转换到WGS84空间直角坐标下的坐标误差方程为：

公式(7)中：

分别为t0时刻及t1时刻车辆坐标系转换WGS84空间直角坐标系的转换矩阵，可根据t0时刻及t1时刻车辆的WGS84空间直角坐标以及姿态角获得，x_t0，x_t1分别是t0时刻及t1时刻同一角反射器的几何中心在毫米波雷达空间直角坐标系下的三维坐标。Cost为毫米波雷达扫描的不同时刻同一个角反射器的几何中心的坐标误差。

根据公式(7)，t0以及t0+0.5s时刻同一个角反射器的几何中心在不同毫米波雷达空间直角坐标系的坐标，经过坐标系转换到WGS84空间直角坐标下的坐标误差方程为：

公式(8)中：

分别为t0时刻及t0+0.5s时刻车辆坐标系转换WGS84空间直角坐标系的转换矩阵，可根据t0时刻及t0+0.5s时刻车辆的WGS84空间直角坐标以及姿态角获得，x_t0，x_t0+0.5分别是t0时刻及t0+0.5s时刻同一角反射器的几何中心在毫米波雷达空间直角坐标系下的三维坐标。argmin表示求取最小值的函数。

由于毫米波雷达远距离定位精度较差，引入低精度的反射器在毫米波雷达空间直角坐标系下的三维坐标会降低毫米波雷达到车辆坐标系的转换矩阵的精度。因此即使t时刻追踪到某一角反射器，但是角反射器的几何中心距离毫米波雷达的重心距离超过200m，则放弃追踪该反射器，不计算其误差。对于距毫米波雷达的重心不超过200m的角反射器，引入权重，降低远距离角反射器位置精度不准引起的误差。数学公式如下：

公式(9)中：Cost_T表示总误差，i表示角反射器序列号，由于一共有5个角反射器，i的取值范围为0～4的整数；t表示数据采集的时刻，x_i，t表示第i个角反射器在t时刻在毫米波雷达空间直角坐标系下的坐标。w_i，t表示第i个角反射器在t时刻的误差权重，w_i，t可以有多种计算方式，公式(9)采用第i个角反射器的几何中心在t时刻到毫米波雷达重心的距离的倒数。

误差方程可以通过梯度下降法迭代求解，方程终止的条件是迭代次数超过阈值或者梯度下降幅度小于相应的阈值。

步骤3、计算相机到车辆坐标系的外参矩阵(附图6所示)

3.1由于相机对环境要求较高，比如光照不强烈，气温合适等。因此如果环境条件不佳时，采集的数据精度会受到影响。如果使用传统方法从相片中提取特征点，然后通过特征点来计算相机到车辆坐标系的外参矩阵，可能因为采集数据的精度低，无法从采集到的相片中提取出角反射器的特征，从而造成计算得到的外参矩阵不精确。本专利使用深度学习的方法，通过训练不同环境条件下的角反射器照片，识别出不同应用场景下的角反射器。例如，对相机在车辆行驶途中采集的每帧影像数据，采用YOLO(You Only Look Once)神经网络识别影像中的角反射器，并获得不同时刻同一角反射器i的几何中心在不同像素平面坐标系下的坐标，以及神经网络检测的角反射器i的置信度P_i。像素平面坐标系是以相片左上角为坐标系原点，横轴为u，指向右，对应相机空间直角坐标系中的X轴，纵轴为v，指向下，对应相机直角空间坐标系中的Y轴。像素平面坐标系是局部坐标系。YOLO神经网络是一个可以一次识别多个物体位置和类别的卷积神经网络，能够快速实现端到端的目标检测和识别。

3.2由于YOLO可以同时识别多个物体，识别的目标可能包括其他物体，因此对识别的目标进行筛选，为了保证识别的角反射器的精度，只选择识别的置信度大于90％的角反射器；

3.3由于同一个角反射器可能出现在多帧影像数据中，因此采用匈牙利算法对步骤3.2得到的角反射器进行追踪，得到同一个角反射器的几何中心在多帧影像数据中的不同像素平面坐标系下的坐标。由于相机视场角以及最大测量距离的影响，相机对远距离以及距离很近物体定位精度较差，因此只追踪几何中心距离相机重心的距离在20～50m的角反射器。即当角反射器A的几何中心出现在距离相机重心距离为50m时，开始在影像数据中追踪该反射器，当距离相机重心距离小于20m，放弃追踪该反射器。值得注意的是，本实施例中相机S1为前视相机，只能拍摄在车前的目标物体，当车辆行驶过物体后，物体不会出现在后续的影像数据中，因此不存在车辆驶离角反射器A20m后，再次拍摄到角反射器A的情况。

3.4根据步骤2.8中的原理，角反射器的几何中心在像素平面坐标系下的坐标通过已知的相机内参矩阵K_In的逆矩阵，可以转换到相机空间直角坐标系，根据相机空间直角坐标系到车辆坐标系之间的外参矩阵

可以转换到车辆坐标系下，再通过车辆坐标系到WGS84空间直角坐标系下的转换矩阵

可以求得角反射器的几何中心在WGS84空间直角坐标系下的坐标。由于角反射器在标定过程中静止，不同时刻下同一角反射器的几何中心在WGS84空间直角坐标系下的坐标相同，计算两个不同时刻的同一个角反射器的几何中心在不同像素平面坐标系下的坐标，经过坐标系转换到WGS84空间直角坐标下后，产生的坐标误差方程为：

公式(10)中：

表示t，t1时刻同一角反射器的几何中心在不同像素平面坐标系下的坐标，

表示相机内参矩阵的逆矩阵，相机内参矩阵为已知，

表示相机空间直角坐标系到车辆坐标系的外参矩阵，即为相机到车辆坐标系的外参矩阵。Cost_C为相机拍摄的同一个角反射器的几何中心的坐标误差。

由于标定系统中有多个角反射器，多个角反射器在不同像素平面坐标系下的坐标，经过坐标系转换到WGS84空间直角坐标下后，产生的坐标误差的数学公式如下：

公式(11)中：

表示总误差，i表示t时刻相机追踪到的角反射器序列号，一共有n+1个角反射器，总的追踪时间是从t＝0到t＝m。argmin表示求最小值的操作。

方程可以通过梯度下降法迭代求解，方程终止的条件是迭代次数超过阈值或者梯度下降幅度小于相应的阈值。

步骤4、计算相机到毫米波雷达的外参矩阵

根据步骤2.8)计算得到的毫米波雷达空间直角坐标系到车辆坐标系的旋转矩阵

以及步骤3.4)计算得到的相空间直角坐标系到车辆坐标系的旋转矩阵

可以得到毫米波雷达空间直角坐标系到相机空间直角坐标系的转换矩阵

即为毫米波雷达到相机的外参矩阵，数学公式如下：

公式(12)中：

为毫米波雷达到相机的外参矩阵。

由于车辆坐标系与车辆上安装传感器的类型以及安装位置无关，因此毫米波雷达与相机互换位置对标定方法没有影响，车辆上安装传感器的数目对标定方法同样没有影响，如果车辆上安装多个毫米波雷达以及多个相机，可以将不同的毫米波雷达、相机分别标定到车辆坐标系下，通过矩阵之间的相互转换，可以分别求得毫米波雷达之间的外参矩阵，相机之间的外参矩阵以及不同毫米波雷达与不同相机之间的外参矩阵。举例说明，假设车辆上安装n个(n＞0)毫米波雷达，m个(m＞0)相机，则毫米波雷达j到毫米波雷达i的外参矩阵计算公式：

公式(13)中：

表示毫米波雷达j到毫米波雷达i的外参矩阵，

表示毫米波雷达j到车辆坐标系的外参矩阵，

表示毫米波雷达i到车辆坐标系的外参矩阵。

毫米波雷达i到相机s的外参矩阵计算公式：

公式(14)中：

表示毫米波雷达i到相机s的外参矩阵，

表示相机s到车辆坐标系的外参矩阵，

表示毫米波雷达i到车辆坐标系的外参矩阵。

相机t到相机s的外参矩阵计算公式

公式(15)中：

表示相机t到相机s的外参矩阵，

表示相机t到车辆坐标系的外参矩阵。

Claims (5)

1.一种毫米波雷达和相机联合标定方法，其特征在于：具体的步骤如下：

步骤1、沿着车辆行驶的道路放置角反射器并使用毫米波雷达和相机采集角反射器的数据；

步骤2、计算毫米波雷达到车辆坐标系的外参矩阵；其中，

2.1由于毫米波雷达扫描得到的点较稀疏，毫米波雷达扫描得到的一帧数据可能无法分辨物体，因此采用多帧数据进行融合处理；由于扫描的同时，车辆在向前低速运动，如果融合的帧数过多，可能出现同一物体在数据中出现在多个位置的现象；考虑到这种情况，利用三维坐标叠加的方法对每5帧毫米波雷达采集数据进行融合；毫米波雷达的扫描频率一般在10帧/s，在采集5帧毫米波雷达数据的时间中，车辆大概行进1.4m，可以忽略车辆行进带来的影响；融合后毫米波雷达数据采集的时间记为融合数据中第一帧数据的采集时间，当t0为数据采集的起始时间时，t0～t0+0.5s，不含t0+0.5s这一时刻，采集的5帧毫米波雷达采集数据融合后，数据的采集时间记为t0,t0+0.5s～t0+1s，不含t0+1s这一时刻，采集的5帧毫米波雷达采集数据融合后，数据的采集时间记为t0+0.5；对于最后几帧数据，如果帧数不满5帧，则剔除数据；每5帧融合后的毫米波雷达数据称为一切片毫米波雷达数据；

2.2对融合后的毫米波雷达扫描数据，采用DBScan算法对数据进行聚类；

2.3由于角反射器在毫米波雷达扫描结果中呈高反射，因此毫米波雷达数据中表示角反射器的点反射强度值较大；根据每个聚类的反射强度值I_G，剔除反射强度不符合条件的聚类；数学方程如下所示：

公式(1)中：I_G表示每个聚类的反射强度，I_i表示该聚类中点i的反射强度，n表示聚类中点的个数；I_T表示聚类反射强度的阈值；

2.4采集5帧毫米波雷达数据的时间中，车辆大概行进1.4m，为了保证角反射器在毫米波雷达空间直角坐标系中的位置精度，计算每个聚类的几何中心，用聚类的几何中心表示该聚类；数学方程如下所示：

公式(2)中：

表示t时刻聚类G的几何中心在毫米波雷达空间直角坐标系下三维坐标，(x_i，y_i，z_i)表示聚类中点i在毫米波雷达空间直角坐标系下的三维坐标；毫米波雷达空间直角坐标系是以毫米波雷达重心为原点，X轴指向车辆行驶方向，Y轴垂直于车辆，指向左，Z轴垂直于车辆指向上，X轴与Y轴、Z轴共同组成了右手坐标系；毫米波雷达空间直角坐标系为局部坐标系，不同毫米波雷达构成不同的毫米波雷达空间直角坐标系，相同的毫米波雷达在不同的位置构成不同的毫米波雷达空间直角坐标系；

2.5根据GPS及IMU观测数据得到t0时刻车辆的WGS84空间直角坐标(X_t0，Y_t0，Z_t0)以及车辆的姿态角(α_t0，β_t0，γ_t0)；由于GPS及IMU的扫描频率远大于毫米波雷达的扫描频率，如果GPS或IMU在t0时刻没有采样，可以通过线性插值得到t0时刻GPS及IMU观测数据；当GPS的采样时刻分别为t0-a与t0+b,且a,b均大于0时，(X_t0，Y_t0，Z_t0)的计算公式为：

公式(3)中：(X_t0-a，Y_t0-a，Z_t0-a)表示GPS在t0-a时刻采样车辆的WGS84空间直角坐标，(X_t0+b，Y_t0+b，Z_t0+b)表示GPS在t0+b时刻采样车辆的WGS84空间直角坐标；WGS84空间直角坐标系是一种国际上采用的地心坐标系，坐标原点为地球质心，其地心空间直角坐标系的Z轴指向BIH，即国际时间服务机构，1984.0定义的协议地球极方向，X轴指向BIH 1984.0 的零子午面和CTP赤道的交点，Y轴与Z轴、X轴垂直构成右手坐标系；WGS84空间直角坐标系为全局坐标系，静态物体在不同时刻的WGS84空间直角坐标不变；

求解t0时刻的姿态角(α_t0，β_t0，γ_t0)比较复杂，由于直接对姿态角进行线性插值会产生死锁，因此需要先将姿态角转换为四元数，通过四元数插值得到t0时刻的四元数，最后将四元数转换为姿态角；当IMU的采样时刻分别为t0-c与t0+d,且c,d均大于0时，(α_t0，β_t0，γ_t0)的计算公式为：

公式(4)中：(α_t0-c，β_t0-c，γ_t0-c),(α_t0+d，β_t0+d，γ_t0+d)分别是t0-c与t0+d时刻的姿态角，q_t0-c，q_t0，q_t0+d分别为t0-c,t0,t0+d时刻的四元数，Quanternion为姿态角转换为四元数的函数，slerp为四元数插值函数，θ为q_t0-c到q_t0+d的旋转角度，可通过四元数运算求得，Euler为四元数转换为姿态角的函数；

2.6对经过步骤2.1数据融合后的每份切片毫米波雷达数据，重复步骤2.2-2.4,得到每份切片毫米波雷达数据中的角反射器的几何中心在毫米波雷达空间直角坐标系下的三维坐标；

2.7由于毫米波雷达的最大测距距离可达300多米，因此同一个角反射器可能出现在多份切片毫米波雷达数据中，因此采用匈牙利算法对步骤2.6得到的聚类进行追踪，得到同一个角反射器的几何中心在每份切片毫米波雷达数据中的不同位置；

2.8当角反射器A的几何中心在WGS84空间直角坐标为(X_A，Y_A，Z_A)时,根据t0时刻车辆的WGS84空间直角坐标(X_t0，Y_t0，Z_t0)以及车辆的姿态角(α_t0，β_t0，γ_t0)，可以计算t0时刻角反射器A的几何中心在车辆坐标系下的坐标为

数学公式如下：

公式(5)中：

为WGS84空间直角坐标系转换到车辆坐标系下的转换矩阵，可以通过(X_t0，Y_t0，Z_t0)以及(α_t0，β_t0，γ_t0)获得；X_WGS84为某点在WGS84空间直角坐标系下的坐标，X_C表示该点在车辆坐标系下的坐标；车辆坐标系是以车辆重心为原点，Y轴平行于车辆行驶的方向，指向前；X轴垂直于车身，指向右；Z轴垂直于车身，指向上；车辆坐标系是局部坐标系，不同的车辆构成不同的车辆坐标系，相同的车辆在不同的位置构成不同的车辆坐标系；

根据车辆坐标系到毫米波雷达的转换矩阵

可以得到角反射器A的几何中心在毫米波雷达空间直角坐标系下的坐标；即满足数学公式：

公式(6)中：x为角反射器A的几何中心在毫米波雷达空间直角坐标系下的三维坐标

当车辆在t0、t1时刻的WGS84坐标分别为(X_t0，Y_t0，Z_t0)以及(X_t1，Y_t1，Z_t1)时，t0时刻的车辆坐标系为O-X_{c_t0}Y_{c_t0}Z_{c_t0},毫米波雷达空间直角坐标系为O-X_{MR_t0}Y_{MR_t0}Z_{MR_t0},t1时刻的车辆坐标系为O-X_{c_t1}Y_{c_t1}Z_{c_t1},毫米波雷达空间直角坐标系为O-X_{MR_t1}Y_{MR_t1}Z_{MR_t1}；角反射器的几何中心在毫米波雷达空间直角坐标系下的坐标x通过毫米波雷达空间直角坐标系到车辆坐标系之间的转换矩阵

可以转换到车辆坐标系下,得到角反射器的几何中心在车辆坐标系下的坐标，再通过车辆坐标系到WGS84空间直角坐标系下的转换矩阵

可以求得角反射器的几何中心在WGS84空间直角坐标系下的坐标x_WGS84；由于角反射器静止不动，不同时刻下同一个角反射器的几何中心在WGS84空间直角坐标系下的坐标相同，即t0时刻角反射器A的几何中心在毫米波雷达空间直角坐标系下的坐标x_t0经过坐标系转换，得到的角反射器A的几何中心在WGS84空间直角坐标系下的坐标

应该等于t1时刻角反射器A的几何中心在毫米波雷达空间直角坐标系下的坐标x_t1经过坐标系转换，得到的角反射器A的几何中心在WGS84空间直角坐标系下的坐标

计算不同时刻同一个角反射器的几何中心在不同毫米波雷达空间直角坐标系的坐标，经过坐标系转换到WGS84空间直角坐标下的坐标误差方程为：

公式(7)中：

分别为t0时刻及t1时刻车辆坐标系转换WGS84空间直角坐标系的转换矩阵，可根据t0时刻及t1时刻车辆的WGS84空间直角坐标以及姿态角获得，x_t0，x_t1分别是t0时刻及t1时刻同一角反射器的几何中心在毫米波雷达空间直角坐标系下的三维坐标；Cost为毫米波雷达扫描的不同时刻同一个角反射器的几何中心的坐标误差；

根据公式(7)，t0以及t0+0.5s时刻同一个角反射器的几何中心在不同毫米波雷达空间直角坐标系的坐标，经过坐标系转换到WGS84空间直角坐标下的坐标误差方程为：

公式(8)中：

分别为t0时刻及t0+0.5s时刻车辆坐标系转换到WGS84空间直角坐标系的转换矩阵，可根据t0时刻及t0+0.5s时刻车辆的WGS84空间直角坐标以及姿态角获得，x_t0，x_t0+0.5分别是t0时刻及t0+0.5s时刻同一角反射器的几何中心在毫米波雷达空间直角坐标系下的三维坐标；

由于毫米波雷达远距离定位精度较差，引入低精度的反射器在毫米波雷达空间直角坐标系下的三维坐标会降低毫米波雷达到车辆坐标系的转换矩阵的精度；因此即使t时刻追踪到某一角反射器，但是角反射器的几何中心距离毫米波雷达的重心距离超过200m，则放弃追踪该反射器，不计算其误差；对于距毫米波雷达的重心不超过200m的角反射器，引入权重，降低远距离角反射器位置精度不准引起的误差；数学公式如下：

公式(9)中：Cost_T表示总误差，i表示角反射器序列号，由于一共有5个角反射器，i的取值范围为0～4的整数；t表示数据采集的时刻，x_i，t表示第i个角反射器在t时刻在毫米波雷达空间直角坐标系下的坐标；w_i，t表示第i个角反射器在t时刻的误差权重，w_i，t可以有多种计算方式，公式(9)采用第i个角反射器的几何中心在t时刻到毫米波雷达重心的距离的倒数；argmin表示求取最小值的函数；

误差方程可以通过梯度下降法迭代求解，方程终止的条件是迭代次数超过阈值或者梯度下降幅度小于相应的阈值；

步骤3、计算相机到车辆坐标系的外参矩阵；

步骤4、计算相机到毫米波雷达的外参矩阵。

2.根据权利要求1所述的一种毫米波雷达和相机联合标定方法，其特征在于：步骤1的具体内容为：车辆沿着道路行驶，车辆上安装多个传感器，其中传感器包括毫米波雷达、相机、GPS、IMU等；角反射器A～E依次放置在车辆前方的道路两侧，要求角反射器之间距离不宜太近，车辆低速匀速在道路上行驶；车辆在行驶过程中打开传感器，采集角反射器的数据。

3.根据权利要求1所述的一种毫米波雷达和相机联合标定方法，其特征在于：步骤3的具体内容为：

3.1由于相机对环境要求较高，因此如果环境条件不佳时，采集的数据精度会受到影响；如果使用传统方法从相片中提取特征点，然后通过特征点来计算相机到车辆坐标系的外参矩阵，可能因为采集数据的精度低，无法从采集到的相片中提取出角反射器的特征，从而造成计算得到的外参矩阵不精确；步骤3通过训练不同环境条件下的角反射器照片，使用深度学习的方法识别出不同应用场景下的角反射器；对相机在车辆行驶途中采集的每帧影像数据，采用YOLO神经网络识别影像中的角反射器，并获得不同时刻同一角反射器i的几何中心在不同像素平面坐标系下的坐标，以及神经网络检测的角反射器i的置信度P_i；像素平面坐标系是以相片左上角为坐标系原点，横轴为u，指向右，对应相机空间直角坐标系中的X轴，纵轴为v，指向下，对应相机直角空间坐标系中的Y轴；像素平面坐标系是局部坐标系；相机空间直角坐标系是以相机重心为坐标原点，Y轴垂直于车辆，指向车底；X轴垂直于车辆行进的方向，指向右；Z轴指向车辆前进的方向；相机空间直角坐标系是局部坐标系，不同相机构成不同的相机空间直角坐标系；YOLO神经网络是一个可以一次识别多个物体位置和类别的卷积神经网络，能够快速实现端到端的目标检测和识别；

3.2由于YOLO可以同时识别多个物体，识别的目标可能包括其他物体，因此对识别的目标进行筛选，为了保证识别的角反射器的精度，只选择识别的置信度大于90％的角反射器；

3.3由于同一个角反射器可能出现在多帧影像数据中，因此采用匈牙利算法对步骤3.2得到的角反射器进行追踪，得到同一个角反射器的几何中心在多帧影像数据中的不同像素平面坐标系下的坐标；由于相机视场角以及最大测量距离的影响，相机对远距离以及距离很近物体定位精度较差，因此只追踪几何中心距离相机重心的距离在20～50m的角反射器；即当角反射器A的几何中心出现在距离相机重心距离为50m时，开始在影像数据中追踪该反射器，当距离相机重心距离小于20m，放弃追踪该反射器；

3.4根据步骤2.8中的原理，角反射器的几何中心在像素平面坐标系下的坐标通过已知的相机内参矩阵K_In的逆矩阵，可以转换到相机空间直角坐标系下，根据相机空间直角坐标系到车辆坐标系之间的外参矩阵

可以转换到车辆坐标系下，再通过车辆坐标系到WGS84空间直角坐标系下的转换矩阵

可以求得角反射器的几何中心在WGS84空间直角坐标系下的坐标；由于角反射器在标定过程中静止，不同时刻下同一角反射器的几何中心在WGS84空间直角坐标系下的坐标相同，计算两个不同时刻的同一个角反射器的几何中心在不同像素平面坐标系下的坐标，经过坐标系转换到WGS84空间直角坐标下后，产生的坐标误差方程为：

公式(10)中：

表示t，t1时刻同一角反射器的几何中心在不同像素平面坐标系下的坐标，

表示相机内参矩阵的逆矩阵，相机内参矩阵为已知，

表示相机空间直角坐标系到车辆坐标系的外参矩阵，即为相机到车辆坐标系的外参矩阵；Cost_C为相机拍摄的同一个角反射器的几何中心的坐标误差；

由于标定系统中有多个角反射器，多个角反射器在不同像素平面坐标系下的坐标，经过坐标系转换到WGS84空间直角坐标下后，产生的坐标误差的数学公式如下：

公式(11)中：

表示总误差，i表示t时刻相机追踪到的角反射器序列号，一共有n+1个角反射器，总的追踪时间是从t＝0到t＝m；argmin表示求最小值的操作；

方程可以通过梯度下降法迭代求解，方程终止的条件是迭代次数超过阈值或者梯度下降幅度小于相应的阈值。

4.根据权利要求1所述的一种毫米波雷达和相机联合标定方法，其特征在于：步骤4的具体内容为：

根据步骤2.8)计算得到的毫米波雷达空间直角坐标系到车辆坐标系的旋转矩阵

以及步骤3.4)计算得到的相空间直角坐标系到车辆坐标系的旋转矩阵

可以得到毫米波雷达空间直角坐标系到相机空间直角坐标系的转换矩阵

即为毫米波雷达到相机的外参矩阵，数学公式如下：

公式(12)中：

为毫米波雷达到相机的外参矩阵。

5.根据权利要求1所述的一种毫米波雷达和相机联合标定方法，其特征在于：步骤4的具体内容为：

由于车辆坐标系与车辆上安装传感器的类型、数目以及安装位置无关，因此车辆上传感器的安装位置、车辆上安装传感器的数目对标定方法同样没有影响；即本标定方法同样适用于毫米波雷达与相机互换位置或标定多毫米波雷达和多相机的情况；当车辆上安装多个毫米波雷达以及多个相机时，可以将不同的毫米波雷达、相机分别标定到车辆坐标系下，通过矩阵之间的相互转换，可以分别求得毫米波雷达之间的外参矩阵，相机之间的外参矩阵以及不同毫米波雷达与不同相机之间的外参矩阵；当车辆上安装n个(n>0)毫米波雷达，m个(m>0)相机时，毫米波雷达j到毫米波雷达i的外参矩阵计算公式：

公式(13)中：

表示毫米波雷达j到毫米波雷达i的外参矩阵,

表示毫米波雷达j到车辆坐标系的外参矩阵，

表示毫米波雷达i到车辆坐标系的外参矩阵；

毫米波雷达i到相机s的外参矩阵计算公式：

公式(14)中：

表示毫米波雷达i到相机s的外参矩阵,

表示相机s到车辆坐标系的外参矩阵，

表示毫米波雷达i到车辆坐标系的外参矩阵；

相机t到相机s的外参矩阵计算公式

公式(15)中：

表示相机t到相机s的外参矩阵，

表示相机t到车辆坐标系的外参矩阵。

Images