一種基于四元數(shù)的空間后方交會全局收斂算法

龔 輝,姜 挺,江剛武,陳密密

1.信息工程大學(xué)測繪學(xué)院,河南鄭州450052;2.61512部隊(duì),北京100088

1 引 言

攝影測量學(xué)中慣用的空間后方交會解法是把成像時(shí)的姿態(tài)采用歐拉角描述,對共線條件方程進(jìn)行線性化,然后通過一定數(shù)量的地面控制點(diǎn)及像片上對應(yīng)的像點(diǎn),利用最小二乘原理進(jìn)行迭代求解[1]。該算法對外方位元素的初值要求很高,當(dāng)初值不好時(shí),迭代不收斂。同時(shí)該算法對像片獲取的傳感器平臺飛行質(zhì)量要求比較高,其姿態(tài)角不能太大,一般為垂直攝影。然而隨著傳感器技術(shù)的發(fā)展,姿態(tài)控制水平的提高,很多影像都是大傾角像片,如無人機(jī)、飛艇等獲取的像片及近景攝影所得到的影像。對于這類像片的空間后方交會,實(shí)踐表明慣用的算法由于無法獲取大傾角情況下良好的外方位元素初值而求解失敗。

為有效解決空間后方交會對外方位元素良好初值依賴的問題,文獻(xiàn)[2]提出一種利用多種幾何特征進(jìn)行求解的線性算法,文獻(xiàn)[3—4]各自提出只利用三個(gè)控制點(diǎn)進(jìn)行后方交會的解析算法,文獻(xiàn)[5]提出利用四個(gè)相關(guān)點(diǎn)進(jìn)行后方交會的求解方法,文獻(xiàn)[6—7]分別提出利用點(diǎn)、線進(jìn)行位置和姿態(tài)估計(jì)的線性解法。上述算法都是直接計(jì)算,無需外方位元素的初值,但是這些算法僅僅在只有少量控制點(diǎn)等情況下有效,當(dāng)控制點(diǎn)較多時(shí),求解非常復(fù)雜。為求解任意控制點(diǎn)情況下的外方位元素,文獻(xiàn)[8—9]提出一種基于單位四元數(shù)的無初值依賴空間后方交會算法。該算法是一種迭代算法,思路上與傳統(tǒng)算法類似,試驗(yàn)表明對外方位元素的初值無特殊要求,但該算法沒有從理論上對此進(jìn)行證明。文獻(xiàn)[10]也提出一種基于四元數(shù)的空間后方交會算法,但該算法的求解需要首先迭代解算出攝站到控制點(diǎn)距離,仍然需要良好的初值。

全局收斂算法,即無論迭代從何處出發(fā),均能收斂到最終結(jié)果,表明計(jì)算與迭代初值無關(guān),該類算法在計(jì)算機(jī)視覺領(lǐng)域中應(yīng)用較多。文獻(xiàn)[11]提出一種從視覺圖像中估計(jì)姿態(tài)的正交迭代算法。該算法具有全局收斂的性質(zhì),然而在利用奇異值分解求解旋轉(zhuǎn)矩陣時(shí)容易出現(xiàn)矩陣行列式值為-1的情況,導(dǎo)致求解結(jié)果不準(zhǔn)確。文獻(xiàn)[12]也提出一種全局收斂條件下的結(jié)構(gòu)與運(yùn)動估計(jì)快速算法。若將上述算法直接用于攝影測量空間后方交會,還存在一些問題,主要原因是二者的坐標(biāo)系統(tǒng)不一致,且旋轉(zhuǎn)矩陣的計(jì)算容易產(chǎn)生錯(cuò)誤結(jié)果?；诖?本文借鑒上述全局收斂思想,結(jié)合四元數(shù)在攝影測量中的良好應(yīng)用[8-9,13],提出一種基于四元數(shù)的空間后方交會全局收斂算法,并從理論上證明其全局收斂的特性。最后通過試驗(yàn)比較,驗(yàn)證了本文算法的正確性和優(yōu)越性。

2 空間后方交會全局收斂算法的基本原理

本文全局收斂算法的基本原理是將中心投影的共線條件方程轉(zhuǎn)化為絕對定向方程和正交投影變換公式,以此來建立數(shù)學(xué)模型和誤差函數(shù)進(jìn)行求解,并在求解過程中應(yīng)用四元數(shù)描述攝影姿態(tài)。該算法無須對共線條件方程進(jìn)行線性化,是一種非線性方程的迭代算法。

2.1 四元數(shù)及其三維旋轉(zhuǎn)表達(dá)形式

四元數(shù)是形如˙q=q0+iq1+jq2+kq3的超復(fù)數(shù)[14],其中 i,j,k為虛數(shù)單位,且滿足 i2=j2= k2=-1,jk=-kj=i,ki=-ik=j,ij=-ji= k?？梢詫⑺脑獢?shù)分解成標(biāo)量q0和矢量 q,即˙q= q0+q,其中 q=iq1+jq2+kq3。

四元數(shù)有自己的運(yùn)算規(guī)則[8],利用四元數(shù)的矢量形式可將兩個(gè)四元數(shù)的乘積寫為矩陣形式

式中

文獻(xiàn)[8]對四元數(shù)的運(yùn)算及其表達(dá)旋轉(zhuǎn)矩陣作了詳細(xì)論述,本文不再重復(fù),直接給出用四元數(shù)表達(dá)的三維旋轉(zhuǎn)形式及其對應(yīng)的旋轉(zhuǎn)矩陣M,如式(1)所示

由式(1)的旋轉(zhuǎn)矩陣求取攝影測量中慣用的歐拉角如式(2)所示。

2.2 數(shù)學(xué)模型

采用四元數(shù)描述影像姿態(tài)時(shí),像片的外方位元素變?yōu)?XS,YS,ZS,q0,q1,q2,q3),此時(shí)利用攝影測量理論[1],可得基于四元數(shù)的共線條件方程為

式中,(x0,y0,f)為像片內(nèi)方位元素;(x,y)為像點(diǎn)坐標(biāo);(X,Y,Z)為地面坐標(biāo);(XS,YS,ZS)為攝站位置;ai,bi,ci,i=1,2,3由式(1)確定。

將式(3)改寫為以下形式

式中

將式(5)表示為矢量形式

式中,t=-MrS;r′=(X′,Y′,Z′)T;r=(X,Y, Z)T;rS=(XS,YS,ZS)T。

此時(shí)共線條件方程式(4)變?yōu)?/p>

式中,v=(x-x0,y-y0,-f)T;r′(3)為矢量 r′的第三個(gè)分量。

由式(7)可以看出,矢量v既是坐標(biāo)原點(diǎn)與投影點(diǎn)(X′,Y′,Z′)連線的方向向量,又是坐標(biāo)原點(diǎn)與像點(diǎn)連線的方向向量,即投影點(diǎn)r′與像點(diǎn)共線。根據(jù)正交投影的性質(zhì),r′=(X′,Y′,Z′)T在方向v上的正交投影與其自身相等[11-12],則

式(6)與絕對定向方程具有相似的形式[1,13],只不過比例因子為1,因此,攝影成像過程可以分解為先對地面點(diǎn)(X,Y,Z)進(jìn)行旋轉(zhuǎn)平移,得到投影點(diǎn)(X′,Y′,Z′),再對投影點(diǎn)(X′,Y′,Z′)進(jìn)行正交投影,得到像點(diǎn)坐標(biāo)(x,y)。

2.3 誤差函數(shù)

借鑒文獻(xiàn)[11]中的目標(biāo)空間誤差,建立本文全局收斂算法的誤差函數(shù)。由于數(shù)據(jù)點(diǎn)誤差的影響,對每一個(gè)控制點(diǎn),等式(8)兩邊存在偏差,即

式中,i為點(diǎn)號。

因此,按最小二乘原理進(jìn)行空間后方交會就是要使式(9)中所有控制點(diǎn)誤差的平方和最小,即使式(10)的誤差函數(shù)最小

式中,M為包含姿態(tài)四元數(shù)的旋轉(zhuǎn)矩陣,且MTM= I,ri為控制點(diǎn)地面坐標(biāo),rS為攝站位置,n為點(diǎn)數(shù)。

3 空間后方交會全局收斂算法的實(shí)現(xiàn)

3.1 攝站位置計(jì)算

式(10)即為本文全局收斂算法的誤差函數(shù)。為求解外方位元素,將式(10)展開,可得

式中,Hi=(I-Vi)T(I-Vi)

當(dāng)旋轉(zhuǎn)矩陣 M已知時(shí),E(M,rS)轉(zhuǎn)化為E(rS),即式(11)僅為 rS的函數(shù),對其求導(dǎo)數(shù),可得

根據(jù)高等數(shù)學(xué)中的極值定理,要使式(11)最小,只需上述導(dǎo)數(shù)為0即可

求解可得

式(12)即為利用旋轉(zhuǎn)矩陣計(jì)算攝站位置的計(jì)算公式。當(dāng)M已知時(shí),rS為關(guān)于M的最優(yōu)解。

3.2 姿態(tài)四元數(shù)計(jì)算

由式(12)可知,攝站位置 rS是旋轉(zhuǎn)矩陣M的函數(shù),代入式(10),可得到誤差函數(shù)為只包含旋轉(zhuǎn)矩陣M的函數(shù),即

式中,T=-MrS(M);r′(M)=M(ri-rS(M))

此時(shí),式(10)的誤差函數(shù) E(M,rS)最小轉(zhuǎn)化為式(13)的 E(M)最小。該表達(dá)式與絕對定向的表達(dá)式[1,13]有類似的形式。本文利用四元數(shù)直接對式(13)進(jìn)行求解[13,15],得到姿態(tài)四元數(shù)。按絕對定向求解方法需對坐標(biāo) ri、Vir′i(M)進(jìn)行重心化,同時(shí)令 rE,i=Vir′i(M),則式(13)變?yōu)?/p>

將式(14)展開[13]

將上式展開,得到

于是求解姿態(tài)四元數(shù)只需在四元數(shù)域求解˙q,使得 F最大時(shí),E最小即可。

利用四元數(shù)的運(yùn)算法則有[8]

上式代入前文四元數(shù)乘積的矩陣為

繼續(xù)推導(dǎo),可得

式中,

由于矩陣N為實(shí)對稱矩陣,式(17)達(dá)到最大值的條件是姿態(tài)四元數(shù)的取值˙q為矩陣N的最大特征值所對應(yīng)的特征向量[13]。求出四元數(shù) ˙q后,便可按式(1)求得旋轉(zhuǎn)矩陣M。

3.3 迭代過程

由于攝站位置的計(jì)算依賴于旋轉(zhuǎn)矩陣M,而構(gòu)成旋轉(zhuǎn)矩陣的姿態(tài)四元數(shù)的計(jì)算又依賴于攝站位置,因此,需要反復(fù)迭代求解。當(dāng)控制點(diǎn)個(gè)數(shù)n≥3時(shí),預(yù)先任意給定旋轉(zhuǎn)矩陣或姿態(tài)四元數(shù)的初值,然后迭代計(jì)算攝站位置和姿態(tài)四元數(shù)的最或然值,直到外方位元素滿足精度要求為止。

上述的迭代過程可以描述為下面6個(gè)步驟:

(1)讀入原始數(shù)據(jù),包括內(nèi)方位元素,像點(diǎn)的觀測值及其對應(yīng)的地面控制點(diǎn)在地面坐標(biāo)系中的坐標(biāo)。

(2)確定外方位元素的初值。本文算法為全局收斂算法,給定任意初值均可求解出正確結(jié)果。同時(shí)按式(8)計(jì)算投影矩陣V。

(3)按式(1)計(jì)算旋轉(zhuǎn)矩陣M。

(4)按式(12)計(jì)算攝站位置 rS(M),并按式(13)計(jì)算r′(M)。

(5)對地面點(diǎn)坐標(biāo)和投影點(diǎn)坐標(biāo)按式(14)進(jìn)行重心化,并按式(18)計(jì)算矩陣N,通過計(jì)算矩陣N的最大特征值所對應(yīng)的特征向量得到姿態(tài)四元數(shù)˙q。

(6)重復(fù)(3)～(5),直到求解的外方位元素的精度小于給定的限差為止。

3.4 全局收斂的證明

為證明上述迭代算法全局收斂的性質(zhì),引入文獻(xiàn)[16]中的全局收斂定理及其在文獻(xiàn)[11—12]中的證明過程。全局收斂定理可以描述為:

當(dāng)算法S滿足下面三個(gè)條件時(shí),則該算法為全局收斂算法:①算法 S為閉算子(其定義在文獻(xiàn)[11]中給出);②算法 S所生成的矩陣集{Sk}為有界閉集;③算法S使得總誤差函數(shù)隨迭代次數(shù)的增加而減小。

對于本文算法來說,文獻(xiàn)[11]中已經(jīng)證明基于奇異值分解進(jìn)行絕對定向得到旋轉(zhuǎn)矩陣M的算法是閉映射,而本文采用四元數(shù)進(jìn)行絕對定向得到旋轉(zhuǎn)矩陣M的方法與文獻(xiàn)[11]中絕對定向類似,也是閉映射,且計(jì)算輸入數(shù)據(jù)為連續(xù)數(shù)據(jù),因此,本文算法是閉算子,條件①滿足。同時(shí)本文算法保證了迭代過程中所得到的旋轉(zhuǎn)矩陣M序列為正交矩陣,是有界閉集,條件②滿足。對于條件③,設(shè)第k次、k+1次迭代后的誤差函數(shù)分別為 E(M(k))、E(M(k+1)),第 k+1次迭代后的投影向量為則根據(jù)式(13),有

利用范數(shù)的運(yùn)算法則,有下面的等式成立[11]

將式(20)代入式(19),得

由于旋轉(zhuǎn)矩陣的求解是使式(14)最小,因此第k+1次求解得到的是使 E(M(k))取得最小的M(k+1),即

將上式代入式(21),可得

上述的證明過程表明,本文提出的基于四元數(shù)的空間后方交會算法是全局收斂算法,計(jì)算結(jié)果與外方位元素初值無關(guān),給定任意的初值均能迭代收斂到正確結(jié)果。

4 試驗(yàn)結(jié)果與分析

為驗(yàn)證本文提出的基于四元數(shù)的空間后方交會全局收斂算法的正確性和有效性,并與慣用的求解方法及文獻(xiàn)[8]中算法進(jìn)行比較,進(jìn)行了兩組試驗(yàn)計(jì)算。

第一組試驗(yàn)采用鄭州地區(qū)的一幅航空影像,其詳細(xì)參數(shù)如表1所示。

表1 實(shí)際像片參數(shù)Tab.1 The elements of actual image

控制點(diǎn)在影像上均勻分布,其像點(diǎn)坐標(biāo)由屏幕坐標(biāo)量測得到,地面坐標(biāo)由外業(yè)測量得到,具體的控制點(diǎn)坐標(biāo)如表2所示。

表2 實(shí)際像片的控制點(diǎn)坐標(biāo)Tab.2 The control coordinates of actual image

續(xù)表2

第二組試驗(yàn),采用文獻(xiàn)[8]中的模擬數(shù)據(jù)。模擬的外方位元素如表3所示,按嚴(yán)密的共線條件方程模擬了6張像片,具體的坐標(biāo)數(shù)據(jù)見文獻(xiàn)[8],其中有大航高、大傾角像片,也有小航高、小傾角像片。相機(jī)焦距為 100 mm,且 x0=0, y0=0。

表3 模擬像片的外方位元素Tab.3 The exterior orientation elements of simulative images

利用本文算法、傳統(tǒng)的迭代方法及文獻(xiàn)[8]中的算法分別對鄭州地區(qū)的實(shí)際航片和模擬生成的像片進(jìn)行空間后方交會計(jì)算。由于本文算法是全局收斂算法,迭代初值的計(jì)算可以任意給定,試驗(yàn)中取q0= 1,q1=q2=q3=0,即認(rèn)為像片為水平姿態(tài)。傳統(tǒng)的迭代解法與文獻(xiàn)[8]中算法的初值確定按文獻(xiàn)[8]中給定。解算中為便于比較,仍然將姿態(tài)四元數(shù)轉(zhuǎn)化為歐拉角,解算結(jié)果分別如表4、表5所示。

表4 實(shí)際像片解算結(jié)果Tab.4 The result of calculation for actual image

表5 模擬像片解算結(jié)果Tab.5 The result of calculation for simulative images

從表4、表5中的解算結(jié)果可以得到看出:

(1)對于實(shí)際數(shù)據(jù)的解算,本文算法解算精度與其他兩種方法基本相當(dāng),攝站位置最大差值約為0.016 m,攝影姿態(tài)最大差值約為0.001°= 3.6″,滿足測量要求,表明本文推導(dǎo)的公式正確可靠,新算法可以用于空間后方交會解算。

(2)當(dāng)姿態(tài)角較小,即近似垂直攝影時(shí),本文算法和其他兩種方法均能正確求解。

(3)對于各種航高下的大傾角像片,本文算法依然能夠正確求解,即算法對各種攝影姿態(tài)具有很好的適應(yīng)性。傳統(tǒng)迭代算法不能求解,主要是因?yàn)閭鹘y(tǒng)迭代算法對共線條件方程線進(jìn)行線性化時(shí)需要良好的初值,當(dāng)初值不好時(shí),迭代不收斂,不能求解。本文算法無需對共線條件方程進(jìn)行線性化,是一種非線性方程的迭代方法,對外方位元素初值沒有任何要求,并在理論上證明算法的全局收斂性,即對初值的無依賴性,給定任意初值均能正確求解。

(4)從模擬數(shù)據(jù)解算結(jié)果來看,本文算法的求解精度非常高,而對實(shí)際數(shù)據(jù)的解算精度稍低,主要原因是受控制點(diǎn)地面坐標(biāo)和像點(diǎn)坐標(biāo)的量測誤差影響。因此,從應(yīng)用的角度來考慮,需要進(jìn)一步研究全局收斂條件下的抗差空間后方交會和多航帶多影像的區(qū)域網(wǎng)平差。特別是在區(qū)域網(wǎng)平差時(shí),未知數(shù)增多,情況變復(fù)雜,全局收斂性的確保成關(guān)鍵。事實(shí)上本文構(gòu)造的誤差函數(shù)在未知數(shù)數(shù)量增多時(shí),求導(dǎo)后仍然可以變換為式(13)的表達(dá)形式,因此可以確保區(qū)域網(wǎng)平差時(shí)算法的全局收斂性。這也是本文算法的優(yōu)勢所在。

5 結(jié)束語

空間后方交會具有十分重要的作用,通過嚴(yán)密的數(shù)學(xué)推導(dǎo),提出一種基于四元數(shù)的空間后方交會全局收斂算法。該算法主要有兩個(gè)特點(diǎn):一是將傳統(tǒng)的中心投影共線條件方程用絕對定向和正交投影兩種變換公式來代替,同時(shí)通過四元數(shù)對攝影姿態(tài)進(jìn)行描述,得出非線性方程的直接迭代解算方法,從而無需對傳統(tǒng)共線條件方程進(jìn)行線性化;二是全局收斂算法,即算法可以擺脫傳統(tǒng)方法對外方位元素初值的依賴,任意給定初值,都可以求解出正確結(jié)果,并從理論上對算法的全局收斂性進(jìn)行了證明,從而徹底解決了初值問題。當(dāng)前,隨著無人機(jī)、無人飛艇等的應(yīng)用,傳感器平臺日益增多。由于這些平臺因其自身結(jié)構(gòu)的特點(diǎn)或任務(wù)的特殊性,很難保證近似垂直攝影條件,如抗震救災(zāi)等應(yīng)急響應(yīng)時(shí)。因此其獲取的影像往往不能得到良好的位置和姿態(tài)初值,對這些平臺獲取的大傾角影像進(jìn)行后方交會將是具有挑戰(zhàn)性的工作。本文提出的全局收斂算法可以彌補(bǔ)傳統(tǒng)的迭代解法不能進(jìn)行大傾角后方交會的不足,具有很好的應(yīng)用潛力。

[1] WANG Zhizhuo.The Principles of Photogrammetry[M]. Beijing:Press of Surveying and Mapping,1990.

[2] J I Qiang,MAURO S C,ROBERT M H,et al.A Robust Linear Least-squares Estimation of Camera Exterior Orientation Using Multiple Geomatric Features[J].ISPRS Journal of Photogrammetry&Remote Sensing.2000,55:75-93.

[3] MENTHON D,DAVIS L S.Exact and Approximate Solutions of the Perspective Three Point Problem[J].IEEE Transactions on PAMI,1992,14(11):1100-1105.

[4] QUAN Long,LAN Zhongdan.Linear N-point Camera Pose Determination[J].IEEE Transactions on PAMI, 1999,21(7):1-7.

[5] LIU M L,WONG K H.Pose Estimation Using Four Corresponding Points[J].Pattern Recogn Lett.1999, 20(1):69-74.

[6] FIORE P D.Efficient Linear Solution of Exterior Orientation[J].IEEE Transactions on PAMI,2001,23(2): 140-148.

[7] ANSAR A,DANIILIDIS K.Linear Pose Estimation from Points or Lines[J].IEEE Transactions on PAMI,2003, 25(5):578-589.

[8] J IANG Gangwu,J IANG Ting,WANG Yong,et al.Space Resection Independent of InitialValue Based on Unit Quaternions[J].Acta Geodaetica et Cartographica Sinica, 2007,36(2):169-175.(江剛武,姜挺,王勇,等.基于單位四元數(shù)的無初值依賴空間后方交會[J].測繪學(xué)報(bào), 2007,36(2):169-175.)

[9] WANG Yong,J IANG Ting,JIANG Gangwu,et al.Space Resection of Single Image Based on the Description of Unit Quaternions[J].Journal of Zhenzhou Institute of Surveying and Mapping.2007,24(2):133-135.(王勇,姜挺,江剛武,等.基于單位四元數(shù)描述的單像空間后方交會[J].測繪科學(xué)技術(shù)學(xué)報(bào),2007,24(2):133-135.)

[10] GUAN Yunlan,CHENG Xiaojun,ZHOU Shijian,et al. A Solution to Space Resection Based on Unit Quaternion [J].Acta Geodaetica et Cartographica Sinica,2008,37 (1):30-35.(官云蘭,程效軍,周世健,等.基于單位四元數(shù)的空間后方交會解算[J],測繪學(xué)報(bào),2008,37(1): 30-35.)

[11] LU CHIENPING,HAGER G D,MJOLSNESS E.Fast and Globally ConvergentPose Estimation from Video Images[J].IEEE Transactions on PAMI,2000,22(6): 610-622.

[12] SCHWEIGHOFER G,PINZ A.Fast and Globally Convergent Structure and Motion Estimation forGeneralCamera Models[J].Proc of 17th British Machine Vision Conference.2006:147-156.

[13] HORN K P.A Closed-form Solution of Absolution Orientation Using Unit Quaternion[J].Journal of the Optical Society of America,1987,4(4):629-642.

[14] KEN S.Animating Rotation with Quaternion Curves[J]. Siggraph,1998,19(3):245-254.

[15] HORN K P.Closed-form Solution of Absolution Orientation Using Orthonormal Matrices[J].Journal of the Optical Society of America:Series A,1988,5(7): 1127-1135.

[16] LUENBERGER D G.Linear and Nonlinear Programming [M].3rd ed.Berlin:Springer,2008.