[發明專利]一種基于機器人的跟隨且同時避障的方法與系統

在審

• 申請號: 202310304869.4
• 申請日: 2023-03-27
• 公開（公告）號: CN116125994A
• 公開（公告）日: 2023-05-16
• 發明（設計）人: 曲元震
• 申請（專利權）人: 森麗康科技（北京）有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 深圳市科吉華烽知識產權事務所(普通合伙) 44248
• 代理人: 孫偉

1.一種基于機器人的跟隨同時避障的方法，其特征在于，包括：

S1:根據所述機器人當前位置與目標的位置關系，規劃導航路徑；

S2:根據機器人當前的狀態，進行路徑向前仿真；

S3:根據評價指標對每條仿真路徑進行評分；

S4:選取得分最高的路徑得到控制指令。

2.如權利要求1所述的基于機器人的跟隨同時避障的方法，其特征在于，

所述S1:根據所述機器人當前位置與目標的位置關系，規劃導航路徑，包括：

S11:通過深度相機獲取目標在map坐標系下的位置；

S12:將目標位置作為機器人導航的目標點的位置發送給movebase；

S13:將目標點輸入到局部規劃器；

S14:對局部代價地圖進行處理；

S15:規劃參考路徑。

3.如權利要求1所述的基于機器人的跟隨同時避障的方法，其特征在于，

所述S2:根據機器人當前的狀態，進行路徑向前仿真，包括：

S21:獲取所述機器人當前狀態；

S22:生成采樣集；

S23:生成仿真路徑集。

4.如權利要求3所述的基于機器人的跟隨同時避障的方法，其特征在于，所述S23中，用戶設定仿真步長還有總的仿真時間；假設機器人在總的仿真時間內勻速運動，對總的采樣集內每個速度往前仿真，從而得到向前仿真路徑集；包括以下步驟：已知機器人當前位姿(x₀,y₀,θ₀)，仿真步長Δt，仿真線速度與角速度(v_i,ω_k)，那么使用下方代碼更新仿真路徑中每個路徑點的位置與朝向，

p_i,k＝[(x₀,y₀,θ₀)?(x_i,k,1,y_i,k,1,θ_i,k,1)?…?(x_i,k,m,y_i,k,m,θ_i,k,m)]

x_i,k,j+1＝x_i,k,j+v_i·Δt·cos(θ_j)

y_i,k,j+1＝y_i,k,j+v_i·Δt·sin(θ_j)

θ_i,k,j+1＝θ_i,k,j+ω_k·Δt

其中，j＝0,1,...,m其中m·Δt≤t_sim，t_sim為仿真的總時長；相同的總時長的情況下，仿真步長越大，則路徑的精度越低，但計算量越小。選擇合適的步長，避免計算耗時超過控制周期。

5.如權利要求1所述的基于機器人的跟隨同時避障的方法，其特征在于，

所述S3:根據評價指標對每條仿真路徑進行評分，這一步驟中，所述評價指標包括：

a)獲取代價地圖上的代價值；

b)最終位姿的角度；

c)與路徑的貼合程度；

d)所述機器人與目標點之間的距離；

e)遠離障礙物的代價。

6.如權利要求5所述的基于機器人的跟隨同時避障的方法，其特征在于，

所述b)最終位姿的角度的獲取方法為：獲取最后一個點的位姿的朝向角ψ，以及最后一個點的指向目標位置的向量的角度φ，評價項a為判斷兩個角度之差的絕對值其中θ_max表示用戶期望的最大偏差角度。

7.如權利要求5所述的基于機器人的跟隨同時避障的方法，其特征在于，

所述c)與路徑的貼合程度的方法包括：

其中d_end為仿真路徑最后一個點離目標點的距離，d2_max表示用戶期望的最大距離。