[发明专利]智能探测机器人同时定位与地图构建系统

权利要求书

1.智能探测机器人SLAM系统，其特征在于：

硬件包括移动载体UGV，传感器模块、中央处理器模块、数据处理模块以及无线通讯模块；

UGV是整个机器人移动基础，由四个履带式摆臂构成，通过中央处理器模块和电机驱动模块控制，至少实现前进、后退、转向；

传感器模块包括激光传感器，摄像头，里程计，IMU以及气体浓度检测器，激光传感器，里程计和IMU用于感知周围环境，为SLAM系统提供输入数据；摄像头以及气体浓度检测器则是监控现场环境信息；

数据处理模块负责将激光传感器，里程计和IMU的信息进行处理和融合，并将处理过后的数据发送给中央处理器模块进行定位与地图构建；

无线通讯模块负责向救援中心提供机器人当前环境信息，机器人状态以及检测气体浓度信息；

步骤如下：

(1)信息预处理：对激光扫描信息进行预处理，减少采样点数目，去除异常云，同时对里程计进行校准和校正；

(2)信息融合：提取移动机器人的里程计信息和IMU信息，并基于扩展卡尔曼滤波融合激光扫描信息，改进激光点云分布，估计机器人位置；

(3)划分栅格：统计每个栅格的点云分布，并利用双线性函数计算栅格内点的得分；

(4)扫描匹配：计算变换的雅克比矩阵以及海森矩阵，根据点云分布变化，将下一帧扫描数据投影到前一帧坐标系中。

2.智能探测机器人同时定位与地图构建方法，其特征在于，具体步骤如下：

(1)基于EKF的信息融合

在2D平面中，通过里程计得到机器人移动的距离，通过IMU得到机器人旋转的角度；矢量X_k描述了机器人在k时刻的系统状态：

X_k＝(x_k,y_k,θ_k)^T (1)

其中(x_k,y_k)是机器人在导航系统的位置坐标，θ_k是通过IMU得到的机器人旋转角度；系统的运动状态方程为：

其中是k时刻的先验估计，是k时刻机器人位置坐标的先验估计，是k时刻机器人角度的先验估计；是k-1时刻的后验估计；u_k-1是k-1时刻的控制量；w_k-1是服从正态分布的过程噪声；

经过一个采样周期，系统的状态方程为：

其中是机器人移动的距离；机器人旋转角度的变化；w_k～N(0,Q_k)，Q_k是k时刻服从正态分布的过程噪声的协方差矩阵；

激光获得的数据是环境和机器人的每个点之间的距离和角度，是离散的数据点，每一个数据点通过极坐标(r_i,α_i)表示，其中r_i是激光传感器的扫描半径，α_i是激光传感器的扫描角；将这些点的坐标转化为直角坐标为：

其中，(x_i,y_i)是导航坐标系中数据点的坐标；系统的观测方程为：

Z_k＝HX_k+μ_k (5)

其中和是激光传感器扫描的横坐标与纵坐标信息；和是通过里程计获得的横坐标与纵坐标位置信息；是机器人角度变化；H是观测方程的变换矩阵；μ_k是服从正态分布的测量噪声，μ_k～N(0,R_k)，R_k是在时间k上服从正态分布的测量噪声的协方差矩阵；

上述获得的状态方程是非线性系统，需要基于EKF方法进行线性化；通过公式(3)，在k时刻得到的雅克比矩阵为：

扩展卡尔曼时间更新过程为：

其中u_k-1是k-1时刻的测量噪声；是k时刻先验误差协方差矩阵，P_k-1是k-1时刻的协方差矩阵；Q_k-1是k-1时刻的过程噪声的协方差矩阵；

测量更新过程为：

其中，I为单位矩阵；K_k是k时刻卡尔曼增益；

通过时间更新过程和测量更新过程，准确地估计出机器人的姿态；设将ξ_k带入到接下来的扫描匹配中；

(2)扫描匹配

使用占据栅格地图表示周围物理的环境，利用双线性函数估计栅格被占用的概率并划分栅格地图；在连续地图中，设一个点记做P_m，其被占据的概率记为M(P_m)，则这个点的梯度为：

基于双线性插值算法，通过使用四个最接近P_m点的整数坐标P₀₀、P₀₁、P₁₀和P₁₁来近似表示点P_m:

其导数近似为：

为了在给定点上找到栅格占用概率M(S_i(ξ))的最小误差，定义目标函数为：

其中ξ_k是在k时刻由EKF信息融合得到系统状态，n为k时刻激光传感器的扫描点数；S_i(ξ_k)是扫描点s_i＝(s_i,x,s_i,y)^T世界坐标，表示为：

其中，是基于EKF的信息融合得出的在k时刻的后验估计值；利用ξ_k先验值去估计下一位姿变化Δξ，使所有激光点误差最小：

对M(S_i(ξ_k+Δξ_k))进行一节泰勒展开，得到：

对Δξ_k求取偏导，设为0得到最小化的值：

使用高斯-牛顿方程求取Δξ_k的最小值：

其中H_ai为海森矩阵：

由公式(14)得到：

通过公式(19)和(20)估计公式(18)中的Δξ_k最小值，最后更新变化ξ_k+Δξ_k→ξ_k，将新的信息投用到已知的栅格地图上。