激光雷达点云运动畸变的矫正方法

技术领域

本发明涉及激光雷达点云运动畸变的矫正方法，属于无人车自动驾驶技术领域。

背景技术

随着人工智能的发展，激光雷达以其分辨率高、测距远、不受光照影响和抗干扰能力强等优点获得了广泛的关注。

在无人车自动驾驶领域，激光雷达可以在未知环境中建立地图信息，为后续定位导航、路径规划等提供良好基础。基于机械旋转扫描的激光雷达在随着无人车运动时，其坐标系不断发生变化，会导致同一帧点云中的点不在同一个坐标系下而产生运动畸变，因此需要对获取的点云数据进行运动畸变矫正。

目前，针对多线激光雷达运动畸变，可利用单一惯性测量单元(IMU)或惯性测量单元与轮速里程计联合作为作为外部传感器，并假设无人车只有航向角变化，推导出激光雷达的运动信息，进而对激光雷达点云进行运动畸变矫正。但是IMU和轮速里程计均需要通过积分获得位移，在长时间工作状态下存在位移累积误差大的缺陷。

发明内容

针对现有激光雷达点云运动畸变的矫正方法中，采用的外部传感器存在位移累积误差大的问题，本发明提供一种激光雷达点云运动畸变的矫正方法。

本发明的一种激光雷达点云运动畸变的矫正方法，包括，

步骤一：采用RTK作为脉冲发生器，同步触发激光雷达和IMU；对采集的激光雷达第一帧点云，以第一帧点云为原点建立全局坐标系；

步骤二：采集非第一帧的激光雷达当前帧点云，以当前帧点云为原点建立局部坐标系；同时将当前帧点云采集时刻对应的IMU坐标系转换到所述局部坐标系下；并同步获得IMU位姿；

步骤三：根据所述局部坐标系，采用插值方法获得当前帧点云中每个数据点对应的IMU位姿，并根据所述IMU位姿矫正激光雷达当前帧点云运动畸变；

步骤四：将所述局部坐标系转换到所述全局坐标系下，再利用RTK位姿校准IMU位姿；然后返回步骤二，直到完成最后一帧激光雷达点云运动畸变的矫正。

根据本发明的激光雷达点云运动畸变的矫正方法，

步骤二中，对于采集的激光雷达点云，在局部坐标系下，第k帧第i点的坐标为

将激光雷达点云表述为矩阵形式为：

根据本发明的激光雷达点云运动畸变的矫正方法，

步骤二中，所述IMU位姿用矩阵形式表达为：

其中

式中s

当i＝0时，

式中

根据本发明的激光雷达点云运动畸变的矫正方法，

步骤三中根据所述IMU位姿矫正激光雷达点云运动畸变包括：

将激光雷达点云第k帧的帧头时间戳记为t

对IMU位姿插值，获得第k帧帧头、帧中和帧尾的位姿依次为

P

式中P

根据点云位姿曲线拟合方程矫正激光雷达当前帧点云运动畸变，得到：

式中

根据本发明的激光雷达点云运动畸变的矫正方法，

步骤四中利用RTK位姿校准IMU位姿包括：

根据局部坐标系下RTK位姿和IMU位姿，判断IMU位姿的时间戳是否小于RTK位姿的时间戳；

若IMU位姿的时间戳小于RTK位姿的时间戳，将RTK位姿作为IMU位姿观测值；若IMU位姿的时间戳大于或等于RTK位姿的时间戳，寻找当前帧IMU位姿时间戳的前后两帧RTK位姿，利用RTK插值得到的位姿作为IMU位姿观测值；

对局部坐标系下IMU位姿和所述IMU位姿观测值进行卡尔曼融合，得到校准后的IMU位姿。

本发明的有益效果：本发明利用IMU位姿插值获得每一帧所有点在局部坐标系下的位姿，再进一步将每一帧点云去畸变后转换到当前帧的局部坐标系下，再将每一帧局部坐标系转换到全局坐标系下，最终完成激光雷达点云运动畸变的矫正。

本发明方法利用了载波相位差分技术RTK，其平面精度和高程精度都能达到厘米级，不存在累计误差，且可作为钟源在硬件上统一各传感器时间。利用RTK实时修正IMU位姿后，可利用IMU计算出无人车的位移、航向角、俯仰角和横滚角，进而对激光雷达点云运动畸变进行矫正，相比只考虑航向角的技术更加接近真实情况。当无人车行驶到遮挡严重的地区时，尽管RTK工作会受到一定影响，但此时经过RTK修正过的IMU依旧能够提供较为准确的位姿，增强了系统鲁棒性和稳定性。

附图说明

图1是本发明所述激光雷达点云运动畸变的矫正方法的整体流程图；

图2是激光雷达点云运动畸变的矫正方法具体实施流程图；

图3是插值IMU位姿矫正激光雷达点云畸变的流程图；

图4是利用RTK位姿校准IMU位姿的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其它实施例，都属于本发明保护的范围。

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。

具体实施方式一、结合图1和图2所示，本发明提供了一种激光雷达点云运动畸变的矫正方法，包括，

步骤一：采用RTK作为脉冲发生器，同步触发激光雷达和IMU；对采集的激光雷达第一帧点云，以第一帧点云为原点建立全局坐标系；RTK在每完成一次触发后，可进行自身时钟的校正，由此完成硬件上的钟源时间同步；

步骤二：采集非第一帧的激光雷达当前帧点云，以当前帧点云为原点建立局部坐标系；同时将当前帧点云采集时刻对应的IMU坐标系转换到所述局部坐标系下；并同步获得IMU位姿；

步骤三：根据所述局部坐标系，采用插值方法获得当前帧点云中每个数据点对应的IMU位姿，并根据所述IMU位姿矫正激光雷达当前帧点云运动畸变；

步骤四：将所述局部坐标系转换到所述全局坐标系下，再利用RTK位姿校准IMU位姿；然后返回步骤二，直到完成最后一帧激光雷达点云运动畸变的矫正。

本实施方式基于惯性测量单元IMU和实时动态载波相位差分技术RTK实现激光雷达点云畸变的矫正。其中RTK具有两个作用，一是实现时间同步，二是由于IMU的位姿会有时间漂移，长时间运行导致数据不准确，RTK可以修正IMU的位姿。这样在RTK被遮挡严重无法传递数据时，由于IMU的位姿已被修正过，所以继续工作的数据仍然可信。由此实现了激光雷达、RTK和IMU三个传感器的互补运行。

本实施方式中，在采集激光雷达点云时，首先判断是否为第一帧点云，依据第一帧点云建立全局坐标系；对于非第一帧点云，建立局部坐标系；在每一次矫正结束后，判断当前帧点云是否为最后一帧点云，若否，则重复进行矫正过程，若是，则结束运行。每完成一帧点云的矫正后，进行一次IMU位姿的修正，直到完成全部矫正任务。

进一步，步骤二中，对于采集的激光雷达点云，在局部坐标系下，第k帧第i点的坐标为

将激光雷达点云表述为矩阵形式为：

再进一步，步骤二中，所述IMU位姿用矩阵形式表达为：

其中

式中s

当i＝0时，

式中

激光雷达点云的每一帧数据都有自己的局部坐标系。

再进一步，结合图3所示，步骤三中根据所述IMU位姿矫正激光雷达点云运动畸变包括：

将激光雷达点云第k帧的帧头时间戳记为t

对IMU位姿插值，获得第k帧帧头、帧中和帧尾的位姿依次为

P

式中P

根据点云位姿曲线拟合方程矫正激光雷达当前帧点云运动畸变，得到：

式中

再进一步，结合图4所示，步骤四中利用RTK位姿校准IMU位姿包括：

根据局部坐标系下RTK位姿和IMU位姿，判断IMU位姿的时间戳是否小于RTK位姿的时间戳；

若IMU位姿的时间戳小于RTK位姿的时间戳，将RTK位姿作为IMU位姿观测值；若IMU位姿的时间戳大于或等于RTK位姿的时间戳，寻找当前帧IMU位姿时间戳的前后两帧RTK位姿，利用RTK插值得到的位姿作为IMU位姿观测值；

对局部坐标系下IMU位姿和所述IMU位姿观测值进行卡尔曼融合，得到校准后的IMU位姿。

本实施方式中，可将原局部坐标系下IMU位姿作为预测值，然后进行IMU位姿预测值和观测值的卡尔曼融合，最后得到校准后的IMU位姿。

本发明中，作为示例，激光雷达的更新频率可以为10HZ，IMU的更新频率可以为300HZ。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其它所述实施例中。