CN116642490A - 基于混合地图的视觉定位导航方法、机器人及存储介质 - Google Patents
基于混合地图的视觉定位导航方法、机器人及存储介质 Download PDFInfo
- Publication number
- CN116642490A CN116642490A CN202310400250.3A CN202310400250A CN116642490A CN 116642490 A CN116642490 A CN 116642490A CN 202310400250 A CN202310400250 A CN 202310400250A CN 116642490 A CN116642490 A CN 116642490A
- Authority
- CN
- China
- Prior art keywords
- camera
- map
- grid
- coordinate system
- image
- 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 40
- 230000000007 visual effect Effects 0.000 title claims abstract description 21
- 238000007781 pre-processing Methods 0.000 claims abstract description 39
- 238000013507 mapping Methods 0.000 claims abstract description 18
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000003287 optical effect Effects 0.000 claims description 12
- 238000013519 translation Methods 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000004040 coloring Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 abstract description 11
- 238000010586 diagram Methods 0.000 description 12
- 238000004422 calculation algorithm Methods 0.000 description 9
- 238000012545 processing Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 5
- 230000008569 process Effects 0.000 description 3
- 239000003086 colorant Substances 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
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/20—Instruments for performing navigational calculations
-
- 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/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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Geometry (AREA)
- Image Processing (AREA)
Abstract
本申请提供一种基于混合地图的视觉定位导航方法、机器人及存储介质,方法包括:获取通过拍摄机器人周边环境得到的相机图像;对相机图像进行预处理,得到相机图像的预处理结果;根据预处理结果,构建对应的混合地图,其中,混合地图是通过将与预处理结果对应的稀疏地图和栅格地图映射在同一坐标系下得到的;获取通过拍摄障碍物得到的深度图像;将深度图像中的图像数据转换为激光雷达数据;基于指定路径终点,并根据混合地图和激光雷达数据,生成路径规划。如此,可以改善单一地图在导航中鲁棒性不强、常规混合地图成本高且传感数据融合难的问题。
Description
技术领域
本发明涉及机器人导航技术领域,具体而言,涉及一种基于混合地图的视觉定位导航方法、机器人及存储介质。
背景技术
移动机器人导航技术处于机器人研究领域的前沿,其中基于同步定位与地图构建理论(Simultaneous Localization and Mapping,SLAM)的导航技术更是该领域的研究热点。在导航中,SLAM主要为其提供地图等先验信息,根据使用传感器的不同可分为视觉SLAM和激光SLAM,其中视觉SLAM以其体积小、成本低,信息丰富等优点,而受到广泛关注。
基于视觉SLAM构建的地图可以分为稀疏地图、稠密地图、栅格地图、混合地图等。稀疏地图一般由点特征、线特征等组成,搭配上词袋模型(Bag of words)非常适合用于全局重定位,但缺乏丰富的环境信息难以应用于移动机器人导航避障任务。稠密地图在稀疏地图基础上丰富了环境信息能够进行导航与避障任务,但这种地图会消耗巨大存储资源与计算资源,不适用于大型环境。栅格地图描述环境比稀疏地图更为细致,同时比稠密地图稀疏,适用于导航与避障,但在视觉导航中定位困难。混合地图是多种地图的结合体,相比于传统地图,在导航中鲁棒性更强、应用范围更广,但这种地图的构建多数基于多传感器数据融合,例如激光+相机,增加成本的同时由于传感器数据的不同也增加了地图融合的难度。
发明内容
有鉴于此,本申请实施例的目的在于提供一种基于混合地图的视觉定位导航方法、机器人及存储介质,能够改善单一地图在导航中鲁棒性不强、常规混合地图成本高且传感数据融合难的问题。
为实现上述技术目的,本申请采用的技术方案如下:
第一方面,本申请实施例提供了一种基于混合地图的视觉定位导航方法,所述方法包括:
获取通过拍摄机器人周边环境得到的相机图像;
对所述相机图像进行预处理,得到所述相机图像的预处理结果,所述预处理结果包括相机图像特征点及相机位置;
根据所述预处理结果,构建对应的混合地图,其中,所述混合地图是通过将与所述预处理结果对应的稀疏地图和栅格地图映射在同一坐标系下得到的;
获取通过拍摄障碍物得到的深度图像;
将所述深度图像中的图像数据转换为激光雷达数据,所述图像数据包括深度图像点在世界坐标系中的坐标;
基于指定路径终点,并根据所述混合地图和所述激光雷达数据,生成路径规划。
结合第一方面,在一些可选的实施方式中,对所述相机图像进行预处理,得到所述相机图像的预处理结果,包括:
对所述相机图像进行特征点提取,以得到所述相机图像特征点;
对相邻时刻的所述相机图像进行特征匹配,得到匹配结果;
基于所述匹配结果,进行运动估计,以确定所述相机位置,并将所述相机图像特征点恢复成地图点。
结合第一方面,在一些可选的实施方式中,根据所述预处理结果,构建对应的混合地图,包括:
根据所述相机图像特征点和所述相机位置,构建得到对应的所述稀疏地图;
根据所述相机图像特征点和所述相机位置,构建得到对应的所述栅格地图;
将所述稀疏地图和所述栅格地图映射在同一坐标系下,以得到混合地图。
结合第一方面,在一些可选的实施方式中,根据所述相机图像特征点和所述相机位置,构建得到对应的所述栅格地图,包括:
将所述相机图像特征点映射在二维平面上;
从所述相机位置投射光线到所述二维平面上的所述相机图像特征点;
为沿着所述光线的每个栅格添加访问计数器,以及,为所述相机图像特征点在所述二维平面上对应的所述栅格添加已占用计数器;
根据所述访问计数器和所述已占用计数器,计算得到所述栅格的占用概率,其中,所述占用概率的计算公式如下:
poccupied(i,j)=occupied(i,j)/visit(i,j)
式中,occupied(i,j)和visit(i,j)分别为所述已占用计数器中的占用数量和所述访问计数器中的访问数量,poccupied(i,j)为所述占用概率;
基于所述占用概率,通过第一预设阈值和第二预设阈值对所述栅格进行分类,得到栅格分类结果,所述栅格分类结果包括空闲栅格、占用栅格和未知栅格;
根据所述栅格分类结果,对所述栅格进行着色,以得到所述栅格地图;
其中,基于所述占用概率,通过第一预设阈值和第二预设阈值对所述栅格进行分类,得到栅格分类结果,包括:
当所述占用概率小于等于所述第一阈值时,确定所述栅格为所述空闲栅格;
当所述占用概率大于等于所述第二阈值时,确定所述栅格为所述占用栅格;
当所述占用概率大于所述第一阈值小于所述第二阈值时,确定所述栅格为所述未知栅格。
结合第一方面,在一些可选的实施方式中,将所述稀疏地图和所述栅格地图映射在同一坐标系下,以得到混合地图,包括:
对所述稀疏地图和所述栅格地图对应的坐标系进行欧式变换,变换公式如下:
式中,a为稀疏地图坐标系下的坐标,a′为栅格地图坐标系下的坐标。
结合第一方面,在一些可选的实施方式中,将所述深度图像中的图像数据转换为激光雷达数据,包括:
获取所述深度图像中的深度图像点;
将所述深度图像点转换为所述世界坐标系下的坐标,以作为所述图像数据;
将所述图像数据转换为所述激光雷达数据。
结合第一方面,在一些可选的实施方式中,将所述深度图像点转换为所述世界坐标系下的坐标,包括:
获取旋转矩阵和平移矩阵,并计算所述深度图像点在所述世界坐标系下的坐标:
式中,u、v为所述深度图像点坐标,u0、v0分别为所述深度图像的中心坐标,xw、yw、zw表示所述世界坐标系下的三维坐标点,f为相机的焦距,dx和dy分别为相机内不同方向上的尺度因子,zc表示相机坐标系的z轴值,R、T分别为外参矩阵的3×3旋转矩阵和3×1的平移矩阵;
其中,所述相机坐标系和所述世界坐标系的坐标原点重合,即zc=zw=z,所述旋转矩阵和所述平移矩阵为:
所述深度图像点到所述世界坐标系的转换公式为:
结合第一方面,在一些可选的实施方式中,将所述图像数据转换为所述激光雷达数据,包括:
计算相机光心0与所述世界坐标系的原点A连线A0、相机光心与所述世界坐标系下任意点投影到所述世界坐标系的x轴上的点C连线AC之间构成的夹角AOC:
θ=tan-1(x/z)
式中,θ为夹角AOC的角度,z和x分别为所述深度图像点投影到与相机同一平面后距相机光心的z轴与x轴上的距离;
将夹角AOC投影到对应的激光数据槽中,其中,所述世界坐标系中的任意点M投影到数组laser[N]中的索引值n表示为:
式中,α、β为激光的范围[α,β],激光束细分为N份,数组laser[N]表示激光数据,laser[n]的值为所述世界坐标系中的任意点M在x轴上的投影点C到相机光心0的距离r,即:
第二方面,本申请实施例还提供一种机器人,所述机器人包括相互耦合的处理器及存储器,所述存储器内存储计算机程序,当所述计算机程序被所述处理器执行时,使得所述机器人执行上述的方法。
第三方面,本申请实施例还提供一种计算机可读存储介质,所述计算机可读存储介质中存储有计算机程序,当所述计算机程序在计算机上运行时,使得所述计算机执行上述的方法。
采用上述技术方案的发明,具有如下优点:
在本申请提供的技术方案中,首先获取通过拍摄机器人周边环境得到的相机图像,并对相机图像进行预处理,得到相机图像的预处理结果,从而根据预处理结果,通过将与预处理结果对应的稀疏地图和栅格地图映射在同一坐标系下得到混合地图;然后获取通过拍摄障碍物得到的深度图像,将深度图像中的图像数据转换为激光雷达数据,最终基于指定路径终点,并根据混合地图和激光雷达数据,生成路径规划。如此,可以改善单一地图在导航中鲁棒性不强、常规混合地图成本高且传感数据融合难的问题。
附图说明
本申请可以通过附图给出的非限定性实施例进一步说明。应当理解,以下附图仅示出了本申请的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1为本申请实施例提供的基于混合地图的视觉定位导航系统的架构示意图。
图2为本申请实施例提供的基于混合地图的视觉定位导航方法。
图3为本申请实施例提供的稀疏地图示意图。
图4为本申请实施例提供的栅格地图变换示意图。
图5为本申请实施例提供的栅格地图示意图。
图6为本申请实施例提供的深度图像点到世界坐标系的映射过程示意图。
图7为本申请实施例提供的深度图像转激光雷达示意图。
具体实施方式
以下将结合附图和具体实施例对本申请进行详细说明,需要说明的是,在附图或说明书描述中,相似或相同的部分都使用相同的图号,附图中未绘示或描述的实现方式,为所属技术领域中普通技术人员所知的形式。在本申请的描述中,术语“第一”、“第二”等仅用于区分描述,而不能理解为指示或暗示相对重要性。
为支撑本实施例的示例性应用,请参照图1,图1是本申请实施例提供的基于混合地图的视觉定位导航系统的架构示意图。其中,视觉定位导航系统框架描述了整个系统的硬件层、软件层、数据层及算法层之间的关系,各层级之间相互作用,以使机器人完成视觉导航任务。
在本实施例中,硬件层包括相机和底盘,主要用于提供机器人的感知信息和运动数据;软件层主要用于完成人机交互,便于用户使用;算法层包括定位算法和导航算法,用于完成机器人视觉定位和导航功能;数据层是连接硬件层、软件层、算法层之间的链路层,用于完成数据传输。
本申请实施例提供一种机器人可以包括处理模块及存储模块。存储模块内存储计算机程序,当计算机程序被所述处理模块执行时,使得机器人能够执行下述基于混合地图的视觉定位导航方法中的相应步骤。
请参照图2,本申请还提供一种基于混合地图的视觉定位导航方法。其中,基于混合地图的视觉定位导航方法可以包括如下步骤:
步骤110,获取通过拍摄机器人周边环境得到的相机图像;
步骤120,对所述相机图像进行预处理,得到所述相机图像的预处理结果,所述预处理结果包括相机图像特征点及相机位置;
步骤130,根据所述预处理结果,构建对应的混合地图,其中,所述混合地图是通过将与所述预处理结果对应的稀疏地图和栅格地图映射在同一坐标系下得到的;
步骤140,获取通过拍摄障碍物得到的深度图像;
步骤150,将所述深度图像中的图像数据转换为激光雷达数据,所述图像数据包括深度图像点在世界坐标系中的坐标;
步骤160,基于指定路径终点,并根据所述混合地图和所述激光雷达数据,生成路径规划。
在上述的实施方式中,首先获取通过拍摄机器人周边环境得到的相机图像,并对相机图像进行预处理,得到相机图像的预处理结果,从而根据预处理结果,通过将与预处理结果对应的稀疏地图和栅格地图映射在同一坐标系下得到混合地图;然后获取通过拍摄障碍物得到的深度图像,将深度图像中的图像数据转换为激光雷达数据,最终基于指定路径终点,并根据混合地图和激光雷达数据,生成路径规划。如此,可以改善单一地图在导航中鲁棒性不强、常规混合地图成本高且传感数据融合难的问题。
下面将对基于混合地图的视觉定位导航方法的各步骤进行详细阐述,如下:
在步骤110中,通过集成在机器人上的相机拍摄机器人周边环境得到相机图像,其中,相机图像通常为RGB图像。
在步骤120中,对相机图像进行预处理,得到相机图像的预处理结果,可以包括:
对所述相机图像进行特征点提取,以得到所述相机图像特征点;
对相邻时刻的所述相机图像进行特征匹配,得到匹配结果;
基于所述匹配结果,进行运动估计,以确定所述相机位置,并将所述相机图像特征点恢复成地图点。
在本实施例中,首先将相机图像转换为灰度图像,然后对转换为灰度图像后的相机图像进行特征点提取,以得到相机图像特征点,例如相机图像中的角点、图像较暗区域的亮点、ORB特征点等;随后通过对当前时刻与前一时刻(即相邻时刻之间)进行特征匹配,从而根据特征匹配得到的匹配结果,进行运动估计并输出相机位置,最后将相机图像特征点恢复成地图点。如此,将相机位置和相机图像特征点作为预处理结果,为后续混合地图的构建提供输入数据。
在步骤130中,根据所述预处理结果,构建对应的混合地图,可以包括:
根据所述相机图像特征点和所述相机位置,构建得到对应的所述稀疏地图;
根据所述相机图像特征点和所述相机位置,构建得到对应的所述栅格地图;
将所述稀疏地图和所述栅格地图映射在同一坐标系下,以得到混合地图。
在本实施例中,参照图3,稀疏地图由相机图像特征点构成。另外,栅格地图的构建方式可以如下:
请参照图4,将所述相机图像特征点映射在二维平面上(即将三位的相机图像特征点的坐标(x,y,z)映射为二维平面上的坐标(x,y));
从所述相机位置投射光线到所述二维平面上的所述相机图像特征点;
为沿着所述光线的每个栅格添加访问计数器,以及,为所述相机图像特征点在所述二维平面上对应的所述栅格添加已占用计数器;
根据所述访问计数器和所述已占用计数器,计算得到所述栅格的占用概率,其中,所述占用概率的计算公式如下:
poccupied(i,j)=occupied(i,j)/visit(i,j) (1)
式中,occupied(i,j)和visit(i,j)分别为所述已占用计数器中的占用数量和所述访问计数器中的访问数量,poccupied(i,j)为所述占用概率;
基于所述占用概率,通过第一预设阈值和第二预设阈值对所述栅格进行分类,得到栅格分类结果,所述栅格分类结果包括空闲栅格、占用栅格和未知栅格;
根据所述栅格分类结果,对所述栅格进行着色,以得到所述栅格地图;
其中,基于所述占用概率,通过第一预设阈值和第二预设阈值对所述栅格进行分类,得到栅格分类结果,包括:
当所述占用概率小于等于所述第一阈值时,确定所述栅格为所述空闲栅格;
当所述占用概率大于等于所述第二阈值时,确定所述栅格为所述占用栅格;
当所述占用概率大于所述第一阈值且小于所述第二阈值时,确定所述栅格为所述未知栅格。
在本实施例中,第一预设阈值和第二预设阈值可以根据实际情况灵活设置,例如0.7、0.5、1等,根据栅格分类结果对栅格进行着色时,不同栅格类型设置不同颜色即可,此处对各类型的栅格对应的颜色不做具体限定。
示例性的,参照图5,第一预设阈值以0.5为例,第二预设阈值以0.7为例,当任意栅格的占用概率小于等于0.5时,确定该栅格的类型为空闲栅格,并将该栅格设置为白色;当任意栅格的占用概率大于等于0.7时,确定该栅格的类型为占用栅格,并将该栅格设置为黑色;当任意栅格的占用概率大于0.5且小于0.7时,确定该栅格的类型为空闲栅格,并将该栅格设置为灰色。
在本实施例中,将所述稀疏地图和所述栅格地图映射在同一坐标系下,以得到混合地图,可以包括:
对所述稀疏地图和所述栅格地图对应的坐标系进行欧式变换,变换公式如下:
式中,a为稀疏地图坐标系下的坐标,a′为栅格地图坐标系下的坐标。
如此,基于单一的视觉传感器(相机)采集数据,并构建用于机器人视觉定位导航的混合地图,以作为全局路径规划的原始输入,改善单一地图在导航中鲁棒性不强、常规混合地图成本高且传感数据融合难的问题。
在步骤140中,通过相机拍摄障碍物得到深度图像。
在本实施例中,可以为相机拍摄机器人周边环境得到的初始深度图像设定一个深度阈值(也可称为距离阈值),当初始深度图像中的初始深度图像点的深度(即该初始深度图像点到相机的距离)大于深度阈值时,可忽略或删除该初始深度图像点,并将初始深度图像中剩余的初始深度图像点的总和作为上述的障碍物对应的深度图像。如此,在不影响后续机器人的路径规划的基础上,缩减相机的视野范围,减少后续机器人避障和路径规划所需的计算量。
在步骤150中,将所述深度图像中的图像数据转换为激光雷达数据,可以包括:
获取所述深度图像中的深度图像点;
将所述深度图像点转换为所述世界坐标系下的坐标,以作为所述图像数据;
将所述图像数据转换为所述激光雷达数据。
在本实施例中,将所述深度图像点转换为所述世界坐标系下的坐标,可以包括:
获取旋转矩阵和平移矩阵,并计算所述深度图像点在所述世界坐标系下的坐标:
式中,u、v为所述深度图像点坐标,u0、v0分别为所述深度图像的中心坐标,xw、yw、zw表示所述世界坐标系下的三维坐标点,f为相机的焦距,dx和dy分别为相机内不同方向上的尺度因子,zc表示相机坐标系的z轴值,R、T分别为外参矩阵的3×3旋转矩阵和3×1的平移矩阵;
其中,所述相机坐标系和所述世界坐标系的坐标原点重合,即zc=zw=z,所述旋转矩阵和所述平移矩阵为:
所述深度图像点到所述世界坐标系的转换公式为:
示例性的,参照图6,以深度图像点m(u,v,z)和世界坐标系中的坐标点M(x,y,z)为例,深度图像点转换到世界坐标系的数字描述如下:
其中,u,v为深度图像坐标系下的任意坐标点,u0,v0分别为深度图像的中心坐标。xw,yw,zw表示世界坐标系下的三维坐标点,f为相机的焦距,dx和dy分别为相机内不同方向上的尺度因子。zc表示相机坐标的z轴值,即目标障碍物到相机的距离。R,T分别为外参矩阵的3×3旋转矩阵和3×1的平移矩阵。其中,对外参矩阵的设置,由于在本实施例中世界坐标系原点和相机坐标系原点是重合的,即没有旋转和平移,因此:
可理解的,相机坐标系和世界坐标系的坐标原点重合,因此相机坐标和世界坐标下的同一个物体具有相同的深度,即zc=zw=z,于是公式进一步简化为:
从上述的变换公式,可以计算得到深度图像点m(u,v,z)到相机坐标系(世界坐标系)中的点M(x,y,z)的变换公式:
在本实施例中,为匹配混合地图的输入数据格式,进一步将图像数据转换为激光雷达数据。其中,将所述图像数据转换为所述激光雷达数据,可以包括:
请参照图7,计算相机光心0与所述世界坐标系的原点A连线A0、相机光心与所述世界坐标系下任意点投影到所述世界坐标系的x轴上的点C连线AC之间构成的夹角AOC:
θ=tan-1(x/z) (10)
式中,θ为夹角AOC的角度,z和x分别为所述深度图像点投影到与相机同一平面后距相机光心的z轴与x轴上的距离;
将夹角AOC投影到对应的激光数据槽中,其中,所述世界坐标系中的任意点M投影到数组laser[N]中的索引值n表示为:
式中,α、β为激光的范围[α,β],激光束细分为N份,数组laser[N]表示激光数据,laser[n]的值为所述世界坐标系中的任意点M在x轴上的投影点C到相机光心O的距离r,即:
如此,基于单一的视觉传感器(相机)采集数据,并模拟激光雷达探测物体边界的方式,作为局部路径规划的原始输入,实现机器人的避障功能,减少硬件成本以及避免传感数据融合困难的问题。
在步骤160中,为机器人指定一个路径终点,基于该指定路径终点,通过混合地图生成全局代价地图,并使用路径规划算法(例如A*算法)进行全局路径规划;通过激光雷达数据为混合地图加入障碍物,并生成对应的局部代价地图,使用路径规划算法(例如D*算法)进行局部路径规划,如此,实现机器人自当前位置到指定路径终点的最优路径规划。
在本实施例中,处理模块可以是一种集成电路芯片,具有信号的处理能力。上述处理模块可以是通用处理器。例如,该处理器可以是中央处理器(Central Processing Unit,CPU)、数字信号处理器(Digital Signal Processing,DSP)、专用集成电路(ApplicationSpecific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable GateArray,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件,可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。
存储模块可以是,但不限于,随机存取存储器,只读存储器,可编程只读存储器,可擦除可编程只读存储器,电可擦除可编程只读存储器等。在本实施例中,存储模块可以用于存储相机图像、预处理结果、混合地图、深度图像、激光雷达数据、路径规划、已占用计数器、访问计数器、占用概率、稀疏地图、栅格地图等。当然,存储模块还可以用于存储程序,处理模块在接收到执行指令后,执行该程序。
需要说明的是,所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的机器人的具体工作过程,可以参考前述方法中的各步骤对应过程,在此不再过多赘述。
本申请实施例还提供一种计算机可读存储介质。计算机可读存储介质中存储有计算机程序,当计算机程序在计算机上运行时,使得计算机执行如上述实施例中所述的基于混合地图的视觉定位导航方法。
通过以上的实施方式的描述,本领域的技术人员可以清楚地了解到本申请可以通过硬件实现,也可以借助软件加必要的通用硬件平台的方式来实现,基于这样的理解,本申请的技术方案可以以软件产品的形式体现出来,该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM,U盘,移动硬盘等)中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本申请各个实施场景所述的方法。
综上所述,本申请实施例提供一种基于混合地图的视觉定位导航方法、机器人及存储介质。在本方案中,首先获取通过拍摄机器人周边环境得到的相机图像,并对相机图像进行预处理,得到相机图像的预处理结果,从而根据预处理结果,通过将与预处理结果对应的稀疏地图和栅格地图映射在同一坐标系下得到混合地图;然后获取通过拍摄障碍物得到的深度图像,将深度图像中的图像数据转换为激光雷达数据,最终基于指定路径终点,并根据混合地图和激光雷达数据,生成路径规划。如此,可以改善单一地图在导航中鲁棒性不强、常规混合地图成本高且传感数据融合难的问题,同时,基于单一的视觉传感器(相机)采集数据,并模拟激光雷达探测物体边界的方式,作为机器人局部路径规划的原始输入,实现机器人的避障功能,减少硬件成本以及避免传感数据融合困难的问题。
在本申请所提供的实施例中,应该理解到,所揭露的装置、系统和方法,也可以通过其它的方式实现。以上所描述的装置、系统和方法实施例仅仅是示意性的,例如,附图中的流程图和框图显示了根据本申请的多个实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用执行规定的功能或动作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。另外,在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分,也可以是各个模块单独存在,也可以两个或两个以上模块集成形成一个独立的部分。
以上所述仅为本申请的实施例而已,并不用于限制本申请的保护范围,对于本领域的技术人员来说,本申请可以有各种更改和变化。凡在本申请的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本申请的保护范围之内。
Claims (10)
1.一种基于混合地图的视觉定位导航方法,其特征在于,所述方法包括:
获取通过拍摄机器人周边环境得到的相机图像;
对所述相机图像进行预处理,得到所述相机图像的预处理结果,所述预处理结果包括相机图像特征点及相机位置;
根据所述预处理结果,构建对应的混合地图,其中,所述混合地图是通过将与所述预处理结果对应的稀疏地图和栅格地图映射在同一坐标系下得到的;
获取通过拍摄障碍物得到的深度图像;
将所述深度图像中的图像数据转换为激光雷达数据,所述图像数据包括深度图像点在世界坐标系中的坐标;
基于指定路径终点,并根据所述混合地图和所述激光雷达数据,生成路径规划。
2.根据权利要求1所述的方法,其特征在于,对所述相机图像进行预处理,得到所述相机图像的预处理结果,包括:
对所述相机图像进行特征点提取,以得到所述相机图像特征点;
对相邻时刻的所述相机图像进行特征匹配,得到匹配结果;
基于所述匹配结果,进行运动估计,以确定所述相机位置,并将所述相机图像特征点恢复成地图点。
3.根据权利要求1所述的方法,其特征在于,根据所述预处理结果,构建对应的混合地图,包括:
根据所述相机图像特征点和所述相机位置,构建得到对应的所述稀疏地图;
根据所述相机图像特征点和所述相机位置,构建得到对应的所述栅格地图;
将所述稀疏地图和所述栅格地图映射在同一坐标系下,以得到混合地图。
4.根据权利要求3所述的方法,其特征在于,根据所述相机图像特征点和所述相机位置,构建得到对应的所述栅格地图,包括:
将所述相机图像特征点映射在二维平面上;
从所述相机位置投射光线到所述二维平面上的所述相机图像特征点;
为沿着所述光线的每个栅格添加访问计数器,以及,为所述相机图像特征点在所述二维平面上对应的所述栅格添加已占用计数器;
根据所述访问计数器和所述已占用计数器,计算得到所述栅格的占用概率,其中,所述占用概率的计算公式如下:
poccupied(i,j)=occupied(i,j)/visit(i,j)
式中,occupied(i,j)和visit(i,j)分别为所述已占用计数器中的占用数量和所述访问计数器中的访问数量,poccupied(i,j)为所述占用概率;
基于所述占用概率,通过第一预设阈值和第二预设阈值对所述栅格进行分类,得到栅格分类结果,所述栅格分类结果包括空闲栅格、占用栅格和未知栅格;
根据所述栅格分类结果,对所述栅格进行着色,以得到所述栅格地图;
其中,基于所述占用概率,通过第一预设阈值和第二预设阈值对所述栅格进行分类,得到栅格分类结果,包括:
当所述占用概率小于等于所述第一阈值时,确定所述栅格为所述空闲栅格;
当所述占用概率大于等于所述第二阈值时,确定所述栅格为所述占用栅格;
当所述占用概率大于所述第一阈值小于所述第二阈值时,确定所述栅格为所述未知栅格。
5.根据权利要求3所述的方法,其特征在于,将所述稀疏地图和所述栅格地图映射在同一坐标系下,以得到混合地图,包括:
对所述稀疏地图和所述栅格地图对应的坐标系进行欧式变换,变换公式如下:
式中,a为稀疏地图坐标系下的坐标,a′为栅格地图坐标系下的坐标。
6.根据权利要求1所述的方法,其特征在于,将所述深度图像中的图像数据转换为激光雷达数据,包括:
获取所述深度图像中的深度图像点;
将所述深度图像点转换为所述世界坐标系下的坐标,以作为所述图像数据;
将所述图像数据转换为所述激光雷达数据。
7.根据权利要求6所述的方法,其特征在于,将所述深度图像点转换为所述世界坐标系下的坐标,包括:
获取旋转矩阵和平移矩阵,并计算所述深度图像点在所述世界坐标系下的坐标:
式中,u、v为所述深度图像点坐标,u0、v0分别为所述深度图像的中心坐标,xw、yw、zw表示所述世界坐标系下的三维坐标点,f为相机的焦距,dx和dy分别为相机内不同方向上的尺度因子,zc表示相机坐标系的z轴值,R、T分别为外参矩阵的3×3旋转矩阵和3×1的平移矩阵;
其中,所述相机坐标系和所述世界坐标系的坐标原点重合,即zc=zw=z,所述旋转矩阵和所述平移矩阵为:
所述深度图像点到所述世界坐标系的转换公式为:
8.根据权利要求6所述的方法,其特征在于,将所述图像数据转换为所述激光雷达数据,包括:
计算相机光心O与所述世界坐标系的原点A连线AO、相机光心与所述世界坐标系下任意点投影到所述世界坐标系的x轴上的点C连线AC之间构成的夹角AOC:
θ=tan-1(x/z)
式中,θ为夹角AOC的角度,z和x分别为所述深度图像点投影到与相机同一平面后距相机光心的z轴与x轴上的距离;
将夹角AOC投影到对应的激光数据槽中,其中,所述世界坐标系中的任意点M投影到数组laser[N]中的索引值n表示为:
式中,α、β为激光的范围[α,β],激光束细分为N份,数组laser[N]表示激光数据,laser[n]的值为所述世界坐标系中的任意点M在x轴上的投影点C到相机光心O的距离r,即:
9.一种机器人,其特征在于,所述机器人包括相互耦合的处理器及存储器,所述存储器内存储计算机程序,当所述计算机程序被所述处理器执行时,使得所述机器人执行如权利要求1-8中任一项所述的方法。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质中存储有计算机程序,当所述计算机程序在计算机上运行时,使得所述计算机执行如权利要求1-8中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310400250.3A CN116642490A (zh) | 2023-04-14 | 2023-04-14 | 基于混合地图的视觉定位导航方法、机器人及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310400250.3A CN116642490A (zh) | 2023-04-14 | 2023-04-14 | 基于混合地图的视觉定位导航方法、机器人及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116642490A true CN116642490A (zh) | 2023-08-25 |
Family
ID=87621931
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310400250.3A Pending CN116642490A (zh) | 2023-04-14 | 2023-04-14 | 基于混合地图的视觉定位导航方法、机器人及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116642490A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117078682A (zh) * | 2023-10-17 | 2023-11-17 | 山东省科霖检测有限公司 | 一种大规模网格化空气质量等级精准评估方法 |
-
2023
- 2023-04-14 CN CN202310400250.3A patent/CN116642490A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117078682A (zh) * | 2023-10-17 | 2023-11-17 | 山东省科霖检测有限公司 | 一种大规模网格化空气质量等级精准评估方法 |
CN117078682B (zh) * | 2023-10-17 | 2024-01-19 | 山东省科霖检测有限公司 | 一种大规模网格化空气质量等级精准评估方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6745328B2 (ja) | 点群データを復旧するための方法及び装置 | |
WO2021233029A1 (en) | Simultaneous localization and mapping method, device, system and storage medium | |
EP3620966A1 (en) | Object detection method and apparatus for object detection | |
CN107194962B (zh) | 点云与平面图像融合方法及装置 | |
CN111563442A (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
CN112861653A (zh) | 融合图像和点云信息的检测方法、系统、设备及存储介质 | |
CN112097732A (zh) | 一种基于双目相机的三维测距方法、系统、设备及可读存储介质 | |
CN113761999A (zh) | 一种目标检测方法、装置、电子设备和存储介质 | |
CN113378605B (zh) | 多源信息融合方法及装置、电子设备和存储介质 | |
CN112446927A (zh) | 激光雷达和相机的联合标定方法、装置、设备及存储介质 | |
WO2022217988A1 (zh) | 传感器配置方案确定方法、装置、计算机设备、存储介质及程序 | |
CN115496923B (zh) | 一种基于不确定性感知的多模态融合目标检测方法及装置 | |
CN114639115B (zh) | 一种人体关键点与激光雷达融合的3d行人检测方法 | |
CN115410167A (zh) | 目标检测与语义分割方法、装置、设备及存储介质 | |
CN114186007A (zh) | 高精地图生成方法、装置、电子设备和存储介质 | |
CN114565916A (zh) | 目标检测模型训练方法、目标检测方法以及电子设备 | |
CN116642490A (zh) | 基于混合地图的视觉定位导航方法、机器人及存储介质 | |
CN116844129A (zh) | 多模态特征对齐融合的路侧目标检测方法、系统及装置 | |
CN116229247A (zh) | 室内场景语义分割方法、装置、设备和介质 | |
CN114386481A (zh) | 一种车辆感知信息融合方法、装置、设备及存储介质 | |
CN112639822B (zh) | 一种数据处理方法及装置 | |
CN117423102A (zh) | 点云数据处理方法以及相关设备 | |
CN116363615B (zh) | 数据融合方法、装置、车辆和存储介质 | |
CN117250956A (zh) | 一种多观测源融合的移动机器人避障方法和避障装置 | |
CN115578432B (zh) | 图像处理方法、装置、电子设备及存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication |