发明内容
本发明要解决的技术问题是提供一种基于旋转结构的激光雷达建图方法及其扫描系统,以使得旋转结构激光雷达实现单三维激光雷达对全三维空间的无间隙密集化测量。
为了解决上述技术问题,本发明采用的技术方案是:
一种基于旋转结构的激光雷达建图方法,包括:
S1:雷达原始点云输入;
S2:根据电机参数标定进行雷达点云校正;
S4:根据IMU数据进行雷达点云校正;
S5:对点云的平移量进行校正,进而对旋转量进行校正;
S6:地图点云输出。
优选的,步骤S2之后还包括:
S3:光栅编码器实时获取电机转速,在预设时间间隔内进行点云校正。
步骤S2中电机参数标定的方法为:
S21:记录电机启动时间,让电机正常运行;
S22:依据脉冲/每转的电机开发信息来选择运行时间;
S23:测定结束时刻电机偏离零位情况;
S24:依据步骤S22和步骤S23计算电机转速特性。
优选的,步骤S4包括:
S41:对IMU特性参数进行二次标定;
S42:对IMU和雷达之间的外参数进行标定;
S43:依据步骤S41和步骤S42的标定数据对雷达点云瞬时运动进行补偿。
优选的,步骤S42包括:
S421:使用步骤S41所述方法初始化IMU内部参数;
S422:在无电机转动情况下,使得雷达与IMU载体在三维空间中运动;
S423:通过匹配求取每帧雷达点云之间的变换矩阵;
S424:通过IMU数据获取雷达点云之间的变换矩阵;
S425:通过匹配求取步骤S423和步骤S424之间的点云变换矩阵并存储;
S426:联立所有步骤S425获得的数据进行无约束优化求解。
优选的,步骤S43具体为:根据步骤S41和步骤S42内容,以及步骤S2点云时间戳进行IMU和点云时间对齐,并假设IMU两个数据采集时间内变化为匀速,进行点云旋转与平移插值处理。
优选的,对点云的平移进行处理,并且对旋转量进行进一步的校正;
优选的,实现步骤S5的方法是:对点云匹配产生的轨迹数据与IMU产生的轨迹数据进行无偏估计。
优选的,实现步骤S5的方法具体包括:
S51:依据步骤S41和步骤S42标定数据进行旋转结构雷达载体运动轨迹估计;
S52:使用步骤S51的估计参数作为点云匹配预估参数,进行点云匹配计算旋转结构雷达载体运动轨迹;
S53:对于步骤S51和步骤S52的轨迹进行kalman滤波无偏估计;
S54:使用估计的常数级误差特性,更新IMU的位姿估计,预测和估计下一个时间段轨迹;
S55:使用步骤S54的估计结果进行匹配并获得新的轨迹信息,判断是否测量结束,结束进入下一步,否则返回S53;
S56:保存三级校正后的点云数据。
一种基于旋转结构的激光雷达扫描系统,包括电机、光栅编码器和IMU,用于实施前述任一项所述的方法。
本发明能有效处理旋转结构激光雷达各传感器数据,以使得旋转结构激光雷达实现单三维激光雷达对全三维空间的无间隙密集化测量,其测量精度达到或者接近静态激光雷达的扫描精度。相较于传统处理方法,本发明较单线旋转结构具有更高实时性及测量密度,相较于多激光雷达组合方法则具有更低廉成本优势,且其广阔的测量视野可以有效处理人员或者设备不易到达的地方,具有更高的应用前景。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明。在此需要说明的是,对于这些实施方式的说明用于帮助理解本发明,但并不构成对本发明的限定。此外,下面所描述的本发明各个实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互组合。
如图1所示,一种基于旋转结构的激光雷达建图方法,包括:
S1:雷达原始点云输入;
S2:根据电机参数标定进行雷达点云校正;
无光栅编码器点云实时运动补偿,主要是根据电机特性进行点云的运动精补偿,这种补偿的精度严重受制于电机性能。设电机转速为N,其减速比为k,则可求得共轴雷达相对载体转速为n=N×k(下文统称公转转速),对雷达公转速相对于时间求导可得每微妙公转角度数θ,则电机的起始时间确定后,求得雷达中每一个点的时间相对于电机零位时间t,可得任一雷达点云相对于载体坐标系的相对关系。在静态情况下(动态情况将在步骤S4进行讨论),该点在载体坐标系之间只存在旋转变换关系,其旋转矩阵如公式1所示。
雷达任意一点点云的时间可以通过雷达驱动UDP数据块发布的时间以及各线雷达发布顺序和充能时间获得。以速腾聚创16线机关国内雷达为例,其任一点云的时间估计函数如下所示。
公式(2)中Time_offset为每个点关于当前帧雷达数据的初始时间戳,sequence_index为一个数据块的索引,每个数据块包含一次完整脉冲发射和充能时间。data_index为一组16线激光发射器的编号索引。Timestamp为接收数据块的初始时间戳,Exact_point_time为当前点的准确时间,误差在微妙级。电机的转速标定基于电机转速恒定架设,其测定方法为:
S21:记录电机启动时间,让电机正常运行;
S22:依据脉冲/每转的电机开发信息来选择运行时间(最大环境测量时间的200%,一般环境测量用时3小时左右,这里取持续运行时间为7小时);
S23:测定结束时刻电机偏离零位情况;
在总体角度中减去该角度,记录电机停止时间(脉冲停止调制时间)。
S24:依据步骤S22和步骤S23计算电机转速特性(角度/微妙)。
依据电机特性的运动补偿,可以在数据采集的同时进行点云运动校正。但是电机转速并不恒定,步进电机会在运动中适当的调速转速,从而使得每两周之间存在几毫秒的偏差,这种偏差的累计一直持续会导致雷达点云的变形。这时需要依据光栅编码器来进行电机转速的实时获取,取用短时性事后处理的方式来进行点云校正。
S3:光栅编码器实时获取电机转速,在预设时间间隔内进行点云校正;
基于先验标定的电机转速从长时性上来看是具有较高可信度的,但是每转的微小偏差还是会影响一定数量的点云。通过光栅编码器,采用小时段事后处理方法进行校正。一般取光栅编码器数据采集速度为雷达数据采集速度的10倍,这样可以保证任一时刻的环境采样都能得实时观察,保证校正精确度。其校正处理方法和步骤S2一致,但是将估计转速转换为实时转速,其校正过程不是在采集过程中,而是在采集后的一个较小时段内。该处理也更改了原雷达的点云发布时间频率,由原来的实时发布,变为在原时间上延后10%,约为10ms,依然保证了实时性的要求。
S4:根据IMU数据进行雷达点云校正;
S41:对IMU特性参数进行二次标定;
IMU(Inertial Measurement Unit惯性测量单元),其依据加速计、磁力计及陀螺仪对其载体的运动状态进行估计。一般在其出厂时已经经过标定及校正操作。这里的二次标定操作主要是指依据当前环境下的特定场景,观察IMU的输出特性。IMU特性参数包括当前环境重力加速度、静态加速协方差矩阵、运动状态下的角度变换测量可行度协方差矩阵及角加速度测量可信度协方差矩阵等。测得可行度矩阵用来选取雷达点云瞬时运动补偿时间段及其误差分析。
S42:对IMU和雷达之间的外参数进行标定;
IMU与雷达外参标定,用以解决雷达和IMU在静态安装上的非同轴性,其标定方案如图2所示。分别采集两个已知位姿点雷达点云数据,在雷达从一个位姿到另一个位姿的过程中采集IMU的数据,根据IMU的加速度二次积分和角度求得两个已知位姿点之间的变换矩阵Ri。在IMU二次积分平移估计误差较大时,可以直接使用已知位姿平移量替代,而旋转量取IMU的测量值。同样依据两个位姿点点云配准,可以求得一个变换矩阵Rp。根据已知两个点云属于同一点云,可得Ri×M=Rp,这里所求M即为IMU和雷达之间的外参数。该参数可以通过两个不同变换后点云之间的匹配获得。基于该思路可以直接使用匹配方法来进行IMU和雷达之间的自动外参标定工作,其实现步骤如下:
S421:使用步骤S41所述方法初始化IMU内部参数;
S422:在无电机转动情况下,使得雷达与IMU载体在三维空间中运动;
S423:通过匹配求取每帧雷达点云之间的变换矩阵;
S424:通过IMU数据获取雷达点云之间的变换矩阵;
S425:通过匹配求取步骤S423和步骤S424之间的点云变换矩阵并存储;
S426:联立所有步骤S425获得的数据进行无约束优化求解。
S43:依据步骤S41和步骤S42的标定数据对雷达点云瞬时运动进行补偿。
根据步骤S41和步骤S42内容,以及步骤S2点云时间戳进行IMU和点云时间对齐,并假设IMU两个数据采集时间内变化为匀速,进行点云旋转与平移插值处理。一般情况下IMU角度估计误差为常数级,而加速的平移误差随时间线性增长,因而第二级点云校正的主要校正参数为点云的角度偏移。其平移偏差量将在步骤S5所述技术方案中进行处理。
S5:对点云的平移量进行校正,进而对旋转量进行校正;
在三级校正中重点是校正平移量,但是也会根据平移量对旋转量作一定的优化处理,以达到在平移和旋转两个量上都更为精准。其实现手段是对点云匹配产生的轨迹数据与IMU产生的轨迹数据进行无偏估计,具体包括:
S51:依据步骤S41和步骤S42标定数据进行旋转结构雷达载体运动轨迹估计;
S52:使用步骤S51的估计参数作为点云匹配预估参数,进行点云匹配计算旋转结构雷达载体运动轨迹;
S53:对于步骤S51和步骤S52的轨迹进行kalman滤波无偏估计;
S54:使用估计的常数级误差特性,更新IMU的位姿估计,预测和估计下一个时间段轨迹;
S55:使用步骤S54的估计结果进行匹配并获得新的轨迹信息,判断是否测量结束,结束进入下一步,否则返回步骤S53;
S56:保存三级校正后点云数据。
S6:地图点云输出。
一种基于旋转结构的激光雷达扫描系统,包括电机、光栅编码器和IMU,其用于实施前述所述的方法。
本发明的最终目标是产生关于全三维空间的测量结果,不同与传统的测绘方法需要先设计和选定大量的测绘点,该方法不必事先选取测绘点,也不需要后期点云数据的手动筛选匹配,当系统运行时,采集者只需持该设备绕行一圈,便能得到完整的三维测量结果。图1中,其输入为激光雷达原始云,输出为三维空间信息表达三维地图点云。通过该三维地图点云可以通过选择测量点或者空间尺寸、比例、空间容量、占据体积等相关信息。
本发明充分考虑获取每一帧数据中每个点的时间不同和姿态不同,分别进行多传感器数据的处理,对激光点云数据中的每个点进行精确的补偿和处理,为后续的精确匹配和优化奠定基础,进而保障生成满足要求的环境重构数据。
以上结合附图对本发明的实施方式作了详细说明,但本发明不限于所描述的实施方式。对于本领域的技术人员而言,在不脱离本发明原理和精神的情况下,对这些实施方式进行多种变化、修改、替换和变型,仍落入本发明的保护范围内。