張皓焱，劉 新

（鄭州大學國際學院，河南 鄭州 450000）

在室內(nèi)或者偏遠地區(qū)，全球定位系統(tǒng)（G l o b a l Positioning System，GPS）信號弱甚至搜星失敗，無人機難以定位，容易造成失控進而引發(fā)安全事故。

基于視覺處理的無人機自主定位導(dǎo)航系統(tǒng)，彌補了傳統(tǒng)定位方式的缺陷，在無GPS信號下也能穩(wěn)定、高效運行。通過飛行實驗來驗證常用視覺算法的性能，實驗中使用樹莓派單獨處理視覺模塊的數(shù)據(jù)流，使用C2000芯片進行姿態(tài)解算、位置融合和動力分配，實現(xiàn)了視覺處理實驗平臺與飛行控制實驗平臺的分離。同時采用Open Vision Studio上位機軟件作為地面監(jiān)視站，直接觀測視覺處理實驗平臺中的圖像信息、位姿信息及傳感器狀態(tài)信息，可實時調(diào)整實驗平臺的參數(shù)。

文章將從視覺信息的獲取、處理、封裝交付以及飛控算法等方面敘述并驗證視覺導(dǎo)航系統(tǒng)的工作原理。

1 四旋翼無人機導(dǎo)航系統(tǒng)驗證平臺構(gòu)建

1.1 視覺驗證平臺設(shè)計

視覺處理平臺的硬件核心是一張樹莓派CM3計算模塊，運行Ubuntu Linux操作系統(tǒng)。該計算模塊具有大量接口，可以根據(jù)外設(shè)的通信協(xié)議建立通信。視覺處理平臺采用基于SONY IMX9的攝像頭，支持1 080 p以及最高60 fps錄制。為保證圖像處理的實時性和準確性，僅裁切畫面中心320×240部分作為原始圖像信息。同時，設(shè)計了用于搭載樹莓派CM3計算模塊的拓展板，包含STM32協(xié)處理器、US-100超聲波測距單元以及平行于地面的相機感光平面[1]。

1.2 飛控驗證平臺設(shè)計

選用Ti公司的C2000 F28379D雙核LaunchPad開發(fā)板作為本驗證平臺的飛控硬件。對于所需驗證平臺而言，單核性能足以滿足全部實驗的運算要求，雙中央處理器（Central Processing Unit，CPU）則可實現(xiàn)系統(tǒng)冗余設(shè)計，保障實驗安全。內(nèi)部運行uC/OS-II操作系統(tǒng)，結(jié)合基于任務(wù)優(yōu)先級的搶占式調(diào)度，便于飛控系統(tǒng)的構(gòu)建[2]。

由于開發(fā)板外設(shè)不足以完成飛控系統(tǒng)所需硬件要求，文章設(shè)計了集成慣性傳感器、氣壓高度計以及藍牙通信等模塊的飛控擴展板，同時，集成藍牙通信功能，便于實驗時實時調(diào)參，其資源包含HC-06藍牙串口透傳單元、MPU-6050慣性測量單元、MS5611氣壓高度測量單元以及含三軸數(shù)字羅盤儀和三軸數(shù)字加速度儀的LSM303D測量單元。

1.3 “視覺+飛控”導(dǎo)航系統(tǒng)驗證平臺設(shè)計

視覺定位導(dǎo)航算法的實現(xiàn)基于Python-OpenCV開發(fā)環(huán)境。樹莓派CM3計算模塊通過拓展板相機采集圖像信息進行處理并傳送至STM32協(xié)處理器，同時超聲波測距單元的高度信息也將被傳至STM32協(xié)處理器。STM32協(xié)處理器將兩者信息整合打包上傳至飛控。飛控還將獲得來自飛控擴展板傳輸?shù)淖藨B(tài)、加速度和磁偏角等數(shù)據(jù)信息以及從氣壓計獲取的高度數(shù)據(jù)信息，通過一系列算法處理兩路數(shù)據(jù)實現(xiàn)位置控制[1]。

2 視覺信息的獲取

以索尼IMX9作為無人機的下視攝像頭獲取視覺信息。視覺模塊啟動時需要初始化串口與攝像頭，設(shè)置輸出圖像的尺寸和幀率，同時，還需對其采集到的圖像進行預(yù)處理，既要保證圖像質(zhì)量，又要保證圖像處理的實時性。常見的預(yù)處理方法有像素變換和圖像增強兩種：

（1）像素變換，有兩種變換方式。第一，類型變換。圖像的類型不同，信息量也不同，所以對RGB圖像進行灰度處理或二值處理，可以有效地壓縮所圖像大小，減輕樹莓派工作負擔。第二，降低像素，對采集到的圖像進行剪裁。以圖像的中心為基準向其四周各取適量的像素點數(shù)，得到一幀低分辨率的圖像。這種保留圖像中心并丟棄圖像邊緣的方法能明顯減少處理器的運算量，提高圖像處理的實時性。

（2）圖像增強，即提高圖像質(zhì)量。圖像易受機身抖動和噪聲等因素的干擾，文章采用中值濾波法[3]降噪，將每一像素點的灰度值設(shè)置為該點某鄰域窗口內(nèi)所有像素點灰度值的中值，消除噪點。有時還需要運用Canny邊緣檢測法，根據(jù)圖像灰度值的不連續(xù)性獲取邊緣信息。若圖像中不同像素位置的明暗程度差距較大，則通過腐蝕和膨脹[4]進行優(yōu)化，微調(diào)亮暗對比度，消除噪聲，尋找圖像中的極大值區(qū)域和極小值區(qū)域，以達到優(yōu)化的目的?？傊?，圖像增強是提取視覺信息的必要手段。

3 視覺信息的處理

3.1 無人機視覺導(dǎo)航算法概述

根據(jù)提取到的視覺信息，樹莓派利用OpenCV所提供的強大接口對圖像進行處理，識別目標物，計算當前無人機的位置和速度等狀態(tài)，指定期望軌跡和期望速度，然后對無人機飛行控制模塊發(fā)出相應(yīng)指令，完成不同的飛行任務(wù)。常用的視覺算法有Blob檢測算法、霍夫變換算法、循跡算法、稀疏光流算法等。

3.2 目標物識別算法—以Blob為例

Blob意為斑點，是圖像中具有相似顏色和紋理特征的一塊連通區(qū)域。連通域中各像素點具有相近的灰度值，然而和連通域之外的像素點卻有明顯的顏色差異和灰度差異。因為斑點代表的是一個區(qū)域，所以相對于單純的角點來說具有更好的穩(wěn)定性和抗干擾能力。

Blob檢測通過幾何分析對圖像中的連通區(qū)域進行識別，并標記一些重要的幾何特征，通常包括區(qū)域的面積、中心點坐標、質(zhì)心坐標、最小外接矩形和主軸等，還可以提供圖像中斑點的數(shù)量、位置、形狀和方向以及斑點間的拓撲結(jié)構(gòu)[5]。通過視覺模塊將斑點的像素位置信息上傳至飛控模塊，可以引導(dǎo)四旋翼無人機的姿態(tài)控制，實現(xiàn)捕獲目標、定點懸停、目標追蹤以及自主降落等功能，如圖1所示。Blob連通域檢測算法的實現(xiàn)如下：

（1）設(shè)定閾值范圍[Min, Max]，步長為s，通過一系列連續(xù)的閾值Min，Min+s，Min+2s，…，Max，把輸入的灰度圖像轉(zhuǎn)換為一個二值圖像的集合。（2）分別檢測每一幅二值圖像的邊界，提取對應(yīng)每幅圖像的連通域，再根據(jù)預(yù)設(shè)的顏色、面積、形狀等限定條件進一步過濾，得到初步的連通域集合。（3）使用findContours（）查找圖像的邊，并計算每一個輪廓的中心，將得到的每幅圖像的中心點放在一起。定義一個最小距離，在這個距離區(qū)域內(nèi)的特征中心點被歸為一個group, 對應(yīng)一個Blob特征，得到特征點集合。（4）確定灰度圖像中的連通域位置和尺寸，估計Blob特征和相應(yīng)的半徑，并以keypoints返回。（5）將得到的連通域中心坐標交付給飛控芯片進行無人機位置控制。

圖1 Blob檢測算法實現(xiàn)運動小車的實時追蹤

3.3 霍夫變換

無人機圖像輪廓檢測的主要目的是從圖像中識別并提取出關(guān)鍵信息，最常用的基本方法是檢測出直線和圓。由于大多數(shù)飛行場地的地面引導(dǎo)線在航拍圖像中都表現(xiàn)為成對出現(xiàn)的、近似直線的邊緣并橫貫整幅圖像，所以可以通過設(shè)定直線長度和傾斜角閾值的方法對直線進行篩選，從中獲取目標物的位置信息，引導(dǎo)無人機巡航[6-7]。

文章使用OpenCV的HoughLinesP（）檢測直線，首先，傳入經(jīng)閾值分割或邊緣檢測后的二值圖，對檢測參數(shù)進行配置。已知直線極坐形式的曲線方程為rθ=x0×cosθ+y0×sinθ，則需分別確定距離r和角度θ的精度，r值越大、θ值越小，考慮的線就越多。其次，確定累加數(shù)閾值，其值越小，考慮的線就越多。還可以根據(jù)飛行任務(wù)的需求靈活地配置最短長度閾值、最大間隔這兩個可選參數(shù)，前者是長度的下限，比它短的線段會被剔除；后者指定兩直線間的最小距離，間隔小于這一值的兩條直線會被視作是一條線。被檢測出的直線以首尾兩個端點坐標的集合形式存儲。通過對集合中各直線長度和斜率的篩選，可以得到符合要求的直線。根據(jù)檢測的結(jié)果還可以進一步計算出兩條霍夫線的交點，將交點坐標交付至飛控芯片，可以完成交叉點的識別和循跡直角轉(zhuǎn)彎等飛行任務(wù)。

經(jīng)典霍夫變換還可以用來檢測圓和橢圓。霍夫圓變換與直線變換類似，無非是直線用（r，θ）表示，圓用（x，y）表示，從二維變成了三維，數(shù)據(jù)量變大了很多，所以一般使用霍夫梯度法減少計算量[8]。

OpenCV中的HoughCircles（）可以實現(xiàn)對圓形目標物的檢測，首先，確定利用霍夫梯度法進行變換，累加器圖像分辨率與原圖一致；其次，設(shè)置兩個不同圓圓心的最短距離，并確定累加數(shù)閾值，同霍夫直線檢測，累加數(shù)閾值越小，考慮的圓就越多。該算法返回的是被檢測圓的圓心坐標與半徑大小，根據(jù)半徑對結(jié)果集合進行篩選，取出目標圓，然后將坐標轉(zhuǎn)化為飛控可以利用的數(shù)據(jù)。霍夫圓檢測算法與Blob檢測算法不同：霍夫圓檢測算法能夠檢測非連通域的空心圓形，所以在圓形目標物的檢測上，霍夫圓檢測算法可以與Blob檢測算法互補，以適應(yīng)不同類型圓形目標物的識別，實現(xiàn)圓形目標的定點懸停和定點追蹤，如圖2所示。

圖2 霍夫圓檢測算法實現(xiàn)定點懸停

3.4 循跡飛行

循跡是指依照給定的賽道軌跡，使用攝像頭拍攝賽道圖片，并采用圖像處理算法提取有用的信息，來引導(dǎo)無人機飛行。在實際的測試應(yīng)用中，地面引導(dǎo)線多為黑色直線或者弧線，與背景有著明顯的特征區(qū)分。文章通過OpenCV視覺模塊采集分辨率為160×120的賽道圖像，在捕獲到的黑線圖像中選取左、中、右3個感興趣區(qū)域進行分割，分別將其標記為區(qū)域1、區(qū)域2、區(qū)域3，區(qū)域分割示意如圖3所示。

圖3 圖像分割示意

經(jīng)圖像分割，原圖的左、中、右3個感興趣區(qū)域分別對應(yīng)3幅掩模圖像，然后對每幅掩模圖進行灰度處理，再根據(jù)預(yù)設(shè)的閾值將3幅灰度圖像二值化，進而利用OpenCV中的FindCounters（）函數(shù)分別提取各掩模圖像中的最大矩形輪廓，得到的輪廓即是該幅掩模圖像中被截斷的黑線片段，再將這3幅掩模圖像依照左、中、右的順序分別拼接回原圖對應(yīng)的位置，即可得到如圖4所示的航拍效果—原圖中貫穿整張圖像的黑線被劃分為3個獨立不相連的條狀矩形。同時利用OpenCV的minAreaRect（）提取區(qū)域2中矩形的中心坐標以及區(qū)域1、區(qū)域2中矩形的長邊與x軸的夾角α1和α2。

圖4 循跡航拍

首先，循跡需要滿足無人機在飛行過程中始終位于黑線上方并且機頭方向與黑線的切線方向重合，所以區(qū)域2中矩形的中心應(yīng)恰好位于圖像中心；其次，還要將兩矩形的長邊與x軸的夾角α1和α2做差以求得目標航向角α=α1－α2。將目標航向角和區(qū)域2中矩形的質(zhì)心坐標上傳至飛控芯片，即可控制無人機循跡飛行，循跡算法的步驟如下：

（1）分割圖像，將原圖剪裁成3幅等分辨率的掩模圖。

（2）進行圖像預(yù)處理，利用inRange函數(shù)，依照預(yù)設(shè)的雙邊閾值將各掩模圖黑白二值化。

（3）提取各掩模圖中的黑色長條狀矩形輪廓，即被切割分段的黑線。

（4）計算三幅圖像中各黑線的位置信息，包括質(zhì)心點坐標、矩形的長邊與x軸的夾角。

（5）計算期望偏航角，將區(qū)域1和區(qū)域2中兩段黑線的角度做差，差值作為期望偏航角，差值的正負分別對應(yīng)航向上的左右。

（6）糾正橫向位置，根據(jù)區(qū)域2黑線的質(zhì)心坐標糾正無人機在橫向上的左右位置。

（7）將偏航角和橫向位置數(shù)據(jù)交付至飛控芯片。

3.5 光流控速

3.5.1 光流控速算法概述

四旋翼無人機采用基于圖像金字塔的稀疏光流（Lucas-Kanade，LK）方法計算某些點集的光流，通過計算關(guān)聯(lián)特征點在序列圖像中像素的位置變化求解速度信息。首先，檢測出一幀圖像中的特征點并保存其坐標，進而跟蹤特征點，獲取它在下一幀圖形中的像素位置信息，將兩幀圖像的特征點坐標作差，即可得知兩幀圖像之間對應(yīng)特征點越過的像素點數(shù)；其次，根據(jù)攝像頭的幀率計算出無人機的速度信息，將其交付至飛控芯片，控制無人機速度，維持穩(wěn)定的飛行狀態(tài)。所謂的特征點即指角點，OpenCV提供了可以獲取準確角點位置的goodFeaturesToTrack（）函數(shù)，以獲取圖像中的角點集合。在循跡飛行時檢測到的特征點如圖5所示。

圖5 特征點檢測航拍

3.5.2 特征點處理

利用OpenCV的接口calcOpticalFlowPyrLK（）逐幀跟蹤檢測到的特征點?？紤]到圖像處理的實時性需求，LK光流算法不必對每幀圖都像進行特征點檢測。由于圖像幀的實時更新，一部分特征點隨著無人機的位置變化而丟失，另一部分特征點則仍存在于接下來的若干幀圖像中。例如無人機在定點懸停時，其位姿變化的幅度小，特征點的更新速率較低，那么就無需重新進行特征點的檢測，繼續(xù)對尚存的特征點進行跟蹤即可。

3.5.3 基于特征點檢測的金字塔LK光流算法

首先，設(shè)置閾值，在特征點檢測前比較剩余特征點數(shù)量與閾值的大小，若剩余特征點數(shù)目大于閾值，則繼續(xù)進行光流跟蹤，否則重新檢測特征點[9]。其次，提取當前幀的特征點坐標，并作為初始值，通過LK光流法對其下一時刻圖像上的坐標進行估計，再經(jīng)誤差補償對LK光流估計的誤差進行修正，從而準確估計特征點光流[1]，求得無人機的速度信息。其主要步驟如下：

（1）采集t時刻圖像It，對其灰度處理得灰度圖Gt。

（2）利用特征點檢測算法，獲取Gt得特征點集pt。

（3）采集t+1時刻圖像It+1，對其灰度處理得灰度圖Gt+1。

（4）將Gt，Gt+1，pt作為輸入，采用金字塔LK光流算法估算Gt+1圖像上特征點坐標pt+1。

（5）將Gt+1，Gt，pt+1作為輸入，利用（4）中步驟計算Gt圖像特征點坐標^pt。

（6）計算FB誤差，并以誤差的中值作為閾值，將FB誤差大于閾值的特征點刪除，計算特征點的位移光流。

（7）將無人機速度信息交付至飛控芯片，進行速度控制。

3.6 多核并發(fā)視覺處理

3.6.1 平臺優(yōu)勢

出于科研及競賽的各種需求，無人機在執(zhí)行循跡算法平穩(wěn)飛行的過程中，往往還需要不斷地識別目標物，執(zhí)行光流控速、Blob探測等任務(wù)以實現(xiàn)飛行控制，僅靠單一的循跡進程難以自主飛行。

視覺模塊基于BCM2837四核ARM處理器，能實現(xiàn)多核并發(fā)運算，除去打包上傳視覺數(shù)據(jù)占用的一個核，剩余的3個核都可以寫入不同的視覺處理進程，各個核并發(fā)運行，充分地利用多核CPU的資源，增強圖像處理的實時性，提高視覺模塊的計算效率。同時Python提供了優(yōu)秀的多進程模塊—multiprocessing，支持子進程通信、子進程之間的數(shù)據(jù)共享、不同視覺處理進程的并發(fā)執(zhí)行等，提供了Process，Queue，Pipe，Lock等組件，可以輕松完成從單進程到多進程并發(fā)執(zhí)行的轉(zhuǎn)換[10]。

3.6.2 多核處理算法

以光流算法與Blob算法的并發(fā)執(zhí)行為例，實現(xiàn)主進程執(zhí)行Blob檢測算法，子進程執(zhí)行光流算法的多核并發(fā)處理。在飛控中光流信息可用于速度環(huán)控制，同時Blob算法可用于位置環(huán)控制。多核處理的實現(xiàn)如下：

（1）初始化串口與攝像頭。

（2）主進程創(chuàng)建光流進程。

（3）在主進程中創(chuàng)建圖像數(shù)據(jù)隊列frameQueue、光流速度信息隊列flowX，flowY。

（4）主進程中初始化Blob相關(guān)參數(shù)。

（5）光流進程中初始化光流。

（6）在主進程中進入死循環(huán)。

（7）在死循環(huán)中獲取圖像，塞入圖像隊列。

（8）分別在主進程的循環(huán)中處理Blob，在光流進程中處理光流算法，最終在主進程中獲取光流進程的結(jié)果。

其中frameQueue為圖像隊列，用于兩進程之間的數(shù)據(jù)交換，其長度為1，方向為主進程壓入，光流進程取出；flowX，flowY分別為光流算法中x軸速度與y軸速度的存儲隊列，長度為1，與frameQueue的存取方向相反，即從光流進程壓入，從主進程取出。算法執(zhí)行時，在主進程的死循環(huán)中不斷獲取圖像幀并將其壓入隊列frameQueue，然后主進程繼續(xù)處理的Blob算法；光流進程從frameQueue隊列中獲取圖像幀，由于隊列長度為1，每當取出一幀圖像，隊列變空，故無需手動清理隊列中已被使用的圖像幀。光流進程計算結(jié)束后，向flowX，flowY兩個隊列中壓入光流算法的當次計算結(jié)果值xSpeed，ySpeed，再由主進程中接收。周而復(fù)始，視覺模塊可以實時地將兩進程處理完畢的目標物位置坐標和無人機速度信息同時上傳至飛控芯片。

4 視覺信息的封裝與交付

樹莓派實時地將數(shù)據(jù)打包并傳送至STM32，再由STM32將超聲波模塊的高度信息整合到數(shù)據(jù)包中，形成完整的信令進而交付至飛控。這一系列的數(shù)據(jù)傳送需要統(tǒng)一的信令格式：將包頭和包尾作為定界符，進行差錯檢測并丟棄錯誤的數(shù)據(jù)包；功能位（MODE）表示期望飛行模式，包括定點懸停模式、循跡模式、識別模式等；超聲波高度數(shù)據(jù)占用尾部4個字節(jié)，其余字節(jié)在不同的飛行模式下被賦予不同的意義，如目標點坐標、偏航角度和速度等。對樹莓派中運行的可執(zhí)行文件的編寫，可以實現(xiàn)由下位機間接控制上位機更改視覺處理模式。

數(shù)據(jù)交付至飛控之后，飛控通過產(chǎn)生串口中斷的方式接收數(shù)據(jù)，由Ti芯片的接收端寄存器RX中的先入先出隊列（First Input First Output，F(xiàn)IFO）來觸發(fā)中斷。凡是從STM32上傳至飛控的數(shù)據(jù)全部由FIFO接收，滿16個字節(jié)（3字節(jié)包頭包尾+9字節(jié)視覺數(shù)據(jù)+4字節(jié)超聲波數(shù)據(jù)）觸發(fā)一次中斷并加以檢查。當FIFO緩沖區(qū)滿16個字節(jié)即產(chǎn)生中斷而后拋出信號量，觸發(fā)飛控主函數(shù)中的下位機數(shù)據(jù)接收任務(wù)：取出視覺數(shù)據(jù)、清除FIFO的中斷標志位等信息以及將信號量置零，從而完成數(shù)據(jù)接收的任務(wù)。

5 基于視覺處理的無人機飛行控制

5.1 視覺位置數(shù)據(jù)的解算

為滿足進一步的飛行控制，需要從視覺數(shù)據(jù)中解算出期望軌跡，期望軌跡是一系列目標位置的點集，目標位置的確定則是基于相機對目標的捕捉，得到其在相機坐標系X’O’Y’中的位置，而目標實際位置是在世界坐標系XOY下。根據(jù)針孔成像原理[11]，假設(shè)空間內(nèi)任何一點都成像于距離攝像機光心垂直距離一倍焦距f的平面處，并且由超聲波模塊已獲取當前無人機對地高度h，由相似三角形法可以得出空間中一點P距無人機的水平距離L，目標定位示意如圖6所示。

圖6 針孔成像原理

空間中一點P在相機中成像于P’，P’ 距相機中心點O’的距離為L’，而無人機距P的實際距離L，那么L與L’的關(guān)系為：

以定點跟蹤為例，如果規(guī)定無人機需要識別目標物并始終懸停在目標物的正上方，則要確保當前圖像幀中目標物的質(zhì)心必須與圖像中心重合，即根據(jù)功能位Bytes[2]得知當前無人機執(zhí)行定點模式，飛機需要迅速調(diào)整位置，移動到目標物的中心點的坐標位置處。假設(shè)當前圖像分辨率是160×120，那么中心點的坐標是（80，60），所以通過計算x-80，y-60可知連通域中心偏離了相機中心的像素點數(shù)，然后將偏差像素點數(shù)與單個像素點的尺寸長度相乘，轉(zhuǎn)換為對應(yīng)的長度（本機攝像頭1像素≈0.571 cm）。再根據(jù)針孔成像原理求得無人機實際偏離目標點的距離，最后將數(shù)據(jù)交付至飛控芯片做進一步處理。

5.2 飛行控制模型概述

飛控將獲得兩路信息，一路是來自STM32整合后的視覺位置信息數(shù)據(jù)，另一路是來自飛控擴展版上慣性測量單元（Inertial Measurement Unit，IMU）的姿態(tài)、加速度和磁偏角等數(shù)據(jù)。由上文所述，飛控根據(jù)針孔成像原理，從視覺模塊數(shù)據(jù)中求解期望軌跡，那么期望軌跡與實際軌跡的差值即為軌跡誤差，視其為位置觀測量，IMU獲取加速度觀測量，由預(yù)測模型對當前的位置數(shù)據(jù)和加速度數(shù)據(jù)進行融合，得到修正后的位置數(shù)據(jù)，視其為預(yù)測數(shù)據(jù)。調(diào)節(jié)卡爾曼增益，融合觀測數(shù)據(jù)與預(yù)測數(shù)據(jù)，進行飛行控制。飛控模型示意如圖7所示，其中飛行控制大致分為4步：

（1）位置控制，將融合結(jié)果作為輸入量，由位置模塊進行處理。在位置控制中，數(shù)據(jù)先后經(jīng)過高度環(huán)控制、速度環(huán)控制、加速度環(huán)控制，最后將加速度環(huán)輸出的加速度與重力加速度疊加，得到期望加速度。依照控制模型，將加速度在軸上的分量分別轉(zhuǎn)換為目標翻滾角（roll）、目標俯仰角（pitch）、目標偏航角（yaw），最后將三軸的期望姿態(tài)角疊加后輸出最終期望姿態(tài)。

（2）姿態(tài)控制，將期望姿態(tài)作為輸入量，輸入到姿態(tài)控制模塊，根據(jù)機身的期望姿態(tài)角度計算出期望力矩。

（3）控制分配，將期望力矩作為輸入量輸入到控制模塊中進行控制分配，將期望力矩的效果分配到四旋翼無人機的4個電機上，輸出期望升力F1，F(xiàn)2，F(xiàn)3，F(xiàn)4。

（4）電機控制，分別將各個電機的目標升力轉(zhuǎn)換成對應(yīng)的油門百分比[即脈寬調(diào)制（Pulse Width Modulation，PWM）占空比]。

圖7 四旋翼飛行控制框架

5.3 無人機姿態(tài)控制基礎(chǔ)

為了定量描述定位導(dǎo)航系統(tǒng)驗證平臺的姿態(tài)信息，必須建立以地面為參考的四旋翼無人機的坐標系。現(xiàn)確定以當?shù)氐乩砦恢脼閰⒄?，以東、北、天所對應(yīng)方向為XYZ軸的正方向建立導(dǎo)航坐標系。以四旋翼無人機為參照，以前、右、上所對應(yīng)方向為XYZ軸的正方形建立無人機坐標系。

四元數(shù)法具有運算量小、不會出現(xiàn)萬向節(jié)鎖等優(yōu)點，可進行全姿態(tài)解算。本文研究采用四元數(shù)法求解無人機姿態(tài)。在初始狀態(tài)下可以獲得準確的姿態(tài)角和初始矩陣。由于實際的角速度測量中存在零偏等誤差，更新四元數(shù)數(shù)據(jù)也會有漂移發(fā)生，因而使用卡爾曼濾波進行矯正，初始四元數(shù)值不必過于精準[12]。

5.4 位姿控制器設(shè)計

通過針孔模型以及姿態(tài)解算求解到期望角度后，設(shè)計位姿控制器實現(xiàn)目標定位導(dǎo)航。為了確保在定位導(dǎo)航中保持機頭前進方向始終對準興趣點方向，采用偏航角與俯仰角和翻滾角分離的控制器設(shè)計，確保定位興趣點始終在畫面像素中心點附近。

5.4.1 偏航角控制器

偏航角控制器采用單級閉環(huán)比例—積分—導(dǎo)數(shù)（Proportion Integral Derivative，PID）控制。期望偏航角Ye1與無人機當前位置Yo1做差得到PID的輸入值。

通過控制對角線上兩對電機的速度得到期望力矩，在PID調(diào)節(jié)基礎(chǔ)上增加uy(0)項確保無人機向上的總拉力始終不變，無人機平面始終在同一高度平面。

5.4.2 俯仰角和翻滾角控制器

為了準確控制位姿并提高位姿控制的實時性，俯仰角和翻滾角控制器采用內(nèi)外環(huán)串級PID控制。將期望角度[xe，ye，ze]與無人機當前位置[xo，yo，zo]做差，得到外環(huán)PID的輸入值。外環(huán)控制中：

e(k)是指期望角度值和當前（度值的誤差，dt為控制周期，為比例系數(shù)，Ki1為積分系數(shù)，Kd1為微分系數(shù)。在控制周期較短的情況下，外環(huán)輸出值X(k)是角度關(guān)于時間t的微分，即角速度。對于內(nèi)環(huán)PID控制，期望角速度為0。將外環(huán)PID總輸出[ωout1，ωout2，ωout3]與當前角速度[ωo1，ωo2，ωo3]做差，得到內(nèi)環(huán)PID的輸入值：

up(0)為電機調(diào)速的基礎(chǔ)量，推算出的各個電機期望拉力可以對電機既定的油門動力曲線通過up(0)進行修正，提升無人機控制實時性和穩(wěn)定性。

以上控制器可以得到各個電機調(diào)速控制量，使興趣點始終位于畫面中心附近，機頭方向始終與興趣點移動方向一致，確保無人機能穩(wěn)定、高效地追蹤感興趣點，提高了定位導(dǎo)航系統(tǒng)的響應(yīng)速度和可靠性。

6 結(jié)語

文章提出了一種對圖像數(shù)據(jù)進行像素變換、圖像增強和計算機視覺化算法方案，自主設(shè)計了以PID為控制器基本控制原理的四旋翼無人機驗證平臺。在視覺處理核心硬件部分（即樹莓派）附加STM32協(xié)同處理，在減輕視覺處理核心的處理器負載的同時提升了視覺處理的實時性和同步性。在俯仰角和翻滾角控制器中，外環(huán)使用角度作為受控對象，內(nèi)環(huán)使用角速度作為受控對象，偏航角控制器使用偏航角偏差作為受控對象，從而實現(xiàn)定方向快速精準位置移動。經(jīng)驗證，在室內(nèi)無GPS條件下，文章研究方案的定位精確度高，定點直徑為20 cm，并能穩(wěn)定追蹤速度5 m/s以下物體，優(yōu)于現(xiàn)有算法。