劉祚時 饒 遠

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

0 引 言

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

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

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

1.1 實驗平臺

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

圖1 飛行器實物圖

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

圖2 實驗系統(tǒng)整體架構(gòu)圖

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

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

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

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

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

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

圖4 俯仰角受力分析圖

當(dāng)飛行器在做俯仰運動時，電機1和電機2的輸出升力為正且方向與z軸平行，現(xiàn)選取電機4和電機6的向下運動的方向為俯仰角的正方向，其中飛行器機架上電機軸心到機架重心點的距離為L，則有如下方程：

(1)

L3,5=L4,6=Lcosθ1

Fi=KscUii=3,4,5,6

故可得：

(2)

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

圖5 橫滾角受力分析圖

當(dāng)飛行器在做橫滾運動時，選取電機2向上運動的方向為橫滾角的正方向，則有如下方程：

(3)

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

由此可得：

U4cosθ2-U5cosθ2)

(4)

(3) 偏航角的力矩平衡模型。當(dāng)飛行器在做偏航運動時，選取繞z軸逆時針轉(zhuǎn)動的方向為正，則有：

(5)

(6)

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

(7)

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

(8)

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

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

(9)

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

(10)

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

對六旋翼飛行器實際參數(shù)的測量，各項具體參數(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)動的姿態(tài)角(偏航角，俯仰角，橫滾角)可表示為：ζ=[α，β，γ]T。因此，可定義飛行器坐標(biāo)系相對于大地坐標(biāo)系的轉(zhuǎn)動角速度在其自身各軸上的投影為σ=[σ1，σ2，σ3]T，并假設(shè)光流模塊安裝在飛行器的重心點處，同時PX4Flow光流相機的鏡頭焦距為16 mm。

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

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

(11)

圖6 目標(biāo)點動態(tài)映射圖

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

(12)

(13)

(14)

式中：K的正方向為圖6中豎直向下的慣性方向；μ為目標(biāo)點映射到鏡頭曲面上的夾角。

2.2 水平移動光流的獲取

(15)

(16)

(17)

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

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

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

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

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

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

(18)

(19)

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

(20)

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

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

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

(21)

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

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

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

(22)

(23)

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

(24)

4 實驗過程及其結(jié)果

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

4.1 實驗1

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

圖8 飛行器飛行軌跡

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

4.2 實驗2

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

圖10 飛行器飛行軌跡

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

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

表2 實驗數(shù)據(jù)統(tǒng)計

4.3 實驗實時控制分析

由式(6)中六旋翼飛行器的狀態(tài)空間模型，可提取相關(guān)參數(shù)，并令其增益矩陣Kp、Kr和Ky分別為：Kp=[1.54,2.30]，Kr=[0.83,1.28]，Ky=[0.62,1.03]。而針對俯仰角、橫滾角和偏航角的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)量較小，可達到較為理想的控制效果。同時由LQR控制的特點可知，對內(nèi)部姿態(tài)角速度的干擾輸入，系統(tǒng)將把該誤差作為狀態(tài)變量，使姿態(tài)角速度響應(yīng)能夠快速到達目標(biāo)點位置，且該調(diào)整過程中實時性較為理想，在3 s內(nèi)即可完成飛行器的穩(wěn)定定點。

5 結(jié) 語

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