[發明專利]一種固定翼無人機的軌跡規劃方法

有效

• 申請號: 201910169620.0
• 申請日: 2019-02-27
• 公開（公告）號: CN109871031B
• 公開（公告）日: 2022-02-22
• 發明（設計）人: 秦小林;劉佳;許洋;張力戈
• 申請（專利權）人: 中科院成都信息技術股份有限公司
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種固定翼無人機的軌跡規劃方法，其特征在于，該方法包含以下步驟：

步驟一、根據固定翼無人機基本參數設定控制策略的固定參數，初始化任務要求和相關控制參數；

步驟二、固定翼無人機通過傳感器實時檢測飛行前方狀況，根據預判式碰撞檢測方法，判斷障礙物是否真正對固定翼無人機構成威脅；

步驟三、根據實時檢測的威脅結果，采取不同的控制策略進行飛行；

步驟四、重復步驟二和步驟三，控制固定翼無人機飛行，直到到達目標位置；

所述步驟一具體為：建立xoy二維平面坐標系，將固定翼無人機的基本參數——固定翼無人機的起始位置p₀、最小速度v_m0、最大速度v_m1、最大加速度a、最大轉彎角φ_m、最小安全距離r，傳感器最大檢測半徑R設定為控制策略的固定參數；初始化任務要求——目標位置p₁，同時，初始化基于滾動時域控制策略的模糊粒子群優化方法的滾動時域控制步數T、粒子群最大迭代次數K、粒子群數量N、采樣時間Δt；

步驟二所述的預判式碰撞檢測方法具體為：(1)使用傳感器實時檢測同一障礙物的多個輪廓點，篩選出其中相鄰連線為凸的多個輪廓點，連接為一個凸多邊形，并求該凸多邊形的最小外接圓圓心坐標為O_i，半徑為R_i，其中i表示第i個障礙物；(2)如果d_i-R_i≤R，此時視障礙物i為潛在威脅源；d_i表示固定翼無人機當前位置p與障礙物最小外接圓圓心O_i之間的距離；(3)當潛在威脅源滿足時，將障礙物i視為真正的威脅源，將繼續向此方向飛行視為不可飛區域，其中，ψ_i為障礙物i和固定翼無人機當前位置連線與x軸正方向的夾角，α_i為固定翼無人機當前位置到障礙物i外接圓兩切線的夾角，為固定翼無人機當前位置與障礙物i相對速度v_i與x軸正方向的夾角；(4)將固定翼無人機當前飛行速度v分解到相對速度v_i方向，設為并判斷是否近似相等，即為靜態障礙物威脅，否則為動態障礙物威脅，其中ε為人為設定的誤差范圍；

所述步驟三的具體控制策略為：當沒有檢測到障礙物威脅時，固定翼無人機朝目標位置方向最大輸出前進；當檢測到靜態障礙物威脅時，采用基于滾動時域控制策略的模糊粒子群優化方法(R-RHC-FPSO)進行靜態避障飛行控制；當檢測到動態障礙物威脅時，采用分段線性人工勢場法(PLAPF)完成動態避障飛行控制；

所述的基于滾動時域控制策略的模糊粒子群優化方法(R-RHC-FPSO)的靜態避障飛行控制具體如下：

(1)建立固定翼無人機的動力學模型如下：

其中，I₂和O₂表示二維單位矩陣和零矩陣，p(k+j|k)、v(k+j|k)、a(k+j|k)分別為第k時刻預測得到的第k+j時刻的固定翼無人機位置向量、速度向量和加速度向量，當其中的j＝0時，分別等于第k時刻的固定翼無人機位置向量p(k)、速度向量v(k)和加速度向量a(k)；

(2)利用粒子濾波算法，用隨機分布在固定翼無人機當前位置附近的粒子替代固定翼無人機進行預測估計，用粒子預測位置與障礙物和目標位置的距離來構建最優化目標函數，更新粒子濾波參數，計算N個粒子在之后步數T內的位置和速度，同時，判斷每一個粒子的每一步是否在可行域②內，如果不在，則直接刪除該粒子；

其中，φ(k+j+1|k)為第k時刻預測得到的第k+j+1時刻的固定翼無人機轉角，D代表當前可檢測到的區域，F代表不可飛區域；

(3)選擇最優的粒子路徑，計算出預測的加速度a(k+j|k)，代回固定翼無人機動力學模型①，指導更新固定翼無人機下一時刻的位置；

所述的分段線性人工勢場法(PLAPF)的動態避障飛行控制具體如下：

(1)由固定翼無人機到目標點的引力勢場和固定翼無人機自身速度勢場構建第k時刻的引力勢場U_a，即：

U_a(k)＝ε_qρ²_g(k)+ε_v||v(k)||² ③

其中，ρ_g(k)＝||p(k)-p₁||表示第k時刻固定翼無人機與目標位置的距離，ε_q，ε_v為人為設定的引力增益因數；

(2)由障礙物勢場和固定翼無人機與障礙物之間的相對速度勢場構建第k時刻的斥力勢場U_ri，即：

其中，ρ_i(k)表示第k時刻固定翼無人機到障礙物i之間的距離，η_q、η_v為人為設定的斥力增益因數；

(3)將引力勢場和斥力勢場進行分段線性處理，并計算第k時刻的固定翼無人機與目標位置的連線相對于x軸的夾角α(k)，固定翼無人機與檢測半徑內障礙物i的連線相對于x軸的夾角θ_i(k)；

(4)根據公式⑤，指導更新固定翼無人機下一時刻的位置；

其中，p_x、p_y分別為固定翼無人機位置p在x軸和y軸方向上的分量。