激光感知（二）：激光雷达标定

1. 激光雷达标定介绍

所谓激光雷达标定，就是将计算激光雷达自身坐标系与我们所需坐标系（如车身坐标系）之间的差值，从而在车辆坐标系下得到激光雷达的检测结果，方便后续的感知计算。

这个误差值具体可分为旋转角度误差与平移误差，其中平移误差可通过激光测距仪等测量工具测得，且对最终输出的结果精度影响较小。重要的是角度误差，具体分为3个方向的角度误差，pitch、roll、yaw，这3个角度不方便通过工具直接测量，且对最终输出的结果精度影响较大，故在这里介绍一种激光雷达角度标定方法，此方法相较于其他方法，对标定环境及工具要求较低，便于实现。

2. 标定算法介绍

我将此方法称为“最小方差法”。

1）标定环境

需将车辆面朝墙面停放，注意墙面以及停放的路面需要尽可能平整，车身与墙面保持垂直关系，如何保持垂直呢？可以使用激光测距仪测出车辆2侧固定位置到墙面的距离，调整车身角度使2侧距离值一致即可。标定环境如此即可，无需其他工具。

2）墙面、地面点云提取

可用RANSAC算法提取墙面点与地面点，由于PCL库中几行代码即可实现，我就不展开讲解了，详情可见其他博客。

3）最小方差标定算法

假定我们将激光雷达标定到了车身，那么地面这个平面上所有点云的高度值一定相等，激光雷达的pitch和roll角（俯仰和横滚）会影响这个高度值，因此我们可以求出地面点云的高度方差，通过角度补偿的方式找出一个pitch和roll角使得地面上的点云的方差最小，接近为0，此时这个pitch和roll就是我们当前激光雷达相对与车身坐标系的误差角。

同理可得：假定我们将激光雷达标定到了车身，那么墙面这个平面上所有点云的X值（纵向距离）一定相等，激光雷达的yaw角（航向）会影响这个距离度值，因此我们可以求出墙面点云的纵向距离方差，通过角度补偿的方式找出一个yaw角使得墙面上点云的方差最小，接近为0，此时这个yaw角就是我们当前激光雷达相对与车身坐标系的误差角。

如此即求出了3个误差角度值，全程可自动计算，输入点云，输出3个误差角，算法确实十分方便快捷。唯一麻烦的可能就是停车摆角度，但是对各位老司机来讲应该不是问题。

3. 附录–变换矩阵

1）only roll:

x = x;
y = cos(roll)*y’ - sin(roll)*z’;
z = sin(roll)*y’ + cos(roll)*z’;

2）only pitch:

x = cos(pitch)*x’ + sin(pitch)*z’
y = y’;
z = -sin(pitch)*x’ + cos(pitch)*z’;

3）only yaw:

x = cos(yaw)*x’ - sin(yaw)*y’;
y = sin(yaw)*x’ + cos(yaw)*y’;
z = z’;

4） all:

x = cos(yaw)*cos(pitch)*x’ + ( cos(yaw)*sin(pitch)*sin(roll) - sin(yaw)*cos(roll) )*y’ + ( cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll) )*z’;

y = sin(yaw)*cos(pitch)*x’ + ( sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll) )*y’ + ( sin(yaw)*sin(pitch)*cos(roll) - cos(yaw)*sin(roll) )*z’;

z = -sin(pitch)*x’ + cos(pitch)*sin(roll)*y’ + cos(pitch)*cos(roll)*z’;

注：
1）以上旋转公式是点旋转公式，而非坐标系旋转公式，点方差最小旋转角度即雷达误差角度
2）遵从右手定则，逆时针为正
3）all公式为xyz顺序外旋，不同的旋转轴顺序以及内外旋方式，有各自不同的公式，注意区分