基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法与流程

本发明属于深空探测自主导航领域，应用在小天体探测着陆过程中，具体涉及一种基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法。

背景技术：

近年来，小天体探索已成为一项艰巨但具有科学意义的深空探测任务。然而，相比于行星探测任务，小天体任务具有探测目标远、飞行时间长、所处环境动态多变、地面测控站到探测器的通讯有较长的时间延迟等特点。因此，小天体探测器需要依靠自身所携带的设备自主完成深空探测任务。

随着高性能星载计算机和成像敏感器技术的发展，导航图像分辨率越来越高，基于光学信息的视觉自主导航系统成为了各国学者研究的热点。回顾小天体探测视觉导航算法，其大多是以特征点、陨石坑或自身携带反光球作为导航陆标。

比如，以特征点作为导航陆标，早在1999年，johnson等研究了基于特征点跟踪与匹配的相对导航算法，通过利用shi-tomasi-kanade算法跟踪图像序列中的特征点，得到探测器帧间相对运动信息(johnson,andrewe.,larryh.matthies.preciseimage-basedmotionestimationforautonomoussmallbodyexploration.proceedingsofthe5thinternationalsymposiumonartificialintelligence,roboticsandautomationinspace,noordwijk,netherlands,1-3june1999:627-634.)。该算法具有较强的自主性，但容易出现误匹配或无法匹配的情况。后续又发展了如sift、surf、asift、camshift等诸多具有尺度、旋转不变性的特征点提取匹配算法。但通过实验发现，正确匹配特征点个数的增多也不能明显提高导航精度，反而增加了计算复杂度，仅特征点描述符的存储就需要几百兆的内存空间，而且还需要进行多维数据间的筛选与匹配，现有星载计算机的性能很难满足这些算法对内存和运行速度的要求，实时性也难以保证。可见，特征点提取和匹配数量多、计算量大、运行时间长；且特征点一般不具备绝对坐标，一般只能应用于相对导航。

对于陨石坑作为导航路标，虽然国内外学者对陨石坑作为导航陆标进行提取和匹配方法进行了一些相关研究，但相比月球、火星等大天体而言，小天体由于质量较小，表面分布的陨石坑数量稀少，光照角度变化对其陨石坑提取和匹配影响较大，且先验信息不足，利用陨石坑作为小天体视觉导航陆标难度较大。同样陨石坑作为导航陆标时也存在陨石坑提取和匹配困难，易出现误匹配；而且陨石坑在天梯表面分布不均，当着陆区域没有陨石坑或陨石坑较少时该算法难以估计着陆器位姿。

而自身携带反光球作为导航陆标会增加探测器的自身重量而且投放位置难以精确控制，针对该问题，日本宇航局研发的隼鸟号、隼鸟2号小天体探测器，利用投放反光球作为导航陆标(参见tsudayuichi,yoshikawamakoto,abemasanao.systemdesignofthehayabusa2—asteroidsamplereturnmissionto1999ju3.actaastronautica,2013,91:356-362.)。该方法简单有效，但增加了探测器的自身重量，探测成本增加；同时，由于小天体引力较小，反光球的投放位置难以控制精确，若其被岩石遮挡或几何构型不佳，会直接影响导航精度。

综上，无论利用特征点、陨石坑或自身携带反光球等作为导航陆标，都具有各自的局限性。在小天体探测器着陆过程中，着陆前得到的小天体的特征信息量少，小天体模型及物理参数主要通过绕飞段的探测得到，无法实现精确建模。小天体本身由于质量较小，一般无法束缚住大气，探测器在着陆过程中，太阳照射下的太阳帆板投影可较为清晰地投射到小天体表面，且太阳帆板及其投影形状规则、已知，便于后续的提取、建模及计算过程。随着探测器高度的下降，探测器太阳帆板投影在导航图像中越发明显，如果能将太阳帆板投影信息用于导航算法当中，作为导航陆标，可以克服传统导航陆标的不足，提高探测器在着陆过程中的导航精度。

技术实现要素：

本发明为了解决小天体探测器在下降着陆段位姿估计的问题，提供了一种基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法，在不增加探测器自身重量的基础上，为小天体探测器着陆提供一种新的位姿估计的视觉自主导航算法，同时满足星载计算机实时性和运行速度的要求。

本发明是采用以下的技术方案实现的：一种基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法，包括以下步骤：

步骤a、建立导航及投影坐标系；

选取着陆点坐标系作为世界坐标系os-xsyszs，用于描述导航相机和其载体在空间中的具体位置；并假设探测器本体坐标系与导航相机坐标系oc-xcyczc重合，从着陆点坐标系转换到导航相机坐标系的变换关系记为其中为旋转矩阵，t为平移向量，导航相机成像模型采用理想针孔模型；

导航相机获取图像后，图像点在图像坐标系中的齐次坐标记为：u＝[uv1]^t；物体点在着陆点坐标系中的齐次坐标为：xs＝[xyz1]^t，物体点在导航相机坐标系中的齐次坐标为xc＝[xcyczc1]^t；图像坐标系中图像点在世界坐标系下的坐标为：矩阵k3×3为已知的导航相机的内参数矩阵；

步骤b、基于导航图像提取的探测器太阳帆板投影信息求取探测器滚动角和俯仰角变化角度；

步骤c、基于步骤b得到的角度变化信息对前一时刻导航图像进行模板校正，得到校正后的模板图像；

步骤d、利用模板匹配得到校正后的模板图像与待匹配图像之间的旋转角度及位置变化，得到探测器的实时位姿信息。

进一步的，所述步骤b中，在解算探测器在不同时刻的滚动角和俯仰角信息时，具体采用以下方式：

(1)假设小天体探测器太阳帆板边缘形状为矩形，其中平行于导航相机ocxc轴的边长为n米，平行于导航相机ocyc轴的边长为m米；设探测器理想姿态σ0为滚动角γ＝0，俯仰角即导航相机xcocyc面和着陆点坐标系xsosys面平行，此时探测器于星球表面投影形状、大小已知，|lm0|＝m、|ln0|＝n；

(2)将ti时刻探测器的姿态记为σi，获取导航图像中探测器太阳帆板长边投影长度|lni|及两边缘夹角角度θi，将|lni[及θi代入约束方程：中，解算出ti时刻探测器姿态相对于理想姿态σ0的滚动角γ0,i和俯仰角hi为探测器距星表高度高度，f为导航相机焦距；

(3)同理，解算出ti+1时刻的γ0,i+1和代入式中，即可得到探测器滚动角和俯仰角从ti时刻到ti+1时刻角度变化。

进一步的，所述步骤c中，在进行图像校正时，具体采用以下方式：

(1)引入图像校正坐标系对ti时刻导航图像进行旋转校正，图像校正坐标系与原始图像坐标系之间的转换关系如下：其中，r′为坐标转换旋转矩阵；

(2)将上述转换关系代入由步骤b解算的γi,i+1，同时令ψi,i+1＝0，可得：

若ti时刻导航图像上某一点的坐标为(x0,y0)，则在校正后的图像其对应该点的坐标为：(x1,y1)，进而得到校正后图像上各点的坐标，得到模板图像。

进一步的，所述步骤c中，若计算得到的转换参数是非整数，则利用坐标转换公式对ti时刻图像进行转换时，进行插值操作：

f(x,y)表示校正后图像(x,y)处的像素值，q11(x1,y1)、q12(x1,y2)、q21(x2,y1)、q22(x2,y2)为点f(x,y)相邻已知四点像素值，则：

完成对ti时刻导航图像的校正。

进一步的，所述步骤d中，利用模板匹配解算探测器偏航角和位移信息时，具体采用以下方式：

(1)ti时刻校正后的导航图像f1(x,y)与ti+1时刻获取的导航图像f2(x,y)之间存在平移、旋转和缩放关系，即：

f2(x,y)＝f1[a(xcosψ+ysinψ)-δx,a(-xsinψ+ycosψ)-δy]

对其进行傅里叶变换、频域幅度谱提取后，通过对-极数坐标变换，得到二者的互功率谱函数：然后进行傅里叶反变换可得到一个冲激响应函数δ(w-μ,α-ψ)，通过寻找其峰值的坐标即可求得在极坐标系下的平移量(μ,ψ)，进而得到旋转角ψ和缩放量a，δz＝(a-1)hi；

(2)对f2(x,y)进行旋转和缩放校正后，与f1(x,y)之间只存在平移关系，则再通过互功率谱的计算，得到平移量(δx,δy)，重复步骤b至步骤d，解算出小天体探测器的实时位姿参数。

与现有技术相比，本发明的优点和积极效果在于：

本方案通过导航图像中探测器太阳帆板投影信息并结合模板匹配进行自主视觉导航，具有以下优势：

(1)无须考虑星表特征的形状特性和分布构型，充分利用导航相机在小天体表面所拍摄到的探测器太阳帆板投影信息，完全基于图像内容的匹配与分析，避免了着陆末端星表特征数目不足以进行导航定位，以及传统导航陆标提取与匹配困难、时间长的问题；

(2)阴影信息容易提取，对图像旋转、缩放，图像模糊，视角变化，噪声等都有较强的鲁棒性；而且计算量小、内存占用少，利用太阳帆板投影进行视觉导航时，只需对太阳帆板投影形状、大小特征进行提取，减少了算法运行时间、提高了导航算法的实时性，保证探测器位姿的精确估计。

附图说明

图1为本发明实施例所述视觉导航方法流程示意图；

图2为本发明实施例所构建的坐标系示意图，(1)为导航坐标系示意图；(2)为局部坐标系示意图；

图3为本发明实施例ti时刻模板图像与待匹配图像示意图，(a)为模板图像，(b)为待匹配图像；

图4为本发明实施例探测器位姿估计误差曲线示意图。

具体实施方式

为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用不同于在此描述的其他方式来实施，因此，本发明并不限于下面公开的具体实施例。

一种基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法，如图1所示，包括以下步骤：

步骤一、建立导航及投影坐标系；

步骤二、基于导航图像提取的探测器太阳帆板投影信息求取探测器滚动角和俯仰角变化角度；

步骤三、基于步骤二得到的角度变化信息对前一时刻导航图像进行模板校正，得到校正后的模板图像；

步骤四、利用模板匹配得到校正后的模板图像与待匹配图像之间的旋转角度及位置变化，得到探测器的实时位姿信息。

具体的：

步骤一：导航坐标系及投影模型的建立

在导航过程中，导航问题是一个涉及多坐标系的问题，任何两个坐标系之间都存在相对位置和姿态关系，为了实现精确导航，需要对不同坐标系之间的关系进行严格建模。

本实施例中，选取着陆点坐标系作为世界坐标系os-xsyszs，用于描述导航相机和其载体在空间中的具体位置，本实施例中的着陆器位置、姿态参数均在此坐标系下描述。如图2所示，该坐标系将着陆任务中选定的着陆点定义为原点os，从小天体质心到着陆点的方向矢量定义为oszs轴，沿着陆点经线的切线方向指向小天体南极方向的方向矢量定义为osxs轴，osys轴与oszs轴和osxs轴满足右手准则。

假设探测器本体坐标系与导航相机坐标系oc-xcyczc重合，选择导航相机光心作为原点oc，ocxc轴和ocyc轴所组成的平面与像平面平行，沿光心到成像平面中心的连线定义为oczc轴。因此，从着陆点坐标系转换到导航相机坐标系的变换关系为其中为旋转矩阵，t为平移向量。

由欧拉角与旋转角关系得该旋转矩阵为：

其中，滚动角γ为围绕osxs轴旋转的角度，俯仰角为围绕osys轴旋转的角度，偏航角ψ为围绕oszs轴旋转的角度，定义正方向为逆时针方向。

本实施例中探测器所采用的导航相机成像模型是理想针孔模型，相机类型和内部参数已知，即为线性模型。导航相机获取图像后，图像点在图像像素坐标系中的齐次坐标为u＝[uv1]^t，物体点在着陆点坐标系中的齐次坐标为xs＝[xyz1]^t，在导航相机坐标系中的齐次坐标为xc＝[xcyczc1]^t。

则导航相机坐标系与世界坐标系之间的变换关系为：

图像像素坐标系中图像点在世界坐标系下的坐标为：

其中矩阵k3×3为已知的导航相机的内参数矩阵，令则矩阵p被称为导航相机投影矩阵。旋转矩阵实际上只含有3个独立变量，连同平移向量t的三个元素，共有六个参数。这六个参数决定了导航相机在着陆点坐标系中的位置和姿态，且导航相机在着陆点坐标系中的位置和姿态与相机内参数无关，因此矩阵称为导航相机外参数矩阵，也是在导航问题找需要解算的参数。

本方案所提出的利用投影信息解算探测器位姿的算法在推导约束方程时，为了简化推导，建立投影局部坐标系op-xpypzp。由于小天体探测器太阳帆板边缘形状多数为矩形，以探测器边缘左前方端点为原点op，长端延长线为opxp轴，短端延长线为opyp轴，opzp轴与opxp轴、opyp轴满足右手准则，且xpopyp面与导航相机xcocyc面平行。导航图像中探测器边缘直线投影被定义为li＝(si,ei)，其中si和ei为边缘直线投影的两个端点，li为探测器边缘直线在小天体星球表面的真实投影。

步骤二：基于探测器着陆过程中的投影信息解算探测器在不同时刻的滚动角和俯仰角信息

假设小天体探测器边缘形状为矩形，其中平行于导航相机ocxc轴的边长度为n米，平行于导航相机ocyc轴的边长度为m米。在探测器高度为h米时，实际投影长度|li|与导航相机拍摄的导航图像中探测器边缘长度|li|的关系如式(4)所示：

其中，f为导航相机焦距，设探测器理想姿态σ0为滚动角γ＝0，俯仰角即导航相机xcocyc面和着陆点坐标系xsosys面平行，此时探测器于星球表面投影形状、大小已知，|lm0|＝m、|ln0|＝n，在局部坐标系中位于坐标轴上的探测器三个顶点坐标依次为：x1(0，0，0)^t、x2(0，m，0)^t、x3(n，0，0)^t。同时，由公式(4)可获得导航图像中探测器边缘长度|lm0|、|ln0|。

将ti时刻探测器的姿态记为σi，获取的导航图像中探测器太阳帆板长边投影长度|lni|及两边缘夹角角度θi为已知，令此时局部坐标系中位于坐标轴上的探测器三个顶点坐标依次为：x1′、x′2、x′3，则有：

xi′＝r0ixi(5)

其中，r0i为探测器从σ0到σi的变换矩阵。同时，令坐标转换矩阵可得长度约束条件：

|lni|＝a·|(ax1′)(ax3′)|(6)

将式(1)、(5)代入可得：

在导航图像中，边x1′x3′和x1′x2′投影对应方向向量分别为：

则角度约束如式(9)所示：

联立式(7)、(9)可得探测器在ti滚动角和俯仰角的约束方程为：

通过惯性测量元件判断旋转角度正负，其中逆时针为正。

通过输入不同时刻所获取的导航图像，即ti时刻导航图像中探测器长边投影长度|lni|及两边缘夹角角度θi，即可解算出对应时刻相对于理想姿态σ0探测器滚动角γ0,i和俯仰角变化。

同理，解算出ti+1时刻的γ0,i+1和代入下式，

由式(11)可得，探测器滚动角和俯仰角从ti时刻到ti+1时刻角度变化。

步骤三：ti时刻导航图像校正

直接对ti时刻和ti+1时刻获取的导航图像进行模板匹配解算探测器位姿信息，导航相机相对视角变化越大，导航图像畸变也越大。

故本方案首先将ti时刻获取的导航图像进行旋转校正，引入图像校正坐标系，其与图像坐标系之间的转换关系如式(12)所示，

其中，r′为坐标转换旋转矩阵，代入由式(11)解算的γi,i+1，同时令ψi,i+1＝0，可得：

若ti时刻导航图像上某一点的坐标为：(x0,y0)，则在校正后的图像其对应该点的坐标为：(x1,y1)，由式(12)便可计算校正后图像上各点的坐标。

一般情况下，计算得到的转换参数是非整数，当利用坐标转换公式对ti时刻图像进行转换时，输出像素坐标通常被映射到新坐标系下的非整数位置，而非整数的坐标是无法在图像这种离散数据上使用的，因此需要进行插值。

f(x,y)表示校正后图像(x,y)处的像素值，q11(x1,y1)、q12(x1,y2)、q21(x2,y1)、q22(x2,y2)为点f(x,y)相邻已知四点像素值，则：

由式(12)-(14)完成了对ti时刻导航图像的校正。

步骤四：利用模板匹配解算探测器偏航角和位移信息

ti时刻校正后的导航图像f1(x,y)与ti+1时刻获取的导航图像f2(x,y)之间存在平移、旋转和缩放关系，即：

f2(x,y)＝f1[a(xcosψ+ysinψ)-δx,a(-xsinψ+ycosψ)-δy](15)

其中a为缩放量，ψ为旋转量，(δx,δy)为平移量。在频域中表现为相位变化、频谱图旋转和缩放。即可通过模板匹配的方法解算探测器在ti时刻和ti+1时刻之间的偏航角和位移变换信息。

首先，对式(15)进行傅里叶变换得：

由于二者的相对位移不影响傅氏变换的幅值，只影响相位，故令m1(u,v)和m2(u,v)分别为f1(x,y)和f2(x,y)的频域幅度谱，并对其进行对-极数坐标变换得：

令w＝lgρ，μ＝lga，则可得到：

由于系数1/a²只对取值产生影响，对于参数的计算没有影响。所以m2(w,α)可以看作由m1(w,α)平移得到。其中(μ,ψ)为二者的平移量。通过对式(18)进行傅里叶变换，然后求出二者的互功率谱为：

其中，为的复共轭。对其进行傅里叶反变换可得到一个冲激响应函数δ(w-μ,α-ψ)，通过寻找这个峰值的坐标即可求得在极坐标系下的平移量(μ,ψ)，进而得到旋转角ψ和缩放量a。对f2(x,y)进行旋转和缩放校正后，与f1(x,y)之间只存在平移关系，再通过互功率谱的计算，即可求得平移量(δx,δy)。

重复步骤二至步骤四，则可解算出小天体探测器位姿参数，本方案充分利用了导航相机在小天体表面所拍摄到的探测器太阳帆板投影信息，避免了着陆末端星表特征数目不足以进行导航定位，以及传统导航陆标提取与匹配困难、时间长的问题，提高了导航算法的实时性，保证探测器位姿的精确估计。

本发明采用小天体下降着陆段基于太阳帆板投影与模板匹配小天体探测器位姿估计算法为实例分析，在进行算法验证时，以探测器初始高度为200m，从200m高度下降到10m高度为例，利用本发明算法进行位姿估计，其中每隔5m对探测器位姿进行一次估计，最终得到探测器的位姿估计误差曲线，具体仿真参数如表1所示。

表1着陆任务仿真参数

图3为ti时刻模板图像与待匹配图像示意图，探测器位姿估计误差曲线如图4，从图4可以看出，基于太阳帆板投影与模板匹配的小天体探测器位姿估计算法能够对探测器的位置进行实时估计，最终能得到高精度的位姿估计信息。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。