本發(fā)明屬于深空探測技術(shù)領(lǐng)域，具體涉及一種基于立體導(dǎo)航影像的巡視器定位方法。

背景技術(shù)：

巡視器的高精度導(dǎo)航定位技術(shù)是深空探測領(lǐng)域中的關(guān)鍵技術(shù)之一，高精度的定位結(jié)果不僅可以確保巡視器的安全，而且可以使巡視器與地面控制系統(tǒng)通信時具有較好的指向角度，進(jìn)而為實現(xiàn)資源勘查等科學(xué)考察任務(wù)提供基礎(chǔ)。目前基于攝影測量的巡視器定位方法研究現(xiàn)狀如下：

石德樂等(2006)提出了以著陸器為基站，采用立體視覺測量和彩色圖像分割技術(shù)進(jìn)行跟蹤測量的月面巡視探測器定位方法，對慣導(dǎo)系統(tǒng)加里程計的定位方法進(jìn)行修正，減小月面巡視器的計算工作量，但是由于里程計的誤差，導(dǎo)致該方法實際應(yīng)用受到限制。劉傳凱等(2014)提出了一種基于拼接圖像的巡視器定位方法，首先將多幅導(dǎo)航影像進(jìn)行拼接，然后通過特征匹配得到大量的匹配點(diǎn)，最終通過光束法平差完成巡視器定位。馬友青等(2014)提出了基于lm法的光束法平差巡視器導(dǎo)航定位，利用lm算法代替牛頓迭代法完成光束法求解過程。王保豐等(2014)提出了一種基于計算機(jī)視覺的定位方法，將sift匹配、相關(guān)系數(shù)匹配、最小二乘匹配和光束法平差等多項技術(shù)融合，實現(xiàn)了相鄰站間月面巡視器的導(dǎo)航定位。馬友青等(2014)提出了基于立體圖像的月球車導(dǎo)航定位算法，通過匹配得到了連接點(diǎn)，然后通過相對定向和絕對定向過程完成了巡視器定位。劉少創(chuàng)等通過像點(diǎn)匹配、前方交會完成圖像的立體模型構(gòu)建，然后根據(jù)不同攝站間的連接點(diǎn)序列建立立體圖像條帶網(wǎng)，最終通過最小二乘平差，直接獲得月面巡視探測器的位置與姿態(tài)信息。吳偉仁等采用光束法平差完成了巡視器定位。劉召芹等(2014)通過對導(dǎo)航相機(jī)原始影像進(jìn)行匹配定位實現(xiàn)月球車的連續(xù)定位；采用導(dǎo)航相機(jī)生成的dom與降落相機(jī)生成的dom進(jìn)行sift匹配，并輔助人工選點(diǎn)匹配，完成巡視器基于dom與底圖匹配的定位。但是上述光束法定位方法中需要大量的匹配點(diǎn)作為定位起算數(shù)據(jù)。徐辛超等(2015)提出了基于空間后方交會迭代法的巡視器單攝站定位方法，但需要至少3個以上的控制點(diǎn)才能完成解算。

此外，針對巡視器的定位方法，國內(nèi)外專家學(xué)者還提出了結(jié)合里程計和imu的定位方法、imu和攝影測量結(jié)合的方法等。里程計和imu的定位方法中里程計會由于巡視器的打滑現(xiàn)象出現(xiàn)較大誤差，從而導(dǎo)致定位結(jié)果累積誤差增大。imu和攝影測量結(jié)合的方法定位結(jié)果較好，但是該方法的起算數(shù)據(jù)不僅依賴導(dǎo)航影像，而且依賴imu姿態(tài)數(shù)據(jù)，當(dāng)imu數(shù)據(jù)精度會隨著時間增加出現(xiàn)一定的累積誤差，會導(dǎo)致定位精度下降；甚至當(dāng)imu數(shù)據(jù)缺失的情況下也就無法完成巡視器定位。因此，需要研究一種依賴外部參數(shù)較少，且需要的起算匹配點(diǎn)數(shù)量也比較少的巡視器定位方法。

技術(shù)實現(xiàn)要素：

針對上述現(xiàn)有技術(shù)存在的不足，本發(fā)明提供一種基于立體導(dǎo)航影像的巡視器定位方法。

本發(fā)明的技術(shù)方案如下：

一種基于立體導(dǎo)航影像的巡視器定位方法，包括如下步驟：

步驟1：獲取左導(dǎo)航相機(jī)和右導(dǎo)航相機(jī)同時拍攝的一對立體導(dǎo)航影像即左導(dǎo)航影像和右導(dǎo)航影像；

步驟2：分別定義著陸器當(dāng)?shù)劂U垂坐標(biāo)系、巡視器零位坐標(biāo)系、左像平面直角坐標(biāo)系、右像平面直角坐標(biāo)系和像空間直角坐標(biāo)系；

步驟3：從左導(dǎo)航影像和右導(dǎo)航影像中提取同名像點(diǎn)的坐標(biāo)，并根據(jù)導(dǎo)航相機(jī)標(biāo)定的畸變參數(shù)對像點(diǎn)坐標(biāo)進(jìn)行誤差糾正，得到左導(dǎo)航影像中同名像點(diǎn)在左像平面直角坐標(biāo)系下的坐標(biāo)[(xl1,yl1),(xl2,yl2),…，(xli,yli)，…，(xln,yln)]和右導(dǎo)航影像中同名像點(diǎn)在右像平面直角坐標(biāo)系下的坐標(biāo)[(xr1,yr1),(xr2,yr2),…,(xri,yri),…,(xrn,yrn)]，其中，i＝1,2,…,n，n為同名像點(diǎn)對數(shù)；

步驟4：從著陸區(qū)dom及著陸區(qū)數(shù)字高程模型dem中提取同名像點(diǎn)對應(yīng)的月面點(diǎn)在著陸器當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)[(xa1,ya1,za1),(xa2,ya2,za2),…，(xai,yai,zai),…，(xan,yan,zan)]；

步驟5：根據(jù)巡視器初始狀態(tài)以及左右導(dǎo)航相機(jī)安裝位置計算在左導(dǎo)航影像的像空間直角坐標(biāo)系下右導(dǎo)航影像攝影中心的坐標(biāo)(lx,ly,lz)；

步驟6：提取導(dǎo)航相機(jī)焦距f，并根據(jù)巡視器俯仰角度和偏航角度分別計算r俯和r偏；

步驟7：按照最小二乘求解公式x＝(a^ta)^-1a^tl，計算左導(dǎo)航影像攝影中心在著陸器當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)(xs1,ys1,zs1)及像空間輔助坐標(biāo)系到著陸器當(dāng)?shù)劂U垂坐標(biāo)系三個軸的旋轉(zhuǎn)角(ω，κ)，其中：

nx＝b2·lz-b3·ly，ny＝b3·lx-b1·lz，nz＝b1·ly-b2·lx，

為像空間直角坐標(biāo)系與著陸器當(dāng)?shù)劂U垂坐標(biāo)系的旋轉(zhuǎn)矩陣r對應(yīng)的三角函數(shù)，

步驟8：根據(jù)巡視器零位時刻相對于巡視器零位坐標(biāo)系原點(diǎn)在三個坐標(biāo)軸方向的偏移量(δx，δy，δz)、旋轉(zhuǎn)矩陣r、r俯、r偏、左導(dǎo)航相機(jī)與巡視器零位坐標(biāo)系原點(diǎn)之間的偏移量(ox，oy，oz)及左導(dǎo)航影像攝影中心(xs1，ys1，zs1)，按照如下公式計算巡視器質(zhì)心坐標(biāo)(xc，yc，zc)，完成巡視器定位：

有益效果：本發(fā)明提出一種基于立體導(dǎo)航影像的巡視器定位方法，與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)：

(1)相對于傳統(tǒng)攝影測量定位的方法，至少只需要兩對匹配特征點(diǎn)即可完成定位，匹配特征點(diǎn)極少的情況下也能完成定位，增加了方法的適用情況；

(2)首次結(jié)合左導(dǎo)航相機(jī)和右導(dǎo)航相機(jī)的安裝位置參數(shù)，將右導(dǎo)航相機(jī)轉(zhuǎn)換為左導(dǎo)航相機(jī)的位置的相關(guān)量，即只獲取左導(dǎo)航相機(jī)的位置即可得到右導(dǎo)航相機(jī)的位置以及巡視器位置等信息；

(3)克服了巡視器行駛過程中由于打滑現(xiàn)象造成的里程計累積誤差，進(jìn)而導(dǎo)致采用imu和里程計組合導(dǎo)航定位過程中，定位誤差累積增大的缺點(diǎn)；

(4)由于起算數(shù)據(jù)中不存在imu和里程計等傳感器的數(shù)據(jù)，因此不存在累積誤差，定位精度優(yōu)于imu和里程計組合定位方法；

(5)起算數(shù)據(jù)僅為左右導(dǎo)航相機(jī)的導(dǎo)航影像，依賴的傳感器參數(shù)少，即使imu、里程計等設(shè)備參數(shù)丟失也可以完成定位，增加了定位方法的適用性。

附圖說明

圖1為本發(fā)明一種實施方式的基于立體導(dǎo)航影像的巡視器定位方法流程圖；

圖2為本發(fā)明一種實施方式的巡視器零位坐標(biāo)系示意圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明的一種實施方式作詳細(xì)說明。

如圖1所示，本實施方式的基于立體導(dǎo)航影像的巡視器定位方法，包括：

步驟1：獲取左導(dǎo)航相機(jī)和右導(dǎo)航相機(jī)同時拍攝的一對立體導(dǎo)航影像即左導(dǎo)航影像和右導(dǎo)航影像；

步驟2：分別定義著陸器當(dāng)?shù)劂U垂坐標(biāo)系、巡視器零位坐標(biāo)系、左像平面直角坐標(biāo)系、右像平面直角坐標(biāo)系、像空間直角坐標(biāo)系和像空間輔助坐標(biāo)系；

本實施方式中，所述陸器當(dāng)?shù)劂U垂坐標(biāo)系原點(diǎn)位于著陸器質(zhì)心在地面的投影點(diǎn)，z軸為鉛垂線向下方向，xy軸位于與鉛垂線垂直的平面內(nèi)，x軸指向北方向，y軸指向東方向。月面巡視器導(dǎo)航定位是以著陸器當(dāng)?shù)劂U垂坐標(biāo)系為參考。

如圖2所示，所述巡視器零位坐標(biāo)系原點(diǎn)位于巡視器桅桿偏航旋轉(zhuǎn)軸線與云臺俯仰旋轉(zhuǎn)軸線的交點(diǎn)o，x軸與云臺俯仰旋轉(zhuǎn)軸線重合，y軸為桅桿旋轉(zhuǎn)角為零度時刻，在水平面內(nèi)垂直于云臺俯仰旋轉(zhuǎn)軸線向前，z軸與xy軸構(gòu)成右手系；巡視器立體視覺系統(tǒng)中的導(dǎo)航相機(jī)安裝在桅桿上方的云臺上，可以在水平方向繞桅桿進(jìn)行旋轉(zhuǎn)和垂直方向的俯仰。導(dǎo)航影像在拍攝時，為了拍攝周圍地形，圍繞桅桿進(jìn)行一定角度的旋轉(zhuǎn)，稱為偏航角，記為p；圍繞云臺旋轉(zhuǎn)軸進(jìn)行一定角度的旋轉(zhuǎn)，稱為俯仰角，記為f。

所述左像平面直角坐標(biāo)系(左影像平面內(nèi))原點(diǎn)在左像像主點(diǎn)，x軸方向為水平方向向右，平行于影像像素排列的行方向，y軸方向為垂直向上，平行于像素排列的列方向。

所述右像平面直角坐標(biāo)系(右影像平面內(nèi))原點(diǎn)在右像像主點(diǎn)，x軸方向為水平方向向右，平行于影像像素排列的行方向，y軸方向為垂直向上，平行于像素排列的列方向。

所述像空間直角坐標(biāo)系原點(diǎn)位于左導(dǎo)航影像攝影中心s處，x軸和y軸分別平行于像平面直角坐標(biāo)系的對應(yīng)各軸，z軸和x軸y軸構(gòu)成右手坐標(biāo)系。

所述像空間輔助坐標(biāo)系原點(diǎn)位于左導(dǎo)航影像攝影中心s處，三軸方向分別平行于著陸器當(dāng)?shù)劂U垂坐標(biāo)系的三軸方向。

步驟3：從兩張導(dǎo)航影像中提取匹配特征點(diǎn)即同名像點(diǎn)的坐標(biāo)，并根據(jù)導(dǎo)航相機(jī)標(biāo)定的畸變參數(shù)對像點(diǎn)坐標(biāo)進(jìn)行誤差糾正，得到左導(dǎo)航影像中像點(diǎn)在左像平面直角坐標(biāo)系下的坐標(biāo)[(xl1,yl1),(xl2,yl2),…，(xli,yli)，…，(xln,yln)]和右導(dǎo)航影像中同名像點(diǎn)在右像平面直角坐標(biāo)系下的坐標(biāo)[(xr1,yr1),(xr2,yr2),…,(xri,yri),…,(xrn,yrn)]，其中，i＝1,2,…,n，n為匹配特征點(diǎn)對數(shù)；

步驟4：從著陸區(qū)dom及著陸區(qū)數(shù)字高程模型dem中提取匹配特征點(diǎn)對應(yīng)的各月面點(diǎn)在著陸器當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)[(xa1,ya1,za1),(xa2,ya2,za2),…，(xai,yai,zai),…，(xan,yan,zan)]；

步驟5：根據(jù)巡視器初始狀態(tài)即無俯仰角度和偏航角度時的狀態(tài)以及左右導(dǎo)航相機(jī)安裝位置計算在左導(dǎo)航影像的像空間直角坐標(biāo)系下右導(dǎo)航影像攝影中心的坐標(biāo)(lx,ly,lz)；

步驟6：提取導(dǎo)航相機(jī)焦距f，并根據(jù)巡視器俯仰角度和偏航角度分別計算r俯和r偏；

步驟7：按照最小二乘求解公式x＝(a^ta)^-1a^tl，計算左導(dǎo)航影像攝影中心在著陸器當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)(xs1,ys1,zs1)及像空間輔助坐標(biāo)系到著陸器當(dāng)?shù)劂U垂坐標(biāo)系三個軸的旋轉(zhuǎn)角(ω，κ)：

其中：

上述最小二乘公式x＝(a^ta)^-1a^tl推導(dǎo)過程如下：

在左導(dǎo)航影像和右導(dǎo)航影像中提取月面點(diǎn)對應(yīng)的同名像點(diǎn)坐標(biāo)(xli,yli)和(xri,yri)，同名像點(diǎn)對應(yīng)的像空間直角坐標(biāo)系坐標(biāo)為(xli,yli,-f)和(xri,yri,-f)，像空間輔助坐標(biāo)系坐標(biāo)為(xli,yli,zli)和(xri,yri,zri)，存在如下等式：

其中，r為左導(dǎo)航影像的像空間直角坐標(biāo)系與像空間輔助坐標(biāo)系的三個坐標(biāo)軸之間旋轉(zhuǎn)角(ω，κ)組成的旋轉(zhuǎn)矩陣，同時，由于著陸器當(dāng)?shù)劂U垂坐標(biāo)系與像空間輔助坐標(biāo)系三軸平行，因此，(ω，κ)即為像空間直角坐標(biāo)系與著陸器當(dāng)?shù)劂U垂坐標(biāo)系的三軸旋轉(zhuǎn)角；

為旋轉(zhuǎn)矩陣r對應(yīng)的三角函數(shù)。

左導(dǎo)航影像攝影中心在著陸器當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)為(xs1,ys1,zs1)，右導(dǎo)航影像攝影中心著陸器當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)為(xs2,ys2,zs2)，對左導(dǎo)航影像有：

對右導(dǎo)航影像有：

在左導(dǎo)航影像的像空間直角坐標(biāo)系下，左導(dǎo)航影像攝影中心坐標(biāo)為(0,0,0)，根據(jù)左右導(dǎo)航相機(jī)安裝位置可以得到右導(dǎo)航影像攝影中心的坐標(biāo)(lx,ly,lz)；在左導(dǎo)航影像的像空間輔助坐標(biāo)系下，左導(dǎo)航影像攝影中心坐標(biāo)為(0,0,0)，右導(dǎo)航影像攝影中心坐標(biāo)為(bx,by,bz)，存在如下關(guān)系式：

由于像空間輔助坐標(biāo)系坐標(biāo)軸指向與著陸器當(dāng)?shù)劂U垂線坐標(biāo)系指向一致，存在如下關(guān)系式：

結(jié)合式(1)和式(2)及旋轉(zhuǎn)矩陣r，可以得到：

由于左導(dǎo)航影像和右導(dǎo)航影像姿態(tài)角相同，因此旋轉(zhuǎn)矩陣r也相同，結(jié)合式(1)、式(3)和式(5)，可以得到如下關(guān)系式：

根據(jù)式(6)和式(7)可得：

將式(8)化為誤差方程，整理得到系數(shù)矩陣a、常數(shù)項l、未知數(shù)x，得到最小二乘公式x＝(a^ta)a^tl。所述未知數(shù)x包括(xs1，ys1，zs1)和(ω，κ)。

系數(shù)矩陣a中，各偏導(dǎo)數(shù)的求解過程如下：公式(8)中，式1和式2對于(xs1，ys1，zs1)和(ω，κ)6個變量進(jìn)行線性化，并求解偏導(dǎo)數(shù)的方法，可參照攝影測量學(xué)領(lǐng)域中空間后方交會原理中對各變量求解偏導(dǎo)數(shù)的方法；

針對式3和式4進(jìn)行線性化并求解偏導(dǎo)數(shù)的方法，具體如下：

首先對公式(8)中式3和式4進(jìn)行化簡如下：

對(xs1,ys1,zs1)求偏導(dǎo)得：

同理可得：

對求偏導(dǎo)得：

令則有：

對ω求偏導(dǎo)得：

對κ求偏導(dǎo)得：

步驟8：將攝影中心位置(xs1，ys1，zs1)代入下式，計算巡視器質(zhì)心在當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)(xc，yc，zc)，完成巡視器定位：

上式的推導(dǎo)過程為：巡視器零位坐標(biāo)系下，根據(jù)巡視器零位時刻左導(dǎo)航相機(jī)安裝位置得到左導(dǎo)航影像與巡視器零位坐標(biāo)系原點(diǎn)之間的偏移量為(ox，oy，oz)，可得巡視器零位坐標(biāo)系原點(diǎn)在著陸器當(dāng)?shù)劂U垂坐標(biāo)系下的坐標(biāo)(xo，yo，zo)為：

其中：r俯、r偏分別為根據(jù)巡視器立體視覺系統(tǒng)俯仰角度和滾動角度計算的旋轉(zhuǎn)矩陣；

由巡視器設(shè)計參數(shù)得到巡視器質(zhì)心在著陸器當(dāng)?shù)劂U垂坐標(biāo)系下坐標(biāo)為(xc，yc，zc)，巡視器初始狀態(tài)相對于巡視器零位坐標(biāo)系原點(diǎn)在三個坐標(biāo)軸方向的偏移量分別為(δx，δy，δz)，則可以得到以下關(guān)系式：

將式(10)代入式(11)可得：

巡視器在前10個測站利用基于立體導(dǎo)航影像的巡視器定位方法采用兩點(diǎn)定位的結(jié)果和經(jīng)典攝影測量光束法定位結(jié)果以及兩者的互差，如表1所示：

表1定位結(jié)果及互差表

由表1互差可以得出，基于立體導(dǎo)航影像的巡視器定位方法能夠有效的完成月面巡視器定位。