本发明涉及激光数据恢复重建领域，尤其涉及一种基于惯性测量单元的旋转式激光雷达数据校正方法及系统。
背景技术
随着近年来无人车的发展，多线激光雷达的应用需求越来越大。各大厂商都在加紧研制多线激光雷达。其中旋转式多线激光雷达具有重要的作用，因为其具有百米以上的探测距离，并且具备360度环视功能，且具有正负15度的视野，能较好地用于机器人定位建图。
旋转式激光雷达的工作方式是一个旋转的电机上装有多个激光发射和接收装置。电机旋转，带动激光收发器工作。在数据传输上，激光雷达的传感器每隔一小段时间向外发送一个激光测距的数据包，上位机接收数据包，当数据包累计角度超过360度后将所有激光数据点映射到激光雷达中心坐标系下，打包成帧向外发布。但由于激光雷达的这种旋转工作方式，会导致360度数据不是在同一时刻采集到的，而是需要一段采集时间，这会导致一个严重的问题，传感器在运动过程中采集到的数据不能直接映射到激光雷达中心坐标系下。
未校正映射方式：
激光数据点未校正映射方式如图1所示，其中，r为实测距离，ω为激光的垂直角度，α为激光的水平旋转角度，pi为旋转式激光雷达计算出来的未校正的三维空间点，x、y、z为极坐标投影到x、y、z轴上的坐标。
但由于旋转式激光雷达存在水平运动和旋转运动，需要对采集到的数据进行校正，校正后映射方式：pi＝f(ri,ωi,αi,ti)＝rotationi*f(ri,ωi,αi)+transposei。其中，rotationi为旋转变量，transposei为平移变量。因此，当旋转式激光雷达存在运动时，需要对旋转式激光雷达数据进行校正。
技术实现要素：
本发明提供了一种基于惯性测量单元的旋转式激光雷达数据校正方法及系统，实现了当旋转式激光雷达存在旋转运动时，对旋转式激光雷达数据进行校正。本发明采用以下技术方案。
一种基于惯性测量单元的旋转式激光雷达数据校正方法，所述方法包括以下步骤：
步骤1、通过刚性连接，将惯性测量单元与旋转式激光雷达固定在同一刚体上，所述惯性测量单元坐标系与激光雷达坐标系朝向保持一致；
步骤2、将所述惯性测量单元连接至硬件同步处理板和上位机，将所述旋转式激光雷达连接至硬件同步处理板和上位机；
步骤3、启动硬件同步触发板；硬件同步触发板同时向惯性测量单元与旋转式激光雷达发送pps脉冲电平信号；在硬件同步触发板发送pps脉冲信号后，向激光雷达发送基于gprmc标准格式的时间数据；时间数据会随着硬件同步触发板的主控模块的计时器的累加而改变，输出的时间范围为0-3600秒，超过3600秒后重新从0开始发送；
步骤4、上位机采集并缓存激光雷达数据和惯性测量单元数据；上位机的处理计算平台将采集753.58hz的激光雷达数据包，和400hz的惯性测量单元数据包并进行缓存，所述惯性测量单元数据包中携带有同步标志位；激光雷达缓存队列长度为76，惯性测量单元缓存队列长度为399；每当激光雷达数据的缓存队列到达76的长度就进行一次矫正处理；
步骤5、时间校正；检测76个激光雷达数据包对应的pps脉冲电平信号否存在跳变；如果不存在则不处理，如果存在跳变则在惯性测量单元数据包中查找同步标志位所在数据帧；查找到惯性测量单元同步帧后将同步帧的时间根据激光雷达数据包中的时间进行更新，并对同步帧之前和之后的数据进行更新；
步骤6、数据校正；解析出旋转式激光雷达激光距离测量的时间戳laseri.timestamp；在惯性测量单元缓存队列查找离laseri.timestamp最近的两帧数据的时间戳，第一时间戳imuj.timestamp和第二时间戳imuj+1.timestamp，其中第一时间戳和第二时间戳满足：
imuj.timestamp≤laseri.timestamp≤imuj+1.timestamp，当上式不满足时可选择下式
imuj.timestamp≤imuj+1.timestamp≤laseri.timestamp；
求解旋转变量imui.quaternion，使用四元数进行插值：
最后求解校正后的激光雷达数据：
laseri.xyz_correction＝imui.quaternion*laseri.xyz；
其中，laseri.timestamp，i∈[0,30000)是旋转式激光雷达激光距离测量的时间戳；slerp为四元差值函数；imui.quaternion，i∈[0,30000)是旋转式激光雷达激光距离测量对应的旋转变量；imuj.quaternion，j∈[0,200)，是惯性测量单元采集的四元数；imuj.timestamp，j∈[0,200)是惯性测量单元采集数据的时间戳；laseri.xyz是旋转式激光雷达计算出来的未校正的三维空间点；laseri.xyz_correction是校正后的三维空间点。
优选的，所述旋转式激光雷达与所述惯性测量单元朝向一致。
优选的，所述pps脉冲电平信号为在秒的整数倍发生跳变的信号。
优选的，所述惯性测量单元数据包中携带有同步标志位，所述惯性测量单元采集到所述pps脉冲电平信号的跳变时，将当前惯性测量单元数据包中的标志位置1，否则置0。
一种基于惯性测量单元的旋转式激光雷达数据校正系统，所述系统包括惯性测量单元、旋转式激光雷达、硬件同步处理板、gps模块和上位机；所述惯性测量单元与旋转式激光雷达固定在同一刚体上，惯性测量单元坐标系与激光雷达坐标系保持一致；将惯性测量单元连接至硬件同步处理板和上位机，将旋转式激光雷达连接至硬件同步处理板和上位机。
优选的，所述硬件同步处理板包括电源模块、备用电池和主控模块；所述电源模块为主控模块、gps模块、惯性测量单元和旋转式激光雷达提供工作电压。
优选的，所述旋转式激光雷达通过串口uart和ttl同步电平触发口与所述主控模块连接。
优选的，所述惯性测量单元通过ttl同步电平触发口与所述主控模块连接。
优选的，所述gps模块通过串口uart或ttl同步电平触发口与所述主控模块连接。
本发明的有益效果是：将惯性测量单元与旋转式激光雷达固定在同一刚体上，使得两者的坐标系朝向保持一致，通过惯性测量单元采集的数据作为旋转变量的观测值，对旋转式激光雷达采集的数据进行校正，实现了旋转式激光雷达存在旋转运动时数据的校正，保证激光雷达在剧烈运动的过程中使用时，依旧能输出准确无误的三维环境信息。
附图说明
图1是本发明中未校正映射方式示意图。
图2是本发明中惯性测量单元与旋转式激光雷达连接示意图。
图3是本发明中的校正系统结构示意图。
图4是本发明中pps脉冲电平信号和gprmc信号时序图。
图5是本发明中硬件同步处理板结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
一种基于惯性测量单元的旋转式激光雷达数据校正方法，包括以下步骤：
步骤1、通过刚性连接，将惯性测量单元(imu)与旋转式激光雷达固定在同一刚体上，惯性测量单元坐标系与激光雷达坐标系朝向保持一致，两者坐标系误差小于1°。如图2所示，上方为旋转式激光雷达，下方为imu。这种刚性连接，使得两者同时做相同的旋转运动，可以用imu的旋转来表征旋转式激光雷达的旋转。
步骤2、将所述惯性测量单元连接至硬件同步处理板和上位机，将旋转式激光雷达连接至硬件同步处理板和上位机。上位机可以是电脑。如图3所示，将激光雷达的串口uart与ttl同步电平触发口连接至硬件同步处理板，将imu的ttl同步电平触发口连接至硬件同步处理板。将激光雷达与电脑通过网线连接，将imu与电脑通过串口uart线连接。
步骤3、启动硬件同步触发板；硬件同步触发板同时向惯性测量单元与旋转式激光雷达发送pps脉冲电平信号；在硬件同步触发板发送pps脉冲信号后，向激光雷达发送基于gprmc标准格式的时间数据；时间数据会随着硬件同步触发板的主控模块的计时器的累加而改变，输出的时间范围为0-3600秒，超过3600秒后重新从0开始发送。
步骤4、上位机采集并缓存激光雷达数据和惯性测量单元数据；上位机的处理计算平台将采集753.58hz的激光雷达数据包，和400hz的惯性测量单元数据包并进行缓存，所述惯性测量单元数据包中携带有同步标志位；激光雷达缓存队列长度为76，惯性测量单元缓存队列长度为399；每当激光雷达数据的缓存队列到达76的长度就进行一次矫正处理。pps脉冲电平信号和gprmc信号时序图如图4所示。pps脉冲电平信号为在秒的整数倍发生跳变的信号，在本实施例中pps脉冲电平信号在秒的整数倍时产生上升沿。惯性测量单元数据包中携带有同步标志位，imu采集到pps脉冲电平信号的跳变时，将当前惯性测量单元数据包中的标志位置1，否则置0。激光雷达数据包输出为753.58hz，激光可工作在5-20hz，imu可工作在50hz-400hz情况下。本实施例选取激光雷达工作在10hz(76个数据包对应一帧360°输出)，imu为400hz。每个激光雷达数据包中携带有一个时间戳，代表该数据包第一束激光雷达的测量时刻。该数据包中的其他激光测量数据的时刻可根据第一束激光的时间戳计算。
步骤5、时间校正；检测76个激光雷达数据包对应的pps脉冲电平信号否存在跳变；如果不存在则不处理，如果存在跳变则在惯性测量单元数据包中查找同步标志位所在数据帧；查找到惯性测量单元同步帧后将同步帧的时间根据激光雷达数据包中的时间进行更新，并对同步帧之前和之后的数据进行更新。
步骤6、数据校正；解析出旋转式激光雷达激光距离测量的时间戳laseri.timestamp；在惯性测量单元缓存队列查找离laseri.timestamp最近的两帧数据的时间戳，第一时间戳imuj.timestamp和第二时间戳imuj+1.timestamp，其中第一时间戳和第二时间戳满足：
imuj.timestamp≤laseri.timestamp≤imuj+1.timestamp，当上式不满足时可选择下式
imuj.timestamp≤imuj+1.timestamp≤laseri.timestamp；
求解旋转变量imui.quaternion，使用四元数进行插值：
最后求解校正后的激光雷达数据：
laseri.xyz_correction＝imui.quaternion*laseri.xyz；
其中，laseri.timestamp，i∈[0,30000)是旋转式激光雷达激光距离测量的时间戳；slerp为四元差值函数；imui.quaternion，i∈[0,30000)是旋转式激光雷达激光距离测量对应的旋转变量；imuj.quaternion，j∈[0,200)，是惯性测量单元采集的四元数；imuj.timestamp，j∈[0,200)是惯性测量单元采集数据的时间戳；laseri.xyz是旋转式激光雷达计算出来的未校正的三维空间点；laseri.xyz_correction是校正后的三维空间点。
本实施例仅对旋转变量进行研究，这里假设平移变量为0，因为对于通常激光雷达应用来说，其测量距离范围为100m，测量频率为10hz，一般在两次测量之间的平移量不会超过1m，因此相对于100m的测量距离，该误差可忽略不计。若要将平移量进行补偿，本方法的架构依旧支持。
一种基于惯性测量单元的旋转式激光雷达数据校正系统，如图3所示，包括惯性测量单元、旋转式激光雷达、硬件同步处理板、gps模块和上位机。惯性测量单元与旋转式激光雷达固定在同一刚体上，惯性测量单元坐标系与激光雷达坐标系保持一致。将惯性测量单元连接至硬件同步处理板和上位机，将旋转式激光雷达连接至硬件同步处理板和上位机。将激光雷达的串口uart与ttl同步电平触发口连接至硬件同步处理板，将imu的ttl同步电平触发口连接至硬件同步处理板。gps模块提供gprmc和pps信号给硬件同步处理板做参考。将gps模块的串口uart与ttl同步电平触发口连接至硬件同步处理板。将激光雷达与电脑通过网线连接，将imu与电脑通过串口uart线连接。其中，惯性测量单元可以采用xsens-mti300，旋转式激光雷达可以采用速腾聚创科技有限公司全系列激光雷达或velodyne公司的全系列激光雷达，gps模块可采用三模卫星定位模块neo-m8nublox。
硬件同步处理板包括电源模块、备用电池、主控模块，如图5所示。将激光雷达的串口uart和ttl同步电平触发口连接至主控模块，将imu的ttl同步电平触发口连接至主控模块。gps模块通过串口uart或ttl同步电平触发口与主控模块连接。其中电源模块为主控模块提供3.3v工作电压，以及为外接的gps模块、imu提供5v工作电压，为多线激光雷达分别提供12v的工作电压。除电源模块为主控模块供电外，还有一颗备用的纽扣电池也可以为主控模块供电，满足主控模块处于掉电模式下的低功耗运行。硬件同步处理板，能够为激光雷达和imu模块提供硬件同步信号。
主控模块可以采用意法半导体公司的stm32f103单片机，内置全双工的通用同步异步收发器(usart)以及具有外部触发功能的gpio口，单片机的同步功能包括以下几个具体步骤：1、单片机接收gps模块通过串口发送来的nmea$gprmc消息，并解析出其中的utc时间信息和经纬度等地理位置信息。2、单片机接收gps模块的pps信号，同时向激光雷达和imu模块发送ttl同步脉冲信号。3、单片机判断gps信号是否有效，如果信号有效通过uart串口向激光雷达转发nmea$gprmc消息，如果无效则发送本地时钟消息。
硬件同步板具有如下功能：1、接收并解析gps模块传回的pps信号和nmea$gprmc消息(包含utc时间和地理位置信息)，将硬件同步板的本地时钟设置为gps提供的标准时钟。2、具备输出gps数据功能，能将nmea$gprmc消息发送给多线激光雷达，使激光雷达拥有时钟信息。3、能够使用备用电池以超低功耗在掉电的情况下维持本地时钟运行，在gps信号丢失的情况下发送本地时钟信息给激光雷达。4、具备输出pps信号功能，能将pps同步信号发送给激光雷达和imu模块，使两者的数据同步。5、具备稳压功能，能为gps模块、激光雷达、imu模块提供相应的工作电压。
上位机采集并缓存激光雷达数据和惯性测量单元数据，实现时间校正和数据校正，输出校正后的三维空间点。
上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。