CN106813672A - 移动机器人的导航方法及移动机器人 - Google Patents

移动机器人的导航方法及移动机器人 Download PDF

Info

Publication number
CN106813672A
CN106813672A CN201710045324.0A CN201710045324A CN106813672A CN 106813672 A CN106813672 A CN 106813672A CN 201710045324 A CN201710045324 A CN 201710045324A CN 106813672 A CN106813672 A CN 106813672A
Authority
CN
China
Prior art keywords
camera
mobile robot
pose
transition matrix
video
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.)
Granted
Application number
CN201710045324.0A
Other languages
English (en)
Other versions
CN106813672B (zh
Inventor
刘智成
宋章军
刘璐
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen flying mouse Power Technology Co., Ltd
Original Assignee
Shenzhen Xiluo Robot Co Ltd
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 Shenzhen Xiluo Robot Co Ltd filed Critical Shenzhen Xiluo Robot Co Ltd
Priority to CN201710045324.0A priority Critical patent/CN106813672B/zh
Publication of CN106813672A publication Critical patent/CN106813672A/zh
Application granted granted Critical
Publication of CN106813672B publication Critical patent/CN106813672B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance

Abstract

本发明公开了一种移动机器人的导航方法，包括：使第一摄像头的朝向与第二摄像头的朝向不同；分别从第一摄像头及第二摄像头获取视频帧；检测视频帧的特征点；跟踪特征点，并排除离群点；求取第一摄像头的位姿转换矩阵及第二摄像头的位姿转换矩阵，计算跟踪成功的特征点的三维坐标，形成新三维点集；根据新的三维点集优化第一摄像头的位姿转换矩阵及第二摄像头的位姿转换矩阵，得到移动机器人的位姿；以及根据所述移动机器人的位姿更新定位信息和地图。该移动机器人的导航方法定位精度高，场景识别能力强，系统鲁棒性高。本发明还公开了一种移动机器人。

Description

移动机器人的导航方法及移动机器人
技术领域
本发明涉及机器人领域，尤其涉及一种移动机器人的导航方法和应用该导航方法的移动机器人。
背景技术
移动机器人是自主控制移动、自动执行工作的智能装置，它可以在地面上或其他表面上移动，它既可以接受用户的指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。随着科学技术的进步，移动机器人在诸如生产、军事、服务等领域均有应用，尤其在家居服务领域应用越来越广，例如吸尘、扫地、拖地、擦玻璃等清洁机器人以及用于割草的割草机器人等。目前，在国内外的家用及公共场所均有所应用。
为了使移动机器人更好的执行任务，通常希望移动机器人有定位和导航功能，这样，许多厂商在移动机器人中添加了摄像头和SLAM(同步定位及建图，SimultaneousLocalization And Mapping)单元，以使移动机器人能够自动定位并创建地图，从而达到导航的目的。然而，现有技术中，设置了单摄像头的移动机器人在复杂多变的室内环境下，仍会检测不到足够多的特征点，例如，当摄像头朝向白墙或白天花板时，由于大面积为无变化特点的白色，很难检测到特征点，从而难以定位；或者容易跟丢特征点，例如，移动机器人旋转或行走时朝向窗口，窗外射进来明亮的阳光，此时摄像头逆光拍摄，光照变化大，导致特征点不易匹配继而跟踪失败，系统鲁棒性低。
发明内容
本发明所要解决的技术问题在于，提供一种移动机器人的导航方法，该导航方法定位精度高，场景识别能力强，从而提高系统的鲁棒性。
为了解决上述技术问题，本发明采用以下技术方案：
一方面，本发明的实施例提供一种移动机器人的导航方法，应用于所述移动机器人，所述移动机器人包括机器人主体、第一摄像头及第二摄像头，所述第一摄像头及所述第二摄像头设置于所述机器人主体上，所述导航方法包括步骤：
使所述第一摄像头的朝向与所述第二摄像头的朝向不同；
分别从所述第一摄像头及所述第二摄像头获取视频帧；
检测所述视频帧的特征点；
跟踪特征点，并排除离群点；
求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2；
根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿；以及
根据所述移动机器人的位姿更新定位信息和地图。
优选地，所述检测所述视频帧的特征点具体可以是：检测所述视频帧的特征点，将所述特征点转化为词袋特征向量并保存于场景数据库DB；
所述跟踪特征点，并排除离群点具体可以是：根据所述场景数据库DB跟踪特征点，并排除离群点。
另一方面，本发明的实施例提供一种移动机器人，所述移动机器人包括机器人主体、第一摄像头及第二摄像头，所述第一摄像头及所述第二摄像头设置于所述机器人主体上，并且所述第一摄像头的朝向与所述第二摄像头的朝向不同，所述移动机器人还包括同步定位及建图模块和控制模块，所述控制模块用于控制所述第一摄像头、所述第二摄像头以及所述同步定位及建图模块，所述同步定位及建图模块包括：
视频帧采集单元，用于从所述第一摄像头及所述第二摄像头获取视频帧；
特征点检测单元，用于检测所述视频帧的特征点，并将所述特征点转化为词袋特征向量，并保存于场景数据库DB；
特征点跟踪单元，配置为根据所述场景数据库DB跟踪所述特征点，并排除离群点；
计算单元，用于求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，以及计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2；
优化单元，配置为根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿；以及
更新单元，用于根据所述移动机器人的位姿更新定位信息和地图。
与现有技术相比，本发明的技术方案至少具有以下有益效果：
本发明的实施例中，由于所述导航方法包括：使所述第一摄像头的朝向与所述第二摄像头的朝向不同，且分别从所述第一摄像头及所述第二摄像头获取视频帧，因此，所述移动机器人移动时所采集到的图像变化较大，更容易抓取到更多的特征点，并且大面积白墙同时占据两个摄像头所拍摄的图像的几率很小，有利于提升定位精度；且由于所述第一摄像头和所述第二摄像头朝向不同，所以同时面对光照强的位置或同时面对窗户的概率大幅度下降，提高在对场景的识别能力。
此外，所述导航方法还包括：求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2；根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿。由此，通过简单的方法将两个摄像头的特征点快速提取，又高效整合，定位精度高、定位速度快，场景识别能力强，极大地提升了系统的鲁棒性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的变形形式。
图1是本发明的实施例中导航方法的流程示意图；
图2是本发明的实施例中导航方法中优化移动机器人位姿的流程示意图；
图3是本发明的一个实施例中导航方法的流程示意图；
图4是本发明的实施例中导航方法的位姿初始化流程示意图；
图5是本发明的实施例中导航方法在所述移动机器人被人为放置于一未知位置时进行全局重定位的流程示意图；以及
图6是本发明实施例中的移动机器人的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
除非另作定义，此处使用的技术术语或科学术语应对作为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“第一”、“第二”、“第三”和“第四”等是用于区别不同对象，而不是用于描述特定顺序、数量或者重要性。同样，“一个”、“一”或“该”等类似词语也不表示数量限制，而只是用来表示存在至少一个。“包括”或者“包含”等类似的词语意指出现该词语前面的元件或物件涵盖出现在该词后面列举的元件或者物件及其等同，而不排除其他元件或物件。“连接”或者相连等类似的词语并非限定于物理的或者机械的连接，而是可以包含电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也可能相应地改变。
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本发明的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
请参阅图1，图1是本发明的实施例中导航方法的流程示意图。本发明的实施例中，所述导航方法用于移动机器人(图1未示出)，所述移动机器人可以是用于吸尘、扫地、拖地或擦玻璃的清洁机器人、用于割草的割草机器人以及其他可以自主移动的机器人。所述移动机器人至少包括机器人主体、第一摄像头及第二摄像头。可以理解的是，在别的实施例中，所述移动机器人可以包括更多个摄像头。所述第一摄像头及所述第二摄像头设置于所述机器人主体上。可以理解的是，所述移动机器人包括控制单元，所述控制单元控制所述第一摄像头及所述第二摄像头，还可以用于控制所述导航方法的运行。如图1所示意，所述移动机器人的导航方法至少包括步骤S101、S102、S103、S104、S105、S106及S107，下面进行具体描述。
步骤S101：使所述第一摄像头的朝向与所述第二摄像头的朝向不同。
本发明的实施例中，所述第一摄像头及所述第二摄像头可以是普通摄像头，也可以是宽视角的鱼眼摄像头。所述第一摄像头和所述第二摄像头用于拍摄所述移动机器人的作业空间的图片，形成视频帧。所述第一摄像头的朝向是指所述第一摄像头的光轴的延伸方向，所述第二摄像头的朝向是指所述第二摄像头的光轴的延伸方向，换句话说，本实施例中所述第一摄像头的光轴与所述第二摄像头的光轴不平行。如此，有利于使所述第一摄像头与所述第二摄像头采集视频帧的方向不同，从而可以采集到更多不同的特征点，提高定位精度和场景识别能力。
本发明的实施例中，所述第一摄像头及所述第二摄像头可以是固定方向的；也可以是具有方向调节装置的，通过控制调节所述方向调节装置能够调节所述第一摄像头和所述第二摄像头的方向，从而使所述第一摄像头的朝向与所述第二摄像头的朝向不同，以及可以调节所述第一摄像头的朝向与所述第二摄像头的朝向之间的夹角。
步骤S102：分别从所述第一摄像头及所述第二摄像头获取视频帧。
所述第一摄像头及所述第二摄像头采集的图像，形成一帧帧视频帧。
步骤S103：检测所述视频帧的特征点。
在本实施例中，可以将所述特征点转化为词袋特征向量并保存于场景数据库DB。
可以采用特征点检测算法检测所述视频帧的特征点，例如采用FAST(即Featuresfrom Accelerated Segment Test)特征点检测算法或SIFT(即Scale Invariant FeatureTransform)特征点检测算法。并且，采用合适的描述子进行描述，例如，ORB(即OrientedFAST and Rotated BRIEF)，然后用DBoW2库将描述子转化为词袋特征向量，并将词袋特征向量保存于场景数据库DB中，以便于为下一帧视频帧或后续的其他视频帧跟踪特征点，以及进行场景识别。所述特征点是指图像(所述视频帧)灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点(即两个边缘的交点)以及其他可以凸显所述视频帧的特征的点或部位。
步骤S104：跟踪特征点，并排除离群点；
在一个实施例中，根据场景数据库DB跟踪特征点，并排除离群点。根据描述子及场景数据库DB的词袋特征向量，采用DBoW2库来加速当前视频帧的特征点的匹配，匹配成功的特征点即为被跟踪成功的特征点。此外，在特征点的匹配过程中，可以采用RANSAC(RANdomSAmple Consensus，随机抽样一致性)算法排除离群点。
步骤S105：求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2。
在一个实施例中，采用PnP算法求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，并可以采用三角化法求取步骤S104中匹配成功的特征点所对应的三维点的坐标，形成新的三维点集，即形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2，每更新一帧视频帧，就更新一次所述三维点集。
步骤S106：根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿。
在一个实施例中，在步骤S102之前，先校正所述第一摄像头和第二摄像头，得到所述第一摄像头的校正参数矩阵K1及所述第二摄像头的校正参数矩阵K2，校正摄像头在现有技术中有所描述，不在此处赘述。在步骤S106中，将校正参数矩阵K1、校正矩阵参数K2，以及步骤S105得到的第一摄像头的位姿转换矩阵T1、第二摄像头的位姿转换矩阵T2、对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2作为输入参数，调用g2o库对第一摄像头的位姿转换矩阵T1及第二摄像头的位姿转换矩阵T2进行优化，从而得到所述移动机器人的位姿矩阵T，即得到所述移动机器人的位姿。
具体地，请参阅图2，图2是本发明的实施例中导航方法中优化移动机器人位姿的流程示意图。由图中可见，调用g2o库对第一摄像头的位姿转换矩阵T1及第二摄像头的位姿转换矩阵T2进行优化从而得到所述移动机器人的位姿，具体包括步骤S201、S202、S203、S204、S205及S206。
步骤S201：计算所述第一摄像头坐标系到所述第二摄像头坐标系的转换矩阵T12。
在一个实施例中，将所述移动机器人的坐标系原点设定在所述移动机器人的中心(旋转中心)，水平向前为X轴正向，水平向左为Y轴正向、竖直向上为Z轴正向，并通过实验得到，所述移动机器人坐标系到所述第一摄像头坐标系的转换矩阵为[R1|T1]，所述移动机器人坐标系到所述第二摄像头坐标系的转换矩阵为[R2|T2]，其中，R1表示移动机器人坐标系到第一摄像头坐标系的旋转矩阵，R2表示所述移动机器人坐标系到所述第二摄像头坐标系的旋转矩阵，T1表示移动机器人坐标系到第一摄像头坐标系的平移向量，T2表示移动机器人坐标系到第二摄像头坐标系的平移向量。旋转矩阵和平移向量的用法为：假设移动机器人的坐标系中某一三维点坐标为P，则转换到所述第一摄像头坐标系后的坐标为R1(P+T1)。在本实施例中，将启动时刻的移动机器人的坐标系设定为世界坐标系，世界坐标系是固定的，而移动机器人坐标系和摄像头坐标系是运动的，所述移动机器人的位姿、第一摄像头及第二摄像头的位姿均相对于世界坐标系而言。
本步骤(即步骤S201)用[R1|T1]及[R2|T2]计算所述第一摄像头坐标系到所述第二摄像头坐标系的转换矩阵T12，求得T12＝[R2·(R1 -1)|R2·(T2-T1)]。
步骤S202：选择g2o库中的类“g2o::VertexSE3Expmap”作为待优化顶点的数据类型，新建该类型的两个类对象A和B，并将A的预估值设置为所述第一摄像头的位姿转换矩阵T1，B的预估值设置为所述第二摄像头的位姿转换矩阵T2。
g2o(即General Graph Optimization，通用图优化)是通过定义顶点和边，将待优化问题表示成图，从而进行求解的项目。本步骤中，选择g2o库中的类“g2o::VertexSE3Expmap”作为待优化顶点的数据类型，新建该类型的两个类对象A和B，并将A的预估值设置为所述第一摄像头的位姿转换矩阵T1，B的预估值设置为所述第二摄像头的位姿转换矩阵T2。向图中添加这两个顶点A和B。
步骤S203：选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第一约束边的数据类型，所述第一约束边为只连接一个顶点的一元边，所连接的顶点设置为所述类对象A，将所述第一约束边的测量值设置为所述三维点集New_PtSet1中的点在所述第一摄像头的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量。对应于New_PtSet1中的每个点，都如本步骤中所叙述的向图中添加一条约束边(均记为第一约束边)。
步骤S204：选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第二约束边的数据类型，所述第二约束边所连接的顶点设置为所述类对象B，将所述第二约束边的测量值设置为所述三维点集New_PtSet2中的点在所述第二摄像头的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量。对应于New_PtSet2中的每个点，都如本步骤中所叙述的向图中添加一条约束边(均记为第二约束边)。
步骤S205：新建类型为g2o::EdgeSE3的第三约束边，所述第三约束边为二元边，所连接的两个顶点分别设置为类对象A和类对象B，并将所述第三约束边的测量值设置为所述转换矩阵T12，向图中添加该约束边(即所述第三约束边)。
步骤S206：调用g2o::SparseOptimizer::initializeOptimization()对优化器进行初始化，然后调用g2o::SparseOptimizer::optimize()完成优化，得到所述第一摄像头的优化后的位姿转换矩阵T1_New和所述第二摄像头的优化后的位姿转换矩阵T2_New，设T2_New＝[R|T]，据此求得所述移动机器人的优化后的位姿为T12_New＝[(R1 -1)·R|T-T1]，从而得到新的定位信息。
步骤S107：根据所述移动机器人的位姿更新定位信息和地图。
所述位姿是指被描述物的位置和姿态，即所述移动机器人在室内二维平面上的二维坐标和朝向角。根据步骤S206所求得的所述移动机器人的位姿，更新当前定位信息，并结合所述移动机器人的历史移动轨迹，更新地图，从而实现实时同步定位和建图(即建立地图)。
进一步地，在本发明的一个实施例中，所述导航方法还包括以下步骤：
初始化定位信息；
启动全路径覆盖算法，发出移动所述移动机器人的指令；
接收所述指令并移动所述移动机器人；
获取更新的所述定位信息和地图；
判断所述移动机器人是否将待作业表面覆盖完毕，若未覆盖完毕，则继续发出移动所述移动机器人的指令，若覆盖完毕，则保存所述地图和所述场景数据库DB，供后续作业时调用。
所述全路径覆盖算法指设计合适的运动路径使得扫地机在尽量短的时间内将室内所有可以达到的区域覆盖完毕，例如，所述移动机器人是扫地机时，则是指将室内所有可以达到的区域清扫完毕。所述全路径覆盖算法根据初始定位信息、其它传感器信息(现有的移动机器人中通常包括碰撞传感器、悬崖传感器、超声波测距传感器等，本实施例所述的移动机器人也可以包括其中一个或几个传感器)和时刻更新的地图及定位信息(参见图1)，经过计算对扫地机发出前进、后退、旋转、加速、减速、转向等运动指令。
结合前文所述的步骤，下面对本实施例中导航方法做进一步描述。请参阅图3，图3是本发明的一个实施例中导航方法的流程示意图。本实施例中，所述导航方法包括步骤S401、S402、S403、S404、S405、S406、S407及S408。
步骤S401：启动。即启动所述移动机器人。
步骤S402：初始化所述移动机器人的定位信息，亦即对所述移动机器人的位姿进行初始化。
具体地，请参阅图4，图4是本发明的实施例中导航方法的位姿初始化流程示意图。初始化所述移动机器人的位姿包括步骤S4021、S4022、S4023及S4024。
步骤S4021：使所述第一摄像头在第一位置拍摄一帧图像I1-1，所述第二摄像头在所述第一位置拍摄一帧图像I2-1，保存所述图像I1-1和所述图像I2-1；
在该步骤中，所述移动机器人位于一位置，记为第一位置P1，所述第一摄像头及所述第二摄像头分别拍摄一帧图像(即视频帧)，得到图像I1-1和图像I2-1。
步骤S4022：使所述移动机器人移动距离L到达第二位置，所述第一摄像头在所述第二位置拍摄一帧图像I1-2，所述第二摄像头在所述第二位置拍摄一帧图像I2-2，保存所述距离L、所述图像I1-2和所述图像I2-2，所述距离L使得所述第一摄像头及所述第二摄像头的视差足够大。
具体地，所述移动机器人在所述第一位置P1采集到图像I1-1和图像I2-1后，直行一段距离L到达第二位置P2，换句话说，所述第一位置P1与所述第二位置P2的距离为L。所述第一摄像头及所述第二摄像头在所述第二位置P2再分别拍摄一帧图像(即视频帧)，得到图像I1-2和图像I2-2。所述第一位置P1与所述第二位置P2的优选能使图像I1-1、图像I2-1、图像I1-2和图像I2-2采集到足够多的特征点的位置。所述距离L使得所述第一摄像头及所述第二摄像头在两个位置的视差足够大。所述距离L的大小可以采用码盘、光流传感器或者IMU传感器来精确测量。
步骤S4023：根据所述图像I1-1、所述图像I2-1、所述距离L、所述图像I1-2及所述图像I2-2，求取所述第一摄像头的初始位姿转换矩阵T1-0、所述第二摄像头的初始位姿转换矩阵T2-0、对应于所述第一摄像头的初始三维点集Init_PtSet1及对应于所述第二摄像头的初始三维点集Init_PtSet2。
具体地，求取所述第一摄像头的初始位姿转换矩阵T1-0及所述第二摄像头的初始位姿转换矩阵T2-0可以采用PnP算法；求取对应于所述第一摄像头的初始三维点集Init_PtSet1及对应于所述第二摄像头的初始三维点集Init_PtSet2，可以采用三角化法。PnP算法及三角化法均为本领域的技术人员所知晓的技术，不在此处赘述。
步骤S4024：将所述第一摄像头的校正参数矩阵K1、所述第二摄像头的校正参数矩阵K2、所述第一摄像头的初始位姿转换矩阵T1-0、所述第二摄像头的初始位姿转换矩阵T2-0、对应于所述第一摄像头的初始三维点集Init_PtSet1及对应于所述第二摄像头的初始三维点集Init_PtSet2作为输入参数调用g2o库进行优化，得到所述移动机器人的优化的初始位姿。
具体地，步骤S4024请参照步骤S201、S202、S203、S204、S205、S206及S207，具体实施时，将步骤S201、S202、S203、S204、S205、S206及S207中的参数对应替换为步骤S4024中的相应参数即可，不再赘述。
步骤S403：全路径覆盖算法经计算发出移动指令。
在得到步骤S402的初始化信息后，全路径覆盖算法结合所述初始化信息及其它辅助传感器的信息发出移动指令。可以理解的是，当所述第一摄像头及所述第二摄像头采集更多帧图片，或者移动机器人运行一定距离或一段时间之后，全路径覆盖算法结合所述初始化信息、新的地图和定位信息、及其它辅助传感器的信息进行计算后发出移动指令。
步骤S404：移动所述移动机器人。
具体地，所述移动机器人包含移动模块，所述移动模块根据所述移动指令带动所述移动机器人前进、后退、转向或旋转等，使所述移动机器人在作业空间的表面或地面移动。
步骤S405：获取更新后的定位信息和地图。
所述移动机器人根据所述移动指令向前、向后、转向或旋转，均会使所述移动机器人的位置和运动轨迹有所变化，亦即所述移动机器人的位姿和运动轨迹都有所更新。所述更新后的定位信息和地图是执行了步骤S107所更新的定位信息和地图，具体过程请参阅本文前面所述的步骤S101、S102、S103、S104、S105、S106及S107。
步骤S406：判断是否覆盖完毕。
具体地，所述移动机器人边移动边通过所获取的更新后的定位信息和地图来判断是否已经遍历(即覆盖)所述作业空间的地面或表面，例如。所述移动机器人为扫地机器人时，需要遍历(即覆盖)整个打扫房间，才能将房间清洁完毕。若作业空间的地面或表面已被覆盖完毕，则执行步骤S407，将更新的地图以及在覆盖过程中形成的场景数据库DB保存，以供以后使用；若作业空间的地面或表面未覆盖完毕，则执行步骤S403，如此循环直至覆盖完毕。
步骤S407：保存地图和场景数据库DB。
步骤S408：结束。
由于环境的多变性和用户需求，经常会将在执行任务的移动机器人搬起而放置到另外的地方，即一个未知位置，此时，移动机器人可以根据已保存的地图及场景数据库DB进行全局重定位，若能识别所述未知位置的场景，则不必重新从零开始建图。
具体地，请参阅图5，图5是本发明的实施例中导航方法在所述移动机器人被人为放置于一未知位置时进行全局重定位的流程示意图，所述全局重定位流程包括步骤S501、S502、S503、S504及S505。
步骤S501：在所述未知位置上采集视频帧。
具体地，所述第一摄像头和所述第二摄像头分别在所述未知位置上采集一帧图像(即视频帧)，得到图像NewImg1和图像NewImg2。
步骤S502：检测在所述未知位置上采集的视频帧的特征点，并将所述特征点转化为词袋特征向量。
具体地，检测所述图像NewImg1和图像NewImg2的特征点，并将所述特征点转化为词袋特征向量。
步骤S503：将所述未知位置上采集的视频帧的特征点对应的词袋特征向量与所述场景数据库DB中的已有的所有词袋特征向量逐一进行对比。
具体地，将所述未知位置上采集的视频帧的特征点(即所述图像NewImg1和图像NewImg2的特征点)对应的词袋特征向量与所述场景数据库DB中的已有的所有词袋特征向量逐一进行对比。若所述场景数据库DB中存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量(将所述场景数据库DB中与所述未知位置的词袋特征向量相同或相似的词袋特征向量所对应的特征点集记为Simi_PtSet)，则执行步骤S504。
若所述场景数据库DB中不存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，则重复所述步骤S501。
步骤S504：将所述未知位置的视频帧的特征点集与所述场景数据库DB中的特征点集Simi_PtSet进行匹配。
步骤S505：匹配成功后求取该未知位置上所述移动机器人的位姿。
步骤S505的具体过程请参阅所述步骤S105、S106(包括步骤步骤S201、S202、S203、S204、S205及S206)及S107。
综上所述，本发明的实施例中，由于所述导航方法包括：使所述第一摄像头的朝向与所述第二摄像头的朝向不同，且分别从所述第一摄像头及所述第二摄像头获取视频帧，因此，所述移动机器人移动时所采集到的图像变化较大，更容易抓取到更多的特征点，并且大面积白墙同时占据两个摄像头所拍摄的图像的几率很小，有利于提升定位精度；且由于所述第一摄像头和所述第二摄像头朝向不同，所以同时面对光照强的位置或同时面对窗户的概率大幅度下降，提高在对场景的识别能力。
所述导航方法还包括：检测所述视频帧的特征点，将所述特征点转化为词袋特征向量并保存于场景数据库DB；根据场景数据库DB跟踪特征点，并排除离群点。由于将所述特征点转化为词袋特征向量，因此，减小了数据复杂度，降低数据存储量，为后续特征点跟踪及场景识别加快速度和提高精度，并且，由于跟踪特征点时，匹配过程会排除离群点，因此，可以进一步提高定位和场景识别的精度及速度。
此外，所述导航方法还包括：求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2；根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿。由此，通过简单的方法将两个摄像头的特征点快速提取，又高效整合，定位精度高、定位速度快，场景识别能力强，极大地提升了系统的鲁棒性。
本发明还提供一种采用上述导航方法的移动机器人，请参阅图6，图6是本发明实施例中的移动机器人的结构示意图。所述移动机器人包括机器人主体(图中未示出)、第一摄像头110及第二摄像头120，所述第一摄像头110及第二摄像头120设置于所述机器人主体上，并且第一摄像头110的朝向与所述第二摄像头120的朝向不同，第一摄像头110和第二摄像头120用于拍摄所述移动机器人的作业空间的图片，形成视频帧。第一摄像头120的朝向是指所述第一摄像头110的光轴的延伸方向，所述第二摄像头120的朝向是指所述第二摄像头120的光轴的延伸方向，换句话说，本实施例中所述第一摄像头110的光轴与所述第二摄像头120的光轴不平行。如此，有利于使所述第一摄像头110与所述第二摄像头120采集视频帧的方向不同，从而可以采集到更多不同的特征点，提高定位精度和场景识别能力。
所述移动机器人还包括同步定位及建图模块300和控制模块200，所述控制模块200用于控制所述第一摄像头110、所述第二摄像头120以及所述同步定位及建图模块300，所述同步定位及建图模块300包括视频帧采集单元310、特征点检测单元320、特征点跟踪单元330、计算单元340、优化单元350及更新单元360。
所述视频帧采集单元310从所述第一摄像头110及所述第二摄像头120获取视频帧。
所述特征点检测单元320，检测所述视频帧的特征点，并将所述特征点转化为词袋特征向量，并保存于场景数据库DB。
特征点跟踪单元330，根据所述场景数据库DB跟踪所述特征点，并排除离群点。
计算单元340与所述特征点跟踪单元330及优化单元350连接。所述计算单元340求取所述第一摄像头110的位姿转换矩阵T1及所述第二摄像头120的位姿转换矩阵T2，以及计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头110的三维点集New_PtSet1及对应于所述第二摄像头120的三维点集New_PtSet2。
优化单元350根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头110的位姿转换矩阵T1及所述第二摄像头120的位姿转换矩阵T2，得到所述移动机器人的位姿。
更新单元360根据所述移动机器人的位姿以及所述移动机器人的历史移动轨迹更新定位信息和地图。
进一步的，所述计算单元340还用于计算所述第一摄像头110坐标系到所述第二摄像头120坐标系的转换矩阵T12。
所述优化单元350还用于选择g2o库中的类g2o::VertexSE3Expmap作为待优化顶点的数据类型，其中，g2o(即General Graph Optimization，通用图优化)是通过定义顶点和边，将待优化问题表示成图，从而进行求解的项目。新建类g2o::VertexSE3Expmap的两个类对象A和B，并将A的预估值设置为所述第一摄像头110的位姿转换矩阵T1，B的预估值设置为所述第二摄像头120的位姿转换矩阵T2。向图中添加这两个顶点A和B。
所述优化单元350还用于：选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第一约束边的数据类型，所述第一约束边为只连接一个顶点的一元边，所连接的顶点设置为所述类对象A，将所述第一约束边的测量值设置为所述三维点集New_PtSet1中的点在所述第一摄像头110的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量。对应于New_PtSet1中的每个点，都如本步骤中所叙述的向图中添加一条约束边(均记为第一约束边)。
所述优化单元350还用于：选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第二约束边的数据类型，所述第二约束边所连接的顶点设置为所述类对象B，将所述第二约束边的测量值设置为所述三维点集New_PtSet2中的点在所述第二摄像头的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量。对应于New_PtSet2中的每个点，都如本步骤中所叙述的向图中添加一条约束边(均记为第二约束边)。
所述优化单元350还用于：新建类型为g2o::EdgeSE3的第三约束边，所述第三约束边为二元边，所连接的两个顶点分别设置为类对象A和类对象B，并将所述第三约束边的测量值设置为所述转换矩阵T1。向图中添加该约束边(即所述第三约束边)。
所述优化单元350还用于：调用g2o::SparseOptimizer::initializeOptimization()进行优化，得到所述第一摄像头的优化后的位姿转换矩阵T1_New和所述第二摄像头的优化后的位姿转换矩阵T2_New，据此求得所述移动机器人的优化后的位姿，从而得到新的定位信息。
具体地，所述第一摄像头110和所述第二摄像头120可以均是鱼眼摄像头。
所述移动机器人还包括与所述控制模块200连接的校正模块400，所述校正模块400用于校正所述第一摄像头110及所述第二摄像头120，得到所述第一摄像头110的校正参数矩阵K1及所述第二摄像头120的校正参数矩阵K2；
所述第一摄像头110在第一位置P1拍摄一帧图像I1-1，所述第二摄像头120在所述第一位置P1拍摄一帧图像I2-1，保存所述图像I1-1和所述图像I2-1；
所述移动机器人移动距离L到达第二位置P2，所述第一摄像头在所述第二位置拍摄一帧图像I1-2，所述第二摄像头在所述第二位置拍摄一帧图像I2-2，保存所述距离L、所述图像I1-2和所述图像I2-2，所述距离L使得所述第一摄像头及所述第二摄像头的视差足够大；第一位置P1及第二位置P2使得图像I1-1、图像I2-1、图像I1-2和图像I2-2的特征点最多。
所述计算单元340根据所述图像I1-1、所述图像I2-1、所述距离L、所述图像I1-2及所述图像I2-2，求取所述第一摄像头110的初始位姿转换矩阵T1-0、所述第二摄像头120的初始位姿转换矩阵T2-0、对应于所述第一摄像头110的初始三维点集Init_PtSet1及对应于所述第二摄像头120的初始三维点集Init_PtSet2；
所述优化单元350将所述第一摄像头110的校正参数矩阵K1、第二摄像头120的校正参数矩阵K2、第一摄像头110的初始位姿转换矩阵T1-0、第二摄像头120的初始位姿转换矩阵T2-0、对应于所述第一摄像头110的初始三维点集Init_PtSet1及对应于所述第二摄像头120的初始三维点集Init_PtSet2作为输入参数调用g2o库进行优化，得到所述移动机器人的初始位姿。
进一步地，所述移动机器人还包括全路径覆盖算法模块500、移动模块600、信息获取模块700、判断模块800和存储模块900，全路径覆盖算法模块500、移动模块600、信息获取模块700、判断模块800和存储模块900均与所述控制模块连接。
全路径覆盖算法模块500至少部分基于已有的定位信息及地图发出移动所述移动机器人的指令。
移动模块600接收所述全路径覆盖算法单元500发出的指令，并按所述指令移动所述移动机器人；
信息获取模块700获取所述移动单元移动所述移动机器人后所述更新单元360更新的定位信息和地图。
所述判断模块800用于判断所述移动机器人是否将待作业表面或地面覆盖完毕，所述存储单元900用于存储定位信息、地图及场景数据库DB；若所述移动机器人未覆盖完毕，则所述全路径覆盖算法单元500继续发出移动所述移动机器人的指令；若覆盖完毕，则所述存储单元900保存所述地图和所述场景数据库DB，供后续作业时调用，以进行定位及识别场景。
当所述移动机器人被认为移到或搬到某一未知位置时，所述视频帧采集单元310还用于：在未知位置上采集视频帧。其中，所述未知位置与所述移动机器人最近所经历的位置不连贯。
所述特征检测单元320还用于检测在所述未知位置上采集的视频帧的特征点，并将所述特征点转化为词袋特征向量‘’
所述特征跟踪单元330将所述未知位置上采集的视频帧的特征点对应的词袋特征向量与所述场景数据库DB中的已有的所有词袋特征向量逐一进行对比；若所述场景数据库DB中存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，将所述场景数据库DB中与所述未知位置的词袋特征向量相同或相似的词袋特征向量所对应的特征点集记为Simi_PtSet,则将所述未知位置的视频帧的特征点集与所述场景数据库DB中的特征点集Simi_PtSet进行匹配，所述计算单元340根据匹配成功的特征点求取所述未知位置上所述移动机器人的位姿。
若所述场景数据库DB中不存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，则所述视频帧采集单元310继续采集所述未知位置的视频帧，继续进行对比，直至识别当前位置。在一个实施例中，当采集的视频帧的数量足够大时，若所述场景数据库DB中仍然不存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，则重新初始化定位信息，重新定位建图。
可以理解的是，所述定位及建图模块300、控制模块200、全路径覆盖算法模块500、移动模块600、校正模块400、信息获取模块700、判断模块800及存储模块900可以是纯软件模块、硬件模块或者软件模块与硬件模块的组合。
在一个实施例中，所述定位及建图模块300、控制模块200、全路径覆盖算法模块500、移动模块600、校正模块400、信息获取模块700、判断模块800及存储模块900中的一个或多个模块或全部模块可以集成于同一模块中，例如，均集成于同一中央处理器上。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上所述的实施方式，并不构成对该技术方案保护范围的限定。任何在上述实施方式的精神和原则之内所作的修改、等同替换和改进等，均应包含在该技术方案的保护范围之内。

Claims (12)

1.一种移动机器人的导航方法，其特征在于，所述移动机器人包括机器人主体、第一摄像头及第二摄像头，所述第一摄像头及所述第二摄像头设置于所述机器人主体上，所述导航方法包括：
使所述第一摄像头的朝向与所述第二摄像头的朝向不同；
分别从所述第一摄像头及所述第二摄像头获取视频帧；
检测所述视频帧的特征点；
跟踪特征点，并排除离群点；
求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2；
根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿；以及
根据所述移动机器人的位姿更新定位信息和地图。
2.如权利要求1所述的移动机器人的导航方法，其特征在于，所述根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿矩阵T，具体包括步骤：
计算所述第一摄像头坐标系到所述第二摄像头坐标系的转换矩阵T12；
选择g2o库中的类“g2o::VertexSE3Expmap”作为待优化顶点的数据类型，新建该类型的两个类对象A和B，并将A的预估值设置为所述第一摄像头的位姿转换矩阵T1，B的预估值设置为所述第二摄像头的位姿转换矩阵T2；
选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第一约束边的数据类型，所述第一约束边为只连接一个顶点的一元边，所连接的顶点设置为所述类对象A，将所述第一约束边的测量值设置为所述三维点集New_PtSet1中的点在所述第一摄像头的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量；
选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第二约束边的数据类型，所述第二约束边所连接的顶点设置为所述类对象B，将所述第二约束边的测量值设置为所述三维点集New_PtSet2中的点在所述第二摄像头的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量；
新建类型为g2o::EdgeSE3的第三约束边，所述第三约束边为二元边，所述第三约束边的一个顶点设置为类对象A，另一个顶点设置为类对象B，并将所述第三约束边的测量值设置为所述转换矩阵T12；以及
调用g2o::SparseOptimizer::initializeOptimization()对优化器进行初始化，然后调用g2o::SparseOptimizer::optimize()完成优化，得到所述第一摄像头的优化后的位姿转换矩阵T1_New和所述第二摄像头的优化后的位姿转换矩阵T2_New，据此求取所述移动机器人的优化后的位姿。
3.如权利要求1或2所述的移动机器人的导航方法，其特征在于，所述检测所述视频帧的特征点，具体为：
检测所述视频帧的特征点，将所述特征点转化为词袋特征向量并保存于场景数据库DB；
所述跟踪特征点，并排除离群点，具体为：
根据所述场景数据库DB跟踪特征点，并排除离群点。
4.如权利要求3所述的移动机器人的导航方法，其特征在于，还包括步骤：
将所述移动机器人置于未知位置；
在所述未知位置上采集视频帧；
检测在所述未知位置上采集的视频帧的特征点，并将所述特征点转化为词袋特征向量；
将所述未知位置上采集的视频帧的特征点对应的词袋特征向量与已有的所有词袋特征向量逐一进行对比；
若所述场景数据库DB中存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，将所述场景数据库DB中与所述未知位置的词袋特征向量相同或相似的词袋特征向量所对应的特征点集记为Simi_PtSet,则将所述未知位置的视频帧的特征点集与所述场景数据库DB中的特征点集Simi_PtSet进行匹配，求取该未知位置上所述移动机器人的位姿；
若所述场景数据库DB中不存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，则重复所述步骤采集所述未知位置的视频帧。
5.如权利要求1或2所述的移动机器人的导航方法，其特征在于，所述导航方法还包括：
校正所述第一摄像头及所述第二摄像头，得到所述第一摄像头的校正参数矩阵K1及所述第二摄像头的校正参数矩阵K2；
进行初始化，包括：
使所述第一摄像头在第一位置拍摄一帧图像I1-1，所述第二摄像头在所述第一位置拍摄一帧图像I2-1，保存所述图像I1-1和所述图像I2-1；
使所述移动机器人移动距离L到达第二位置，所述第一摄像头在所述第二位置拍摄一帧图像I1-2，所述第二摄像头在所述第二位置拍摄一帧图像I2-2，保存所述距离L、所述图像I1-2和所述图像I2-2，所述距离L使得所述第一摄像头及所述第二摄像头的视差足够大；
根据所述图像I1-1、所述图像I2-1、所述距离L、所述图像I1-2及所述图像I2-2，求取所述第一摄像头的初始位姿转换矩阵T1-0、所述第二摄像头的初始位姿转换矩阵T2-0、对应于所述第一摄像头的初始三维点集Init_PtSet1及对应于所述第二摄像头的初始三维点集Init_PtSet2；
将所述第一摄像头的校正参数矩阵K1、所述第二摄像头的校正参数矩阵K2、所述第一摄像头的初始位姿转换矩阵T1-0、所述第二摄像头的初始位姿转换矩阵T2-0、对应于所述第一摄像头的初始三维点集Init_PtSet1及对应于所述第二摄像头的初始三维点集Init_PtSet2作为输入参数调用g2o库进行优化，得到所述移动机器人的优化的初始位姿。
6.如权利要求5所述的移动机器人的导航方法，其特征在于，还包括：
启动全路径覆盖算法，发出移动所述移动机器人的指令；
接收所述指令并移动所述移动机器人；
获取更新的所述定位信息和地图；
判断所述移动机器人是否将待作业表面覆盖完毕，若未覆盖完毕，则继续发出移动所述移动机器人的指令，若覆盖完毕，则保存所述地图和所述场景数据库DB，供后续作业时调用。
7.一种移动机器人，其特征在于，包括机器人主体、第一摄像头及第二摄像头，所述第一摄像头及所述第二摄像头设置于所述机器人主体上，并且所述第一摄像头的朝向与所述第二摄像头的朝向不同，所述移动机器人还包括同步定位及建图模块和控制模块，所述控制模块用于控制所述第一摄像头、所述第二摄像头以及所述同步定位及建图模块，所述同步定位及建图模块包括：
视频帧采集单元，用于从所述第一摄像头及所述第二摄像头获取视频帧；
特征点检测单元，用于检测所述视频帧的特征点，并将所述特征点转化为词袋特征向量，并保存于场景数据库DB；
特征点跟踪单元，配置为根据所述场景数据库DB跟踪所述特征点，并排除离群点；
计算单元，用于求取所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，以及计算跟踪成功的特征点的三维坐标，形成新的对应于所述第一摄像头的三维点集New_PtSet1及对应于所述第二摄像头的三维点集New_PtSet2；
优化单元，用于根据所述三维点集New_PtSet1及所述三维点集New_PtSet2优化所述第一摄像头的位姿转换矩阵T1及所述第二摄像头的位姿转换矩阵T2，得到所述移动机器人的位姿；以及
更新单元，用于根据所述移动机器人的位姿更新定位信息和地图。
8.如权利要求7所述的移动机器人，其特征在于，所述计算单元还用于计算所述第一摄像头坐标系到所述第二摄像头坐标系的转换矩阵T12；
所述优化单元还用于：选择g2o库中的类“g2o::VertexSE3Expmap”作为待优化顶点的数据类型，新建该类型的两个类对象A和B，并将A的预估值设置为所述第一摄像头的位姿转换矩阵T1，B的预估值设置为所述第二摄像头的位姿转换矩阵T2；
所述优化单元还用于：选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第一约束边的数据类型，所述第一约束边为只连接一个顶点的一元边，所连接的顶点设置为所述类对象A，将所述第一约束边的测量值设置为所述三维点集New_PtSet1中的点在所述第一摄像头的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量；
所述优化单元还用于：选择g2o库中的类g2o::EdgeSE3ProjectXYZOnlyPose作为第二约束边的数据类型，所述第二约束边所连接的顶点设置为所述类对象B，将所述第二约束边的测量值设置为所述三维点集New_PtSet2中的点在所述第二摄像头的图像中对应的像素坐标，并将该点的三维坐标赋值给g2o::EdgeSE3ProjectXYZOnlyPose对应的成员变量；
所述优化单元还用于：新建类型为g2o::EdgeSE3的第三约束边，所述第三约束边为二元边，所述第三约束边的一个顶点设置为类对象A，另一个顶点设置为类对象B，并将所述第三约束边的测量值设置为所述转换矩阵T1；
所述优化单元还用于：调用g2o::SparseOptimizer::initializeOptimization()进行优化，得到所述第一摄像头的优化后的位姿转换矩阵T1_New和所述第二摄像头的优化后的位姿转换矩阵T2_New，据此求得所述移动机器人的优化后的位姿。
9.如权利要求7或8所述的移动机器人，其特征在于，所述第一摄像头和所述第二摄像头均为鱼眼摄像头。
10.如权利要求7或8所述的移动机器人，其特征在于，所述移动机器人还包括与所述控制模块连接的校正模块，所述校正模块用于校正所述第一摄像头及所述第二摄像头，得到所述第一摄像头的校正参数矩阵K1及所述第二摄像头的校正参数矩阵K2；
所述第一摄像头在第一位置拍摄一帧图像I1-1，所述第二摄像头在所述第一位置拍摄一帧图像I2-1，保存所述图像I1-1和所述图像I2-1；
所述移动机器人移动距离L到达第二位置，所述第一摄像头在所述第二位置拍摄一帧图像I1-2，所述第二摄像头在所述第二位置拍摄一帧图像I2-2，保存所述距离L、所述图像I1-2和所述图像I2-2，所述距离L使得所述第一摄像头及所述第二摄像头的视差足够大；
所述计算单元根据所述图像I1-1、所述图像I2-1、所述距离L、所述图像I1-2及所述图像I2-2，求取所述第一摄像头的初始位姿转换矩阵T1-0、所述第二摄像头的初始位姿转换矩阵T2-0、对应于所述第一摄像头的初始三维点集Init_PtSet1及对应于所述第二摄像头的初始三维点集Init_PtSet2；
所述优化单元将所述第一摄像头的校正参数矩阵K1、所述第二摄像头的校正参数矩阵K2、所述第一摄像头的初始位姿转换矩阵T1-0、所述第二摄像头的初始位姿转换矩阵T2-0、对应于所述第一摄像头的初始三维点集Init_PtSet1及对应于所述第二摄像头的初始三维点集Init_PtSet2作为输入参数调用g2o库进行优化，得到所述移动机器人的初始位姿。
11.如权利要求10所述的移动机器人，其特征在于，所述移动机器人还包括：
全路径覆盖算法模块，配置为：至少部分基于已有的定位信息及地图发出移动所述移动机器人的指令；
移动模块，用于接收所述全路径覆盖算法单元发出的指令，并按所述指令移动所述移动机器人；
信息获取模块，用于获取所述移动单元移动所述移动机器人后所述更新单元更新的定位信息和地图；
判断模块和存储模块，所述判断单元用于判断所述移动机器人是否将待作业表面覆盖完毕，所述存储单元用于存储定位信息、地图及场景数据库DB；若所述移动机器人未覆盖完毕，则所述全路径覆盖算法单元继续发出移动所述移动机器人的指令；若覆盖完毕，则所述存储单元保存所述地图和所述场景数据库DB，供后续作业时调用。
12.如权利要求7所述的移动机器人，其特征在于，所述视频帧采集单元还用于：在未知位置上采集视频帧，所述未知位置与所述移动机器人最近所经历的位置不连贯；
所述特征检测单元还用于：检测在所述未知位置上采集的视频帧的特征点，并将所述特征点转化为词袋特征向量；
所述特征跟踪单元还用于：将所述未知位置上采集的视频帧的特征点对应的词袋特征向量与所述场景数据库DB中的已有的所有词袋特征向量逐一进行对比；若所述场景数据库DB中存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，将所述场景数据库DB中与所述未知位置的词袋特征向量相同或相似的词袋特征向量所对应的特征点集记为Simi_PtSet,则将所述未知位置的视频帧的特征点集与所述场景数据库DB中的特征点集Simi_PtSet进行匹配，所述计算单元根据匹配成功的特征点求取所述未知位置上所述移动机器人的位姿；
若所述场景数据库DB中不存在与所述未知位置的词袋特征向量相同或相似的词袋特征向量，则所述视频帧采集单元继续采集所述未知位置的视频帧。
CN201710045324.0A 2017-01-22 2017-01-22 移动机器人的导航方法及移动机器人 Active CN106813672B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710045324.0A CN106813672B (zh) 2017-01-22 2017-01-22 移动机器人的导航方法及移动机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710045324.0A CN106813672B (zh) 2017-01-22 2017-01-22 移动机器人的导航方法及移动机器人

Publications (2)

Publication Number Publication Date
CN106813672A true CN106813672A (zh) 2017-06-09
CN106813672B CN106813672B (zh) 2020-02-14

Family

ID=59111064

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710045324.0A Active CN106813672B (zh) 2017-01-22 2017-01-22 移动机器人的导航方法及移动机器人

Country Status (1)

Country Link
CN (1) CN106813672B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107297748A (zh) * 2017-07-27 2017-10-27 南京理工大学北方研究院 一种餐厅服务机器人系统及应用
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
CN108665541A (zh) * 2018-04-09 2018-10-16 北京三快在线科技有限公司 一种基于激光传感器的地图生成方法及装置和机器人
CN109635692A (zh) * 2018-11-30 2019-04-16 禾多科技（北京）有限公司 基于超声波传感器的场景再识别算法
WO2020024182A1 (zh) * 2018-08-01 2020-02-06 深圳市大疆创新科技有限公司 一种参数处理方法、装置及摄像设备、飞行器
CN111649724A (zh) * 2020-06-04 2020-09-11 百度在线网络技术（北京）有限公司 基于移动边缘计算的视觉定位方法和装置
CN113095227A (zh) * 2021-04-13 2021-07-09 京东数科海益信息科技有限公司 一种机器人定位方法、装置、电子设备及存储介质
WO2022110928A1 (zh) * 2020-11-24 2022-06-02 追觅创新科技(苏州)有限公司 机器人的建图方法、机器人、存储介质、电子装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104121902A (zh) * 2014-06-28 2014-10-29 福州大学 基于Xtion摄像机的室内机器人视觉里程计实现方法
CN104680522A (zh) * 2015-02-09 2015-06-03 浙江大学 基于智能手机前后摄像头同时工作的视觉定位方法
CN104748727A (zh) * 2013-12-31 2015-07-01 中国科学院沈阳自动化研究所 阵列式高速视觉里程计及其实现方法
CN104850615A (zh) * 2015-05-14 2015-08-19 西安电子科技大学 一种基于g2o的SLAM后端优化算法方法
CN105469405A (zh) * 2015-11-26 2016-04-06 清华大学 基于视觉测程的同时定位与地图构建方法
CN106272423A (zh) * 2016-08-31 2017-01-04 哈尔滨工业大学深圳研究生院 一种针对大尺度环境的多机器人协同制图与定位的方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104748727A (zh) * 2013-12-31 2015-07-01 中国科学院沈阳自动化研究所 阵列式高速视觉里程计及其实现方法
CN104121902A (zh) * 2014-06-28 2014-10-29 福州大学 基于Xtion摄像机的室内机器人视觉里程计实现方法
CN104680522A (zh) * 2015-02-09 2015-06-03 浙江大学 基于智能手机前后摄像头同时工作的视觉定位方法
CN104850615A (zh) * 2015-05-14 2015-08-19 西安电子科技大学 一种基于g2o的SLAM后端优化算法方法
CN105469405A (zh) * 2015-11-26 2016-04-06 清华大学 基于视觉测程的同时定位与地图构建方法
CN106272423A (zh) * 2016-08-31 2017-01-04 哈尔滨工业大学深圳研究生院 一种针对大尺度环境的多机器人协同制图与定位的方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
MICHAEL KAESS 等: "Probabilistic structure matching for visual SLAM with a multi-camera rig", 《COMPUTER VISION AND IMAGE UNDERSTANDING》 *
徐冬云: "基于Kinect的视觉同步定位与建图研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107297748A (zh) * 2017-07-27 2017-10-27 南京理工大学北方研究院 一种餐厅服务机器人系统及应用
CN107297748B (zh) * 2017-07-27 2024-03-26 南京理工大学北方研究院 一种餐厅服务机器人系统及应用
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
US11315264B2 (en) 2018-04-09 2022-04-26 Beijing Sankuai Online Technology Co., Ltd Laser sensor-based map generation
CN108665541A (zh) * 2018-04-09 2018-10-16 北京三快在线科技有限公司 一种基于激光传感器的地图生成方法及装置和机器人
CN108665541B (zh) * 2018-04-09 2019-06-07 北京三快在线科技有限公司 一种基于激光传感器的地图生成方法及装置和机器人
WO2020024182A1 (zh) * 2018-08-01 2020-02-06 深圳市大疆创新科技有限公司 一种参数处理方法、装置及摄像设备、飞行器
CN109635692A (zh) * 2018-11-30 2019-04-16 禾多科技（北京）有限公司 基于超声波传感器的场景再识别算法
CN109635692B (zh) * 2018-11-30 2021-07-06 禾多科技（北京）有限公司 基于超声波传感器的场景再识别方法
CN111649724B (zh) * 2020-06-04 2022-09-06 百度在线网络技术（北京）有限公司 基于移动边缘计算的视觉定位方法和装置
CN111649724A (zh) * 2020-06-04 2020-09-11 百度在线网络技术（北京）有限公司 基于移动边缘计算的视觉定位方法和装置
WO2022110928A1 (zh) * 2020-11-24 2022-06-02 追觅创新科技(苏州)有限公司 机器人的建图方法、机器人、存储介质、电子装置
CN113095227A (zh) * 2021-04-13 2021-07-09 京东数科海益信息科技有限公司 一种机器人定位方法、装置、电子设备及存储介质
CN113095227B (zh) * 2021-04-13 2023-11-07 京东科技信息技术有限公司 一种机器人定位方法、装置、电子设备及存储介质

Also Published As

Publication number Publication date
CN106813672B (zh) 2020-02-14

Similar Documents

Publication Publication Date Title
CN106813672A (zh) 移动机器人的导航方法及移动机器人
CN111202472B (zh) 扫地机器人的终端地图构建方法、终端设备及清扫系统
JP6987797B2 (ja) リアルタイムオンラインエゴモーション推定を有するレーザスキャナ
CN111210463B (zh) 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统
CN109974712A (zh) 一种基于图优化的变电站巡检机器人建图方法
US8195331B2 (en) Method, medium, and apparatus for performing path planning of mobile robot
CN110146099B (zh) 一种基于深度学习的同步定位与地图构建方法
CN109816696A (zh) 一种机器人定位与建图方法、计算机装置及计算机可读存储介质
CN110163968A (zh) Rgbd相机大型三维场景构建方法及系统
CN110000786A (zh) 一种基于视觉机器人的历史地图利用方法
CN111590595A (zh) 一种定位方法、装置、移动机器人及存储介质
CN105606092B (zh) 一种室内机器人定位方法及系统
CN107713915B (zh) 扫地机器人以及扫地机器人的定位方法
Courbon et al. Indoor navigation of a non-holonomic mobile robot using a visual memory
CN111182174A (zh) 一种扫地机器人补光的方法及装置
CN110163963A (zh) 一种基于slam的建图装置和建图方法
CN108225328A (zh) 一种室内三维数据采集方法
CN112767546A (zh) 移动机器人基于双目图像的视觉地图生成方法
CN111609854A (zh) 基于多个深度相机的三维地图构建方法及扫地机器人
CN102034092A (zh) 基于独立多自由度视觉模块的主动复合双目快速目标搜索与捕获系统
CN108927807A (zh) 一种基于点特征的机器人视觉控制方法
CN112882479A (zh) 一种基于内外角累积状态的路径规划方法
CN105806331A (zh) 一种应用于室内机器人的定位方法和室内机器人
CN116147618A (zh) 一种适用动态环境的实时状态感知方法及系统
CN114758011B (zh) 融合离线标定结果的变焦相机在线标定方法

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
GR01 Patent grant
GR01 Patent grant
CP01 Change in the name or title of a patent holder

Address after: 518110, Longhua New District incubator building, 1301 sightseeing Road, Guanlan District, Longhua New District, Guangdong, Shenzhen province C901

Patentee after: Shenzhen flying mouse Power Technology Co., Ltd

Address before: 518110, Longhua New District incubator building, 1301 sightseeing Road, Guanlan District, Longhua New District, Guangdong, Shenzhen province C901

Patentee before: Shenzhen Xiluo Robot Co.,Ltd.

CP01 Change in the name or title of a patent holder