(西南交通大學(xué) 機(jī)械工程學(xué)院，四川 成都 610031)

近幾年，隨著四旋翼無(wú)人機(jī)的普及，四旋翼無(wú)人機(jī)的應(yīng)用和研究越來(lái)越多。在有障礙的環(huán)境中提高四旋翼無(wú)人機(jī)的可操作性和位置精度需要對(duì)四旋翼無(wú)人機(jī)進(jìn)行飛行軌跡優(yōu)化[1]。在現(xiàn)有的四旋翼無(wú)人機(jī)軌跡優(yōu)化研究中大多是把四旋翼無(wú)人機(jī)放在完全空曠的環(huán)境中，在軌跡優(yōu)化過(guò)程中沒(méi)有考慮四旋翼無(wú)人機(jī)的避障問(wèn)題，或者將四旋翼無(wú)人機(jī)假設(shè)為一個(gè)質(zhì)點(diǎn)，沒(méi)有考慮四旋翼無(wú)人機(jī)在飛行過(guò)程中的姿態(tài)問(wèn)題[2]。本文針對(duì)上述問(wèn)題，考慮四旋翼無(wú)人機(jī)在有障礙和控制四旋翼無(wú)人機(jī)姿態(tài)的條件下，離線(xiàn)優(yōu)化四旋翼無(wú)人機(jī)的軌跡。

四旋翼無(wú)人機(jī)軌跡優(yōu)化問(wèn)題的本質(zhì)是帶有控制變量、狀態(tài)變量、約束的非線(xiàn)性最優(yōu)控制問(wèn)題。這類(lèi)問(wèn)題之前是通過(guò)解析解來(lái)進(jìn)行求解，但是隨著系統(tǒng)的復(fù)雜性越來(lái)越高，其解析解的求解難度越來(lái)越大[3]。隨著計(jì)算機(jī)的普遍使用，非線(xiàn)性最優(yōu)控制問(wèn)題的數(shù)值方法逐漸代替解析法，采用數(shù)值方法中的hp自適應(yīng)Radau偽譜法(Radau Pseudospectral Method，RPM)進(jìn)行四旋翼無(wú)人機(jī)避障軌跡優(yōu)化，具有全局優(yōu)化、收斂速度快、對(duì)初值不敏感等優(yōu)點(diǎn)[4]。并已經(jīng)應(yīng)用于工程實(shí)際中，在2006年和2007年，美國(guó)的Ross等人利用偽譜法實(shí)現(xiàn)了國(guó)際空間站在“零燃料”下大角度姿態(tài)機(jī)動(dòng)，驗(yàn)證了偽譜法的可行性[5]。hp自適應(yīng)是結(jié)合了有限元區(qū)間劃分的思想將區(qū)間長(zhǎng)度h和基函數(shù)階次p進(jìn)行自適應(yīng)配置以更高的精度、更小的計(jì)算量逼近狀態(tài)變量和控制變量。

1 四旋翼無(wú)人機(jī)動(dòng)力學(xué)模型

在建立四旋翼無(wú)人機(jī)的動(dòng)力學(xué)模型時(shí)，以機(jī)體質(zhì)心為坐標(biāo)原點(diǎn)，符合右手定則的坐標(biāo)系B(O,X,Y,Z)為機(jī)體坐標(biāo)系，以固結(jié)于地面和符合右手定則的坐標(biāo)系E(O,X,Y,Z)為慣性坐標(biāo)系，四旋翼無(wú)人機(jī)的坐標(biāo)示意圖如圖1所示。四旋翼無(wú)人機(jī)在空中無(wú)約束運(yùn)動(dòng)，存在6個(gè)自由度，選用12個(gè)狀態(tài)變量進(jìn)行描述，分別為:位置(x，y，z)、速度(vx，vy，vz)、姿態(tài)(φ，θ，ψ)、角速度(p，q，r)。參考文獻(xiàn)[6]，整理可得四旋翼無(wú)人機(jī)的非線(xiàn)性動(dòng)力學(xué)模型為

(1)

式中，l為四旋翼無(wú)人機(jī)電機(jī)中心到質(zhì)心的距離；m為四旋翼無(wú)人機(jī)的質(zhì)量；U1,U2,U3,U4分別為四旋翼無(wú)人機(jī)的4個(gè)輸入控制量；Ix,Iy,Iz分別為四旋翼無(wú)人機(jī)相對(duì)x,y,z軸的轉(zhuǎn)動(dòng)慣量；g為重力加速度。

圖1 四旋翼無(wú)人機(jī)坐標(biāo)系示意圖

根據(jù)空氣動(dòng)力學(xué)原理，一般認(rèn)為單獨(dú)的一個(gè)螺旋槳所能提供的動(dòng)力與轉(zhuǎn)速的平方成正比，即F=ktω2，M=kdω2，kt,kd為比例系數(shù)。根據(jù)四旋翼無(wú)人機(jī)的運(yùn)動(dòng)原理控制量Ui(i=1,2,3,4)與電機(jī)轉(zhuǎn)速的關(guān)系如下：

(2)

經(jīng)查閱相關(guān)資料和實(shí)際測(cè)量，四旋翼無(wú)人機(jī)避障軌跡優(yōu)化時(shí)所用到的參數(shù)如系統(tǒng)參數(shù)表1所示[7]。

表1 系統(tǒng)參數(shù)表

2 最優(yōu)化問(wèn)題描述

以實(shí)驗(yàn)室四旋翼無(wú)人機(jī)為研究對(duì)象，四旋翼無(wú)人機(jī)實(shí)物圖如圖2所示。

圖2 四旋翼無(wú)人機(jī)實(shí)物圖

假設(shè)四旋翼無(wú)人機(jī)在障礙已知的環(huán)境中飛行，根據(jù)四旋翼無(wú)人機(jī)的動(dòng)力學(xué)模型、約束和優(yōu)化目標(biāo)，以及路徑約束、邊界約束、控制量約束，進(jìn)行軌跡優(yōu)化。根據(jù)實(shí)驗(yàn)室四旋翼無(wú)人機(jī)的性能參數(shù)，路徑約束為

(3)

根據(jù)系統(tǒng)參數(shù)表1和式(2)可得控制量約束為

(4)

以四旋翼無(wú)人機(jī)的飛行路徑最短為優(yōu)化目標(biāo)，可以降低四旋翼無(wú)人機(jī)的整體速度，使其更平穩(wěn)飛行，即

minJ=L

(5)

3 優(yōu)化方法

3.1 遺傳算法進(jìn)行軌跡優(yōu)化

圖3 遺傳算法所得三維軌跡俯視圖

3.2 hp自適應(yīng)RPM理論

3.2.1 RPM理論

由于遺傳算法的隨機(jī)性，每次計(jì)算結(jié)果并不完全一致，可能存在“偽最優(yōu)軌跡”。采用hp自適應(yīng)RPM進(jìn)行四旋翼無(wú)人機(jī)避障軌跡優(yōu)化。以LGR配點(diǎn)，將連續(xù)的最優(yōu)控制問(wèn)題離散化為非線(xiàn)性規(guī)劃問(wèn)題，在離散點(diǎn)之間以拉格朗日多項(xiàng)式逼近狀態(tài)變量和控制變量，拉格朗日插值多項(xiàng)式的導(dǎo)數(shù)將連續(xù)的動(dòng)力學(xué)微分方程轉(zhuǎn)化為離散的代數(shù)約束，在離散點(diǎn)處求控制變量和狀態(tài)變量最優(yōu)解[11]。hp自適應(yīng)是應(yīng)用有限元思想，在全局配點(diǎn)離散的基礎(chǔ)上判斷各個(gè)離散點(diǎn)之間的誤差，在不連續(xù)或非光滑處采用h法增加分段數(shù)目，在光滑處采用p方法增加配點(diǎn)數(shù)獲得指數(shù)的收斂效果[12]。

偽譜法的一般步驟如下[13-17]。

① 將系統(tǒng)狀態(tài)方程的時(shí)間域轉(zhuǎn)換到偽譜法的時(shí)間域。

(6)

② LGR配點(diǎn)，包含起點(diǎn)和K個(gè)LGR點(diǎn)，K維的LGR點(diǎn)是K階Legendre多項(xiàng)式與K-1階Legendre多項(xiàng)式和的零點(diǎn)，即PK(τ)+PK-1(τ)=0的根，PK(τ)為K階Legendre多項(xiàng)式，其表達(dá)式的遞推公式為

P0(τ)=1
P1(τ)=τ
nPn(τ)=(2n-1)τPn-1(τ)-(n-1)Pn-2(τ)(n≥2)

(7)

③ 根據(jù)式(7)求解K個(gè)LGR點(diǎn)和τ=-1共K+1個(gè)離散點(diǎn)，以拉格朗日插值多項(xiàng)式為基函數(shù)近似狀態(tài)變量和控制變量如下：

(8)

且

其中，Lj(τ)為拉格朗日基函數(shù)；X(τj)為第j個(gè)離散點(diǎn)。

(9)

其中，U(τj)為第j個(gè)離散點(diǎn)。

④ 對(duì)式(8)求導(dǎo)將動(dòng)力學(xué)微分方程離散化。

(10)

其中，

⑤ 以Gauss積分逼近終端狀態(tài)為

(11)

經(jīng)過(guò)上述五步轉(zhuǎn)化即可將連續(xù)的最優(yōu)控制問(wèn)題轉(zhuǎn)化為離散的非線(xiàn)性問(wèn)題，可以利用非線(xiàn)性求解器(IPOPT、SNOPT等)進(jìn)行求解。

3.2.2 hp自適應(yīng)規(guī)則

① 判斷在評(píng)判點(diǎn)處的約束誤差εmax是否均小于εp，若小于則停止計(jì)算，輸出計(jì)算結(jié)果，否則進(jìn)行第②步。

⑤ 返回第①步進(jìn)行迭代計(jì)算，直到滿(mǎn)足精度要求或者達(dá)到最大迭代次數(shù)。

4 系統(tǒng)仿真

圖4 偽譜法所得三維軌跡俯視圖

圖5 四旋翼無(wú)人機(jī)XYZ方向的速度

圖6 四旋翼無(wú)人機(jī)姿態(tài)

圖7 四旋翼無(wú)人機(jī)輸入控制量

通過(guò)分析圖4～圖7，四旋翼無(wú)人機(jī)能夠在規(guī)定時(shí)間和約束條件下到達(dá)規(guī)定的狀態(tài)，通過(guò)hp自適應(yīng)RPM優(yōu)化的避障軌跡能夠?qū)崿F(xiàn)避障功能，各項(xiàng)指標(biāo)均在容許范圍之內(nèi)。位移、速度、姿態(tài)等沒(méi)有達(dá)到約束邊界，但是控制量存在達(dá)到約束值的時(shí)刻，說(shuō)明四旋翼無(wú)人機(jī)系統(tǒng)的動(dòng)力不足，響應(yīng)較慢，可以通過(guò)提高四旋翼無(wú)人機(jī)的動(dòng)力，實(shí)現(xiàn)四旋翼無(wú)人機(jī)的平穩(wěn)、快速飛行。另外分析軌跡俯視圖(見(jiàn)圖4)和四旋翼無(wú)人機(jī)速度圖(見(jiàn)圖5)可以發(fā)現(xiàn)，四旋翼無(wú)人機(jī)避障軌跡在靠近障礙時(shí)速度會(huì)降低，可避免四旋翼無(wú)人機(jī)在障礙附近因?yàn)楸苷隙Э匕l(fā)生碰撞。

5 結(jié)束語(yǔ)

本文基于hp自適應(yīng)RPM的四旋翼無(wú)人機(jī)避障軌跡優(yōu)化進(jìn)行研究，分析了四旋翼無(wú)人機(jī)的動(dòng)力學(xué)模型和獲取實(shí)驗(yàn)室四旋翼無(wú)人機(jī)的部分參數(shù)，利用遺傳算法和hp自適應(yīng)RPM分別進(jìn)行四旋翼無(wú)人機(jī)避障軌跡優(yōu)化。實(shí)驗(yàn)結(jié)果表明，遺傳算法存在隨機(jī)性，可能會(huì)出現(xiàn)“偽最優(yōu)解”。通過(guò)對(duì)比hp自適應(yīng)RPM求解的四旋翼無(wú)人機(jī)避障軌跡路徑比遺傳算法要短，在優(yōu)化過(guò)程中充分考慮了四旋翼無(wú)人機(jī)的各性能指標(biāo)，包括各向位移、速度、姿態(tài)、控制變量等，能夠使四旋翼無(wú)人機(jī)更平穩(wěn)、安全地飛達(dá)目的地。