機器人分布式表征智能語義地圖創建方法
【專利摘要】機器人分布式表征智能語義地圖創建方法。首先，機器人遍歷室內環境，采用基于拓展卡爾曼濾波算法的視覺定位方法和基于邊界虛擬標簽算法的射頻識別系統分別對機器人和帶有快速識別碼的人工路標進行定位，建立度量層；然后，利用最小二乘法優化采樣點坐標，并且使用自適應譜聚類方法對定位結果進行分類，建立拓撲層；最后，根據攝像機快速識別的QR code語義信息，更新地圖的語義屬性，建立語義層。本發明在對室內環境中物體狀態進行檢測時，采用帶有QR code的人工路標，大大提高了語義地圖創建的效率，降低了創建的難度；同時采用QR code與RFID技術相結合的方法，提高了機器人定位的精確度和地圖創建的可靠度。
【專利說明】機器人分布式表征智能語義地圖創建方法
【技術領域】
[0001] 本發明屬于移動機器人導航【技術領域】，涉及一種室內環境下面向移動機器人同步 定位與地圖構建（SimultaneousLocalizationandMapping,SLAM)問題的機器人分布式 表征智能語義地圖創建方法。
【背景技術】
[0002] 近年來，隨著移動機器人在服務、探測、物流等領域的廣泛應用，SLAM問題受到了 學術界及工程界的密切關注，被視為移動機器人實現完全智能化的關鍵。
[0003] 然而，大部分的SLAM研究仍停留在機器人自身的定位和物理環境的重建上，沒有 涉及到機器人與人的交互，這使得機器人無法完成人類語義命令，大大限制了機器人的工 作職能，降低了工作效率。事實上，室內辦公環境存在多種帶有語義信息的大物體，當移動 機器人在室內辦公環境中工作時，機器人需要獲得復雜工作環境的區域信息和全局信息， 創建具有語義信息的全局地圖，實現與人類的語義交互，協助人類完成多種復雜工作。因 此，一個有效的室內環境下移動機器人精確定位和智能語義地圖構建方法具有重要的理論 意義及應用價值。
[0004]C.Nieto-Granda等人利用高斯區域分析方法，將環境特征按照概率進行劃分，從 而實現了基于語義的地圖劃分；而H.Wu等人提出利用快速識別碼（QuickResponseCode, QRcode)存儲環境信息，機器人通過視覺傳感器來捕獲信息，進而確定空間語義關系。然 而，前者只考慮了自然路標，在室內工作環境中，一般缺少易于捕獲的自然環境特征，故無 法解決自然路標稀疏帶來的問題；而后者雖然考慮到了自然路標稀疏，輔以QRcode來表述 環境，但視覺傳感器容易受到視角和距離的限制，且在大物體阻擋情況下會失效，進而導致 定位精度下降，影響語義地圖構建的可靠性。
【發明內容】
[0005] 針對移動機器人在室內辦公環境下難以獲取語義信息、完成語義命令的問題，提 供一種面向移動機器人SLAM問題的分布式表征智能語義地圖創建方法，機器人在環境中 進行遍歷式掃描時，利用視覺傳感器和射頻識別系統（RadioFrequencyIdentification, RFID)聯合定位，同時獲取大物體上的QRcode信息，將室內環境按照功能進行劃分與語義 更新，既提高機器人定位的準確性，又實現了語義地圖的構建。
[0006] 室內環境下機器人分布式表征語義地圖創建方法，包括以下步驟：
[0007] 步驟1:采集環境圖像，實現機器人視覺定位。
[0008] 步驟1. 1，移動機器人通過雙目視覺傳感器采集環境圖像，獲取人工路標集合。
[0009] 步驟1. 2,考慮到極幾何約束，基于三角測量原理，利用視差法獲取圖像中人工路 標的深度信息，并計算該路標的全局坐標。
[0010] 步驟1. 3,通過拓展卡爾曼濾波（ExtendedKalmanFilter,EKF)算法，對機器人 位姿及人工路標的位置進行定位。
[0011] 步驟2 :利用RFID系統進行機器人接收信號強度值（ReceivedSignalStrength， RSS)定位。
[0012] 步驟2. 1，根據實際參考標簽的坐標和閱讀器讀取的RSS值，求取虛擬參考標簽的 坐標值和RSS值。
[0013] 選用對數距離路徑損耗模型表述虛擬參考標簽與實際參考標簽上的能量差值隨 兩者距離的變化，獲取虛擬標簽的RSS值。
[0014] 步驟2. 2,基于邊界虛擬標簽（BoundaryVirtualLabels,BVIRE)算法確定目標 標簽（機器人和人工路標）的坐標值。
[0015] 根據目標標簽和參考標簽RSS值的差來選取最近鄰標簽。設定閾值，構建模糊地 圖。對于目標標簽來說，由于每個閱讀器都有自己對應的一個模糊地圖，將多個模糊地圖求 取交集可以求得最近鄰參考標簽，從而確定目標標簽的坐標。
[0016] 步驟3:修正機器人的全局坐標值，并利用自適應譜聚類算法進行地圖劃分。
[0017] 步驟3. 1:修正機器人及人工路標的坐標值。
[0018] 根據視覺傳感器和RFID系統獲得的機器人坐標值，利用最小二乘法（Least Square，LS)對兩者進行修正，得到更加準確的機器人全局坐標值。
[0019]步驟3. 2:利用自適應譜聚類算法，將修正后的機器人坐標集合進行拓撲層劃分。
[0020] 步驟4:語義信息更新。
[0021] 步驟4. 1 :機器人判斷視界中的紅色圓形區域，并利用橢圓擬合方法快速識別和 讀取QRcode中的語義彳目息。
[0022] 步驟4. 2 :根據QRcode中的語義信息，將機器人觀測點和相應物品之間進行關 聯，每個子集的語義拓撲地圖可以實時更新。
[0023] 步驟4. 3:根據更新的語義信息實時修正拓撲層子地圖分類，直至機器人完成遍 歷性掃描。
[0024] 與現有技術相比，本發明具有以下有益效果：
[0025] -方面，本發明在室內地圖進行構建時，采用視覺定位與RFID定位相結合的方 法，解決了攝像機視界局限和障礙物阻擋等問題，提高了機器人定位的精度；另一方面，考 慮了室內環境下可辨識環境路標缺失帶來的語義地圖構建困難，采用QRcode技術表述環 境語義特征，提高了語義地圖構建的準確度，為實現人機交互和機器人完成語義命令打下 良好的基礎。
【專利附圖】
【附圖說明】
[0026] 圖1是本發明所涉及方法的框架結構圖；
[0027] 圖2是本發明機載雙目視覺傳感器機器人模型示意圖；
[0028] 圖3是本發明RFID系統示意圖；
[0029] 圖4是基于視覺和RFID聯合定位的算法流程圖；
[0030] 圖5是本發明中采用QRcode人工路標的示意圖；
[0031] 圖6是機器人分布式表征智能語義地圖創建算法圖；
[0032] 圖7是機器人定位結果圖；（a)為視覺定位結果，（b)為RFID定位結果；
[0033] 圖8是LS法修正結果圖；
[0034] 圖9是本發明中基于語義信息的移動機器人的語義地圖劃分仿真結果圖；(a)為 期望結果，（b)為仿真聚類結果。
【具體實施方式】
[0035] 以下結合附圖對本發明作進一步詳細說明。
[0036] 如圖1-9所示，本發明所涉及方法的框架結構如圖1所示，包括以下步驟：
[0037]步驟1 :采集環境圖像，實現機器人視覺定位。
[0038] 步驟1. 1，移動機器人通過雙目視覺傳感器采集環境圖像，獲取人工路標集合。
[0039] 步驟1. 2,考慮到極幾何約束，基于三角測量原理，利用視差法獲取圖像中人工路 標的深度信息，并計算該路標的全局坐標。
[0040] (1)移動機器人具有兩個獨立的驅動車輪，并在機身上裝備雙目攝像機，機器人模 型示意圖如圖2所示。基于幾何學，可以獲得：
【權利要求】
1. 室內環境下機器人分布式表征語義地圖創建方法，其特征在于：該方法包括以下步 驟， 步驟1:采集環境圖像，實現機器人視覺定位； 步驟1.1，移動機器人通過雙目視覺傳感器采集環境圖像，獲取人工路標集合； 步驟1. 2,考慮到極幾何約束，基于三角測量原理，利用視差法獲取圖像中人工路標的 深度信息，并計算該路標的全局坐標； 步驟1. 3,通過拓展卡爾曼濾波算法，對機器人位姿及人工路標的位置進行定位； 步驟2 :利用RFID系統進行機器人接收信號強度值定位； 步驟2. 1，根據實際參考標簽的坐標和閱讀器讀取的RSS值，求取虛擬參考標簽的坐標 值和RSS值； 選用對數距離路徑損耗模型表述虛擬參考標簽與實際參考標簽上的能量差值隨兩者 距離的變化，獲取虛擬標簽的RSS值； 步驟2. 2,基于邊界虛擬標簽算法確定目標標簽的坐標值； 根據目標標簽和參考標簽RSS值的差來選取最近鄰標簽；設定閾值，構建模糊地圖；對 于目標標簽來說，由于每個閱讀器都有自己對應的一個模糊地圖，將多個模糊地圖求取交 集可以求得最近鄰參考標簽，從而確定目標標簽的坐標； 步驟3 :修正機器人的全局坐標值，并利用自適應譜聚類算法進行地圖劃分； 步驟3. 1 :修正機器人及人工路標的坐標值； 根據視覺傳感器和RFID系統獲得的機器人坐標值，利用最小二乘法（Least Square， LS)對兩者進行修正，得到更加準確的機器人全局坐標值； 步驟3. 2 :利用自適應譜聚類算法，將修正后的機器人坐標集合進行拓撲層劃分； 步驟4 :語義信息更新； 步驟4. 1 :機器人判斷視界中的紅色圓形區域，并利用橢圓擬合方法快速識別和讀取 QR code中的語義信息； 步驟4. 2 :根據QRcode中的語義信息，將機器人觀測點和相應物品之間進行關聯，每個 子集的語義拓撲地圖可以實時更新； 步驟4. 3 :根據更新的語義信息實時修正拓撲層子地圖分類，直至機器人完成遍歷性 掃描。
2. 根據權利要求1所述的室內環境下機器人分布式表征語義地圖創建方法，其特征在 于：該方法的具體實施流程如下步驟， 步驟1 :采集環境圖像，實現機器人視覺定位； 步驟1. 1，移動機器人通過雙目視覺傳感器采集環境圖像，獲取人工路標集合； 步驟1. 2,考慮到極幾何約束，基于三角測量原理，利用視差法獲取圖像中人工路標的 深度信息，并計算該路標的全局坐標； (1)移動機器人具有兩個獨立的驅動車輪，并在機身上裝備雙目攝像機；基于幾何學， 可以獲得：
其中，dt表示目標的深度信息，f為焦距，兩個攝像頭之間的基準寬度是L，目標與光學 中心線的距離為D1和D2，目標與投影距離為Cl1和d2 ; ⑵在機器人坐標系中，目標的坐標Hlpvisim'可以描述為：
其中，IVvisim表示機器人在全球坐標系中的位置，e為機器人坐標系相對全局坐標系 的偏轉角度； 步驟1. 3,通過EKF算法，對機器人位姿及人工路標的位置進行定位； (1) 初始化機器人的位置狀態和誤差協方差矩陣，根據當前時間k的位置，可以估計下 一時刻k+1的位置： iK-mimik + \lk、= f{fnr-Yiswn{klk)) P (k+l/k) = F (k) P (k/k) F (k) T+Q (k) 其中，屯表示機器人位置估計值，f?為狀態傳遞函數，P表示誤差協方差矩陣，Q為 噪聲協方差矩陣，F是f的雅可比矩陣； (2) 利用機載雙目視覺攝像機觀測人工路標的位置信息，并利用機器人位置估計值 +1 /幻和地標的觀測值Iiwsim來預測觀測估計； z{k +1) = h{mr ^ +1 / k\m,_vM〇tt) 其中，5表示觀測估計值，h為觀測傳遞函數； (3) 新息及其協方差矩陣可以表示為： y(k + \) = z{k + \)-z{k + \) S (k+1) = H (k) P (k+1 /k) H (k) T+R (k) 其中，Y表示新息值，z為實際觀測值，S表示新息協方差矩陣，R為噪聲協方差矩陣， H是h的雅可比矩陣； (4) 增益矩陣、機器人位置狀態和它的協方差矩陣可以更新如下： W (k+1) = P (k+1 /k) HV1 (k+1) >k ,/,/?"(k + \!k + \)^ mr (k + /k) + \v(k + I)y{k + I) P(k+l/k+l) = (I-ff(k+l)H(k))P(k+l/k) 其中，W表示增益矩陣，w表示各采樣點的增益值，I表示單位矩陣； 在全局掃描結束后，基于視覺傳感器的機器人同步定位與度量層地圖構建即完成； 步驟2 :利用RFID系統進行機器人RSS定位； 步驟2. 1，根據實際參考標簽的坐標和閱讀器讀取的RSS值，求取虛擬參考標簽的坐標 值和RSS值； 將每4個參考標簽作為頂點形成一個單元網格，對每個網格做N*N的劃分，添加網格虛 擬參考標簽，根據實際參考標簽位置坐標可求得網絡虛擬參考標簽的坐標； (1) 選用對數距離路徑損耗模型獲取虛擬標簽的RSS值：
其中，PWij)表示虛擬標簽的RSS值，Ptl表示實際參考標簽的RSS值，dp Cltl分別表示 虛擬標簽和實際參考標簽與閱讀器的距離，Iu表示噪聲，n為路徑損耗指數；室內各特定 位置的路徑損耗指數n是不同的，需要進行實時的測量，影響室內定位的及時性； (2) 采用參考標簽來實現動態測量路徑損耗指數n，提高室內定位的準確度；即在得到 兩條靠近待定位標簽路徑的路徑損耗后，可以根據待定位標簽與閱讀器構成的直線和參考 標簽與閱讀器構成的直線之間的角度求取權值進行加權：
其中，I^n2分別為兩條靠近待定位標簽路徑的路徑損耗指數，0 p 0 2為兩者與帶定位 路標的夾角； 步驟2. 2,基于BVIRE算法確定目標標簽的坐標值； (1)根據目標標簽和參考標簽RSS值的差來選取最近鄰標簽；設定閾值，構建模糊地 圖；對于目標標簽來說，由于每個閱讀器都有自己對應的一個模糊地圖，將多個模糊地圖求 取交集可以求得最近鄰參考標簽，從而確定目標標簽的坐標：
其中，m1KFID、Hvkfid為通過RFID系統獲取的人工路標和機器人坐標值，n a為各閱讀器獲 得的坐標信息總數;Wi為權值，可通過下式計算：
其中，Sk(Ti)表示第K個閱讀器讀到的（i，j)位置上的場強值，na為整個區域內選出的 最近鄰參考標簽的數量，Ii與鄰近參考標簽i連接在一起的區域的個數； 通過上述算法，可以實現基于RFID的機器人室內定位和度量層地圖創建； 步驟3 :修正機器人的全局坐標值，并利用自適應譜聚類算法進行地圖劃分； 步驟3. 1 :修正機器人及人工路標的坐標值； 根據視覺傳感器和RFID系統獲得的機器人坐標值，利用LS方法對兩者進行修正，得到 更加準確的機器人全局坐標值； (1)以人工路標為例，視覺定位和RFID定位的的線性校正為： nil - Q kiirfid+ (1_ Q ) ^1-vision 其中，ml為修正的人工路標坐標值，a為修正參數； 人工路標的定位誤差可表示為： e = {[^(-?////) ~ ^!-vision )+ (Xl-riSw,i - xO )]' +ia(yIRFID - y 1-vision ) + (>"/-,?/〇? ' >"〇 )]~ } 2 其中，e為定位誤差，（Xtl, yj為初始坐標值； (2)基于LS方法，要使得定位誤差為極小值，則e' =0;參數a可以計算為：
由上式可以得到糾正后人工路標的坐標；同理可以獲得機器人觀測點坐標的校正結 果； 步驟3. 2 :利用自適應譜聚類算法，將修正后的機器人坐標集合進行拓撲層劃分； (1) 設將無向加權圖G分成兩個子圖A和B，且A U B = V，A n B = f ;根據譜圖理論 建立規范割目標函數：
其中，Ncut (A，B)為規范割目標函數，cut (A，B)、cut (B，A)表示圖G分成兩個子圖A和 B的代價函數，asso (A，V)、asso (B，V)分別為A、B中各節點與所有節點連接權值總和； 其中：
其中，w (U, V)、w (u, V)表示各節點的連接權值； (2) 最小化Ncut值即獲得最理想的組劃分： \A, B^ = arg min Ncut( A, B) 這種分組的本質是使得大集合V下的兩個小集合A，B的元素對對方的影響最小化，即 實現了基于譜聚類的區分算法； (3) 基于BVIRE算法的RFID信息和QRcode識別信息進行不同位姿下的環境特征匹配； 根據傳感空間交疊情況設置節點間連線的權值《，兩個觀測節點i、j對應的匹配程度超過 某一閾值K，則認為兩節點相似，否則判定兩節點無關； (4) 確定所有節點間的權值，形成對稱相似矩陣W ;將相似矩陣的每行元素相加，即得 到該頂點的度屯，以所有度值為對角元素構成度矩陣，用D表示；其中Dii = Cli ;
其中，X表示譜聚類樣本； (5) 將X松弛到連續域[-1. 1]，求解minNcut (A, B)的問題轉化為：
上式的優化問題等于求解下列等式的第二小特征值問題：(D-W)x= X Dx;其中，X表 示其特征值；與第二小特征值對應的特征向量就包含圖的劃分信息；基于此特征向量對節 點進行組劃分，最終獲得全局拓撲地圖； 步驟4 :語義信息更新； 步驟4. 1 :機器人判斷視界中的紅色圓形區域，并利用橢圓擬合方法快速識別和讀取 QR code中的語義信息； (1) 在其外圍設定紅色圓形識別圈，方便機器人在距離較遠時準確找到QR code，進而 靠近它并掃描； (2) 機器人到達距離閥值后，機器人實時檢測機載攝像機采集的圖像信息中是否存在 紅色區域；檢測到紅色區域后利用橢圓擬合確定圖像中是否存在橢圓，即可確定視野中存 在人工地標； (3) 依據視野中人工地標的外層橢圓輪廓控制機器人運動，調整機器人與人工地標之 間的距離，使之到達QR Code的識讀距離閾值之內；期間須時刻保證檢測到的橢圓輪廓處 于攝像機視野的中間區域；調整策略為將機載攝像機視野做3*3分割，從左到右從上到下 依次為1到9區；當橢圓的中心處于不同區域時，采取相應的運動控制方式；例如，當橢圓 中心在1區即左上方時，機器人應向左轉，同時向前行進；當橢圓中心在6區即右側時，機器 人應向右轉； 步驟4. 2 :根據QR code中的語義信息，將機器人觀測點和相應物品之間進行關聯，每 個子集的語義拓撲地圖可以實時更新； 步驟4. 3 :根據更新的語義信息實時修正拓撲層子地圖分類，直至機器人完成遍歷性 掃描；QP code人工路標中寫入了編號、名字、功能和所屬區域等信息；機器人在觀測圖像 中讀取QR code，并立即向數據庫添加語義信息；當機器人進行全局掃描時，將機器人觀測 點和相應物品之間進行關聯，從而每個子集的語義拓撲地圖可以實時更新；數據庫中存儲 著所有大物體的語義信息，以及根據QR code中的信息實現的拓撲地圖的語義分類。
【文檔編號】G01C21/32GK104330090SQ201410573274
【公開日】2015年2月4日 申請日期:2014年10月23日 優先權日:2014年10月23日
【發明者】曹政才, 陳嵩, 付宜利, 翁志萍, 王炅, 趙琪 申請人:北京化工大學