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

权利要求书

1.一种移动机器人故障下同时FDD和SLAM的方法，其特征在于，包括：

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

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

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

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

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

2.根据权利要求1所述的移动机器人故障下同时FDD和SLAM的方法，其特征在于，所述根据t时刻机器人的姿态样本、t时刻机器人的环境点云数据以及t-1时刻机器人的环境地图估计，确定各姿态样本的权重，具体包括：

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

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

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

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

3.根据权利要求1所述的移动机器人故障下同时FDD和SLAM的方法，其特征在于，所述根据t时刻各姿态样本所属的状态模式以及t时刻各姿态样本的权重，计算t时刻发生各状态模式的估计概率，具体包括：

根据计算t时刻发生各状态模式的估计概率其中，S^k表示第k状态模式，s_t表示t时刻的状态模式，当t时刻的第i姿态样本属于状态模式S^k时，为1，否则为0，表示t时刻的第i姿态样本归一化后的权重，N表示t时刻姿态样本的数量。

4.根据权利要求1所述的移动机器人故障下同时FDD和SLAM的方法，其特征在于，在所述获取t时刻机器人的姿态样本之前，还包括：

遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的线速度正态分布，采样线速度样本；

遵循t时刻各状态模式发生的经验概率以及机器人各状态模式下的偏航率正态分布，采样偏航率样本；

根据所述线速度样本和所述偏航率样本确定t时刻机器人的姿态样本；

其中，所述机器人姿态的正态分布包括线速度正态分布和偏航率正态分布，所述线速度正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述线速度正态分布中的标准差由对应状态模式的经验标准差确定，所述偏航率正态分布中的均值由对应状态模式下的左右车轮线速度估计值确定，所述偏航率正态分布中的标准差由对应状态模式的经验标准差确定。