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

在審

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

本發明涉及一種應用于災后救援環境下的移動消防機器人建圖方法。本發明首先進行系統初始化，包括機器人的狀態向量；然后將地標狀態加入到機器人運動模型中，并建立觀測模型；其次獲取機器人的最優控制輸入，并進行時間更新。如果沒有發現新的地標，只進行預測不更新。如果發現新地標進行量測更新，如果該地標已被其他機器人發現，對該地標估計進行凸組和融合。最后判斷是否遍歷完整個環境，確定地標的位置；輸出全部地標，完成建圖。本發明通過采用凸組合融合，引入吸引因子，既提升了機器人自身定位建圖準確性，又引導探索環境，降低不確定度，其定位建圖精度大大提升，可以有效提升災后救援的效率。

技術領域

本發明屬于救災領域，涉及一種應用于災后救援環境下的移動消防機器人建圖方法。

背景技術

同步定位與地圖構建技術廣泛應用于移動機器人、無人駕駛汽車、無人機、增強現實、導航或水下無人潛航器等領域。同時，隨著科技的進步和發展，在災后搜索和救援領域，該技術也逐漸得到廣泛使用。但是目前已有的建圖方法，大多是單機器人系統且沿著已知運動軌跡前進，這種被動式建圖與災難后現場環境未知相矛盾，導致建圖效果不佳，影響災后救援的效率。所以，在此基礎上，多機器人建圖逐步成為移動消防機器人領域的熱門。

在災后救援領域，多機器人主動建圖在探索能力、魯棒性、容錯性等方面比單機器人更具優勢，各個機器人通過通信網絡進行信息傳遞與共享，相互協調工作，共享環境信息，極大擴展了機器人系統對環境的感知范圍與定位的精度。但是，受限于機器人自身位置和傳感器觀測的誤差，多機器人主動建圖的精度仍會受到影響。因此，需要對建圖方法進行改進，使其能夠更好地適應于同步定位與地圖構建問題，同時提高機器人自身位置估計準確性以及指引多機器人進行環境的探索，降低被估計地標的不確定度，提升定位與建圖的精度。

發明內容

本發明的目的是針對現有移動消防機器人在觀測定位和建圖方面的不足，提供了一種應用于災后救援環境下，基于EKF-SLAM算法的多移動消防機器人主動建圖方法。

本發明包括以下步驟：

步驟一：初始化參數。

在多機器人主動建圖過程中，初始化機器人的數量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)進行決策。