学习笔记7--激光雷达标定

本系列博客包括6个专栏，分别为：《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》，笔者不是自动驾驶领域的专家，只是一个在探索自动驾驶路上的小白，此系列丛书尚未阅读完，也是边阅读边总结边思考，欢迎各位小伙伴，各位大牛们在评论区给出建议，帮笔者这个小白挑出错误，谢谢！
此专栏是关于《自动驾驶汽车环境感知》书籍的笔记。

---

---

2.激光雷达标定

2.1 概述

1. 激光雷达内参标定：内部激光发射器坐标系与雷达本身坐标系的转换关系，在出厂前已经标定完成，可以直接使用；

2. 自动驾驶系统需要进行外参标定，即激光雷达自身坐标系与车体坐标系的关系；

3. 激光雷达与车体为刚性连接，两者间的相对姿态和位移固定不变；为建立激光雷达之间及激光雷达与车辆之间的相对坐标关系，需要对激光雷达的安装进行标定，并使激光雷达数据从激光雷达坐标系转换至车体坐标系上；

4. Velodyne VLP-16激光雷达例子：该激光雷达以正上方为z轴，电缆线接口方向为y轴的负方向，通过右手法则确定x轴方向；
   
   

   1. 车体坐标系以车辆后轴中心为坐标原点，垂直地面向上为z轴，朝前为x轴，右手坐标系，确定坐标系y轴方向；
   2. 两个三维空间直角坐标系之间的转换关系可以用旋转矩阵加平移矩阵表示；
   3. 上图，$P$点在$Oxyz$坐标系下的坐标为$P(x,y,z)$，在$O{x}^{\prime }{y}^{\prime }{z}^{\prime }$坐标系下的坐标为$P^{\prime }(x^{\prime },y^{\prime },z^{\prime })$；$P$点和$P^{\prime }$的坐标转换关系：$$\left(\begin{array}{c}x\\ y\\ z\end{array}\right)=R\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right)+T$$
      进一步推导：
      $$\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)=\left(\begin{array}{cccc}\cos \beta \cos \gamma & \cos \alpha \cos \gamma -\cos \gamma \sin \alpha \sin \beta & \sin \alpha \sin \gamma +\cos \alpha \cos \gamma \cos \beta & \Delta x\\ -\cos \beta \sin \gamma & \cos \alpha \cos \gamma +\sin \alpha \sin \beta \sin \gamma & \cos \alpha \sin \beta \sin \gamma & \Delta y\\ -\sin \beta & -\cos \beta \sin \alpha & \cos \alpha \cos \beta & \Delta z\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\\ 1\end{array}\right)$$
   4. 通过实验采集同一个点在两个坐标系下的真实坐标，即同名点，建立一系列方程组，可以求出这16个未知参数；
   5. 自动驾驶汽车上，需要将激光雷达与惯性导航单元(IMU)坐标系进行标定，来建立激光雷达与车体坐标系之间的关系；
   6. 激光雷达与IMU之间的外参标定，除了能够建立激光雷达与车体之间的关系，还可以应用于车辆定位；
   7. 激光雷达与IMU一起为车辆提供精确的姿态估计；IMU可以快速测量车体的运动状态，但随着时间的推移，IMU的定位误差不断累积，导致位置的测量发生漂移；激光雷达通过激光光束精确测量出从传感器到物体的距离和方位角，与IMU的定位信息结合，为车辆提供高精度的定位信息；

2.2 激光雷达与激光雷达之间的外参标定

1. 当存在多个激光雷达时，需要对多个激光雷达的相对位置进行标定和校准；
2. 激光雷达外参标定常用方法：通过不同激光雷达与车体之间的坐标转换关系来间接推导出激光雷达之间的坐标转换关系；
3. 一种应用在移动车辆平台上的自动在线的激光雷达标定方法：设定一条标定路线，让车辆沿着这条线行驶，进行自标定；在标定路线上的垂直杆上贴上反光带作为特征点，以这些特征点建立外参的约束条件，进行标定；
4. 设在标定路线上有$N_t$个路标点，车上有$N_l$个激光雷达；设激光雷达$L_i$扫描到一个特征点$X_{j,L_i}$，根据坐标转换，在时刻$t$，其在空间中的真实坐标为：$$X_{j,W}=R_V^W(t)(R_{L_i}^V{X}_{j,L_i}+T_{L_i}^V)+T_V^W(t)$$
   其中，$[R_V^W(t)|T_V^W(t)]$可通过IMU测量出来，上式的未知量仅有$[R_{L_i}^V|T_{L_i}^V]$；当我们能得到特征点在空间中的位置估计时，可以使用最小二乘法求解参数：
   $$\underset{R_{L_i}^V，T_{L_i}^V，X_{j,W}^{\ast }}{\min }\sum _{i=1}^{N_l}\sum _{j=1}^{N_t}||X_{j,W}-X_{j,W}^{\ast }|{|}^2$$
   式中，$X_{j,W}^{\ast }$为特征点在空间中的位置估计，将上述的最小二乘问题转化为二阶锥优化(SOCP)来求解，求解出激光雷达与车体间的关系，并进一步推导出激光雷达与激光雷达之间的外参；

2.3 激光雷达与摄像机的标定

1. 自动驾驶车辆上，激光雷达与车体为刚性连接，两者间的相对姿态和位移固定不变，激光雷达扫描获得的数据点，在环境坐标系中有唯一的位置坐标与之对应；
2. 摄像机在环境坐标系中也有唯一的位置坐标，激光雷达与摄像机之间存在固定的坐标转换；
3. 激光雷达与摄像机的联合标定，通过提取标定物在单线激光雷达和图像上的对应特征点，完成单线激光雷达坐标、摄像机坐标、图像像素坐标等多个传感器坐标的统一，实现激光雷达与摄像机的空间校准；
4. 当摄像机与激光雷达同时观察点$P$时，点$P$在摄像机自身环境坐标系中的坐标为$P_{vc}(x_{vc},y_{vc},z_{vc})$，在摄像机图像的图像像素坐标系下的投影坐标为$U=(u,v,1{)}^T$$P_l(x_l,y_l,z_l)$；
5. 设激光雷达-摄像机的转换关系为$[R^{\ast }|T^{\ast }]$，则激光点云中的扫描点$(x_l,y_l,z_l)$在图像像素坐标系中的坐标$(u,v)$通过以下公式计算：
   $$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=K\left(\begin{array}{c}{R}^{\ast }\left(\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right)+T^{\ast }\end{array}\right)，其中K=\left(\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right)为摄像机的内参矩阵$$
   对上式进行变换，可得：
   $$R^{\ast }\left(\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right)+T^{\ast }=K^{-1}\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)$$
   激光雷达与摄像机的标定，即求解上式的$[R^{\ast }|T^{\ast }]$；$[R^{\ast }|T^{\ast }]$共有12个参数需要求解，理论上需要至少4组激光雷达的对应点求解，为了提高精度，标定过程一般使对应点的数量n>4，使用最下二乘法进行求解；

注：此篇只是在表层介绍激光雷达的标定理论，常用标定方法和常用标定算法需读者去查阅对应的资料。