專利名稱：基于視覺(jué)信息的無(wú)人飛行器自主著陸的姿態(tài)參數(shù)估算方法
技術(shù)領(lǐng)域：
本發(fā)明涉及無(wú)人飛行器技術(shù)領(lǐng)域，尤其是一種無(wú)人飛行器
(Unmanned Aerial Vehicle, UAV)導(dǎo)航技術(shù)，側(cè)重視覺(jué)信息的UAV 姿態(tài)參數(shù)估計(jì)。
背景技術(shù)：
無(wú)人飛行器用于航空攝影，災(zāi)情監(jiān)測(cè)以及地球物理探礦等，有著 廣泛的應(yīng)用前景。UAV自主著陸是指UAV依賴機(jī)載的導(dǎo)航設(shè)備和飛 行控制系統(tǒng)來(lái)進(jìn)行定位導(dǎo)航并最終控制UAV降落在著陸場(chǎng)的過(guò)程。 要想實(shí)現(xiàn)自主著陸，UAV必須具備自主導(dǎo)航能力，即需要實(shí)時(shí)估計(jì) 其姿態(tài)參數(shù)以用于自主著陸導(dǎo)航。
目前國(guó)內(nèi)外研究的用于UAV自主著陸的導(dǎo)航技術(shù)包括慣性導(dǎo) 航系統(tǒng)(INS)、 GPS導(dǎo)航系統(tǒng)和INS/GPS組合導(dǎo)航系統(tǒng)。其中，慣 性導(dǎo)航是最早最成熟的導(dǎo)航技術(shù)，它是利用陀螺，加速度機(jī)等慣性元 器件感受UAV在運(yùn)動(dòng)過(guò)程中的加速度，然后通過(guò)積分計(jì)算，得到機(jī) 體大概位置與速度等導(dǎo)航參數(shù)，它最大的缺點(diǎn)是誤差會(huì)隨時(shí)間的推移 而不斷累加；GPS應(yīng)用最為廣泛，技術(shù)也相對(duì)成熟，它利用衛(wèi)星進(jìn)行 導(dǎo)航定位，具有精度高，使用簡(jiǎn)單等優(yōu)點(diǎn)，但在山區(qū)等特殊地方易阻 塞。對(duì)于小型UAV，由于負(fù)荷有限，視覺(jué)導(dǎo)航系統(tǒng)是理想選擇，一 方面其體積小，另一方面功耗低，同時(shí)對(duì)著陸地形有直觀的感知。
對(duì)于UAV，視覺(jué)導(dǎo)航系統(tǒng)側(cè)重飛行路徑的規(guī)劃、障礙物的檢測(cè) 等方面，并且多數(shù)算法基于單目視覺(jué)信息的感知與理解。隨著機(jī)器視
5覺(jué)的發(fā)展，立體視覺(jué)的研究成果逐漸用于工業(yè)智能化領(lǐng)域。雙目立體
視覺(jué)用于UAV處于起步階段。

發(fā)明內(nèi)容
為了克服已有無(wú)人飛行器的導(dǎo)航系統(tǒng)的可靠性差的不足，本發(fā)明 提供一種有效提高可靠性的基于視覺(jué)信息的無(wú)人飛行器自主著陸的姿 態(tài)參數(shù)估算方法。
為解決上述技術(shù)問(wèn)題的技術(shù)方案是
一種基于視覺(jué)信息的無(wú)人飛行器自主著陸姿態(tài)參數(shù)估算方法，所 述估算方法包括以下步驟
1) 、俯仰角估計(jì)
設(shè)置雙目攝像機(jī)與無(wú)人飛行器的機(jī)體平行，攝像機(jī)坐標(biāo)系與無(wú)人 飛行器的坐標(biāo)系重合，假設(shè)無(wú)人飛行器沿著直線下滑，無(wú)人飛行器位 于在離地面高/Z'的位置，其飛行方向與地面成"角，俯仰角"通過(guò)公 式(l)求得
<formula>formula see original document page 6</formula>(1)
上式中，力為攝像機(jī)的焦距，//為FOVC點(diǎn)和C點(diǎn)之間的距離，丄為 FOE點(diǎn)和FOVC點(diǎn)之間的距離，F(xiàn)OVC是攝像機(jī)視野區(qū)EF的中心， FOE為膨脹中心，C點(diǎn)是像平面內(nèi)FOE點(diǎn)和FOVC點(diǎn)連線與平面B 的滅線的交點(diǎn)；
2) 深度估計(jì)
采用雙目立體視覺(jué)方法測(cè)量攝像機(jī)到地面特征點(diǎn)的深度，從雙目 圖像中找到匹配點(diǎn)，形成視差圖，根據(jù)公式(2)求出深度參數(shù)其中，K是深度，6是兩個(gè)攝像機(jī)之間的基線長(zhǎng)度，乂是攝像機(jī)的焦距， D是雙目視差。
作為優(yōu)選的一種方案所述估算方法還包括以下步驟 3)高度估計(jì)
在攝像機(jī)坐標(biāo)系下獲得相關(guān)參數(shù)，通過(guò)坐標(biāo)變換得到UAV相對(duì) 地面的高度信息，從攝像機(jī)坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換關(guān)系見(jiàn)公式
(3):<formula>formula see original document page 7</formula>其中，"是UAV的俯仰角，、義A是UAV在攝像機(jī)坐標(biāo)系下的位置， 、,i人是UAV在世界坐標(biāo)系下的位置，所求的UAV高度i/'即公式中 的V
//'=zw =ycxsina + zcxcosa (4)
利用基于特征點(diǎn)的匹配方法形成稀疏的視差圖，就由公式(2)求 得UAV相對(duì)各特征點(diǎn)深度信息，再通過(guò)公式(4)將每個(gè)特征點(diǎn)求得 的高度進(jìn)行統(tǒng)計(jì)平均，獲得該時(shí)刻UAV的高度參數(shù)。
作為優(yōu)選的另一種方案在所述步驟2)中，利用Kalman濾波器
提高高度參數(shù)估計(jì)精度的過(guò)程為-
設(shè)UAV運(yùn)動(dòng)為直線運(yùn)動(dòng)，定義狀態(tài)矢量為
凡-Cyp("),凡(")Ja(")] (5)
式中力(")為第"個(gè)采樣時(shí)刻UAV的高度，;^W為第"個(gè)采樣時(shí)刻UAV 的速度，而凡(")為第M個(gè)采樣時(shí)刻UAV的加速度；
根據(jù)牛頓運(yùn)動(dòng)定理，分別建立系統(tǒng)模型和測(cè)量模型，系統(tǒng)模型、t t2/:
4= 0 1 t 0 0 1
(6)
式中，下標(biāo)"為第"個(gè)采樣時(shí)刻，凡為第n個(gè)采樣時(shí)刻的狀態(tài)矢量，
^為狀態(tài)轉(zhuǎn)移矩陣，r為采樣時(shí)間間隔，^為第"個(gè)采樣時(shí)刻的系統(tǒng)噪
聲矢量；
測(cè)量模型. A=/^"+V" (7)-
式中、為第"個(gè)采樣時(shí)刻的測(cè)量值，即UAV的高度，^為輸出矩陣，
""為第"個(gè)采樣時(shí)刻的測(cè)量噪聲矢量。
本發(fā)明的技術(shù)效果基于視覺(jué)信息的感知與理解為無(wú)人飛行器提 供自主著陸所需的導(dǎo)航信息。假設(shè)無(wú)人飛行器沿著直線下滑，對(duì)于單 目視覺(jué)信息，基于時(shí)間處理計(jì)算運(yùn)動(dòng)矢量、估計(jì)膨脹中心，根據(jù)幾何 滅點(diǎn)模型估計(jì)俯仰角。利用雙目立體視覺(jué)信息，基于特征點(diǎn)匹配形成 稀疏的視差圖，進(jìn)一步估計(jì)UAV到地面特征點(diǎn)的深度信息，結(jié)合俯仰 角參數(shù)，通過(guò)攝像機(jī)坐標(biāo)到世界坐標(biāo)的轉(zhuǎn)換獲得UAV相對(duì)地面的高度。 建立無(wú)人飛行器運(yùn)動(dòng)模型，基于Kalman濾波進(jìn)一步提高UAV高度參數(shù) 估計(jì)精度。


圖1是UAV俯仰角測(cè)量模型圖。
圖2是RANSAC算法擬合滅點(diǎn)圖。
圖3是立體視覺(jué)模型示意圖。
圖4是俯仰角估計(jì)結(jié)果的示意圖。
圖5是UAV高度估計(jì)結(jié)果的示意圖。
具體實(shí)施方式
下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步描述。
參照?qǐng)Dl一圖5，基于視覺(jué)信息的無(wú)人飛行器自主著陸的姿態(tài)參數(shù) 估計(jì)方法具體實(shí)現(xiàn)包括以下部分 l.俯仰角估計(jì)
設(shè)置雙目攝像機(jī)與無(wú)人飛行器的機(jī)體平行，攝像機(jī)坐標(biāo)系與無(wú)人 飛行器的坐標(biāo)系重合，假設(shè)無(wú)人飛行器沿著直線下滑，無(wú)人飛行器位 于在離地面高/Z'的位置，其飛行方向與地面成"角，參考圖1和圖2。
俯仰角"通過(guò)公式(l)求得
a = tan--tan — = tan -
、 /P +〃丄 (1)
上式中，人為攝像機(jī)的焦距，//為FOVC點(diǎn)和C點(diǎn)之間的距離，Z為 FOE點(diǎn)和FOVC點(diǎn)之間的距離，F(xiàn)OVC是攝像機(jī)視野區(qū)EF的中心， FOE為膨脹中心，C點(diǎn)是像平面內(nèi)FOE點(diǎn)和FOVC點(diǎn)連線與平面B 的滅線的交點(diǎn)；
因此，只要確定了￡、 //和/p就可以得到俯仰角，/,可以通過(guò)攝 像機(jī)標(biāo)定得到，得到和丄的關(guān)鍵是提取出滅線和膨脹中心。
根據(jù)透視學(xué)理論，水平面的滅線即為視平線。UAV下降俯沖的過(guò) 程中如果沒(méi)有左右搖晃，而且兩臺(tái)攝像機(jī)被固定在同一水平面上，則 兩臺(tái)攝像機(jī)的光心距地面的高度(視高)是相同的，那么兩臺(tái)攝像機(jī) 的視平線共線；水平面上任一平行直線投影在像平面后形成的滅點(diǎn)落 在視平線上。因此，可以分別提取左右圖中的滅點(diǎn)來(lái)確定滅線。 一組 平行線具有相同的滅點(diǎn)，相互平行的道路邊界和斑馬線在2D圖像中 交于一個(gè)滅點(diǎn)，通過(guò)改進(jìn)型的Hough變換提取道路中的邊緣，利用 RANSAC (RANdom SAmple Consensus)算法擬合出魯棒性高的滅點(diǎn)，如圖2所示。
根據(jù)運(yùn)動(dòng)視覺(jué)理論，當(dāng)人們經(jīng)過(guò)靜止景物時(shí)，景物在視網(wǎng)膜上的 投影好像向后流動(dòng)，事實(shí)上，對(duì)給定的平移運(yùn)動(dòng)方向和給定的注視方 向來(lái)說(shuō)，景物的視覺(jué)投影似乎是從一個(gè)特定的視網(wǎng)膜點(diǎn)流出來(lái)，這個(gè)
點(diǎn)就是膨脹中心FOE。由于固定翼無(wú)人機(jī)在下降過(guò)程中速度很快，位 移較大，選用特征點(diǎn)的運(yùn)動(dòng)矢量法計(jì)算FOE。
2. 深度估計(jì)
采用雙目立體視覺(jué)方法測(cè)量攝像機(jī)到地面特征點(diǎn)的深度，從雙目
圖像中找到匹配點(diǎn)，形成稀疏的視差圖，根據(jù)公式(2)求出深度參數(shù)
K《/Z) (2)
其中，K是深度，6是兩個(gè)攝像機(jī)之間的基線長(zhǎng)度，義是攝像機(jī)的焦距， D是雙目視差，且" '表示p,與f之間的距離，V表示^與
尸f之間的距離，見(jiàn)圖3。
3. 高度估計(jì)
在攝像機(jī)坐標(biāo)系下獲得相關(guān)參數(shù)，需要進(jìn)一步通過(guò)坐標(biāo)變換，得 到UAV相對(duì)地面的高度信息。從攝像機(jī)坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換 關(guān)系見(jiàn)公式(3)。
—100 _一(
0cos a一sino;
0sincecos a陽(yáng)4_
其中，a是UAV的俯仰角，A,^^是UAV在攝像機(jī)坐標(biāo)系下的位置， ^,凡,，；是UAV在世界坐標(biāo)系下的位置。因此，所求的UAV高度W'即 公式中的；
i/'=zw =>>cxsina + zcxcosa (4)利用基于特征點(diǎn)的匹配方法形成稀疏的視差圖，就可由公式(2)
求得UAV相對(duì)各特征點(diǎn)深度信息，進(jìn)一步通過(guò)公式(4)將每個(gè)特征
點(diǎn)求得的高度進(jìn)行統(tǒng)計(jì)平均，獲得該時(shí)刻UAV的高度參數(shù)。
4.利用Kalman濾波器提高高度參數(shù)估計(jì)精度 設(shè)UAV運(yùn)動(dòng)為直線運(yùn)動(dòng)，定義狀態(tài)矢量為
V"=[力("),K ("),少a (")]T (5)
式中yp(")為第n個(gè)采樣時(shí)刻UAV的高度，乂(")為第n個(gè)采樣時(shí)刻
UAV的速度，而凡(")為第"個(gè)采樣時(shí)刻UAV的加速度。根據(jù)牛頓運(yùn) 動(dòng)定理，分別建立系統(tǒng)模型和測(cè)量模型。
乂，=々"-i+w"
4 =
1 T T2/2 0 1 T 0 0 1
(6)
式中下標(biāo)"為第"個(gè)采樣時(shí)刻，凡為第"個(gè)采樣時(shí)刻的狀態(tài)矢量，^為
狀態(tài)轉(zhuǎn)移矩陣，r為采樣時(shí)間間隔，w,，為第n個(gè)采樣時(shí)刻的系統(tǒng)噪聲
(7)
x =取+v 好=[1, 0, 0]
式中x"為第"個(gè)采樣時(shí)刻的測(cè)量值，即UAV的高度，i/為輸出矩 陣，v"為第"個(gè)采樣時(shí)刻的測(cè)量噪聲矢量。
模擬UAV著陸的仿真實(shí)驗(yàn)平臺(tái)模型，實(shí)際實(shí)驗(yàn)構(gòu)架由雙目攝像機(jī) (JVC1481)，時(shí)鐘同步卡(GL202)，圖像采集卡(Daheng CQ300)，位 移臺(tái)和主機(jī)(主頻p4 3.06GHz，內(nèi)存512M)組成。將攝像機(jī)固定在位移臺(tái)上，使攝像機(jī)從位移臺(tái)的頂端平移至底端，模擬UAV著陸時(shí)攝像 機(jī)的運(yùn)動(dòng)，在下滑過(guò)程中，利用雙目視覺(jué)系統(tǒng)采集道路視頻數(shù)據(jù)。
圖4為姿態(tài)角估計(jì)結(jié)果，橫坐標(biāo)的幀數(shù)與時(shí)間對(duì)應(yīng)，從中可以看
出算法估計(jì)的結(jié)果與真實(shí)值24.5。相吻合。平均誤差為0.66。。圖5是 對(duì)高度估計(jì)的仿真結(jié)果，虛線為Harris特征點(diǎn)匹配的結(jié)果，實(shí)線為 Kalman濾波器濾波后結(jié)果，濾波后平均誤差1. 57cm，可以看出，由 于部分特征點(diǎn)失配和系統(tǒng)誤差引起的高度偏差已被Kalman濾波器有 效的抑制了。
無(wú)人直升機(jī)實(shí)驗(yàn)平臺(tái)攜帶嵌入式立體視覺(jué)數(shù)據(jù)采集設(shè)備。在飛行 過(guò)程中，圖像數(shù)據(jù)以5幀/秒的速率存入CF卡中。由于無(wú)人機(jī)航模本 身的局限性，在飛行過(guò)程中直升機(jī)基本處于水平運(yùn)動(dòng)，其俯仰角可視 為0°，因此，只對(duì)直升機(jī)的高度參數(shù)進(jìn)行估計(jì)。在這樣的前提條件下， 本發(fā)明開(kāi)發(fā)的算法基于直升機(jī)空中實(shí)拍圖像序列估計(jì)出直升機(jī)的高度 值為10.848m。直升機(jī)實(shí)際飛行高度在10m到llm米之間，因此，本
實(shí)施例開(kāi)發(fā)的算法是有效的。
1權(quán)利要求
1、一種基于視覺(jué)信息的無(wú)人飛行器自主著陸姿態(tài)參數(shù)估算方法，其特征在于所述估算方法包括以下步驟1)俯仰角估計(jì)設(shè)置雙目攝像機(jī)與無(wú)人飛行器的機(jī)體平行，攝像機(jī)坐標(biāo)系與無(wú)人飛行器的坐標(biāo)系重合，假設(shè)無(wú)人飛行器沿著直線下滑，無(wú)人飛行器位于在離地面高H′的位置，其飛行方向與地面成α角，俯仰角α通過(guò)公式(1)求得上式中，fp為攝像機(jī)的焦距，H為FOVC點(diǎn)和C點(diǎn)之間的距離，L為FOE點(diǎn)和FOVC點(diǎn)之間的距離，F(xiàn)OVC是攝像機(jī)視野區(qū)EF的中心，F(xiàn)OE為膨脹中心，C點(diǎn)是像平面內(nèi)FOE點(diǎn)和FOVC點(diǎn)連線與平面B的滅線的交點(diǎn)；2)深度估計(jì)采用雙目立體視覺(jué)方法測(cè)量攝像機(jī)到地面特征點(diǎn)的深度，從雙目圖像中找到匹配點(diǎn)，形成視差圖，根據(jù)公式(2)求出深度參數(shù)Yc＝bfc/D (2)其中，Yc是深度，b是兩個(gè)攝像機(jī)之間的基線長(zhǎng)度，fc是攝像機(jī)的焦距，D是雙目視差。
2、 如權(quán)利要求1所述的基于視覺(jué)信息的無(wú)人飛行器自主著陸姿態(tài)參數(shù)估算方法，其特征在于所述估算方法還包括以下步驟3) 高度估計(jì)在攝像機(jī)坐標(biāo)系下獲得相關(guān)參數(shù)，通過(guò)坐標(biāo)變換得到UAV相對(duì) 地面的高度信息，從攝像機(jī)坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換關(guān)系見(jiàn)公式 (3):<formula>formula see original document page 3</formula>其中，"是UAV的俯仰角，^KA是UAV在攝像機(jī)坐標(biāo)系下的位置， 、,尺,，、是UAV在世界坐標(biāo)系下的位置，所求的UAV高度/f'即公式中 的V//'=zw "cXsin" + ZcXcosa (4)利用基于特征點(diǎn)的匹配方法形成稀疏的視差圖，就由公式(2)求得UAV相對(duì)各特征點(diǎn)深度信息，再通過(guò)公式(4)將每個(gè)特征點(diǎn)求得的高度進(jìn)行統(tǒng)計(jì)平均，獲得該時(shí)刻UAV的高度參數(shù)。
3、如權(quán)利要求1或2所述的基于視覺(jué)信息的無(wú)人飛行器自主著陸姿態(tài)參數(shù)估算方法，其特征在于在所述步驟2)中，利用Kalman濾波器提高高度參數(shù)估計(jì)精度的過(guò)程為設(shè)UAV運(yùn)動(dòng)為直線運(yùn)動(dòng)，定義狀態(tài)矢量為凡=[力("),yv (")， _ya (")]T (5)式中力(")為第"個(gè)采樣時(shí)刻UAV的高度，凡(")為第"個(gè)采樣時(shí)刻UAV的速度，而凡(")為第"個(gè)采樣時(shí)刻UAV的加速度；根據(jù)牛頓運(yùn)動(dòng)定理，分別建立系統(tǒng)模型和測(cè)量模型，系統(tǒng)模型 凡=4)^+^—1 T T2/2— v4= 0 1 T 0 0 1式中，下標(biāo)"為第"個(gè)釆樣時(shí)刻，凡為第"個(gè)采樣時(shí)刻的狀態(tài)矢量，^為狀態(tài)轉(zhuǎn)移矩陣，r為采樣時(shí)間間隔，化為第"個(gè)采樣時(shí)刻的系統(tǒng)噪(6)(7);聲矢量；fl"-[i， o, 0]式中x"為第w個(gè)采樣時(shí)刻的測(cè)量值，即UAV的高度，H為輸出矩陣， v 為第n個(gè)采樣時(shí)刻的測(cè)量噪聲矢量。
全文摘要
一種基于視覺(jué)信息的無(wú)人飛行器自主著陸的姿態(tài)參數(shù)估算方法，包括以下步驟1)假設(shè)無(wú)人飛行器沿著直線下滑，對(duì)于單目視覺(jué)信息，基于時(shí)間處理計(jì)算運(yùn)動(dòng)矢量、估計(jì)膨脹中心，根據(jù)幾何滅點(diǎn)模型估計(jì)俯仰角；2)利用雙目立體視覺(jué)信息，基于特征點(diǎn)匹配形成稀疏的視差圖，進(jìn)一步估計(jì)UAV到地面特征點(diǎn)的深度信息，結(jié)合俯仰角參數(shù)，通過(guò)攝像機(jī)坐標(biāo)到世界坐標(biāo)的轉(zhuǎn)換獲得UAV相對(duì)地面的高度；3)建立無(wú)人飛行器運(yùn)動(dòng)模型，基于Kalman濾波進(jìn)一步提高UAV高度參數(shù)估計(jì)精度。本發(fā)明提供一種有效提高可靠性的基于視覺(jué)信息的無(wú)人飛行器自主著陸的姿態(tài)參數(shù)估算方法。
文檔編號(hào)G01C21/20GK101504287SQ20091009578
公開(kāi)日2009年8月12日 申請(qǐng)日期2009年1月22日 優(yōu)先權(quán)日2009年1月22日
發(fā)明者吳貽軍, 姜哲圣, 翔 潘, 童丸丸, 馬德強(qiáng) 申請(qǐng)人:浙江大學(xué)