买专利,只认龙图腾
首页 专利交易 科技果 科技人才 科技服务 商标交易 会员权益 IP管家助手 需求市场 关于龙图腾
 /  免费注册
到顶部 到底部
清空 搜索

【发明授权】一种基于双目视觉的非合作目标相对状态解算方法_上海交通大学_201810041155.8 

申请/专利权人:上海交通大学

申请日:2018-01-16

公开(公告)日:2021-09-21

公开(公告)号:CN108376411B

主分类号:G06T7/73(20170101)

分类号:G06T7/73(20170101)

优先权:

专利状态码:有效-授权

法律状态:2021.09.21#授权;2019.01.18#实质审查的生效;2018.08.07#公开

摘要:本发明提供了一种基于双目视觉的非合作目标相对状态解算方法,包括如下步骤:步骤S1:解算载体表面特征点在相机坐标系下的3D坐标;步骤S2:解算目标坐标系相对相机坐标系的相对姿态;步骤S3:解算目标坐标系相对相机坐标系的相对转速。本发明可以计算相对位置、相对姿态和相对转速;可以不需要跟踪一组特征点,只要保证相邻时刻匹配成功特征点数不少于4即可实现相对状态估计;相对状态估计精度高。

主权项:1.一种基于双目视觉的非合作目标相对状态解算方法,其特征在于,包括如下步骤:步骤S1:解算载体表面特征点在相机坐标系下的3D坐标;步骤S2:解算目标坐标系相对相机坐标系的相对姿态;步骤S3:解算目标坐标系相对相机坐标系的相对转速;步骤S1,包括如下步骤:步骤S1.1:在航天器的质心处安装双目相机,航天器记为L,以左相机的相机坐标系作为参考坐标系,记为非合作目标记为F,由初始时刻获取的特征点建立的坐标系建立目标坐标系,记为利用双目相机采集图像,记录图像采集的时刻ti或记录相邻两个图像采集的间隔Δti,其中i表示采集图像的索引;Δti=ti+1-ti1;步骤S1.2:利用SIFT或SURF算法匹配双目相机的左、右相机图像的载体表面特征点,记iN表示在ti时刻匹配成功的特征点个数;步骤S1.3:利用双目视觉原理计算载体表面特征点在双目相机的左相机坐标系下的3D坐标;利用双目相机计算特征点的3D坐标时涉及到计算机图像坐标系,成像平面坐标系,相机坐标系和目标坐标系;设,OF-xyz表示目标坐标系,即lO0-uv和rO0-uv分别表示左右相机的计算机图像坐标系;和分别表示左右相机的成像平面坐标系,Ol-xyz和Or-xyz分别表示左右相机坐标系,u0,v0为lO0-uv和rO0-uv的中心,也是和的原点,Ol-xyz即为空间中一点P在中的坐标为x,y,z,该点在lO0-uv和rO0-uv中的坐标记为ul,vl和ur,vr;特征点P在lO0-uv和rO0-uv中的坐标与x,y,z的关系用1a~1b表示,联立式1a~1b可解出x,y,z,如式1c所示: 其中,f表示相机的焦距;dx,dy分别表示每个像素点在计算机成像平面坐标系的x轴和y轴方向上的物理尺寸;b表示左右相机之间的基线长度;Tb=[-b,0,0]′;记表示在ti时刻匹配成功的第k个特征点在下的3D坐标,k=1,2,…,iN;步骤S2,包括以下步骤:步骤S2.1:建立目标坐标系利用载体表面特征点在相机坐标系的坐标建立目标坐标系在目标坐标系下的坐标为二者存在以下关系: 其中,T3×1=[T1T2T3]′;R表示旋转矩阵,T表示平移向量;r1、r2、r3分别表示旋转矩阵的列向量rij表示旋转矩阵的元素;T1、T2、T3分别表示平移向量的元素;符号[]′表示矩阵或向量的转置,x′k,y′k,z′k表示在目标坐标系下的坐标值;0表示在t0时刻;目标坐标系建立方法如下:1假设在t0时刻,三个特征点在中的坐标值分别为0P1L、相应地在中的坐标值分别记为0P1F、2因相机坐标系遵守右手法则,为使目标坐标系与其一致,0P1L为坐标系原点,选择向量的方向为坐标系的x轴正向,此时,应保证x2>x1,改变特征点的顺序,易满足条件;3坐标系的y轴在由点确定的平面内,且与x轴垂直;4z轴的方向与0P1L、确定的平面垂直,遵守右手法则;根据坐标系建立的原则可知,特征点在的坐标为:0P1F=0,0,0、x′2为特征点0P1L、之间的距离,式3c~3d确定及y轴正向: 其中,α表示向量与的夹角;将式3a~3d代入式2求解R和T: r3=r1×r23gT=0P1L3h;步骤S2.2:建立七参数模型坐标系与之间的变换关系利用七参数模型表示,即: μi,Ri,Ti分别表示在ti时刻变换到的尺度因子、旋转矩阵和平移向量;为解算出旋转矩阵,应保证在ti时刻iN≥4;欧拉角与旋转矩阵存在变换关系,即用欧拉角表示旋转矩阵,在ti时刻,经过一定顺序的三次欧拉角θi,ψi旋转可以与平行,相应的旋转矩阵采用下式表示,为便于简洁表达,在此将欧拉角的下标省略掉: 步骤S2.3:求解载体表面特征点在下的坐标在t0时刻,已建立目标坐标系利用ti-1时刻的变换关系计算ti时刻载体表面特征点在目标坐标系中的坐标,进而得到ti时刻同一组载体表面特征点分别在和中的坐标;相邻时刻,匹配成功的特征点iN≥4即可解算旋转矩阵Ri、μi和Ti; 步骤S2.4:求解目标坐标系与相机坐标系的相对姿态从步骤S2.3,得到在ti时刻iN个匹配成功的特征点在和下的坐标,分别记为和利用两个坐标系下的特征点计算出七参数其中,Δx,Δy,Δz分别表示和之间的平移量,也即Ti=[Δx,Δy,Δz]′;在处做泰勒展开如下: 其中,Y表示在ti时刻特征点在中坐标组成的列向量,大小为3iN×1;A表示系数矩阵,矩阵维数为3iN×7;l表示在ti时刻与dx无关的常数项,大小为3iN×1; A=[E3×3,μM,N]8b其中,E3×3表示单位矩阵,M表示对旋转矩阵求导结果相关的系数矩阵,N表示与dμ相关的系数向量; 当iN>3时,利用最小二乘方法求解七参数向量,迭代过程如下:步骤a,设置初始参数步骤b,将代入上式8a~8d,计算A,l;步骤c,根据误差方程求解其中,V表示误差;步骤d,根据最小二乘原理求解改正数其中,A'表示A的转置;步骤e,更新七参数值步骤f,若则停止迭代;否则执行步骤g;步骤g,重复步骤b~步骤f;步骤S2.5:利用整体最小二乘算法进一步提高解算精度;在步骤S2.4迭代过程中,仅考虑特征点在中的误差,而实际应用中,系数矩阵也存在误差;针对这一问题,采用整体最小二乘原理进一步提高七参数的解算精度;新建立的EIV模型如式10a所示: eA=vecEAi,vec·表示将矩阵按列堆叠转化为列向量;EA表示系数矩阵的残差,表示每次迭代中系数矩阵残差的预测值;目标函数: 其中,ey表示观测量Y的残差,eA表示系数矩阵的残差重构的列向量,λ表示m×1拉格朗日乘数向量,Qy和QA分别表示观测量Y权逆阵和系数矩阵残差的权逆阵,表示直乘,Im表示m×m单位矩阵,Y表示由特征点在坐标系下的坐标构成的列向量;为求解Φey,eA,λ,dζ=min,分别对变量求导,求目标函数最值过程如下,其中“~”表示相应变量的预测值;对ey求导得: 对eA求导得: 对λ求导得: 对dζ求导得: 由式12a~12b可得: 将式13a~13b代入12c得: 其中,其中,下标i表示第i次迭代,表示单位权方差,Ql表示Q0表示系数矩阵方差,QE表示权逆阵,表示第i+1次迭代的观测量误差,表示第i+1次迭代的系数矩阵误差;步骤S2.5中,迭代计算的初始值由步骤S2.4的结果给出,根据以上迭代计算,当时迭代停止,求解出的x即为进一步迭代后的七参数;步骤S2.6:计算相对姿态根据步骤S2.5中得到的欧拉角求解出旋转矩阵,依据下式求解出四元数q=[ξ1ξ2ξ3ξ4]T: 步骤S3,包括如下步骤:步骤S3.1,建立动力学模型 步骤S3.2,建立运动学模型 ρ=[ξ1ξ2ξ3]T17步骤S3.3,相对转速计算公式ω=DqωF-ωL18其中:ωL,ωF,ω分别表示相机坐标系的转速、目标坐标系的转速及目标坐标系相对相机坐标系的转速;表示相对转速的导数;IL和IF分别表示航天器和非合作目标的转动惯量,NL和NF分别表示航天器和非合作目标受到的外力,Dq表示由四元数表示的旋转矩阵,q表示四元数,表示四元数的导数,ρ表示四元数的矢量部分;Dq=[ξ42-ρTρ]I3×3+2ρρT-2ξ4[ρ×]19[ρ×]表示叉乘;步骤3.4,无损卡尔曼滤波估计转速定义状态方程如下: 其中,表示状态量的导数,η=[ξ1,ξ2,ξ3,ξ4,ωx,ωy,ωz,IT1,IT2,IT3,IT4,IT5,IT6]Tfη表示状态量与其导数映射函数,w表示均值为零的高斯噪声;观测方程如下:Z=hη+v21其中,z表示观测量,η表示状态量,v表示均值为零的高斯白噪声,hη表示状态量与观测量的映射关系;其中,ξ1,ξ2,ξ3,ξ4,分别为相对姿态的四元数;ω=[ωx,ωy,ωz]T分别表示相对转速;IT1,IT2,IT3,IT4,IT5,IT6分别表示转动惯量的元素;其中,转动惯量为观测量为:Z=[ξ1,ξ2,ξ3,ξ4]T根据观测量和测量量的关系建立模型,采用UKF滤波解算状态,包括如下步骤:步骤A:利用UT变换计算Sigma点 其中,ζ=α2n+κ-nα表示Sigma点围绕ηk的分布特性,κ表示比例因子;步骤B:一步预测 步骤C:二次采样 步骤D:测量更新Zk+1|ki=hχk+1|ki27 步骤E:滤波更新 β结合η的先验分布信息得出;χk表示k时刻的Sigma点集;ηk表示第k次迭代的状态量; 表示变换后Sigma点集的第i列;Wm表示均值的加权值; Pk+1|k表示后验方差阵;Wc表示方差的权值;Qk表示噪声的方差矩阵;Zk+1|k表示k时刻预测k+1的测量值的估计值; 表示预测观测值;χk+1|k表示根据一步预测值再次使用UT变换,产生的新Sigma点集;Kk+1表示在k+1时刻的滤波增益矩阵; 表示k+1时刻的状态量估计值; 表示量测输出变量的方差矩阵; 是协方差阵;ζ表示一个标量,由经验得到;n表示系统状态维数;UKF滤波器的输入量为由四元数组成的测量值,根据不同时刻的输入通过不断迭代,最终可求出不同时刻,非合作目标相对相机坐标系的相对状态;非合作目标相对相机坐标系的相对状态表示为:η′=[ξ1,ξ2,ξ3,ξ4,ωx,ωy,ωz,T1,T2,T3]。

全文数据:一种基于双目视觉的非合作目标相对状态解算方法技术领域[0001]本发明涉及计算机视觉技术领域,尤其涉及一种基于双目视觉的非合作目标相对状态解算方法,该方法利用双目视觉原理解算目标坐标系相对相机坐标系状态。背景技术[0002]双目视觉作为两坐标系之间相对状态的测量方案,一直都是科学研究的热点,尤其在室内定位、非合作目标相对姿态估计等领域,相关研究的相对状态包括,两坐标系之间的相对姿态、相对转速和相对位置。然而相对转速解算的研究相对较少,且解算精度较低,导致该结果的原因包括:[0003]1特征点位置精度较低;[0004]2由于复杂环境下,前后时刻特征点匹配难度较大,即跟踪同一组测量点难度较大;[0005]3载体运动信息未知。[0006]经过检索发现:[0007]文章号为1674-1579201001-0024-07刘智勇等在《空间控制技术与应用》在2010年第1期36卷24〜29页上发表的《转动惯量未知的非合作目标角速度估计方法研究》,提出了一种针对转动惯量未知的非合作目标的姿态角速度估计问题,将解算得到的非合作目标的惯性姿态作为测量信息,估计姿态角速度和姿态动力学参数(即转动惯量比)。首先,应用非线性控制系统的几何理论对待估计的状态扩展系统进行能观性分析。然后,利用UKF设计相应的滤波估计方法。仿真结果表明,该文章所设计的方法能够估计出非合作目标的姿态角速度与转动惯量比。但是,该文章所设计的方法,仍然存在如下问题:[0008]该文中提出的观测量未给出具体的计算方法,而基于双目视觉实现特征匹配,其坐标计算、四算数估计时误差较大,该文章所提出的方法仍无法解决解算精度较低的问题。[0009]目前没有发现同本发明类似技术的说明或报道,也尚未收集到国内外类似的资料。发明内容[0010]针对现有技术中存在的上述不足,本发明的目的是提供一种基于双目视觉的非合作目标相对状态解算方法,该方法能够提高相机坐标系与目标坐标系相对姿态解算精度以及提高相机坐标系与目标坐标系相对转速解算精度。[0011]为实现上述目的,本发明是通过以下技术方案实现的。[0012]—种基于双目视觉的非合作目标相对状态解算方法,包括如下步骤:[0013]步骤SI:解算载体表面特征点在相机坐标系下的3D坐标;[0014]步骤S2:解算目标坐标系相对相机坐标系的相对姿态;[0015]步骤S3:解算目标坐标系相对相机坐标系的相对转速。[0016]优选地,步骤Sl,包括如下步骤:[0017]步骤SI.I:如图1所示,基于双目视觉原理解算非合作目标相对航天器的相对状态的原理是在航天器的质心处安装双目相机,航天器记为L,以左相机的相机坐标系作为参考坐标系,记为·非合作目标记为F,由初始时刻获取的特征点建立的坐标系建立目标坐标系,记为利用双目相机采集图像,记录图像采集的时刻^或记录相邻两个图像采集的间隔Δtl,其中i表示采集图像的索引;[0019]步骤SI.2:利用SIFT或SURF算法匹配双目相机的左、右相机图像的载体表面特征点,记1N表示在^时刻匹配成功的特征点个数;[0020]步骤Sl.3:利用双目视觉原理计算载体表面特征点在双目相机的左相机坐标系下的3D坐标。如图6所示,利用双目相机计算特征点的3D坐标时涉及到计算机图像坐标系,成像平面坐标系,相机坐标系和目标坐标系。〇F-xyz表示目标坐标系,也即A1Oq-UV和1Oq-UV分别表示左右相机的计算机图像坐标系分别表示左右相机的成像平面坐标系,Oi-Xyz和Or-Xyz分别表示左右相机坐标系,(UQ,VQ为1Oq-UV和1Oo-UV的中心,也是的原点,在本发明中,Oi-xyz即为^空间中一点P在中的坐标为x,y,z,该点在1Oq-UV和1Oq-UV中的坐标记为U1,VI和Ur,Vr。[0021]特征点P在1Oq-UV和1Oq-UV中的坐标与(x,y,z的关系用(Ia〜(Ib表示,联立式Ia〜(Ib可解出(x,y,z,如式(Ic所示。[0025]其中,f表示相机的焦距;dx,dy分别表示每个像素点在计算机成像平面坐标系的X轴和y轴方向上的物理尺寸;b表示左右相机之间的基线长度;Tb=[_b,0,0]。[0026]记i表示在ti时刻匹配成功的第k个特征点在下的3D坐标,k=l,2,…,iN0[0027]优选地,步骤S2,包括以下步骤:[0028]步骤S2.1:建立目标坐标系[0029]利用载体表面特征点在相机坐标系的坐标建立目标坐标系丨在目标坐标系下的坐标为二者存在以下关系:[0031]其中,';R表示旋转矩阵,T表示平移向量;ri,r2,r3分别表示旋转矩阵的列向量rij表示旋转矩阵的元素;TiT2T3分别表示平移向量的元素;符号[·]表示矩阵或向量的转置,表示在目标坐标系尹下的坐标值;0表示在to时刻;[0032]目标坐标系建立方法如下:1假设在to时刻,三个特征点在X中的坐标值分别为相应地在F中的坐标值分别记为如图3所示;2因相机坐标系遵守右手法则,为使目标坐标系与其一致,.为坐标系原点,选择向量的方向为坐标系51的X轴正向,此时,应保证x2X1j^变特征点的顺序,易满足条件;3坐标系的y轴在由点确定的平面内,且与X轴垂直;4z轴的方向与确定的平面垂直,遵守右手法则。[0033]根据坐标系建立的原则可知,特征点在F的坐标为,为特征点之间的距离,如3b所示,式3c〜3d确定及y轴正向:[0038]其中,α表示向量的夹角;[0039]将式3a〜3d代入式⑵求解R和T:[0044]步骤S2.2:建立七参数模型[0045]坐标系之间的变换关系利用七参数模型表示,即:[0047]PuR1,T1分别表示在t时刻F变换到i:的尺度因子、旋转矩阵和平移向量;为解算出旋转矩阵,应保证在ti时刻iNS4;[0048]欧拉角与旋转矩阵存在变换关系,即可以用欧拉角表示旋转矩阵,在U时刻,!F经过一定顺序的三次欧拉角队旋转可以与Z平行,相应的旋转矩阵采用下式表示,为便于简洁表达,在此将欧拉角的下标省略掉:[0050]步骤S2.3:求解载体表面特征点在’下的坐标[0051]在to时刻,已建立目标坐标系,利用时刻的变换关系计算^时刻载体表面特征点在目标坐标系J7中的坐标,进而得到U时刻同一组载体表面特征点分别在£和^中的坐标。只要相邻时刻,匹配成功的特征点1N多4即可解算旋转矩阵R1^dPT1;[0053]步骤S2.4:求解目标坐标系与相机坐标系的相对姿态[0054]从步骤S2.3,得到在ti时刻屮个匹配成功的特征点在!F和X下的坐标,分别记为利用两个坐标系下的特征点计算出七参数,其中,Αχ,Ay,ΔΖ分别表示F和£之间的平移量,也即Ti=[Ax,Ay,ΔΖ];[0055]在处做泰勒展开如下:[0058]其中,,Υ表示在ti时刻特征点在X中坐标组成的列向量,大小为S1NXl5A表示系数矩阵,矩阵维数为3^\7;1表示在t时刻与dx无关的常数项,大小为3^X1。[0061]其中,E3x3表示单位矩阵,M表示对旋转矩阵求导结果相关的系数矩阵,N表示与dy相关的系数向量。[0064]当1Ν3时,利用最小二乘方法求解七参数向量,迭代过程如下:[0065]步骤a,设置初始参数[0066]步骤b,将ί代入上式8a〜8d,计算A,1:[0067]步骤c,根据误差方程求解I其中,V表示误差;[0068]步骤d,根据最小二乘原理求解改正数,其中,A’表示A的转置;[0069]步骤e,更新七参数值[0070]步骤f,若(则停止迭代;否则执行步骤g[0071]步骤g,重复步骤b〜步骤f;[0072]步骤S2.5:利用整体最小二乘算法进一步提尚解算精度[0073]在步骤S2.4迭代过程中,仅考虑特征点在X中的误差,而实际应用中,系数矩阵也存在误差。针对这一问题,采用整体最小二乘原理进一步提高七参数的解算精度。新建立的EIV模型如式IOa所示。[0078]eA=VeCEAi,vec·表示将矩阵按列堆叠转化为列向量;Ea表示系数矩阵的残差,表示每次迭代中系数矩阵残差的预测值;[0079]目标函数:[0081]其中,ey表示观测量Y的残差,eA表示系数矩阵的残差重构的列向量λ分别表示mX1拉格朗日乘数向量,Qy和Qa分别表示观测量Y权逆阵和系数矩阵残差的权逆阵,#表示直乘,Im表示mXm单位矩阵,Y表示由特征点在尤坐标系下的坐标构成的列向量;[0082]为求解,分别对变量求导,求目标函数最值过程如下,其中“〜”表示相应变量的预测值。[0083]对ey求导得:[0085]对eA求导得:[0087]对λ求导得:[0089]对求导得:[0091]由式(12a〜12b可得[0094]将式13a〜(I3b代入12c得[0102]其中,下标i表示第i次迭代,_表示单位权方差,Q1表示Qo表示系数矩阵方差,Qe表示权逆阵,表示第i+Ι次迭代的观测量误差,表示第i+1次迭代的系数矩阵误差;[0103]步骤S2.5中,迭代计算的初始值由步骤S2.4的结果给出,根据以上迭代计算,当时迭代停止,求解出的X即为进一步迭代后的七参数;[0104]步骤S2.6:计算相对姿态[0105]根据步骤S2.5中得到的欧拉角求解出旋转矩阵,依据下式求解出四元数[0107]优选地,步骤S3,包括如下步骤:[0108]步骤S3.1,建立动力学模型[0110]步骤S3.2,建立运动学模型[0113]其中:[0114]步骤S3.3,相对转速计算公式[0116]oh,coF,ω分别表示相机坐标系的转速、目标坐标系的转速及目标坐标系相对相机坐标系的转速;必表不相对转速的导数;Il和If分别表不航天器和非合作目标的转动惯量,Nl和Nf分别表示航天器和非合作目标受到的外力,Dq表示由四元数表示的旋转矩阵,q表示四元数,#表示四元数的导数,P表示四元数的矢量部分;[0118][PX]表示叉乘。[0119]步骤3.4,无损卡尔曼滤波估计转速[0120]定义状态方程如下:[0122]其中,表示状态量的导数,[0123]fη表示状态量与其导数映射函数,W表示均值为零的高斯噪声;[0124]观测方程如下:[0125]Z=hn+v21[0126]其中,z表示观测量,η表示状态量,V表示均值为零的高斯白噪声,hη表示状态量与观测量的映射关系;[0127]其中,|1,|2,|3,|4,分别为相对姿态的四元数;《=[0^,0^,《2]7分别表示相对转速;Ιπ,Ιτ2,Ιτ3,Ιτ4,Ιτ5,Ιτ6分另U表不转动惯量的元素;[0128]其中,转动惯M为,观测M为:[0129]根据观测量和测量量的关系建立模型,采用UKF滤波解算状态,包括如下步骤:[0130]步骤Α:利用UT变换计算Sigma点[0132]其中^,λ=α2η+κ-ηα表示Sigma点围绕%的分布特性,κ表示比例因子;[0133]步骤Β:—步预测[0137]步骤C:二次采样[0139]步骤D:测量更新[0144]步骤E:滤波更新[0151]ί3结合Tl的先验分布信息得出,最优值可设置为2;[0Ί52]Xk表示k时刻的Sigma点集;[0153]%表示第k次迭代的状态量;[0154]表示变换后Sigma点集的第i列;[0155]Wm表示均值的加权值;[0156]表示一步预测状态;[0157]Pk+lIk表不后验方差阵;[0158]Wc表示方差的权值;[0159]Qk表示噪声的方差矩阵;[0160]表示k时刻预测k+Ι的测量值的估计值;[0161]表示预测观测值;[0162]xk+1Ik表示根据一步预测值再次使用UT变换,产生的新Sigma点集;[0163]Kk+1表示在k+Ι时刻的滤波增益矩阵;[0164]表示k+Ι时刻的状态量估计值;[0165].表不量测输出变量的方差矩阵;[0166]是协方差阵;[0167]ζ表示一个标量,由经验得到;[0168]η表示系统状态维数;[0169]UKF滤波器的输入量为由四元数组成的测量值,根据不同时刻的输入通过不断迭代,最终可求出不同时刻,非合作目标相对相机坐标系的相对状态。[0170]非合作相对相机坐标系的相对状态可表示为[0172]与现有技术相比,本发明具有如下有益效果:[0173]1本发明利可以计算相对位置、相对姿态和相对转速;[0174]2本发明可以不需要跟踪一组特征点,只要保证相邻时刻匹配成功特征点数不少于3即可实现相对状态估计;[0175]3本发明相对状态估计精度高;[0176]4本发明不仅估计两坐标系的相对转速,还估计两坐标系的相对位置和相对姿ίέτO[0177]5本发明提出了一种基于双目视觉的非合作目标相对状态解算方法,该方法不需要持续跟踪同一组特征点,只需在前后时刻匹配成功3个以上特征点即可,在相对转速解算算法中,采用UKF滤波器提高了相对转速解算精度。附图说明[0178]通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明的其它特征、目的和优点将会变得更明显:[0179]图1为本发明方法过程示意图;[0180]图2为航天器与非合作目标示意图;[0181]图3为目标坐标系与相机坐标系关系图;[0182]图4为特征点匹配过程示意图;[0183]图5为本发明方法流程图;[0184]图6为双目视觉原理图。具体实施方式[0185]下面对本发明的实施例作详细说明:本实施例在以本发明技术方案为前提下进行实施,给出了详细的实施方式和具体的操作过程。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。[0186]实施例[0187]本实施例提供了一种基于双目视觉的非合作目标相对状态解算方法,包括以下步骤:[0188]步骤SI:解算载体表面特征点在相机坐标系下的3D坐标;[0189]步骤S2:解算目标坐标系相对相机坐标系的相对姿态;[0190]步骤S3:解算目标坐标系相对相机坐标系的相对转速。[0191]其中:[0192]步骤1包括如下步骤:[0193]步骤SI.I:如图1所示,基于双目视觉原理解算非合作目标相对航天器的相对状态的原理是在航天器的质心处安装双目相机,航天器记为L,以左相机的相机坐标系作为参考坐标系,记为.非合作目标记为F,由初始时刻获取的特征点建立的坐标系建立目标坐标系,记为利用双目相机采集图像,记录图像采集的时刻^或记录相邻两个图像采集的间隔Δtl,其中i表示采集图像的索引;[0195]步骤SI.2:利用SIFT或SURF算法匹配双目相机的左、右相机图像的载体表面特征点,记1N表示在^时刻匹配成功的特征点个数;[0196]步骤Sl.3:利用双目视觉原理计算载体表面特征点在双目相机的左相机坐标系下的3D坐标。如图6所示,利用双目相机计算特征点的3D坐标时涉及到计算机图像坐标系,成像平面坐标系,相机坐标系和目标坐标系。〇F_xyz表示目标坐标系,也即分别表示左右相机的计算机图像坐标系;分别表示左右相机的成像平面坐标系,Oi-xyz和0r-xyz分别表示左右相机坐标系,(UQ,Vo为1Oq-Uv和1Όο-uv的中心,也是的原点,在本发明专利中,〇i-xyz即为.空间中一点P在X中的坐标为X,y,Z,该点在1Oq-UV和%-UV中的坐标记为U1,V1和Ur,Vr。[0197]特征点P在1Oq-UV和1Oq-UV中的坐标与(x,y,z的关系用(Ia〜(Ib表示,联立式Ia〜(Ib可解出(x,y,z,如式(Ic所示。[0201]其中,f表示相机的焦距;dx,dy分别表示每个像素点在计算机成像平面坐标系的X轴和y轴方向上的物理尺寸;b表示左右相机之间的基线长度;Tb=[_b,0,0]。[0202]记I示在ti时刻匹配成功的第k个特征点在X下的3D坐标,k=l,2,…,iN0[0203]步骤2包括以下步骤:[0204]步骤S2.1:建立目标坐标系[0205]利用载体表面特征点在相机坐标系i:的坐标建立目标坐标系在目标坐标系下的坐标为I二者存在以下关系:[0207]其中,,T3X1=[T1T2T3]';R表示旋转矩阵,T表示平移向量;ri,r2,r3分别表示旋转矩阵的列向量rij表示旋转矩阵的元素;TiΤ2Τ3分别表示平移向量的元素;符号[·]表示矩阵或向量的转置,在目标坐标系J7下的坐标值;〇表示在to时刻;[0208]目标坐标系建立方法可以用图3表示,相机坐标系遵守右手法则,为使目标坐标系与其一致,为坐标系原点,选择向量的方向为坐标系沪的X轴正向,此时,应保证X2X1,改变特征点的顺序,易满足条件。坐标系^的7轴在由点确定的平面内,且与X轴垂直。[0209]根据坐标系建立的原则可以,特征点在;F的坐标为,为特征点的距离,如3b所示,式3c〜3d确定的及y轴正向。[0214]其中,α表示向量的夹角;[0215]将式3a〜3d代入式⑵求解R和T:[0220]步骤S2.2:建立七参数模型[0221]坐标系之间的变换关系利用七参数模型表示,即:[0223]PuR1,T1分别表示在U时刻y变换到i:的尺度因子、旋转矩阵和平移向量;为解算出旋转矩阵,应保证在ti时刻iNS4;[0224]欧拉角与旋转矩阵存在变换关系,即可以用欧拉角表示旋转矩阵,在t时刻,经过一定顺序的三次欧拉角約』^队旋转可以与平行,相应的旋转矩阵采用下式表示,为便于简洁表达,在此将欧拉角的下标省略掉:[0226]步骤S2.3:求解载体表面特征点在;F下的坐标[0227]在to时刻,已建立目标坐标系F,利用时刻的变换关系计算^时刻载体表面特征点在目标坐标系:F中的坐标,进而得到U时刻同一组载体表面特征点分别在i:和;F中的坐标。理论上讲,只要相邻时刻,匹配成功的特征点1NM即可解算旋转矩阵R1^dPT1;[0229]步骤S2.4:求解目标坐标系与相机坐标系的相对姿态[0230]从步骤S2.3,得到在ti时刻4个匹配成功的特征点在:下的坐标,分别记为和1¾用两个坐标系下的特征点计算出七参数,其中,Ax,Ay,AZ分别表示之间的平移量,也即Ti=[Δχ,Ay,Δζ]%[0231]在处做泰勒展开如下:[0234]其中:,Υ表示在ti时刻特征点在.X中坐标组成的列向量,大小为S1NXl5A表示系数矩阵,矩阵维数为3^\7;1表示在t时刻与dx无关的常数项,大小为3^X1。[0237]其中,E3x3表示单位矩阵,M表示对旋转矩阵求导结果相关的系数矩阵,N表示与dy相关的系数向量。[0240]当1Ν3时,利用最小二乘方法求解七参数向量,迭代过程如下:[0241]步骤a,设置初始参数[0242]步骤b,将f代入上式8a〜8d,计算A,1;[0243]步骤c,根据误差方程求解I其中,V表示误差;[0244]步骤d,根据最小二乘原理求解改正数,其中,A’表示A的转置;[0245]步骤e,更新七参数值[0246]步骤f,若则停止迭代;否则执行步骤g[0247]步骤g,重复步骤b〜步骤f;[0248]步骤S2.5:利用整体最小二乘算法进一步提尚解算精度[0249]在步骤S2.4迭代过程中,仅考虑特征点在£中的误差,而实际应用中,系数矩阵也存在误差。针对这一问题,采用整体最小二乘原理进一步提高七参数的解算精度。新建立的EIV模型如式IOa所示:[0254]eA=VeCEAi,vec·表示将矩阵按列堆叠转化为列向量;Ea表示系数矩阵的残差,表示每次迭代中系数矩阵残差的预测值;[0255]目标函数:[0257]其中,ey表示观测量Y的残差,eA表示系数矩阵的残差重构的列向量λ分别表示mX1拉格朗日乘数向量,Qy和Qa分别表示观测量Y权逆阵和系数矩阵残差的权逆阵,表示直乘,Im表示mXm单位矩阵,Y表示由特征点在£_坐标系下的坐标构成的列向量;[0258]为求解泠别对变量求导,求目标函数最值过程如下,其中“〜”表示相应变量的预测值;[0259]对ey求导得:[0261]对eA求导得:[0263]对λ求导得:[0265]对也求导得:[0267]由式12a〜12b可得[0270]将式(I3a〜(I3b代入12c得[0278]其中,下标i表示第i次迭代,表示单位权方差,Q1表示Qo表示系数矩阵方差,Qe表示权逆阵,表示第i+Ι次迭代的观测量误差,表示第i+1次迭代的系数矩阵误差;[0279]步骤S2.5中,迭代计算的初始值由步骤S2.4的结果给出,根据以上迭代计算,当时迭代停止,求解出的X即为进一步迭代后的七参数;[0280]步骤S2.6:计算相对姿态[0281]根据步骤S2.5中得到的欧拉角求解出旋转矩阵,依据下式求解出四元数[0283]步骤3包括如下步骤:[0284]步骤S3.1,建立动力学模型[0286]步骤S3.2,建立运动学模型[0289]其中:[0290]步骤S3.3,相对转速计算公式[0292]on,coF,ω分别表示相机坐标系的转速、目标坐标系的转速及目标坐标系相对相机坐标系的转速;汾表不相对转速的导数;Il和If分别表不航天器和非合作目标的转动惯量,Nl和Nf分别表示航天器和非合作目标受到的外力,Dq表示由四元数表示的旋转矩阵,q表示四元数,4表示四元数的导数,P表示四元数的矢量部分;[0294][pX]表示叉乘。[0295]步骤3.4,无损卡尔曼滤波估计转速[0296]定义状态方程如下:[0298]其中,_表示状态量的导数:[0299]f㈨表示状态量与其导数映射函数,w表示均值为零的高斯噪声;[0300]观测方程如下:[0301]Z=hn+v21[0302]其中,z表示观测量,η表示状态量,V表示均值为零的高斯白噪声,hη表示状态量与观测量的映射关系;[0303]其中,I1,ξ2,ξ3,ξ4,分别为相对姿态的四元数;ω=[ωχ,ωy,ωz]T分别表示相对转速;Ιπ,Ιτ2,Ιτ3,Ιτ4,Ιτ5,Ιτ6分另U表不转动惯量的元素;[0304]其中,转动惯量为,观测量为:[0305]根据观测量和测量量的关系建立模型,采用UKF滤波解算状态,包括如下步骤:[0306]步骤Α:利用UT变换计算Sigma点[0308]其中,Λ=α2η+κ-ηα表示Sigma点围绕%的分布特性,κ表示比例因子;[0309]步骤B:—步预测[0313]步骤C:二次采样[0315]步骤D:测量更新[0320]步骤E:滤波更新[0327]ί3结合ri的先验分布信息得出,最优值可设置为2;[0328]Xk表示k时刻的Sigma点集;[0329]%表示第k次迭代的状态量;[0330]表示变换后Sigma点集的第i列;[0331]Wm表示均值的加权值;[0332]表示一步预测状态;[0333]Pk+i|k表不后验方差阵;[0334]W。表示方差的权值;[0335]Qk表不噪声的方差矩阵;[0336]Zk+iIk表示k时刻预测k+1的测量值的估计值;[0337]表示预测观测值;[0338]xk+1|k表示根据一步预测值再次使用UT变换,产生的新Sigma点集;[0339]Kk+1表示在k+Ι时刻的滤波增益矩阵;[0340]表示k+Ι时刻的状态量估计值;[0341].表不量测输出变量的方差矩阵;[0342]是协方差阵;[0343]ζ表示一个标量,由经验得到;[0344]η表示系统状态维数;[0345]UKF滤波器的输入量为由四元数组成的测量值,根据不同时刻的输入通过不断迭代,最终可求出不同时刻,非合作目标相对相机坐标系的相对状态。[0346]非合作相对相机坐标系的相对状态可表示为[0348]本实施例提供的方法,可以应用于非合作目标相对状态的估计,在航天器质心位置或者相对质心平移一定距离安装双目相机,可以测量太空中非合作目标姿态、速度、位置未知的目标相对相机坐标系的位置、相对姿态和相对转速。具体实施过程包括:1安装相机;2采集双目图像;3特征坐标解算;4相对姿态和位置解算;5相对转速估计。[0349]以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变形或修改,这并不影响本发明的实质内容。

权利要求:1.一种基于双目视觉的非合作目标相对状态解算方法,其特征在于,包括如下步骤:步骤SI:解算载体表面特征点在相机坐标系下的3D坐标;步骤S2:解算目标坐标系相对相机坐标系的相对姿态;步骤S3:解算目标坐标系相对相机坐标系的相对转速。2.根据权利要求1所述的基于双目视觉的非合作目标相对状态解算方法,其特征在于,步骤Sl,包括如下步骤:步骤SI.1:在航天器的质心处安装双目相机,航天器记为L,以左相机的相机坐标系作为参考坐标系,记为X,非合作目标记为F,由初始时刻获取的特征点建立的坐标系建立目标坐标系,记为巧利用双目相机采集图像,记录图像采集的时刻^或记录相邻两个图像采集的间隔Atl,其中i表示采集图像的索引;Δti=ti+i-ti1;步骤SI.2:利用SIFT或SURF算法匹配双目相机的左、右相机图像的载体表面特征点,记1N表示在^时刻匹配成功的特征点个数;步骤Sl.3:利用双目视觉原理计算载体表面特征点在双目相机的左相机坐标系下的3D坐标;利用双目相机计算特征点的3D坐标时涉及到计算机图像坐标系,成像平面坐标系,相机坐标系和目标坐标系;设,θ-xyz表示目标坐标系,即.J^1Oq-UV和1Oq-UV分别表示左右相机的计算机图像坐标系分别表示左右相机的成像平面坐标系,Oi-xyz和Or-Xyz分别表示左右相机坐标系,(UQ,VQ为1Oq-UV和1Oq-UV的中心,也是的原点,Oi-xyzS卩为空间中一点P在中的坐标为x,y,z,该点在1Oq-Uv和1Oq-uv中的坐标记为Ul,VI和Ur,Vr;特征点P在1〇『uv和1Oq-UV中的坐标与x,y,z的关系用(Ia〜(Ib表示,联立式(Ia〜Ib可解出X,y,Z,如式IC所示:其中,f表示相机的焦距;dx,dy分别表示每个像素点在计算机成像平面坐标系的X轴和y轴方向上的物理尺寸;b表示左右相机之间的基线长度;Tb=[-b,O,0];记.表示在ti时刻匹配成功的第k个特征点在下的3D坐标,k=l,2,…,1Nc33.根据权利要求2所述的基于双目视觉的非合作目标相对状态解算方法,其特征在于,步骤S2,包括以下步骤:步骤S2.1:建立目标坐标系利用载体表面特征点在相机坐标系的坐标建立目标坐标系在目标坐标系下的坐标为二者存在以下关系:其中,,T3xl=ET1T2T3]SR表示旋转矩阵,T表示平移向量;ri、r2、r3分别表示旋转矩阵的列向量rij表示旋转矩阵的元素;Τι、Τ2、Τ3分别表示平移向量的元素;符号[·]表示矩阵或向量的转置,表示在目标坐标系f下的坐标值;0表示在to时刻;目标坐标系建立方法如下:1假设在to时刻,三个特征点在中的坐标值分别为,相应地在中的坐标值分别记为,如图3所示;2因相机坐标系遵守右手法则,为使目标坐标系与其一致:为坐标系原点,选择向量的方向为坐标系_的X轴正向,此时,应保证X2XI,改变特征点的顺序,易满足条件;3坐标系37的y轴在由点确定的平面内,且与X轴垂直;4z轴的方向与确定的平面垂直,遵守右手法则;根据坐标系建立的原则可知,特征点在的坐标为:_;χ2为特征点之间的距离,式3c〜3d确定及y轴正向:其中,α表示向量的夹角;将式3a〜3d代入式⑵求解R和T:步骤S2.2:建立七参数模型坐标系’之间的变换关系利用七参数模型表示,即:yi,Ri,Ti分别表示在ti时刻:F变换到£的尺度因子、旋转矩阵和平移向量;为解算出旋转矩阵,应保证在ti时刻iNS4;欧拉角与旋转矩阵存在变换关系,即用欧拉角表示旋转矩阵,在U时刻,经过一定顺序的三次欧拉角队旋转可以与平行,相应的旋转矩阵采用下式表示,为便于简洁表达,在此将欧拉角的下标省略掉:步骤S2.3:求解载体表面特征点在F下的坐标在to时刻,已建立目标坐标系F,利用时刻的变换关系计算^时刻载体表面特征点在目标坐标系F中的坐标,进而得到tl时刻同一组载体表面特征点分别在X和:F中的坐标;相邻时刻,匹配成功的特征点,多4即可解算旋转矩阵R1^jPT1;步骤S2.4:求解目标坐标系与相机坐标系的相对姿态从步骤S2.3,得到在ti时刻屮个匹配成功的特征点在7和£下的坐标,分别记为和,利用两个坐标系下的特征点计算出七参数,其中,AX,Ay,Δz分别表示之间的平移量,也即Ti=[Δχ,Ay,Δζ]%在处做泰勒展开如下:其中,Υ表示在ti时刻特征点在X中坐标组成的列向量,大小为S1NXl5A表示系数矩阵,矩阵维数为3^X7;1表示在U时刻与dx无关的常数项,大小为3^X1;其中,E3x3表示单位矩阵,M表示对旋转矩阵求导结果相关的系数矩阵,N表示与1μ相关的系数向量;当1Ν3时,利用最小二乘方法求解七参数向量,迭代过程如下:步骤a,设置初始参数步骤b,将ί代入上式8a〜8d,计算A,1;步骤c,根据误差方程求解,其中,V表示误差;步骤d,根据最小二乘原理求解改正数,,其中,A’表示A的转置;步骤e,更新七参数值:步骤f,老则停止迭代;否则执行步骤g;步骤g,重复步骤b〜步骤f;步骤S2.5:利用整体最小二乘算法进一步提尚解算精度;在步骤S2.4迭代过程中,仅考虑特征点在£中的误差,而实际应用中,系数矩阵也存在误差;针对这一问题,采用整体最小二乘原理进一步提高七参数的解算精度;新建立的EIV模型如式IOa所示:eA=VeCEm,vec·表示将矩阵按列堆叠转化为列向量;Ea表示系数矩阵的残差,表示每次迭代中系数矩阵残差的预测值;目标函数:其中,ey表不观测量Y的残差,θα表不系数矩阵的残差重构的列向量λ分别表不mX1拉格朗日乘数向量,Qy和Qa分别表示观测量Y权逆阵和系数矩阵残差的权逆阵,表示直乘,1^表示mXm单位矩阵,Y表示由特征点在坐标系下的坐标构成的列向量;为求解,分别对变量求导,求目标函数最值过程如下,其中“〜”表示相应变量的预测值;对ey求导得:对eA求导得:对λ求导得:对求导得:由式(12a〜12b可得:将式13a〜(13b代入12c得:其中,下标i表示第i次迭代,表示单位权方差,Qi表示Qo表示系数矩阵方差,Qe表示权逆阵表示第i+Ι次迭代的观测量误差,表示第i+1次迭代的系数矩阵误差;步骤S2.5中,迭代计算的初始值由步骤S2.4的结果给出,根据以上迭代计算,当时迭代停止,求解出的X即为进一步迭代后的七参数;步骤S2.6:计算相对姿态根据步骤S2.5中得到的欧拉角求解出旋转矩阵,依据下式求解出四元数4.根据权利要求3所述的基于双目视觉的非合作目标相对状态解算方法,其特征在于,步骤S3,包括如下步骤:步骤S3·1,建立动力学模型步骤S3.2,建立运动学模型其中:步骤S3.3,相对转速计算公式ωΡ,ω分别表示相机坐标系的转速、目标坐标系的转速及目标坐标系相对相机坐标系的转速;^表不相对转速的导数;Il和If分别表不航天器和非合作目标的转动惯量,Nl和Nf分别表示航天器和非合作目标受到的外力,Dq表示由四元数表示的旋转矩阵,q表示四元数,表示四元数的导数,P表示四元数的矢量部分;[PX]表示叉乘;步骤3.4,无损卡尔曼滤波估计转速定义状态方程如下:其中表示状态量的导数,fη表示状态量与其导数映射函数,w表示均值为零的高斯噪声;观测方程如下:其中,ζ表示观测量,η表示状态量,V表示均值为零的高斯白噪声,h⑻表示状态量与观测量的映射关系;其中,分别为相对姿态的四元数;分别表示相对转速;Iti,Ιτ2,Ιτ3,Ιτ4,Ιτ5,Ιτ6分别表不转动惯量的兀素;其中,转动惯量为,观测量为:根据观测量和测量量的关系建立模型,采用UKF滤波解算状态,包括如下步骤:步骤Α:利用UT变换计算Sigma点其中,表示Sigma点围绕%的分布特性,κ表示比例因子;步骤B:—步预测步骤C:二次采样步骤D:测量更新步骤E:滤波更新β结合η的先验分布信息得出;Xk表示k时刻的Sigma点集;%表不第k次迭代的状态量;表示变换后Sigma点集的第i列;Wm表示均值的加权值;表示一步预测状态;表示后验方差阵;We表不方差的权值;Qk表不噪声的方差矩阵;表示k时刻预测k+Ι的测量值的估计值;表示预测观测值;表示根据一步预测值再次使用UT变换,产生的新Sigma点集;Kk+1表示在k+Ι时刻的滤波增益矩阵;表示k+1时刻的状态量估计值;表示量测输出变量的方差矩阵;是协方差阵;ζ表示一个标量,由经验得到;η表示系统状态维数;UKF滤波器的输入量为由四元数组成的测量值,根据不同时刻的输入通过不断迭代,最终可求出不同时刻,非合作目标相对相机坐标系的相对状态;非合作相对相机坐标系的相对状态表示为:5.根据权利要求4所述的基于双目视觉的非合作目标相对状态解算方法,其特征在于,β最优值设置为2。

百度查询: 上海交通大学 一种基于双目视觉的非合作目标相对状态解算方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息,力求客观、公正,但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解,仅供参考使用,不能作为本公司承担任何法律责任的依据或者凭证。