[發明專利]一種應用于災后救援環境下的移動消防機器人建圖方法

在審

• 申請號: 202110378149.3
• 申請日: 2021-04-08
• 公開（公告）號: CN113093760A
• 公開（公告）日: 2021-07-09
• 發明（設計）人: 王國勇
• 申請（專利權）人: 洛陽理工學院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 杭州奧創知識產權代理有限公司 33272
• 代理人: 王佳健

1.一種應用于災后救援環境下的移動消防機器人建圖方法，其特征在于包括以下步驟：

步驟一：初始化參數；

在多機器人主動建圖過程中，初始化機器人的數量n，機器人的狀態向量x^r＝[x,y,θ]^T，其中x,y為位置，θ為其方位角，離散控制輸入U＝{(v,ω)}，其中v為速度，ω為角速度，以及吸引因子的觸發條件e、t₀；

步驟二：考慮一個機器人系統，該系統中包含n個機器人，系統符合以下運動模型：

將地標狀態加入到中得到x_i,t+1：

其中w_i,t為系統噪聲，u_i,t為控制輸入，y_t為地標，包含了m個靜態地標的位置，f(·)為非線性系統函數，為機器人狀態，x_i,t+1為機器人和地標狀態；

步驟三：建立觀測模型z_i,t；

z_i,t＝h(x_i,t,y_t)+v_i,t,i∈{1,2,...,n} (3)

其中z_i,t為在t時刻第i個機器人的觀測值，h(·)是非線性量測函數，v_i,t是量測噪聲；

步驟四：獲取機器人的最優控制輸入；

吸引因子出現時機器人基于吸引因子朝向進行最優的控制決策；如果機器人自身的位置估計協方差大于一定閾值ε時，吸引因子會出現，其朝向為定位最精確的地標；如果機器人在t₀步內無新增擴展面積時，則會觸發吸引因子，其朝向為最近的虛擬探索地標；其余情況機器人基于公式(4)進行決策；

其中det(P_i,t+1)為估計誤差協方差的行列式，α，β為權重參數，C為歸一化符號；

步驟五：時間更新：

其中F_i,t和G_i,t分別為f(·)對機器人狀態向量與系統噪聲w_i,t的雅可比矩陣，Q_i,t為系統噪聲方差，和為一步預測狀態和估計誤差協方差；

步驟六：1)如果沒有發現新的地標，只進行預測不更新；2)如果發現新地標進行量測更新：

其中H_i,t+1是h(×)對一步預測狀態的雅可比矩陣，R_i,t+1為觀測噪聲方差，K_i,t+1為增益，為更新后的狀態，z_t+1為觀測值；

3)如果地標已被其他機器人發現，對地標估計進行凸組和融合；

其中P′_i,t+1∈P_i,t+1、P′_j,t+1∈P_j,t+1，P′_i,t+1和P′_j,t+1為第i和j個地標的估計和協方差；

步驟七：判斷是否遍歷完整個環境，確定地標的位置；不滿足，則轉到步驟四繼續執行；

步驟八：輸出全部地標，完成建圖。