CN113175929B - 一种基于upf的空间非合作目标相对位姿估计方法 - Google Patents
一种基于upf的空间非合作目标相对位姿估计方法 Download PDFInfo
- Publication number
- CN113175929B CN113175929B CN202110269577.2A CN202110269577A CN113175929B CN 113175929 B CN113175929 B CN 113175929B CN 202110269577 A CN202110269577 A CN 202110269577A CN 113175929 B CN113175929 B CN 113175929B
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 92
- 238000005259 measurement Methods 0.000 claims abstract description 48
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 238000005070 sampling Methods 0.000 claims description 135
- 239000002245 particle Substances 0.000 claims description 81
- 239000011159 matrix material Substances 0.000 claims description 59
- 239000013598 vector Substances 0.000 claims description 34
- 230000007704 transition Effects 0.000 claims description 24
- 230000008569 process Effects 0.000 claims description 23
- 238000012952 Resampling Methods 0.000 claims description 13
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 9
- 238000001514 detection method Methods 0.000 claims description 8
- 230000003287 optical effect Effects 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000012216 screening Methods 0.000 claims description 4
- 238000012850 discrimination method Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 238000012935 Averaging Methods 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 230000033001 locomotion Effects 0.000 abstract description 8
- 230000000007 visual effect Effects 0.000 abstract description 5
- 230000007123 defense Effects 0.000 abstract 1
- 238000004088 simulation Methods 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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的空间非合作目标相对位姿估计方法。
背景技术
空间技术发展对完成复杂任务的需求日益增加,如抓捕或转移空间碎片和废弃卫星、维修或更换有故障的在轨航天器、通过加注燃料延长寿命的卫星等,这些任务要求追踪/任务航天器近距离精确估计目标的相对位置和姿态。目标在非合作状态下,运动情况和结构完全未知或部分未知,并且没有星间链路等辅助进行位姿测量,因此非合作目标相对位姿测量具有挑战性,难度大。根据非合作目标的几何模型是否已知,非合作目标又可被分为模型已知非合作目标和模型未知非合作目标。依赖于非合作目标的明显的几何特征或已知的三维模型,模型已知非合作目标的相对位姿估计问题已经得到了很好的解决。相反,模型未知非合作目标先验信息缺失、运动状态不确定,其相对位姿估计问题更为复杂。
近年来,国内外学者开始考虑引入移动机器人视觉即时定位和地图构建SLAM方法来解决模型未知非合作目标的相对位姿估计问题,斯坦福大学的Sean Augenstein等人从FastSLAM算法受到启发,提出了一种基于FastSLAM算法的非合作目标位姿跟踪和三维重构方法,并通过水下目标实验验证了算法的可行性。北京装备学院的郝刚涛等人针对此方法中的相对位姿估计部分进行改进,联合无迹卡尔曼滤波UKF和粒子滤波PF实现了目标位姿参数的快速平滑估计。但是,上述两种方法均假定目标初始位姿已知,不利于实际应用。德累斯顿工业大学的Schnitzer等人针对空间失效航天器的自主维修问题,提出基于EKF-SLAM和RANSAC算法来估计目标相对位姿,恢复目标三维结构,但该方法依赖于扩展卡尔曼滤波EKF对非线性方程的线性化处理,当系统非线性程度较高时,过大的线性化误差会导致滤波性能下降。
发明内容
本发明所要解决的技术问题是克服现有技术的不足而提供一种基于UPF的空间非合作目标相对位姿估计方法,将视觉即时定位和地图构建SLAM领域中用于相对位姿估计的贝叶斯滤波和测量反演方法引入至空间视觉导航领域,在无目标先验信息和运动状态信息情况下,能够实现非合作目标的相对位姿估计,为后续的空间近场操作提供必要的位姿信息。
本发明为解决上述技术问题采用以下技术方案:
根据本发明提出的一种基于UPF的空间非合作目标相对位姿估计方法,包括以下步骤:
步骤一、读取单目相机实时采集的图像,从开始帧中筛选连续两帧图像作为初始两帧图像,检测初始两帧图像上的SURF特征点,并进行SURF特征点匹配,删除离群点,得到特征匹配点对,并依据对极几何关系计算本质矩阵,分解本质矩阵后得到初始两帧图像间的相对位姿;
任取一SURF特征点作为目标本体坐标系的原点,建立目标本体坐标系B,目标本体坐标系固连于目标上,目标本体坐标系的指向和相机坐标系平行,记目标本体坐标系的原点在相机坐标系中的坐标为获得相机和目标间的相对姿态初始值α0和相对位置初始值ρ0以及第j个SURF特征点在目标本体坐标系下的三维坐标初始值
步骤二、以k表示图像采样时刻索引,跟踪匹配第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标zj,k=[xj,k yj,k]T,其中,xj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的水平坐标,yj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的垂直坐标,上标T为转置;
步骤三、以目标和相机间的相对位姿向量作为状态变量,构建目标状态转移方程,以zj,k作为实际测量值,构建测量方程,利用贝叶斯滤波预测目标和相机间的相对姿态;利用测量反演法预测目标和相机间的相对位置,根据预测得到的相对姿态和相对位置,优化目标状态转移方程;
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案,在步骤一中,选取SURF特征点数目大于预设的特征点数目阈值的两张连续两帧图像作为初始两帧图像,利用快速最近邻逼近搜索FLANN方法进行SURF特征点匹配,用比值判别法删除离群点;
当特征匹配点对数目大于预设的特征点对数目阈值时,由特征匹配点对计算本质矩阵,分解本质矩阵后得到初始两帧图像间的相对位姿;利用三角测量法计算第j个SURF特征点在相机坐标系C下的初始位置任取一SURF特征点作为目标本体坐标系的原点,建立目标本体坐标系B,目标本体坐标系固连于目标上,目标本体坐标系的指向和相机坐标系平行,记目标本体坐标系的原点在相机坐标系中的坐标为则目标和相机间的相对姿态初始值α0=[0 0 0]T,以欧拉角描述目标和相机间的相对姿态,规定欧拉旋转顺序为X-Y-Z,即相机坐标系依次绕其X轴、旋转后的Y轴、再旋转后的Z轴旋转后,与目标本体坐标系重合,目标和相机间的相对位置初始值令表示第j个SURF特征点在目标本体坐标系下的三维坐标,因此有
作为本发明所述的一种基于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表示采样时刻间隔,则根据刚体的动力学、运动学模型,目标的状态转移方程表示为:
其中,α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帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度的转换关系,其中上标·表示对时间的导数;假定目标以X-Y-Z的欧拉旋转顺序分别旋转φ,θ,ψ,已知目标在旋转前的目标本体坐标系下的旋转角速度为 分别为ωB在旋转前的目标本体坐标系下的X、Y、Z三轴角速度分量,其中RB/C为相机坐标系到目标本体坐标系的变换矩阵,则存在
则有
其中,M(α)为矩阵,该矩阵用于描述第k帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度的转换关系;公式(1)给出的速度分量状态转移方程中的J为惯量矩阵,m为目标质量,r表示目标参考点到质心的位置偏移量,Ttotal表示作用于目标的外部总力矩,F表示作用于目标质心的总外力;在此Ttotal、F、r均不知,由Ttotal、F、r所引起的角速度和线速度变化通过引入过程噪声qk~N(0,Qk)来考虑,Qk为过程噪声qk的协方差矩阵,则(1)式改写为
其中,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特征点在目标本体坐标系下的坐标不随时间变化,即分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标上的第j个SURF特征点在目标本体坐标系的三维坐标;
假设为第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标,分别为在相机坐标系下的X、Y、Z坐标;ρk为第k帧图像采样时刻目标在相机坐标系下的位置,表示第k帧图像采样时刻目标本体坐标系到相机坐标系的转换矩阵;则和存在以下转换关系:
定义, 表示目标上的第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)构建测量方程:
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案,在步骤三中,利用测量反演法预测目标和相机间的相对位置,根据预测得到的相对姿态和相对位置,优化目标状态转移方程的具体流程如下:
其中,上标∧表示估计值,上标∧和下标“k/k-1”构成预测值;
然后根据第k帧图像采样时刻相对姿态预测值目标上的第j个SURF特征点在目标本体坐标系下的三维坐标和第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值zj,k,由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的过程以函数h(·)表示,即
由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的具体方法为:由式(7)得到的目标上的第j个SURF特征点在目标本体坐标系下的三维坐标和第k帧图像采样时刻目标和相机间的相对位置预测值 为未知量,根据式(4)和式(5)计算第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值
其中,表示第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标预测值,分别为的X、Y、Z轴坐标,表示第k帧图像采样时刻目标本体坐标系到相机坐标系的变换矩阵预测值, 分别为的X、Y轴坐标,根据步骤二得到的zj,k=[xj,k yj,k]T以及由式(9)获得的第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值构建第k帧图像采样时刻第j个SURF特征点的投影误差代价函数ej,k
令ej,k=0,将(9)式代入式(10)并化简,得
依据上述对第k帧图像采样时刻目标和相机间的相对位置ρk的预测,重新选取第k帧图像采样时刻目标的状态向量sk=[αkωkρk],优化目标状态转移方程为
其中sk-1表示第k-1帧图像采样时刻目标的状态向量。
作为本发明所述的一种基于UPF的空间非合作目标相对位姿估计方法进一步优化方案,步骤三中,采用无迹粒子滤波UPF算法估计相机和目标间的相对位姿的具体过程如下:
在初始采样时刻k=0,由步骤一中初始化得到的α0和ρ0作为状态向量的初始值为初始状态误差协方差矩阵,以一个高斯分布随机产生N个 为第i个粒子所对应的相对位姿状态向量的初始值,i=1,2,…,N,N为粒子的总数;初始化粒子权值对N个粒子加权求和,即求粒子的平均值以作为每个粒子的均值计算每个粒子协方差矩阵其中为第i个粒子所对应的相对位姿状态向量的初始值,为第i个粒子的均值;
对后续时刻的具体处理步骤如下:
步骤S1:利用无迹卡尔曼滤波UKF生成粒子滤波PF的重要性概率密度函数,更新第k帧图像采样时刻第i个粒子的均值和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵并根据和生成新的粒子集合步骤S1的具体方法为:
假设第k-1帧图像采样时刻第i个粒子均值为第k-1帧图像采样时刻的第i个粒子状态误差协方差矩阵为粒子权值为状态向量维数为n,总粒子数为N,对每个粒子,以为基准,对称分布采样2n+1个样本点其中u=0,...,2n表示样本点索引:
令分别表示第k帧图像采样时刻第i个粒子的第u个样本点预测值中的相对姿态和相对位置分量,根据式(6)测量方程计算第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标预测测量值所构成的矩阵 表示第k帧图像采样第i个粒子的第u个样本点所描述的相对位姿状态下,目标上的第j个SURF特征点图像物理坐标预测测量值;得到:
其中似然函数服从均值为协方差矩阵为Nk的高斯分布,即 表示第k帧图像采样时刻第i个粒子所描述的相对位姿状态下目标上所有M个SURF特征点图像物理坐标的预测测量值,状态转移概率服从均值为协方差矩阵为Qk的高斯分布,即 表示第k帧图像采样时刻第i个粒子的预测值;归一化第k帧图像采样时刻第i个粒子权值
步骤S2:采用系统重采样算法进行粒子重采样:重采样前第k帧图像采样时刻第i个粒子重采样后更新第k帧图像采样时刻第i个粒子的均值和协方差矩阵为和即重采样后更新第k帧图像采样时刻第i个粒子平均第k帧图像采样时刻第i个粒子权值
作为本发明所述的一种基于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接近目标,其中
初始时刻目标在相机坐标系下的位置为ρ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滤波迭代输出。由于单目初始化所存在的尺度不确定性,所得到的目标相对位置ρ和三维点坐标与真实值存在着一个尺度因子,且均为无量纲坐标,不便于计算真实值与估计值间误差。为验证本发明的有效性,仿真实验中假定目标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 (4)
1.一种基于UPF的空间非合作目标相对位姿估计方法,其特征在于,包括以下步骤:
步骤一、读取单目相机实时采集的图像,从开始帧中筛选连续两帧图像作为初始两帧图像,检测初始两帧图像上的SURF特征点,并进行SURF特征点匹配,删除离群点,得到特征匹配点对,并依据对极几何关系计算本质矩阵,分解本质矩阵后得到初始两帧图像间的相对位姿;
任取一SURF特征点作为目标本体坐标系的原点,建立目标本体坐标系B,目标本体坐标系固连于目标上,目标本体坐标系的指向和相机坐标系平行,记目标本体坐标系的原点在相机坐标系中的坐标为获得相机和目标间的相对姿态初始值α0和相对位置初始值ρ0以及第j个SURF特征点在目标本体坐标系下的三维坐标初始值
步骤二、以k表示图像采样时刻索引,跟踪匹配第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标zj,k=[xj,k yj,k]T,其中,xj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的水平坐标,yj,k是第k帧图像采样时刻目标上的第j个SURF特征点在图像物理坐标系下的垂直坐标,上标T为转置;
步骤三、以目标和相机间的相对位姿向量作为状态变量,构建目标状态转移方程,以zj,k作为实际测量值,构建测量方程,利用贝叶斯滤波预测目标和相机间的相对姿态;利用测量反演法预测目标和相机间的相对位置,根据预测得到的相对姿态和相对位置,优化目标状态转移方程;
步骤三中,构建目标状态转移方程和测量方程的具体过程如下:
定义目标的状态向量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表示采样时刻间隔,则根据刚体的动力学、运动学模型,目标的状态转移方程表示为:
其中,α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帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度的转换关系,其中上标·表示对时间的导数;假定目标以X-Y-Z的欧拉旋转顺序分别旋转φ,θ,ψ,已知目标在旋转前的目标本体坐标系下的旋转角速度为 分别为ωB在旋转前的目标本体坐标系下的X、Y、Z三轴角速度分量,其中RB/C为相机坐标系到目标本体坐标系的变换矩阵,则存在
则有
其中,M(α)为矩阵,该矩阵用于描述第k帧图像采样时刻下、目标在相机坐标系下的旋转角速度矢量ω到欧拉角速度的转换关系;公式(1)给出的速度分量状态转移方程中的J为惯量矩阵,m为目标质量,r表示目标参考点到质心的位置偏移量,Ttotal表示作用于目标的外部总力矩,F表示作用于目标质心的总外力;在此Ttotal、F、r均不知,由Ttotal、F、r所引起的角速度和线速度变化通过引入过程噪声qk~N(0,Qk)来考虑,Qk为过程噪声qk的协方差矩阵,则(1)式改写为
sk=f(sk-1)+qk
其中,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特征点在目标本体坐标系下的坐标不随时间变化,即 分别表示第k帧图像采样时刻和第k-1帧图像采样时刻目标上的第j个SURF特征点在目标本体坐标系的三维坐标;
假设为第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标,分别为在相机坐标系下的X、Y、Z坐标;ρk为第k帧图像采样时刻目标在相机坐标系下的位置,表示第k帧图像采样时刻目标本体坐标系到相机坐标系的转换矩阵;则和存在以下转换关系:
定义, 表示目标上的第j个SURF特征点在目标本体坐标系下的三维坐标,XB表示目标上所有M个SURF特征点在目标本体坐标系下的三维坐标构成的矩阵,zk=[z1,k z2,k…zM,k]T,zj,k为第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值,zk表示第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标实际测量值所构成的矩阵;测量噪声nk~N(0,Nk),Nk为测量噪声nk的协方差矩阵,函数g(·)为测量函数、表示所有M个SURF特征点透视投影函数gj(·)的组合,结合式(4)和式(5)构建测量方程:
在步骤三中,利用测量反演法预测目标和相机间的相对位置,根据预测得到的相对姿态和相对位置,优化目标状态转移方程的具体流程如下:
其中,上标∧表示估计值,上标∧和下标“k/k-1”构成预测值;
然后根据第k帧图像采样时刻相对姿态预测值目标上的第j个SURF特征点在目标本体坐标系下的三维坐标和第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标的实际测量值zj,k,由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的过程以函数h(·)表示,即
由测量反演法计算第k帧图像采样时刻目标和相机间的相对位置预测值的具体方法为:由式(7)得到的目标上的第j个SURF特征点在目标本体坐标系下的三维坐标和第k帧图像采样时刻目标和相机间的相对位置预测值 为未知量,根据式(4)和式(5)计算第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值
其中,表示第k帧图像采样时刻目标上的第j个SURF特征点在相机坐标系下的坐标预测值,分别为的X、Y、Z轴坐标,表示第k帧图像采样时刻目标本体坐标系到相机坐标系的变换矩阵预测值, 分别为的X、Y轴坐标,根据步骤二得到的zj,k=[xj,k yj,k]T以及由式(9)获得的第k帧图像采样时刻目标上的第j个SURF特征点的图像物理坐标预测值构建第k帧图像采样时刻第j个SURF特征点的投影误差代价函数ej,k
令ej,k=0,将(9)式代入式(10)并化简,得
依据上述对第k帧图像采样时刻目标和相机间的相对位置ρk的预测,重新选取第k帧图像采样时刻目标的状态向量sk=[αk ωk ρk],优化目标状态转移方程为
sk=f(sk-1)+qk
其中sk-1表示第k-1帧图像采样时刻目标的状态向量;
步骤三中,采用无迹粒子滤波UPF算法估计相机和目标间的相对位姿的具体过程如下:
在初始采样时刻k=0,由步骤一中初始化得到的α0和ρ0作为状态向量的初始值为初始状态误差协方差矩阵,以一个高斯分布随机产生N个 为第i个粒子所对应的相对位姿状态向量的初始值,i=1,2,…,N,N为粒子的总数;初始化粒子权值对N个粒子加权求和,即求粒子的平均值以作为每个粒子的均值计算每个粒子协方差矩阵其中为第i个粒子所对应的相对位姿状态向量的初始值,为第i个粒子的均值;
对后续时刻的具体处理步骤如下:
步骤S1:利用无迹卡尔曼滤波UKF生成粒子滤波PF的重要性概率密度函数,更新第k帧图像采样时刻第i个粒子的均值和第k帧图像采样时刻第i个粒子的状态误差协方差矩阵并根据和生成新的粒子集合步骤S1的具体方法为:
假设第k-1帧图像采样时刻第i个粒子均值为第k-1帧图像采样时刻的第i个粒子状态误差协方差矩阵为粒子权值为状态向量维数为n,总粒子数为N,对每个粒子,以为基准,对称分布采样2n+1个样本点其中u=0,...,2n表示样本点索引:
令分别表示第k帧图像采样时刻第i个粒子的第u个样本点预测值中的相对姿态和相对位置分量,根据式(6)测量方程计算第k帧图像采样时刻目标上所有M个SURF特征点图像物理坐标预测测量值所构成的矩阵 表示第k帧图像采样第i个粒子的第u个样本点所描述的相对位姿状态下,目标上的第j个SURF特征点图像物理坐标预测测量值;得到:
其中似然函数服从均值为协方差矩阵为Nk的高斯分布,即 表示第k帧图像采样时刻第i个粒子所描述的相对位姿状态下目标上所有M个SURF特征点图像物理坐标的预测测量值,状态转移概率服从均值为协方差矩阵为Qk的高斯分布,即 表示第k帧图像采样时刻第i个粒子的预测值;归一化第k帧图像采样时刻第i个粒子权值
步骤S2:采用系统重采样算法进行粒子重采样:重采样前第k帧图像采样时刻第i个粒子重采样后更新第k帧图像采样时刻第i个粒子的均值和协方差矩阵为和即重采样后更新第k帧图像采样时刻第i个粒子平均第k帧图像采样时刻第i个粒子权值
2.根据权利要求1所述的一种基于UPF的空间非合作目标相对位姿估计方法,其特征在于,在步骤一中,选取SURF特征点数目大于预设的特征点数目阈值的两张连续两帧图像作为初始两帧图像,利用快速最近邻逼近搜索FLANN方法进行SURF特征点匹配,用比值判别法删除离群点;
当特征匹配点对数目大于预设的特征点对数目阈值时,由特征匹配点对计算本质矩阵,分解本质矩阵后得到初始两帧图像间的相对位姿;利用三角测量法计算第j个SURF特征点在相机坐标系C下的初始位置任取一SURF特征点作为目标本体坐标系的原点,建立目标本体坐标系B,目标本体坐标系固连于目标上,目标本体坐标系的指向和相机坐标系平行,记目标本体坐标系的原点在相机坐标系中的坐标为则目标和相机间的相对姿态初始值α0=[0 0 0]T,以欧拉角描述目标和相机间的相对姿态,规定欧拉旋转顺序为X-Y-Z,即相机坐标系依次绕其X轴、旋转后的Y轴、再旋转后的Z轴旋转后,与目标本体坐标系重合,目标和相机间的相对位置初始值令表示第j个SURF特征点在目标本体坐标系下的三维坐标,因此有
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.根据权利要求2所述的一种基于UPF的空间非合作目标相对位姿估计方法,其特征在于,在初始两帧图像中的第二帧图像中找出与第一帧图像中每个SURF特征点对应的两个待匹配点,将这两个待匹配点按照分别距第一帧图像中对应的SURF特征点的欧式距离的远近分为最近邻点和次近邻点,当最近邻点距第一帧图像中对应的SURF特征点的欧式距离与次近邻点距第一帧图像中对应的SURF特征点的欧式距离的比值小于预设阈值时,则认为最近邻点匹配为正确匹配,得到特征匹配点对,并删除次近邻点,即删除离群点。
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 CN113175929A (zh) | 2021-07-27 |
CN113175929B true 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) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114419259B (zh) * | 2022-03-30 | 2022-07-12 | 中国科学院国家空间科学中心 | 一种基于物理模型成像仿真的视觉定位方法及系统 |
CN114549592B (zh) * | 2022-04-24 | 2022-08-05 | 之江实验室 | 一种非合作抛体的轨迹预测与捕获方法和装置 |
CN114964266B (zh) * | 2022-07-26 | 2022-10-21 | 中国人民解放军国防科技大学 | 基于多视觉矢量的运动状态协同群组相对姿态确定方法 |
CN116681733B (zh) * | 2023-08-03 | 2023-11-07 | 南京航空航天大学 | 一种空间非合作目标近距离实时位姿跟踪方法 |
CN117647243B (zh) * | 2024-01-30 | 2024-04-16 | 山东星辰卫星技术有限公司 | 一种基于6u立方星的凝视监测方法及系统 |
Citations (4)
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 | 哈尔滨工业大学 | 基于计算机视觉的气浮运动模拟器位姿测量装置及方法 |
-
2021
- 2021-03-12 CN CN202110269577.2A patent/CN113175929B/zh active Active
Patent Citations (4)
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)
Title |
---|
《空间翻滚非合作目标相对位姿估计的视觉 SLAM 方法》;郝刚涛等;《宇航学报》;20150630;第32卷(第6期);第70-714页 * |
A Localization Based on Unscented Kalman Filter and Particle Filter Localization Algorithms;INAM ULLAH,et al;《IEEE Access》;20200630;第2233-2246页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113175929A (zh) | 2021-07-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113175929B (zh) | 一种基于upf的空间非合作目标相对位姿估计方法 | |
CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及系统 | |
CN105856230B (zh) | 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法 | |
CN111076733B (zh) | 一种基于视觉与激光slam的机器人室内建图方法及系统 | |
Huang et al. | Towards acoustic structure from motion for imaging sonar | |
CN105976353B (zh) | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 | |
CN114234967B (zh) | 一种基于多传感器融合的六足机器人定位方法 | |
CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
CN110187337B (zh) | 一种基于ls和neu-ecef时空配准的高机动目标跟踪方法及系统 | |
CN108151713A (zh) | 一种单目vo快速位姿估计方法 | |
Xu et al. | A pose measurement method of a non-cooperative GEO spacecraft based on stereo vision | |
CN115453599A (zh) | 一种多传感器协同的管道机器人精准定位方法 | |
CN109724586A (zh) | 一种融合深度图和点云的航天器相对位姿测量方法 | |
CN115218889A (zh) | 一种基于点线特征融合的多传感器室内定位方法 | |
CN118111430A (zh) | 基于最小误差熵卡尔曼的交互多模型auv组合导航方法 | |
CN103791901A (zh) | 一种星敏感器数据处理系统 | |
CN116577801A (zh) | 一种基于激光雷达和imu的定位与建图方法及系统 | |
CN115344033B (zh) | 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法 | |
CN113124881B (zh) | 一种基于磁信标的同步定位与构图系统的故障恢复方法 | |
Li et al. | Multicam-SLAM: Non-overlapping Multi-camera SLAM for Indirect Visual Localization and Navigation | |
Wang et al. | 4d radar/imu/gnss integrated positioning and mapping for large-scale environments | |
CN111366162B (zh) | 基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法 | |
Comellini et al. | Vision-based navigation for autonomous space rendezvous with non-cooperative targets | |
Mirisola et al. | Trajectory recovery and 3d mapping from rotation-compensated imagery for an airship | |
Yilmaz et al. | AV-SLAM: Autonomous vehicle SLAM with gravity direction initialization |
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 |