CN113175929A - 一种基于upf的空间非合作目标相对位姿估计方法 - Google Patents

一种基于upf的空间非合作目标相对位姿估计方法 Download PDF

Info

Publication number
CN113175929A
CN113175929A CN202110269577.2A CN202110269577A CN113175929A CN 113175929 A CN113175929 A CN 113175929A CN 202110269577 A CN202110269577 A CN 202110269577A CN 113175929 A CN113175929 A CN 113175929A
Authority
CN
China
Prior art keywords
target
coordinate system
frame image
camera
image
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
CN202110269577.2A
Other languages
English (en)
Other versions
CN113175929B (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 CN202110269577.2A priority Critical patent/CN113175929B/zh
Publication of CN113175929A publication Critical patent/CN113175929A/zh
Application granted granted Critical
Publication of CN113175929B publication Critical patent/CN113175929B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于UPF的空间非合作目标相对位姿估计方法，包括以下步骤：相对位姿和SURF特征点三维坐标的初始化，SURF特征点跟踪匹配以及基于UPF的相对位姿估计三部分。本发明将视觉即时定位和地图构建SLAM领域中用于位姿估计的贝叶斯滤波方法和测量反演方法引入至空间视觉导航领域，可对完全非合作目标的相对位姿进行测量，可应用于在轨服务，比如空间碎片的捕获，也可应用于空间攻防，获取非合作目标的状态信息，服务于进一步的识别和操作决策。本发明可提供高精度的位姿输出，具有良好的抗噪声性能，无需目标的外形、运动状态等先验信息，不受限于关于目标状态的假设，具有良好的自主性、通用性和便捷性。

Description

一种基于UPF的空间非合作目标相对位姿估计方法
技术领域
本发明涉及空间视觉导航技术领域，特别是一种基于无迹粒子滤波UPF的空间非合作目标相对位姿估计方法。
背景技术
空间技术发展对完成复杂任务的需求日益增加，如抓捕或转移空间碎片和废弃卫星、维修或更换有故障的在轨航天器、通过加注燃料延长寿命的卫星等，这些任务要求追踪/任务航天器近距离精确估计目标的相对位置和姿态。目标在非合作状态下，运动情况和结构完全未知或部分未知，并且没有星间链路等辅助进行位姿测量，因此非合作目标相对位姿测量具有挑战性，难度大。根据非合作目标的几何模型是否已知，非合作目标又可被分为模型已知非合作目标和模型未知非合作目标。依赖于非合作目标的明显的几何特征或已知的三维模型，模型已知非合作目标的相对位姿估计问题已经得到了很好的解决。相反，模型未知非合作目标先验信息缺失、运动状态不确定，其相对位姿估计问题更为复杂。
近年来，国内外学者开始考虑引入移动机器人视觉即时定位和地图构建SLAM方法来解决模型未知非合作目标的相对位姿估计问题，斯坦福大学的Sean Augenstein等人从FastSLAM算法受到启发，提出了一种基于FastSLAM算法的非合作目标位姿跟踪和三维重构方法，并通过水下目标实验验证了算法的可行性。北京装备学院的郝刚涛等人针对此方法中的相对位姿估计部分进行改进，联合无迹卡尔曼滤波UKF和粒子滤波PF实现了目标位姿参数的快速平滑估计。但是，上述两种方法均假定目标初始位姿已知，不利于实际应用。德累斯顿工业大学的Schnitzer等人针对空间失效航天器的自主维修问题，提出基于EKF-SLAM和RANSAC算法来估计目标相对位姿，恢复目标三维结构，但该方法依赖于扩展卡尔曼滤波EKF对非线性方程的线性化处理，当系统非线性程度较高时，过大的线性化误差会导致滤波性能下降。
发明内容
本发明所要解决的技术问题是克服现有技术的不足而提供一种基于UPF的空间非合作目标相对位姿估计方法，将视觉即时定位和地图构建SLAM领域中用于相对位姿估计的贝叶斯滤波和测量反演方法引入至空间视觉导航领域，在无目标先验信息和运动状态信息情况下，能够实现非合作目标的相对位姿估计，为后续的空间近场操作提供必要的位姿信息。
本发明为解决上述技术问题采用以下技术方案：
根据本发明提出的一种基于UPF的空间非合作目标相对位姿估计方法，包括以下步骤：
步骤一、读取单目相机实时采集的图像，从开始帧中筛选连续两帧图像作为初始两帧图像，检测初始两帧图像上的SURF特征点，并进行SURF特征点匹配，删除离群点，得到特征匹配点对，并依据对极几何关系计算本质矩阵，分解本质矩阵后得到初始两帧图像间的相对位姿；
根据初始两帧图像间的相对位姿，利用三角测量法获取第j个SURF特征点在相机坐标系C下的初始位置
Figure BDA0002973693460000021
j＝1,2,...,M，其中下标j表示SURF特征点的索引，M为SURF特征点个数；
任取一SURF特征点作为目标本体坐标系的原点，建立目标本体坐标系B，目标本体坐标系固连于目标上，目标本体坐标系的指向和相机坐标系平行，记目标本体坐标系的原点在相机坐标系中的坐标为
Figure BDA0002973693460000022
获得相机和目标间的相对姿态初始值α0和相对位置初始值ρ0以及第j个SURF特征点在目标本体坐标系下的三维坐标初始值
Figure BDA0002973693460000023
Figure BDA0002973693460000024
步骤二、以k表示图像采样时刻索引，跟踪匹配第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标zj,k＝[xj,k yj,k]T，其中，xj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的水平坐标，yj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的垂直坐标，上标T为转置；
步骤三、以目标和相机间的相对位姿向量作为状态变量，构建目标状态转移方程，以zj,k作为实际测量值，构建测量方程，利用贝叶斯滤波预测目标和相机间的相对姿态；利用测量反演法预测目标和相机间的相对位置，根据预测得到的相对姿态和相对位置，优化目标状态转移方程；
根据测量方程和优化后的状态转移方程，将步骤一所得到的α0，ρ0和
Figure BDA0002973693460000025
作为初始值，采用无迹粒子滤波UPF方法估计相机和目标间的相对位姿。
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案，在步骤一中，选取SURF特征点数目大于预设的特征点数目阈值的两张连续两帧图像作为初始两帧图像，利用快速最近邻逼近搜索FLANN方法进行SURF特征点匹配，用比值判别法删除离群点；
当特征匹配点对数目大于预设的特征点对数目阈值时，由特征匹配点对计算本质矩阵，分解本质矩阵后得到初始两帧图像间的相对位姿；利用三角测量法计算第j个SURF特征点在相机坐标系C下的初始位置
Figure BDA0002973693460000031
任取一SURF特征点作为目标本体坐标系的原点，建立目标本体坐标系B，目标本体坐标系固连于目标上，目标本体坐标系的指向和相机坐标系平行，记目标本体坐标系的原点在相机坐标系中的坐标为
Figure BDA0002973693460000032
则目标和相机间的相对姿态初始值α0＝[0 0 0]T，以欧拉角描述目标和相机间的相对姿态，规定欧拉旋转顺序为X-Y-Z，即相机坐标系依次绕其X轴、旋转后的Y轴、再旋转后的Z轴旋转后，与目标本体坐标系重合，目标和相机间的相对位置初始值
Figure BDA0002973693460000033
令
Figure BDA0002973693460000034
表示第j个SURF特征点在目标本体坐标系下的三维坐标，因此有
Figure BDA0002973693460000035
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案，步骤二中，采用KLT光流跟踪法与特征检测相结合的方法对第k帧图像上的SURF特征点进行跟踪匹配，即采用KLT光流法跟踪SURF特征点的同时也进行SURF特征点的检测，当检测的SURF特征点的位置和光流跟踪法跟踪的SURF特征点位置之间的像素差小于预设的像素阈值时，则配对成功，即完成第k帧图像和第k-1帧图像中SURF特征点的匹配，并抛弃未匹配到的SURF特征点；根据相机内参，将匹配到的第k帧图像第j个SURF特征点的像素坐标转换为第k帧图像第j个SURF特征点的图像物理坐标zj,k＝[xj,k yj,k]T，作为实际测量值。
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案，步骤三中，构建目标状态转移方程和测量方程的具体过程如下：
定义目标的状态向量s＝[α ω ρ v]T，其中α＝[φθψ]T表示目标本体坐标系相对于相机坐标系的旋转欧拉角，ω＝[ωx ωy ωz]T表示目标在相机坐标系下的旋转角速度矢量，ρ＝[ρx ρy ρz]T表示目标在相机坐标系下的位置，即目标本体坐标系原点在相机坐标系下的三维坐标，v＝[vx vy vz]T表示目标在相机坐标系下的平移线速度；其中φ，θ，ψ分别表示α绕相机坐标系的X轴、Y轴、Z轴旋转的旋转欧拉角，ωx，ωy，ωz为ω的X、Y、Z三轴角速度分量，ρx，ρy，ρz分别为目标本体坐标系原点在相机坐标系下的X、Y、Z坐标，vx，vy，vz分别表示v的X、Y、Z三轴平移线速度分量；
以下标k，k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻，Δt表示采样时刻间隔，则根据刚体的动力学、运动学模型，目标的状态转移方程表示为：
Figure BDA0002973693460000041
其中，αk，αk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标本体坐标系相对于相机坐标系的旋转欧拉角，ωk，ωk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的旋转角速度矢量，ρk、ρk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的位置，vk、vk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的平移线速度，M(αk-1)为矩阵，该矩阵用于描述第k-1帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度
Figure BDA0002973693460000042
的转换关系，其中上标·表示对时间的导数；假定目标以X-Y-Z的欧拉旋转顺序分别旋转φ，θ，ψ，已知目标在旋转前的目标本体坐标系下的旋转角速度为
Figure BDA0002973693460000043
Figure BDA0002973693460000044
分别为ωB在旋转前的目标本体坐标系下的X、Y、Z三轴角速度分量，其中RB/C为相机坐标系到目标本体坐标系的变换矩阵，则存在
Figure BDA0002973693460000051
则有
Figure BDA0002973693460000052
其中，M(α)为矩阵，该矩阵用于描述第k帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度
Figure BDA0002973693460000053
的转换关系；公式(1)给出的速度分量状态转移方程中的J为惯量矩阵，m为目标质量，r表示目标参考点到质心的位置偏移量，Ttotal表示作用于目标的外部总力矩，F表示作用于目标质心的总外力；在此Ttotal、F、r均不知，由Ttotal、F、r所引起的角速度和线速度变化通过引入过程噪声qk～N(0,Qk)来考虑，Qk为过程噪声qk的协方差矩阵，则(1)式改写为
Figure BDA0002973693460000054
其中，sk，sk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标的状态向量，qk表示第k帧图像采样时刻的过程噪声，角速度过程噪声qω,k、线速度过程噪声qv,k均为零均值的高斯噪声，即qω,k～N(0,Qω,k)，qv,k～N(0,Qv,k)，Qω,k和Qv,k分别为角速度过程噪声qω,k和线速度过程噪声qv,k的协方差矩阵，f(·)为状态转移函数；目标为刚体，则目标上的SURF特征点在目标本体坐标系下的坐标不随时间变化，即
Figure BDA0002973693460000061
分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标上的第j个SURF特征点在目标本体坐标系的三维坐标；
假设
Figure BDA0002973693460000062
为第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标，
Figure BDA0002973693460000063
分别为
Figure BDA0002973693460000064
在相机坐标系下的X、Y、Z坐标；ρk为第k帧图像采样时刻目标在相机坐标系下的位置，
Figure BDA0002973693460000065
表示第k帧图像采样时刻目标本体坐标系到相机坐标系的转换矩阵；则
Figure BDA0002973693460000066
和
Figure BDA0002973693460000067
存在以下转换关系：
Figure BDA0002973693460000068
定义第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值为
Figure BDA0002973693460000069
Figure BDA00029736934600000610
分别为
Figure BDA00029736934600000611
的X轴、Y轴坐标，相机模型为针孔模型，假设焦距为fc，
Figure BDA00029736934600000612
和
Figure BDA00029736934600000613
存在透视投影关系，以函数gj(·)表示如下，
Figure BDA00029736934600000614
定义，
Figure BDA00029736934600000615
Figure BDA00029736934600000616
表示目标上的第j个SURF特征点在目标本体坐标系下的三维坐标，XB表示目标上所有M个SURF特征点在目标本体坐标系下的三维坐标构成的矩阵，zk＝[z1,k z2,k … zM,k]T，根据权利要求3中，zj,k为第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值，zk表示第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标实际测量值所构成的矩阵；测量噪声nk～N(0,Nk)，Nk为测量噪声nk的协方差矩阵，函数g(·)为测量函数、表示所有M个SURF特征点透视投影函数gj(·)的组合，结合式(4)和式(5)构建测量方程：
Figure BDA00029736934600000617
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案，在步骤三中，利用测量反演法预测目标和相机间的相对位置，根据预测得到的相对姿态和相对位置，优化目标状态转移方程的具体流程如下：
首先根据式(3)给出的状态转移方程，利用第k-1帧图像采样时刻目标相对姿态估计值
Figure BDA0002973693460000071
和目标在相机坐标系下的旋转角速度矢量估计值
Figure BDA0002973693460000072
计算第k帧图像采样时刻相对姿态预测值
Figure BDA0002973693460000073
和旋转角速度矢量预测值
Figure BDA0002973693460000074
Figure BDA0002973693460000075
其中，上标∧表示估计值，上标∧和下标“k/k-1”构成预测值；
然后根据第k帧图像采样时刻相对姿态预测值
Figure BDA0002973693460000076
目标上的第j个SURF特征点在目标本体坐标系下的三维坐标
Figure BDA0002973693460000077
和第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值zj,k，由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值
Figure BDA0002973693460000078
由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的过程以函数h(·)表示，即
Figure BDA0002973693460000079
由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值
Figure BDA00029736934600000710
的具体方法为：由式(7)得到的
Figure BDA00029736934600000711
目标上的第j个SURF特征点在目标本体坐标系下的三维坐标
Figure BDA00029736934600000712
和第k帧图像采样时刻目标和相机间的相对位置预测值
Figure BDA00029736934600000713
Figure BDA00029736934600000714
为未知量，根据式(4)和式(5)计算第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值
Figure BDA00029736934600000715
Figure BDA00029736934600000716
其中，
Figure BDA00029736934600000717
表示第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标预测值，
Figure BDA00029736934600000718
分别为
Figure BDA00029736934600000719
的X、Y、Z轴坐标，
Figure BDA00029736934600000720
表示第k帧图像采样时刻目标本体坐标系到相机坐标系的变换矩阵预测值，
Figure BDA00029736934600000721
Figure BDA00029736934600000722
分别为
Figure BDA00029736934600000723
的X、Y轴坐标，根据步骤二得到的zj,k＝[xj,k yj,k]T以及由式(9)获得的第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值
Figure BDA0002973693460000081
构建第k帧图像采样时刻第j个SURF特征点的投影误差代价函数ej,k
Figure BDA0002973693460000082
令ej,k＝0，将(9)式代入式(10)并化简，得
Figure BDA0002973693460000083
其中
Figure BDA0002973693460000084
分别对应
Figure BDA0002973693460000085
的每一行向量，A为2M×3的矩阵，B为2M×1向量；如果检测的特征点数量M大于2，则式(11)为超定方程，由最小二乘法求解得到
Figure BDA0002973693460000086
依据上述对第k帧图像采样时刻目标和相机间的相对位置ρk的预测，重新选取第k帧图像采样时刻目标的状态向量sk＝[αkωkρk]，优化目标状态转移方程为
Figure BDA0002973693460000087
其中sk-1表示第k-1帧图像采样时刻目标的状态向量。
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案，步骤三中，采用无迹粒子滤波UPF算法估计相机和目标间的相对位姿的具体过程如下：
在初始采样时刻k＝0，由步骤一中初始化得到的α0和ρ0作为状态向量的初始值
Figure BDA0002973693460000088
为初始状态误差协方差矩阵，以一个高斯分布
Figure BDA0002973693460000089
随机产生N个
Figure BDA00029736934600000810
Figure BDA00029736934600000811
为第i个粒子所对应的相对位姿状态向量的初始值，i＝1,2,…,N，N为粒子的总数；初始化粒子权值
Figure BDA00029736934600000812
对N个粒子加权求和，即求粒子的平均值
Figure BDA0002973693460000091
以
Figure BDA0002973693460000092
作为每个粒子的均值
Figure BDA0002973693460000093
计算每个粒子协方差矩阵
Figure BDA0002973693460000094
其中
Figure BDA0002973693460000095
为第i个粒子所对应的相对位姿状态向量的初始值，
Figure BDA0002973693460000096
为第i个粒子的均值；
对后续时刻的具体处理步骤如下：
步骤S1:利用无迹卡尔曼滤波UKF生成粒子滤波PF的重要性概率密度函数，更新第k帧图像采样时刻第i个粒子的均值
Figure BDA0002973693460000097
和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵
Figure BDA0002973693460000098
并根据
Figure BDA0002973693460000099
和
Figure BDA00029736934600000910
生成新的粒子集合
Figure BDA00029736934600000911
步骤S1的具体方法为：
假设第k-1帧图像采样时刻第i个粒子
Figure BDA00029736934600000912
均值为
Figure BDA00029736934600000913
第k-1帧图像采样时刻的第i个粒子
Figure BDA00029736934600000914
状态误差协方差矩阵为
Figure BDA00029736934600000915
粒子权值为
Figure BDA00029736934600000916
状态向量维数为n，总粒子数为N，对每个粒子，以
Figure BDA00029736934600000917
为基准，对称分布采样2n+1个样本点
Figure BDA00029736934600000918
其中u＝0,...,2n表示样本点索引：
Figure BDA00029736934600000919
并计算第i个粒子的第u个样本点权值
Figure BDA00029736934600000920
Figure BDA00029736934600000921
其中λ为微调参数，控制样本点到均值的距离；由
Figure BDA00029736934600000922
组成的样本点集合也被称为Sigma点集合；
由式(12)目标状态转移方程计算样本点预测值
Figure BDA00029736934600000923
Figure BDA00029736934600000924
并计算样本点加权均值的一步预测
Figure BDA00029736934600000925
和状态误差协方差矩阵的一步预测
Figure BDA0002973693460000101
Figure BDA0002973693460000102
令
Figure BDA0002973693460000103
分别表示第k帧图像采样时刻第i个粒子的第u个样本点预测值
Figure BDA0002973693460000104
中的相对姿态和相对位置分量，根据式(6)测量方程计算第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标预测测量值所构成的矩阵
Figure BDA0002973693460000105
Figure BDA0002973693460000106
表示第k帧图像采样第i个粒子的第u个样本点所描述的相对位姿状态下，目标上的第j个SURF特征点图像物理坐标预测测量值；得到：
Figure BDA0002973693460000107
进一步形成
Figure BDA0002973693460000108
的加权均值
Figure BDA0002973693460000109
和其自协方差矩阵
Figure BDA00029736934600001010
以及第i个粒子的第u个样本点预测值
Figure BDA00029736934600001011
和
Figure BDA00029736934600001012
的互协方差矩阵
Figure BDA00029736934600001013
如下式所示：
Figure BDA00029736934600001014
根据上式
Figure BDA00029736934600001015
和
Figure BDA00029736934600001016
计算无迹卡尔曼滤波UKF增益
Figure BDA00029736934600001017
更新第k帧图像采样时刻第i个粒子的均值
Figure BDA00029736934600001018
和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵
Figure BDA00029736934600001019
Figure BDA00029736934600001020
由
Figure BDA00029736934600001021
和
Figure BDA00029736934600001022
生成粒子滤波PF的重要性概率密度函数
Figure BDA00029736934600001023
采样第k帧图像采样时刻第i个粒子
Figure BDA00029736934600001024
其中，N(a,b)表示均值为a，协方差矩阵为b的高斯分布；更新第k帧图像采样时刻第i个粒子权值
Figure BDA0002973693460000111
Figure BDA0002973693460000112
其中似然函数
Figure BDA0002973693460000113
服从均值为
Figure BDA0002973693460000114
协方差矩阵为Nk的高斯分布，即
Figure BDA0002973693460000115
Figure BDA0002973693460000116
表示第k帧图像采样时刻第i个粒子所描述的相对位姿状态下目标上所有M个SURF特征点图像物理坐标的预测测量值，状态转移概率
Figure BDA0002973693460000117
服从均值为
Figure BDA0002973693460000118
协方差矩阵为Qk的高斯分布，即
Figure BDA0002973693460000119
Figure BDA00029736934600001110
表示第k帧图像采样时刻第i个粒子的预测值；归一化第k帧图像采样时刻第i个粒子权值
Figure BDA00029736934600001111
步骤S2:采用系统重采样算法进行粒子重采样：重采样前第k帧图像采样时刻第i个粒子
Figure BDA00029736934600001112
重采样后更新第k帧图像采样时刻第i个粒子的均值和协方差矩阵为
Figure BDA00029736934600001113
和
Figure BDA00029736934600001114
即重采样后更新第k帧图像采样时刻第i个粒子
Figure BDA00029736934600001115
平均第k帧图像采样时刻第i个粒子权值
Figure BDA00029736934600001116
步骤S3:重采样后，对第k帧图像采样时刻第i个粒子
Figure BDA00029736934600001117
进行加权求和，得到第k帧图像采样时刻状态向量估计值
Figure BDA00029736934600001118
从而即得到相机和目标间的相对姿态估计值
Figure BDA00029736934600001119
和相对位置估计值
Figure BDA00029736934600001120
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案，在初始两帧图像中的第二帧图像中找出与第一帧图像中每个SURF特征点对应的两个待匹配点，将这两个待匹配点按照分别距第一帧图像中对应的SURF特征点的欧式距离的远近分为最近邻点和次近邻点，当最近邻点距第一帧图像中对应的SURF特征点的欧式距离与次近邻点距第一帧图像中对应的SURF特征点的欧式距离的比值小于预设阈值时，则认为最近邻点匹配为正确匹配，得到特征匹配点对，并删除次近邻点，即删除离群点。
本发明采用以上技术方案与现有技术相比，具有以下技术效果：
(1)本发明采用SURF特征点进行初始化，计算速度快，对空间光照变化具有良好的不变性；
(2)本发明在KLT光流追踪法基础上引入特征检测来跟踪匹配特征点，减少了在目标处于遮挡情况下特征点的追踪错误，鲁棒性好；
(3)本发明主要采用测量反演法预测相对位置，计算量较小；
(4)本发明采用系统重采样算法对粒子进行重采样，可有效抑制粒子退化现象；
(5)本发明对于先验信息完全未知的非合作目标，能够得到较高的位姿估计精度。
附图说明
图1是基于UPF的空间非合作目标相对位姿估计方法。
图2是立方星仿真模型。
图3是目标运动图像序列；其中，(a)是目标运动过程中相机采集到的第1帧图像，(b)是第10帧图像，(c)是第20帧图像，(d)是第30帧图像，(e)是第40帧图像，(f)第50帧图像。
图4是SURF检测匹配。
图5是三维投影误差。
图6是特征点跟踪。
图7是按比例缩放的目标特征点三维重建。
图8是相对姿态估计结果；其中，(a)是俯仰角φ估计结果，(b)是偏航角θ估计结果，(c)是滚转角ψ估计结果。
图9是相对位置估计结果；其中，(a)是X为位置ρx估计结果，(b)是Y位置ρy估计结果，(c)是Z位置ρz估计结果。
图10是旋转角速度估计结果；其中，(a)是绕X轴旋转角速度ωx，(b)是绕Y轴旋转角速度ωy，(c)是绕Z轴旋转角速度ωz。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图及具体实施例对本发明进行详细描述。
本发明针对背景技术中的模型未知非合作目标相对位姿估计方法中存在的问题，提供了一种基于UPF的空间非合作目标相对位姿估计方法，采用单目相机获取目标图像，在初始两帧通过SURF(Speeded Up Robust Features，简称SURF)特征点匹配、本质矩阵的计算和分解、以及三角测量方法计算相机和目标间的相对位姿初始值和SURF特征点在目标本体坐标系下的三维坐标初始值，在后续帧结合特征检测和KLT(Kanade-Lucas-Tomasi，简称KLT)算法跟踪匹配图像帧间SURF特征点，将其图像物理坐标作为测量值，利用对非线性系统具有普遍适应性的UPF追踪相对位姿，实现无任何辅助信息的空间非合作目标相对位姿的连续测量。
如图1所示，本发明提供的一种实施例，具体实施步骤如下：步骤一、读取单目相机实时采集的图像，从开始帧中筛选连续两帧图像作为初始两帧图像进行SURF特征点检测和SURF特征点匹配，计算本质矩阵并分解，求解初始两帧图像间的相对位姿，利用三角测量法获取SURF特征点在相机坐标系下的位置，建立目标本体坐标系，得到相机和目标间的相对位姿初始值和SURF特征点在目标本体坐标系下的三维坐标初始值；步骤二、追踪匹配目标SURF特征点在后续帧图像中的图像物理坐标，并作为测量值；步骤三、结合刚体的运动学和动力学模型，并引入高斯过程噪声模拟角速度和线速度变化，构建目标状态转移方程和测量方程，在此情况下，仅由目标外部总力矩未知所引起的角速度变化较小，而由未知的目标外部总力矩、目标质心所受总外力以及目标本体坐标系的原点到质心的位置偏移量共同影响的线速度变化较大，因此线速度过程噪声的协方差矩阵需要取较大的值，以充分模拟线速度变化的不确定性。对此，为避免过大的线速度噪声协方差矩阵导致滤波收敛过慢或发散，相机与目标间的相对位置将通过测量反演法来预测，相机与目标间的相对姿态则通过贝叶斯滤波来预测，从而优化目标状态转移方程，之后根据优化后的目标状态转移方程和测量方程，采用UPF算法估计相机和目标间的相对位姿。
以立方星(CubeSat)为非合作目标完成算法的仿真验证。目标如图2所示，尺寸设计为0.1×0.1×0.2m。假设目标以角速度ω绕滚动轴旋转，同时相机以速度v接近目标，其中
Figure BDA0002973693460000131
初始时刻目标在相机坐标系下的位置为ρ0＝[-0.15 -0.15 0.8]T m。相机焦距fc＝109mm，图像分辨率为640×480pixel，图像采样速率为1帧/秒(Fps)，采集图像帧数为61，图3给出了相机视图下目标的运动情况，从左到右、从上到下依次为第1、10、20、30、40、50帧。图3中的(a)是目标运动过程中相机采集到的第1帧图像，图3中的(b)是第10帧图像，图3中的(c)是第20帧图像，图3中的(d)是第30帧图像，图3中的(e)是第40帧图像，图3中的(f)第50帧图像。
选取初始两帧图像进行SURF特征点检测，FLANN特征点匹配，利用比值判别法删除误匹配点，其匹配结果如图4所示。由SURF特征匹配点对计算本质矩阵并分解得到初始两帧图像间的相对位姿，利用三角测量法得到目标SURF特征点在相机坐标系下的位置，任取一特征点为目标本体坐标系原点，坐标轴方向与相机坐标系平行，可得SURF特征点在目标本体坐标系下的位置。为说明初始化精度，图5显示了SURF特征点在像素坐标系下的二维投影坐标和跟踪匹配特征点。“+”和“O”基本重合表明相机和目标间的相对位姿初始值和SURF特征点在目标本体坐标系下的三维坐标初始值取得了较高的精度。
获得相机和目标间的相对位姿初始值和SURF特征点在目标本体坐标系下的初始值后，利用KLT光流追踪法结合特征检测来进行后续帧的SURF特征点跟踪匹配，并根据所跟踪匹配到的SURF特征点来筛选其对应的三维特征点。图6“+”号表示某一时刻追踪匹配到的特征点。图7显示了追踪匹配到的特征点对应的按比例缩放的三维特征点。
在后续帧图像采样时刻，相机和目标间的的相对位姿由UPF滤波迭代输出。由于单目初始化所存在的尺度不确定性，所得到的目标相对位置ρ和三维点坐标
Figure BDA0002973693460000141
与真实值存在着一个尺度因子，且均为无量纲坐标，不便于计算真实值与估计值间误差。为验证本发明的有效性，仿真实验中假定目标SURF特征点在目标本体坐标系下的三维坐标已知，相对位姿初始值为0，过程噪声和测量噪声为零均值的高斯白噪声，采样粒子数为50，共仿真10次。利用UPF估计相机与目标间的相对位姿结果如图8-图10所示，其中图8为相对姿态估计结果，图9为相对位置估计结果，图10为旋转角速度估计结果。实线表示真实值，虚线表示本发明方法所得出的估计值。图8是相对姿态估计结果；其中，图8中的(a)是俯仰角φ估计结果，图8中的(b)是偏航角θ估计结果，图8中的(c)是滚转角ψ估计结果。图9是相对位置估计结果；其中，图9中的(a)是X为位置ρx估计结果，图9中的(b)是Y位置ρy估计结果，图9中的(c)是Z位置ρz估计结果。图10是旋转角速度估计结果；其中，图10中的(a)是绕X轴旋转角速度ωx，图10中的(b)是绕Y轴旋转角速度ωy，图10中的(c)是绕Z轴旋转角速度ωz。
由图8-图10可知，估计值曲线与真实值曲线基本一致，相对姿态估计误差小于0.05rad，相对位置误差小于0.1m，说明本发明方法具有较高的精度，能够有效的估计出完全非合作目标的相对位姿，尤其是在第31帧角速度发生突变，平动保持不变时，本发明方法仍能够跟踪目标，误差均在允许的范围内，说明具有良好的稳健性。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围内。

Claims (7)

1.一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，包括以下步骤：
步骤一、读取单目相机实时采集的图像，从开始帧中筛选连续两帧图像作为初始两帧图像，检测初始两帧图像上的SURF特征点，并进行SURF特征点匹配，删除离群点，得到特征匹配点对，并依据对极几何关系计算本质矩阵，分解本质矩阵后得到初始两帧图像间的相对位姿；
根据初始两帧图像间的相对位姿，利用三角测量法获取第j个SURF特征点在相机坐标系C下的初始位置
Figure FDA0002973693450000011
其中下标j表示SURF特征点的索引，M为SURF特征点个数；
任取一SURF特征点作为目标本体坐标系的原点，建立目标本体坐标系B，目标本体坐标系固连于目标上，目标本体坐标系的指向和相机坐标系平行，记目标本体坐标系的原点在相机坐标系中的坐标为
Figure FDA0002973693450000012
获得相机和目标间的相对姿态初始值α0和相对位置初始值ρ0以及第j个SURF特征点在目标本体坐标系下的三维坐标初始值
Figure FDA0002973693450000013
Figure FDA0002973693450000014
步骤二、以k表示图像采样时刻索引，跟踪匹配第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标zj,k＝[xj,k yj,k]T，其中，xj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的水平坐标，yj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的垂直坐标，上标T为转置；
步骤三、以目标和相机间的相对位姿向量作为状态变量，构建目标状态转移方程，以zj,k作为实际测量值，构建测量方程，利用贝叶斯滤波预测目标和相机间的相对姿态；利用测量反演法预测目标和相机间的相对位置，根据预测得到的相对姿态和相对位置，优化目标状态转移方程；
根据测量方程和优化后的状态转移方程，将步骤一所得到的α0，ρ0和
Figure FDA0002973693450000015
作为初始值，采用无迹粒子滤波UPF方法估计相机和目标间的相对位姿。
2.根据权利要求1所述的一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，在步骤一中，选取SURF特征点数目大于预设的特征点数目阈值的两张连续两帧图像作为初始两帧图像，利用快速最近邻逼近搜索FLANN方法进行SURF特征点匹配，用比值判别法删除离群点；
当特征匹配点对数目大于预设的特征点对数目阈值时，由特征匹配点对计算本质矩阵，分解本质矩阵后得到初始两帧图像间的相对位姿；利用三角测量法计算第j个SURF特征点在相机坐标系C下的初始位置
Figure FDA0002973693450000021
任取一SURF特征点作为目标本体坐标系的原点，建立目标本体坐标系B，目标本体坐标系固连于目标上，目标本体坐标系的指向和相机坐标系平行，记目标本体坐标系的原点在相机坐标系中的坐标为
Figure FDA0002973693450000022
则目标和相机间的相对姿态初始值α0＝[0 0 0]T，以欧拉角描述目标和相机间的相对姿态，规定欧拉旋转顺序为X-Y-Z，即相机坐标系依次绕其X轴、旋转后的Y轴、再旋转后的Z轴旋转后，与目标本体坐标系重合，目标和相机间的相对位置初始值
Figure FDA0002973693450000023
令
Figure FDA0002973693450000024
表示第j个SURF特征点在目标本体坐标系下的三维坐标，因此有
Figure FDA0002973693450000025
3.根据权利要求1所述的一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，步骤二中，采用KLT光流跟踪法与特征检测相结合的方法对第k帧图像上的SURF特征点进行跟踪匹配，即采用KLT光流法跟踪SURF特征点的同时也进行SURF特征点的检测，当检测的SURF特征点的位置和光流跟踪法跟踪的SURF特征点位置之间的像素差小于预设的像素阈值时，则配对成功，即完成第k帧图像和第k-1帧图像中SURF特征点的匹配，并抛弃未匹配到的SURF特征点；根据相机内参，将匹配到的第k帧图像第j个SURF特征点的像素坐标转换为第k帧图像第j个SURF特征点的图像物理坐标zj,k＝[xj,k yj,k]T，作为实际测量值。
4.根据权利要求3所述的一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，步骤三中，构建目标状态转移方程和测量方程的具体过程如下：
定义目标的状态向量s＝[α ω ρ v]T，其中α＝[φ θ ψ]T表示目标本体坐标系相对于相机坐标系的旋转欧拉角，ω＝[ωx ωy ωz]T表示目标在相机坐标系下的旋转角速度矢量，ρ＝[ρx ρy ρz]T表示目标在相机坐标系下的位置，即目标本体坐标系原点在相机坐标系下的三维坐标，v＝[vx vy vz]T表示目标在相机坐标系下的平移线速度；其中φ，θ，ψ分别表示α绕相机坐标系的X轴、Y轴、Z轴旋转的旋转欧拉角，ωx，ωy，ωz为ω的X、Y、Z三轴角速度分量，ρx，ρy，ρz分别为目标本体坐标系原点在相机坐标系下的X、Y、Z坐标，vx，vy，vz分别表示v的X、Y、Z三轴平移线速度分量；
以下标k，k-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻，Δt表示采样时刻间隔，则根据刚体的动力学、运动学模型，目标的状态转移方程表示为：
Figure FDA0002973693450000031
其中，αk，αk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标本体坐标系相对于相机坐标系的旋转欧拉角，ωk，ωk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的旋转角速度矢量，ρk、ρk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的位置，vk、vk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标在相机坐标系下的平移线速度，M(αk-1)为矩阵，该矩阵用于描述第k-1帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度
Figure FDA0002973693450000032
的转换关系，其中上标·表示对时间的导数；假定目标以X-Y-Z的欧拉旋转顺序分别旋转φ，θ，ψ，已知目标在旋转前的目标本体坐标系下的旋转角速度为
Figure FDA0002973693450000033
Figure FDA0002973693450000034
分别为ωB在旋转前的目标本体坐标系下的X、Y、Z三轴角速度分量，其中RB/C为相机坐标系到目标本体坐标系的变换矩阵，则存在
Figure FDA0002973693450000035
则有
Figure FDA0002973693450000041
其中，M(α)为矩阵，该矩阵用于描述第k帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度
Figure FDA0002973693450000042
的转换关系；公式(1)给出的速度分量状态转移方程中的J为惯量矩阵，m为目标质量，r表示目标参考点到质心的位置偏移量，Ttotal表示作用于目标的外部总力矩，F表示作用于目标质心的总外力；在此Ttotal、F、r均不知，由Ttotal、F、r所引起的角速度和线速度变化通过引入过程噪声qk～N(0,Qk)来考虑，Qk为过程噪声qk的协方差矩阵，则(1)式改写为
sk＝f(sk-1)+qk
Figure FDA0002973693450000043
其中，sk，sk-1分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标的状态向量，qk表示第k帧图像采样时刻的过程噪声，角速度过程噪声qω,k、线速度过程噪声qv,k均为零均值的高斯噪声，即qω,k～N(0,Qω,k)，qv,k～N(0,Qv,k)，Qω,k和Qv,k分别为角速度过程噪声qω,k和线速度过程噪声qv,k的协方差矩阵，f(·)为状态转移函数；目标为刚体，则目标上的SURF特征点在目标本体坐标系下的坐标不随时间变化，即
Figure FDA0002973693450000044
Figure FDA0002973693450000045
分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标上的第j个SURF特征点在目标本体坐标系的三维坐标；
假设
Figure FDA0002973693450000046
为第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标，
Figure FDA0002973693450000047
分别为
Figure FDA0002973693450000048
在相机坐标系下的X、Y、Z坐标；ρk为第k帧图像采样时刻目标在相机坐标系下的位置，
Figure FDA0002973693450000049
表示第k帧图像采样时刻目标本体坐标系到相机坐标系的转换矩阵；则
Figure FDA0002973693450000051
和
Figure FDA0002973693450000052
存在以下转换关系：
Figure FDA0002973693450000053
定义第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值为
Figure FDA0002973693450000054
Figure FDA0002973693450000055
分别为
Figure FDA0002973693450000056
的X轴、Y轴坐标，相机模型为针孔模型，假设焦距为fc，
Figure FDA0002973693450000057
和
Figure FDA0002973693450000058
存在透视投影关系，以函数gj(·)表示如下，
Figure FDA0002973693450000059
定义，
Figure FDA00029736934500000510
Figure FDA00029736934500000511
表示目标上的第j个SURF特征点在目标本体坐标系下的三维坐标，XB表示目标上所有M个SURF特征点在目标本体坐标系下的三维坐标构成的矩阵，zk＝[z1,k z2,k…zM,k]T，根据权利要求3中，zj,k为第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值，zk表示第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标实际测量值所构成的矩阵；测量噪声nk～N(0,Nk)，Nk为测量噪声nk的协方差矩阵，函数g(·)为测量函数、表示所有M个SURF特征点透视投影函数gj(·)的组合，结合式(4)和式(5)构建测量方程：
Figure FDA00029736934500000512
5.根据权利要求4所述的一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，在步骤三中，利用测量反演法预测目标和相机间的相对位置，根据预测得到的相对姿态和相对位置，优化目标状态转移方程的具体流程如下：
首先根据式(3)给出的状态转移方程，利用第k-1帧图像采样时刻目标相对姿态估计值
Figure FDA00029736934500000513
和目标在相机坐标系下的旋转角速度矢量估计值
Figure FDA00029736934500000514
计算第k帧图像采样时刻相对姿态预测值
Figure FDA00029736934500000515
和旋转角速度矢量预测值
Figure FDA00029736934500000516
Figure FDA00029736934500000517
其中，上标^表示估计值，上标^和下标“k/k-1”构成预测值；
然后根据第k帧图像采样时刻相对姿态预测值
Figure FDA00029736934500000518
目标上的第j个SURF特征点在目标本体坐标系下的三维坐标
Figure FDA0002973693450000061
和第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值zj,k，由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值
Figure FDA0002973693450000062
由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的过程以函数h(·)表示，即
Figure FDA0002973693450000063
由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值
Figure FDA0002973693450000064
的具体方法为：由式(7)得到的
Figure FDA0002973693450000065
目标上的第j个SURF特征点在目标本体坐标系下的三维坐标
Figure FDA0002973693450000066
和第k帧图像采样时刻目标和相机间的相对位置预测值
Figure FDA0002973693450000067
Figure FDA0002973693450000068
为未知量，根据式(4)和式(5)计算第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值
Figure FDA0002973693450000069
Figure FDA00029736934500000610
其中，
Figure FDA00029736934500000611
表示第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标预测值，
Figure FDA00029736934500000612
分别为
Figure FDA00029736934500000613
的X、Y、Z轴坐标，
Figure FDA00029736934500000614
表示第k帧图像采样时刻目标本体坐标系到相机坐标系的变换矩阵预测值，
Figure FDA00029736934500000615
Figure FDA00029736934500000616
分别为
Figure FDA00029736934500000617
的X、Y轴坐标，根据步骤二得到的zj,k＝[xj,k yj,k]T以及由式(9)获得的第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值
Figure FDA00029736934500000618
构建第k帧图像采样时刻第j个SURF特征点的投影误差代价函数ej,k
Figure FDA00029736934500000619
令ej,k＝0，将(9)式代入式(10)并化简，得
Figure FDA0002973693450000071
其中
Figure FDA0002973693450000072
分别对应
Figure FDA0002973693450000073
的每一行向量，A为2M×3的矩阵，B为2M×1向量；如果检测的特征点数量M大于2，则式(11)为超定方程，由最小二乘法求解得到
Figure FDA0002973693450000074
依据上述对第k帧图像采样时刻目标和相机间的相对位置ρk的预测，重新选取第k帧图像采样时刻目标的状态向量sk＝[αk ωk ρk]，优化目标状态转移方程为
sk＝f(sk-1)+qk
Figure FDA0002973693450000075
其中sk-1表示第k-1帧图像采样时刻目标的状态向量。
6.根据权利要求5所述的一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，步骤三中，采用无迹粒子滤波UPF算法估计相机和目标间的相对位姿的具体过程如下：
在初始采样时刻k＝0，由步骤一中初始化得到的α0和ρ0作为状态向量的初始值
Figure FDA0002973693450000076
Figure FDA0002973693450000077
为初始状态误差协方差矩阵，以一个高斯分布
Figure FDA0002973693450000078
随机产生N个
Figure FDA0002973693450000079
Figure FDA00029736934500000710
为第i个粒子所对应的相对位姿状态向量的初始值，i＝1,2,…,N，N为粒子的总数；初始化粒子权值
Figure FDA00029736934500000711
对N个粒子加权求和，即求粒子的平均值
Figure FDA00029736934500000712
以
Figure FDA00029736934500000713
作为每个粒子的均值
Figure FDA00029736934500000714
计算每个粒子协方差矩阵
Figure FDA00029736934500000715
其中
Figure FDA00029736934500000716
为第i个粒子所对应的相对位姿状态向量的初始值，
Figure FDA00029736934500000717
为第i个粒子的均值；
对后续时刻的具体处理步骤如下：
步骤S1:利用无迹卡尔曼滤波UKF生成粒子滤波PF的重要性概率密度函数，更新第k帧图像采样时刻第i个粒子的均值
Figure FDA0002973693450000081
和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵
Figure FDA0002973693450000082
并根据
Figure FDA0002973693450000083
和
Figure FDA0002973693450000084
生成新的粒子集合
Figure FDA0002973693450000085
步骤S1的具体方法为：
假设第k-1帧图像采样时刻第i个粒子
Figure FDA0002973693450000086
均值为
Figure FDA0002973693450000087
第k-1帧图像采样时刻的第i个粒子
Figure FDA0002973693450000088
状态误差协方差矩阵为
Figure FDA0002973693450000089
粒子权值为
Figure FDA00029736934500000810
状态向量维数为n，总粒子数为N，对每个粒子，以
Figure FDA00029736934500000811
为基准，对称分布采样2n+1个样本点
Figure FDA00029736934500000812
其中u＝0,...,2n表示样本点索引：
Figure FDA00029736934500000813
并计算第i个粒子的第u个样本点权值
Figure FDA00029736934500000814
Figure FDA00029736934500000815
其中λ为微调参数，控制样本点到均值的距离；由
Figure FDA00029736934500000816
组成的样本点集合也被称为Sigma点集合；
由式(12)目标状态转移方程计算样本点预测值
Figure FDA00029736934500000817
Figure FDA00029736934500000818
并计算样本点加权均值的一步预测
Figure FDA00029736934500000819
和状态误差协方差矩阵的一步预测
Figure FDA00029736934500000820
Figure FDA00029736934500000821
令
Figure FDA0002973693450000091
分别表示第k帧图像采样时刻第i个粒子的第u个样本点预测值
Figure FDA0002973693450000092
中的相对姿态和相对位置分量，根据式(6)测量方程计算第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标预测测量值所构成的矩阵
Figure FDA0002973693450000093
Figure FDA0002973693450000094
表示第k帧图像采样第i个粒子的第u个样本点所描述的相对位姿状态下，目标上的第j个SURF特征点图像物理坐标预测测量值；得到：
Figure FDA0002973693450000095
进一步形成
Figure FDA0002973693450000096
的加权均值
Figure FDA0002973693450000097
和其自协方差矩阵
Figure FDA0002973693450000098
以及第i个粒子的第u个样本点预测值
Figure FDA0002973693450000099
和
Figure FDA00029736934500000910
的互协方差矩阵
Figure FDA00029736934500000911
如下式所示：
Figure FDA00029736934500000912
根据上式
Figure FDA00029736934500000913
和
Figure FDA00029736934500000914
计算无迹卡尔曼滤波UKF增益
Figure FDA00029736934500000915
更新第k帧图像采样时刻第i个粒子的均值
Figure FDA00029736934500000916
和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵
Figure FDA00029736934500000917
Figure FDA00029736934500000918
由
Figure FDA00029736934500000919
和
Figure FDA00029736934500000920
生成粒子滤波PF的重要性概率密度函数
Figure FDA00029736934500000921
采样第k帧图像采样时刻第i个粒子
Figure FDA00029736934500000922
其中，N(a,b)表示均值为a，协方差矩阵为b的高斯分布；更新第k帧图像采样时刻第i个粒子权值
Figure FDA00029736934500000923
Figure FDA00029736934500000924
其中似然函数
Figure FDA00029736934500000925
服从均值为
Figure FDA00029736934500000926
协方差矩阵为Nk的高斯分布，即
Figure FDA0002973693450000101
Figure FDA0002973693450000102
表示第k帧图像采样时刻第i个粒子所描述的相对位姿状态下目标上所有M个SURF特征点图像物理坐标的预测测量值，状态转移概率
Figure FDA0002973693450000103
服从均值为
Figure FDA0002973693450000104
协方差矩阵为Qk的高斯分布，即
Figure FDA0002973693450000105
Figure FDA0002973693450000106
表示第k帧图像采样时刻第i个粒子的预测值；归一化第k帧图像采样时刻第i个粒子权值
Figure FDA0002973693450000107
步骤S2:采用系统重采样算法进行粒子重采样：重采样前第k帧图像采样时刻第i个粒子
Figure FDA0002973693450000108
重采样后更新第k帧图像采样时刻第i个粒子的均值和协方差矩阵为
Figure FDA0002973693450000109
和
Figure FDA00029736934500001010
即重采样后更新第k帧图像采样时刻第i个粒子
Figure FDA00029736934500001011
平均第k帧图像采样时刻第i个粒子权值
Figure FDA00029736934500001012
步骤S3:重采样后，对第k帧图像采样时刻第i个粒子
Figure FDA00029736934500001013
进行加权求和，得到第k帧图像采样时刻状态向量估计值
Figure FDA00029736934500001014
从而即得到相机和目标间的相对姿态估计值
Figure FDA00029736934500001015
和相对位置估计值
Figure FDA00029736934500001016
7.根据权利要求2所述的一种基于UPF的空间非合作目标相对位姿估计方法，其特征在于，在初始两帧图像中的第二帧图像中找出与第一帧图像中每个SURF特征点对应的两个待匹配点，将这两个待匹配点按照分别距第一帧图像中对应的SURF特征点的欧式距离的远近分为最近邻点和次近邻点，当最近邻点距第一帧图像中对应的SURF特征点的欧式距离与次近邻点距第一帧图像中对应的SURF特征点的欧式距离的比值小于预设阈值时，则认为最近邻点匹配为正确匹配，得到特征匹配点对，并删除次近邻点，即删除离群点。
CN202110269577.2A 2021-03-12 2021-03-12 一种基于upf的空间非合作目标相对位姿估计方法 Active CN113175929B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110269577.2A CN113175929B (zh) 2021-03-12 2021-03-12 一种基于upf的空间非合作目标相对位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110269577.2A CN113175929B (zh) 2021-03-12 2021-03-12 一种基于upf的空间非合作目标相对位姿估计方法

Publications (2)

Publication Number Publication Date
CN113175929A true CN113175929A (zh) 2021-07-27
CN113175929B CN113175929B (zh) 2021-12-21

Family

ID=76921993

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110269577.2A Active CN113175929B (zh) 2021-03-12 2021-03-12 一种基于upf的空间非合作目标相对位姿估计方法

Country Status (1)

Country Link
CN (1) CN113175929B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114419259A (zh) * 2022-03-30 2022-04-29 中国科学院国家空间科学中心 一种基于物理模型成像仿真的视觉定位方法及系统
CN114549592A (zh) * 2022-04-24 2022-05-27 之江实验室 一种非合作抛体的轨迹预测与捕获方法和装置
CN114964266A (zh) * 2022-07-26 2022-08-30 中国人民解放军国防科技大学 基于多视觉矢量的运动状态协同群组相对姿态确定方法
CN116681733A (zh) * 2023-08-03 2023-09-01 南京航空航天大学 一种空间非合作目标近距离实时位姿跟踪方法
CN117647243B (zh) * 2024-01-30 2024-04-16 山东星辰卫星技术有限公司 一种基于6u立方星的凝视监测方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106441151A (zh) * 2016-09-30 2017-02-22 中国科学院光电技术研究所 一种基于视觉和主动光学融合的三维目标欧式空间重建的测量系统
CN110823214A (zh) * 2019-10-18 2020-02-21 西北工业大学 一种空间完全非合作目标相对位姿和惯量估计方法
CN111339629A (zh) * 2019-11-22 2020-06-26 北京理工大学 一种用于天基观测的空间目标机动轨道确定方法
CN112066879A (zh) * 2020-09-11 2020-12-11 哈尔滨工业大学 基于计算机视觉的气浮运动模拟器位姿测量装置及方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106441151A (zh) * 2016-09-30 2017-02-22 中国科学院光电技术研究所 一种基于视觉和主动光学融合的三维目标欧式空间重建的测量系统
CN110823214A (zh) * 2019-10-18 2020-02-21 西北工业大学 一种空间完全非合作目标相对位姿和惯量估计方法
CN111339629A (zh) * 2019-11-22 2020-06-26 北京理工大学 一种用于天基观测的空间目标机动轨道确定方法
CN112066879A (zh) * 2020-09-11 2020-12-11 哈尔滨工业大学 基于计算机视觉的气浮运动模拟器位姿测量装置及方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
INAM ULLAH,ET AL: "A Localization Based on Unscented Kalman Filter and Particle Filter Localization Algorithms", 《IEEE ACCESS》 *
郝刚涛等: "《空间翻滚非合作目标相对位姿估计的视觉 SLAM 方法》", 《宇航学报》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114419259A (zh) * 2022-03-30 2022-04-29 中国科学院国家空间科学中心 一种基于物理模型成像仿真的视觉定位方法及系统
CN114549592A (zh) * 2022-04-24 2022-05-27 之江实验室 一种非合作抛体的轨迹预测与捕获方法和装置
CN114549592B (zh) * 2022-04-24 2022-08-05 之江实验室 一种非合作抛体的轨迹预测与捕获方法和装置
CN114964266A (zh) * 2022-07-26 2022-08-30 中国人民解放军国防科技大学 基于多视觉矢量的运动状态协同群组相对姿态确定方法
CN116681733A (zh) * 2023-08-03 2023-09-01 南京航空航天大学 一种空间非合作目标近距离实时位姿跟踪方法
CN116681733B (zh) * 2023-08-03 2023-11-07 南京航空航天大学 一种空间非合作目标近距离实时位姿跟踪方法
CN117647243B (zh) * 2024-01-30 2024-04-16 山东星辰卫星技术有限公司 一种基于6u立方星的凝视监测方法及系统

Also Published As

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

Similar Documents

Publication Publication Date Title
CN113175929B (zh) 一种基于upf的空间非合作目标相对位姿估计方法
CN112985416B (zh) 激光与视觉信息融合的鲁棒定位和建图方法及系统
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN105856230B (zh) 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
CN111076733B (zh) 一种基于视觉与激光slam的机器人室内建图方法及系统
CN105976353B (zh) 基于模型和点云全局匹配的空间非合作目标位姿估计方法
Huang et al. Towards acoustic structure from motion for imaging sonar
Pizarro et al. Large area 3-D reconstructions from underwater optical surveys
Aghili et al. Fault-tolerant position/attitude estimation of free-floating space objects using a laser range sensor
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
CN111665512B (zh) 基于3d激光雷达和惯性测量单元的融合的测距和绘图
CN110187337B (zh) 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统
Xu et al. A pose measurement method of a non-cooperative GEO spacecraft based on stereo vision
CN113763548B (zh) 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
CN112669354A (zh) 一种基于车辆非完整约束的多相机运动状态估计方法
Guo et al. Real-time measurement and estimation of the 3D geometry and motion parameters for spatially unknown moving targets
CN114234967A (zh) 一种基于多传感器融合的六足机器人定位方法
CN115453599A (zh) 一种多传感器协同的管道机器人精准定位方法
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
CN112767481B (zh) 一种基于视觉边缘特征的高精度定位及建图方法
Liu et al. Real-time model-based monocular pose tracking for an asteroid by contour fitting
CN116577801A (zh) 一种基于激光雷达和imu的定位与建图方法及系统
CN111145267A (zh) 基于imu辅助的360度全景视图多相机标定方法
Zhu et al. A hybrid relative navigation algorithm for a large–scale free tumbling non–cooperative target
CN113124881B (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