LiDAR点云转换到大地坐标系——简单粗标定

---

Windows 10(64bits) + VMware 16 Pro + Ubuntu 20.04 + noetic
速腾聚创激光雷达：RS-LIDAR-32
华测导航接收机：CGI-610

---

一、LiDAR和IMU位姿标定

为了获取更多的车前信息，将激光雷达向下倾斜一定角度安装。这样就会导致点云的Z坐标不对，前方离平台越远的点的Z轴坐标越大，甚至是个正值，这很不利于进行直通滤波等点云预处理。

1.1安装角度标定

在设计雷达安装结构的时候，设计了两个定位销，而且雷达是360°水平视角，暂不考虑点云yaw的安装误差，仅对pitch和roll进行标定。

下面介绍一种简单的半自动标定方法：

1. 紧固连接激光雷达及其连接结构于平台小车
2. 小车静置于走廊，且雷达前方朝着走廊方向
3. 选择无动态物体和人的时刻，采集几秒钟激光点云
4. 将bag格式的点云数据转换为pcd文件
5. 选择50帧pcd文件并用cloudcompare合并为一个整体文件，帧数越多越精确
6. 查看地面点云集中区域的点云坐标，以及两侧墙壁的点云坐标，使用pcl直通滤波提取出地面点
7. RANSAC拟合出平面表达式，阈值越小越精确
8. 根据拟合所得表达式可以得到平面法向量，求法向量和理想水平地面法向量（0,0,1）的旋转矩阵
9. 使用这个旋转矩阵进行点云变换，查看标定效果

做以下几点解释：

1. 位置变了当然还得重新标定，不如一次性固定好
2. 合并50帧点云后，可以看出来激光雷达的测距误差，即形成一条条的点云带
3. 好多算法都是利用迭代优化的方法，我认为选择50帧点云也是一种形式的优化
4. 设置滤波范围时尽量滤除没有意义的点，减小误差
5. 得到的是点云变换矩阵，而非坐标系变换矩阵

以下图片是滤波提取的走廊地面点云，白色是原始点云，青色是经旋转后的点云，第二张图是主视图视角，旋转的还可以。

1.2安装位置标定

标定完了相对姿态（旋转矩阵），还有激光雷达和组合导航接收机IMU之间的相对位置（平移向量）。
由1.1拟合出的平面方程，可以求出激光雷达中心到地面的垂直距离z，再量取组合导航接收机的安装高度，结合IMU在设备中的位置图纸，即可求出补偿量。
x和y的标定方法：

1. 将小车侧边与墙壁平行，同样用RANSAC拟合平面方程，求得激光雷达Y坐标的补偿量
2. 将小车后部贴着墙壁，且下倾的激光雷达可以扫到前方墙壁，用上述方法求X坐标的补偿量

最终将得到的xyz坐标放到变换矩阵的最后一列，与旋转矩阵组成一个4*4的变换矩阵。

1.3部分代码

直通滤波

pcl::PassThrough<pcl::PointXYZI> pass;
pass.setInputCloud (cloud_nonan);
pass.setFilterFieldName ("x");
pass.setFilterLimits (0.0, 8.0);
pass.setFilterLimitsNegative (false);
pass.filter (*cloud_passthrough);
pcl::io::savePCDFileASCII("../pass_x.pcd",*cloud_passthrough);

RANSAC分割

pcl::SACSegmentation<pcl::PointXYZI> plane_seg;
pcl::PointIndices::Ptr plane_inliers ( new pcl::PointIndices );
pcl::ModelCoefficients::Ptr plane_coefficients ( new pcl::ModelCoefficients );
plane_seg.setOptimizeCoefficients (true);
plane_seg.setModelType ( pcl::SACMODEL_PLANE );
plane_seg.setMethodType ( pcl::SAC_RANSAC );
plane_seg.setDistanceThreshold ( 0.005 );
plane_seg.setInputCloud ( cloud_passthrough );
plane_seg.segment (*plane_inliers, *plane_coefficients);

索引滤波

pcl::ExtractIndices<pcl::PointXYZI> extract;
extract.setInputCloud (cloud_passthrough);
extract.setIndices (plane_inliers);	
extract.setNegative (false);	
extract.filter (*cloud_ransac);
std::cerr << "PointCloud representing the planar component: " 
<< cloud_ransac->points.size() << " data points." << std::endl;

二、点云转换到当地水平坐标系

2.1基本理论

2.1.1坐标系

2.1.1.1激光雷达

激光雷达极坐标（有论文叫做瞬时激光束坐标系）和笛卡尔坐标映射，根据激光束的垂直角度、水平旋转角度和实测距离，将激光雷达极坐标投影到XYZ坐标系上。下图是速腾官方说明书中的说明：

注意：

• 激光雷达的坐标虽然是X右，Y前，Z上符合右手坐标系，
• 但是在ROS包里面，X 轴定义指向图 13 中的 Y 正方向， ROS 下面的 Y轴定义指向图 13 中的 X 负方向，同样符合右手坐标系，只是旋转了90度，即一个三维旋转矩阵。

2.1.1.2导航坐标系/当地水平坐标系/大地坐标系

常用的有北东地和东北天两种。相对应的载体坐标系有前右下和右前上两种。
本人感觉东北天坐标系舒服一点，同时选用右前上载体坐标系，按道理组合导航接收机要放在载体小车的中心，但是无法做到，就会产生“杆臂效应”，也就是配置组合导航接收机的那些值，这些数值的初始化和校准由接收机内算法实现，个人认为组合导航接收机的精度和价格主要就是差在这里的融合补偿算法。
东北天坐标系———— 右前上坐标系
 X轴：指东 ———— X轴：指向载体右侧
 Y轴：指北 ———— Y轴：指向载体前进方向
 Z轴：指天 ———— Z轴：指向上
都符合右手坐标系，拿出右手按照下图比划一下，没毛病。
坐标系xyz和手指对应关系记忆：XYZ——大拇指、食指、中指依次

2.1.2惯性传感器原理

IMU通过感知地球重力加速度和自转角速度，输出自身相对于大地坐标系的三轴角速度和角加速度，以及姿态更新推算出的自身姿态，通常用四元数表达。
具体地，可以看以下链接学习一下：
参考链接: 惯性导航学习笔记——惯性技术基础知识

姿态表示方法有欧拉角、四元数和旋转矩阵。
欧拉角：yaw、pitch和roll，存在万向节死锁，便于直观显示，不利于计算
四元数：超复数，很不直观，计算量和旋转矩阵一样，但是存储时只需要存四个数
有好多介绍这几种姿态表达方式的博客，这里只附上用Eigen实现的四元数和欧拉角的转换代码：
欧拉角转四元数

Eigen::Quaterniond q=
Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())*
Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitY())*
Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitX());

四元数转欧拉角

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

2.2点云转换试验

2.2.1试验场地

• 室外，较为平坦的水平地面，采集数据集1
• 室外，GNSS信号；小车前轮在平地，后轮放在台阶上，采集数据集2

2.2.2试验内容

• 验证标定激光雷达安装姿态的变换矩阵
• 验证组合导航接收机输出的姿态角，使点云转换到当地水平坐标系

2.2.3点云变换效果

数据集1：
白色是原始点云，青色是经旋转后的点云


数据集2：
白色是原始点云，青色是经激光雷达标定矩阵旋转后的点云

青色是经激光雷达标定矩阵旋转后的点云，橙色是IMU位姿变换后的点云