CN111899303B - 一种新的考虑空间逆投影约束的特征匹配与相对定位方法 - Google Patents

一种新的考虑空间逆投影约束的特征匹配与相对定位方法 Download PDF

Info

Publication number
CN111899303B
CN111899303B CN202010675135.3A CN202010675135A CN111899303B CN 111899303 B CN111899303 B CN 111899303B CN 202010675135 A CN202010675135 A CN 202010675135A CN 111899303 B CN111899303 B CN 111899303B
Authority
CN
China
Prior art keywords
feature
camera
sequence
image
characteristic
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
CN202010675135.3A
Other languages
English (en)
Other versions
CN111899303A (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.)
Unit 63920 Of Pla
Original Assignee
Unit 63920 Of Pla
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 Unit 63920 Of Pla filed Critical Unit 63920 Of Pla
Priority to CN202010675135.3A priority Critical patent/CN111899303B/zh
Publication of CN111899303A publication Critical patent/CN111899303A/zh
Application granted granted Critical
Publication of CN111899303B publication Critical patent/CN111899303B/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
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/344—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
    • G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Computing Systems (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Multimedia (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Image Processing (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供了一种新的考虑空间逆投影约束的特征匹配与相对定位方法，属于视觉导航与定位技术领域。本发明利用地外天体探测器的巡视器在未知环境前后站点的行进过程中使用双目摄像头拍摄陆面图像提取到的特征点，反向使用透视投影变换模型解算出前后站点图像提取出的每对特征点对应的特征光束距离；根据特征光束距离约束和传统的特征点描述子相似度约束建立图像匹配和平差定位交叉联合的新的特征匹配模型；根据新的特征匹配模型确定的正确匹配点对通过光束法平差求解巡视器在新站点的位置和姿态。本发明通过将定位模型与特征配准融合，实现定位精度与匹配准确度的相互约束，提高特征配准率和定位精度从而实现巡视器高精度定姿定位。

Description

一种新的考虑空间逆投影约束的特征匹配与相对定位方法
技术领域
本发明属于视觉导航与定位技术领域，具体涉及一种新的考虑空间逆投影约束的特征匹配与相对定位方法。
背景技术
月面巡视器是在复杂非结构化月面环境中执行近距离科学探测任务的移动机器人，能够在月面自主巡游或者根据地面遥操作中心的指挥在月面几百米甚至几百公里范围内，通过自身携带的科学仪器实现月面环境的探测。巡视器在自主巡游或者遥控行驶过程中，需要了解其自身在月面坐标系中的位置和方向，才能将其重构的月面地形统一到同一基准下，指导后续的路径规划和行进。因此，巡视器的定位功能是进行路径规划和避障行进的前提和基础，对月面巡视器完成科学探测任务具有重要的意义。
月面环境的特殊性使得地面移动机器人定位常用的超声传感器定位、激光传感器定位和罗盘定位等方法无法使用；另一方面月球与地球的距离也远远超出了GPS定位和北斗定位系统导航服务的范围。因此，需要依赖专用的导航定位技术实现月面巡视器的定位导航。现阶段应用于月面巡视器导航定位的方法主要包括航位推算法、惯性导航定位法、天文导航定位法、无线电导航定位法和视觉导航定位法。在众多的定位方法中，视觉导航定位方法具有定位灵活、容错性强、定位精度高、定位精度不受地形地貌影响且容易与地形地貌三维恢复相结合等特点，能够提高月面巡视器的智能性，为巡视器成功实现科学探测任务提供有力的指导。
现有技术方案中，巡视器的视觉导航定位通常是利用双目视觉系统一定隔距离范围内连续拍摄图像，然后通过图像特征提取与匹配求取匹配特征点的空间立体关系，解算其巡视器自身的位置和姿态。然而在月球表面环境中，纹理较为匮乏且光照条件极为复杂，巡视器在不同摄站获取图像众多且图像的仿射投影关系复杂，可能存在较大尺度、旋转和光照变化，使得传统依靠比对尺度旋转不变性特征描述子相似度的算法如SIFT算法无法很好地配准图像，存在大量的有效特征点未被正确匹配和一定量的错误匹配信息的问题，所以难以保证巡视器定位的成功。因此亟需改进特征描述方式和剔除错误匹配点从而实现提高特征配准率和定位精度。
发明内容
针对现有技术中存在的上述缺陷，本发明目的是提供一种新的考虑空间逆投影约束的特征匹配与相对定位方法，以解决巡视器在未知环境中难以实现自身定位、定位精度不高、容错性差和定位精度容易受路况影响等问题。本发明利用巡视器在未知环境前后站点的行进过程中使用双目摄像头拍摄陆面图像提取到的特征点，反向使用透视投影变换模型，解算出前后站点图像提取出的每对特征点对应的特征光束距离；根据特征光束距离约束和传统的特征点描述子相似度约束建立图像匹配和平差定位交叉联合的新的特征匹配模型；根据新的特征匹配模型确定的正确匹配点对通过光束法平差求解巡视器在新站点的位置和姿态。
为实现上述目的，本发明具体采取以下技术方案。
一种新的考虑空间逆投影约束的特征匹配与相对定位方法，包括以下步骤：
步骤S1、将巡视器的起始位置作为已知站点，巡视器向前行进一定距离到达的位置作为当前站点，利用双目立体相机系统在已知站点和当前站点以一定位姿拍摄陆面图像作为原始图像；
步骤S2、对双目立体相机系统的左相机或右相机拍摄获取的原始图像进行特征提取，获得相应图像的特征点的像素坐标和特征点的描述子；
步骤S3、将步骤S2中提取到的特征点通过透视投影模型逆向投影到空间中，形成对应的两条特征光束，计算两条特征光束之间的最近距离作为两个空间投影世界坐标之间的距离；由两个空间投影世界坐标之间的距离和两个特征点描述子之间的欧氏距离建立特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型；
步骤S4、根据步骤S3的特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型对步骤S2提取的特征点集的立体空间关系进行检查，删除错误的匹配点，确定前后站点左相机或右相机图像的正确匹配点集；
步骤S5、根据前一站点的巡视器位置和姿态，将左相机或右相机图像的正确匹配点对作为输入，利用光束法平差求解巡视器在新站点的位置和姿态。
进一步地，步骤S1中，巡视器在前后站点拍摄图像的过程具体包括：
巡视器初始位于前一站点，即已知站点P1，两自由度云台机构(3)初始状态下，安装于其上的双目相机系统(4)的左相机(4-2)和右相机(4-3)的俯仰和偏航角均为0°；两自由度云台机构(3)的俯仰自由度机构(3-1)和偏航自由度机构(3-2)转动带动双目相机系统(4)的左相机(4-2)和右相机(4-3)到达一定的俯仰和偏航角度，左相机(4-2)和右相机(4-3)开始以一定的角度拍摄图像，从而得到已知站点左、右相机原始图像；然后控制巡视器向前行驶一段距离到达当前站点P2位置；通过双目相机系统左相机(4-2)和右相机(4-3)以一定角度拍摄图像，从而得到新站点左、右相机原始图像；由此得到巡视器在前后站点不同俯仰、偏航状态下，左相机(4-2)和右相机(4-3)拍摄的陆面图像。
进一步地，步骤S2具体包括：
对上述S1步骤中双目立体相机的左相机或右相机在已知站点和当前站点拍摄的图像进行特征提取，获得前后两幅图像的特征点的像素坐标uij和ui′j以及它们对应的特征点描述子p(uij)和p(ui′j)。
优选地，步骤S2中的特征提取具体采用Affine-SIFT特征提取。
进一步地，步骤S3中，所述特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型具体为：
Figure BDA0002583777270000031
其中，
Figure BDA0002583777270000041
和
Figure BDA0002583777270000042
分别表示在相机Ci和相机Ci′视角下表面观察点Pj的世界坐标，uij和ui′j分别表示前后两幅不同图像中两个特征点的像素坐标，Ωi和Ωi′分别表示相机Ci和相机Ci′的位姿，p(uij)和p(ui′j)分别表示在前后两幅不同图像中两个特征点的局部描述子；p(*)表示特征描述子的向量，并且α∈(0,1)是表示特征光束的距离和匹配点的描述子相似度之间的平衡因子。
进一步地，利用前后站点巡视器初始位置坐标和姿态信息计算所述特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型中
Figure BDA0002583777270000043
具体方法为：
假设左相机光心位于Oi，相机位姿为
Figure BDA0002583777270000044
表示相机的姿态相对于世界坐标系的姿态，R(θi)表示相机坐标系相对于世界坐标系的旋转矩阵。P为前方的任一观测点，其坐标为
Figure BDA0002583777270000045
观测点投影至图像中对应的像素坐标
Figure BDA0002583777270000046
利用透视投影成像模型计算为：
Figure BDA0002583777270000047
其中，
Figure BDA0002583777270000048
分别表示第j个陆面观测点在相机坐标系和世界坐标系中的齐次坐标，
Figure BDA0002583777270000049
表示从第j个陆面观测点到第i幅图像的透视投影变换矩阵，
Figure BDA00025837772700000410
表示与投影坐标相关的畸变参数矩阵，定义如下:
Figure BDA00025837772700000411
其中
Figure BDA00025837772700000412
和
Figure BDA00025837772700000413
表示以像素为单位的相机焦距，(u0,v0)为图像主点坐标，Δuij和Δvij表示由于径向畸变、偏心畸变和薄透镜畸变引起的投影点偏移量；
公式(2)中的
Figure BDA00025837772700000414
为考虑畸变影响的像素坐标，消除畸变影响后观测点的投影坐标
Figure BDA00025837772700000415
计算为:
Figure BDA00025837772700000416
Figure BDA0002583777270000051
其中
Figure BDA0002583777270000052
表示图像主点的坐标，Δuij＝[Δuij Δvij 0]T表示畸变偏移，
Figure BDA0002583777270000053
是月面点在世界坐标系中的齐次坐标，
Figure BDA0002583777270000054
表示对角矩阵，R(θi)简写为Ri，
Figure BDA0002583777270000055
和
Figure BDA0002583777270000056
分别表示相机坐标系相对于世界坐标系的旋转矩阵和平移向量，
Figure BDA0002583777270000057
表示陆面观测点在相机坐标系中的z坐标，
令:
Figure BDA0002583777270000058
则将
Figure BDA0002583777270000059
和xij代入公式(3)得到：
Figure BDA00025837772700000510
由
Figure BDA00025837772700000511
得到以下公式：
Figure BDA00025837772700000512
其中，系数矩阵Aij和解向量
Figure BDA00025837772700000513
分别为:
Figure BDA00025837772700000514
利用最小化代数距离方法对上述方程进行求解，令
Figure BDA00025837772700000515
将齐次矩阵方程的求解转化为最小化范数
Figure BDA00025837772700000516
的优化问题；该问题转化为求取系数矩阵Aij奇异值分解后的最小奇异值对应的右向量；将Aij进行奇异值分解，即
Figure BDA00025837772700000517
Figure BDA00025837772700000518
得到奇异值由大到小分别为σ1,σ2,σ3，对应的右向量矩阵
Figure BDA00025837772700000519
Figure BDA00025837772700000520
则
Figure BDA00025837772700000521
近似为最小的奇异值σ3对应的右向量
Figure BDA00025837772700000522
即
Figure BDA00025837772700000523
其物理含义为表示从相机光心出发指向
Figure BDA00025837772700000524
的方向向量，因此，
Figure BDA00025837772700000525
计算为:
Figure BDA00025837772700000526
其中
Figure BDA0002583777270000061
表示从相机光心Oi出发指向
Figure BDA0002583777270000062
的方向向量；
Figure BDA0002583777270000063
为
Figure BDA0002583777270000064
的第三行，表示
Figure BDA0002583777270000065
在Z方向的分量，
Figure BDA0002583777270000066
表示从相机光心Oi指向观测点
Figure BDA0002583777270000067
的向量在Z方向上的分量；
Figure BDA0002583777270000068
位于从相机i光心Oi开始的并经过像素点
Figure BDA0002583777270000069
的径向线上；
Figure BDA00025837772700000610
也位于从相机i′的光心Oi′开始的并经过像素点
Figure BDA00025837772700000611
的径向线上；考虑到投影误差，计算两条径向线的共同垂线的中点作为
Figure BDA00025837772700000612
计算两条径向线之间的最小距离作为共同垂直线的长度；用ti和ti′表示两个相机光心的世界坐标，
Figure BDA00025837772700000613
和
Figure BDA00025837772700000614
表示两条径向线的方向向量；通过计算
Figure BDA00025837772700000615
和
Figure BDA00025837772700000616
的外积得到公共垂直线的方向向量；通过计算两条径向线之间的最小距离得到公共垂直线的长度为:
Figure BDA00025837772700000617
其中
Figure BDA00025837772700000618
是两条径向线公共垂直线方向向量，
Figure BDA00025837772700000619
和
Figure BDA00025837772700000620
表示为
Figure BDA00025837772700000621
在两条径向线Oiuij和Oi′ui′j上的投影，
Figure BDA00025837772700000622
被估计为两个投影的中间点，即
Figure BDA00025837772700000623
和
Figure BDA00025837772700000624
表示为:
Figure BDA00025837772700000625
Figure BDA00025837772700000626
其中
Figure BDA00025837772700000627
步骤S2中提取到的特征点像素坐标uij和ui′j分别对应着在相机Ci和相机Ci′视角下表面观察点Pj的经过透视投影方程产生的像素坐标
Figure BDA00025837772700000628
和
Figure BDA00025837772700000629
将像素坐标uij和ui′j替换公式(3)中的
Figure BDA00025837772700000630
根据上述的步骤即可求得它们在空间中的映射点Pj的两个不同的世界坐标
Figure BDA00025837772700000631
和
Figure BDA00025837772700000632
进一步地，由不正确的投影束产生的误差
Figure BDA00025837772700000633
的计算方法具体如下：
将公式(10)与(11)相减得：
Figure BDA0002583777270000071
其中e1、e2、e3表示三个正交单位向量，
Figure BDA0002583777270000072
Figure BDA0002583777270000073
这里A1、A2、A3表示为:
Figure BDA0002583777270000074
Figure BDA0002583777270000075
Figure BDA0002583777270000076
dii′j＝|A1|，根据矢量三乘积公式(a×b)×c＝b(a·c)-a(b·c)和标量三乘积公式a·(b×c)＝b·(a×c)＝c·(a×b)得到：
Figure BDA0002583777270000077
Figure BDA0002583777270000078
Figure BDA0002583777270000079
A2、A3计算为：
Figure BDA0002583777270000081
Figure BDA0002583777270000085
将A2＝A3＝0带入式(12)得，
Figure BDA0002583777270000082
则
Figure BDA0002583777270000083
从而计算得到
Figure BDA0002583777270000084
进一步地，||p(uij)-p(ui′j)||2表示前后两张图像提取的特征点的相似程度，通过计算前后两张图像提取特征点描述子之间的欧式距离来获得。
进一步地，步骤S4具体包括：
假设S2步骤中从已知站点和当前站点拍摄的图像中分别提取出M和N个ASIFT待匹配特征点分别为x1m,m＝1,2,…,M、x2n,n＝1,2,…,N，则特征点像素坐标分别为u1m,m＝1,2,…,M、u2n,n＝1,2,…,N,对应的特征点描述子分别为p(u1m),m＝1,2,…,M、p(u2n),n＝1,2,…,N；
从已知站点图像中提取出的M个特征点x1m,m＝1,2,…,M中取任意一个特征点x1i,则它的像素坐标为u1i,它的表观描述子为p(u1i)；
i.将特征点x1i的像素坐标u1i和当前站点图像提取出的所有的N个特征点x2n,n＝1,2,…,N的像素坐标u2n,n＝1,2,…,N分别带入公式(3)中的
Figure BDA0002583777270000091
依据步骤S3依次求得空间中它们的特征光束之间的距离
Figure BDA0002583777270000092
Figure BDA0002583777270000093
此时会求出N个特征光束距离的值；将它们存入一个序列，将此序列命名为X，则X的长度为N；寻找出序列X中N个空间距离的最大值X_MAX和最小值X_MIN；将X序列中的所有元素即N个空间距离的值除以最大值X_MAX，即将此序列中元素归一化；此时产生一个新的归一化序列，序列中元素均大于0小于或等于1；将此序列称为new_X，new_X中元素个数也为N；
ii.求特征点x1i的描述子p(u1i)和当前站点拍摄的图像提取出的N个特征点x2n,n＝1,2,…,N的描述子p(u2n),n＝1,2,…,N之间的欧氏距离||p(u1i)-p(u2n)||2；；求出N个欧式距离的值，将它们存入一个序列，将此序列命名为Y，则序列Y的长度为N；寻找出序列Y中的N个欧式距离的最大值Y_MAX和最小值Y_MIN，将Y序列中的所有元素的值即N个欧式距离的值除以此最大值Y_MAX，即将此序列中的元素归一化；此时会产生一个新序列，序列中元素均大于0小于或等于1，将此序列命名为new_Y，new_Y中元素个数也为N；
iii.将i.ii.求得的归一化序列new_X、new_Y中的元素按照公式(1)中分配的权重分别对应相加α·new_X+(1-α)·new_Y；得到一个新序列，将此序列命名为Z，则序列Z的长度也为N，Z中的元素均大于0小于或等于2；寻找出序列Z中的最小值Z_MIN，称Z_MIN为本次考虑空间逆投影约束的匹配过程的最佳匹配值并返回最佳匹配值Z_MIN在序列Z中的序号Z_arg；
经过步骤i.ii.iii.可以得到从已知站点图像中提取到的M个特征点中任意一个特征点x1i与当前站点图像中提取到的N个特征点x2n,n＝1,2,…,N在联合特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN及最佳匹配值Z_MIN在序列Z中的序号Z_arg；此时Z_arg即为在本次联合特征表观描述子相似度与特征光束距离约束的匹配的过程中，从已知站点图像任意提取出的一个特征点x1i与N个从当前站点图像提取的特征点x2n,n＝1,2,…,N中第Z_arg个特征点的匹配效果最佳；
在上述步骤中仅仅使用在已知站点图像中提取到的M个特征点中的任意一个特征点，即可确定它与当前站点图像提取到的N个特征点在联合特征表观描述子相似度和特征光束距离约束下的最佳匹配值及其序号；利用已知站点的图像中提取到的全部的M个特征点x1m,m＝1,2,…,M，分别确定它与当前站点图像提取到的N个特征点x2n,n＝1,2,…,N在特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN以及最佳匹配对；最佳匹配对由从已知站点图像中提取的全部的M个特征点x1m,m＝1,2,…,M中第i个特征点在全部的M个特征点中的索引和第i个特征点与当前站点图像提取到的全部N个特征点x2n,n＝1,2,…,N在特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN在序列Z中的序号Z_arg构成；因为此时是利用了在已知站点图像中提取到的M个特征点中的每一个特征点，所以会产生M对最佳匹配对及M个最佳匹配值；
最佳匹配对和最佳匹配值是一一对应关系，将上述的M个最佳匹配值存入一个序列，称此序列为R，则R序列的长度也为M；对序列R中的最佳匹配值按从小到大的关系进行排序，取前Q个最佳匹配值并返回其相应的最佳匹配对，得到在特征表观描述子相似度与特征光束距离相互约束下已知站点图像特征点与当前站点图像特征点的前Q个最佳匹配对。
进一步地，步骤S5具体包括：
根据在已知站点的巡视器位置和姿态，将步骤S4得到的Q个最佳匹配对作为输入，利用光束法平差求解巡视器在当前站点的位置和姿态；
巡视器的视觉定位模型被归纳为一个关于优化重投影误差的非线性最小二乘问题，定义为:
Figure BDA0002583777270000111
这里
Figure BDA0002583777270000112
表示第j个观察点投影到第i个像平面的像素坐标，uij表示从第i个图像提取的特征点的像素坐标，
Figure BDA0002583777270000113
表示第i台摄像机相对于世界坐标系的平移和旋转，M和N表示摄像机和观察点的数量；
Figure BDA0002583777270000114
由公式(2)计算；
方程(15)的优化问题通过非线性函数
Figure BDA0002583777270000115
的泰勒级数展开来线性化，将其转换为如下线性最小二乘问题:
Figure BDA0002583777270000116
J(Ω,X)∈R2MN×(6M+3N)是雅可比矩阵，并且b∈R2MN×1是误差向量，它们被定义为:
Figure BDA0002583777270000117
Figure BDA0002583777270000118
因此，方程的优化问题(15)可以通过迭代求解方程(16)和使用公式
Figure BDA0002583777270000119
Figure BDA00025837772700001110
更新
Figure BDA00025837772700001111
和
Figure BDA00025837772700001112
来解决。
相较于现有技术，本发明取得以下显著的有益效果。
本发明将定位模型与特征配准进行融合，实现定位精度与匹配准确度的相互约束，提高特征配准率和定位精度从而实现巡视器高精度定姿定位，能够为巡视器在未知月面等地外星球的行进提供精确的位置指导。
附图说明
图1是本发明实施例一种新的考虑空间逆投影约束的特征匹配与相对定位方法流程图；
图2是本发明实施例巡视器在前后站点拍摄月面图像过程示意图；
图3是本发明实施例特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型示意图。
具体实施方式
以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
图1示出了本实施例一种新的考虑空间逆投影约束的特征匹配与相对定位方法的流程图，其给出了发明方法的各个步骤，并且给出了各个步骤执行的前后逻辑关系。下面将对每一个步骤的实施进行详细描述。
本实施例中，地外探测器具体为月球探测器，其巡视器的相机系统所能拍摄的陆面图像即为月面图像，陆面观测点即是月面观测点。
步骤S1：将巡视器的起始位置作为已知站点，巡视器向前行进一定距离到达的位置作为当前站点，利用双目立体相机系统在已知站点和当前站点以一定位姿拍摄月面图像作为原始图像。
图2是对步骤S1描述的关于巡视器在已知站点和新站点利用双目立体相机拍摄图像过程的示意图，巡视器是一个具有四个驱动轮的可移动机器人，包括巡视器车体、位于车体前部上方的竖直桅杆机构、桅杆顶部的两自由度云台机构和安装于云台支撑架上的双目相机；云台能够在俯仰和偏航两个自由度上转动，通过云台转动可控制相机在不同的方向上拍摄图像；桅杆、云台和双目相机相对于巡视器车体的相对位置和姿态关系预先精确标定为已知参数。巡视器的关键部件解释如下：巡视器车体(1)，位于车体前部上方竖直的桅杆机构(2)，桅杆顶部的两自由度云台机构(3)，包括俯仰自由度机构(3-1)和偏航自由度机构(3-2)，双目相机系统(4)，包括相机支撑架(4-1)，安装于云台支撑架上的左相机(4-2)和右相机(4-3)。左相机(4-2)和右相机(4-3)的视场均为45°×45°。云台能够通过俯仰自由度机构(3-1)和偏航自由度机构(3-2)在俯仰和偏航两个自由度上转动，通过云台转动可控制相机在不同的方向上拍摄图像。桅杆机构(2)、云台机构(3)和双目相机系统(4)相对于巡视器车体(1)的相对位置和姿态关系预先精确标定为已知参数。
巡视器在前后站点拍摄图像的过程如下：
巡视器初始位于前一站点(已知站点P1)，两自由度云台机构(3)初始状态下，安装于其上的双目相机系统(4)的左相机(4-2)和右相机(4-3)的俯仰和偏航角均为0°。两自由度云台机构(3)的俯仰自由度机构(3-1)和偏航自由度机构(3-2)转动带动双目相机系统(4)的左相机(4-2)和右相机(4-3)到达一定的俯仰和偏航角度，左相机(4-2)和右相机(4-3)开始以一定的角度拍摄图像，从而可得到步骤S1中已知站点左、右相机原始图像。然后控制巡视器向前行驶一段距离到达当前站点P2位置，向前行驶的距离一般大于7米。与巡视器在P1位置的拍照过程类似，通过双目相机系统左相机(4-2)和右相机(4-3)以一定角度拍摄图像，从而可得到步骤S1中新站点左、右相机原始图像。由此可以得到巡视器在前后站点不同俯仰、偏航状态下，左相机(4-2)和右相机(4-3)拍摄的月面图像。R1表示巡视器在前后站点拍摄图像的重叠区域。
步骤S2：对双目立体相机系统的左相机或右相机拍摄获取的原始图像进行特征提取，获得相应图像的特征点像素坐标和特征点描述子。
现有传统的外观匹配算法(如SIFT及其派生方法)通常无法处理存在较大尺度、旋转和光照变化条件下两幅图像特征点的匹配问题。一些改进的外观匹配算法，例如仿射SIFT(Affine-sift)，考虑了图像的仿射变换，在处理大规模变换问题上比传统算法表现稍好，但是，这些方法仍存在一定的局限性：
对于尺度跨度超过一定阈值(如图像尺度比例超过8)、仿射变化量太大(如对望图像)和光照变化导致的明暗关系转变的三类图像，匹配效果仍然欠佳。本实施例使用仿射SIFT(Affine-sift)作为特征点提取的算法可以略微改善传统特征描述方式，获得相对准确的特征信息，作为新的特征匹配算法的基础。
对上述S1步骤中双目立体相机的左相机或右相机在已知站点和当前站点拍摄的图像进行Affine-SIFT特征提取，获得前后两幅图像的ASIFT特征点的像素坐标uij和ui′j以及它们对应的特征点描述子p(uij)和p(ui′j)。
步骤S3：利用巡视器在前后站点粗略的位置和姿态信息等已知参数将步骤S2中提取到的特征点通过透视投影模型逆向投影到空间中，形成对应的两条特征光束，计算两条特征光束之间的最近距离作为两个空间投影世界坐标之间的距离；由两个空间投影世界坐标之间的距离和两个特征点描述子之间的欧氏距离建立特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型。
图3是特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型示意图。
与已有的技术方案不同，本实施例提出了一种新的考虑空间逆投影的特征匹配方法来实现两个图像特征点的最佳匹配，其定义为:
Figure BDA0002583777270000141
其中，
Figure BDA0002583777270000142
和
Figure BDA0002583777270000143
分别表示在相机Ci和相机Ci′视角下表面观察点Pj的世界坐标，uij和ui′j分别表示前后两幅不同图像中两个特征点的像素坐标，Ωi和Ωi′分别表示相机Ci和相机Ci′的位姿，p(uij)和p(ui′j)分别表示在前后两幅不同图像中两个特征点的局部描述子；p(*)表示特征描述子的向量，并且α∈(0,1)是表示特征光束的距离和匹配点的描述子相似度之间的平衡因子。
等式(1)的模型包含两个部分。第一部分
Figure BDA0002583777270000144
表示由不正确的投影束产生的误差，它是由两个因素引起的，即初始位姿误差和错误匹配的点。第二部分与特征点的类型有关，即当确定提取特征点的方法(例如选择ASIFT特征提取算法)时，||p(uij)-p(ui′j)||2可以很容易地计算出来。接下来，从基本的空间透视投影变换关系出发，利用前后站点巡视器初始位置坐标和姿态信息介绍
Figure BDA0002583777270000145
的求法。
假设月球探测器的左相机光心位于Oi，相机位姿为
Figure BDA0002583777270000146
Figure BDA0002583777270000151
表示相机的姿态相对于世界坐标系的姿态，R(θi)表示相机坐标系相对于世界坐标系的旋转矩阵。P为前方的任一观测点，其坐标为
Figure BDA0002583777270000152
观测点投影至图像中对应的像素坐标
Figure BDA0002583777270000153
可以利用透视投影成像模型计算为：
Figure BDA0002583777270000154
其中，
Figure BDA0002583777270000155
分别表示第j个月面观测点在相机坐标系和世界坐标系中的齐次坐标，
Figure BDA0002583777270000156
表示从第j个月面观测点到第i幅图像的透视投影变换矩阵，
Figure BDA0002583777270000157
表示与投影坐标相关的畸变参数矩阵，定义如下:
Figure BDA0002583777270000158
其中fi u和fi v表示以像素为单位的相机焦距，(u0,v0)为图像主点坐标，Δuij和Δvij表示由于径向畸变、偏心畸变和薄透镜畸变引起的投影点偏移量。
公式(2)中的
Figure BDA0002583777270000159
为考虑畸变影响的像素坐标，因此消除畸变影响后观测点的投影坐标
Figure BDA00025837772700001510
计算为:
Figure BDA00025837772700001511
其中
Figure BDA00025837772700001512
表示图像主点的坐标，Δuij＝[Δuij Δvij 0]T表示畸变偏移，
Figure BDA00025837772700001513
是月面点在世界坐标系中的齐次坐标，diag(fi u,fi v,1)表示对角矩阵，R(θi)简写为Ri，
Figure BDA00025837772700001514
和
Figure BDA00025837772700001515
分别表示相机坐标系相对于世界坐标系的旋转矩阵和平移向量，
Figure BDA00025837772700001516
表示月面观测点在相机坐标系中的z坐标。
令:
Figure BDA00025837772700001517
则将
Figure BDA0002583777270000161
和xij代入公式(3)可以得到：
Figure BDA0002583777270000162
由
Figure BDA0002583777270000163
可得到以下公式：
Figure BDA0002583777270000164
其中，系数矩阵Aij和解向量
Figure BDA0002583777270000165
分别为:
Figure BDA0002583777270000166
利用最小化代数距离方法对上述方程进行求解。为了能够得到唯一解，我们令
Figure BDA0002583777270000167
则齐次矩阵方程的求解转化为最小化范数
Figure BDA0002583777270000168
的优化问题。该问题可以转化为求取系数矩阵Aij奇异值分解后的最小奇异值对应的右向量。将Aij进行奇异值分解，即
Figure BDA0002583777270000169
得到奇异值由大到小分别为σ1,σ2,σ3，对应的右向量矩阵
Figure BDA00025837772700001610
则
Figure BDA00025837772700001611
近似为最小的奇异值σ3对应的右向量
Figure BDA00025837772700001612
即
Figure BDA00025837772700001613
其物理含义为表示从相机光心(相机坐标系原点)出发指向
Figure BDA00025837772700001614
的方向向量。因此，
Figure BDA00025837772700001615
可以计算为:
Figure BDA00025837772700001616
其中
Figure BDA00025837772700001617
表示从相机光心Oi(相机坐标系原点)出发指向
Figure BDA00025837772700001618
的方向向量。
Figure BDA00025837772700001619
为
Figure BDA00025837772700001620
的第三行，表示
Figure BDA00025837772700001621
在Z方向的分量，
Figure BDA00025837772700001622
表示从相机光心Oi指向观测点
Figure BDA00025837772700001623
的向量在Z方向上的分量。
Figure BDA00025837772700001624
位于从相机i光心Oi开始的并经过像素点
Figure BDA00025837772700001625
的径向线上。同样，
Figure BDA00025837772700001626
也位于从相机i′的光心Oi′开始的并经过像素点
Figure BDA00025837772700001627
的径向线上。从理论上讲，这两条径向线应会聚到同一点
Figure BDA00025837772700001628
考虑到投影误差，我们计算两条径向线的共同垂线的中点作为
Figure BDA00025837772700001629
计算两条径向线之间的最小距离作为共同垂直线的长度。用ti和ti′表示两个相机光心的世界坐标，
Figure BDA00025837772700001630
和
Figure BDA0002583777270000171
表示两条径向线的方向向量。通过计算
Figure BDA0002583777270000172
和
Figure BDA0002583777270000173
的外积可以得到公共垂直线的方向向量。通过计算两条径向线之间的最小距离可以得到公共垂直线的长度为：
Figure BDA0002583777270000174
其中
Figure BDA0002583777270000175
是两条径向线公共垂直线方向向量。
Figure BDA0002583777270000176
和
Figure BDA0002583777270000177
表示为
Figure BDA0002583777270000178
在两条径向线
Figure BDA0002583777270000179
和
Figure BDA00025837772700001710
上的投影。
Figure BDA00025837772700001711
被估计为两个投影的中间点，即
Figure BDA00025837772700001712
在这里，
Figure BDA00025837772700001713
和
Figure BDA00025837772700001714
表示为:
Figure BDA00025837772700001715
Figure BDA00025837772700001716
其中
Figure BDA00025837772700001717
步骤S2中提取到的特征点像素坐标uij和ui′j分别对应着在相机Ci和相机Ci′视角下表面观察点Pj的经过透视投影方程产生的像素坐标
Figure BDA00025837772700001718
和
Figure BDA00025837772700001719
将像素坐标uij和ui′j替换公式(3)中的
Figure BDA00025837772700001720
根据上述的步骤即可求得它们在空间中的映射点Pj的两个不同的世界坐标
Figure BDA00025837772700001721
和
Figure BDA00025837772700001722
以下推导由不正确的投影束产生的误差
Figure BDA00025837772700001723
的求解方法。
将公式(10)与(11)相减得：
Figure BDA00025837772700001724
Figure BDA0002583777270000181
其中e1、e2、e3表示三个正交单位向量，
Figure BDA0002583777270000182
Figure BDA0002583777270000183
这里A1、A2、A3表示为:
Figure BDA0002583777270000186
Figure BDA0002583777270000184
Figure BDA0002583777270000187
很明显dii′j＝|A1|。根据矢量三乘积公式(a×b)×c＝b(a·c)-a(b·c)和标量三乘积公式a·(b×c)＝b·(a×c)＝c·(a×b)可以得到：
Figure BDA0002583777270000185
Figure BDA0002583777270000188
Figure BDA0002583777270000189
那么A2、A3可以计算为：
Figure BDA0002583777270000191
Figure BDA0002583777270000194
所以将A2＝A3＝0带入式(12)得，
Figure BDA0002583777270000192
则
Figure BDA0002583777270000193
至此公式(1)的第一项推导完毕。
公式(1)的第二项||p(uij)-p(ui′j)||2表示前后两张图像提取的特征点的相似程度，可以通过计算前后两张图像提取特征点描述子之间的欧式距离来获得。
步骤S4：根据步骤S3的特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型对步骤S2提取的特征点集的立体空间关系进行检查，删除错误的匹配点，确定前后站点左相机或右相机图像的正确匹配点集。具体说明如下：
根据步骤S3建立的特征光束距离和特征点描述子相似度相互约束的新的特征匹配模型，对步骤S2中从已知站点和当前站点拍摄的图像中提取的特征点的匹配关系进行检查，确定已知站点和当前站点左相机或右相机图像间的正确匹配点对。
假设S2步骤中从已知站点和当前站点拍摄的图像中分别提取出M和N个ASIFT待匹配特征点分别为x1m,m＝1,2,…,M、x2n,n＝1,2,…,N，则特征点像素坐标分别为u1m,m＝1,2,…,M、u2n,n＝1,2,…,N,对应的特征点描述子分别为p(u1m),m＝1,2,…,M、p(u2n),n＝1,2,…,N。
从已知站点图像中提取出的M个特征点x1m,m＝1,2,…,M中取任意一个特征点x1i,则它的像素坐标为u1i,它的表观描述子为p(u1i)。
i.将特征点x1i的像素坐标u1i和当前站点图像提取出的所有的N个特征点x2n,n＝1,2,…,N的像素坐标u2n,n＝1,2,…,N分别带入公式(3)中的
Figure BDA0002583777270000201
依据步骤S3依次求得空间中它们的特征光束之间的距离
Figure BDA0002583777270000202
Figure BDA0002583777270000203
此时会求出N个特征光束距离的值。将它们存入一个序列，将此序列命名为X，则X的长度为N。寻找出序列X中N个空间距离的最大值X_MAX和最小值X_MIN。将X序列中的所有元素即N个空间距离的值除以最大值X_MAX，即将此序列中元素归一化。此时产生一个新的归一化序列，序列中元素均大于0小于或等于1。将此序列称为new_X，new_X中元素个数也为N。
ii.求特征点x1i的描述子p(u1i)和当前站点拍摄的图像提取出的N个特征点x2n,n＝1,2,…,N的描述子p(u2n),n＝1,2,…,N之间的欧氏距离||p(u1i)-p(u2n)||2。与(i).中的过程类似，此时会求出N个欧式距离的值，将它们存入一个序列。将此序列命名为Y，则序列Y的长度为N。寻找出序列Y中的N个欧式距离的最大值Y_MAX和最小值Y_MIN。将Y序列中的所有元素的值即N个欧式距离的值除以此最大值Y_MAX，即将此序列中的元素归一化。此时会产生一个新序列，序列中元素均大于0小于或等于1。将此序列命名为new_Y，new_Y中元素个数也为N。
iii.将i.ii.求得的归一化序列new_X、new_Y中的元素按照公式(1)中分配的权重分别对应相加α·new_X+(1-α)·new_Y。得到一个新序列，将此序列命名为Z，则序列Z的长度也为N，Z中的元素均大于0小于或等于2。寻找出序列Z中的最小值Z_MIN，称Z_MIN为本次考虑空间逆投影约束的匹配过程的最佳匹配值并返回最佳匹配值Z_MIN在序列Z中的序号Z_arg。
经过步骤i.ii.iii.可以得到从已知站点图像中提取到的M个特征点中任意一个特征点x1i与当前站点图像中提取到的N个特征点x2n,n＝1,2,…,N在联合特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN及最佳匹配值Z_MIN在序列Z中的序号Z_arg。此时Z_arg即为在本次联合特征表观描述子相似度与特征光束距离约束的匹配的过程中，从已知站点图像任意提取出的一个特征点x1i与N个从当前站点图像提取的特征点x2n,n＝1,2,…,N中第Z_arg个特征点的匹配效果最佳。
在上述步骤中仅仅使用在已知站点图像中提取到的M个特征点中的任意一个特征点，即可确定它与当前站点图像提取到的N个特征点在联合特征表观描述子相似度和特征光束距离约束下的最佳匹配值及其序号。相似的，可以利用已知站点的图像中提取到的全部的M个特征点x1m,m＝1,2,…,M，分别确定它与当前站点图像提取到的N个特征点x2n,n＝1,2,…,N在特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN以及最佳匹配对。最佳匹配对是一对数，它是由从已知站点图像中提取的全部的M个特征点x1m,m＝1,2,…,M中第i个特征点在全部的M个特征点中的索引和第i个特征点与当前站点图像提取到的全部N个特征点x2n,n＝1,2,…,N在特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN在序列Z中的序号Z_arg构成的。因为此时是利用了在已知站点图像中提取到的M个特征点中的每一个特征点，所以会产生M对最佳匹配对及M个最佳匹配值。
最佳匹配对和最佳匹配值是一一对应关系，将上述的M个最佳匹配值存入一个序列，称此序列为R，则R序列的长度也为M。对序列R中的最佳匹配值按从小到大的关系进行排序，取前Q个最佳匹配值并返回其相应的最佳匹配对，这样就可以得到在特征表观描述子相似度与特征光束距离相互约束下已知站点图像特征点与当前站点图像特征点的前Q个最佳匹配对。
步骤S5：根据前一站点的巡视器位置和姿态，将左相机或右相机图像的正确匹配点对作为输入，利用光束法平差求解巡视器在新站点的位置和姿态。
根据在已知站点的巡视器位置和姿态，将步骤S4得到的Q个最佳匹配对作为输入，利用光束法平差求解巡视器在当前站点的位置和姿态。
月面巡视器的视觉定位模型被归纳为一个关于优化重投影误差的非线性最小二乘问题，其目的是最小化提取自图像的特征点与通过透视投影方程计算的投影点之间的偏差的二范数，定义为:
Figure BDA0002583777270000221
这里
Figure BDA0002583777270000222
表示第j个观察点投影到第i个像平面的像素坐标，uij表示从第i个图像提取的特征点的像素坐标，
Figure BDA0002583777270000223
表示第i台摄像机相对于世界坐标系的平移和旋转，M和N表示摄像机和观察点的数量。
Figure BDA0002583777270000224
由公式(2)计算。
方程(15)的优化问题可以通过非线性函数
Figure BDA0002583777270000225
的泰勒级数展开来线性化，将其转换为如下线性最小二乘问题:
Figure BDA0002583777270000226
这里J(Ω,X)∈R2MN×(6M+3N)是雅可比矩阵，并且b∈R2MN×1是误差向量，它们被定义为：
Figure BDA0002583777270000227
Figure BDA00025837772700002212
因此，方程的优化问题(15)可以通过迭代求解方程(16)和使用公式
Figure BDA0002583777270000228
Figure BDA0002583777270000229
更新
Figure BDA00025837772700002210
和
Figure BDA00025837772700002211
来解决。
以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

Claims (10)

1.一种新的考虑空间逆投影约束的特征匹配与相对定位方法，其特征在于包括：
步骤S1、将巡视器的起始位置作为已知站点，巡视器向前行进一定距离到达的位置作为当前站点，利用双目立体相机系统在已知站点和当前站点以一定位姿拍摄陆面图像作为原始图像；
步骤S2、对双目立体相机系统的左相机或右相机拍摄获取的原始图像进行特征提取，获得相应图像的特征点的像素坐标和特征点的描述子；
步骤S3、将步骤S2中提取到的特征点通过透视投影模型逆向投影到空间中，形成对应的两条特征光束，计算两条特征光束之间的最近距离作为两个空间投影世界坐标之间的距离；由两个空间投影世界坐标之间的距离和两个特征点描述子之间的欧氏距离建立特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型；
步骤S4、根据步骤S3的特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型对步骤S2提取的特征点集的立体空间关系进行检查，删除错误的匹配点，确定前后站点左相机或右相机图像的正确匹配点集；
步骤S5、根据前一站点的巡视器位置和姿态，将左相机或右相机图像的正确匹配点对作为输入，利用光束法平差求解巡视器在新站点的位置和姿态。
2.根据权利要求1所述的方法，其特征在于：
步骤S1中，巡视器在前后站点拍摄图像的过程具体包括：
巡视器初始位于前一站点，即已知站点P1，两自由度云台机构(3)初始状态下，安装于其上的双目相机系统(4)的左相机(4-2)和右相机(4-3)的俯仰和偏航角均为0°；两自由度云台机构(3)的俯仰自由度机构(3-1)和偏航自由度机构(3-2)转动带动双目相机系统(4)的左相机(4-2)和右相机(4-3)到达一定的俯仰和偏航角度，左相机(4-2)和右相机(4-3)开始以一定的角度拍摄图像，从而得到已知站点左、右相机原始图像；然后控制巡视器向前行驶一段距离到达当前站点P2位置；通过双目相机系统左相机(4-2)和右相机(4-3)以一定角度拍摄图像，从而得到新站点左、右相机原始图像；由此得到巡视器在前后站点不同俯仰、偏航状态下，左相机(4-2)和右相机(4-3)拍摄的陆面图像。
3.根据权利要求1所述的方法，其特征在于：
步骤S2具体包括：
对上述S1步骤中双目立体相机的左相机或右相机在已知站点和当前站点拍摄的图像进行特征提取，获得前后两幅图像的特征点的像素坐标uij和ui′j以及它们对应的特征点描述子p(uij)和p(ui′j)。
4.根据权利要求1所述的方法，其特征在于：
步骤S3中，所述特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型具体为：
Figure FDA0003078047210000021
其中，
Figure FDA0003078047210000022
和
Figure FDA0003078047210000023
分别表示在相机Ci和相机Ci′视角下表面观察点Pj的世界坐标，uij和ui′j分别表示前后两幅不同图像中两个特征点的像素坐标，Ωi和Ωi′分别表示相机Ci和相机Ci′的位姿，p(uij)和p(ui′j)分别表示在前后两幅不同图像中两个特征点的局部描述子；p(*)表示特征描述子的向量，并且α∈(0，1)是表示特征光束的距离和匹配点的描述子相似度之间的平衡因子。
5.根据权利要求4所述的方法，其特征在于：
利用前后站点巡视器初始位置坐标和姿态信息计算所述特征表观描述子相似度与特征光束距离相互约束的新的特征匹配模型中
Figure FDA0003078047210000031
具体方法为：
假设左相机光心位于Oi，相机位姿为
Figure FDA0003078047210000032
Figure FDA0003078047210000033
表示相机的姿态相对于世界坐标系的姿态，R(θi)表示相机坐标系相对于世界坐标系的旋转矩阵；P为前方的任一观测点，其坐标为
Figure FDA0003078047210000034
观测点投影至图像中对应的像素坐标
Figure FDA0003078047210000035
利用透视投影成像模型计算为：
Figure FDA0003078047210000036
其中，
Figure FDA0003078047210000037
分别表示第j个陆面观测点在相机坐标系和世界坐标系中的齐次坐标，
Figure FDA0003078047210000038
表示从第j个陆面观测点到第i幅图像的透视投影变换矩阵，
Figure FDA0003078047210000039
表示与投影坐标相关的畸变参数矩阵，定义如下:
Figure FDA00030780472100000310
其中
Figure FDA00030780472100000311
和
Figure FDA00030780472100000312
表示以像素为单位的相机焦距，(u0，v0)为图像主点坐标，Δuij和Δvij表示由于径向畸变、偏心畸变和薄透镜畸变引起的投影点偏移量；
公式(2)中的
Figure FDA00030780472100000313
为考虑畸变影响的像素坐标，消除畸变影响后观测点的投影坐标
Figure FDA00030780472100000314
计算为:
Figure FDA00030780472100000315
其中
Figure FDA00030780472100000316
表示图像主点的坐标，Δuij＝[Δuij Δvij 0]T表示畸变偏移，
Figure FDA0003078047210000041
是月面点在世界坐标系中的齐次坐标，
Figure FDA0003078047210000042
表示对角矩阵，R(θi)简写为Ri，
Figure FDA0003078047210000043
和
Figure FDA0003078047210000044
分别表示相机坐标系相对于世界坐标系的旋转矩阵和平移向量，
Figure FDA0003078047210000045
表示陆面观测点在相机坐标系中的z坐标，
令:
Figure FDA0003078047210000046
则将
Figure FDA0003078047210000047
和xij代入公式(3)得到：
Figure FDA0003078047210000048
由
Figure FDA0003078047210000049
得到以下公式:
Figure FDA00030780472100000410
其中，系数矩阵Aij和解向量
Figure FDA00030780472100000411
分别为:
Figure FDA00030780472100000412
利用最小化代数距离方法对上述方程进行求解，令
Figure FDA00030780472100000413
将齐次矩阵方程的求解转化为最小化范数
Figure FDA00030780472100000414
的优化问题；该问题转化为求取系数矩阵Aij奇异值分解后的最小奇异值对应的右向量；将Aij进行奇异值分解，即
Figure FDA00030780472100000415
得到奇异值由大到小分别为σ1，σ2，σ3，对应的右向量矩阵
Figure FDA00030780472100000416
则
Figure FDA00030780472100000417
近似为最小的奇异值σ3对应的右向量
Figure FDA00030780472100000418
即
Figure FDA00030780472100000419
其物理含义为表示从相机光心出发指向
Figure FDA00030780472100000420
的方向向量，因此，
Figure FDA00030780472100000421
计算为:
Figure FDA0003078047210000051
其中
Figure FDA0003078047210000052
表示从相机光心Oi出发指向
Figure FDA0003078047210000053
的方向向量；
Figure FDA0003078047210000054
为
Figure FDA0003078047210000055
的第三行，表示
Figure FDA0003078047210000056
在Z方向的分量，
Figure FDA0003078047210000057
表示从相机光心Oi指向观测点
Figure FDA0003078047210000058
的向量在Z方向上的分量；
Figure FDA0003078047210000059
位于从相机i光心Oi开始的并经过像素点
Figure FDA00030780472100000510
的径向线上；
Figure FDA00030780472100000511
也位于从相机i′的光心Oi′开始的并经过像素点
Figure FDA00030780472100000512
的径向线上；考虑到投影误差，计算两条径向线的共同垂线的中点作为
Figure FDA00030780472100000513
计算两条径向线之间的最小距离作为共同垂直线的长度；用ti和ti′表示两个相机光心的世界坐标，
Figure FDA00030780472100000514
和
Figure FDA00030780472100000515
表示两条径向线的方向向量；通过计算
Figure FDA00030780472100000516
和
Figure FDA00030780472100000517
的外积得到公共垂直线的方向向量；通过计算两条径向线之间的最小距离得到公共垂直线的长度为:
Figure FDA00030780472100000518
其中
Figure FDA00030780472100000519
是两条径向线公共垂直线方向向量，
Figure FDA00030780472100000520
和
Figure FDA00030780472100000521
表示为
Figure FDA00030780472100000522
在两条径向线
Figure FDA00030780472100000523
和
Figure FDA00030780472100000524
上的投影，
Figure FDA00030780472100000525
被估计为两个投影的中间点，即
Figure FDA00030780472100000526
Figure FDA00030780472100000527
和
Figure FDA00030780472100000528
表示为:
Figure FDA00030780472100000529
Figure FDA0003078047210000061
其中
Figure FDA0003078047210000062
步骤S2中提取到的特征点像素坐标uij和ui′j分别对应着在相机Ci和相机Ci′视角下表面观察点Pj的经过透视投影方程产生的像素坐标
Figure FDA0003078047210000063
和
Figure FDA0003078047210000064
将像素坐标uij和ui′j替换公式(3)中的
Figure FDA0003078047210000065
根据上述的步骤即可求得它们在空间中的映射点Pj的两个不同的世界坐标
Figure FDA0003078047210000066
和
Figure FDA0003078047210000067
6.根据权利要求5所述的方法，其特征在于：
由不正确的投影束产生的误差
Figure FDA0003078047210000068
的计算方法具体如下：
将公式(10)与(11)相减得：
Figure FDA0003078047210000069
其中e1、e2、e3表示三个正交单位向量，
Figure FDA00030780472100000610
Figure FDA0003078047210000071
这里A1、A2、A3表示为:
Figure FDA0003078047210000072
Figure FDA0003078047210000073
Figure FDA0003078047210000074
dii′j＝|A1|，根据矢量三乘积公式(a×b)×c＝b(a·c)-a(b·c)和标量三乘积公式a·(b×c)＝b·(a×c)＝c·(a×b)得到：
Figure FDA0003078047210000075
Figure FDA0003078047210000076
Figure FDA0003078047210000077
A2、A3计算为：
Figure FDA0003078047210000078
Figure FDA0003078047210000081
Figure FDA0003078047210000082
将A2＝A3＝0带入式(12)得，
Figure FDA0003078047210000083
则
Figure FDA0003078047210000084
从而计算得到
Figure FDA0003078047210000085
7.根据权利要求4所述的方法，其特征在于：
||p(uij)-p(ui′j)||2表示前后两张图像提取的特征点的相似程度，通过计算前后两张图像提取特征点描述子之间的欧式距离来获得。
8.根据权利要求5所述的方法，其特征在于：
步骤S4具体包括：
假设S2步骤中从已知站点和当前站点拍摄的图像中分别提取出M和N个待匹配特征点分别为x1m，m＝1，2，...，M、x2n，n＝1，2，...，N，则特征点像素坐标分别为u1m，m＝1，2，...，M、u2n，n＝1，2，...，N,对应的特征点描述子分别为p(u1m)，m＝1，2，...，M、p(u2n)，n＝1，2，...，N；
从已知站点图像中提取出的M个特征点x1m，m＝1，2，...，M中取任意一个特征点x1i,则它的像素坐标为u1i,它的表观描述子为p(u1i)；
i.将特征点x1i的像素坐标u1i和当前站点图像提取出的所有的N个特征点x2n，n＝1，2，...，N的像素坐标x2n，n＝1，2，...，N分别带入公式(3)中的
Figure FDA0003078047210000091
依据步骤S3依次求得空间中它们的特征光束之间的距离
Figure FDA0003078047210000092
此时会求出N个特征光束距离的值；将它们存入一个序列，将此序列命名为X，则X的长度为N；寻找出序列X中N个空间距离的最大值X_MAX和最小值X_MIN；将X序列中的所有元素即N个空间距离的值除以最大值X_MAX，即将此序列中元素归一化；此时产生一个新的归一化序列，序列中元素均大于0小于或等于1；将此序列称为new_X，new_X中元素个数也为N；
ii.求特征点x1i的描述子p(u1i)和当前站点拍摄的图像提取出的N个特征点x2n，n＝1，2，...，N的描述子p(u2n)，n＝1，2，...，N之间的欧氏距离||p(u1i)-p(u2n)||2；；求出N个欧式距离的值，将它们存入一个序列，将此序列命名为Y，则序列Y的长度为N；寻找出序列Y中的N个欧式距离的最大值Y_MAX和最小值Y_MIN，将Y序列中的所有元素的值即N个欧式距离的值除以此最大值Y_MAX，即将此序列中的元素归一化；此时会产生一个新序列，序列中元素均大于0小于或等于1，将此序列命名为new_Y，new_Y中元素个数也为N；
iii.将i.ii.求得的归一化序列new_X、new_Y中的元素按照公式(1)中分配的权重分别对应相加α·new_X+(1-α)·new_Y；得到一个新序列，将此序列命名为Z，则序列Z的长度也为N，Z中的元素均大于0小于或等于2；寻找出序列Z中的最小值Z_MIN，称Z_MIN为本次考虑空间逆投影约束的匹配过程的最佳匹配值并返回最佳匹配值Z_MIN在序列Z中的序号Z_arg；
经过步骤i.ii.iii.可以得到从已知站点图像中提取到的M个特征点中任意一个特征点x1i与当前站点图像中提取到的N个特征点x2n，n＝1，2，...，N在联合特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN及最佳匹配值Z_MIN在序列Z中的序号Z_arg；此时Z_arg即为在本次联合特征表观描述子相似度与特征光束距离约束的匹配的过程中，从已知站点图像任意提取出的一个特征点x1i与N个从当前站点图像提取的特征点x2n，n＝1，2，...，N中第Z_arg个特征点的匹配效果最佳；
在上述步骤中仅仅使用在已知站点图像中提取到的M个特征点中的任意一个特征点，即可确定它与当前站点图像提取到的N个特征点在联合特征表观描述子相似度和特征光束距离约束下的最佳匹配值及其序号；利用已知站点的图像中提取到的全部的M个特征点x1m，m＝1，2，...，M，分别确定它与当前站点图像提取到的N个特征点x2n，n＝1，2，...，N在特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN以及最佳匹配对；最佳匹配对由从已知站点图像中提取的全部的M个特征点x1m，m＝1，2，...，M中第i个特征点在全部的M个特征点中的索引和第i个特征点与当前站点图像提取到的全部N个特征点x2n，n＝1，2，...，N在特征表观描述子相似度与特征光束距离相互约束下的最佳匹配值Z_MIN在序列Z中的序号Z_arg构成；因为此时是利用了在已知站点图像中提取到的M个特征点中的每一个特征点，所以会产生M对最佳匹配对及M个最佳匹配值；
最佳匹配对和最佳匹配值是一一对应关系，将上述的M个最佳匹配值存入一个序列，称此序列为R，则R序列的长度也为M；对序列R中的最佳匹配值按从小到大的关系进行排序，取前Q个最佳匹配值并返回其相应的最佳匹配对，得到在特征表观描述子相似度与特征光束距离相互约束下已知站点图像特征点与当前站点图像特征点的前Q个最佳匹配对。
9.根据权利要求5所述的方法，其特征在于：
步骤S5具体包括：
根据在已知站点的巡视器位置和姿态，将步骤S4得到的Q个最佳匹配对作为输入，利用光束法平差求解巡视器在当前站点的位置和姿态；
巡视器的视觉定位模型被归纳为一个关于优化重投影误差的非线性最小二乘问题，定义为：
Figure FDA0003078047210000111
这里
Figure FDA0003078047210000112
表示第j个观察点投影到第i个像平面的像素坐标，uij表示从第i个图像提取的特征点的像素坐标，
Figure FDA0003078047210000113
表示第i台摄像机相对于世界坐标系的平移和旋转，M和N表示摄像机和观察点的数量；
Figure FDA0003078047210000114
由公式(2)计算；
方程(15)的优化问题通过非线性函数
Figure FDA0003078047210000115
的泰勒级数展开来线性化，将其转换为如下线性最小二乘问题：
Figure FDA0003078047210000116
J(Ω，X)∈R2MN×(6M+3N)是雅可比矩阵，并且b∈R2MN×1是误差向量，它们被定义为:
Figure FDA0003078047210000117
Figure FDA0003078047210000121
因此，方程的优化问题(15)可以通过迭代求解方程(16)和使用公式
Figure FDA0003078047210000122
更新
Figure FDA0003078047210000123
和
Figure FDA0003078047210000124
来解决。
10.根据权利要求3所述的方法，其特征在于：
步骤S2中的特征提取具体采用仿射尺度不变性特征提取方法，即Affine-SIFT特征提取法。
CN202010675135.3A 2020-07-14 2020-07-14 一种新的考虑空间逆投影约束的特征匹配与相对定位方法 Active CN111899303B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010675135.3A CN111899303B (zh) 2020-07-14 2020-07-14 一种新的考虑空间逆投影约束的特征匹配与相对定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010675135.3A CN111899303B (zh) 2020-07-14 2020-07-14 一种新的考虑空间逆投影约束的特征匹配与相对定位方法

Publications (2)

Publication Number Publication Date
CN111899303A CN111899303A (zh) 2020-11-06
CN111899303B true CN111899303B (zh) 2021-07-13

Family

ID=73191744

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010675135.3A Active CN111899303B (zh) 2020-07-14 2020-07-14 一种新的考虑空间逆投影约束的特征匹配与相对定位方法

Country Status (1)

Country Link
CN (1) CN111899303B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112861878B (zh) * 2021-02-05 2022-05-20 中国地质大学（武汉） 基于结构偏移特征的异常匹配识别方法
CN114659556B (zh) * 2022-03-03 2024-03-12 中国科学院计算技术研究所 一种面向巡视器的可分离式星表材质识别方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103927739A (zh) * 2014-01-10 2014-07-16 北京航天飞行控制中心 一种基于拼接图像的巡视器定位方法
CN106885571A (zh) * 2017-03-07 2017-06-23 辽宁工程技术大学 一种结合imu和导航影像的月面巡视器快速定位方法
CN106979787A (zh) * 2017-05-23 2017-07-25 辽宁工程技术大学 一种基于立体导航影像的巡视器定位方法
CN108171732A (zh) * 2017-11-24 2018-06-15 中国人民解放军63920部队 一种基于多源图像融合的探测器月面着陆绝对定位方法
CN110021039A (zh) * 2018-11-15 2019-07-16 山东理工大学 序列图像约束的多视角实物表面点云数据初始配准方法
CN110443840A (zh) * 2019-08-07 2019-11-12 山东理工大学 实物表面采样点集初始配准的优化求解方法
CN110489807A (zh) * 2019-07-23 2019-11-22 北京控制工程研究所 一种摇臂悬架结构巡视器的局部精确定位方法
CN110554708A (zh) * 2019-07-24 2019-12-10 北京控制工程研究所 一种适用于月背环境下巡视器自主分离控制方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9390342B2 (en) * 2011-10-17 2016-07-12 Sharp Laboratories Of America, Inc. Methods, systems and apparatus for correcting perspective distortion in a document image

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103927739A (zh) * 2014-01-10 2014-07-16 北京航天飞行控制中心 一种基于拼接图像的巡视器定位方法
CN106885571A (zh) * 2017-03-07 2017-06-23 辽宁工程技术大学 一种结合imu和导航影像的月面巡视器快速定位方法
CN106979787A (zh) * 2017-05-23 2017-07-25 辽宁工程技术大学 一种基于立体导航影像的巡视器定位方法
CN108171732A (zh) * 2017-11-24 2018-06-15 中国人民解放军63920部队 一种基于多源图像融合的探测器月面着陆绝对定位方法
CN110021039A (zh) * 2018-11-15 2019-07-16 山东理工大学 序列图像约束的多视角实物表面点云数据初始配准方法
CN110489807A (zh) * 2019-07-23 2019-11-22 北京控制工程研究所 一种摇臂悬架结构巡视器的局部精确定位方法
CN110554708A (zh) * 2019-07-24 2019-12-10 北京控制工程研究所 一种适用于月背环境下巡视器自主分离控制方法
CN110443840A (zh) * 2019-08-07 2019-11-12 山东理工大学 实物表面采样点集初始配准的优化求解方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
《MER Spirit rover localization: Comparison of ground image– and orbital image–based methods and science applications》;Rongxing Li,et al；;《JOURNAL OF GEOPHYSICAL RESEARCH》;20111231;第116卷(第16期);第1-12页； *
《玉兔2号月球车大间距导航成像的空间分辨率建模分析》;刘传凯 等；;《中国科学: 技术科学》;20191231;第49卷(第11期);第1275-1285页； *

Also Published As

Publication number Publication date
CN111899303A (zh) 2020-11-06

Similar Documents

Publication Publication Date Title
CN108571971B (zh) 一种agv视觉定位系统及方法
WO2020134254A1 (zh) 一种基于强化学习的喷涂机器人轨迹优化方法
CN108051002B (zh) 基于惯性测量辅助视觉的运输车空间定位方法及系统
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN102722697B (zh) 一种无人飞行器视觉自主导引着陆的目标跟踪方法
CN107192376B (zh) 基于帧间连续性的无人机多帧图像目标定位校正方法
CN103927739A (zh) 一种基于拼接图像的巡视器定位方法
CN111899303B (zh) 一种新的考虑空间逆投影约束的特征匹配与相对定位方法
JP2013187862A (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN113706619B (zh) 一种基于空间映射学习的非合作目标姿态估计方法
CN110887486B (zh) 一种基于激光线辅助的无人机视觉导航定位方法
CN112669354A (zh) 一种基于车辆非完整约束的多相机运动状态估计方法
CN116222543B (zh) 用于机器人环境感知的多传感器融合地图构建方法及系统
CN108594255B (zh) 一种激光测距辅助光学影像联合平差方法及系统
CN113074725A (zh) 一种基于多源信息融合的小型水下多机器人协同定位方法及系统
CN115546289A (zh) 一种基于机器人的复杂结构件三维形貌测量方法
CN113570662A (zh) 3d定位来自真实世界图像中地标的系统和方法
CN113345032B (zh) 一种基于广角相机大畸变图的初始化建图方法及系统
CN112508933B (zh) 一种基于复杂空间障碍物定位的柔性机械臂运动避障方法
CN116681733B (zh) 一种空间非合作目标近距离实时位姿跟踪方法
CN111145267B (zh) 基于imu辅助的360度全景视图多相机标定方法
Konoplin et al. System for identifying target objects to perform manipulative operations by unmanned underwater vehicles
So et al. Visual odometry for a hopping rover on an asteroid surface using multiple monocular cameras
Fuchs et al. Advanced 3-D trailer pose estimation for articulated vehicles

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