融合单目视觉和编码器信息的机器人定位方法和系统

摘要

本发明涉及一种融合单目视觉和编码器数据的机器人定位方法和系统，包括基于滑动窗口法的外参数及视觉尺度因子的实时标定、基于滑动窗口法的位姿松耦合融合。其中基于滑动窗口法的外参数及视觉尺度因子的实时标定利用单目视觉和编码器输出的位姿信息构建滑动窗口位姿图，自动估计视觉传感器和编码器之间的相对位姿，并计算出单目视觉的尺度因子。同时，利用构建的滑动窗口位姿图对单目视觉定位信息和编码器定位信息进行融合，克服了编码器长时定位精度差和视觉定位鲁棒性差的问题，而基于滑动窗口位姿图的方式有效的保证了融合方法的计算量不会随时间增加，因此该算法适用于移动机器人机载嵌入式设备。

权利要求

1.一种融合单目视觉和编码器数据的机器人定位方法，其特征在于，包括：

步骤1、通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；

步骤2、根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；

步骤3、以机器人在机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点构建位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；

步骤4、当该位姿图内的节点数量超过预设的滑动窗口大小时，则将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将该边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。

2.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。

3.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，该步骤2包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子初值。

4.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。

5.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，该步骤4包括：当位姿图中节点个数超过该滑动窗口时，将最旧的节点通过边际化进行移除。

6.一种融合单目视觉和编码器数据的机器人定位系统，其特征在于，包括：

变化矩阵建立模块，用于通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；

计算模块，用于根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；

位姿图构建模块，用于构建以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点的位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；

位姿信息计算模块，用于在该位姿图内的节点数量超过预设的滑动窗口大小时，将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将该边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。

7.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位系统，其特征在于，该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。

8.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位系统，其特征在于，该计算模块包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子初值。

9.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位系统，其特征在于，每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。

10.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位系统，其特征在于，该位姿信息计算模块包括：当位姿图中节点个数超过该滑动窗口时，需要将最旧的节点通过边际化进行移除。

说明书

融合单目视觉和编码器信息的机器人定位方法和系统

技术领域

[0001] 本发明涉及移动机器人定位技术领域，特别涉及一种融合单目视觉和编码器信息的机器人定位方法和系统。

背景技术

[0002] 随着机器人技术的不断发展，人们对服务机器人在自然环境中能够自主的定位导航的要求越来强烈。利用编码器根据机器人运动学模型进行里程计计算是机器人领域常用的方法，但是存在累计误差无法消除，长时间运行定位精度差的问题。利用单目视觉进行定位近来发展快速，陆续出现了svo(semi direct visual odometry)，dso(direct sparse odometry)等视觉里程计算法。特点是累计误差小，定位精度高，但是容易受光照等外部环境影响而鲁棒性差，且单目视觉不能获得真实尺度因子。因此，将编码器和单目视觉进行融合定位应运而生，充分利用编码器和视觉的优点，鲁棒的完成机器人的定位。

[0003] 目前主要的融合定位方式分为滤波和位姿图优化两类。滤波的方法大部分采用扩展卡尔曼(EKF)滤波器框架，编码器里程计对机器人位姿进行预测，利用视觉测量进行更新。位姿图优化将机器人位姿构成图的节点，传感器测量作为图的边，通过构建位姿图对机器人状态进行迭代优化。标准的EKF方法中测量值都只被利用了一次，而位姿图优化的方法能够多次线性化操作，因此图优化能够获得更高的定位精度，近来被广泛采用。然而位姿图会随着加入的节点个数增加而运算复杂度增大，不利于机器人嵌入式设备上的实时计算。

发明内容

[0004] 为了解决上述技术问题，本发明通过滑动窗口限制位姿图节点个数，移除最旧的位姿节点，同时又尽量少的丢失移除节点的数据信息来完成机器人的实时定位要求，提供一种基于滑动窗口位姿图的融合单目视觉和编码器信息的松耦合定位方法和系统，将单目视觉估计的位姿信息和编码器里程计估计的位姿信息通过位姿图融合，满足机器人对定位精度和鲁棒性的需求。

[0005] 具体地说，本发明公开了一种融合单目视觉和编码器数据的机器人定位方法，其中包括：

[0006] 步骤1、通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；

[0007] 步骤2、根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；

[0008] 步骤3、以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点构建位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；

[0009] 步骤4、当该位姿图内的节点数量超过预设的滑动窗口大小时，则将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。

[0010] 该融合单目视觉和编码器数据的机器人定位方法，其中该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。

[0011] 该融合单目视觉和编码器数据的机器人定位方法，其中该步骤2包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。

[0012] 该融合单目视觉和编码器数据的机器人定位方法，其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。

[0013] 该融合单目视觉和编码器数据的机器人定位方法，其中该步骤4包括：当位姿图中节点个数超过该滑动窗口时，将最旧的节点通过边际化进行移除。

[0014] 本发明还提出了一种融合单目视觉和编码器数据的机器人定位系统，其中包括：

[0015] 变化矩阵建立模块，用于通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；

[0016] 计算模块，用于根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；

[0017] 位姿图构建模块，用于构建以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点的位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；

[0018] 位姿信息计算模块，用于在该位姿图内的节点数量超过预设的滑动窗口大小时，将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。

[0019] 该融合单目视觉和编码器数据的机器人定位系统，其中该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。

[0020] 该融合单目视觉和编码器数据的机器人定位系统，其中该计算模块包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。

[0021] 该融合单目视觉和编码器数据的机器人定位系统，其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。

[0022] 该融合单目视觉和编码器数据的机器人定位系统，其中该位姿信息计算模块包括：当位姿图中节点个数超过该滑动窗口时，需要将最旧的节点通过边际化进行移除。

[0023] 本发明的有益效果包括：可以自动计算并优化相机坐标系到机器人坐标系之间的变换矩阵以及单目里程计尺度因子；本发明使用滑动窗口位姿图能使系统计算量不随时间增加，同时能将单目视觉里程计信息和编码器里程计信息进行融合，有效的消除里程计累计误差，得到较优的定位结果。

附图说明

[0024] 图1为本发明系统结构图；

[0025] 图2为系统坐标系示意图；

[0026] 图3为位姿图示意图；

[0027] 图4为本发明滑动窗口位姿图流程示意图。

具体实施方式

[0028] 为让本发明的上述特征和效果能阐述的更明确易懂，下文特举实施例，并配合说明书附图作详细说明如下。

[0029] 如附图1示出本发明流程示意图，首先利用编码器和单目视觉模块计算得到的定位信息，解算出相机坐标系到机器人坐标系之间的外参数矩阵以及尺度因子初始值。解算完成以后，利用初始值以及各模块定位信息构造滑动窗口位姿图，通过非线性最小二乘对位姿图求解得到优化以后的外参数矩阵，尺度因子，以及定位信息。

[0030] 附图2中21为世界坐标系，22为机器人坐标系，23为相机坐标系。世界坐标系同初始时刻机器人坐标系相同。此处以两轮差分驱动移动机器人为例，机器人坐标系原点为两轮中心，x轴指向机器人正前方，z轴垂直地面向上，通过右手定则可以确定y轴。相机坐标系的中心为摄像机光心，z轴垂直图像平面，x轴为图像平面水平向右，y轴为图像平面水平向下。

[0031] 下面结合附图3，附图4，对本发明整个过程进行详细说明。

[0032] 步骤1、通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵。其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。

[0033] 该步骤1具体包括，利用相邻时间点t到t+1之间获取的相机图像以及编码器数据进行位姿计算。根据机器人运动模型和相邻时间点传感器采集的编码器数据，计算出第i时刻机器人坐标系ri到第j时刻机器人坐标系rj的位姿变化矩阵 并根据计算机视觉几何算法，对该相邻时间点单目里程计中相机采集的单目视觉图像进行处理，计算出第i时刻相机坐标系ci到第j时刻相机坐标系cj的位姿变化矩阵 其中该相机具有单目摄像头，其中变化矩阵为

[0034] R表示旋转矩阵，t＝[tx ty tz]T表示x，y，z三个轴方向的平移。

[0035] 步骤2、根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量。通过一段时间内的运动，利用步骤1中获取的一系列编码器和单目视觉测得的两种位姿变化矩阵解算出相机坐标系到机器人坐标系之间的变换矩阵初值以及单目里程计的尺度因子初值，其中需要采集一段时间的数据才能计算相机到机器人坐标系之间的外参数矩阵，因为仅凭一组数据无法计算。

[0036] 根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。机器人坐标系和相机坐标系都固定在机器人上，两坐标系之间是刚体变换，因此编码器计算的机器人位姿变换和单目视觉里程计之间存在对应的该约束方程组：

[0037]

[0038]

[0039] 式中q是旋转矩阵R对应的四元素表达形式。 表示四元数乘法，通过该方程组可以求解出相机坐标系c到机器人坐标系r之间的旋转矩阵rRc，以及平移向量里的tx,ty，以及单目尺度因子λ，其中T表示表示刚体变换矩阵，单目尺度因子即为该变尺度因子初值。由于机器人一般只在一个平面上运动，导致z轴方向的平移量tz无法估计，但tz对后续计算没有影响，因此可以忽略。

[0040] 步骤3、以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点构建位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束。定义t＝0时刻，机器人本体坐标系为世界坐标系，将机器人坐标系在世界坐标系中的位姿，相机坐标系到机器人坐标系之间的变换矩阵以及尺度因子作为位姿图中的节点，各传感器测得的相邻时间点间的位姿变化作为位姿图中的边约束，构建基于位姿图的融合框架。其中，该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。

[0041] 图3为位姿图示意图，机器人位姿 作为位姿图的节点，如图3中的圆圈，它表示第i时刻机器人坐标系r在世界坐标系w中的位姿。位姿到位姿之间通过传感器测量值进行约束，构成图中节点之间的边，如附图中的正方形块表示边约束。传感器的测量与节点之间的关系通用方程如下：

[0042] zij＝hij(xi,xj)+nij

[0043] 其中hij(xi,xj)将节点i和节点j的位姿隐射到一个测量值zij，nij是零均值的高斯白噪声，nij～N(0,Σij)。根据位姿图中的节点和边约束优化位姿的问题转化为求解最大似然估计：

[0044]

[0045] 更具体的，本发明中由于需要对外参数变换矩阵以及尺度因子进行优化，因此位姿图中的节点变量如下：

[0046] X＝{xi,xi+1,...,xi+N,s}

[0047] 其中， 表示相机坐标系c到机器人坐标系r的变换矩阵。编码器里程计和单目视觉里程计分别对应的测量函数为：

[0048] rzij＝rhij(xi,xj)+rnij

[0049]

[0050] 式中rzij,czij分别为i节点和j节点之间通过编码器或单目相机测得的测量值，rnij,cnij为零均值白噪声，对应的测量函数表达式为：

[0051]

[0052]

[0053] 步骤4、当该位姿图内的节点数量超过预设的滑动窗口大小时，则将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。具体包括，当位姿图节点数量超过滑动窗口大小，则将最旧的位姿节点去掉，并将去掉的位姿节点的边约束转变为一个先验约束加入位姿图，同时把新观测到的位姿节点及其约束加入位姿图。对构建好的位姿图进行优化迭代求解，更新机器人位姿，从而完成位姿信息的融合，输出定位信息，其中该滑动窗口由用户根据系统硬件水平和实际精度需求进行设定，因为该滑动窗口设置越大，计算精度越高，但计算量也会随之增大。

[0054] 如附图4所示，以最开始的5个状态为例，对基于滑动窗口位姿图的单目视觉和编码器信息松耦合定位方法的更新过程进行具体说明，图中滑动窗口设定大小为4。圆形节点xi表示机器人的位姿，s表示相机坐标系到机器人坐标系的外参数矩阵以及单目视觉尺度因子，圆形虚线节点表示已移除出位姿图的节点。黑色实心正方形表示编码器测量值构成的边，空心正方形表示单目视觉里程计测量值构成的边，斜线正方形表示移除节点后新加入的先验边。

[0055] 状态机器人从初始时刻开始陆续构造了位姿图中的4个位姿节点以及一个外参数节点X＝{x0,x1,x2,x3,s}。在第5个位姿x4以及相关测量加入位姿图前，需要从当前位姿图中最旧的节点x0通过边际化进行移除：

[0056]

[0057] 其中，xm＝{x0}表示位姿图中要移除的第0个节点，xb＝{x1,s}表示和第0个节点相连的节点变量，zm表示xm和xb直接的测量约束， 表示移除i-1个节点后，xb所满足的高斯分布。为了求解该高斯分布，需要计算如下最大似然问题：

[0058]

[0059] 等效于求解如下非线性最小二乘问题：

[0060]

[0061] 当移除节点xm以及相关边约束后， 作为xb的先验值，需要新构造一个先验边与xb相连接，如图中的斜线正方形块。

[0062] 新的位姿节点x4加入位姿图，对新构造的位姿图转化为非线性最小二乘问题进行迭代求解：

[0063]

[0064] 迭代收敛后输出的最优结果即是外参数矩阵，单目尺度因子，以及融合以后的机器人位姿信息(pose information)。

[0065] 以下为与上述方法实施例对应的系统实施例，本实施系统可与上述实施方式互相配合实施。上述施方式中提到的相关技术细节在本实施系统中依然有效，为了减少重复，这里不再赘述。相应地，本实施系统中提到的相关技术细节也可应用在上述实施方式中。

[0066] 本发明还提出了一种融合单目视觉和编码器数据的机器人定位系统，其中包括：

[0067] 变化矩阵建立模块，用于通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；

[0068] 计算模块，用于根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；

[0069] 位姿图构建模块，用于构建以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点的位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；

[0070] 位姿信息计算模块，用于在该位姿图内的节点数量超过预设的滑动窗口大小时，将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。

[0071] 该融合单目视觉和编码器数据的机器人定位系统，其中该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。

[0072] 该融合单目视觉和编码器数据的机器人定位系统，其中该计算模块包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。

[0073] 该融合单目视觉和编码器数据的机器人定位系统，其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。

[0074] 该融合单目视觉和编码器数据的机器人定位系统，其中该位姿信息计算模块包括：当位姿图中节点个数超过该滑动窗口时，需要将最旧的节点通过边际化进行移除。

[0075] 虽然本发明以上述实施例公开，但具体实施例仅用以解释本发明，并不用于限定本发明，任何本技术领域技术人员，在不脱离本发明的构思和范围内，可作一些的变更和完善，故本发明的权利保护范围以权利要求书为准。