一种基于多传感器决策融合的抗冲击扫描匹配方法及系统技术方案

本发明专利技术公开一种基于多传感器决策融合的抗冲击扫描匹配方法及系统，所述算法包括如下步骤：步骤1：通过融合车轮编码器的信息与陀螺仪的信息生成局部第一决策；步骤2：通过融合加速度计的信息与陀螺仪的信息生成局部第二决策；步骤3：根据决策融合规则从局部第一决策和局部第二决策中选取其一作为全局决策；步骤4：将全局决策作为扫描匹配的初始位姿，通过以初始位姿为中心的搜索窗口进行扫描匹配，以获取地图被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配。本发明专利技术通过融合决策提供良好的初始位姿估计，使得扫描匹配算法具有抗冲击性，提高扫描匹配算法的鲁棒性和准确性。提高扫描匹配算法的鲁棒性和准确性。提高扫描匹配算法的鲁棒性和准确性。

【技术实现步骤摘要】
一种基于多传感器决策融合的抗冲击扫描匹配方法及系统


[0001]本专利技术涉及机器人控制算法领域，具体涉及一种基于多传感器决策融合的抗冲击扫描匹配方法及系统。

技术介绍

[0002]移动机器人在室内环境中成功的路径规划与导航需要对当前机器人位置的可靠估计。实现这种可靠性的基本思想之一是融合来自多个传感器的信息，这能提高机器人定位的鲁棒性与准确性。目前最流行的移动机器人定位方案往往将激光雷达，车轮编码器以及惯性传感器相结合。然而，在复杂的动态环境下，机器人极易受到横向冲击。当机器人在低摩擦地面上移动时，若机器人轮胎附着力较低，将极易发生侧滑。机器人侧滑一般是指机器人车轮发生横向移动的现象。该现象可被视为机器人的绑架问题，即：机器人在没有先验知识或传感器无感知的情况下被瞬间移动到其他位置，车轮编码器无法感知冲击造成的横向位移，将会为扫描匹配算法提供不准确的先验知识，进而影响扫描匹配结果。

技术实现思路

[0003]为解决上述问题，本专利技术提供了一种基于多传感器决策融合的抗冲击扫描匹配方法及系统，通过融合决策实现更良好的初始位姿估计，使得扫描匹配算法具有抗冲击性，基于扫描匹配算法提高机器人的定位精准度。本专利技术的具体技术方案如下：
[0004]一种基于多传感器决策融合的抗冲击扫描匹配方法，该算法包括如下步骤：步骤1：通过融合车轮编码器的信息与陀螺仪的信息生成局部第一决策；步骤2：通过融合加速度计的信息与陀螺仪的信息生成局部第二决策；步骤3：根据决策融合规则从局部第一决策和局部第二决策中选取其一作为全局决策；步骤4：将全局决策作为扫描匹配的初始位姿，结合搜索窗口进行扫描匹配，获取占用栅格地图被激光点云占用概率最大的位姿作为最终位姿，将全局决策更新为所述最终位姿，完成全局决策的扫描匹配。
[0005]与现有技术相比，本专利技术基于多个传感器的测量结果融合生成两个局部决策，以提高机器人定位的鲁棒性和准确性，通过决策融合规则选取出其中一个决策作为全局决策，为扫描匹配算法提供初始位姿进行扫描匹配，使得扫描匹配算法具有抗冲击性，避免在机器人受到横向冲击时出现扫描匹配不准确的情况。相较于传统的扫描匹配算法，本专利技术通过多传感器决策融合使得扫描匹配算法具备抗冲击性。
[0006]进一步地，所述基于多传感器决策融合的抗冲击扫描匹配方法在执行步骤1前还包括：步骤00：缓存原始数据，判断原始数据的缓存帧数是否已缓存两帧或两帧以上，若是，则进入步骤01；步骤01：对缓存的原始数据进行预处理，获取原始数据的预处理结果；步骤02：判断车轮编码器测量值是否发生变化，若否，则进入步骤03，若是，则确定机器人处于非静止状态，进入步骤05；步骤03：判断滑动窗口内加速度测量值的标准差是否小于预设加速度标准差阈值，若是，则确定机器人处于静止状态，进入步骤04，若否，则确定机器人处于非静止状态，进入步骤05；步骤04：将机器人处于静止状态时先后连续缓存的全部加速度测量
值记录为先验知识；步骤05：机器人处于非静止状态，连续缓存的两帧之间加速度测量值的偏置b的更新满足b
k+1
＝b
k
，其中，k是原始数据的缓存帧的序号；其中，原始数据包括车轮编码器测量值、陀螺仪测量值、加速度测量值和各个传感器测量值的每一帧所对应的采集时刻；所述对缓存的原始数据进行预处理包括：计算先后连续缓存的两帧车轮编码器测量值差值、计算连续两帧机器人的姿态变化Δθ、计算先后连续缓存的两帧传感器测量值的采集时刻差值Δt、计算滑动窗口内加速度测量值的均值和标准差、将传感器测量值按照采集时刻先后顺序依次存储，以生成传感器测量值序列；将连续两帧机器人的姿态变化Δθ、先后连续缓存的两帧传感器测量值的采集时刻差值Δt、滑动窗口内加速度测量值的均值和标准差以及传感器测量值序列作为所述原始数据的预处理结果；其中，所述传感器测量值序列包括加速度测量值序列、陀螺仪测量值序列和车轮编码器测量值序列；所述滑动窗口内加速度测量值是指基于滑动窗口技术，从加速度测量值序列中提取的最新缓存的预设帧数的加速度测量值。本专利技术通过对原始数据的预处理，基于原始数据的预处理结果判断机器人的运动状态，根据机器人的运动状态决定是否对传感器的偏置进行校正，以使得传感器测量值值更加精准，减少获取的局部决策或全局决策的误差。
[0007]进一步地，当检测到启动后的机器人重新处于静止状态时，将先验知识更新为机器人重新处于静止状态时先后缓存的全部连续时刻帧的加速度测量值。考虑到传感器具有温度敏感等特性，当启动后的机器人重新处于静止状态时，立即更新先验知识，以避免出现先验知识更新不及时导致机器人是否受到横向冲击的判断结果不准确而影响决策融合的情况。
[0008]进一步地，所述步骤1具体包括：步骤11：获取第k帧到第k+1帧的机器人的姿态变化Δθ1，获取第k+1帧的机器人姿态θ
(k+1)1
，θ
(k+1)1
＝θ
k1
+
△
θ1；步骤12：定义机器人在机器人行进平面上第k帧的第一坐标变换矩阵R
k1
；步骤13：基于机器人车轮直径、第k帧车轮编码器测量值与第k+1帧车轮编码器测量值的差值，获取机器人左轮弧长ΔL和机器人右轮弧长ΔR；步骤14：基于步骤13获取的机器人左轮弧长ΔL和机器人右轮弧长ΔR，获取机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1和机器人从第k帧到第k+1帧在Y轴方向相对位移ΔY1；步骤15：获取第k+1帧的机器人位置步骤15：获取第k+1帧的机器人位置其中，为第k帧的机器人位置；步骤16：将第k+1帧的机器人位姿信息(X
(k+1)1
,Y
(k+1)
,θ
(k+1)1
)作为局部第一决策生成；其中，所述机器人左轮弧长是指机器人左轮上的一点从第k帧到第k+1帧所转过的弧长；所述机器人右轮弧长是指机器人右轮上的一点从第k帧到第k+1帧所转过的弧长；帧数k是大于或等于1的整数。局部第一决策作为全局决策的选项之一，是基于车轮编码器和陀螺仪这两种内部感受型传感器感知机器人的位置和姿态变化进而生成的决策。
[0009]进一步地，所述步骤14具体包括：计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的和值的二分之一，将计算结果确认为机器人的中心从第k帧到第k+1帧的移动轨迹长度；当机器人从第k帧到第k+1帧之间转动的圆心角的角度小于或等于预设圆心角阈值时，机器人的中心从第k帧到第k+1帧的移动轨迹长度等于机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1；计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值；计算机器人左轮和机器人右轮之间的直线距离；机器人从第k帧到第k+1帧之间转动的圆心角等于所述机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值与所述机器人左轮和机器人右
轮之间的直线距离之比；根据勾股定理，机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1等于所述机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1与所述机器人从第本文档来自技高网...

【技术保护点】

【技术特征摘要】
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所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，当检测到启动后的机器人重新处于静止状态时，将先验知识更新为机器人重新处于静止状态时先后缓存的全部连续时刻帧的加速度测量值。4.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤1具体包括：步骤11：获取第k帧到第k+1帧的机器人的姿态变化Δθ1，获取第k+1帧的机器人姿态θ
(k+1)1
，θ
(k+1)1
＝θ
k1
+
△
θ1；
步骤12：定义机器人在机器人行进平面上第k帧的第一坐标变换矩阵R
k1
；步骤13：基于机器人车轮直径、第k帧车轮编码器测量值与第k+1帧车轮编码器测量值的差值，获取机器人左轮弧长ΔL和机器人右轮弧长ΔR；步骤14：基于步骤13获取的机器人左轮弧长ΔL和机器人右轮弧长ΔR，获取机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1和机器人从第k帧到第k+1帧在Y轴方向相对位移ΔY1；步骤15：获取第k+1帧的机器人位置步骤15：获取第k+1帧的机器人位置其中，为第k帧的机器人位置；步骤16：将第k+1帧的机器人位姿信息(X
(k+1)1
,Y
(k+1)1
,θ
(k+1)1
)作为局部第一决策生成；其中，所述机器人左轮弧长是指机器人左轮上的一点从第k帧到第k+1帧所转过的弧长；所述机器人右轮弧长是指机器人右轮上的一点从第k帧到第k+1帧所转过的弧长；帧数k是大于或等于1的整数。5.根据权利要求4所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤14具体包括：计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的和值的二分之一，将计算结果确认为机器人的中心从第k帧到第k+1帧的移动轨迹长度；当机器人从第k帧到第k+1帧之间转动的圆心角的角度小于或等于预设圆心角阈值时，机器人的中心从第k帧到第k+1帧的移动轨迹长度等于机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1；计算机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值；计算机器人左轮和机器人右轮之间的直线距离；机器人从第k帧到第k+1帧之间转动的圆心角等于所述机器人左轮弧长ΔL与机器人右轮弧长ΔR的差值的绝对值与所述机器人左轮和机器人右轮之间的直线距离之比；根据勾股定理，机器人从第k帧到第k+1帧在X轴方向相对位移ΔX1等于所述机器人从第k帧到第k+1帧在Y轴方向的相对位移ΔY1与所述机器人从第k帧到第k+1帧之间转动的二分之一的圆心角的正切值的乘积；其中，所述机器人的中心是指机器人的左轮与机器人的右轮的连线的中点。6.根据权利要求2所述的基于多传感器决策融合的抗冲击扫描匹配方法，其特征在于，所述步骤2具体包括：步骤21：获取第k帧到第k+1帧的机器人的姿态变化Δθ2，获取第k+1帧机器人的姿态θ
(k+1)2
，θ
(k+1)2
＝θ
k2
+
△
θ2；步骤22：定义机器人欧拉角依次为：相对于机器人本体坐标系，绕Z轴旋转为偏航角α，绕Y轴旋转为俯仰角β，绕X轴旋转为横滚角γ，根据机器人欧拉角，定义机器人在所运动的空间中第k帧的第二坐标变换矩阵R
k2
；步骤23：根据原始数据中第k帧加速度测量值a
k
、第k+1帧加速度测量值a
k+1
、第k帧加速度测量值偏置b
k
、第k+1帧加速度测量值偏置b
k+1
、第k帧的第二坐标变换矩阵R
k2
和第k+1帧的第二坐标变换矩阵R

【专利技术属性】
技术研发人员：王珂，肖刚军，包敏杰，周和文，孙明，
申请(专利权)人：珠海一微半导体股份有限公司，
类型：发明
国别省市：