一种面向动态料场的激光雷达定位方法及装置与流程
本技术属于设备自主定位,具体而言,涉及一种面向动态料场的激光雷达定位方法及装置。
背景技术:
1、铰接式装载机是港口、矿山散料料场的核心作业设备,无人化自主作业已成为行业发展趋势,高精度鲁棒定位是实现装载机自主作业的核心基础。动态料场的作业环境与装载机运动特性,对现有定位系统提出了三类难以同时解决的核心挑战:(1)料堆动态变化导致静态地图定位退化:料堆几何形态随铲装、卸料作业持续变化,传统先建图后定位的静态地图方案,在形变区域会产生大量错误匹配约束,易引发定位抖动甚至发散;现有动态更新方案不对静态基础设施与动态料堆做区分,且点云更新与定位共用同一地图,容易形成“定位误差→地图污染→更大误差”的误差自循环放大。(2)冲击振动与低纹理场景导致匹配退化:装载机铲装作业伴随强烈冲击振动,易造成imu短时饱和失真;料堆表面多为低纹理土石面,几何特征贫乏,轮胎打滑也属于作业常态,导致激光点面匹配约束在横向、垂向方向退化,仅靠激光匹配无法抑制该方向漂移,现有方法缺乏利用铰接式装载机本体运动学特征补充退化方向可观测性的技术手段。(3)定位漂移缺乏独立检测与自动恢复机制:定位漂移多为渐进累积产生,漂移初期单帧匹配分数仍可维持合格,等到匹配分数明显下降时位姿已偏离真实值较远;现有退化检测多为单帧层面的几何约束评估,无法检测“每帧匹配合格但多帧累积缓慢漂移”的隐蔽故障,且无gnss封闭料场环境下,检测到漂移后仅能告警等待人工干预,缺乏自动恢复能力。
2、激光雷达-惯性里程计是工程机械自主定位的主流技术路线,但均未针对动态料场环境与铰接式装载机的运动特性作出专门适配,现有技术方案普遍存在以下不足:(1)静态地图定位方案:将全局点云地图作为不可变定位基准,仅通过剔除料堆解决形变问题的方案,属于消极避让策略,剔除后料堆区域完全丧失定位基准,无法满足连续作业需求;(2)全量增量地图更新方案:不对动态料堆区域与静态结构做区分,长期运行后地图整体精度劣化,且更新触发条件仅依赖匹配相关指标,未采用独立于匹配结果的车辆稳定态物理判据保障更新精度;(3)现有双层地图方案:多采用随车滑动的局部动态子图结构,未针对料场场景限定固定的料堆更新区域,也未设计稳定态更新准入机制与双层联合配准逻辑;(4)车辆运动学约束方案:现有运动学约束的权重多为固定值或仅基于运动状态识别自适应调整,未实现基于约束残差的自适应权重调整,也未将其融入激光匹配型误差状态卡尔曼滤波框架;(5)定位漂移检测方案:均为单帧层面的几何约束强度评估,无法检测多帧累积的隐蔽缓慢漂移,也未提出适配无gnss封闭料场的全自动漂移恢复方案。
3、因此,针对上述问题,急需要提供一种面向动态料场的激光雷达定位方法,以提高装载机定位的准确度。
技术实现思路
1、本技术的主要目的在于提供一种面向动态料场的激光雷达定位方法及装置。该方法能够有效提高装载机定位系统定位的准确度
2、为了实现上述目的,根据本技术的第一方面,提供一种面向动态料场的激光雷达定位方法,所述方法应用于装载机的定位系统;所述装载机上安装有激光雷达和惯性测量单元imu;至少包括如下步骤:基于目标料场所对应的全局静态层地图以及动态增量层地图,构建双层地图;其中,所述全局静态层地图用于指示在所述装载机未执行装卸作业前所构建的目标料场的只读式全局点云地图,动态增量层地图用于指示在所述装载机执行装卸作业过程中所构建的预设料堆更新区域的局部点云地图;在所述装载机执行装卸作业过程中,实时采集目标料场的激光点云数据,生成点云数据集;针对所述点云数据集中任一当前帧点云:基于imu运动先验,对所述当前帧点云进行预处理,生成预处理后点云;基于所述双层地图,对所述预处理后点云进行双层联合匹配,构建点面观测约束;基于所述点面观测约束和后轮中心运动学约束,对所述当前帧点云执行误差状态卡尔曼滤波eskf更新,生成所述装载机的车体最优位姿;基于所述车体最优位姿和所述当前帧点云,更新所述动态增量层地图,生成更新后动态增量层地图;将更新后动态增量层地图作为下一个当前帧点云进行双层联合匹配的参考。
3、可选的,所述方法还包括:基于所述车体最优位姿,对当前帧点云进行跨帧几何一致性检验,生成所述装载机的车体校正位姿。
4、可选的,所述基于所述车体最优位姿和所述当前帧点云,更新所述动态增量层地图,生成更新后动态增量层地图;包括:将所述当前帧点云输入第一门控中,进行滤波收敛检测;若第一门控检测结果表征所述当前帧点云对应的滤波协方差已收敛且所述定位系统的自身坐标系与所述全局静态层地图的全局坐标系已完成配准对齐,则将所述当前帧点云输入第二门控中;基于所述当前帧点云对应的车体最优位姿,确定所述装载机在后轮中心系下车体的横向速度和垂向速度;基于所述横向速度和垂向速度,对第二门控中所述当前帧点云进行稳定性检测;若第二门控检测结果表征所述横向速度和垂向速度均满足预设条件,则确定所述装载机处于稳定状态,并将所述当前帧点云输入第三门控中;从所述第三门控的激光点云数据中每间隔预设数量的帧选取目标帧点云,基于车体校正位姿,将所述目标帧点云从激光雷达的自身坐标系下变换至全局坐标系下,生成变换后目标帧点云;针对所述变换后目标帧点云中任一目标点:判断所述目标点是否落入预设料堆更新区域;若判断结果表征所述目标点落入预设料堆更新区域内,则将所述目标点插入动态增量层地图;基于所述变换后目标帧点云中每个所述目标点的判断结果,生成更新后动态增量层地图;其中,所述预设料堆更新区域用于指示框选的料堆动态区域。
5、可选的,所述基于imu运动先验,对所述当前帧点云进行预处理,生成预处理后点云;包括:基于imu运动先验,对所述当前帧点云做运动去畸变,生成去畸变后点云;对所述去畸变后点云进行体素降采样,生成预处理后点云。
6、可选的,所述基于所述双层地图,对所述预处理后点云进行双层联合匹配,构建点面观测约束;包括:针对所述预处理后点云中任一目标点:从所述全局静态层地图中查询距离所述目标点的最短距离小于第一预设阈值的第一样本点;若查询结果表征第一样本点的数量小于第二预设阈值,则从所述动态增量层地图中查询距离所述目标点的最短距离小于第一预设阈值的第二样本点;若所述第一样本点的数量和所述第二样本点的数量之和小于第二预设阈值,则将所述目标点标记为无效点;若所述第一样本点的数量和所述第二样本点的数量之和不小于第二预设阈值,则通过最小二乘法将所述第一样本点和所述第二样本点拟合成局部平面;若查询结果表征所述第一样本点的数量不小于第二预设阈值,则通过最小二乘法将所述第一样本点拟合成局部平面;计算所述目标点到所述局部平面之间的距离绝对值;若所述距离绝对值小于第三预设阈值,则将所述距离绝对值作为残差门限;对所述残差门限进行残差超阈值自适应加权,构建点面观测约束。
7、可选的,所述方法还包括构建后轮中心运动学约束;所述构建后轮中心运动学约束,包括:基于imu在世界坐标系下对应的第一位姿以及在后轮中心系下对应的第二位姿;确定后轮中心在世界坐标系中的旋转矩阵;基于所述装载机的车体最优位姿和所述旋转矩阵,生成所述装载机在后轮中心系下的车体速度;基于所述车体速度对应的横向单位向量和纵向单位向量,以及旋转矩阵,确定横向观测雅可比和纵向观测雅可比;基于所述车体速度对应的横向速度和理论横向速度之间的差值确定横向残差,并基于所述车体速度对应的纵向速度和理论纵向速度之间的差值确定纵向残差;基于所述横向观测雅可比、所述纵向观测雅可比、所述横向速度对应的自适应权重,以及纵向速度对应的自适应权重,构建第一约束;并基于所述横向观测雅可比、所述纵向观测雅可比、横向残差、纵向残差、所述横向速度对应的自适应权重,以及纵向速度对应的自适应权重,构建第二约束;基于所述第一约束和所述第二约束,构建后轮中心运动学约束。
8、可选的,所述基于车体最优位姿,对当前帧点云进行跨帧几何一致性检验,生成所述装载机的车体校正位姿;包括:从当前帧点云中提取特征点,生成特征点集;针对特征点集中任一特征点:基于k个历史帧对所述特征点进行最近邻匹配;若匹配结果表征所述特征点在至少连续n个历史帧中被匹配成功,则将所述特征点添加至一致性检验池;基于所述当前帧点云对应的车体最优位姿,将所述特征点投影至全局坐标系下,获得特征点坐标;基于所述特征点坐标和形成配对关系的历史点坐标,确定特征点的重投影残差;其中,k为大于n的正整数;针对所述特征点集中每个所述特征点对应的重投影残差,确定所述当前帧点云对应的中位数残差、高分位残差以及匹配成功率;若匹配成功率大于预设阈值,则基于所述当前帧点云对应的中位数残差以及高分位残差进行漂移检测;若漂移检测结果表征连续n帧点云中每个所述当前帧点云对应的中位数残差均大于预设阈值,或者所述当前帧点云对应的高分位残差增幅大于预设阈值,则确定所述当前帧点云的定位发生了漂移;在预设滑动窗口内,基于历史帧点云地图对所述当前帧点云进行局部匹配校正,生成装载机的车体校正位姿。
9、可选的,所述在预设滑动窗口内,基于历史帧点云地图对所述当前帧点云进行局部匹配校正,生成装载机的车体校正位姿;包括:在预设滑动窗口内,将未发生漂移的激光点云数据帧标记为绿帧,并将发生漂移的激光点云数据帧标记为红帧;将发生漂移前的最后一个绿帧作为参考帧,并基于距离所述参考帧最近的l个连续绿帧,构建参考点云;基于所述参考点云,对所述当前帧点云执行ndt和gicp两级配准,生成校正位姿;基于所述校正位姿,将所述当前帧点云从激光坐标系下重投影至全局坐标系下,重新计算中位数残差;若所述中位数残差不大于预设阈值,则将所述校正位姿作为装载机的车体校正位姿;并将所述车体校正位姿作为基准,基于下一个当前帧点云进行车体定位;若所述中位数残差大于预设阈值,则对所述当前帧点云发出用于指示漂移状态的告警提示。
10、为实现上述目的,根据本实施例第二方面,还提供一种面向动态料场的激光雷达定位装置,所述装置应用于装载机的定位系统;所述装载机上安装有激光雷达和惯性测量单元imu;至少包括:构建模块,用于基于目标料场所对应的全局静态层地图以及动态增量层地图,构建双层地图;其中,所述全局静态层地图用于指示在所述装载机未执行装卸作业前所构建的目标料场的只读式全局点云地图,动态增量层地图用于指示在所述装载机执行装卸作业过程中所构建的预设料堆更新区域的局部点云地图;采集模块,用于在所述装载机执行装卸作业过程中,实时采集目标料场的激光点云数据,生成点云数据集;位姿更新模块,用于针对所述点云数据集中任一当前帧点云:基于imu先验,对所述当前帧点云进行预处理,生成预处理后点云;基于所述双层地图,对所述预处理后点云进行双层联合匹配,构建点面观测约束;基于所述点面观测约束和后轮中心运动学约束,对所述当前帧点云执行eskf更新,生成所述装载机的车体最优位姿;基于所述车体最优位姿和所述当前帧点云,更新所述动态增量层地图,生成更新后动态增量层地图;将更新后动态增量层地图作为下一个当前帧点云进行双层联合匹配的参考。
11、为实现上述目的,根据本实施例第三方面,还提供一种计算机可读介质,其上存储有计算机程序,所述程序被处理器执行时实现如第一方面所述的方法。
12、本技术的实施例提供的技术方案可以包括以下有益效果:
13、1、本实施例将地图解耦为“只读全局静态层地图”与“更新区域可写的动态增量层地图”;并在双层联合匹配时采用静态层优先、近邻不足时动态层补充;而且,动态层写入采用三重门控,从根本上切断“定位误差→地图污染→更大定位误差”的自循环;从而实现了精准定位。
14、2、本实施例利用铰接式装载机后桥固定于后车架、后轮不参与转向、正常行驶中无侧滑且贴地的运动学特性,将“后轮中心车体系横/垂向速度为零”作为软约束,以“残差超阈值自适应增权”机制注入迭代eskf,与点面观测约束同帧联合求解,从而为匹配退化方向提供了独立可观测性,进而提高了定位的准确性。
15、3、本实施例通过提取连续帧间可稳定追踪的几何特征点,仅选取那些在多帧中均能被持续观测到的点,计算其在全局坐标系下的重投影残差,作为独立于匹配分数的定位质量判定依据;当检测到漂移时,定位系统自动触发基于一致性历史帧子图的局部重匹配校正,实现无外部基准的定位自恢复。由此,能够有效规避快速运动导致视野突变的误报问题。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com