[發明專利]基于距離-角度優先級的多移動機器人編隊避障方法

有效

• 申請號: 202010670773.6
• 申請日: 2020-07-13
• 公開（公告）號: CN111813121B
• 公開（公告）日: 2023-07-18
• 發明（設計）人: 張鵬超;李海婷;咼生富
• 申請（專利權）人: 陜西理工大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京深川專利代理事務所(普通合伙) 16058
• 代理人: 謝雪梅

1.基于距離-角度優先級的多移動機器人編隊避障方法，其特征在于，包括以下步驟：

(1)根據機器人數量建立隊形數據庫；步驟(1)所述的建立隊形數據庫，設有i個機器人參與編隊，虛擬領航者表示為L，其位置坐標表示為[x_l?y_lθ_l]^T，跟隨者表示為F_i，根據機器人數量設定相應的隊形數據庫；

(2)形成距離-角度優先級避障策略；對單雙側障礙物與隊形的距離進行分析，判斷是否需要避障，若需要則根據距離-角度優先級判斷以何種隊形避障，并調用步驟(1)中的隊形數據庫進行避障準備；步驟(2)中形成距離-角度優先級避障策略的方法為：設機器人自身安全距離為半徑為r_s，則兩個機器人并排運動的最小安全距離為4r_s；

A單側障礙物避障策略

當虛擬領航機器人檢測到前方為單側障礙物時：

(a1)判斷是否需要避障，若d₀≤asinα+r_s，則需要避障，若d₀asinα+r_s，則不需要避障；

(a2)判斷障礙物方向，位于前進方向的上側或下側，若為上側，則機器人系統保持初始隊形右拐前進進行避障，若為下側，則機器人系統保持初始隊形左拐前進進行避障；

(a3)當編隊整體通過障礙物后隊形收斂到原軌跡水平線繼續前行；

B雙側障礙物避障策略

當虛擬領航機器人檢測到前方為雙側障礙物時，

(b1)首先判斷是否需要避障，若d₀≤d_l，則需要避障，若d₀d_l，則不需要避障，繼續保持原隊形運動，其中d_l＝2asinα+2r_s；

(b2)若需要避障，則判斷以哪種隊形避障；

若d₀2r_s，則按單側避障方案繞行避障；

若2r_sd₀4r_s，則需變換一字隊形通過障礙物；

若d₀4r_s時，則無需改變隊形，只改變角度,成樹杈狀三角形隊形通過；即a值不變，減小α；若障礙物沿機器人運動方向的沿面障礙距離較小，為縮短避障時間，則隊形成比例縮小通過障礙物；

通過對一般環境中可能出現的單雙側障礙物，通過機器人自身安全距離及隊形整體與障礙物之間距離計算，得出距離-角度優先級，即先判斷是否需要避障，其次優先考慮距離判斷是否需要改變隊形，最后決定是否需要改變角度以何種隊形通過；根據以上策略分析，得出最短路徑即消耗最少的方案；

(3)設計運動控制器，根據步驟(2)中決策的避障隊形開始避障；步驟(3)運動控制器設計，包括：

設虛擬領航者的位姿為q_d＝[x_d?y_dθ_d],u_d＝[v_d?w_d],跟隨機器人的位姿為q_i＝[x_i?y_iθ_i],u_i＝[v_i?w_i]，跟隨機器人i與虛擬領航者的相對位姿方程：

其中，Δx＝x_d-x_i，Δy＝y_d-y_i；

機器人的運動學方程：

其中x,y,θ分別為二維坐標系位置坐標及航向角分別為x,y,θ的導數；由反步法對線速度v和角速度w設計控制率得：

v＝v_dcos(p_θid)+k₁p_θid?(4)

w＝w_d+k₂v_dp_yid+k₃v_rsin(p_θid)???(5)

其中k₁,k₂,k₃為大于0的常數，v_d,w_d分別為理想線速度和角速度；

通過對跟隨者與虛擬領航者的相對位姿分析，結合運動學方程，得到線速度和角速度控制率，通過控制率調節機器人實際位姿，使其無限逼近理想位姿，實現機器人高精度編隊；

(4)避障結束后即最后一個機器人通過障礙物，快速恢復隊形；步驟(4)隊形快速恢復算法，包括：

為提高多機器人的工作效率，當機器人編隊整體通過障礙物后需及時恢復隊形，沿原軌跡繼續前行，設避障后虛擬領航者位姿為[x_l?y_l?θ_l]^T，設隊形快速恢復函數為：

p(t)＝(p₀-p_∞)e^-mt+p_∞??(1)

其中,m0,p₀為的初始值即避障后虛擬領航者位置狀態量，p_∞為p(t)的穩態值即恢復隊形后的位置狀態量,p(t)按指數快速遞減到p_∞的值，可實現隊形以指數形式快速恢復隊形。