CN112184824A - 一种相机外参标定方法、装置 - Google Patents

一种相机外参标定方法、装置 Download PDF

Info

Publication number
CN112184824A
CN112184824A CN201910603598.6A CN201910603598A CN112184824A CN 112184824 A CN112184824 A CN 112184824A CN 201910603598 A CN201910603598 A CN 201910603598A CN 112184824 A CN112184824 A CN 112184824A
Authority
CN
China
Prior art keywords
visual beacon
camera
pose
coordinate system
image frame
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.)
Pending
Application number
CN201910603598.6A
Other languages
English (en)
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.)
Hangzhou Hikrobot Technology Co Ltd
Original Assignee
Hangzhou Hikrobot Technology 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 Hangzhou Hikrobot Technology Co Ltd filed Critical Hangzhou Hikrobot Technology Co Ltd
Priority to CN201910603598.6A priority Critical patent/CN112184824A/zh
Publication of CN112184824A publication Critical patent/CN112184824A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G06T7/85—Stereo camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30204—Marker

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Processing (AREA)
  • Image Analysis (AREA)

Abstract

公开一种相机外参的标定方法，获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；获取相机外参位姿初始化结果；基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果；对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得相机外参优化结果。本发明不需对视觉信标位姿或机器人本体位姿进行外部测量。

Description

一种相机外参标定方法、装置
技术领域
本发明涉及计算视觉领域，特别地，涉及一种相机外参标定方法。
背景技术
计算视觉的主要任务是通过对采集的图片或视频进行处理以获得相应场景的三维信息；相机作为获取场景图像信息的设备被应用。相机成像的过程实际是将真实的三维空间中的三维点映射到成像平面(二维空间)过程，由于在成像平面的像点和真实三维场景存在着一种对应关系，也就是图像帧中的二维像素点和真实三维世界的三维点存在某种变换关系，如果找到了这种变换关系，就可以利用图像帧中的二维像素点信息来恢复场景的三维信息。
按照相机的成像原理，图像帧中的二维像素点和真实三维世界的三维点之间的变换关系与相机的内部参数(简称内参)和外部参数(简称外参)有关，其中，内参包括，
射影变换本身的参数：相机的焦点到成像平面的距离，也就是相机的焦距f，以及相机透镜畸变；
从成像平面坐标系到像素坐标系的变换：由于在成像平面坐标系下，以成像平面的中心为原点，像点使用该坐标系下的坐标；而实际像素点以像素来描述，像素坐标原点通常是图像的左上角，X轴沿着水平方向向左，Y轴竖直向下，这样，像素坐标系和成像平面坐标系之间，相差了缩放和坐标原点的平移，由此带来的从成像平面坐标系到像素坐标系的变换。
相机的外参：在图像拾取的过程中，相机的位姿并不是固定不变的，这导致相机坐标系并不是一个稳定的坐标系，会随着相机的移动而改变相机坐标系的原点和各个坐标轴的方向，这就需要引进一个稳定不变坐标系：世界坐标系，该坐标系是绝对坐标系。从相机坐标系到世界坐标系的变换需要旋转变换和平移变换，这些旋转变换和平移变换就成为了相机的外参。
基于上述成像原理，为确定真实三维场景的三维点与其在图像中成像点之间的变换关系，建立了相机成像的几何模型，这些几何模型参数对应着相机参数。在大多数条件下这些参数必须通过实验与计算才能得到，这个求解参数的过程称为相机标定(或摄像机标定)。其中，外参的标定实际是计算相机和移动机器人本体的相对位姿。
无论是在图像测量或者机器视觉应用中，相机参数的标定都是非常关键的环节，其标定结果的精度及算法的稳定性直接影响相机工作产生结果的准确性。因此，做好相机标定是做好后续工作的前提，提高标定精度是科研工作的重点所在。
现有的外参标定方法通常采用外部测量设备(如激光跟踪仪等)对机器人以及固定在环境中的视觉信标的全局位姿进行测量，再利用相机对视觉信标的局部观测，计算相机和机器人本体的相对位姿。该方法不仅高度依赖于外部测量设备，还依赖在环境中的视觉信标的全局位姿的测量，标定过程繁琐复杂。
发明内容
本发明提供了一种单目相机外参的标定方法，以减少标定过程中对外部测量设备的依赖。
本发明提供的一种相机外参的标定方法是这样实现的：
获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；
获取相机外参位姿初始化结果；
基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果；
对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得相机外参优化结果；
其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述相机外参位姿的初始值为所述相机外参位姿初始化结果，所述视觉信标全局位姿的初始值为所述各个视觉信标全局位姿初始化结果。
较佳地，所述基于视觉信标检测数据，获取视觉信标全局位姿初始化结果之后，进一步包括，
获取图像帧中视觉信标的局部位置测量约束残差，
对所述图像帧的里程计约束残差、以及所述局部位置测量约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、所述相机外参位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，获得图像帧机器人本体全局位姿的初步优化结果、相机外参位姿的初步优化结果、和视觉信标全局位置的初步优化结果；其中，所述视觉信标全局位置的初始值为所述视觉信标全局位姿初始化结果所包括的视觉信标全局位置初始化结果；
将所述图像帧机器人本体全局位姿的初步优化结果作为所述图像帧机器人本体全局位姿初始化结果；
将所述相机外参位姿的初步优化结果作为所述相机外参位姿初始化结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括，
基于视觉信标首次被观测的图像帧相机初始位姿、及视觉信标检测数据，获取视觉信标全局姿态初始化结果；
将所述视觉信标全局位置的初步优化结果、以及视觉信标全局姿态初始化结果作为所述视觉信标全局位姿初始化结果。
较佳地，进行以所述图像帧机器人本体全局位姿为状态变量、所述相机外参位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述视觉信标的局部位置测量约束残差的第一目标函数，通过以各个所述各个图像帧机器人本体全局位姿初始化结果、相机外参位姿初始化结果、和各个所述信标全局位置初始化结果为迭代初始值的迭代，获取使得所述第一目标函数为最小值时的各个图像帧机器人本体全局位姿的初步优化结果、相机外参位姿初步优化结果、和各个视觉信标全局位置的初步优化结果。
较佳地，所进行以所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述重投影约束残差的第二目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、相机外参位姿初始化结果、和各个所述视觉信标全局位姿初始化结果为迭代初始值的迭代，获取使得所述第二目标函数为最小值时的相机外参位姿优化结果。
较佳地，所述基于所述里程计测量数据获取的所有图像帧的里程计约束残差，包括：
对任一所述图像帧，
基于所述里程计测量数据，获取所述图像帧与其相邻图像帧之间的机器人本体相对位姿、和所述图像帧与其相邻图像帧之间的里程计相对位姿；
由所述机器人本体相对位姿和里程计相对位姿的差异，计算所述图像帧的里程计约束残差，
累计各个所述图像帧的里程计约束残差，得到所有所述图像帧的里程计约束残差。
较佳地，所述获取图像帧中视觉信标的局部位置测量约束残差，包括，
对包括视觉信标图像的任一图像帧中的任一视觉信标，获取该视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置与该视觉信标被观测测量值之间的差异，得到该视觉信标的局部位置测量约束残差，
其中，
所述视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置通过相机外参位姿当前状态变量、图像帧机器人本体全局位姿当前状态变量、以及视觉信标的全局位置当前状态变量获得；所述图像帧机器人本体全局位姿当前状态变量在第一次迭代时为图像帧机器人本体全局位姿初始化结果；所述视觉信标的全局位置当前状态变量在第一次迭代时为视觉信标的全局位置初始化结果；相机外参位姿当前状态变量在第一次迭代时为相机外参位姿初始化结果；
所述视觉信标被观测测量值通过该视觉信标相对于相机的局部位姿中的局部位置获得；
累计各个图像帧中的各个视觉信标的局部位置测量约束残差，得到所有图像帧中所有视觉信标的局部位置测量约束残差。
较佳地，所述基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上的重投影约束残差，包括：
根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差。
较佳地，所述根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差，包括，
对于任一图像帧中的任一视觉信标所具有的任一特征点，根据相机内参矩阵、图像帧机器人本体全局位姿当前状态变量、该视觉信标的全局位姿当前状态变量、以及该特征点在该视觉信标坐标系下的局部位置的投影函数与该特征点的图像坐标的测量值之差异，获得该视觉信标中该特征点在其图像帧上重投影误差，
累计各个图像帧中的各个视觉信标的各个特征点的重投影误差，得到所有图像帧中所有视觉信标的所有特征点在图像帧上的重投影约束残差。
较佳地，所述基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果包括，
将各个所述图像帧机器人本体全局位姿的初始化结果直接采用对应图像帧的里程计测量数据结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括：
对于图像帧中任一视觉信标，通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，
其中，所述视觉信标检测数据包括视觉信标被观测测量值，从视觉信标相对相机的局部位姿获得；
所述视觉信标相对于相机的局部位姿根据视觉信标中特征点的图像坐标、相机内参、以及信标参数获得。
较佳地，所述通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，包括：
根据图像帧机器人本体全局位姿向量、相机外参位姿初始化向量、以及视觉信标相对相机的局部位置向量，获得所述视觉信标全局位置初始化结果；
根据图像帧机器人本体坐标系的全局旋转向量、相机外参初始化旋转向量、视觉信标在相机坐标系中的局部旋转向量，获得所述视觉信标全局姿态初始化结果，
其中，
所述图像帧机器人本体坐标系的全局旋转向量从所述图像帧机器人本体全局位姿初始化结果获得，
所述相机外参初始化旋转向量从所述相机外参位姿初始化结果获得，
所述视觉信标在相机坐标系中的局部旋转向量从所述视觉信标相对相机的局部位姿获得。
较佳地，所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧被采集时刻机器人本体的里程计测量数据包括，对每个视觉信标进行一次以上的观测，在采集各个视觉信标检测数据的同一时刻记录里程计测量数据；
所述获取相机外参位姿初始化结果包括，基于所述各个视觉信标检测数据和里程计测量数据，获取相机外参位姿初始化结果。
较佳地，所述基于所述各个视觉信标检测数据和里程计测量数据，获取相机外参位姿初始化结果包括，
根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，基于所述各个视觉信标检测数据中各个视觉信标的局部位置，获得地平面法向量；
通过地平面法向量，获得相机坐标系与相机投影坐标系之间的投影旋转向量；其中，相机投影坐标系为相机坐标系原点在地平面上的投影，相机投影坐标系的z轴与移动机器人本体坐标系的z轴平行且方向相同；
基于同一视觉信标均被观测到的连续两图像帧，根据该视觉信标所在连续两图像帧的里程计测量数据和视觉信标检测数据，构建同一视觉信标在连续两图像帧之间的约束，通过该约束依次获得移动机器人本体坐标系与相机投影坐标系之间的偏航角、移动机器人本体坐标系与相机投影坐标系之间的平移向量；
根据所述偏航角、投影旋转向量，获得相机外参的旋转向量初始化结果，根据所述平移向量，获得相机外参位置向量的初始化结果。
较佳地，所述各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束包括：各个辅助平面在相机坐标系下的齐次向量与各个视觉信标相对于相机坐标系的局部位置的齐次向量的乘积为零；
所述根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，基于所述各个视觉信标检测数据中各个视觉信标的局部位置，获得地平面法向量，包括，
根据所述各个辅助平面在相机坐标系下的齐次向量与所述各个辅助平面所共有的地平面法向量、和各个视觉信标距离地面的高度相关的标量所构成的等同关系，构建以所述地平面法向量、以及各个视觉信标距离地面的高度相关的标量为元素的第一矩阵；该第一矩阵与所述各个辅助平面在相机坐标系下的齐次向量对应；
构建包括有视觉信标局部位置信息且具有i行、j列第二矩阵，所述第二矩阵的任一行向量i的元素包括任一图像帧i中任一视觉信标j相对于相机坐标系的局部位置向量，任一行向量i中非零元位置为j+3；
根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，构建所述第二矩阵与第一矩阵的乘积为零的关系式；
对第二矩阵进行分解，获得地平面法向量；
其中，i＝1，2…N，j＝1，2…M，N为图像帧的数量，M为N个图像帧中所有信标累加的总数。
较佳地，所述基于同一视觉信标均被观测到的连续两图像帧，根据该视觉信标所在连续两图像帧的里程计测量数据和检测数据，构建同一视觉信标在连续两图像帧之间的约束，包括，
根据第一图像帧中的视觉信标j在移动机器人本体坐标系下的位置向量，等于：在移动机器人本体坐标系下，所述第一图像帧连续的第二图像帧与所述第一图像帧之间的相对位姿向量，与从移动机器人本体坐标系到相机投影坐标系变换的第二图像帧中的同一视觉信标j在相机投影坐标系下的位置向量的投影运算的乘积，得到第一关系式，
基于所述第一关系式的展开，得到所述同一视觉信标在连续两图像帧之间的约束，该约束中包括：不考虑相机与移动机器人本体的高度差时，移动机器人本体坐标系相对相机投影坐标系的二维旋转矩阵、和二维平移向量；以及，所述第一图像帧和第二图像帧的里程计测量结果，所述视觉信标在所述第一图像帧、和第二图像帧的相机投影坐标系下的局部位置；该局部位置基于该信标在所述第一图像帧和第二图像帧的相机坐标系下的局部位置，通过所述投影旋转向量获得；
所述通过该约束依次获得移动机器人本体坐标系与相机投影坐标系之间的偏航角、移动机器人本体坐标系与相机投影坐标系之间的平移向量，包括，
按照两图像帧之间移动机器人的平移运动，将所述第一图像帧和第二图像帧的里程计测量结果中的旋转矩阵近似为单位阵，以消除所述同一视觉信标在连续两图像帧之间的约束中的平移向量，得到不包括平移向量的约束，基于该约束，构建以移动机器人本体坐标系与相机投影坐标系之间的偏航角为未知量的线性方程组，通过求解该线性方程组，获得所述偏航角；
基于所述同一视觉信标在连续两图像帧之间的约束和所述偏行角，求解所述同一视觉信标在连续两图像帧之间的约束中的平移向量。
较佳地，所述根据所述偏航角、投影旋转向量，获得相机外参的旋转向量初始化结果，根据所述平移向量，获得相机外参位置向量的初始化结果，包括，
根据所述偏行角，得到移动机器人本体坐标系和相机投影坐标系之间的旋转向量，将该旋转向量与投影旋转向量相乘，得到移动机器人本体坐标系相对于相机坐标系的旋转向量；
设置相机的高度为0，基于所述平移向量，得到移动机器人本体坐标系相对于相机坐标系的位置向量；
将所述移动机器人本体坐标系相对于相机坐标系的旋转向量、以及位置向量作为相机外参位姿初始化结果。
较佳地，所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少包括所述视觉信标图像帧采样时刻的里程计测量数据之后，进一步包括，按照如下任一条件或其组合筛选出关键帧，从所述图像帧中至少筛选出关键帧：
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧作为关键帧；
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧作为为关键帧；
第一帧为关键帧；
所述图像帧为筛选出的关键帧。
较佳地，该方法还包括，
通过所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得各个视觉信标全局位姿优化结果，并基于该优化结果，构建信标地图。
本发明还提供一种相机外参的标定装置，包括
数据采集模块，获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
初始化模块，基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；基于视觉信标检测数据，获取视觉信标全局位姿初始化结果；获取相机外参位姿初始化结果；
里程计约束与视觉信标特征点图像重投影约束联合优化模块，对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得相机外参位姿优化结果；
其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述相机外参位姿的初始值为所述相机外参位姿初始化结果，所述视觉信标全局位姿的初始值为所述视觉信标全局位姿初始化结果。
本发明还提供一种相机外参标定的电子设备，其特征在于，该电子设备包括存储器和处理器，其中，
所述存储器用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序，实现上述任一所述相机外参的标定方法。
本发明还提供一种计算机可读存储介质，其特征在于，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行上述任一所述相机外参的标定方法。
本发明根据视觉信标检测数据和里程计测量数据，计算相机与机器人本体的相对位姿，即相机外参；视觉信标布置灵活，且不需对视觉信标位姿或机器人本体位姿进行外部测量，也不依赖任何其他定位方法/设备，因而大幅简化了标定流程，提高了标定的效率；通过构建和求解包含相机外参、关键帧位姿、信标位姿的优化问题，计算获得更加精确的外参标定结果；进一步地，本发明包含外参自动初始化步骤，可以直接通过视觉信标检测数据和里程计测量数据，计算相机外参初值，不需提供相机外参初值。
附图说明
图1为相机坐标系、相机投影坐标系、机器人本体坐标系之间的关联关系的一种示意图。
图2为本发明实施例相机外参标定方法的一种流程图。
图3为数据采集的一种示意图。
图4示出了步骤201-210所涉及的图像数据的关联关系的一种示意图。
图5为步骤204所述基于里程计测量及视觉信标测量中的局部位置进行相机外参初始化的一种流程示意图。
图6示出了同一视觉信标在连续两关键帧时各坐标系下关联关系的一种示意图。
图7示出了步骤2041-2044所涉及的图像数据的关联关系的一种示意图。
图8为本发明实施例基于视觉信标检测数据和里程计测量数据进行相机外参标定的一种装置示意图。
具体实施方式
为了使本申请的目的、技术手段和优点更加清楚明白，以下结合附图对本申请做进一步详细说明。
本发明利用视觉信标检测数据和里程计测量数据，以相机外参(相机与机器人本体的相对位姿)、关键帧机器人本体全局位姿、视觉信标的全局位姿为状态变量，通过分别构建里程计约束与视觉信标局部位置测量约束的第一目标函数、以及里程计约束与视觉信标特征点图像重投影约束的第二目标函数，并通过对第一目标函数和第二目标函数的求解联合优化问题，来获得相机外参，进一步地，还可获得视觉信标的全局位姿，并可基于视觉信标的全局位姿构建信标地图。通过对原始数据进行筛选，获取关键帧数据，并针对关键帧进行优化计算，大幅提升了优化算法的计算效率。
为便于理解本发明实施例，对涉及的概念说明如下。
在本实施例中，涉及世界坐标系、摄像机坐标系(相机坐标系)、信标坐标系、里程计坐标系、机器人本体坐标系、相机投影坐标系，其中，
世界坐标系是系统的绝对坐标系，在环境中选择一个基准坐标系来描述摄像机和环境中任何物体的位置；在本发明实施例中，xy轴所在平面垂直与地平面平行。
摄像机坐标系是原点为摄像机光心，x轴与y轴与图像的X,Y轴平行，z轴为摄像机光轴，它与成像平面垂直，以此构成的空间直角坐标系，也称为相机坐标系，摄像机坐标系是三维坐标系。其中，光轴与图像平面的交点，即为图像坐标系的原点，与图像的X、Y轴构成的直角坐标系即为图像坐标系，图像坐标系是二维坐标系。
信标坐标系用于描述视觉信标中特征点的三维空间位置，例如，二维码中位于四个角上的定位特征点的位置。各个视觉信标分别具有各自信标坐标系。
移动机器人本体坐标系，与移动机器人中心重合，为机器人的本体的坐标系，以机器人前进的方向平行的方向为x轴，与x轴在同一平面、与x轴垂直向左的方向为y轴、与x、y轴所在平面垂直向上的方向为z轴，其中，x、y轴所在平面与地平面平行。
相机投影坐标系为本申请实施例中所定义的三维空间坐标系，坐标原点为相机坐标系原点在地平面的投影，z轴与机器人本体坐标系的z轴平行，且方向相同；x、y轴所在平面与z轴垂直即可。参见图1所示，图1示出了相机坐标系、相机投影坐标系、机器人本体坐标系之间的关联关系。
里程计坐标系，属于世界固定坐标系，用于描述机器人本体(机器人坐标系)在二维平面内位姿的坐标系，原点为开始计算位姿那个时刻的机器人的位置，坐标系中的数据来源于里程计。
里程计,一种针对移动机器人的累加式定位算法，根据所采用的传感器及机械结构的不同，可以有多种实现方式，其输出结果为机器人本体(机器人本体坐标系)在里程计坐标系二维平面内的位姿。
PnP(Perspective-n-Point)，又称为n点透视，通过图像中一组特征点(n个点)的坐标以及该特征点的三维位置，计算相机坐标系的位姿。其中，n至少为不相同的4个特征点。
在以下文中，“局部”是与“全局”相对的概念，全局系指在世界坐标系而言，局部系指在非世界坐标系而言。
参见图2所示，图2为本发明实施例相机外参标定方法的一种流程图。该建图方法包括，
步骤201，获取视觉信标检测数据和里程计测量数据。
参见图3所示，图3为数据采集的一种示意图。控制机器人移动，以使得机器人通过安装于机器人本体的一个相机获得视觉信标图像，以获得至少包括视觉信标图像帧的视觉信标检测数据，并同时利用轮式编码器等方式采集里程计测量数据，该里程计测量数据至少包括一个图像帧采集时刻机器人本体测量数据。所述机器人移动轨迹没有限制，可以自由移动，尽量确保对每个信标的多次观测、以及机器人运动轨迹的多样性，例如，直线移动、弧线移动、原地旋转等；如图3中，共布置了4个视觉信标，机器人移动轨迹为里程计轨迹。所述视觉信标可以采用棋盘格式或二维码形式，视觉信标相互之间能够区分，标定环境所在地面平整。
这样，数据采集过程中所采集的原始数据分别包括里程计测量数据和视觉信标检测数据，其中，里程计测量数据为各帧对应时刻下机器人本体坐标系在里程计坐标系(二维平面内)下的位姿；视觉信标检测数据包括采集的图像帧、信标ID、以及信标各个特征点的图像坐标。
此外，还可预先获取系统参数，包括：相机内参(光心位置、镜头焦距、畸变系数等)、信标参数(各个特征点在信标坐标系中的局部坐标位置)。
在视觉信标图像数据的采集的过程中，通常还会采集到无视觉信标图像的图像帧以及该图像帧的里程计测量数据，这些数据也可以分别作为视觉信标检测数据和里程计测量数据，形成采集的原始数据。
步骤202，考虑到计算效率，需要对原始数据进行筛选，以便从大量的原始帧中筛选出关键帧。故而，基于所采集的原始数据，按照关键帧筛选策略，从原始数据中筛选出一定数量的关键帧。其中，关键帧筛选策略可以有多种，本实施例中的一种策略是：
当前帧中存在信标图像时，必须选择该帧为关键帧；和/或
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧添加为关键帧，即，使得相邻关键帧的里程计平移偏差大于设定的平移阈值；和/或
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧添加为关键帧，即，使得相邻关键帧的里程计旋转偏差大于设定的旋转阈值；和/或
第一帧为关键帧。
这样，所筛选的关键帧包括了具有信标图像的关键帧和不具有信标图像的关键帧。
上述当前帧里程计测量是指当前帧被采样时刻所对应的机器人本体在里程计坐标系下的位姿，关键帧里程计测量是指关键帧被采样时刻所对应的机器人本体在里程计坐标系下的位姿。
步骤203，根据包括有信标图像的关键帧、以及步骤201所获得的该信标图像的信标参数，获得该信标与该关键帧相机的相对位姿，即，获得信标相对于相机的局部位姿，并将该局部位姿添加至信标检测数据中，作为视觉信标被观测测量值。
在该步骤中，信标关于相机的局部位姿的解算，实质是求解PnP(Perspective-n-Point)问题，即，根据信标特征点的图像坐标、相机内参、信标参数，获得信标关于相机坐标系的局部位姿。考虑到求解PnP问题对特征点数量的要求，本发明中至少包含4个特征点。
步骤204，获取相机外参位姿初始值。
如果相机外参初始值能够被直接提供，则可直接使用所提供的相机外参初始值；否则，可基于里程计测量及视觉信标测量中的局部位置，利用机器人平面运动约束，通过分步估计相机坐标系下的地平面齐次向量、相机投影坐标系和机器人本体坐标系的相对偏航角、相机投影坐标系和机器人本体坐标系的二维相对位置偏差，获得相机外参位姿初始值(即相机外参位姿初始化结果)。
步骤205，对关键帧机器人本体全局位姿初始化、及视觉信标全局位置坐标初始化；其中，关键帧机器人本体全局位姿系关键帧被采样时刻所对应的机器人本体在世界坐标系下的位姿，
关键帧机器人本体全局位姿的初始化直接采用里程计测量数据结果，该结果为机器人本体在里程计二维坐标系平面内的位姿。视觉信标全局位置的初始化通过该信标首次被观测时的关键帧相机初始位姿、及观测结果计算获得，其中，关键帧相机位姿系指相机获取该帧时的相机位姿，信标首次被观测时所对应的关键帧相机位姿为关键帧相机初始位姿；
具体计算公式可以如下：
Figure BDA0002119986170000121
其中，符号左侧上下角标为坐标系标识，
Figure BDA0002119986170000122
为视觉信标全局位置向量(在信标坐标系m中的信标相对世界坐标系w的坐标位置)，
Figure BDA0002119986170000123
为关键帧机器人本体全局位姿向量(在机器人坐标系b中关键帧被采样时刻所对应的机器人本体相对世界坐标系w的位姿)，可表示为
Figure BDA0002119986170000131
Figure BDA0002119986170000132
为机器人本体坐标系的全局旋转向量，
Figure BDA0002119986170000133
为机器人本体坐标系的全局位置向量，该
Figure BDA0002119986170000134
通过将机器人本体全局位姿向量
Figure BDA0002119986170000135
进行从二维空间到三维空间的转换而得到，其中，机器人本体全局位姿向量可表示为
Figure BDA0002119986170000136
Figure BDA0002119986170000137
为机器人本体偏航角；所述关键帧机器人本体全局位姿向量得初始值以里程计测量数据结果为初始值，
Figure BDA0002119986170000138
为相机外参位姿向量(在相机坐标系c中相机相对机器人坐标系b的位姿)，为步骤204获得的相机外参初始值，可表示为
Figure BDA0002119986170000139
Figure BDA00021199861700001310
为相机外参的旋转向量，
Figure BDA00021199861700001311
为相机外参的位置向量；
Figure BDA00021199861700001312
为视觉信标局部位置向量(在信标坐标系m中的信标相对相机坐标系c的坐标位置，即，被观测的测量值)，由步骤103获得，可表示为
Figure BDA00021199861700001313
符号⊕表示右乘，有例如，
Figure BDA00021199861700001314
以及
Figure BDA00021199861700001315
符号⊙表示投影，有例如，
Figure BDA00021199861700001316
由
Figure BDA00021199861700001317
得到视觉信标首次被观测时的关键帧相机初始位姿。
步骤206，基于关键帧机器人本体全局位姿初始化、相机外参初始化、及信标全局位置坐标初始化结果，分别计算里程计约束残差、视觉信标局部位置测量约束残差，在本申请中，视觉信标局部位置测量约束残差简称为M约束残差；其中，对于任一关键帧i的里程计约束残差，由相邻关键帧机器人本体相对位姿与该相邻关键帧里程计相对位姿计算。实施方式之一，计算相邻关键帧机器人本体相对位姿与该相邻关键帧里程计相对位姿之间的差异，例如为两者之间的差值，可表示为：
Figure BDA00021199861700001318
其中，eo,<i>表示关键帧i的里程计残差项，
Figure BDA00021199861700001319
表示与关键帧i相邻的关键帧i+1机器人本体全局位姿向量，
Figure BDA00021199861700001320
表示关键帧i+1的里程计位姿向量(在机器人坐标系b相对里程计坐标系o的位姿)，从
Figure BDA00021199861700001321
得到相邻关键帧机器人本体相对位姿，从
Figure BDA0002119986170000141
得到相邻关键帧里程计相对位姿。
对于任一关键帧i中任一视觉信标j，视觉信标局部位置测量约束残差为视觉信标在其所在关键帧相机坐标系下的局部位置与该信标的被观测测量值之间的差异，实施方式之一可以是，视觉信标在其所在关键帧相机坐标系下的局部位置与该信标的被观测测量值之间的差值，可表示为：
Figure BDA0002119986170000142
其中，em,<i,j>表示关键帧i中视觉信标j的局部位置测量约束残差，
Figure BDA0002119986170000143
为视觉信标j全局位置向量，其初始值由步骤205所获得，
Figure BDA0002119986170000144
表示关键帧i中视觉信标j的局部位置向量，由
Figure BDA0002119986170000145
得到视觉信标j在其所在关键帧i相机坐标系下的局部位置。
步骤207，对筛选出的所有关键帧的里程计约束残差、以及所有关键帧中的所有视觉信标的局部位置测量约束残差进行联合优化，以生成关键帧机器人本体全局位姿的初步优化结果、视觉信标全局位置的初步优化结果、和相机外参的优化结果，为后续的信标特征点图像重投影约束的联合优化提供初始值；
在该步骤中，根据筛选出的所有关键帧的里程计约束残差、以及所有视觉信标局部位置测量约束残差，构建融合上述约束残差的第一目标函数，采用针对非线性优化问题的迭代式优化算法，例如，LM(Levenberg-Marquardt)算法，通过对以关键帧机器人本体全局位姿初始化结果、相机外参初始化结果和视觉信标全局位置初始化结果为初始值进行多次迭代收敛，在迭代过程中以关键帧机器人本体全局位姿、相机外参、和视觉信标全局位置为状态变量，求解使得第一目标函数获得最小值时的关键帧机器人本体全局位姿、相机外参、和视觉信标全局位置。其中，第一目标函数可表示为：
Figure BDA0002119986170000146
其中，
Figure BDA0002119986170000147
其中,
Figure BDA0002119986170000148
xp为系统参数，至少包含相机外参
Figure BDA0002119986170000151
还可以根据具体需求进行扩充，加入时延、里程计内参、相机内参等其他参数，xk为关键帧全局位姿，xm为信标全局位姿，N为所有关键帧，M为所有关键帧中的所有信标数；
Ωo,<i>为关键帧i的里程计约束信息矩阵，Ωm,<i,j>为关键帧i中视觉信标j的局部位置测量约束信息矩阵，信息矩阵的取值与系统误差模型相关，通常由人工给定。
Figure BDA0002119986170000152
表示累计筛选出的所有关键帧N的里程计约束残差，
Figure BDA0002119986170000153
表示累计集合
Figure BDA0002119986170000154
中所有视觉信标j局部位置测量约束残差，所述集合
Figure BDA0002119986170000155
由具有任一视觉信标j的关键帧i构成。
在优化过程中，第一次迭代时，关键帧机器人本体全局位姿状态变量初始值为步骤205获取的关键帧机器人本体全局位姿初始化结果，视觉信标全局位置状态变量初始值为步骤205获取的视觉信标全局位置初始化结果，相机外参状态变量初始值位步骤204获取的外参初始化结果；在第一次之后的迭代过程中，则将上一次迭代所求得的状态变量作为当前状态变量，以进行当前迭代时所优化的各个状态变量的求解。
通过步骤207的联合优化，能够使得关键帧机器人本体全局位姿和视觉信标局部位置具有更好的鲁棒性，更加不容易在迭代过程中陷入局部极小值，有利于提高后续的信标特征点图像重投影约束的联合优化的效率。
步骤208，基于具有视觉信标图像的关键帧，对视觉信标的全局姿态(角度)进行初始化；
视觉信标的全局姿态(角度)初始化根据该视觉信标首次被观测的关键帧相机初始位姿、相机外参旋转向量初始化结果、及对应信标检测结果计算获得，可以表示为：
Figure BDA0002119986170000156
其中，
Figure BDA0002119986170000157
为信标全局旋转向量(信标坐标系m相对世界坐标系w的旋转角度)，
Figure BDA0002119986170000158
为机器人本体坐标系的全局旋转向量(机器人本体坐标系b相对世界坐标系w的旋转角度)，
Figure BDA0002119986170000159
为相机外参旋转向量(相机坐标系c相对机器人本体坐标系b的旋转角度)，其初始值从步骤204获得，
Figure BDA00021199861700001510
为信标在相机坐标系中的局部旋转向量(信标坐标系m相对相机坐标系c的旋转角度)，在步骤203中从所获得的信标与该关键帧相机的相对位姿中获得。
步骤209，基于视觉信标的全局姿态初始化结果，计算视觉信标中的特征点在关键帧上的重投影约束残差，即，视觉信标特征点在关键帧相机图像上的重投影误差，在本申请中简称为V约束，可以表示为：
Figure BDA0002119986170000161
其中，e,<i,j,k>为关键帧i中信标j所具有的特征点k在该关键帧上的重投影约束残差，proj(.)为图像投影函数，A为相机内参矩阵，
Figure BDA0002119986170000162
特征点k在视觉信标j下的局部位置向量(在图像坐标系f下的特征点k相对于视觉信标系m中信标j的坐标位置，即信标参数)，
Figure BDA0002119986170000163
为特征点k在相机图像下的局部位置向量(图像坐标系f下的特征点k相对于相机坐标系c下关键帧i的图像坐标位置，即特征点k的图像坐标测量值)，
Figure BDA0002119986170000164
为信标j的全局位姿初始值，由步骤107中的视觉信标的全局姿态(角度)初始化结果和步骤207得到的视觉信标全局位置的初步优化结果获得。
步骤210，对筛选出的所有关键帧的里程计约束残差、以及所有信标中的所有特征点在关键帧上的重投影约束残差进行联合优化，构建融合前述两者约束残差的第二目标函数；
将步骤207获得的关键帧机器人本体全局位姿的初步优化结果作为关键帧机器人本体全局位姿迭代的初始值，将步骤207获得的视觉信标全局位置的初步优化结果和步骤208获得的视觉信标全局姿态初始化结果作为视觉信标全局位姿的迭代初始值，将步骤204获得的相机外参初始化结果作为相机外参初始值，采用针对非线性优化问题的迭代式优化算法，例如，LM(Levenberg-Marquardt)算法，通过关键帧机器人本体全局位姿初始值、相机外参位姿初始值、和视觉信标全局位姿初始值进行多次迭代收敛，在迭代过程中，以关键帧机器人本体全局位姿、相机外参、和视觉信标全局位姿为状态变量，求解使得目标函数获得最小值时的关键帧机器人本体全局位姿、相机外参和视觉信标全局位姿。其中，第二目标函数可表示为：
Figure BDA0002119986170000165
其中，
Figure BDA0002119986170000171
其中
Figure BDA0002119986170000172
里程计约束残差项与步骤106中的里程计约束残差一致，ev,<i,j,k>为关键帧i中信标j所具有的特征点k在关键帧上的重投影约束残差，Ωv,<i,j,k>为V约束残差信息矩阵，
Figure BDA0002119986170000173
表示累计所有信标j中的所有特征点k的V约束残差。
在优化过程中，第一次迭代时，关键帧机器人本体全局位姿状态变量初始值为步骤205获取的关键帧机器人本体全局位姿初始化结果，视觉信标全局位置状态变量初始值为步骤205获取的视觉信标全局位置初始化结果，相机外参状态变量初始值由步骤204获得；在第一次之后的迭代过程中，则将上一次迭代所求得的状态变量作为当前状态变量，以进行各个当前迭代时所优化的状态变量的求解。
进一步地，还可以基于所获得的视觉信标全局位姿，构建信标地图。
在上述步骤中，采用分步式的联合优化策略，先采用M约束联合优化估计信标全局位置和相机外参，再执行V约束联合优化估计信标的全局位姿，降低了优化算法陷入局部极小点的概率。当然，里程计约束残差与M约束残差的联合优化可以不执行，这样，第二目标函数的进行迭代收敛的初始值可以采用步骤205得到的关键帧机器人本体全局位姿初始化结果、以及视觉信标全局位姿初始化结果，其中，视觉信标全局位置坐标初始化结果由步骤205获得，视觉信标全局姿态初始化结果由步骤208获得。
参见图4所示，图4示出了以上述步骤201-210所涉及的图像数据的关联关系的一种示意图。图中：
采集的原始数据(图像帧)经过筛选，得到至少包括视觉信标的关键帧；
基于包括有视觉信标图像的关键帧，根据相机内参矩阵、信标参数，特征点在关键帧的图像坐标位置，得到各视觉信标相对相机的局部位姿，包括视觉信标相对相机的局部位置
Figure BDA0002119986170000174
视觉信标相对相机的局部姿态
Figure BDA0002119986170000175
即，视觉信标被观测测量值；
基于里程计测量数据，将里程计测量数据结果作为关键帧机器人本体全局位姿初始化结果，包括关键帧机器人本体全局位姿坐标初始化结果
Figure BDA0002119986170000176
关键帧机器人本体全局角度初始化结果
Figure BDA0002119986170000177
从相机外参获得相机外参位姿
Figure BDA0002119986170000178
以及相机外参姿态
Figure BDA0002119986170000179
根据所述
Figure BDA00021199861700001710
(椭圆虚线中)，获得各个视觉信标全局位置初始化结果
Figure BDA0002119986170000181
通过所述
Figure BDA0002119986170000182
获得各视觉信标在其所在关键帧相机坐标系下的局部位置，根据该局部位置与被观测测量值之间的差值，得到各视觉信标的M约束残差，对所有关键帧里程计约束残差和所有关键帧中视觉信标的约束残差，进行以以所述各个关键帧机器人本体全局位姿初始化结果、及各个视觉信标全局位置初始化结果为初始值(图中未示出)的联合优化，获得所有关键帧机器人本体全局位姿初步优化结果和所有关键帧中所有视觉信标全局位置初步优化结果；其中，所述所有关键帧里程计约束残差基于里程计测量数据获得；
通过视觉信标的全局位姿初始化结果(包括视觉信标全局姿态初始化结果、视觉信标全局位置初始化结果)、相机内参矩阵、信标参数、特征点在相机图像中的局部位置，计算所有关键桢中各个视觉信标的各个特征点的V约束残差，对所有关键帧里程计约束残差和所有关键桢中所有视觉信标的所有特征点的V约束残差，进行以所述各个关键帧机器人本体全局位姿初始化结果、和所述各个视觉信标全局位姿初始化结果为初始值的联合优化，获得所有关键帧机器人本体全局位姿优化结果和所有关键帧中所有视觉信标全局位姿优化结果。
参见图5所示，图5为步骤204所述基于里程计测量及视觉信标测量中的局部位置进行相机外参初始化的一种流程示意图。
步骤2041，基于视觉信标检测数据中的各个视觉信标在相机坐标系下局部位置信息，将地平面在相机坐标系中的齐次向量的估计转换为获取地平面法向量的估计值。
在该步骤中，鉴于移动机器人的平面运动约束，即，移动机器人在理想平面内运动，移动机器人本体坐标系原点始终位于地平面内，且移动机器人本体坐标系z轴与地平面垂直，则获得平行于地平面的各个辅助平面经过各个信标坐标系原点的线性约束，即，各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，可以表示为：各个辅助平面在相机坐标系下的齐次向量与各个视觉信标相对于相机坐标系的局部位置的齐次向量的乘积为零；用数学式表达为：
Figure BDA0002119986170000183
其中，四维向量
Figure BDA0002119986170000184
为关键帧i中的信标j在相机坐标系下局部位置的齐次向量；坐标系{ci}表示序号为i的关键帧相机坐标系，{mj}表示序号为j的信标坐标系，四维向量
Figure BDA0002119986170000185
为辅助平面在相机坐标系下的齐次向量。
辅助平面{gj}平行于地平面且经过信标{mj}坐标原点，各个辅助平面具有相同的地平面法向量，因此
Figure BDA0002119986170000191
也可以表达为如下形式，
Figure BDA0002119986170000192
其中三维向量
Figure BDA0002119986170000193
为相机坐标系下的地平面法向量，标量
Figure BDA0002119986170000194
与经过信标j的辅助平面距地面高度相关。
根据上述平面运动约束，可建立线性方程组A·x＝0，即：根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，构建所述第二矩阵与第一矩阵的乘积为零的关系式；
其中，
根据所述各个辅助平面在相机坐标系下的齐次向量与所述各个辅助平面所共有的地平面法向量、和各个视觉信标距离地面的高度相关的标量所构成的等同关系，构建以所述地平面法向量、以及各个视觉信标距离地面的高度相关的标量为元素的第一矩阵x；该第一矩阵x与所述各个辅助平面在相机坐标系下的齐次向量对应；第一矩阵x数学式表达为：
Figure BDA0002119986170000195
构建包括有视觉信标局部位置信息且具有i行、j列第二矩阵A，所述第二矩阵A的任一行向量i的元素包括任一图像帧i中任一视觉信标j相对于相机坐标系的局部位置向量，任一行向量i中非零元位置为j+3，第二矩阵A数学式表达为：
Figure BDA0002119986170000196
其中，aij为矩阵A中某行向量，因地平面法向量为三维向量，故矩阵A中某行向量非零元位置j+3，
i＝1，2…N，j＝1，2…M，N为图像帧的数量，M为N个图像帧中所有信标累加的总数，包括同一信标在多个图像帧中出现时的累计。通过对矩阵A进行SVD分解可求得地平面法向量
Figure BDA0002119986170000197
的估计值。
考虑到数值计算精度，也可以在构建线性方程组之前，先对所有
Figure BDA0002119986170000198
进行归一化计算，例如，对各个信标在相机坐标系下局部位置信息分别乘以用于归一化的矩阵，求得归一化结果之后，再基于矩阵反变换的
Figure BDA0002119986170000199
通过对矩阵A进行SVD获取地平面法向量的估计值。
步骤2042，由于投影坐标系{di}的z轴平行于地平面法向量，则在获得地平面法向量之后，可将相机坐标系旋转至相机投影坐标系，从而获得相机坐标系和相机投影坐标系之间的投影旋转向量。
投影旋转矩阵
Figure BDA0002119986170000201
存在无穷多组解，任意一解即可满足算法需求。本实施例中，采用一种较为简便的计算方法求解相机坐标系和相机投影坐标系之间的投影旋转向量
Figure BDA0002119986170000202
具体公式如下。
Figure BDA0002119986170000203
其中，
Figure BDA0002119986170000204
其中，e3为三维的单位向量。
这样，相机外参的标定问题可简化为对机器人本体坐标系至投影坐标系二维变换，即对向量
Figure BDA0002119986170000205
的估计。
其中，相机投影坐标系为相机坐标系原点在地平面上的投影，相机投影坐标系的z轴与移动机器人本体坐标系的z轴平行且方向相同。
步骤2043，基于同一视觉信标均被观测到的连续两关键帧，根据该视觉信标在该连续两关键帧的里程计测量数据和视觉信标检测数据，构建同一视觉信标在连续两关键帧图像之间的约束，通过该约束依次获得移动机器人本体坐标系与相机投影坐标系之间的偏航角、移动机器人本体坐标系与相机投影坐标系之间的平移向量。
参见图6所示，图6示出了同一视觉信标在连续两关键帧时各坐标系下关联关系的一种示意图。对于第一关键帧中的视觉信标，该视觉信标的观测结果(该视觉信标在第一帧的相机坐标系下的位置)与其在第一帧的相机投影坐标系下的位置具有对应关系，其在第一帧的相机投影坐标系下的位置与其在第一帧的移动机器人本体坐标系下的位置具有对应关系，该对应关系与相机外参有关；同理，对于第二关键帧中的视觉信标，该视觉信标的观测结果(该视觉信标在第二帧的相机坐标系下的位置)与其在第二帧的相机投影坐标系下的位置具有对应关系，其在第二帧的相机投影坐标系下的位置与其在第二帧的移动机器人本体坐标系下的位置具有对应关系，该对应关系与相机外参有关。同一视觉信标在第一关键帧中与在第二关键帧中之间的相对位置可体现在该两关键帧的里程计测量数据中。这样，在第一关键帧中的该信标在移动机器人本体坐标系下的位置满足：
根据第一图像帧中的视觉信标j在移动机器人本体坐标系下的位置向量，等于：在移动机器人本体坐标系下，所述第一图像帧连续的第二图像帧与所述第一图像帧之间的相对位姿向量，与从移动机器人本体坐标系到相机投影坐标系变换的第二图像帧中的同一视觉信标j在相机投影坐标系下的位置向量的投影运算的乘积，由此得到第一关系式，该第一关系式的数学式表达为：
Figure BDA0002119986170000211
其中，角标数字1、2分别表示连续的第一关键帧和第二关键帧；
对上式(第一关系式)进行展开，可得到：
Figure BDA0002119986170000212
其中，01×2表示1行2列的零向量；
将上式再展开，并取前两行，可以得到所述同一视觉信标在连续两图像帧之间的约束，可以用数学式表达为：
Figure BDA0002119986170000213
其中，不考虑相机与移动机器人本体的高度差，外参变量包括：二维旋转矩阵
Figure BDA0002119986170000214
二维平移向量
Figure BDA0002119986170000215
观测结果包括：里程计测量
Figure BDA0002119986170000216
和
Figure BDA0002119986170000217
信标在相机投影坐标系下的局部位置
Figure BDA0002119986170000218
和
Figure BDA0002119986170000219
该局部位置可基于该信标在关键帧相机坐标系下的局部位置，通过投影旋转向量获得。
当两关键帧之间移动机器人以平移运动或以平移为主运动，旋转矩阵
Figure BDA00021199861700002110
近似为单位阵I，则上式为：
Figure BDA00021199861700002111
整理上式，消除其中的平移向量，可得不包括平移向量的约束，用数学表达为：
Figure BDA00021199861700002112
基于上式，可构建以移动机器人本体坐标系与相机投影坐标系之间的偏航角
Figure BDA00021199861700002113
为未知量的线性方程组：
Figure BDA00021199861700002114
其中，
Figure BDA00021199861700002115
求解上述线性方程组，可获得对偏航角的估计。
当两关键帧间运动为旋转运动或以旋转为主的运动，由所述同一视觉信标在连续两图像帧之间的约束(式3)，可得：
Figure BDA0002119986170000221
基于获得的偏航角的估计结果，求解上式中的平移向量，获得对移动机器人本体坐标系与相机投影坐标系之间的平移向量
Figure BDA0002119986170000222
的估计。
由此，获得除相机相对移动机器人本体高度之外的5个自由度相机外参的初始化结果。
步骤2044，根据所述偏航角、投影旋转向量，获得相机外参的旋转向量初始化结果，根据所述平移向量，获得相机外参位置向量的初始化结果。
设置相机高度默认值为0；基于步骤2043得到的移动机器人本体坐标系和相机投影坐标系之间的旋转向量
Figure BDA0002119986170000223
和步骤2042获得的相机坐标系与相机投影坐标系之间的投影旋转向量
Figure BDA0002119986170000224
将该旋转向量与投影旋转向量相乘，得到移动机器人本体坐标系相对于相机坐标系的旋转向量，即外参旋转向量初始化结果，基于步骤2043得到的机器人本体坐标系与相机投影坐标系之间的平移向量，得到移动机器人本体坐标系相对于相机坐标系的位置向量，即外参位置初始化结果；相机外参的初始化结果可表示为：
Figure BDA0002119986170000225
其中，
Figure BDA0002119986170000226
由步骤2042中获得的投影旋转向量
Figure BDA0002119986170000227
获得。
参见图7所示，图7示出了以上述步骤2041-2044所涉及的图像数据的关联关系的一种示意图。图中：
基于信标检测数据中的各个信标在相机坐标系下局部位置，可获得平行于地平面且经过信标坐标原点的各个辅助平面所共有的地平面法向量；
通过地平面法向量可获得相机坐标系与相机投影坐标系之间的旋转向量；
基于同一视觉信标均被观测到的连续两关键帧，根据该信标里程计测量数据和该信标的检测数据，构建同一视觉信标在连续两关键帧图像之间的约束，从而依次获得以移动机器人本体坐标系与相机投影坐标系之间的偏航角、移动机器人本体坐标系与相机投影坐标系之间的平移向量。
根据所述旋转向量、偏航角、平移向量，得到相机外参初始化结果。
参见图8所示，图8为本发明实施例基于视觉信标检测数据和里程计测量数据进行相机外参标定的一种装置示意图。该装置包括：
数据采集模块，获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
初始化模块，基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果；基于视觉信标检测数据，获取各个视觉信标全局位姿初始化结果；获取相机外参位姿初始化结果；
里程计约束与视觉信标特征点图像重投影约束联合优化模块，对基于里程计测量数据获取的所有图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的所有特征点在其图像帧上重投影约束残差，进行以所述各个图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述各个视觉信标全局位姿为状态变量的联合优化，获得相机外参位姿优化结果；
其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述相机外参位姿的初始值为所述相机外参位姿初始化结果，所述视觉信标全局位姿的初始值为所述各个视觉信标全局位姿初始化结果。
其中，所述数据采集模块还包括，对每个视觉信标进行一次以上的观测，在采集各个视觉信标检测数据的同一时刻记录里程计测量数据。
所述数据采集模块包括，用于获得里程计测量数据的里程计测量模块，和用于获得视觉信标检测数据的视觉信标检测模块。
所述初始化模块包括，用于基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果的图像帧机器人本体全局位姿初始化模块；用于基于所述视觉信标检测数据，获取各个视觉信标全局位姿初始化结果的视觉信标全局位姿初始化模块；和，用于获取相机外参位姿初始值的相机外参初始化模块，其中，
所述图像帧机器人本体全局位姿初始化模块包括，将各个所述图像帧机器人本体全局位姿的初始化结果直接采用对应图像帧的里程计测量数据结果；
所述视觉信标全局位姿初始化模块包括，对于图像帧中任一视觉信标，通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，
其中，所述视觉信标检测数据包括视觉信标被观测测量值，从视觉信标相对相机的局部位姿获得；
所述视觉信标相对于相机的局部位姿根据视觉信标中特征点的图像坐标、相机内参、以及信标参数获得。
所述视觉信标全局位姿初始化模块还包括，根据图像帧机器人本体全局位姿初始化向量、相机外参位姿初始化向量、以及视觉信标相对相机的局部位置向量，获得所述视觉信标全局位置初始化结果；
根据图像帧机器人本体坐标系的全局旋转向量、相机外参初始化旋转向量、视觉信标在相机坐标系中的局部旋转向量，获得所述视觉信标全局姿态初始化结果，
其中，
所述图像帧机器人本体坐标系的全局旋转向量从所述图像帧机器人本体全局位姿初始化结果获得，
所述相机外参初始化旋转向量从所述相机外参初始化位姿获得，
所述视觉信标在相机坐标系中的局部旋转向量从所述视觉信标相对相机的局部位姿获得。
该装置还包括，
里程计约束与视觉信标局部位置测量约束联合优化模块，对所述所有图像帧的里程计约束残差、以及包括视觉信标图像的所有图像帧中所有视觉信标的局部位置测量约束残差，进行以所述各个关键帧机器人本体全局位姿初始化结果、所述相机外参位姿为状态变量、及各个视觉信标全局位置初始化结果为初始值的联合优化，获得各个关键帧机器人本体全局位姿的初步优化结果、相机外参位姿的初步优化结果、和各个视觉信标全局位置的初步优化结果，并输出给所述里程计约束与视觉信标特征点图像重投影约束联合优化模块；
所述视觉信标全局位姿初始化模块还包括，将所述各个图像帧机器人本体全局位姿的初步优化结果分别作为所述各个图像帧机器人本体全局位姿初始化结果；将所述各个视觉信标全局位置的初步优化结果、以及各个视觉信标全局姿态初始化结果作为所述各个视觉信标全局位姿初始化结果；
所述里程计约束与视觉信标特征点图像重投影约束联合优化模块还包括，构建融合所述里程计约束残差和所述重投影约束残差的第二目标函数，通过以所述各个图像帧机器人本体全局位姿初始化结果、相机外参位姿初始化结果、和所述各个视觉信标全局位姿初始化结果为迭代初始值的迭代，获取使得所述第二目标函数为最小值时的的相机外参位姿优化结果。
所述里程计约束与视觉信标局部位置测量约束联合优化模块还包括，构建融合所述里程计约束残差和所述视觉信标的局部位置测量约束残差的第一目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、相机外参位姿初始化结果、和各个所述视觉信标全局位置初始化结果为迭代初始值的迭代，获取使得所述第一目标函数为最小值时的各个图像帧机器人本体全局位姿的初步优化结果、相机外参位姿初步优化结果、和各个视觉信标全局位置的初步优化结果。
所述里程计约束与视觉信标局部位置测量约束联合优化模块还包括，
对任一所述图像帧，
基于所述里程计测量数据，获取所述图像帧与其相邻图像帧之间的机器人本体相对位姿、和所述图像帧与其相邻图像帧之间的里程计相对位姿；
由所述机器人本体相对位姿和里程计相对位姿的差异计算所述图像帧的里程计约束残差，
累计各个所述图像帧的里程计约束残差，得到所有所述图像帧的里程计约束残差。所述里程计约束与视觉信标局部位置测量约束联合优化模块还包括，
对包括视觉信标图像的任一图像帧中的任一视觉信标，获取该视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置与该视觉信标被观测测量值之间的差异，得到该视觉信标的局部位置测量约束残差，
累计各个图像帧中的各个视觉信标的局部位置测量约束残差，得到所有图像帧中所有视觉信标的局部位置测量约束残差，
其中，
所述视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置通过该视觉相机外参位姿初始化结果、图像帧机器人本体全局位姿初始化结果、以及视觉信标的全局位置初始化结果获得；
所述视觉信标被观测测量值通过该视觉信标相对于相机的局部位姿中的局部位置获得。
所述里程计约束与视觉信标特征点图像重投影约束联合优化模块还包括，对于包括视觉信标图像的任一图像帧中的任一视觉信标所具有的任一特征点，根据相机内参矩阵、该视觉信标的全局位姿初始化结果、以及该特征点在该视觉信标坐标系下的局部位置的投影函数与该特征点的图像坐标的测量值之差异，获得该视觉信标中该特征点在其图像帧相机图像上重投影误差，
累计各个图像帧中的各个视觉信标的各个特征点的重投影误差，得到所有图像帧中所有视觉信标的所有特征点在图像帧上的重投影约束残差。
所述装置进一步包括，关键帧筛选模块，从所述图像中至少筛选出包括视觉信标图像的关键帧；
所述关键帧筛选模块还包括，按照如下任一条件或其组合筛选出关键帧：
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧作为关键帧；
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧作为为关键帧；
第一帧为关键帧。
所述装置进一步包括，建图模块，获得各个视觉信标全局位姿优化结果，并基于该优化结果，构建信标地图。
所述相机外参初始化模块包括，根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，基于所述各个视觉信标检测数据中各个视觉信标的局部位置，获得地平面的法向量；
通过地平面的法向量，获得相机坐标系与相机投影坐标系之间的投影旋转向量；其中，相机投影坐标系为相机坐标系原点在地平面上的投影，相机投影坐标系的z轴与移动机器人本体坐标系的z轴平行且方向相同；
基于同一视觉信标均被观测到的连续两图像帧，根据该视觉信标所在连续两图像帧的里程计测量数据和检测数据，构建同一视觉信标在连续两图像帧之间的约束，通过该约束依次获得移动机器人本体坐标系与相机投影坐标系之间的偏航角、移动机器人本体坐标系与相机投影坐标系之间的平移向量；
根据所述偏航角、投影旋转向量，获得相机外参的旋转向量初始化结果，根据所述平移向量，获得相机外参位置向量的初始化结果。
本发明还提供了一种基于视觉信标建立信标地图的电子设备，该电子设备包括存储器和处理器，其中，
所述存储器用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序，实现上述基于视觉信标建立信标地图的方法。
存储器可以包括随机存取存储器(Random Access Memory，RAM)，也可以包括非易失性存储器(Non-Volatile Memory，NVM)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
本发明实施例还提供了一种计算机可读存储介质，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现如下步骤：
获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果；
基于所述视觉信标检测数据，获取各个视觉信标全局位姿初始化结果；
获取相机外参位姿初始化结果；
对基于里程计测量数据获取的所有图像帧里程计约束残差、和基于所述各个视觉信标检测数据获取的所有图像帧中所有视觉信标的所有特征点在其图像帧上重投影约束残差，进行以所述各个图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述各个视觉信标全局位姿为状态变量的联合优化，获得相机外参优化结果；
其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述相机外参位姿的初始值为所述相机外参位姿初始化结果，所述视觉信标全局位姿的初始值为所述各个视觉信标全局位姿初始化结果。
对于装置/网络侧设备/存储介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (20)

1.一种相机外参的标定方法，其特征在于，
获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；
获取相机外参位姿初始化结果；
基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果；
对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得相机外参优化结果；
其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述相机外参位姿的初始值为所述相机外参位姿初始化结果，所述视觉信标全局位姿的初始值为所述各个视觉信标全局位姿初始化结果。
2.如权利要求1所述的方法，其特征在于，所述基于视觉信标检测数据，获取视觉信标全局位姿初始化结果之后，进一步包括，
获取图像帧中视觉信标的局部位置测量约束残差，
对所述图像帧的里程计约束残差、以及所述局部位置测量约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、所述相机外参位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，获得图像帧机器人本体全局位姿的初步优化结果、相机外参位姿的初步优化结果、和视觉信标全局位置的初步优化结果；其中，所述视觉信标全局位置的初始值为所述视觉信标全局位姿初始化结果所包括的视觉信标全局位置初始化结果；
将所述图像帧机器人本体全局位姿的初步优化结果作为所述图像帧机器人本体全局位姿初始化结果；
将所述相机外参位姿的初步优化结果作为所述相机外参位姿初始化结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括，
基于视觉信标首次被观测的图像帧相机初始位姿、及视觉信标检测数据，获取视觉信标全局姿态初始化结果；
将所述视觉信标全局位置的初步优化结果、以及视觉信标全局姿态初始化结果作为所述视觉信标全局位姿初始化结果。
3.如权利要求2所述的方法，其特征在于，进行以所述图像帧机器人本体全局位姿为状态变量、所述相机外参位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述视觉信标的局部位置测量约束残差的第一目标函数，通过以各个所述各个图像帧机器人本体全局位姿初始化结果、相机外参位姿初始化结果、和各个所述信标全局位置初始化结果为迭代初始值的迭代，获取使得所述第一目标函数为最小值时的各个图像帧机器人本体全局位姿的初步优化结果、相机外参位姿初步优化结果、和各个视觉信标全局位置的初步优化结果。
4.如权利要求1至3任一所述的方法，其特征在于，所进行以所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述重投影约束残差的第二目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、相机外参位姿初始化结果、和各个所述视觉信标全局位姿初始化结果为迭代初始值的迭代，获取使得所述第二目标函数为最小值时的相机外参位姿优化结果。
5.如权利要求4所述的方法，其特征在于，所述基于所述里程计测量数据获取的所有图像帧的里程计约束残差，包括：
对任一所述图像帧，
基于所述里程计测量数据，获取所述图像帧与其相邻图像帧之间的机器人本体相对位姿、和所述图像帧与其相邻图像帧之间的里程计相对位姿；
由所述机器人本体相对位姿和里程计相对位姿的差异，计算所述图像帧的里程计约束残差，
累计各个所述图像帧的里程计约束残差，得到所有所述图像帧的里程计约束残差。
6.如权利要求4所述的方法，其特征在于，所述获取图像帧中视觉信标的局部位置测量约束残差，包括，
对包括视觉信标图像的任一图像帧中的任一视觉信标，获取该视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置与该视觉信标被观测测量值之间的差异，得到该视觉信标的局部位置测量约束残差，
其中，
所述视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置通过相机外参位姿当前状态变量、图像帧机器人本体全局位姿当前状态变量、以及视觉信标的全局位置当前状态变量获得；所述图像帧机器人本体全局位姿当前状态变量在第一次迭代时为图像帧机器人本体全局位姿初始化结果；所述视觉信标的全局位置当前状态变量在第一次迭代时为视觉信标的全局位置初始化结果；相机外参位姿当前状态变量在第一次迭代时为相机外参位姿初始化结果；
所述视觉信标被观测测量值通过该视觉信标相对于相机的局部位姿中的局部位置获得；
累计各个图像帧中的各个视觉信标的局部位置测量约束残差，得到所有图像帧中所有视觉信标的局部位置测量约束残差。
7.如权利要求4所述的方法，其特征在于，所述基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上的重投影约束残差，包括：
根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差。
8.如权利要求7所述的方法，其特征在于，所述根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差，包括，
对于任一图像帧中的任一视觉信标所具有的任一特征点，根据相机内参矩阵、图像帧机器人本体全局位姿当前状态变量、该视觉信标的全局位姿当前状态变量、以及该特征点在该视觉信标坐标系下的局部位置的投影函数与该特征点的图像坐标的测量值之差异，获得该视觉信标中该特征点在其图像帧上重投影误差，
累计各个图像帧中的各个视觉信标的各个特征点的重投影误差，得到所有图像帧中所有视觉信标的所有特征点在图像帧上的重投影约束残差。
9.如权利要求1所述的方法，其特征在于，所述基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果包括，
将各个所述图像帧机器人本体全局位姿的初始化结果直接采用对应图像帧的里程计测量数据结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括：
对于图像帧中任一视觉信标，通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，
其中，所述视觉信标检测数据包括视觉信标被观测测量值，从视觉信标相对相机的局部位姿获得；
所述视觉信标相对于相机的局部位姿根据视觉信标中特征点的图像坐标、相机内参、以及信标参数获得。
10.如权利要求9所述的方法，其特征在于，所述通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，包括：
根据图像帧机器人本体全局位姿向量、相机外参位姿初始化向量、以及视觉信标相对相机的局部位置向量，获得所述视觉信标全局位置初始化结果；
根据图像帧机器人本体坐标系的全局旋转向量、相机外参初始化旋转向量、视觉信标在相机坐标系中的局部旋转向量，获得所述视觉信标全局姿态初始化结果，
其中，
所述图像帧机器人本体坐标系的全局旋转向量从所述图像帧机器人本体全局位姿初始化结果获得，
所述相机外参初始化旋转向量从所述相机外参位姿初始化结果获得，
所述视觉信标在相机坐标系中的局部旋转向量从所述视觉信标相对相机的局部位姿获得。
11.如权利要求4所述的方法，其特征在于，所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧被采集时刻机器人本体的里程计测量数据包括，对每个视觉信标进行一次以上的观测，在采集各个视觉信标检测数据的同一时刻记录里程计测量数据；
所述获取相机外参位姿初始化结果包括，基于所述各个视觉信标检测数据和里程计测量数据，获取相机外参位姿初始化结果。
12.如权利要求11所述的方法，其特征在于，所述基于所述各个视觉信标检测数据和里程计测量数据，获取相机外参位姿初始化结果包括，
根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，基于所述各个视觉信标检测数据中各个视觉信标的局部位置，获得地平面法向量；
通过地平面法向量，获得相机坐标系与相机投影坐标系之间的投影旋转向量；其中，相机投影坐标系为相机坐标系原点在地平面上的投影，相机投影坐标系的z轴与移动机器人本体坐标系的z轴平行且方向相同；
基于同一视觉信标均被观测到的连续两图像帧，根据该视觉信标所在连续两图像帧的里程计测量数据和视觉信标检测数据，构建同一视觉信标在连续两图像帧之间的约束，通过该约束依次获得移动机器人本体坐标系与相机投影坐标系之间的偏航角、移动机器人本体坐标系与相机投影坐标系之间的平移向量；
根据所述偏航角、投影旋转向量，获得相机外参的旋转向量初始化结果，根据所述平移向量，获得相机外参位置向量的初始化结果。
13.如权利要求12所述的方法，其特征在于，所述各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束包括：各个辅助平面在相机坐标系下的齐次向量与各个视觉信标相对于相机坐标系的局部位置的齐次向量的乘积为零；
所述根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，基于所述各个视觉信标检测数据中各个视觉信标的局部位置，获得地平面法向量，包括，
根据所述各个辅助平面在相机坐标系下的齐次向量与所述各个辅助平面所共有的地平面法向量、和各个视觉信标距离地面的高度相关的标量所构成的等同关系，构建以所述地平面法向量、以及各个视觉信标距离地面的高度相关的标量为元素的第一矩阵；该第一矩阵与所述各个辅助平面在相机坐标系下的齐次向量对应；
构建包括有视觉信标局部位置信息且具有i行、j列第二矩阵，所述第二矩阵的任一行向量i的元素包括任一图像帧i中任一视觉信标j相对于相机坐标系的局部位置向量，任一行向量i中非零元位置为j+3；
根据各个视觉信标坐标系原点分别位于平行于地平面且经过各个视觉信标坐标系原点的各个辅助平面的约束，构建所述第二矩阵与第一矩阵的乘积为零的关系式；
对第二矩阵进行分解，获得地平面法向量；
其中，i＝1，2…N，j＝1，2…M，N为图像帧的数量，M为N个图像帧中所有信标累加的总数。
14.如权利要求12所述的方法，其特征在于，所述基于同一视觉信标均被观测到的连续两图像帧，根据该视觉信标所在连续两图像帧的里程计测量数据和检测数据，构建同一视觉信标在连续两图像帧之间的约束，包括，
根据第一图像帧中的视觉信标j在移动机器人本体坐标系下的位置向量，等于：在移动机器人本体坐标系下，所述第一图像帧连续的第二图像帧与所述第一图像帧之间的相对位姿向量，与从移动机器人本体坐标系到相机投影坐标系变换的第二图像帧中的同一视觉信标j在相机投影坐标系下的位置向量的投影运算的乘积，得到第一关系式，
基于所述第一关系式的展开，得到所述同一视觉信标在连续两图像帧之间的约束，该约束中包括：不考虑相机与移动机器人本体的高度差时，移动机器人本体坐标系相对相机投影坐标系的二维旋转矩阵、和二维平移向量；以及，所述第一图像帧和第二图像帧的里程计测量结果，所述视觉信标在所述第一图像帧、和第二图像帧的相机投影坐标系下的局部位置；该局部位置基于该信标在所述第一图像帧和第二图像帧的相机坐标系下的局部位置，通过所述投影旋转向量获得；
所述通过该约束依次获得移动机器人本体坐标系与相机投影坐标系之间的偏航角、移动机器人本体坐标系与相机投影坐标系之间的平移向量，包括，
按照两图像帧之间移动机器人的平移运动，将所述第一图像帧和第二图像帧的里程计测量结果中的旋转矩阵近似为单位阵，以消除所述同一视觉信标在连续两图像帧之间的约束中的平移向量，得到不包括平移向量的约束，基于该约束，构建以移动机器人本体坐标系与相机投影坐标系之间的偏航角为未知量的线性方程组，通过求解该线性方程组，获得所述偏航角；
基于所述同一视觉信标在连续两图像帧之间的约束和所述偏行角，求解所述同一视觉信标在连续两图像帧之间的约束中的平移向量。
15.如权利要求12所述的方法，其特征在于，所述根据所述偏航角、投影旋转向量，获得相机外参的旋转向量初始化结果，根据所述平移向量，获得相机外参位置向量的初始化结果，包括，
根据所述偏行角，得到移动机器人本体坐标系和相机投影坐标系之间的旋转向量，将该旋转向量与投影旋转向量相乘，得到移动机器人本体坐标系相对于相机坐标系的旋转向量；
设置相机的高度为0，基于所述平移向量，得到移动机器人本体坐标系相对于相机坐标系的位置向量；
将所述移动机器人本体坐标系相对于相机坐标系的旋转向量、以及位置向量作为相机外参位姿初始化结果。
16.如权利要求1所述的方法，其特征在于，所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少包括所述视觉信标图像帧采样时刻的里程计测量数据之后，进一步包括，按照如下任一条件或其组合筛选出关键帧，从所述图像帧中至少筛选出关键帧：
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧作为关键帧；
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧作为为关键帧；
第一帧为关键帧；
所述图像帧为筛选出的关键帧。
17.如权利要求1所述的方法，其特征在于，该方法还包括，
通过所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得各个视觉信标全局位姿优化结果，并基于该优化结果，构建信标地图。
18.一种相机外参的标定装置，其特征在于，
数据采集模块，获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
初始化模块，基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；基于视觉信标检测数据，获取视觉信标全局位姿初始化结果；获取相机外参位姿初始化结果；
里程计约束与视觉信标特征点图像重投影约束联合优化模块，对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、相机外参位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得相机外参位姿优化结果；
其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述相机外参位姿的初始值为所述相机外参位姿初始化结果，所述视觉信标全局位姿的初始值为所述视觉信标全局位姿初始化结果。
19.一种相机外参标定的电子设备，其特征在于，该电子设备包括存储器和处理器，其中，
所述存储器用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序，实现权利要求1-16任一所述相机外参的标定方法。
20.一种计算机可读存储介质，其特征在于，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行权利要求1-16任一所述相机外参的标定方法。
CN201910603598.6A 2019-07-05 2019-07-05 一种相机外参标定方法、装置 Pending CN112184824A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910603598.6A CN112184824A (zh) 2019-07-05 2019-07-05 一种相机外参标定方法、装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910603598.6A CN112184824A (zh) 2019-07-05 2019-07-05 一种相机外参标定方法、装置

Publications (1)

Publication Number Publication Date
CN112184824A true CN112184824A (zh) 2021-01-05

Family

ID=73914721

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910603598.6A Pending CN112184824A (zh) 2019-07-05 2019-07-05 一种相机外参标定方法、装置

Country Status (1)

Country Link
CN (1) CN112184824A (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112907654A (zh) * 2021-02-08 2021-06-04 上海汽车集团股份有限公司 多目相机外参数优化方法、装置、电子设备及存储介质
CN112965063A (zh) * 2021-02-11 2021-06-15 深圳市安泽智能机器人有限公司 一种机器人建图定位方法
CN113269840A (zh) * 2021-05-27 2021-08-17 深圳一清创新科技有限公司 一种用于相机和多激光雷达的联合标定方法及电子设备
CN113470121A (zh) * 2021-09-06 2021-10-01 深圳市普渡科技有限公司 自主移动平台、外参优化方法、装置及存储介质
CN113524216A (zh) * 2021-07-20 2021-10-22 成都朴为科技有限公司 一种基于多帧融合的果蔬采摘机器人及其控制方法
CN113592951A (zh) * 2021-07-14 2021-11-02 阿波罗智联(北京)科技有限公司 车路协同中路侧相机外参标定的方法、装置、电子设备
CN114092569A (zh) * 2022-01-19 2022-02-25 安维尔信息科技(天津)有限公司 一种基于多传感器融合的双目相机在线标定方法及系统
CN114359411A (zh) * 2022-01-10 2022-04-15 杭州巨岩欣成科技有限公司 泳池防溺水目标检测方法、装置、计算机设备及存储介质
CN114415698A (zh) * 2022-03-31 2022-04-29 深圳市普渡科技有限公司 机器人、机器人的定位方法、装置和计算机设备

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109242912A (zh) * 2018-08-29 2019-01-18 杭州迦智科技有限公司 采集装置外参标定方法、电子设备、存储介质

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109242912A (zh) * 2018-08-29 2019-01-18 杭州迦智科技有限公司 采集装置外参标定方法、电子设备、存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
HENGBO TANG等: "A Fully Automatic Calibration Algorithm for a Camera Odometry System", 《IEEE SENSORS JOURNAL》 *

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112907654B (zh) * 2021-02-08 2024-03-26 上海汽车集团股份有限公司 多目相机外参数优化方法、装置、电子设备及存储介质
CN112907654A (zh) * 2021-02-08 2021-06-04 上海汽车集团股份有限公司 多目相机外参数优化方法、装置、电子设备及存储介质
CN112965063A (zh) * 2021-02-11 2021-06-15 深圳市安泽智能机器人有限公司 一种机器人建图定位方法
CN113269840A (zh) * 2021-05-27 2021-08-17 深圳一清创新科技有限公司 一种用于相机和多激光雷达的联合标定方法及电子设备
CN113592951A (zh) * 2021-07-14 2021-11-02 阿波罗智联(北京)科技有限公司 车路协同中路侧相机外参标定的方法、装置、电子设备
CN113524216B (zh) * 2021-07-20 2022-06-28 成都朴为科技有限公司 一种基于多帧融合的果蔬采摘机器人及其控制方法
CN113524216A (zh) * 2021-07-20 2021-10-22 成都朴为科技有限公司 一种基于多帧融合的果蔬采摘机器人及其控制方法
CN113470121B (zh) * 2021-09-06 2021-12-28 深圳市普渡科技有限公司 自主移动平台、外参优化方法、装置及存储介质
CN113470121A (zh) * 2021-09-06 2021-10-01 深圳市普渡科技有限公司 自主移动平台、外参优化方法、装置及存储介质
CN114359411A (zh) * 2022-01-10 2022-04-15 杭州巨岩欣成科技有限公司 泳池防溺水目标检测方法、装置、计算机设备及存储介质
CN114359411B (zh) * 2022-01-10 2022-08-09 杭州巨岩欣成科技有限公司 泳池防溺水目标检测方法、装置、计算机设备及存储介质
CN114092569A (zh) * 2022-01-19 2022-02-25 安维尔信息科技(天津)有限公司 一种基于多传感器融合的双目相机在线标定方法及系统
CN114415698A (zh) * 2022-03-31 2022-04-29 深圳市普渡科技有限公司 机器人、机器人的定位方法、装置和计算机设备
CN114415698B (zh) * 2022-03-31 2022-11-29 深圳市普渡科技有限公司 机器人、机器人的定位方法、装置和计算机设备

Similar Documents

Publication Publication Date Title
CN112184824A (zh) 一种相机外参标定方法、装置
CN108717712B (zh) 一种基于地平面假设的视觉惯导slam方法
US10225473B2 (en) Threshold determination in a RANSAC algorithm
JP7300550B2 (ja) 視覚標識に基づき標識マップを構築する方法、装置
KR20150119337A (ko) 환경의 3d 모델의 발생
US8432442B2 (en) Method for self localization using parallel projection model
CN110969665B (zh) 一种外参标定方法、装置、系统及机器人
GB2506411A (en) Determination of position from images and associated camera positions
CN108332752B (zh) 机器人室内定位的方法及装置
US7333133B2 (en) Recursive least squares approach to calculate motion parameters for a moving camera
Schneider et al. Incremental real-time bundle adjustment for multi-camera systems with points at infinity
CN112444246A (zh) 高精度的数字孪生场景中的激光融合定位方法
CN102147918A (zh) 基于体育场标志线的摄像机外参数确定方法及系统
CN108444452B (zh) 目标经纬度和拍摄装置的三维空间姿态的检测方法及装置
Keivan et al. Constant-time monocular self-calibration
CN113345032A (zh) 一种基于广角相机大畸变图的初始化建图方法及系统
Perez-Yus et al. A novel hybrid camera system with depth and fisheye cameras
JP2007034964A (ja) カメラ視点運動並びに３次元情報の復元及びレンズ歪パラメータの推定方法、装置、カメラ視点運動並びに３次元情報の復元及びレンズ歪パラメータの推定プログラム
Comport et al. Efficient model-based tracking for robot vision
Miyagawa et al. Motion and shape recovery based on iterative stabilization for modest deviation from planar motion
Tahri et al. Efficient decoupled pose estimation from a set of points
Tommaselli et al. Line based camera calibration in machine vision dynamic applications
Förstner et al. Bundle adjustment
Zhang et al. An Online Automatic Calibration Method Based on Feature Descriptor for Non-Overlapping Multi-Camera Systems
Zhang et al. LiDAR and Camera Calibration Using Pyramid and Checkerboard Calibrators

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
CB02 Change of applicant information

Address after: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant after: Hangzhou Hikvision Robot Co.,Ltd.

Address before: 310052 5 / F, building 1, building 2, no.700 Dongliu Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant before: HANGZHOU HIKROBOT TECHNOLOGY Co.,Ltd.

CB02 Change of applicant information