[發明專利]一種基于軌跡預測的編隊圍捕方法

在審

• 申請號: 202111317808.9
• 申請日: 2021-11-09
• 公開（公告）號: CN114089748A
• 公開（公告）日: 2022-02-25
• 發明（設計）人: 陳博;鮑元康;李同祥;胡中堯;胡明南
• 申請（專利權）人: 浙江工業大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 杭州天正專利事務所有限公司 33201
• 代理人: 王兵

1.一種基于軌跡預測的編隊圍捕方法，包括以下步驟：

步驟1：為解決傳感器視野丟失問題，對機器人運動狀態進行估計與預測，具體包括：

1.1通過先驗知識建立目標運動軌跡的擬合函數，如式(1)所示

F_k(t；C_k)＝c₀φ₀(t)+c₁φ₁(t)+…+c_γφ_γ(t) (1)

其中，F_k(t；C_k)為k時刻軌跡擬合函數，{φ_i(t)}_{i＝0,1,…,γ}為先驗函數，根據先驗知識設置為單項式或者三角函數，C_k＝{c_i}_{i＝0,1,…γ}為k時刻待確定的參數，γ為擬合函數的階數，根據實際需要選定；

1.2初始化{φ_i(t)}_{i＝0,1,…,γ}，C_k；

1.3機器人傳感器對環境進行感知，尋找目標，若感知到目標時，其量測信息為Y_k，則存儲k時刻量測數據y_k＝Y_k；

1.4設置合適的滑動時間窗口[k-l,k]，其窗口長度為l，定義時間窗口內擬合殘差函數如式(2)所示

并通過求解最優化問題，最小化擬合殘差求解軌跡時間函數的參數估計如式(3)所示

1.5通過時刻k求解的參數估計獲得最優軌跡時間函數通過F_k(t)計算目標在k時刻的位置估計F_k(k)與下一時刻的位置預測F_k(k+1)；

1.6令k→k+1，循環執行上述步驟1.3、1.4、1.5；當執行至步驟1.3時，先判斷傳感器是否感知到目標，若感知到目標則繼續執行；若未感知到目標則執行步驟1.7；

1.7通過上一時刻的最優軌跡時間函數F_k-1(t)計算當前時刻的目標位置預測F_k-1(k)，存儲k時刻量測數據y_k＝F_k-1(k)，返回執行步驟1.4；

步驟2：建立多機器人編隊圍捕模型，具體包括：

2.1建立單個機器人動力學模型，如式(4)所示

其中，[sx_k sy_k]^T為k時刻機器人的位置坐標，θ_k為k時刻機器人的角度，v_k為機器人的線速度，w_k為機器人的角速度，T₀為系統的時間周期；

2.2建立多機器人系統模型，如式(5)所示

X_i,k+1＝X_i,k+Qu_i,k (5)

其中，i為機器人的編號，X_i,k∈R^3×1為k時刻機器人i的坐標與角度，u_i,k∈R^2×1為k時機器人i的控制量輸入，Q∈R^3×2為控制量的傳遞函數；

步驟3：計算多機器人系統的控制量，對目標進行編隊圍捕，具體包括：

3.1初始化編隊圍捕方法，如選擇方法1利用當前時刻目標信息進行圍捕則執行步驟3.2；如選擇方法2利用下一時刻目標信息進行圍捕則執行步驟3.3；

3.2選取目標在k時刻的位置估計F_k(k)，作為多機器人系統的圍捕目標位置，q_k＝F_k(k)；

3.3選取目標下一時刻的位置預測F_k(k+1)，作為多機器人系統的圍捕目標位置，q_k＝F_k(k+1)；

3.4計算多機器人自身的期望圍捕點，以環形隊形為例，如式(6)所示

其中，為機器人的期望圍捕點坐標，R為圓環隊形的半徑，N為編隊中移動機器人的數量；

3.5計算k時刻機器人與期望圍捕點的距離和角度，如式(7)所示

其中，d_k為k時刻機器人與期望圍捕點的距離，為k時刻機器人與期望圍捕點的角度；

3.6計算k時刻機器人的控制量，如式(8)所示

其中，λ∈R^2×1為控制參數。

2.如權利要求1所述的一種基于軌跡預測的編隊方法，其特征在于，在所述步驟1中，利用最小化擬合殘差算法迭代求得當前時刻特定長度時間窗口內目標運動的軌跡時間函數，通過軌跡擬合計算目標位置的狀態估計，在視野丟失時利用上一時刻的軌跡時間函數計算當前時刻目標位置的狀態預測值作為補償，提高了定位精度，減少了目標視野丟失帶來的影響。在步驟3執行在編隊圍捕時，通過對目標位置的預測值進行預測圍捕，提高了圍捕跟蹤的效率。