[發明專利]基于在線安全學習的四旋翼無人機路徑跟蹤控制方法

有效

• 申請號: 201710758952.3
• 申請日: 2017-08-29
• 公開（公告）號: CN107563044B
• 公開（公告）日: 2020-08-25
• 發明（設計）人: 楊艷華;程歡;柴利
• 申請（專利權）人: 武漢科技大學
• 主分類號: G06F17/11
• 分類號: G06F17/11;G06F30/15;G06F119/14
• 代理公司: 武漢科皓知識產權代理事務所(特殊普通合伙) 42222
• 代理人: 張火春

1.一種基于在線安全學習的四旋翼無人機路徑跟蹤控制方法，其特征在于所述控制方法包含如下步驟：

步驟1、四旋翼無人機名義模型的建立

構建四旋翼無人機機體坐標系，所述機體坐標系是：以北為坐標系x_e軸的正方向，以東為坐標系y_e軸的正方向，以地為坐標系z_e軸的正方向；

建立四旋翼無人機的名義模型

式(1)中：φ表示四旋翼無人機的滾轉角，rad，

表示四旋翼無人機的滾轉角角速度，rad/s，

θ表示四旋翼無人機的俯仰角，rad，

表示四旋翼無人機的俯仰角角速度，rad/s，

ψ表示四旋翼無人機的偏航角，rad，

表示四旋翼無人機的偏航角角速度，rad/s，

z表示四旋翼無人機在z_e軸的位置，m，

表示四旋翼無人機z_e軸方向的線速度，m/s，

x表示四旋翼無人機在x_e軸的位置，m，

表示四旋翼無人機x_e軸方向的線速度，m/s，

y表示四旋翼無人機在y_e軸的位置，m，

表示四旋翼無人機y_e軸方向的線速度，m/s，

X表示四旋翼無人機12維的狀態向量，

U表示虛擬控制量，U＝[U₁ U₂ U₃ U₄]，

U₁表示通道z的控制量，

U₂表示通道φ的控制量，

U₃表示通道θ的控制量，

U₄表示通道ψ的控制量，

m表示四旋翼無人機的質量，kg，

g表示重力加速度常量，9.8m/s²，

步驟2、四旋翼無人機的學習模型

假設在k時刻，傳感器測得的四旋翼無人機實際狀態根據k-1時刻的狀態和k時刻的狀態得到狀態微分再將狀態微分與k時刻名義模型的值相減，得到k時刻殘差g(X(k),U(k))

式(2)中：表示k時刻的狀態微分，

表示k時刻的名義模型；

隨著時間k的增加，得到M個殘差g(X,U)的數據；

然后，利用所述M個殘差g(X,U)的數據，得到學習模型對四旋翼無人機的12個狀態分別建立相應的高斯過程學習模型X_l表示狀態向量X的第l個變量，l＝1,2,…，12，學習模型

定義狀態向量X的第l個學習樣本結構s_l＝[X_l,U]，l＝1,2,…，12，取M個時刻(200～1000)個樣本，則i時刻狀態向量X的第l個樣本為g_l(s_li)，i＝1,2,…,M，l＝1,2,…,12，j時刻狀態向量X的第l個樣本為g_l(s_lj)，j＝1,2,…,M，l＝1,2,…,12；假設樣本的數據均值為0，i時刻狀態向量X的第l個樣本g_l(s_li)和j時刻狀態向量X的第l個樣本g_l(s_lj)的協方差為：

式(3)中：σ_lω表示測量噪聲標準差，

σ²_lω表示測量噪聲方差，

σ_lη表示過程噪聲標準差，

σ²_lη表示過程噪聲方差，

M_l表示對角矩陣，l＝1,2,…，12，

s_li表示i時刻狀態向量X的第l個樣本，i＝1,2,…,M，l＝1,2,…,12，

s_lj表示j時刻狀態向量X的第l個樣本，j＝1,2,…,M，l＝1,2,…,12，

δ_lij表示測量噪聲方差系數，l＝1,2,…,12，

采用梯度法求取最大似然，獲得測量噪聲標準差σ_lω、過程噪聲標準差σ_lη和對角矩陣M_l；對于下一時刻的樣本s*，學習模型的預測參數為：

式(4)中：K表示M行M列矩陣，

k(s*,s*)表示下一時刻的樣本s*的方差，

k(s*)表示下一時刻的樣本s*與之前M個時刻樣本分別的協方差，

k(s*)＝[k(s*,s₁),...,k(s*,s_M)]，

μ(s*)表示學習模型的預測值，

σ²(s*)表示學習模型更新的協方差值；

步驟3、學習模型在線更新

在所述第l個學習模型的基礎上，l＝1,2,…,12，依據置信度3σ(s_l)建立第l個學習模型的估計的擾動集合

式(5)中：表示第l個學習模型的估計的擾動集合，R^m表示m維空間，

表示第l個學習模型期望值，l＝1,2,…,12，

3σ(s_l)表示置信度，l＝1,2,…,12；

然后，采用下述判別函數λ(s_l)，判斷第l個學習模型的準確性

式(6)中：表示第l個學習模型的估計的擾動集合，R^m表示m維空間，

表示第l個估計的擾動集的補集，

g(s_l)表示第l個殘差，l＝1,2,…,12，

δ表示估計的擾動集里任一點，

dist表示有符號距離函數，定義為：對于給定的Rⁿ上的范數||.||，x∈Rⁿ到的符號距離為

式(7)中：Rⁿ表示n維空間，

A表示第l個估計的擾動集的補集

x表示第l個殘差g(s_l)中的一點，l＝1,2,…,12，

y表示A內的任一點，

x-y表示第l個殘差g(s_l)中的一點x與A內任一點y的差；

當判別函數λ(s_l)∈[0,1]時，實際的第l個殘差g_l(s_l)在估計的擾動集內,l＝1,2,…,12，設定經驗閾值λ_L∈(0,1)，當判別函數λ(s_l)＞λ_L時，不用更新學習模型否則重復步驟2和步驟3更新學習模型

步驟4、在線學習的四旋翼無人機跟蹤控制方法

步驟4.1、四旋翼無人機在線實時避障的概率約束條件

對于預測時域N，假設四旋翼無人機實際狀態服從正態分布：

式(8)中：表示期望為和方差為P(k+j)的正態分布，

表示k+j時刻四旋翼無人機的預測狀態，j＝0,1,2,…,N_p，

P(k+j)表示k+j時刻四旋翼無人機狀態不確定性的方差，j＝0,1,2,…,N_p；

設障礙物位置估計的不確定性w～N(0,Z)，則障礙物集合：

式(9)中：Ο_i表示環境中的障礙物，

R³表示3維空間，

Z表示障礙物位置估計不確定性的方差，

N(0,Z)表示期望為0和方差為Z的正態分布；

若四旋翼無人機的位置為p(k)，定義四旋翼無人機占據的空間為采用高斯分布描述四旋翼無人機位置的不確定性，則四旋翼無人機和障礙物的碰撞概率小于等于給定值P_h的約束條件為

式(10)中：表示期望為和方差為P_c(k+j)+Z的正態分布，

R(p(k+j))表示k+j時刻四旋翼無人機占據的空間，j＝0,1,2,…,N，

Ο表示障礙物集合，

表示k+j時刻四旋翼無人機估計的位置，

P_c(k+j)表示k+j時刻四旋翼無人機估計位置不確定性的方差，

Z表示障礙物位置估計不確定性的方差；

步驟4.2、四旋翼無人機狀態安全域約束方法

假設四旋翼無人機狀態約束集為Φ、控制輸入約束為Γ和一個較為保守的域Ω，使得殘差g(X,U)∈Ω；根據估計的擾動集合得到四旋翼無人機的安全域可達條件V(X)＞0和四旋翼無人機的安全域

式(11)中：Rⁿ表示n維空間，

X表示四旋翼無人機的狀態向量，

表示第l個學習模型的估計的擾動集合，l＝1,2,…,12；

當四旋翼無人機狀態滿足安全域可達條件V(X)＞0且判別函數λ(s_l)＞λ_L時，安全域采用并且以完整動力學模型的預測狀態作為狀態約束；當四旋翼無人機狀態不滿足安全域可達條件V(X)＞0或判別函數λ(s_l)≤λ_L時，安全域依賴名義模型構建，根據名義模型得到預測狀態將作為狀態約束；

步驟4.3、非線性模型預測控制器的設計

首先采用擴展卡爾曼濾波方法估計四旋翼無人機的狀態，然后將所述四旋翼無人機名義模型分為四旋翼無人機位置動力學模型和四旋翼無人機姿態動力學模型，分別采用反饋線性化方法獲得線性模型，最后針對線性模型在相應的約束條件下設計模型預測控制器；

考慮四旋翼無人機的位置，在滿足姿態約束、在線避障約束和安全域約束的條件下，將四旋翼無人機巡檢的自主控制轉化為二次優化：

式(12)中：N_p表示預測時域，

表示四旋翼無人機在k+ε時刻的預測狀態向量，

表示四旋翼無人機在k+ε時刻的參考狀態向量，

表示四旋翼無人機在k+N_p時刻的預測狀態向量，

表示四旋翼無人機在k+N_p時刻的參考狀態向量，

表示四旋翼無人機在k+ε時刻預測狀態向量與參考狀態向量的差，

表示四旋翼無人機在k+N_p時刻預測狀態向量與參考狀態向量的差，

表示四旋翼無人機在k+ε-1時刻的控制向量，

表示在k+ε時刻的誤差權矩陣，

表示的控制權矩陣，

表示在k+N_p時刻的誤差權矩陣；

當安全域滿足安全域可達條件V(X)＞0且學習模型滿足步驟2和步驟3時，采用學習模型和最大安全域作為狀態的約束條件，當安全域可達條件V(X)＞0不成立或學習模型不滿足步驟2和步驟3時，則采用預先設定的較為保守的域Ω和名義模型的狀態作為約束條件。