基于碰撞平面约束的位姿修正方法、装置和移动载体与流程

本申请涉及导航定位，尤其是涉及到一种基于碰撞平面约束的位姿修正方法、装置和移动载体。

背景技术：

1、随着无人机、移动机器人等移动载体在复杂环境中的广泛应用，自主导航定位技术已成为关键支撑技术。在gnss（global navigation satellite system，全球导航卫星系统）信号可用的开阔环境中，移动载体可通过卫星定位获得较高精度的位姿估计。然而，在管廊、隧道、锅炉内部、矿井等gnss拒止或信号严重衰减的环境中，移动载体通常依赖惯性导航系统、激光雷达slam（同步定位与建图）或视觉里程计等手段进行自主定位。为保障移动载体在复杂环境中安全运行，通常在其外部包络有防护结构，以允许在发生碰撞时继续执行任务。

2、现有技术中，传统系统将碰撞视为异常噪声或安全事故，仅采取急停、反弹或数据剔除等处理方式，未利用碰撞所蕴含的几何信息。直接对原始加速度设置阈值进行碰撞检测的方式，容易将急转弯、螺旋桨振动或控制加速度误判为碰撞。此外，仅输出二值碰撞标签的检测方式不能为定位因子提供碰撞法向与可信度信息，无法将碰撞事件转化为具有明确几何意义和不确定度的位姿约束。

技术实现思路

1、有鉴于此，本申请提供了一种基于碰撞平面约束的位姿修正方法、装置和移动载体，通过将移动载体与环境之间的碰撞事件转换为因子图中的碰撞平面约束因子，与惯性运动约束因子和地图匹配约束因子联合优化，实现了在gnss拒止或几何退化环境下利用碰撞信息补充位置可观测性的容错定位，提高了移动载体的定位精度和鲁棒性。

2、根据本申请的第一个方面，提供了一种基于碰撞平面约束的位姿修正方法，应用于外部包络有防护结构的移动载体，所述方法包括：

3、响应于所述移动载体发生碰撞事件，基于所述移动载体在碰撞时刻前后时间窗内的冲击残差确定碰撞法向；

4、基于所述移动载体碰撞前后沿所述碰撞法向的速度变化量与所述防护结构的外包络参数，计算所述防护结构的接触变形量；

5、基于所述碰撞法向、所述接触变形量、所述外包络参数以及碰撞平面参考点，构建碰撞平面约束因子；其中，所述碰撞平面约束因子用于表征所述移动载体的中心到接触平面距离；

6、将所述碰撞平面约束因子加入包括惯性运动约束因子和地图匹配约束因子的因子图中；

7、对所述因子图进行联合优化，以获得所述移动载体的修正位姿。

8、根据本申请的第二个方面，提供了一种基于碰撞平面约束的位姿修正装置，应用于外部包络有防护结构的移动载体，所述装置包括：

9、约束构建模块，用于响应于所述移动载体发生碰撞事件，基于所述移动载体在碰撞时刻前后时间窗内的冲击残差确定碰撞法向；以及，基于所述移动载体碰撞前后沿所述碰撞法向的速度变化量与所述防护结构的外包络参数，计算所述防护结构的接触变形量；以及，基于所述碰撞法向、所述接触变形量、所述外包络参数以及碰撞平面参考点，构建碰撞平面约束因子；其中，所述碰撞平面约束因子用于表征所述移动载体的中心到接触平面距离；

10、定位修正模块，用于将所述碰撞平面约束因子加入包括惯性运动约束因子和地图匹配约束因子的因子图中；以及，对所述因子图进行联合优化，以获得所述移动载体的修正位姿。

11、根据本申请第三个方面，提供了一种可读存储介质，其上存储有程序或指令，程序或指令被处理器执行时实现上述基于碰撞平面约束的位姿修正方法的步骤。

12、根据本申请第四个方面，提供了一种移动载体，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现上述基于碰撞平面约束的位姿修正方法的步骤。

13、借由上述技术方案，在移动载体与外部环境发生碰撞后，基于包含碰撞时刻前后各一段时间的时间窗内的冲击残差精准确定碰撞法向。再依据碰撞前后沿该法向的速度变化量并结合防护结构的外包络参数反演接触变形量，以将瞬时的动态冲击过程量化为静态的接触几何信息。基于碰撞法向、接触变形量、外包络参数及碰撞平面参考点构建出能够实时表征移动载体中心至接触平面距离的碰撞平面约束因子。将该因子与保障局部运动平滑性和高频连续性的惯性运动约束因子、以及保障全局绝对位置一致性的地图匹配约束因子共同接入因子图框架进行联合非线性优化，以在统一优化准则下融合异构约束并迭代求解修正位姿。从而能够在传统惯性导航和地图匹配失效或精度下降的碰撞瞬时提供强附加几何约束，有效抑制位姿漂移。同时还可以通过因子图框架将异构约束统一优化，既保留了惯性运动的高频连续性和地图匹配的全局校正能力，又引入了碰撞事件特有的瞬时接触几何信息，使得优化后的位姿在碰撞时刻及前后时间段内更加连续、准确，有助于提升移动载体在复杂动态环境中遭遇碰撞后的定位精度与状态估计可靠性。

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

1.一种基于碰撞平面约束的位姿修正方法，应用于外部包络有防护结构的移动载体，其特征在于，所述方法包括：

2.根据权利要求1所述的基于碰撞平面约束的位姿修正方法，其特征在于，基于所述碰撞法向、所述接触变形量、所述外包络参数以及碰撞平面参考点，构建碰撞平面约束因子，包括：

3.根据权利要求1所述的基于碰撞平面约束的位姿修正方法，其特征在于，所述方法还包括：

4.根据权利要求3所述的基于碰撞平面约束的位姿修正方法，其特征在于，所述基于所述冲击残差进行碰撞事件检测，包括：

5.根据权利要求4所述的基于碰撞平面约束的位姿修正方法，其特征在于，

6.根据权利要求1所述的基于碰撞平面约束的位姿修正方法，其特征在于，所述基于所述移动载体碰撞前后沿所述碰撞法向的速度变化量与所述防护结构的外包络参数，计算所述防护结构的接触变形量，具体包括：

7.根据权利要求1所述的基于碰撞平面约束的位姿修正方法，其特征在于，所述构建碰撞平面约束因子之前，所述方法还包括：

8.根据权利要求1所述的基于碰撞平面约束的位姿修正方法，其特征在于，所述方法还包括以下至少一项：

9.一种基于碰撞平面约束的位姿修正装置，应用于外部包络有防护结构的移动载体，其特征在于，所述装置包括：

10.一种移动载体，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至8中任一项所述的基于碰撞平面约束的位姿修正方法。