CN110631554B - 机器人位姿的确定方法、装置、机器人和可读存储介质 - Google Patents
机器人位姿的确定方法、装置、机器人和可读存储介质 Download PDFInfo
- Publication number
- CN110631554B CN110631554B CN201810649163.0A CN201810649163A CN110631554B CN 110631554 B CN110631554 B CN 110631554B CN 201810649163 A CN201810649163 A CN 201810649163A CN 110631554 B CN110631554 B CN 110631554B
- Authority
- CN
- China
- Prior art keywords
- frame image
- robot
- feature
- current frame
- line segments
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
- G01C11/04—Interpretation of pictures
Landscapes
- Engineering & Computer Science (AREA)
- Multimedia (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本公开涉及一种机器人位姿的确定方法、装置、机器人和可读存储介质，涉及计算机技术领域。本公开的方法包括：通过在机器人拍摄的当前帧图像和前一帧图像中匹配相同的特征点和特征线段，并参考机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态；根据地图中的特征点和特征线段在当前帧图像中的匹配信息，以及估计的机器人拍摄当前帧图像时的位置和姿态，确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差；根据误差对估计的机器人拍摄当前帧图像时的位置和姿态进行修正，从而确定机器人拍摄当前帧图像时的位置和姿态。本公开的方案对SVO算法进行改进，提高了机器人的位置和姿态的估计的准确性。
Description
技术领域
本公开涉及计算机技术领域，特别涉及一种机器人位姿的确定方法、装置、机器人和可读存储介质。
背景技术
视觉里程计(Visual Odometry，VO)是通过处理分析相机的序列图像来估计机器人位置和姿态的相对变化，是视觉SLAM(Simultaneous Localization and Mapping，即时定位与地图构建)系统中的前端工作，是SLAM系统不可或缺的组成部分。作为移动机器人自主定位和导航的关键技术，视觉里程计也是移动智能机器人方向的研究热点，在整个机器人智能化方面起着举足轻重的作用。
2014年，苏黎世大学机器人感知组的Forster等人提出了的SVO(Semi-DirectMonocular Visual Odometry，半直接单目视觉里程计里程计)算法。SVO算法是目前移动机器人自主定位和导航中普遍应用的算法。
发明内容
发明人发现：SVO算法根据图像中的点特征估计机器人的位置和姿态。在低纹理和结构化的情景中点特征不丰富，例如图1中，大面积的图像纹理相同(例如墙、门)、结构化明显，属于低纹理和结构化的情景。因此，基于点特征的SVO算法的鲁棒性和准确性大大降低机器人的位置和姿态的估计的准确性。
本公开所要解决的一个技术问题是：如何低纹理和结构化场景中如何提高。
根据本公开的一些实施例，提供的一种机器人位姿的确定方法，包括：通过在机器人拍摄的当前帧图像和前一帧图像中匹配相同的特征点和特征线段，并参考机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态；根据地图中的特征点和特征线段在当前帧图像中的匹配信息，以及估计的机器人拍摄当前帧图像时的位置和姿态，确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差；根据误差对估计的机器人拍摄当前帧图像时的位置和姿态进行修正，从而确定机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，估计机器人拍摄当前帧图像时的位置和姿态包括：通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在前一帧图像中的投影信息，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化；根据机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化，以及机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化包括：确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第一亮度信息；确定地图中的特征点和特征线段上的点在前一帧图像中的投影的亮度信息作为第二亮度信息；使第一亮度信息和第二亮度信息误差最小的情况下，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化。
在一些实施例中，确定地图中的特征点和特征线段在当前帧图像中的投影信息包括：确定地图中的特征点和特征线段上的点在拍摄对应的关键帧图像时机器人坐标系下的三维坐标；根据机器人拍摄当前帧图像相对于拍摄关键帧图像的位移和姿态变化，将特征点和特征线段上的点的三维坐标由拍摄关键帧图像时机器人坐标系下，转换至拍摄当前帧图像时机器人坐标系下；根据相机的内置参数，以及特征点和特征线段上的点在拍摄当前帧图像时机器人坐标系下的三维坐标，将特征点和特征线段的点投影到当前帧图像上，得到二维坐标作为在当前帧图像中的投影信息。
在一些实施例中，确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差包括：根据估计的机器人拍摄当前帧图像时的位置和姿态，确定地图中的特征点和特征线段在当前帧图像中的估计位置；根据地图中的特征点和特征线段在当前帧图像中的匹配信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置；根据估计位置与预测位置确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
在一些实施例中，确定地图中的特征点和特征线段在当前帧图像中的预测位置包括:通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在关键帧图像中的投影信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
在一些实施例中，确定地图中的特征点和特征线段在当前帧图像中的预测位置包括：确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第三亮度信息；确定地图中的特征点和特征线段上的点在关键帧图像中的投影的亮度信息作为第四亮度信息；使第三亮度信息和第四亮度信息误差最小的情况下，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
在一些实施例中，确定机器人拍摄当前帧图像时的位置和姿态包括：通过修正估计的机器人拍摄当前帧图像时的位置和姿态调整估计位置与预测位置的误差；在估计位置与预测位置的误差最小的情况下，确定机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，地图中的特征点和特征线段包括从关键帧图像中提取的特征点和特征线段，特征点用该特征点及其周围预设范围内的像素信息和该特征点的坐标信息描述，特征线段用该特征线段上的点及其周围预设范围内的像素信息和该特征线段的端点的坐标信息描述。
在一些实施例中，特征线段上的点包括：特征线段的端点；或者，特征线段上的点包括：特征线段的端点和预设数量的均匀分布在特征线段上的点。
在一些实施例中，该方法还包括：判断当前帧图像是否为关键帧图像；在当前帧图像为关键帧图像的情况下，清除地图中的特征点和特征线段，重新提取当前帧图像中的特征点和特征线段重新生成地图，以便利用重新生成的地图确定机器人拍摄下一帧图像时的位置和姿态。
在一些实施例中，判断当前帧图像是否为关键帧图像包括：在地图中的特征点和特征线段在当前帧图像中的投影数目小于第一阈值的情况下，确定当前帧图像为关键帧图像；或者，在关键帧图像在当前帧图像中的匹配到的特征点和特征线段的数目小于第二阈值的情况下，确定当前帧图像为关键帧图像。
在一些实施例中，生成地图包括：根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，利用三角测量算法确定特征点和特征线段端点的三维坐标。
在一些实施例中，根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，确定机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化；根据机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化，以及特征点和特征线段在关键帧和对应的辅助帧中的位置，利用三角测量算法确定特征点和特征线段端点的三维坐标。
在一些实施例中，确定地图中的特征点和特征线段包括：计算关键帧中每个特征点和特征线段在辅助帧中重投影距离，选取重投影距离大于阈值的特征点和特征线段作为地图中的特征点和特征线段。
在一些实施例中，辅助帧图像为与关键帧图像中的特征点和特征线段的匹配度在预设范围内的一帧图像。
根据本公开的另一些实施例，提供的一种机器人位姿的确定装置，包括：位姿估计模块，用于通过在机器人拍摄的当前帧图像和前一帧图像中匹配相同的特征点和特征线段，并参考机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态；误差确定模块，用于根据地图中的特征点和特征线段在当前帧图像中的匹配信息，以及估计的机器人拍摄当前帧图像时的位置和姿态，确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差；位姿确定模块，用于根据误差对估计的机器人拍摄当前帧图像时的位置和姿态进行修正，从而确定机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，位姿估计模块用于通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在前一帧图像中的投影信息，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化；根据机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化，以及机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，位姿估计模块用于确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第一亮度信息；确定地图中的特征点和特征线段上的点在前一帧图像中的投影的亮度信息作为第二亮度信息；使第一亮度信息和第二亮度信息误差最小的情况下，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化。
在一些实施例中，位姿估计模块用于确定地图中的特征点和特征线段上的点在拍摄对应的关键帧图像时机器人坐标系下的三维坐标；根据机器人拍摄当前帧图像相对于拍摄关键帧图像的位移和姿态变化，将特征点和特征线段上的点的三维坐标由拍摄关键帧图像时机器人坐标系下，转换至拍摄当前帧图像时机器人坐标系下；根据相机的内置参数，以及特征点和特征线段上的点在拍摄当前帧图像时机器人坐标系下的三维坐标，将特征点和特征线段的点投影到当前帧图像上，得到二维坐标作为在当前帧图像中的投影信息。
在一些实施例中，误差确定模块用于根据估计的机器人拍摄当前帧图像时的位置和姿态，确定地图中的特征点和特征线段在当前帧图像中的估计位置；根据地图中的特征点和特征线段在当前帧图像中的匹配信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置；根据估计位置与预测位置确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
在一些实施例中，误差确定模块用于通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在关键帧图像中的投影信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
在一些实施例中，误差确定模块用于确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第三亮度信息；确定地图中的特征点和特征线段上的点在关键帧图像中的投影的亮度信息作为第四亮度信息；使第三亮度信息和第四亮度信息误差最小的情况下，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
在一些实施例中，位姿确定模块用于通过修正估计的机器人拍摄当前帧图像时的位置和姿态调整估计位置与预测位置的误差；在估计位置与预测位置的误差最小的情况下，确定机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，地图中的特征点和特征线段包括从关键帧图像中提取的特征点和特征线段，特征点用该特征点及其周围预设范围内的像素信息和该特征点的坐标信息描述，特征线段用该特征线段上的点及其周围预设范围内的像素信息和该特征线段的端点的坐标信息描述。
在一些实施例中，特征线段上的点包括：特征线段的端点；或者，特征线段上的点包括：特征线段的端点和预设数量的均匀分布在特征线段上的点。
在一些实施例中，该装置还包括：关键帧判断模块，用于判断当前帧图像是否为关键帧图像；地图生成模块，用于在当前帧图像为关键帧图像的情况下，清除地图中的特征点和特征线段，重新提取当前帧图像中的特征点和特征线段重新生成地图，以便利用重新生成的地图确定机器人拍摄下一帧图像时的位置和姿态。
在一些实施例中，关键帧判断模块用于在地图中的特征点和特征线段在当前帧图像中的投影数目小于第一阈值的情况下，确定当前帧图像为关键帧图像；或者，在关键帧图像在当前帧图像中的匹配到的特征点和特征线段的数目小于第二阈值的情况下，确定当前帧图像为关键帧图像。
在一些实施例中，地图生成模块用于根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，利用三角测量算法确定特征点和特征线段端点的三维坐标。
在一些实施例中，地图生成模块用于根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，确定机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化；根据机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化，以及特征点和特征线段在关键帧和对应的辅助帧中的位置，利用三角测量算法确定特征点和特征线段端点的三维坐标。
在一些实施例中，地图生成模块用于计算关键帧中每个特征点和特征线段在辅助帧中重投影距离，选取重投影距离大于阈值的特征点和特征线段作为地图中的特征点和特征线段。
在一些实施例中，辅助帧图像为与关键帧图像中的特征点和特征线段的匹配度在预设范围内的一帧图像。
根据本公开的又一些实施例，提供的一种机器人，包括：前述任意实施例中的机器人位姿的确定装置。
根据本公开的再一些实施例，提供的一种机器人位姿的确定装置，包括：存储器；以及耦接至存储器的处理器，处理器被配置为基于存储在存储器设备中的指令，执行如前述任意实施例中的机器人位姿的确定方法的步骤。
根据本公开的再一些实施例，提供的一种计算机可读存储介质，其上存储有计算机程序，其中，该程序被处理器执行时实现前述任意实施例中的机器人位姿的确定方法的步骤。
本公开中利用图像中的特征点和特征线段估计机器人的位姿。首先在相邻两帧图像中追踪特征点和特征线段，估计机器人拍摄当前帧图像时的位置和姿态，进一步，根据地图中的特征点和特征线段在当前帧中的匹配信息，对估计的机器人拍摄当前帧图像时的位置和姿态进行优化，从而最终确定机器人拍摄当前帧图像时的位置和姿态。本公开的方案提取图像中的特征线段更能反映低纹理和结构化图像中的特征，对SVO算法进行改进，提高了机器人的位置和姿态的估计的准确性。
通过以下参照附图对本公开的示例性实施例的详细描述，本公开的其它特征及其优点将会变得清楚。
附图说明
为了更清楚地说明本公开实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1示出本公开的一些实施例的低纹理、结构化的场景示意图。
图2示出本公开的一些实施例的机器人位姿的确定方法的流程示意图。
图3示出本公开的另一些实施例的机器人位姿的确定方法的流程示意图。
图4示出本公开的一些实施例的机器人位姿的确定装置的结构示意图。
图5示出本公开的另一些实施例的机器人位姿的确定装置的结构示意图。
图6示出本公开的又一些实施例的机器人的结构示意图。
图7示出本公开的又一些实施例的机器人位姿的确定装置的结构示意图。
图8示出本公开的再一些实施例的机器人位姿的确定装置的结构示意图。
具体实施方式
下面将结合本公开实施例中的附图，对本公开实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本公开一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本公开及其应用或使用的任何限制。基于本公开中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。
本公开改进SVO算法，提出一种机器人位姿的确定方法，下面结合图2进行描述。
图2为本公开机器人位姿的确定方法一些实施例的流程图。如图2所示，该实施例的方法包括：步骤S202～S206。
在步骤S202中，通过在机器人拍摄的当前帧图像和前一帧图像中匹配相同的特征点和特征线段，并参考机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态。
SVO算法主要包括两部分，一部分是运动估计，一部分是构建地图，两部分相辅相成。运动估计部分主要完成机器人位姿(位置和姿态)的估计，需要用到地图中的特征点或特征线段的信息，构建地图部分主要是完成特征点或线段的深度信息)的估计，需要利用通过运动估计获得的机器人的位姿。本实施例中主要描述运动估计部分的改进算法。本公开中对构建地图部分也进行了改进，在后续实施例中将进行详细描述。
运动估计的第一步是在连续两帧图像中进行特征点和特征线段的追踪，也就是匹配相同的特征点和特征线段。通过比对连续两帧图像，并且已知前一帧图像拍摄时机器人的位姿，则可以粗略估计当前帧图像拍摄时机器人的位姿。
上述问题可以描述为：已知前一帧图像对地图中特征点和特征线段的观测，以及当前帧图像，如何计算当前帧图像对应的机器人的位姿。用数学语言描述为，已知k-1帧的位姿Tk-1，并且可以获得地图上的特征点和特征线段在k-1帧图像上的的观测量ui,i＝1,…,Nu(ui表示特征点i，Nu表示特征点的个数)，lj,j＝1,…,Nl(lj表示特征点j，Nl表示特征线段的个数)时，求解机器人拍摄第k帧图像时相对于第k-1帧图像的位姿变化Tk-1,k。
特征点和特征线段是从关键帧图像中提取的特征点和特征线段，例如通过LSD(aLine Segment Detector，线段检测法)提取线段特征，ORB(Oriented FAST and RotatedBRIEF)算法提取特征点。这些特征点和特征线段被确定深度信息后用于生成地图，这些过程将在后续实施例进行描述。地图中的特征点和特征线段可以在机器人拍摄的其他帧图像中观测到，例如，机器人在行走过程中多帧图像中都可以拍到门上的特征点和特征线段。
图像中特征点可以采用两种信息进行表示：一是特征点的描述，也就是描述子，通常描述特征点及其周围像素的信息(图像块)；二是特征点在图像坐标系的位置信息。特征线段的可以表示为：特征线段上点的描述子和特征线段上点图像坐标系的位置信息。地图中特征点表示为上述图像中特征点对应的信息和特征点对应的深度信息。地图中特征线段表示为上述图像中特征线段对应的信息还包括特征线段的端点对应的深度信息。
在一些实施例中，通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在前一帧图像中的投影信息，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化；根据机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化，以及机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态。
进一步，确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第一亮度信息；确定地图中的特征点和特征线段上的点在前一帧图像中的投影的亮度信息作为第二亮度信息；使第一亮度信息和第二亮度信息误差最小的情况下，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化。
上述方法可以采用以下公式和数学方法进行描述。
前面提到Tk-1,k表示在标定的机器人相机坐标系中两个连续帧图像对应的位姿的相对变化。设Tk-1,w表示为在世界坐标系中在第k时刻的机器人的姿态。Ik表示第k帧图像的灰度图像。将图像上点特征表示为u，对应的深度为du。针对特征线段，设线段的两个端点分别由p和q表示，线方程用l表示。π(·)表示相机投影模型，表示将特征点或特征线段从三维坐标系中投影到图像的二维坐标系中。π-1(·)表示相机逆投影模型，表示将特征点或特征线段从图像中反投影到三维坐标系中。
针对图像上线段的追踪，与特征点的追踪方法不同，如果直接对齐两帧之间的线段所占据的整个区域，计算量很高。为此，本公开选取特征线段的端点、或者特征线段的端点和均匀分布在特征线段上的点来表示特征线段。
在一些实施例中，地图上特征线段l在当前帧图像中投影的像素与在前一帧图像中投影的像素的亮度差δIl(Tk-1,k，l)可以采用以下公式表示。
公式(1)中，sn，n＝0,...,Nl，表示特征线段上均匀分布的点的三维坐标(拍摄k-1帧图像时机器人坐标系)，n＝0或Nl时表示特征线段的端点的三维坐标。其他字符已在之前描述，在此不再赘述。
进一步，机器人拍摄当前帧图像与拍摄前一帧图像时的位姿变化可采用以下公式计算。
其中，L为在前一帧图像中特征线段对应的图像区域，并且特征线段的端点具有已知深度，在当前帧图像中可见；R为在前一帧图像中特征点对应的图像区域，并且特征点具有已知深度，在当前帧图像中可见。δIu(Tk-1,k，ui)表示地图上特征点在当前帧图像中投影的像素与在前一帧图像中投影的像素的亮度差，与传统SVO算法中的计算方法一样，在此不再赘述。公式(2)可以采用高斯-牛顿算法进行求解，属于现有技术，在此不再赘述。
上述实施例中用到了地图中的特征点和特征线段在其他帧中的投影。可以参考以下方法进行投影：确定地图中的特征点和特征线段对应的关键帧图像，以及特征点和特征线段上的点在拍摄关键帧图像时机器人坐标系下的三维坐标；根据机器人拍摄待投影帧图像相对于拍摄关键帧图像的位移和姿态变化，以及特征点和特征线段上的点在拍摄关键帧图像时机器人坐标系下的三维坐标，确定特征点和特征线段上的点在拍摄待投影帧图像时机器人坐标系下的三维坐标；根据相机的内置参数，以及特征点和特征线段上的点在拍摄待投影帧图像时机器人坐标系下的三维坐标，将特征点和特征线段的点投影到待投影帧图像上，得到二维坐标；待投影帧图像包括：当前帧和前一帧图像。
地图中一般存储的是关键帧中特征点和特征线段的三维坐标，这个三维坐标可以是世界坐标系下的坐标也可以是拍摄关键帧时机器人坐标系下的坐标，两者可以根据机器人当时的位姿进行转换。特征点和特征线段在拍摄关键帧时机器人坐标系下的三维坐标可以表示成在关键帧图像中的二维坐标加深度信息。这些都与SVO算法中相同。
地图中的特征点和特征线段在其他帧图像中投影的过程可以简述为：已知IKF中的某个特征在图像平面的位置，以及它的深度，能够将该特征投影到三维空间得到三维坐标表示的特征pKF，该三维空间的坐标系是定义在IKF摄像机坐标系的。要将它投影到当前帧Ik中，需要位姿转换TKF,k，得到该点在当前帧拍摄时机器人坐标系中的三维坐标pk。最后通过相机内参数，投影到Ik的当前帧图像平面完成重投影。
上述在当前帧图像和前一帧图像中匹配相同的特征点和特征线段可以采用LK(Lukas Kanade)光流算法，在此不再赘述。在连续帧间追踪特征前，需要进行初始化。初始化例如包括：第一帧图像上提取的特征点和特征线段，作为关键帧，后续各帧图像分别与第一帧图像的特征点和特征线段匹配，直到匹配到的特征点和特征线段平均视差比较大，就认为初始化成功，计算对应特征点和特征线段端点的深度。之后进入正常工作模式。上述初始化过程与SVO算法中相似，具体过程不再赘述。
在步骤S204中，根据地图中的特征点和特征线段在当前帧图像中的匹配信息，以及估计的机器人拍摄当前帧图像时的位置和姿态，确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
在一些实施例中，根据估计的机器人拍摄当前帧图像时的位置和姿态，确定地图中的特征点和特征线段在当前帧图像中的估计位置；根据地图中的特征点和特征线段在当前帧图像中的匹配信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置；根据估计位置与预测位置确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
通过前一步骤得到了拍摄当前帧图像时机器人位姿的粗略估计。因为估计的位姿是基于上一帧图像的结果来计算的，所以如果把它当作真实位姿估计的话，不可避免的会带来累计误差从而导致漂移。因此，需要进一步根据已经建立好的地图模型，将当前帧图像和地图之间进行特征点和特征线段比对，来对拍摄当前帧图像时机器人位姿进一步优化。
通过地图保存了很多三维空间点(关键帧的特征点或线段)，当前帧图像是可能看到地图中的某些特征点或特征线段的。由于机器人拍摄当前帧时的位姿已经通过步骤S202中估计出来，因此，根据估计的拍摄当前帧时的位姿可以得到地图上的特征点或特征线段在当前帧中的投影。而实际上，估计的位姿可能存在误差，使地图上的征点或特征线段在当前帧中的预测投影可能不同于通过估计的位姿得到的投影。
在一些实施例中，通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在关键帧图像中的投影信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
可以基于亮度不变性假设找到地图中特征点和特征线段在当前帧中应该出现的位置即预测位置。特征点和特征线段对应的图像块(描述子)在以前参考帧(即地图对应的关键帧)中的亮度应该和当前帧中的亮度差不多。所以根据亮度可以重新构造一个残差，对特征点和特征线段在当前帧中预测位置进行优化。可采用Lucas Kanade光流算法解决。
在一些实施例中，确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第三亮度信息；确定地图中的特征点和特征线段上的点在关键帧图像中的投影的亮度信息作为第四亮度信息；使第三亮度信息和第四亮度信息误差最小的情况下，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
可以采用以下公式确定地图中的特征点和特征线段在当前帧图像中的预测位置。
其中，s′j是当前帧图像中特征点或特征线段的预测位置(s′j针对特征线段维持两个端点都相等)，sj是特征点或特征线段在参考帧r中的位置，如果地图中存储了多个关键帧对应的特征点和特征线段，则选取与当前帧观测角度最小的关键帧作为参考帧。针对本公开后续对地图的改进方案，该处的参考帧可以为关键帧，或者关键帧和辅助帧。Aj为放射变换参数。
通过上述方法能够得到优化后的地图中特征点和特征线段在当前帧图像中的预测位置，它比通过估计的当前帧图像对应位姿来确定地图中特征点和特征线段在当前帧图像的估计位置更准，两者之间存在误差。因此，可以利用预测位置和估计位置进一步去优化机器人拍摄当前帧时的位姿以及特征点和特征线段的三维坐标即执行下一步骤。
在步骤S206中，根据误差对估计的机器人拍摄当前帧图像时的位置和姿态进行修正，从而确定机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，通过修正估计的机器人拍摄当前帧图像时的位置和姿态调整估计位置与预测位置的误差；在估计位置与预测位置的误差最小的情况下，确定机器人拍摄当前帧图像时的位置和姿态。
可以采用以下公式确定机器人拍摄当前帧图像时的位置和姿态。
公式(4)中，eu(·)表示特征点在当前帧图像中的预测位置与估计位置的误差，el(·)表示特征线段在当前帧图像中的预测位置与估计位置的误差，包括特征线段端点和均匀分布在特征线段上的点在当前帧图像中的预测位置与估计位置的误差。其他符号与之前的实施例中含义相同，不再赘述。上述公式可以采用高斯牛顿算法进行求解。
上述实施例的方法中利用图像中的特征点和特征线段估计机器人的位姿。首先在相邻两帧图像中追踪特征点和特征线段，估计机器人拍摄当前帧图像时的位置和姿态，进一步，根据地图中的特征点和特征线段在当前帧中的匹配信息，对估计的机器人拍摄当前帧图像时的位置和姿态进行优化，从而最终确定机器人拍摄当前帧图像时的位置和姿态。上述实施例的方法提取图像中的特征线段更能反映低纹理和结构化图像中的特征，对SVO算法进行改进，提高了机器人的位置和姿态的估计的准确性。
至此，运动估计部分的方案描述完。下面描述构建地图的方案，这一部分也将介绍上述实施例中提到的地图中的特征点和线特征段是如何得到的。在传统的SVO算法中采用的是增量式地图，即地图中的信息不断增加，而本公开中采用半固定式地图，下面结合图3进行具体描述。
图3为本公开机器人位姿的确定方法另一些实施例的流程图。如图3所示，该实施例的方法包括：步骤S302～S304。
在步骤S302中，判断当前帧图像是否为关键帧图像。
根据前述的实施例确定了机器人拍摄当前帧图像时的位置和姿态。针对当前帧可以进一步确定其是否为关键帧，如果是关键帧则可以利用其生成地图，如果不是，则不用于生成地图，不执行步骤S304。
本公开中改进了关键帧的选取方法。在一些实施例中，关键帧的选取方法按照以下至少一种准则执行:在地图中的特征点和特征线段在当前帧图像中的投影数目小于第一阈值的情况下，确定当前帧图像为关键帧图像。或者，在关键帧图像在当前帧图像中的匹配到的特征点和特征线段的数目小于第二阈值的情况下，确定当前帧图像为关键帧图像。
上述关键帧的选取方法主要考虑机器人在拍摄辅助帧与拍摄下一个关键帧之间的时间中运动时，很可能出现大尺度运动，造成特征跟踪的数量急剧下降而使机器人运动估计发生严重漂移。因此，提出的关键帧选取准则跟场景的深度没有关系，则对机器人指向不作限制。
在步骤S304中，在当前帧图像为关键帧图像的情况下，清除地图中的特征点和特征线段，重新提取当前帧图像中的特征点和特征线段重新生成地图。
重新生成的地图可用来确定机器人拍摄下一帧图像时的位置和姿态。不同于传统的SVO算法中的增量地图，本公开中地图属于半固定式地图，即在下一个关键帧到来之前，地图中的特征点和特征线段是不变的。通过简化地图的生成方式，减少了计算量和存储量，提高了构建地图的效率，进而提高了算法的整体效率。
在一些实施例中，根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，利用三角测量算法确定特征点和特征线段端点的三维坐标。
针对每一个关键帧图像需要选取对应的辅助帧图像，用于确定关键帧图像中特征点和特征线段的三维坐标信息。辅助帧图像例如为与关键帧图像中的特征点和特征线段的匹配度在预设范围内的一帧图像。辅助帧是相对于关键帧而言的，通过与关键帧之间的特征追踪(参考上述实施例)，确定与关键帧的特征匹配数量在预设比例范围内(例如70％～80％)且距离关键帧最近的一帧图像作为辅助帧。
进一步，可以选取部分关键帧中提取的特征点和特征线段作为地图中的特征点和特征线段。具体的，计算关键帧中每个特征点和特征线段在辅助帧中重投影距离，选取重投影距离大于阈值的特征点和特征线段作为地图中的特征点和特征线段。
构建地图的主要步骤包括：确定关键帧中特征点和特征线段的深度信息，从而确定它们的三维坐标。
在一些实施例中，根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，确定机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化；根据机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化，以及特征点和特征线段在关键帧和对应的辅助帧中的位置，利用三角测量算法确定特征点和特征线段端点的三维坐标。
本公开中针对线段特征在构建地图时选取线段特征的端点，这样估计端点的深度，使地图具有实际意义。构建地图的方法可以具体包括以下步骤：
1)提取关键帧图像的特征点和特征线段。
可以将关键帧划分为不同的区域，在每一个区域中提取一个特征点或特征线段，例如具有最高响应值的特征点或特征线段。
2)在辅助帧图像中匹配关键帧图像中的特征点和特征线段。
例如，可以采用LK算法匹配特征点以及特征线段的端点。
3)计算基本矩阵。
可以根据RANSAC(Random Sample Consensus，随机抽样一致算法)选取特征点(包括线段端点)，并计算基本矩阵，基本矩阵为对极几何中概念，在此不再赘述。
4)估计机器人在拍摄关键帧图像和辅助帧图像时的相对位姿变化。
可以通过对基本矩阵分解对机器人相对运动进行初始估计，属于现有技术，在此不再赘述。
5)根据机器人相机的位姿和相机内部参数利用三角测量算法估计特征点的三维坐标。
上述构建地图的方法可以应用于初始化阶段时选取第一帧图像为关键帧，第二帧为利用上述对极几何约束原理和RANSAC算法等来来估计机器人在两帧之间的相对位姿。然后根据三角测量估计特征对应的真实场景点中的深度。从第三帧图像到第二个关键帧图像之间采用前述实施例的方法对当前针对应的位姿和半固定地图进行优化。在下一个关键帧到来时，清除地图重复上述流程。这样将初始化过程分摊到各个局部地图的建立中，提高算法的灵活性。
在一些实施例中，在关键帧来临时，可以进行重定位。由关键帧选取准则可知，当关键帧来临时，用于求解相对位姿的特征点数目已经较少，所以估计的相机的相对运动误差就会变大。重定位的作用就是终止局部里程计的累积漂移，提高定位的精度。具体的，将机器人拍摄前一帧图像时的位姿确定为拍摄当前帧图像时的位置，重新根据前述实施例优化拍摄当前帧图像的位姿，并将下一帧作为关键帧。重定位就是在当前帧跟丢以后重新回到跟丢的位置进行找回。
上述实施例中构建地图的方法，使得运动估计部分的算法效率也提高了。因为地图中关键帧的数量减少，对机器人拍摄当前帧时位姿进行优化只需要参考一个关键帧图像或者关键帧图像和辅助帧图像，提高了优化效率。
上述运动估计和构建地图的部分相互配合形成一个完整的方案，估计机器人在各个时刻的位置和姿态，可以用于机器人自动导航。针对每一帧图像都可以根据上述实施例中首先估计拍摄该帧图像时机器人的位姿，进一步如果该帧图像为关键帧，则更新地图用于后续帧的定位，整体是一个循环过程。步骤S302可以作为步骤S206之后的步骤，进一步，执行步骤S304之后，将下一帧图像作为当前帧图像，返回步骤S202重新执行。
本公开主要对SVO算法改进的部分进行了详细描述，其他部分可以具体参考SVO算法，在此不再赘述。
本公开还提供一种机器人位姿的确定装置，下面结合图4进行描述。
图4为本公开机器人位姿的确定装置的一些实施例的结构图。如图4所示，该实施例的装置40包括：位姿估计模块402，误差确定模块404，位姿确定模块406。
位姿估计模块402，用于通过在机器人拍摄的当前帧图像和前一帧图像中匹配相同的特征点和特征线段，并参考机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，位姿估计模块402用于通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在前一帧图像中的投影信息，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化；根据机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化，以及机器人拍摄前一帧图像时的位置和姿态，估计机器人拍摄当前帧图像时的位置和姿态。
进一步，在一些实施例中，位姿估计模块402用于确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第一亮度信息；确定地图中的特征点和特征线段上的点在前一帧图像中的投影的亮度信息作为第二亮度信息；使第一亮度信息和第二亮度信息误差最小的情况下，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化。
进一步，在一些实施例中，位姿估计模块402用于确定地图中的特征点和特征线段上的点在拍摄对应的关键帧图像时机器人坐标系下的三维坐标；根据机器人拍摄当前帧图像相对于拍摄关键帧图像的位移和姿态变化，将特征点和特征线段上的点的三维坐标由拍摄关键帧图像时机器人坐标系下，转换至拍摄当前帧图像时机器人坐标系下；根据相机的内置参数，以及特征点和特征线段上的点在拍摄当前帧图像时机器人坐标系下的三维坐标，将特征点和特征线段的点投影到当前帧图像上，得到二维坐标作为在当前帧图像中的投影信息。
误差确定模块404，用于根据地图中的特征点和特征线段在当前帧图像中的匹配信息，以及估计的机器人拍摄当前帧图像时的位置和姿态，确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
在一些实施例中，误差确定模块404用于根据估计的机器人拍摄当前帧图像时的位置和姿态，确定地图中的特征点和特征线段在当前帧图像中的估计位置；根据地图中的特征点和特征线段在当前帧图像中的匹配信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置；根据估计位置与预测位置确定对机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
进一步，在一些实施例中，误差确定模块404用于通过比对地图中的特征点和特征线段在当前帧图像中的投影信息和在关键帧图像中的投影信息，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
进一步，在一些实施例中，误差确定模块404用于确定地图中的特征点和特征线段上的点在当前帧图像中的投影的亮度信息作为第三亮度信息；确定地图中的特征点和特征线段上的点在关键帧图像中的投影的亮度信息作为第四亮度信息；使第三亮度信息和第四亮度信息误差最小的情况下，确定地图中的特征点和特征线段在当前帧图像中的预测位置。
在一些实施例中，地图中的特征点和特征线段包括从关键帧图像中提取的特征点和特征线段，特征点用该特征点及其周围预设范围内的像素信息和该特征点的坐标信息描述，特征线段用该特征线段上的点及其周围预设范围内的像素信息和该特征线段的端点的坐标信息描述。
在一些实施例中，特征线段上的点包括：特征线段的端点；或者，特征线段上的点包括：特征线段的端点和预设数量的均匀分布在特征线段上的点。
位姿确定模块406，用于根据误差对估计的机器人拍摄当前帧图像时的位置和姿态进行修正，从而确定机器人拍摄当前帧图像时的位置和姿态。
在一些实施例中，位姿确定模块406用于通过修正估计的机器人拍摄当前帧图像时的位置和姿态调整估计位置与预测位置的误差；在估计位置与预测位置的误差最小的情况下，确定机器人拍摄当前帧图像时的位置和姿态。
下面结合图5描述本公开的机器人位姿的确定装置的另一些实施例。
图5为本公开机器人位姿的确定装置的另一些实施例的结构图。如图5所示，该实施例的装置50包括：位姿估计模块502，误差确定模块504，位姿确定模块506分别与位姿估计模块402，误差确定模块404，位姿确定模块406实现类似功能。装置50还包括：关键帧判断模块508，地图生成模块510。
关键帧判断模块508，用于判断当前帧图像是否为关键帧图像。
在一些实施例中，关键帧判断模块508用于在地图中的特征点和特征线段在当前帧图像中的投影数目小于第一阈值的情况下，确定当前帧图像为关键帧图像；或者，在关键帧图像在当前帧图像中的匹配到的特征点和特征线段的数目小于第二阈值的情况下，确定当前帧图像为关键帧图像。
地图生成模块510，用于在当前帧图像为关键帧图像的情况下，清除地图中的特征点和特征线段，重新提取当前帧图像中的特征点和特征线段重新生成地图，以便利用重新生成的地图确定机器人拍摄下一帧图像时的位置和姿态。
在一些实施例中，地图生成模块510用于根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，利用三角测量算法确定特征点和特征线段端点的三维坐标。
进一步，在一些实施例中，地图生成模块510用于根据关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，确定机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化；根据机器人在拍摄关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化，以及特征点和特征线段在关键帧和对应的辅助帧中的位置，利用三角测量算法确定特征点和特征线段端点的三维坐标。
进一步，在一些实施例中，地图生成模块510用于计算关键帧中每个特征点和特征线段在辅助帧中重投影距离，选取重投影距离大于阈值的特征点和特征线段作为地图中的特征点和特征线段。
在一些实施例中，辅助帧图像为与关键帧图像中的特征点和特征线段的匹配度在预设范围内的一帧图像。
本公开还提供一种机器人，下面结合图6进行描述。图6为本公开机器人的一些实施例的结构图。如图6所示，该实施例的机器人6包括：前述任意实施例中的机器人位姿的确定装置40或50。
本公开的实施例中的机器人位姿的确定装置可各由各种计算设备或计算机系统来实现，下面结合图7以及图8进行描述。
图7为本公开机器人位姿的确定装置的一些实施例的结构图。如图7所示，该实施例的装置70包括：存储器710以及耦接至该存储器110的处理器720，处理器720被配置为基于存储在存储器710中的指令，执行本公开中任意一些实施例中的机器人位姿的确定装置方法。
其中，存储器710例如可以包括系统存储器、固定非易失性存储介质等。系统存储器例如存储有操作系统、应用程序、引导装载程序(Boot Loader)、数据库以及其他程序等。
图8为本公开机器人位姿的确定装置的另一些实施例的结构图。如图8所示，该实施例的装置80包括：存储器810以及处理器820，分别与存储器710以及处理器720类似。还可以包括输入输出接口830、网络接口840、存储接口850等。这些接口830，840，850以及存储器810和处理器820之间例如可以通过总线860连接。其中，输入输出接口830为显示器、鼠标、键盘、触摸屏等输入输出设备提供连接接口。网络接口840为各种联网设备提供连接接口，例如可以连接到数据库服务器或者云端存储服务器等。存储接口850为SD卡、U盘等外置存储设备提供连接接口。
本领域内的技术人员应当明白，本公开的实施例可提供为方法、系统、或计算机程序产品。因此，本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解为可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅为本公开的较佳实施例，并不用以限制本公开，凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。
Claims (24)
1.一种机器人位姿的确定方法，包括：
通过在机器人拍摄的当前帧图像和前一帧图像中匹配相同的特征点和特征线段，并参考机器人拍摄前一帧图像时的位置和姿态，估计所述机器人拍摄当前帧图像时的位置和姿态，其中，所述估计所述机器人拍摄当前帧图像时的位置和姿态包括：通过比对地图中的特征点和特征线段在所述当前帧图像中的投影信息和在前一帧图像中的投影信息，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化；根据机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化，以及机器人拍摄前一帧图像时的位置和姿态，估计所述机器人拍摄当前帧图像时的位置和姿态，其中所述地图包括关键帧图像中的特征点和特征线段的三维坐标；
根据所述地图中的特征点和特征线段与当前帧图像中的特征点和特征线段的匹配信息，以及估计的所述机器人拍摄当前帧图像时的位置和姿态，确定对所述机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差；
根据所述误差对估计的所述机器人拍摄当前帧图像时的位置和姿态进行修正，从而确定所述机器人拍摄当前帧图像时的位置和姿态。
2.根据权利要求1所述的确定方法，其中，所述确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化包括：
确定所述地图中的特征点和特征线段上的点在所述当前帧图像中的投影的亮度信息作为第一亮度信息；
确定所述地图中的特征点和特征线段上的点在所述前一帧图像中的投影的亮度信息作为第二亮度信息；
使所述第一亮度信息和第二亮度信息误差最小的情况下，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化。
3.根据权利要求1所述的确定方法，确定所述地图中的特征点和特征线段在所述当前帧图像中的投影信息包括：
确定所述地图中的特征点和特征线段上的点在拍摄对应的关键帧图像时机器人坐标系下的三维坐标；
根据机器人拍摄当前帧图像相对于拍摄所述关键帧图像的位移和姿态变化，将所述特征点和特征线段上的点的三维坐标由拍摄所述关键帧图像时机器人坐标系下，转换至拍摄所述当前帧图像时机器人坐标系下；
根据相机的内置参数，以及所述特征点和特征线段上的点在拍摄所述当前帧图像时机器人坐标系下的三维坐标，将所述特征点和特征线段的点投影到所述当前帧图像上，得到二维坐标作为在所述当前帧图像中的投影信息。
4.根据权利要求1所述的确定方法，其中，所述确定对所述机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差包括：
根据估计的所述机器人拍摄当前帧图像时的位置和姿态，确定所述地图中的特征点和特征线段在当前帧图像中的估计位置；
根据所述地图中的特征点和特征线段在当前帧图像中的匹配信息，确定所述地图中的特征点和特征线段在当前帧图像中的预测位置；
根据所述估计位置与所述预测位置确定对所述机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
5.根据权利要求4所述的确定方法，其中，所述确定所述地图中的特征点和特征线段在当前帧图像中的预测位置包括:
通过比对所述地图中的特征点和特征线段在所述当前帧图像中的投影信息和在关键帧图像中的投影信息，确定所述地图中的特征点和特征线段在当前帧图像中的预测位置。
6.根据权利要求5所述的确定方法，其中，所述确定所述地图中的特征点和特征线段在当前帧图像中的预测位置包括：
确定所述地图中的特征点和特征线段上的点在所述当前帧图像中的投影的亮度信息作为第三亮度信息；
确定所述地图中的特征点和特征线段上的点在关键帧图像中的投影的亮度信息作为第四亮度信息；
使所述第三亮度信息和第四亮度信息误差最小的情况下，确定所述地图中的特征点和特征线段在当前帧图像中的预测位置。
7.根据权利要求4所述的确定方法，其中，所述确定所述机器人拍摄当前帧图像时的位置和姿态包括：
通过修正所述估计的所述机器人拍摄当前帧图像时的位置和姿态调整所述估计位置与所述预测位置的误差；
在所述估计位置与所述预测位置的误差最小的情况下，确定所述机器人拍摄当前帧图像时的位置和姿态。
8.根据权利要求1所述的确定方法，其中，
所述地图中的特征点和特征线段包括从关键帧图像中提取的特征点和特征线段，所述特征点用该特征点及其周围预设范围内的像素信息和该特征点的坐标信息描述，所述特征线段用该特征线段上的点及其周围预设范围内的像素信息和该特征线段的端点的坐标信息描述。
9.根据权利要求2、3、6、8任一项所述的确定方法，其中，
所述特征线段上的点包括：所述特征线段的端点；
或者，所述特征线段上的点包括：所述特征线段的端点和预设数量的均匀分布在所述特征线段上的点。
10.根据权利要求1所述的确定方法，还包括：
判断所述当前帧图像是否为关键帧图像；
在所述当前帧图像为关键帧图像的情况下，清除地图中的特征点和特征线段，重新提取所述当前帧图像中的特征点和特征线段重新生成地图，以便利用重新生成的地图确定机器人拍摄下一帧图像时的位置和姿态。
11.根据权利要求10所述的确定方法，其中，所述判断所述当前帧图像是否为关键帧图像包括：
在所述地图中的特征点和特征线段在当前帧图像中的投影数目小于第一阈值的情况下，确定所述当前帧图像为关键帧图像；
或者，在关键帧图像在当前帧图像中的匹配到的特征点和特征线段的数目小于第二阈值的情况下，确定所述当前帧图像为关键帧图像。
12.根据权利要求10所述的确定方法，其中，所述生成所述地图包括：
根据所述关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，利用三角测量算法确定特征点和特征线段端点的三维坐标。
13.根据权利要求12所述的确定方法，其中，所述生成所述地图包括：
根据所述关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，确定所述机器人在拍摄所述关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化；
根据所述机器人在拍摄所述关键帧图像和对应的辅助帧图像时的相对位置和姿态的变化，以及特征点和特征线段在所述关键帧和对应的辅助帧中的位置，利用三角测量算法确定特征点和特征线段端点的三维坐标。
14.根据权利要求10所述的确定方法，其中，确定所述地图中的特征点和特征线段包括：
计算关键帧中每个特征点和特征线段在辅助帧中重投影距离，选取重投影距离大于阈值的特征点和特征线段作为所述地图中的特征点和特征线段。
15.根据权利要求12-14任一项所述的确定方法，其中，
所述辅助帧图像为与所述关键帧图像中的特征点和特征线段的匹配度在预设范围内的一帧图像。
16.一种机器人位姿的确定装置，包括：
位姿估计模块，用于通过在机器人拍摄的当前帧图像和前一帧图像中匹配相同的特征点和特征线段，并参考机器人拍摄前一帧图像时的位置和姿态，估计所述机器人拍摄当前帧图像时的位置和姿态，其中，所述估计所述机器人拍摄当前帧图像时的位置和姿态包括：通过比对地图中的特征点和特征线段在所述当前帧图像中的投影信息和在前一帧图像中的投影信息，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化；根据机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化，以及机器人拍摄前一帧图像时的位置和姿态，估计所述机器人拍摄当前帧图像时的位置和姿态，其中所述地图包括关键帧图像中的特征点和特征线段的三维坐标；
误差确定模块，用于根据所述地图中的特征点和特征线段与当前帧图像中的特征点和特征线段的匹配信息，以及估计的所述机器人拍摄当前帧图像时的位置和姿态，确定对所述机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差；
位姿确定模块，用于根据所述误差对估计的所述机器人拍摄当前帧图像时的位置和姿态进行修正，从而确定所述机器人拍摄当前帧图像时的位置和姿态。
17.根据权利要求16所述的确定装置，其中，
所述位姿估计模块用于确定所述地图中的特征点和特征线段上的点在所述当前帧图像中的投影的亮度信息作为第一亮度信息；确定所述地图中的特征点和特征线段上的点在所述前一帧图像中的投影的亮度信息作为第二亮度信息；使所述第一亮度信息和第二亮度信息误差最小的情况下，确定机器人拍摄当前帧图像时相对于拍摄前一帧图像时的相对位移和姿态变化；
或者，所述位姿估计模块用于确定所述地图中的特征点和特征线段上的点在拍摄对应的关键帧图像时机器人坐标系下的三维坐标；根据机器人拍摄当前帧图像相对于拍摄所述关键帧图像的位移和姿态变化，将所述特征点和特征线段上的点的三维坐标由拍摄所述关键帧图像时机器人坐标系下，转换至拍摄所述当前帧图像时机器人坐标系下；根据相机的内置参数，以及所述特征点和特征线段上的点在拍摄所述当前帧图像时机器人坐标系下的三维坐标，将所述特征点和特征线段的点投影到所述当前帧图像上，得到二维坐标作为在所述当前帧图像中的投影信息。
18.根据权利要求16所述的确定装置，其中，
所述误差确定模块用于根据估计的所述机器人拍摄当前帧图像时的位置和姿态，确定所述地图中的特征点和特征线段在当前帧图像中的估计位置；根据所述地图中的特征点和特征线段在当前帧图像中的匹配信息，确定所述地图中的特征点和特征线段在当前帧图像中的预测位置；根据所述估计位置与所述预测位置确定对所述机器人拍摄当前帧图像时的位置和姿态进行估计而产生的误差。
19.根据权利要求18所述的确定装置，其中，
所述误差确定模块用于通过比对所述地图中的特征点和特征线段在所述当前帧图像中的投影信息和在关键帧图像中的投影信息，确定所述地图中的特征点和特征线段在当前帧图像中的预测位置。
20.根据权利要求16所述的确定装置，还包括：
关键帧判断模块，用于判断所述当前帧图像是否为关键帧图像；
地图生成模块，用于在所述当前帧图像为关键帧图像的情况下，清除地图中的特征点和特征线段，重新提取所述当前帧图像中的特征点和特征线段重新生成地图，以便利用重新生成的地图确定机器人拍摄下一帧图像时的位置和姿态。
21.根据权利要求20所述的确定装置，其中，
所述关键帧判断模块用于在所述地图中的特征点和特征线段在当前帧图像中的投影数目小于第一阈值的情况下，确定所述当前帧图像为关键帧图像；或者，在关键帧图像在当前帧图像中的匹配到的特征点和特征线段的数目小于第二阈值的情况下，确定所述当前帧图像为关键帧图像；
或者，所述地图生成模块用于根据所述关键帧图像中的特征点和特征线段在对应的辅助帧图像中的匹配信息，利用三角测量算法确定特征点和特征线段端点的三维坐标；
或者，所述地图生成模块用于计算关键帧中每个特征点和特征线段在辅助帧中重投影距离，选取重投影距离大于阈值的特征点和特征线段作为所述地图中的特征点和特征线段。
22.一种机器人位姿的确定装置，包括：
存储器；以及
耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器设备中的指令，执行如权利要求1-15任一项所述的机器人位姿的确定方法。
23.一种计算机可读存储介质，其上存储有计算机程序，其中，该程序被处理器执行时实现权利要求1-15任一项所述方法的步骤。
24.一种机器人，包括：权利要求16-22任一项所述的机器人位姿的确定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810649163.0A CN110631554B (zh) | 2018-06-22 | 2018-06-22 | 机器人位姿的确定方法、装置、机器人和可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810649163.0A CN110631554B (zh) | 2018-06-22 | 2018-06-22 | 机器人位姿的确定方法、装置、机器人和可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110631554A CN110631554A (zh) | 2019-12-31 |
| CN110631554B true CN110631554B (zh) | 2021-11-30 |
Family
ID=68966442
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810649163.0A Active CN110631554B (zh) | 2018-06-22 | 2018-06-22 | 机器人位姿的确定方法、装置、机器人和可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110631554B (zh) |
Families Citing this family (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111409104B (zh) * | 2020-03-27 | 2022-11-04 | 伯朗特机器人股份有限公司 | 工业机器人位姿特性中位姿特性漂移的测量方法 |
| CN111489439B (zh) * | 2020-04-15 | 2024-06-07 | 京东方科技集团股份有限公司 | 一种三维线图重建方法、装置和电子设备 |
| CN113554703B (zh) * | 2020-04-23 | 2024-03-01 | 北京京东乾石科技有限公司 | 机器人定位方法、装置、系统和计算机可读存储介质 |
| CN113761965B (zh) * | 2020-06-01 | 2024-03-12 | 北京达佳互联信息技术有限公司 | 动作捕捉方法、装置、电子设备和存储介质 |
| CN111649724B (zh) * | 2020-06-04 | 2022-09-06 | 百度在线网络技术（北京）有限公司 | 基于移动边缘计算的视觉定位方法和装置 |
| CN111928861B (zh) * | 2020-08-07 | 2022-08-09 | 杭州海康威视数字技术股份有限公司 | 地图构建方法及装置 |
| CN114119885A (zh) * | 2020-08-11 | 2022-03-01 | 中国电信股份有限公司 | 图像特征点匹配方法、装置及系统、地图构建方法及系统 |
| CN112001970A (zh) * | 2020-08-25 | 2020-11-27 | 哈尔滨工业大学 | 一种基于点线特征的单目视觉里程计方法 |
| CN112433211B (zh) * | 2020-11-27 | 2022-11-29 | 浙江商汤科技开发有限公司 | 一种位姿确定方法及装置、电子设备和存储介质 |
| CN112686951A (zh) * | 2020-12-07 | 2021-04-20 | 深圳乐动机器人有限公司 | 用于确定机器人位置的方法、装置、终端及存储介质 |
| CN112734839B (zh) * | 2020-12-31 | 2022-07-08 | 浙江大学 | 一种提高鲁棒性的单目视觉slam初始化方法 |
| CN113034594A (zh) * | 2021-03-16 | 2021-06-25 | 浙江商汤科技开发有限公司 | 位姿优化方法、装置、电子设备及存储介质 |
| CN113034582A (zh) * | 2021-03-25 | 2021-06-25 | 浙江商汤科技开发有限公司 | 位姿优化装置及方法、电子设备及计算机可读存储介质 |
| CN113420590B (zh) * | 2021-05-13 | 2022-12-06 | 北京航空航天大学 | 弱纹理环境下的机器人定位方法、装置、设备及介质 |
| CN113379911A (zh) * | 2021-06-30 | 2021-09-10 | 深圳市银星智能科技股份有限公司 | Slam方法、slam系统及智能机器人 |
| CN114049376A (zh) * | 2021-09-17 | 2022-02-15 | 北京航空航天大学 | 位姿实时跟踪方法、设备、计算机可读介质和程序产品 |
| CN114415698B (zh) * | 2022-03-31 | 2022-11-29 | 深圳市普渡科技有限公司 | 机器人、机器人的定位方法、装置和计算机设备 |
| CN115235500B (zh) * | 2022-09-15 | 2023-04-14 | 北京智行者科技股份有限公司 | 基于车道线约束的位姿校正方法及装置、全工况静态环境建模方法及装置 |
| CN115619860A (zh) * | 2022-09-15 | 2023-01-17 | 珠海一微半导体股份有限公司 | 基于图像信息的激光定位方法及机器人 |
| CN116758157B (zh) * | 2023-06-14 | 2024-01-30 | 深圳市华赛睿飞智能科技有限公司 | 一种无人机室内三维空间测绘方法、系统及存储介质 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105928505B (zh) * | 2016-04-19 | 2019-01-29 | 深圳市神州云海智能科技有限公司 | 移动机器人的位姿确定方法和设备 |
| CN108090958B (zh) * | 2017-12-06 | 2021-08-27 | 上海阅面网络科技有限公司 | 一种机器人同步定位和地图构建方法和系统 |
-
2018
- 2018-06-22 CN CN201810649163.0A patent/CN110631554B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN110631554A (zh) | 2019-12-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110631554B (zh) | 机器人位姿的确定方法、装置、机器人和可读存储介质 | |
| US10953545B2 (en) | System and method for autonomous navigation using visual sparse map | |
| CN107025668B (zh) | 一种基于深度相机的视觉里程计的设计方法 | |
| CN110945565B (zh) | 利用概率面元地图的密集视觉slam | |
| CN114862949B (zh) | 一种基于点线面特征的结构化场景视觉slam方法 | |
| CN111210463B (zh) | 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统 | |
| CN110176032B (zh) | 一种三维重建方法及装置 | |
| CN108615246B (zh) | 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法 | |
| CN111445526B (zh) | 一种图像帧之间位姿的估计方法、估计装置和存储介质 | |
| CN109816696A (zh) | 一种机器人定位与建图方法、计算机装置及计算机可读存储介质 | |
| CN111462207A (zh) | 一种融合直接法与特征法的rgb-d同时定位与地图创建方法 | |
| CN111709973B (zh) | 目标跟踪方法、装置、设备及存储介质 | |
| EP2854104A1 (en) | Semi-dense simultaneous localization and mapping | |
| CN110555901A (zh) | 动静态场景的定位和建图方法、装置、设备和存储介质 | |
| WO2019157922A1 (zh) | 一种图像处理方法、装置及ar设备 | |
| CN110827321B (zh) | 一种基于三维信息的多相机协同的主动目标跟踪方法 | |
| CN110490222B (zh) | 一种基于低性能处理器设备的半直接视觉定位方法 | |
| CN112734837B (zh) | 图像匹配的方法及装置、电子设备及车辆 | |
| WO2018214086A1 (zh) | 场景的三维重建方法、装置及终端设备 | |
| KR20160123871A (ko) | 영상 광류를 추정하는 방법 및 이를 위한 장치 | |
| CN110738730A (zh) | 点云匹配方法、装置、计算机设备和存储介质 | |
| CN112967340A (zh) | 同时定位和地图构建方法、装置、电子设备及存储介质 | |
| CN113532420A (zh) | 一种融合点线特征的视觉惯性里程计方法 | |
| WO2024051591A1 (zh) | 用于估算视频旋转的方法、装置、电子设备和存储介质 | |
| CN117011660A (zh) | 一种低纹理场景下融合深度信息的点线特征slam方法 |
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 | ||
| TA01 | Transfer of patent application right | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20210303 Address after: 101, 1st floor, building 2, yard 20, Suzhou street, Haidian District, Beijing 100080 Applicant after: Beijing Jingbangda Trading Co.,Ltd. Address before: 100195 Beijing Haidian Xingshikou Road 65 West Cedar Creative Garden 4 District 11 Building East 1-4 Floor West 1-4 Floor Applicant before: BEIJING JINGDONG SHANGKE INFORMATION TECHNOLOGY Co.,Ltd. Applicant before: BEIJING JINGDONG CENTURY TRADING Co.,Ltd. Effective date of registration: 20210303 Address after: Room a1905, 19 / F, building 2, No. 18, Kechuang 11th Street, Daxing District, Beijing, 100176 Applicant after: Beijing Jingdong Qianshi Technology Co.,Ltd. Address before: 101, 1st floor, building 2, yard 20, Suzhou street, Haidian District, Beijing 100080 Applicant before: Beijing Jingbangda Trading Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |