激光雷达

激光雷达简介

通过持续不断的发射激光束，激光束遇到障碍物会产生反射，部分反射会被激光雷达（Lidar）传感器再次接收到，通过测量激光束发送和返回传感器的耗时（Round Trip Time）可以获得周围物体距离激光雷达的距离。除了距离（Distance）之外，激光雷达(Lidar)还返回反射值强度（Intensity），不同的障碍物材质反射的激光束的强度（Intensity）不同。

激光雷达的测量模型为距离-方位角-俯仰角模型（Range-Azimuth-Elevation, RAE）。

P是激光雷达的观测点，r是P点距离激光雷达(Lidar)传感器的距离，它通过激光脉冲传播的时间乘以光速除以2获得；

$\alpha$是方位角（Azimuth），$\epsilon$是俯仰角（Elevation），$\alpha$和$\epsilon$是激光束的发射角度。

点P在激光雷达坐标系 $F_s$下的坐标为$(x,y,z)$，其坐标值与测量值之间的关系为： $$ \begin{bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} r \cdot \cos(\alpha) \cos(\epsilon) \\ r \cdot \sin(\alpha) \cos(\epsilon) \\ r \cdot \sin(\epsilon) \end{bmatrix} $$

$$ \begin{bmatrix} r \\ \alpha \\ \epsilon \\ \end{bmatrix} = \begin{bmatrix} \sqrt{x^2 + y^2 + z^2} \\ \tan^{-1}(\frac{y}{x}) \\ \sin^{-1}(\frac{z}{\sqrt{x^2 + y^2 + z^2}}) \end{bmatrix} $$

简化到二维平面，即当P在xy平面时，$z = 0,\epsilon = 0$，此时RAE模型可以简化为距离-方位角模型： $$ \begin{bmatrix} r \\ \alpha \end{bmatrix} = \begin{bmatrix} \sqrt{x^2 + y^2} \\ \tan^{-1}(\frac{y}{x}) \end{bmatrix} $$ 误差来源

激光雷达发送和接收激光束的精确耗时误差。受限于激光雷达配备的计时设备的精度，耗时统计存在精度上限。
激光雷达激光束的朝向误差。受限于激光雷达朝向测量设备的测量精度，朝向测量存在精度上限。
目标材质的反射值特性。比如全黑的材料吸收了光的大部分能量，使得反射量极低；或者像镜子一样的材料会将大部分光反射到其它地方，从而使得激光雷达无法测量到这些物体的位置。
运动畸变。由于激光雷达在跟随自动驾驶车辆前进的同时，对周围环境进行扫描建模，也就是说车辆相对于周围的环境是运动的，导致对环境测量的实际位置与真实位置存在偏差。

数学模型

光束模型（beam model）

光束模型

高斯噪声：高斯噪声使得观测值在真实值周围形成高斯分布（大部分时候是符合这个分布）
物体被遮挡：因为物体被遮挡所以观测值会比真实值小很多，形成指数分布（出现动态物体的时候会出现）
没扫到物体：因为物体没被扫到所以观测值无穷大（可以通过判断是否超出有效距离过滤掉）
均匀噪声：均匀噪声使得观测值在真实值周围形成均匀分布（正常传感器应该不太会出现这种噪声）

似然场模型（likelihood mode）

似然场模型的原理如下图所示，左边是实际环境图

似然场模型

雷达数据解析

大部分算法按照Velodyne的数据协议进行开发，因此以此为标准进行解析。

其他雷达（速腾，镭神）其相关格式和精确时间的计算都不同，但基本框架一致。

Velodyne

雷达采用UDP协议传输数据，激光雷达每一帧的数据包含76个UDP数据包，每一帧实际有效数据为$76*(12 * 16 * 2) = 29184$ ，按10hz16线雷达（水平分辨率为0.2度）单回波模式计算，一帧实际数据为$10 * 16 * 1800 = 28800$。

每个UDP数据包长度固定为1248（有效数据为1206）字节，UDP数据包的组成为：

数据包标识：42字节
数据包：12组数据包（Data Block）
时间戳：4字节
雷达型号参数：2字节

数据包组成为：

Flag（开始标识）：0xFFEE
Azimuth（当前旋转角度）：2字节
距离和强度信息：2字节距离值+1字节强度值，总共2*16（channel 0-15）组，旋转角度指前16组数据的角度，后16组数据对应的旋转角度通过前后两次旋转角度计算平均值获得

回波模式：激光雷达打出一束激光后可以接收几个回波

单回波：可选最强回波还是最后回波

双回波：同时包含最强回波和最后回波

单回波模式

单回波模式时，一次单点激光发射测量一次回波数据，每个数据块包含了2组按照打包顺序（不同雷达不一样）测量的16个通道点云数据，每个数据块只返回一个方位角。

单回波模式

双回波模式

当使用双回波模式时，一次单点激光发射测量两次回波数据。数据包包含6个奇偶数据块对（类似0,1;2,3），每2个数据块包含2组按照打包顺序测量的16个通道两次回波值。

Block(0,1)数据块为第一个2组16个点云数据的两次回波数据，奇数块为一次回波数据，偶数块为二次回波数据，每个奇偶数据块对只返回一个方位角。

双回波模式

精确时间计算

激光发射时间

16线激光发光一次的时间为$2.304\mu s * 16$，然后需要$18.432 \mu s$时间充电，因此一个发射周期（Sequence）时间为$2.304 * 16 + 18.432 = =55.296 \mu s$。

在计算精确时间时，先获取当前帧的时间，然后加上时间偏置： $$ ExactPointTime = Timestamp + TimeOffset $$ 时间通过点的位置计算获得，其中DataPointIndex为channel位置，并不是和垂直角度对应： $$ TimeOffset = (55.296 \mu s * SequenceIndex) +(2.304 \mu s * DataPointIndex) $$

通道数据对应固定的垂直角度