【技术实现步骤摘要】
本专利技术属于车辆定位，尤其涉及工程车辆智能化slam定位方法。
技术介绍
1、工程车辆智能化环境感知的主要任务是获取周围环境信息和车辆位姿感知，在无人作业时，工程车辆必须能够实时定位与追踪自身的位置，才能明确自身所处位置及周围环境，从而规划并控制行驶路线。因此，准确的位姿估计是实现工程车辆智能化的关键。
2、激光同步定位与建图(simultaneous localization and mapping，slam)技术为工程车辆在未知环境中实时估计自身位姿并建立周围环境的地图提供了可能。然而，在实际工作环境中，动态物体的存在是不可避免的。由于这些动态物体的速度、航向、位置等信息对于工程车辆而言都是无法预知的，因此它们的轨迹可能会在点云地图中被保留下来，形成真实环境中不存在的“鬼影”，这会干扰工程车辆后续的导航与规划。因此，识别并去除动态点云对于工程车辆的智能化感知至关重要。
3、目前，常用的动态识别算法removert是基于可视性原理的动态识别方法，该方法将周围环境360°的信息通过投影转化为二维平面图像，即深度图，其中像素值表示点云点到激光雷达(lidar)的距离。通过设置不同的分辨率可以得到不同大小的深度图，在已知当前帧位姿的情况下，可以将当前帧点云从lidar坐标系下转到世界坐标系，然后通过当前帧与其若干前后帧点云在世界坐标系下的深度图作差，就可以识别出动态点。这种可视性法的原理也可以概述为：一个新激光点的光路穿过了另一个旧激光点，那么旧激光点则为动态点。但这种算法也存在一些问题：首先，可视性法判断
4、针对上述问题，本专利技术提出了工程车辆智能化slam定位方法。
技术实现思路
1、本专利技术的目的在于提供工程车辆智能化slam定位方法，旨在解决上述
技术介绍
中提出的问题。
2、本专利技术的目的通过以下技术方案得以实现：
3、工程车辆智能化slam定位方法，包括以下步骤：
4、步骤s1、在imu模块中通过运动模型建立预积分约束，并在上一帧位姿的基础上输出imu里程计；
5、步骤s2、通过imu里程计对激光点云进行畸变矫正并对当前帧位姿进行初始化估计；
6、步骤s3、通过去畸变后的当前帧点云与当前帧初始化位姿去除当前帧点云中的动态点；
7、步骤s4、对当前帧激光点云进行线面特征提取；
8、步骤s5、使用静态点云配准来优化当前帧点云，以输出当前帧点云的精确位姿；
9、步骤s6、将优化后的当前帧点云位姿作为历史帧位姿，继续对下一帧点云位姿进行实时优化。
10、进一步的，所述步骤s1的具体过程如下：
11、在激光slam中，引入imu数据进行融合，imu模块通过运动模型建立预积分约束，预积分约束利用当前帧与上一帧间的imu数据计算当前帧与上一帧之间的位姿变化量，作为对位姿变换量的约束；同时，imu模块在上一帧位姿的基础上输出imu里程计；
12、激光slam中的非线性优化变量为关键帧时刻的位移量p、速度量v、旋转量q以及imu的加速度计零偏ba和陀螺仪零偏bg；
13、假设两关键帧之间存在数十帧imu数据，利用imu数据，通过预积分方法计算从第k帧imu到第k+1帧imu的预积分量，表示为：
14、式1：
15、其中，下标i表示第i帧点云帧；下标k和k+1分别表示在i和i+1帧点云帧之间的第k，k+1帧的imu数据帧；δpi,k+1表示第k+1帧的位移变化量；δpi,k表示第k帧的位移变化量；δvi,k+1表示第k+1帧的速度变化量；δvi,k表示第k帧的速度变化量；δqi,k+1表示第k+1帧的旋转变化量(四元数表示)；δqi,k表示第k帧的旋转变化量(四元数表示)；表示第k+1帧的加速度计零偏；表示第k帧的加速度计零偏；表示第k+1帧的陀螺仪零偏；表示第k帧的陀螺仪零偏；△t表示从k与k+1帧的时间间隔；a表示加速度；表示四元数乘法；w表示角速度，表示第k帧的加速度噪声，表示第k帧陀螺仪的噪声。
16、进一步的，所述步骤s2的具体过程如下：
17、imu里程计通过积分计算，得出当前时刻的预测轨迹，即里程计位姿信息；
18、依据激光雷达扫描的先后顺序，结合水平旋转角度和扫描速度计算每个点的时间戳，具体的计算公式如下：
19、
20、其中，φt是某点相对于起始点的时间差，p0为起始点，pi为待计算的点，hh是激光雷达的频率，pi,x表示第i帧在x方向上的位移量，pi,v表示第i帧在v方向上的位移量，p0,x表示起始帧在x方向上的位移量，p0,v表示起始帧在v方向上的位移量；
21、在获取每个点的时间戳后，采用线性插值法计算去畸变点的位姿：对于任意一点，假设其时间戳为ti，该点前后的imu里程计位姿时间戳分别为tk和tk+1，tk≤ti≤tk+1，tk和tk+1对应的位姿分别为tk和tk+1，利用以下公式计算该点对应的位姿ti：
22、
23、进一步的，所述步骤s3的具体过程如下：
24、从imu里程计中获取前后两个位姿，利用式3进行线性插值处理，得到最新帧的初始化位姿；
25、在进行动态点去除时，将历史帧点云加入，构建成全局子地图，基于位姿点云信息建立kd-tree，以快速查找出当前帧的历史帧；
26、引入鸟瞰图，并将其与深度图结合使用；在进行鸟瞰图转换时，在激光雷达坐标系中划分出感兴趣区域，将核心范围内的点云信息保留下来，然后将点云数据转换为鸟瞰图；
27、通过对比关键帧和全局子地图的鸟瞰图高度差，设置阈值来识别动态点；如果是静态物体，物体在关键帧和全局子地图的位置不发生变化，即在鸟瞰图中的高度一致；如果是动态物体，全局子地图中会存在动态点云，在关键帧中没有相应的点，即物体在全局子地图的鸟瞰图中的高度大于在关键帧的鸟瞰图中的高度；
28、假设关键帧对应的鸟瞰图为bq，全局子地图对应的鸟瞰图为bm，通过作差得到差值鸟瞰图bdiff，即：
29、式4：bdiff＝bq-bm；
30、对差值鸟瞰图bdiff中的高度差进行分析，小于阈值的点即可判断为动态点。<本文档来自技高网...
【技术保护点】
1.工程车辆智能化SLAM定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的工程车辆智能化SLAM定位方法，其特征在于，所述步骤S1的具体过程如下：
3.根据权利要求2所述的工程车辆智能化SLAM定位方法，其特征在于，所述步骤S2的具体过程如下：
4.根据权利要求3所述的工程车辆智能化SLAM定位方法，其特征在于，所述步骤S3的具体过程如下：
5.根据权利要求4所述的工程车辆智能化SLAM定位方法，其特征在于，所述步骤S4的具体过程如下：
6.根据权利要求5所述的工程车辆智能化SLAM定位方法，其特征在于，所述步骤S5的具体过程如下：
【技术特征摘要】
1.工程车辆智能化slam定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的工程车辆智能化slam定位方法，其特征在于，所述步骤s1的具体过程如下：
3.根据权利要求2所述的工程车辆智能化slam定位方法，其特征在于，所述步骤s2的具体过程如下：
4.根据权...
【专利技术属性】
技术研发人员：毕秋实，何海腾，商敬斌，姚宗伟，李学飞，毕延泽，
申请(专利权)人：吉林大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。