[发明专利]一种基于多传感器决策融合的抗冲击扫描匹配方法及系统

权利要求书

1.一种基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，该算法包括如下步骤：

步骤1：通过融合车轮编码器的信息与陀螺仪的信息生成局部第一决策；

步骤2：通过融合加速度计的信息与陀螺仪的信息生成局部第二决策；

步骤3：从局部第一决策和局部第二决策中选取其一作为全局决策；

步骤4：将全局决策作为扫描匹配的初始位姿，结合搜索窗口进行扫描匹配，获取占用栅格地图被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配。

2.根据权利要求1所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述基于多传感器决策融合的抗冲击扫描匹配方法在执行步骤1前还包括：

步骤00：缓存原始数据，判断原始数据的缓存帧数是否已缓存两帧或两帧以上，若是，则进入步骤01；

步骤01：对缓存的原始数据进行预处理，获取原始数据的预处理结果；

步骤02：判断车轮编码器测量值是否发生变化，若否，则进入步骤03，若是，则确定机器人处于非静止状态，进入步骤05；

步骤03：判断滑动窗口内加速度测量值的标准差是否小于预设加速度标准差阈值，若是，则确定机器人处于静止状态，进入步骤04，若否，则确定机器人处于非静止状态，进入步骤05；

步骤04：将加速度测量值的偏置更新为滑动窗口内加速度测量值的均值，将机器人处于静止状态时先后连续缓存的全部加速度测量值记录为先验知识；

步骤05：机器人处于非静止状态，连续缓存的两帧之间加速度测量值的偏置b的更新满足b_k+1＝b_k，其中，k是原始数据的缓存帧的序号；

其中，原始数据包括车轮编码器测量值、陀螺仪测量值、加速度测量值和各个传感器测量值的每一帧所对应的采集时刻；所述对缓存的原始数据进行预处理包括：计算先后连续缓存的两帧车轮编码器测量值差值、计算连续两帧机器人的姿态变化Δθ、计算先后连续缓存的两帧传感器测量值的采集时刻差值Δt、计算滑动窗口内加速度测量值的均值和标准差、将传感器测量值按照采集时刻先后顺序依次存储，以生成传感器测量值序列；将连续两帧机器人的姿态变化Δθ、先后连续缓存的两帧传感器测量值的采集时刻差值Δt、滑动窗口内加速度测量值的均值和标准差、以及传感器测量值序列作为所述原始数据的预处理结果；其中，所述传感器测量值序列包括加速度测量值序列、陀螺仪测量值序列和车轮编码器测量值序列；所述滑动窗口内加速度测量值是指基于滑动窗口技术，从加速度测量值序列中提取的最新缓存的预设帧数的加速度测量值。

3.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，当检测到启动后的机器人重新处于静止状态时，将先验知识更新为机器人重新处于静止状态时先后缓存的全部连续时刻帧的加速度测量值。