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

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

Info

Publication number
CN113822934A
CN113822934A CN202111068322.6A CN202111068322A CN113822934A CN 113822934 A CN113822934 A CN 113822934A CN 202111068322 A CN202111068322 A CN 202111068322A CN 113822934 A CN113822934 A CN 113822934A
Authority
CN
China
Prior art keywords
camera
target
features
pose
projection
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.)
Granted
Application number
CN202111068322.6A
Other languages
English (en)
Other versions
CN113822934B (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 BDA0003259423170000031
Ri＝R(θi)表示相机i的坐标系相对于世界坐标系的旋转变换矩阵，N和M分别表示相机和观测点的数量；空间中任意一个观测点
Figure BDA0003259423170000032
其在相机i的坐标系中坐标为
Figure BDA0003259423170000033
投影坐标uij＝(uij,vij)T可以利用透视投影成像模型计算为：
Figure BDA0003259423170000034
其中，
Figure BDA0003259423170000035
分别表示对应的齐次坐标，
Figure BDA0003259423170000036
表示从第j个观测点到第i个相机图像的透视投影变换，
Figure BDA0003259423170000037
表示与投影坐标相关的畸变参数矩阵；
如附图4所示，根据空间中任意特征点的逆投影描述方式，任意目标特征点
Figure BDA0003259423170000038
可表示为从相机光心(相机坐标系原点)出发指向空间目标特征点的方向向量,则
Figure BDA0003259423170000039
可以计算为：
Figure BDA00032594231700000310
其中,
Figure BDA00032594231700000311
为连接相机光心与图像坐标uij校正后坐标u′ij的方向向量，令
Figure BDA00032594231700000312
为u′ij的齐次坐标，则满足：
Figure BDA00032594231700000313
其中
Figure BDA00032594231700000314
表示图像主点的坐标,Δuij＝[Δuij Δvij 0]T表示畸变偏移,
Figure BDA00032594231700000315
表示目标观测点在相机坐标系中的z坐标，vij计算为：
Figure BDA00032594231700000316
sij是尺度因子，表示沿着vij方向伸展的长度。上述
Figure BDA00032594231700000317
的表达可解释为以ti为起点，沿着vij方向伸展任意长度的点的集合；si＞0，则
Figure BDA00032594231700000318
表示空间中以ti为起点，方向向量为vij的射线；
将相机视场内的特征按照一定准则分为K组，其中人工设置特征和自然特征分别分在Kd和Kr组；以特征的共面性作为准则，对si的表征计算如下：令第k组特征平面上某一特征点的空间坐标为
Figure BDA0003259423170000041
该平面的法向为
Figure BDA0003259423170000042
则：
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 BDA0003259423170000043
其中zkj为
Figure BDA0003259423170000044
在以
Figure BDA0003259423170000045
为原点的局部坐标系中的坐标向量在
Figure BDA0003259423170000046
方向上的投影分量；将公式(10)带入平面的代数方程可得：
Figure BDA0003259423170000047
由此空间点世界坐标
Figure BDA0003259423170000048
可计算为：
Figure BDA0003259423170000049
步骤四：建立相机对自然特征(如圆形特征)和人工设置靶标特征(如棋盘格)的相对测量的统一的测量优化模型。根据已知人工设置特征的相对位置和自然特征的几何形状特性构建误差方程；假设仅有靶标和圆形目标两类特征，则逆投影误差函数写作：
Figure BDA00032594231700000410
其中
Figure BDA00032594231700000411
为第i个相机中靶标特征逆投影产生的误差，
Figure BDA00032594231700000412
为第i个相机中圆形目标特征逆投影产生的误差，
Figure BDA00032594231700000413
表示第k组特征平面中各个靶标点相对于参考位置
Figure BDA00032594231700000414
的偏移量，rk表示圆形目标的半径；α表示联合靶标特征和圆形目标特征建立的误差函数在两类特征之间分配的权重因子；根据上述建立的多特征融合的误差表达式，最小化逆投影误差
Figure BDA00032594231700000415
可实现对
Figure BDA00032594231700000416
Ψk或ti,θi的求解，即得到以下优化模型：
Figure BDA0003259423170000051
在实际应用中可根据测量需求对变量Ωi,
Figure BDA0003259423170000052
nk进行已知量和未知量的确定。对于静态相机测量动态目标的情况，如采样区双目相机测量采样机械臂末端位姿，Ωi为固定已知量，
Figure BDA0003259423170000053
nk为未知量；对于动态相机对静态目标的测量，如测量执行放样、抓罐、放罐操作时测量采样器相对目标罐的相对位姿，
Figure BDA0003259423170000054
nk为已知量，Ωi为未知量。
步骤五：针对公式(7)提出的最小化逆投影误差
Figure BDA0003259423170000055
可以将其构造为一个非线性最小二乘问题即：
Figure BDA0003259423170000056
那么可以采用高斯-牛顿法或Levenberg-Marquardt(L-M)算法等非线性优化算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息
Figure BDA0003259423170000057
Ψk或相机位姿信息ti，θi的精确求解。下表I为使用L-M算法计算空间目标或相机的精确位姿的具体步骤。
表I 使用L-M法计算空间目标或相机的精确位姿
Figure BDA0003259423170000058
得到空间目标相对于相机的相对位姿关系或相机相对于空间目标的精确位姿后可以进一步转换到世界坐标系下实现对机械臂精确的引导控制，保证地外天体采样遥操作中采样、放样、抓罐和放罐等操作的顺利进行。
附图说明
图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 BDA0003259423170000081
Ri＝R(θi)表示相机i的坐标系相对于世界坐标系的旋转变换矩阵，M和N分别表示观测点和相机的数量。空间中任意一个观测点
Figure BDA0003259423170000082
其在相机i的坐标系中坐标为
Figure BDA0003259423170000083
投影坐标uij＝(uij,vij)T可以利用透视投影成像模型计算为：
Figure BDA0003259423170000084
其中，
Figure BDA0003259423170000085
分别表示对应的齐次坐标，
Figure BDA0003259423170000086
表示从第j个月面观测点到第i个相机图像的透视投影变换，
Figure BDA0003259423170000087
表示与投影坐标相关的畸变参数矩阵。
根据空间中的任意目标特征点的逆投影描述方式，空间中的任意目标特征点
Figure BDA0003259423170000088
可表示为从相机光心(相机坐标系原点)出发指向目标特征点的方向向量,如图4所示,则
Figure BDA0003259423170000089
可以计算为：
Figure BDA00032594231700000810
其中,
Figure BDA0003259423170000091
为连接相机光心与图像坐标uij校正后坐标u′ij的方向向量,令
Figure BDA0003259423170000092
为u′ij的齐次坐标，则满足：
Figure BDA0003259423170000093
其中
Figure BDA0003259423170000094
表示图像主点的坐标,Δuij＝[Δuij Δvij 0]T表示畸变偏移,
Figure BDA0003259423170000095
表示目标观测点在相机坐标系中的z坐标,则vij计算为：
vij＝Ri(u′ij,v′ij,fi)T/||(u′ij,v′ij,fi)||2 (4)
sij是尺度因子,表示沿着vij方向伸展的长度。上述
Figure BDA0003259423170000096
的表达可解释为以ti为起点,沿着vij方向伸展任意长度的点的集合,si＞0则
Figure BDA0003259423170000097
表示空间中过ti点,方向向量为vij的射线。
由公式(2)可知，多类特征融合的关键是对各类特征的尺度因子si的统一描述，为此提出以下表征方式。假设可将相机视场内的特征按照一定准则分为K组，其中人工设置特征(如靶标特征)和自然特征(如圆形目标)分别分在Kd和Kr组。以特征的共面性作为准则，对si的表征计算如下：令第k组特征平面上某一特征点的空间坐标为
Figure BDA0003259423170000098
该平面的法向为
Figure BDA0003259423170000099
则：
Figure BDA00032594231700000910
其中n0＝[0 0 1]T，Ψk＝[ψk,κk,φk]T表示平面坐标系相对于世界坐标系的姿态，由此可得该平面的代数方程为
Figure BDA00032594231700000911
其中zkj为
Figure BDA00032594231700000912
在以
Figure BDA00032594231700000913
为原点的局部坐标系中的坐标向量在
Figure BDA00032594231700000914
方向上的投影分量。将公式(2)带入可得
Figure BDA00032594231700000915
由此
Figure BDA00032594231700000916
可计算为：
Figure BDA00032594231700000917
步骤四：根据重建出的空间坐标利用已知的特征之间的固定约束建立相机对自然特征和人工特征的相对测量的统一的测量优化模型。具体步骤为：
根据已知人工设置的特征之间的相对位置和自然特征的几何形状特性构建误差方程。假设仅有靶标和圆形目标两类特征，则逆投影误差可以写作：
Figure BDA0003259423170000101
其中
Figure BDA0003259423170000102
为第i个相机中靶标特征逆投影产生的误差，
Figure BDA0003259423170000103
为第i个相机中圆形目标特征逆投影产生的误差，
Figure BDA0003259423170000104
表示第k组特征平面中各个靶标点相对于参考位置
Figure BDA0003259423170000105
的偏移量，rk表示圆形目标的半径。最小化逆投影误差
Figure BDA0003259423170000106
可实现对
Figure BDA0003259423170000107
Ψk和ti,θi求解，即得到以下优化模型：
Figure BDA0003259423170000108
在实际应用中可根据测量需求对变量Ωi,
Figure BDA0003259423170000109
nk进行已知量和未知量的确定。对于静态相机测量动态目标的情况，如采样区双目相机测量采样机械臂末端位姿，Ωi为固定已知量，
Figure BDA00032594231700001010
nk为未知量；对于动态相机对静态目标的测量，如测量执行放样、抓罐、放罐操作时测量采样器相对目标罐的相对位姿，
Figure BDA00032594231700001011
nk为已知量，Ωi为未知量。
步骤五：利用非线性优化方法迭代求解位姿测量优化模型，获取空间目标或机械臂臂载相机自身的精确位姿信息，从而实现对机械臂逐步引导控制，执行预定的工作程序，具体为：
针对上述公式(8)提出的最小化逆投影误差
Figure BDA00032594231700001012
将其构造为一个非线性最小二乘问题：
Figure BDA00032594231700001013
那么可以采用高斯-牛顿法或Levenberg-Marquardt(L-M)算法等非线性优化算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息
Figure BDA0003259423170000111
Ψk或相机位姿信息ti，θi的精确求解。下表I为使用L-M算法计算空间目标或相机的精确位姿的具体步骤。
表I 使用L-M法计算空间目标或相机的精确位姿
Figure BDA0003259423170000112
得到空间目标相对于相机的相对位姿关系或相机相对于空间目标的精确位姿后可以进一步转换到世界坐标系下实现对机械臂精确的引导控制，保证地外天体采样遥操作中采样、放样、抓罐和放罐等操作的顺利进行。

Claims (8)

1.基于逆投影的多特征融合视觉定位方法，包括以下步骤：
步骤一：在相机图像中提取目标特征的像素坐标，针对自然特征检测其轮廓像素坐标，针对人工设置的特征检测其标志点像素坐标；
步骤二：确定空间中存在N个相机，其中共存在N1个为静态相机和N2个为动态相机；当测量情况属于相机位姿固定，需要测量图像中的特征在空间中的位姿情况下，设置合适的目标位姿X0,Ψ0作为算法初始迭代值；反之，当属于目标位姿固定，需要反求相机在空间中的位姿情况下，设置合适的相机位姿t0,θ0作为算法初始迭代值；
步骤三：根据前述提取到的像素坐标，已知的目标位姿信息或相机位姿信息和相机或目标的位姿初始迭代值在空间中重建特征的三维坐标；
步骤四：建立相机对自然特征和人工设置特征的相对测量的统一的测量优化模型；根据已知人工设置特征的相对位置和自然特征的几何形状特性构建误差方程；
步骤五：迭代求解非线性误差函数，实现对目标的位姿信息或相机位姿信息的精确求解。
2.根据权利要求1所述的一种基于逆投影的多特征融合视觉定位方法，其特征在于：所述重建特征的三维坐标，具体为：
设第i个相机的相机坐标系在世界坐标系的位姿为
Figure FDA0003259423160000011
Ri＝R(θi)表示相机i的坐标系相对于世界坐标系的旋转变换矩阵，N和M分别表示相机和观测点的数量；空间中任意一个观测点
Figure FDA0003259423160000012
其在相机i的坐标系中坐标为
Figure FDA0003259423160000013
投影坐标uij＝(uij,vij)T可以利用透视投影成像模型计算为：
Figure FDA0003259423160000014
其中，
Figure FDA0003259423160000015
分别表示对应的齐次坐标，
Figure FDA0003259423160000016
表示从第j个观测点到第i个相机图像的透视投影变换，
Figure FDA0003259423160000017
表示与投影坐标相关的畸变参数矩阵；
根据空间中任意特征点的逆投影描述方式，任意目标特征点
Figure FDA0003259423160000021
可表示为从相机光心出发指向空间目标特征点的方向向量,则
Figure FDA0003259423160000022
可以计算为：
Figure FDA0003259423160000023
其中,
Figure FDA0003259423160000024
为连接相机光心与图像坐标uij校正后坐标u′ij的方向向量,令
Figure FDA0003259423160000025
为u′ij的齐次坐标，则满足：
Figure FDA0003259423160000026
其中
Figure FDA0003259423160000027
表示图像主点的坐标,Δuij＝[Δuij Δvij 0]T表示畸变偏移,
Figure FDA0003259423160000028
表示目标观测点在相机坐标系中的z坐标,vij计算为：
Figure FDA0003259423160000029
sij是尺度因子,表示沿着vij方向伸展的长度；上述
Figure FDA00032594231600000210
的表达可解释为以ti为起点,沿着vij方向伸展任意长度的点的集合；si＞0,则
Figure FDA00032594231600000211
表示空间中以ti为起点，方向向量为vij的射线；
将相机视场内的特征按照一定准则分为K组，其中人工设置特征和自然特征分别分在Kd和Kr组；以特征的共面性作为准则，对si的表征计算如下：令第k组特征平面上某一特征点的空间坐标为
Figure FDA00032594231600000212
该平面的法向为
Figure FDA00032594231600000213
则：
Figure FDA00032594231600000214
其中n0＝[0 0 1]T，Ψk＝[ψk,κk,φk]T表示该平面坐标系相对于世界坐标系的姿态角度，由此可得该平面的代数方程为
Figure FDA00032594231600000215
其中zkj为
Figure FDA00032594231600000216
在以
Figure FDA00032594231600000217
为原点的局部坐标系中的坐标向量在
Figure FDA00032594231600000218
方向上的投影分量；将公式(2)带入平面的代数方程可得：
Figure FDA0003259423160000031
由此空间点世界坐标
Figure FDA0003259423160000032
可计算为：
Figure FDA0003259423160000033
3.根据权利要求1所述的一种基于逆投影的多特征融合视觉定位方法，其特征在于：所述构建的误差方程为：
假设仅存在靶标和圆形目标两类特征，则逆投影误差函数写作：
Figure FDA0003259423160000034
其中
Figure FDA0003259423160000035
为第i个相机中靶标特征逆投影产生的误差，
Figure FDA00032594231600000315
为第i个相机中圆形目标特征逆投影产生的误差，
Figure FDA0003259423160000036
表示第k组特征平面中各个靶标点相对于参考位置
Figure FDA0003259423160000037
的偏移量，rk表示圆形目标的半径；α表示联合靶标特征和圆形目标特征建立的误差函数在两类特征之间分配的权重因子；根据上述建立的多特征融合的误差表达式，最小化逆投影误差
Figure FDA0003259423160000038
可实现对
Figure FDA0003259423160000039
Ψk或ti,θi的求解，即得到以下优化模型：
Figure FDA00032594231600000310
根据测量需求对变量
Figure FDA00032594231600000311
进行已知量和未知量的确定；对于静态相机测量动态目标的情况，如采样区双目相机测量采样机械臂末端位姿，Ωi为固定已知量，
Figure FDA00032594231600000312
nk为未知量；对于动态相机对静态目标的测量，如测量执行放样、抓罐、放罐操作时测量采样器相对目标罐的相对位姿，
Figure FDA00032594231600000313
nk为已知量，Ωi为未知量。
4.根据权利要求1所述的一种基于逆投影的多特征融合视觉定位方法，其特征在于：所述位姿信息的精确求解方法为：
针对公式(7)提出的最小化逆投影误差
Figure FDA00032594231600000314
可以将其构造为一个非线性最小二乘问题即：
Figure FDA0003259423160000041
接下来迭代求解此非线性最小二乘问题实现对目标位姿信息
Figure FDA0003259423160000042
Ψk或相机位姿信息ti,θi的精确求解。
5.根据权利要求1所述的一种基于逆投影的多特征融合视觉定位方法，其特征在于：所说的静态相机N1固定在探测器系统着陆器上，也可以固定在上升器上。
6.根据权利要求1所述的一种基于逆投影的多特征融合视觉定位方法，其特征在于：所说的动态相机N2为机械臂臂载相机，也可以是采样器相机。
7.根据权利要求1所述的一种基于逆投影的多特征融合视觉定位方法，其特征在于：所说的相机包括采样区观测双目相机、采样器相机、臂载相机。
8.根据权利要求1所述的一种基于逆投影的多特征融合视觉定位方法，其特征在于：所说的步骤三中采用高斯-牛顿法，也可以采用Levenberg-Marquardt(L-M)算法等非线性优化算法将此非线性最小二乘问题转化为线性最小二乘问题，迭代求解此问题实现对目标位姿信息
Figure FDA0003259423160000043
Ψk或相机位姿信息ti,θi的精确求解。
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 true CN113822934A (zh) 2021-12-21
CN113822934B 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)

Cited By (1)

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

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100232728A1 (en) * 2008-01-18 2010-09-16 Leprince Sebastien Ortho-rectification, coregistration, and subpixel correlation of optical satellite and aerial images
CN105675266A (zh) * 2016-02-03 2016-06-15 上海仪万光电科技有限公司 无限共轭光路测量光学镜头的调制传递函数的装置及方法
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姿态参数计算方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100232728A1 (en) * 2008-01-18 2010-09-16 Leprince Sebastien Ortho-rectification, coregistration, and subpixel correlation of optical satellite and aerial images
CN105675266A (zh) * 2016-02-03 2016-06-15 上海仪万光电科技有限公司 无限共轭光路测量光学镜头的调制传递函数的装置及方法
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
DONG RF ET AL: "《Line-based Monocular Graph SLAM》", 《IEEE》 *
王志超等: "《基于特征融合的非合作航天器位姿测量方法》", 《高技术通讯》 *

Cited By (2)

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

Also Published As

Publication number Publication date
CN113822934B (zh) 2022-08-02

Similar Documents

Publication Publication Date Title
JP5627325B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
JP6004809B2 (ja) 位置姿勢推定装置、情報処理装置、情報処理方法
CN110116407A (zh) 柔性机器人位姿测量方法及装置
Peng et al. Virtual stereovision pose measurement of noncooperative space targets for a dual-arm space robot
CN108324373B (zh) 一种基于电磁定位系统的穿刺手术机器人精确定位实现方法
JP5924862B2 (ja) 情報処理装置、情報処理方法及びプログラム
WO2012014545A1 (ja) ３次元物体認識装置及び３次元物体認識方法
CN108665499B (zh) 一种基于视差法的近距飞机位姿测量方法
CN114011608B (zh) 基于数字孪生的喷涂工艺优化系统及其喷涂优化方法
CN110849331B (zh) 基于三维点云数据库模型的单目视觉测量与地面试验方法
CN109877840A (zh) 一种基于相机光轴约束的双机械臂标定方法
CN110298854A (zh) 基于在线自适应与单目视觉的飞行蛇形臂协同定位方法
CN113822934B (zh) 基于逆投影的多特征融合视觉定位方法
CN109087355A (zh) 基于迭代更新的单目相机位姿测量装置与方法
CN112712565A (zh) 基于视觉与imu融合的飞机蒙皮损伤无人机绕检定位方法
Lim Point cloud modeling using the homogeneous transformation for non-cooperative pose estimation
Sun et al. An orthogonal iteration pose estimation algorithm based on an incident ray tracking model
JP2022183032A (ja) カメラガイド装置のハンドアイ較正
CN113740864A (zh) 基于激光三维点云的探测器软着陆末段自主位姿估计方法
Bao et al. 3D perception-based collision-free robotic leaf probing for automated indoor plant phenotyping
CN110490934B (zh) 基于单目相机与机器人的混合机立式桨叶姿态检测方法
Li Relative pose measurement of moving rigid bodies based on binocular vision
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

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