组合导航中的传感器标定问题

参考文献：《车载激光扫描仪外参数标定方法研究》 康永伟，钟若飞，吴 俣
https://kns.cnki.net/kcms/detail/detail.aspx?filename=HWYJ2008S3060&dbcode=CJFQ&dbname=CJFD2008&v=

车载多传感器集成系统中激光扫描仪与 GPS、 IMU （惯性测量单元）联合使用， GPS 得到的是 GPS 天线中心位置在 WGS-84 高斯克吕格三度带投影下的坐标；IMU 得到的是 IMU 本身的姿态信息。
要想得到扫描点在高斯投影下的坐标，就必须将 GPS 中心位置和IMU 姿态实时传递给激光扫描仪，这样才能把每一时刻扫描仪平面极坐标系下的坐标点转换到 WGS-84 高斯克吕格三度带投影坐标系下的三维坐标。

也就是准确标定出激光扫描仪的外参数，即三个平移参数 （反应 GPS 天线中心与激光扫描仪中心位置关系） 和三个旋转参数 （反应 IMU 姿态与激光扫描仪的姿态关系）以及一个尺度变换参数。

难点：激光扫描仪的扫描中心是不可见的，其扫描光束也是不可见的，这样就很难确定一条扫描线是否扫到了用来联合解算的控制点上。

激光扫描仪工作时得到的是极坐标系（在扫描面内以激光扫描仪中心为极点，以扫描线为极轴）。
像方坐标系指激光扫描仪的坐标系， 它以激光扫描仪中心为坐标原点， 以激光扫描仪最中间的扫描线方向为 X 轴，垂直激光扫描仪扫描平面方向为 Y 轴， Z 轴垂直于 XOY 面，构成右手坐标系。
物方坐标系一般指当地水平坐标系， 主要看标定的用途和所处的试验场。 全站仪测得这些控制点在物方坐标系下的坐标。

得到物方坐标系和像方坐标系后，进行外参的求解。相当于是一个旋转+平移的过程，k为尺度：
$${\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]}_{物}=\left[\begin{array}{c}\Delta X\\ \Delta Y\\ \Delta Z\end{array}\right]+k\ast R\ast {\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]}_{像}$$

误差方程：
$$V=B\ast dX-l$$
其中：B为系数矩阵；
$dX=\left[\begin{array}{ccccccc}d\Delta X& d\Delta Y& d\Delta Z& dk& d\phi & d\psi & d\theta \end{array}\right]$;
$V=\left[\begin{array}{c}{V}_x\\ V_y\\ V_z\end{array}\right],l=\left[\begin{array}{c}{l}_x\\ l_y\\ l_z\end{array}\right]$

当拥有3个以上的基准点时，可以使用最小二乘法求出7个外参数，但是太过于依赖初值。参考文献中提出改进的高斯-牛顿法，将前面利用最小二乘迭代得到的$d{X}^k$，在迭代过程中加入适当的搜索步长${\lambda }^k$:
$$X^{k+1}=X^k+{\lambda }^k\ast d{X}^k$$

其中关于${\lambda }^k$的计算如下：
$${\lambda }^k=\frac{1}{2}+\frac{1}{4}\frac{[R(X^k)-R(X^k+d{X}^k)]}{[R(X^k)-2\ast R(X^k+\frac{1}{2}d{X}^k)+R(X^k+d{X}^k)]}$$

在上式中，$R(X^k)$为第k次迭代计算的目标函数，有：
$$R(X^k)=f^T(X^k)\ast f(X^k)-2\ast f^T(X^k)\ast l(X^k)$$

式中：
$$f(X^k)=X_{物}-(\Delta X^k+k^k\ast R^k\ast X_{像})$$
$l(X^k)$为第k次迭代计算公式中的$l$向量。

 
 
 

参考文献《一种基于点云匹配的激光雷达/IMU 联合标定方法》
http://www.cnki.com.cn/Article/CJFDTotal-DZJY201912025.html

首先通过区域分割、地面滤除和标志点提取的预处理方法来提取标志点 ；然后借鉴 3D点云匹配的方法，将坐标系联合标定转化为点云匹配问题 ，使用迭代最近邻点(Iterative Closest Point，ICP)算法求得两坐标系的坐标转换矩阵。

方法：
1：程金龙采用三面靶标的激光雷达外参数标定的方法，使用随机采样一致性算法完成了平面分割和同名向量的提取 ，最后解出标定参数；
2：韩正勇提出了一 种可以在采样帧数比较少的情况下获得较高精度的参数矩阵的方法，该方法采用棋盘面对应性的性质，将坐标系标定问题转换为三维空间中旋转和缩放矩阵的求解问题；
3：韩栋斌提出了一种在非理想参数初值的条件下依然可以获得较好标定结果的方法，该方法采用多对点云同时匹配迭代生成外参数来进行参数解算。

参考文献提出一种基于点云匹配思想的车载激光雷达/IMU 联合标定的方法， 并与基于最小二乘法的标定结果进行对比。

激光雷达与 IMU 之间存在安装误差角和位置误差，因此两个传感器测量得到的同一组标志点的三维坐标不同 ，可以通过对应坐标点的关系来计算得到坐标系之间的转换矩阵，完成激光雷达/IMU 坐标系的联合标定。标 志 点
在两个坐标系之间的坐标分别为$(x_1,y_1,z_1)、(x_2,y_2,z_2)$，假设两者之间的变换矩阵$T_{3D}$：
$$(x_1,y_1,z_1{)}^T=T_{3D}^T(x_2,y_2,z_2{)}^T$$
$T_{3D}$可以表示如下；
$$T_{3D}=\left[\begin{array}{cccc}{t}_{11}& t_{12}& t_{13}& t_{14}\\ t_{21}& t_{22}& t_{23}& t_{24}\\ t_{31}& t_{32}& t_{33}& t_{34}\\ t_{41}& t_{42}& t_{43}& t_{44}\end{array}\right]=\left[\begin{array}{cc}R& 0\\ T& 1\end{array}\right]$$

其中R对应的是比例、旋转、错切等几何变换，$T=[t_{41},t_{42},t_{43}]$对应平移变换，$[t_{14},t_{24},t_{34}]$对应投影变换，$t_{44}$反映的是整体比例的变换。由于是刚体变换，则$[t_{14},t_{24},t_{34}{]}^T=[0,0,0{]}^T,[t_{44}=1]$

标定步骤如下：
（1）采集地理坐标系标志点的原属数据和激光雷达坐标系下的点云数据。
（2）为 IMU 和差分全球导航卫星系统的原点基本重合，可使用IMU 传感 器测量得到的姿态角计算 IMU 与地理坐标系
之间的转换矩阵，求解 IMU 坐标系下标志点的坐标数据。
（3）对激光雷达测量的原始点云数据进行预处理，找到标志点对应的点云坐标。
（4）对IMU和雷达坐标系下的标志点数据进行数据拟合，求得坐标系转换矩阵。
（5）将步骤(2)得到的 IMU 坐标系下的标志点坐标经过步骤(4)中求解得到的转换矩阵计算，转移到雷达坐标系中，与步骤(3)中得到的测量数据进行对比，进行误差分析。