CN115046541A - 一种地下矿山环境下的拓扑地图构建与矿车定位系统 - Google Patents
一种地下矿山环境下的拓扑地图构建与矿车定位系统 Download PDFInfo
- Publication number
- CN115046541A CN115046541A CN202210642614.4A CN202210642614A CN115046541A CN 115046541 A CN115046541 A CN 115046541A CN 202210642614 A CN202210642614 A CN 202210642614A CN 115046541 A CN115046541 A CN 115046541A
- Authority
- CN
- China
- Prior art keywords
- information
- topological
- mine car
- sensor
- point
- 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
Links
- 238000010276 construction Methods 0.000 title claims abstract description 33
- 238000000034 method Methods 0.000 claims description 26
- 230000012010 growth Effects 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 13
- 238000012544 monitoring process Methods 0.000 claims description 12
- 238000004891 communication Methods 0.000 claims description 8
- 230000004927 fusion Effects 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 5
- 230000004888 barrier function Effects 0.000 claims description 4
- 230000005540 biological transmission Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 230000003993 interaction Effects 0.000 claims description 4
- 239000003607 modifier Substances 0.000 claims description 4
- 238000012216 screening Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000012797 qualification Methods 0.000 claims description 3
- SAZUGELZHZOXHB-UHFFFAOYSA-N acecarbromal Chemical compound CCC(Br)(CC)C(=O)NC(=O)NC(C)=O SAZUGELZHZOXHB-UHFFFAOYSA-N 0.000 claims description 2
- 238000000354 decomposition reaction Methods 0.000 claims description 2
- 230000001788 irregular Effects 0.000 claims description 2
- 238000013178 mathematical model Methods 0.000 claims description 2
- 238000005096 rolling process Methods 0.000 claims description 2
- 230000034655 secondary growth Effects 0.000 claims description 2
- 238000001228 spectrum Methods 0.000 claims description 2
- 239000000126 substance Substances 0.000 claims description 2
- 230000007547 defect Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 9
- 238000012545 processing Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000011218 segmentation Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 241000158728 Meliaceae Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000007789 sealing Methods 0.000 description 1
- 230000003595 spectral effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- 239000002699 waste material Substances 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/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
- 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)
Abstract
本发明属于机器人建图与定位领域,提出了一种地下矿山环境下的拓扑地图构建以及矿车定位系统。基于雷达获得栅格地图,通过探索轨迹构建拓扑地图,栅格地图与拓扑地图结合获得层级地图辅助信息,系统根据辅助信息进行自主定位,获得周围环境以及移动设备于层级地图中的位置;通过对外部两种传感器信息采集,将二者信息有效融合。本发明可以得到更稳定精准的位置信息,又能弥补两种传感器单独使用时的弊端。同时有着对周围环境更高的容错率,且使用功耗更低,所以续航能力更强,运行的稳定性更高,可以满足多变复杂的环境,并且应用成本较低,为广泛的使用提供有效更多可能。
Description
技术领域
本发明涉及机器人建图与定位领域,具体涉及一种地下矿山环境下的拓扑地图构建与矿车定位系统。
背景技术
随着互联网的兴起,很多高科技技术已经从实验室应用到实际的社会生活中,传统工业的发展离不开自动化技术的更新。大型运载车辆工作过程的定位和实时监控始终是一个值得讨论的话题,在稍复杂的环境中建图和定位问题显的更为重要。
对于建图问题,在大型交互场景中,既需要障碍物信息又需要目的地信息,并且必须同时保存和使用这两种信息。障碍物信息可以很容易地通过度量图来保存和使用,但是目的地信息保存和使用的方式一直不够成熟。目前,大部分的研究通常通过拓扑地图来实现,所以对于拓扑地图的构建就是一个关键问题了。
文献“郭帅,马书根,李斌,王明辉,王越超.基于Voronoi地图表示方法的同步定位与地图创建[J].自动化学报,2011,37(09):1095-1104.”提出了基于Voronoi地图表示方法的同步定位与地图创建算法VorSLAM.该算法在全局坐标系下创建特征地图,并根据此特征地图使用Voronoi图唯一地划分地图空间,在每一个划分内部创建一个相对于特征的局部稠密地图.特征地图与各个局部地图最终一起连续稠密地描述了环境.Voronoi地图表示方法解决了地图划分的唯一性问题,理论证明局部地图可以完整描述该划分所对应的环境轮廓.该地图表示方法一个基本特点是特征与局部地图一一对应,每个特征都关联一个定义在该特征上的局部地图.但是,Voronoi图是根据距离原则或者特殊结构对空间进行划分的,这样得到的区域是“大小基本相同的”,大空旷场景等特殊结构也限制了算法的适用性。还有通过谱聚类分割度量图,这种方法提高了拓扑分割效率和适用范围,同样也带来了分割结果的冗余性和不可控性,在某些区域容易导致拓扑顶点堆积。
对于定位,目前利用工业级激光雷达和GPS的定位方案确实可以很好的解决定位问题,但是工业级激光雷达应用成本很高。而对于GPS定位,它在某些复杂环境(例如室内、密封性较好环境、矿山环境等)下其定位精度会产生较大偏差对于位置信息监控不精确。因为单独的超宽带定位技术不能确定出移动设备的具体朝向信息,而单独的惯性定位由于惯性传感器的长时间的积分累计误差,会因为时间的增长而产生比较大的位置误差。综合上述因素,采用二者融合传递定位方法,因为融合的定位可以更精确的定位出移动设备的位置,可以消除每种定位方法单独定位产生的致命误差,能够适应多变的环境并且具有较好的容错性。
由于以上的原因,我们提出了一种地下矿山环境下的拓扑地图构建与矿车定位系统,用于地下矿山无人驾驶系统的大场景拓扑地图构建与超宽带定位技术和惯性定位融合。
发明内容
为了解决对大场景下移动平台的建图与实时精确定位监控问题,本发明提出一种地下矿山环境下的拓扑地图构建与矿车定位系统。
本发明采用的技术方案为:一种地下矿山环境下的拓扑地图构建与矿车定位系统,基于雷达获得栅格地图,通过探索轨迹构建拓扑地图,栅格地图与拓扑地图结合获得层级地图辅助信息,系统根据辅助信息进行自主定位,获得周围环境以及矿车于层级地图中的位置;
该拓扑地图构建与矿车定位系统包括拓扑地图构建模块、拓扑地图表示模块、中央控制模块、传感器模块、驱动模块、通讯模块和监测模块;
所述拓扑地图构建模块基于区域动态生长的拓扑地图构建方法将区域分割为与环境无关的几何结构;
所述拓扑地图表示模块基于圆锥体模型,将拓扑地图信息和栅格地图占有信息进行整合,形成RGB图像信息;
所述中央控制模块分别和传感器模块、驱动模块、通讯模块、监测模块进行信息交互,基于传感器模块信息,确定矿车位姿;
所述传感器模块分为超宽带传感器和惯性传感器;超宽带传感器包含标签传感器和基站传感器两类传感器,标签传感器搭载在矿车上,基站传感器布置在周围环境的固定位置,二者利用时间差测距法实时传递矿车到障碍物的距离信息;惯性传感器固定在矿车上,以牛顿力学定律为基础,将矿车在惯性参考系的加速度对时间进行积分,再变换至导航坐标系中,得到矿车的位姿信息;惯性传感器信息作为预测量,超宽带传感器信息作为观测量,利用滤波算法对二者进行融合,得到精确的位姿信息并上传至中央控制模块;
通讯模块和主控制台进行信息交互,将中央控制模块处理的位置信息上传至主控制台,将主控制台目标位置坐标信息传递至中央控制模块;
驱动模块按照中央控制模块的命令要求驱动动力设备工作并调节速度;
监控模块负责实时监控速度信息,并检测判断动力设备是否达到目标位置。
所述区域动态生长的拓扑地图构建方法基于生长点初始化器、环形扫描器、合格点筛选器、合格点添加器、生长半径修改器、拓扑区域分割器和拓扑区域调整器实现将区域分割为与环境无关的几何结构;具体步骤如下:
S1.1根据机器人于栅格地图上运动轨迹,通过生长点初始化器随机选取初始化生长点,经环形扫描器扫描当前区域顶点外围的所有直接相邻点;
S1.2合格点筛选器使用Bresenham划线算法计算生长点到每个直接相邻点的射线,筛选所有合格点;
合格点满足条件如下:
1)该点所处区域于栅格地图为空闲状态;
2)该点所处区域于拓扑地图中为非拓扑区域;
3)由生长点向该点发出的射线经过的所有点同时满足条件1)和条件2);
S1.3合格点添加器使用Graham扫描算法计算合格点添加进当前区域后的凸包,要求合并区域的障碍物比率小于设置的阈值,保留障碍物比率满足要求的合格点;
S1.4生长半径修改器将S1.3保留的合格点添加到拓扑区域;计算当前状态下的点合格比率和点添加比率;若点添加比率小于设置阈值则完成拓扑区域生长;点添加比率处于设置阈值内,以点合格比率修改拓扑区域生长最大值,公式如下:
S1.5拓扑区域分割器判断区域动态生长至最大半径或者点添加比率大于设定阈值时退出当前区域生长;对区域进行二次生长,重复S2.1-S2.4生长其他区域至拓扑区域覆盖大于95%的环境;
S1.6拓扑区域调整器控制每一区域生长结束后,更新该区域中心;每一拓扑区域均为由该区域的凸包点组成的不规则凸多边形,以多边形的重心作为新的中心点;在区域生长结束之后,受到动态生长的影响,整个环境会被分割成大大小小不同的区域,过小的区域浪费拓扑地图的顶点资源,所以需要对小拓扑区域进行合并,优化拓扑地图。各拓扑区域合并前,利用Kruskal算法对拓扑地图进行调整,得到拓扑地图的最小生成树形式,简化地图结构。
所述拓扑地图表示模块,利用信息转换器将拓扑地图信息和占用信息转换为彩色图片的RGB值,步骤如下:
S2.1拓扑地图信息由角度测量,用于表示拓扑邻域,范围为[0,360];该值用于表示拓扑邻域,最多可以表示360个拓扑邻域。为了表现出更高的区分度,可以间隔地取值,例如用于识别拓扑区域的20个颜色值,并且可以使用18种拓扑区域标识符。
S2.2占用信息表示与光谱颜色的接近程度,范围为[0,1];值越大,该点存在障碍的可能性越大;
S2.3转换占用信息和拓扑地图信息为HSV信息格式;转换公式如下所示;
其中,OM为占用信息,TM为拓扑地图信息,a为起始值,b是间隔值,k为模型系数
根据OM和TM计算中间变量g,p,q,公式如下所示
V=1;
S2.4 HSV-RGB颜色空间转换器V=1;通过下式获得RGB信息;
所述惯性传感器,其得到矿车的位姿信息具体步骤如下:
步骤3.1:惯性信息数据采集;不计滚转角和俯仰角的变化;惯性传感器发送数据至中央控制模块;惯性信息数据包括2位数据信息和9位惯性信息原始数据;
步骤3.2:惯性信息数据结算;中央控制模块设定三种数据信息:分别为加速度、角速度和角度;根据数据信息进行结算,结算后的数据信息传到中央控制模块,用于数据融合;数据信息计算方式如下:
数据信息为加速度:
ax=((AxH)<<8|AxL)/32768*16g
ay=((AyH)<<8|AyL)/32768*16g(5)
az=((AzH)<<8|AzL)/32768*16g
数据信息为角速度:
Wx=((wxH<<8)|wxL))/32768*2000(°/s)
Wy=((wyH<<8)|wyL))/32768*2000(°/s)(6)
Wz=((wzH<<8)|wzL))/32768*2000(°/s)
数据信息为角度:
Roll=((RollH<<8)|RollL)/32768*180(°)
Pitch=((PitchH<<8)|PitchL)/32768*180(°)(7)
Yaw=((YawH<<8)|YawL)/32768*180(°)
其中,g代表重力加速度;x,y,z分别代表三个坐标系;Wx,Wy,Wz为角速度的xyz轴分解;ax,ay,az为加速度的xyz轴分解,ROLL为翻滚角,Pitch为俯仰角,Yaw为偏航角;H代表二进制数据高八位,L代表二进制数据低八位;
加速度信息进行积分得到矿车的坐标信息,角速度和角度的计算得到矿车的航向信息。
所述超宽带传感器,其计算位置坐标具体步骤为:
S4.1标签传感器在时间t向基站传感器发送消息,在t′时间接收到基站传感器返回的信息,通过时间差、信号传播速度和信号地址测得矿车到达具体固定点的距离;同时标签传感器通过无线传输设备把地址和距离信息发送至中央控制模块;
S4.2根据地址和距离信息,根据三点定位算法计算出矿车位置坐标,具体如下公式:
(x-x1)2+(y-y1)2=d1 2
(x-x2)2+(y-y2)2=d2 2(8)
(x-x3)2+(y-y3)2=d3 2
其中(x,y)表示此时矿车位置实时坐标信息,(x1,y1)、、(x2,y2),(x3,y3)分别表示基站传感器所在位置坐标,其中di表示超宽带模块基站传感器和标签传感器之间的距离;i=1,2,3
最小二乘法进行优化计算距离,具体如下:
将式(8)展开得到:
xi 2+yi 2-2xix-2yiy+x2+y2=di 2(9)
矩阵形式为:
dxy为矿车携带的标签类传感器在世界坐标系中与原点的距离,duwbi为固定基站类传感器在世界坐标系中与原点的距离;利用最小二乘法求解:X=(ATA)-1ATb结算出目标点的坐标;
S4.3惯性传感器测得的位姿坐标和S4.2计算出的位置坐标通过滤波方法进行融合求得矿车的精确的位置坐标信息。
所述通过滤波方法进行融合求得矿车的精确的位置坐标信息如下:
首先确定拓扑地图构建与矿车定位系统的状态方程,xk=(x,y)T;其中(x,y)代表矿车的实时位置坐标信息,xk代表的是k时刻的系统状态向量;惯性信息传感器通过前一个时刻的状态值对下一个时刻进行下一步预算,建立系统k时刻以k-1时刻为基础的函数模型:
根据泰勒级数展开将上式写成如下形式:
其中xk表示k时刻的状态,xk-1表示k-1时刻状态,表示k-1时刻的后验状态估计值,表示后验误差,表示先验预测,表示先验误差,表示k-1时刻函数fk-1对于状态xk的雅可比矩阵,表示k-1时刻函数fk-1对于状态ωk-1的雅可比矩阵。
先验协方差矩阵为:
对于超宽带传感器测量到的数据建立相关的数学模型,矿车在xk位置上,产生观测数据zk,hk描述关系如下:
根据泰勒级数展开上式:
其中系数K为:
其中Rk代表噪声协方差矩阵;
先验协方差矩阵Pk|k-1为:
考虑到整体系统需要低压供电,因此需要一个降压模块给实际系统提供合适电压。
本发明的有益效果:本发明通过对外部超宽带传感器和惯性传感器两种传感器信息采集,将二者信息有效融合。可以得到更稳定精准的位置信息,又能弥补超宽带传感器和惯性传感器两种传感器单独使用时的弊端。同时有着对周围环境更高的容错率,且使用功耗更低,所以续航能力更强,运行的稳定性更高,可以满足多变复杂的环境,并且应用成本较低,为广泛的使用提供有效更多可能。
附图说明
图1为拓扑地图构建系统整体框图;
图2为合格点构建示意图;
(a)确定生长点;
(b)筛选合格点;
(c)保留满足要求的点;
(d)合格点添加到拓扑区域;
图3为拓扑地图的保存过程图;
图4为定位系统各模块整体框图;
图5为移动平台模型;
图6为超宽带传感器布置图;
图7为超宽带定位原理框图;
图8为信息融合框图;
图9为实验结果对比图。
具体实施方式
下面结合附图对本发明具体实施方式进行进一步详细阐述。
本发明的地下矿山环境下的拓扑地图构建与矿车定位系统如图1所示,整体结构包括拓扑地图构建模块和拓扑地图显示模块。其中拓扑地图构建模块中包含生长点初始化器、筛选器、扫描器以及分割器。拓扑地图表示部分包含HSV-RGB颜色转换器,其中转换原理如图3所示。
本发明的地下矿山环境下的拓扑地图构建与矿车定位系统示意图如图4所示,整体结构包括拓扑地图构建模块、拓扑地图表示模块、中央控制模块、传感器模块、驱动模块、通讯模块和监测模块。其中中央控制模块包含有一个处理器,负责对传感器信息的处理和与其它模块的信息交互。其中的移动设备是四轮移动平台如图5所示。
本实施例中实现定位是采用超宽带定位和惯性定位融合的方法。对于超宽带传感器,它包含标签传感器和基站传感器两类。其中,1个标签传感器放置在移动设备上,基站传感器至少布置3个与固定点处,其具体的布置图如图6所示。每一个传感器含有一个32位处理器和1个DWM1000模块且具有收发功能。标签传感器的处理器通过飞行时间法测得此传感器到每个基站即固定坐标点的距离,最后处理器通过无线设备上传给中央控制模块,最后中央控制模块根据定位算法求解出位置坐标原理如图7。其中超宽带传感器设备如图7具体参数如表1所示。
表1超宽带传感器参数
指标名称 | 参数 | 指标名称 | 参数 |
电源 | 5VDC±5%, | 传输接口 | ESP8266 |
通讯距离 | 100--300m | 供电接口 | micro-USB |
测量精度 | 10cm--30cm | 工作温度 | -40℃--85℃ |
传输速率 | 6.8Mb/s | 中央处理器 | 32位 |
发射功率 | -35dB/MHZ--62dB/MHZ |
标签传感器通过处理获得与基站传感器的距离信息和地址信息,此时通过无线设备把地址和距离信息上传给中央控制模块。
惯性传感器直接搭载在移动设备上,移动设备的z轴垂直于地面。惯性传感器的z轴与移动设备重合,x轴指向移动设备前进方向,y轴与x轴在同一水平面且与它垂直。惯性传感器部分包含惯性测量单元和处理器两个部分;通过惯性测量单元对移动设备的检测将信息发送到处理器进行处理计算,最后通过USB设备把传感器信息实时上传给中央控制模块。应用的惯性传感器参数信息如表2所示;
表2惯性传感器参数
以上两种传感器把信息发送给中央控制模块处理,其处理器配置是AMD A10-5750M 2.5GHZRAM:4GB。图8表示整个系统的整体框架,通过此框架可以进行对传感器信息的采集和定位算法的验证。
通过本发明的方法获得计算坐标,并与真实坐标进行对比;通过图9可以看出同实际坐标相比,实验所得的计算坐标取得了较相似的结果,拟合率为0.98,因此,本发明实现的可能性得以验证。
Claims (8)
1.一种地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,该拓扑地图构建与矿车定位系统基于雷达获得栅格地图,通过探索轨迹构建拓扑地图,栅格地图与拓扑地图结合获得层级地图辅助信息,系统根据辅助信息进行自主定位,获得周围环境以及矿车于层级地图中的位置;
该拓扑地图构建与矿车定位系统包括拓扑地图构建模块、拓扑地图表示模块、中央控制模块、传感器模块、驱动模块、通讯模块和监测模块;
拓扑地图构建模块基于区域动态生长的拓扑地图构建方法将区域分割为与环境无关的几何结构;
拓扑地图表示模块将拓扑地图信息和栅格地图占有信息进行整合,形成RGB图像信息;
中央控制模块分别和传感器模块、驱动模块、通讯模块、监测模块进行信息交互,基于传感器模块信息,确定矿车位姿;
传感器模块分为超宽带传感器和惯性传感器;惯性传感器信息作为预测量,超宽带传感器信息作为观测量,利用滤波算法对二者进行融合,得到精确的位姿信息并上传至中央控制模块;
通讯模块和主控制台进行信息交互,将中央控制模块处理的位置信息上传至主控制台,将主控制台目标位置坐标信息传递至中央控制模块;
驱动模块按照中央控制模块的命令要求驱动动力设备工作并调节速度;
监控模块负责实时监控速度信息,并检测判断动力设备是否达到目标位置。
2.根据权利要求1所述的地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,所述超宽带传感器包含标签传感器和基站传感器两类传感器;标签传感器搭载在矿车上,基站传感器布置在周围环境的固定位置,二者利用时间差测距法实时传递矿车到障碍物的距离信息。
3.根据权利要求1所述的地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,所述惯性传感器固定在矿车上,以牛顿力学定律为基础,将矿车在惯性参考系的加速度对时间进行积分,再变换至导航坐标系中,得到矿车的位姿信息。
4.根据权利要求1所述的地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,所述区域动态生长的拓扑地图构建方法基于生长点初始化器、环形扫描器、合格点筛选器、合格点添加器、生长半径修改器、拓扑区域分割器和拓扑区域调整器实现将区域分割为与环境无关的几何结构;具体步骤如下:
S1.1根据机器人于栅格地图上运动轨迹,通过生长点初始化器随机选取初始化生长点,经环形扫描器扫描当前区域顶点外围的所有直接相邻点;
S1.2合格点筛选器使用Bresenham划线算法计算生长点到每个直接相邻点的射线,筛选所有合格点;
合格点满足条件如下:
1)该点所处区域于栅格地图为空闲状态;
2)该点所处区域于拓扑地图中为非拓扑区域;
3)由生长点向该点发出的射线经过的所有点同时满足条件1)和条件2);
S1.3合格点添加器使用Graham扫描算法计算合格点添加进当前区域后的凸包,要求合并区域的障碍物比率小于设置的阈值,保留障碍物比率满足要求的合格点;
S1.4生长半径修改器将S1.3保留的合格点添加到拓扑区域;计算当前状态下的点合格比率和点添加比率;点添加比率小于设置阈值完成拓扑区域生长;点添加比率处于设置阈值内,以点合格比率修改拓扑区域生长最大值,公式如下:
S1.5拓扑区域分割器判断区域动态生长至最大半径或者点添加比率大于设定阈值时退出当前区域生长;对区域进行二次生长,重复S1.1-S1.4生长其他区域至拓扑区域覆盖大于95%的环境;
S1.6拓扑区域调整器控制每一区域生长结束后,更新该区域中心;每一拓扑区域均为由该区域的凸包点组成的不规则凸多边形,以多边形的重心作为新的中心点;各拓扑区域合并前,利用Kruskal算法对拓扑地图进行调整,得到拓扑地图的最小生成树形式,简化地图结构。
5.根据权利要求1或4所述的地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,所述拓扑地图表示模块,利用信息转换器将拓扑地图信息和占用信息转换为彩色图片的RGB值,步骤如下:
S2.1拓扑地图信息由角度测量,用于表示拓扑邻域,范围为[0,360];
S2.2占用信息表示与光谱颜色的接近程度,范围为[0,1];值越大,该点存在障碍的可能性越大;
S2.3转换占用信息和拓扑地图信息为HSV信息格式;转换公式如下所示;
其中,OM为占用信息,TM为拓扑地图信息,a为起始值,b是间隔值,k为模型系数;
根据OM和TM计算中间变量g,p,q,公式如下所示;
V=1;
S2.4 HSV-RGB颜色空间转换器V=1;通过下式获得RGB信息;
6.根据权利要求3或5所述的地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,所述惯性传感器,其得到矿车的位姿信息具体步骤如下:
步骤3.1:惯性信息数据采集;不计滚转角和俯仰角的变化;惯性传感器发送数据至中央控制模块;惯性信息数据包括2位数据信息和9位惯性信息原始数据;
步骤3.2:惯性信息数据结算;中央控制模块设定三种数据信息:分别为加速度、角速度和角度;根据数据信息进行结算,结算后的数据信息传到中央控制模块,用于数据融合;数据信息计算方式如下:
数据信息为加速度:
数据信息为角速度:
数据信息为角度:
其中,g代表重力加速度;x,y,z分别代表三个坐标系;Wx,Wy,Wz为角速度的xyz轴分解;ax,ay,az为加速度的xyz轴分解,ROLL为翻滚角,Pitch为俯仰角,Yaw为偏航角;H代表二进制数据高八位,L代表二进制数据低八位;
加速度信息进行积分得到矿车的坐标信息,角速度和角度的计算得到矿车的航向信息。
7.根据权利要求6所述的地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,所述超宽带传感器,其计算位置坐标具体步骤为:
S4.1标签传感器在时间t向基站传感器发送消息,在t′时间接收到基站传感器返回的信息,通过时间差、信号传播速度和信号地址测得矿车到达具体固定点的距离;同时标签传感器通过无线传输设备把地址和距离信息发送至中央控制模块;
S4.2根据地址和距离信息,根据三点定位算法计算出矿车位置坐标,具体如下公式:
其中(x,y)表示此时矿车位置实时坐标信息,(x1,y1)、(x2,y2),(x3,y3)分别表示基站传感器所在位置坐标,其中di表示超宽带模块基站传感器和标签传感器之间的距离;i=1,2,3
最小二乘法进行优化计算距离,具体如下:
将式(8)展开得到:
xi 2+yi 2-2xix-2yiy+x2+y2=di 2 (9)
矩阵形式为:
dxy为矿车携带的标签类传感器在世界坐标系中与原点的距离,duwbi为固定基站类传感器在世界坐标系中与原点的距离;利用最小二乘法求解:X=(ATA)-1ATb结算出目标点的坐标;
S4.3惯性传感器测得的位姿坐标和S4.2计算出的位置坐标通过滤波方法进行融合求得矿车的精确的位置坐标信息。
8.根据权利要求4或5所述的地下矿山环境下的拓扑地图构建与矿车定位系统,其特征在于,所述通过滤波方法进行融合求得矿车的精确的位置坐标信息如下:
首先确定拓扑地图构建与矿车定位系统的状态方程,xk=(x,y)T;其中(x,y)代表矿车的实时位置坐标信息,xk代表的是k时刻的系统状态向量;惯性信息传感器通过前一个时刻的状态值对下一个时刻进行下一步预算,建立系统k时刻以k-1时刻为基础的函数模型:
对于超宽带传感器测量到的数据建立相关的数学模型,矿车在xk位置上,产生观测数据zk,hk描述关系如下:
根据后验值来更新下式,后验值为真值;获得矿车的位置坐标:
其中系数K为:
其中Rk代表噪声协方差矩阵;
先验协方差矩阵Pk|k-1为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210642614.4A CN115046541A (zh) | 2022-06-08 | 2022-06-08 | 一种地下矿山环境下的拓扑地图构建与矿车定位系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210642614.4A CN115046541A (zh) | 2022-06-08 | 2022-06-08 | 一种地下矿山环境下的拓扑地图构建与矿车定位系统 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115046541A true CN115046541A (zh) | 2022-09-13 |
Family
ID=83161184
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210642614.4A Pending CN115046541A (zh) | 2022-06-08 | 2022-06-08 | 一种地下矿山环境下的拓扑地图构建与矿车定位系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115046541A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117849708A (zh) * | 2024-03-07 | 2024-04-09 | 奥鼎智通(北京)科技有限公司 | 一种基于多传感器建图的超宽带定位方法及系统 |
-
2022
- 2022-06-08 CN CN202210642614.4A patent/CN115046541A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117849708A (zh) * | 2024-03-07 | 2024-04-09 | 奥鼎智通(北京)科技有限公司 | 一种基于多传感器建图的超宽带定位方法及系统 |
CN117849708B (zh) * | 2024-03-07 | 2024-05-17 | 奥鼎智通(北京)科技有限公司 | 一种基于多传感器建图的超宽带定位方法及系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108225302B (zh) | 一种石化工厂巡检机器人定位系统和方法 | |
CN108955702B (zh) | 基于三维激光和gps惯性导航系统的车道级地图创建系统 | |
CN111240319B (zh) | 室外多机器人协同作业系统及其方法 | |
CN111337030B (zh) | 一种基于背负式的激光雷达扫描系统、导航定位方法 | |
CN112066982B (zh) | 一种在高动态环境下的工业移动机器人定位方法 | |
CN107817509A (zh) | 基于rtk北斗和激光雷达的巡检机器人导航系统及方法 | |
CN109143259A (zh) | 面向港口无人集卡的高精地图制作方法 | |
CN109946731A (zh) | 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法 | |
CN109145677A (zh) | 障碍物检测方法、装置、设备及存储介质 | |
CN108088445A (zh) | 基于八叉树表示的三维栅格地图路径规划系统及方法 | |
CN113534844B (zh) | 一种未知环境下的旋翼飞行器输电线路巡检方法及装置 | |
EP4148385A1 (en) | Vehicle navigation positioning method and apparatus, and base station, system and readable storage medium | |
CN110208783B (zh) | 基于环境轮廓的智能车辆定位方法 | |
CN116993928B (zh) | 一种基于无人机遥感技术的城市工程测绘方法及系统 | |
CN113075686B (zh) | 一种基于多传感器融合的电缆沟智能巡检机器人建图方法 | |
CN111754638A (zh) | 一种堆场内的自动抑尘降尘系统及堆场内抑尘降尘方法 | |
CN109146990B (zh) | 一种建筑轮廓的计算方法 | |
CN115046541A (zh) | 一种地下矿山环境下的拓扑地图构建与矿车定位系统 | |
CN111024059A (zh) | 用于厂区内三维空间的高精度人员定位系统及方法 | |
CN113804182A (zh) | 一种基于信息融合的栅格地图创建方法 | |
CN112985428B (zh) | 基于安全角度的高精传感地图的图层的优先级参取方法 | |
CN113223168B (zh) | 一种隧道三维形貌快速建模的方法 | |
CN112348950B (zh) | 一种基于激光点云分布特性的拓扑地图节点生成方法 | |
CN112747752B (zh) | 基于激光里程计的车辆定位方法、装置、设备和存储介质 | |
Qingquan et al. | Research on laser range scanning and its application |
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 |