蔡志浩，左一鳴，王英勛

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

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

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

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

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

1 四旋翼模型

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

圖1 大疆F330Fig.1 DJI F330

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

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

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

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

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

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

式中：r為四旋翼位置向量；v為四旋翼速度向量；ω為四旋翼角速度向量；e3=［0 0 1］為單位向量；τ為四旋翼所受三軸力矩；R為地面坐標(biāo)系到體坐標(biāo)系的變換矩陣；Ga為旋翼的陀螺力矩；J為機(jī)體轉(zhuǎn)動慣量；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)角都較小，總拉力約等于四旋翼的重力，可對剛體模型作線性化處理：

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

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

式中：KP、KI、KD分別為速度環(huán)的比例、積分、微分環(huán)節(jié)增益；e為四旋翼誤差；下標(biāo)dh代表物理量水平通道上的期望值，下標(biāo)h代表物理量水平通道上的實際值，下標(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 追蹤控制器

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

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

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

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

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

3.1.1 原 理

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

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

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

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

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

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

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

如圖3所示，可將三維的軌跡求解問題轉(zhuǎn)為對二維拋物線軌跡的求解，在加速度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)

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

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

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

用l和d來描述2個約束條件：

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

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

根據(jù)計算及觀察，當(dāng)取d的最小值時，可取得最小即最優(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兩個方向上都有加速度，四旋翼穿越后并不會回到p0關(guān)于斜縫的對稱點(diǎn)上，因此，考慮到此時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ā)，追蹤此軌跡，最終能實現(xiàn)期望的拋出狀態(tài)。據(jù)此建立最優(yōu)控制問題，設(shè)四旋翼的加加速度j為控制量，取s=［r v a］，以四旋翼的位置r、速度v、加速度a為狀態(tài)量，建立靠近段任務(wù)的問題模型：

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

3.2.1 原 理

考慮到實際場景中，穿縫時間并不任意，飛機(jī)需綜合考慮危險性和效率盡可能快得穿越斜縫，在調(diào)研國外學(xué)者對此問題的研究后，本文設(shè)定末態(tài)時間tf的取值范圍為0～3 s，則此問題是一個積分型指標(biāo)、末態(tài)時間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)

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

則由正則方程有

由此可解得

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

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

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

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

式中：

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

4 仿真試驗

4.1 PID控制器仿真

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

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

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

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

4.1.2 追蹤控制器仿真

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

圖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)降落在指定的位置。

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

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

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

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

4.2.1 任務(wù)流程演示

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

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

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

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

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

圖11 傳感器檢測情況Fig.11 Sensor detection

4.2.2 仿真試驗1：不同斜縫傾角及速度上限

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

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

4.2.3 仿真試驗2：不同起始位置及斜縫朝向

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

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

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

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

5 結(jié) 論

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

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

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

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