多传感器融合系列之传感器的标定

1. 关于标定：传感器内参及外参的介绍

（1）标定的目的

​ 标定的目的是如何将多个传感器整合为一套传感器，比如说为了确定sensor A看到的车和sensor B看到的车是否为同一辆车，通常是将所有传感器的成像结果统一到同一坐标系下。

（2）关于传感器的内参

​ 下图为各传感器的内参含义，以及因相应的误差导致的标定简介。由图可知，激光雷达、毫米波雷达和超声波雷达在出厂时就已标定完成。对于相机，需要通过相关的算法来实现标定。

（3）关于传感器的外参

​ 传感器作为独立的个体，它与车体坐标系之间的转换关系称为外参。通过外参可将多个传感器观察到的物体坐标统一到一个坐标系下，由此可判断它们观察到的是否为同一个目标。标定方法可分为如下两种：

• 直接标各传感器与车体坐标系之间的位姿关系

• 先标部分传感器与车体坐标系之间的位姿关系，另一部分通过传感器之间实现标定
  

（4）传感器之间的标定顺序

• 车体在世界坐标系中的位姿由定位确定
• 对车体与固定在其上的IMU之间进行标定
• 对IMU与LiDAR之间进行标定
• 以LiDAR为介质，通过传感器之间的变换进行外参标定，包括：LiDAR与LiDAR，LiDAR与Camera，Lidar与Radar，Camera与Camera等，由此获取各传感器与车体之间的外参
  

（5）总结：如何将所有传感器的成像结果统一到同一坐标系下

• 内参标定：标定相机坐标系到像素坐标系的投影关系
• 外参标定：标定各传感器到车体坐标系之间的变换关系，包括如下：
  • IMU(GNSS) — Car
  • LiDAR — Camera
  • Camera — Camera
  • LiDAR — LiDAR
  • LiDAR — IMU(GNSS)
  • LiDAR — Radar
  • 超声波雷达 — Car
    

2. 非线性优化简介

（1）非线性优化问题

​ 求解$mi{n}_x\frac{1}{2}||f(x)|{|}_x^2$，其中$x\in R^n$，$f$是任意非线性函数，$f(x)\in R^m$

思路：

• 给定某个初始值$x_0$
• 对于第$k$次迭代，寻找一个增量$\Delta x_k$，使得$||f(x_k+\Delta x_k)|{|}_2^2$达到极小值
• 若$\Delta x_k$足够小，则停止
• 否则，令$x_{k+1}=x_k+\Delta x_k$。返回第（2）步

（2）非线性优化——如何确定$\Delta x$

方法1：将$||f(x+\Delta x)|{|}_2^2$在$x$处二阶泰勒展开
$$||f(x+\Delta x)|{|}_2^2\approx ||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H(x)\Delta x$$
​ 其中，$J(x)$是$||f(x)|{|}_2^2$在$x$处的一阶导数（Jacobian矩阵），$H(x)$是$||f(x)|{|}_2^2$在$x$处的二阶导数（Hessian矩阵）

• 仅保留一阶项——最速下降法
  $$\Delta x^{\ast }=-J(x{)}^T$$
  通常这个梯度会乘上一个步长$\lambda$，即$\Delta x^{\ast }=-\lambda J(x{)}^T$，但最速下降法会存在梯度震荡的缺点

• 保留二阶项——牛顿法
  $$\Delta x^{\ast }=argmin(||f(x)|{|}_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^{T}H(x)\Delta x)$$
  右侧对$\Delta x$求导可得：$H(x)\Delta x=-J(x{)}^T$。牛顿法的优点是可以保留二阶特性，对空间平面的拟合状态更好，缺点是需要求取二维的Hessian矩阵，这在高维空间的计算代价非常大

方法2（高斯牛顿法）：对$f(x+\Delta x)$在$x$处一阶泰勒展开
$$f(x+\Delta x)\approx f(x)+J(x)\Delta x$$
​ 其中，$J(x)$是$f(x)$在$x$处的导数，目标函数：
$$\begin{gathered} ||f(x+\Delta x)|{|}_2^2=||f(x)+J(x)\Delta x|{|}_2^2\Rightarrow \Delta x^{\ast }=argmin(||f(x)+J(x)\Delta x|{|}_2^2) \\ ||f(x)+J(x)\Delta x|{|}_2^2=(f(x)+J(x)\Delta x{)}^T(f(x)+J(x)\Delta x) \\ =||f(x)|{|}_2^2+2f(x{)}^{T}J(x)\Delta x+\Delta x^{T}J(x{)}^{T}J(x)\Delta x \end{gathered}$$
​ 对$\Delta x$求导，并令导数为零：
$$2J(x{)}^{T}f(x)+2J(x{)}^{T}J(x)\Delta x=0\Rightarrow J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}f(x)$$
​ 等价于求解线性方程$H(x)\Delta x=g$，其中$H(x)=J(x{)}^{T}J(x)$，$g=-J(x{)}^{T}f(x)$

（3）标定问题中的非线性优化

​ 传感器标定中涉及到外参$T$，由于$T$中存在旋转矩阵$R$，它满足约束：$R\in {\mathbb{R}}^{3\times 3},R{R}^T=I,det(R)=1$，因此标定中的非线性优化是带约束的非线性优化问题，通常的求解方法为：

• 用李代数表达位姿，利用李代数求导方法，可转化为无约束优化问题
• 对于无约束优化问题，沿用非线性优化方法进行求解

3. Camera成像与内参标定

3.1 Camera成像建模的过程

• 自车坐标系 ==> 相机坐标系

​ 三维空间点$(X,Y,Z)$，$R$为旋转矩阵，$t$为平移量
$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$

• 相机坐标系 ==> 图像坐标系（不考虑畸变）
  $$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\frac{1}{z_c}\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$$

• 图像坐标系（含去畸变）

​ 将$(x_c,y_c,z_c)$投影到归一化平面上$(x,y)=(x_c/z_c,y_c/z_c)$，再进行（径向与切向）畸变修正，得到去畸变后的归一化坐标
{ x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r