基于IMU 和融合滤波的LiDAR 点云数据矫正

魏宝源，姚壮润，张喜杰，梁俊轩，王坚辉，薛秀云

（华南农业大学电子工程学院、人工智能学院，广州510642）

0 引言

目前，源于精准农业的变量喷雾技术迅速发展，该技术是基于通过测量传感器，实时地获取作物的病虫害、形貌和密度等喷雾对象信息，以及喷雾机位置、速度和喷雾压力等机器状态信息，对喷雾对象按需施药[1-3]。果树的特征参数的精准检测是实现变量喷雾的前提条件[4]，对果树特征参数的检测方法常用的有红外线探测、超声波探测和激光探测[1]。其中激光探测相对于红外线探测和超声波探测[5-7]具有速度快，精度高，量程大，抗光、电干扰能力强等优点[8]。激光传感器可以测量靶标外形尺寸等特征信息且可对靶标进行体积测量及三维重构[9-10]。

其中车载激光扫描传感器的应用，在一定程度上获取准确的果树特征参数后，改善了变量喷雾的效果。但是激光扫描传感器对地形的要求较高，实际喷雾场景会造成车辆的颠簸，激光传感器检测目标物特征信息容易出现偏差，进而影响喷雾效果[11-12]。所以对点云数据进行位姿矫正，有助于变量喷雾精准化[13]。其中对于激光雷达位姿的姿态角检测和解算的精确化影响着矫正效果，一般使用MEMS（Micro-Electro-Me⁃chanical System，微机电系统）惯性测量单元检测载体的位姿，其中姿态传感器输出数据的融合滤波算法时获取精确姿态角的关键环节[14]。

本文主要研究的是在地面崎岖的非理想情况下，基于激光雷达和惯性测量辅助测量单元对变量喷雾对象的特征参数进行精确测量，融合了激光检测技术、姿态检测和解算技术，以此达到良好的矫正效果。

1 试验平台与方法

1.1 试验平台

试验平台整体结构由匀速滑台运动导轨、检测采集数据传感器单元两部分组成。该平台如图1 所示。

匀速滑台运动导轨装置主要由铝制水平、垂直滑台同步导轨，型号为台达ASDA-B2D 的伺服电机，伺服电机驱动器电控箱组成；伺服电机配有高精度的编码器，可以通过设置调节控制伺服电机的参数，就可以实现滑台的精准速度的控制，速度控制可以达到0.01m/s 以上，可在水平方向上最大距离为3950mm 和垂直高度940mm～2440mm 进行位移。检测采集数据传感器单元架设在滑台装置的水平导轨上，主要由SICK 公司型号 TIM561 激光雷达传感器、STM32F103ZET6 控制芯片及ATK-MPU6050 六轴姿态传感器组成；激光雷达传感器与计算机通过Ethernet接口使用TCP/IP（Transmission Control Protocol/Internet Protocol，传输控制协议/网际协议）进行通信。所用的SICK TIM561 激光雷达传感器角度分辨率0.33°，具有10m、270°扫描范围，扫描频率为15Hz，抗环境光强度80000lx，供电电压为DC9V-28V。MPU6050 姿态传感器 通过I2C（Inter-Integrated Circuit，内部总线）与STM32 F103ZET6 进行数字通信，STM32 F103ZET6 通过UART 串口与便携式计算机进行通信。上位机软件使用C#在.NET 框架和C++在Qt 框架上联合开发，实现计算机与激光雷达传感器和IMU 姿态传感器进行通信，控制其工作。

图1 试验平台装置图

其中，滑台上搭载着可调固定姿态角平台，如图2所示，该平台分为两部分：支架底座和支架侧座，两者通过螺丝连接，并可松动调整至特定姿态角；可实现俯仰角±10°、横滚角±10°、±20°和偏航角±10°、±20°的姿态角组合变化，考虑到现实情况各姿态角的变化幅度不大，所以该平台基本满足姿态角变化需求。

图2 姿态角平台实物示意图

1.2 试验方案

实验检测对象是正方体，该正方体边长为11.4cm，实验时雷达到物体中心的垂直距离为2m，这与喷雾的实际距离贴近，正方体将放在高度50cm 的方凳上，导轨平台速度可选择控制在0.33m/s，雷达高度为1.1m。正方体如图3 所示。

图3 检测对象实物图

LiDAR 测量系统实验一共分为三步。第一步，实验对象为单个正方体，分别调节偏航角±10°、横滚角±10°、俯仰角±10°进行实验，验证单姿态角的矫正公式；第二步，实验对象为单个正方体，考虑到现实情况多姿态角是融合的且幅度不会太大，调节偏航角10°、横滚角10°、俯仰角10°进行试验，第三步，对象为多个正方体的组合，调节偏航角10°、横滚角10°、俯仰角10°进行试验；以上的所有实验皆重复三次，提高实验数据的可靠性。

实验获得的数据通过MATLAB 离线处理，获得的姿态角数据通过互补滤波和卡尔曼滤波相结合的滤波方式得到比较准确地姿态角信息，LiDAR 获得的点云数据需要先转换成卡尔坐标系保存起来，再与姿态角信息进行匹配矫正，矫正后的点云数据再进行环境点云数据的去除，再进行特征参数的测量。

2 理论基础

2.1 激光雷达检测方法

试验所选用检测目标物的传感器为SICK 公司生产的型号为TIM561 二维激光雷达扫描传感器，扫描最远距离为10m，角度分辨率为0.33°，扫描角度范围为270°，则扫描一帧可获得811 个数据形式为极坐标数值的扫描点数据。在无姿态角变化的情况下，激光雷达平躺架设在滑台导轨上，其架设示意图如图4 所示，90°的扫描盲区朝向地面。

图4 激光雷达架设示意图

LiDAR 扫描所得的极坐标值点云数据，需要转换成笛卡尔坐标系数值形式（x，y，z），以方便后续处理。以LiDAR 中心为原点，水平导轨正向运动为x 轴正向，以右手螺旋定则建立笛卡尔坐标系，如图5 所示。

图5 点云数据极坐标映射到笛卡尔坐标示意图

试验中控制水平导轨特定速度匀速运动，通过Li⁃DAR 的固有扫描频率和扫描点云数据的帧数可知一次扫描目标的时间，则可利用式（1）计算笛卡尔坐标系下的点云数据x 坐标值。

式中f 为LiDAR 的扫描频率，单位为Hz；v 为Li⁃DAR 在水平导轨运动的速度，单位为m/s；n 为扫描点云数据组的帧数序号。

利用式（2）计算y、z 坐标值。

式中ρ为LiDAR 扫描原始点云数据的距离长度，单位为mm；θ为LiDAR 扫描原始点云数据的各数据点对应角度，单位为°。

2.2 姿态角检测及解算方法

LiDAR 位姿辅助测量单元框图如图6 所示，由STM32 主控板控制外围设备MPU6050 姿态传感器组成，使用USB 进行供电。惯性测量单元参照激光雷达坐标系紧密装备在激光雷达上，这样可避免在后续数据处理中带来的坐标系转换统一的冗余计算。

图6 位姿辅助测量单元框图

MPU6050 的陀螺仪在使用过程中，在稳态状态下，随着误差累积，输出姿态角很快偏移实际姿态角，从而导致测量失效；三轴加速度计在对象运动过程中会受到附加加速度干扰；三轴磁强计在检测地磁场的过程中易受到周边环境干扰从而无法输出姿态测量结果，所以为了获取精确的姿态角数据需要结合MEMS 传感器的各自优势，对输出数据进行融合滤波，从而给出较精确的姿态角，且滤波算法在保证输出精度的条件下，还要符合低功耗微处理器实时计算能力的要求。因此，传感器输出数据的滤波融合算法是获取姿态角的关键环节，本文根据黄镇等人[15]提出的一种二阶互补滤波与卡尔曼滤波混合的方法解算姿态，提高位姿检测的准确性和实时性，实现该融合算法的设计框图如图7所示。

图7 互补滤波与卡尔曼滤波混合算法设计框图

2.3 点云数据与姿态角数据的矫正匹配

由于激光雷达传感器和惯性测量单元相互独立，激光雷达检测得到的点云数据和惯性测量单元检测得到的位姿数据并不匹配，本文参考张迪[16]提出利用统一时钟源实现激光扫描系统的时间同步系统设计。实现该方法的框图如图8 所示，使用STM32 MCU 定时器资源产生统一时钟源，并设定激光雷达和惯性测量单元在同一频率下进行扫描，实现了点云数据与姿态角的匹配。

图8 激光雷达和惯性测量单元检测时间同步实现框图

由于位姿变化，将会影响到激光雷达所检测的点云数据，所以必须对产生位姿变化的点云数据进行矫正，本文根据刘慧等人[12]提出的融合翻滚角的极坐标值与三角函数重新匹配法、分段融合俯仰角平均值的检测帧与检测点重新组合法和融合偏航角平均值的深度值系数矫正法，对位姿影响的点云数据进行矫正，对该矫正算法数学建模可由式（3）表征和具体实现。

式中ρβ(i→j)原帧第i 点距离经俯仰角（β）矫正映射到矫正后第j 点数据，α为偏航角，γ为横滚角。

3 试验结果分析与讨论

由1.2 小节所述实验方法进行目标物激光扫描检测，使用MATLAB 按照2.3 小节所示理论方法对点云数据进行姿态矫正，具有单一角度姿态偏移的点云数据和矫正后的点云数据重构图如图9 所示。

具有复合角度姿态偏移的点云数据和矫正后的点云数据重构图如图10 所示。

图9 反映出当偏航角存在时，检测目标物在横向上会被倾斜压缩，当横滚角存在时，检测目标物在纵向上会被倾斜压缩，当俯仰角存在时，检测目标物在横纵向均会被倾斜压缩；图10 反映出当姿态偏移复合存在时，检测目标物在横纵向深度均受到影响；通过矫正算法，目标检测物在重构上得到较大改善。

图9 单一姿态偏移10°目标物矫正前后点云图

图10 复合角度（10°，10°，10°）姿态偏移目标物矫正前后点云图

4 结语

在使用激光雷达获取检测目标物的过程中，由于路面崎岖的非理想状态导致点云数据出现姿态偏移，为了克服姿态角偏差带来的影响，使用姿态角矫正点云数据的方法，融合互补滤波和卡尔曼滤波的方式解算姿态角，提高姿态角的精度，通过矫正算法获取精确的位姿姿态角数据，并重构三维图像。试验证明，使用姿态角矫正后的点云数据重构的三维模型得到了较好的改善。