基于影像特征的机器人定位的方法

技术领域

本发明涉及自动化控制技术领域，具体涉及一种基于影像特征的机器人定位的方法和系统。

背景技术

随着时代与科技的发展，机器人逐渐走进人们的生活，而不再局限于工厂里的生产工具，日常生活中包括扫地、安保、送餐、看护等场合都能看到机器人的身影，而当机器人开始行走时，其导航定位就成为十分重要的课题，进而发展出同步定位与建图技术(SLAM)。

目前SLAM的方法，有的使用范围传感器，有的使用雷达测距仪、红外线测距仪，这些传感器的优点在于精确度比较高且较迅速，但成本较高且范围传感器只能提供一维信息，加上机器人的左右轮，利用里程传感器最多能够提供两维环境信息，或者是利用视觉传感器，相比较与范围传感器，成本比较低廉，且能够获取到三维空间信息，此外还有组合两者的复合传感器，例如kinect；在1991年John J.leonard和Hugh Durrant-Whyt完成了利用扩展性卡尔曼滤波器，结合声呐传感器的运动状态估计，达到了SLAM功能的机器人；2003年开始，A.J.Davision等人提出利用单镜头相机完成同步定位与建图功能，即运用相机在不同位置，取得影像平面上环境特征点坐标的相对变化作为测量向量值；

在特征点选取与匹配的方法，从早期C.Harris与M.Stephens提出的Harris角点与边缘检测，还有由David.G.Lowe提出的SIFT算法与H.Bay提出的SURF算法。在SLAM可行后，开始出现利用影像资料进行机器人路径复原与拼接的研究，在1999年Dellaert,F.等人提出利用蒙特卡洛方法进行机器人路径复原，但此方法使用的前提是必须在已知空间内。

发明内容

本发明的目的是提供一种基于影像特征的机器人定位的方法，利用向上镜头相机作为感测器，能够达到较低成本的同步定位与建图，同时克服传统向前相机定位稳定性差、误差大的问题，而且能够有效解决在机器人运动时，由某些因素造成机器人脱离主要路径，从而导致定位失败；或是因为某些特殊工作需求，使得初始绝对位置与原始路径不同，从而造成同步定位与建图无法延续的问题。

为解决上述问题，本发明的提供了一种基于影像特征的机器人定位的方法，包括：实时采集所述机器人的影像数据和里程编码器测量数据；对影像数据进行镜头形变校正处理，存储镜头形变校正处理后的影像数据于影像数据库和存储里程编码器测量数据。

根据所述机器人的空间坐标转化成影像平面坐标，根据所述影像平面坐标和所述里程编码器测量数据定义所述机器人的运动模型；

根据所述影像数据的影像特征点更新同步定位与建图SLAM数据库。

匹配每次所述SLAM计算的影像特征数据与其前一次SLAM计算的影像特征数据，得到匹配值。根据所述匹配值计算所述机器人下一时刻的位置和旋转角度，得到机器人的移动路径。

根据本发明的一个实施例，所述根据所述影像平面坐标和所述里程编码器测量数据定义所述机器人的运动模型，包括：

根据所述里程编码器测量数据定义所述机器人的左轮速度、右轮速度、中心速度和角速度。

根据所述机器人的影像平面坐标、所述里程编码器测量数据定义所述所述机器人位移向量和位移角度。

根据本发明的一个实施例，所述实时采集所述机器人的影像数据和里程编码器测量数据的时间间隔为Δt。

根据本发明的一个实施例，所述对影像数据进行镜头形变校正处理，包括根据设定的相机模型对影像数据进行镜头形变校正处理。

根据本发明的一个实施例，所述相机模型与相机内部参数、相机外部参数相关；所述相机内部参数与所述相机种类特性相关，所述相机外部参数用于表示空间坐标与影像平面坐标之间关系。

根据本发明的一个实施例，所述根据所述影像数据的影像特征点更新同步定位与建图SLAM数据库，包括：

S301：分别对当下采集的影像数据与影像数据库中的影像数据进行特征提取与匹配，剔除错误匹配数据。

S302：判断当下采集的影像数据与影像数据库中的影像数据中匹配成功的数据是否为新的特征点；所述新的特征点为首次匹配不成功且第二次匹配成功的特征点。

S303：如果所述特征点为新的特征点，则初始化其逆深度参数；更新测量向量值；所述测量向量值与所述特征点的上一时刻的状态变量、里程编码器测量数据相关。

S304：判断所述更新后的测量向量值是否有效，如果所述测量向量值无效则将其删除；如果所述测量向量值有效，则将其机器人位置数据、特征点数据更新到SLAM资料库。

根据本发明的一个实施例，所述S303中所述如果所述特征点为新的特征点，则初始化其逆深度参数，更新测量值，包括：所述新的特征点位置状态利用逆深度参数坐标进行表示。设定特征点的状态表示。根据新的特征点投影影像平面的坐标与实际观察的影像平面特征点坐标更新测量向量值。

根据本发明的一个实施例，在根据所述影像数据的影像特征点更新同步定位与建图SLAM数据库步骤后，还包括判断是否此次影像数据是否为首次更新SLAM数据库，并存储此影像数据更新SLAM数据库的次数。

根据本发明的一个实施例，所述根据所述匹配值计算所述机器人下一时刻的位置和旋转角度，得到机器人的移动路径步骤之后，还包括，将此次更新的路径与此影像数据前几次更新的路径进行相似度比较，根据比较结果得到此次移动路径。

本发明的上述技术方案具有如下有益的技术效果：该方法运用扩展性卡尔曼滤波器，整合里程编码器信息与向上镜头相机的影像资料，同时借助SURF算法来对影像特征点进行描述匹配，实现机器人的同步定位与建图；通过将同步定位与建图过程中的机器人位置与影像特征点信息存储为资料库，针对机器人定位失败或者重新运行的情况，借助该资料库结果与影像定位方法可定位出机器人位置，从而达到机器人位置复原与路径拼接的目的。此方法能有效解决某些因素导致的定位失效，改善机器人同步定位与建图无法延续的问题，具有广泛的工程应用价值。

附图说明

图1是本发明实施例公开的基于影像特征的机器人定位的方法流程图；

图2是本发明实施例公开的SLAM计算子流程图；

图3本发明实施例公开的路径拼接方法流程；

图4是本发明实施例公开的又一基于影像特征的机器人定位的方法流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

本发明公开了一种基于影像特征的机器人定位的方法，如图1所示，包括：

S1：实时采集所述机器人的影像数据和里程编码器测量数据；对影像数据进行镜头形变校正处理，存储镜头形变校正处理后的影像数据于影像数据库和存储里程编码器测量数据。

对影像数据进行镜头形变校正处理，包括根据设定的相机模型对影像数据进行镜头形变校正处理。

所述相机模型与相机内部参数、相机外部参数相关；所述相机内部参数与所述相机种类特性相关，所述相机外部参数用于表示空间坐标与影像平面坐标之间关系。

所述相机模型定义如下：空间中任一点表示为P

dx为在u方向每单位像素的大小，dy表示在v方向每单位像素的大小，得到关系式式(3)。

其中，矩阵K即为相机的内部参数，p点在x方向上的分量、p点在y方向上的分量为式(4)、式(5)。

相机外部参数用于表示空间坐标与影像平面坐标之间关系，由3×3旋转矩阵R与3×1平移矩阵t组成，描述相机的位置与姿势为式(6)。

合并相机内外参数表达式即为相机模型为式(7)。

利用相机参数的投影矩阵将特征点投影到影像平面上，其转换关系式为式(8)。

其中，λ为一比例常数，P

上述式9、式10通过利用奇异值分解法即可求出投影矩阵H＝[h

S2：根据所述机器人的位置坐标和所述里程编码器测量数据定义所述机器人的运动模型。

根据所述里程编码器测量数据定义所述机器人的左轮速度、右轮速度、中心速度和角速度。根据所述机器人的位置坐标、所述里程编码器测量数据定义所述机器人位移向量和位移角度，如下式所示。

其中，Encoder(L)为左轮里程编码器在时间Δt内的测量值，Encoder(R)为右轮里程编码器在时间Δt内的测量值，D为轮子直径，L为两轮之间的距离，V

假设初始位置为(x

S

θ

其中，(x

所述实时采集所述机器人的影像数据和里程编码器测量数据的时间间隔为Δt。

S3：根据所述机器人位置信息、影像特征点更新同步定位与建图SLAM数据库，如图2所示，包括：

S301：分别对当下采集的影像数据与影像数据库中的影像数据进行特征提取与匹配，剔除错误匹配数据；

S302：判断当下采集的影像数据与影像数据库中的影像数据中匹配成功的数据是否为新的特征点；所述新的特征点为首次匹配不成功，第二次匹配成功的特征点；

S303：如果所述特征点为新的特征点，则初始化其逆深度参数；更新测量向量值；所述测量向量值与所述特征点的上一时刻的状态变量、里程编码器测量数据相关。

特征点位置状态利用逆深度参数坐标表示法表示为：

由式(16)、(17)、(18)可推导出机器人的运动表达式为式(19)所示：

Δx(k)＝V

Δy(k)＝V

Δθ(k)＝ω

其中，x

设定特征点的状态表示，而整体系统状态表示为式(20)：

其中，x

利用影像特征点投影到影像平面的坐标与实际观察到的影像平面特征点来进行测量向量值的更新，实际观测到的特征点位置为

其中，(x

由于测量状态与估计状态的维度不同，所以必须进行线性化，而线性化过程的Jacobian矩阵H表示为式(22)。

其中，h

利用上述系统状态方程式的时间更新与测量方程式的状态修正，便可以对机器人的位置与姿势进行实时计算。

利用扩展性卡尔曼滤波器同时处理系统的状态方程式与测量方程式，将两个方程式做结合，更新及修正误差，其非线性离散状态方程式及测量方程式为如式(23)、式(24)。

x

z

其中，x

其线性化表示为，如式(25)、式(26)。

其中，A是f对x偏微的Jacobian矩阵，如式(27)。

H是h对x偏微的Jacobian矩阵，如式(28)。

S304：判断所述更新后的测量值是否有效，如果所述测量向量值无效则将其删除；如果所述测量值有效，则将其机器人位置数据、特征点数据更新到SLAM资料库。

S4：匹配每次所述SLAM计算的影像特征数据与其前一次SLAM计算的影像特征数据，得到匹配值；根据所述匹配值、计算所述机器人下一时刻的位置和旋转角度，得到机器人的移动路径。如图3所示，具体步骤包括：

将机器人最新SLAM计算中每一步的影像特征，与先前SLAM计算资料库中所有的特征数据进行匹配，然后判断匹配数量是否超过指定阈值。

利用匹配成功的特征点进行影像定位，得出对应机器人的位置与旋转角度，进行路径的拼接。

本发明第二方面公开了一种基于影像特征的机器人定位的方法，与第一方面不同之处在于，如图4所示，在所述S3根据所述机器人位置信息、影像特征点更新同步定位与建图SLAM数据库步骤后判断是否为首次更新SLAM数据库，并存储由此影像数据更新SLAM数据库的次数。

所述S4根据所述匹配值计算所述机器人下一时刻的位置和旋转角度，得到机器人的移动路径步骤之后，还包括，将此次更新的路径与此影像数据前几次更新的路径进行相似度比较，根据比较结果得到此次移动路径。其他方法均与第一方面公开的基于影像特征的机器人定位的方法相同或相似，这里不再赘述。

本发明在进行机器人同步定位与成图时，借助向上镜头相机在不同位置取得影像内环境特征点的位置或相对变化，通过运用扩展型卡尔曼滤波器，结合里程编码器与相机影像数据，由机器人运动模型推导出其状态方程式，利用里程编码器的测量数据作为位置的估计值，再借助SURF算法所表达影像特征点在影像平面的差值来进行测量向量值更新，从而可以不断修正特征点的实际位置，达到同步定位与成图的目的，该方法能够有效解决在机器人运动时，由某些因素造成机器人脱离主要路径，从而导致定位失败；或是因为某些特殊工作需求，使得初始绝对位置与原始路径不同，从而造成同步定位与建图无法延续的问题，具有广泛的工程应用价值。

应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。