[发明专利]一种用于多传感器融合的异步在线标定方法

摘要

一种用于多传感器融合的异步在线标定方法，通过标定三个传感器的坐标系之间的相互转换关系，将多个传感器的测量信息统一到相同的坐标系下，传统的标定方法局限于两个传感器之间的标定，并且要求严格的时间同步，标定过程较为复杂；本发明分别计算激光雷达、相机、惯性测量单元(IMU)的运动，然后通过使用线性插值获得相同时间间隔内不同传感器的运动，最后通过对齐三个传感器的旋转序列得到两两之间的外部旋转。本发明能够得到较为精确的激光雷达、相机、惯性测量单元(IMU)之间的外部朝向，为后续的标定提供初值。

主权项

1.一种用于多传感器融合的异步在线标定方法，其特征在于，所述方法包括以下步骤：1)计算相机的旋转设点P为相机坐标系下的一个点，其坐标为[x y z]T，设点p1,p2分别是点P在图Fi,Fj中的投影，Ci,Cj分别是在时刻i和时刻j的相机坐标系，使用针孔相机模型，得到尺度无关的表达式：p1＝KPp2＝K(RP+t)             (1.1)其中K是相机的内参矩阵，R和t是从Ci到Cj的旋转矩阵和平移向量，进行转换，得到对极约束：其中表示向量t的斜对称矩阵，t＝[a b c]^T,定义本质矩阵则式(1.2)写作：在已知内参矩阵的情况下，给定一系列匹配的特征点，就估计出本质矩阵E，对E进行分解，得到旋转矩阵R和平移向量t；2)计算惯性测量单元的旋转IMU具有较高的采样频率，通过预积分集成一定时间段内的IMU测量，以避免高频率的状态估计和大量的位姿状态，惯性积分公式写成下面的形式：其中和分别是以t时刻IMU坐标系为参照坐标系的线性加速度和角速度，是向量的斜对称矩阵，g^w是世界坐标系下的重力加速度，分别为IMU在时刻i在世界坐标系下的位置、速度和朝向；IMU从时刻i到时刻j在i时刻IMU坐标系下的相对变化表示为：3)计算激光雷达的旋转定义雷达完成一次扫描覆盖为sweep，P_m表示在sweep m期间接受的点云集，假设一次sweep期间的角速度和线性加速度为常量，通过插值的方法获得不同时刻的变换，设t为当前时间，t_m为sweep m的开始时间，为雷达在[t_m,t]期间的位姿变换，给定点p_i,p_i∈P_m,t_i是点p_i的时间戳，是雷达在[t_m,t_i]期间的位姿变换，通过线性插值估计：定义Γ_m为从P_m中提取的边缘点和平面的集合，将边缘点和平面点投影到sweep m开始的时候，即t_m，得到点集构造如下的几何关系：其中是点p_i的坐标，p_i∈Γ_m，是点p_i投影到t_m时刻的雷达坐标系下的坐标，在每次sweep结束的时刻，在该sweep期间接收到的点云都会被投影到该sweep结束的时刻，比如在sweep m‑1期间接受到的点云P_m‑1，被投影到t_m时刻的雷达坐标系下，得到点集点集是其中的边缘点和平面点的集合，在集合和中寻找匹配点，得到匹配点之间距离的计算公式：对于Γm中的所有点，构造如下的非线性方程：通过求解最小二乘解，得到的最优解；4)异步测量的对齐基于短时间内匀速运动的假设，求得相同时间段内的不同传感器的旋转；选择相机作为基准传感器，t_i和t_j是两帧连续图片的采集时间，在这段时间内，惯性测量单元有多次采样，t′_i是最接近且晚于或等于t_i的IMU采样时间，t′_j是最接近且晚于或等于t_j的IMU采样时间；首先用步骤2)中的方法对[t′_i，t′_j]间的IMU数据进行预积分，得到这段时间的IMU旋转雷达在期间的旋转由步骤3)中的方法计算得到，是最接近时间[t_i,t_j]的雷达sweep时间，定义分数γ表示相对旋转需要被扩展或者缩小的程度：其中t0和t1是根据传感器测量直接计算得到的相对旋转的起始时间和结束时间，t′0和t′1是期望得到的相对旋转的起始时间和结束时间；定义相对旋转的插值如下：其中Exp(·)是李代数和李群之间的指数映射，Log(·)是李群和李代数之间的对数映射；5)计算传感器之间的外参对齐相同时间的不同传感器的旋转序列可以估计传感器之间的外部朝向，通过步骤1),2)，3),4)得到[t_i,t_j]期间的相机旋转IMU旋转激光雷达的旋转对于任何i和j，i<j，下面的式子成立：其表示从IMU坐标系到相机坐标系的旋转，表示从相机坐标系到雷达坐标系的旋转，表示从雷达到IMU坐标系的旋转；该线性系统的每一行都有相同的形式：为了恢复旋转矩阵使用单位四元数代替上式中的矩阵：符号表示四元数的乘法，定义单位四元数为：其中将不断到达的旋转桟入线性系统，得到超定线性方程组：其中N是旋转序列的大小，α是异常值处理的权重；随着新的测量的增加，这个线性系统会进行重构，因此使用之前计算的外部朝向来估计误差，误差被定义为：其中‖·‖是向量的模，权重定义为：其中s是放大残差影响的因子。对于三个传感器之间的标定，得到下式：通过奇异值分解求解上面的超定线性方程，最小奇异值对应的奇异向量就是最小二乘解；随着新的数据不断到来，标定的外参朝向会逐渐收敛到固定值，当局部时间段τ内的外部旋转的对应的欧拉角的方差分别小于阈值时，认为标定成功，表达式如下：其中分别是四元数对应的偏航、俯仰和滚转角在时间段τ内的方差，η是根据经验设置的阈值，