【自动驾驶传感器融合系列】03 传感器之间的标定

Wupke

已于 2023-02-28 14:20:18 修改

阅读量2.8k

点赞数 5

分类专栏：自动驾驶多传感器融合文章标签：自动驾驶传感器融合

于 2021-11-30 16:12:02 首次发布

本文链接：https://blog.csdn.net/Kefenggewu_/article/details/121621777

版权

自动驾驶多传感器融合专栏收录该内容

5 篇文章 29 订阅

订阅专栏

文前白话

传感器标定是自动驾驶的基本需求，也是多传感器融合的基础，需要首先确定各传感器之间的坐标关系以及各传感器与整车坐标系之间的转换关系。

传感器标定的方法

一般传感器安装完，需要对车辆进行整车的标定。标定分为基于标定设备的标定和基于自然场景的标定。

整车标定：可以理解多所有传感器的联合标定，即在一个专门设计过的封闭开阔区域，有很多信息比较明显的参照物（如车辆周围的标定板或者周围轮廓比较明细的障碍物），利用标定方法进行整车所有感知传感器的标定。

基于标定设备的标定方法，如棋盘格, aruco码或April tag，如下图，如果整车采用这种方式，需要很大的整车标定间和摆正器，对于量产车型和主机厂比较有实力搭建。

基于自然场景的标定方法，这种方法利用场景中静止的物体（如树木、电线杆、路灯杆、交通标识牌等）和清晰车道线进行标定，通过算法的设计等，可以有效地提高标定结果的准确度。如下图：

不同情况下的传感器标定

1、单传感器标定

单传感器标定

相机的标定

Radar的标定

LiDAR的标定

IMU的标定

2、传感器之间标定

传感器之间标定

相机与相机

相机与Radar

相机与LiDAR

LiDAR与LiDAR

相机与IMU

传感器之间标定的概述

相机与相机标定：

在自动驾驶车辆中，一般会有多个相机，长焦距相机是用来检测远处场景，短焦距相机用来检测近处的。对于多相机，一般采用长焦相机投影到短焦相机的融合图像进行标定（已经不再是简单的共视，而是目标对齐）。在融合图像的融合区域内，选择较远物体进行对齐判断，如果重合度高，那么精度则高，如果出现错位，那么就存在一定误差。如果误差大于一定值，就需要重新标定。并且，近处物体因为相机畸变，存在水平错位，且距离越近，错位量越大。纵向不受畸变影响。

相机与激光雷达LiDAR

基本思想：相机和激光的标定有很多种，基本思想都是建立相机与激光雷达的约束关系，即精确的激光雷达坐标系，车体坐标系，相机坐标系，图像坐标系，像素坐标系之间的坐标转换关系；

相机和激光的标定是目前经常采用方法是投影法，就是将激光产生的点云投影到图像内，然后寻找标记物（可能是标定设备，也可能是具有明显边缘的静止物体），查看其边缘轮廓对齐情况，一般算法包括语义分割和点云分割等。如果在一定距离内（一般选50-60m）的目标，点云边缘和图像边缘能够重合，则可以证明标定结果的精度很高 ，如果出现错位，那么标定结果存在一定的误差，当误差超出一定范围时，该外参不可用，标定失败。另外，相机和激光联合标定有很多种工具可以直接使用，如Autoware，apollo，kalibr，lidar_camera_calibration等。

关于空间标定算法，最常见的就是手眼标定。即先获取摄像头和激光雷达的初始外参，然后利用初始外参和点云数据（ICP 对激光进行姿态估计），重新计算带尺度的相机运动，并根据这些运动重新计算传感器外部参数，并反复进行相机运动（一般采用特征匹配的方法）和外部参数的交互，直到收敛为止。

如下图，假设 A 和 B 分别为两个固定传感器观测到的位置和方向变化，X 为两个传感器之间未知的相对位置和方向，所以有 AX=XB，求解 X，得到传感器之间的外参。

并根据噪声对传感器的影响，采用卡尔曼滤波对传感器的偏差进行补偿。其大概流程图如下：

手眼标定的经典解法一般是先求解旋转矩阵，然后估计平移向量。假设相机与激光第 i个位置和姿态变化分别为 4*4 的矩阵 Ai、Bi，两个传感器之间外参为 4*4 的矩阵 X，可以建立 ，所以模型如下：

假设 $K_{\alpha }^{i}$ 和 $K_{b}^{i}$ 为旋转矩阵 $R_{\alpha }^{i}$ 和 $R_{b}^{i}$ 的旋转轴，加上无法计算相机帧间平移的绝对尺度，引入比例因子 $S_{i}$ ，所以，上述模型可以写成：

即可以得到传感器的旋转和平移了。以上是基于手眼标定的方法，具体方法就是：将点云中的一个点 p 投影到相机的图像上，如下图：

上图就是 P3P 的原理图，可以采用 P3P 来求解传感器之间的对应关系：

其中 V 是从相机中心点到相应像素的矢量方向。

在相机位置和相机姿态发生转换后，使用相机和激光雷达的运动再次优化外部参数，每次迭代中，R 和 t 都是线性和非线性求解。在非线性优化中，R 与 t 由下列公式进行优化：

但是，因为运动估计和外部参数估计都存在一定的误差，且依赖于被测环境和运动次数（如路面颠簸），很难获得精确的收敛条件，如下图外参误差对相机姿态估计的影响，所以需要考虑特征纹理信息和相机运动信息：

相机与毫米波雷达 Radar

标定方法：直接标定法、激光雷达辅助标定法

1、直接标定法（相机与Radar）

将雷达和视觉信息在确保时间同步的基础上完成标定。为了保证数据的可靠性，一般采用相机的采样速率为基准，相机每采一帧图像，则选取毫米波上一帧缓存的数据，即完成毫米波与相机融合的一帧数据（共同采样数据），从而保证了毫米波雷达数据与相机数据时间上的同步。完成时间同步后，需要完成空间同步。而空间同步一般需要在标定场进行：把一个角架放置在不同的位置，记录毫米波雷达返回的距离和角度，同时记录摄像头测量的角架下边缘的距离和角度。

那么图像平面与雷达反射面之间关系如下：

然后，将图片转换为BEV图（鸟瞰图），然后利用如下关系，固定住平移量，计算旋转关系：

其中，𝑀 = 𝑅 ℎ − 𝜌𝑐𝑜𝑠𝛽𝑐𝑜𝑠𝛼 ，𝑁 = 𝑅𝑐𝑜𝑠𝛽𝑐𝑜𝑠𝛼 + 𝑡y， Q= 𝑅𝜌sin𝛽 + 𝑡𝑥

且，雷达在相机坐标系下，极坐标系中，其中ρ为毫米波雷达与目标之间的距离，ɑ为毫米波雷达的俯仰角，β为毫米波雷达的扫描角度，h为毫米波雷达的安装高度，R为旋转矩阵，tx，ty是毫米波相对相机的平移量，Cx, Cy是相机主点，k为相机内参，f 为相机焦距。通过这个关系，可以将毫米比雷达采集到的空间点转换到图像坐标系中，从而完成了传感器标定。

2、激光雷达辅助标定方法（相机与Radar）

激光雷达辅助标定方法则是为了更好地验证毫米波雷达与相机间外参的标定结果，引入激光雷达作为桥梁，通过同一系统中毫米波雷达与相机的外参和相机与激光雷达的外参，计算得到毫米波雷达与激光雷达的外参，将毫米波雷达数据投影到激光雷达坐标系中与激光点云进行融合，并画出相应的鸟瞰图进行辅助验证。

如下图融合图像中，白色点为激光雷达点云，绿色实心圆为毫米波雷达目标，通过图中毫米波雷达目标是否与激光雷达检测目标是否重合匹配进行判断，如果大部分目标均能对应匹配，则满足精度要求，否则不满足，需重新标定。

LiDAR与LiDAR

当多激光雷达应用时，需要考虑拼接以获得更大范围的3D覆盖面，那么这时就需要外参来将多个激光统一到车辆坐标系下，以便能把各个雷达的点云进行很好的拼接。除了多个激光均单独标定外，还可以利用激光SLAM建图的方式来完成点云图的拼接。

基于数据和轨迹是同步的假设，首先用LiDAR0进行移动SLAM建图，获得子地图M。然后将LiDAR1的点云基于同步的时间戳与运动轨迹、与粗图估计初值旋转平移变换到对应子地图M附近，通过几何结构一致性约束和近邻匹配算法不断迭代，减少匹配误差，直到算法收敛并满足标定矩阵刚性不变特性（六条平行线），最后用一致性算法计算出最终标定矩阵。在移动采集数据期间，需保证基准激光雷达扫描的区域（地图M）能被其余激光雷达所探测，尽量选用室内地下车库等特征丰富的环境，采集数据时避免四周有移动物体，运动时尽量缓慢，特别是转弯处，以最小化运动畸变。（低速运动的情况下）激光雷达的安装不局限于重叠FOV，可任意安装，只要能保证获取外参初值即可。

相机与IMU

相机与IMU标定其原理是利用视觉建图与IMU轨迹重合来完成标定的。

补充：传感器标定的基础知识

（一）外积

外积就是两个向量的向量积。外积公式如下：

在传感器标定上，可以使用外积进行坐标系转换。如下图：

（二）欧式变换

在自动驾驶系统中，往往需要有一个统一的坐标系，通常称为车体坐标系（有些也叫世界坐标系），如下图中的 x_w,y_w,z_w，xw ,yw ,zw 定义的坐标系；同时，各传感器也都有各自的坐标系，假设相机坐标系，如下图中 x_c,y_c,z_c，xc ,yc ,zc 定义的坐标系。那么在相机视野中的某个向量坐标为 p_c，pc ，在世界坐标系下坐标则为 p_w，pw。相机运动时，两个坐标之间的转换关系由变换矩阵 T 来表示。

假设相机为刚性运动，那么同一个向量在各个坐标系下的长度和夹角不会发生变化，这种变换就可以称为欧式变换（由旋转+平移两部分组成）。

（三）旋转矩阵与旋转向量

欧拉变换是由旋转矩阵和平移矩阵组成。假设单位正交基 (e_1,e_2,e_3)(e1 ,e2 ,e3 ) 经过一次旋转变成了 (e_1',e_2',e_3')(e1′,e2′ ,e3′ ) ，对于同一个向量 a, 它在两个坐标系下的的坐标分别为 [a_1,a_2,a_3][a1,a2 ,a3 ] 和 [a_1',a_2',a_3'][a1′ ,a2′ ,a3′ ] ，则有：

为了描述两个坐标之间的关系，对等式左右同乘 [e_1^T,e_2^T,e_3^T]^T[e1T ,e2T,e3T ]T，左式系数即变为单位矩阵，右式系数则变成由两组标准正交基的内积组成的矩阵：

其中，R 就是旋转矩阵。且，R 有一个特点，为正交矩阵（转置=逆），即：

那么欧拉变换就可以表示为：

由于 R 是正交矩阵，这使得优化旋转矩阵会比较困难。结合最开始提到的外积可以表示向量的旋转，那么可以选择方向与旋转轴相同、长度等于旋转角的向量，来描述旋转，称为旋转向量。易知旋转向量用三个量来表示三个自由度的旋转。坐标系的旋转可以由一个旋转轴和一个旋转角来表示，如下式：

其中 n 表示旋转轴， $\theta$ 表示旋转角度。

那么，也可以得出旋转矩阵到旋转向量的转换公式：

（四）变换矩阵和齐次坐标

欧式变换 涉及非线性关系，当进行两次变换时，则有：

变换一下，使用 a 来描述 c ，则有：

为了简化多次变换后的复杂度，这里简化形式，引入齐次坐标和变换矩阵，(将多次转换的过程包含在形式中，只表现为转换前后的结果形式)则有：

给三维向量加一维变成四维向量，称为齐次坐标；

把旋转和平移写在同一个矩阵里，称为变换矩阵。

齐次坐标：

通过添加最后一维，用四个数表示一个三维向量，这样就多了一个自由度，多了一个自由度就可以写出线性的形式。
齐次坐标的每一个分量同时乘一个非零常数仍然表示同一个点，所以一个点的齐次坐标不唯一。
用齐次坐标运算得到的也是齐次坐标，将齐次坐标转换为非齐次坐标的方法为把所有坐标除以最后一项，强制最后一项为 1，然后忽略最后一项即可得到欧氏空间中的非齐次坐标。

变换矩阵：

变换矩阵左上角为旋转矩阵，右上角为平移向量，左下角为 00 向量，右下角为 1.
变换矩阵的逆也表示一个反向的变换

通过引入齐次坐标和旋转矩阵，可以大大简化欧式变换的计算：

（五）欧拉角

除了旋转矩阵和旋转向量，还可以用欧拉角来直观的表式旋转。欧拉角使用三个分离的转角来表述旋转，分别为：

1.   绕 Z 轴旋转的偏航角 yaw；
2.   绕 Y 轴旋转的俯仰角 pitch；
3.   绕 X 轴旋转的滚转角 roll；

这样就可以通过 [r,p,y]^T[r,p,y]T 这样一个三维向量来描述任意旋转。但是使用欧拉角会遇到万向锁问题：

当俯仰角 pitch 为±90° 时，第三次旋转与第一次旋转将会使用同一个轴，这样会使系统丢失一个自由度，称为奇异性问题。所以欧拉角不适用于插值和迭代，也很少用于 SLAM 程序，往往只用于人机交互中，方便直观理解。

（六）四元数

四元数是一种扩展复数，在表示旋转时具有紧凑性，主要用来解决带有奇异性的三维向量的描述。如上面提到的 pitch 为 90°时的状况。一个四元数 q 包含一个实部和三个虚部，可以表示为：q=q_0+q_1i+q_2j+q_3kq=q0 +q1 i+q2 j+q3 k，其中 i,j,k 为三个虚部，且满足以下关系式：