[發(fā)明專利]一種空間時間聯(lián)合優(yōu)化的四旋翼無人飛行器軌跡規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202010774434.2
• 申請日: 2020-08-04
• 公開（公告）號: CN112068586B
• 公開（公告）日: 2021-08-13
• 發(fā)明（設(shè)計）人: 耿莉;申學(xué)偉;張良基
• 申請（專利權(quán)）人: 西安交通大學(xué)
• 主分類號: G05D1/10
• 分類號: G05D1/10;B64C27/08
• 代理公司: 西安通大專利代理有限責(zé)任公司 61200
• 代理人: 高博

1.一種空間時間聯(lián)合優(yōu)化的四旋翼無人飛行器軌跡規(guī)劃方法，其特征在于，包括以下步驟：

S1、基于分段多項式來表示四旋翼無人飛行器軌跡，對前端路徑搜索得到離散路徑點，采用梯形時間分配法給各段路徑分配時間間隔T_i，通過無最大飛行速度與最大飛行加速度約束的多項式系數(shù)優(yōu)化環(huán)節(jié)得到初始軌跡路徑；

S2、對步驟S1得到的初始軌跡路徑進行分段處理，基于初始軌跡的持續(xù)時間代價與能量消耗代價，在考慮最大飛行速度與最大飛行加速度約束的情況下對時間間隔T_i進行比例調(diào)節(jié)，求解最優(yōu)調(diào)節(jié)比例并更新所有軌跡段的時間間隔；具體的，設(shè)某段初始軌跡為p_i(t)，計算代價函數(shù)，對于每段多項式軌跡，比例調(diào)節(jié)T_i為kT_i，k為某段多項式軌跡的時間調(diào)整比例，求解在滿足約束條件v(t)≤v_max,a(t)≤a_max下的最優(yōu)調(diào)整比例best_k，v(t)為多項式軌跡速度，a(t)為多項式軌跡加速度，v_max為無人機最大飛行速度，a_max為無人機最大飛行加速度，依據(jù)求得的最優(yōu)比例best_k更新多項式時間間隔；最優(yōu)比例best_k為：

其中，k_tight為每段軌跡時間間隔調(diào)整的臨界比例，cost_t為軌跡持續(xù)時間代價，cost_d為軌跡能量消耗代價；

按比例調(diào)節(jié)T_i為kT_i后，對應(yīng)的軌跡代價函數(shù)為：

cost_t'＝kcost_t

其中，cost_t'為比例調(diào)整之后的軌跡持續(xù)時間代價，cost_t為軌跡持續(xù)時間代價，cost_d'為比例調(diào)整之后的軌跡能量消耗代價，cost_d為軌跡能量消耗代價；

S3、交替進行無最大飛行速度與最大飛行加速度約束的多項式系數(shù)優(yōu)化、有最大飛行速度與最大飛行加速度約束的多項式時間間隔調(diào)節(jié)兩個環(huán)節(jié)，軌跡代價為軌跡持續(xù)時間代價與軌跡能量消耗代價的加權(quán)和，當(dāng)軌跡代價趨于收斂時結(jié)束循環(huán)步驟；

S4、對整個分段多項式軌跡路徑進行時間間隔的整體比例調(diào)節(jié)，確保整條軌跡滿足大飛行速度與最大飛行加速度約束，得到最終優(yōu)化軌跡，利用最終優(yōu)化軌跡時間離散得到的路徑點來控制飛行器跟隨軌跡，具體的，當(dāng)循環(huán)進行帶約束的、最優(yōu)比例時間間隔調(diào)節(jié)環(huán)節(jié)與無約束多項式系數(shù)優(yōu)化環(huán)節(jié)至代價函數(shù)的減小收斂到規(guī)定閾值，進行整體比例調(diào)節(jié)環(huán)節(jié)，使約束條件v(t)≤v_max,a(t)≤a_max在整個軌跡上都得到滿足；某段軌跡進行整體時間比例調(diào)節(jié)的過程為：

p_i(t)＝a₅t⁵+a₄t⁴+a₃t³+a₂t²+a₁t+a₀,t∈[0,T_i]

其中，軌跡p_i'(t)為軌跡p_i(t)依據(jù)整體調(diào)節(jié)比例kf進行更新后的軌跡。

2.根據(jù)權(quán)利要求1所述的空間時間聯(lián)合優(yōu)化的四旋翼無人飛行器軌跡規(guī)劃方法，其特征在于，步驟S1中，基于分段多項式表示四旋翼無人飛行器軌跡，每段四旋翼無人飛行器的三維軌跡表示為：

P(t)＝Aβ(t),t∈[0,T]

其中，A∈R^3×(N+1)，為多項式系數(shù)矩陣，N為多項式階數(shù)，β(t)＝(t^N…t²,t,1)^T為多項式基函數(shù)，T為對應(yīng)段多項式的時間間隔。

3.根據(jù)權(quán)利要求1所述的空間時間聯(lián)合優(yōu)化的四旋翼無人飛行器軌跡規(guī)劃方法，其特征在于，步驟S3中，無約束多項式系數(shù)優(yōu)化環(huán)節(jié)保持當(dāng)前多項式時間間隔不變，在不考慮最大速度和最大加速度的約束求解令軌跡能量消耗代價cost_d最小的多項式系數(shù)，優(yōu)化問題表達成QP問題求解，推導(dǎo)對應(yīng)的閉式最優(yōu)解。

4.根據(jù)權(quán)利要求1所述的空間時間聯(lián)合優(yōu)化的四旋翼無人飛行器軌跡規(guī)劃方法，其特征在于，整體調(diào)節(jié)比例kf為：

kf＝scale,if scale1

kf＝1,if scale≤1

其中，v_all_max為整條軌跡最大速度，a_all_max為整條軌跡最大加速度。