[發明專利]一種復雜環境下無人機目標跟蹤的軌跡生成方法

有效

• 申請號: 201910908599.1
• 申請日: 2019-09-25
• 公開（公告）號: CN110632941B
• 公開（公告）日: 2020-12-15
• 發明（設計）人: 彭志紅;奚樂樂;陳杰;王星博;蔣卓
• 申請（專利權）人: 北京理工大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 北京理工大學專利中心 11120
• 代理人: 高會允

1.一種復雜環境下無人機目標跟蹤的軌跡生成方法，其特征在于，采用由激光雷達、機載計算機、云臺相機以及無人機組成的系統進行目標跟蹤，包括如下步驟：

分段建立無人機運動軌跡多項式以及目標運動軌跡多項式；具體為：

將無人機的飛行時間進行分段，對于第i段，建立如下無人機運動軌跡多項式：

f_μi(t)＝a_0i+a_1it+a_2it²+…+a_nitⁿ＝q(t)A_μi

其中f_μi(t)為第μ∈x,y,z維的第i段的無人機運動軌跡多項式；μ用于指代世界坐標系的xyz軸，x，y，z為世界坐標系的xyz軸；

a_0i a_1i a_2i … a_ni分別為第i段的無人機運動軌跡多項式的各項系數；

A_μi為無人機運動軌跡多項式的系數矩陣，

tⁿ為t的n階導數

q(t)為關于時間t的基向量q(t)＝[1 t t² … tⁿ]；

對于第i段，建立如下目標運動軌跡多項式：

T_μi(t)＝b_0i+b_1it+b_2it²+…+b_nitⁿ＝q(t)B_μi

T_μi(t)為第μ∈x,y,z維第i段的目標運動軌跡多項式；

b_0i b_1i b_2i … b_ni分別為第i段的目標運動軌跡多項式的各項系數；

B_μi為目標運動軌跡多項式的系數矩陣，

采用云臺相機實時觀測獲取目標的三維位置信息，依據目標在設定時間段內的三維位置信息，通過擬合的方式對目標在未來時域內的運動軌跡進行預測；具體為：所述云臺相機實時采集目標圖像并輸出；選取一個時間段內所有時刻對應的目標圖像，機載計算機對所選取的目標圖像進行目標識別處理以及圖像定位，獲取目標的三維位置信息；

依據所選取的時間段內所有時刻下目標的三維位置信息，利用多項式擬合公式解算所述目標運動軌跡多項式的系數矩陣；

所述多項式擬合公式為最小二乘擬合公式和正則項的組合：

其中為最小二乘擬合公式，σB_ui^TB_ui為正則項，所述所選取的時間段為[t_l,t₀]，t_l為始時間，t₀為結束時間；p_k表示在時間t∈[t_l,t₀]區間內目標的第k個三維位置信息，共L個；T_ui(t_k)為根據p_k得到的目標運動軌跡多項式在t_k時刻的三維位置信息；σ為設置的權值；B_μi為待解算的目標運動軌跡多項式的系數矩陣，

所述機載計算機和所述激光雷達進行環境感知，建立環境八叉樹地圖，并利用A*路徑搜索算法在所述環境八叉樹地圖中進行搜索得到一條安全路徑，利用歐式距離場與膨脹方法生成無人機安全飛行通道；具體為：所述安全路徑由路徑節點組成；利用歐氏距離場方法得到與路徑節點距離最近的障礙物，以路徑節點為球心，以對應距離最近的障礙物與路徑節點的距離為半徑，生成初始化球體和所述初始化球體的內切立方體，對所述內切立方體進行膨脹，直到碰到所述距離最近的障礙物，得到凸立方體，所有路徑節點對應的凸立方體共同組成安全飛行通道；

建立軌跡生成的目標函數，所述目標函數為位置約束、速度約束以及能耗約束的加權求和函數；為所述目標函數添加幾何約束、動態約束以及多段多項式軌跡間的平滑性約束，并將目標函數轉化為凸優化問題；

所述位置約束為無人機與目標之間的相對距離等于設定的距離閾值；

所述速度約束為無人機與目標之間的相對速度最小化；

所述能耗約束為無人機運動軌跡的能耗最小化；

所述幾何約束為所述無人機安全飛行通道約束；

所述動態約束為所述目標的速度約束、加速度約束以及加加速度約束；

采用凸優化求解器求解所述凸優化問題，生成多項式飛行軌跡發送至所述無人機的飛行控制器，完成飛行軌跡跟蹤；

具體為：

所述目標函數為：

其中，

J₁為位置約束目標函數，J₂為速度約束目標函數，J₃為能耗約束目標函數；

λ₁為位置約束的權值，λ₂為速度約束的權值，λ₃為能耗約束的權值；

u_μi為轉換成二次型形式后的位置約束目標函數的系數矩陣，C_μi為轉換成二次型形式后的速度約束目標函數的系數矩陣，A_μi為轉換成二次型形式后的能耗約束目標函數的系數矩陣；

Q₁為轉換成二次型形式后的位置約束目標函數的海塞矩陣，Q₂為轉換成二次型形式后的速度約束目標函數的海塞矩陣，Q₃為轉換成二次型形式后的能耗約束目標函數的海塞矩陣；

q(t)為關于時間t的基向量，q⁽¹⁾(t)為q(t)的一階導數，q⁽³⁾(t)為q(t)的三階導數；

T_μi(t)為目標的運動軌跡多項式；f_μi(t)為無人機運動軌跡多項式；f_μi⁽¹⁾(t)和T_μi⁽¹⁾(t)分別無人機和目標的移動速度，f_μi⁽²⁾(t)表示無人機軌跡的加速度；

D_μi(t)為無人機與目標之間的相對距離，為設定的距離閾值；

為所述目標函數添加幾何約束、動態約束以及多段多項式軌跡間的平滑性約束，得到最終的目標函數：

min J

s.t.f_μi(t)∈D_corridor

其中，s.t.f_μi(t)∈D_corridor為安全約束，D_corridor為所述安全飛行通道，

為動態約束，v_min、a_min、v_max、a_max、分別為無人機最小速度、加速度和最大速度、加速度；

為平滑約束，f_μi^(k)(t)為f_μi(t)的第k階導數；

將最終的目標函數轉化為凸優化問題：

Q表示最終的目標函數的海塞矩陣；M_μi為最終的目標函數的系數矩陣；

凸優化問題中的約束項轉化為：

A_eqM_μi＝b_eq

A_ieM_μi≤b_ie

b_eq＝0

其中A_eq與A_ie分別為凸優化問題中等式約束與不等式的系數矩陣，b_eq與b_ie分別為等式約束與不等式約束向量；q(t)為關于時間t的基向量；和分別表示安全飛行通道中的最大值和最小值。