基于时空动态置信度与运动学一致性的移动机器人鲁棒融合定位方法及系统

本发明属于移动机器人自主定位与导航，具体涉及一种基于时空动态置信度与运动学一致性的移动机器人鲁棒融合定位方法及系统。

背景技术：

1、在gps受限的室内复杂环境中，具备持续、准确且具备全局一致性的定位能力是移动机器人实现自主导航与调度的核心需求。

2、现有技术中，室内移动机器人定位方法主要包括：（1）视觉slam类方法，受光照变化影响大，在纹理缺失或动态干扰环境下鲁棒性差；（2）uwb或蓝牙定位方法，需额外布设大量基础设施，成本高且受多径效应干扰明显；（3）轮式里程计方法，存在随距离增加而累积的漂移问题；（4）传统多源融合方法（如扩展卡尔曼滤波）强依赖于观测噪声为零均值高斯分布的假设，难以应对非高斯特性的复杂干扰。

3、wifi指纹定位技术虽无需额外信标，但受多径效应与非视距传播干扰，rss信号易畸变，导致定位结果出现大尺度跳变。现有基于扩展卡尔曼滤波的融合方案在wifi定位出现拖尾离群点时易产生显著估计偏差。

4、因此，亟需一种能够实时感知环境干扰、识别wifi观测畸变并抑制里程计积分漂移的鲁棒融合定位技术方案。

技术实现思路

1、本发明提供一种基于时空动态置信度与运动学一致性的移动机器人鲁棒融合定位方法及系统，通过前端信号域的动态置信度评估与后端运动学一致性约束的深度耦合，实现复杂环境下平滑且全局一致的位姿估计。

2、本发明的技术方案：

3、本发明提供了一种基于时空动态置信度与运动学一致性的移动机器人鲁棒融合定位方法，包括：

4、采集作业环境的wifi接收信号强度，利用高斯过程回归构建wifi指纹定位模型，以获取机器人全局位置观测；

5、基于滑动时间窗实时监测接收信号强度序列的局部方差，计算反映环境干扰程度的时空动态置信度因子，并结合高斯过程回归的空间预测方差，输出测量协方差矩阵；

6、获取轮式里程计数据，通过重构状态转移雅可比矩阵并将航向角维度对应的偏导项强制清零，构建运动学一致性约束的状态预测模型；

7、引入基于马氏距离的动态协方差缩放机制，对所述测量协方差矩阵进行动态缩放，并利用所述状态预测模型及动态缩放后的测量协方差矩阵，构建鲁棒扩展卡尔曼滤波框架，通过识别并抑制wifi定位离群点实现全局一致的定位轨迹。

8、进一步地，所述采集作业环境的wifi接收信号强度，利用高斯过程回归构建wifi指纹定位模型，以获取机器人全局位置观测，具体包括：

9、采集包含空间坐标与多路访问节点信号强度的wifi训练数据集；

10、利用径向基核函数建立关于空间位置的非线性信号场高斯过程模型，以表征信号强度的先验期望值与空间方差；

11、将实时接收的wifi信号强度向量代入对数似然函数，通过极大化求解获取使观测似然函数值最大化的物理坐标，作为全局定位基准。

12、进一步地，所述基于滑动时间窗实时监测接收信号强度序列的局部方差，计算反映环境干扰程度的时空动态置信度因子，并结合高斯过程回归的空间预测方差，输出测量协方差矩阵，具体包括：

13、提取过去连续w个时刻的接收信号强度观测序列，计算滑窗内信号强度的统计方差，其中w为预设的大于1的整数；

14、构造时空动态置信度因子，量化当前空间的动态遮挡与信道波动程度；

15、结合高斯过程回归的空间预测方差与所述时空动态置信度因子，输出带有环境感知属性的测量协方差矩阵。

16、进一步地，所述获取轮式里程计数据，通过重构状态转移雅可比矩阵并将航向角维度对应的偏导项强制清零，构建运动学一致性约束的状态预测模型，具体包括：

17、获取轮式里程计提供的线速度与角速度；

18、配置相对于名义值增大的过程噪声协方差矩阵，以降低先验预测的理论置信度；

19、基于所述线速度、角速度及增大的过程噪声协方差矩阵进行非线性状态预测推算；

20、计算状态转移函数相对于上一时刻状态的雅可比矩阵，并将航向角维度对应的偏导项强制清零，以阻断航向积分误差链条。

21、进一步地，所述引入基于马氏距离的动态协方差缩放机制，对所述测量协方差矩阵进行动态缩放，并利用所述状态预测模型及动态缩放后的测量协方差矩阵，构建鲁棒扩展卡尔曼滤波框架，通过识别并抑制wifi定位离群点实现全局一致的定位轨迹，具体包括：

22、计算wifi定位观测值与预测状态之间的新息向量，并基于马氏距离评估观测值与先验分布的统计一致性；

23、利用卡方分布检验，根据观测向量的维度设定马氏距离的判定阈值，计算所述马氏距离的平方与所述阈值的比值，并取该比值与1中的最大值作为动态协方差缩放自适应因子；

24、根据所述自适应因子实时缩放所述测量协方差矩阵，基于缩放后的测量协方差矩阵计算卡尔曼增益，以识别并剔除wifi定位中的非高斯拖尾离群点。

25、本发明还提供了一种基于时空动态置信度与运动学一致性的移动机器人鲁棒融合定位系统，其特征在于，用于执行所述的方法，所述系统包括：

26、指纹定位模块，用于采集wifi信号并基于高斯过程回归模型输出具有全局参考意义的初始定位结果；

27、置信度评估模块，用于通过滑动窗口监测信号方差并生成自适应的时空动态置信度因子；

28、运动学约束模块，用于重构状态转移雅可比矩阵并实施航向角偏导项强制清零，以构建运动学一致性预测模型；

29、鲁棒融合模块，用于引入动态协方差缩放机制，基于所述时空动态置信度因子与所述运动学一致性预测模型，执行鲁棒扩展卡尔曼滤波，以实现里程计漂移与wifi跳变的双重抑制，输出平滑且全局一致的定位轨迹。

30、进一步地，所述运动学约束模块具体用于：配置相对于名义值增大的过程噪声协方差矩阵，基于轮式里程计数据进行非线性状态预测推算，计算状态转移雅可比矩阵并将航向角维度对应的偏导项强制清零，以阻断航向积分误差链条并降低先验预测的理论置信度。

31、进一步地，所述鲁棒融合模块具体用于执行权利要求5中计算wifi定位观测值与预测状态之间的新息向量、基于马氏距离评估一致性、计算马氏距离平方与阈值比值并取最大值作为自适应因子、基于缩放后的测量协方差矩阵计算卡尔曼增益的操作。

32、与现有技术相比，本发明具有以下有益效果：

33、（1）本发明通过构建基于高斯过程回归的wifi指纹场模型，利用其预测方差表征空间不确定性，相较于传统确定性匹配方法，能够提供更丰富的观测统计特性，为后续融合权重分配提供物理依据；

34、（2）本发明设计了前端rss信号特征方差监测与后端dcs机制的级联异常抑制体系。前端自适应降权与后端马氏距离检验双重护航，使系统在面临非高斯拖尾干扰时能迅速隔离异常离群点，显著提升了系统在环境骤变下的鲁棒性；

35、（3）本发明创新性地提出了基于运动学一致性的预测重构策略，通过在雅可比矩阵中截断不可靠航向角误差的传播链条，从数学逻辑上阻断了里程计长距离积分漂移的扩散，保障了机器人运动轨迹的物理连贯性。