曹 寧，許有熊

（1.江蘇聯(lián)合職業(yè)技術(shù)學(xué)院，江蘇 蘇州 215000；2.南京工程學(xué)院 ，江蘇 南京 211167）

關(guān)鍵字：多旋翼無人機(jī)；變速運(yùn)動(dòng)平臺(tái)；模型預(yù)測(cè)控制（MPC）算法；自主著陸

多旋翼無人機(jī)高機(jī)動(dòng)性及其懸停能力使其在巡檢、航拍、消防、軍事等眾多領(lǐng)域中廣泛使用，無人機(jī)在移動(dòng)平臺(tái)的自主著陸成為各國飛行器研發(fā)領(lǐng)域的熱點(diǎn)，在變速運(yùn)動(dòng)平臺(tái)上的著陸更是本領(lǐng)域的前沿技術(shù)，具有廣闊應(yīng)用場(chǎng)景。目前，針對(duì)基于移動(dòng)平臺(tái)的無人機(jī)自主著陸的研究，金紅新等[1]提出了一種基于魯棒高階容積濾波的慣導(dǎo)/視覺相對(duì)導(dǎo)航方法，在仿真實(shí)驗(yàn)中實(shí)現(xiàn)無人機(jī)與移動(dòng)平臺(tái)相對(duì)位置、速度和姿態(tài)的估算，但其算法復(fù)雜、工程實(shí)現(xiàn)難度大；賈配洋等[2]提出改進(jìn)的AprilTags算法計(jì)算出目標(biāo)和相機(jī)之間的三維位置坐標(biāo)和姿態(tài)數(shù)據(jù)，采用最小二乘法結(jié)合PID算法控制無人機(jī)對(duì)移動(dòng)目標(biāo)的智能識(shí)別、跟蹤及移動(dòng)降落，但其穩(wěn)定性不足；洪亮等[3]采用模糊預(yù)測(cè)同步視覺預(yù)測(cè)數(shù)據(jù)，提高卡爾曼濾波對(duì)無人機(jī)位姿估計(jì)的實(shí)時(shí)性，速度快但精度較低；修素樸等[4]通過解耦無人機(jī)的位置和姿態(tài)提升對(duì)無人機(jī)控制的穩(wěn)定性并用5次多項(xiàng)式結(jié)合PID控制來規(guī)劃軌跡，實(shí)現(xiàn)無人機(jī)在斜面上或移動(dòng)平臺(tái)的降落，但系統(tǒng)的穩(wěn)定性和跟蹤精度有待提高；王曉海等[5]采用基于擴(kuò)展的雙反饋模型預(yù)測(cè)控制算法設(shè)計(jì)控制器，以期解決固定翼無人機(jī)BTT的轉(zhuǎn)彎控制問題。綜上所述，一方面無人機(jī)自主降落過程涉及計(jì)算機(jī)視覺、控制算法、軌跡預(yù)測(cè)等知識(shí)，存在魯棒性差、控制響應(yīng)延時(shí)等問題；另一方面，目前國內(nèi)暫時(shí)對(duì)多旋翼無人機(jī)降落于勻速運(yùn)動(dòng)平臺(tái)的研究較為深入，鮮有對(duì)多旋翼無人機(jī)在變速運(yùn)動(dòng)平臺(tái)上著陸的控制方法做深入研究。

基于此，本文選擇多旋翼無人機(jī)的組合導(dǎo)航傳感系統(tǒng)確定與移動(dòng)平臺(tái)相對(duì)位置，結(jié)合模型預(yù)測(cè)控制（MPC）算法實(shí)現(xiàn)無人機(jī)在變速運(yùn)動(dòng)平臺(tái)的自主著陸。該算法能夠顯式地考慮各種約束條件，并滾動(dòng)實(shí)時(shí)控制輸入，具有設(shè)計(jì)簡(jiǎn)單、魯棒性強(qiáng)、穩(wěn)定性好等優(yōu)點(diǎn)，故在多機(jī)協(xié)同領(lǐng)域中廣泛應(yīng)用[6]。

1 自主著陸流程

一般來說，多旋翼無人機(jī)在移動(dòng)平臺(tái)上的自主著陸流程[7]為通過GNSS導(dǎo)航及氣壓傳感器模塊采集信息，獲取無人機(jī)與移動(dòng)平臺(tái)在大地坐標(biāo)系中的位置矢量和速度矢量，通過坐標(biāo)變換后將各種矢量參數(shù)轉(zhuǎn)化到統(tǒng)一坐標(biāo)系中，經(jīng)過卡爾曼濾波器提升各參數(shù)精度，并將濾波后的數(shù)據(jù)通過模型預(yù)測(cè)控制算法計(jì)算出無人機(jī)最優(yōu)控制矢量。根據(jù)所得的計(jì)算結(jié)果實(shí)現(xiàn)多旋翼無人機(jī)的速度矢量和位置矢量調(diào)整，如此往復(fù)實(shí)現(xiàn)閉環(huán)控制，最終實(shí)現(xiàn)無人機(jī)在移動(dòng)平臺(tái)上的自主著陸?？傮w流程如圖1所示。

圖1 無人機(jī)（UAV）自主著陸流程圖

2 自主著陸方法

2.1 著陸系統(tǒng)坐標(biāo)定義

由于多旋翼無人機(jī)飛行速度低，無彈性形變，故可被視為六自由度剛體對(duì)象。為了能更精確地描述無人機(jī)與移動(dòng)平臺(tái)的運(yùn)動(dòng)特性，在研究其運(yùn)動(dòng)軌跡時(shí)需要涉及多個(gè)不同坐標(biāo)系，具體如下[8]:

（1）北東地坐標(biāo)系（NED）。原點(diǎn)為無人機(jī)質(zhì)心，遵循右手法則，z軸垂直于地球表面豎直向下，x軸指向地球正北方向，y軸指向正東方向。本文通過上標(biāo)n表示矢量處于此坐標(biāo)系。為便于后續(xù)算法的數(shù)據(jù)處理，無人機(jī)與移動(dòng)平臺(tái)的各矢量參數(shù)統(tǒng)一至此坐標(biāo)系后，再進(jìn)行運(yùn)算處理。

（2）地心地固坐標(biāo)系（ECEF）。與地球固聯(lián)，隨地球一起旋轉(zhuǎn)，坐標(biāo)系原點(diǎn)為地球質(zhì)量中心，z指向地球北極，x指向格林尼子午面與赤道的交點(diǎn)，y軸位于赤道平面內(nèi)正x軸東側(cè)90°方向，三個(gè)坐標(biāo)軸相互正交。本文通過上標(biāo)f表示矢量處于此坐標(biāo)系。

（3）東北天坐標(biāo)系（ENU）。原點(diǎn)為無人機(jī)質(zhì)心，遵循右手法則，x軸指向東，y軸指向北，z軸指向天。本文通過上標(biāo)e表示矢量處于此坐標(biāo)系。

（4）大地坐標(biāo)系（GCF）。廣泛應(yīng)用于GNSS系統(tǒng)中。該坐標(biāo)系用于經(jīng)度、緯度和高度來描述接近地球表面的點(diǎn)的位置。本文通過上標(biāo)ge表示矢量處于此坐標(biāo)系。

2.2 多旋翼無人機(jī)著陸位姿信息獲取

無人機(jī)與移動(dòng)平臺(tái)在大地坐標(biāo)系中的位置矢量分別為：

其中，使用GPS傳感器測(cè)得地球經(jīng)度λ與緯度Φ；用氣壓傳感器測(cè)得相對(duì)于海平面的高度h。

將Puge與Plge轉(zhuǎn)化為ECEF坐標(biāo)系中對(duì)應(yīng)的位置矢量[9]為：

通過文獻(xiàn)[9]中坐標(biāo)轉(zhuǎn)換方法，可求出無人機(jī)和移動(dòng)平臺(tái)在NED坐標(biāo)系中的位置矢量分別為：

其中，Pfloc為無人機(jī)起飛點(diǎn)在ECEF坐標(biāo)系中的坐標(biāo)；Pfu為無人機(jī)當(dāng)前在ECEF坐標(biāo)系中的位置矢量；Pfl為移動(dòng)平臺(tái)當(dāng)前在ECEF坐標(biāo)系中的位置矢量；和ωpu分別為無人機(jī)的測(cè)量誤差和高斯白噪聲；βnl/n和ωl分別為移動(dòng)平臺(tái)的測(cè)量誤差和高斯白噪聲；為從ECEF到NED的轉(zhuǎn)換矩陣。

其中，loc和λloc分別為無人機(jī)起飛點(diǎn)的經(jīng)度及緯度。

根據(jù)文獻(xiàn)[9]可得ENU坐標(biāo)系到NED坐標(biāo)系的轉(zhuǎn)換方法，則無人機(jī)和移動(dòng)平臺(tái)在NED坐標(biāo)系中的速度矢量為：

其中，通過GPS傳感器測(cè)量得到無人機(jī)和移動(dòng)平臺(tái)在ENU坐標(biāo)系中的速度矢量veu/e和vel/e；ωv為無人機(jī)和移動(dòng)平臺(tái)速度矢量的高斯白噪聲；(Θne)為ENU坐標(biāo)系到NED坐標(biāo)系的轉(zhuǎn)換矩陣。

2.3 卡爾曼濾波器

無人機(jī)中各傳感器干擾較為嚴(yán)重，直接測(cè)得的信號(hào)中夾雜大量噪聲，難以精準(zhǔn)獲得無人機(jī)的位置矢量和速度矢量，需加入濾波算法提高其信號(hào)測(cè)量精度。卡爾曼濾波算法[10]是用狀態(tài)方程和遞推方法實(shí)現(xiàn)估算的常用濾波算法，在系統(tǒng)中的作用如圖2所示。

圖2 卡爾曼濾波器在系統(tǒng)中的應(yīng)用

卡爾曼濾波器由預(yù)測(cè)模塊和更新模塊兩部分構(gòu)成。在預(yù)測(cè)模塊中，卡爾曼濾波器產(chǎn)生當(dāng)前狀態(tài)變量的估計(jì)值（x-k）及其不確定性（均方差矩陣 P-k）；檢測(cè)到下一次測(cè)量結(jié)果（包含測(cè)量誤差及隨機(jī)噪聲）后，采用加權(quán)平均的方式更新估計(jì)值，更高確定性的狀態(tài)變量占更高權(quán)重。卡爾曼濾波算法利用當(dāng)前觀測(cè)量和先驗(yàn)估算量及相應(yīng)不確定性矩陣進(jìn)行一種實(shí)時(shí)遞歸線性最小方差無偏估計(jì)。同時(shí)，由于該算法為遞推算法，僅利用當(dāng)前輸入測(cè)量值和前一步計(jì)算的狀態(tài)量及其不確定性矩陣實(shí)時(shí)運(yùn)行，不需要額外過去信息。其建模流程如圖3所示。

圖3 卡爾曼濾波算法建模流程

其中，xk是在tk時(shí)刻的狀態(tài)矢量，在無人機(jī)自主降落于移動(dòng)平臺(tái)的過程中，最重要的過程量即為兩者之間的相對(duì)位置矢量及移動(dòng)平臺(tái)的速度矢量。故本系統(tǒng)中選取狀態(tài)變量為控制量為輸出量為和分別為輸入轉(zhuǎn)換矩陣，I是3×3的對(duì)角矩陣；0是3×3矩陣；Δt是對(duì)角元素為tk-tk-1的3×3對(duì)角矩陣。Qk為系統(tǒng)噪聲協(xié)方差矩陣，Rk為觀測(cè)噪聲協(xié)方差矩陣，兩者均為半正定矩陣，可以通過調(diào)整Qk和Rk的值來使濾波器獲得更好的性能。

2.4 模型預(yù)測(cè)控制（MPC）算法

為使無人機(jī)精準(zhǔn)著陸，需要無人機(jī)與移動(dòng)平臺(tái)的物理位置與速度高度一致。其矢量模型如圖4所示。

圖4 無人機(jī)自主降落矢量模型

此任務(wù)中的無人機(jī)著陸狀態(tài)數(shù)學(xué)模型為：

其中，Pnu/n是無人機(jī)所處位置矢量；vnu/n是無人機(jī)的速度矢量；vnl/n是移動(dòng)平臺(tái)速度矢量；Pnl/n是移動(dòng)平臺(tái)所處位置矢量；Pnl/u是無人機(jī)相對(duì)于移動(dòng)平臺(tái)的位置矢量。為使無人機(jī)平穩(wěn)降落于移動(dòng)平臺(tái)上，須使無人機(jī)與移動(dòng)平臺(tái)之間的距離和相對(duì)速度盡量趨近于零。將該系統(tǒng)離散化，狀態(tài)方程如下所示：

其中，k=tk+1-tk為單位時(shí)間步長(zhǎng)，本研究中的單位時(shí)間步長(zhǎng)為定值。在無人機(jī)自主降落過程中，考慮到對(duì)其速度矢量和加速度進(jìn)行硬性約束，向量xlow，xhigh, ulow和uhigh分別為狀態(tài)量和輸入量的上下限。此外，向量uhigh和ulow分別為輸入變化的最大、最小值。Ak和Bk為轉(zhuǎn)換矩陣；xk是無人機(jī)系統(tǒng)狀態(tài)參量，uk為無人機(jī)系統(tǒng)輸入?yún)⒘?，代表含義如式（20）所示。

給出Ak和Bk矩陣：

I是3×3單位矩陣，0是3×3矩陣，所有元素都等于0，Δt是3×3矩陣，其對(duì)角線元素為相對(duì)時(shí)間步長(zhǎng)tk-tk-1，本研究中的相對(duì)時(shí)間步長(zhǎng)取固定值t，而非對(duì)角元素為0。本系統(tǒng)可視作時(shí)不變系統(tǒng)，則可在等式約束中建立離散時(shí)間模型，如下：

移動(dòng)平臺(tái)的無人機(jī)自主著陸系統(tǒng)是典型的多約束系統(tǒng)[11]，故選擇模型預(yù)測(cè)控制（MPC）算法對(duì)其控制。MPC算法控制過程如下：

（1）利用研究系統(tǒng)的預(yù)測(cè)信息優(yōu)化求解，獲取有限時(shí)域內(nèi)的最優(yōu)控制序列，并將控制序列中第一組控制變量作為系統(tǒng)的控制變量。

（2）根據(jù)傳感器測(cè)得的當(dāng)前實(shí)際輸出值，修正MPC模型預(yù)測(cè)的輸出值并重新求解最優(yōu)控制序列，并實(shí)現(xiàn)閉環(huán)反饋。

（3）在下一時(shí)刻重復(fù)以上過程，實(shí)現(xiàn)滾動(dòng)優(yōu)化和閉環(huán)控制。

本研究被控系統(tǒng)的數(shù)學(xué)模型為公式（14），則從k時(shí)刻開始，系統(tǒng)的輸入為：

則根據(jù)公式可以推測(cè)出系統(tǒng)狀態(tài)變量值[12-13]預(yù)測(cè)為：

用狀態(tài)方程形式描述為：

其中：P為預(yù)測(cè)步長(zhǎng)，M為控制步長(zhǎng)，通常P≥M。

在使用模型預(yù)測(cè)控制算法解決問題時(shí)會(huì)有一個(gè)優(yōu)化目標(biāo)，該優(yōu)化目標(biāo)與預(yù)測(cè)模型息息相關(guān)，通過對(duì)優(yōu)化目標(biāo)的最小化進(jìn)行求解，就會(huì)得到最優(yōu)的控制變量序列[14-15]。離散時(shí)間線性約束的系統(tǒng)優(yōu)化函數(shù)目標(biāo)可表示為公式（26）：

公式（26）中，Q和R矩陣用來平衡測(cè)量變量與輸入變量的權(quán)重，最優(yōu)序列的表達(dá)式為：

則實(shí)時(shí)控制變量為：

其中：

根據(jù)上述方法求得u(k)，然后將u(k)作為實(shí)際控制變量作用于系統(tǒng)對(duì)其進(jìn)行控制，隨著時(shí)間推移，不斷預(yù)測(cè)、滾動(dòng)優(yōu)化和控制。

3 仿真結(jié)果與分析

在MATLAB平臺(tái)下對(duì)上文中的無人機(jī)自主著陸方法進(jìn)行驗(yàn)證。仿真中使用的加權(quán)矩陣Q和R在公式（30）和（31）中給出，限制狀態(tài)的約束在公式（31）中給出，并且限制輸入的約束在公式（32）中給出。

公式（32）中在飛行區(qū)域?qū)︼w行高度沒有限制；公式（33）中的控制約束將無人機(jī)速度限制為±15 m/s，無人機(jī)加速度限制為±2.5 m/s2。

3.1 MPC算法不同預(yù)測(cè)步長(zhǎng)仿真結(jié)果

圖5說明了模型預(yù)測(cè)控制算法對(duì)無人機(jī)進(jìn)行控制過程中，選擇不同預(yù)測(cè)步長(zhǎng)時(shí)無人機(jī)的控制效果。圖5(b)顯示了仿真過程中無人機(jī)和移動(dòng)平臺(tái)各自的速度。從圖中可以看出，當(dāng)P=30時(shí)無人機(jī)自主著陸效果最佳。

圖5 移動(dòng)平臺(tái)恒速時(shí)具有不同預(yù)測(cè)長(zhǎng)度的無人機(jī)自主著陸軌跡圖

3.2 移動(dòng)平臺(tái)加速運(yùn)行

在此仿真任務(wù)中，移動(dòng)平臺(tái)以1 m/s2的加速度從0加速到10 m/s,然后以恒定速度繼續(xù)行駛。圖6（a）顯示當(dāng)預(yù)測(cè)步長(zhǎng)P=30時(shí)，無人機(jī)和移動(dòng)平臺(tái)的行駛路徑；圖6（b）表示無人機(jī)和移動(dòng)平臺(tái)各自行駛速度。可看出，在移動(dòng)平臺(tái)勻加速運(yùn)動(dòng)狀態(tài)下，無人機(jī)自主著陸性能表現(xiàn)良好。

圖6 移動(dòng)平臺(tái)勻加速運(yùn)動(dòng)下無人機(jī)自主著陸軌跡

3.3 移動(dòng)平臺(tái)隨機(jī)路徑行駛

在圖7所示的仿真任務(wù)中，移動(dòng)平臺(tái)在速度保持恒定的同時(shí)，將隨機(jī)噪聲添加于移動(dòng)平臺(tái)轉(zhuǎn)向角φ上，以控制移動(dòng)平臺(tái)以不規(guī)則路徑勻速行駛。圖7（a）與圖7（c）表示無人機(jī)和移動(dòng)平臺(tái)的行駛軌跡；圖7（b）表示圖7（a）中所示的隨機(jī)驅(qū)動(dòng)速度；圖7（d）表示圖7（c）中來自隨機(jī)驅(qū)動(dòng)的速度?？煽闯觯苿?dòng)平臺(tái)在隨機(jī)路徑行駛的狀態(tài)下，無人機(jī)自主著陸性能表現(xiàn)良好。

圖7 移動(dòng)平臺(tái)不規(guī)則行駛路徑下無人機(jī)自主著陸軌跡

4 結(jié)論

本文提出一種無人機(jī)自主降落于變速運(yùn)動(dòng)平臺(tái)的控制方法。通過加入卡爾曼濾波器提升無人機(jī)與移動(dòng)平臺(tái)相對(duì)位置矢量和速度矢量參數(shù)精度，并選用模型預(yù)測(cè)控制（MPC）算法對(duì)無人機(jī)在降落過程中進(jìn)行控制。通過MATLAB仿真證明，在移動(dòng)平臺(tái)做變速運(yùn)動(dòng)的情況下，該方法可有效實(shí)現(xiàn)無人機(jī)降落于移動(dòng)平臺(tái)上。