[发明专利]移动机器人故障下同时FDD和SLAM的方法及系统

说明书

本发明公开一种移动机器人故障下同时FDD和SLAM的方法及系统。包括：获取t时刻机器人的姿态样本；根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据和t‑1时刻的环境地图估计，确定各姿态样本的权重；根据t时刻各姿态样本所属的状态模式和t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，选取估计概率最大的状态模式作为t时刻的状态模式估计；将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，目标姿态样本包括t时刻的状态模式估计对应的姿态样本；根据t时刻机器人环境点云数据以及位姿估计，确定t时刻的环境地图估计。本发明实现了移动机器人故障下同时进行故障诊断和并发定位与建图。

技术领域

本发明涉及机器人故障诊断与建图领域，特别是涉及一种移动机器人故障下同时FDD和SLAM的方法及系统。

背景技术

故障检测与诊断(Fault Detection and Diagnosis,FDD)包含检测与诊断两个过程，检测是确定系统是否发生故障，诊断则是确定发生故障的部件以及故障类型，即确定各部件的行为模式。并发定位与建图(Simutaneous Localization and Mapping，SLAM)是移动机器人自主导航的关键问题之一。当移动机器人的左轮编码器和/或右轮编码器发生故障时，如何进行并发定位与建图十分关键，但目前，尚未出现同时解决移动机器人故障诊断和并发定位与建图问题的方案。

发明内容

本发明的目的是提供一种移动机器人故障下同时FDD和SLAM的方法及系统。

为实现上述目的，本发明提供了如下方案：

一种移动机器人故障下同时FDD和SLAM的方法，包括：

获取t时刻机器人的姿态样本；其中，t时刻机器人的姿态样本在各状态模式上的分布概率由t时刻机器人发生各状态模式的经验概率确定，机器人t时刻发生各状态模式的经验概率根据机器人t-1时刻的状态模式估计以及状态转移概率确定；t时刻任一状态模式下的姿态样本分布遵循对应状态模式下机器人姿态的正态分布，所述正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述左右车轮线速度估计值由t时刻左右车轮的编码器测量线速度确定或由t时刻左右车轮的编码器测量线速度以及驱动速度确定，所述正态分布中的标准差由对应状态模式的经验标准差确定；所述状态模式包括正常模式、左轮编码器故障模式、右轮编码器故障模式以及左右轮编码器同时故障模式；

根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重；其中，所述环境点云数据为实际测量数据；

根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，并选取估计概率最大的状态模式作为t时刻的状态模式估计；

将目标姿态样本中后验概率最大的姿态样本作为t时刻机器人的位姿估计，所述目标姿态样本包括所述t时刻的状态模式估计对应的姿态样本；

根据t时刻机器人的环境点云数据以及t时刻机器人的位姿估计，确定t时刻的环境地图估计。

可选的，所述根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重，具体包括：

对t时刻任一所述姿态样本：

以所述姿态样本为初始位姿估计，利用迭代最近点方法确定t时刻环境点云数据与t-1时刻的环境地图估计在配准过程中的配准点对；

计算各所述配准点对中两点的距离；

确定所述配准点对中所述距离小于设定阈值的配准点对的比例，并将所述比例作为t时刻所述姿态样本的权重。

可选的，所述根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，具体包括：