CN113822934B - 基于逆投影的多特征融合视觉定位方法 - Google Patents

基于逆投影的多特征融合视觉定位方法 Download PDF

Info

Publication number
CN113822934B
CN113822934B CN202111068322.6A CN202111068322A CN113822934B CN 113822934 B CN113822934 B CN 113822934B CN 202111068322 A CN202111068322 A CN 202111068322A CN 113822934 B CN113822934 B CN 113822934B
Authority
CN
China
Prior art keywords
camera
target
pose
features
coordinates
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
CN202111068322.6A
Other languages
English (en)
Other versions
CN113822934A (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 CN202111068322.6A priority Critical patent/CN113822934B/zh
Publication of CN113822934A publication Critical patent/CN113822934A/zh
Application granted granted Critical
Publication of CN113822934B publication Critical patent/CN113822934B/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
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/253—Fusion techniques of extracted features
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/80—Geometric correction
    • 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/10004—Still image; Photographic image
    • 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/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • General Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

基于逆投影的多特征融合视觉定位方法，该方法包括：1)在相机图像中提取空间目标正向投影形成的特征像素坐标；2)确定空间中有N个相机，其中N1个为静态相机；N2个为动态相机；根据相机位姿是否为固定参数设置合适的相机位姿或空间目标位姿作为算法初始迭代值；3)根据已知的相机位姿信息或目标位姿信息和目标特征的像素坐标在空间中重建特征的三维坐标；4)根据重建出的空间坐标利用已知的特征之间的固定约束建立相机对多类特征的相对测量的统一的位姿优化模型；5)利用非线性优化方法迭代求解位姿测量优化模型，获取空间目标或机械臂臂载相机自身的精确位姿信息，从而实现对机械臂逐步精确地引导控制，执行预定的工作程序和任务。

Description

基于逆投影的多特征融合视觉定位方法
技术领域
本发明涉及视觉测量、视觉定位技术领域，具体涉及一种基于逆投影的多特征融合视觉定位方法。
背景技术
随着科学技术的发展，机械臂不但在地面的应用越来越广泛，在地外天体表面着陆探测中也发挥越来越重要的作用，尤其对于地外天体表面的采样操作具有无可替代性。
地外天体表面着陆采样与返回是世界航天大国探索深空和拓展人类认知外星球和太阳系的重要手段。机械臂是地外天体表面着陆采样任务中必不可少的关键设备，能够按照预定的程序自主进行采样任务或者在地面遥操作中心的控制下进行采样作业，可以代替航天员完成各种采样和样品转移、装罐等操作，实现地外天体表面的无人化采样。无论是执行自主进行采样任务或在地面遥操作中心的控制下进行采样作业，对机械臂进行精确的引导控制是实现采样作业的关键技术之一。然而现有采样任务中机械臂执行采样、放样、抓罐和放罐等操作中的视觉引导操作问题未建立统一约束求解模型进而给出稳定精度的求解方法。
中国发明专利申请(申请号：201611069552.3)公开了一种“视觉定位方法和视觉定位装置”，该视觉定位方法包括：在识别对象上设置多个标识点：根据该多个标识点之间的位置关系，对该多个标识点进行拓扑区分；根据该多个标识点之间的拓扑关系，建立标识点三维坐标的线性方程组，并求解该多个标识点的三维坐标；根据标识点的三维坐标，建立标识点坐标系；和求解该标识点坐标系的旋转矩阵，以得到该识别对象的姿态信息。通过根据本发明的视觉定位方法和视觉定位装置，可以实现不具有显著的自然特征的识别对象的姿态的实时和高精度定位。
该专利的执行需前提是在识别位姿的对象上设置多个标识点，根据该多个标识点之间的位置关系进一步计算得到该识别对象的姿态信息，而我们提出的方法综合利用了人工设置的标志特征和识别对象本身具有的显著自然特征的特点，可以极大地提升视觉定位的精度以及鲁棒性，适用范围更广泛。
发明内容
针对地外天体采样任务中机械臂执行采样、放样、抓罐和放罐等操作中的视觉引导操作存在的精度不高以及未建立统一优化模型求解的问题，本发明提出了基于逆投影的多特征融合视觉定位方法，通过对静态相机测量动态目标和动态相机测量静态目标的过程进行抽象建模，建立了双目、单目相机对自然特征(如圆形目标)和人工设置特征(如靶标特征)的组合定位优化模型，实现了不同操作应用场景下适应不同相机数量的多类特征组合定位，有效提高地外天体采样遥操作中利用机械臂采集样品、放置样品、抓取密封罐和放置密封罐等关键环节的操作精度和自动化程度。
为实现上述目的，本发明采用以下技术方案：基于逆投影的多特征融合视觉定位方法包括：
步骤一：在相机图像中提取目标特征的像素坐标，针对自然特征，如圆形特征，检测其轮廓像素坐标，针对人工设置的靶标特征如棋盘格特征检测其标志点像素坐标。
步骤二：确定空间中存在N个相机，其中N1个为静态相机；N2个为动态相机，当属于相机位姿固定，需要测量特征在空间中的位姿情况下，设置合适的目标位姿X0,Ψ0作为初始迭代值；反之，当属于目标位姿固定，需要反求相机在空间中的位姿情况下，设置合适的相机位姿t0,θ0作为初始迭代值。
步骤三：根据前述步骤一中的像素坐标，已知的目标位姿信息或已知的相机位姿信息和相机位姿初始迭代值或目标位姿初始迭代值在空间中重建特征的三维坐标。具体为：
设第i个相机坐标系在世界坐标系的位姿为
Figure GDA0003634904910000031
Ri＝R(θi)表示相机i的坐标系相对于世界坐标系的旋转变换矩阵，N和M分别表示相机和观测点的数量；空间中任意一个观测点
Figure GDA0003634904910000032
其在相机i的坐标系中坐标为
Figure GDA0003634904910000033
投影图像坐标uij＝(uij,vij)T可以利用透视投影成像模型计算为：
Figure GDA0003634904910000034
其中，
Figure GDA0003634904910000035
分别表示对应的齐次坐标，
Figure GDA0003634904910000036
表示从第j个观测点到第i个相机图像的透视投影变换，
Figure GDA0003634904910000037
表示与投影图像坐标相关的畸变参数矩阵；
如附图4所示，根据空间中任意特征点的逆投影描述方式，任意目标特征点
Figure GDA0003634904910000038
可表示为从相机光心(相机坐标系原点)出发指向空间目标特征点的方向向量,则
Figure GDA0003634904910000039
可以计算为：
Figure GDA00036349049100000310
其中,
Figure GDA00036349049100000311
为连接相机光心与投影图像坐标uij校正后坐标u′ij的方向向量,令
Figure GDA00036349049100000312
为u′ij的齐次坐标，则满足：
Figure GDA0003634904910000041
其中
Figure GDA0003634904910000042
表示图像主点的坐标,Δuij＝[Δuij Δvij 0]T表示畸变偏移,R(θi)简写为Ri，
Figure GDA0003634904910000043
表示第i个相机坐标系相对于世界坐标系的旋转矩阵，
Figure GDA0003634904910000044
表示目标观测点在相机坐标系中的z坐标,vij计算为：
vij＝Ri(u′ij,v′ij,fi)T/||(u′ij,v′ij,fi)||2 (4)
sij是尺度因子,表示沿着vij方向伸展的长度；fi代表第i个相机的焦距。上述
Figure GDA0003634904910000045
的表达可解释为以ti为起点,沿着vij方向伸展任意长度的点的集合；si＞0,则
Figure GDA0003634904910000046
表示空间中以ti为起点，方向向量为vij的射线；
将相机视场内的特征按照一定准则分为K组，其中人工设置特征和自然特征分别分在Kd和Kr组；以特征的共面性作为准则，对si的表征计算如下：令第k组特征平面上某一特征点的空间坐标为
Figure GDA0003634904910000047
该平面的法向为
Figure GDA0003634904910000048
则：
nk＝Rz(φk)Rx(ψk)n0,
＝[sinφk sinψk,-cosφk sinψk,cosψk]T,
其中n0＝[0 0 1]T，Ψk＝[ψk,κk,φk]T表示该平面坐标系相对于世界坐标系的姿态角度，由此可得该平面的代数方程为
Figure GDA0003634904910000049
其中zkj为
Figure GDA00036349049100000410
在以
Figure GDA00036349049100000411
为原点的局部坐标系中的坐标向量在
Figure GDA00036349049100000412
方向上的投影分量；将公式(10)带入平面的代数方程可得：
Figure GDA00036349049100000413
由此空间点世界坐标
Figure GDA00036349049100000414
可计算为：
Figure GDA0003634904910000051
步骤四：建立相机对自然特征(如圆形特征)和人工设置靶标特征(如棋盘格)的相对测量的统一的测量优化模型。根据已知人工设置特征的相对位置和自然特征的几何形状特性构建误差方程；假设仅有靶标和圆形目标两类特征，则逆投影误差函数写作：
Figure GDA0003634904910000052
其中
Figure GDA0003634904910000053
为第i个相机中靶标特征逆投影产生的误差，
Figure GDA0003634904910000054
为第i个相机中圆形目标特征逆投影产生的误差，
Figure GDA0003634904910000055
表示第k组特征平面中各个靶标点相对于参考位置
Figure GDA0003634904910000056
的偏移量，rk表示圆形目标的半径；α表示联合靶标特征和圆形目标特征建立的误差函数在两类特征之间分配的权重因子；根据上述建立的多特征融合的误差表达式，最小化逆投影误差
Figure GDA0003634904910000057
可实现对
Figure GDA0003634904910000058
或ti,θi的求解，即得到以下优化模型：
Figure GDA0003634904910000059
在实际应用中可根据测量需求对变量
Figure GDA00036349049100000510
进行已知量和未知量的确定。对于静态相机测量动态目标的情况，如采样区双目相机测量采样机械臂末端位姿，Ωi为固定已知量，
Figure GDA00036349049100000511
为未知量；对于动态相机对静态目标的测量，如测量执行放样、抓罐、放罐操作时测量采样器相对目标罐的相对位姿，
Figure GDA00036349049100000512
为已知量，Ωi为未知量。
步骤五：针对公式(7)提出的最小化逆投影误差
Figure GDA00036349049100000513
可以将其构造为一个非线性最小二乘问题即：
Figure GDA0003634904910000061
那么可以采用高斯-牛顿法或Levenberg-Marquardt(L-M)算法等非线性优化算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息
Figure GDA0003634904910000062
或相机位姿信息ti,θi的精确求解。下表I为使用L-M算法计算空间目标或相机的精确位姿的具体步骤。
表I 使用L-M法计算空间目标或相机的精确位姿
Figure GDA0003634904910000063
得到空间目标相对于相机的相对位姿关系或相机相对于空间目标的精确位姿后可以进一步转换到世界坐标系下实现对机械臂精确的引导控制，保证地外天体采样遥操作中采样、放样、抓罐和放罐等操作的顺利进行。
附图说明
图1.本发明定位方法框图；
图2.是着陆器、上升器和机械臂上的各类相机；
图3.多特征融合视觉测量模型示意图；
图4.基于逆投影的目标特征点几何表征示意图。
其中：上升器1；着陆器2；四自由度机械臂3；采样器相机甲4；采样器相机乙5；臂载相机6；采样区观测双目相机A/B 7；监视相机C 8；监视相机D 9；机械臂基座10；机械臂末端圆盘11；密封罐12；棋盘状靶标13；机械臂第1关节3.1；机械臂第2关节3.2；机械臂第3关节3.3；机械臂第4关节3.4。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
我国嫦娥五号探测器包括着陆器携带细长结构的四自由度机械臂3，在地面遥操作中心的控制下，利用该机械臂完成表取采样、样品放入密封罐12、样品密封罐12抓取和样品密封罐12放置到上升器1上等操作任务。由于细长机械臂在月面微重力作用下的柔性形变不可预知，因此利用视觉引导的方式对其完成精确操作控制。在嫦娥五号着陆器2、上升器1上带有多个相机，如图2所示，主要包括采样区观测双目相机7、采样器相机甲4、采样器相机乙5、臂载相机6，利用这些相机测量引导完成对采样、放样、抓罐和放罐等操作任务。其中采样区双目观测相机A/B 11固定在着陆器2上，用来在采样过程中对机械臂末端自然特征目标进行视觉定位，监视相机C 8固定在着陆器2上，监视相机D 9固定在上升器1上；臂载相机6固定在四自由度机械臂3上，采样器甲4、采样器乙5相机固定在机械臂末端，臂载相机6和采样器相机用来完成在采样、放样、抓罐和放罐等操作过程中对相机的自身位姿进行视觉定位。如图3所示，机械臂末端圆盘11作为空间圆形目标，在相机中成像为圆形特征，静态相机固定在着陆器2基座上。在放样密封罐12附近装有人工设置的棋盘状靶标13，动态相机即臂载相机和放样器相机安装在机械臂末端。四自由度机械臂包括第一关节3.1；机械臂第2关节3.2；机械臂第3关节3.3；机械臂第4关节3.4，机械臂整体固定在机械臂基座10上。
本发明针对地外天体表取采样过程中的采样、放样、抓罐和放罐等操作中的视觉引导问题，提出了基于逆投影的多特征融合视觉定位算法框架，通过对静态相机测量动态目标和动态相机测量静态目标的过程进行抽象建模，建立了双目、单目相机对自然特征如圆形目标和人工设置特征如靶标特征的组合定位优化模型，最后利用非线性优化算法解决此优化问题，从而获取各类特征在空间中的精确位姿或反解动态相机在空间中的精确位姿达到对各类操作精确视觉引导的效果。
如图1所示，本发明基于逆投影的多特征融合视觉定位方法包括以下步骤：
步骤一：在相机图像中提取空间目标正向投影形成的特征像素坐标；针对自然特征检测其轮廓像素坐标，针对人工设置的特征如靶标特征检测其标志点像素坐标；
步骤二：确定空间中有N个相机，其中N1个为静态相机；N2个为动态相机。当属于相机位姿固定，需要测量空间目标位姿的情况下，设置合适的目标位姿X0,Ψ0作为算法初始迭代值；当属于目标位姿为已知，需要反求相机在空间中的位姿情况下，设置合适的相机位姿t0,θ0作为算法初始迭代值；在相机视场中包含自然特征如圆形目标、人工设置特征如靶标特征两类特征，自然特征的几何尺寸需要为已知、人工设置特征之间的相对三维坐标需要为已知；
步骤三：根据已知的相机位姿信息或目标位姿信息和目标特征的像素坐标反向利用透视投影模型在空间中重建特征的三维坐标；具体步骤为：
假设第i个相机在世界坐标系的位姿为
Figure GDA0003634904910000091
Ri＝R(θi)表示相机i的坐标系相对于世界坐标系的旋转变换矩阵，M和N分别表示观测点和相机的数量。空间中任意一个观测点
Figure GDA0003634904910000092
其在相机i的坐标系中坐标为
Figure GDA0003634904910000093
投影图像坐标uij＝(uij,vij)T可以利用透视投影成像模型计算为：
Figure GDA0003634904910000094
其中，
Figure GDA0003634904910000095
分别表示对应的齐次坐标，
Figure GDA0003634904910000096
表示从第j个月面观测点到第i个相机图像的透视投影变换，
Figure GDA0003634904910000097
表示与投影图像坐标相关的畸变参数矩阵。
根据空间中的任意目标特征点的逆投影描述方式，空间中的任意目标特征点
Figure GDA0003634904910000098
可表示为从相机光心(相机坐标系原点)出发指向目标特征点的方向向量,如图4所示,则
Figure GDA0003634904910000101
可以计算为：
Figure GDA0003634904910000102
其中,
Figure GDA0003634904910000103
为连接相机光心与投影图像坐标uij校正后坐标ui′j的方向向量,令
Figure GDA0003634904910000104
为ui′j的齐次坐标，则满足：
Figure GDA0003634904910000105
其中
Figure GDA0003634904910000106
表示图像主点的坐标,Δuij＝[Δuij Δvij 0]T表示畸变偏移,R(θi)简写为Ri，
Figure GDA0003634904910000107
表示第i个相机坐标系相对于世界坐标系的旋转矩阵，
Figure GDA0003634904910000108
表示目标观测点在相机坐标系中的z坐标,则vij计算为：
vij＝Ri(u′ij,v′ij,fi)T/||(u′ij,v′ij,fi)||2 (4)
sij是尺度因子,表示沿着vij方向伸展的长度；fi代表第i个相机的焦距。上述
Figure GDA0003634904910000109
的表达可解释为以ti为起点,沿着vij方向伸展任意长度的点的集合,si＞0则
Figure GDA00036349049100001010
表示空间中过ti点,方向向量为vij的射线。
由公式(2)可知，多类特征融合的关键是对各类特征的尺度因子si的统一描述，为此提出以下表征方式。假设可将相机视场内的特征按照一定准则分为K组，其中人工设置特征(如靶标特征)和自然特征(如圆形目标)分别分在Kd和Kr组。以特征的共面性作为准则，对si的表征计算如下：令第k组特征平面上某一特征点的空间坐标为
Figure GDA00036349049100001011
该平面的法向为
Figure GDA00036349049100001012
则：
Figure GDA00036349049100001013
其中n0＝[0 0 1]T，Ψk＝[ψk,κk,φk]T表示平面坐标系相对于世界坐标系的姿态，由此可得该平面的代数方程为
Figure GDA00036349049100001014
其中zkj为
Figure GDA00036349049100001015
在以
Figure GDA00036349049100001016
为原点的局部坐标系中的坐标向量在
Figure GDA0003634904910000111
方向上的投影分量。将公式(2)带入可得
Figure GDA0003634904910000112
由此
Figure GDA0003634904910000113
可计算为：
Figure GDA0003634904910000114
步骤四：根据重建出的空间坐标利用已知的特征之间的固定约束建立相机对自然特征和人工特征的相对测量的统一的测量优化模型。具体步骤为：
根据已知人工设置的特征之间的相对位置和自然特征的几何形状特性构建误差方程。假设仅有靶标和圆形目标两类特征，则逆投影误差可以写作：
Figure GDA0003634904910000115
其中
Figure GDA0003634904910000116
为第i个相机中靶标特征逆投影产生的误差，
Figure GDA0003634904910000117
为第i个相机中圆形目标特征逆投影产生的误差，
Figure GDA0003634904910000118
表示第k组特征平面中各个靶标点相对于参考位置
Figure GDA0003634904910000119
的偏移量，rk表示圆形目标的半径。最小化逆投影误差
Figure GDA00036349049100001110
可实现对
Figure GDA00036349049100001111
和ti,θi求解，即得到以下优化模型：
Figure GDA00036349049100001112
在实际应用中可根据测量需求对变量
Figure GDA00036349049100001113
进行已知量和未知量的确定。对于静态相机测量动态目标的情况，如采样区双目相机测量采样机械臂末端位姿，Ωi为固定已知量，
Figure GDA00036349049100001114
为未知量；对于动态相机对静态目标的测量，如测量执行放样、抓罐、放罐操作时测量采样器相对目标罐的相对位姿，
Figure GDA00036349049100001115
为已知量，Ωi为未知量。
步骤五：利用非线性优化方法迭代求解位姿测量优化模型，获取空间目标或机械臂臂载相机自身的精确位姿信息，从而实现对机械臂逐步引导控制，执行预定的工作程序，具体为：
针对上述公式(8)提出的最小化逆投影误差
Figure GDA0003634904910000121
将其构造为一个非线性最小二乘问题：
Figure GDA0003634904910000122
那么可以采用高斯-牛顿法或Levenberg-Marquardt(L-M)算法等非线性优化算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息
Figure GDA0003634904910000123
或相机位姿信息ti,θi的精确求解。下表I为使用L-M算法计算空间目标或相机的精确位姿的具体步骤。
表I 使用L-M法计算空间目标或相机的精确位姿
Figure GDA0003634904910000124
得到空间目标相对于相机的相对位姿关系或相机相对于空间目标的精确位姿后可以进一步转换到世界坐标系下实现对机械臂精确的引导控制，保证地外天体采样遥操作中采样、放样、抓罐和放罐等操作的顺利进行。

Claims (7)

1.基于逆投影的多特征融合视觉定位方法，包括以下步骤：
步骤一：在相机图像中提取目标特征的像素坐标，针对自然特征检测其轮廓像素坐标，针对人工设置的特征检测其标志点像素坐标；
步骤二：确定空间中存在N个相机，其中共存在N1个为静态相机和N2个为动态相机；当测量情况属于相机位姿固定，需要测量图像中的特征在空间中的位姿情况下，设置合适的目标位姿X0，Ψ0作为算法初始迭代值；反之，当属于目标位姿固定，需要反求相机在空间中的位姿情况下，设置合适的相机位姿t0，Θ0作为算法初始迭代值；
步骤三：根据前述提取到的像素坐标，已知的目标位姿信息或相机位姿信息和相机或目标的位姿初始迭代值在空间中重建特征的三维坐标；
步骤四：建立相机对自然特征和人工设置特征的相对测量的统一的测量优化模型；根据已知人工设置特征的相对位置和自然特征的几何形状特性构建误差方程；
所述构建的误差方程为：
假设仅存在靶标和圆形目标两类特征，则逆投影误差函数写作：
Figure 811775DEST_PATH_IMAGE003
其中
Figure DEST_PATH_IMAGE004
为第i个相机中靶标特征逆投影产生的误差，
Figure 60354DEST_PATH_IMAGE005
为第i个相机中圆形目标特征逆投影产生的误差，
Figure DEST_PATH_IMAGE006
表示第k组特征平面中各个靶标点相对于参考位置
Figure 658825DEST_PATH_IMAGE007
的偏移量，rk表示圆形目标的半径；
Figure 942039DEST_PATH_IMAGE009
表示联合靶标特征和圆形目标特征建立的误差函数在两类特征之间分配的权重因子；
Figure DEST_PATH_IMAGE010
表示空间中任意一个观测点；根据上述建立的多特征融合的误差表达式，最小化逆投影误差
Figure 230413DEST_PATH_IMAGE011
可实现对
Figure DEST_PATH_IMAGE012
或
Figure 966288DEST_PATH_IMAGE013
的求解，即得到以下优化模型：
Figure DEST_PATH_IMAGE014
根据测量需求对变量
Figure 368451DEST_PATH_IMAGE015
进行已知量和未知量的确定；对于静态相机测量动态目标的情况，如采样区双目相机测量采样机械臂末端位姿，
Figure DEST_PATH_IMAGE016
为固定已知量，
Figure 506171DEST_PATH_IMAGE017
为未知量；对于动态相机对静态目标的测量，如测量执行放样、抓罐、放罐操作时测量采样器相对目标罐的相对位姿，
Figure DEST_PATH_IMAGE018
为已知量，
Figure 968376DEST_PATH_IMAGE019
为未知量；
步骤五：迭代求解非线性误差函数，实现对目标的位姿信息或相机位姿信息的精确求解。
2.根据权利要求1所述的基于逆投影的多特征融合视觉定位方法，其特征在于：所述重建特征的三维坐标，具体为：
设第i个相机的相机坐标系在世界坐标系的位姿为
Figure DEST_PATH_IMAGE020
表示相机i的坐标系相对于世界坐标系的旋转变换矩阵，N和M分别表示相机和观测点的数量；空间中任意一个观测点
Figure 925968DEST_PATH_IMAGE021
其在相机i的坐标系中坐标为
Figure DEST_PATH_IMAGE022
投影图像坐标
Figure 131821DEST_PATH_IMAGE023
可以利用透视投影成像模型计算为：
Figure DEST_PATH_IMAGE024
其中，
Figure 124048DEST_PATH_IMAGE025
分别表示对应的齐次坐标，
Figure DEST_PATH_IMAGE026
表示从第j个观测点到第i个相机图像的透视投影变换，
Figure 757155DEST_PATH_IMAGE027
表示与投影坐标相关的畸变参数矩阵；
根据空间中任意特征点的逆投影描述方式，任意观测点
Figure DEST_PATH_IMAGE028
可表示为从相机光心出发指向空间目标特征点的方向向量, 则
Figure 467622DEST_PATH_IMAGE029
可以计算为：
Figure DEST_PATH_IMAGE030
其中,
Figure 946008DEST_PATH_IMAGE031
为连接相机光心与投影图像坐标
Figure DEST_PATH_IMAGE032
校正后坐标
Figure 58320DEST_PATH_IMAGE033
的方向向量,令
Figure DEST_PATH_IMAGE034
为
Figure 862328DEST_PATH_IMAGE035
的齐次坐标，则满足：
Figure DEST_PATH_IMAGE036
其中
Figure 794512DEST_PATH_IMAGE037
表示图像主点的坐标,
Figure DEST_PATH_IMAGE038
表示畸变偏移,R（Θi）简写为
Figure DEST_PATH_IMAGE040
表示第i个相机坐标系相对于世界坐标系的旋转矩阵，
Figure 323636DEST_PATH_IMAGE041
表示目标观测点在相机坐标系中的z坐标,
Figure DEST_PATH_IMAGE042
计算为：
Figure 298545DEST_PATH_IMAGE043
Figure DEST_PATH_IMAGE044
是尺度因子,表示沿着
Figure 718025DEST_PATH_IMAGE045
方向伸展的长度；fi代表第i个相机的焦距；上述
Figure 69372DEST_PATH_IMAGE047
的表达可解释为以
Figure DEST_PATH_IMAGE048
为起点,沿着
Figure 625118DEST_PATH_IMAGE049
方向伸展任意长度的点的集合；
Figure DEST_PATH_IMAGE050
则
Figure 36508DEST_PATH_IMAGE051
表示空间中以
Figure DEST_PATH_IMAGE052
为起点，方向向量为
Figure 943284DEST_PATH_IMAGE053
的射线；
将相机视场内的特征按照一定准则分为K组，其中人工设置特征和自然特征分别分在
Figure 832742DEST_PATH_IMAGE054
和
Figure DEST_PATH_IMAGE055
组；以特征的共面性作为准则，对si的表征计算如下：令第k组特征平面上某一特征点的空间坐标为
Figure DEST_PATH_IMAGE057
该平面的法向为
Figure 356445DEST_PATH_IMAGE058
则：
Figure DEST_PATH_IMAGE059
其中
Figure 484938DEST_PATH_IMAGE060
表示该平面坐标系相对于世界坐标系的姿态角度，由此可得该平面的代数方程为
Figure DEST_PATH_IMAGE061
其中
Figure 178087DEST_PATH_IMAGE062
为
Figure DEST_PATH_IMAGE063
在以
Figure 708426DEST_PATH_IMAGE064
为原点的局部坐标系中的坐标向量在
Figure DEST_PATH_IMAGE065
方向上的投影分量；将公式(2)带入平面的代数方程可得：
Figure 193109DEST_PATH_IMAGE066
由此空间点世界坐标
Figure DEST_PATH_IMAGE067
可计算为：
Figure 74477DEST_PATH_IMAGE068
。
3.根据权利要求1所述的基于逆投影的多特征融合视觉定位方法，其特征在于：所述位姿信息的精确求解方法为：
针对公式（7）提出的最小化逆投影误差
Figure DEST_PATH_IMAGE069
可以将其构造为一个非线性最小二乘问题即：
Figure 571318DEST_PATH_IMAGE070
接下来迭代求解此非线性最小二乘问题实现对目标位姿信息
Figure DEST_PATH_IMAGE071
或相机位姿信息
Figure 956163DEST_PATH_IMAGE072
的精确求解。
4.根据权利要求1所述的基于逆投影的多特征融合视觉定位方法，其特征在于：所说的静态相机N1固定在探测器系统着陆器上，也可以固定在上升器上。
5.根据权利要求1所述的基于逆投影的多特征融合视觉定位方法，其特征在于：所说的动态相机N2为机械臂臂载相机，也可以是采样器相机。
6.根据权利要求1所述的基于逆投影的多特征融合视觉定位方法，其特征在于：所说的相机包括采样区观测双目相机、采样器相机、臂载相机。
7.根据权利要求1所述的基于逆投影的多特征融合视觉定位方法，其特征在于：所说的步骤三中采用高斯-牛顿法，也可以采用Levenberg-Marquardt(L-M)算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息
Figure DEST_PATH_IMAGE073
或相机位姿信息
Figure 614677DEST_PATH_IMAGE074
的精确求解。
CN202111068322.6A 2021-09-13 2021-09-13 基于逆投影的多特征融合视觉定位方法 Active CN113822934B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111068322.6A CN113822934B (zh) 2021-09-13 2021-09-13 基于逆投影的多特征融合视觉定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111068322.6A CN113822934B (zh) 2021-09-13 2021-09-13 基于逆投影的多特征融合视觉定位方法

Publications (2)

Publication Number Publication Date
CN113822934A CN113822934A (zh) 2021-12-21
CN113822934B true CN113822934B (zh) 2022-08-02

Family

ID=78914418

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111068322.6A Active CN113822934B (zh) 2021-09-13 2021-09-13 基于逆投影的多特征融合视觉定位方法

Country Status (1)

Country Link
CN (1) CN113822934B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115493499B (zh) * 2021-12-30 2024-04-19 北京航天飞行控制中心 一种圆柱体或类圆柱体的装配方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN205538161U (zh) * 2016-02-03 2016-08-31 上海仪万光电科技有限公司 一种无限共轭光路测量光学镜头的调制传递函数的装置
CN112862874A (zh) * 2021-04-23 2021-05-28 腾讯科技（深圳）有限公司 点云数据匹配方法、装置、电子设备及计算机存储介质
CN113052905A (zh) * 2021-03-31 2021-06-29 中国人民解放军63920部队 基于双目逆投影变换的圆形目标位姿测量方法和装置
CN113313810A (zh) * 2021-06-18 2021-08-27 广东工业大学 一种透明物体的6d姿态参数计算方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8452123B2 (en) * 2008-01-18 2013-05-28 California Institute Of Technology Distortion calibration for optical sensors
CN105675266B (zh) * 2016-02-03 2018-06-26 上海仪万光电科技有限公司 无限共轭光路测量光学镜头的调制传递函数的装置及方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN205538161U (zh) * 2016-02-03 2016-08-31 上海仪万光电科技有限公司 一种无限共轭光路测量光学镜头的调制传递函数的装置
CN113052905A (zh) * 2021-03-31 2021-06-29 中国人民解放军63920部队 基于双目逆投影变换的圆形目标位姿测量方法和装置
CN112862874A (zh) * 2021-04-23 2021-05-28 腾讯科技（深圳）有限公司 点云数据匹配方法、装置、电子设备及计算机存储介质
CN113313810A (zh) * 2021-06-18 2021-08-27 广东工业大学 一种透明物体的6d姿态参数计算方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
《Line-based Monocular Graph SLAM》;Dong RF et al;《IEEE》;20171211;全文 *
《基于特征融合的非合作航天器位姿测量方法》;王志超等;《高技术通讯》;20140113;第23卷(第11期);正文第2、3部分 *

Also Published As

Publication number Publication date
CN113822934A (zh) 2021-12-21

Similar Documents

Publication Publication Date Title
JP5627325B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
Peng et al. Virtual stereovision pose measurement of noncooperative space targets for a dual-arm space robot
JP6004809B2 (ja) 位置姿勢推定装置、情報処理装置、情報処理方法
CN110116407A (zh) 柔性机器人位姿测量方法及装置
US9355453B2 (en) Three-dimensional measurement apparatus, model generation apparatus, processing method thereof, and non-transitory computer-readable storage medium
CN105910535B (zh) 一种用于舱段自动对接的单双目位姿偏差测量方法
CN108324373B (zh) 一种基于电磁定位系统的穿刺手术机器人精确定位实现方法
WO2012014545A1 (ja) ３次元物体認識装置及び３次元物体認識方法
CN110849331B (zh) 基于三维点云数据库模型的单目视觉测量与地面试验方法
JP2012128661A (ja) 情報処理装置、情報処理方法及びプログラム
CN109877840A (zh) 一种基于相机光轴约束的双机械臂标定方法
CN114011608B (zh) 基于数字孪生的喷涂工艺优化系统及其喷涂优化方法
CN110298854A (zh) 基于在线自适应与单目视觉的飞行蛇形臂协同定位方法
CN109087355A (zh) 基于迭代更新的单目相机位姿测量装置与方法
CN113822934B (zh) 基于逆投影的多特征融合视觉定位方法
Lim Point cloud modeling using the homogeneous transformation for non-cooperative pose estimation
Liu et al. A high-accuracy pose measurement system for robotic automated assembly in large-scale space
JP2022183032A (ja) カメラガイド装置のハンドアイ較正
Bao et al. 3D perception-based collision-free robotic leaf probing for automated indoor plant phenotyping
Li Relative pose measurement of moving rigid bodies based on binocular vision
CN110490934B (zh) 基于单目相机与机器人的混合机立式桨叶姿态检测方法
Rüther et al. The narcissistic robot: Robot calibration using a mirror
Figueroa et al. Joint origin identification of articulated robots with marker-based multi-camera optical tracking systems
Lim et al. Pose estimation using a flash lidar
CN111366162B (zh) 基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法

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