基于光流的六旋翼飛行器的控制系統(tǒng)研究

劉祚時(shí) 饒 遠(yuǎn)

(江西理工大學(xué)機(jī)電工程學(xué)院 江西 贛州 341000)

0 引 言

隨著微電子機(jī)械系統(tǒng)(MEMS)技術(shù)的飛速發(fā)展，微型控制系統(tǒng)的整體體積越來越小，這為機(jī)械構(gòu)件、驅(qū)動(dòng)部件、光學(xué)系統(tǒng)、電控系統(tǒng)以及數(shù)字處理系統(tǒng)集成在一個(gè)微型的整體單元上提供了技術(shù)基礎(chǔ)。在這樣的技術(shù)條件下，有關(guān)無人機(jī)的相關(guān)研究也逐漸增多，但由于飛行器的控制系統(tǒng)作為一個(gè)多輸入輸出的非線性系統(tǒng)，具有多變量、強(qiáng)耦合的特點(diǎn)，因此經(jīng)典的控制算法已經(jīng)無法適用于飛行器的時(shí)變非線性控制系統(tǒng)。其中光流技術(shù)作為一項(xiàng)成熟技術(shù)，已經(jīng)在多旋翼飛行器上被大量應(yīng)用。例如：國外的部分學(xué)者將光流技術(shù)用在無人機(jī)的姿態(tài)估計(jì)恢復(fù)[1]或無人機(jī)降落及其地貌跟蹤[2]等研究上；國內(nèi)的部分科研機(jī)構(gòu)則是利用光流實(shí)現(xiàn)了目標(biāo)跟蹤[3]、自主懸停[4]、運(yùn)動(dòng)小目標(biāo)檢測(cè)[5]等功能。郭力等[6]提出了一種在無人機(jī)上裝載單目攝像機(jī)來實(shí)現(xiàn)目標(biāo)點(diǎn)定位的方法，但由于該方法在小范圍空間內(nèi)使用時(shí)定位偏移誤差較大，不適用于低空環(huán)境下。呂強(qiáng)等[7]采用最小絕對(duì)誤差和塊匹配(SAD)算法來求解機(jī)身相對(duì)與大地坐標(biāo)系的水平速度，但存在位置估計(jì)精度低的問題，且SAD匹配法易受外界光照強(qiáng)度的影響會(huì)導(dǎo)致光流傳感器定位精度變差。李亞斌[8]提出的基于VOD塊匹配準(zhǔn)則的連續(xù)消除算法(SEA)能夠有效地避免光流傳感器受外界光照強(qiáng)度變化的影響，但是為了采集到更加完整連續(xù)的光流信息，其利用多個(gè)相機(jī)采集光流信號(hào)導(dǎo)致控制器處理數(shù)據(jù)的時(shí)間過長(zhǎng)，進(jìn)而降低了控制系統(tǒng)的實(shí)時(shí)性。

本文提出一種利用光流模塊提取目標(biāo)點(diǎn)所在平面的平移光流并設(shè)計(jì)相應(yīng)的外部控制回路，同時(shí)結(jié)合內(nèi)部姿態(tài)角速度控制回路，實(shí)時(shí)獲取飛行器水平速度及其高度信息，并將光流信息作為控制系統(tǒng)的反饋信號(hào)，有效提高飛行器控制系統(tǒng)的定位精度。

1 建立飛行器數(shù)學(xué)模型

1.1 實(shí)驗(yàn)平臺(tái)

本文采用PX4Flow光流模塊，并在此基礎(chǔ)上對(duì)六旋翼飛行器進(jìn)行整機(jī)搭建，其實(shí)物圖如圖1所示。

圖1 飛行器實(shí)物圖

在飛行器飛行過程中，可通過PX4Flow光流模塊對(duì)地面的光流信息進(jìn)行采集。隨后飛控系統(tǒng)將光流模塊采集的光流信息通過數(shù)傳發(fā)送至PC端，其中數(shù)傳發(fā)送數(shù)據(jù)時(shí)按照MAVLink協(xié)議進(jìn)行實(shí)時(shí)通信，從而便可從上位機(jī)軟件QGC(QGroundControl)中觀測(cè)飛行器在飛行過程中的光流信息的實(shí)際變化情況。圖2為實(shí)驗(yàn)系統(tǒng)整體架構(gòu)圖。

圖2 實(shí)驗(yàn)系統(tǒng)整體架構(gòu)圖

1.2 飛行器動(dòng)力學(xué)分析

依照六旋翼飛行器的整體結(jié)構(gòu)建立空間直角坐標(biāo)系，以機(jī)架幾何中心點(diǎn)O為坐標(biāo)原點(diǎn)，電機(jī)4底座和電機(jī)6底座之間遠(yuǎn)離點(diǎn)O的角平分線方向作為x軸，垂直于x軸并通過電機(jī)1底座中心的方向?yàn)閥軸，故可由右手定則確定z軸的正方向。而圖中標(biāo)注的F1至F6分別為各自所對(duì)應(yīng)的電機(jī)產(chǎn)生的輸出力，其方向規(guī)定為與坐標(biāo)軸同向?yàn)檎⑶译姍C(jī)1、3、6順時(shí)針轉(zhuǎn)動(dòng)，電機(jī)2、4、5逆時(shí)針轉(zhuǎn)動(dòng)，如圖3所示。

圖3 六旋翼機(jī)架結(jié)構(gòu)圖

為了能夠簡(jiǎn)化飛行器的數(shù)學(xué)模型，并且便于后續(xù)的運(yùn)動(dòng)分析，現(xiàn)根據(jù)六旋翼飛行器的系統(tǒng)結(jié)構(gòu)特點(diǎn)做出下列假設(shè)：(1) 假定飛行器是理想狀態(tài)下的剛體，其幾何中心點(diǎn)與重心點(diǎn)相重合；(2) 將飛行器整體所受的升力視為線性的，同時(shí)螺旋槳正反轉(zhuǎn)產(chǎn)生的力大小相等，并忽略飛行器正反槳到達(dá)所需轉(zhuǎn)速的時(shí)間；(3) 當(dāng)飛行器處于靜止平衡狀態(tài)時(shí)，整體的重心處于z軸(偏航軸)上，同時(shí)飛行器的姿態(tài)角全為零(即俯仰角、橫滾角和偏航角為0)，并且忽略摩擦力與阻尼力矩。

在上述假設(shè)條件的基礎(chǔ)上，分析飛行器各姿態(tài)角方向的力矩平衡模型。

(1) 俯仰角的力矩平衡模型。針對(duì)六旋翼飛行器繞y軸轉(zhuǎn)動(dòng)即俯仰角方向，進(jìn)行平衡受力分析。由于此時(shí)電機(jī)1、2的升力F1、F2在xoz的投影通過點(diǎn)O，故不考慮其對(duì)俯仰運(yùn)動(dòng)的影響。如圖4所示。

圖4 俯仰角受力分析圖

當(dāng)飛行器在做俯仰運(yùn)動(dòng)時(shí)，電機(jī)1和電機(jī)2的輸出升力為正且方向與z軸平行，現(xiàn)選取電機(jī)4和電機(jī)6的向下運(yùn)動(dòng)的方向?yàn)楦┭鼋堑恼较颍渲酗w行器機(jī)架上電機(jī)軸心到機(jī)架重心點(diǎn)的距離為L(zhǎng)，則有如下方程：

(1)

L3,5=L4,6=Lcosθ1

Fi=KscUii=3,4,5,6

故可得：

(2)

(2) 橫滾角的力矩平衡模型。針對(duì)六旋翼飛行器繞x軸轉(zhuǎn)動(dòng)即橫滾角方向，進(jìn)行平衡受力分析，如圖5所示。

圖5 橫滾角受力分析圖

當(dāng)飛行器在做橫滾運(yùn)動(dòng)時(shí)，選取電機(jī)2向上運(yùn)動(dòng)的方向?yàn)闄M滾角的正方向，則有如下方程：

(3)

L1=L2=LL3,6=L4,5=Lcosθ2

由此可得：

U4cosθ2-U5cosθ2)

(4)

(3) 偏航角的力矩平衡模型。當(dāng)飛行器在做偏航運(yùn)動(dòng)時(shí)，選取繞z軸逆時(shí)針轉(zhuǎn)動(dòng)的方向?yàn)檎瑒t有：

(5)

(6)

將飛行器各結(jié)構(gòu)部分簡(jiǎn)化并視作質(zhì)點(diǎn)時(shí)，由式(2)、式(4)、式(6)可知轉(zhuǎn)動(dòng)慣量分別為：

(7)

同時(shí)得到姿態(tài)角的力矩平衡方程為：

(8)

1.3 建立飛行器狀態(tài)方程

由式(8)可知各姿態(tài)角輸出與輸入電壓之間的關(guān)系，考慮到姿態(tài)角(主要為橫滾角與俯仰角)之間的耦合關(guān)系，對(duì)其進(jìn)行解耦處理。現(xiàn)定義五個(gè)輸入變量Vi(i=1,2,3,4,5)，V=[V1,V2,V3,V4,V5]T，并滿足以下條件：

(9)

通過式(9)的分配關(guān)系來確定輸入的控制量，同時(shí)選定九個(gè)系統(tǒng)狀態(tài)變量建立空間狀態(tài)方程：

(10)

式(10)中的系數(shù)矩陣為：

對(duì)六旋翼飛行器實(shí)際參數(shù)的測(cè)量，各項(xiàng)具體參數(shù)如表1所示。

表1 六旋翼飛行器參數(shù)

2 獲取光流信息

為了確定飛行器與地面之間的位姿關(guān)系，現(xiàn)規(guī)定大地坐標(biāo)系G和飛行器坐標(biāo)系F，則飛行器在大地坐標(biāo)系中的位置為：η=[X,Y,Z],而飛行器繞飛行器坐標(biāo)系轉(zhuǎn)動(dòng)的姿態(tài)角(偏航角，俯仰角，橫滾角)可表示為：ζ=[α，β，γ]T。因此，可定義飛行器坐標(biāo)系相對(duì)于大地坐標(biāo)系的轉(zhuǎn)動(dòng)角速度在其自身各軸上的投影為σ=[σ1，σ2，σ3]T，并假設(shè)光流模塊安裝在飛行器的重心點(diǎn)處，同時(shí)PX4Flow光流相機(jī)的鏡頭焦距為16 mm。

2.1 目標(biāo)點(diǎn)與鏡頭的映射關(guān)系

目標(biāo)點(diǎn)動(dòng)態(tài)映射如圖6所示。其中點(diǎn)N為三維空間內(nèi)的靜止目標(biāo)點(diǎn)，其坐標(biāo)為(X,Y,Z)∈R3,該點(diǎn)映射至球面鏡頭上的點(diǎn)Ms處，P2為鏡頭捕獲光流的窗口曲面，v為飛行器質(zhì)心相對(duì)于地面的平移速度，σ為飛行器繞飛行器坐標(biāo)系F轉(zhuǎn)動(dòng)的角速度，S2為球面鏡頭曲面。因此可知映射關(guān)系為：

(11)

圖6 目標(biāo)點(diǎn)動(dòng)態(tài)映射圖

由于目標(biāo)點(diǎn)相對(duì)大地坐標(biāo)系而言處于靜止?fàn)顟B(tài)，所以當(dāng)鏡頭捕捉到目標(biāo)點(diǎn)的水平運(yùn)動(dòng)時(shí)，即表示飛行器在相對(duì)目標(biāo)點(diǎn)發(fā)生位置移動(dòng)。因此，從目標(biāo)點(diǎn)至鏡頭曲面的映射半徑可表示為[9]：

(12)

(13)

(14)

式中：K的正方向?yàn)閳D6中豎直向下的慣性方向；μ為目標(biāo)點(diǎn)映射到鏡頭曲面上的夾角。

2.2 水平移動(dòng)光流的獲取

(15)

(16)

(17)

3 系統(tǒng)控制器的設(shè)計(jì)

本文利用LQR(線性二次型)內(nèi)控制回路在飛行器通過外部光流控制回路進(jìn)行定點(diǎn)飛行，有效避免光流傳感器受到外界光照強(qiáng)度變化(比如在環(huán)境光照強(qiáng)度較差或地面反光嚴(yán)重的情況)的影響，以此實(shí)現(xiàn)六旋翼飛行器在環(huán)境照明條件較差或強(qiáng)光情況下的光流定點(diǎn)的穩(wěn)定姿態(tài)控制[10]。

3.1 內(nèi)部姿態(tài)角速度控制回路的設(shè)計(jì)

當(dāng)飛行器控制系統(tǒng)的狀態(tài)方程簡(jiǎn)化為線性系統(tǒng)狀態(tài)方程時(shí)，LQR作為適用于線性系統(tǒng)最優(yōu)化控制的方法，可通過確定飛行器的系統(tǒng)狀態(tài)變量及其被控輸入變量所構(gòu)成的二次型函數(shù)來作為衡量系統(tǒng)是否達(dá)到了最優(yōu)化控制的穩(wěn)定狀態(tài)。同時(shí)求解出該二次型積分函數(shù)的最小值，使其控制系統(tǒng)達(dá)到最優(yōu)控制狀態(tài)，完成最優(yōu)化控制器模型的設(shè)計(jì)[11]。

本文將采用LQR控制方法來實(shí)現(xiàn)內(nèi)部姿態(tài)角速度控制回路的設(shè)計(jì)，即分別針對(duì)三個(gè)姿態(tài)角速度(橫滾角速度、俯仰角速度以及偏航角速度)設(shè)計(jì)相應(yīng)的LQR內(nèi)回路控制器。在這個(gè)控制過程中將系統(tǒng)的狀態(tài)變量輸入內(nèi)回路后所產(chǎn)生的誤差視為新的變量，并把新變量作為輸入信號(hào)再次輸入該控制回路。因此，可通過這樣的方式消除該內(nèi)控制回路中引入系統(tǒng)狀態(tài)變量時(shí)所帶來的誤差[12]，達(dá)到提高系統(tǒng)實(shí)時(shí)性、精確跟蹤輸入變量的目的。

由于在內(nèi)部控制回路中重新將誤差變量e引入進(jìn)系統(tǒng)控制回路，故設(shè)參照輸入變量為j，其有待持續(xù)觀測(cè)的輸出變量為p，并滿足p=Ccx，則誤差變量為e=j-p，結(jié)合式(10)得到新的系統(tǒng)狀態(tài)空間表達(dá)式：

(18)

(19)

式中：W和M都為半正定陣；R為正定陣；t1為固定的終端時(shí)間，且系統(tǒng)終端狀態(tài)z(t1)自由；W為系統(tǒng)狀態(tài)變量的加權(quán)矩陣；M為輸入向量的加權(quán)矩陣。因此只需保證性能函數(shù)最小，就能得到輸入信號(hào)的最優(yōu)控制狀態(tài)，所以先構(gòu)造一個(gè)與性能指標(biāo)函數(shù)相關(guān)的Hamilton函數(shù)。

(20)

當(dāng)不對(duì)輸入變量進(jìn)行限制時(shí)，對(duì)式(20)求導(dǎo)，則有：

圖7 內(nèi)回路控制框圖

故可知最佳輸入信號(hào)應(yīng)為：

(21)

所以在進(jìn)行內(nèi)控制回路的設(shè)計(jì)時(shí)，要選取合適的加權(quán)矩陣W和R來保證控制系統(tǒng)的整體性能，即當(dāng)需要系統(tǒng)有良好的輸出性能時(shí)，應(yīng)將W主對(duì)角線上的系數(shù)調(diào)高，倘若需要降低系統(tǒng)輸出信號(hào)時(shí)，則應(yīng)將R的參數(shù)調(diào)高。當(dāng)選取的加權(quán)矩陣W和R能夠較好地平衡輸出信號(hào)與輸出性能時(shí)，就實(shí)現(xiàn)了系統(tǒng)的最優(yōu)化控制。此時(shí)，在確定了矩陣A、B和C及其加權(quán)矩陣W、R的前提下，可通過MATLAB求解出全狀態(tài)反饋增益矩陣K。

3.2 外部光流控制回路的設(shè)計(jì)

本文在利用LQR實(shí)現(xiàn)內(nèi)部姿態(tài)角速度控制的基礎(chǔ)上，采用PI控制器對(duì)光流信號(hào)進(jìn)行調(diào)節(jié)控制，以實(shí)現(xiàn)飛行器的懸停控制。由第2節(jié)的內(nèi)容可知λ和d(t)/ds0是可計(jì)算量，故結(jié)合式(16)和式(17)可知穩(wěn)定懸停狀態(tài)下飛行器的動(dòng)力學(xué)方程為[9]：

(22)

(23)

式中：kP、kI都大于0，Rd表示預(yù)期飛行器飛行的方向；F表示飛行器的升力；EZ表示大小為[0,0,1]T的列向量；FG表示飛行器的重力；Δ表示為外界施加給飛行器的干擾力。因此可知飛行器的線速度v趨向于0。當(dāng)目標(biāo)點(diǎn)所處的平面位于大地坐標(biāo)系的水平面(xoy)上，且飛行器的期望方向與xoy平面平行時(shí)，飛行器在穩(wěn)定懸停的狀態(tài)下，可認(rèn)為目標(biāo)點(diǎn)到飛行器坐標(biāo)系的正交距離d，即為光流鏡頭到目標(biāo)點(diǎn)所處目標(biāo)平面的高度，這時(shí)PI控制器的閉環(huán)控制可簡(jiǎn)化為：

(24)

4 實(shí)驗(yàn)過程及其結(jié)果

在進(jìn)行實(shí)驗(yàn)前對(duì)飛行器定點(diǎn)飛行的穩(wěn)定性進(jìn)行了相關(guān)的分析驗(yàn)證，同時(shí)對(duì)本次實(shí)驗(yàn)的各項(xiàng)參數(shù)進(jìn)行設(shè)定。其中光流鏡頭可觀測(cè)的范圍為80°，飛行器實(shí)驗(yàn)時(shí)飛行高度為1 m，外回路光流PI控制器中kP、kI分別為1.05、0.32，飛行器前向速度1.2 m/s，光流模塊采樣頻率25 Hz。假設(shè)實(shí)驗(yàn)時(shí)飛行器的最大偏移量為12 cm，各姿態(tài)角的誤差在±3°內(nèi)，而偏移速度為1.6 m/s。實(shí)驗(yàn)采用類比法對(duì)實(shí)驗(yàn)結(jié)果進(jìn)行比對(duì)，并采用Lucas-Kanade方法估算光流。

4.1 實(shí)驗(yàn)1

進(jìn)行實(shí)驗(yàn)1前，需用鏡頭蓋遮擋住光流鏡頭，其目的是為了模擬在實(shí)驗(yàn)地面上無可識(shí)別目標(biāo)點(diǎn)的定位情況，同時(shí)選取紋理清晰的大理石地面作為飛行器試飛地面即目標(biāo)點(diǎn)所處平面。由圖8和圖9中的飛行器的軌跡信息與光流信息可知，在遮擋光流的情況下飛行器的漂移軌跡范圍較大，其偏移量幾乎達(dá)到了允許偏移量的最大值，且飛行器在xoy平面上的偏移方向不確定，但在z軸方向上的位移量大致為±4 cm。就整體而言，飛行器在內(nèi)姿態(tài)角速度回路的控制下，其漂移軌跡都在預(yù)設(shè)邊界的內(nèi)部，即表明無人機(jī)懸停的位置在允許范圍內(nèi)。同時(shí)可以看出飛行器位置的偏移變化大致與各軸的光流變化趨勢(shì)一致。因此，實(shí)驗(yàn)1充分驗(yàn)證了內(nèi)控制回路的可行性。

圖8 飛行器飛行軌跡

圖9 實(shí)際位置與對(duì)應(yīng)光流

4.2 實(shí)驗(yàn)2

進(jìn)行實(shí)驗(yàn)2前，將鏡頭蓋取下保證光流鏡頭不被遮擋，且處于正常工作狀態(tài)。飛行器軌跡信息與光流信息如圖10和圖11所示。可以看出：雖然光流在某個(gè)時(shí)間段變化劇烈，但整體上都在遠(yuǎn)小于±0.1的范圍，故可認(rèn)為光流趨于0且變化穩(wěn)定；而z軸的位置變化量在±3 cm范圍內(nèi)，x、y軸的位置偏移量也在±3 cm內(nèi)。所以，各軸所對(duì)應(yīng)的光流變化趨勢(shì)都與相應(yīng)的各軸位置偏移大致保持一致。因此，實(shí)驗(yàn)2在實(shí)驗(yàn)1的基礎(chǔ)上充分證明了外控制回路的可行性，同時(shí)也驗(yàn)證了本文提出的內(nèi)、外回路控制方法是有效的。

圖10 飛行器飛行軌跡

圖11 實(shí)際位置與對(duì)應(yīng)光流

實(shí)驗(yàn)1的實(shí)驗(yàn)結(jié)果表明在遮擋光流的情況下，由于光流無法正常工作致使飛行器無法獲取光流數(shù)據(jù)，進(jìn)而導(dǎo)致外部控制回路的失效。雖然飛行器能夠在預(yù)設(shè)邊界內(nèi)正常飛行，但此時(shí)飛行器的水平飛行軌跡漂移量較大，且漂移方向不明確。而實(shí)驗(yàn)2則是在光流正常工作的情況下進(jìn)行，當(dāng)前向速度保持不變時(shí)，飛行器各軸位置及其各軸所對(duì)應(yīng)的光流值都趨近于預(yù)想期望值，且飛行器在±3 cm的水平方向范圍內(nèi)穩(wěn)定飛行，同時(shí)在z軸方向上的偏移量也為±3 cm。表2為20組飛行實(shí)驗(yàn)的統(tǒng)計(jì)結(jié)果，其中各項(xiàng)數(shù)據(jù)都以平均值±標(biāo)準(zhǔn)差來表示。

表2 實(shí)驗(yàn)數(shù)據(jù)統(tǒng)計(jì)

4.3 實(shí)驗(yàn)實(shí)時(shí)控制分析

由式(6)中六旋翼飛行器的狀態(tài)空間模型，可提取相關(guān)參數(shù)，并令其增益矩陣Kp、Kr和Ky分別為：Kp=[1.54,2.30]，Kr=[0.83,1.28]，Ky=[0.62,1.03]。而針對(duì)俯仰角、橫滾角和偏航角的PI控制器其比例系數(shù)為1.35、1.51、1.70，積分系數(shù)為0.24、0.29、0.33。故由本文提出的雙回路非線性控制器的方法，可得其姿態(tài)角速度的階躍響應(yīng)，如圖12所示。

圖12 姿態(tài)角速度階躍響應(yīng)

由此可知，本文提出的控制方法能夠較快地跟上輸入量且超調(diào)量較小，可達(dá)到較為理想的控制效果。同時(shí)由LQR控制的特點(diǎn)可知，對(duì)內(nèi)部姿態(tài)角速度的干擾輸入，系統(tǒng)將把該誤差作為狀態(tài)變量，使姿態(tài)角速度響應(yīng)能夠快速到達(dá)目標(biāo)點(diǎn)位置，且該調(diào)整過程中實(shí)時(shí)性較為理想，在3 s內(nèi)即可完成飛行器的穩(wěn)定定點(diǎn)。

5 結(jié) 語

本文提出了一種應(yīng)用雙回路非線性控制器的方法，結(jié)合外部光流PI控制回路和內(nèi)部姿態(tài)角速度LQR控制回路，實(shí)現(xiàn)了飛行器定點(diǎn)懸停任務(wù)。實(shí)驗(yàn)結(jié)果表明當(dāng)光流失效時(shí)，飛行器的漂移范圍較大且漂移方向未知，同時(shí)z軸方向上的偏移量在±4 cm范圍內(nèi)，此時(shí)飛行器仍在預(yù)設(shè)邊界的范圍內(nèi)正常飛行，但定點(diǎn)效果較差。而在光流正常工作時(shí)，其平均誤差在0.04±0.03 m范圍內(nèi)，而水平方向上的偏移量和z軸方向上的偏移量都在±3 cm內(nèi)，此時(shí)飛行器定點(diǎn)精度較為理想。