车载激光雷达标定到组合导航

在无人驾驶领域，经常需要把激光雷达标定到组合导航的坐标系中，这其中就涉及到了两个三维点集之间的旋转变换。标定的大致思路为：设计一块具有特殊几何形状的标定板，标定板上安装有组合导航，因此可以时时刻刻获取组合导航的经纬度坐标，并转化为utm坐标。然后根据标定板中心与组合导航之间的几何位置关系可以计算得到标定板中心的utm坐标。另一方面，在激光雷达的视野下移动标定板，利用一定的算法处理可以从原始点云数据中提取出标定板中心坐标。因此，就得到了两个三维点集，根据这两个三维点集就可以计算出激光雷达到组合导航的旋转矩阵以及平移矩阵。

下图显示了3个对应点（需要求解的最小点）之间的旋转变换。

假设Dataset A为在激光雷达坐标系下测得的标定板中心坐标集，Dataset B为在组合导航坐标系下测得的标定板中心的坐标。相应的点具有相同的颜色，RRR表示旋转，ttt表示平移。我们希望找到将数据集A中的点与数据集B中的点对齐的最佳旋转和平移。这里，“最优”或“最佳”是指最小平方误差。这种变换有时称为欧几里德变换或刚性变换，因为它保留了形状和大小。这与仿射变换不同，仿射变换包括缩放和剪切。

根据上述描述，假设两个数据集没有噪声，则可以得到如下数学模型：
B=R∗A+t(1)B = R * A + t \tag{1} B=R∗A+t(1)
如果两个数据集带有噪声，则需要最小化下列最小二乘误差：
err=∑i=1n∥R∗Ai+t−Bi∥2(2)err = \sum_{i=1}^{n}\left \| R*A^{i}+t-B^{i} \right \|^{2} \tag{2} err=i=1∑n∥∥R∗Ai+t−Bi∥∥2(2)

为了寻找RRR和ttt，通常需要一下三个步骤：
∙\bullet∙ 计算点集合的中心点
∙\bullet∙ 将点集合移动到原点，计算最优旋转矩阵RRR
∙\bullet∙ 计算转移矩阵TTT

计算点集合的中心点
点集合的中心点计算非常简单，即只需要计算所有点每个维度坐标的平均值，可按如下公式进行计算：
centroidA=1N∑i=1NPAi(3)centroid_{A}=\frac{1}{N}\sum_{i=1}^{N} P_{A}^{i} \tag{3} centroidA=N1i=1∑NPAi(3)
centroidB=1N∑i=1NPBi(4)centroid_{B}=\frac{1}{N}\sum_{i=1}^{N} P_{B}^{i} \tag{4} centroidB=N1i=1∑NPBi(4)
其中PAiP_{A}^{i}PAi和PBiP_{B}^{i}PBi是3×13\times 13×1的矢量[xyz]\begin{bmatrix}x\\ y\\ z\end{bmatrix}⎣⎡xyz⎦⎤。

计算最优旋转矩阵
可以使用奇异值分解（SVD）来求解得到最优旋转矩阵，首先将两个数据集重新居中，使两个中心点都位于原点，如下图所示。

通过将两个数据集居中之后，消除了两个数据集之间的平移成分，从而只剩下旋转需要进行处理。接下来需要计算居中之后两个数据集之间的协方差矩阵，然后利用SVD求解得到旋转矩阵：
H=∑i=1N(PAi−centroidA)(PBi−centroidB)T(5)H=\sum_{i=1}^{N}(P_{A}^{i}-centroid_{A})(P_{B}^{i}-centroid_{B})^{T} \tag{5} H=i=1∑N(PAi−centroidA)(PBi−centroidB)T(5)
[U,S,V]=SVD(H)(6)[U,S,V]=SVD(H) \tag{6} [U,S,V]=SVD(H)(6)
R=VUT(7)R=VU^{T} \tag{7} R=VUT(7)
在求解旋转矩阵时，有一种特殊情况需要注意。有时SVD会返回一个“反射”矩阵，这在数值上是正确的，但在现实生活中实际上是无意义的。因此需要通过检查R的行列式（来自上面的SVD）并查看它是否为负（-1）来解决这种特殊情况。如果是，则V的第三列乘以-1。

if determinant(R) < 0[U,S,V] = svd(R)multiply 3rd column of V by -1R = V * transpose(U)
end if

计算平移矩阵
对两个数据集中的每个点运用公式（1），可以得到：
∑i=1N(R×PAi+T)=PBi(8)\sum_{i=1}^{N}(R \times P_{A}^{i} + T) = P_{B}^{i} \tag{8} i=1∑N(R×PAi+T)=PBi(8)
上式等式两边分别除以数据集的个数NNN可以得到：
T=centroidB−R×centroidA(9)T = centroid_{B} - R \times centroid_{A} \tag{9} T=centroidB−R×centroidA(9)

C++代码实现

/*** @brief 对激光雷达进行标定，输出激光雷达相对于东北天的旋转矩阵和平移矩阵* @param pts1  输入--根据组合导航数据计算得到的标定板中心东北天坐标，相对于选定的东北天坐标系原点* @param pts2  输入--根据对激光雷达原始点云数据进行处理提取出来的标定板在激光雷达坐标系下的中心坐标* @param R     输出--旋转矩阵* @param T     输出--平移矩阵*/
void LidarCalib::pose_estimation_3d3d(const vector<Eigen::Vector3d> &pts1, const vector<Eigen::Vector3d> &pts2,Eigen::Matrix3d &R, Eigen::Vector3d &T){// pts1 = R * pts2 + T;Eigen::Vector3d p1(0,0,0), p2(0,0,0); // center of massint N = pts1.size();for (int i = 0; i < N; i++){p1 += pts1[i];p2 += pts2[i];}p1 = p1 / N;p2 = p2 / N;vector<Eigen::Vector3d> q1, q2; // remove the centerfor (int i = 0; i < N; i++){Eigen::Vector3d q1_temp, q2_temp;q1_temp= pts1[i] - p1;q2_temp = pts2[i] - p2;q1.push_back(q1_temp);q2.push_back(q2_temp);}// compute q1*q2^TEigen::Matrix3d W = Eigen::Matrix3d::Zero();for(int i = 0; i < N; i++){W += q2[i] * q1[i].transpose();}// SVD on WEigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::Matrix3d U = svd.matrixU();Eigen::Matrix3d V = svd.matrixV();Eigen::Matrix3d E;E << 1,0,0,0,1,0,0,0,(V * (U.transpose())).determinant();// pts1 = R * pts2 + T;// ENU = R * Lidar + T;R = V * E * (U.transpose());T = p1 - R * p2;
}

车载激光雷达标定到组合导航相关推荐

GNSS+IMU+MM车载高精度组合导航定位系统
前言:近年来,随着定位业务的迅速发展,用户对于车载端定位精度提出了越来越高的要求,由原来的导航级逐渐更替到车道级.特别是在城市峡谷环境下(高楼.高架),用户无法接收到GNSS信号或GNSS信号受干扰, ...
IMU选型、标定误差分析、AHRS组合导航
IMU选型.标定误差分析.AHRS组合导航惯性导航简介 1.引起器件误差的原因 2.确定性误差 3.随机性误差 4.惯性器件精度应用领域惯性技术 1.IMU传感器选型 2.阿仑Allan方差分析 ...
搭建实验室3d slam 移动小车 3.3jackal移动平台组合导航POMS-GI201C、镭神32线激光雷达卫星授时
搭建实验室3d slam 移动小车 3.3jackal移动平台组合导航POMS-GI201C.镭神32线激光雷达卫星授时首先,为什么要授时: 参考博客: 激光雷达中是如何做到和GPS时间同步的? ...
无人驾驶汽车系统入门（二十二）——使用Autoware实践激光雷达与摄像机组合标定
无人驾驶汽车系统入门(二十二)--使用Autoware实践激光雷达与摄像机组合标定单目相机分辨率高,我们可以使用各种深度学习算法完成对目标检测,但是缺乏深度,坐标等信息.激光雷达能够获得目标相当精确 ...
多源传感器GNSS INS 视觉 LiDAR 组合导航与SLAM开源项目总结
多源传感器GNSS INS 视觉 LiDAR 组合导航与SLAM开源项目总结本文基于吴桐wutong 微信公众号文章完善而来. 开源代码总览名称传感器类型组合类型滤波方法备注 RTKLI ...
INS/GNSS组合导航（十二）如何读懂MEMS惯性器件精度?
系列1跟大家解释了惯性器件中几种不同的陀螺零偏指标以及如何正确地对比器件性能.本系列我们以一款典型MEMS IMU模块为例来带着大家过一遍MEMS的常见精度指标,希望能对大家正确理解和选用MEMS惯性 ...
万字综述智能驾驶车载激光雷达关键技术与应用算法
来源 | 智驾最前沿编辑 | 焉知智能汽车随着全球智能驾驶进入产业化与商业化的准备期,车载激光雷达凭借其优异性能已成为不可或缺的环境感知传感器并在硬件技术和应用算法上得到迅猛发展.文章以激光雷达扫 ...
组合导航+多传感器融合算法
1.INS/GPS的组合导航系统可以输出高频率的导航参数信息(位置.速度.姿态),并且在长.短期的导航过程中均能具备较高精度.(输出数据的意义什么,PDR是步长+航向) 2.误差反馈系统 (1)在开环 ...
SINS/DR组合导航(一)
1.1 关于SINS/DR组合导航 SINS/DR组合导航是一种很常见的,也是很传统的一种组合导航方式,这里的DR叫做航位推算,定义是利用姿态.航向和行驶里程信息来推算机器人相对于起始点的相对位置.对 ...

车载激光雷达标定到组合导航

车载激光雷达标定到组合导航

车载激光雷达标定到组合导航相关推荐

最新文章

热门文章