激光雷达和IMU标定

激光雷达简介

激光雷达，即LiDAR（Light Detection and Ranging），是激光探测及测距系统的简称，另外也称Laser Radar或LADAR（Laser Detection and Ranging）。激光雷达可以探测目标位置获取反射强度，有些也可以监测移动速度。激光光束可以准确测量视场中物体轮廓边沿与设备间的相对距离这些轮廓信息组成点云并绘制出3D环境地图，精度可达到厘米级别，从而提高测量精度。激光雷达按照有无机械旋转部分，可分为机械式激光雷达和固态激光雷达，目前普遍应用的为机械式的。固态激光雷达按实现方式不同可分为微机电系统（MEMS）、面阵闪光（Flash）和光学相控阵（OPA），其中OPA为近年来热点，具有无惯性器件、精确稳定、方向可任意控制等优点。

相比于摄像头，激光雷达的最大优势在于使用环境限制较小，因为使用激光主动探测，不依赖于外界光照条件或目标本身的辐射特性，所以不管在白天还是夜晚都能正常识别。激光雷达类似于雷达，但是分辨率更高，因为激光的波长是微米的量级，可以探测非常小的目标。它可以区分真实移动中的行人和人物海报、在三维立体的空间中建模、检测静态物体、精确测距。同时，LiDAR具有抗干扰性强的特点，激光波长短，可发射发散角非常小（μrad量级）的激光束，多路径效应小（不会形成定向发射，与微波或者毫米波产生多路径效应），可探测低空/超低空目标。所以，在自动驾驶汽车中，激光雷达在感知和定位模块中发挥着非常重要的作用。但是激光雷达最大的缺点是容易受到大气条件以及工作环境的烟尘的影响，而且价格昂贵，未来要在量产车上应用，必须大幅降价。

IMU简介

IMU全称Inertial Measurement Unit，即惯性测量单元，它由三个单轴的加速度计和三个单轴陀螺仪组成。加速度计检测物体在载体坐标系系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，对这些信号进行处理之后，便可解出物体的姿态。IMU大多用在需要进行运动控制的设备，如汽车和机器人上，以及需要用姿态进行精密位移推算的场合，如潜艇、飞机、导弹和航天器的惯性导航设备上。IMU提供的是相对的定位信息，它的作用是测量相对于起点物体所运动的路线，不能提供具体位置的信息，因此常常和GNSS一起使用。在一些GPS信号微弱的地方，IMU可让汽车继续获得绝对位置的信息和物体的姿态。

算法的数学原理

四元数

四元数是简单的超复数，是由实数加上三个虚数单位 i、j、k 组成，而且它们有如下的关系： $$i^2=j^2=k^2=-1，i^0=j^0=k^0=1$$每个四元数都是 1、i、j 和 k 的线性组合，即是四元数一般可表示为a + bi+ cj + dk，其中a、b、c 、d是实数。

对于i、j、k本身的几何意义可以理解为一种旋转，其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转，j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转，k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转，-i、-j、-k分别代表i、j、k旋转的反向旋转。

四元数是复数的不可交换延伸，如把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。四元数的优点是：非奇异表达（和例如欧拉角之类的表示相比）、比矩阵更紧凑（更快速）、单位四元数的对可以表示四维空间中的一个转动。

ENU坐标系

地理坐标系，也称当地垂线坐标系，圆点位于运载体所在点，z轴沿当地地理垂线的方向，x,y轴在当地水平面内沿当地经线和纬线的切向方向可选为“东北天（ENU）”、“东北地（END）”、“北西天（NWU）”等右手直角坐标系。ENU坐标系为x指向东，y轴指向北，z轴垂直于当地水平面，沿当地垂线向上。

三维旋转

欧拉角(Euler Angle)与旋转矩阵(Rotation Matrix)

欧拉角旋转如按照ZXY顺归，分别对应着Z-Roll，X-Pitch，Y-Yaw，如下图：

欧拉角构造旋转矩阵可直接把三个Elemental Rotation Matrix乘在一起：

四元数(Quaternion)与旋转矩阵

欧拉旋转是有万向节死锁(Gimbal Lock)的问题的，而四元数(Quaternion)这种数学工具可以避免这个情况。一般来说，我们都会用单位四元数q = w+xi+yj+zk来表示旋转，其中$$||q||=x^2+y^2+z^2+w^2=1$$。那么给定一个单位四元数，可以构造旋转矩阵：

而也能通过运算，从旋转矩阵转换到四元数：

其中，m为R中的元素，例如$m_{32}$为第三行第二列元素。

四元数与欧拉角

给定一个欧拉旋转(X, Y, Z)（即分别绕x轴、y轴和z轴旋转X、Y、Z度），则对应的四元数为q = (x, y, x, w)：

三维空间的位姿描述和齐次变换

首先规定一个坐标系，相对于该坐标系，点的位置可以用3维列向量表示;刚体的方位可用3×3的旋转矩阵来表示。而4×4的齐次变换矩阵则可将刚体位置和姿态（位姿）的描述统一起来，它具有以下优点：

(1) 它可描述刚体的位姿，描述坐标系的相对位姿（描述）。

(2) 它可表示点从一个坐标系的描述转换到另一坐标系的描述（映射）。

(3)它可表示刚体运动前、后位姿描述的变换（算子）。

位置矢量

旋转矩阵
经常用到的旋转变换矩阵是绕x轴、绕y轴或绕z轴按顺序各转角度：




3x3旋转矩阵:

向量a经过一次旋转R和一个平移T后，得到a’:
非齐次表述

齐次表述（M为4x4 变换矩阵）

优化器

标定算法中采用的NLopt是一个非线性优化库，包括了多种优化算法，可进行大量参数的优化，包含全局和局部优化算法。在LiDAR和IMU/GNSS标定算法中，采用了全局优化算法GN_DIRECT_L，和局部优化算法LN_BOBYQA。

NLopt库中GN_DIRECT_L是DIRECT_L从零开始的再实现，是无梯度全局优化算法。DIRECT是全局优化的矩形分割算法（Dividing RECTangles algorithm），DIRECT-L是它“局部偏向”的变体。这些是基于将查找域系统性分割成更小的超矩形的决定性查找算法。DIRECT-L算法“更偏向于局部查找”，所以对于没有很多局部最小值的方程更有效率。DIRECT_L将边界约束再划分成超立方，在搜索过程中给所有维度相同的权重。上述算法对于非约束性问题不适用，也不能处理任意非线性约束。

LN_BOBYQA是无梯度边界约束局部优化算法基于迭代建立的二次最优化不断优化目标函数。它接受不同参数的不相等的初始步长，这当参数之间有非常大尺度差别时很重要。

标定方法

现有的方法利用了来自激光雷达的点云在校准正确时看起来更“清晰”的特性进行激光雷达和IMU的标定，具体方法如下:
1)设置激光雷达与IMU之间的转换矩阵；
2)将点云相对坐标、IMU运动变换与上述转换相结合，将所有激光雷达点融合成一个点云；
3)求出每一点与其最近邻点云之间的距离之和；
这个过程在一个优化过程中重复，不断迭代找到最小化这个距离的转换矩阵。

IMU或者GNSS里程计输入的是类似geometry_msgs::TransformStamped的消息类型，也就是我们关注的是积分后的平移量和旋转量。

最终的标定质量与转换源的质量和观测到的运动范围密切相关。为了确保准确的校准，数据集应该包含大量的旋转和平移。近似规划器的运动(例如，一辆汽车沿着街道行驶)不会在垂直于平面的方向上提供关于系统的任何信息，这将导致优化器在这个方向上给出不正确的估计，比如在Z轴上的平移量就标定不准确。

优化过程包含了粗略角度的全局优化和局部6自由度优化。标定结果包含平移和旋转变换参数。

标定流程图