[發明專利]基于組合向量場法的無人機集群自主避障方法及裝置

有效

• 申請號: 202110705077.9
• 申請日: 2021-06-24
• 公開（公告）號: CN113433969B
• 公開（公告）日: 2023-02-17
• 發明（設計）人: 趙彥杰;梁月乾;袁莞邁;隋維舜
• 申請（專利權）人: 中國電子科技集團公司電子科學研究院
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 工業和信息化部電子專利中心 11010
• 代理人: 羅丹

1.一種基于組合向量場法的無人機集群自主避障方法，其特征在于，包括：

獲取無人機當前位置與障礙物中心的連線，與障礙物表面最近的一個交點；

確定所述障礙物表面在所述交點處的切平面，并計算所述切平面的法向量和所述切平面內經過所述交點的一個切向量；

基于所述法向量和所述切向量，為所述無人機設計當前制導律，以使得所述無人機在朝向目標飛行的前提下避開所述障礙物；

所述障礙物的形狀包括球體、圓柱體、圓錐體、平行六面體和圓臺；

所述獲取無人機當前位置與障礙物中心的連線，與障礙物表面最近的一個交點，包括：

用公式1表示所述障礙物：

其中，(x,y,z)^T表示空間中任一位置，q₀＝(x₀,y₀,z₀)^T表示所述障礙物中心，a_i、p_i(i＝1,2,3)均為實數，且當p₁＝p₂＝p₃＝1、a₁＝a₂＝a₃時，所述障礙物為球體；當p₁＝p₂＝1、p₃＞1、a₁＝a₂時，所述障礙物為圓柱體；當p₁＝p₂＝1、0＜p₃＜1、a₁＝a₂時，所述障礙物為圓錐體；當p₁,p₂,p₃＞1時，所述障礙物為平行六面體；當p₁＝p₂＝1、p₃＞1、R₁＞R₂＞0時，所述障礙物為圓臺；

所述計算所述切平面的法向量和所述切平面內經過所述交點的一個切向量，包括：

根據公式2計算所述切平面的法向量：

其中，F_x(x_i,y_i,z_i)、F_y(x_i,y_i,z_i)、F_z(x_i,y_i,z_i)分別表示函數F(x,y,z)關于x、y、z的偏導數在所述交點q_i＝(x_i,y_i,z_i)^T處的取值；

根據公式3計算所述切平面內經過所述交點的一個切向量：

其中，×表示向量的外積，q₀＝(x₀,y₀,z₀)^T表示所述障礙物中心，p_u＝(x_u,y_u,z_u)^T表示所述無人機當前位置，(x_t,y_t,z_t)^T表示目標位置；

所述基于所述法向量和所述切向量，為所述無人機設計當前制導律，包括：

根據公式4確定組合向量場：

其中，κ＞0表示可調整參數，F＝F(x,y,z)，s＝±1表示控制向量場旋轉方向的參數，V_nom＞0表示所述無人機當前位置的一個可設計速度；

基于所述組合向量場，根據公式5和公式6計算所述無人機期望航向角χ_d和期望爬升角γ_d：

χ_d＝atan2(N_dy,N_dx) 公式5，

其中，atan2表示四象限正切函數。