[發明專利]基于混合整數二階錐優化的無人機避障實時軌跡規劃方法

有效

• 申請號: 202210049148.9
• 申請日: 2022-01-17
• 公開（公告）號: CN114594785B
• 公開（公告）日: 2023-07-14
• 發明（設計）人: 劉新福;張國旭
• 申請（專利權）人: 北京理工大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 北京正陽理工知識產權代理事務所(普通合伙) 11639
• 代理人: 鄔曉楠

1.基于混合整數二階錐優化的無人機避障實時軌跡規劃方法，其特征在于：包括如下步驟，

步驟一：建立以無人機當前位置為原點的慣性坐標系；

步驟一實現方法為，

選擇以無人機當前位置為原點O來建立慣性坐標系，x軸指向目標點，y軸與x軸構成右手直角坐標系，即完成以無人機當前位置為原點的慣性坐標系的建立；

步驟二：在步驟一建立的慣性坐標系下建立無人機的運動學方程以及約束條件，并構建最短時間的最優控制問題；所述約束條件包括控制約束、終端約束、二維平面任意形狀的避障約束，通過集合的形式建立二維平面任意形狀的避障約束；

步驟二實現方法為，

步驟2.1：在步驟一建立的慣性坐標系下建立無人機的運動學方程；建立控制約束、終端約束，并通過集合的形式建立二維平面任意形狀的避障約束，進而適用于二維平面任意形狀障礙物規劃；

無人機在該慣性坐標系下的運動學方程表示為：

其中x表示無人機的橫坐標位置，y表示無人機的縱坐標位置，θ代表無人機的航向角，V表示無人機的飛行速度，ω代表無人機的航向角的變化率，是控制量；

無人機的控制約束如下：

|ω|≤ω_max??????(2)

其中，ω_max表示所允許的最大航向角速率；

使用集合的形式表示無人機的避障約束如下：

其中，X表示整個二維平面空間，X_o,j表示被第j個障礙物占據的平面空間，M表示障礙物的總個數；

無人機的終端約束如下：

x(t_f)＝x_f,y(t_f)＝y_f,θ(t_f)＝θ_f?????????(4)

其中，t_f表示無人機的飛行時間，x_f、y_fθ_f分別表示給定的終端橫坐標、縱坐標以及航向角；

步驟2.2：建立最短時間的最優控制問題；

考慮時間最短的優化指標，得到如下最優控制問題

其中，t_f為飛行時間是需要優化求解的變量；

步驟三：使用橫坐標x作為自變量，將步驟二建立的最短時間最優控制問題轉為固定時間最優控制問題，便于離散化處理；并引入無量綱狀態量θ對航向角的正切值進行替換，將動力學中的非線性集中到動力學方程的最后一項，并引入約束繼續將動力學與目標函數中的非凸項轉至此約束中，并定義變量u將非凸項轉移至不等式約束|u|≤ω_maxδ³/V，方便后續步驟六凸化，提高優化效率；

步驟三實現方法為，

步驟3.1：使用橫坐標x作為自變量，將步驟二建立的最短時間最優控制問題轉為固定時間最優控制問題，便于離散化處理；

在最短時間問題以及步驟一建立的慣性坐標系下，橫坐標x是單調遞增的；選擇橫坐標x作為自變量，運動學方程變為：

其中，y′和θ′表示變量y和θ對橫坐標x的導數；式的形式不發生變化；式中的避障約束變為：

其中，并且表示集合X_o,j的邊界值；式變為：

y(x_f)＝y_f,θ(x_f)＝θ_f???????(8)

式中的優化目標變為：

步驟3.2：引入無量綱狀態量對航向角的正切值進行替換，將動力學中的非線性集中到動力學方程的最后一項，并引入約束繼續將動力學與目標函數中的非凸項轉至此約束中，并定義變量u將非凸項轉移至不等式約束|u|≤ω_maxδ³/V，方便后續步驟六凸化，提高優化效率；

通過公式(10)引入無量綱狀態量對航向角的正切值進行替換：

利用式(10)，式中運動學方程變為：

此時，原運動學中的狀態θ已經被替換掉，將動力學中的非線性集中到動力學方程的最后一項，式中的目標函數變為：

通過公式(13)引入約束將動力學與目標函數中的非凸項轉至過程此約束：

此時式中運動學變為：

而式中的優化目標變為下列線性形式：

通過公式定義變量u如下所示：

u:＝δ³ω/V??????(16)

利用式，式中的動力學變為下列線性形式：

通過式，式中的非凸項被轉移至式中

|u|≤ω_maxδ³/V??????(18)

而式中的終端約束變為

步驟四：在步驟三的使用橫坐標x作為自變量后，通過引入整型變量η_j，使步驟二中的二維平面任意形狀的避障約束成為包含整數變量的線性約束，進一步提高規劃求解效率；

步驟四實現方法為，

對于任意一個障礙物j，通過引入整數變量η_j，二維平面任意形狀的避障約束式變為線性約束(20)，進一步提高規劃求解效率；

其中，D是一個足夠大的常數；

步驟五：對步驟三中的過程約束進行松弛處理，得到約束的凸化形式，并結合步驟三、步驟四得到最短時間的最優控制問題的松弛形式，提高規劃求解效率；

步驟五實現方法為，

對步驟三中的過程約束式進行松弛處理，式被松弛為如式(22)所示的凸化形式：

式是凸的，即得到松弛問題如下：

步驟六：對步驟三中產生的非凸不等式約束|u|≤ω_maxδ³/V進行線性化處理，離散化后得到混合整數二階錐優化問題；迭代求解該混合整數二階錐優化問題獲得最短時間的最優控制問題的最優解，實現基于混合整數二階錐優化實現無人機避障實時軌跡最優規劃；不同于常規凸優化方法在未收斂之前無法產生可行解，若只求解一次混合整數二階錐優化問題亦能夠獲得原問題的近似最優解，實現無人機避障實時軌跡規劃，能夠用于提高規劃效率；

步驟六實現方法為，

對步驟三中產生的非凸不等式約束|u|≤ω_maxδ³/V進行線性化處理，在給定的初值δ⁽⁰⁾后，式能夠被線性化為：

離散點的數量為(N+1)，在本問題中的優化變量包括：y＝[y₀y₁...y_N]^T，u＝[u₀u₁...u_N]^T，δ＝[δ₀δ₁...δ_N]^T，η＝[η₀η₁...η_M]^T；我們定義變量離散化后得到混合整數二階錐優化問題如下：

其中，c，p和b是帶有恰當維度的向量，Θ和H是帶有恰當維度的矩陣，_K是二階錐的笛卡爾乘積；g_m代表式中的約束；問題Problem?D(δ⁽⁰⁾)是一個混合整數二階錐優化問題，給定一個初始的剖面δ⁽⁰⁾，迭代求解該混合整數二階錐優化問題獲得最短時間的最優控制問題的最優解，實現基于混合整數二階錐優化實現無人機避障實時軌跡最優規劃；不同于常規凸優化方法在未收斂之前無法產生可行解，若只求解一次混合整數二階錐優化問題亦能夠獲得原問題的近似最優解，實現無人機避障實時軌跡規劃。