基于攝像機(jī)標(biāo)定方法的無(wú)人機(jī)位置解算

成 怡，崔利云

（天津工業(yè)大學(xué)電氣工程與自動(dòng)化學(xué)院，天津 300387）

近年來(lái)無(wú)人機(jī)的研究無(wú)論在軍用和民用領(lǐng)域都越來(lái)越廣泛.傳統(tǒng)的無(wú)人機(jī)導(dǎo)航多采用GPS和慣性導(dǎo)航方式.GPS導(dǎo)航受到信號(hào)的限制，在飛行任務(wù)隱蔽或室內(nèi)飛行的情況下無(wú)法使用；只采用慣性導(dǎo)航技術(shù)，其導(dǎo)航誤差會(huì)隨著時(shí)間的增加而積累，長(zhǎng)時(shí)間飛行后需要校正載體位置.因此，需要尋求其它無(wú)源的輔助導(dǎo)航技術(shù).視覺導(dǎo)航是一種基于計(jì)算機(jī)視覺的導(dǎo)航方式，近年來(lái)由于圖像處理技術(shù)和攝像機(jī)[1]硬件的發(fā)展，視覺導(dǎo)航的精度有所提高，而且具有成本低、信息量豐富、體積小、抗電磁干擾能力強(qiáng)、自主性好等優(yōu)點(diǎn).視覺導(dǎo)航[2]根據(jù)載體上相機(jī)實(shí)時(shí)拍攝的圖像來(lái)計(jì)算載體的運(yùn)動(dòng)參數(shù)，從而進(jìn)行載體的導(dǎo)航和控制.文獻(xiàn)[3]利用已知跑道的圖像信息確定載體位置，需要對(duì)跑道進(jìn)行識(shí)別及特征提取，會(huì)有較大誤差以及滯后，對(duì)導(dǎo)航實(shí)時(shí)性產(chǎn)生影響.本文基于攝像機(jī)標(biāo)定進(jìn)行無(wú)人機(jī)位置的解算.以四軸飛行器為飛行載體，四軸飛行器由4個(gè)十字交叉結(jié)構(gòu)的電機(jī)驅(qū)動(dòng)，具有良好的飛行穩(wěn)定性.通過(guò)對(duì)飛行器所載攝像機(jī)進(jìn)行標(biāo)定，從而得到所拍攝每幅圖像的信息，進(jìn)而由坐標(biāo)系之間的變換關(guān)系解算出攝像機(jī)即飛行器的空間位置坐標(biāo)，解算過(guò)程更加簡(jiǎn)便，實(shí)時(shí)性更高.

1 解算原理

本文涉及的導(dǎo)航坐標(biāo)系定義如圖1所示.

圖1 導(dǎo)航坐標(biāo)示意圖Fig.1 Sketch map of navigation coordinate

圖1中：Ow系為世界坐標(biāo)系；Oc系為攝像機(jī)坐標(biāo)系.因?yàn)閿z像機(jī)懸掛于四軸飛行器的中心下端位置，所以攝像機(jī)坐標(biāo)系與載體坐標(biāo)系重合于Ob（如果不重合，它們之間存在轉(zhuǎn)換矩陣Pcb，可實(shí)現(xiàn)二者之間的轉(zhuǎn)換）.定義P（x，y）和I（U，V）分別為模板上特征點(diǎn)在相平面的坐標(biāo)和圖像坐標(biāo).

由針孔模型描述攝像機(jī)成像模型[4]，則點(diǎn)P在攝像機(jī)坐標(biāo)系下坐標(biāo)PC（x，y，z，1）與其成像點(diǎn)在圖像坐標(biāo)系下坐標(biāo)PI（u，v，1）之間的投影關(guān)系如式（1）所示：

點(diǎn)P的世界坐標(biāo)Pw（Xw，Yw，Zw，1）和攝像機(jī)坐標(biāo)PC（x，y，z，1）之間的轉(zhuǎn)換關(guān)系如式（2）所示：

式中：R，t分別為旋轉(zhuǎn)矩陣和平移矩陣；M2為攝像機(jī)外部參數(shù)矩陣，由攝像機(jī)位姿狀態(tài)決定.R和t描述世界坐標(biāo)系在攝像機(jī)坐標(biāo)系下的變換關(guān)系，而視覺導(dǎo)航確定載體的位置，需要攝像機(jī)坐標(biāo)系在世界坐標(biāo)系下的變換矩陣（Ω，Γ），因此，有式（3）所示的變換關(guān)系：

即通過(guò)攝像機(jī)標(biāo)定技術(shù)解算的M2矩陣中的R和t矩陣，分別為Ω-1和-Ω-1Γ，即可確定載體的位置坐標(biāo).

2 飛行器位置的解算

飛行器位置解算[5]的流程如圖2所示.機(jī)載攝像機(jī)拍攝至少2個(gè)不同位置（相對(duì)標(biāo)定模板）的模板圖像，模板選取棋盤格，尺寸為15×11，每小格的邊長(zhǎng)為20 cm.通過(guò)機(jī)載攝像機(jī)拍攝的照片序列解算式（1）和式（2）中的矩陣，并確定載體的位置.

圖2 飛行器位置解算流程圖Fig.2 Aircraft position calculation flow chart

載體在運(yùn)動(dòng)過(guò)程中拍攝一組圖片，利用VC++6.0及OpenCV提供的庫(kù)函數(shù)建立如圖2所述過(guò)程的程序，利用cvFindChessboardCorners（）和cvFindCorner－SubPix（）函數(shù)可以得到子像素級(jí)精度的角點(diǎn)位置坐標(biāo).其中2張圖片的角點(diǎn)檢測(cè)結(jié)果如圖3所示.

圖3 角點(diǎn)檢測(cè)結(jié)果Fig.3 Corner detection results

利用cvCalibrateCamera2（）函數(shù)，通過(guò)角點(diǎn)在世界坐標(biāo)系中的坐標(biāo)值以及在圖像坐標(biāo)系中的坐標(biāo)值，計(jì)算攝像機(jī)的內(nèi)參數(shù)矩陣、畸變系數(shù)向量、旋轉(zhuǎn)向量R、平移向量t.調(diào)用函數(shù)cvFindExtrinsicCameraParams2（）得到更精確的每幅圖片的外參旋轉(zhuǎn)向量R、平移向量t[6].由于該函數(shù)中得到的旋轉(zhuǎn)向量是旋轉(zhuǎn)矩陣的緊湊表示方式，先用函數(shù)cvRodrigues2（）將旋轉(zhuǎn)向量轉(zhuǎn)換成旋轉(zhuǎn)矩陣，再利用函數(shù)cvInvert（）對(duì)旋轉(zhuǎn)矩陣求逆，然后用函數(shù)cvGEMM（）使旋轉(zhuǎn)矩陣的逆矩陣乘上平移向量得到一個(gè)新的矩陣.將這個(gè)新矩陣的每個(gè)元素求反，即可得到所求解的攝像機(jī)的位置參數(shù)[7].

3 實(shí)驗(yàn)驗(yàn)證

本文利用四軸飛行器的機(jī)載相機(jī)進(jìn)行了大量實(shí)驗(yàn)，并在用機(jī)載相機(jī)拍攝圖像的同時(shí)對(duì)攝像機(jī)的空間位置進(jìn)行測(cè)量和記錄.表1所示為效果理想的15幅圖片的解算結(jié)果和測(cè)量結(jié)果.

將表1所示解算出的攝像機(jī)位置坐標(biāo)（x，y，z）以及實(shí)驗(yàn)中所測(cè)量的攝像機(jī)位置坐標(biāo)（x*，y*，z*）在Matlab中進(jìn)行分析對(duì)比，并將對(duì)比結(jié)果以圖形的形式輸出，如圖4所示，圓形位置為實(shí)驗(yàn)中的測(cè)量值（x*，y*，z*），五角星位置為解算值（x，y，z）.計(jì)算解算值與測(cè)量值的誤差絕對(duì)值，如表2所示.

由圖4、表1和表2可以看出，解算值與測(cè)量值基本吻合，且在忽略測(cè)量誤差的情況下該算法的解算誤差最大不超過(guò)1 mm，平均誤差為0.5 mm.由此說(shuō)明，本文方法可以較精確地解算出攝像機(jī)位置，若將攝像機(jī)安置在四軸飛行器的下端便可實(shí)時(shí)地得到飛行器位置，為飛行器的導(dǎo)航及著陸提供有用信息[8].

圖4 攝像機(jī)空間坐標(biāo)解算值與測(cè)量值的對(duì)比Fig.4 Camera coordinate calculation value compared with measuring value

表2 攝像機(jī)空間坐標(biāo)解算值誤差Tab.2 Error of camera coordinate calculation valuemm

4 結(jié)束語(yǔ)

本文中攝像機(jī)位置的解算可以應(yīng)用到飛行器上用來(lái)實(shí)現(xiàn)飛行器的導(dǎo)航，但導(dǎo)航所需的信息要求有很高的精確性和實(shí)時(shí)性，本文方法及程序?qū)崿F(xiàn)可以滿足一定的實(shí)時(shí)性，但其精度由于角點(diǎn)檢測(cè)精度的限制還有待提高.經(jīng)過(guò)多次實(shí)驗(yàn)發(fā)現(xiàn)，采用OpenCV對(duì)拍攝角度變化太大時(shí)拍攝到的圖片進(jìn)行角點(diǎn)檢測(cè)時(shí)會(huì)有檢測(cè)失敗的點(diǎn)，無(wú)法從圖像中提取到所需數(shù)據(jù)進(jìn)行解算，這就需要攝像機(jī)拍攝過(guò)程中能截取足夠的有效圖片進(jìn)行解算.綜上所述，在保證獲取足夠有效圖像的情況下，本文方法可以實(shí)現(xiàn)飛行器的位置解算.

[1] ZHANG Zhengyou.Flexible camera calibration by viewing a plane from unknown orientations[C]//Proceedings Seventh International Conference on Computer Vision.Liege：Elsevier Science Publishers，1999：666-674.

[2] 沈?qū)?，唐大全，李飛.無(wú)人機(jī)視覺導(dǎo)航中的圖像處理及位姿解算[J].光電技術(shù)應(yīng)用，2012，6（27）：60-64.

[3] 馬頌德，張正友.計(jì)算機(jī)視覺—理論與算法基礎(chǔ)[M].北京：北京科學(xué)出版社，1998.

[4]BRADSKIGary，KAEHLERAdrian.學(xué)習(xí)OpenCV中文版[M].于仕琪，劉瑞禎，譯.北京：清華大學(xué)出版社，2009.

[5]吳健.移動(dòng)機(jī)器人視覺導(dǎo)航系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].成都：西南科技大學(xué)，2007.

[6] 唐忞旻，吳向前.基于機(jī)器視覺的導(dǎo)航定位技術(shù)[J].湖南農(nóng)機(jī)，2011，38（11）：68-70.

[7] 潘翔，馬德強(qiáng)，吳貽軍，等.基于視覺著陸的無(wú)人機(jī)俯仰角與高度估計(jì)[J].浙江大學(xué)學(xué)報(bào)：工學(xué)版，2009，43（4）：692-696.

[8] 崔紅超，雷鳴.基于四元數(shù)視覺導(dǎo)航算法確定目標(biāo)位姿[J].測(cè)繪與空間地理信息，2013，36（1）：131-133.