CN116681733B - 一种空间非合作目标近距离实时位姿跟踪方法 - Google Patents

一种空间非合作目标近距离实时位姿跟踪方法 Download PDF

Info

Publication number
CN116681733B
CN116681733B CN202310971528.2A CN202310971528A CN116681733B CN 116681733 B CN116681733 B CN 116681733B CN 202310971528 A CN202310971528 A CN 202310971528A CN 116681733 B CN116681733 B CN 116681733B
Authority
CN
China
Prior art keywords
straight line
circle
pose
key frame
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
CN202310971528.2A
Other languages
English (en)
Other versions
CN116681733A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202310971528.2A priority Critical patent/CN116681733B/zh
Publication of CN116681733A publication Critical patent/CN116681733A/zh
Application granted granted Critical
Publication of CN116681733B publication Critical patent/CN116681733B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/181—Segmentation; Edge detection involving edge growing; involving edge linking
    • 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
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种空间非合作目标近距离实时位姿跟踪方法，步骤如下：首先，对初始帧深度图提取椭圆和直线，从中挑选符合规则的同心圆特征和直线特征并解算相对位姿。然后在关键帧中跟踪同心圆特征和直线特征，并利用多个关键帧数据进行优化以消去测量误差获得精度更高的相对位姿。接着，采用ICP算法跟踪当前帧和距离最近的关键帧之间的相对位姿变化。最后，融合最近关键帧的相对位姿和当前帧相对于最近关键帧的相对位姿的变化，获得实时的位姿跟踪结果。

Description

一种空间非合作目标近距离实时位姿跟踪方法
技术领域
本发明属于人工智能（智能导航制导与控制技术）领域，具体涉及一种空间非合作目标近距离实时位姿跟踪方法。
背景技术
目前，越来越多的空间失效飞行器挤占着有限的轨道资源并且威胁到了空间任务的安全。因此，能够实现对非合作目标抓取、维修和其他服务的自主在轨服务技术成为了当下空间技术发展的核心领域之一。在自主在轨服务任务中，自主获取非合作目标高精度相对位姿信息是能否成功执行在轨任务的前提条件之一。尤其是在抓捕前的近距离阶段，对算法的实时性也提出了较高的要求。在目前的非合作目标相对导航研究中，根据采用的传感器类型，可以分为基于被动传感器：单目相机，立体视觉和基于主动传感器：激光雷达，ToF相机两类。在基于被动传感器的方法中，通常会使用点、线和圆三类特征来实现相对位姿的测量。点特征的方法被定义为透视n点问题 (PnP)，利用图像中多个特征点对应的几何约束来求解位姿。线特征比点特征更加鲁棒，通常采用透视n线方法求解。然而，图像中存在大量的点、线特征，在利用特征测量位置和姿态之前，必须要从大量特征中挑选出正确的特征。圆特征是鲁棒性最强的特征，即使在强噪声环境下也有很好的跟踪效果。圆特征在卫星中也是普遍存在的，并且数量不多，不需要复杂的挑选过程。但是圆特征存在二义性并且单纯依靠圆特征无法恢复运动中的滚转角。主动传感器相对于被动传感器具有可以直接获得目标深度信息的优势。然而从无序的点云中提取特征是困难的，并且针对点云的操作计算上很费时，尤其是距离目标很近时，获得的点云通常比较稠密。
发明内容
针对于上述现有技术的不足，本发明的目的在于提供一种空间非合作目标近距离实时位姿跟踪方法，以解决现有技术在空间非合作目标观测中，基于被动传感器的方法往往需要复杂的位姿解算算法，而基于主动传感器的方法则在实时性上存在巨大劣势的问题。
本发明基于ToF相机获取的深度图，在初始帧深度图中提取椭圆和直线，利用同心圆约束挑选符合要求的同心圆特征，利用直线长度和位置约束挑选符合要求的直线特征。以同心圆离卫星中心最远的一个圆的圆心为中心，以同心圆的法线方向和直线三维点和相机原点确定的平面的法线方向确定一个坐标系的三个轴，即可解算初始的相对位姿。同心圆特征和直线特征在关键帧之间被跟踪，并利用多个关键帧数据进行优化以消去测量误差获得精度更高的相对位姿。深度图被用来辅助稀疏点云。稀疏后的点云用ICP求解当前帧与距离最近的关键帧之间的相对位姿变化。带优化的关键帧位姿估计结果和通过ICP获得的相对位姿变化跟踪结果被融合以获得高精度的近距离实时位姿跟踪结果。
为达到上述目的，本发明采用的技术方案如下：
一种空间非合作目标近距离实时位姿跟踪方法，所述跟踪方法步骤如下：
S1，从第1帧开始隔设定时间添加一个关键帧，通过对初始关键帧深度图提取空间非合作目标表明的椭圆和直线特征，挑选出符合设定规则的同心圆特征和直线特征，解算无优化的空间非合作目标和相机之间的相对位姿；
S2，在关键帧中跟踪步骤S1中挑选出的同心圆特征和直线特征，利用多个关键帧数据进行优化以消去测量误差获得精度更高的带优化的相对位姿；
S3，融合所述步骤S2中的带优化的相对位姿和采用ICP算法跟踪获得的当前帧与最近关键帧之间的相对位姿变化，获得实时的相对位姿跟踪结果。
进一步的，所述步骤S1具体包括：
S11，在初始帧深度图中采用基于边缘连接方法的椭圆检测算法AAMED提取深度图中的椭圆；在提取的椭圆中剔除满足如下要求的椭圆：
1)椭圆的周长小于100个像素，2)半长轴与半短轴的比例大于设定数值；
将深度图中所有明显的椭圆都提取出来；
之后，将得到的明显的椭圆上的点通过相机投影模型和深度图的缩放，生成空间圆的三维点；获得空间圆的三维点后，通过最小二乘问题求解空间圆所在平面的法向量；通过空间圆所在平面的法向量得到该空间圆的圆心，拟合出所有空间圆；
S12，基于LSD算法来提取一条主特征直线以及三条辅助特征直线；
S13，将主特征直线的三维点与相机光心构成一个平面，由最小二乘问题求得该平面的法向量，记为；将同心圆中离卫星主体最远的圆的圆心记为/>，法向量记为/>；/>即为无优化的空间非合作目标的位置，设定无优化的空间非合作目标的姿态求取为：
。
进一步的，所述基于LSD算法来提取一条主特征直线以及四条特征直线具体为：LSD算法从深度图像中提取出若干特征线，从中选择四条直线特征作为跟踪的特征，一条为主特征直线，配合同心圆特征确定目标坐标系；另外三条为辅助特征直线，所述辅助特征直线用来在无法观测到主特征直线时，根据直线之间的相对关系确定主特征直线的状态，确定目标坐标系；
确定主特征直线的标准如下：
1)直线不在图像边缘，2)直线距离同心圆圆心的距离大于最大圆的半径且小于图像宽度的一半，3）符合上述条件中最长的一条直线；
确定三条辅助特征直线的标准如下：其中一条是满足与主特征直线相平行且距离圆心的距离与主特征直线相同但是方向相反的直线中最长的一条直线；其他两条是分别满足垂直于主特征直线且距离圆心的距离与主特征直线相同但方向相反的所有直线中最长的两条。
进一步的，同心圆的挑选遵从如下准则：将所述空间圆的所有圆心都在一条与它们的支撑面垂直的公共线上的空间圆称为同心圆；将同心圆中所有圆的圆心和半径作为同心圆特征保存。
进一步的，所述步骤S2具体包括：
S21，在所述步骤S1中挑选的同心圆特征以及四条特征直线，以同心圆中离卫星主体最远的那个圆的圆心和四条直线中点位置这五个点的三维坐标为媒介，根据ICP算法跟踪的关键帧之间的相对位姿的变化估计以上五个点在下一个关键帧中的投影坐标；对于关键帧深度图中的椭圆特征来说，将椭圆中心距离投影过来的圆心大于阈值的椭圆直接去除；在剩下的椭圆中检查是否满足同心圆的约束，如果不满足同心圆的约束，将不满足约束的椭圆去除后即可获得关键帧中的同心圆特征；如果同心圆中没有当前空间非合作目标的位置，则根据保存的同心圆中圆心的位置关系估计当前空间非合作目标的位置；如果同心圆中出现了比当前空间非合作目标的位置距离卫星中心更远的圆心位置，则将下一帧作为关键帧继续观测；若下一帧依然观测到了比当前空间非合作目标的位置距离卫星中心更远的圆心位置，则修改空间非合作目标的位置为新观测到的圆心；
如果距离投影过来的特征直线的中点小于阈值的距离上有直线，则利用该直线和投影之前的特征直线的普吕克坐标计算两条直线之间的相对距离和夹角以判断该直线是否为投影之前的特征直线在当前帧对应的特征直线；如果最终四条特征直线都没有找到当前帧相对应的特征直线，则用关键帧深度图中提取到的所有直线与世界坐标系中的四条特征直线做对比，直到找到一条当前帧对应的特征直线为止；如果投影点不在像素平面上，则认为该特征不在相机视场中；如果圆心投影不在像素平面上，则认为该次观测失败，开始下一次观测；
S22、结合多个关键帧数据进行非线性优化，设定所有待优化变量x为：
；
其中是位姿的李代数，a表示关键帧的数量；P是特征点；L表示直线特征；
将位姿也加入进优化变量中，优化后的点特征和线特征被用来计算更为精确的相对位姿
所有的状态向量通过最小化所有测量残差的平方来优化：
；
其中，是所有特征点的重投影误差的和，/>是所有特征线的重投影误差的和；
采用Levenberg–Marquard算法来解决上式中的非线性优化问题，设定是由上式求解出的状态向量的增量，最优状态估计/>通过下式迭代更新：
；
其中，是通过/>更新状态向量的操作符，对于向量，这个操作符表示直接加法；对于位姿的李代数/>，由如下表达式进行更新：
；
其中，是位姿李代数的估计值，/>表示将一个向量转化为反对称矩阵，/>表示将一个反对称矩阵转化为向量，/>是/>的增量；
对于直线的标准正交表示的更新表示为：
；
其中，是直线的标准正交表示，/>和/>是标准正交表示的估计值，/>表示一个旋转，/>是一个旋转角度；
增量方程写为：
；
其中，和/>分别是特征点和特征线重投影误差的Hessian矩阵，并且，，/>，其中，/>是残差，/>是残差/>关于/>的雅可比矩阵。
作为本申请的一种优选实施方案，步骤S21中同心圆和特征直线跟踪过程中的对比阈值取40像素。
进一步的，所述步骤S3具体包括：
从第1帧开始隔设定时间添加一个关键帧，对关键帧通过所述步骤S2的操作获得关键帧的带优化的相对位姿；对每一帧帧深度图中提取Canny边缘，然后恢复所有边缘点的三维点生成稀疏后的点云作为ICP 算法的输入；通过ICP算法获得当前帧相对于最近关键帧之间的相对位姿的变化；将求得的当前帧相对于最近关键帧之间的相对变化添加到最近关键帧的带优化的相对位姿中即可获得当前帧的相对位姿，也即获得了空间非合作目标实时的位姿跟踪结果。
本发明的有益效果：
1、本发明提出了一种新的空间非合作目标相对导航框架，实现了空间非合作目标近距离实时位姿跟踪；
2、本发明采用四条直线特征，能够解决位姿跟踪过程中特征离开相机视场的问题。
附图说明
图1为本发明方法的流程图；
图2为模拟空间非合作目标成像获得的深度图；
图3a为无优化位置跟踪误差结果图；
图3b为无优化姿态跟踪误差结果图；
图4a为带优化位置跟踪误差结果图；
图4b为带优化姿态跟踪误差结果图；
图5a为ICP位置跟踪误差结果图；
图5b为ICP姿态跟踪误差结果图；
图6a优化+ICP组合位置跟踪误差结果图；
图6b优化+ICP组合姿态跟踪误差结果图。
实施方式
为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。
参照图1所示，本发明的一种空间非合作目标近距离实时位姿跟踪方法，步骤如下：
S1，从第1帧开始隔设定时间添加一个关键帧，通过对初始关键帧深度图提取空间非合作目标表明的椭圆和直线特征，挑选出符合设定规则的同心圆特征和直线特征，解算无优化的空间非合作目标和相机之间的相对位姿；
进一步的，所述步骤S1具体包括：
S11，在初始帧深度图中采用基于边缘连接方法的椭圆检测算法AAMED提取深度图中的椭圆；在提取的椭圆中剔除满足如下要求的椭圆：
1)椭圆的周长小于100个像素，2)半长轴与半短轴的比例大于2；
将深度图中所有明显的椭圆都提取出来；
之后，将得到的明显的椭圆上的点通过相机投影模型和深度图的缩放，生成空间圆的三维点；获得空间圆的三维点后，通过最小二乘问题求解空间圆所在平面的法向量；通过空间圆所在平面的法向量得到该空间圆的圆心，拟合出所有空间圆；
S12，基于LSD算法来提取一条主特征直线以及三条辅助特征直线；
S13，将主特征直线的三维点与相机光心构成一个平面，由最小二乘问题求得该平面的法向量，记为；将同心圆中离卫星主体最远的圆的圆心记为/>，法向量记为/>；/>即为无优化的空间非合作目标的位置，设定无优化的空间非合作目标的姿态求取为：
。
进一步的，所述基于LSD算法来提取一条主特征直线以及四条特征直线具体为：LSD算法从深度图像中提取出若干特征线，从中选择四条直线特征作为跟踪的特征，一条为主特征直线，配合同心圆特征确定目标坐标系；另外三条为辅助特征直线，所述辅助特征直线用来在无法观测到主特征直线时，根据直线之间的相对关系确定主特征直线的状态，确定目标坐标系；
确定主特征直线的标准如下：
1)直线不在图像边缘，2)直线距离同心圆圆心的距离大于最大圆的半径且小于图像宽度的一半，3）符合上述条件中最长的一条直线；
确定三条辅助特征直线的标准如下：其中一条是满足与主特征直线相平行且距离圆心的距离与主特征直线相同但是方向相反的直线中最长的一条直线；其他两条是分别满足垂直于主特征直线且距离圆心的距离与主特征直线相同但方向相反的所有直线中最长的两条。
进一步的，同心圆的挑选遵从如下准则：将所述空间圆的所有圆心都在一条与它们的支撑面垂直的公共线上的空间圆称为同心圆；将同心圆中所有圆的圆心和半径作为同心圆特征保存。
S2，在关键帧中跟踪步骤S1中挑选出的同心圆特征和直线特征，利用多个关键帧数据进行优化以消去测量误差获得精度更高的带优化的相对位姿；进一步的，所述步骤S2具体包括：
S21，在所述步骤S1中挑选的同心圆特征以及四条特征直线，以同心圆中离卫星主体最远的那个圆的圆心和四条直线中点位置这五个点的三维坐标为媒介，根据ICP算法跟踪的关键帧之间的相对位姿的变化估计以上五个点在下一个关键帧中的投影坐标；对于关键帧深度图中的椭圆特征来说，将椭圆中心距离投影过来的圆心大于阈值的椭圆直接去除；在剩下的椭圆中检查是否满足同心圆的约束，如果不满足同心圆的约束，将不满足约束的椭圆去除后即可获得关键帧中的同心圆特征；如果同心圆中没有当前空间非合作目标的位置，则根据保存的同心圆中圆心的位置关系估计当前空间非合作目标的位置；如果同心圆中出现了比当前空间非合作目标的位置距离卫星中心更远的圆心位置，则将下一帧作为关键帧继续观测；若下一帧依然观测到了比当前空间非合作目标的位置距离卫星中心更远的圆心位置，则修改空间非合作目标的位置为新观测到的圆心；
如果距离投影过来的特征直线的中点小于阈值的距离上有直线，则利用该直线和投影之前的特征直线的普吕克坐标计算两条直线之间的相对距离和夹角以判断该直线是否为投影之前的特征直线在当前帧对应的特征直线；如果最终四条特征直线都没有找到当前帧相对应的特征直线，则用关键帧深度图中提取到的所有直线与世界坐标系中的四条特征直线做对比，直到找到一条当前帧对应的特征直线为止；如果投影点不在像素平面上，则认为该特征不在相机视场中；如果圆心投影不在像素平面上，则认为该次观测失败，开始下一次观测；
S22、结合多个关键帧数据进行非线性优化，设定所有待优化变量x为：
；
其中是位姿的李代数，a表示关键帧的数量；P是特征点；L表示直线特征；
将位姿也加入进优化变量中，优化后的点特征和线特征被用来计算更为精确的相对位姿
所有的状态向量通过最小化所有测量残差的平方来优化：
；
其中，是所有特征点的重投影误差的和，/>是所有特征线的重投影误差的和；
采用Levenberg–Marquard算法来解决上式中的非线性优化问题，设定是由上式求解出的状态向量的增量，最优状态估计/>通过下式迭代更新：
；
其中，是通过/>更新状态向量的操作符，对于向量，这个操作符表示直接加法；对于位姿的李代数/>，由如下表达式进行更新：
；
其中，是位姿李代数的估计值，/>表示将一个向量转化为反对称矩阵，/>表示将一个反对称矩阵转化为向量，/>是/>的增量；
对于直线的标准正交表示的更新表示为：
；
其中，是直线的标准正交表示，/>和/>是标准正交表示的估计值，/>表示一个旋转，/>是一个旋转角度；
增量方程写为：
；
其中，和/>分别是特征点和特征线重投影误差的Hessian矩阵，并且，，/>，其中，/>是残差，/>是残差/>关于/>的雅可比矩阵。
作为本申请的一种优选实施方案，步骤S21中同心圆和特征直线跟踪过程中的对比阈值取40像素。
S3，融合所述步骤S2中的带优化的相对位姿和采用ICP算法跟踪获得的当前帧与最近关键帧之间的相对位姿变化，获得实时的相对位姿跟踪结果。
进一步的，所述步骤S3具体包括：
从第1帧开始隔设定时间添加一个关键帧，对关键帧通过所述步骤S2的操作获得关键帧的带优化的相对位姿，本实施例中的时间设定为0.5秒；对每一帧帧深度图中提取Canny边缘，然后恢复所有边缘点的三维点生成稀疏后的点云作为ICP 算法的输入；通过ICP算法获得当前帧相对于最近关键帧之间的相对位姿的变化；将求得的当前帧相对于最近关键帧之间的相对变化添加到最近关键帧的带优化的相对位姿中即可获得当前帧的相对位姿，也即获得了空间非合作目标实时的位姿跟踪结果。
以下通过具体实例说明算法的使用流程：
设定如下计算条件和技术参数：
（1）以空间非合作目标抓取背景，空间非合作目标与追逐航天器上的ToF相机之间的距离固定为3m；
（2）ToF相机的参数为：分辨率640×640，焦距5.52mm，相元尺寸0.01mm，帧率24fps/s；
（3）相对运动参数为：初始位置[0,0,3]m，初始姿态[0,-5,0]deg，速度[0.5，0.5，0]m/s，角速度[1.2，2.4，1.2]deg/s；
采用C++和Matlab进行仿真验证，可以获得图2-图6b的仿真结果。图2是模拟空间非合作目标成像获得的深度图。图3a为无优化位置跟踪误差结果图；图3b为无优化姿态跟踪误差结果图；图4a为带优化位置跟踪误差结果图；图4b为带优化姿态跟踪误差结果图；图5a为ICP位置跟踪误差结果图；图5b为ICP姿态跟踪误差结果图；图6a优化+ICP组合位置跟踪误差结果；图6b优化+ICP组合姿态跟踪误差结果。从图3a、图3b中可以看出，无优化的位姿跟踪的误差是收敛的，但是因为结果只受单一数据影响，因此存在某次测量结果特别大的情况。对比图4a、图4b和图3a、图3b中可以看出，带优化的位姿跟踪结果误差比无优化的位姿跟踪结果的误差小，且带优化的位姿跟踪结果不会受单一测量数据的影响。从图5a和图5b中可以看到，ICP的位姿跟踪结果是发散的，存在误差累积的情况。并且单次计算偶尔会导致较大的误差，使得误差结果在某一时刻显著增加。从图6a和图6b中可以看到，ICP算法使得误差逐渐累积，而带优化的位姿估计算法则可以在关键帧中把误差消去，使得位姿的跟踪误差收敛在较小的范围。
本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (4)

1.一种空间非合作目标近距离实时位姿跟踪方法，其特征在于，所述跟踪方法步骤如下：
S1，从第1帧开始隔设定时间添加一个关键帧，通过对初始关键帧深度图提取空间非合作目标表明的椭圆和直线特征，挑选出符合设定规则的同心圆特征和直线特征，解算无优化的空间非合作目标和相机之间的相对位姿；
同心圆的挑选遵从如下准则：将空间圆的所有圆心都在一条与它们的支撑面垂直的公共线上的空间圆称为同心圆；将同心圆中所有圆的圆心和半径作为同心圆特征保存；
所述步骤S1具体包括：
S11，在初始帧深度图中采用基于边缘连接方法的椭圆检测算法AAMED提取深度图中的椭圆；在提取的椭圆中剔除满足如下要求的椭圆：
1)椭圆的周长小于100个像素，2)半长轴与半短轴的比例大于设定数值；
将深度图中所有明显的椭圆都提取出来；
之后，将得到的明显的椭圆上的点通过相机投影模型和深度图的缩放，生成空间圆的三维点；获得空间圆的三维点后，通过最小二乘问题求解空间圆所在平面的法向量；通过空间圆所在平面的法向量得到该空间圆的圆心，拟合出所有空间圆；
S12，基于LSD算法来提取一条主特征直线以及三条辅助特征直线；
S13，将主特征直线的三维点与相机光心构成一个平面，由最小二乘问题求得该平面的法向量，记为mL；将同心圆中离卫星主体最远的圆的圆心记为PO，法向量记为mO；PO即为无优化的空间非合作目标的位置，设定无优化的空间非合作目标的姿态求取为：
Xo＝mO
yo＝mO×mL
Zo＝Xo×yo；
S2，在关键帧中跟踪步骤Sl中挑选出的同心圆特征和直线特征，利用多个关键帧数据进行优化以消去测量误差获得精度更高的带优化的相对位姿；所述步骤S2具体包括：
S21，在所述步骤S1中挑选的同心圆特征以及四条特征直线，以同心圆中离卫星主体最远的那个圆的圆心和四条直线中点位置这五个点的三维坐标为媒介，根据ICP算法跟踪的关键帧之间的相对位姿的变化估计以上五个点在下一个关键帧中的投影坐标；对于关键帧深度图中的椭圆特征来说，将椭圆中心距离投影过来的圆心大于阈值的椭圆直接去除；在剩下的椭圆中检查是否满足同心圆的约束，如果不满足同心圆的约束，将不满足约束的椭圆去除后即可获得关键帧中的同心圆特征；如果同心圆中没有当前空间非合作目标的位置，则根据保存的同心圆中圆心的位置关系估计当前空间非合作目标的位置；如果同心圆中出现了比当前空间非合作目标的位置距离卫星中心更远的圆心位置，则将下一帧作为关键帧继续观测；若下一帧依然观测到了比当前空间非合作目标的位置距离卫星中心更远的圆心位置，则修改空间非合作目标的位置为新观测到的圆心；
如果距离投影过来的特征直线的中点小于阈值的距离上有直线，则利用该直线和投影之前的特征直线的普吕克坐标计算两条直线之间的相对距离和夹角以判断该直线是否为投影之前的特征直线在当前帧对应的特征直线；如果最终四条特征直线都没有找到当前帧相对应的特征直线，则用关键帧深度图中提取到的所有直线与世界坐标系中的四条特征直线做对比，直到找到一条当前帧对应的特征直线为止；如果投影点不在像素平面上，则认为该特征不在相机视场中；如果圆心投影不在像素平面上，则认为该次观测失败，开始下一次观测；
S22、结合多个关键帧数据进行非线性优化，设定所有待优化变量x为：
x＝[ξ1…ξa，P1…P5，L1…L4]T；
其中ξ是位姿的李代数，a表示关键帧的数量；P是特征点；L表示直线特征；将位姿也加入进优化变量中，优化后的点特征和线特征被用来计算更为精确的相对位姿；
所有的状态向量通过最小化所有测量残差的平方来优化：
其中，ep是所有特征点的重投影误差的和，eL是所有特征线的重投影误差的和；
采用Levenberg-Marquard算法来解决上式中的非线性优化问题，设定δX是
由上式求解出的状态向量的增量，最优状态估计X′t+1通过下式迭代更新：
其中，是通过δx更新状态向量的操作符，对于向量，这个操作符表示直接加法；对于位姿的李代数ξ，由如下表达式进行更新：
其中，ξ′是位姿李代数的估计值，∧表示将一个向量转化为反对称矩阵，∨表示将一个反对称矩阵转化为向量，δξ是ξ的增量；
对于直线的标准正交表示的更新表示为：
U′≈U(I+[δθ]^)
其中，(U，W)∈SO(3)×SO(2)是直线的标准正交表示，U′和W′是标准正交表示的估计值，θ表示一个旋转，φ是一个旋转角度；
增量方程写为：
(Hp+Hl)δx＝(dp+dl)，
其中，Hp和Hl分别是特征点和特征线重投影误差的Hessian矩阵，并且，其中，e(·)是残差，J(·)是残差e(·)关于δξ的雅可比矩阵；
S3，融合所述步骤S2中的带优化的相对位姿和采用ICP算法跟踪获得的当前帧与最近关键帧之间的相对位姿变化，获得实时的相对位姿跟踪结果。
2.根据权利要求1所述的空间非合作目标近距离实时位姿跟踪方法，其特征在于，所述基于LSD算法来提取一条主特征直线以及三条特征直线具体为：LSD算法从深度图像中提取出若干特征线，从中选择四条直线特征作为跟踪的特征，一条为主特征直线，配合同心圆特征确定目标坐标系；另外三条为辅助特征直线，所述辅助特征直线用来在无法观测到主特征直线时，根据直线之间的相对关系确定主特征直线的状态，确定目标坐标系；
确定主特征直线的标准如下：
1)直线不在图像边缘，2)直线距离同心圆圆心的距离大于最大圆的半径且小于图像宽度的一半，3)符合上述条件中最长的一条直线；
确定三条辅助特征直线的标准如下：其中一条是满足与主特征直线相平行且距离圆心的距离与主特征直线相同但是方向相反的直线中最长的一条直线；其他两条是分别满足垂直于主特征直线且距离圆心的距离与主特征直线相同但方向相反的所有直线中最长的两条。
3.根据权利要求1所述的空间非合作目标近距离实时位姿跟踪方法，其特征在于，步骤S21中同心圆和特征直线跟踪过程中的对比阈值取40像素。
4.根据权利要求1所述的空间非合作目标近距离实时位姿跟踪方法，其特征在于，所述步骤S3具体包括：
从第1帧开始隔设定时间添加一个关键帧，对关键帧通过所述步骤S2的操作获得关键帧的带优化的相对位姿；对每一帧帧深度图中提取Canny边缘，然后恢复所有边缘点的三维点生成稀疏后的点云作为ICP算法的输入；通过ICP算法获得当前帧相对于最近关键帧之间的相对位姿的变化；将求得的当前帧相对于最近关键帧之间的相对变化添加到最近关键帧的带优化的相对位姿中即可获得当前帧的相对位姿，也即获得了空间非合作目标实时的位姿跟踪结果。
CN202310971528.2A 2023-08-03 2023-08-03 一种空间非合作目标近距离实时位姿跟踪方法 Active CN116681733B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310971528.2A CN116681733B (zh) 2023-08-03 2023-08-03 一种空间非合作目标近距离实时位姿跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310971528.2A CN116681733B (zh) 2023-08-03 2023-08-03 一种空间非合作目标近距离实时位姿跟踪方法

Publications (2)

Publication Number Publication Date
CN116681733A CN116681733A (zh) 2023-09-01
CN116681733B true CN116681733B (zh) 2023-11-07

Family

ID=87787728

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310971528.2A Active CN116681733B (zh) 2023-08-03 2023-08-03 一种空间非合作目标近距离实时位姿跟踪方法

Country Status (1)

Country Link
CN (1) CN116681733B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117541614B (zh) * 2024-01-09 2024-03-29 南京航空航天大学 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法

Citations (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101789126A (zh) * 2010-01-26 2010-07-28 北京航空航天大学 基于体素的三维人体运动跟踪方法
CN102759358A (zh) * 2012-03-14 2012-10-31 南京航空航天大学 基于失效卫星表面参考点的相对位姿动力学建模方法
CN103994755A (zh) * 2014-05-29 2014-08-20 清华大学深圳研究生院 一种基于模型的空间非合作目标位姿测量方法
CN105976353A (zh) * 2016-04-14 2016-09-28 南京理工大学 基于模型和点云全局匹配的空间非合作目标位姿估计方法
US10024664B1 (en) * 2014-09-30 2018-07-17 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Range and intensity image-based terrain and vehicle relative pose estimation system
CN108680198A (zh) * 2018-04-11 2018-10-19 北京空间飞行器总体设计部 一种基于羽流扰动的相对导航目标惯性参数辨识方法
CN108986037A (zh) * 2018-05-25 2018-12-11 重庆大学 基于半直接法的单目视觉里程计定位方法及定位系统
CN110782494A (zh) * 2019-10-16 2020-02-11 北京工业大学 一种基于点线融合的视觉slam方法
CN110823214A (zh) * 2019-10-18 2020-02-21 西北工业大学 一种空间完全非合作目标相对位姿和惯量估计方法
CN111780745A (zh) * 2020-06-29 2020-10-16 南京航空航天大学 一种面向深空探测光学导航的短弧椭圆拟合优化方法
CN112017188A (zh) * 2020-09-09 2020-12-01 上海航天控制技术研究所 一种空间非合作目标语义识别与重构方法
CN112070832A (zh) * 2020-08-07 2020-12-11 清华大学 基于点云dca特征的非合作目标位姿跟踪方法及装置
CN112085790A (zh) * 2020-08-14 2020-12-15 香港理工大学深圳研究院 点线结合的多相机视觉slam方法、设备及存储介质
CN113175929A (zh) * 2021-03-12 2021-07-27 南京航空航天大学 一种基于upf的空间非合作目标相对位姿估计方法
CN113706619A (zh) * 2021-10-21 2021-11-26 南京航空航天大学 一种基于空间映射学习的非合作目标姿态估计方法
WO2022002150A1 (zh) * 2020-06-30 2022-01-06 杭州海康机器人技术有限公司 一种视觉点云地图的构建方法、装置
US11340079B1 (en) * 2018-05-21 2022-05-24 AI Incorporated Simultaneous collaboration, localization, and mapping
CN115170381A (zh) * 2022-07-05 2022-10-11 北京航空航天大学 一种基于深度学习的视觉slam加速系统及方法
CN115658949A (zh) * 2022-12-26 2023-01-31 南京航空航天大学 一种基于关键帧的空间非合作目标自旋周期估计方法
CN115760984A (zh) * 2022-11-23 2023-03-07 南京理工大学 一种立方星基于单目视觉的非合作目标位姿测量方法
CN115965686A (zh) * 2022-11-30 2023-04-14 北京化工大学 一种融合点线特征的半直接视觉定位方法
CN116030136A (zh) * 2023-03-29 2023-04-28 中国人民解放军国防科技大学 基于几何特征的跨视角视觉定位方法、装置和计算机设备
CN116148804A (zh) * 2021-11-19 2023-05-23 中国科学院沈阳自动化研究所 一种基于结构光的空间非合作目标位姿测量方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2010063844A1 (fr) * 2008-12-05 2010-06-10 Thales Procede de geo-localisation d'un objet par multitelemetrie
WO2018129715A1 (zh) * 2017-01-13 2018-07-19 浙江大学 一种同时定位与稠密三维重建方法

Patent Citations (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101789126A (zh) * 2010-01-26 2010-07-28 北京航空航天大学 基于体素的三维人体运动跟踪方法
CN102759358A (zh) * 2012-03-14 2012-10-31 南京航空航天大学 基于失效卫星表面参考点的相对位姿动力学建模方法
CN103994755A (zh) * 2014-05-29 2014-08-20 清华大学深圳研究生院 一种基于模型的空间非合作目标位姿测量方法
US10024664B1 (en) * 2014-09-30 2018-07-17 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Range and intensity image-based terrain and vehicle relative pose estimation system
CN105976353A (zh) * 2016-04-14 2016-09-28 南京理工大学 基于模型和点云全局匹配的空间非合作目标位姿估计方法
CN108680198A (zh) * 2018-04-11 2018-10-19 北京空间飞行器总体设计部 一种基于羽流扰动的相对导航目标惯性参数辨识方法
US11340079B1 (en) * 2018-05-21 2022-05-24 AI Incorporated Simultaneous collaboration, localization, and mapping
CN108986037A (zh) * 2018-05-25 2018-12-11 重庆大学 基于半直接法的单目视觉里程计定位方法及定位系统
CN110782494A (zh) * 2019-10-16 2020-02-11 北京工业大学 一种基于点线融合的视觉slam方法
CN110823214A (zh) * 2019-10-18 2020-02-21 西北工业大学 一种空间完全非合作目标相对位姿和惯量估计方法
CN111780745A (zh) * 2020-06-29 2020-10-16 南京航空航天大学 一种面向深空探测光学导航的短弧椭圆拟合优化方法
WO2022002150A1 (zh) * 2020-06-30 2022-01-06 杭州海康机器人技术有限公司 一种视觉点云地图的构建方法、装置
CN112070832A (zh) * 2020-08-07 2020-12-11 清华大学 基于点云dca特征的非合作目标位姿跟踪方法及装置
CN112085790A (zh) * 2020-08-14 2020-12-15 香港理工大学深圳研究院 点线结合的多相机视觉slam方法、设备及存储介质
CN112017188A (zh) * 2020-09-09 2020-12-01 上海航天控制技术研究所 一种空间非合作目标语义识别与重构方法
CN113175929A (zh) * 2021-03-12 2021-07-27 南京航空航天大学 一种基于upf的空间非合作目标相对位姿估计方法
CN113706619A (zh) * 2021-10-21 2021-11-26 南京航空航天大学 一种基于空间映射学习的非合作目标姿态估计方法
CN116148804A (zh) * 2021-11-19 2023-05-23 中国科学院沈阳自动化研究所 一种基于结构光的空间非合作目标位姿测量方法
CN115170381A (zh) * 2022-07-05 2022-10-11 北京航空航天大学 一种基于深度学习的视觉slam加速系统及方法
CN115760984A (zh) * 2022-11-23 2023-03-07 南京理工大学 一种立方星基于单目视觉的非合作目标位姿测量方法
CN115965686A (zh) * 2022-11-30 2023-04-14 北京化工大学 一种融合点线特征的半直接视觉定位方法
CN115658949A (zh) * 2022-12-26 2023-01-31 南京航空航天大学 一种基于关键帧的空间非合作目标自旋周期估计方法
CN116030136A (zh) * 2023-03-29 2023-04-28 中国人民解放军国防科技大学 基于几何特征的跨视角视觉定位方法、装置和计算机设备

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Satellite Pose Acquisition and Tracking with Variable Dimensional Local Shape Descriptors;Babak Taati等;《IEEE/RSJ IROS 2005 Workshop on Robot Vision for Space Applications》;第4-9页 *
基于直线特征的空间非合作目标位姿视觉测量方法研究;张跃强;《中国博士学位论文全文数据库 工程科技II辑》(第11期);第C031-91页 *
基于融合伽马变换全卷积神经网络的火星地貌分割方法;胡茄乾等;《上海航天（中英文）》;第40卷(第1期);第11-18页 *
空间非合作目标的近距离相对位姿测量技术研究;魏许;《中国优秀硕士学位论文全文数据库 信息科技辑》(第06期);第I138-689页 *
非合作大目标位姿测量的线结构光视觉方法;高学海等;《宇航学报》;第33卷(第6期);第728-735页 *

Also Published As

Publication number Publication date
CN116681733A (zh) 2023-09-01

Similar Documents

Publication Publication Date Title
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN110068335B (zh) 一种gps拒止环境下无人机集群实时定位方法及系统
CN110116407B (zh) 柔性机器人位姿测量方法及装置
CN111076733B (zh) 一种基于视觉与激光slam的机器人室内建图方法及系统
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN102722697B (zh) 一种无人飞行器视觉自主导引着陆的目标跟踪方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
WO2022183785A1 (zh) 机器人定位方法、装置、机器人和可读存储介质
CN114419147A (zh) 一种救援机器人智能化远程人机交互控制方法及系统
CN112435262A (zh) 基于语义分割网络和多视图几何的动态环境信息检测方法
CN116681733B (zh) 一种空间非合作目标近距离实时位姿跟踪方法
CN111238467A (zh) 一种仿生偏振光辅助的无人作战飞行器自主导航方法
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
CN111273312A (zh) 一种智能车辆定位与回环检测方法
Sun et al. Autonomous state estimation and mapping in unknown environments with onboard stereo camera for micro aerial vehicles
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
Zhao et al. Review of slam techniques for autonomous underwater vehicles
CN117152249A (zh) 基于语义一致性的多无人机协同建图与感知方法及系统
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN114648584A (zh) 一种用于多源融合定位的鲁棒性控制方法和系统
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
CN111121779B (zh) 一种无人机所处飞行区域的实时检测方法
CN117451032A (zh) 一种低算力与松耦合的激光雷达和imu的slam方法及系统
CN117115271A (zh) 无人机飞行过程中的双目相机外参数自标定方法及系统
CN117029870A (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