CN111735445A - 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法 - Google Patents

融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法 Download PDF

Info

Publication number
CN111735445A
CN111735445A CN202010582033.7A CN202010582033A CN111735445A CN 111735445 A CN111735445 A CN 111735445A CN 202010582033 A CN202010582033 A CN 202010582033A CN 111735445 A CN111735445 A CN 111735445A
Authority
CN
China
Prior art keywords
image
camera
coordinate system
robot
monocular
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.)
Granted
Application number
CN202010582033.7A
Other languages
English (en)
Other versions
CN111735445B (zh
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.)
Coal Science Research Institute
China Coal Research Institute CCRI
Original Assignee
Coal Science Research Institute
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 Coal Science Research Institute filed Critical Coal Science Research Institute
Priority to CN202010582033.7A priority Critical patent/CN111735445B/zh
Publication of CN111735445A publication Critical patent/CN111735445A/zh
Application granted granted Critical
Publication of CN111735445B publication Critical patent/CN111735445B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Traffic Control Systems (AREA)
  • Navigation (AREA)
  • Manipulator (AREA)

Abstract

一种融合单目视觉与IMU的煤矿巷道巡检机器人系统及导航方法，属于机器视觉与机器人导航技术领域。该系统包括：单目相机、巡检机器人、机载惯性测量单元、机载处理器、照明灯；巡检机器人载体用于搭载机载处理器、照明灯等设备，构成导航系统的硬件部分；单目相机用于拍摄煤矿井下巷道的高清图片，构成系统的数据采集部分；所述的机载惯性测量单元融合单目相机，对齐后获得尺度信息；的照明灯用于巷道内部的照明，提高单目相机成像质量；机载处理器用于图像处理、计算巡检机器人的偏航角和机器人姿态解算。优点：采用融合视觉与惯性传感器的导航方案，具有更高的精度，结合了巷道等狭长空间的结构特点，通过交线检测，能够满足巡检机器人的实时性。

Description

融合单目视觉与IMU的煤矿巷道巡检机器人系统及导航方法
技术领域
本发明涉及机器人导航技术领域，特别是一种融合单目视觉与IMU的煤矿巷道巡检机器人系统及导航方法。
背景技术
煤矿井下巷道环境恶劣，多种事故灾害时有发生，煤矿安全机器人已经成为煤矿安全基础设施的一部分。
煤矿开采过程中会释放多种有害气体，当这些气体得不到及时的监测，会给生产带来巨大的安全隐患。巡检机器人可以通过携带最多6种气体检测传感器，对这些气体及时的有效的检测。巷道内空间狭小距离长，作为设备进出和人员通过的生命通道，如何减少铺设导航设备、根据巷道的实际情况和减少空间的占用，又能达到精确安全的导航要求已经成为煤矿巷道导航的研究热点。
传统的人工检测存在作业复杂、效率低、实时性差、主观意识和人员安全性等问题，随着矿井自动化程度的加快，最大限度降低检测员工的劳动强度，很多研究人员面对类似煤矿井下巷道的导航环境，提出了多种解决方案。其中，
专利(曹现刚,靳子浩,殷玉萍,等.井下巷道悬线巡检防爆机器人,2016.)在井下巷道顶部架设钢丝绳轨道，通过对机械机构上的设计，使其防爆机器人能够在钢丝绳轨道上以设定的速度行驶，通过机器人携带的传感器对巷道进行环境监测。
文献(田丰.煤矿探测机器人导航关键技术研究[D].中国矿业大学2014)采用三角形履带式的探测机器人，研究基于仲裁融合协调策略的导航方法。
文献(白云.煤矿蛇形探测机器人位姿控制方法研究[D].西安科技大学.2019)构建蛇形的机器人模型，定位采用卡尔曼滤波和深度学习结合的算法，通过建立蛇形机器人运动姿态数学模型，对蛇形机器人的俯仰角、偏航角和转角进行控制。
以上三种方法针对煤矿这一特殊场景，设计了不同的检测机器人，并针对检测机器人的不同结构模型进行了不同算法的导航控制。但是对于第一种方案，巷道狭长，轨道铺设困难，挤占了巷道的空间。其余两种不同形态的机器人很容易受到环境的干扰，诸如面对塌陷、遮挡等恶劣环境时，同时导航算法相对复杂，对硬件要求高。
发明内容
本发明的目的是要提供一种融合单目视觉与IMU的煤矿巷道巡检机器人系统及导航方法，解决导航中单目相机尺度的恢复、改善巷道有限光照对单目相机成像的影响、快速求解用于机器人导航的交线检测和偏航角计算的问题。
本发明的目的是这样实现的：本发明的融合单目视觉与IMU的煤矿井下巷道巡检包括用于巡检的机器人系统以及基于惯性测量单元(IMU)和单目视觉融合的机器人系统的导航方法。
用于巡检的机器人系统包括：单目相机、巡检机器人、机载惯性测量单元、机载处理器和照明灯；巡检机器人载体用于搭载机载处理器、照明灯等设备，构成导航系统的硬件部分；单目相机用于拍摄煤矿井下巷道的高清图片，构成系统的数据采集部分；机载惯性测量单元融合单目相机，对齐后获得尺度信息；照明灯用于巷道内部的照明，提高单目相机成像质量；机载处理器用于图像处理、计算巡检机器人的偏航角和机器人姿态解算。
融合单目视觉与IMU的煤矿巷道巡检机器人导航方法：首先将惯性测量单元与单目相机对齐，进行初始化操作，获得相机的真实尺度信息。然后通过单目相机拍摄的高清图片，经过一系列图像处理，获得巷道地平面与两侧墙壁的交线在图像中的交线方程，在图像中交线通过延长后会相交于一点。将两个方程联立可求得交点的图像坐标，查找距离交点最近的匹配特征点恢复此帧相机坐标系的三维坐标，通过光轴向量和所求的特征点向量求得巡检机器人的偏航角，最后根据求得偏航角进行机器人的航向控制。
具体步骤如下：
步骤1)单目相机标定内参；
步骤2)单目相机与惯性测量单元初始化，求解尺度信息等参数，将相机坐标系对齐到世界坐标系；
步骤3)根据单目相机拍摄的巷道高清图片，检测出巷道地平面与两侧墙壁的交线；
步骤4)结合步骤3)对检测的两条交线构建方程，求解巡检机器人在巷道中的偏航角；
步骤5)根据机器人的偏航角进行航向调节。
所述的步骤1)，具体步骤如下：
11)采用张氏标定法标定相机内参；
12)单目相机内参标定后，得到内参矩阵为
Figure BDA0002552650020000021
和相机的畸变向量V＝[k1 k2 k3 q1 q2]，其中fx、fy分别为单目相机在x、y方向上的焦距，(cx,cy)为像素坐标中相机光轴所对应的坐标，k1、k2、k3为相机的切向畸变系数，q1、q2为相机径向畸变系数。
所述的步骤2)，具体步骤如下：
21)单目相机与惯性测量单元构建坐标系方程：
Figure BDA0002552650020000022
Figure BDA0002552650020000023
其中，下标c、b分别表示相机坐标系和惯性测量单元坐标系，s表示尺度因子，
Figure BDA0002552650020000024
表示非米制单位轨迹，
Figure BDA0002552650020000025
为IMU坐标系到相机坐标系的旋转；所述的惯性测量单元英文表示为Inertial Measurement Unit，英文缩写为IMU；
22)利用步骤21)第一个公式估计外参数qbc：对k、k+1时刻,IMU旋转积分为
Figure BDA0002552650020000026
视觉测量为
Figure BDA0002552650020000027
满足公式
Figure BDA0002552650020000028
公式进一步可写成
Figure BDA0002552650020000029
其中[·]L、[·]R表示四元数的左右算子；
Figure BDA00025526500200000210
累计不同时刻的上述方程，加入鲁棒核权重，最后通过奇异值分解(Singular Value Decomposition，SVD)，所求解即为最小奇异向量；
23)利用21)中第一个公式求解陀螺仪偏置：外参qbc已经由22)求解出，根据公式
Figure BDA0002552650020000031
公式为最小二乘问题，对其求雅可比矩阵(Jacobi Matrix)，构建HX＝b求解；其中，B是所有图像帧集合，
Figure BDA0002552650020000032
Figure BDA0002552650020000033
表示通过相机求取的bk和bk+1的旋转的逆与陀螺仪求取的差异，
Figure BDA00025526500200000317
表示取旋转四元数的虚部；
24)初始化尺度、速度和重力：待估计的变量为
Figure BDA0002552650020000034
其中，
Figure BDA0002552650020000035
为k时刻IMU坐标系中的速度，
Figure BDA0002552650020000036
表示重力向量在相机初始帧中的表示，s为尺度因子；把世界坐标系换为相机坐标系c0，IMU的预积分为：
Figure BDA0002552650020000037
Figure BDA0002552650020000038
把21)中所述公式代入预积分，并且将要求的变量整理到方程的右边，对其进行最小二乘求解：
Figure BDA0002552650020000039
其中，
Figure BDA00025526500200000310
Figure BDA00025526500200000311
25)相机坐标系转换为世界坐标系：相机坐标系到世界坐标系的转换矩阵为
Figure BDA00025526500200000312
其中
Figure BDA00025526500200000313
把相对于初始相机坐标系下全部变量转换到世界坐标系，根据24)中求得的尺度因子恢复真实尺度。
所述的步骤3)，具体步骤如下：
31)将单目相机拍摄的巷道图片进行灰度化处理，根据公式gray(x,y)＝0.299*Red(x,y)+0.587*Green(x,y)+0.114*Blue(x,y)进行RGB到灰度图像的转换，其中(x,y)为原图的像素坐标；
32)将灰度化处理后的图像高斯滤波，剔除图像的噪点；调用OpenCV的高斯滤波函数GaussianBlur(image_gray,image_out,Size(5,5),0,0)，其中，image_gray为输入图像，image_out为输出图像，Size(5,5)表示高斯内核的大小；
33)采用Canny边缘检测算法，对图像进行边缘检测，获取二值图的轮廓信息；
34)将图像上像素点p(xi,yj)转化为归一化坐标,其中i，j为图像上的像素点位置；径向、切向畸变纠正后：
Figure BDA00025526500200000314
Figure BDA00025526500200000315
其中
Figure BDA00025526500200000316
将畸变处理后的点通过u＝fxxdistorted+cx v＝fyydistorted+cy投影到像素坐标系；
35)通过上述步骤将在图像中得到巷道地平面与两侧墙壁的两条交线提供导航信息，去除其他无用的环境信息；交线总是位于图像下方的梯形区域，把梯形区域作为感兴趣区域，进行掩膜操作，非ROI区域设置为黑色；所述的感兴趣区域英文表示为Region ofInterest，英文缩写为ROI；
36)采用Hough算法，检测边缘二值图中的直线。
所述的步骤4)，具体步骤如下：
41)根据36)Hough算法拟合交线后，得到交线对应的参数(ρ,θ)，ρ、θ的定义，把极坐标下的直线方程通过公式：
Figure BDA0002552650020000041
转化到笛卡尔坐标系下；令
Figure BDA0002552650020000042
则两条交线的方程为l1：
Figure BDA0002552650020000043
l2：
Figure BDA0002552650020000044
42)联立41)中直线l1和l2的方程：
Figure BDA0002552650020000045
设交点坐标为p′(xi,yi)，对应三维坐标P(xm,ym,zm)，在此相机坐标系下一个光轴的向量为Q(xn,yn,zn)，由于要求的是偏航角，计算只在XZ平面，所以此时Q的坐标取(0，1)，P的坐标为(xm,zm)，则偏航角
Figure BDA0002552650020000046
根据机器人实际巡检时，偏航角调整非常小，若
Figure BDA0002552650020000047
即
Figure BDA0002552650020000048
为所求偏航角。
所述的步骤42)，具体步骤如下：
421)若l1和l2直线方程的交点在图像之外，则根据l1和l2直线方程的斜率进行巡检机器人位姿的调整，直至交点重新出现在相机视野中；机器人在巷道中的状态，当Z轴偏向L1，会导致
Figure BDA0002552650020000049
此时顺时针并带有位移调整机器人航向，直至交点出现在图像中；当Z轴偏向L2，导致
Figure BDA00025526500200000410
逆时针并带有位移调整机器人航向；
422)结合421)两直线方程的交点在图像中，为了避免极限搜索、块匹配带来的复杂计算，若交点不属于与上一帧匹配的特征点，找寻距离交点最近的特征点作为恢复三维点的像素点，设特征点p″(xi,yj)满足条件为
Figure BDA00025526500200000411
423)由于外参R、t通过图像特征点匹配获得且尺度已知，通过n点透视方法(Perspective-n-Point，PnP)或者三角化，求得交点在相机坐标系下的坐标；所述的三角化英文表示为Triangulation，所述的n点透视方法英文表示为Perspective-n-Point，英文缩写为PnP。
所述的步骤5)，具体步骤如下：根据机器人的偏航角进行航向调节。
有益效果，由于采用了上述方案，系统由移动机器人、机载惯性测量单元(Inertial Measurement Unit，IMU)、单目相机和照明灯构成。单目相机与机载IMU以捷联式方式安装，采用松耦合的传感器融合方式进行初始化，获得导航中需要的尺度信息；在巡检机器人上均匀安装高亮度的照明灯，降低光照对单目相机的影响，通过单目相机拍摄的高清图片，采用视觉算法对图像中巷道两侧的两条交线进行检测，获得两条交线延长线的交点，解算出相机坐标系下交点的三维坐标，计算巡检机器人在巷道的偏航角，并反馈给系统进行机器人航向调整。
采用机载惯性测量单元(Inertial Measurement Unit，IMU)和单目相机所拍摄的巷道环境图像信息融合实现机器人的实时导航控制。导航系统通过计算机视觉技术解决了尺度恢复的问题，提高了导航的精度；避免了对巷道空间的占用，机动性强，能够适应巷道的极端路况，机载照明灯降低了巷道有限光照对单目相机成像的影响。
本发明无需在巷道内铺设轨道和导航装置，减少开发成本和巷道空间的占用；机器人能够克服巷道有限光照和地面障碍物对导航系统的影响，同时融合惯性测量单元(Inertial Measurement Unit，IMU)提高了导航的精度，算法简单可靠，能够满足巷道巡检的实时性要求，从而适用于矿井巷道中用于巡检作业的巡检机器人导航和避障。采用本发明的技术方案，解决了导航中单目相机尺度的恢复问题、改善巷道有限光照对单目相机成像的影响、快速求解用于机器人导航的交线检测和偏航角的计算问题。
优点：采用融合视觉与惯性传感器的导航方案，具有更高的精度，结合了巷道等狭长空间的结构特点，通过交线检测，能够满足巡检机器人的实时性。
附图说明
图1是本发明的导航方法示意图。
图2是本发明的极坐标下直线表示图。
图3是本发明的机器人状态图和在图像中的表示图。
图4是本发明的相机坐标系下求取偏航角图。
具体实施方式
下面结合附图对本发明作进一步描述，以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
实施例1：本发明的融合单目视觉与IMU的煤矿井下巷道巡检包括用于巡检的机器人系统以及基于惯性测量单元(IMU)和单目视觉融合的机器人系统的导航方法。
用于巡检的机器人系统包括：单目相机、巡检机器人、机载惯性测量单元、机载处理器和照明灯；巡检机器人载体用于搭载机载处理器、照明灯等设备，构成导航系统的硬件部分；单目相机用于拍摄煤矿井下巷道的高清图片，构成系统的数据采集部分；机载惯性测量单元融合单目相机，对齐后获得尺度信息；照明灯用于巷道内部的照明，提高单目相机成像质量；机载处理器用于图像处理、计算巡检机器人的偏航角和机器人姿态解算。
融合单目视觉与IMU的煤矿巷道巡检机器人导航方法：首先将惯性测量单元与单目相机对齐，进行初始化操作，获得相机的真实尺度信息。然后通过单目相机拍摄的高清图片，经过一系列图像处理，获得巷道地平面与两侧墙壁的交线在图像中的交线方程，在图像中交线通过延长后会相交于一点。将两个方程联立可求得交点的图像坐标，查找距离交点最近的匹配特征点恢复此帧相机坐标系的三维坐标，通过光轴向量和所求的特征点向量求得巡检机器人的偏航角，最后根据求得偏航角进行机器人的航向控制。
具体步骤如下：
步骤1)单目相机标定内参；
步骤2)单目相机与惯性测量单元初始化，求解尺度信息等参数，将相机坐标系对齐到世界坐标系；
步骤3)根据单目相机拍摄的巷道高清图片，检测出巷道地平面与两侧墙壁的交线；
步骤4)结合步骤3)对检测的两条交线构建方程，求解巡检机器人在巷道中的偏航角；
步骤5)根据机器人的偏航角进行航向调节。
所述的步骤1)，具体步骤如下：
11)采用张氏标定法标定相机内参；
12)单目相机内参标定后，得到内参矩阵为
Figure BDA0002552650020000061
和相机的畸变向量V＝[k1 k2 k3 q1 q2]，其中fx、fy分别为单目相机在x、y方向上的焦距，(cx,cy)为像素坐标中相机光轴所对应的坐标，k1、k2、k3为相机的切向畸变系数，q1、q2为相机径向畸变系数。
所述的步骤2)，具体步骤如下：
21)单目相机与惯性测量单元构建坐标系方程：
Figure BDA0002552650020000062
Figure BDA0002552650020000063
其中，下标c、b分别表示相机坐标系和惯性测量单元坐标系，s表示尺度因子，
Figure BDA0002552650020000064
表示非米制单位轨迹，
Figure BDA0002552650020000065
为IMU坐标系到相机坐标系的旋转；所述的惯性测量单元英文表示为Inertial Measurement Unit，英文缩写为IMU；
22)利用步骤21)第一个公式估计外参数qbc：对k、k+1时刻,IMU旋转积分为
Figure BDA0002552650020000066
视觉测量为
Figure BDA0002552650020000067
满足公式
Figure BDA0002552650020000068
公式进一步可写成
Figure BDA0002552650020000069
其中[·]L、[·]R表示四元数的左右算子；
Figure BDA00025526500200000610
累计不同时刻的上述方程，加入鲁棒核权重，最后通过奇异值分解(Singular Value Decomposition，SVD)，所求解即为最小奇异向量；
23)利用21)中第一个公式求解陀螺仪偏置：外参qbc已经由22)求解出，根据公式
Figure BDA00025526500200000611
公式为最小二乘问题，对其求雅可比矩阵(Jacobi Matrix)，构建HX＝b求解；其中，B是所有图像帧集合，
Figure BDA0002552650020000071
Figure BDA0002552650020000072
表示通过相机求取的bk和bk+1的旋转的逆与陀螺仪求取的差异，
Figure BDA00025526500200000716
表示取旋转四元数的虚部；
24)初始化尺度、速度和重力：待估计的变量为
Figure BDA0002552650020000073
其中，
Figure BDA0002552650020000074
为k时刻IMU坐标系中的速度，
Figure BDA00025526500200000715
表示重力向量在相机初始帧中的表示，s为尺度因子；把世界坐标系换为相机坐标系c0，IMU的预积分为：
Figure BDA0002552650020000075
Figure BDA0002552650020000076
把21)中所述公式代入预积分，并且将要求的变量整理到方程的右边，对其进行最小二乘求解：
Figure BDA0002552650020000077
其中，
Figure BDA0002552650020000078
Figure BDA0002552650020000079
25)相机坐标系转换为世界坐标系：相机坐标系到世界坐标系的转换矩阵为
Figure BDA00025526500200000710
其中
Figure BDA00025526500200000711
把相对于初始相机坐标系下全部变量转换到世界坐标系，根据24)中求得的尺度因子恢复真实尺度。
所述的步骤3)，具体步骤如下：
31)将单目相机拍摄的巷道图片进行灰度化处理，根据公式gray(x,y)＝0.299*Red(x,y)+0.587*Green(x,y)+0.114*Blue(x,y)进行RGB到灰度图像的转换，其中(x,y)为原图的像素坐标；
32)将灰度化处理后的图像高斯滤波，剔除图像的噪点；调用OpenCV的高斯滤波函数GaussianBlur(image_gray,image_out,Size(5,5),0,0)，其中，image_gray为输入图像，image_out为输出图像，Size(5,5)表示高斯内核的大小；
33)采用Canny边缘检测算法，对图像进行边缘检测，获取二值图的轮廓信息；
34)将图像上像素点p(xi,yj)转化为归一化坐标,其中i，j为图像上的像素点位置；径向、切向畸变纠正后：
Figure BDA00025526500200000712
Figure BDA00025526500200000713
其中
Figure BDA00025526500200000714
将畸变处理后的点通过u＝fxxdistorted+cxv＝fyydistorted+cy投影到像素坐标系；
35)通过上述步骤将在图像中得到巷道地平面与两侧墙壁的两条交线提供导航信息，去除其他无用的环境信息；交线总是位于图像下方的梯形区域，把梯形区域作为感兴趣区域，进行掩膜操作，非ROI区域设置为黑色；所述的感兴趣区域英文表示为Region ofInterest，英文缩写为ROI；
36)采用Hough算法，检测边缘二值图中的直线。
所述的步骤4)，具体步骤如下：
41)根据36)Hough算法拟合交线后，得到交线对应的参数(ρ,θ)，ρ、θ的定义，把极坐标下的直线方程通过公式：
Figure BDA0002552650020000081
化到笛卡尔坐标系下；令
Figure BDA0002552650020000082
则两条交线的方程为l1：
Figure BDA0002552650020000083
l2：
Figure BDA0002552650020000084
42)联立41)中直线l1和l2的方程：
Figure BDA0002552650020000085
设交点坐标为p′(xi,yi)，对应三维坐标P(xm,ym,zm)，在此相机坐标系下一个光轴的向量为Q(xn,yn,zn)，由于要求的是偏航角，计算只在XZ平面，所以此时Q的坐标取(0，1)，P的坐标为(xm,zm)，则偏航角
Figure BDA0002552650020000086
根据机器人实际巡检时，偏航角调整非常小，若
Figure BDA0002552650020000087
即
Figure BDA0002552650020000088
为所求偏航角。
所述的步骤42)，具体步骤如下：
421)若l1和l2直线方程的交点在图像之外，则根据l1和l2直线方程的斜率进行巡检机器人位姿的调整，直至交点重新出现在相机视野中；机器人在巷道中的状态，当Z轴偏向L1，会导致
Figure BDA0002552650020000089
此时顺时针并带有位移调整机器人航向，直至交点出现在图像中；当Z轴偏向L2，导致
Figure BDA00025526500200000810
逆时针并带有位移调整机器人航向；
422)结合421)两直线方程的交点在图像中，为了避免极限搜索、块匹配带来的复杂计算，若交点不属于与上一帧匹配的特征点，找寻距离交点最近的特征点作为恢复三维点的像素点，设特征点p″(xi,yj)满足条件为
Figure BDA00025526500200000811
423)由于外参R、t通过图像特征点匹配获得且尺度已知，通过n点透视方法(Perspective-n-Point，PnP)或者三角化，求得交点在相机坐标系下的坐标；所述的三角化英文表示为Triangulation，所述的n点透视方法英文表示为Perspective-n-Point，英文缩写为PnP。
所述的步骤5)，具体步骤如下：根据机器人的偏航角进行航向调节。

Claims (9)

1.一种融合单目视觉与IMU的煤矿巷道巡检机器人系统，其特征是：用于巡检的机器人系统包括：单目相机、巡检机器人、机载惯性测量单元、机载处理器和照明灯；巡检机器人载体用于搭载机载处理器、照明灯等设备，构成导航系统的硬件部分；单目相机用于拍摄煤矿井下巷道的高清图片，构成系统的数据采集部分；机载惯性测量单元融合单目相机，对齐后获得尺度信息；照明灯用于巷道内部的照明，提高单目相机成像质量；机载处理器用于图像处理、计算巡检机器人的偏航角和机器人姿态解算。
2.基于权利要求1所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是：首先将惯性测量单元与单目相机对齐，进行初始化操作，获得相机的真实尺度信息；然后通过单目相机拍摄的高清图片，经过一系列图像处理，获得巷道地平面与两侧墙壁的交线在图像中的交线方程，在图像中交线通过延长后会相交于一点；将两个方程联立可求得交点的图像坐标，查找距离交点最近的匹配特征点恢复此帧相机坐标系的三维坐标，通过光轴向量和所求的特征点向量求得巡检机器人的偏航角，最后根据求得偏航角进行机器人的航向控制。
3.根据权利要求2所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是：具体步骤如下：
步骤1)单目相机标定内参；
步骤2)单目相机与惯性测量单元初始化，求解尺度信息等参数，将相机坐标系对齐到世界坐标系；
步骤3)根据单目相机拍摄的巷道高清图片，检测出巷道地平面与两侧墙壁的交线；
步骤4)结合步骤3)对检测的两条交线构建方程，求解巡检机器人在巷道中的偏航角；
步骤5)根据机器人的偏航角进行航向调节。
4.根据权利要求3所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤1)，具体步骤如下：
11)采用张氏标定法标定相机内参；
12)单目相机内参标定后，得到内参矩阵为
Figure FDA0002552650010000011
和相机的畸变向量V＝[k1k2 k3 q1 q2]，其中fx、fy分别为单目相机在x、y方向上的焦距，(cx,cy)为像素坐标中相机光轴所对应的坐标，k1、k2、k3为相机的切向畸变系数，q1、q2为相机径向畸变系数。
5.根据权利要求3所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤2)，具体步骤如下：
21)单目相机与惯性测量单元构建坐标系方程：
Figure FDA0002552650010000021
Figure FDA0002552650010000022
其中，下标c、b分别表示相机坐标系和惯性测量单元坐标系，s表示尺度因子，
Figure FDA0002552650010000023
表示非米制单位轨迹，
Figure FDA0002552650010000024
为IMU坐标系到相机坐标系的旋转；所述的惯性测量单元英文表示为Inertial Measurement Unit，英文缩写为IMU；
22)利用步骤21)第一个公式估计外参数qbc：对k、k+1时刻,IMU旋转积分为
Figure FDA0002552650010000025
视觉测量为
Figure FDA0002552650010000026
满足公式
Figure FDA0002552650010000027
公式进一步可写成
Figure FDA0002552650010000028
其中[·]L、[·]R表示四元数的左右算子；
Figure FDA0002552650010000029
累计不同时刻的上述方程，加入鲁棒核权重，最后通过奇异值分解(Singular Value Decomposition，SVD)，所求解即为最小奇异向量；
23)利用21)中第一个公式求解陀螺仪偏置：外参qbc已经由22)求解出，根据公式
Figure FDA00025526500100000210
公式为最小二乘问题，对其求雅可比矩阵(JacobiMatrix)，构建HX＝b求解；其中，B是所有图像帧集合，
Figure FDA00025526500100000211
Figure FDA00025526500100000212
表示通过相机求取的bk和bk+1的旋转的逆与陀螺仪求取的差异，
Figure FDA00025526500100000213
表示取旋转四元数的虚部；
24)初始化尺度、速度和重力：待估计的变量为
Figure FDA00025526500100000214
其中，
Figure FDA00025526500100000215
为k时刻IMU坐标系中的速度，
Figure FDA00025526500100000216
表示重力向量在相机初始帧中的表示，s为尺度因子；把世界坐标系换为相机坐标系c0，IMU的预积分为：
Figure FDA00025526500100000217
Figure FDA00025526500100000218
把21)中所述公式代入预积分，并且将要求的变量整理到方程的右边，对其进行最小二乘求解：
Figure FDA00025526500100000219
其中，
Figure FDA00025526500100000220
Figure FDA00025526500100000221
25)相机坐标系转换为世界坐标系：相机坐标系到世界坐标系的转换矩阵为
Figure FDA00025526500100000222
其中
Figure FDA00025526500100000223
把相对于初始相机坐标系下全部变量转换到世界坐标系，根据24)中求得的尺度因子恢复真实尺度。
6.根据权利要求3所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤3)，具体步骤如下：
31)将单目相机拍摄的巷道图片进行灰度化处理，根据公式gray(x,y)＝0.299*Red(x,y)+0.587*Green(x,y)+0.114*Blue(x,y)进行RGB到灰度图像的转换，其中(x,y)为原图的像素坐标；
32)将灰度化处理后的图像高斯滤波，剔除图像的噪点；调用OpenCV的高斯滤波函数GaussianBlur(image_gray,image_out,Size(5,5),0,0)，其中，image_gray为输入图像，image_out为输出图像，Size(5,5)表示高斯内核的大小；
33)采用Canny边缘检测算法，对图像进行边缘检测，获取二值图的轮廓信息；
34)将图像上像素点p(xi,yj)转化为归一化坐标,其中i，j为图像上的像素点位置；径向、切向畸变纠正后：
Figure FDA0002552650010000031
Figure FDA0002552650010000032
其中
Figure FDA0002552650010000033
将畸变处理后的点通过u＝fxxdistorted+cx v＝fyydistorted+cy投影到像素坐标系；
35)通过上述步骤将在图像中得到巷道地平面与两侧墙壁的两条交线提供导航信息，去除其他无用的环境信息；交线总是位于图像下方的梯形区域，把梯形区域作为感兴趣区域，进行掩膜操作，非ROI区域设置为黑色；所述的感兴趣区域英文表示为Region ofInterest，英文缩写为ROI；
36)采用Hough算法，检测边缘二值图中的直线。
7.根据权利要求3所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤4)，具体步骤如下：
41)根据36)Hough算法拟合交线后，得到交线对应的参数(ρ,θ)，ρ、θ的定义，把极坐标下的直线方程通过公式：
Figure FDA0002552650010000034
转化到笛卡尔坐标系下；令
Figure FDA0002552650010000035
则两条交线的方程为l1：
Figure FDA0002552650010000036
l2：
Figure FDA0002552650010000037
42)联立41)中直线l1和l2的方程：
Figure FDA0002552650010000038
设交点坐标为p′(xi,yi)，对应三维坐标P(xm,ym,zm)，在此相机坐标系下一个光轴的向量为Q(xn,yn,zn)，由于要求的是偏航角，计算只在XZ平面，所以此时Q的坐标取(0，1)，P的坐标为(xm,zm)，则偏航角
Figure FDA0002552650010000039
根据机器人实际巡检时，偏航角调整非常小，若
Figure FDA00025526500100000310
即
Figure FDA00025526500100000311
为所求偏航角。
8.根据权利要求7所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤42)，具体步骤如下：
421)若l1和l2直线方程的交点在图像之外，则根据l1和l2直线方程的斜率进行巡检机器人位姿的调整，直至交点重新出现在相机视野中；机器人在巷道中的状态，当Z轴偏向L1，会导致
Figure FDA0002552650010000041
此时顺时针并带有位移调整机器人航向，直至交点出现在图像中；当Z轴偏向L2，导致
Figure FDA0002552650010000042
逆时针并带有位移调整机器人航向；
422)结合421)两直线方程的交点在图像中，为了避免极限搜索、块匹配带来的复杂计算，若交点不属于与上一帧匹配的特征点，找寻距离交点最近的特征点作为恢复三维点的像素点，设特征点p″(xi,yj)满足条件为
Figure FDA0002552650010000043
423)由于外参R、t通过图像特征点匹配获得且尺度已知，通过n点透视方法(Perspective-n-Point，PnP)或者三角化，求得交点在相机坐标系下的坐标；所述的三角化英文表示为Triangulation，所述的n点透视方法英文表示为Perspective-n-Point，英文缩写为PnP。
9.根据权利要求3所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤5)，具体步骤如下：根据机器人的偏航角进行航向调节。
CN202010582033.7A 2020-06-23 2020-06-23 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法 Active CN111735445B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010582033.7A CN111735445B (zh) 2020-06-23 2020-06-23 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010582033.7A CN111735445B (zh) 2020-06-23 2020-06-23 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法

Publications (2)

Publication Number Publication Date
CN111735445A true CN111735445A (zh) 2020-10-02
CN111735445B CN111735445B (zh) 2022-02-11

Family

ID=72650725

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010582033.7A Active CN111735445B (zh) 2020-06-23 2020-06-23 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法

Country Status (1)

Country Link
CN (1) CN111735445B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112288712A (zh) * 2020-10-28 2021-01-29 山东黄金矿业(莱州)有限公司三山岛金矿 一种基于实景建模的黄金矿山溜井入料可视化检测方法
CN113140040A (zh) * 2021-04-26 2021-07-20 北京天地玛珂电液控制系统有限公司 一种多传感器融合的煤矿井下空间定位及建图方法与装置
CN113298727A (zh) * 2021-05-17 2021-08-24 中国矿业大学 一种基于多标识线的井下辅助运输车辆导航系统与方法
CN113361539A (zh) * 2021-05-21 2021-09-07 煤炭科学技术研究院有限公司 一种井下巡检机器人的仪表读取方法、装置及电子设备
CN116518984A (zh) * 2023-07-05 2023-08-01 中国矿业大学 一种煤矿井下辅助运输机器人车路协同定位系统及方法
CN114333096B (zh) * 2021-04-09 2023-12-15 北京市燃气集团有限责任公司 一种巡检机器人
CN117308900A (zh) * 2023-11-30 2023-12-29 中国矿业大学 井下运输车移动测量系统、运载通行状态模拟与监测方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102288176A (zh) * 2011-07-07 2011-12-21 中国矿业大学(北京) 基于信息融合的煤矿救灾机器人导航系统及方法
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN107014380A (zh) * 2017-05-26 2017-08-04 西安科技大学 基于飞行器的视觉导航与惯性导航的组合导航方法
JP2017191022A (ja) * 2016-04-14 2017-10-19 有限会社ネットライズ 三次元点群データに実寸法を付与する方法とそれを用いた管路等の位置測定
CN109141433A (zh) * 2018-09-20 2019-01-04 江阴市雷奥机器人技术有限公司 一种机器人室内定位系统及定位方法
CN110262546A (zh) * 2019-06-18 2019-09-20 武汉大学 一种隧道智能无人机巡检系统及方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110411443A (zh) * 2019-06-05 2019-11-05 西京学院 一种采煤机摇臂惯性/视觉组合定姿装置和方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102288176A (zh) * 2011-07-07 2011-12-21 中国矿业大学(北京) 基于信息融合的煤矿救灾机器人导航系统及方法
JP2017191022A (ja) * 2016-04-14 2017-10-19 有限会社ネットライズ 三次元点群データに実寸法を付与する方法とそれを用いた管路等の位置測定
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN107014380A (zh) * 2017-05-26 2017-08-04 西安科技大学 基于飞行器的视觉导航与惯性导航的组合导航方法
CN109141433A (zh) * 2018-09-20 2019-01-04 江阴市雷奥机器人技术有限公司 一种机器人室内定位系统及定位方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110411443A (zh) * 2019-06-05 2019-11-05 西京学院 一种采煤机摇臂惯性/视觉组合定姿装置和方法
CN110262546A (zh) * 2019-06-18 2019-09-20 武汉大学 一种隧道智能无人机巡检系统及方法

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112288712A (zh) * 2020-10-28 2021-01-29 山东黄金矿业(莱州)有限公司三山岛金矿 一种基于实景建模的黄金矿山溜井入料可视化检测方法
CN112288712B (zh) * 2020-10-28 2022-07-22 山东黄金矿业(莱州)有限公司三山岛金矿 一种基于实景建模的黄金矿山溜井入料可视化检测方法
CN114333096B (zh) * 2021-04-09 2023-12-15 北京市燃气集团有限责任公司 一种巡检机器人
CN113140040A (zh) * 2021-04-26 2021-07-20 北京天地玛珂电液控制系统有限公司 一种多传感器融合的煤矿井下空间定位及建图方法与装置
CN113298727A (zh) * 2021-05-17 2021-08-24 中国矿业大学 一种基于多标识线的井下辅助运输车辆导航系统与方法
CN113298727B (zh) * 2021-05-17 2023-08-18 中国矿业大学 一种基于多标识线的井下辅助运输车辆导航系统与方法
CN113361539A (zh) * 2021-05-21 2021-09-07 煤炭科学技术研究院有限公司 一种井下巡检机器人的仪表读取方法、装置及电子设备
CN116518984A (zh) * 2023-07-05 2023-08-01 中国矿业大学 一种煤矿井下辅助运输机器人车路协同定位系统及方法
CN116518984B (zh) * 2023-07-05 2023-09-08 中国矿业大学 一种煤矿井下辅助运输机器人车路协同定位系统及方法
CN117308900A (zh) * 2023-11-30 2023-12-29 中国矿业大学 井下运输车移动测量系统、运载通行状态模拟与监测方法
CN117308900B (zh) * 2023-11-30 2024-02-09 中国矿业大学 井下运输车移动测量系统、运载通行状态模拟与监测方法

Also Published As

Publication number Publication date
CN111735445B (zh) 2022-02-11

Similar Documents

Publication Publication Date Title
CN111735445B (zh) 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法
CN109945858B (zh) 用于低速泊车驾驶场景的多传感融合定位方法
McGee et al. Obstacle detection for small autonomous aircraft using sky segmentation
CN105783913A (zh) 一种融合车载多传感器的slam装置及其控制方法
CN205426175U (zh) 一种融合车载多传感器的slam装置
CN104217441A (zh) 一种基于机器视觉的机械臂定位抓取方法
CN112101160B (zh) 一种面向自动驾驶场景的双目语义slam方法
CN113223161B (zh) 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法
Momeni-k et al. Height estimation from a single camera view
CN112233221B (zh) 基于即时定位与地图构建的三维地图重建系统及方法
WO2019144289A1 (en) Systems and methods for calibrating an optical system of a movable object
CN114519472B (zh) 一种基于三维模型的应急管理监测方法
CN112396656A (zh) 一种视觉与激光雷达融合的室外移动机器人位姿估计方法
Ma et al. Crlf: Automatic calibration and refinement based on line feature for lidar and camera in road scenes
CN112102403A (zh) 用于输电塔场景下的自主巡检无人机的高精度定位方法及其系统
CN116619358A (zh) 一种矿山自主探测机器人自适应定位优化与建图方法
CN112033400A (zh) 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及系统
CN117523461B (zh) 一种基于机载单目相机的运动目标跟踪与定位方法
Gökçe et al. Recognition of dynamic objects from UGVs using Interconnected Neuralnetwork-based Computer Vision system
Chen et al. Aerial robots on the way to underground: An experimental evaluation of VINS-mono on visual-inertial odometry camera
Roozing et al. Low-cost vision-based 6-DOF MAV localization using IR beacons
Gomez-Balderas et al. Vision-based autonomous hovering for a miniature quad-rotor
CN114782639A (zh) 一种基于多传感器融合的快速式差速潜伏式agv稠密三维重建方法
CN115147344A (zh) 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法
CN113673462A (zh) 一种基于车道线的物流agv定位方法

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
GR01 Patent grant
GR01 Patent grant