方登建，戴宇進，趙紅超，康宇航

（海軍航空工程學院a.7系；b.研究生管理大隊，山東煙臺264001）

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

方登建a，戴宇進a，趙紅超a，康宇航b

（海軍航空工程學院a.7系；b.研究生管理大隊，山東煙臺264001）

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

四旋翼無人飛行器；定位；導航；建圖

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

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

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

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

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

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

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

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

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

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

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

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

2 四旋翼無人飛行器的自定位

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

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

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

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

本文選起飛點作為全局坐標系OXY的原點O，四旋翼無人飛行器起飛點至窗戶中線的連線方向為Y軸，向前為正，以垂直于Y軸方向為X軸，向右為正。機體坐標系OrXrYr的原點Or為四旋翼無人飛行器的中心，旋翼1與旋翼2之間的中線為Yr軸，機頭方向為正，以垂直于Yr方向為Xr軸，向右為正。建立四旋翼無人飛行器飛行示意圖如圖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時刻和k-1時刻四旋翼無人飛行器在全局坐標系的坐標；分別為k時刻和k-1時刻四旋翼無人飛行器與第i個特征點在機體坐標系中的橫坐標和縱坐標；Δdk為k時刻到k-1時刻四旋翼無人飛行器的位移；φk為k時刻四旋翼無人飛行器航向角（此航向角是相對于全局坐標系OXY的航向角）；φk-1為k-1時刻四旋翼無人飛行器航向角；αk-1為k時刻和k-1時刻的航向角之差；γk為k時刻四旋翼無人飛行器相對于起點方位偏轉(zhuǎn)的角度；γk-1為k-1時刻四旋翼無人飛行器相對于起點方位偏轉(zhuǎn)的角度。

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

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

四旋翼無人飛行器以起飛點為原點，在起飛點檢測特征點并得到其坐標（在這個時刻特征點相對于全局坐標系的坐標與機體坐標系的坐標是一致的），以此對其自身進行定位，在飛行的過程中，檢測出環(huán)境中的新特征點并得到新特征點相對于機體坐標系OrXrYr的坐標，通過無人飛行器自身的坐標，計算出新特征點相對于全局坐標系OXY的坐標，然后再以新特征點來對無人飛行器進行定位，檢測后續(xù)的特征點，以此實現(xiàn)其自定位。

3 四旋翼無人飛行器的導航

所謂導航，就是把飛機、航天器、艦船等航行體按預先制定的計劃和要求，從一個地方引導到目的地的過程[12]。本文采用的導航方法是基于激光測距儀的視線導航，導航點通過特征檢測的特征點計算得出。

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

1）在機體坐標系下，當k時刻在四旋翼無人飛行器的前方（Xr為正的方向）出現(xiàn)至少2個特征點時，從這些特征點中找出xr坐標值最小的2個特征點，將這2個特征點的中值作為導航點。

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

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

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

4 地圖創(chuàng)建

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

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

4.1 點特征匹配

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

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

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

4.2 線段特征匹配

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

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

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

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

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

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

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

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

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

由于3個條件的量綱不一樣，需要對各個條件進行歸一化處理。

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

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

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

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

圖3 國際空中機器人大賽比賽場地示意圖Fig.3 Competition arena schematic diagram of international aerial robotics competition

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

5 結(jié)論

本文首先建立了機體坐標系與全局坐標系，在機體坐標系下無人飛行器能夠運用特征檢測算法將激光測距儀采集數(shù)據(jù)中的特征點提取出來，結(jié)合四旋翼無人飛行器的方位信息，實現(xiàn)無人飛行器的自定位；針對室內(nèi)環(huán)境特征點個數(shù)的情況，制定了四旋翼無人飛行器的導航策略與避障策略，并設(shè)計了利用2個同向特征點來確定導航點的導航算法，使其能夠安全、準確地導航；最后，通過特征點與特征線段的匹配得到了地圖，實驗結(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)境下的拓撲地圖創(chuàng)建與導航[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] 陶輝，吳懷宇，程磊，等.基于特征地圖的移動機器人EKF-SLAM和FastSLAM算法自主導航研究[J].北京聯(lián)合大學學報：自然科學版，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]張國良.組合導航原理技術(shù)[M].西安：西安交通大學出版社，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] 鄭宏.移動機器人導航和SLAM系統(tǒng)研究[D].上海：上海交通大學，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].杭州：浙江大學，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

國家自然科學基金資助項目（61174031）

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