CN107516326B - 融合单目视觉和编码器信息的机器人定位方法和*** - Google Patents

融合单目视觉和编码器信息的机器人定位方法和*** Download PDF

Info

Publication number
CN107516326B
CN107516326B CN201710574132.9A CN201710574132A CN107516326B CN 107516326 B CN107516326 B CN 107516326B CN 201710574132 A CN201710574132 A CN 201710574132A CN 107516326 B CN107516326 B CN 107516326B
Authority
CN
China
Prior art keywords
pose
robot
monocular
coordinate system
initial value
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
Application number
CN201710574132.9A
Other languages
English (en)
Other versions
CN107516326A (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.)
Institute of Computing Technology of CAS
Original Assignee
Institute of Computing Technology of CAS
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 Institute of Computing Technology of CAS filed Critical Institute of Computing Technology of CAS
Priority to CN201710574132.9A priority Critical patent/CN107516326B/zh
Publication of CN107516326A publication Critical patent/CN107516326A/zh
Application granted granted Critical
Publication of CN107516326B publication Critical patent/CN107516326B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/12—Simultaneous equations, e.g. systems of linear equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Operations Research (AREA)
  • Manipulator (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Processing (AREA)

Abstract

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

Description

融合单目视觉和编码器信息的机器人定位方法和***
技术领域
本发明涉及移动机器人定位技术领域，特别涉及一种融合单目视觉和编码器信息的机器人定位方法和***。
背景技术
随着机器人技术的不断发展，人们对服务机器人在自然环境中能够自主的定位导航的要求越来强烈。利用编码器根据机器人运动学模型进行里程计计算是机器人领域常用的方法，但是存在累计误差无法消除，长时间运行定位精度差的问题。利用单目视觉进行定位近来发展快速，陆续出现了svo(semi direct visual odometry)，dso(direct sparseodometry)等视觉里程计算法。特点是累计误差小，定位精度高，但是容易受光照等外部环境影响而鲁棒性差，且单目视觉不能获得真实尺度因子。因此，将编码器和单目视觉进行融合定位应运而生，充分利用编码器和视觉的优点，鲁棒的完成机器人的定位。
目前主要的融合定位方式分为滤波和位姿图优化两类。滤波的方法大部分采用扩展卡尔曼(EKF)滤波器框架，编码器里程计对机器人位姿进行预测，利用视觉测量进行更新。位姿图优化将机器人位姿构成图的节点，传感器测量作为图的边，通过构建位姿图对机器人状态进行迭代优化。标准的EKF方法中测量值都只被利用了一次，而位姿图优化的方法能够多次线性化操作，因此图优化能够获得更高的定位精度，近来被广泛采用。然而位姿图会随着加入的节点个数增加而运算复杂度增大，不利于机器人嵌入式设备上的实时计算。
发明内容
为了解决上述技术问题，本发明通过滑动窗口限制位姿图节点个数，移除最旧的位姿节点，同时又尽量少的丢失移除节点的数据信息来完成机器人的实时定位要求，提供一种基于滑动窗口位姿图的融合单目视觉和编码器信息的松耦合定位方法和***，将单目视觉估计的位姿信息和编码器里程计估计的位姿信息通过位姿图融合，满足机器人对定位精度和鲁棒性的需求。
具体地说，本发明公开了一种融合单目视觉和编码器数据的机器人定位方法，其中包括：
步骤1、通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；
步骤2、根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；
步骤3、以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点构建位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；
步骤4、当该位姿图内的节点数量超过预设的滑动窗口大小时，则将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。
该融合单目视觉和编码器数据的机器人定位方法，其中该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。
该融合单目视觉和编码器数据的机器人定位方法，其中该步骤2包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。
该融合单目视觉和编码器数据的机器人定位方法，其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。
该融合单目视觉和编码器数据的机器人定位方法，其中该步骤4包括：当位姿图中节点个数超过该滑动窗口时，将最旧的节点通过边际化进行移除。
本发明还提出了一种融合单目视觉和编码器数据的机器人定位***，其中包括：
变化矩阵建立模块，用于通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；
计算模块，用于根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；
位姿图构建模块，用于构建以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点的位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；
位姿信息计算模块，用于在该位姿图内的节点数量超过预设的滑动窗口大小时，将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。
该融合单目视觉和编码器数据的机器人定位***，其中该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。
该融合单目视觉和编码器数据的机器人定位***，其中该计算模块包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。
该融合单目视觉和编码器数据的机器人定位***，其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。
该融合单目视觉和编码器数据的机器人定位***，其中该位姿信息计算模块包括：当位姿图中节点个数超过该滑动窗口时，需要将最旧的节点通过边际化进行移除。
本发明的有益效果包括：可以自动计算并优化相机坐标系到机器人坐标系之间的变换矩阵以及单目里程计尺度因子；本发明使用滑动窗口位姿图能使***计算量不随时间增加，同时能将单目视觉里程计信息和编码器里程计信息进行融合，有效的消除里程计累计误差，得到较优的定位结果。
附图说明
图1为本发明***结构图；
图2为***坐标系示意图；
图3为位姿图示意图；
图4为本发明滑动窗口位姿图流程示意图。
具体实施方式
为让本发明的上述特征和效果能阐述的更明确易懂，下文特举实施例，并配合说明书附图作详细说明如下。
如附图1示出本发明流程示意图，首先利用编码器和单目视觉模块计算得到的定位信息，解算出相机坐标系到机器人坐标系之间的外参数矩阵以及尺度因子初始值。解算完成以后，利用初始值以及各模块定位信息构造滑动窗口位姿图，通过非线性最小二乘对位姿图求解得到优化以后的外参数矩阵，尺度因子，以及定位信息。
附图2中21为世界坐标系，22为机器人坐标系，23为相机坐标系。世界坐标系同初始时刻机器人坐标系相同。此处以两轮差分驱动移动机器人为例，机器人坐标系原点为两轮中心，x轴指向机器人正前方，z轴垂直地面向上，通过右手定则可以确定y轴。相机坐标系的中心为摄像机光心，z轴垂直图像平面，x轴为图像平面水平向右，y轴为图像平面水平向下。
下面结合附图3，附图4，对本发明整个过程进行详细说明。
步骤1、通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵。其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。
该步骤1具体包括，利用相邻时间点t到t+1之间获取的相机图像以及编码器数据进行位姿计算。根据机器人运动模型和相邻时间点传感器采集的编码器数据，计算出第i时刻机器人坐标系ri到第j时刻机器人坐标系rj的位姿变化矩阵
Figure BDA0001350469930000051
并根据计算机视觉几何算法，对该相邻时间点单目里程计中相机采集的单目视觉图像进行处理，计算出第i时刻相机坐标系ci到第j时刻相机坐标系cj的位姿变化矩阵
Figure BDA0001350469930000052
其中该相机具有单目摄像头，其中变化矩阵为
Figure BDA0001350469930000053
R表示旋转矩阵，t＝[tx ty tz]T表示x，y，z三个轴方向的平移。
步骤2、根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量。通过一段时间内的运动，利用步骤1中获取的一系列编码器和单目视觉测得的两种位姿变化矩阵解算出相机坐标系到机器人坐标系之间的变换矩阵初值以及单目里程计的尺度因子初值，其中需要采集一段时间的数据才能计算相机到机器人坐标系之间的外参数矩阵，因为仅凭一组数据无法计算。
根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。机器人坐标系和相机坐标系都固定在机器人上，两坐标系之间是刚体变换，因此编码器计算的机器人位姿变换和单目视觉里程计之间存在对应的该约束方程组：
Figure BDA0001350469930000054
Figure BDA0001350469930000055
式中q是旋转矩阵R对应的四元素表达形式。
Figure BDA0001350469930000056
表示四元数乘法，通过该方程组可以求解出相机坐标系c到机器人坐标系r之间的旋转矩阵rRc，以及平移向量
Figure BDA0001350469930000057
里的tx,ty，以及单目尺度因子λ，其中T表示表示刚体变换矩阵，单目尺度因子即为该变尺度因子初值。由于机器人一般只在一个平面上运动，导致z轴方向的平移量tz无法估计，但tz对后续计算没有影响，因此可以忽略。
步骤3、以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点构建位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束。定义t＝0时刻，机器人本体坐标系为世界坐标系，将机器人坐标系在世界坐标系中的位姿，相机坐标系到机器人坐标系之间的变换矩阵以及尺度因子作为位姿图中的节点，各传感器测得的相邻时间点间的位姿变化作为位姿图中的边约束，构建基于位姿图的融合框架。其中，该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。
图3为位姿图示意图，机器人位姿
Figure BDA0001350469930000061
作为位姿图的节点，如图3中的圆圈，它表示第i时刻机器人坐标系r在世界坐标系w中的位姿。位姿到位姿之间通过传感器测量值进行约束，构成图中节点之间的边，如附图中的正方形块表示边约束。传感器的测量与节点之间的关系通用方程如下：
zij＝hij(xi,xj)+nij
其中hij(xi,xj)将节点i和节点j的位姿隐射到一个测量值zij，nij是零均值的高斯白噪声，nij～N(0,Σij)。根据位姿图中的节点和边约束优化位姿的问题转化为求解最大似然估计：
Figure BDA0001350469930000062
更具体的，本发明中由于需要对外参数变换矩阵以及尺度因子进行优化，因此位姿图中的节点变量如下：
X＝{xi,xi+1,...,xi+N,s}
其中，
Figure BDA0001350469930000063
表示相机坐标系c到机器人坐标系r的变换矩阵。编码器里程计和单目视觉里程计分别对应的测量函数为：
rzij＝rhij(xi,xj)+rnij
Figure BDA0001350469930000075
式中rzij,czij分别为i节点和j节点之间通过编码器或单目相机测得的测量值，rnij,cnij为零均值白噪声，对应的测量函数表达式为：
Figure BDA0001350469930000071
Figure BDA0001350469930000072
步骤4、当该位姿图内的节点数量超过预设的滑动窗口大小时，则将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。具体包括，当位姿图节点数量超过滑动窗口大小，则将最旧的位姿节点去掉，并将去掉的位姿节点的边约束转变为一个先验约束加入位姿图，同时把新观测到的位姿节点及其约束加入位姿图。对构建好的位姿图进行优化迭代求解，更新机器人位姿，从而完成位姿信息的融合，输出定位信息，其中该滑动窗口由用户根据***硬件水平和实际精度需求进行设定，因为该滑动窗口设置越大，计算精度越高，但计算量也会随之增大。
如附图4所示，以最开始的5个状态为例，对基于滑动窗口位姿图的单目视觉和编码器信息松耦合定位方法的更新过程进行具体说明，图中滑动窗口设定大小为4。圆形节点xi表示机器人的位姿，s表示相机坐标系到机器人坐标系的外参数矩阵以及单目视觉尺度因子，圆形虚线节点表示已移除出位姿图的节点。黑色实心正方形表示编码器测量值构成的边，空心正方形表示单目视觉里程计测量值构成的边，斜线正方形表示移除节点后新加入的先验边。
状态机器人从初始时刻开始陆续构造了位姿图中的4个位姿节点以及一个外参数节点X＝{x0,x1,x2,x3,s}。在第5个位姿x4以及相关测量加入位姿图前，需要从当前位姿图中最旧的节点x0通过边际化进行移除：
Figure BDA0001350469930000073
其中，xm＝{x0}表示位姿图中要移除的第0个节点，xb＝{x1,s}表示和第0个节点相连的节点变量，zm表示xm和xb直接的测量约束，
Figure BDA0001350469930000074
表示移除i-1个节点后，xb所满足的高斯分布。为了求解该高斯分布，需要计算如下最大似然问题：
Figure BDA0001350469930000081
等效于求解如下非线性最小二乘问题：
Figure BDA0001350469930000082
当移除节点xm以及相关边约束后，
Figure BDA0001350469930000083
作为xb的先验值，需要新构造一个先验边与xb相连接，如图中的斜线正方形块。
新的位姿节点x4加入位姿图，对新构造的位姿图转化为非线性最小二乘问题进行迭代求解：
Figure BDA0001350469930000084
迭代收敛后输出的最优结果即是外参数矩阵，单目尺度因子，以及融合以后的机器人位姿信息(pose information)。
以下为与上述方法实施例对应的***实施例，本实施***可与上述实施方式互相配合实施。上述施方式中提到的相关技术细节在本实施***中依然有效，为了减少重复，这里不再赘述。相应地，本实施***中提到的相关技术细节也可应用在上述实施方式中。
本发明还提出了一种融合单目视觉和编码器数据的机器人定位***，其中包括：
变化矩阵建立模块，用于通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；
计算模块，用于根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；
位姿图构建模块，用于构建以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点的位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；
位姿信息计算模块，用于在该位姿图内的节点数量超过预设的滑动窗口大小时，将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将去掉的边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。
该融合单目视觉和编码器数据的机器人定位***，其中该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。
该融合单目视觉和编码器数据的机器人定位***，其中该计算模块包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子。
该融合单目视觉和编码器数据的机器人定位***，其中每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。
该融合单目视觉和编码器数据的机器人定位***，其中该位姿信息计算模块包括：当位姿图中节点个数超过该滑动窗口时，需要将最旧的节点通过边际化进行移除。
虽然本发明以上述实施例公开，但具体实施例仅用以解释本发明，并不用于限定本发明，任何本技术领域技术人员，在不脱离本发明的构思和范围内，可作一些的变更和完善，故本发明的权利保护范围以权利要求书为准。

Claims (10)

1.一种融合单目视觉和编码器数据的机器人定位方法，其特征在于，包括：
步骤1、通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；
步骤2、根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；
步骤3、以机器人在机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点构建位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；
步骤4、当该位姿图内的节点数量超过预设的滑动窗口大小时，则将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将该边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。
2.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。
3.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，该步骤2包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子初值。
4.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。
5.根据权利要求1所述的融合单目视觉和编码器数据的机器人定位方法，其特征在于，该步骤4包括：当位姿图中节点个数超过该滑动窗口时，将最旧的节点通过边际化进行移除。
6.一种融合单目视觉和编码器数据的机器人定位***，其特征在于，包括：
变化矩阵建立模块，用于通过机器人具有的多个定位传感器采集编码器数据，根据该编码器数据和机器人运动模型，计算出机器人坐标系的编码位姿变化矩阵，并通过该机器人搭载的相机采集单目视觉图像，根据计算机视觉几何算法，对该单目视觉图像进行处理，计算出相机坐标系的视觉位姿变化矩阵；
计算模块，用于根据该视觉位姿变化矩阵和该编码位姿变化矩阵，计算出该相机坐标系到该机器人坐标系之间的变换矩阵初值、单目尺度因子初值和平移向量；
位姿图构建模块，用于构建以机器人在该机器人坐标系中的位姿、该变换矩阵初值和该单目尺度因子初值为节点的位姿图，并将各传感器测得的位姿变化作为该位姿图中的边约束；
位姿信息计算模块，用于在该位姿图内的节点数量超过预设的滑动窗口大小时，将该位姿图内最旧的位姿节点及其具有的边约束去掉，并将该边约束作为先验约束加入该位姿图，同时将最新的位姿节点及其边约束加入该位姿图，采用非线性最小二乘法对当前该位姿图进行迭代求解，生成外参数矩阵、单目尺度因子和机器人位姿信息，其中用该外参数矩阵对该变换矩阵初值进行更新，用该单目尺度因子对该单目尺度因子初值进行更新，并将该机器人位姿信息作为该机器人的定位结果进行输出。
7.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位***，其特征在于，该编码器根据该编码器数据和里程计模型，计算该机器人的位姿；该相机作为单目里程计，根据该单目视觉图像和视觉里程计算法，计算该相机的位姿。
8.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位***，其特征在于，该计算模块包括：根据该视觉位姿变化矩阵和该编码位姿变化矩阵，通过手眼标定求解法构建约束方程组，直接计算出该相机坐标系到该机器人坐标系的该变换矩阵初值和单目里程计的该尺度因子初值。
9.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位***，其特征在于，每个该定位传感器独立工作，并将每个该定位传感器输出的定位信息进行集合，作为该集编码器数据。
10.根据权利要求6所述的融合单目视觉和编码器数据的机器人定位***，其特征在于，该位姿信息计算模块包括：当位姿图中节点个数超过该滑动窗口时，需要将最旧的节点通过边际化进行移除。
CN201710574132.9A 2017-07-14 2017-07-14 融合单目视觉和编码器信息的机器人定位方法和*** Active CN107516326B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710574132.9A CN107516326B (zh) 2017-07-14 2017-07-14 融合单目视觉和编码器信息的机器人定位方法和***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710574132.9A CN107516326B (zh) 2017-07-14 2017-07-14 融合单目视觉和编码器信息的机器人定位方法和***

Publications (2)

Publication Number Publication Date
CN107516326A CN107516326A (zh) 2017-12-26
CN107516326B true CN107516326B (zh) 2020-04-03

Family

ID=60721872

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710574132.9A Active CN107516326B (zh) 2017-07-14 2017-07-14 融合单目视觉和编码器信息的机器人定位方法和***

Country Status (1)

Country Link
CN (1) CN107516326B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108534757B (zh) * 2017-12-25 2021-01-15 达闼科技（北京）有限公司 一种基于云端的视觉地图比例尺检测方法及装置
CN110967027B (zh) * 2018-09-30 2021-12-03 北京地平线信息技术有限公司 地图校正方法、装置和电子设备
CN109671120A (zh) * 2018-11-08 2019-04-23 南京华捷艾米软件科技有限公司 一种基于轮式编码器的单目slam初始化方法及***
CN109540140B (zh) * 2018-11-23 2021-08-10 宁波智能装备研究院有限公司 一种融合ssd目标识别和里程计信息的移动机器人定位方法
CN109579844B (zh) * 2018-12-04 2023-11-21 电子科技大学 定位方法及***
WO2021004416A1 (zh) * 2019-07-05 2021-01-14 杭州海康机器人技术有限公司 一种基于视觉信标建立信标地图的方法、装置
CN110827395B (zh) * 2019-09-09 2023-01-20 广东工业大学 一种适用于动态环境的即时定位与地图构建方法
CN110689513B (zh) * 2019-09-26 2022-09-02 石家庄铁道大学 彩色图像融合方法、装置和终端设备
CN111080699B (zh) * 2019-12-11 2023-10-20 中国科学院自动化研究所 基于深度学习的单目视觉里程计方法及***
CN111932637B (zh) * 2020-08-19 2022-12-13 武汉中海庭数据技术有限公司 一种车身相机外参自适应标定方法和装置
CN112269187B (zh) * 2020-09-28 2024-05-14 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN112700505B (zh) * 2020-12-31 2022-11-22 山东大学 一种基于双目三维跟踪的手眼标定方法、设备及存储介质
CN113066127B (zh) * 2021-04-02 2024-04-19 视辰信息科技(上海)有限公司 一种在线标定设备参数的视觉惯性里程计方法和***
CN113624226A (zh) * 2021-04-28 2021-11-09 上海有个机器人有限公司 一种平面运动约束方法、电子设备及存储介质
CN113494886A (zh) * 2021-08-05 2021-10-12 唐山市宝凯科技有限公司 一种基于视觉相机和旋转编码器的焦炉大车定位***及方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法
CN105629970A (zh) * 2014-11-03 2016-06-01 贵州亿丰升华科技机器人有限公司 一种基于超声波的机器人定位避障方法
CN106767833A (zh) * 2017-01-22 2017-05-31 电子科技大学 一种融合rgbd深度传感器与编码器的机器人定位方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法
CN105629970A (zh) * 2014-11-03 2016-06-01 贵州亿丰升华科技机器人有限公司 一种基于超声波的机器人定位避障方法
CN106767833A (zh) * 2017-01-22 2017-05-31 电子科技大学 一种融合rgbd深度传感器与编码器的机器人定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
An Iterative Kalman Smoother for Robust 3D Localization on Mobile and Wearable devices;Dimitrios G et.al;《2015 IEEE International Conference on Robotics and Automation (ICRA)》;20150330(第1期);第16-20页 *
ROBUST VISUAL-INERTIAL SLAM:COMBINATION OF EKF AND OPTIMIZATION METHOD;Meixiang Quan et.al;《arXiv:1706.03648v1 [cs.RO]》;20170712;第1-11页 *
基于单目视觉的自主寻迹智能车设计与实现;李亭 等;《重庆理工大学学报（自然科学）》;20161031;第30卷(第10期);第6336-6340页 *

Also Published As

Publication number Publication date
CN107516326A (zh) 2017-12-26

Similar Documents

Publication Publication Date Title
CN107516326B (zh) 融合单目视觉和编码器信息的机器人定位方法和***
CN107941217B (zh) 一种机器人定位方法、电子设备、存储介质、装置
Zhang et al. Pose estimation for ground robots: On manifold representation, integration, reparameterization, and optimization
CN110595466B (zh) 轻量级的基于深度学习的惯性辅助视觉里程计实现方法
CN111795686A (zh) 一种移动机器人定位与建图的方法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN112254729A (zh) 一种基于多传感器融合的移动机器人定位方法
CN116222543B (zh) 用于机器人环境感知的多传感器融合地图构建方法及***
CN111890373A (zh) 车载机械臂的感知定位方法
CN116929338B (zh) 地图构建方法、设备及存储介质
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114494629A (zh) 一种三维地图的构建方法、装置、设备及存储介质
CN116619358A (zh) 一种矿山自主探测机器人自适应定位优化与建图方法
CN117392241B (zh) 自动驾驶中的传感器标定方法、装置及电子设备
Juan-Rou et al. The implementation of imu/stereo vision slam system for mobile robot
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及***
CN113155152B (zh) 基于李群滤波的相机与惯性传感器空间关系自标定方法
CN110967017A (zh) 一种用于双移动机器人刚体协作搬运的协同定位方法
Sibley Sliding window filters for SLAM
CN117388830A (zh) 激光雷达与惯性导航的外参标定方法、装置、设备及介质
CN117073720A (zh) 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备
CN114046800B (zh) 一种基于双层滤波框架的高精度里程估计方法
Tian et al. Adaptive-frame-rate monocular vision and imu fusion for robust indoor positioning
CN114018284B (zh) 一种基于视觉的轮速里程计校正方法
WO2022179047A1 (zh) 一种状态信息估计方法及装置

Legal Events

Date Code Title Description
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