四旋翼飛行器航跡規(guī)劃和控制研究

王彪，周杰，孔大慶，唐超穎

(南京航空航天大學(xué) 自動(dòng)化學(xué)院，江蘇 南京 211106)

0 引言

四旋翼無人機(jī)是一種由四個(gè)旋翼驅(qū)動(dòng)的飛行器，與單旋翼直升機(jī)相比，具有體積小、成本低、維護(hù)和操作簡單等優(yōu)點(diǎn)。因此非常適合在城市低空、狹窄街道以及室內(nèi)完成高難度、高危險(xiǎn)的任務(wù)[1]。由于室內(nèi)環(huán)境復(fù)雜，能夠提供的飛行空間有限，因此需要以平滑且曲折度低的較優(yōu)路徑躲避障礙物，確保安全、快速地到達(dá)終點(diǎn)。

RRT路徑規(guī)劃算法在無障礙的空間內(nèi)進(jìn)行采樣，以逐級遞增的方式生成一顆連接起點(diǎn)和目標(biāo)點(diǎn)的搜索樹。該算法不需要啟發(fā)函數(shù)，具有很強(qiáng)的搜索傾向，適用于小型四旋翼飛行器的室內(nèi)航跡規(guī)劃問題[2]。因此本文采用RRT算法規(guī)劃航跡，進(jìn)而考慮四旋翼飛行器的非線性運(yùn)動(dòng)特性，很多國內(nèi)外研究人員提出了多種基于非線性控制方法的控制器，如H無窮控制[3]、反演控制[4]、滑膜控制[5]、自適應(yīng)控制[6]等等，以提高四旋翼飛行器的運(yùn)動(dòng)性能?？墒欠蔷€性控制器的結(jié)構(gòu)復(fù)雜，工程可行性較低，而當(dāng)四旋翼飛行器處于低速巡航模式時(shí)，姿態(tài)運(yùn)動(dòng)和質(zhì)心運(yùn)動(dòng)各通道近似解耦，可用傳遞函數(shù)來描述其輸入輸出特性，因此可基于六自由度通道的傳遞函數(shù)模型進(jìn)行航跡和航姿控制器的設(shè)計(jì)。

本文使用激光掃描儀對室內(nèi)場景建模并規(guī)劃避障路徑，采用四旋翼六自由度MPC控制策略，完成對避障航跡的準(zhǔn)確跟蹤。與其他文獻(xiàn)不同，本文采用完整的四旋翼動(dòng)力學(xué)模型，不僅考慮執(zhí)行機(jī)構(gòu)的動(dòng)態(tài)特性，并且控制輸入為傳統(tǒng)的操縱桿量，使得所設(shè)計(jì)的控制律可以與物理平臺(tái)直接對接。綜上，給出了工程可行性高的室內(nèi)飛行探索方案。

1 室內(nèi)二維航跡規(guī)劃

1.1 二維環(huán)境建模

本文以室內(nèi)實(shí)際飛行環(huán)境為背景，如圖1所示。通過激光掃描儀獲得的環(huán)境信息對該場景建模，結(jié)果如圖2所示。圖中黑色線條為真實(shí)場景內(nèi)的障礙物，如紙盒和墻邊界。場景建模的范圍為：

圖1 實(shí)驗(yàn)場景

(1)

式中：xc和yc分別為四旋翼飛行器的x、y軸坐標(biāo)，單位為mm。

圖2 場景模型

1.2 RRT算法

Cfree表示去除場景障礙物后的無障礙區(qū)域，用Tk表示建立的擴(kuò)展樹，為Tk擴(kuò)展的步長，并令xinit和xgoal分別為初始點(diǎn)和目標(biāo)點(diǎn)。則RRT算法流程如圖3所示。

圖3 RRT算法流程

1.3 規(guī)劃航跡生成

由RRT算法原理可知其并不具備啟發(fā)信息，而是通過隨機(jī)生成采樣點(diǎn)的方式來搜索空間，這會(huì)導(dǎo)致規(guī)劃出的路徑比較曲折，增加了航跡的長度和四旋翼飛行器的飛行難度。因此，需要對規(guī)劃出的初始航跡進(jìn)行平滑處理進(jìn)生成參考飛行軌跡。

初始航跡平滑處理方法：針對規(guī)劃出的航跡點(diǎn)，按10個(gè)1組的方式得到航跡段Pi(i=1,2,3…)。針對Pi和Pi+1，在2個(gè)航跡段中分別隨機(jī)采樣，連接隨機(jī)采得的2個(gè)航跡點(diǎn)得到參考航跡段Ri[7]。并考慮3條限制條件：1)Ri與附近障礙物不發(fā)生碰撞；2)Ri與Ri-1斜率之差的絕對值

經(jīng)過平滑處理得到每一段參考軌跡段Ri，考慮按長度平均的方式得到10個(gè)航跡點(diǎn)，其坐標(biāo)作為航跡控制器的參考輸入。

2 航跡、航姿控制器設(shè)計(jì)

設(shè)計(jì)控制器對四旋翼位置和姿態(tài)的6個(gè)通道分別進(jìn)行控制，控制結(jié)構(gòu)如圖4所示。外環(huán)是位置控制，給定輸入為x、y、z三軸參考位置，輸出是標(biāo)準(zhǔn)總距、期望滾轉(zhuǎn)角和俯仰角的偏差量。內(nèi)環(huán)完成姿態(tài)控制，輸入是外環(huán)提供的期望滾轉(zhuǎn)角和俯仰角偏差量以及期望偏航角，輸出為滾轉(zhuǎn)、俯仰、偏航通道標(biāo)準(zhǔn)桿量偏差量。

圖4 MPC控制器結(jié)構(gòu)示意圖

2.1 預(yù)測模型建立

首先將文獻(xiàn)[8] 的四旋翼動(dòng)力學(xué)模型進(jìn)行線性化處理得到空間描述形式如式(2)：

(2)

控制量U、測量輸出Y、狀態(tài)量X依次為：

U=[δcolδlonδlatδped]T

Y=[xyzuvwφθψpqr]T

X=[xyzuvwφθψpqrΩ1Ω2Ω3Ω4]T

其中，x、y、z為導(dǎo)航坐標(biāo)系中機(jī)體重心位置，φ、θ、ψ為機(jī)體姿態(tài)角，p、q、r為角速度機(jī)體坐標(biāo)分量。特別注意以上各變量含義是相對于平衡點(diǎn)的偏差量，并且各旋翼轉(zhuǎn)速不可測量。由于四旋翼飛行器為線性時(shí)變系統(tǒng)，即反映特性的系數(shù)矩陣A、B會(huì)隨著飛行狀態(tài)的變化而改變，為了降低控制器設(shè)計(jì)復(fù)雜度，選擇耦合程度最低的飛行器懸停點(diǎn)狀態(tài)模型來設(shè)計(jì)。

由式(2)可得四旋翼六自由度通道輸入輸出傳遞函數(shù)。本節(jié)以俯仰通道為例，介紹其預(yù)測模型建立過程，其他通道類似。

俯仰通道輸入輸出傳遞函數(shù)：

(3)

將式(3)離散化，定義k時(shí)刻俯仰通道控制輸入量δlat(k)和輸出量θ(k)，再定義其z變換分別為u(z)和yo(z)，則俯仰通道的離散特性可用如下脈沖傳遞函數(shù)來描述：

(4)

在此基礎(chǔ)上可得到俯仰通道離散化模型為：

(5)

式中：xpitch=[yo(k)yo(k-1)yo(k-2)u(k-1)u(k-2)]T

對于上述離散化模型，參考經(jīng)典預(yù)測模型建立方法[9]。定義狀態(tài)增量與控制增量分別為：

(6)

再定義增廣狀態(tài)向量：

(7)

其中，r(k)為參考量。建立以xarg(k)為狀態(tài)量，Δu(k)為控制量, 以跟蹤誤差為輸出的滾轉(zhuǎn)通道增廣模型：

(8)

由式(8)可建立如下形式的經(jīng)典預(yù)測模型：

Xarg=Exarg(k)+ΦΔU

(9)

式中：E=[AA2A3…ANp]T，

使用拉蓋爾基函數(shù)，以低維向量表征控制量序列，從而減少運(yùn)算量[10]。拉蓋爾基函數(shù)可用來表示各時(shí)刻最優(yōu)控制增量，形式如式(10)所示：

Δu(k+i)=L(k)η

(10)

式中:η=[c1c2c3…cN]T。

將式(10)代入式(9)得到拉蓋爾函數(shù)形式預(yù)測模型：

(11)

2.2 控制量推導(dǎo)

預(yù)測控制器的增益通過最小化目標(biāo)函數(shù)得到，本文使用成熟的二次型表達(dá)式[11]：

(12)

把增廣狀態(tài)向量和用拉蓋爾離散表達(dá)的最優(yōu)控制增量式代入目標(biāo)函數(shù)可得：

(13)

式中：QL=CTQC，RL為對角矩陣，其階數(shù)為所使用拉蓋爾基函數(shù)維度N，對角線元素仍為權(quán)重值rw。為了將目標(biāo)函數(shù)寫成待求參數(shù)η的表達(dá)形式，代入式(11)的預(yù)測模型得：

(14)

由于采用懸停點(diǎn)線性化設(shè)計(jì)，外環(huán)輸出的期望姿態(tài)角偏差量需有0.2 rad的幅值約束，因此外環(huán)控制未來第k時(shí)刻控制量的幅值應(yīng)該滿足：

umin≤Mkη+u(k-1)≤umax

(15)

由式(15)可以寫出如下線性不等式，M為約束邊界。

(16)

M的列數(shù)n表示要考慮未來第1到第n個(gè)時(shí)刻的約束條件。式(16)和(14)的求解為標(biāo)準(zhǔn)二次型優(yōu)化問題。使用簡單易行的希爾德雷思算法解決該問題[12]，得到最優(yōu)解：

η=-H-1(G+MT*)

(17)

Δu(k)=L(0)Tη

(18)

最優(yōu)控制偏差量為u(k)=u(k-1)+Δu(k)，該偏差加上配平值最終可得到最優(yōu)輸出量。

3 仿真結(jié)果

下面驗(yàn)證本文提出的室內(nèi)環(huán)境飛行探索方案可行性。仿真結(jié)果的x、y軸位置跟蹤誤差如圖5所示，實(shí)線是x軸方向跟蹤誤差，虛線是y軸方向跟蹤誤差。誤差收斂至0.25 m范圍內(nèi)，有較好跟蹤精度。最終軌跡跟蹤效果如圖6所示。第二節(jié)給出的參考航跡點(diǎn)坐標(biāo)作為航跡控制器的參考輸入，注意仿真中z軸參考位置設(shè)置為-1m。圖7為航跡規(guī)劃結(jié)果，RRT算法以增加節(jié)點(diǎn)的方式擴(kuò)展隨機(jī)樹，完成該過程后，從目標(biāo)位置點(diǎn)依次找父節(jié)點(diǎn)，直至找到根節(jié)點(diǎn)為止。這樣規(guī)劃出一條從初始位置到達(dá)目標(biāo)點(diǎn)的無障礙最短路徑，如圖7虛線所示。經(jīng)過平滑處理的參考航跡段如圖7實(shí)線所示，可見曲折度明顯減小而且航跡長度也被有效縮短 。

圖5 軌跡跟蹤誤差

圖6 軌跡跟蹤效果

圖7 航跡規(guī)劃結(jié)果

4 結(jié)語

基于激光掃描儀傳感器建立的真實(shí)室內(nèi)場景，由RRT算法規(guī)劃出的路徑并通過平滑處理得到曲折度較低的飛行軌跡。再基于單通道輸入輸出傳遞函數(shù)模型建立預(yù)測模型，進(jìn)而設(shè)計(jì)級聯(lián)形式的MPC控制器方案。最后以MATLAB仿真的形式驗(yàn)證了飛行控制效果，結(jié)果表明所設(shè)計(jì)的控制器有較好的控制性能，該飛行探索方案具有一定工程可行性。