移动载体运动轨迹质量评价方法与装置与流程

本申请涉及定位，尤其是涉及到一种移动载体运动轨迹质量评价方法与装置。

背景技术：

1、slam（simultaneous localization and mapping，即时定位与地图构建）算法通常通过将估计运动轨迹与参考轨迹之间的误差进行比较，评价slam算法的定位与建图性能。在gnss（global navigation satellite system，全球导航卫星系统）拒止的地下停车场、地下管网、隧洞及工业厂房等大尺度环境中，卫星信号被遮挡，rtk-gnss（real-timekinematic global navigation satellite system，实时动态差分全球导航卫星系统）直接失效，此时只剩ins（inertial navigation system，惯性导航系统）单独工作，误差会快速累积，无法持续提供高精度的绝对位置参考；动作捕捉系统和激光跟踪仪又存在部署成本高、视场受限以及空间覆盖范围不足等问题。相关技术可以利用高精度静态先验地图、激光雷达点云和imu（inertial measurement unit，惯性测量单元）数据，通过点云地图匹配、imu预积分以及因子图联合优化生成连续运动轨迹，但是通常仅输出整体轨迹或附加理论协方差信息，缺少对轨迹中不同区段的质量进行逐帧检验、分级标注和原因分析的机制。当轨迹中存在观测退化、点云遮挡、匹配异常、运动突变或残差异常等低质量区段时，若将整段轨迹直接作为评测参考轨迹，可能导致低质量区段污染slam算法的评测结果，降低评测结论的准确性和可信度。

技术实现思路

1、有鉴于此，本申请提供了一种移动载体运动轨迹质量评价方法与装置，解决如何在gnss拒止的大尺度环境中对基于先验地图和因子图优化生成的运动轨迹进行逐帧质量评价的技术问题。

2、根据本申请的一个方面，提供了一种移动载体运动轨迹质量评价方法，包括：

3、获取移动载体的待评价运动轨迹；所述待评价运动轨迹通过对所述移动载体对应的因子图进行因子图优化得到，所述因子图包括与所述移动载体在运动过程中各关键帧时刻下的状态变量关联的因子；

4、将因子图优化收敛后的状态变量代入各所述因子的残差函数，计算所述因子的后验残差；

5、基于因子图优化收敛后的关键帧时刻对应的局部费歇尔信息矩阵，确定所述关键帧时刻对应的可观测指标；

6、基于所述后验残差和所述可观测指标，确定所述关键帧时刻对应的综合质量评分；

7、基于所述综合质量评分与独立控制点的实测位置，确定所述关键帧时刻对应的预测位置误差，所述独立控制点为所述移动载体运动的目标空间中位置固定的空间参考点；

8、基于所述预测位置误差确定所述关键帧时刻对应的质量等级，并基于所述质量等级对所述待评价运动轨迹进行区间划分，得到所述待评价运动轨迹的轨迹质量区间。

9、根据本申请的另一方面，提供了一种移动载体运动轨迹质量评价装置，包括：

10、获取模块，用于获取移动载体的待评价运动轨迹；所述待评价运动轨迹通过对所述移动载体对应的因子图进行因子图优化得到，所述因子图包括与所述移动载体在运动过程中各关键帧时刻下的状态变量关联的因子；

11、预测模块，用于将因子图优化收敛后的状态变量代入各所述因子的残差函数，计算所述因子的后验残差；以及，基于因子图优化收敛后的关键帧时刻对应的局部费歇尔信息矩阵，确定所述关键帧时刻对应的可观测指标；以及，基于所述后验残差和所述可观测指标，确定所述关键帧时刻对应的综合质量评分；以及，基于所述综合质量评分与独立控制点的实测位置，确定所述关键帧时刻对应的预测位置误差，所述独立控制点为所述移动载体运动的目标空间中位置固定的空间参考点；

12、评价模块，用于基于所述预测位置误差确定所述关键帧时刻对应的质量等级，并基于所述质量等级对所述待评价运动轨迹进行区间划分，得到所述待评价运动轨迹的轨迹质量区间。

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

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

15、借由上述技术方案，本申请提供了一种移动载体运动轨迹质量评价方法与装置，通过对因子图优化后的各因子后验残差进行归一化处理和卡方一致性检验，并结合局部费歇尔信息矩阵确定的可观测性指标，对运动轨迹进行逐帧质量评分和质量等级判定；同时，利用独立控制点的实测位置对质量评分进行外部校准，使质量等级与实际位置误差建立对应关系。由此，可以识别运动轨迹中的有效区间、可疑区间和无效区间，从而减少低质量轨迹区段对slam评测结果的影响，提高轨迹质量评价的准确性、可解释性和工程可用性。

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

1.一种移动载体运动轨迹质量评价方法，其特征在于，所述方法包括：

2.根据权利要求1所述的移动载体运动轨迹质量评价方法，其特征在于，所述可观测指标包括理论不确定度总量指标和条件数指标，所述基于因子图优化收敛后的关键帧时刻对应的局部费歇尔信息矩阵，确定所述关键帧时刻对应的可观测指标，具体包括：

3.根据权利要求2所述的移动载体运动轨迹质量评价方法，其特征在于，所述基于所述关键帧时刻对应的局部费歇尔信息矩阵的逆矩阵，确定所述关键帧时刻对应的理论不确定度总量指标，具体包括：

4.根据权利要求1所述的移动载体运动轨迹质量评价方法，其特征在于，所述可观测指标包括位置归一化退化度指标与姿态归一化退化度指标，所述基于因子图优化收敛后的关键帧时刻对应的局部费歇尔信息矩阵，确定所述关键帧时刻对应的可观测指标，具体包括：

5.根据权利要求1所述的移动载体运动轨迹质量评价方法，其特征在于，所述基于所述后验残差和所述可观测指标，确定所述关键帧时刻对应的综合质量评分的步骤之前，所述方法还包括：

6.根据权利要求5所述的移动载体运动轨迹质量评价方法，其特征在于，所述可观测指标包括理论不确定度总量指标、条件数指标、位置归一化退化度指标与姿态归一化退化度指标，所述基于所述后验残差和所述可观测指标，确定所述关键帧时刻对应的综合质量评分，具体包括：

7.根据权利要求1所述的移动载体运动轨迹质量评价方法，其特征在于，所述基于所述综合质量评分与独立控制点的实测位置，确定所述关键帧时刻对应的预测位置误差，具体包括：

8.根据权利要求1所述的移动载体运动轨迹质量评价方法，其特征在于，所述基于所述预测位置误差确定所述关键帧时刻对应的质量等级，具体包括：

9.根据权利要求8所述的移动载体运动轨迹质量评价方法，其特征在于，所述基于所述质量等级对所述待评价运动轨迹进行区间划分，得到所述待评价运动轨迹的轨迹质量区间，具体包括：

10.一种移动载体运动轨迹质量评价装置，其特征在于，所述装置包括：