WO2019075948A1 - 移动机器人的位姿估计方法 - Google Patents

移动机器人的位姿估计方法 Download PDF

Info

Publication number
WO2019075948A1
WO2019075948A1 PCT/CN2018/073928 CN2018073928W WO2019075948A1 WO 2019075948 A1 WO2019075948 A1 WO 2019075948A1 CN 2018073928 W CN2018073928 W CN 2018073928W WO 2019075948 A1 WO2019075948 A1 WO 2019075948A1
Authority
WO
WIPO (PCT)
Prior art keywords
mobile robot
pose
image
matching
panoramic image
Prior art date
Application number
PCT/CN2018/073928
Other languages
English (en)
French (fr)
Inventor
钱飞帆
杨楷
郑卓斌
丁璜
Original Assignee
广东宝乐机器人股份有限公司
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 广东宝乐机器人股份有限公司 filed Critical 广东宝乐机器人股份有限公司
Publication of WO2019075948A1 publication Critical patent/WO2019075948A1/zh

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

Definitions

  • the invention belongs to the field of mobile robots and relates to positioning and navigation of mobile robots, in particular to pose estimation methods of mobile robots.
  • the existing visual pose estimation methods of mobile robots mainly include the following two types: one is the front view vision represented by irobot company for pose estimation; the other is the ceiling vision represented by LG company for pose estimation.
  • the existing solution is to obtain more environmental information by installing multiple cameras on the mobile robot, but this method will lead to an increase in the cost of the mobile robot, and in addition, due to the calibration between multiple cameras and between different cameras Matching, the algorithm is more complicated.
  • One of the objects of the present invention is to overcome the deficiencies in the background art and to provide a pose estimation method for a mobile robot, the specific scheme of which is as follows:
  • a pose estimating method for a mobile robot comprising: a panoramic camera and a stroke measuring device, comprising the following steps: S1: initializing or acquiring pose posture information of the mobile robot at a reference position and a panoramic image P 0 ; S2: panorama the image P 0 is divided into at least two images P 0-j; S3: acquiring a panoramic image of the surroundings in the mobile robot of time t i P i; S4: the panoramic image is divided into P i corresponding to the image of at least P 0-j two images P ij; S5: T ij calculating pose of the mobile robot based on the matching relationship of the images P 0-j and P ij, respectively; S6: T ij mobile robot pose at the time t i in accordance with the position and orientation.
  • the method further includes the step S7: returning the position of the time t i as the reference position to the step S1.
  • the reference position in the step S1 is a position when the mobile robot starts working.
  • step S2 is to divide the panoramic image by a cube model.
  • the panoramic image P 0 of the step S2 and the panoramic image P i of the step S4 are respectively divided into an upper view image and a forward view image.
  • step of calculating the pose of the mobile robot according to the top view image comprises: step a1: extracting the top view image features of the panoramic image P 0 and the panoramic image P i and matching; step a2: matching the extracted features according to the image matching algorithm Point; step a3: obtaining the homography matrix H according to the matching relationship of the matching feature points; step a4: decomposing the homography matrix H according to the parameters in the camera to obtain the pose estimation T i- .
  • step of calculating the pose of the mobile robot according to the front view image comprises: step b1: extracting the front view image features of the panoramic image P 0 and the panoramic image P i and matching; step b2: matching the extracted features according to the image matching algorithm Point b3: obtaining the basic matrix F according to the matching relationship of the matching feature points; step b4: decomposing the basic matrix F according to the parameters in the camera to obtain the pose estimation T i-front .
  • step S6 includes: S61: calculating the error of each T ij respectively; S62: selecting the pose with the smallest error range as the pose of the mobile robot at time t i .
  • step S61 is based on the error calculation of the Euler rotation angles of the images P 0-j and P ij .
  • the technical solution of the present invention acquires richer environmental information through the panoramic camera, and calculates the pose of each angle of the mobile robot by separately dividing the panoramic image, and then determines according to the error range of each pose.
  • the final pose improves the accuracy of the pose estimation of the mobile robot.
  • Figure 1 is a flow chart of a preferred embodiment of the method of the present invention
  • FIG. 2 is a flow chart for calculating a pose of a mobile robot based on a top view image
  • FIG. 3 is a flow chart for calculating a pose of a mobile robot based on a front view image
  • Figure 4 is a schematic diagram of a cubic model
  • Figure 5 is a reference image of a spherical map acquired by a panoramic camera
  • Figure 6 is a segmentation corrected image obtained after the cube mapping of Figure 5.
  • the mobile robot in the embodiment of the method of the present invention includes a panoramic camera and a stroke measuring device.
  • the panoramic camera is a wide-angle camera including a fisheye camera, and the panoramic camera is tilted upwardly on the front side of the moving direction of the mobile robot, so that the panoramic camera can acquire the front, the upper, the left, the right, and the rear of the mobile robot.
  • Environmental information; the travel measuring device is used to measure the moving distance of the robot, such as the odometer (code wheel); in addition, the mobile robot also includes the control system, the drive system, the sensor system, the power system and the function system, etc., and will not be described in detail here. .
  • the method of the invention is particularly suitable for position estimation of mobile robots working indoors, such as sweeping robots, air purifying robots and indoor security patrol robots, etc., because indoor mobile robots have working boundary definitions (such as walls) and can obtain ceiling pictures.
  • the upper view image and the odometer data calculate the robot pose (when there is no ceiling, the robot needs to move a large distance, and the upper view image will change its angle).
  • S1 Initialize or acquire the pose information of the mobile robot at the reference position and the panoramic image P 0 .
  • the mobile robot first determines a reference position and acquires a panoramic image P 0 of the reference position through the fisheye camera.
  • the reference position includes the starting point of the mobile robot or a memory point position.
  • the mobile robot starts a working point position (such as a charging seat position) as a reference position and initializes to a map origin; or acquires a certain memory point position stored in the map as a reference position and acquires coordinate information of the memory point position.
  • the pose information here refers to the coordinates and direction of travel of the mobile robot in the map.
  • the panoramic image acquired by the fisheye camera is used as a reference image of the spherical map, and then converted by the cube mapping to become a cube model image. Since the information of the upper viewing angle (ceiling angle of view) and the front viewing angle of the mobile robot during the moving process is the most valuable and the matching effect is relatively good, the present embodiment will focus on the upper vision and the front vision in detail.
  • X1 is the feature point on the ceiling of the working environment of the mobile robot
  • X2 is the feature point of the mobile robot walking forward
  • the feature point X1 is mapped to the U imaging plane as shown in the figure through the cube mapping
  • the feature point X2 is mapped to As shown in the F imaging plane.
  • a reference image of a spherical map acquired by a panoramic camera (fisheye camera), as shown in FIG. 6, is a segmentation correction image obtained by using a cube map, and a top view area abcd of a spherically mapped reference image.
  • the forward looking area dcfe are respectively projected into the corresponding top view image abcd (image P 0-up ) and front view image dcfe (image P 0-front ) in the cube model image using the cube map.
  • the panoramic image P i of the surrounding environment of the mobile robot at this time is acquired by the fisheye camera.
  • the panoramic image P i is divided into at least two images P ij corresponding to the images P 0-j .
  • the top view image and the front view image are respectively calculated for the mobile robot pose.
  • the step of calculating the pose of the mobile robot from the top view image is as follows.
  • Step a1 extracting and matching the top view image features of the panoramic image P 0 and the panoramic image P i .
  • the step of extracting the ORB feature descriptor is: using the FAST algorithm to obtain the feature point; generating the OFAST for the FAST feature point allocation direction; using the direction of the allocation to obtain the directed BRIEF; the greedy algorithm screening the high-resolution directed BRIEF, ie ORB descriptor.
  • ORB feature derivation extraction see Rublee E, Rabaud V, Konolige K, et al. ORB: an efficient alternative to SIFT or SURF. Computer Vision (ICCV), 2011 IEEE International Conference on. IEEE, 2011: 2564 -2571.
  • Step a2 matching the extracted feature points according to the image matching algorithm.
  • the extracted feature descriptors are matched according to the image matching algorithm; the extracted ORB descriptors are binary code strings, and the Hamming distance is used to measure the similarity of the ORB descriptors to obtain matching point pairs.
  • Step a3 Obtaining the homography matrix H according to the matching relationship of the matching feature points.
  • the homography matrix H is obtained, the random matching consistency (RANSAC) algorithm is used to filter the mismatch, and the RANSAC algorithm is used to calculate the homography between the upper image and the upper reference image. matrix.
  • RANSAC random matching consistency
  • Step a4 Decompose the homography matrix H according to the parameters in the camera to obtain the pose estimation T i- .
  • the camera's internal parameter matrix K includes the focal length f, the optical center offset coefficient cx, cy, which is obtained by camera calibration, combined with the internal parameter matrix.
  • the SVD decomposition yields a rotation matrix R and a translation vector t.
  • the decomposed translation vector is a normalized translation amount, with a scale, combined with the moving distance of the mobile robot measured by the odometer, to obtain a true translation t i- .
  • the combination of the rotation matrix R and the true translation t t is the pose estimate T i- of the camera.
  • the step of calculating the pose of the mobile robot by the front view image is as follows.
  • Step b1 extracting and matching the front view image features of the panoramic image P 0 and the panoramic image P i .
  • the current image i.e. a panoramic image P i
  • the reference image i.e. a panoramic image P 0
  • a feature point calculated a feature point calculated
  • the feature descriptor, the extracted feature descriptor is an ORB feature descriptor.
  • Step b2 matching the extracted feature points according to the image matching algorithm.
  • the extracted feature descriptors are matched according to the image matching algorithm, and Hamfing distance matching feature descriptors are used.
  • Step b3 The basic matrix F is obtained according to the matching relationship of the matching feature points.
  • the RANSAC algorithm is used to eliminate outliers, obtain the largest uniform set (ie, the correct match pair) and estimate the base matrix F using the normalized 8-point algorithm.
  • Step b4 Decompose the basic matrix F according to the parameters in the camera to obtain the pose estimation T i-front .
  • the eigenmatrix E is obtained from the basic matrix in combination with the inner parameter matrix, and then the eigenmatrix E is decomposed using SVD to obtain the rotation matrix R and the translation vector t.
  • W T ⁇ 0,-1,0; 1,0,0; 0,0,1 ⁇ .
  • the decomposed translation vector is a normalized translation amount, with a scale, combined with the moving distance of the mobile robot measured by the odometer, to obtain a true translation t i-front .
  • the rotation matrix R and the true translation t i-pre- combination are the pose estimation T i-front of the camera. Details of the SVD decomposition here can be found in: R. Hartley and A. Zisserman, Multiple View Geometry in ComputerVision, 2nd ed. Cambridge University Press, 2004.
  • the reason for using the H matrix to estimate the camera pose for the top view image is that the image captured by the top view image is a ceiling, and the three-dimensional plane scene is rich, and the effect of solving the homography matrix and estimating the pose algorithm is better;
  • the front view image captures the object in front of the robot, and the non-three-dimensional plane scene is rich. It is better to use the polar constraint model to find the basic matrix and estimate the pose algorithm.
  • Determining the pose of the mobile robot at time t i in this embodiment includes two steps.
  • T i- pose in step S5 rotation and translation pose before T i-, wherein converting the rotational pose T i- Euler angles expressed as (a [Psi], [theta] a, ), Expressed as a translation (the tx, on TY, TZ on); a pose T i- front into rotational angles represented by Euler front ([Psi], [theta] before, ), The translation is expressed as (former tx, before ty, before tz). Comparison of Euler angles size variation value, i.e. (on the ⁇ + ⁇ + ) and ( before + ⁇ before + )the size of.
  • the former will be determined to pose T i- mobile robot pose at the time t i; if (the upper ⁇ + ⁇ + ) is less than ( before + ⁇ before + ), the pose T i- is determined as the pose of the mobile robot at time t i .
  • Step S7 returning the position at time t i as a reference position to step S1.
  • This step is an optional step. After the mobile robot determines the pose at time t i , the map information of the point is saved, and then returns to step S1 and then steps S2 to S6 are repeated. In addition, in order to reduce the cumulative error, the reference position of the pose estimation of the subsequent mobile robot at time t i preferentially selects the initial reference point, and if the initial reference point cannot be matched, the position of the estimated pose can be selected as the reference point.
  • the method of the present invention can also calculate the pose posture T i-left , the pose T i-right , the pose T i -post pose of the mobile robot at time t i by the method in the above embodiment. Then, the pose with the smallest error range is selected from the plurality of (three or more) calculated poses as the pose of the mobile robot at time t i .

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)
  • Manipulator (AREA)

Abstract

本发明揭示了一种移动机器人的位姿估计方法，包括以下步骤：S1：初始化或获取移动机器人在参考位置的位姿信息及全景图像P 0；S2：将全景图像P 0分割成至少两幅图像P 0-j；S3：获取移动机器人在t i时刻的周围环境的全景图像P i；S4：将全景图像P i分割成与图像P 0-j对应的至少两幅图像P i-j；S5：根据图像P 0-j与P i-j的匹配关系分别计算移动机器人的位姿T i-j；S6：根据位姿T i-j确定移动机器人在t i时刻的位姿。与现有技术相比，本发明技术方案通过全景摄像头获取更为丰富的环境信息，并通过对分割后的全景图像分别计算移动机器人各角度的位姿，然后根据各位姿的误差范围大小来确定最终位姿，提高了移动机器人的位姿估计精度。

Description

移动机器人的位姿估计方法 技术领域
本发明属于移动机器人领域，涉及移动机器人的定位及导航，特别是移动机器人的位姿估计方法。
背景技术
现有的移动机器人的视觉位姿估计方法主要包括以下两种：一是以irobot公司为代表的前视视觉进行位姿估计；二是以LG公司为代表的天花板视觉进行位姿估计。
然后，不管是前视视觉还是天花板视觉，其视角范围限制在某个特定角度，缺乏对整个环境的感知，导致对移动机器人的位姿估计存在误差甚至错误，进而影响移动机器人的地图创建及导航。
现有方案是通过在移动机器人上安装多个摄像头以获得更多的环境信息，但这种方式将导致移动机器人的成本增加，此外，因涉及多个摄像头之间的标定及不同摄像头之间的匹配，算法较为复杂。
因此有必要提供一种能解决上述问题的移动机器人的位姿估计方案。
发明内容
本发明的目的之一在于克服背景技术中的缺陷，提供一种移动机器人的位姿估计方法，其具体方案如下：
一种移动机器人的位姿估计方法，所述移动机器人包括全景摄像头和行程测量装置，包括以下步骤：S1：初始化或获取移动机器人在参考位置的位姿信息及全景图像P 0；S2：将全景图像P 0分割成至少两幅图像P 0-j；S3：获取移动机器人在t i时刻的周围环境的全景图像P i；S4：将全景图像P i分割成与图像P 0-j对应的至少两幅图像P i-j；S5：根据图像P 0-j与P i-j的匹配关系分别计算移动机器人的位姿T i-j；S6：根据位姿T i-j确定移动机器人在t i时刻的位姿。
进一步地，在本发明的一个实施例中，还包括步骤S7：将t i时刻的位置作 为参考位置再返回步骤S1。
进一步地，所述步骤S1中的参考位置为移动机器人开始工作时的位置。
进一步地，所述步骤S2是通过立方体模型将全景图像进行分割的。
进一步地，在本发明的一个实施例中，将所述步骤S2的全景图像P 0、步骤S4全景图像P i分别分割成上视图像和前视图像。
进一步地，所述根据上视图像计算移动机器人位姿的步骤包括：步骤a1：提取全景图像P 0、全景图像P i的上视图像特征并匹配；步骤a2：根据图像匹配算法匹配提取的特征点；步骤a3：根据匹配特征点的匹配关系得到单应矩阵H；步骤a4：根据相机内参数分解单应矩阵H以得到位姿估计T i-上。
进一步地，所述根据前视图像计算移动机器人位姿的步骤包括：步骤b1：提取全景图像P 0、全景图像P i的前视图像特征并匹配；步骤b2：根据图像匹配算法匹配提取的特征点；步骤b3：根据匹配特征点的匹配关系得到基础矩阵F；步骤b4：根据相机内参数分解基础矩阵F以得到位姿估计T i-前。
进一步地，所述步骤S6包括：S61：分别计算各T i-j的误差；S62：选择误差范围最小的位姿作为移动机器人在t i时刻的位姿。
进一步地，所述步骤S61是基于图像P 0-j与P i-j的欧拉旋转角进行误差计算的。
与现有技术相比，本发明技术方案通过全景摄像头获取更为丰富的环境信息，并通过对分割后的全景图像分别计算移动机器人各角度的位姿，然后根据各位姿的误差范围大小来确定最终位姿，提高了移动机器人的位姿估计精度。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明方法的较佳实施例的流程图；
图2是根据上视图像计算移动机器人位姿的流程图；
图3是根据前视图像计算移动机器人位姿的流程图；
图4是立方体模型示意图；
图5是全景摄像头获取的球面映射的基准图像；
图6是图5经立方体映射后得到的分段矫正图像。
具体实施方式
下面将结合附图和具体实施例对本发明技术方案进行清楚、完整地描述，显然，这里所描述的实施例仅仅是发明的一部分实施例，而不是全部的实施例。基于本发明描述的具体实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明权利要求所限定的保护范围内。
本发明方法实施例中的移动机器人包括全景摄像头和行程测量装置。其中，全景摄像头为包括鱼眼摄像头在内的广角摄像头，全景摄像头在移动机器人移动方向的前侧向上倾斜安装，使得全景摄像头能获取包括移动机器人前方、上方、左方、右方以及部分后方的环境信息；行程测量装置用于测量利用机器人的移动距离，如里程计(码盘)；另外，移动机器人还包括控制系统、驱动系统、传感器系统、电源系统及功能系统等，这里不再详细说明。
本发明方法尤其适用于在室内工作的移动机器人的位姿估计，如扫地机器人、空气净化机器人及室内安防巡逻机器人等，由于室内移动机器人有工作边界限定(如墙)以及可获取天花板图片，可提高图片特征点的匹配率并能根据里程计数据计算机器人位姿；反之，在无边界和天花板的空旷工作区域，将降低图片特征点的匹配率，同时需要移动机器人移动较大距离后才能根据上视图像及里程计数据计算机器人位姿(无天花板时，需机器人移动较大距离后，其上视图像才会发生角度变化)。
本实施例中的移动机器人的位姿估计方法包括以下步骤：
S1：初始化或获取移动机器人在参考位置的位姿信息及全景图像P 0。
移动机器人要使用本方法进行位姿估计，首先要确定一个参考位置并通过鱼眼摄像头获取该参考位置的全景图像P 0。参考位置包括移动机器人开始工作点或某个记忆点位置。具体地，可以将移动机器人开始工作点位置(如充电座位置)作为参考位置并初始化为地图原点；或获取保存在地图中的某个记忆点位置作为参考位置并获取该记忆点位置的坐标信息，这里的位姿信息指移动机器人在地图中的坐标及行进方向。
S2：将全景图像P 0分割成至少两幅图像P 0-j；
本实施例中将鱼眼摄像头获取的全景图像作为球面映射的基准图像，然后通过立方体映射进行转换，使其变为立方体模型图像。由于移动机器人在移动过程中的上方视角(天花板视角)和前方视角的信息最有价值且匹配效果相对较好，本实施例将重点对上方视觉和前方视觉进行具体说明。如图4所示，X1为移动机器人工作环境的天花板上特征点，X2为移动机器人行走前方特征点，通过立方体映射将特征点X1映射到如图的U成像平面上，将特征点X2映射到如图的F成像平面上。如图5所示，为通过全景摄像头(鱼眼摄像机)获取的球面映射的基准图像，如图6所示，为使用立方体映射得到的分段矫正图像，球面映射的基准图像的上视区域abcd和前视区域dcfe使用立方体映射分别投影到立方体模型图像中对应的上视图像abcd(图像P 0-上)和前视图像dcfe(图像P 0-前)中。
S3：获取移动机器人在t i时刻的周围环境的全景图像P i；
移动机器人运行一段时间或距离后，将通过鱼眼摄像头获取此时，即t i时刻的移动机器人周围环境的全景图像P i。
S4：将全景图像P i分割成与图像P 0-j对应的至少两幅图像P i-j；
参考上述步骤S2中的方法，将全景图像P i分割成与图像P 0-j对应的至少两幅图像P i-j。
S5：根据图像P 0-j与P i-j的匹配关系分别计算移动机器人的位姿T i-j；
本步骤中分别就上视图像和前视图像计算移动机器人位姿分别进行说明。
本实施例中，上视图像计算移动机器人位姿的步骤如下。
步骤a1：提取全景图像P 0、全景图像P i的上视图像特征并匹配。
提取当前图像(即全景图像P i)的上视图像(即图像P i-上)和参考图像(即全景图像P 0)的上视图像(即图像P 0-上)的特征点并计算得到特征描绘子，提取ORB特征描绘子的步骤为：使用FAST算法求特征点；对FAST特征点分配方向生成OFAST；利用分配的方向求有向BRIEF；贪婪算法筛选高区分度的有向BRIEF,即ORB描绘子。关于ORB特征描绘子提取的详细资料可参考：Rublee E,Rabaud V,Konolige K,et al.ORB:an efficient alternative to SIFT or SURF.Computer Vision(ICCV),2011IEEE International Conference on.IEEE,2011:2564-2571。
步骤a2：根据图像匹配算法匹配提取的特征点。
根据图像匹配算法匹配提取的特征描绘子；提取的ORB描绘子是二进制码串，使用Hamming距离来度量ORB描绘子的相似度，得到匹配点对。
步骤a3：根据匹配特征点的匹配关系得到单应矩阵H。
根据匹配特征点的匹配关系得到单应矩阵H，对匹配点对使用随机抽样一致性(RANSAC)算法过滤误匹配，并使用RANSAC算法计算上视当前图像和上视参考图像之间的单应性矩阵。
步骤a4：根据相机内参数分解单应矩阵H以得到位姿估计T i-上。
根据相机内参数分解单应矩阵H以得到位姿估计T i-上；相机的内参数矩阵K包括焦距f，光心的偏移量系数cx,cy，由相机标定得到，结合内参数矩阵使用SVD分解得到旋转矩阵R和平移向量t。其中分解后的平移向量为归一化的平移量，具有尺度，结合里程计测得的移动机器人的移动距离,得到真实的平移t i-上。旋转矩阵R和真实的平移t t组合就是相机的位姿估计T i-上。关于SVD分解可参考：O.D.Faugeras and F.Lustman,“Motion and structure from motionin a piecewise planar environment,”International Journal of PatternRecognition and Artificial Intelligence,vol.2,no.03,pp.485–508,1988.
本实施例中，前视图像计算移动机器人位姿的步骤如下。
步骤b1：提取全景图像P 0、全景图像P i的前视图像特征并匹配。
提取当前图像(即全景图像P i)的前视图像(即图像P i-前)和参考图像(即全景图像P 0)的前视图像(即图像P 0-前)的特征点并计算得到特征描绘子，提取的特征描绘子为ORB特征描绘子。
步骤b2：根据图像匹配算法匹配提取的特征点。
根据图像匹配算法匹配提取的特征描绘子，使用Hamming距离匹配特征描绘子。
步骤b3：根据匹配特征点的匹配关系得到基础矩阵F。
使用RANSAC算法剔除异常点,获取最大一致集(即正确匹配对)并使用归一化8点算法估计基本矩阵F。8点算法估计基本矩阵F过程为：设正确匹配的点对为x,x',基础矩阵F＝{f1,f2,f3；f4,f5,f6；f7,f8,f9},由对极集合约束,xFx'＝0,解方程得到基本矩阵F。
步骤b4：根据相机内参数分解基础矩阵F以得到位姿估计T i-前。
首先由基础矩阵结合内参数矩阵得到本征矩阵E，然后对本征矩阵E使用 SVD分解得到旋转矩阵R和平移向量t。SVD分解过程为：对E进行奇异值分解，如公式E＝U∑V T求得U、V T，其中∑为3×3对角矩阵，U和V T为3×3正交矩阵。令W T＝{0,-1,0；1,0,0；0,0,1}。其中,旋转矩阵R 1＝UWV T,R 2＝UW TV T,t 1＝U 3，t 2＝-U 3，其中，U 3表示U的第三列组成的向量。由于有4种可能，取匹配的正确点对使用三角化投影得到在两个相机下的深度，检测该点在两个相机下的深度，得到正确的旋转向量R和平移向量t。其中分解后的平移向量为归一化的平移量，具有尺度，结合里程计测得的移动机器人的移动距离,得到真实的平移t i-前。旋转矩阵R和真实的平移t i-前组合就是相机的位姿估计T i-前。这里的SVD分解详细信息可参考：R.Hartley and A.Zisserman，Multiple View Geometry in ComputerVision，2nd ed.Cambridge University Press，2004。
需要说明的是：对于上视图像使用H矩阵估计相机位姿的原因是：上视图像拍摄到的图像是天花板，三维平面场景比较丰富，求解单应矩阵并估计位姿算法效果更好；而前视图像拍摄的是机器人前方的物体，非三维平面场景比较丰富，用对极约束模型求基础矩阵并估计位姿算法效果较好。
S6：根据位姿T i-j确定移动机器人在t i时刻的位姿。
本实施例中确定移动机器人在t i时刻的位姿包括两个步骤。
S61：分别计算各T i-j的误差。
通过上述步骤S5中的位姿T i-上、位姿T i-前进行旋转和平移，其中位姿T i-上转换为旋转用欧拉角表示为(ψ 上,θ 上,
Figure PCTCN2018073928-appb-000001
)，平移表示为(tx 上,ty 上,tz 上)；由位姿T i-前转换为旋转用欧拉角表示为(ψ 前,θ 前,
Figure PCTCN2018073928-appb-000002
),平移表示为(tx 前,ty 前,tz 前)。比较欧拉角的变化值大小，即(ψ 上+θ 上+
Figure PCTCN2018073928-appb-000003
)与(ψ 前+θ 前+
Figure PCTCN2018073928-appb-000004
)的大小。
S62：选择误差范围最小的位姿作为移动机器人在t i时刻的位姿。
如果(ψ 上+θ 上+
Figure PCTCN2018073928-appb-000005
)大于(ψ 前+θ 前+
Figure PCTCN2018073928-appb-000006
)，则将位姿T i-前确定为移动机器人在t i时刻的位姿；如果(ψ 上+θ 上+
Figure PCTCN2018073928-appb-000007
)小于(ψ 前+θ 前+
Figure PCTCN2018073928-appb-000008
)，则将位姿T i-上确定为移动机器人在t i时刻的位姿。
步骤S7：将t i时刻的位置作为参考位置再返回步骤S1。
本步骤为可选步骤，当移动机器人确定了在t i时刻的位姿后，保存该点的地图信息，然后再返回步骤S1后重复步骤S2至步骤S6。另外，为了减少累计误差，后续移动机器人在t i时刻的位姿估计的参考位置优先选择最初的参考点，在不能与最初参考点匹配的情况下可选择经估计位姿的位置作为参考点。
需要说明的是，本发明方法还可以通过上述实施例中的方法计算移动机器人在t i时刻的位姿位姿T i-左、位姿T i-右、位姿T i-后等位姿，然后从多个(三个或以上)计算的位姿中选择误差范围最小的位姿作为移动机器人在t i时刻的位姿。
以上所揭露的仅为本发明技术方案的实施例而已，不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (11)

  1. 一种移动机器人的位姿估计方法，其特征在于，所述移动机器人包括全景摄像头和行程测量装置，包括以下步骤：
    S1：初始化或获取移动机器人在参考位置的位姿信息及全景图像P 0；
    S2：将全景图像P 0分割成至少两幅图像P 0-j；
    S3：获取移动机器人在t i时刻的周围环境的全景图像P i；
    S4：将全景图像P i分割成与图像P 0-j对应的至少两幅图像P i-j；
    S5：根据图像P 0-j与P i-j的匹配关系分别计算移动机器人的位姿T i-j；
    S6：根据位姿T i-j确定移动机器人在t i时刻的位姿。
  2. 根据权利要求1所述的移动机器人的位姿估计方法，其特征在于，
    还包括步骤S7：将t i时刻的位置作为参考位置再返回步骤S1。
  3. 根据权利要求1所述的移动机器人的位姿估计方法，其特征在于，
    所述步骤S1中的参考位置为移动机器人开始工作时的位置。
  4. 根据权利要求1所述的移动机器人的位姿估计方法，其特征在于，
    所述步骤S2是通过立方体模型将全景图像进行分割的。
  5. 根据权利要求1所述的移动机器人的位姿估计方法，其特征在于，
    将所述步骤S2的全景图像P 0、步骤S4全景图像P i分别分割成上视图像和前视图像。
  6. 根据权利要求5所述的移动机器人的位姿估计方法，其特征在于，所述根据上视图像计算移动机器人位姿的步骤包括：
    步骤a1：提取全景图像P 0、全景图像P i的上视图像特征并匹配；
    步骤a2：根据图像匹配算法匹配提取的特征点；
    步骤a3：根据匹配特征点的匹配关系得到单应矩阵H；
    步骤a4：根据相机内参数分解单应矩阵H以得到位姿估计T i-上。
  7. 根据权利要求5所述的移动机器人的位姿估计方法，其特征在于，所述根据前视图像计算移动机器人位姿的步骤包括：
    步骤b1：提取全景图像P 0、全景图像P i的前视图像特征并匹配；
    步骤b2：根据图像匹配算法匹配提取的特征点；
    步骤b3：根据匹配特征点的匹配关系得到基础矩阵F；
    步骤b4：根据相机内参数分解基础矩阵F以得到位姿估计T i-前。
  8. 根据权利要求1所述的移动机器人的位姿估计方法，其特征在于，所述步骤S6包括：
    S61：分别计算各T i-j的误差；
    S62：选择误差范围最小的位姿作为移动机器人在t i时刻的位姿。
  9. 根据权利要求8所述的移动机器人的位姿估计方法，其特征在于，
    所述步骤S61是基于图像P 0-j与P i-j的欧拉旋转角进行误差计算的。
  10. 一种移动机器人的位姿估计方法，其特征在于，所述移动机器人包括全景摄像头和行程测量装置，包括以下步骤：
    S1：初始化或获取移动机器人在参考位置的位姿信息及全景图像P 0；
    S2：将全景图像P 0分割成至少两幅图像P 0-j；
    S3：获取移动机器人在t i时刻的周围环境的全景图像P i；
    S4：将全景图像P i分割成与图像P 0-j对应的至少两幅图像P i-j；
    S5：根据图像P 0-j与P i-j的匹配关系分别计算移动机器人的位姿T i-j；
    S6：根据位姿T i-j确定移动机器人在t i时刻的位姿；
    其中，在步骤S1中将移动机器人开始工作点位置作为参考位置并初始化为地图原点或获取保存在地图中的某个记忆点位置作为参考位置并获取该记忆点位置的坐标信息，所述位姿信息指移动机器人在地图中的坐标及行进方向。
  11. 根据权利要求10所述的移动机器人的位姿估计方法，其特征在于，
    还包括步骤S7：将t i时刻的位置作为参考位置再返回步骤S1；所述步骤S2是通过立方体模型将全景图像进行分割的；所述步骤S2是通过立方体模型将全景图像进行分割的；将所述步骤S2的全景图像P 0、步骤S4全景图像P i分别分割成上视图像和前视图像；
    所述根据上视图像计算移动机器人位姿的步骤包括：
    步骤a1：提取全景图像P 0、全景图像P i的上视图像特征并匹配，提取全景图像P i的上视图像和全景图像P 0的上视图像的特征点并计算得到特征描绘子，提取ORB特征描绘子的步骤为：使用FAST算法求特征点；对FAST特征点分配方向生成OFAST；利用分配的方向求有向BRIEF；贪婪算法筛选高区分度的有向BRIEF,即ORB描绘子；
    步骤a2：根据图像匹配算法匹配提取的特征点，根据图像匹配算法匹配提取的特征描绘子；提取的ORB描绘子是二进制码串，使用Hamming距离来度量 ORB描绘子的相似度，得到匹配点对；
    步骤a3：根据匹配特征点的匹配关系得到单应矩阵H；
    步骤a4：根据相机内参数分解单应矩阵H以得到位姿估计T i-上；
    所述根据前视图像计算移动机器人位姿的步骤包括：
    步骤b1：提取全景图像P 0、全景图像P i的前视图像特征并匹配；
    步骤b2：根据图像匹配算法匹配提取的特征点；
    步骤b3：根据匹配特征点的匹配关系得到基础矩阵F；
    步骤b4：根据相机内参数分解基础矩阵F以得到位姿估计T i-前；
    所述步骤S6包括：
    S61：分别计算各T i-j的误差；
    S62：选择误差范围最小的位姿作为移动机器人在t i时刻的位姿；
    所述步骤S61是基于图像P 0-j与P i-j的欧拉旋转角进行误差计算的。
PCT/CN2018/073928 2017-10-18 2018-01-24 移动机器人的位姿估计方法 WO2019075948A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201710969938.8A CN107843251B (zh) 2017-10-18 2017-10-18 移动机器人的位姿估计方法
CN201710969938.8 2017-10-18

Publications (1)

Publication Number Publication Date
WO2019075948A1 true WO2019075948A1 (zh) 2019-04-25

Family

ID=61662534

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2018/073928 WO2019075948A1 (zh) 2017-10-18 2018-01-24 移动机器人的位姿估计方法

Country Status (2)

Country Link
CN (1) CN107843251B (zh)
WO (1) WO2019075948A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112583069A (zh) * 2020-11-19 2021-03-30 深圳拓邦股份有限公司 一种机器人充电保护方法、装置、机器人及存储介质

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108682038B (zh) * 2018-04-27 2021-12-14 腾讯科技（深圳）有限公司 位姿确定方法、装置及存储介质
CN110567469B (zh) * 2018-06-05 2021-07-20 北京市商汤科技开发有限公司 视觉定位方法、装置、电子设备及系统
CN109099915B (zh) * 2018-06-27 2020-12-25 未来机器人（深圳）有限公司 移动机器人定位方法、装置、计算机设备和存储介质
CN110874818B (zh) * 2018-08-31 2023-06-23 阿里巴巴集团控股有限公司 图像处理和虚拟空间构建方法、装置、系统和存储介质
CN111292420B (zh) 2020-02-28 2023-04-28 北京百度网讯科技有限公司 用于构建地图的方法和装置
CN112068128B (zh) * 2020-09-19 2024-02-02 重庆大学 一种直道场景线段型雷达数据处理及位姿获取方法
CN112927362A (zh) * 2021-04-07 2021-06-08 Oppo广东移动通信有限公司 地图重建方法及装置、计算机可读介质和电子设备
CN113686332A (zh) * 2021-09-08 2021-11-23 上海快仓智能科技有限公司 移动机器人及其导航方法、装置、设备和存储介质
CN115775325B (zh) * 2023-01-29 2023-06-06 摩尔线程智能科技(北京)有限责任公司 一种位姿确定方法及装置、电子设备和存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1493247A (zh) * 2002-10-31 2004-05-05 ������������ʽ���� 机器人吸尘器系统及其与充电设备对接方法
US20050065655A1 (en) * 2003-09-16 2005-03-24 Samsung Electronics Co., Ltd. Apparatus and method for estimating a position and an orientation of a mobile robot
CN103020957A (zh) * 2012-11-20 2013-04-03 北京航空航天大学 移动机器人机载摄像机位置标定方法
CN103252778A (zh) * 2011-12-23 2013-08-21 三星电子株式会社 用于估计机器人位置的设备及其方法
CN106646441A (zh) * 2016-11-16 2017-05-10 杭州国辰牵星科技有限公司 结合环境信息的室内移动机器人定位系统及方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100702663B1 (ko) * 2005-08-27 2007-04-02 한국과학기술원 입자 필터 프레임 워크에서 전방향 시각센서 기반 위치추정 및 매핑을 위한 방법
CN102914967B (zh) * 2012-09-21 2015-01-28 浙江工业大学 采摘机器人的自主导航和人机协同采摘作业系统
CN104359464A (zh) * 2014-11-02 2015-02-18 天津理工大学 基于立体视觉的移动机器人定位方法
CN106338287A (zh) * 2016-08-24 2017-01-18 杭州国辰牵星科技有限公司 基于天花板的室内移动机器人视觉定位方法
CN107063228B (zh) * 2016-12-21 2020-09-04 上海交通大学 基于双目视觉的目标姿态解算方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1493247A (zh) * 2002-10-31 2004-05-05 ������������ʽ���� 机器人吸尘器系统及其与充电设备对接方法
US20050065655A1 (en) * 2003-09-16 2005-03-24 Samsung Electronics Co., Ltd. Apparatus and method for estimating a position and an orientation of a mobile robot
CN103252778A (zh) * 2011-12-23 2013-08-21 三星电子株式会社 用于估计机器人位置的设备及其方法
CN103020957A (zh) * 2012-11-20 2013-04-03 北京航空航天大学 移动机器人机载摄像机位置标定方法
CN106646441A (zh) * 2016-11-16 2017-05-10 杭州国辰牵星科技有限公司 结合环境信息的室内移动机器人定位系统及方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112583069A (zh) * 2020-11-19 2021-03-30 深圳拓邦股份有限公司 一种机器人充电保护方法、装置、机器人及存储介质
CN112583069B (zh) * 2020-11-19 2023-08-08 深圳拓邦股份有限公司 一种机器人充电保护方法、装置、机器人及存储介质

Also Published As

Publication number Publication date
CN107843251B (zh) 2020-01-31
CN107843251A (zh) 2018-03-27

Similar Documents

Publication Publication Date Title
WO2019075948A1 (zh) 移动机器人的位姿估计方法
WO2021196294A1 (zh) 一种跨视频人员定位追踪方法、系统及设备
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN106940704B (zh) 一种基于栅格地图的定位方法及装置
US10659768B2 (en) System and method for virtually-augmented visual simultaneous localization and mapping
US10469828B2 (en) Three-dimensional dense structure from motion with stereo vision
CN110176032B (zh) 一种三维重建方法及装置
US20200334842A1 (en) Methods, devices and computer program products for global bundle adjustment of 3d images
CN110799921A (zh) 拍摄方法、装置和无人机
CN109472820B (zh) 单目rgb-d相机实时人脸重建方法及装置
CN111127524A (zh) 一种轨迹跟踪与三维重建方法、系统及装置
CN109785373B (zh) 一种基于散斑的六自由度位姿估计系统及方法
JP4132068B2 (ja) 画像処理装置及び三次元計測装置並びに画像処理装置用プログラム
CN112083403B (zh) 用于虚拟场景的定位追踪误差校正方法及系统
GB2567245A (en) Methods and apparatuses for depth rectification processing
CN115423863B (zh) 相机位姿估计方法、装置及计算机可读存储介质
JP6410231B2 (ja) 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム
JP2017027417A (ja) 画像処理装置及び電気掃除器
CN113345084B (zh) 三维建模系统及三维建模方法
CN111105467A (zh) 一种图像标定方法、装置及电子设备
CN111914790B (zh) 基于双摄像头的不同场景下实时人体转动角度识别方法
WO2020153264A1 (ja) 校正方法および校正装置
WO2023098737A1 (zh) 三维重建方法、电子设备和计算机可读存储介质
EP2879090B1 (en) Aligning ground based images and aerial imagery
CN112767481B (zh) 一种基于视觉边缘特征的高精度定位及建图方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 18868208

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 18868208

Country of ref document: EP

Kind code of ref document: A1