視覺輔助的交叉雙旋翼無人直升機自主降落控制系統

席鵬航， 李京陽，*， 印明威， 寶音賀西

（1. 天津工業大學 機械工程學院，天津 300387；2. 北京清航紫荊裝備科技有限公司，北京 102101；3. 清華大學 航天航空學院，北京 100084；4. 內蒙古工業大學 航空學院，呼和浩特 010051）

1 引 言

交叉雙旋翼無人直升機具有載重大、側向穩定性好等優點［1］，非常適合進行運輸、起重和吊運等工作。自主降落是完成這些工作的關鍵一環［2］。自主降落是指無人直升機利用機載導航設備、環境感知設備、飛行控制設備等進行飛行導引、目標定位及姿態控制，平穩著陸在指定區域的過程［3］。交叉雙旋翼無人直升機主要工作在城市中，高樓遮擋、天氣因素和噪聲干擾等會影響全球定位系統（Global Positioning System，GPS）導航的信號穩定性［4］。相比于GPS導航［5］，視覺導航具有更好的抗干擾能力和獨立性；相比于慣性導航［6］，視覺導航能夠避免誤差累積，通過外部環境獲取位姿信息，因此，視覺導航非常適用于GPS 信號不穩定時無人直升機的自主降落。

在位姿估計方面，2009 年，任沁源和李平［7］采用基于尺度不變特征變換（SIFT）和基于隨機抽樣一致性算法（RANSAC）的匹配策略獲取無人直升機的位姿信息；2010 年，蔣鴻翔和徐錦法［8］采用相鄰兩幀圖像對應特征點像點位置為位姿估計算法提供數據，一幀圖像特征點像點位置及其對應像點平移速度為線速度與角速度估計算法提供數據；2016 年，黃楠楠等［9］設計了“T”形降落地標，通過輪廓提取和角點檢測實現無人機的位姿估計。在圖像跟蹤方面，2012 年，Daewon Lee 和Tyler Ryan［10］采用基于圖像的視覺伺服（IBVS）生成速度參考指令，并作為自適應滑模控制器的輸入，在二維圖像空間中跟蹤平臺。針對自主降落圖像的匹配，2012 年，Yunji Zhao 和Hailong Pei［11］采用改進的加速魯棒特征算法解決了自主降落系統中SURF 算法效率低下的問題；2016 年，Saravanan 和Prakash［12］采用SURF算法獲取“H”形狀降落地標的特征信息，和降落地標的原始模板進行匹配；2019 年，Zhou Li和Yang Chen［13］采用基于單目攝像機的AprilTags 視覺定位算法定位和估計無人機的狀態。針對自主降落的位置計算，2015 年Youeyun Jung 和Dongjin Lee［14］提出由“H”型和同心圓組成的標記，采用直接最小二乘法擬合橢圓，通過計算二次曲線方程系數獲取圓心像素坐標，計算相對位置。2016 年，高九州和賈宏光［15］對自主降落位置的控制，采用自適應內模控制（AIMC）原理設計了自主降落縱向飛行控制律，通過對AIMC 濾波參數進行自調整改善了系統的動態特性。

本文提出了一種交叉雙旋翼無人直升機自主降落控制系統，它主要由導航處理單元、飛行控制器和姿態控制機構組成。其中，導航處理單元采用擴展卡爾曼濾波融合視覺導航和慣性導航，解算相對位姿信息并輸出給飛行控制器。飛行控制器采用串級PID 控制結構對相對位姿進行反饋控制，并將控制信號輸出給姿態控制機構。姿態控制機構采用6 個數字舵機實現交叉雙旋翼的周期變距，控制無人直升機位姿穩定。最終，通過視覺輔助的交叉雙旋翼無人直升機自主降落控制系統解決了GPS 信號不穩定時的自主降落問題。

2 降落地標設計

無人直升機的自主降落過程中需要實時獲取降落地標圖像，通過視覺導航解算出相對位姿信息［16］。因此，降落地標的設計是完成自主降落的關鍵環節。

無人直升機降落地標的設計要求［17］主要包括：（1）能夠區別于地面輪廓信息，避免錯誤識別，保證視覺導航的正確性；（2）能夠迅速識別，保證視覺導航的實時性；（3）能夠有效識別出圖像特征點，為相對位姿解算提供數據。

本文使用AprilTags 視覺基準系統，AprilTags 可以用來完成相機校準、機器人視覺和增強現實（Augmented Reality，AR）等工作。常用的有TAG16H5 家族和TAG36H11 家族，如圖1 所示。TAG16H5 的有效區域是4×4 的方塊，TAG36H11 的有效區域是6×6 的方塊，所以TAG16H5 能檢測更遠的距離，但由于TAG16H 5 的校驗信息少，TAG16H5 的錯誤率比TAG36H11 高很多。因此，本文選取TAG36H1 1 家族作為降落地標。

圖1 降落地標Fig.1 Landing landmarks

目標圖像經過線段聚類、圖像分割、深度優先搜索確定矩形和關鍵點閾值篩選識別特征信息是否為降落地標。識別后，檢測出圖像中心點的像素坐標，經過坐標系轉換和相機成像原理可以得出中心點的實際位置偏差，通過無人直升機位姿控制實現降落地標的對齊。

交叉雙旋翼無人直升機的旋翼具有傾斜角度，著陸時旋翼距離地面較近，為避免旋翼與地面發生碰撞，同時考慮地效區的影響，將無人直升機自主降落分為兩個階段。第一階段是無人直升機快速下降，獲取降落地標的信息進行視覺導航；第二階段是無人直升機緩慢下降，此時相機不能完整拍攝降落地標，通過姿態控制保持無人直升機穩定，使它平穩著陸。

3 圖像處理與檢測

圖像處理與檢測是視覺導航中的核心環節。圖像處理就是將拍攝圖像中的降落地標提取出來，并減弱環境噪聲的影響［18］。圖像檢測就是獲取圖像中的特征點信息，利用相機模型和坐標系轉換解算出實際位置信息［19］，為無人直升機位姿控制提供數據。

3.1 圖像處理

由于色差、噪聲等影響，圖像檢測會出現誤檢，進而影響相對位姿解算的正確性。因此，圖像處理是圖像檢測的前提，其流程如圖2 所示。

圖像灰度化是為了將圖像畫質變得更清晰，同時占用更少的儲存空間，將攝像機捕捉到的RGB 彩色圖像轉化為灰度圖像，即：

式中：I(x，y) 為該像素點灰度值；R(x，y)，G(x，y)，B(x，y)分別為該點的紅、綠、藍分量；α，β，γ分別為紅、綠、藍分量系數。常用的灰度化方法有均值法、最值法等，但由于人眼對綠色敏感度最高，藍色敏感度最低，因此采用加權平均法更為合理，取α=0.3，β=0.59，γ=0.11。

圖像濾波是為了削弱噪聲干擾，對圖像進行濾波處理。這里采用中值濾波處理，相較于均值濾波和高斯濾波，中值濾波具有去噪效果好和計算量小等優點。設計一個3×3 的濾波模板，遍歷模板中心像素點及其8 個鄰域內的像素點灰度值，取中值作為中心像素點的灰度值。

圖像分割是為了將降落地標從圖像中提取出來，對圖像進行Otsu 閾值處理［20］，即灰度值大于閾值的像素點變為255（最黑），灰度值小于閾值的像素點變為0（最白）。

3.2 圖像檢測

本文采用Canny 邊緣檢測、Hough 變換直線檢測和矩形檢測相結合的方法，共同完成圖像特征信息的檢測。

Canny 邊緣檢測就是尋找灰度值變化最強的像素位置，用梯度來表示灰度值變化的強度和方向。采用Sobel 算子進行邊緣檢測，即：

式中：Sx和Sy為水平x和垂直y方向的檢測算子，取3×3 的卷積模板A，即：

式中：A為以f(x，y)為中心像素點組成的8 鄰域卷積模板。利用Sobel 算子進行卷積運算，有：

式中：Gx和Gy分別為水平x和垂直y方向的梯度幅值；G為梯度幅值；θ為梯度方向。

再利用非極大抑制技術（Nonmaximum Supression， NMS）來消除邊緣誤檢，設計一個3×3 的模板，將像素點的梯度方向近似為鄰域8 個像素點中的1 個，并計算梯度方向正負方向像素點的梯度值，如果該像素點梯度值最大，則保留，否則刪除。

最后采用雙閾值技術，即設置一個閾值上界Tmax和閾值下界Tmin，本文取Tmax/Tmin=3。如果G>Tmax，則認定是強邊緣點；如果G

滯后跟蹤技術通過3×3 模板檢測弱邊緣點鄰域內是否存在強邊緣點，如果存在，則認為是邊緣點；如果不存在，則認為不是邊緣點。

Hough 變換就是利用點和直線的對偶性檢測圖像空間中的直線，包括：圖像空間中的點和參數空間中的直線一一對應；圖像空間中的直線和參數空間中的點一一對應。為了防止直角坐標系中直線垂直于x軸，導致參數空間中的直線沒有交點，參數空間選擇極坐標系。

已知圖像空間中的一條黑白分界直線，通過圖像坐標系原點作直線的垂線，如圖3 所示。圖中，(p，q)點用極坐標表示，即：

垂線的斜率為：

那么，參考航向線的斜率為：

設參考航向線上任意點(x，y)，則直線斜率可表示為：

參考航向線上兩個角點分別為(x1，y1)，(x2，y2)，根據式（8）和式（9）可得：

聯立求解，可得出β的值。

矩形檢測就是利用聚類算法［21］處理Canny邊緣檢測出的邊緣，利用Hough 變換直線檢測圖中直線，得到具有兩個端點L1，L2的有向線段，方向為線段左側亮度比右側亮度低。

對所有檢測到的線段進行分組，分組規則為前一條線段的末端點和下一條邊的始端點之間的距離小于閾值Tr，同時兩條線段組成的方向為逆時針方向。分組完成后，將所有的線段構成一個樹，第一層為所有線段，第二層到最后一層的節點為同一組線段，利用深度優先搜索遍歷整個樹，如果在深度為4 的時候，讓第四條線段和第一條線段構成了閉環，則組成一個矩形。

圖像檢測完成后，識別二維碼中內部每個小矩形范圍內的平均像素值是否大于設定閾值，如果大于設定閾值，則此小矩形編碼為1；如果小于設定閾值，則此小矩形編碼為0。通過圖像編碼進行匹配，識別AprilTag 目標。

識別降落地標后，對目標利用矩形框進行框選，返回一個AprilTag 類對象，通過索引對象中的矩形框中心點函數返回AprilTag 目標的中心點像素坐標，為位置解算提供數據。

4 位置與姿態解算

4.1 位置解算

無人直升機自主降落過程中，相對位置包括相對高度?h和相對水平位置?x，?y。其中，?h利用激光測距進行計算；?x，?y利用相機成像原理檢測圖像中的特征點信息，再通過坐標系轉換得出。

相機成像原理和特征點投影關系如圖4 所示。圖中，定義OwXwYwZw為地面坐標系，ObXbYbZb為機體坐標系，OcXcYcZc為相機坐標系，OpXpYpZp為圖像坐標系，ouv為像素坐標系。由于相機位置靠近無人直升機質心，因此假設機體坐標系和相機坐標系近于重合。

圖4 相機成像投影坐標系Fig.4 Coordinate system of camera imaging projection

已知降落地標中有一點Nw（xw，yw，zw），在相機坐標系中為Nc（xc，yc，zc），經過相機成像投影后，落在圖像坐標系上變成點Np（xp，yp，zp），Np在圖像坐標系中，因此zp=0。通過相機成像原理的幾何關系轉換，可以得出點Nc和Np的關系如下：

式中f為相機的焦距。即：

圖像坐標系和像素坐標系在同一平面內，只存在原點和尺度上的區別，因此，圖像坐標系可以通過平移和縮放變為像素坐標系，即：

式中：εx和εy分別為x和y方向上的縮放系數；μx和μy分別為x和y方向上的平移系數。根據式（12）和式（13）得出：

式中：fx=εx f，fy=εy f。將式（14）轉化為齊次矩陣形式，有：

式中：K為相機內參，可以根據張正友標定法［22］獲得；Nc為降落地標特征點Nw在相機坐標系中的點。已知Nw是世界坐標，世界坐標系可以經過旋轉和平移得到相機坐標系，因此Nc可以用Nw來表示，即：

式中：R和T分別為旋轉矩陣和平移矩陣；有：

式中：φ，θ，ψ分別為無人直升機的滾轉角、俯仰角和偏航角；Tx，Ty，Tz分別為坐標系的x軸、y軸和z軸的偏移量。

綜上所述，世界坐標系上的特征點對應到像素坐標系上的轉換關系的齊次矩陣形式如下：

式中：λ=zc=Tz=Δh，Δh為無人直升機高度，由機載激光測距儀測量得出。因此，通過對降落地標中心點像素坐標與其在世界坐標系中的實際位置，即可求解出Tx和Ty，進而得出無人直升機的位置偏差Δx和Δy。

4.2 姿態解算

無人直升機的姿態包括滾轉角φ、俯仰角θ和偏航角ψ。其中，滾轉角φ和俯仰角θ由機載慣性導航中的慣性測量單元（IMU）得出。由于城市間對于GPS 信號的干擾，偏航角ψ通過視覺定位中的Hough 直線變換檢測得出。

根據式（10）可求解出圖像坐標系原點到參考航向線的垂線的角度β，取無人直升機右偏為正。根據垂線所在圖像坐標系象限不同分為三種情況：當垂線在第一、四象限時，偏航角φ=β；當垂線在第二象限時，偏航角φ=β-π；當垂線在第三象限時，偏航角φ=β+π，如圖5 所示。

圖5 參考航向線與坐標系象限Fig.5 Reference direction line and coordinate system quadrant

5 自主降落控制設計

自主降落包括位置控制和姿態控制，位置控制分為水平位置控制和高度控制。為使加速度在可控范圍內，設計開方控制器作為位置控制的最外環。姿態控制中設計了一種交叉雙旋翼無人直升機的姿態控制機構。視覺輔助飛行控制系統如圖6 所示。

圖6 視覺輔助飛行控制系統Fig.6 Visual assisted flight control system

圖6 中，視覺模塊對降落地標進行圖像處理，得出無人直升機的航向偏差和位置偏差。航向偏差輸入姿態控制中的航向控制律，保持航向穩定；位置偏差輸入位置控制中的水平位置控制律和高度控制律，水平位置控制律解算出期望姿態角后，輸入姿態控制律，高度控制律解算出期望總距后，輸入姿態控制機構，保持無人直升機自主降落過程中的位姿穩定。

5.1 位置控制設計

5.1.1 水平位置控制律設計

為了實現水平位置控制，設計一種串級PID控制結構，將期望原點位置與視覺解算出的實際位置作差，通過開方控制器執行反饋控制輸出期望速度；將期望速度與IMU 解算出的實際速度作差，通過PID 控制器執行反饋控制，得到期望加速度；經過旋轉矩陣轉化為期望姿態角，將期望姿態角輸入姿態控制，通過姿態控制實現無人直升機的水平位置控制，如圖 7 所示。

圖7 中，xe和ye為期望水平位置；xr和yr為實際水平位置；Δx和Δy為水平位置的偏差；vxe和vye為期望速度；vxr和vyr為實際速度；axe和aye為期望加速度；θe為期望俯仰角；φe為期望滾轉角。

圖7 水平位置控制律Fig.7 Horizontal position control law

5.1.2 高度控制律設計

為了實現交叉雙旋翼無人直升機的高度控制，需要對其特有的結構進行升力計算。旋翼截面積為S，弦長為c，來流速度為V∞，旋翼迎角為α，空氣密度為ρ∞，則交叉雙旋翼的升力F為：

式中a為升力線斜率。通過實驗可知交叉雙旋翼的傾斜角度γ=12.5°時，升力效果最好，則升力計算公式為：

本文采用串級PID 控制結構進行高度控制，將期望地面高度與激光測距得出的實際高度作差，通過開方控制器執行反饋控制，輸出期望速度；將期望速度與IMU 得出的實際速度作差，通過PID 控制器執行反饋控制，輸出期望加速度，將期望加速度與IMU 得出的實際加速度作差；通過PID 控制執行反饋控制，輸出期望總距，將期望總距輸入姿態控制機構，通過姿態控制機構實現高度控制，如圖8 所示。

圖8 高度控制律Fig.8 Altitude control law

圖8 中，he為期望高度；hr為實際高度；Δh為高度偏差，由激光測距儀測得；vhe為期望速度；vhr為實際速度；Δvh為速度偏差；ahe為期望加速度；ahr為實際加速度；Δah為加速度偏差；Le為期望總距。

5.1.3 開方控制器設計

開方控制器是一個優化改進的P 控制器。飛行過程中，若期望位置在控制系統合理的范圍內，則不需要對期望位置做任何限制；若期望位置不在控制系統的能力范圍內，不符合無人直升機的物理極限規則，則期望位置無法實現，甚至出現炸機。為了避免上述情況的發生，設計一個開方控制器來限制跟隨的期望位置，以此達到更好的飛行效果。

普通的P 控制器設計為：

其中：vtar為期望速度；kp為比例系數；epos為位置偏差。假設t時間內的速率恒定，則：

對式（22）求導，有：

可以看出，當誤差收斂為恒定值時，加速度a與成正比。

在實際飛行過程中，加速度是有上限的。假設無人直升機能提供的最大加速度為amax，則在整個飛行過程中a不應超過amax。根據式（24）可知，當位置偏差時，加速度為變化量；當位置偏差時，加速度為恒定值，有：

式中：v0為初始速度，x為距離，有：

根據式（25）和式（26）可得：

結合普通P 控制器得到完整的開方控制器如下：

開方控制器效果如圖9 所示。

圖9 開方控制器效果Fig.9 Effect of square root controller

本文的開方控制器設置參數kp=1，amax=5，則從圖9 可以看出，當|epos|≤5 時，是一條斜率為1 的直線，開方控制器相當于P 控制器；當|epos|>5 時，是一條單調的曲線，類似于開方函數。

5.2 姿態控制設計

5.2.1 姿態控制律設計

為了使無人直升機具有優秀的飛行控制能力，本文姿態控制瞬時響應是通過提高控制頻率來解決的，采用400 Hz 的控制頻率，串級PID 控制加前饋控制的控制結構，外環采用P 控制器，內環采用PID 控制器。將位置控制輸出的期望姿態角和IMU 或視覺解算出的實際姿態角作差，得出姿態角偏差，通過P 控制器執行反饋控制，輸出期望角速度；將期望角速度和IMU 解算出的實際角速度作差，通過PID 控制器執行反饋控制，輸出舵機控制量；將舵機控制量輸入姿態控制機構，通過姿態控制機構實現無人直升機的姿態控制，如圖10 所示。

圖10 姿態控制律Fig.10 Attitude control law

圖10 中，φe為期望姿態角；φa為實際姿態角；Δφ為姿態角偏差；ωe為期望角速度；ωa為實際角速度；Δω為角速度偏差；H為舵機控制量；ωf為角速度前饋量，ωf=ωe。

5.2.2 姿態控制機構設計

交叉雙旋翼無人直升機的姿態控制主要通過機體6 個數字舵機來實現。它們3 個為一組控制傾斜盤的角度，實現對無人直升機的滾轉、俯仰和偏航的姿態控制，如圖11 所示。

圖11 姿態控制機構Fig.11 Attitude control mechanism

（1） 滾轉控制：當無人直升機做滾轉運動向右飛行時，舵機1，4 向上推傾斜盤，舵機2，5 向下拉傾斜盤，舵機3，6 保持不變，通過改變傾斜盤的左右傾角實現交叉雙旋翼的周期變距，從而實現無人直升機的向右飛行。向左飛行時，各舵機的控制方向與向右飛行相反。

（2） 俯仰控制：當無人直升機做俯仰運動向前飛行時，舵機1，2 和舵機4，5 向下拉傾斜盤，舵機3，6 向上推傾斜盤，通過改變傾斜盤的前后傾角實現交叉雙旋翼的周期變距，從而實現無人直升機的向前飛行。向后飛行時，各舵機的控制方向與向前飛行相反。

（3） 偏航控制：當無人直升機做偏航運動向右旋轉時，舵機1，2 和舵機6 向下拉傾斜盤，舵機4，5 和舵機3 向上推傾斜盤，通過改變傾斜盤的傾角實現交叉雙旋翼的周期變距，從而實現無人直升機的向右旋轉。向左旋轉時，各舵機的控制方向與向右旋轉相反。

6 實 驗

通過Mission Planner 飛控地面站搭建仿真環境，根據圖6 所示的系統搭建仿真控制律，進行仿真驗證。仿真過程中，首先解鎖使無人直升機起飛，切換導引模式，在15 s 內模擬OpenMV 檢測的標志在左上方像素（120，80）處，15 s 到30 s內模擬OpenMV 檢測的標志在右下方像素（200，160）處。仿真結果如圖12 和圖13 所示，可以看出，視覺輔助飛控系統的跟蹤性能良好，可以很好地跟蹤無人直升機的期望位置。

圖12 北向位置與期望位置的仿真曲線Fig.12 Simulation curves of north position and desired position

圖13 東向位置與期望位置的仿真曲線Fig.13 Simulation curves of east position and desired position

實驗中，無人直升機采用JZ120 交叉雙旋翼無人直升機，如圖14 所示。攝像頭采用Open-MV4 H7 Plus，如圖15 所示。

圖14 交叉雙旋翼無人直升機Fig.14 Intermeshing-rotor unmanned helicopter

圖15 OpenMV4 H7 Plus 攝像頭Fig.15 OpenMV4 H7 Plus camera

圖16 ~圖18 為無人直升機自主降落過程中的姿態曲線，其中滾轉角偏移量在5°左右，修正時最大偏移量在4°左右；俯仰角偏移量在3°左右，修正時最大偏移量在7°左右；偏航角偏移量在7°左右，修正時最大偏移量在7°左右。圖19和圖20 為無人直升機自主降落過程中的位置曲線，其中北向（x方向）位置期望的修正偏移量在0.1 m 左右，實際修正偏移量約為0.05 m；東向（y方向）位置期望的修正偏移量約為0.2 m，實際修正偏移量約為0.2 m。修正后無人直升機自主降落位置的北向（x方向）誤差在0.05 m 左右，東向（y方向）誤差在0.02 m 左右，跟蹤性能良好，可以實現無人直升機的自主降落。

圖16 滾轉角曲線Fig.16 Roll angle curve

圖17 俯仰角曲線Fig.17 Pitch angle curve

圖18 偏航角曲線Fig.18 Yaw angle curve

圖19 北向位置與期望位置曲線Fig.19 North position and desired position curves

圖20 東向位置與期望位置曲線Fig.20 East position and desired position curves

7 結 論

本文提出了使用視覺輔助的交叉雙旋翼無人直升機自主降落控制系統，采用相機成像原理對AprilTag 視覺標志進行位姿信息的獲取，傳遞給控制系統執行位置控制和姿態控制。設計了一種適用于交叉雙旋翼無人直升機的姿態控制機構，并給出了無人直升機位置和姿態控制律，實現目標的定位。無人直升機自主降落過程姿態角度的偏移量在4°以內，位置偏移量在0.05 m以內，跟蹤定位狀態良好，可以實現交叉雙旋翼無人直升機的自主降落控制。