[發明專利]一種基于速度勢場的無人機避障方法和系統

在審

• 申請號: 202110963231.2
• 申請日: 2021-08-20
• 公開（公告）號: CN113655810A
• 公開（公告）日: 2021-11-16
• 發明（設計）人: 范琦涵;胡士強
• 申請（專利權）人: 上海微電機研究所（中國電子科技集團公司第二十一研究所）
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 北京五洲洋和知識產權代理事務所(普通合伙) 11387
• 代理人: 劉春成;劉素霞

1.一種基于速度勢場的無人機避障方法，其特征在于，包括：

步驟S1、根據無人機的位置和速度、障礙物的位置和速度，檢測無人機和障礙物的碰撞可能性；

步驟S2、若無人機與障礙物存在碰撞可能性，計算引力場對無人機產生的引力、改進的斥力場對無人機產生的斥力以及速度勢場對無人機產生的速度勢場力；其中，所述引力場是目標點對無人機產生引力的勢場，方向從無人機位置指向目標點，使無人機向目標點運動；所述改進的斥力場是根據無人機與目標點之間的位置關系，基于傳統人工勢場法進行改進，得到的障礙物對無人機產生斥力的勢場，控制無人機規避障礙物；速度勢場為與無人機速度矢量的方向、障礙物速度矢量的方向均垂直的勢場，對無人機產生速度勢場力，控制無人機規避運動障礙物；

步驟S3、根據步驟S20所述引力、改進的斥力場對無人機產生的斥力和速度勢場力，計算無人機第i步所受合力，其中，i取值為正整數；

步驟S4、根據無人機第i步所受合力，計算得到無人機第i步的方向單位角，并根據方向單位角計算無人機下一步運動的速度矢量、俯仰角和偏航角，控制無人機避開障礙物。

2.根據權利要求1所述的方法，其特征在于，在步驟S1中，按照碰撞錐檢測條件，對所述無人機和障礙物的碰撞可能性進行檢測；所述碰撞錐檢測條件為：

其中，v_r為無人機與障礙物的相對速度矢量，由v_r＝v-v_ob計算得到，v為無人機速度矢量，v_ob為所述障礙物的速度矢量，L_ob為所述無人機到所述障礙物的距離矢量，由L_ob＝p_ob-p計算得到，其中p_ob為所述障礙物的位置，p為所述無人機的位置，d_safe為所述無人機和所述障礙物需要保持的最小安全距離，d_safe＝r_ob+r，其中r_ob為所述障礙物半徑，r為所述無人機的最小安全半徑。

3.根據權利要求2所述的方法，其特征在于，根據碰撞錐檢測條件，按照公式：

計算得到所述改進的斥力場對無人機產生的斥力；

式中：F_rep,cone為所述改進的斥力場對無人機產生的斥力；▽為梯度符號；U_rep,att為根據無人機與目標點之間的位置關系，基于傳統人工勢場法，得到的改進的斥力場：

式中：U_rep,att為改進的斥力場，η為障礙物斥力因子，p為無人機位置，p_ob為障礙物位置，p_goal為目標點位置，ρ₀為障礙物對無人機運動產生影響的最大距離，ρ(p,p_ob)表示障礙物到無人機的距離，ρ(p，p_ob)＝||p_ob-p||，ρ(p,p_goal)表示目標點到無人機的距離；

λ_cone為根據所述碰撞錐檢測條件得到的碰撞錐判斷系數；

按照公式：

確定碰撞錐判斷系數λ_cone；

其中，v_r為無人機與障礙物的相對速度矢量，由v_r＝v-v_ob計算得到，v為無人機速度矢量，v_ob為所述障礙物的速度矢量，L_ob為所述無人機到所述障礙物的距離矢量，由L_ob＝p_ob-p計算得到，d_safe為所述無人機和所述障礙物需要保持的最小安全距離，d_safe＝r_ob+r，其中r_ob為所述障礙物半徑，r為所述無人機的最小安全半徑。