CN111830519B - 多传感器融合的测距方法 - Google Patents

Abstract

一种多传感器融合的测距方法，包括：(1)联合标定，获取相机的内参和相机到雷达的转换矩阵，获取雷达目标和图像目标数据；(2)将雷达目标框信息投影到图像坐标系下，匹配雷达目标和图像目标；(3)保持车辆在平坦的直线路线上行驶一定距离，得到雷达目标和图像目标匹配成功的组，使用匹配成功组的雷达测距结果和视觉测距结果拟合视觉系统测距误差曲线；(4)使用匹配成功组的雷达测距结果和系统误差校正后视觉测距结果计算俯仰角；(5)对多组俯仰角结果进行加权融合；(6)根据俯仰角校正视觉测距结果。提高了自动驾驶中前车目标检测的精度和效率。适用于大探测范围内的高精度目标测距。

Description

多传感器融合的测距方法

技术领域

本发明涉及测量、测试领域，具体涉及自动驾驶领域的传感器测量技术，特别涉及一种多传感器融合的测距方法及装置。

背景技术

随着技术进步和科技发展，自动驾驶成为现阶段交通运输工具领域的研究热点。通过感知系统实现自动驾驶是交通发展史上重大的进步和创新。

在自动驾驶领域的前车目标检测中，传感器检测结果的精度和效率将直接影响交通工具的自动驾驶性能，成为影响自动驾驶安全性和可靠性的重要因素，是自动驾驶感知系统的重要研究内容。目前通常采用的前车目标检测方法分为图像测距和雷达测距两种。

申请号为CN201310070139的中国专利文献提出了一种弯道车距测量方法，使用激光雷达探测结果校正相机的俯仰角对相机检测结果进行校正，俯仰角计算公式为：

但是该方法针对每个目标分别校正，不同目标计算的俯仰角存在偏差时无法统一。

申请号为CN201610608255的中国专利文献提出了一种基于测距补偿的车辆测距方法及装置，利用陀螺仪测量车辆的俯仰角对图像测距进行补偿，补偿后距离公式为：

但该方法中使用陀螺仪增加了系统复杂度，且测距过程需要使用消失点计算结果，而消失点的计算受图像的影响，适用范围有限，稳定性低，间接影响了测距结果的准确性。

申请号为CN20191057104的中国专利文献提出了一种基于激光雷达、图像前融合的目标检测方法，使用投影融合后的数据训练神经网络来进行目标检测和定位，但神经网络占用系统资源多，不利于在嵌入式平台上的使用，并且系统的实时性差。

美国专利文献US20190293782A1提出了一种基于图像和雷达目标识别融合的测距方法，采用比例关系表征图像测距结果和雷达测距结果的关系，以此对图像结果进行校正，计算误差大，导致系统可靠性变差。

因此，如何在较大范围内对目标进行实时精确地检测成为亟待解决的技术问题。

发明内容

本发明针对上述技术问题，为了研究探测范围大、测量精度高同时又计算消耗少的自动驾驶感知系统，提高自动驾驶中前车目标检测的精度和效率，提出了新的多传感器融合的测距方法，采用将图像探测和雷达测距相结合的方法，克服了图像探测范围大但是测距结果受俯仰角影响，以及雷达测距结果精确但对遮挡鲁棒性差、探测范围小于图像、受环境影响大的问题，能够对检测结果进行有效校正，检测结果精度高，适于对大范围内目标的实时检测。

为解决上述技术问题，根据本发明的一个方面，提供了一种多传感器融合的测距方法，包括以下步骤：

步骤一、联合标定，获取相机的内参和相机到雷达的转换矩阵，获取雷达目标数据和图像目标数据；

步骤二、将雷达目标框信息投影到图像坐标系下，匹配雷达目标和图像目标；

步骤三、保持车辆在平坦的直线路线上行驶一定距离，得到雷达目标和图像目标匹配成功的组，使用匹配成功组的雷达测距结果和视觉测距结果拟合视觉系统测距误差曲线；

步骤四、使用匹配成功组的雷达测距结果和系统误差校正后视觉测距结果计算俯仰角；

步骤五、对多组俯仰角结果进行加权融合；

步骤六、根据俯仰角校正视觉测距结果。

优选地，相机内参参数表示为内参矩阵K：

其中f_x、f_y表示相机x，y方向焦距，p_x、p_y表示相机中心点相对于左上角偏移。

优选地，相机到雷达的转换矩阵M为：

其中r_ij(i,j＝1,2,3)表示相机坐标系相对于雷达坐标系的旋转矩阵的元素，向量[t₁,t₂,t₃]^T表示相机坐标系相对于雷达坐标系的平移关系，其中T为转置符号。

优选地，所述获取雷达目标数据和图像目标数据包括：使用行驶过程中实时获取经过时间同步的雷达目标数据和图像目标数据，分别包括目标框信息和距离信息；所述雷达目标数据通过雷达获取，所述图像目标数据通过相机获取；所述雷达包括激光雷达或毫米波雷达；所述相机包括RGB相机、红外相机或深度相机。

优选地，所述匹配雷达目标和图像目标，其匹配项包括目标框中心点位置、目标框重叠区域面积比例、横向距离差和纵向距离差，当上述四个匹配项均小于各自的阈值时认为目标匹配成功。

优选地，视觉测距结果和雷达测距结果的误差如公式(1)所示：

其中，k_i为拟合后第i个目标视觉测距结果和雷达测距结果的系统误差；Dⁱ _lidar为第i个目标雷达检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离。

优选地，使用多组目标分别计算视觉测距结果和雷达测距结果的误差k_i。

优选地，采用最小二乘法拟合k_i和D_i的二次关系曲线为：

其中，a、b、c为拟合所得的二次曲线参数。

优选地，采用最小二乘法拟合k_i和D_i，则校正系统误差后的视觉测距输出结果如公式(2)所示：

其中Dⁱ _image为第i个目标图像检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离，k_i表示拟合后第i个目标视觉测距结果和雷达测距结果的系统误差。

优选地，由单个目标计算的车辆的俯仰角θ为：

其中H为相机距地面高度，D_lidar为单个目标的雷达检测距离结果，D_image为单个目标的图像检测距离结果。

优选地，车辆的俯仰角θ为：

θ＝β-α (6)

其中

其中H为相机距地面高度，D_lidar为单个目标的雷达检测距离结果，D_image为单个目标的图像检测距离结果。

优选地，当存在多组匹配成功的目标对时，根据距离远近对结果进行加权融合，作为最终的俯仰角检测结果，当共有n组匹配成功目标对时，第i个目标的雷达检测距离为Dⁱ _lidar，使用第i个目标计算的车辆俯仰角为θ_i，则第i个目标权重为：

其中函数f()为权值函数。

最终俯仰角计算结果为：

通过最终计算得出的俯仰角，对相机测距结果进行校正，针对图像视野范围内的所有目标，每个目标的最终检测距离为：

其中H为相机距离地面的高度，D_image为当前目标的图像检测距离结果，θ_result为最终俯仰角计算结果。

为解决上述技术问题，根据本发明的另一个方面，提供了一种多传感器融合的测距装置，包括：

数据获取装置，进行联合标定，获取相机的内参和相机到雷达的转换矩阵，获取雷达目标数据和图像目标数据；

目标匹配装置，将雷达目标框信息投影到图像坐标系下，匹配雷达目标和图像目标；

匹配及拟合装置，保持车辆在平坦的直线路线上行驶一定距离，得到雷达目标和图像目标匹配成功的组，使用匹配成功组的雷达测距结果和视觉测距拟合视觉系统测距误差曲线；

俯仰角计算装置，使用匹配成功组的雷达测距结果和系统误差校正后视觉测距结果计算俯仰角；

加权融合装置，对多组俯仰角结果进行加权融合；

校正装置，根据俯仰角校正视觉测距结果。

优选地，相机内参参数表示为内参矩阵K：

其中f_x、f_y表示相机x，y方向焦距，p_x、p_y表示相机中心点相对于左上角偏移。

优选地，相机到雷达的转换矩阵M为：

其中r_ij(i,j＝1,2,3)表示相机坐标系相对于雷达坐标系的旋转矩阵的元素，向量[t₁,t₂,t₃]^T表示相机坐标系相对于雷达坐标系的平移关系，其中T为转置符号。

优选地，所述获取雷达目标数据和图像目标数据包括：使用行驶过程中实时获取经过时间同步的雷达目标数据和图像目标数据，分别包括目标框信息和距离信息；所述雷达目标数据通过雷达获取，所述图像目标数据通过相机获取；所述雷达包括激光雷达或毫米波雷达；所述相机包括RGB相机、红外相机或深度相机。

优选地，所述匹配雷达目标和图像目标，其匹配项包括目标框中心点位置、目标框重叠区域面积比例、横向距离差和纵向距离差，当上述四个匹配项均小于各自的阈值时认为目标匹配成功。

优选地，视觉测距结果和雷达测距结果的误差如公式(1)所示：

其中，k_i为拟合后第i个目标视觉测距结果和雷达测距结果的系统误差；Dⁱ _lidar为第i个目标雷达检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离。

优选地，使用多组目标分别计算视觉测距结果和雷达测距结果的误差k_i。

优选地，采用最小二乘法拟合k_i和D_i的二次关系曲线为：

其中，a、b、c为拟合所得的二次曲线参数。

优选地，采用最小二乘法拟合k_i和D_i，则校正系统误差后的视觉测距输出结果如公式(2)所示：

其中Dⁱ _image为第i个目标图像检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离，k_i表示拟合后第i个目标视觉测距结果和雷达测距结果的系统误差。

优选地，由单个目标计算的车辆的俯仰角θ为：

其中H为相机距地面高度，D_lidar为单个目标的雷达检测距离结果，D_image为单个目标的图像检测距离结果。

优选地，车辆的俯仰角θ为：

θ＝β-α (6)

其中

其中H为相机距地面高度，D_lidar为单个目标的雷达检测距离结果，D_image为单个目标的图像检测距离结果。

优选地，当存在多组匹配成功的目标对时，根据距离远近对结果进行加权融合，作为最终的俯仰角检测结果，当共有n组匹配成功目标对时，第i个目标的雷达检测距离为Dⁱ _lidar，使用第i个目标计算的车辆俯仰角为θ_i则第i个目标权重为：

其中函数f()为权值函数。

最终俯仰角计算结果为：

通过最终计算得出的俯仰角，对相机测距结果进行校正，针对图像视野范围内的所有目标，每个目标的最终检测距离为：

其中H为相机距离地面的高度，D_image为当前目标的图像检测距离结果，θ_result为最终俯仰角计算结果。

本发明的有益效果：

1、使用无俯仰状态下的雷达测距结果和视觉测距结果推算出视觉测距的系统误差，并拟合系统误差与视觉测距结果的关系曲线，使用系统误差对图像测距结果进行校正，能够在采用俯仰角校正前去除视觉测距系统的系统误差，提高测距精度；

2、根据雷达测距结果和校正后的视觉测距结果的相互关系，推算出车辆当前的俯仰角，并使用俯仰角对图像测距结果进行修正，提高目标的测距精度；

3、当存在多组匹配结果时，采用距离相关的加权融合方式得出车体的最终俯仰角检测结果，提高俯仰角计算结果的准确性；

4、克服了视觉探测方法虽然探测范围大但是测距结果受俯仰角影响的缺陷，提高了在颠簸路段的测距准确性；

5、克服了雷达测距方法虽然探测结果精确但对遮挡鲁棒性差、探测距离小于视觉、受环境影响大的问题；雷达测距方法只能应用于近距离目标检测而不适于远距离目标检测；而本发明既能应用于近距离目标检测又适于远距离目标检测；

6、实现了探测范围大、测量精度高同时又计算消耗少的自动驾驶感知系统，提高了自动驾驶中对前车目标检测的精度和效率，系统可靠性高。

附图说明

附图示出了本发明的示例性实施方式，并与其说明一起用于解释本发明的原理，其中包括了这些附图以提供对本发明的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。通过结合附图对本发明的实施例进行详细描述，本发明的上述和其它目的、特征、优点将更加清楚。

图1是视觉测距示意图；

图2是拟合曲线图；

图3是几何关系示意图；

图4是权值函数f()的图像；

图5是测距方法流程图。

具体实施方式

下面结合附图和实施方式对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分。

需要说明的是，在不冲突的情况下，本发明中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本发明。

本发明的多传感器融合的测距方法根据几何关系，不需要使用消失点估计结果和陀螺仪进行姿态测量，降低了系统复杂度，首先利用雷达测距和视觉测距结果推算出系统误差，之后使用校正后结果关系推算出车辆的俯仰角大小，并根据距离进行加权融合得出最终俯仰角结果，充分发挥图像和雷达的优点，弥补图像测距不准、激光雷达对遮挡鲁棒性差且探测距离较近或毫米波雷达两侧存在盲区等问题，实现对视野范围内多目标的精准测距。具体地，所述多传感器融合的测距方法包括如下步骤：

步骤一、联合标定，获取相机的内参和相机到雷达的转换矩阵，获取雷达目标数据和图像目标数据；

其中，标定相机内参、外参，获得相机内参参数，表示为内参矩阵K：

其中f_x、f_y表示相机x，y方向焦距，p_x、p_y表示相机中心点相对于左上角偏移，都可通过标定的方法获得。

相机到雷达的转换矩阵M为：

其中r_ij(i,j＝1,2,3)表示相机坐标系相对于雷达坐标系的旋转矩阵的元素，向量[t1,t2,t3]^T表示相机坐标系相对于雷达坐标系的平移关系，其中T为转置符号。

使用行驶过程中实时获取经过时间同步的雷达目标数据和图像目标数据，分别包括目标框信息和距离信息；

步骤二、将雷达目标信息投影到图像坐标系下，匹配雷达目标和图像目标；

其中，使用步骤一中获取的转换矩阵M和内参矩阵K将雷达获取的目标框信息投影到图像坐标系下，并与图像目标框进行匹配。匹配项包括目标框中心点位置、目标框重叠区域面积比例、横向距离差和纵向距离差，当上述四个匹配项均小于各自的阈值时认为目标匹配成功。.

步骤三、保持车辆在平坦的直线路线上行驶一定距离，得到雷达目标和图像目标匹配成功的组，使用匹配成功组的雷达测距结果和视觉测距拟合视觉系统测距误差曲线；

其中，保持车辆在平坦的直线路线上行驶一定距离。图1为视觉测距示意图，图1中，O为相机位置，I为相机的像平面，0为图像的中心点，f为相机焦距，H为相机距地面高度，y1，y2分别为B目标、C目标与地面交点在图像中位置的纵坐标，D1、D2分别为B目标、C目标的距离。

视觉测距结果D_i为：

其中H代表相机距离地面高度，f代表相机焦距，y_i为图像中目标框底边距离图像中心的竖直方向距离。

选择匹配成功的目标对视觉测距的系统误差进行校正，视觉测距结果和雷达测距结果的误差如公式(1)所示：

其中，k_i为拟合后第i个目标视觉测距结果和雷达测距结果的系统误差；Dⁱ _lidar为第i个目标雷达检测距离结果。

使用多组目标分别计算视觉测距结果和雷达测距结果的误差k_i，如图2所示，采用最小二乘法拟合k_i和D_i的二次关系曲线为：

其中，a、b、c为拟合所得的二次曲线参数。

则校正系统误差后的视觉测距输出结果如公式(2)所示：

其中Dⁱ _image为第i个目标图像检测距离结果，k_i为拟合后第i个目标视觉测距结果和雷达测距结果的系统误差；。

步骤四、针对单个目标使用匹配成功组的雷达测距结果和系统误差校正后视觉测距结果计算俯仰角；

图3是几何关系示意图；选择匹配成功的目标，根据图3所示的几何关系可以得出由单个目标计算所得的车辆的俯仰角θ为：

其中H为相机距地面高度，D_lidar为单个目标的雷达检测距离结果，D_image为单个目标的图像检测距离结果。其中

β应大于90度，α和θ均小于90度，则俯仰角计算公式也可以缩写为：

θ＝β-α (6)

图3中，实线代表真实水平线，虚线代表本车存在俯仰时相机视线的平行线。O为相机位置，实线框C为目标实际位置，D为相机的像平面，虚线框B为本车存在俯仰时根据相机观测结果认为目标所在的位置。

步骤五、对多组俯仰角结果进行加权融合；

当存在多组匹配成功的目标对时，根据距离远近对结果进行加权融合，作为最终的俯仰角检测结果，当共有n组匹配成功目标对时，第i个目标的雷达检测距离为Dⁱ _lidar，使用第i个目标对计算的车辆俯仰角为θ_i，则第i个目标权重为：

其中函数f()为权值函数，优选地，一种权值函数是如图4所示的图形函数。

最终俯仰角计算结果为：

步骤六、根据俯仰角校正视觉测距结果；

通过计算得出的俯仰角，可以对相机测距结果进行校正，针对图像视野范围内的n个目标，每个目标的最终检测距离为：

其中H为相机距离地面的高度，D_image为当前目标的图像检测距离结果，θ_result为最终俯仰角计算结果。

测距方法流程图如图5所示。

所述雷达包括激光雷达和毫米波雷达等；所述相机包括能够用于测距的RGB相机、红外相机、深度相机等。

本发明提出的新的多传感器融合的测距方法，采用将图像探测和雷达测距相结合的方法，克服了图像探测范围大但是测距结果受俯仰角影响，以及雷达测距结果精确但对遮挡鲁棒性差、探测范围小于图像、受环境影响大的问题。

本发明同时使用雷达测距和相机测距结果计算俯仰角，存在多个目标时能够采用加权融合的方式得出更为准确的俯仰角。克服了激光雷达探测结果校正相机的俯仰角时，需要针对每个目标分别校正，不同目标计算的俯仰角存在偏差时无法统一的问题。

本发明不需要增加其他传感器和消失点计算等过程，系统复杂度低，测距误差小。在无需增加额外成本的条件下，即可克服采用消失点计算准确性低的缺点。

本发明根据几何原理推断出测距误差和俯仰角，不必训练神经网络来进行目标检测和定位，占用系统资源少，具有良好的实时性，能够在嵌入式平台实时运行。

本发明采用曲线拟合出雷达测距和图像测距结果的误差关系，并且使用误差校正后的测距结果计算俯仰角，与采用比例关系表征图像测距结果和雷达测距结果关系的相比，测距准确性大大提高。

由此可见，本发明的多传感器融合的测距方法能够实时准确地检测和定位前车目标，能够对检测结果进行有效校正，检测结果精度高，适于对大范围内目标的实时检测；无需针对每个目标进行分别校正，检测效率高，提高了检测精度，减少了计算误差，降低了计算复杂度，减少了对系统计算资源的占用，有利于在嵌入式平台上使用，增强了系统的可靠性。

至此，已经结合附图所示的优选实施方法描述了本发明的技术方案，但是，本领域技术人员应当理解，上述实施方式仅仅是为了清楚地说明本发明，而并非是对本发明的范围进行限定，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (18)

1.一种多传感器融合的测距方法，其特征在于，包括以下步骤：

步骤一、联合标定，获取相机的内参和相机到雷达的转换矩阵，获取雷达目标数据和图像目标数据；

步骤二、将雷达目标框信息投影到图像坐标系下，匹配雷达目标和图像目标；

步骤三、保持车辆在平坦的直线路线上行驶一定距离，得到雷达目标和图像目标匹配成功的组，使用匹配成功组的雷达测距结果和视觉测距结果拟合视觉系统测距误差曲线；

步骤四、使用匹配成功组的雷达测距结果和系统误差校正后视觉测距结果计算俯仰角；

步骤五、对多组俯仰角结果进行加权融合；

步骤六、根据俯仰角校正视觉测距结果；

其中，

当存在多组匹配成功的目标对时，根据距离远近对结果进行加权融合，作为最终的俯仰角检测结果，当共有n组匹配成功目标对时，第i个目标雷达检测距离结果为Dⁱ _lidar，使用第i个目标计算的车辆俯仰角为θ_i，则第i个目标权重为：

其中函数f()为权值函数；

其中，weight(i)为第i个目标权重，θ_i为使用第i个目标计算的车辆俯仰角；

通过最终计算得出的俯仰角，对相机测距结果进行校正，针对图像视野范围内的所有目标，每个目标的最终检测距离为：

其中H为相机距离地面的高度，D_image为当前目标的图像检测距离结果，θ_result为最终俯仰角计算结果。

2.根据权利要求1所述的多传感器融合的测距方法，其特征在于，

相机的内参参数表示为内参矩阵K：

其中f_x、f_y表示相机x，y方向焦距，p_x、p_y表示相机中心点相对于左上角偏移。

3.根据权利要求1所述的多传感器融合的测距方法，其特征在于，

相机到雷达的转换矩阵M为：

其中r_ij(i,j＝1,2,3)表示相机坐标系相对于雷达坐标系的旋转矩阵的元素，向量[t₁,t₂,t₃]^T表示相机坐标系相对于雷达坐标系的平移关系，其中T为转置符号。

4.根据权利要求1所述的多传感器融合的测距方法，其特征在于，

所述获取雷达目标数据和图像目标数据包括：使用行驶过程中实时获取经过时间同步的雷达目标数据和图像目标数据，分别包括目标框信息和距离信息；所述雷达目标数据通过雷达获取，所述图像目标数据通过相机获取；所述雷达包括激光雷达或毫米波雷达；所述相机包括RGB相机、红外相机或深度相机。

5.根据权利要求1所述的多传感器融合的测距方法，其特征在于，

所述匹配雷达目标和图像目标，其匹配项包括目标框中心点位置、目标框重叠区域面积比例、横向距离差和纵向距离差，当上述四个匹配项均小于各自的阈值时认为目标匹配成功。

6.根据权利要求1所述的多传感器融合的测距方法，其特征在于，

视觉测距结果和雷达测距结果的误差如公式(1)所示：

其中，k_i为拟合后第i个目标视觉测距结果和雷达测距结果的系统误差；Dⁱ _lidar为第i个目标雷达检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离。

7.根据权利要求6所述的多传感器融合的测距方法，其特征在于，

使用多组目标分别计算视觉测距结果和雷达测距结果的误差k_i。

8.根据权利要求6或7所述的多传感器融合的测距方法，其特征在于，

采用最小二乘法拟合k_i和D_i的二次关系曲线为：

其中，a、b、c为拟合所得的二次曲线参数；

采用最小二乘法拟合k_i和D_i，则校正系统误差后的视觉测距输出结果如公式(2)所示：

其中Dⁱ _image为第i个目标图像检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离，k_i表示拟合后第i个目标视觉测距结果和雷达测距结果的系统误差。

9.根据权利要求1所述的多传感器融合的测距方法，其特征在于，

由单个目标计算的车辆的俯仰角θ为：

其中H为相机距离地面高度，D_lidar为单个目标的雷达检测距离结果，D_image为单个目标的图像检测距离结果。

10.一种多传感器融合的测距装置，其特征在于，包括：

数据获取装置，进行联合标定，获取相机的内参和相机到雷达的转换矩阵，获取雷达目标数据和图像目标数据；

目标匹配装置，将雷达目标框信息投影到图像坐标系下，匹配雷达目标和图像目标；

匹配及拟合装置，保持车辆在平坦的直线路线上行驶一定距离，得到雷达目标和图像目标匹配成功的组，使用匹配成功组的雷达测距结果和视觉测距拟合视觉系统测距误差曲线；

俯仰角计算装置，使用匹配成功组的雷达测距结果和系统误差校正后视觉测距结果计算俯仰角；

加权融合装置，对多组俯仰角结果进行加权融合；

校正装置，根据俯仰角校正视觉测距结果；

其中，

当存在多组匹配成功的目标对时，根据距离远近对结果进行加权融合，作为最终的俯仰角检测结果，当共有n组匹配成功目标对时，第i个目标雷达检测距离结果为Dⁱ _lidar，使用第i个目标计算的车辆俯仰角为θ_i，则第i个目标权重为：

其中函数f()为权值函数；

其中，weight(i)为第i个目标权重，θ_i为使用第i个目标计算的车辆俯仰角；

通过最终计算得出的俯仰角，对相机测距结果进行校正，针对图像视野范围内的所有目标，每个目标的最终检测距离为：

其中H为相机距离地面的高度，D_image为当前目标的图像检测距离结果，θ_result为最终俯仰角计算结果。

11.根据权利要求10所述的多传感器融合的测距装置，其特征在于，

相机内参参数表示为内参矩阵K：

其中f_x、f_y表示相机x，y方向焦距，p_x、p_y表示相机中心点相对于左上角偏移。

12.根据权利要求10所述的多传感器融合的测距装置，其特征在于，

相机到雷达的转换矩阵M为：

其中r_ij(i,j＝1,2,3)表示相机坐标系相对于雷达坐标系的旋转矩阵的元素，向量[t₁,t₂,t₃]^T表示相机坐标系相对于雷达坐标系的平移关系，其中T为转置符号。

13.根据权利要求10所述的多传感器融合的测距装置，其特征在于，

所述获取雷达目标数据和图像目标数据包括：使用行驶过程中实时获取经过时间同步的雷达目标数据和图像目标数据，分别包括目标框信息和距离信息；所述雷达目标数据通过雷达获取，所述图像目标数据通过相机获取；所述雷达包括激光雷达或毫米波雷达；所述相机包括RGB相机、红外相机或深度相机。

14.根据权利要求10所述的多传感器融合的测距装置，其特征在于，

所述匹配雷达目标和图像目标，其匹配项包括目标框中心点位置、目标框重叠区域面积比例、横向距离差和纵向距离差，当上述四个匹配项均小于各自的阈值时认为目标匹配成功。

15.根据权利要求10所述的多传感器融合的测距装置，其特征在于，

视觉测距结果和雷达测距结果的误差如公式(1)所示：

其中，k_i为拟合后第i个目标视觉测距结果和雷达测距结果的系统误差；Dⁱ _lidar为第i个目标雷达检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离。

16.根据权利要求15所述的多传感器融合的测距装置，其特征在于，

使用多组目标分别计算视觉测距结果和雷达测距结果的误差k_i。

17.根据权利要求15或16所述的多传感器融合的测距装置，其特征在于，

采用最小二乘法拟合k_i和D_i的二次关系曲线为：

其中，a、b、c为拟合所得的二次曲线参数；

采用最小二乘法拟合k_i和D_i，则校正系统误差后的视觉测距输出结果如公式(2)所示：

其中Dⁱ _image为第i个目标图像检测距离结果；H代表相机距离地面高度，f代表相机焦距，y_i为图像中第i个目标的目标框底边距离图像中心的竖直方向距离，k_i表示拟合后第i个目标视觉测距结果和雷达测距结果的系统误差。

18.根据权利要求10所述的多传感器融合的测距装置，其特征在于，

由单个目标计算的车辆的俯仰角θ为：

其中H为相机距地面高度，D_lidar为单个目标的雷达检测距离结果，D_image为单个目标的图像检测距离结果。

Images