[發(fā)明專利]一種基于單相機(jī)導(dǎo)航影像的巡視探測器定位方法

有效

• 申請?zhí)枺?/td>: 201910047092.1
• 申請日: 2019-01-18
• 公開（公告）號: CN109631876B
• 公開（公告）日: 2022-04-12
• 發(fā)明（設(shè)計(jì)）人: 徐辛超;徐宗秋;鄭濤;劉明躍;李旭佳
• 申請（專利權(quán)）人: 遼寧工程技術(shù)大學(xué)
• 主分類號: G01C21/00
• 分類號: G01C21/00
• 代理公司: 沈陽東大知識產(chǎn)權(quán)代理有限公司 21109
• 代理人: 劉曉嵐

1.一種基于單相機(jī)導(dǎo)航影像的巡視探測器定位方法，其特征在于：包括以下步驟：

步驟1、定義巡視坐標(biāo)系、巡視器本體坐標(biāo)系、巡視器零位坐標(biāo)系、像平面直角坐標(biāo)系、像空間直角坐標(biāo)系及像空間輔助坐標(biāo)系；

步驟2、巡視探測器與著陸器分離后開始巡視考察任務(wù)；分離后的巡視探測器停留并拍照的位置，即當(dāng)前攝站巡視探測器在巡視坐標(biāo)系下的位置(X_E,Y_E,Z_E)，第一攝站巡視探測器位置為(0,0,0)；巡視坐標(biāo)系與巡視器本體坐標(biāo)系三軸方向夾角組成的旋轉(zhuǎn)矩陣為R_E；

步驟3、巡視器零位時(shí)刻，桅桿偏航角度為0，俯仰角度為0，由安裝參數(shù)或地面標(biāo)定過程獲得巡視器零位坐標(biāo)系與巡視器本體坐標(biāo)系在三軸方向的偏移量分別為(△X₀,△Y₀,△Z₀)；

步驟4、巡視器零位時(shí)刻，某一導(dǎo)航相機(jī)攝影中心S的在巡視器零位坐標(biāo)系下的坐標(biāo)為(△X_S,△Y_S,△Z_S)，由巡視器安裝時(shí)的安裝參數(shù)或地面標(biāo)定過程獲得；

步驟5、對于俯仰角度F，偏航角度P，某一導(dǎo)航相機(jī)攝影中心S在巡視器零位坐標(biāo)系下的坐標(biāo)(X_SP,Y_SP,Z_SP)如下公式所示：

其中，R_P、R_F分別為偏航角度P和俯仰角度F組成的旋轉(zhuǎn)矩陣；

步驟6、設(shè)定導(dǎo)航相機(jī)焦距為f，導(dǎo)航影像中某一特征點(diǎn)在導(dǎo)航影像中對應(yīng)的像點(diǎn)的像平面直角坐標(biāo)為(x,y)，其對應(yīng)的像空間輔助坐標(biāo)系坐標(biāo)為(X_T,Y_T,Z_T)，對應(yīng)地面點(diǎn)在巡視器零位坐標(biāo)系下的坐標(biāo)為(X₀,Y₀,Z₀)，該導(dǎo)航相機(jī)的攝影中心S在巡視器零位坐標(biāo)系下的位置和三軸旋轉(zhuǎn)角為(X_SP,Y_SP0,Z_SP,0,F,P)，則：

其中，λ為比例系數(shù)；

令進(jìn)一步整理，得：

步驟7、當(dāng)導(dǎo)航影像中某一特征點(diǎn)在多幅導(dǎo)航影像中出現(xiàn)時(shí)，將當(dāng)前導(dǎo)航影像對應(yīng)的俯仰和偏航角度帶入公式(3)，連列求解得到該特征點(diǎn)在巡視器零位坐標(biāo)系下的坐標(biāo)(X₀,Y₀,Z₀)；

步驟8、求解特征點(diǎn)在巡視器本體坐標(biāo)系下的坐標(biāo)(X_B,Y_B,Z_B)，如下公式所示：

步驟9、求解特征點(diǎn)在巡視坐標(biāo)系下的坐標(biāo)(X_D,Y_D,Z_D)，如下公式所示：

步驟10、導(dǎo)航相機(jī)在順時(shí)針方向繞固定偏航角度拍攝影像序列，令左導(dǎo)航相機(jī)在第i次偏航時(shí)拍攝的照片為I_i；在導(dǎo)航相機(jī)拍攝的序列影像中提取多個(gè)特征點(diǎn)，通過步驟5-步驟9求解出各特征點(diǎn)對應(yīng)的巡視坐標(biāo)系下的坐標(biāo)，并將它們作為巡視器下一個(gè)攝站的定位控制點(diǎn)；

步驟11、以步驟10中得到的特征點(diǎn)作為控制點(diǎn)，開始巡視器在第二攝站的定位；設(shè)定控制點(diǎn)出現(xiàn)在第二攝站的導(dǎo)航影像I_i和I_i+1中，像點(diǎn)坐標(biāo)分別為(x_i,y_i)和(x_i+1,y_i+1)，其對應(yīng)的俯仰角同為F，偏航角分別為P_i和P_i+1，影像對應(yīng)的外方位元素分別為和其中，為導(dǎo)航影像I_i的攝影中心的位置，為導(dǎo)航影像I_i在像空間直角坐標(biāo)系三軸方向的旋轉(zhuǎn)角，為導(dǎo)航影像I_i+1的攝影中心的位置，為導(dǎo)航影像I_i+1在像空間直角坐標(biāo)系三軸方向的旋轉(zhuǎn)角，令兩張影像的外方位元素中的三軸方向的夾角組成的旋轉(zhuǎn)矩陣分別為R_i和R_i+1，巡視器零位坐標(biāo)系此刻原點(diǎn)位置為(△X_S2,△Y_S2,△Z_S2)，則：

將公式7整理得：

結(jié)合步驟10中的像點(diǎn)坐標(biāo)和影像外方位元素，建立共線方程，如下公式所示：

其中，

步驟12、巡視器定位時(shí)外方位元素和為未知量，將公式8和公式9連列，并將其轉(zhuǎn)為可按照最小二乘求解的誤差方程的形式，如下公式所示：

X＝(B^TB)^-1B^TL (10)

其中：B為公式8和9對各項(xiàng)未知數(shù)的偏導(dǎo)數(shù)組成的系數(shù)矩陣，

步驟13、找到步驟11中提取的控制點(diǎn)在前一攝站的導(dǎo)航影像序列中的對應(yīng)像點(diǎn)，并通過步驟5-步驟9求解其對應(yīng)的巡視坐標(biāo)系下的坐標(biāo)；

步驟14、結(jié)合步驟13求解的控制點(diǎn)巡視坐標(biāo)系下的坐標(biāo)，在當(dāng)前攝站導(dǎo)航影像序列中的像點(diǎn)坐標(biāo)，代入公式10即可求解和

步驟15、根據(jù)導(dǎo)航影像攝影中心位置結(jié)合巡視器零位坐標(biāo)系原點(diǎn)與攝影中心的偏移量、巡視器本體坐標(biāo)系原點(diǎn)與巡視器零位坐標(biāo)系原點(diǎn)在巡視坐標(biāo)系下的偏移量，得到巡視器本體坐標(biāo)系原點(diǎn)在巡視坐標(biāo)系下的坐標(biāo)，實(shí)現(xiàn)最終定位；

步驟16、當(dāng)巡視器行駛到下一攝站時(shí)，根據(jù)指令獲取的相機(jī)俯仰角和偏航角，重復(fù)步驟11-步驟15實(shí)現(xiàn)定位。