CN109341705A - 智能探测机器人同时定位与地图构建系统 - Google Patents

Abstract

智能探测机器人同时定位与地图构建系统属于智能机器人多传感器组合定位导航领域。通过硬件模块搭建，结合激光传感器、里程计和惯性测量单元测量信息(Inertial measurement unit,IMU)，形成一套完整的智能机器人SLAM系统，实现探测机器人自主定位与环境感知能力。本发明提出的SLAM系统首先对激光传感器、里程计和IMU信息进行预处理及校正。接着，基于扩展卡尔曼滤波进行多传感器信息融合，改善激光点云的分布变化，提高机器人定位精度。之后通过双线性函数计算栅格点云的概率大小，根据点云分布变化将点云信息投影到栅格地图中。相比于单一传感器的SLAM系统，本发明提出的SLAM系统提高了系统的稳定性和可靠性，保障了定位与地图构建的精确度。

Description

智能探测机器人同时定位与地图构建系统

技术领域

本发明设计了一种可以应用于突发性大气事故的智能探测机器人同时定位与地图构建(Simultaneous Localization and Mapping,SLAM)系统，通过结合激光传感器信息、里程计信息和惯性测量单元(Inertial measurement unit,IMU)，搭建硬件平台，形成一套基于信息融合的智能探测机器人SLAM系统。该系统提高了智能探测机器人定位精度和地图构建准确度，可以应用于室内环境或者灾难现场中。通过控制机器人移动，系统可以实时反映现场环境。智能探测机器人SLAM系统属于智能机器人传感器信息融合的定位导航领域。

背景技术

在公共安全领域中，大气安全关乎人类的生命安全和生活质量。近年来，我国突发性大气污染事故频繁发生，直接影响了社会安全、生态环境和经济的可持续发展，成为了当前环境污染控制和安全保障的重大议题。事故发生时，救援中心需要及时掌握事故现场信息，以便正确决策。但由于事故现场情况复杂，为避免救援人员的二次伤亡，需要能工作于事故现场的智能探测机器人系统。应用智能探测机器人系统可以及时获取事故现场环境信息和人员信息，并将该信息传递给救援指挥中心，辅助其迅速确定救援方案。

随着机器人技术的发展，自主移动机器人被用在越来越多的地方，比如家用机器人、工业机器人以及特种机器人等。感知环境模型和定位自身位置的能力也成为智能移动机器人重要能力之一。SLAM系统包括机器人估计自身运动的状态以及感知周围环境模型。环境模型是机器人所处环境的描述，它可以显示诸如地标位置和障碍物等信息。机器人的运动状态可以通过机器人自身位置和方向来描述。激光传感器被广泛的应用在SLAM系统中，它不易受到光线和天气变化的影响，具有较强的鲁棒性而且它们能够准确的反映自身与障碍物之间的距离。里程计可以准确的反映机器人移动的距离，IMU可以精确反映机器人旋转角度。研究基于信息融合的智能机器人SLAM系统，获取全面准确的信息，提高定位精度以及更准确地反映周围环境模型。相比于传统单一传感器的SLAM系统，基于信息融合的SLAM在稳定性、控制精度以及抗干扰能力方面都有相当优势。

本发明设计的智能探测机器人SLAM系统平台主要包括数据采集、数据传输以及智能控制。通过通信网络实现数据采集、传输以及控制信号的发布与执行，形成SLAM系统，提高了系统的稳定性和可靠性，保障了定位与地图构建的精确度。

发明内容

本发明主要是设计了一种智能探测机器人SLAM系统，该系统可以用于大气灾难现场，同时也可以用于室内环境的模型构建，在定位自身位置姿态的同时完成地图构建任务。

1.智能探测机器人硬件系统搭建：

智能探测机器人硬件主要包括移动载体(UGV)，传感器模块、中央处理器模块、数据处理模块以及无线通讯模块。主要功能如下：

UGV是整个机器人移动基础，由四个履带式摆臂构成，通过中央处理器模块和电机驱动模块控制，可以实现前进、后退、转向等运动动作，可以适应复杂的环境。

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

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

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

2.智能探测机器人SLAM系统设计

主要步骤如下：

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

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

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

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

本发明主要体现在：

(1)设计了一款可以用于大气事故现场的智能探测机器人系统，可以应用在突发性大气事故现场中。

(2)设计了一套基于EKF信息融合的SLAM系统。通过将激光传感器信息，里程计信息和IMU数据进行融合，通过改善点云的分布变化，提高机器人定位的精度，同时也增加了地图构建的准确度。

附图说明

图1是本发明智能探测机器人控制系统结构图。

图2是本发明基于EKF信息融合的SLAM系统结构图。

图3是基于EKF信息融合的SLAM算法流程图。

具体实施方式

本发明设计的智能探测机器人SLAM系统，提高了智能探测机器人定位精度和地图构建准确度，可以应用于室内环境或者灾难现场中。通过控制机器人移动，系统可以实时反映现场环境。

本发明设计的SLAM系统主要通过基于EKF信息融合提供准确的点云变化，将变化信息投影到栅格地图中，完成定位与地图构建任务。具体步骤如下：

(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，将新的信息投用到已知的栅格地图上。

Claims (2)

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，将新的信息投用到已知的栅格地图上。