CN114119752A - 基于gnss和视觉的室内外衔接的机器人定位方法 - Google Patents
基于gnss和视觉的室内外衔接的机器人定位方法 Download PDFInfo
- Publication number
- CN114119752A CN114119752A CN202111447907.9A CN202111447907A CN114119752A CN 114119752 A CN114119752 A CN 114119752A CN 202111447907 A CN202111447907 A CN 202111447907A CN 114119752 A CN114119752 A CN 114119752A
- Authority
- CN
- China
- Prior art keywords
- robot
- image
- gnss
- points
- pixel
- 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
- 238000000034 method Methods 0.000 title claims abstract description 42
- 230000008569 process Effects 0.000 claims abstract description 10
- 238000013519 translation Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000001914 filtration Methods 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 3
- 230000002093 peripheral effect Effects 0.000 claims description 3
- 238000011524 similarity measure Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims 1
- 230000000007 visual effect Effects 0.000 abstract description 18
- 230000004927 fusion Effects 0.000 abstract description 3
- 230000008878 coupling Effects 0.000 description 5
- 238000010168 coupling process Methods 0.000 description 5
- 238000005859 coupling reaction Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000003032 molecular docking Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- 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
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
-
- 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
- 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
- G06T7/85—Stereo 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/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Software Systems (AREA)
- Biophysics (AREA)
- Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- Evolutionary Computation (AREA)
- Computational Linguistics (AREA)
- Biomedical Technology (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Algebra (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Databases & Information Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
Abstract
本发明提供了基于GNSS和视觉的室内外衔接的机器人定位方法,属于配网工程现场移动机器人定位导航领域。本发明在机器人操作***上使用,机器人操作***上解析GNSS数据,根据GNSS状态判断机器人所处环境,如机器人处在室外,则采用GNSS和视觉里程计融合定位,如机器人在室内,则以视觉传感器估计的视觉里程计作为机器人的位姿。在室内外局部环境中,通过导向标签模板匹配结果为机器人运动做出决策,并在运动途中,使用机器人操作***检测二维码形式的定位标签为机器人提供精确的定位信息。本发明在配网工程现场室内外局部环境达到移动机器人分米级定位精度,并实现了在配网工程现场室内外局部环境自适应导航。
Description
技术领域
本发明属于配网工程现场移动机器人定位导航领域,具体涉及基于GNSS和视觉的室内外衔接的机器人定位方法。
背景技术
现有的机器人导航方法包括:视觉导航和卫星导航。
视觉导航的视觉传感器设备简单、成本低、经济性好、应用范围较广,同时计算机硬件的发展为高吞吐量的数据处理提供了支持。但是,在视觉导航中光照变化、目标运动等因素给图像处理带来噪声,同时特征点误匹配产生误差以及误差累计,导致机器人运动过程中定位精度逐渐下降,甚至可能在运动过程中由于图像中的特征点不足而导致定位丢失。因此,单一的视觉传感器无法完成移动机器人的定位。与此同时,视觉导航在室内外的定位效果常常会有较大差距。
全球卫星导航技术(GNSS)在室外空旷地区可以提供最高厘米级的定位精度,但在城市中,由于特殊的人工环境,例如:平整的路面、高大的建筑物等,常常使得接收机在接到卫星信号的同时,也接收到周围环境反射的卫星信号,从而产生多路径误差。对于多路径效应误差的处理方法很多,但目前并没有完全消除的方法。在遮挡比较严重的情况下,特别是室内环境,甚至接收不到卫星信号,卫星定位彻底失效。
目前,关于GNSS和视觉的组合导航方法,主要是GNSS和视觉SLAM融合。对于GNSS和视觉SLAM融合,常常通过两者单独运动估计的松耦合和合并两者状态进行总体状态估计的紧耦合两种方法对二者进行不同程度的融合。另外,还有关于图像处理辅助GNSS和视觉里程计融合的方法,一般是针对结构化道路而进行的,通过对车道线这样的路径标识进行图像处理判断大体误差,进而采用不同的方法对GNSS和视觉里程计进行融合。
对于GNSS和视觉SLAM融合的方法而言,无论是松耦合还是紧耦合,对于室内外情况下移动机器人的导航定位都能达到较好的效果,但是视觉SLAM框架包括前端视觉里程计、后端优化、回环检测和建图四个部分,是运算密集型的算法,运行过程中CPU负荷大,因此其对CPU的依赖程度高。另外,环境的场景相似度对回环检测影响较大,当场景相似度过高,可能导致***错误闭环,影响机器人的导航定位。
在机器人的自适应导航上,一般需要构建出全局或局部地图。一方面,视觉SLAM无法直接建成栅格地图用于机器人自主导航;另一方面,视觉构建栅格地图,需要稠密立体匹配,再将稠密空间点云降维处理,最后通过二值贝叶斯滤波器实现建图,对GPU和CUP要求较高,在嵌入式上应用常常实时性难以满足。
发明内容
为了克服上述现有技术存在的不足,本发明提供了基于GNSS和视觉的室内外衔接的机器人定位方法。
为了实现上述目的,本发明提供如下技术方案:
基于GNSS和视觉的室内外衔接的机器人定位方法,包括以下步骤:
当无法获取机器人位置真值时,通过GNSS信号和视觉传感器的输出图像判断机器人的位姿和速度,根据机器人的位姿和速度进行机器人定位;否则,通过机器人操作***识别周边二维码形式的定位标签获取机器人位置真值,利用位置真值覆盖估计信息,进行重定位。
优选的,所述通过GNSS信号和视觉传感器的输出图像判断机器人的位姿和速度,根据机器人的位姿和速度进行机器人定位的步骤包括:
通过GNSS信号判断机器人的位姿、速度;
对视觉传感器的输出图像进行特征匹配获得机器人位姿;
通过机器人操作***对GNSS信号和视觉传感器的输出信号进行时间戳同步;
通过机器人操作***融合GNSS信号判断的机器人的位姿、速度和通过视觉传感器获得机器人位姿信息,获得机器人位姿和速度估计信息,利用估计信息对机器人定位。
优选的,所述通过GNSS信号判断机器人的位姿、速度的步骤包括:
解析GNSS的GPGGA语句信息,获取GNSS状态和使用卫星数量;
当GNSS状态为0且使用卫星数量为00,判定机器人在室内;当GNSS状态为1或2且使用卫星数量不小于04,判定机器人在室外,当接收到GNSS定位数据时,判定机器人所处环境有信号,否则判定机器人所处环境无信号;
当机器人在室外且信号良好,利用GNSS信息计算机器人的位姿、速度,包括以下步骤:
设定机器人运动过程中的起始点为s0,任意时刻机器人运动过程中连续两点为sn和sn+1,sn和sn+1之间的时间间隔为tn,n+1,sn的经纬度为(lonsn,latsn),sn+1的经纬度为(lonsn+1,latsn+1);
sn和sn+1之间的距离dn,n+1表示如下,
其中,R为地球平均半径;
sn+1相对于sn的方位角θn表示如下,
设定s0为坐标系原点,以正北方向为坐标系的x轴建立遵循右手定则的GNSS坐标系;
通过任意连续两点之间的距离和方位角估计任意时刻机器人位姿如下,
通过任意连续两点之间的距离、方位角和时间间隔估计任意时刻机器人速度如下:
其中,vx为机器人在GNSS坐标系x轴方向的运动速度,vy为机器人在GNSS坐标系y轴方向的运动速度,ω为机器人在GNSS坐标系运动的角速度。
优选的,所述对视觉传感器的输出图像进行特征匹配获得机器人位姿的步骤包括:
检测获得多个ORB角点;计算每个ORB角点方向;计算每个ORB角点的描述子;
对相邻两帧图像中的ORB角点通过描述子进行暴力匹配,获得成功匹配的2d路标点;
利用opencv中SGBM算法,获取输出图像的视差图;
对视差图中像素值非零的像素点进行深度计算,获得深度图;
通过深度图和2d路标点获取匹配的3d路标点;
通过SVD分解3d路标点获取机器人位姿。
优选的,
所述检测获得ORB角点的步骤包括:
设定阈值t,假设输出图像上任一个像素点p的亮度为Ip;
以每个一个像素点为圆心,以r为半径画圆;
取每个圆上均匀分布的r+1个像素,设定该r+1个像素中任一个像素的亮度为I;
当任一个圆上满足|I-Ip|<t的像素点数目小于r,则该圆的圆心所在的像素点不是ORB角点;否则,对该圆上的所有像素点进行判断,当任一个圆上满足|I-Ip|<t的连续像素点的数目大于等于r2,则该圆的圆心所在的像素点是ORB角点,否则,则该圆的圆心所在的像素点不是ORB角点;
所述计算ORB角点方向的步骤包括:
对每个ORB角点,取以该ORB角点为圆心,半径为R的图像块A,计算图像块A的矩,
其中0<p<1,0<q<1;x为图像块A上的一个像素点的横坐标,y为图像块A上的一个像素点的z纵坐标,I(x,y)为坐标在(x,y)处像素点的灰度值;
通过图像块A的矩计算图像块A的质心,
连接每个图像块的几何中心和质心得到向量,将该向量的方向定义为每个ORB角点的方向,表示为,
α=tan-12(cy,cx)=tan-12(m01,m10)
其中,cy表示向量的纵坐标,cx表示向量的横坐标;
所述计算每个ORB角点的描述子的步骤包括:
以每个ORB角点为中心设定邻域窗口;
在每邻域窗口中任取128对像素点,通过下式计算每对像素点对应的特征向量,
其中,p(x)<p(y)分别为一对像素点中两个像素点的像素值;
每个ORB角点所在领域窗口中128对像素点对应的特征向量即为该ORB角点的描述子。
优选的,所述对相邻两帧图像中的ORB角点通过描述子进行暴力匹配的步骤包括:
在相邻的每两帧图像中,计算一张图像的每一个ORB角点与另一张图像中的所有每一个ORB角点的汉明距离;
判定相邻的两帧图像中,汉明距离最近的点为两帧图像中的匹配点;
剔除所有匹配点中汉明距离大于两倍最小汉明距离的匹配点;
利用opencv中SGBM算法,获取输出图像的视差图。
优选的,通过下式对视差图中像素值非零的像素点进行深度计算:
其中,Z为像素点深度,d为像素点视差,b为双目基线长度,f为相机焦距。
优选的,所述通过SVD分解3d路标点获取机器人位姿的步骤包括:
通过深度图和2d路标点获取任意两个时刻匹配的两组3d路标点,
P={p1,…,pn}
P′={p1′,…,pn′}
将两组3d路标点之间存在的坐标变换表示如下,
其中,R和t分别为任意两个时刻相机的旋转矩阵和平移向量;
将两组3d路标点配对,每对路标点pi和pi′之间存在的误差表示为,
ei=pi-(Rpi′+t)
通过每对3d路标点pi和pi′的误差对其构造最小二乘问题,
采用SVD算法对最小二乘问题求解获得最优的旋转矩阵和平移向量,其中SVD算法表示如下,
最优的旋转矩阵表示如下,
R=UVT
最优的平移向量表示如下,
t=p-RP′
其中,
qi=pi-p
qi′=pi′-p′
通过每个时刻相机的旋转矩阵和平移向量进行机器人位姿估计。
优选的,还包括以下步骤:根据定位信息,利用机器人的导引标签模板指引机器人运动。
优选的,所述利用机器人的导引标签模板指引机器人运动的步骤包括:
获得输出图像的ROI图像;
创建ROI图像和模板图像的图像金字塔;
利用图像金字塔获取输出图和模板图像的相似性度量;根据相似性度量判断最匹配的模板图像;
根据模板图像的导引标签指引机器人运动;其中,导引标签信息包括:平移、转动、停止和掉头;
所述创建ROI图像和模板图像的图像金字塔的步骤如下:
创建ROI图像和模板图像对应的4层金字塔图像;
对4层金字塔图像进行降采样;
取5×5的卷积核K,遍历降采样后的4层金字塔图像的每个像素,进行均值平滑滤波,其中卷积核K表示如下,
所述利用图像金字塔获取输出图和模板图像的相似性度量;根据相似性度量判断最匹配的模板图像的步骤包括:
设定阈值;
利用归一化相关系数的相似性度量准则,计算每个模板图像与ROI图像的相似性值,
其中,I1为模板图像,I2为ROI图像,u1为图像I1的均值,u2为图像I2的均值,x为一个图像内像素点的值;
将归一化相关系数的相似度最高且大于阈值的模板图像作为最匹配的模板图像。
本发明提供的基于GNSS和视觉的室内外衔接的机器人定位方法具有以下有益效果:本发明通过定位标签实现精准重定位,消除传感器估计误差随着时间推移增大的现象,在室内外不同场景下实现高精度快速定位,为机器人导航***提供可靠基础,此外,能在室内转向室外或由室外进入室内环境时自适应的切换导航***,并利用导引标签实现自主导航,为机器人在室内外局部安全区域规划出一条安全路径,实现局部导航。
附图说明
为了更清楚地说明本发明实施例及其设计方案,下面将对本实施例所需的附图作简单地介绍。下面描述中的附图仅仅是本发明的部分实施例,对于本领域普通技术人员来说,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例1的基于GNSS和视觉的室内外衔接的机器人定位方法的流程图;
图2为本发明实施例1的对视觉传感器的输出图像进行特征匹配获得机器人位姿的流程图;
图3为本发明实施例1的利用机器人的导引标签模板指引机器人运动的流程图。
具体实施方式
为了使本领域技术人员更好的理解本发明的技术方案并能予以实施,下面结合附图和具体实施例对本发明进行详细说明。以下实施例仅用于更加清楚地说明本发明的技术方案,而不能以此来限制本发明的保护范围。
实施例1
本发明应用ROS框架,基于室内与室外GNSS信号的不同,通过检测卫星接收数量判断机器人是处于室内或是室外,以此为基础通过GNSS和视觉松耦合方式实现室内室外自适应局部重定位的功能。
参阅图1,本发明提供了基于GNSS和视觉的室内外衔接的机器人定位方法,包括以下步骤:当无法获取机器人位置真值时,通过GNSS信号和视觉传感器的输出图像判断机器人的位姿和速度,根据机器人的位姿和速度进行机器人定位;否则,通过机器人操作***识别周边二维码形式的定位标签获取机器人位置真值,利用位置真值覆盖估计信息,进行重定位。
具体的,通过GNSS信号和视觉传感器的输出图像判断机器人的位姿和速度,根据机器人的位姿和速度进行机器人定位的步骤包括:通过GNSS信号判断机器人的位姿、速度;对视觉传感器的输出图像进行特征匹配获得机器人位姿;通过机器人操作***对GNSS信号和视觉传感器的输出信号进行时间戳同步;通过机器人操作***融合GNSS信号判断的机器人的位姿、速度和通过视觉传感器获得机器人位姿信息,获得机器人位姿和速度估计信息,利用估计信息对机器人定位;据定位信息,利用机器人的导引标签模板指引机器人运动。
具体的,信号判断机器人的位姿、速度的步骤包括:解析GNSS的GPGGA语句信息,获取GNSS状态和使用卫星数量;当GNSS状态为0且使用卫星数量为00,判定机器人在室内;当GNSS状态为1或2且使用卫星数量不小于04,判定机器人在室外,当接收到GNSS定位数据时,判定机器人所处环境有信号,否则判定机器人所处环境无信。机器人在室外且信号良好,利用GNSS信息计算机器人的位姿、速度。
利用GNSS信息计算机器人的位姿、速度包括下步骤:设定机器人运动过程中的起始点为s0,任意时刻机器人运动过程中连续两点为sn和sn+1,sn和sn+1之间的时间间隔为tn,n+1,sn的经纬度为(lonsn,latsn),sn+1的经纬度为(lonsn+1,latsn+1)。
sn和sn+1之间的距离dn,n+1表示如下,
其中,R为地球平均半径。
sn+1相对于sn的方位角θn表示如下,
设定s0为坐标系原点,以正北方向为坐标系的x轴建立遵循右手定则的GNSS坐标系;通过任意连续两点之间的距离和方位角估计任意时刻机器人位姿如下,
通过任意连续两点之间的距离、方位角和时间间隔估计任意时刻机器人速度如下:
其中,vx为机器人在GNSS坐标系x轴方向的运动速度,vy为机器人在GNSS坐标系y轴方向的运动速度,ω为机器人在GNSS坐标系运动的角速度。
参阅图2,对视觉传感器的输出图像进行特征匹配获得机器人位姿的步骤包括:检测获得多个ORB角点;计算每个ORB角点方向;计算每个ORB角点的描述子;对相邻两帧图像中的ORB角点通过描述子进行暴力匹配,获得成功匹配的2d路标点;利用opencv中SGBM算法,获取输出图像的视差图;对视差图中像素值非零的像素点进行深度计算,获得深度图;通过深度图和2d路标点获取匹配的3d路标点;通过SVD分解3d路标点获取机器人位姿。
具体的,检测获得ORB角点的步骤包括:设定阈值t,假设输出图像上任一个像素点p的亮度为Ip;以每个一个像素点为圆心,以r为半径画圆;取每个圆上均匀分布的r+1个像素,设定该r+1个像素中任一个像素的亮度为I;当任一个圆上满足|I-Ip|<t的像素点数目小于r,则该圆的圆心所在的像素点不是ORB角点;否则,对该圆上的所有像素点进行判断,当任一个圆上满足|I-Ip|<t的连续像素点的数目大于等于r2,则该圆的圆心所在的像素点是ORB角点,否则,则该圆的圆心所在的像素点不是ORB角点。
具体的,计算ORB角点方向的步骤包括:对每个ORB角点,取以该ORB角点为圆心,半径为R的图像块A,计算图像块A的矩,
其中0<p<1,0<q<1;x为图像块A上的一个像素点的横坐标,y为图像块A上的一个像素点的z纵坐标,I(x,y)为坐标在(x,y)处像素点的灰度值。
通过图像块A的矩计算图像块A的质心,
连接每个图像块的几何中心和质心得到向量,将该向量的方向定义为每个ORB角点的方向,表示为,
α=tan-12(cy,cx)=tan-12(m01,m10)
其中,cy表示向量的纵坐标,cx表示向量的横坐标。
具体的,计算每个ORB角点的描述子的步骤包括:以每个ORB角点为中心设定邻域窗口;在每邻域窗口中任取128对像素点,通过下式计算每对像素点对应的特征向量,
其中,p(x)<p(y)分别为一对像素点中两个像素点的像素值;每个ORB角点所在领域窗口中128对像素点对应的特征向量即为该ORB角点的描述子。
具体的,对相邻两帧图像中的ORB角点通过描述子进行暴力匹配的步骤包括:在相邻的每两帧图像中,计算一张图像的每一个ORB角点与另一张图像中的所有每一个ORB角点的汉明距离;判定相邻的两帧图像中,汉明距离最近的点为两帧图像中的匹配点;剔除所有匹配点中汉明距离大于两倍最小汉明距离的匹配点;利用opencv中SGBM算法,获取输出图像的视差图。
具体的,通过下式对视差图中像素值非零的像素点进行深度计算:
其中,Z为像素点深度,d为像素点视差,b为双目基线长度,f为相机焦距。
在本实施例中,通过SVD分解3d路标点获取机器人位姿的步骤包括:通过深度图获得任意两个时刻的两组路标点,
P={p1,…,pn}
P′={p1′,…,pn′}
将两组路标点之间存在的坐标变换表示如下,
其中,R和t分别为任意两个时刻相机的旋转矩阵和平移向量。
将两组路标点配对,每对路标点pi和pi′之间存在的误差表示为,
ei=pi-(Rpi′+t)
通过每对路标点pi和pi′的误差对其构造最小二乘问题,
采用SVD算法对最小二乘问题求解获得最优的旋转矩阵和平移向量,其中SVD算法表示如下,
最优的旋转矩阵表示如下,
R=UVT
最优的平移向量表示如下,
t=p-RP′
其中,
qi=pi-p
qi′=pi′-p′
通过每个时刻相机的旋转矩阵和平移向量进行机器人位姿估计。
参阅图3,利用机器人的导引标签模板指引机器人运动的步骤包括:获得输出图像的ROI图像;创建ROI图像和模板图像的图像金字塔;利用图像金字塔获取输出图和模板图像的相似性度量;根据相似性度量判断最匹配的模板图像;根据模板图像的导引标签指引机器人运动;其中,导引标签信息包括:平移、转动、停止和掉头。
进一步的,创建ROI图像和模板图像的图像金字塔的步骤如下:创建ROI图像和模板图像对应的4层金字塔图像;对4层金字塔图像进行降采样;取5×5的卷积核K,遍历降采样后的4层金字塔图像的每个像素,进行均值平滑滤波,其中卷积核K表示如下,
具体的,利用图像金字塔获取输出图和模板图像的相似性度量;根据相似性度量判断最匹配的模板图像的步骤包括:设定阈值;利用归一化相关系数的相似性度量准则,计算每个模板图像与ROI图像的相似性值,
其中,I1为模板图像,I2为ROI图像,u1为图像I1的均值,u2为图像I2的均值,x为一个图像内像素点的值;将归一化相关系数的相似度最高且大于阈值的模板图像作为最匹配的模板图像。
机器人主要由运动底盘和运动控制***构成,在激光、视觉、IMU、GNSS辅助下实现自主定位和导航。例如:通过判断卫星信号确定室内场景,并在其它传感器的帮助下实现自主定位导航,当由室内转向室外时,接收到卫星信号,利用本发明的基于GNSS和视觉的室内外衔接的机器人定位方法自动切换到室外导航,并在标签的引导下实现自主导航。在不同场景下的定位准确且导航精度有显著提高。
以上实施例仅为本发明较佳的具体实施方式,本发明的保护范围不限于此,任何熟悉本领域的技术人员在本发明披露的技术范围内,可显而易见地得到的技术方案的简单变化或等效替换,均属于本发明的保护范围。
Claims (10)
1.基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,包括以下步骤:
当无法获取机器人位置真值时,通过GNSS信号和视觉传感器的输出图像判断机器人的位姿和速度,根据机器人的位姿和速度进行机器人定位;
否则,通过机器人操作***识别周边二维码形式的定位标签获取机器人位置真值,利用位置真值覆盖估计信息,进行重定位。
2.权利要求1所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,所述通过GNSS信号和视觉传感器的输出图像判断机器人的位姿和速度,根据机器人的位姿和速度进行机器人定位的步骤包括:
通过GNSS信号判断机器人的位姿、速度;
对视觉传感器的输出图像进行特征匹配获得机器人位姿;
通过机器人操作***对GNSS信号和视觉传感器的输出信号进行时间戳同步;
通过机器人操作***融合GNSS信号判断的机器人的位姿、速度和通过视觉传感器获得机器人位姿信息,获得机器人位姿和速度估计信息,利用估计信息对机器人定位。
3.根据权利要求2所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,所述通过GNSS信号判断机器人的位姿、速度的步骤包括:
解析GNSS的GPGGA语句信息,获取GNSS状态和使用卫星数量;
当GNSS状态为0且使用卫星数量为00,判定机器人在室内;当GNSS状态为1或2且使用卫星数量不小于04,判定机器人在室外,当接收到GNSS定位数据时,判定机器人所处环境有信号,否则判定机器人所处环境无信号;
当机器人在室外且信号良好,利用GNSS信息计算机器人的位姿、速度,包括以下步骤:
设定机器人运动过程中的起始点为s0,任意时刻机器人运动过程中连续两点为sn和sn+1,sn和sn+1之间的时间间隔为tn,n+1,sn的经纬度为(lonsn,latsn),sn+1的经纬度为(lonsn+1,latsn+1);
sn和sn+1之间的距离dn,n+1表示如下,
其中,R为地球平均半径;
sn+1相对于sn的方位角θn表示如下,
设定s0为坐标系原点,以正北方向为坐标系的x轴建立遵循右手定则的GNSS坐标系;
通过任意连续两点之间的距离和方位角估计任意时刻机器人位姿如下,
通过任意连续两点之间的距离、方位角和时间间隔估计任意时刻机器人速度如下:
其中,vx为机器人在GNSS坐标系x轴方向的运动速度,vy为机器人在GNSS坐标系y轴方向的运动速度,ω为机器人在GNSS坐标系运动的角速度。
4.根据权利要求2所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,所述对视觉传感器的输出图像进行特征匹配获得机器人位姿的步骤包括:
检测获得多个ORB角点;计算每个ORB角点方向;计算每个ORB角点的描述子;
对相邻两帧图像中的ORB角点通过描述子进行暴力匹配,获得成功匹配的2d路标点;
利用opencv中SGBM算法,获取输出图像的视差图;
对视差图中像素值非零的像素点进行深度计算,获得深度图;
通过深度图和2d路标点获取匹配的3d路标点;
通过SVD分解3d路标点获取机器人位姿。
5.根据权利要求4所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,所述检测获得ORB角点的步骤包括:
设定阈值t,假设输出图像上任一个像素点p的亮度为Ip;
以每个一个像素点为圆心,以r为半径画圆;
取每个圆上均匀分布的r+1个像素,设定该r+1个像素中任一个像素的亮度为I;
当任一个圆上满足|I-Ip|<t的像素点数目小于r,则该圆的圆心所在的像素点不是ORB角点;否则,对该圆上的所有像素点进行判断,当任一个圆上满足|I-Ip|<t的连续像素点的数目大于等于r2,则该圆的圆心所在的像素点是ORB角点,否则,则该圆的圆心所在的像素点不是ORB角点;
所述计算ORB角点方向的步骤包括:
对每个ORB角点,取以该ORB角点为圆心,半径为R的图像块A,计算图像块A的矩,
其中0<p<1,0<q<1;x为图像块A上的一个像素点的横坐标,y为图像块A上的一个像素点的z纵坐标,I(x,y)为坐标在(x,y)处像素点的灰度值;
通过图像块A的矩计算图像块A的质心,
连接每个图像块的几何中心和质心得到向量,将该向量的方向定义为每个ORB角点的方向,表示为,
α=tan-12(cy,cx)=tan-12(m01,m10)
其中,cy表示向量的纵坐标,cx表示向量的横坐标;
所述计算每个ORB角点的描述子的步骤包括:
以每个ORB角点为中心设定邻域窗口;
在每邻域窗口中任取128对像素点,通过下式计算每对像素点对应的特征向量,
其中,p(x)<p(y)分别为一对像素点中两个像素点的像素值;
每个ORB角点所在领域窗口中128对像素点对应的特征向量即为该ORB角点的描述子。
6.根据权利要求4所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,所述对相邻两帧图像中的ORB角点通过描述子进行暴力匹配的步骤包括:
在相邻的每两帧图像中,计算一张图像的每一个ORB角点与另一张图像中的所有每一个ORB角点的汉明距离;
判定相邻的两帧图像中,汉明距离最近的点为两帧图像中的匹配点;
剔除所有匹配点中汉明距离大于两倍最小汉明距离的匹配点;
利用opencv中SGBM算法,获取输出图像的视差图。
8.根据权利要求4所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,所述通过SVD分解3d路标点获取机器人位姿的步骤包括:
通过深度图和2d路标点获取任意两个时刻匹配的两组3d路标点,
P={p1,…,pn}
P′={p1′,...,pn′}
将两组3d路标点之间存在的坐标变换表示如下,
其中,R和t分别为任意两个时刻相机的旋转矩阵和平移向量;
将两组3d路标点配对,每对路标点pi和pi′之间存在的误差表示为,
ei=pi-(Rpi′+t)
通过每对3d路标点pi和pi′的误差对其构造最小二乘问题,
采用SVD算法对最小二乘问题求解获得最优的旋转矩阵和平移向量,其中SVD算法表示如下,
最优的旋转矩阵表示如下,
R=UVT
最优的平移向量表示如下,
t=p-RP′
其中,
qi=pi-p
qi′=pi′-p′
通过每个时刻相机的旋转矩阵和平移向量进行机器人位姿估计。
9.根据权利要求1所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,还包括以下步骤:根据定位信息,利用机器人的导引标签模板指引机器人运动。
10.根据权利要求9所述的基于GNSS和视觉的室内外衔接的机器人定位方法,其特征在于,所述利用机器人的导引标签模板指引机器人运动的步骤包括:
获得输出图像的ROI图像;
创建ROI图像和模板图像的图像金字塔;
利用图像金字塔获取输出图和模板图像的相似性度量;根据相似性度量判断最匹配的模板图像;
根据模板图像的导引标签指引机器人运动;其中,导引标签信息包括:平移、转动、停止和掉头;
所述创建ROI图像和模板图像的图像金字塔的步骤如下:
创建ROI图像和模板图像对应的4层金字塔图像;
对4层金字塔图像进行降采样;
取5×5的卷积核K,遍历降采样后的4层金字塔图像的每个像素,进行均值平滑滤波,其中卷积核K表示如下,
所述利用图像金字塔获取输出图和模板图像的相似性度量;根据相似性度量判断最匹配的模板图像的步骤包括:
设定阈值;
利用归一化相关系数的相似性度量准则,计算每个模板图像与ROI图像的相似性值,
其中,I1为模板图像,I2为ROI图像,u1为图像I1的均值,u2为图像I2的均值,x为一个图像内像素点的值;
将归一化相关系数的相似度最高且大于阈值的模板图像作为最匹配的模板图像。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111447907.9A CN114119752A (zh) | 2021-11-30 | 2021-11-30 | 基于gnss和视觉的室内外衔接的机器人定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111447907.9A CN114119752A (zh) | 2021-11-30 | 2021-11-30 | 基于gnss和视觉的室内外衔接的机器人定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114119752A true CN114119752A (zh) | 2022-03-01 |
Family
ID=80368995
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111447907.9A Pending CN114119752A (zh) | 2021-11-30 | 2021-11-30 | 基于gnss和视觉的室内外衔接的机器人定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114119752A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115578426A (zh) * | 2022-10-25 | 2023-01-06 | 哈尔滨工业大学 | 一种基于稠密特征匹配的室内服务机器人重定位方法 |
CN117036753A (zh) * | 2023-07-18 | 2023-11-10 | 北京观微科技有限公司 | 一种基于模板匹配和InfoGAN的SAR图像扩容方法 |
-
2021
- 2021-11-30 CN CN202111447907.9A patent/CN114119752A/zh active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115578426A (zh) * | 2022-10-25 | 2023-01-06 | 哈尔滨工业大学 | 一种基于稠密特征匹配的室内服务机器人重定位方法 |
CN115578426B (zh) * | 2022-10-25 | 2023-08-18 | 哈尔滨工业大学 | 一种基于稠密特征匹配的室内服务机器人重定位方法 |
CN117036753A (zh) * | 2023-07-18 | 2023-11-10 | 北京观微科技有限公司 | 一种基于模板匹配和InfoGAN的SAR图像扩容方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10788830B2 (en) | Systems and methods for determining a vehicle position | |
Alonso et al. | Accurate global localization using visual odometry and digital maps on urban environments | |
Kümmerle et al. | Large scale graph-based SLAM using aerial images as prior information | |
Guivant et al. | Localization and map building using laser range sensors in outdoor applications | |
EP2660777B1 (en) | Image registration of multimodal data using 3D geoarcs | |
Gonzalez et al. | Combined visual odometry and visual compass for off-road mobile robots localization | |
Suzuki et al. | N-LOS GNSS signal detection using fish-eye camera for vehicle navigation in urban environments | |
上條俊介 et al. | Autonomous vehicle technologies: Localization and mapping | |
Senlet et al. | Satellite image based precise robot localization on sidewalks | |
Callmer et al. | Radar SLAM using visual features | |
CN113776519B (zh) | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 | |
CN114119752A (zh) | 基于gnss和视觉的室内外衔接的机器人定位方法 | |
CN111551171A (zh) | 目标对象的定位方法、装置、机器人和存储介质 | |
Shin et al. | Drivable road region detection using a single laser range finder for outdoor patrol robots | |
CN112068152A (zh) | 使用3d扫描仪同时进行2d定位和2d地图创建的方法和*** | |
Bhamidipati et al. | SLAM-based integrity monitoring using GPS and fish-eye camera | |
Bhamidipati et al. | Integrity monitoring of Graph‐SLAM using GPS and fish‐eye camera | |
Bai et al. | Real-time GNSS NLOS detection and correction aided by sky-pointing camera and 3D LiDAR | |
Wang et al. | UGV‐UAV robust cooperative positioning algorithm with object detection | |
Hara et al. | Vehicle localization based on the detection of line segments from multi-camera images | |
Gu et al. | SLAM with 3dimensional-GNSS | |
Kaiser et al. | Position and orientation of an aerial vehicle through chained, vision-based pose reconstruction | |
Hassani et al. | Analytical and empirical navigation safety evaluation of a tightly integrated LiDAR/IMU using return-light intensity | |
Mounier et al. | High-precision positioning in GNSS-challenged environments: a LiDAR-based multi-sensor fusion approach with 3D digital maps registration | |
Wei | Multi-sources fusion based vehicle localization in urban environments under a loosely coupled probabilistic framework |
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 |