方登建，戴宇進(jìn)，趙紅超，康宇航

（海軍航空工程學(xué)院a.7系；b.研究生管理大隊(duì)，山東煙臺(tái)264001）

四旋翼無(wú)人飛行器的室內(nèi)定位、導(dǎo)航與建圖

方登建a，戴宇進(jìn)a，趙紅超a，康宇航b

（海軍航空工程學(xué)院a.7系；b.研究生管理大隊(duì)，山東煙臺(tái)264001）

根據(jù)室內(nèi)環(huán)境半結(jié)構(gòu)化的特點(diǎn)，通過(guò)激光掃描儀采集的數(shù)據(jù)得到特征點(diǎn)與特征線段，從而建立出室內(nèi)環(huán)境的特征地圖。利用特征點(diǎn)實(shí)現(xiàn)四旋翼無(wú)人飛行器在室內(nèi)對(duì)自身的定位，同時(shí)根據(jù)現(xiàn)階段特征點(diǎn)的個(gè)數(shù)制定導(dǎo)航算法，當(dāng)四旋翼無(wú)人飛行器飛到下一位置時(shí)，再次建立局部地圖，與此前建立的全局地圖進(jìn)行融合，實(shí)現(xiàn)全局地圖的更新。

四旋翼無(wú)人飛行器；定位；導(dǎo)航；建圖

四旋翼無(wú)人飛行器通過(guò)自身攜帶的激光測(cè)距儀獲得室內(nèi)未知環(huán)境中的距離信息，增量式地構(gòu)建室內(nèi)地圖，同時(shí)利用所構(gòu)建的室內(nèi)地圖實(shí)現(xiàn)定位，并用所獲得的距離信息與所構(gòu)建的室內(nèi)地圖進(jìn)行導(dǎo)航。該研究始于上世紀(jì)80年代中期[1]，目前已有多種解決此問(wèn)題的方法，主要包括非概率方法和基于概率估計(jì)方法，其中基于概率估計(jì)方法為主流[2-3]。

本文針對(duì)室內(nèi)半結(jié)構(gòu)化環(huán)境特征線段與特征點(diǎn)比較多的特點(diǎn)，制定了基于特征地圖的定位、導(dǎo)航與建圖方法。

依靠自身攜帶的激光掃描儀，四旋翼無(wú)人飛行器能夠?qū)⑹覂?nèi)環(huán)境中的數(shù)據(jù)采集；再通過(guò)特征檢測(cè)算法（加權(quán)最小二乘算法、哈夫變換、小波變換）將特征點(diǎn)與特征線段提取出來(lái)，根據(jù)特征點(diǎn)的個(gè)數(shù)確定導(dǎo)航點(diǎn)與導(dǎo)航策略，并根據(jù)特征線段制定相應(yīng)的避障策略；特征點(diǎn)與特征線段是幾何地圖的基本組成要素，利用提取出的特征點(diǎn)與特征線段可以將局部地圖創(chuàng)建出來(lái)，結(jié)合已經(jīng)建立起來(lái)的全局地圖進(jìn)行特征匹配，根據(jù)匹配后的結(jié)果將全局地圖進(jìn)行更新，實(shí)現(xiàn)對(duì)四旋翼無(wú)人飛行器的自定位，定位、導(dǎo)航與建圖算法結(jié)構(gòu)圖如圖1所示。

圖1 定位、導(dǎo)航與建圖算法結(jié)構(gòu)圖Fig.1 Algorithm structure of localization, navigation and mapping

1 環(huán)境地圖表示方法

四旋翼無(wú)人飛行器在未知環(huán)境中工作首先要解決定位問(wèn)題。而定位問(wèn)題與創(chuàng)建地圖是交織在一起的，這就是同步地圖創(chuàng)建與定位——SLAM問(wèn)題[4-6]。地圖創(chuàng)建的精確性將直接影響四旋翼無(wú)人飛行器定位的精度與無(wú)人飛行器后續(xù)導(dǎo)航工作的好壞。

目前環(huán)境地圖的表示方法主要有3種：柵格地圖、特征地圖和拓?fù)涞貓D。

柵格地圖是由Moravec和Elfes提出，將整個(gè)環(huán)境分成若干個(gè)大小相同的小柵格，根據(jù)每個(gè)小柵格中是否有障礙物來(lái)創(chuàng)建地圖。這一方法的優(yōu)點(diǎn)是：①地圖的創(chuàng)建與維護(hù)相對(duì)簡(jiǎn)單；②可以很方便地根據(jù)實(shí)際需求來(lái)更改地圖精度；③地圖模型比較簡(jiǎn)單，就是二維數(shù)組。缺點(diǎn)是：①當(dāng)未知環(huán)境比較大且地圖的精度要求比較高時(shí)，地圖的維護(hù)將會(huì)變得非常難；②當(dāng)柵格劃分比較細(xì)時(shí)，對(duì)處理器的要求非常高，實(shí)時(shí)性一般不好。

幾何地圖是根據(jù)機(jī)載傳感器采集的數(shù)據(jù)，從中提取抽象的幾何特征，比如線段、點(diǎn)或者圓，通過(guò)這些幾何特征來(lái)描述環(huán)境。這一方法的優(yōu)點(diǎn)是：①信息量小，便于存儲(chǔ)；②實(shí)時(shí)性比較高，對(duì)處理器要求也沒(méi)有那么高；③可以將多種傳感器的數(shù)據(jù)融合在一起使用。缺點(diǎn)是：①各種傳感器采集的數(shù)據(jù)需要處理后才能一起使用；②不適合太復(fù)雜的環(huán)境；③在小環(huán)境的地圖中，地圖的精度可能不夠。

拓?fù)涞貓D是將環(huán)境表示成一張拓?fù)鋱D，圖中的節(jié)點(diǎn)對(duì)應(yīng)于環(huán)境中的一個(gè)特征狀態(tài)，節(jié)點(diǎn)間的連線表示2節(jié)點(diǎn)直接是相通的。這一方法的優(yōu)點(diǎn)是：①比較適合做大范圍航路規(guī)劃的任務(wù)；②信息量最小，非常容易存儲(chǔ)；③對(duì)處理器要求最低，可以使用很多比較成熟的搜索算法。缺點(diǎn)是：①地圖精度最低；②容易出現(xiàn)誤匹配的情況；③在非結(jié)構(gòu)環(huán)境中，這種方法很可能失效。

文獻(xiàn)[7-8]采用柵格地圖對(duì)機(jī)器人進(jìn)行定位導(dǎo)航，雖然精度比較高，但是僅適合小環(huán)境。而本文針對(duì)的是面積適中的室內(nèi)環(huán)境，如果使用柵格地圖很難滿(mǎn)足實(shí)時(shí)性的要求。

文獻(xiàn)[9]采用了拓?fù)涞貓D對(duì)移動(dòng)機(jī)器人進(jìn)行定位導(dǎo)航，但該方法對(duì)于結(jié)構(gòu)性很強(qiáng)的室內(nèi)環(huán)境非常容易造成誤匹配（尤其是線段特征），所以最終選擇用激光測(cè)距儀采集的數(shù)據(jù)來(lái)建立幾何特征地圖，實(shí)現(xiàn)無(wú)人飛行器的定位與導(dǎo)航。

2 四旋翼無(wú)人飛行器的自定位

四旋翼無(wú)人飛行器是在三維空間中運(yùn)動(dòng)，如果需要描述其精確位姿，需要用(x,y,z)描述其位置，用(ψ,θ,φ)描述其在三維坐標(biāo)中的姿態(tài)，其中：ψ為四旋翼無(wú)人飛行器的滾轉(zhuǎn)角；θ為四旋翼無(wú)人飛行器的俯仰角；φ為四旋翼無(wú)人飛行器的偏航角。

本文搭建的四旋翼無(wú)人飛行器的IMU并不能提供加速度、角速度這些信息，僅僅能通過(guò)激光測(cè)距離提供距離信息。

在室內(nèi)未知環(huán)境飛行時(shí)，無(wú)人飛行器的高度保持不變，主要是在二維平面內(nèi)運(yùn)動(dòng)。結(jié)合上述特點(diǎn)，可以用三維狀態(tài)向量(x,y,γ)來(lái)描述四旋翼無(wú)人飛行器的位姿信息，其中(x,y)為無(wú)人飛行器在所飛行平面內(nèi)的坐標(biāo)，γ為四旋翼無(wú)人飛行器的朝向，即相對(duì)于起飛時(shí)偏轉(zhuǎn)的角度。

四旋翼無(wú)人飛行器所采用的定位方法與傳感器密切相關(guān)。文中選用激光測(cè)距儀和磁力計(jì)，但是激光測(cè)距儀和磁力計(jì)都是安裝在四旋翼無(wú)人飛行器的中心，用這2個(gè)傳感器測(cè)得的數(shù)據(jù)都是基于機(jī)體坐標(biāo)系OrXrYr的，若是想要建立全局地圖的話，還需要選定參考點(diǎn)建立全局坐標(biāo)系OXY。

本文選起飛點(diǎn)作為全局坐標(biāo)系OXY的原點(diǎn)O，四旋翼無(wú)人飛行器起飛點(diǎn)至窗戶(hù)中線的連線方向?yàn)閅軸，向前為正，以垂直于Y軸方向?yàn)閄軸，向右為正。機(jī)體坐標(biāo)系OrXrYr的原點(diǎn)Or為四旋翼無(wú)人飛行器的中心，旋翼1與旋翼2之間的中線為Yr軸，機(jī)頭方向?yàn)檎?，以垂直于Yr方向?yàn)閄r軸，向右為正。建立四旋翼無(wú)人飛行器飛行示意圖如圖2所示。

圖2 飛行示意圖Fig.2 Schematic diagram of flying

其位姿方程為：

式（1）～（5）中：k=1,2,…,n,i=1,2,…,n；xk、yk和xk-1、yk-1分別為k時(shí)刻和k-1時(shí)刻四旋翼無(wú)人飛行器在全局坐標(biāo)系的坐標(biāo)；分別為k時(shí)刻和k-1時(shí)刻四旋翼無(wú)人飛行器與第i個(gè)特征點(diǎn)在機(jī)體坐標(biāo)系中的橫坐標(biāo)和縱坐標(biāo)；Δdk為k時(shí)刻到k-1時(shí)刻四旋翼無(wú)人飛行器的位移；φk為k時(shí)刻四旋翼無(wú)人飛行器航向角（此航向角是相對(duì)于全局坐標(biāo)系OXY的航向角）；φk-1為k-1時(shí)刻四旋翼無(wú)人飛行器航向角；αk-1為k時(shí)刻和k-1時(shí)刻的航向角之差；γk為k時(shí)刻四旋翼無(wú)人飛行器相對(duì)于起點(diǎn)方位偏轉(zhuǎn)的角度；γk-1為k-1時(shí)刻四旋翼無(wú)人飛行器相對(duì)于起點(diǎn)方位偏轉(zhuǎn)的角度。

此外，對(duì)于室內(nèi)環(huán)境的特征點(diǎn)，主要包括由墻到門(mén)（由墻到走廊）的突變點(diǎn)、由門(mén)到墻（由走廊到墻）的突變點(diǎn)、內(nèi)墻角特征點(diǎn)，在該環(huán)境中這些特征點(diǎn)相對(duì)于全局坐標(biāo)系是靜止不動(dòng)的量，其在全局坐標(biāo)系中的坐標(biāo)表示如下：

式（8）、（9）中，R為轉(zhuǎn)移矩陣[10-11]。

四旋翼無(wú)人飛行器以起飛點(diǎn)為原點(diǎn)，在起飛點(diǎn)檢測(cè)特征點(diǎn)并得到其坐標(biāo)（在這個(gè)時(shí)刻特征點(diǎn)相對(duì)于全局坐標(biāo)系的坐標(biāo)與機(jī)體坐標(biāo)系的坐標(biāo)是一致的），以此對(duì)其自身進(jìn)行定位，在飛行的過(guò)程中，檢測(cè)出環(huán)境中的新特征點(diǎn)并得到新特征點(diǎn)相對(duì)于機(jī)體坐標(biāo)系OrXrYr的坐標(biāo)，通過(guò)無(wú)人飛行器自身的坐標(biāo)，計(jì)算出新特征點(diǎn)相對(duì)于全局坐標(biāo)系OXY的坐標(biāo)，然后再以新特征點(diǎn)來(lái)對(duì)無(wú)人飛行器進(jìn)行定位，檢測(cè)后續(xù)的特征點(diǎn)，以此實(shí)現(xiàn)其自定位。

3 四旋翼無(wú)人飛行器的導(dǎo)航

所謂導(dǎo)航，就是把飛機(jī)、航天器、艦船等航行體按預(yù)先制定的計(jì)劃和要求，從一個(gè)地方引導(dǎo)到目的地的過(guò)程[12]。本文采用的導(dǎo)航方法是基于激光測(cè)距儀的視線導(dǎo)航，導(dǎo)航點(diǎn)通過(guò)特征檢測(cè)的特征點(diǎn)計(jì)算得出。

激光測(cè)距儀掃描范圍是-45°到270°，通過(guò)其采集的數(shù)據(jù)能夠提取出線段特征與點(diǎn)特征（內(nèi)墻角點(diǎn)、外墻角點(diǎn)），當(dāng)四旋翼無(wú)人飛行器飛行時(shí)，激光測(cè)距儀能夠找到左右兩側(cè)的特征點(diǎn)，利用該特征點(diǎn)無(wú)人飛行器能夠?qū)ψ陨磉M(jìn)行定位。

1）在機(jī)體坐標(biāo)系下，當(dāng)k時(shí)刻在四旋翼無(wú)人飛行器的前方（Xr為正的方向）出現(xiàn)至少2個(gè)特征點(diǎn)時(shí)，從這些特征點(diǎn)中找出xr坐標(biāo)值最小的2個(gè)特征點(diǎn)，將這2個(gè)特征點(diǎn)的中值作為導(dǎo)航點(diǎn)。

2）當(dāng)k時(shí)刻在四旋翼無(wú)人飛行器的前方出現(xiàn)1個(gè)特征點(diǎn)，且其后方出現(xiàn)至少2個(gè)特征點(diǎn)時(shí)，則無(wú)人飛行器需要找出后方距離最近的2個(gè)特征點(diǎn)，算出2個(gè)特征點(diǎn)的中值，將其作為導(dǎo)航點(diǎn)。

3）當(dāng)k時(shí)刻在四旋翼無(wú)人飛行器的前方出現(xiàn)1個(gè)特征點(diǎn)，且其后方出現(xiàn)的特征點(diǎn)少于2個(gè)時(shí)，此時(shí)僅以前方的特征點(diǎn)對(duì)四旋翼無(wú)人飛行器進(jìn)行定位，控制無(wú)人飛行器沿著平行于走廊兩側(cè)的墻飛行，直至前方出現(xiàn)2個(gè)特征點(diǎn)后再計(jì)算導(dǎo)航點(diǎn)。

4）當(dāng)k時(shí)刻在四旋翼無(wú)人飛行器的前方?jīng)]有出現(xiàn)特征點(diǎn)，且其后方出現(xiàn)的特征點(diǎn)為1個(gè)時(shí)，此時(shí)僅以后方的特征點(diǎn)對(duì)四旋翼無(wú)人飛行器進(jìn)行定位，控制無(wú)人飛行器沿著平行于走廊兩側(cè)墻飛行，直至出現(xiàn)2個(gè)特征點(diǎn)后再計(jì)算導(dǎo)航點(diǎn)。

4 地圖創(chuàng)建

四旋翼無(wú)人飛行器的定位與導(dǎo)航是同步進(jìn)行的，需要作為統(tǒng)一的過(guò)程來(lái)處理。當(dāng)無(wú)人飛行器的激光采集一次數(shù)據(jù)處理后，提取特征點(diǎn)和特征線段，構(gòu)建局部地圖。構(gòu)建的局部地圖需要與已經(jīng)構(gòu)建的全局地圖進(jìn)行特征匹配，合并特征點(diǎn)與特征線段。無(wú)人飛行器根據(jù)合并后的特征點(diǎn)實(shí)現(xiàn)定位，這樣在完成其定位的同時(shí)也把未知環(huán)境的全局地圖進(jìn)行了更新，建立新的全局地圖。

本文只是使用激光測(cè)距儀來(lái)完成地圖的創(chuàng)建，通過(guò)特征檢測(cè)可以將每一組激光掃描數(shù)據(jù)中的特征點(diǎn)和特征線段提取出來(lái)，這樣將能建立起局部地圖。而定位與建圖的關(guān)鍵在于將每一時(shí)刻建立的局部地圖與全局地圖進(jìn)行特征匹配，完成全局地圖的更新。

4.1 點(diǎn)特征匹配

對(duì)于特征點(diǎn)的匹配，本文采用最小距離法則。假設(shè)在k-1時(shí)刻檢測(cè)出m1個(gè)特征點(diǎn)，為其坐標(biāo)，而在k時(shí)刻檢測(cè)出m2個(gè)特征點(diǎn)為其坐標(biāo)，對(duì)于k-1時(shí)刻的特征點(diǎn)，依次計(jì)算k時(shí)刻所有特征點(diǎn)與k-1時(shí)刻特征點(diǎn)的距離，當(dāng)滿(mǎn)足如下條件時(shí)，表示相鄰時(shí)刻檢測(cè)出的特征點(diǎn)為同一個(gè)特征點(diǎn)。

式中：i=1,2,…,m1;j=1,2,…,m2；disvalue是設(shè)定的一個(gè)門(mén)限值，通過(guò)實(shí)驗(yàn)得到。

如果出現(xiàn)多個(gè)特征點(diǎn)滿(mǎn)足上述條件，則距離最近的點(diǎn)為同一特征點(diǎn)。特征點(diǎn)的每一次匹配都是在分類(lèi)之后進(jìn)行，即都是在由墻到門(mén)（由墻到走廊）的特征點(diǎn)、由門(mén)到墻（由走廊到墻）的特征點(diǎn)、內(nèi)墻角點(diǎn)這些特征點(diǎn)內(nèi)部進(jìn)行特征點(diǎn)匹配，不會(huì)出現(xiàn)外墻角點(diǎn)與內(nèi)墻角點(diǎn)進(jìn)行特征點(diǎn)匹配。對(duì)于不滿(mǎn)足上述條件的k時(shí)刻的特征點(diǎn)，則將其作為新出現(xiàn)的特征點(diǎn)，加入到特征點(diǎn)集中。特征點(diǎn)的匹配更多的是為四旋翼無(wú)人飛行器的自定位提供信息，在確定相鄰兩次特征點(diǎn)為同一個(gè)特征點(diǎn)時(shí)，才能根據(jù)特征點(diǎn)變化的距離而確定其位姿。

4.2 線段特征匹配

線段特征是構(gòu)成地圖的主要元素，全局地圖更新的準(zhǔn)確度取決于線段匹配的好壞。假設(shè)全局地圖共有m3條特征線段，記為L(zhǎng)={l1,l2,…,lm3}，而局部地圖共有m4條特征線段，記為G={g1,g2,…,gm4}，L與G構(gòu)成了匹配空間，線段特征由斜率k、截距b，線段起點(diǎn)坐標(biāo)、線段終點(diǎn)坐標(biāo)構(gòu)成，且先線段特征都已經(jīng)轉(zhuǎn)換在了全局坐標(biāo)系下表示，不需要再進(jìn)行坐標(biāo)轉(zhuǎn)換。

qij=(li,gj)表示線段特征li與gj的相似度，即表示li與gj為同一條線段的可能性，影響線段相似度的匹配條件主要有：

1）同向性。線段特征li與gj之間的夾角小于某一閾值。

式中：θli與θgj分別為線段特征li與gj的傾角；δθ為閾值。

2）共線性。原點(diǎn)到線段特征li與gj的距離相近，且線段特征li的中心點(diǎn)到gj的距離小于某一閾值。

式（14）、（15）中：bli與bgj分別為線段特征li與gj方程的截距，kli為線段特征li的斜率，δb與δd都是閾值。

3）一致性。線段特征li與gj中心點(diǎn)之間的距離不能大于線段特征li與gj長(zhǎng)度之和的一半。

式中，dli與dgj分別為線段特征li與gj的長(zhǎng)度[13-14]。

對(duì)以上3個(gè)條件中的4個(gè)因子分別賦予不同的權(quán)重wl1、wl2、wl3、wl4，并且wl1〉wl2〉wl3〉wl4，qij等于以上3個(gè)條件乘以權(quán)重因子的值[15-16]。

由于3個(gè)條件的量綱不一樣，需要對(duì)各個(gè)條件進(jìn)行歸一化處理。

建立一個(gè)m3×m4的相似矩陣Q，Q中的元素由qij組成，其中m3與m4分別表示線段特征li與gj的數(shù)目。qij越大，則線段特征li與gj越相似，兩者也越可能是同一條線段。找出相似矩陣Q中的最大值qab，其所對(duì)應(yīng)的2條線段匹配成功；將上一次相似矩陣中最大值所對(duì)應(yīng)行（第a行）的所有元素與列（第b列）的所有元素去除，組成新的矩陣Q1，找出新的相似矩陣Q1中的最大值，繼續(xù)匹配，直到匹配完全局地圖中的所有線段。

如果在線段的匹配過(guò)程中，某一行（某一列）出現(xiàn)多個(gè)最大值，說(shuō)明全局地圖中有一條線段與局部地圖中的多條線段相似（全局地圖中有多條線段與局部地圖中的某一條線段相似），此時(shí)需要比較影響相似度值的各個(gè)條件，依照各個(gè)條件權(quán)重的不同依次比較匹配條件。首先，比較同一行或者同一列中擁有相同相似度值元素的越大則2條線段越可能是同一條線段，匹配成功；若是也相等，則再依次比較它們的，直到匹配成功。

在整個(gè)線段特征的匹配過(guò)程中，越來(lái)越多的線段被匹配，使得剩余元素的匹配值qij越來(lái)越小，這樣可能不是同一條線段的2條線段也進(jìn)行了匹配，這就出現(xiàn)了誤匹配。為了防止這一情況，可以設(shè)定1個(gè)匹配閾值δmat，當(dāng)相似度值小于匹配閾值δmat時(shí)，匹配終止，而局部地圖中剩余未匹配的線段則作為新的線段特征加入到全局地圖中。匹配閾值越大，則匹配出來(lái)的線段越準(zhǔn)確，但是由此可能出現(xiàn)局部地圖中與全局地圖中的同一條線段未進(jìn)行匹配，而將局部地圖中的線段特征作為新的線段特征加入到全局地圖中的情況；而匹配閾值越小，局部地圖中將有越多的線段特征被匹配，隨之帶來(lái)的誤匹配的概率也越大。

對(duì)特征點(diǎn)的匹配可以實(shí)現(xiàn)四旋翼無(wú)人飛行器的定位，同時(shí)對(duì)特征點(diǎn)與特征線段的匹配能夠更新全局地圖，實(shí)現(xiàn)無(wú)人飛行器未知環(huán)境中的建圖。在圖3所示的比賽環(huán)境中，對(duì)四旋翼無(wú)人飛行器進(jìn)行定位、導(dǎo)航與建圖實(shí)驗(yàn)。通過(guò)特征匹配建圖，在比賽過(guò)程完成了室內(nèi)環(huán)境地圖的建立，如圖4所示。四旋翼無(wú)人飛行器在起飛之前先進(jìn)行起始點(diǎn)的定位，起飛后，從左上角的入口進(jìn)入室內(nèi)，通過(guò)尋找室內(nèi)環(huán)境中的特征點(diǎn)（圖中的拐角點(diǎn)）確定導(dǎo)航點(diǎn)，同時(shí)將室內(nèi)局部地圖構(gòu)建出來(lái)，與全局地圖進(jìn)行特征匹配；根據(jù)構(gòu)建出來(lái)的局部地圖再對(duì)其進(jìn)行自定位。圖4中四旋翼無(wú)人飛行器從起飛點(diǎn)出發(fā)，在室內(nèi)進(jìn)行定位導(dǎo)航，進(jìn)入右下角的房間的時(shí)候?qū)崿F(xiàn)了室內(nèi)環(huán)境地圖構(gòu)建。地圖建立結(jié)果與實(shí)際地圖吻合，證明了本文所用方法的可行性與準(zhǔn)確性。

圖3 國(guó)際空中機(jī)器人大賽比賽場(chǎng)地示意圖Fig.3 Competition arena schematic diagram of international aerial robotics competition

圖4 室內(nèi)環(huán)境地圖Fig.4 Indoor environment map

5 結(jié)論

本文首先建立了機(jī)體坐標(biāo)系與全局坐標(biāo)系，在機(jī)體坐標(biāo)系下無(wú)人飛行器能夠運(yùn)用特征檢測(cè)算法將激光測(cè)距儀采集數(shù)據(jù)中的特征點(diǎn)提取出來(lái)，結(jié)合四旋翼無(wú)人飛行器的方位信息，實(shí)現(xiàn)無(wú)人飛行器的自定位；針對(duì)室內(nèi)環(huán)境特征點(diǎn)個(gè)數(shù)的情況，制定了四旋翼無(wú)人飛行器的導(dǎo)航策略與避障策略，并設(shè)計(jì)了利用2個(gè)同向特征點(diǎn)來(lái)確定導(dǎo)航點(diǎn)的導(dǎo)航算法，使其能夠安全、準(zhǔn)確地導(dǎo)航；最后，通過(guò)特征點(diǎn)與特征線段的匹配得到了地圖，實(shí)驗(yàn)結(jié)果證明了此方法的可行性。

[1] SMITH R，SELF M，CHEESEMAN P.A stochastic map for uncertain spatial relationships[C]//Proceedings of the 4th International Symposium of Robotics Research.California：MIT Press，1987：467-474.

[2] THRUN S，BURGARD W，F(xiàn)OX D，et al.Probabilistic Robotics[M].Cambridge：MIT Press，2005：248-256.

[3] DURRANT-WHYTE H F，BAILEY T.Simultaneous localization and mapping：part I[J].IEEE Transactions on Robotics and Automation，2006，13（2）：99-108.

[4] FILLIAT D，MEYER J A.Map-based navigation in mobile robots：I.a review of localization strategies[J].Cognitive systems Research，2003，4（4）：243-282.

[5] THRUN S.Robotic mapping：a survey exploring artificial intelligence in the new millenium[M].San Francisco：Morgan Kaufmann.2002：1-25.

[6] MEYER J A FILLIAT D.Map-based navigation in mobile robots：II.A review of map-learning and path-planning strategies[J].Cognitive Systems Research，2003，4（4）：283-317.

[7] MORAVEC H.Sensor fusion in certainty grids for mobile robots[J].AI Magezine，1988，34（2）：61-74.

[8] GRZONKA S，GRISETTI G，BURGARD W.Towards a navigation system for autonomous indoor flying[C]//Proceedings of IEEE International Conference on Robotics and Automation.Kobe，2009：2878-2883.

[9] 石朝俠，洪炳镕，周彤，等.大規(guī)模環(huán)境下的拓?fù)涞貓D創(chuàng)建與導(dǎo)航[J].機(jī)器人，2007，29（5）：433-438. SHI CHAOXIA，HONG BINGRONG，ZHOU TONG，et al.Navigation based on topology map in large-scale environments[J].Robot，2007，29（5）：433-438.（in Chinese）

[10] 陶輝，吳懷宇，程磊，等.基于特征地圖的移動(dòng)機(jī)器人EKF-SLAM和FastSLAM算法自主導(dǎo)航研究[J].北京聯(lián)合大學(xué)學(xué)報(bào)：自然科學(xué)版，2010，24（2）：18-24. TAO HUI，WU HUAIYU，CHENG LEI，et al.Research on EKF-SLAMand FastSLAMautonomous navigation algorithms of mobile robots based on feature maps[J]. Journal of Beijing Union University：Natural Science，2010，24（2）：18-24.（in Chinese）

[11] JOSE GUIVANT，EDUARDO NEBOT.Optimization of the simultaneous location and map building algorithm for real time implementation[J].IEEE Transactions on Robotics and Automation，2001，17（3）：242-257.

[12]張國(guó)良.組合導(dǎo)航原理技術(shù)[M].西安：西安交通大學(xué)出版社，2008：1-10. ZHANG GUOLIANG.Combination navigation principles and techniques[M].Xi'an：Xi'an Jiaotong University Press，2008：1-10.（in Chinese）

[13] IP Y L，RAD A B，CHOW K M，et al.Segment-based map building using enhanced adaptive fuzzy clustering algorithm for mobile robot applications[J].Journal of Intelligent and Robotic Systems，2002，35（3）：221-245.

[14] GE KUNFENG，CHEN WEIDONG.Scan matching using line segment relationships for map-based localization of mobile robot[J].Journal of Harbin Institute of Technology：English Edition，2008，15（6）：757-761.

[15] 鄭宏.移動(dòng)機(jī)器人導(dǎo)航和SLAM系統(tǒng)研究[D].上海：上海交通大學(xué)，2007：32-41. ZHENG HONG.Research on mobile robot navigation and SLAM[D].Shanghai：Shanghai Jiaotong University，2007：32-41.（in Chinese）

[16] 熊蓉.室內(nèi)未知環(huán)境線段特征地圖構(gòu)建[D].杭州：浙江大學(xué)，2009：100-105. XIONG RONG.Line features map building in indoor unknown environment[D].Hangzhou：Zhejiang University，2009：100-105.（in Chinese）

Indoor Location,Navigation and Map Building of Four Rotor UAV

FANG Deng-jiana,DAI Yu-jina,ZHAO Hong-chaoa,KANG Yu-hangb
（Naval Aeronautical and Astronautical University a.No.7 Department; b.Graduate Students'Brigade,Yantai Shandong 264001,China）

According to semi-structured indoor environment characteristics,obtaining the feature point and line segments from data points collected by the laser scanner,and then the indoor environment feature map was established.Making use of feature points to realize four rotor UAV self-location indoor,while developing navigation algorithm based on the number of feature points at this stage.When the four rotor UAV fly to the next position,a local map was established,and it was integrated with the established global maps,to achieve the global map updates.

four rotor UAV;location;navigation;map building

V249.122

A

1673-1522（2014）04-0345-06

10.7682/j.issn.1673-1522.2014.04.010

2014-03-20；

2014-05-10

國(guó)家自然科學(xué)基金資助項(xiàng)目（61174031）

方登建（1977-），男，講師，碩士。