VLP-16激光雷达数据解析及坐标转换

一、数据包格式

VLP-16输出两种类型的UDP以太网包：数据包和位置包。数据包由激光返回值、校准反射率值、方位角值、时间戳和指示传感器型号和回波模式（最强、最后和双）的两个工厂字节组成。数据包长1248字节，在端口2368上发送。
每个VLP-16数据包由一个42字节的报头和一个1206字节的有效数据组成，其中包含12个100字节数据记录块。数据后跟一个四字节的时间戳数据和两个工厂字节，首先传输最低有效字节。

如上图所示，共有12个数据块，每个数据块有2次扫描数据，即共有24个点云。

二、数据块解析

1.1 起始标识符

以0xFF，0xEE作为起始标识符

1.2 角度值

1.2.1 数据块N、N+2、…、N+22给出的角度值

（1）获得角度值：0x33 & 0x71
（2）反转字节：0x71 & 0x33
（3）组合字节：0x7133
（4）转成十进制：28979
（5）除以100
（6）结果：289.79°

1.2.2 数据块N+1、N+3、…、N+23未给出的角度值

方位插值：第一组16次点火（数据块1）和第三组16次激光点火（数据块2）的第一次点火之间的时间约为110.6μs。如果你假设短时间间隔内的旋转速度是恒定的，你可以假设（N+1）组16次激光发射的方位角在第N组16次激光发射报告的方位角和（N+2）组激光发射报告的方位角之间的一半。下面是执行插值的伪代码。代码检查方位角是否从359.99°翻转到0°在点火装置N和N+2之间。
（1）首先，调整翻转从359.99°到0°

If (Azimuth[3] < Azimuth[1])
	Then Azimuth[3]:= Azimuth[3]+360;
Endif

（2）进行插值

Azimuth[2]:=Azimuth[1]+((Azimuth[3]-Azimuth[1])/2);

（3）正确的角度值范围是0°到359.99°

If (Azimuth[2]>360)
	Then Azimuth[2]:= Azimuth[2]-360;
Endif

1.3 距离值

（1）获得距离值：0x89 & 0x59
（2）反转字节：0x59 & 0x89
（3）组合字节：0x5989
（4）转成十进制：22921
（5）乘以2.0mm
（6）结果：45842mm
（7）距离目标：45.842米

1.4 反射率

反射率为0x00

1.5 时间戳

（1）获取时间戳值：0x61,0x67,0xB9,0x5A
（2）反转字节：0x5A,0xB9,0x67,0x61
（3）组合字节：0x5AB96761
（4）转成十进制：1522100065
（5）除以1000000:（获取的十进制为微秒）
（6）结果：一小时后1522.100065秒
（7）如果有需要除以60获得一小时后的分钟数

1.6 工厂字节

0x39表示：双回波模式
0x22表示：VLP-16

三、坐标转换

Vlp-16以球面坐标(r，ω，α)报告坐标。因此，需要进行转换才能转换为XYZ坐标。垂直/仰角(ω)是固定的，由激光器ID给出。数据包中的回波位置指示激光器ID。水平角度/方位角(α)在每隔一个发射序列的开头报告，距离以两个距离字节报告。利用该信息，可以计算每个测量点的X、Y、Z坐标。距离小于一米的点应忽略。

利用上述的角度值（α）、距离值（R）、仰角（ω）即可得到XYZ坐标下的点坐标信息。