基于地標的無人機倉庫環境導航方法研究

漢鑫，劉云清，董巖，顏飛，張華鑫

（長春理工大學 電子信息工程學院，長春 130022）

微小型無人機的導航技術包括位置估計和位姿控制問題，是無人機完成任務與應用的關鍵技術。近年來四旋翼飛行器的室內導航技術得到了廣泛的研究和關注，文獻［1-3］研究了單目相機的定位與導航問題，單目視覺技術的難點在于無法得到絕對尺度信息，通常需要聲吶傳感器或事先的測量標定來實現尺度估計。文獻［4］利用了雙目相機與卡爾曼濾波算法實現自主導航，雙目視覺技術利用兩套圖像傳感器在提取特征點信息的同時對目標進行尺度信息的估算，具備實時處理未知特征點的優點，由于計算時延較大，其使用的嵌入式圖像處理硬件有較高規格。文獻［5］利用運動捕捉系統對飛行器進行實時位姿檢測，運動捕捉系統依托環繞場景布置的運動捕捉相機對被測物體上反光特征點進行位置解算，適用于開闊的室內環境。文獻［6-7］設計了結合光流及地標的室內導航方法，光流算法利用地面紋理特征估計航速，地面紋理及環境光照是影響估計精度的重要因素。文獻［8］提出一種結合激光測距儀與單目視覺的室內導航方法，激光雷達地圖構建精細，需要多種算法共同協作。針對存在磁場干擾的環境，文獻［9］利用磁向計與GPS傳感器組合導航，通過計算實際與預定路線的航向角偏差量來實現室外環境導航。文獻［10］通過激光雷達傳感器與電磁場檢測傳感器設計了電力巡線場景中的避障與導航方法。文獻［11］設計了跟蹤地面路徑的導航系統以完成在工廠內部的巡視任務。

綜上所述，針對四旋翼無人機在倉庫環境中結合地標與進行視覺伺服進行導航設計的研究較少，在室內導航的研究中大多沒有考慮到環境中磁場干擾對導航精度的影響。本文針對以上問題及無人機的成本等因素，提出一種通過地標表示倉庫立體環境，使用視覺伺服控制技術實現四旋翼飛行器在倉庫貨架環境的導航方法，并利用卡爾曼濾波算法解決了在倉庫環境中磁場對無人機航向角控制的干擾問題。

1 基于地標的單目相機定位

為了實現飛行器在倉庫環境中對自身的精確定位，本文設計了兩種地標作為飛行器定位的指引。在貨架側方粘貼色塊組合標志，用于表示貨物的存儲位置，提供給飛行器相對貨架的位置信息與橫向距離信息，在地面輔設了用于飛行器循跡與校準方向的直線標志，地面直線標志鋪設在貨架過道的正中央，標志顏色與地面顏色有較大反差即可。本文設計的導航方法如圖1所示。

圖1 組合地標導航示意圖

地標圖像的特征信息需要被轉換成飛行器的控制量，首先提取兩種地標各自的圖像特征，然后結合應用場景案建立倉庫導航環境的相機坐標系、機體坐標系、世界坐標系，最后通過相機的外參進行地標特征信息的坐標變換得到可供飛行控制器直接使用的導航信息。

1.1 地標的圖像特征提取

本文利用單目相機模塊對地標進行特征解算。飛行器側面安置的圖像模塊解析色塊地標，在飛行器下方安置的圖像模塊用于解析地面的直線地標。其中色塊識別基于色調過濾算法［12］，對可精確識別的七種顏色進行編碼，使視覺模塊可按順序輸出色塊地標的顏色編碼S和組合色塊整體的角度信息φI，如圖2所示。對相機進行標定，根據已知的色塊大小與像素坐標間的關系得到圖像的絕對尺度信息。

圖2 色塊標志的編碼處理

色塊組合地標的圖像處理結果為識別的顏色組合值、角度值、色塊中心點的圖像平面坐標、色塊寬度所占像素數及高度像素數。對直線地標的圖像處理結果為圖像平面中矢量的線段端點坐標。接下來會利用兩個模塊得到的信息進行組合地標的位置解算。

1.2 地標特征的位置解算

在得到相機坐標系下的位置特征信息后，還需要建立相關坐標系，將飛控訂閱的姿態數據用作圖像模塊的外參，通過坐標系之間的關系將圖像的特征信息轉換成世界坐標系下的位置偏差量以作為飛行器控制量。飛行器各個坐標系間的相對關系圖如圖3所示。

圖3 飛行器各坐標系轉換關系圖

對色塊坐標進行位置解算，在相機坐標系c1下的色塊中心點坐標轉換到機體坐標系坐標：

式中，ax、ay、az為相機坐標系c1原點指向機體坐標系原點的向量a沿機體坐標系各個軸的分解，向量a由圖像模塊的安裝位置決定，Rot表示關于x、y、z軸旋轉的基本齊次變換矩陣，Trans表示關于x、y、z軸平移的基本齊次變換矩陣。

對地面直線坐標進行位置解算，相機坐標系c2下的直線端點坐標、轉換為機體坐標系下的LB1、LB2坐標：

式中，向量b為相機坐標系c2原點指向機體坐標系原點的向量，由圖像模塊的安裝位置決定。

利用飛行器不同姿態下圖像深度值與飛行器實際高度值之間的幾何關系，Zc2表達式為：

式中，hI為飛行控制器通過氣壓計獲取的飛行器當前高度信息。θ、φ分別為飛行器相對世界坐標系的俯仰角與橫滾角。

根據倉庫環境特點將飛行器所在的世界坐標系w的原點ow設置為飛行器質心在倉庫過道的投影點，其zw軸穿過投影點并與地面垂直且正方向朝下，xw軸正方向與飛行器前進方向相同且平行于地面的直線標志，yw軸與前兩軸正交且滿足右手定則。現將直線端點在機體坐標系下的坐標LB1、LB2轉換到機體水平坐標系w1，其xw1、yw1軸為機體坐標系中xB、yB軸在過oB的水平面的投影且正方向相同，坐標系原點ow1位于飛行器質心：

轉換后的線段端點坐標Lw1(xw1,yw1,zw1)、Lw2(xw2,yw2,zw2)所構成的矢量平行于世界坐標系的xwowyw平面，利用其坐標值對航向角的觀測值Ψg進行表示：

其中，Ψc為磁向計測得的地面直線地標正方向所指的真實角度值。

由于倉庫內貨架等金屬物品帶來的磁場干擾非常嚴重，所以本文將航向角的觀測值Ψg與飛行控制器中IMU模塊輸出的航向角ΨIMU進行了卡爾曼濾波融合，以得到經過濾波后的航向角Ψf。隨后再將融合后的航向角Ψf用于色塊中心點坐標的世界坐標值轉換中，得到坐標：

在獲得色塊中心點在世界坐標系的坐標Pw(xw,yw,zw)后，可解算飛行器相對色塊標志的準確位置，進一步得出位置控制量，即飛行器投影點距地面直線的距離△L及飛行器距色塊的距離△X、△Y：

式中wL為已知的直線距貨架的距離。

2 基于卡爾曼濾波算法的航向角修正算法

位置解算的準確度依賴于飛行器姿態角的準確性，而在倉庫環境下的貨架、貨物等工業制品存在較大的磁場干擾，會導致飛行器的航向角的測定出現偏差，使位置解算產生誤差。本設計利用卡爾曼濾波算法對飛行器的航向角進行修正，將根據地面直線地標解算出的航向角觀測信息與通過飛控數據預測的航向角進行卡爾曼濾波處理。

首先根據預估航向角與陀螺儀角速度ωgyro存在的微分關系對航向角進行狀態空間描述，并根據采樣周期dt對狀態方程與輸出方程進行離散化處理。航向角Ψ作為狀態向量，Ψg作為觀測向量表示如下：

式中，εm、εg分別為陀螺儀的噪聲與航向角觀測的噪聲。

k+1時刻的飛行器航向角可由k時刻的最優航向角與k時刻的角速度進行積分來預估，且不存在控制量，即當前時刻航向角的估計為：

k+1時刻的預估航向角Ψ(k+1|k)對應的噪聲協方差為P(k+1| k)：

式中，Q為過程噪聲的協方差，P(|k k)為上一時刻最優狀態估計的協方差。

根據式（10）、（11）得到當前時刻的卡爾曼增益Kg為：

式中，R為測量噪聲的協方差。

通過上一節得到的當前時刻航向角觀測值Ψg與預估值Ψ(k+1|k)來計算當前時航向角的最優估計Ψ(k+1|k+1)：

更新當前時刻最優估計航向角的協方差P(k+1|k+1)：

式（12）、（13）、（14）為濾波過程的修正階段，在飛行過程中不斷對預測階段與修正階段做遞歸運算，即可得到航向角在當前時刻的最優估計值。隨后將經過濾波處理后得到的最優航向角估計值Ψf與地面直線的航向角做差得到航向角偏差控制量△Ψ：

3 控制邏輯設計

飛行器在室外環境通常依靠GPS與IMU進行速度信息的融合估計，在室內環境下的常用方法則是使用光流法等方式進行速度估計，考慮到系統的復雜性及本設計中飛行控制器的控制方式，本文利用飛控自身的數據進行速度信息的獲取，最后采用了基于小步長的位置控制方法作為系統的控制方式。

無人機在倉庫環境下導航的流程圖如圖4所示，主要基于循線、對準色塊、執行任務、再次循線、再次對準的流程。其中飛行器在循線的過程中在保證了自身距離貨架的安全距離同時使模塊采集的圖像平面與貨架平面接近平行以避免較大的圖像畸變，而自主對準色塊的環節在保證了下一階段中位置控制的準確度的同時也使此方案具備一定的可拓展性，如進行倉庫貨物管理等任務。

圖4 基于地標與單目視覺的導航與控制框架

最后對解算出的位置控制量進行小步長分解，使飛行器能更快地調整位姿，數據時戳的控制更加靈活。小步長控制量的實現不僅需要對API控制量進行分解，而且需要通過調整PID參數改變飛行器響應控制量的感度，使飛行器能更高頻率地達到平衡狀態以保證距貨架的安全距離及圖像處理的準確性。在每個控制周期內，方向控制器（heading）、高度控制器（altitude）、位置控制器（position）以不同頻率不斷執行，以減小控制量間的耦合，其控制頻率間的關系：

4 實驗結果和分析

4.1 實驗環境

本設計采用“單片機控制端+開源飛行控制器”的硬件組成方式。其中飛行控制器為大疆創新公司的N3飛控。控制端的單片機為英飛凌公司的XMC4700。控制端單片機驅動各部分的底層通訊，融合圖像模塊與N3的姿態數據，將位置特征信息解算為飛行器的位置偏差量，依據控制邏輯解算控制量API，實現對四旋翼飛行器的導航控制。圖5為導航試驗平臺，圖6為正在模擬執行任務的飛行器。

圖5 四旋翼飛行器導航實驗平臺

圖6 實驗中的四旋翼飛行器

本設計使用Pixy(CMUcam5)開源圖像處理模塊對地標進行圖像處理。Pixy1代與2代模塊都具有識別色塊編碼的功能，其中Pixy2代模塊相比Pixy1代模塊新增了對“直線”形狀的快速檢測算法。在飛行器側面布置一個Pixy1代模塊用于采集與分析色塊組合圖像信息，在飛行器下方布置一個Pixy2代模塊用于處理地面直線標志信息。

4.2 實驗結果分析

在模擬倉庫貨架過道的環境中進行飛行實驗，圖7為飛行過程中N3飛控輸出的高度數據，其中飛行器首先在第一層貨架執行了色塊對準任務，在對準完成后保持了穩定的高度值，隨后根據導航指令進行高度爬升，在到達預定高度后開始對準第二層貨架上的色塊，再下降高度，最后執行循線任務向第一層的下一個色塊地標移動.在此過程中飛行器的高度控制穩定。圖8為在相同時間段N3飛控輸出的航向角數據，由圖中數據可以看出在執行對第一層貨架色塊的對準任務時航向角控制穩定，在爬升高度時受飛行姿態影響，航向角控制出現輕微擾動，在下降回第一層貨架后，飛行器在執行循線任務過程中受到磁場干擾，但在濾波補償后恢復了對航向角的穩定控制。通過對系統模塊的多項測試及實驗結果分析，系統在3m～6m寬的模擬貨架過道環境內達到了穩定的控制效果。

圖7 系統輸出的高度數據曲線

圖8 系統輸出的航向角數據曲線

5 結論

本文以四旋翼飛行器為研究對象，研究了適用于倉庫貨架環境的四旋翼飛行器室內導航方案。針對貨架環境中存在磁場干擾的問題，提出了融合視覺和慣導信息對航向角進行卡爾曼濾波的算法，使依賴航向角進行解算的位置估計值與導航控制量更加準確。通過設計控制器保證了飛行器在倉庫過道環境中的導航精確度。為說明導航方案的有效性進行了飛行實驗，實驗結果表明了穩定的控制效果。