CN112033400A - 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及*** - Google Patents
一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及*** Download PDFInfo
- Publication number
- CN112033400A CN112033400A CN202010950320.9A CN202010950320A CN112033400A CN 112033400 A CN112033400 A CN 112033400A CN 202010950320 A CN202010950320 A CN 202010950320A CN 112033400 A CN112033400 A CN 112033400A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- camera
- imu
- inertial navigation
- mobile robot
- 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
Links
- 239000003245 coal Substances 0.000 title claims abstract description 49
- 238000000034 method Methods 0.000 title claims abstract description 30
- 230000000007 visual effect Effects 0.000 claims abstract description 21
- 238000012544 monitoring process Methods 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000005259 measurement Methods 0.000 claims description 23
- 230000005484 gravity Effects 0.000 claims description 21
- 230000001133 acceleration Effects 0.000 claims description 15
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 230000004927 fusion Effects 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 9
- 230000010354 integration Effects 0.000 claims description 8
- 238000005457 optimization Methods 0.000 claims description 8
- 238000013519 translation Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000005540 biological transmission Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 4
- 238000007499 fusion processing Methods 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000002401 inhibitory effect Effects 0.000 claims description 3
- 230000004044 response Effects 0.000 claims description 3
- YBJHBAHKTGYVGT-ZKWXMUAHSA-N (+)-Biotin Chemical compound N1C(=O)N[C@@H]2[C@H](CCCCC(=O)O)SC[C@@H]21 YBJHBAHKTGYVGT-ZKWXMUAHSA-N 0.000 claims description 2
- FEPMHVLSLDOMQC-UHFFFAOYSA-N virginiamycin-S1 Natural products CC1OC(=O)C(C=2C=CC=CC=2)NC(=O)C2CC(=O)CCN2C(=O)C(CC=2C=CC=CC=2)N(C)C(=O)C2CCCN2C(=O)C(CC)NC(=O)C1NC(=O)C1=NC=CC=C1O FEPMHVLSLDOMQC-UHFFFAOYSA-N 0.000 claims description 2
- 238000013507 mapping Methods 0.000 claims 1
- 230000007774 longterm Effects 0.000 abstract 1
- 230000006870 function Effects 0.000 description 12
- 239000007789 gas Substances 0.000 description 6
- 238000007689 inspection Methods 0.000 description 6
- 238000004519 manufacturing process Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000005641 tunneling Effects 0.000 description 2
- UGFAIRIUMAVXCW-UHFFFAOYSA-N Carbon monoxide Chemical compound [O+]#[C-] UGFAIRIUMAVXCW-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 229910002091 carbon monoxide Inorganic materials 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
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/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
- 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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明涉及移动机器人智能定位技术领域,具体涉及一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及***,该方法利用捷联惯导***与视觉相机的特性进行互补,融合两个传感器的数据,对移动机器人进行定位,并绘制出移动轨迹,将数据传给监控***,实现移动机器人的远程定位导航。由于应用在煤矿井下,可以利用惯导的数据来修正由于光线不足而造成的视觉定位不准确,而视觉的数据又可以用来修正惯导长期运行造成的累积误差,两种传感器的数据融合,可以实现对煤矿移动机器人的实时、准确的定位。本发明基于“惯导+”的理念,利用捷联惯导与视觉相机相结合,在复杂、黑暗的煤矿井下环境中,实现煤矿移动机器人的精确定位与实时监测。
Description
技术领域
本发明涉及移动机器人智能定位技术领域,具体涉及一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及***。
背景技术
煤矿产业是我国重要的能源产业,近些年,随着国家经济快速增长,各种工业蓬勃发展,煤矿引入了大量的移动机器人来提高煤矿的安全。机器人具有机动性强、导航自主、可应用传感器种类多、网络交互性强的优势,目前已广泛的应用于煤矿生产之中,极大的增加了煤矿的安全生产,提高了生产效率。
应用于煤矿的移动机器人种类众多,如负责掘进的掘进机器人,负责运输的无人运输机器人,负责监测的巡检机器人以及灾后负责救援的救援机器人等。其中应用最为广泛的是巡检机器人和救援机器人。在煤矿井下,需要进行实时巡检,以确保设备的安全工作和防止事故的发生。另外,煤矿井下许多有害气体,如瓦斯、一氧化碳等,巡检机器人携带多种气体浓度监测传感器,可以更加直接方便的监测井下各种气体的浓度,井下温度和湿度等,防患于未然,并独立的完成煤矿井下的巡检工作;在发生矿难之后,由于煤矿井下环境恶劣,能见度低,危险系数高,使得许多救援任务不能直接进行,需要依靠机器人来进行抢救,救援机器人可以实现在恶劣的环境下完成被困人员的营救。
虽然移动机器人已经广泛的应用于煤矿之中,但考虑到井下复杂的自然环境,由于处于地下环境,一些寻常的定位手段很难直接应用,如井下巷道复杂,很少有笔直的路线,且信号多被煤壁所吸收,对无线电通信非常不利。当井下使用移动机器人进行无人化工作时,由于井下无法使用GPS定位,因此不能让后端工作者实时的对其移动轨迹进行跟踪与监测,只监测到很多数据而无法获知其相应的位置,这无异于盲人摸象,这样的信息是毫无用处的,所以,在煤矿井下,针对移动机器人的导航和实时定位显得极其重要。煤矿移动机器人的导航与定位技术研究是一个复杂且重要的课题,它涉及的领域包括:机器人导航、机器人控制、运动装置、多传感器融合及环境感知等。
由此不难发现,煤矿用移动机器人的导航***与定位***的研究,对于解决煤矿移动机器人的定位精度问题,增加移动机器人的机动性和可操作性,提高煤矿工作的安全性,对实现地面人员对井下搜救机器人、巡检机器人的位置跟踪和监测、对掘进机,采煤机等井下工作设备的工作监测、对煤矿井下各种有害气体浓度、环境温度的监测、对煤矿安全生产具有十分重要的研究意义。
发明内容
为解决上述问题,本发明提供了一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航***,通过图优化算法将惯导数据与单目相机数据进行融合处理,增加***的定位精度,使用ORB_SLAM算法,在保证定位精度的同时提高了视觉计算的实时性,与现有煤矿移动机器人定位***相比,具有成本较低,安装简单,定位精度高等优点。
为实现上述目的,本发明采取的技术方案为:
一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航***,利用捷联惯导***与视觉相机的特性进行互补,融合两个传感器的数据,对移动机器人进行定位,并绘制出移动轨迹,将数据传给监控***,实现移动机器人的远程定位导航;具体的:根据机器人在移动的过程中,IMU和单目相机产生的数据,通过无线传输发送给上位机,上位机通过数据处理后将数据融合与优化,并使用处理后的数据,对移动机器人进行定位与运行路线的绘制,监控人员通过观察移动机器人的工作情况,做出判断命令,并下发给机器人,实现移动机器人的远程监控与定位导航。
具体的定位导航方法,包括如下步骤:
S1、在煤矿移动机器人上安装捷联惯导***与单目视觉相机全套设备,并控制移动机器人在煤矿巷道内移动;
S2、建立捷联惯导***的世界坐标系(w系)和载体坐标系(b系),并构建载体坐标系(b系)与世界坐标系(w系)之间的转换矩阵;建立单目相机的世界坐标系(w系)、相机坐标系(c系)和像平面坐标系(i系),并构建世界坐标系(w系)、相机坐标系(c系)与像平面坐标系(i系)之间的转换矩阵:最后确定惯导的载体坐标系(b系)与单目相机的相机坐标系(c系)之间的旋转矩阵;
以移动机器人重心为坐标原点,移动机器人前进的方向作为Y轴正方向,以向上为Z轴正方向,垂直Y轴向右方向为X轴正方向,建立载体坐标系(Xb,Yb,Zb);根据移动机器人后轮的起始位置为原点,确定世界坐标系(Xw,Yw,Zw);根据单目相机的安装位置建立相机坐标系(Xc,Yc,Zc);惯导坐标系通过旋转平移得到视觉坐标系,从而将两个传感器联系起来;
S3、将捷联惯导***和单目相机的原始数据传入计算机,并完成数据的融合与定位建图等处理,具体的:
S301、完成IMU数据的预积分处理:已知IMU每个时刻的加速度a和角速度w的值,对IMU建立观测模型,有:
式中,为角速度和加速度的测量值,BwWB(t),Wa(t)为角速度和加速度的真实值,为世界系到IMU坐标系的旋转关系,Wg为重力加速度,bg(t),ba(t)为IMU的零偏,ηg(t),ηa(t)为***的白噪声;
根据运动学方程:
则:
根据i时刻的IMU状态量,求离散时间下j时刻的关键帧对应的IMU预积分公式为:
S302、将单目相机采集到的数据分成关键帧,提取每个关键帧的特征点,计算特征点的FAST得分值,在特征点的邻域内,若有多个特征点,则判断得分值,若p是响应值最大的则保留,否则抑制,得分计算公式如下:
其中,values为p点周围像素点的值,t为FAST算法的设置阈值;
根据提取出的特征点,通过两个关键帧之间特征点的跟踪和比对,求出单应矩阵,并通过矩阵的分解求出旋转矩阵R与位移向量t;
并利用针孔相机模型,通过矩阵的变换,获得空间点在像素坐标系中的像素坐标,将测量值与真实值相减即为重投影误差,转换公式如下所示:
其中,Si为尺度,K为相机内参;
将旋转矩阵写成四元数形式,有:
其次,实现相机坐标系的转换,由于视觉相机最后是在像素坐标系下进行算法定位,因此需要将世界坐标系转换到像平面坐标系下,而世界坐标系与像平面坐标系之间还存在一个相机坐标系,因此,世界坐标系与相机坐标系的转换为:
摄像机坐标系到像平面坐标系转换为:
其中,Cx、Cy为像平面坐标系下远点的平移量,fx、fy为焦距;世界坐标系到像平面坐标系转换关系如下所示:
最后,将IMU数据与视觉数据对齐,包括修正惯导***陀螺仪的bias;初始化惯导***速度、重力向量和尺度因子;改进重力向量g;
S4、在IMU与视觉传感器初始化完成之后,将数据通过网络传输给上位机监控***,并进行数据融合处理,具体的:
S401、采用图优化的方法实现数据的融合,将滑动窗口中***边缘化的先验信息、IMU的测量残差和视觉的观测残差和视觉的观测残差三者一起进行优化,从而获得机器人准确的位姿信息,需要优化的代价函数为:
在优化过程中,每一次高斯迭代,都会将该代价函数线性化为:
将Λp,ΛB,AC分别代表边缘先验量、IMU的测量残差和视觉的测量残差的信息矩阵,把惯导的测量误差与视觉测量误差联立,可将最小化该代建函数简化为求解该线性方程的问题:
(Λp+ΛB+ΛC)δX=(bp+bB+bC)
S5、最后加入回环检测,利用词袋的形式来做关键帧数据库的构建与查询,使机器人运动到曾经到达过的位置时,当前帧与词袋模型中的关键帧进行匹配,使地图能够闭合,同时增加定位精度和***的鲁棒性;
S6、上位机通过观察机器人的运动轨迹与任务完成情况,对机器人发布控制命令,控制命令通过数据传输传给移动机器人,从而实现对煤矿移动机器人的定位与导航。
进一步地,所述步骤S302中,采用FAST算法,若某像素与其周围邻域内足够多的像素点相差较大,则该像素可能是特征点。
进一步地,所述步骤S303中,陀螺仪的bias修正,在修正陀螺仪bias时,构成了如下约束:
其中,
对构成的约束进行最小二乘法求解,最终可得到bias的变化量。
进一步地,所述步骤S303中,初始化速度v、重力向量g和尺度因子s时,将***的状态向量写为:
最后经过计算与推导,可以将这个问题推导成求解如下最小二程的问题:
进一步地,所述步骤S303中,由于地心引力的影响,重力向量会有b1和b2两个正交方向的分量,则有扰动公式:
δg=w1b1+w2b2;
其中,w1,w2是两个扰动方向的大小,最后通过求解该扰动公式,可以求解出纠正后的重力向量。
本发明具有以下有益效果:
(1)采用多传感器融合,增加了***的定位精度与鲁棒性。
(2)利用捷联惯导的数据恢复了单目相机的尺度问题,同时利用单目相机的数据,克服了纯惯导定位的误差累计问题。
(3)使用处理后的数据,将移动机器人的移动轨迹进行绘制,增加了监控人员的可视性。
(4)采用无线通讯进行移动机器人与上位机之间的远程数据传输,实现对煤矿井下移动机器人的实时监测,解放了劳动力,提高了井下工作的安全性。
附图说明
图1为本发明实施例一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航方法的流程框图。
图2为本发明实施例中的移动机器人的控制***。
图3为本发明的一个具体应用例的***结构图。
图4为本发明实施例中的捷联惯导预积分原理图。
图5为本发明实施例中的单目相机成像原理图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明,但不以任何形式限制本发明。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进。这些都属于本发明的保护范围。
如图1所示,本发明实施例提供了一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航方法,包括如下步骤:
S1、在煤矿移动机器人上安装捷联惯导***与单目视觉相机全套设备,并控制移动机器人在煤矿巷道内移动;
S2、建立捷联惯导***的世界坐标系(w系)和载体坐标系(b系),并构建载体坐标系(b系)与世界坐标系(w系)之间的转换矩阵;建立单目相机的世界坐标系(w系)、相机坐标系(c系)和像平面坐标系(i系),并构建世界坐标系(w系)、相机坐标系(c系)与像平面坐标系(i系)之间的转换矩阵:最后确定惯导的载体坐标系(b系)与单目相机的相机坐标系(c系)之间的旋转矩阵;
以移动机器人重心为坐标原点,移动机器人前进的方向作为Y轴正方向,以向上为Z轴正方向,垂直Y轴向右方向为X轴正方向,建立载体坐标系(Xb,Yb,Zb);根据移动机器人后轮的起始位置为原点,确定世界坐标系(Xw,Yw,Zw);根据单目相机的安装位置建立相机坐标系(Xc,Yc,Zc);惯导坐标系通过旋转平移得到视觉坐标系,从而将两个传感器联系起来;
S3、将捷联惯导***和单目相机的原始数据传入计算机,并完成数据的融合与定位建图等处理,具体的:
S301、由于单目相机存在尺度问题,需要由惯导来提供,而IMU会受到bias的影响,且IMU与单目相机的频率不同,不能直接进行数据融合,因此要对IMU数据进行预积分。
完成IMU数据的预积分处理:已知IMU每个时刻的加速度a和角速度w的值,对IMU建立观测模型,有:
式中,为角速度和加速度的测量值,BwWB(t),Wa(t)为角速度和加速度的真实值,为世界系到IMU坐标系的旋转关系,Wg为重力加速度,bg(t),ba(t)为IMU的零偏,ηg(t),ηa(t)为***的白噪声。
根据运动学方程:
则:
根据i时刻的IMU状态量,求离散时间下j时刻的关键帧对应的IMU预积分公式为:
S302、将单目相机采集到的数据分成关键帧,采用FAST算法提取每个关键帧的特征点,若某像素与其周围邻域内足够多的像素点相差较大,则该像素可能是特征点;计算特征点的FAST得分值,在特征点的邻域内,若有多个特征点,则判断得分值,若p是响应值最大的则保留,否则抑制,得分计算公式如下:
其中,values为p点周围像素点的值,t为FAST算法的设置阈值。
根据提取出的特征点,通过两个关键帧之间特征点的跟踪和比对,求出单应矩阵,并通过矩阵的分解求出旋转矩阵R与位移向量t;
并利用针孔相机模型,通过矩阵的变换,获得空间点在像素坐标系中的像素坐标,将测量值与真实值相减即为重投影误差,转换公式如下所示:
其中,Si为尺度,K为相机内参;
S303、由于IMU的坐标系不同,因此需要预先将传感器的联合初始化。
将旋转矩阵写成四元数形式,有:
其次,实现相机坐标系的转换,由于视觉相机最后是在像素坐标系下进行算法定位,因此需要将世界坐标系转换到像平面坐标系下,而世界坐标系与像平面坐标系之间还存在一个相机坐标系,因此,世界坐标系与相机坐标系的转换为:
摄像机坐标系到像平面坐标系转换为:
其中,Cx、Cy为像平面坐标系下远点的平移量,fx、fy为焦距;世界坐标系到像平面坐标系转换关系如下所示:
最后,将IMU数据与视觉数据对齐,包括修正惯导***陀螺仪的bias;初始化惯导***速度、重力向量和尺度因子;改进重力向量g;
(1)陀螺仪的bias修正,在修正陀螺仪bias时,构成了如下约束:
其中,
(2)初始化速度、重力向量g和尺度因子时,将***的状态向量写为:
最后经过计算与推导,可以将这个问题推导成求解如下最小二程的问题:
(3)所述步骤S303中,由于地心引力的影响,重力向量会有b1和b2两个正交方向的分量,则有扰动公式:
δg=w1b1+w2b2;
其中,w1,w2是两个扰动方向的大小,最后通过求解该扰动公式,可以求解出纠正后的重力向量。
S4、在IMU与视觉传感器初始化完成之后,将数据通过网络传输给上位机监控***,并进行数据融合处理,具体的:
S401、采用图优化的方法实现数据的融合,将滑动窗口中***边缘化的先验信息、IMU的测量残差和视觉的观测残差和视觉的观测残差三者一起进行优化,从而获得机器人准确的位姿信息,需要优化的代价函数为:
在优化过程中,每一次高斯迭代,都会将该代价函数线性化为:
将Λp,ΛB,ΛC分别代表边缘先验量、IMU的测量残差和视觉的测量残差的信息矩阵,把惯导的测量误差与视觉测量误差联立,可将最小化该代建函数简化为求解该线性方程的问题:
(Λp+ΛB+ΛC)δX=(bp+bB+bC)
S5、最后加入回环检测,利用词袋的形式来做关键帧数据库的构建与查询,使机器人运动到曾经到达过的位置时,当前帧与词袋模型中的关键帧进行匹配,使地图能够闭合,同时增加定位精度和***的鲁棒性;
S6、上位机通过观察机器人的运动轨迹与任务完成情况,对机器人发布控制命令,控制命令通过数据传输传给移动机器人,从而实现对煤矿移动机器人的定位与导航。
本具体实施通过单目视觉相机与捷联惯导配合,减小了安装空间,增加了应用范围。通过IMU地数据,恢复单目视觉相机的尺度问题,通过单目相机的数据,校正捷联惯导***的误差累积,利用图优化算法,实现对捷联惯导与单目相机的数据进数据融合处理,增加了***的定位精度,同时加入回环检测,增加煤矿移动机器人视觉与惯导组合定位***的定位精度与鲁棒性;利用处理过后的数据,对移动机器人的移动轨迹进行绘制,增加了控制***的可视性,方便控制人员做出决策;将捷联惯导与视觉相机数据通过无线传输传给上位机,方便工作人员远程进行操控。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。
Claims (6)
1.一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航***,其特征在于,利用捷联惯导***与视觉相机的特性进行互补,融合两个传感器的数据,对移动机器人进行定位,并绘制出移动轨迹,将数据传给监控***,实现移动机器人的远程定位导航。
2.如权利要求1所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航方法,其特征在于,该方法包括如下步骤:
S1、在煤矿移动机器人上安装捷联惯导***与单目视觉相机全套设备,并控制移动机器人在煤矿巷道内移动;
S2、建立捷联惯导***的世界坐标系(w系)和载体坐标系(b系),并构建载体坐标系(b系)与世界坐标系(w系)之间的转换矩阵;建立单目相机的世界坐标系(w系)、相机坐标系(c系)和像平面坐标系(i系),并构建世界坐标系(w系)、相机坐标系(c系)与像平面坐标系(i系)之间的转换矩阵:最后确定惯导的载体坐标系(b系)与单目相机的相机坐标系(c系)之间的旋转矩阵;
以移动机器人重心为坐标原点,移动机器人前进的方向作为Y轴正方向,以向上为Z轴正方向,垂直Y轴向右方向为X轴正方向,建立载体坐标系(Xh,Yb,Zb);根据移动机器人后轮的起始位置为原点,确定世界坐标系(Xw,Yw,Zw);根据单目相机的安装位置建立相机坐标系(Xc,Yc,Zc);惯导坐标系通过旋转平移得到视觉坐标系,从而将两个传感器联系起来;
S3、将捷联惯导***和单目相机的原始数据传入计算机,并完成数据的融合与定位建图处理,具体的:
S301、完成IMU数据的预积分处理:已知IMU每个时刻的加速度a和角速度w的值,对IMU建立观测模型,有:
式中,为角速度和加速度的测量值,BwWB(t),Wa(t)为角速度和加速度的真实值,为世界系到IMU坐标系的旋转关系,Wg为重力加速度,bg(t),ba(t)为IMU的零偏,ηg(t),ηa(t)为***的白噪声;
根据运动学方程:
则:
根据i时刻的IMU状态量,求离散时间下j时刻的关键帧对应的IMU预积分公式为:
S302、将单目相机采集到的数据分成关键帧,提取每个关键帧的特征点,计算特征点的FAST得分值,在特征点的邻域内,若有多个特征点,则判断得分值,若p是响应值最大的则保留,否则抑制,得分计算公式如下:
其中,values为p点周围像素点的值,t为FAST算法的设置阈值;
根据提取出的特征点,通过两个关键帧之间特征点的跟踪和比对,求出单应矩阵,并通过矩阵的分解求出旋转矩阵R与位移向量t;
并利用针孔相机模型,通过矩阵的变换,获得空间点在像素坐标系中的像素坐标,将测量值与真实值相减即为重投影误差,转换公式如下所示:
其中,Si为尺度,K为相机内参;
将旋转矩阵写成四元数形式,有:
其次,实现相机坐标系的转换,由于视觉相机最后是在像素坐标系下进行算法定位,因此需要将世界坐标系转换到像平面坐标系下,而世界坐标系与像平面坐标系之间还存在一个相机坐标系,因此,世界坐标系与相机坐标系的转换为:
摄像机坐标系到像平面坐标系转换为:
其中,Cx、Cy为像平面坐标系下远点的平移量,fx、fy为焦距;世界坐标系到像平面坐标系转换关系如下所示:
最后,将IMU数据与视觉数据对齐,包括修正惯导***陀螺仪的bias;初始化惯导***速度、重力向量和尺度因子;改进重力向量g;
S4、在IMU与视觉传感器初始化完成之后,将数据通过网络传输给上位机监控***,并进行数据融合处理,具体的:
S401、采用图优化的方法实现数据的融合,将滑动窗口中***边缘化的先验信息、IMU的测量残差和视觉的观测残差和视觉的观测残差三者一起进行优化,从而获得机器人准确的位姿信息,需要优化的代价函数为:
在优化过程中,每一次高斯迭代,都会将该代价函数线性化为:
将Λp,ΛB,ΛC分别代表边缘先验量、IMU的测量残差和视觉的测量残差的信息矩阵,把惯导的测量误差与视觉测量误差联立,可将最小化该代建函数简化为求解该线性方程的问题:
(Λp+ΛB+ΛC)δX=(bp+bB+bC)
S5、最后加入回环检测,利用词袋的形式来做关键帧数据库的构建与查询,使机器人运动到曾经到达过的位置时,当前帧与词袋模型中的关键帧进行匹配,使地图能够闭合,同时增加定位精度和***的鲁棒性;
S6、上位机通过观察机器人的运动轨迹与任务完成情况,对机器人发布控制命令,控制命令通过数据传输传给移动机器人,从而实现对煤矿移动机器人的定位与导航。
3.如权利要求2所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法,其特征在于:所述步骤S302中,采用FAST算法,若某像素与其周围邻域内足够多的像素点相差较大,则该像素可能是特征点。
6.如权利要求2所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法,其特征在于:所述步骤S303中,由于地心引力的影响,重力向量会有b1和b2两个正交方向的分量,则有扰动公式:
δg=w1b1+w2b2;
其中,w1,w2是两个扰动方向的大小,最后通过求解该扰动公式,可以求解出纠正后的重力向量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010950320.9A CN112033400B (zh) | 2020-09-10 | 2020-09-10 | 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010950320.9A CN112033400B (zh) | 2020-09-10 | 2020-09-10 | 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112033400A true CN112033400A (zh) | 2020-12-04 |
CN112033400B CN112033400B (zh) | 2023-07-18 |
Family
ID=73588883
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010950320.9A Active CN112033400B (zh) | 2020-09-10 | 2020-09-10 | 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112033400B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113218403A (zh) * | 2021-05-14 | 2021-08-06 | 哈尔滨工程大学 | 惯性视觉组合式定位的agv*** |
CN113701750A (zh) * | 2021-08-23 | 2021-11-26 | 长安大学 | 一种井下多传感器的融合定位*** |
CN114485643A (zh) * | 2022-01-25 | 2022-05-13 | 重庆理工大学 | 一种煤矿井下移动机器人环境感知与高精度定位方法 |
CN117760408A (zh) * | 2023-12-22 | 2024-03-26 | 武汉华测卫星技术有限公司 | 一种基于空间定位的电子导航引导图生成方法和*** |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107014379A (zh) * | 2017-05-25 | 2017-08-04 | 中国矿业大学 | 一种掘进机绝对空间位姿检测装置与方法 |
US20180112985A1 (en) * | 2016-10-26 | 2018-04-26 | The Charles Stark Draper Laboratory, Inc. | Vision-Inertial Navigation with Variable Contrast Tracking Residual |
CN108731672A (zh) * | 2018-05-30 | 2018-11-02 | 中国矿业大学 | 基于双目视觉及惯性导航的采煤机姿态检测***及方法 |
CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
CN109579831A (zh) * | 2018-11-09 | 2019-04-05 | 西安科技大学 | 矿用悬臂式掘进机可视化辅助导引方法和*** |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN110162036A (zh) * | 2019-04-09 | 2019-08-23 | 中国矿业大学 | 一种掘进机自主导航定位***及其方法 |
CN110375738A (zh) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 |
CN111197982A (zh) * | 2020-01-10 | 2020-05-26 | 北京航天众信科技有限公司 | 基于视觉和捷联惯导的掘进机位姿纠偏方法、***及终端 |
-
2020
- 2020-09-10 CN CN202010950320.9A patent/CN112033400B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180112985A1 (en) * | 2016-10-26 | 2018-04-26 | The Charles Stark Draper Laboratory, Inc. | Vision-Inertial Navigation with Variable Contrast Tracking Residual |
CN107014379A (zh) * | 2017-05-25 | 2017-08-04 | 中国矿业大学 | 一种掘进机绝对空间位姿检测装置与方法 |
CN108731672A (zh) * | 2018-05-30 | 2018-11-02 | 中国矿业大学 | 基于双目视觉及惯性导航的采煤机姿态检测***及方法 |
CN109579831A (zh) * | 2018-11-09 | 2019-04-05 | 西安科技大学 | 矿用悬臂式掘进机可视化辅助导引方法和*** |
CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN110162036A (zh) * | 2019-04-09 | 2019-08-23 | 中国矿业大学 | 一种掘进机自主导航定位***及其方法 |
CN110375738A (zh) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 |
CN111197982A (zh) * | 2020-01-10 | 2020-05-26 | 北京航天众信科技有限公司 | 基于视觉和捷联惯导的掘进机位姿纠偏方法、***及终端 |
Non-Patent Citations (5)
Title |
---|
TONG QIN ET.AL: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", 《IEEE TRANSACTIONS ON ROBOTICS》 * |
TONG QIN ET.AL: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", 《IEEE TRANSACTIONS ON ROBOTICS》, vol. 34, no. 4, 31 August 2018 (2018-08-31), pages 1004 - 1020, XP011688854, DOI: 10.1109/TRO.2018.2853729 * |
张旭辉等: "矿用悬臂式掘进机截割头姿态视觉测量***", 《工矿自动化》 * |
张旭辉等: "矿用悬臂式掘进机截割头姿态视觉测量***", 《工矿自动化》, vol. 44, no. 8, 31 August 2018 (2018-08-31), pages 63 - 66 * |
敖龙辉;郭杭;: "室内环境下立体视觉惯导融合定位", 测绘通报, no. 12, pages 12 - 17 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113218403A (zh) * | 2021-05-14 | 2021-08-06 | 哈尔滨工程大学 | 惯性视觉组合式定位的agv*** |
CN113701750A (zh) * | 2021-08-23 | 2021-11-26 | 长安大学 | 一种井下多传感器的融合定位*** |
CN114485643A (zh) * | 2022-01-25 | 2022-05-13 | 重庆理工大学 | 一种煤矿井下移动机器人环境感知与高精度定位方法 |
CN114485643B (zh) * | 2022-01-25 | 2023-09-12 | 重庆理工大学 | 一种煤矿井下移动机器人环境感知与高精度定位方法 |
CN117760408A (zh) * | 2023-12-22 | 2024-03-26 | 武汉华测卫星技术有限公司 | 一种基于空间定位的电子导航引导图生成方法和*** |
CN117760408B (zh) * | 2023-12-22 | 2024-07-19 | 武汉华测卫星技术有限公司 | 一种基于空间定位的电子导航引导图生成方法和*** |
Also Published As
Publication number | Publication date |
---|---|
CN112033400B (zh) | 2023-07-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112033400B (zh) | 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及*** | |
EP3940421A1 (en) | Positioning method and device based on multi-sensor fusion | |
WO2020253854A1 (zh) | 移动机器人姿态角解算方法 | |
CN110262546B (zh) | 一种隧道智能无人机巡检方法 | |
CN109911188B (zh) | 非卫星导航定位环境的桥梁检测无人机*** | |
CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN111735445B (zh) | 融合单目视觉与imu的煤矿巷道巡检机器人***及导航方法 | |
CN113124856B (zh) | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 | |
CN114199240B (zh) | 无gps信号下二维码、激光雷达与imu融合定位***及方法 | |
CA2988326C (en) | Automated mobile geotechnical mapping | |
CN103926933A (zh) | 一种无人飞行器室内同时定位与环境建模方法 | |
CN112102403B (zh) | 用于输电塔场景下的自主巡检无人机的高精度定位方法及其*** | |
CN109945871B (zh) | 一种通信带宽与距离受限情况下的多无人平台同步定位与地图构建方法 | |
CN113075686B (zh) | 一种基于多传感器融合的电缆沟智能巡检机器人建图方法 | |
CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
CN209634595U (zh) | 一种新型的具有自主避障功能的矿井侦测机器人 | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN113790726B (zh) | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 | |
CN110411443B (zh) | 一种采煤机摇臂惯性/视觉组合定姿装置和方法 | |
CN116518984A (zh) | 一种煤矿井下辅助运输机器人车路协同定位***及方法 | |
Si et al. | A novel positioning method of anti-punching drilling robot based on the fusion of multi-IMUs and visual image | |
Wang et al. | Efficient and accurate mapping method of underground metal mines using mobile mining equipment and solid-state lidar | |
Chen et al. | Aerial robots on the way to underground: An experimental evaluation of VINS-mono on visual-inertial odometry camera | |
US11992961B2 (en) | Pose determination method, robot using the same, and computer readable storage medium | |
CN116704019A (zh) | 一种基于锚杆网络的钻锚机器人单目视觉定位方法 |
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 |