单目摄像头与毫米波雷达间的位姿配准方法与流程

本发明属于毫米波雷达和摄像头之间的融合技术领域，具体地涉及一种单目摄像头与毫米波雷达间的位姿配准方法。

背景技术：

自动驾驶环境感知系统是一个信息冗余系统，其环境感知传感器包括如激光雷达、双目摄像头、单目摄像头、超声波、毫米波雷达等。而在这个冗余感知系统中，不同传感器之间的信息配准以及融合是自动驾驶的环境感知模块中的关键步骤。

毫米波雷达和摄像头之间的融合是目前的一个研究热点，其主要原理如图1所示。

毫米波雷达数据到摄像头数据的坐标变换，其实就是一个旋转平移矩阵[r，t]。为了表述方便，如图1所示，我们将摄像机坐标系定义为ocxcyczc，毫米波雷达坐标系定义为olxlylzl，摄像机的图像坐标系定义为ocucvc。对于空间任意一点m，其在摄像机坐标系下的坐标(xc,yc,zc)，其在雷达坐标系下的坐标为(xl,yl,zl)，其在摄像机的图像坐标系下的坐标为(u,v)，这样对于空间之间的坐标转换关系如下：

dx代表x轴方向一个像素的宽度，dy代表y轴方向上一个像素的宽度。dx,dy为摄像机的内参数。(u0,v0)称为图像平面的主点，也是摄像机的内参数。摄像机的内参数可以通过张正友的标定方法进行快速精确的标定。为了表述方便我们将内参矩阵定义为p，空间点在摄像机坐标系以及雷达坐标系的向量定义为mc,ml。

摄像机坐标系和毫米波雷达坐标系之间满足：

r代表旋转，t代表平移。r、t与摄像机无关，所以称这两个参数为摄像机的外参数。有以上关系可以得出摄像机图像坐标系和雷达坐标系存在如下关系：

在公式中，仅有r，t两个待求矩阵，表示毫米波雷达和单目摄像头之间的坐标系位姿变换关系。但是在实际使用过程中会存在2个问题：

1、毫米波雷达只能提供二维信息，即我们以车辆前进的方向为z轴，车辆横向方向为x轴，竖直向下方向为y轴建立笛卡尔坐标系，毫米波雷达只能提供xz平面的信息，在xz平面内障碍物的位置。

2、单目摄像头无法提供zc信息，因此也就无法通过(u,v)坐标值获得障碍物的空间坐标。

由于以上问题的存在，目前主流的校正方案仅仅是通过水平仪，重力仪等工具提升毫米波雷达和单目摄像头安装精度，然后通过手动测量来获取两者之间的位姿关系。但是这样的配准精度不是很高。

技术实现要素：

为了解决上述存在的技术问题，本发明提供了一种单目摄像头与毫米波雷达间的位姿配准方法，可以方便的获得毫米波雷达以及单目摄像头之间的位姿关系，从而可以方便快速的将视觉信息以及毫米波雷达的信息进行融合，并且配准精度高。

本发明的技术方案是：

一种单目摄像头与毫米波雷达间的位姿配准方法，包括以下步骤：

s01：额外放置一单目摄像头，与待配准的单目摄像头组成双目视觉系统，采集含有仿体的图像，获取仿体的空间坐标；

s02：通过将物体在毫米波雷达坐标系的坐标进行仿射变换，转变成物体在摄像机坐标系中的坐标；

s03：建立损失函数，所述损失函数抽象成位姿参数的函数，搜索使得损失函数最大的位姿参数。

优选的技术方案中，所述损失函数为：

其中，n表示标定的仿体的个数，xic、zic表示第i个仿体通过双目视觉系统计算出的x、z坐标，x'ic、z'ic表示第i个仿体通过毫米波雷达位姿变换计算出的x、z坐标。

优选的技术方案中，通过安装水平仪，使毫米波雷达和摄像头的z轴相对转角趋近于0，将标定仿体放置在远处。

优选的技术方案中，所述步骤s03中的每一次位姿更新，需要重新计算毫米波雷达经过位姿变换形成的坐标值，然后根据损失函数评价此次位姿参数的优劣，通过特定的搜索算法来搜索使得损失函数最大的位姿参数。

与现有技术相比，本发明的优点是：

1、可以方便的获得毫米波雷达以及单目摄像头之间的位姿关系，从而可以方便快速的将视觉信息以及毫米波雷达的信息进行融合，避免了安装过程中反复的通过装置进行单独调试的复杂过程。可以十分方便地对位姿进行配准，大大提高了配准精度。

2、通过基于分类设计损失函数，具有极强的鲁棒性和精确度。

附图说明

下面结合附图及实施例对本发明作进一步描述：

图1为毫米波雷达和摄像头之间的位姿变换示意图；

图2为本发明单目摄像头与毫米波雷达间的位姿配准方法的流程图；

图3为位姿参数更新流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

实施例：

下面结合附图，对本发明的较佳实施例作进一步说明。

如图2所示，一种单目摄像头与毫米波雷达间的位姿配准方法，包括以下步骤：

一种单目摄像头与毫米波雷达间的位姿配准方法，包括以下步骤：

s01：额外放置一单目摄像头，与待配准的单目摄像头组成双目视觉系统，采集含有仿体的图像，获取仿体的空间坐标；

s02：通过将物体在毫米波雷达坐标系的坐标进行仿射变换，转变成物体在摄像机坐标系中的坐标；

s03：建立损失函数，所述损失函数抽象成位姿参数的函数，搜索使得损失函数最大的位姿参数。

摄像头主要用于周围环境的视觉感知。在本方案中，需要使用仿体(phantom)，仿体是一个竖直的杆，这样，可以十分方便的将图像进行视差图获取，从而计算其空间坐标，同时杆也容易被毫米波雷达检出。摄像头和pc处理终端通过usb协议或者网口协议传输。

首先关于背景技术中出现的问题1，毫米波雷达仅仅能够提供(xl,zl)信息(公式2)，那么经过公式2进行位姿变换之后计算出的在摄像头坐标系下的坐标全都未知。这个问题可以通过在安装毫米波雷达和摄像头的时候解决。将公式2中的旋转矩阵变换成旋转角度表示为：

其中，α,β,γ表示毫米波雷达相对于摄像头坐标系分别沿着x，y，z轴旋转的角度值。从公式4可以看出，由毫米波雷达坐标值(xl,zl)转换为摄像头坐标系(xc,zc)时，以x坐标为例：

xc＝xl*cosβcosγ+cosβsinγ*yl-sinβ*zl(5)

未知的坐标yl参与了计算，致使xc计算出现问题。只要尽量保证γ角度尽量趋近于0，那么未知的yl就不会影响计算。因此，在实际校正的过程中，采取两种手段解决问题1。

1、通过水平仪安装，使毫米波雷达和摄像头的z轴相对转角尽可能小。

2、尽量将标定仿体放置远处，因为yl表示仿体在竖直方向上的坐标，其坐标值在[-1,1]区间(可以十分容易满足)，增大x，z的坐标，可以使得xc计算时，相对误差变小。

通过这两种手段，可以近似精确的计算出(xc,zc)。

关于背景技术中的问题2，采用了增加一个临时摄像头以构建双目系统的办法进行解决。双目系统可以十分精确的通过公式3获取仿体的空间坐标，即(xc,yc,zc)。

通过双目系统获取的(xc,zc)坐标和毫米波雷达获取的(xc,zc)坐标建立损失函数，损失函数可以为多种，最简单的如均方根误差：

其中n表示标定的仿体的个数，xic、zic表示第i个仿体通过双目视觉系统计算出的x、z坐标，x'ic、z’ic表示第i个仿体通过毫米波雷达位姿变换计算出的x、z坐标。

损失函数抽象成位姿参数的函数，每一次位姿更新，需要重新计算毫米波雷达经过位姿变换形成的坐标值，然后带入损失函数计算公式，然后来评价此次位姿参数的优劣，并继续更新。

建立损失函数之后，就可以通过一些搜索算法来寻找r，t的最优值了。

具体的位姿参数更新流程如图3所示：

额外放置一个单目摄像头，与待配准的单目摄像头组成双目视觉系统，获取摄像机图像，以及毫米波雷达坐标点。初始化r，t(位姿参数)。

通过目前的位姿参数，按照设计的损失函数计算方法计算损失函数值。

通过梯度下降法获取新的位姿参数，并计算新的损失函数值。

比较旧的损失函数值以及新的损失函数值，决定是否跳出循环。

通过梯度下降法，十分容易陷入局部最小值，而使用一些全局搜索策略，如模拟退火就可以解决这个问题。

需要说明的是，这个流程图的更新算法是梯度下降法，也可以使用其它的搜索策略，比如随机梯度法，模拟退火法，遗传算法等很多搜索算法。

应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。