动态阈值与分阶段预测的多传感器位姿融合方法、装置、设备及存储介质与流程

本技术涉及数据融合，尤其涉及动态阈值与分阶段预测的多传感器位姿融合方法、装置、设备及存储介质。

背景技术：

1、在机器人末端执行器位姿估计中，通常配置多路异源传感器（如编码器、激光测距仪、视觉系统等）进行测量，并通过加权融合方法将多路测量数据融合为统一的位姿估计结果，以提高位姿估计的精度和可靠性。然而，传感器在实际运行过程中可能出现偶发异常测量，例如因环境干扰、瞬时遮挡或传感器自身不稳定等因素导致测量值显著偏离真实值。若将这些异常测量数据纳入融合计算，会直接污染融合结果，导致位姿估计精度下降。现有的多传感器融合方法通常依赖于异常检测机制来识别并剔除异常传感器，但异常判定的准确率在不同工况下难以保证，当传感器噪声水平随运行状态变化时，原有的判定条件可能不再适用，容易出现将正常测量误判为异常、或将异常测量漏判为正常的现象，从而影响融合位姿的准确性和连续性。

2、因此，目前方案存在多传感器融合过程中，传感器偶发异常测量会污染融合结果，不同工况下异常判定准确率难以保证的技术问题。

技术实现思路

1、本技术的主要目的在于提供一种动态阈值与分阶段预测的多传感器位姿融合方法、装置、设备及存储介质，旨在解决多传感器融合过程中，传感器偶发异常测量会污染融合结果，不同工况下异常判定准确率难以保证的技术问题。

2、为实现上述目的，本技术提出一种动态阈值与分阶段预测的多传感器位姿融合方法，所述动态阈值与分阶段预测的多传感器位姿融合方法包括：

3、获取多路异源传感器的测量数据、先验精度信息以及已记录的历史融合结果；

4、根据所述历史融合结果进行分阶段预测，确定位姿预测值；

5、确定各传感器的所述测量数据与所述位姿预测值之间的位姿残差，得到残差统计特征；

6、基于各传感器的所述残差统计特征，自适应生成动态判异阈值，并根据所述动态判异阈值从所述多路异源传感器中确定有效传感器；

7、根据所述有效传感器对应的所述测量数据、所述先验精度信息以及所述位姿预测值中的至少一项，确定融合位姿，以通过所述融合位姿进行机器人执行器的位姿估计。

8、在一实施例中，所述基于各传感器的所述残差统计特征，自适应生成动态判异阈值，并根据所述动态判异阈值从所述多路异源传感器中确定有效传感器，包括：

9、获取历史残差样本和预设样本阈值；

10、基于所述历史残差样本、所述预设样本阈值以及所述残差统计特征中的多项，自适应生成动态判异阈值；

11、根据所述动态判异阈值从所述多路异源传感器中确定有效传感器。

12、在一实施例中，所述基于所述历史残差样本、所述预设样本阈值以及所述残差统计特征中的多项，自适应生成动态判异阈值，包括：

13、获取预设的固定判异阈值和倍数因子；

14、在所述历史残差样本小于所述预设样本阈值时，将所述固定判异阈值作为动态判异阈值；

15、在所述历史残差样本大于等于所述预设样本阈值时，基于所述残差统计特征与所述倍数因子，生成各传感器对应的动态判异阈值。

16、在一实施例中，所述根据所述动态判异阈值从所述多路异源传感器中确定有效传感器，包括：

17、根据各传感器的所述测量数据和所述位姿预测值，确定位姿残差；

18、将所述位姿残差大于等于所述动态判异阈值的传感器，判定为异常传感器；

19、将所述异常传感器从所述多路异源传感器中剔除，得到有效传感器。

20、在一实施例中，所述根据所述历史融合结果进行分阶段预测，确定位姿预测值，包括：

21、在所述历史融合结果的数量小于预设数量阈值时，将各传感器的所述测量数据的均值作为位姿预测值；

22、在所述历史融合结果的数量大于等于所述预设数量阈值时，将预设滑动窗口内所述历史融合结果的均值作为位姿预测值。

23、在一实施例中，所述确定各传感器的所述测量数据与所述位姿预测值之间的位姿残差，得到残差统计特征，包括：

24、计算各传感器的所述测量数据与所述位姿预测值的差值绝对值，将所述差值绝对值作为位姿残差；

25、基于各传感器在预设历史窗口的所述位姿残差，确定残差统计特征。

26、在一实施例中，所述根据所述有效传感器对应的所述测量数据、所述先验精度信息以及所述位姿预测值中的至少一项，确定融合位姿，包括：

27、在所述有效传感器的数量为预设值时，将所述位姿预测值作为融合位姿；

28、在所述有效传感器的数量不为预设值时，基于所述有效传感器对应的所述测量数据和所述先验精度信息，确定融合位姿。

29、此外，为实现上述目的，本技术还提出一种动态阈值与分阶段预测的多传感器位姿融合装置，所述动态阈值与分阶段预测的多传感器位姿融合装置包括：

30、数据获取模块，用于获取多路异源传感器的测量数据、先验精度信息以及已记录的历史融合结果；

31、位姿预测模块，用于根据所述历史融合结果进行分阶段预测，确定位姿预测值；

32、残差统计模块，用于确定各传感器的所述测量数据与所述位姿预测值之间的位姿残差，得到残差统计特征；

33、动态判异模块，用于基于各传感器的所述残差统计特征，自适应生成动态判异阈值，并根据所述动态判异阈值从所述多路异源传感器中确定有效传感器；

34、融合位姿模块，用于根据所述有效传感器对应的所述测量数据、所述先验精度信息以及所述位姿预测值中的至少一项，确定融合位姿，以通过所述融合位姿进行机器人执行器的位姿估计。

35、此外，为实现上述目的，本技术还提出一种动态阈值与分阶段预测的多传感器位姿融合设备，所述设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序配置为实现如上文所述的动态阈值与分阶段预测的多传感器位姿融合方法的步骤。

36、此外，为实现上述目的，本技术还提出一种存储介质，所述存储介质为计算机可读存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上文所述的动态阈值与分阶段预测的多传感器位姿融合方法的步骤。

37、此外，为实现上述目的，本技术还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，所述计算机程序被处理器执行时实现如上文所述的动态阈值与分阶段预测的多传感器位姿融合方法的步骤。

38、本技术提出的一个或多个技术方案，至少具有以下技术效果：

39、由于采用了分阶段预测得到位姿预测值，并基于各传感器的位姿残差获取残差统计特征，使得后续的异常判定能够以预测值为可靠基准、以残差统计特征反映传感器当前的噪声水平；进而基于各传感器的残差统计特征自适应生成动态判异阈值，使得判定门限能够随各传感器实时噪声水平自动调节，解决了现有方案中判定条件难以适应动态工况变化导致误判或漏判的问题；在此基础上，从多路传感器中筛选出有效传感器，并结合有效传感器的测量数据、先验精度信息以及位姿预测值确定融合位姿，使得融合结果在剔除异常数据的同时，能够充分利用有效测量信息和先验精度信息。与现有技术相比，本技术解决了多传感器融合过程中传感器偶发异常测量污染融合结果以及不同工况下异常判定准确率难以保证的问题，实现了融合位姿鲁棒性和连续性的提升。