時(shí)空約束條件下多旋翼機(jī)動(dòng)軌跡優(yōu)化方法

蔡志浩，左一鳴，王英勛

（北京航空航天大學(xué) 自動(dòng)化科學(xué)與電氣工程學(xué)院，北京100083）

飛行器的航跡規(guī)劃指在給定的規(guī)劃空間內(nèi)，尋找飛行器從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)，且滿足某些約束條件，并使某項(xiàng)性能指標(biāo)達(dá)到最優(yōu)的軌跡［1］。航跡規(guī)劃可分為軌跡規(guī)劃和路徑規(guī)劃兩大類。軌跡規(guī)劃是一種考慮了飛行器的動(dòng)力學(xué)約束，從控制的角度進(jìn)行飛行軌跡的優(yōu)化，生成基于時(shí)間描述的空間曲線的規(guī)劃方法［2-5］。

近年來，多旋翼飛行器在震后搜救、地質(zhì)勘探、電力巡檢等民生方面得到了越來越多的應(yīng)用，這些任務(wù)場(chǎng)景往往有著環(huán)境復(fù)雜多變的特點(diǎn)，并且考慮到飛行器續(xù)航能力的限制，如何規(guī)劃其機(jī)動(dòng)飛行軌跡，滿足多旋翼在續(xù)航時(shí)間、地形地勢(shì)等時(shí)空上的約束條件有著重要意義。

文獻(xiàn)［6-7］將控制器與規(guī)劃相結(jié)合，生成一條由12維向量表示的軌跡，能夠?qū)崿F(xiàn)optitrack環(huán)境下的90°穿縫等激進(jìn)飛行；Tordesillas等［8］針對(duì)未知環(huán)境，開發(fā)了一種快速規(guī)劃算法，能在確保飛行安全性的同時(shí)提高飛行速度，飛行驗(yàn)證顯示最大飛行速度能達(dá)到7.8m/s；曾佳和申功璋［9］采用了Dijkstar算法進(jìn)行路徑的實(shí)時(shí)規(guī)劃，并基于飛機(jī)的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)特性，采用圓弧擬合方法來生成平滑的軌跡，仿真顯示，能夠生成滿足需求的平滑軌跡；陳杰等［10］針對(duì)螺旋樣條區(qū)域，建立了以飛行時(shí)間為代價(jià)函數(shù)的規(guī)劃問題，并采用二階泰勒展開法生成軌跡控制序列，仿真顯示，相比原先的折線式軌跡，新的平滑軌跡能大大縮減作業(yè)時(shí)間，提高效率；孟少華等［11］針對(duì)微小型直升機(jī)的低空避障問題，通過將飛行器的動(dòng)力學(xué)限制和空間障礙轉(zhuǎn)為不等式約束，建立起非線性最優(yōu)控制問題，采用高斯偽譜法進(jìn)行求解，仿真顯示，該方法可快速且準(zhǔn)確地實(shí)現(xiàn)避障。這些研究都針對(duì)設(shè)定環(huán)境的任務(wù)，從飛行器自身的機(jī)動(dòng)性能等角度，對(duì)飛行軌跡作了優(yōu)化，但對(duì)于穿縫、躲投擲物等激進(jìn)場(chǎng)景，往往還需要飛機(jī)有優(yōu)越的控制器響應(yīng)性能，且上述研究并沒有從任務(wù)環(huán)境本身出發(fā)來思考軌跡優(yōu)化方法，存在一定的局限性。

本文設(shè)定一定時(shí)間內(nèi)穿越室內(nèi)斜縫的典型任務(wù)場(chǎng)景，從針對(duì)性分析環(huán)境的角度出發(fā)，將時(shí)空約束具體化，并結(jié)合生活經(jīng)驗(yàn)和環(huán)境特點(diǎn)，設(shè)計(jì)了一種基于拋物原理和龐特里亞金極小值原理的多約束條件下軌跡優(yōu)化方法。將穿縫過程分解為靠近和穿越2個(gè)階段，在穿越段采用拋物思路優(yōu)化多旋翼的穿縫軌跡，在靠近段采用龐特里亞金極小值原理設(shè)計(jì)軌跡實(shí)現(xiàn)飛行狀態(tài)的轉(zhuǎn)換?？梢暬抡嬖囼?yàn)顯示，多旋翼最大能穿越豎直方向63°或水平方向32°傾斜的斜縫。

1 四旋翼模型

為了能準(zhǔn)確并直觀地觀察穿縫軌跡的優(yōu)化效果，需建立可用于仿真試驗(yàn)的飛行平臺(tái)，綜合考慮任務(wù)環(huán)境和性能需求，本文選用如圖1所示的大疆F330作為建模對(duì)象進(jìn)行研究。

圖1 大疆F330Fig.1 DJI F330

四旋翼的模型分為3個(gè)部分：動(dòng)力系統(tǒng)模型、控制效率模型、剛體模型。動(dòng)力系統(tǒng)模型由電機(jī)、電調(diào)、槳葉組成，由文獻(xiàn)［12-13］可知輸入的油門信號(hào)和輸出電機(jī)轉(zhuǎn)速之間成一次函數(shù)關(guān)系，且電機(jī)的響應(yīng)可由一階慣性環(huán)節(jié)表示，從而可建立動(dòng)力系統(tǒng)的數(shù)學(xué)模型：

式中：ωk為螺旋槳實(shí)際轉(zhuǎn)速；Tm為電機(jī)響應(yīng)時(shí)間常數(shù)；CR為油門和轉(zhuǎn)速線性關(guān)系比例系數(shù)；ω0為怠速轉(zhuǎn)速；σ為油門。

控制效率模型根據(jù)輸入的電機(jī)轉(zhuǎn)速及槳葉的拉力系數(shù)等四旋翼相關(guān)性能參數(shù)計(jì)算其在三軸上受到的力和力矩，可建立數(shù)學(xué)模型如下：

式中：f為四旋翼總拉力大??；τx、τy、τz分別為螺旋槳升力作用在四旋翼三軸方向上的力矩大?。籧t為槳葉拉力系數(shù)；cm為力矩系數(shù)；la為機(jī)臂長(zhǎng)度；ω1～ω4分別為4個(gè)電機(jī)的角速度。

1）剛體模型包括運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)2部分，為了便于四旋翼的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)建模，可作如下假設(shè)［13］：①四旋翼飛行器是剛體；②四旋翼整體完全對(duì)稱；③質(zhì)量和轉(zhuǎn)動(dòng)慣量不變；④四旋翼僅受重力和槳推力，僅受旋翼在機(jī)體產(chǎn)生的力矩和陀螺力矩。

在此基礎(chǔ)上，可建立四旋翼的動(dòng)力學(xué)方程，并由控制效率模型輸入的三軸力和力矩求解出其線加速度和角加速度，再根據(jù)運(yùn)動(dòng)學(xué)方程作積分運(yùn)算，求得其位置和姿態(tài)，實(shí)現(xiàn)對(duì)四旋翼各運(yùn)動(dòng)參數(shù)的求解，其剛體模型的數(shù)學(xué)表達(dá)式如下：

式中：r為四旋翼位置向量；v為四旋翼速度向量；ω為四旋翼角速度向量；e3=［0 0 1］為單位向量；τ為四旋翼所受三軸力矩；R為地面坐標(biāo)系到體坐標(biāo)系的變換矩陣；Ga為旋翼的陀螺力矩；J為機(jī)體轉(zhuǎn)動(dòng)慣量；g為重力加速度；m為四旋翼質(zhì)量；Θ=［φ θ ψ］為歐拉角；W 為機(jī)體角速度到姿態(tài)角速度的轉(zhuǎn)換矩陣；下標(biāo)e代表物理量在地面坐標(biāo)系下的表達(dá)，下標(biāo)b代表物理量在體坐標(biāo)系下的表達(dá)。

至此，得到了由式（1）～式（3）組成的四旋翼模型的數(shù)學(xué)表達(dá)，在MATLAB中按照上述方程搭建Simulink模型，其中剛體模型部分引用6DOF（Euler Angles）模塊表示。

2 PID控制器

2.1 懸?？刂破?/h3>

考慮到懸停段飛行器在飛行過程中俯仰角和滾轉(zhuǎn)角都較小，總拉力約等于四旋翼的重力，可對(duì)剛體模型作線性化處理：

式中：φ、θ、ψ分別為四旋翼的滾轉(zhuǎn)角、俯仰角、偏航角；下標(biāo)xe、ye、ze代表物理量地面坐標(biāo)系表達(dá)下在三軸上的分量。

顯然，外環(huán)的控制器可以解耦到水平和高度2個(gè)通道上。以水平通道為例，可建立位置環(huán)和速度環(huán)的PID控制，控制器的數(shù)學(xué)模型如下：

式中：KP、KI、KD分別為速度環(huán)的比例、積分、微分環(huán)節(jié)增益；e為四旋翼誤差；下標(biāo)dh代表物理量水平通道上的期望值，下標(biāo)h代表物理量水平通道上的實(shí)際值，下標(biāo)w代表控制器位置環(huán)的相關(guān)增益，下標(biāo)s代表控制器速度環(huán)的相關(guān)增益，下標(biāo)r代表位置環(huán)相關(guān)物理量，下標(biāo)v代表速度環(huán)相關(guān)物理量。

根據(jù)式（5）即可得期望姿態(tài)角的表達(dá)式為

式中：Θdh=［φ θ］為姿態(tài)角在水平通道的分量；Aψ為由ψ表示的水平通道加速度與姿態(tài)角間的轉(zhuǎn)換矩陣。

類似可得到高度通道上期望凈推力的表達(dá)式，建立起完整的懸?？刂破鲾?shù)學(xué)模型并在Simulink中搭建出來。

2.2 追蹤控制器

追蹤控制器要追蹤一條在位置、速度、加速度上都是時(shí)變的軌跡，根據(jù)芭芭拉引理，可以設(shè)計(jì)如下所示的水平通道控制器方程：

這樣通過加入前饋通道，可實(shí)現(xiàn)對(duì)軌跡的快速無差跟蹤。

3 機(jī)動(dòng)軌跡規(guī)劃

本文將穿縫軌跡劃為2個(gè)階段：靠近段和穿越段。四旋翼從指定位置出發(fā)，按照靠近段規(guī)劃的軌跡，以確定的速度和姿態(tài)到達(dá)拋出點(diǎn)；再按照拋物線軌跡，被從拋出點(diǎn)拋出，借助慣性穿過斜縫。下文分別對(duì)穿越段和靠近段進(jìn)行具體的理論推導(dǎo)，以得到可行的數(shù)學(xué)解。

3.1 基于拋物原理的穿越軌跡規(guī)劃

3.1.1 原 理

在穿越過程中，飛行器的首要目標(biāo)是避免與斜縫發(fā)生相撞，將其可能性降到最小。為了簡(jiǎn)化分析難度，可將四旋翼按照其帶槳狀態(tài)下的尺寸簡(jiǎn)化為一個(gè)長(zhǎng)方體，則易知為了最大程度避免碰撞，需使飛行器以同樣的傾斜狀態(tài)從斜縫的正中心穿越。設(shè)計(jì)如圖2所示的平面Ⅱ，可知，平面Ⅱ與斜縫所在平面垂直且與斜縫兩長(zhǎng)邊平行并過斜縫中心。則規(guī)劃的穿越軌跡應(yīng)是過斜縫中心且在平面Ⅱ上的曲線，四旋翼姿態(tài)始終保證機(jī)架平面與平面Ⅱ重合。

圖2 斜縫示意圖Fig.2 Schematic diagram of diagonal seam

考慮到斜縫傾斜的角度能達(dá)到30°甚至更多，使四旋翼保持長(zhǎng)期穩(wěn)定的大角度傾斜姿勢(shì)平穩(wěn)飛過斜縫的難度過大，本文學(xué)習(xí)文獻(xiàn)［14］的方法選擇在平面Ⅱ內(nèi)設(shè)計(jì)一條拋物線軌跡，借助四旋翼自身的慣性實(shí)現(xiàn)穿越，以提高規(guī)劃軌跡的魯棒性。

在地面坐標(biāo)系oexeyeze下，用一組單位向量｛e1，e2，e3｝建立坐標(biāo)系C描述平面Ⅱ內(nèi)的運(yùn)動(dòng)，為了將四旋翼的運(yùn)動(dòng)限制在平面Ⅱ內(nèi)，需要補(bǔ)償重力加速度g在平面Ⅱ外的分量，將g投影到e3中，則有重力加速度分量gT：

由四旋翼的質(zhì)量為m，則需大小為m gT推力來抵消分量影響。由此，將三維的軌跡規(guī)劃問題轉(zhuǎn)為平面Ⅱ上的二維軌跡規(guī)劃。此時(shí)，重力加速度g只剩下平面Ⅱ內(nèi)的分量gⅡ，其值如下：

顯然，物體在平面Ⅱ內(nèi)做勻加速度運(yùn)動(dòng)，根據(jù)牛頓第二定律，有

式中：i取值區(qū)間為｛1，2｝，表示e1、e2兩個(gè)方向上的運(yùn)動(dòng)學(xué)公式；t0c和t分別為起始時(shí)間和當(dāng)前時(shí)間；ri（t）為時(shí)間t時(shí)四旋翼在坐標(biāo)軸ei上的位置；vi（t）為時(shí)間t時(shí)四旋翼在坐標(biāo)軸ei方向上的速度；gⅡ，i為gⅡ在坐標(biāo)軸ei方向上的分量?？紤]到斜縫不僅有豎直方向的傾斜，還可能存在水平方向的傾斜，因此，物體運(yùn)動(dòng)需在2個(gè)方向上進(jìn)行描述。例如，當(dāng)斜縫只有豎直方向的傾斜時(shí)，此時(shí)gⅡ，2=0，則四旋翼只有e1方向上的勻加速度運(yùn)動(dòng)，e2方向上為勻速運(yùn)動(dòng)。

如圖3所示，可將三維的軌跡求解問題轉(zhuǎn)為對(duì)二維拋物線軌跡的求解，在加速度gⅡ及斜縫中心已知的情況下，給定拋出點(diǎn)p0的位置，可求得穿越段軌跡的表達(dá)及相關(guān)的狀態(tài)量。

圖3 平面Ⅱ內(nèi)飛行軌跡Fig.3 Flying trajectory in Plane Ⅱ

3.1.2 數(shù)學(xué)推導(dǎo)

由上可知，在斜縫傾斜程度已知的條件下，穿越段軌跡唯一地由拋出位置確定，可以建立一個(gè)最優(yōu)化的問題，選出最優(yōu)的拋出點(diǎn)p0以確保飛行的安全。

考慮式（10）中的r（t0c）和v（t0c），由于在平面Ⅱ內(nèi)，斜縫對(duì)四旋翼的約束僅有2條短邊，從安全性的角度出發(fā)，需要設(shè)計(jì)一條拋物軌跡，確保當(dāng)四旋翼到達(dá)中心pg時(shí)，其在e1方向上沒有速度分量，從而降低四旋翼與斜縫短邊相撞的幾率。假設(shè)到達(dá)pg的時(shí)間為tg，p0與pg在e1方向上的距離為l，在e2方向上的距離為d，則由v1（tg）=0，有

顯然為了安全，tg應(yīng)當(dāng)越小越好，即l越小越好，此時(shí)考慮四旋翼在動(dòng)力系統(tǒng)和e2方向上安全距離的約束，可建立最優(yōu)化問題的數(shù)學(xué)模型如下：

用l和d來描述2個(gè)約束條件：

由于采用的是F330機(jī)型，且槳葉直徑為203mm，e2方向上拋出點(diǎn)位置至少要距離斜縫265mm，考慮到可能存在的飛行器跟蹤誤差及現(xiàn)實(shí)中對(duì)斜縫位置的測(cè)算誤差，設(shè)定e2方向上最小安全距離為350mm，給定不同的最大速度限制，則以l為橫軸，d為縱軸，可得到由如下速度約束曲線族及位置約束曲線所框定的可行域范圍，如圖4所示。

圖4 可行域范圍Fig.4 Feasible range

根據(jù)計(jì)算及觀察，當(dāng)取d的最小值時(shí)，可取得最小即最優(yōu)的l，滿足最優(yōu)化條件，從而可通過坐標(biāo)系C下的三軸單位向量｛e1，e2，e3｝表示出拋出點(diǎn)p0的位置、速度及加速度。本文中設(shè)定斜縫的姿態(tài)角已知，則可建立坐標(biāo)系C與地面坐標(biāo)系的轉(zhuǎn)換矩陣，得到拋出點(diǎn)相關(guān)信息在地面坐標(biāo)系下的表達(dá)。

在此基礎(chǔ)上，還要考慮穿越軌跡結(jié)束的位置，四旋翼到達(dá)此點(diǎn)后即進(jìn)入懸停工作狀態(tài)。由于一般情況下e1、e2兩個(gè)方向上都有加速度，四旋翼穿越后并不會(huì)回到p0關(guān)于斜縫的對(duì)稱點(diǎn)上，因此，考慮到此時(shí)d的取值為最優(yōu)解，且穿越后易與斜縫在e2方向上發(fā)生碰撞，以d為衡量指標(biāo)，保證穿越起始點(diǎn)、結(jié)束點(diǎn)與斜縫在e2軸上的距離相等均為d，利用牛頓第二定律求得軌跡的終止點(diǎn)作為四旋翼穿越后的懸停恢復(fù)點(diǎn)ph。

3.2 基于龐特里亞金極小值原理的靠近軌跡規(guī)劃

靠近段的目標(biāo)是規(guī)劃一條狀態(tài)轉(zhuǎn)換軌跡，確保四旋翼從初始狀態(tài)出發(fā)，追蹤此軌跡，最終能實(shí)現(xiàn)期望的拋出狀態(tài)。據(jù)此建立最優(yōu)控制問題，設(shè)四旋翼的加加速度j為控制量，取s=［r v a］，以四旋翼的位置r、速度v、加速度a為狀態(tài)量，建立靠近段任務(wù)的問題模型：

參考試驗(yàn)場(chǎng)地的尺寸限制，本文設(shè)定靠近段的初態(tài)st0即穿縫起始點(diǎn)為直面斜縫4m遠(yuǎn)與斜縫中心等高的位置，靠近段的末態(tài)stf由穿越段拋出點(diǎn)定義。為了保證飛行軌跡的平滑性，此處采用三軸線加加速度的積分和作為代價(jià)函數(shù)來建立問題。由于此處控制量j存在物理意義上的約束，需采用龐特里亞金極小值原理求解。

3.2.1 原 理

考慮到實(shí)際場(chǎng)景中，穿縫時(shí)間并不任意，飛機(jī)需綜合考慮危險(xiǎn)性和效率盡可能快得穿越斜縫，在調(diào)研國(guó)外學(xué)者對(duì)此問題的研究后，本文設(shè)定末態(tài)時(shí)間tf的取值范圍為0～3 s，則此問題是一個(gè)積分型指標(biāo)、末態(tài)時(shí)間tf約束、末端固定的最優(yōu)控制問題。根據(jù)極小值定理，存在向量λ（t）和非零向量γ，使得最優(yōu)解x*滿足如下的必要條件［15］，其 中，哈 密 頓 函 數(shù) 為：H =L（x，u，t）+λT（t）f（x，u，t）。

正則方程：

邊界條件與橫截條件：

極小值條件：

H變化律：

3.2.2 數(shù)學(xué)推導(dǎo)

針對(duì)本文問題模型，將問題解耦到三軸通道上，為便于描述，下文所有向量在某方向上的分量均不加下標(biāo)表示，則對(duì)任一方向可建立如下的哈密頓函數(shù)：

則由正則方程有

由此可解得

根據(jù)極小值條件，將λ（t）和H（s，j，λ）的具體表達(dá)式代入：

對(duì)其關(guān)于j求導(dǎo)，有

由此即可求最優(yōu)控制律和最優(yōu)狀態(tài)即最優(yōu)軌跡如下：

此處假設(shè)末態(tài)時(shí)間tf為一定值，則在上述由參向量α、β、γ表示的最優(yōu)狀態(tài)量基礎(chǔ)上，令t=tf，可反解出參數(shù)值，計(jì)算α、β、γ公式如下：

式中：

在此基礎(chǔ)上，編寫程序循環(huán)比較每個(gè)末態(tài)時(shí)間tf對(duì)應(yīng)的代價(jià)函數(shù)，以找到最優(yōu)的狀態(tài)量數(shù)學(xué)表達(dá)，完成靠近段軌跡的求解。

4 仿真試驗(yàn)

4.1 PID控制器仿真

4.1.1 懸?？刂破鞣抡?/p>

懸?？刂浦饕獞?yīng)用在起飛、回穩(wěn)、降落3個(gè)階段，起飛和降落階段要求飛行器能夠平穩(wěn)、準(zhǔn)確地到達(dá)指定位置，回穩(wěn)階段要求能快速恢復(fù)穩(wěn)定，因此兩者在設(shè)計(jì)上有一定的差異，回穩(wěn)控制器引入飛行器實(shí)時(shí)位置作為輸入，并適當(dāng)調(diào)整PID控制器參數(shù)以使其盡快懸停。

根據(jù)試驗(yàn)環(huán)境，對(duì)起飛、降落階段的懸?？刂破?，給定［1 0.8 -1.5］m作為期望位置，圖5仿真表明，三軸僅x軸有2%的超調(diào)，且穩(wěn)態(tài)誤差均為0。

對(duì)于回穩(wěn)控制器，給定4m/s的初速度和40°滾轉(zhuǎn)的初始姿態(tài)，圖6仿真表明，飛行器在1 s時(shí)即趨于穩(wěn)定，速度響應(yīng)在1 s時(shí)已降至0.1 m/s內(nèi)，最終滾轉(zhuǎn)和俯仰角均為0，平穩(wěn)懸停。

4.1.2 追蹤控制器仿真

給定螺旋線軌跡讓飛行器跟蹤，三軸上的位置響應(yīng)誤差最終能控制在4mm內(nèi)，且x、y軸上的相角差約在0.7°左右，具體觀察加速度和速度的跟蹤誤差，圖7仿真表明最終能控制在厘米級(jí)。

圖5 起飛懸停控制器位置響應(yīng)Fig.5 Location response of takeoff hovering controller

圖6 回穩(wěn)控制器響應(yīng)Fig.6 Response of stabilization controller

圖7 追蹤控制器速度、加速度響應(yīng)Fig.7 Track controller speed and acceleration response

4.2 任務(wù)規(guī)劃仿真

在前文模型、控制器、軌跡規(guī)劃完成的基礎(chǔ)上，設(shè)定如下任務(wù)：空間中存在一堵僅留有一傾斜斜縫的墻面，四旋翼從墻前內(nèi)任意一點(diǎn)出發(fā)，追蹤確定的軌跡穿過斜縫后，在墻后快速懸停，并平穩(wěn)降落在指定的位置。

針對(duì)此問題，按如圖8所示的架構(gòu)編寫狀態(tài)機(jī)程序來檢測(cè)四旋翼當(dāng)前的狀態(tài)，并據(jù)此打開所需的外環(huán)控制器。四旋翼會(huì)經(jīng)歷起飛、靠近、穿越、回穩(wěn)、降落5個(gè)階段，分別需接入懸停、追蹤、油門、回穩(wěn)、懸?？刂破鲗?shí)現(xiàn)控制。任務(wù)開始時(shí)，系統(tǒng)預(yù)先觀察斜縫，在得到斜縫中心位置和姿態(tài)角后線下完成軌跡規(guī)劃的工作，并將計(jì)算得到的靠近段軌跡數(shù)學(xué)表達(dá)、穿越段所需凈推力等信息發(fā)送給狀態(tài)機(jī)，狀態(tài)機(jī)據(jù)此建立起切換機(jī)制，每當(dāng)四旋翼到達(dá)某關(guān)鍵位置時(shí)狀態(tài)機(jī)即檢測(cè)其是否滿足設(shè)定的容許誤差，并決定是否打開新的控制器通道，控制四旋翼下一步的飛行動(dòng)作。狀態(tài)機(jī)的容許誤差由外環(huán)控制器性能及仿真穿越效果確定，其指標(biāo)如表1所示。

本文選擇給定斜縫中心位置為［40 -2］m，考慮設(shè)定起始位置、斜縫朝向、斜縫豎直方向及水平方向傾斜角、最大飛行速度限制4類變量來設(shè)計(jì)任務(wù)想定，并搭建可視環(huán)境，且在斜縫四邊上加入碰撞檢測(cè)傳感器，以其輸出作為判定指標(biāo)，驗(yàn)證四旋翼的穿縫效果。

圖8 任務(wù)規(guī)劃模型Fig.8 Mission planning model

表1 狀態(tài)機(jī)參數(shù)Table 1 State machine parameters

4.2.1 任務(wù)流程演示

建立如圖9所示由飛機(jī)、斜縫、降落平臺(tái)組成的仿真環(huán)境，并在MATLAB中使用3D工具箱搭建。以穿越豎直方向45°斜縫為例，觀察整個(gè)穿越任務(wù)執(zhí)行的流程，圖9展示了飛行器8個(gè)階段下的飛行狀態(tài)。

具體觀察狀態(tài)的切換情況，如圖10所示，狀態(tài)機(jī)共進(jìn)行了4次切換，經(jīng)歷了5個(gè)狀態(tài)，與預(yù)期相符合。

觀察此時(shí)傳感器檢測(cè)情況，如圖11所示，發(fā)現(xiàn)4條邊上均輸出無效信號(hào)，說明四旋翼未與斜縫發(fā)生碰撞，成功穿越。

圖9 飛行任務(wù)流程Fig.9 Flight mission process

圖10 狀態(tài)機(jī)工作流程Fig.10 State machine workflow

圖11 傳感器檢測(cè)情況Fig.11 Sensor detection

4.2.2 仿真試驗(yàn)1：不同斜縫傾角及速度上限

斜縫傾斜角及最大飛行速度是限制穿縫成功的主要因素，本文綜合豎直方向傾斜、水平方向傾斜及最大飛行速度設(shè)計(jì)了多組想定并進(jìn)行仿真驗(yàn)證，總結(jié)四旋翼穿縫性能如表2所示。

由于整個(gè)穿縫任務(wù)的核心在靠近段的姿態(tài)轉(zhuǎn)換，以豎直方向45°傾斜，水平方向傾斜0°斜縫，最大速度限制4m/s為例分析靠近段外環(huán)追蹤控制器效果。

表2 四旋翼穿縫性能Table 2 Sewing ability of quad rotor

圖12 靠近段軌跡加速度響應(yīng)Fig.12 Acceleration response of tracking of approach trajectory

圖12給出了飛行器三軸在加速度上的響應(yīng)情況。仿真顯示，三軸上位置和速度響應(yīng)最終能較好地跟上期望的軌跡，但加速度在追蹤后期的誤差明顯增大。具體測(cè)算靠近段到穿越段的切換點(diǎn)處響應(yīng)情況，位置誤差為0.03m，速度誤差為0.05m/s，加速度誤差為1.26m/s2，顯然位置和速度跟蹤效果較好，但加速度的跟蹤性能明顯下降。分析其原因是：由于在靠近段的末期姿態(tài)角較大，則先前基于運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)方程線性化設(shè)計(jì)的外環(huán)控制器性能會(huì)明顯下降，為了改善這一情況，后續(xù)需針對(duì)外環(huán)控制器的非線性設(shè)計(jì)繼續(xù)研究。

4.2.3 仿真試驗(yàn)2：不同起始位置及斜縫朝向

考慮到室內(nèi)封閉環(huán)境的任務(wù)特點(diǎn)，四旋翼僅能通過相機(jī)來對(duì)斜縫進(jìn)行觀察，飛行過程中需要機(jī)頭始終朝向斜縫方向，因此，斜縫的朝向會(huì)對(duì)任務(wù)的執(zhí)行產(chǎn)生一定的影響。此外，由于飛行器可能在斜縫墻前任意位置出發(fā)，不同出發(fā)位置對(duì)穿縫效果也存在一定可能的影響。針對(duì)這兩者提出的解決辦法是：在開始穿縫前引導(dǎo)其飛向一個(gè)直面斜縫且距離為4m的等高點(diǎn)，并調(diào)整機(jī)頭方向使正對(duì)斜縫。

給定［2 -2 0］m和［-2 2 0］m兩個(gè)不同的起始位置觀察穿越效果，如圖13（a）所示，四旋翼主要在由起飛點(diǎn)到達(dá)起始點(diǎn)的軌跡上存在差異，從穿縫起始點(diǎn)出發(fā)的穿縫軌跡類似，最終都能成功降落在平臺(tái)上。

再給定斜縫偏航發(fā)生0°、60°、90°偏轉(zhuǎn)，仿真表明都能實(shí)現(xiàn)成功穿越，具體觀察偏航角的偏轉(zhuǎn)，圖13（b）仿真顯示靠近段和穿越段過程中飛機(jī)機(jī)頭始終指向斜縫，最大僅發(fā)生8°左右的偏轉(zhuǎn)，滿足相機(jī)觀測(cè)的需求。

圖13 位置與朝向條件變化下穿越結(jié)果Fig.13 Crossing results at different locations and directions

5 結(jié) 論

針對(duì)時(shí)空約束條件下多旋翼機(jī)動(dòng)軌跡優(yōu)化的問題，設(shè)定了穿越室內(nèi)斜縫的任務(wù)場(chǎng)景，從具體分析任務(wù)對(duì)象特點(diǎn)的角度出發(fā)，運(yùn)用拋物原理和龐特里亞金極小值原理，對(duì)四旋翼的穿縫軌跡進(jìn)行了優(yōu)化。

1）進(jìn)行了四旋翼建模、不同功能外環(huán)PID控制器設(shè)計(jì)、可視仿真環(huán)境搭建，構(gòu)成用于進(jìn)行規(guī)劃效果驗(yàn)證的仿真飛行平臺(tái)。

2）基于拋物原理的軌跡規(guī)劃方法最大能實(shí)現(xiàn)豎直傾斜63°或水平傾斜32°的斜縫穿越。

3）本文方法減輕了軌跡規(guī)劃的算力負(fù)擔(dān)，降低了對(duì)內(nèi)外環(huán)控制器的性能需求，在工程中具有較好的可行性。