hp自適應RPM在四旋翼無人機軌跡優化中的應用

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

近幾年，隨著四旋翼無人機的普及，四旋翼無人機的應用和研究越來越多。在有障礙的環境中提高四旋翼無人機的可操作性和位置精度需要對四旋翼無人機進行飛行軌跡優化[1]。在現有的四旋翼無人機軌跡優化研究中大多是把四旋翼無人機放在完全空曠的環境中，在軌跡優化過程中沒有考慮四旋翼無人機的避障問題，或者將四旋翼無人機假設為一個質點，沒有考慮四旋翼無人機在飛行過程中的姿態問題[2]。本文針對上述問題，考慮四旋翼無人機在有障礙和控制四旋翼無人機姿態的條件下，離線優化四旋翼無人機的軌跡。

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

1 四旋翼無人機動力學模型

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

(1)

式中，l為四旋翼無人機電機中心到質心的距離；m為四旋翼無人機的質量；U1,U2,U3,U4分別為四旋翼無人機的4個輸入控制量；Ix,Iy,Iz分別為四旋翼無人機相對x,y,z軸的轉動慣量；g為重力加速度。

圖1 四旋翼無人機坐標系示意圖

根據空氣動力學原理，一般認為單獨的一個螺旋槳所能提供的動力與轉速的平方成正比，即F=ktω2，M=kdω2，kt,kd為比例系數。根據四旋翼無人機的運動原理控制量Ui(i=1,2,3,4)與電機轉速的關系如下：

(2)

經查閱相關資料和實際測量，四旋翼無人機避障軌跡優化時所用到的參數如系統參數表1所示[7]。

表1 系統參數表

2 最優化問題描述

以實驗室四旋翼無人機為研究對象，四旋翼無人機實物圖如圖2所示。

圖2 四旋翼無人機實物圖

假設四旋翼無人機在障礙已知的環境中飛行，根據四旋翼無人機的動力學模型、約束和優化目標，以及路徑約束、邊界約束、控制量約束，進行軌跡優化。根據實驗室四旋翼無人機的性能參數，路徑約束為

(3)

根據系統參數表1和式(2)可得控制量約束為

(4)

以四旋翼無人機的飛行路徑最短為優化目標，可以降低四旋翼無人機的整體速度，使其更平穩飛行，即

minJ=L

(5)

3 優化方法

3.1 遺傳算法進行軌跡優化

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

3.2 hp自適應RPM理論

3.2.1 RPM理論

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

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

① 將系統狀態方程的時間域轉換到偽譜法的時間域。

(6)

② LGR配點，包含起點和K個LGR點，K維的LGR點是K階Legendre多項式與K-1階Legendre多項式和的零點，即PK(τ)+PK-1(τ)=0的根，PK(τ)為K階Legendre多項式，其表達式的遞推公式為

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

(7)

③ 根據式(7)求解K個LGR點和τ=-1共K+1個離散點，以拉格朗日插值多項式為基函數近似狀態變量和控制變量如下：

(8)

且

其中，Lj(τ)為拉格朗日基函數；X(τj)為第j個離散點。

(9)

其中，U(τj)為第j個離散點。

④ 對式(8)求導將動力學微分方程離散化。

(10)

其中，

⑤ 以Gauss積分逼近終端狀態為

(11)

經過上述五步轉化即可將連續的最優控制問題轉化為離散的非線性問題，可以利用非線性求解器(IPOPT、SNOPT等)進行求解。

3.2.2 hp自適應規則

① 判斷在評判點處的約束誤差εmax是否均小于εp，若小于則停止計算，輸出計算結果，否則進行第②步。

⑤ 返回第①步進行迭代計算，直到滿足精度要求或者達到最大迭代次數。

4 系統仿真

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

圖5 四旋翼無人機XYZ方向的速度

圖6 四旋翼無人機姿態

圖7 四旋翼無人機輸入控制量

通過分析圖4～圖7，四旋翼無人機能夠在規定時間和約束條件下到達規定的狀態，通過hp自適應RPM優化的避障軌跡能夠實現避障功能，各項指標均在容許范圍之內。位移、速度、姿態等沒有達到約束邊界，但是控制量存在達到約束值的時刻，說明四旋翼無人機系統的動力不足，響應較慢，可以通過提高四旋翼無人機的動力，實現四旋翼無人機的平穩、快速飛行。另外分析軌跡俯視圖(見圖4)和四旋翼無人機速度圖(見圖5)可以發現，四旋翼無人機避障軌跡在靠近障礙時速度會降低，可避免四旋翼無人機在障礙附近因為避障而失控發生碰撞。

5 結束語

本文基于hp自適應RPM的四旋翼無人機避障軌跡優化進行研究，分析了四旋翼無人機的動力學模型和獲取實驗室四旋翼無人機的部分參數，利用遺傳算法和hp自適應RPM分別進行四旋翼無人機避障軌跡優化。實驗結果表明，遺傳算法存在隨機性，可能會出現“偽最優解”。通過對比hp自適應RPM求解的四旋翼無人機避障軌跡路徑比遺傳算法要短，在優化過程中充分考慮了四旋翼無人機的各性能指標，包括各向位移、速度、姿態、控制變量等，能夠使四旋翼無人機更平穩、安全地飛達目的地。