CN116619358A - 一种矿山自主探测机器人自适应定位优化与建图方法 - Google Patents

一种矿山自主探测机器人自适应定位优化与建图方法 Download PDF

Info

Publication number
CN116619358A
CN116619358A CN202310530126.9A CN202310530126A CN116619358A CN 116619358 A CN116619358 A CN 116619358A CN 202310530126 A CN202310530126 A CN 202310530126A CN 116619358 A CN116619358 A CN 116619358A
Authority
CN
China
Prior art keywords
robot
pose
map
data
real
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310530126.9A
Other languages
English (en)
Inventor
陈昌兵
刘英杰
齐庆杰
仙文豪
刘思昀
李丹
柴佳美
程会锋
马天放
王月
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
General Coal Research Institute Co Ltd
Original Assignee
General Coal Research Institute Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by General Coal Research Institute Co Ltd filed Critical General Coal Research Institute Co Ltd
Priority to CN202310530126.9A priority Critical patent/CN116619358A/zh
Publication of CN116619358A publication Critical patent/CN116619358A/zh
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3863—Structures of map data
    • G01C21/3867—Geometry of map features, e.g. shape points, polygons or for simplified maps
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/044—Recurrent networks, e.g. Hopfield networks
    • G06N3/0442—Recurrent networks, e.g. Hopfield networks characterised by memory or gating, e.g. long short-term memory [LSTM] or gated recurrent units [GRU]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/0464—Convolutional networks [CNN, ConvNet]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Computation (AREA)
  • Health & Medical Sciences (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Biomedical Technology (AREA)
  • Biophysics (AREA)
  • Computational Linguistics (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Health & Medical Sciences (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Geometry (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

本申请提出了一种矿山自主探测机器人自适应定位优化与建图方法，涉及机器人领域，该方法包括：搭载多种传感器，通过多源信息融合技术对各种探测手段的传感信息进行有效补充与校正，基于CNN‑LSTM的视觉光度自适应补偿解决矿山场景光照明暗变化的问题，基于RGBD相机和IMU实现机器人实时定位与建图功能，得到三维环境地图和机器人初始位姿，以激光雷达点云作为观察数据，以上述机器人位姿为初始值，进行自适应蒙特卡洛算法优化搜索当前观测数据下的机器人最优定位，从而实现矿山自主探测机器人探测过程中的高精定位和建图功能，提升机器人定位***的准确性、鲁棒性及稳定性，对煤矿井下巡检及应急救援工作的顺利开展具有重要意义。

Description

一种矿山自主探测机器人自适应定位优化与建图方法
技术领域
本申请涉及机器人领域，尤其涉及一种矿山自主探测机器人自适应定位优化与建图方法。
背景技术
救援机器人一直是国内外机器人领域研究的热点，随着智能化发展不断推行，应急救援机器人更是被列为重点攻关技术。为保证探测及救援工作顺利开展，机器人需要具备精准的感知能力和现场导航定位能力。煤矿井下，尤其是灾害发生后其环境往往是复杂的非结构化有限空间；灾后光照度极低，光线的变化更使得光照分布不均匀；潮湿、多粉尘环境、路面不平颠簸、通讯中断等对传感信息的获取造成了诸多阻碍，难以找到一种单一有效的探测方法。
救援机器人的探测方法从整体上分为有源探测和无源探测，其中基于单目视觉和双目视觉的视觉SLAM属于典型的无源探测技术，仅从摄像机采集的二维图像中就可以提取大量信息；而基于激光雷达的激光SLAM属于有源探测技术，即发射探测信号获取的环境局部信息累积到机器人世界坐标系中，测定目标到机器人之间的距离。近些年，随着技术的发展，多传感器融合SLAM逐渐成为该领域研究的热点，然而单一的探测信息受限颇多，纯视觉SLAM受天气和曝光的影响，且定位精度较差；RGB-D相机获取的图像受到天气、光照等因素的影响，无法在室外条件下使用，因而应用场景大大受限；激光SLAM存在获取信息的不足，体积过大，应用受限等问题，且目前的SLAM算法往往无法直接应用于矿山环境，无法应对光照度极低，光线的变化更使得光照分布不均匀；潮湿、多粉尘环境、路面不平颠簸、通讯中断等环境干扰，另外SLAM算法在实际应用过程中定位精度较差，并且无法实时更新与矫正，从而造成累计误差。
发明内容
针对上述问题，本申请第一方面提出一种矿山自主探测机器人自适应定位优化与建图方法，包括：
基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，其中，所述多源传感器包括高清相机、深度相机、激光雷达、IMU传感器与里程计传感器；
将高清相机连续探测的原始图像输入至CNN-LSTM深度学习模型，得到光度评估结果，并基于所述光度评估结果对所述原始图像进行光度自适应补偿，其中，所述CNN-LSTM深度学习模型基于矿山实际探测图像样本进行模型训练；
对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到所述机器人的实时位姿；
得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将所述三维稠密点云地图转化为八叉树地图和占据网格地图；
以所述占据网格地图为当前地图，以所述实时位姿为初始位姿，以激光雷达实时数据为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿。
可选的，所述基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，包括：
通过***时间戳，实现高清相机、深度相机、激光雷达、IMU传感器与里程计传感器数据间的时间同步；
基于ROS***中的相机标定工具包与标定工具，并结合标准棋盘标定板对高清相机、深度相机和激光雷达进行标定，建立图像数据、激光雷达数据和深度数据之间的空间对应关系，得到高清相机与深度相机捕获的二维平面数据与激光雷达捕获的三维点云数据间的转化矩阵；
基于不同姿态下的所述标准棋盘标定板，得到多个线性方程，对所述线性方程求解得到标定参数，其中，所述标定参数包括高清相机的内参、深度相机的内参、摄像机与激光雷达之间的外参以及深度相机和激光雷达之间的外参。
可选的，在所述IMU传感器程序中设定零点漂移消除机制，当机器人上电后，所述IMU传感器读取稳定的初始角速度值作为漂移值，并将所述漂移值作为偏置量实现数据的零点消除。
可选的，所述将高清相机连续探测的原始图像输入至CNN-LSTM深度学习模型，得到光度评估结果，并基于所述光度评估结果对所述原始图像进行光度自适应补偿，包括：
对于所述CNN-LSTM深度学习模型，其中，CNN模型为5层神经网络，包含4个卷积层和1个全连接层，LSTM模型为5层神经网络，包含3个LSTM层、1个全连接层和1个输出层，所述LSTM层设置的激活函数为tanh函数，所述全连接层设置的激活函数为Relu函数，所述输出层的输出量为1，所述全连接层与所述输出层间设置Dropout和L2正则化机制；
根据探测过程中所述高清相机的采集频率，将采集的连续三通道原始图像输入至CNN模型中，并进行特征提取得到一个一维的特征向量，连续获取20帧原始图像的特征向量组成所述LSTM模型的输入数据，其中，Masking层对不定长的所述输入数据进行处理，经过处理后将变长输入序列整合为LSTM模型中第一个LSTM层输入序列的特征长度；
通过LSTM模型对处理后的所述输入数据进行处理，得到光度评估结果，其中，所述光度评估结果为当前探测环境下的光度与矿山环境的标准光度的评估差值。
可选的，所述对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到所述机器人的实时位姿，包括：
对连续多幅图像采用ORBSLAM中的特征点提取算法进行ORB特征提取，得到ORB特征点，并通过灰度质心法计算出ORB特征点的特征方向，并BREIF描述子对ORB特征点进行描述；
基于所述ORB特征点的特征方向，将所有ORB特征点的方向向量旋转至x轴正方向，在任意两幅图像间计算旋转后ORB特征点的汉明距离，以汉明距离最小的匹配对作为特征点匹配结果，并根据预设阈值筛选匹配度较高的点减少误匹配，得到最终匹配结果，并基于所述最终匹配结果解算当前机器人运动的位姿；
对于机器人随时间变化的状态方程及观测方程Xk＝AkXk-1+BkUk+wk与Zk＝HkXk+vk，基于ORB特征点解算出的位姿对机器人测量值的进行估计，其中，ORB特征点解算出的位姿为：
其中，Xk为k时刻机器人状态的估计值，Uk表示k时刻给机器人施加的控制量，为IMU传感器得到的预积分结果，Ak是作用在k-1时刻的状态转移矩阵，Bk是作用在Uk上的控制矩阵，用来描述控制量对***状态的影响，wk为过程噪声，服从均值为0、协方差为Qk的独立多元正态分布，Zk为k时刻机器人状态的测量值，Hk为观测矩阵，用于描述实际状态空间到观测空间的映射，为基于ORB特征点解算出的位姿，Vk为观测噪声，服从均值为0、协方差为Rk的独立多元正态分布；
利用卡尔曼滤波算法进行运动估计，计算出***状态变量的最优估计值，并对后验误差协方差矩阵进行更新，得到IMU传感器与基于ORB特征点解算位姿的融合优化结果。
可选的，所述利用卡尔曼滤波算法进行运动估计，计算出***状态变量的最优估计值，并对后验误差协方差矩阵进行更新，得到IMU传感器与基于ORB特征点解算位姿的融合优化结果，包括：
在卡尔曼滤波算法的预测阶段，所述卡尔曼滤波算法的表达式如下：
其中，为根据k-1时刻的状态估计值得到k时刻的***先验预测，/>为k-1时刻机器人状态的最优估计，/>为k时刻机器人的预测值与实际值之间的估计误差，Qk为过程噪声wk的协方差矩阵；
在卡尔曼滤波算法的更新，所述卡尔曼滤波算法的表达式如下：
其中，Kk为卡尔曼增益，Rk为观测噪声vk的协方差矩阵，参量尔曼增益Kk用于描述k时刻机器人状态预测值与实测值间的关系；
综合先验预测测量值Zk和卡尔曼增益Kk，计算机器人状态变量的最优估计值并对后验误差协方差矩阵Pk进行更新，基于多次更新迭代，IMU传感器与基于ORB特征点解算位姿的融合优化结果。
可选的，所述得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将所述三维稠密点云地图转化为八叉树地图和占据网格地图，包括：
将每一帧深度图像数据投影到世界坐标系下得到每个像素点对应的世界坐标下的地图点，生成三维点云，坐标转换公式为：
其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从深度图像中获取的深度，cx为深度相机光轴坐标在x轴方向上的偏移量，cy为深度相机的光轴坐标y轴方向上的偏移量，fx为深度相机在x轴方向上的焦距，fy为深度相机在y轴方向上的焦距；
对每一个像素点生成其世界坐标下的地图点后得到三维环境模型的物理结构，并通过原始图像数据与深度图像间的匹配关系，为所述三维环境模型增加颜色和纹理信息，获得三维稠密点云地图；
对所述三维稠密点云地图进行体素滤波，在所述三维稠密点云地图中按设定大小划分三维体素栅格，每个体素栅格内的所有点都由该体素栅格内所有点的重心来显示，对所述三维稠密点云地图通过八叉树进行稀疏表达，1个稠密点云地图中点云对应1个节点，得到八叉树地图，其中，每个节点通过该节点被占据的概率来表达，1个节点被占据的概率y通过下式计算：
其中，x为该节点对应的点云从第一帧到当前帧图像样本中累积被观测到的次数，初值为0，若在一帧图像样本中被观测到一次，则次数加1，反之减1，重复上面的步骤，不断地对八叉树地图进行更新，得到实时更新后的八叉树地图；
对于点云数据xi(i＝1,2,…,m)的k个近邻距离矩阵，根据欧氏距离，在{x1,x2,...,xm}中找出与每个数据xi最相近的k个样本点，并将这k个样本点所表示的集合记为Nk(x)，在Nk(x中根据分类规则确定xi的类别yi，公式化为：
其中，n代表数据空间维度，K为类别yi的总数目，进而为数据xi(i＝1,2,…,m)建立k-MST结构；
计算每个类别yi中的数据的最大欧式距离和平均欧式距离，实现离散点数据的剔除，得到剔除离散点的八叉树地图，将剔除离散点的八叉树地图投影到XY平面生成占据网格地图。
可选的，为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿，包括：
以似然域模型表示激光雷达实时数据，公式化为：
式中，为t时刻激光点云中第k束扫描激光终点于世界坐标系下的坐标，(x,y,θ)为机器人在世界坐标系下的位姿坐标，ΔxL为传感器与机器人车体的相对安装信息，θk,sens表示第k束激光与激光雷达坐标系x轴正方向的夹角，zk t为t时刻激光点云第k束扫描激光距离反馈信息，(δθL,δyL,δθL)为激光雷达外参误差补偿量；
基于蒙特卡罗定位算法近似估计机器人位姿估计的后验置信度，并基于所述后验置信度获得机器人最优位姿，其中，所述后验置信度的表达式如下：
其中，xt为t时刻机器人估计位姿，分别是t时刻用于表征机器人实时姿态的第i个粒子信息和粒子权重，其中，粒子权重大小代表了该粒子姿态与机器人真实姿态的相关度。
可选的，所述基于蒙特卡罗定位算法近似估计机器人位姿估计的后验置信度，并基于所述后验置信度获得机器人最优位姿，包括：
根据所述初始位姿，在空间内创建粒子集Xt＝{xt 1,xt 2,xt 3,…,xt m},其中每个粒子互相独立，权重值均为1/m，用于表征机器人在真实环境中的可能姿态或位置；
基于上一时刻的粒子集Xt-1＝{xt-1 1,xt-1 2,xt-1 3,…,xt-1 m}和运动控制量ut，将里程计传感器的实时运动反馈量带入隐马尔可夫模型中，计算出Af时间段内的位姿增量，得到预测粒子集；
基于粒子集中各个粒子的姿态信息xt与当前时刻基于真实环境观测量zt，根据似然域观测模型计算预测粒子集中的粒子权重，获得更新粒子集Xt，其中粒子权重的更新表达式为：
式中，和/>分别为运动预测、环境感知更新后粒子xi t的预测位置置信分布与测量后验位姿置信分布，η为归一化系数，权重值ωi t越大，证明粒子与真实位姿的重合程度较高，机器人定位过程趋于收敛；
在环境感知更新后，剔除低权重的粒子，并复制高权重粒子，采用库尔贝克－莱布勒散度进行采样，计算基于粒子滤波近似概率分布和真实概率分布之间的距离改变粒子集规模，公式化为：
其中，ML为粒子集规模大小，ε为近似概率分布与真实分布之间的误差阈值，z1-δ为标准正态分布的上1-δ分位点，k为所有粒子落在直方图不同直方块的块数。
本申请第二方面提出一种矿山自主探测机器人自适应定位优化与建图***，包括：
运动载体模块，用于实现机器人的移动功能；
探测模块，用于基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，其中，所述多源传感器包括高清相机、深度相机、激光雷达、IMU传感器与里程计传感器；
光度自适应补偿模块，用于将连续探测的原始图像输入至CNN-LSTM模型中，得到光度评估结果，并基于所述光度评估结果对所述原始图像进行光度自适应补偿；
定位模块，用于对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到所述机器人的实时位姿；
建图模块，用于在得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将所述三维稠密点云地图转化为八叉树地图和占据网格地图；
位姿优化模块，用于以所述占据网格地图为当前地图，以所述实时位姿为初始位姿，以激光雷达实时数据为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿。
本申请的实施例提供的技术方案至少带来以下有益效果：
基于搭载的多种传感器，通过多源信息融合技术对各种探测手段的传感信息进行有效补充与校正，结合深度学习方法进行智能化特征提取、特征匹配、位姿估计、几何语义分析，基于CNN-LSTM的视觉光度自适应补偿解决矿山场景光照明暗变化的问题，基于RGBD相机和IMU实现机器人实时定位与建图功能，实现矿山自主探测机器人探测过程中的高精定位和建图功能，提升救援机器人SLAM技术的准确性、鲁棒性及稳定性，对煤矿井下巡检及应急救援工作的顺利开展具有重要意义。
本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1是根据本申请实施例示出的一种矿山自主探测机器人自适应定位优化与建图方法的流程图；
图2是根据本申请实施例示出的基于CNN-LSTM的光度智能评估模型的架构图；
图3是根据本申请实施例示出的基于深度学习的视觉光度自适应补偿效果对比图；
图4是根据本申请实施例示出ORB特征点的特征匹配结果图；
图5是根据本申请实施例示出机器人自主探测生成占据网格地图效果图；
图6是根据本申请实施例示出的一种矿山自主探测机器人自适应定位优化与建图***的框图。
具体实施方式
下面详细描述本申请的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。
图1是根据本申请实施例示出的一种矿山自主探测机器人自适应定位优化与建图方法，包括：
步骤101，基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，其中，多源传感器包括高清相机、深度相机、激光雷达、IMU传感器与里程计传感器。
本申请实施例中，通过***时间戳，实现高清相机、深度相机、激光雷达、IMU传感器与里程计传感器数据间的时间同步。
需要说明的是，由于IMU传感器中的三轴陀螺仪不可以避免的存在零点漂移的问题，因此本申请在程序中设定了零点漂移消除机制，当机器人上电后，***读取稳定的初始角速度值作为漂移值，并以此作为偏置量实现数据的零点消除。
本申请实施例中，基于ROS***中的相机标定工具包与标定工具，并结合标准棋盘标定板对高清相机、深度相机和激光雷达进行标定，建立图像数据、激光雷达数据和深度数据之间的空间对应关系，得到高清相机与深度相机捕获的二维平面数据与激光雷达捕获的三维点云数据间的转化矩阵。
一种可能的实施例中，利用ROS下的camera_calibration工具包和autoware的CalibrationTookit模块。
本申请中转化矩阵M如下所示：
最终，基于不同姿态下的标准棋盘标定板，得到多个线性方程，对线性方程求解得到标定参数，其中，标定参数包括高清相机的内参、深度相机的内参、摄像机与激光雷达之间的外参以及深度相机和激光雷达之间的外参，标定参数如表1所示。
表1
另外，需要说明的是，通过产品官方安装软件获取ros下数据读取格式并进行配置，从而发布ROS下的摄像头图像话题/camera/rgb/image_raw、深度图话题/camera/depth/image_raw、激光雷达话题/scan、里程计话题/odom、IMU话题/imu，在机器人多源传感***框架中，各传感器的驱动功能包读取传感信息，并安装固定的格式发布传感数据，通过数据帧头中的时间戳实现多源传感器间的时间同步，通过frame_id获取该数据发布的数据来源，根据各帧之间已建立好的tf坐标变换关系，从而实现了多源传感数据的获取、发布、使用、转化和数据关系建立。
步骤102，将高清相机连续探测的原始图像输入至CNN-LSTM深度学习模型，得到光度评估结果，并基于光度评估结果对原始图像进行光度自适应补偿，其中，CNN-LSTM深度学习模型基于矿山实际探测图像样本进行模型训练。
本申请实施例中，构建基于CNN-LSTM的矿山环境下的光度智能评估模型，定义模型训练集、测试集、算法优化器，迭代次数、初始学习率、损失函数、迭代终止条件等关键参量。将矿山自主探测机器人在探测过程中的实时图像数据作为数据集，同时将搭载光敏传感器探测到的光度值与矿山环境的标准光度值I-I0的差作为光度智能评估模型的输出。
具体的，对于图2所示的CNN-LSTM深度学习模型，CNN模型为5层神经网络，包含4个卷积层(Conv1，Conv2，Conv3，Conv4)和1个全连接层(FC)，此外还设置了一个残差层来保证训练效果的优化选择。在CNN模型中，每个卷积层的关键参数包括：卷积核数量(Nc),卷积核尺寸(FH*FW),步长(SL),填充量(Pn)，设置均如表2所示。
表2
LSTM也为5层神经网络，包含3个LSTM层(LSTM1，LSTM2，LSTM3)，1个全连接层(Dense)和1个输出层(Output)，各层的关键参数如表3所示。
表3
另外，本申请实施例中，LSTM层设置的激活函数为tanh函数，全连接层设置的激活函数为Relu函数，输出层的输出量为1，全连接层与输出层间设置Dropout和L2正则化机制。
如图2所示，一种可能的实施例中，本申请3个LSTM层的隐含层层数分别为128、256、256，全连接层的特征数量为256。
在对CNN-LSTM深度学习模型进行初步说明后，该模型的处理过程为：根据探测过程中高清相机的采集频率，将采集的连续三通道原始图像输入至CNN模型中，并进行特征提取得到一个一维的特征向量，连续获取20帧原始图像的特征向量组成LSTM模型的输入数据，其中，Masking层对不定长的输入数据进行处理，经过处理后将变长输入序列整合为LSTM模型中第一个LSTM层输入序列的特征长度，通过LSTM模型对处理后的输入数据进行处理，得到光度评估结果，其中，光度评估结果为当前探测环境下的光度与矿山环境的标准光度的评估差值。
一种可能的实施例中，对原始图像进行光度自适应补偿的图像如图3所示，其中左侧为原始图片，右侧为经过度自适应补偿后的图片。
步骤103，对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到机器人的实时位姿。
本申请实施例中，首先对连续多幅图像采用ORBSLAM中的特征点提取算法进行ORB特征提取，得到ORB特征点，并通过灰度质心法计算出ORB特征点的特征方向，并BREIF描述子对ORB特征点进行描述。
具体的，得到ORB特征点的步骤为：
1.选取像素点p，假设像素点的亮度为lp；
2.设置一个阈值T(比如lp的20％)；
3.以像素p为中心，选取半径为3的圆上的16个像素点；
4.假如选取的圆上，有连续的N个点的亮度大于lp+T或者小于lp+T，那么像素p可以被认为是特征点，其中，N通常取12即FAST-12，其他常用的N取值为9或者11，分别被称为FAST-9和FAST-11；
5.循环以上4步，对每个像素执行相同操作。
然后通过灰度质心法计算出特征方向，保证ORB特征点的旋转不变性，在一个小的图像块B中，定义图像块的矩为：
mpq＝∑x，y∈BxpyqI(x，y)q,p＝{0,1}),
通过矩可以找到图像块的质心：
连接图像块的几何中心O与质心C，得到一个向量OC，于是特征点的方向可以定义为：
θ＝arctan(m01/m10),
在公式中，I(x,y)表示像素坐标(x,y)处的灰度值，m10/m00是x加权的总量与总和相比显示处x在图像的那一列的时候像素值比较大，代表了图像像素在x方向上的偏重，即是质心的x坐标，同样的，m10/m00是图像像素在y方向上的偏重，即是质心的y坐标。
计算BREIF描述子，为了增加描述子的抗噪性，算法首先需要对图像进行高斯平滑处理，在一个特征点的领域Path内，通过一种选定的方式来挑选N个点对，然后通过对每个点对(p,q)比较这两点的亮度值，如果I(p)>I(q)则这个点对生成的二进制串中一个值为1，如果I(p)≤I(q)则对应二进制串中的值为0，其中，N取128、256或512，OpenCV默认为256。
随后，基于ORB特征点的特征方向，将所有ORB特征点的方向向量旋转至x轴正方向，在任意两幅图像间计算旋转后ORB特征点的汉明距离，以汉明距离最小的匹配对作为特征点匹配结果，并根据预设阈值筛选匹配度较高的点减少误匹配，得到最终匹配结果，并基于最终匹配结果解算当前机器人运动的位姿。
一种可能的实施例中，ORB特征点提取及特征匹配的结果如图4所示。
另外，对于机器人随时间变化的状态方程及观测方程Xk＝AkXk-1+BkUk+wk与Zk＝HkXk+vk，基于ORB特征点解算出的位姿对机器人测量值的进行估计，其中，ORB特征点解算出的位姿为：
/>
其中，Xk为k时刻机器人状态的估计值，Uk表示k时刻给机器人施加的控制量，为IMU传感器得到的预积分结果，Ak是作用在k-1时刻的状态转移矩阵，Bk是作用在Uk上的控制矩阵，用来描述控制量对***状态的影响，wk为过程噪声，服从均值为0、协方差为Qk的独立多元正态分布，Zk为k时刻机器人状态的测量值，Hk为观测矩阵，用于描述实际状态空间到观测空间的映射，为基于ORB特征点解算出的位姿，Vk为观测噪声，服从均值为0、协方差为Rk的独立多元正态分布。
具体的，卡尔曼滤波算法包括预测、更新两个阶段。
在本申请实施例中，在卡尔曼滤波算法的预测阶段，卡尔曼滤波算法的表达式如下：
其中，为根据k-1时刻的状态估计值得到k时刻的***先验预测，/>为k-1时刻机器人状态的最优估计，/>为k时刻机器人的预测值与实际值之间的估计误差，Qk为过程噪声wk的协方差矩阵；
在卡尔曼滤波算法的更新，卡尔曼滤波算法的表达式如下：
其中，Kk为卡尔曼增益，Rk为观测噪声vk的协方差矩阵，参量尔曼增益Kk用于描述k时刻机器人状态预测值与实测值间的关系；
综合先验预测测量值Zk和卡尔曼增益Kk，计算机器人状态变量的最优估计值并对后验误差协方差矩阵Pk进行更新，基于多次更新迭代，IMU传感器与基于ORB特征点解算位姿的融合优化结果。
步骤104，得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将三维稠密点云地图转化为八叉树地图和占据网格地图。
本申请实施例中，将每一帧深度图像数据投影到世界坐标系下得到每个像素点对应的世界坐标下的地图点，生成三维点云，坐标转换公式为：
其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从深度图像中获取的深度，cx为深度相机光轴坐标在x轴方向上的偏移量，cy为深度相机的光轴坐标y轴方向上的偏移量，fx为深度相机在x轴方向上的焦距，fy为深度相机在y轴方向上的焦距；
对每一个像素点生成其世界坐标下的地图点后得到三维环境模型的物理结构，并通过原始图像数据与深度图像间的匹配关系，为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图；
对三维稠密点云地图进行体素滤波，在三维稠密点云地图中按设定大小划分三维体素栅格，每个体素栅格内的所有点都由该体素栅格内所有点的重心来显示，对三维稠密点云地图通过八叉树进行稀疏表达，1个稠密点云地图中点云对应1个节点，得到八叉树地图，其中，每个节点通过该节点被占据的概率来表达，1个节点被占据的概率y通过下式计算：
其中，x为该节点对应的点云从第一帧到当前帧图像样本中累积被观测到的次数，初值为0，若在一帧图像样本中被观测到一次，则次数加1，反之减1，重复上面的步骤，不断地对八叉树地图进行更新，得到实时更新后的八叉树地图；
对于点云数据xi(i＝1,2,…,m)的k个近邻距离矩阵，根据欧氏距离，在{x1,x2,...,xm}中找出与每个数据xi最相近的k个样本点，并将这k个样本点所表示的集合记为Nk(x)，在Nk(x中根据分类规则确定xi的类别yi，公式化为：
其中，n代表数据空间维度，K为类别yi的总数目，进而为数据xi(i＝1,2,…,m)建立k-MST结构；
计算每个类别yi中的数据的最大欧式距离和平均欧式距离，实现离散点数据的剔除，得到剔除离散点的八叉树地图，将剔除离散点的八叉树地图投影到XY平面生成占据网格地图。
一种可能的实施例中，占据网格地图如图5所示。
步骤105，以占据网格地图为当前地图，以实时位姿为初始位姿，以激光雷达实时数据为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿。
本申请实施例中，为保证移动机器人作业定位可靠性、实时性，以似然域模型表示激光雷达实时数据，采用似然域模型作为机器人的观测模型，用于计算机器人后验位姿、粒子滤波粒子权重，公式化为：
式中，为t时刻激光点云中第k束扫描激光终点于世界坐标系下的坐标，(x,y,θ)为机器人在世界坐标系下的位姿坐标，ΔxL为传感器与机器人车体的相对安装信息，θk,sens表示第k束激光与激光雷达坐标系x轴正方向的夹角，zk t为t时刻激光点云第k束扫描激光距离反馈信息，(δθL,δyL,δθL)为激光雷达外参误差补偿量；
基于蒙特卡罗定位算法近似估计机器人位姿估计的后验置信度，并基于后验置信度获得机器人最优位姿，其中，后验置信度的表达式如下：
其中，xt为t时刻机器人估计位姿，分别是t时刻用于表征机器人实时姿态的第i个粒子信息和粒子权重，其中，粒子权重大小代表了该粒子姿态与机器人真实姿态的相关度。
本申请实施例中，对蒙特卡罗定位算法进行具体说明，蒙特卡罗定位算法是基于粒子滤波的一种概率定位方法，自适应蒙特卡洛优化算法求解机器人最优位姿的步骤如下：
初始化粒子集：根据初始位姿，在空间内创建粒子集Xt＝{xt 1,xt 2,xt 3,…,xt m},其中每个粒子互相独立，权重值均为1/m，用于表征机器人在真实环境中的可能姿态或位置；
运动预测：基于上一时刻的粒子集Xt-1＝{xt-1 1,xt-1 2,xt-1 3,…,xt-1 m}和运动控制量ut，将里程计传感器的实时运动反馈量带入隐马尔可夫模型中，计算出Af时间段内的位姿增量，得到预测粒子集；
环境感知更新：基于粒子集中各个粒子的姿态信息xt与当前时刻基于真实环境观测量zt，根据似然域观测模型计算预测粒子集中的粒子权重，获得更新粒子集Xt，其中粒子权重的更新表达式为：
式中，和/>分别为运动预测、环境感知更新后粒子xi t的预测位置置信分布与测量后验位姿置信分布，η为归一化系数，权重值ωi t越大，证明粒子与真实位姿的重合程度较高，机器人定位过程趋于收敛；
粒子重采样：在环境感知更新后，剔除低权重的粒子，并复制高权重粒子，采用库尔贝克－莱布勒散度进行采样，计算基于粒子滤波近似概率分布和真实概率分布之间的距离改变粒子集规模，公式化为：
其中，ML为粒子集规模大小，ε为近似概率分布与真实分布之间的误差阈值，z1-δ为标准正态分布的上1-δ分位点，k为所有粒子落在直方图不同直方块的块数。
重复执行上述运动预测、环境感知更新与粒子重采样的过程，完成可靠的位姿估计过程，实现自适应高精定位功能。
本申请采用的自适应蒙特卡洛定位算法可根据机器人不同定位状态改变粒子集大小，减少计算资源，保证机器人在线定位实时性。
本申请基于搭载的多种传感器，通过多源信息融合技术对各种探测手段的传感信息进行有效补充与校正，结合深度学习方法进行智能化特征提取、特征匹配、位姿估计、几何语义分析，基于CNN-LSTM的视觉光度自适应补偿解决矿山场景光照明暗变化的问题，基于RGBD相机和IMU实现机器人实时定位与建图功能，实现矿山自主探测机器人探测过程中的高精定位和建图功能，提升救援机器人SLAM技术的准确性、鲁棒性及稳定性，对煤矿井下巡检及应急救援工作的顺利开展具有重要意义。
图6是根据本申请实施例提出的一种矿山自主探测机器人自适应定位优化与建图***的框图，包括：
运动载体模块，用于实现机器人的移动功能；
探测模块，用于基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，其中，多源传感器包括高清相机、深度相机、激光雷达、IMU传感器与里程计传感器；
光度自适应补偿模块，用于将连续探测的原始图像输入至CNN-LSTM模型中，得到光度评估结果，并基于光度评估结果对原始图像进行光度自适应补偿；
定位模块，用于对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到机器人的实时位姿；
建图模块，用于在得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将三维稠密点云地图转化为八叉树地图和占据网格地图；
位姿优化模块，用于以占据网格地图为当前地图，以实时位姿为初始位姿，以激光雷达实时数据为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本发公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本公开公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本公开保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等，均应包含在本公开保护范围之内。

Claims (10)

1.一种矿山自主探测机器人自适应定位优化与建图方法，其特征在于，包括：
基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，其中，所述多源传感器包括高清相机、深度相机、激光雷达、IMU传感器与里程计传感器；
将高清相机连续探测的原始图像输入至CNN-LSTM深度学习模型，得到光度评估结果，并基于所述光度评估结果对所述原始图像进行光度自适应补偿，其中，所述CNN-LSTM深度学习模型基于矿山实际探测图像样本进行模型训练；
对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到所述机器人的实时位姿；
得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将所述三维稠密点云地图转化为八叉树地图和占据网格地图；
以所述占据网格地图为当前地图，以所述实时位姿为初始位姿，以激光雷达实时数据为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿。
2.根据权利要求1所述的方法，其特征在于，所述基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，包括：
通过***时间戳，实现高清相机、深度相机、激光雷达、IMU传感器与里程计传感器数据间的时间同步；
基于ROS***中的相机标定工具包与标定工具，并结合标准棋盘标定板对高清相机、深度相机和激光雷达进行标定，建立图像数据、激光雷达数据和深度数据之间的空间对应关系，得到高清相机与深度相机捕获的二维平面数据与激光雷达捕获的三维点云数据间的转化矩阵；
基于不同姿态下的所述标准棋盘标定板，得到多个线性方程，对所述线性方程求解得到标定参数，其中，所述标定参数包括高清相机的内参、深度相机的内参、摄像机与激光雷达之间的外参以及深度相机和激光雷达之间的外参。
3.根据权利要求2所述的方法，其特征在于，在所述IMU传感器程序中设定零点漂移消除机制，当机器人上电后，所述IMU传感器读取稳定的初始角速度值作为漂移值，并将所述漂移值作为偏置量实现数据的零点消除。
4.根据权利要求1所述的方法，其特征在于，所述将高清相机连续探测的原始图像输入至CNN-LSTM深度学习模型，得到光度评估结果，并基于所述光度评估结果对所述原始图像进行光度自适应补偿，包括：
对于所述CNN-LSTM深度学习模型，其中，CNN模型为5层神经网络，包含4个卷积层和1个全连接层，LSTM模型为5层神经网络，包含3个LSTM层、1个全连接层和1个输出层，所述LSTM层设置的激活函数为tanh函数，所述全连接层设置的激活函数为Relu函数，所述输出层的输出量为1，所述全连接层与所述输出层间设置Dropout和L2正则化机制；
根据探测过程中所述高清相机的采集频率，将采集的连续三通道原始图像输入至CNN模型中，并进行特征提取得到一个一维的特征向量，连续获取20帧原始图像的特征向量组成所述LSTM模型的输入数据，其中，Masking层对不定长的所述输入数据进行处理，经过处理后将变长输入序列整合为LSTM模型中第一个LSTM层输入序列的特征长度；
通过LSTM模型对处理后的所述输入数据进行处理，得到光度评估结果，其中，所述光度评估结果为当前探测环境下的光度与矿山环境的标准光度的评估差值。
5.根据权利要求4所述的方法，其特征在于，所述对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到所述机器人的实时位姿，包括：
对连续多幅图像采用ORBSLAM中的特征点提取算法进行ORB特征提取，得到ORB特征点，并通过灰度质心法计算出ORB特征点的特征方向，并BREIF描述子对ORB特征点进行描述；
基于所述ORB特征点的特征方向，将所有ORB特征点的方向向量旋转至x轴正方向，在任意两幅图像间计算旋转后ORB特征点的汉明距离，以汉明距离最小的匹配对作为特征点匹配结果，并根据预设阈值筛选匹配度较高的点减少误匹配，得到最终匹配结果，并基于所述最终匹配结果解算当前机器人运动的位姿；
对于机器人随时间变化的状态方程及观测方程Xk＝AkXk-1+BkUk+wk与Zk＝HkXk+vk，基于ORB特征点解算出的位姿对机器人测量值的进行估计，其中，ORB特征点解算出的位姿为：
其中，Xk为k时刻机器人状态的估计值，Uk表示k时刻给机器人施加的控制量，为IMU传感器得到的预积分结果，Ak是作用在k-1时刻的状态转移矩阵，Bk是作用在Uk上的控制矩阵，用来描述控制量对***状态的影响，wk为过程噪声，服从均值为0、协方差为Qk的独立多元正态分布，Zk为k时刻机器人状态的测量值，Hk为观测矩阵，用于描述实际状态空间到观测空间的映射，为基于ORB特征点解算出的位姿，Vk为观测噪声，服从均值为0、协方差为Rk的独立多元正态分布；
利用卡尔曼滤波算法进行运动估计，计算出***状态变量的最优估计值，并对后验误差协方差矩阵进行更新，得到IMU传感器与基于ORB特征点解算位姿的融合优化结果。
6.根据权利要求5所述的方法，其特征在于，所述利用卡尔曼滤波算法进行运动估计，计算出***状态变量的最优估计值，并对后验误差协方差矩阵进行更新，得到IMU传感器与基于ORB特征点解算位姿的融合优化结果，包括：
在卡尔曼滤波算法的预测阶段，所述卡尔曼滤波算法的表达式如下：
其中，为根据k-1时刻的状态估计值得到k时刻的***先验预测，/>为k-1时刻机器人状态的最优估计，/>为k时刻机器人的预测值与实际值之间的估计误差，Qk为过程噪声wk的协方差矩阵；
在卡尔曼滤波算法的更新，所述卡尔曼滤波算法的表达式如下：
其中，Kk为卡尔曼增益，Rk为观测噪声vk的协方差矩阵，参量尔曼增益Kk用于描述k时刻机器人状态预测值与实测值间的关系；
综合先验预测测量值Zk和卡尔曼增益Kk，计算机器人状态变量的最优估计值/>并对后验误差协方差矩阵Pk进行更新，基于多次更新迭代，IMU传感器与基于ORB特征点解算位姿的融合优化结果。
7.根据权利要求5所述的方法，其特征在于，所述得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将所述三维稠密点云地图转化为八叉树地图和占据网格地图，包括：
将每一帧深度图像数据投影到世界坐标系下得到每个像素点对应的世界坐标下的地图点，生成三维点云，坐标转换公式为：
其中，(u,v)为地图点在图像坐标系下的坐标，(x,y,z)为地图点在世界坐标系下的坐标，d从深度图像中获取的深度，cx为深度相机光轴坐标在x轴方向上的偏移量，cy为深度相机的光轴坐标y轴方向上的偏移量，fx为深度相机在x轴方向上的焦距，fy为深度相机在y轴方向上的焦距；
对每一个像素点生成其世界坐标下的地图点后得到三维环境模型的物理结构，并通过原始图像数据与深度图像间的匹配关系，为所述三维环境模型增加颜色和纹理信息，获得三维稠密点云地图；
对所述三维稠密点云地图进行体素滤波，在所述三维稠密点云地图中按设定大小划分三维体素栅格，每个体素栅格内的所有点都由该体素栅格内所有点的重心来显示，对所述三维稠密点云地图通过八叉树进行稀疏表达，1个稠密点云地图中点云对应1个节点，得到八叉树地图，其中，每个节点通过该节点被占据的概率来表达，1个节点被占据的概率y通过下式计算：
其中，x为该节点对应的点云从第一帧到当前帧图像样本中累积被观测到的次数，初值为0，若在一帧图像样本中被观测到一次，则次数加1，反之减1，重复上面的步骤，不断地对八叉树地图进行更新，得到实时更新后的八叉树地图；
对于点云数据xi(i＝1,2,…,m)的k个近邻距离矩阵，根据欧氏距离，在{x1,x2,...,xm}中找出与每个数据xi最相近的k个样本点，并将这k个样本点所表示的集合记为Nk(x)，在Nk(x中根据分类规则确定xi的类别yi，公式化为：
其中，n代表数据空间维度，K为类别yi的总数目，进而为数据xi(i＝1,2,…,m)建立k-MST结构；
计算每个类别yi中的数据的最大欧式距离和平均欧式距离，实现离散点数据的剔除，得到剔除离散点的八叉树地图，将剔除离散点的八叉树地图投影到XY平面生成占据网格地图。
8.根据权利要求7所述的方法，其特征在于，所述以所述占据网格地图为当前地图，以所述实时位姿为初始位姿，以激光雷达实时数据为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿，包括：
以似然域模型表示激光雷达实时数据，公式化为：
式中，为t时刻激光点云中第k束扫描激光终点于世界坐标系下的坐标，(x,y,θ)为机器人在世界坐标系下的位姿坐标，ΔxL为传感器与机器人车体的相对安装信息，θk,sens表示第k束激光与激光雷达坐标系x轴正方向的夹角，zk t为t时刻激光点云第k束扫描激光距离反馈信息，(δθL,δyL,δθL)为激光雷达外参误差补偿量；
基于蒙特卡罗定位算法近似估计机器人位姿估计的后验置信度，并基于所述后验置信度获得机器人最优位姿，其中，所述后验置信度的表达式如下：
其中，xt为t时刻机器人估计位姿，分别是t时刻用于表征机器人实时姿态的第i个粒子信息和粒子权重，其中，粒子权重大小代表了该粒子姿态与机器人真实姿态的相关度。
9.根据权利要求1所述的方法，其特征在于，所述基于蒙特卡罗定位算法近似估计机器人位姿估计的后验置信度，并基于所述后验置信度获得机器人最优位姿，包括：
根据所述初始位姿，在空间内创建粒子集Xt＝{xt 1,xt 2,xt 3,…,xt m},其中每个粒子互相独立，权重值均为1/m，用于表征机器人在真实环境中的可能姿态或位置；
基于上一时刻的粒子集Xt-1＝{xt-1 1,xt-1 2,xt-1 3,…,xt-1 m}和运动控制量ut，将里程计传感器的实时运动反馈量带入隐马尔可夫模型中，计算出Af时间段内的位姿增量，得到预测粒子集；
基于粒子集中各个粒子的姿态信息xt与当前时刻基于真实环境观测量zt，根据似然域观测模型计算预测粒子集中的粒子权重，获得更新粒子集Xt，其中粒子权重的更新表达式为：
式中，和/>分别为运动预测、环境感知更新后粒子xi t的预测位置置信分布与测量后验位姿置信分布，η为归一化系数，权重值ωi t越大，证明粒子与真实位姿的重合程度较高，机器人定位过程趋于收敛；
在环境感知更新后，剔除低权重的粒子，并复制高权重粒子，采用库尔贝克－莱布勒散度进行采样，计算基于粒子滤波近似概率分布和真实概率分布之间的距离改变粒子集规模，公式化为：
其中，ML为粒子集规模大小，ε为近似概率分布与真实分布之间的误差阈值，z1-δ为标准正态分布的上1-δ分位点，k为所有粒子落在直方图不同直方块的块数。
10.一种矿山自主探测机器人自适应定位优化与建图***，其特征在于，包括：
运动载体模块，用于实现机器人的移动功能；
探测模块，用于基于ROS***完成多源传感器的空间位姿标定与时间戳同步，并实时读取多源传感数据，其中，所述多源传感器包括高清相机、深度相机、激光雷达、IMU传感器与里程计传感器；
光度自适应补偿模块，用于将连续探测的原始图像输入至CNN-LSTM模型中，得到光度评估结果，并基于所述光度评估结果对所述原始图像进行光度自适应补偿；
定位模块，用于对进行自适应补偿后的连续原始图像中提取ORB特征点，解算当前机器人运动的位姿，并将机器人***运行期间IMU传感器获取的位移信息与解算位姿通过卡尔曼滤波进行融合与优化，得到所述机器人的实时位姿；
建图模块，用于在得到机器人实时位姿后，根据深度图像获取当前帧数据在世界坐标下的坐标，构建三维环境模型的物理结构，通过原始图像为三维环境模型增加颜色和纹理信息，获得三维稠密点云地图，将所述三维稠密点云地图转化为八叉树地图和占据网格地图；
位姿优化模块，用于以所述占据网格地图为当前地图，以所述实时位姿为初始位姿，以激光雷达实时数据为观测数据，通过自适应蒙特卡洛优化算法求解机器人最优位姿。
CN202310530126.9A 2023-05-11 2023-05-11 一种矿山自主探测机器人自适应定位优化与建图方法 Pending CN116619358A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310530126.9A CN116619358A (zh) 2023-05-11 2023-05-11 一种矿山自主探测机器人自适应定位优化与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310530126.9A CN116619358A (zh) 2023-05-11 2023-05-11 一种矿山自主探测机器人自适应定位优化与建图方法

Publications (1)

Publication Number Publication Date
CN116619358A true CN116619358A (zh) 2023-08-22

Family

ID=87620550

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310530126.9A Pending CN116619358A (zh) 2023-05-11 2023-05-11 一种矿山自主探测机器人自适应定位优化与建图方法

Country Status (1)

Country Link
CN (1) CN116619358A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117437290A (zh) * 2023-12-20 2024-01-23 深圳市森歌数据技术有限公司 一种多传感器融合的自然保护区无人机三维空间定位方法
CN117494538A (zh) * 2023-12-28 2024-02-02 哈尔滨工业大学（威海） 一种单光子激光雷达***观测噪声模型建立方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117437290A (zh) * 2023-12-20 2024-01-23 深圳市森歌数据技术有限公司 一种多传感器融合的自然保护区无人机三维空间定位方法
CN117437290B (zh) * 2023-12-20 2024-02-23 深圳市森歌数据技术有限公司 一种多传感器融合的自然保护区无人机三维空间定位方法
CN117494538A (zh) * 2023-12-28 2024-02-02 哈尔滨工业大学（威海） 一种单光子激光雷达***观测噪声模型建立方法
CN117494538B (zh) * 2023-12-28 2024-03-22 哈尔滨工业大学（威海） 一种单光子激光雷达***观测噪声模型建立方法

Similar Documents

Publication Publication Date Title
US11747823B2 (en) Monocular modes for autonomous platform guidance systems with auxiliary sensors
CN111798475B (zh) 一种基于点云深度学习的室内环境3d语义地图构建方法
US11953910B2 (en) Autonomous platform guidance systems with task planning and obstacle avoidance
US10192113B1 (en) Quadocular sensor design in autonomous platforms
US10496104B1 (en) Positional awareness with quadocular sensor in autonomous platforms
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
CN112505065B (zh) 一种实现室内无人机对大部件表面缺陷进行检测的方法
CN116619358A (zh) 一种矿山自主探测机器人自适应定位优化与建图方法
CN112184812B (zh) 提升无人机相机对AprilTag识别定位精度方法及定位方法和***
CN112734765A (zh) 基于实例分割与多传感器融合的移动机器人定位方法、***及介质
CN116222543B (zh) 用于机器人环境感知的多传感器融合地图构建方法及***
CN111340939A (zh) 一种室内三维语义地图构建方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN114708293A (zh) 基于深度学习点线特征和imu紧耦合的机器人运动估计方法
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
Yang et al. Laser beams-based localization methods for boom-type roadheader using underground camera non-uniform blur model
CN109785428B (zh) 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN114419259B (zh) 一种基于物理模型成像仿真的视觉定位方法及***
CN112767482B (zh) 一种多传感器融合的室内外定位方法及***
CN114993293B (zh) 室内弱纹理环境下移动无人***同步定位与建图方法
Yao Resource-Constrained State Estimation with Multi-Modal Sensing.
CN116045965A (zh) 一种融合多传感器的环境地图构建方法
Liu Visual-Inertial State Estimation With Information Deficiency

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination