移动载体定位方法与装置与流程

本申请涉及定位，尤其是涉及到一种移动载体定位方法与装置。

背景技术：

1、无人机、机器人等移动载体在执行自主导航、环境探测或基础设施巡检等任务时，需要确定自身在目标空间中的位姿，即定位。在室外开阔环境中，全球导航卫星系统（global navigation satellite system，gnss）可提供米级或厘米级的绝对定位服务，是移动载体最常用的定位手段。然而，当移动载体进入地下停车场、长距离隧洞等场景时，卫星信号会受到建筑物结构、地下设施等物理遮挡，导致gnss定位失效或精度急剧下降，使得移动载体难以利用gnss持续获得可靠的绝对位置。这里，gnss信号不可用或不可靠的环境，称为gnss拒止环境。

2、在gnss拒止环境下，相关技术通常利用移动载体自身搭载的激光雷达（lightdetection and ranging，lidar）与惯性测量单元（inertial measurement unit，imu），配合预先构建的目标空间的高精度先验地图，实现移动载体的融合定位。然而，当移动载体处于长直通道、平整壁面或结构重复区域时，激光雷达扫描得到的环境几何特征较为单一，或者环境几何特征在不同运动方向上的约束能力不均衡，使得部分位姿自由度缺少充分约束，定位结果容易受到测量噪声和匹配误差的影响，造成定位精度下降，严重时甚至发生定位失效，形成定位退化现象。并且，在狭窄或复杂的空间中作业时，移动载体不可避免地会与墙壁、管道等障碍物发生物理接触或撞击。而在相关技术中的融合定位方法中，碰撞事件通常被视为安全事故或异常干扰，会采取急停或简单剔除该时刻传感器数据等处理方式，碰撞信息本身不会被用于辅助定位。

3、因此，相关技术在gnss拒止、环境几何特征退化且可能发生物理碰撞的复杂场景中，无法有效缓解定位退化，仍存在定位精度较低、连续性较差及可靠性不足的问题。

技术实现思路

1、有鉴于此，本申请提供了一种移动载体定位方法与装置，解决了在退化且可能发生物理碰撞的gnss拒止场景中定位精度低、连续性差、可靠性不足的问题。

2、根据本申请的一个方面，提供了一种移动载体定位方法，包括：

3、获取所述移动载体在运动时所述激光雷达采集的点云数据与所述惯性测量单元测量的运动数据；

4、基于所述运动数据执行预积分处理与去畸变处理，得到关键帧时刻下移动载体的状态变量的预测值与所述点云数据中关键帧点云对应的去畸变点云，所述关键帧时刻为所述关键帧点云的扫描时刻；

5、基于所述去畸变点云，构建关键帧时刻下的地图匹配因子，以及相邻关键帧时刻之间的惯性测量单元因子；

6、基于所述地图匹配因子与惯性测量单元因子，确定关键帧时刻下状态变量的退化方向；

7、基于所述运动数据与退化方向，确定所述防护结构与障碍物之间发生的有效碰撞事件及其碰撞因子；

8、基于所述地图匹配因子、惯性测量单元因子与碰撞因子，构建因子图；

9、基于状态变量的预测值调整所述因子图中关键帧时刻下状态变量的取值，并基于调整后的状态变量确定移动载体在关键帧时刻下的目标位姿。

10、根据本申请的另一方面，提供了一种移动载体定位装置，包括：

11、获取模块，用于获取所述移动载体在运动时所述激光雷达采集的点云数据与所述惯性测量单元测量的运动数据；

12、确定模块，用于基于所述运动数据执行预积分处理与去畸变处理，得到关键帧时刻下移动载体的状态变量的预测值与所述点云数据中关键帧点云对应的去畸变点云，所述关键帧时刻为所述关键帧点云的扫描时刻；以及，基于所述去畸变点云，构建关键帧时刻下的地图匹配因子，以及相邻关键帧时刻之间的惯性测量单元因子；以及，基于所述地图匹配因子与惯性测量单元因子，确定关键帧时刻下状态变量的退化方向；以及，基于所述运动数据与退化方向，确定所述防护结构与障碍物之间发生的有效碰撞事件及其碰撞因子；

13、定位模块，用于基于所述地图匹配因子、惯性测量单元因子与碰撞因子，构建因子图；以及，基于状态变量的预测值调整所述因子图中关键帧时刻下状态变量的取值，并基于调整后的状态变量确定移动载体在关键帧时刻下的目标位姿。

14、根据本申请再一个方面，提供了可读存储介质，其上存储有程序或指令，程序或指令被处理器执行时实现上述移动载体定位方法的步骤。

15、根据本申请又一个方面，提供了一种计算机设备，包括存储介质、处理器及存储在存储介质上并可在处理器上运行的计算机程序，处理器执行程序时实现上述移动载体定位方法的步骤。

16、借由上述技术方案，本申请提供了一种移动载体定位方法与装置，通过各关键帧时刻下的地图匹配因子与惯性测量单元因子构造局部费歇尔信息矩阵并计算位置归一化退化度，从而确定移动载体位置变量的退化方向与退化类型，并根据退化程度自适应调节地图匹配因子与惯性测量单元因子的权重，同时在退化方向上进行各向异性压缩，有效抑制了不可靠观测对移动载体状态变量估计的污染；进一步地，通过将防护结构与障碍物的物理碰撞事件转化为具有明确几何意义的碰撞因子，并基于互补度与碰撞观测信噪比选择性地将其加入因子图，使碰撞信息能够针对性补偿退化方向上的信息缺失，有效解决了相关技术在退化且可能发生物理碰撞的复杂场景中定位精度低、连续性差、可靠性不足的问题，在gnss拒止且环境几何特征退化的复杂场景中，实现了移动载体的高精度、高鲁棒性定位。

17、上述说明仅是本申请技术方案的概述，为了能够更清楚了解本申请的技术手段，而可依照说明书的内容予以实施，并且为了让本申请的上述和其它目的、特征和优点能够更明显易懂，以下特举本申请的具体实施方式。

1.一种移动载体定位方法，其特征在于，应用于搭载有激光雷达与惯性测量单元的移动载体，所述移动载体外部包络有防护结构，所述方法包括：

2.根据权利要求1所述的移动载体定位方法，其特征在于，所述状态变量包括位置变量与姿态变量，所述基于所述地图匹配因子与惯性测量单元因子，确定关键帧时刻下状态变量的退化方向，具体包括：

3.根据权利要求2所述的移动载体定位方法，其特征在于，所述方法还包括：基于所述退化方向、位置归一化退化度与姿态归一化退化度，分别确定关键帧时刻下地图匹配因子与惯性测量单元因子的权重；

4.根据权利要求1所述的移动载体定位方法，其特征在于，状态变量包括位置变量与姿态变量，运动数据包括惯性测量单元在不同测量时刻下测量的移动载体的加速度测量值，测量时刻与关键帧时刻处于同一时间基准下，所述基于所述运动数据与退化方向，确定所述防护结构与障碍物之间发生的有效碰撞事件，具体包括：

5.根据权利要求4所述的移动载体定位方法，其特征在于，所述基于所述归一化冲击方向，判断判定时间窗内是否发生可信碰撞事件，具体包括：

6.根据权利要求4所述的移动载体定位方法，其特征在于，所述基于所述碰撞法向，判断目标碰撞事件是否为有效碰撞事件，具体包括：

7.根据权利要求1所述的移动载体定位方法，其特征在于，所述确定有效碰撞事件的碰撞因子，具体包括：

8.根据权利要求1所述的移动载体定位方法，其特征在于，所述基于状态变量的预测值调整所述因子图中关键帧时刻下状态变量的取值，具体包括：

9.根据权利要求8所述的移动载体定位方法，其特征在于，所述求解优化目标函数，具体包括：

10.一种用于实现权利要求1-9中任一项所述的移动载体定位方法的装置，其特征在于，所述装置包括：