[發明專利]改進的柵格拓撲語義環境地圖的構建方法

在審

• 申請號: 201710141571.0
• 申請日: 2017-03-10
• 公開（公告）號: CN106970614A
• 公開（公告）日: 2017-07-21
• 發明（設計）人: 白迪;趙旦譜;臺憲青
• 申請（專利權）人: 江蘇物聯網研究發展中心
• 主分類號: G05D1/02
• 分類號: G05D1/02;G01C21/00
• 代理公司: 無錫市大為專利商標事務所(普通合伙)32104
• 代理人: 曹祖良,屠志力

1.一種改進的柵格拓撲語義環境地圖的構建方法，其特征在于，

建立一種柵格拓撲語義層次地圖：層次自下而上包括柵格層、拓撲層、語義層；

底層的柵格地圖使用機器人里程計和激光傳感器數據通過Gmapping算法構建；

最上面一層是語義層，用于用戶理解環境記錄地點名稱；上層的語義地圖是通過抽取拓撲節點包含的語義名稱構建；

中間的拓撲層將語義層的語義名稱和柵格層的位置坐標聯系在一起，成為上下層地圖之間聯系的橋梁；中間的拓撲地圖是在語音標注出拓撲節點的基礎上通過增量式算法構建的；拓撲路徑描述了拓撲節點之間的位置關系。

2.如權利要求1所述的改進的柵格拓撲語義環境地圖的構建方法，其特征在于，

柵格地圖的構建使用的是開源函數庫OpenSlam中提供的Gmapping算法，Gmapping算法是基于RBPF方法的一種高效SLAM解決方法；

SLAM基本原理：

SLAM問題可以被描述成：當移動機器人進入一個未知環境時，從一未知的位置開始探索，通過觀測模型和運動模型來完成身位置定位，同時不斷使用傳感器對周圍環境進行觀測，遞增地構建環境地圖；

x_t表示時刻移動機器人的位姿狀態向量，包括機器人的位置和方向；

m_i表示第i個路標的位置狀態向量；

u_t表示控制向量，由機器人里程計記錄，它在t時刻作用于機器人，使得機器人在t時刻到達狀態x_t；

z_t表示機器人在t時刻的路標觀測，由機器人激光傳感器記錄；

x_0:t＝{x₀,…,x_t}＝{X_0:t-1,x_t}表示機器人的位姿集合；

M＝{m₀,…,m_n}表示所有環境地圖路標的集合；

U_0:t＝{u₀,…,u_t}＝{U_0:t-1,u_t}表示控制輸入的集合；

Z_0:t＝{z₀,…,z_t}＝{Z_0:t-1,z_t}表示對路標觀測的集合；

根據概率的角度，SLAM問題是求解概率分布：

P(x_t,M|Z_0:t,U_0:,x₀)

SLAM的狀態空間可以分解為位姿分量和地圖分量，也就是說可以把地圖構建分為獨立的制圖和定位兩個部分:

P(x_t,M|Z_0:t,U_0:t,x₀)＝P(M|X_0:t,Z_0:t,)*P(X_0:t|Z_0:t,U_0:t,x₀)

RBPF算法使用粒子濾波器來估計粒子中的機器人軌跡，使用算法來更新粒子中的環境地圖，其基本算法流程如下：

首先確定機器人的初始坐標，根據初始坐標隨機生成個粒子并分配權重1/N,假設t-1時刻的粒子集為每個粒子維護建立一條機器人軌跡和環境地圖；

1)、采樣：

根據采樣分布對時刻的粒子集S_t-1(i)進行采樣并且預測出時刻的粒子軌跡；其中，采用運動模型作為采樣分布π；

$x_t^{\left(i\right)}~P\left(x_t\right|x_{t-1}^{\left(i\right)},u_t)$

2)、更新權值：

根據激光讀數，使用修正的VascoScan-Matching方法對每個粒子中新掃描的地圖和已經存在的地圖進行匹配并計算出粒子權重；匹配的點的數量越多，粒子的權重越大；其中采用觀測模型作為重要性采樣函數，如下式:

$w_t^{\left(i\right)}=w_{t-1}^{\left(i\right)}*P\left(z_t\right|x_t^{\left(i\right)},M_{t-1}^{\left(i\right)})$

3)、重采樣：

因為粒子集合權值的方差會隨著時間而不斷增加，使得權值聚集到少數粒子上，在若干次迭代后，可能只有很少數粒子的權重保持非零值，最終導致該粒子集合無法近似被估計出狀態的后驗概率，引發粒子退化現象；當有效樣本數目小于設定的閾值時需耍進行重采樣：首先重新選擇粒子集保留權值高的粒子，副除權值低的粒子，加入一部分新粒子，最后將粒子權值均一化，

4)、更新地圖：

對每一個粒子，在位姿已知時，使用EKF算法更新地圖；

$P\left(M_t^{\left(i\right)}\right|X_{0:t}^{\left(i\right)},Z_{0:t})$

通過以上4步可以計算出時刻t的粒子集

3.如權利要求1所述的改進的柵格拓撲語義環境地圖的構建方法，其特征在于，

拓撲地圖-語義地圖構建：

在機器人跟隨用戶移動過程中，當機器人到達對于人類來說有特殊意義的地點，用戶通過終端語音識別向機器人發出標注命令，機器人會將語音識別出的語義信息結合柵格地圖中的坐標信息來定義一個節點，將語義名稱和柵格坐標信息保存在一個MAP容器中；

MAP_top＝{＜sem_i,coor_i＞|sem_i∈S,coor_i∈R³}

S表示系統中出現的拓撲節點名字的集合，柵格點的坐標信息用＜x,y,θ＞即橫縱坐標和朝向角來表示；sem_i表示節點i處拓撲語義，coor_i表示節點i處的坐標信息；通過語音標注，可以就完成了對拓撲節點的提取；

在柵格地圖構建過程中，如果到達用戶認為有特殊意義的位置時，可以通過語音標注在柵格地圖上完成標記，記錄此處地點的柵格坐標信息；

使用對拓撲地圖的定義方法：

G＝{N,E,S,C,P}

N:是節點的集合，每個節點由語音標注獲取，具有一個獨一無二的名字；

E:是邊的集合，表示節點之間的約束關系；

S:是在當前節點位置處柵格地圖中機器人的激光，里程計信息集合；

C:是一個邊的約束，代表兩幀之間的高斯轉化；

P:是節點的全局最優位姿的集合；

通過語音標注獲得拓撲節點，再利用增量式拓撲地圖構建算法，可以在柵格層之上建立拓撲層的拓撲地圖；在這里每一個拓撲節點必須滿足與其他的拓撲節點之間三個約束條件：里程計約束；激光掃描約束；閉環回路約束；因為拓撲節點的提取是使用人類語音標注的方法完成，每一個拓撲節點都含有人類的語義信息，所以在拓撲層之上自然就抽取出一層語義層的語義地圖。