[发明专利]复杂环境下多传感器的移动机器人slam建图方法和系统

权利要求书

1.一种复杂环境下多传感器的移动机器人slam建图方法，其特征是，包括以下步骤：

步骤S1，同时获取同一环境的激光雷达数据、深度数据以及超声数据；

步骤S2，从激光雷达数据中提取角度和距离构成的数值对，并将深度数据和超声数据也转换成角度和距离构成的数值对格式；

步骤S3，将激光雷达、深度和超声类型所对应的三组数值对进行时间同步，并分别进行卡尔曼滤波；

步骤S4，对三组数值对中对应同一时刻的三个数据，将两两不同类型之间的数据差值与预设阈值进行对比，若大于阈值则挑选出最大优先级类型的数值对输出作为最终的数值对，重复此过程直至完成整组数值对；

步骤S5，利用输出的最终一组数值对进行建图。

2.根据权利要求1所述的一种复杂环境下多传感器的移动机器人slam建图方法，其特征是，步骤S2中，采用多线程同时处理。

3.根据权利要求1所述的一种复杂环境下多传感器的移动机器人slam建图方法，其特征是，以激光雷达数据格式为标准对深度数据进行格式转换的具体步骤如下：

步骤S2.1，提取深度图水平中线上下设定值像素区域作为敏感区域；

步骤S2.2，对敏感区域中每列像素点取灰度最小值；

步骤S2.3，对敏感区域中水平范围内，取整数角度对应列上灰度最小值作为距离数据，加上对应角度值，转换为激光雷达数据格式的角度和距离数值对。

4.根据权利要求1所述的一种复杂环境下多传感器的移动机器人slam建图方法，其特征是，以激光雷达数据格式为标准对超声数据格式转换的具体过程如下：

以激光雷达的数值对中的角度，为超声数组中每一个距离数据添加角度数据，形成激光雷达格式的角度和距离数值对。

5.根据权利要求1所述的一种复杂环境下多传感器的移动机器人slam建图方法，其特征是，针对三组数值对进行卡尔曼滤波数据融合，具体计算步骤如下：

步骤S3.1，获取激光雷达、深度和超声类型所对应的三组数值对数据；

步骤S3.2，根据数据类型不同选择卡尔曼参数对卡尔曼滤波器模型初始化；

步骤S3.2.1，由式(1)和式(2)初始化卡尔曼滤波器模型的时间更新公式：

X(K|K-1)＝X(K-1|K-1) (1)

P(K|K-1)＝P(K-1|K-1)+Q (2)

其中式(1)中X(K|K-1)是K状态的预测值，X(K-1|K-1)是K-1状态的估计值；式(2)中P(K|K-1)是预测值的协方差，P(K-1|K-1)是K-1状态的估计值协方差，Q为测量过程的噪声的协防差可以理解为不确定度；

步骤S3.2.2，由式(3)更新计算卡尔曼权值：

K_g＝P(K|K-1)/(P(K|K-1)+R) (3)

其中K_g为卡尔曼权值，R为测量噪声的协方差；

步骤S3.2.3，由式(4)和式(5)更新计算当前数据最优估计值及估计值的协方差：

X(K|K)＝X(K|K-1)+K_g(y_k-X(K|K-1)) (4)

其中X(K|K)为K状态下的估计值，y_k为K状态下的测量值。

P(K|K)＝(1-K_g)P(K|K-1) (5)

其中P(K|K)为K状态估计值的协方差；

步骤S3.3，将下一个时刻的数值对，及更新后的最优估计值和不确定度进行步骤S3.2，直到当前批数据处理完成。