目標即時定位和構建地圖的方法與系統(tǒng)的制作方法
【專利摘要】本申請公開了目標即時定位和構建地圖的方法和裝置。其中方法包括：1)對在活動范圍內移動的目標進行即時定位；2)根據(jù)對所述移動目標的定位，即時更新目標所在活動范圍的地磁場地圖；3)根據(jù)所述更新的地磁場地圖，對所述目標移動的下一位置進行定位；重復執(zhí)行步驟2)和3)，直到目標停止移動。該方法可以解決室內目標在沒有室內先驗磁場地圖的條件下，進行即時的目標定位和準確的地磁場地圖構建。
【專利說明】目標即時定位和構建地圖的方法與系統(tǒng)
【技術領域】
[0001]本發(fā)明涉及室內定位及地圖構建領域，特別涉及將室內目標的定位和地圖構建相結合的方法及系統(tǒng)。
【背景技術】
[0002]同時定位與地圖構建的問題是目標利用具有噪聲的傳感器數(shù)據(jù)，既要構建出環(huán)境中物體的分布情況，同時還要確定自身的位置。同時定位與地圖構建的問題的解法可使目標實現(xiàn)真正的自主導航。在過去的十幾年里，同時定位與地圖構建的問題逐漸成為移動機器人領域的研究熱點。通常的做法是利用EKF(Extended Kalman Filter:擴展卡爾曼濾波器)對機器人位置和地標進行同時估計。但是，當?shù)貥宋恢迷龆?，機器人移動的范圍變大時，該方法相應的計算量會變大，實時性變差。此外，該方法且需要事先對環(huán)境進行部署，布置好路標點，耗費人力，物力。

【發(fā)明內容】

[0003]針對上述現(xiàn)有技術的缺陷，本發(fā)明提供了目標定位與地圖構建的方法，該方法采用運動模型，通過對當前地磁場的測量，對當前位置進行矯正，和利用空間插值算法對局部地圖數(shù)據(jù)進行更新，并融合到全局地圖，達到了目標定位和地圖構建的同步進行，同時大大減小了運算量，也不需要事先對環(huán)境進行部署，節(jié)省資源。
[0004]根據(jù)本發(fā)明的一個方面，提供一種目標定位與地圖構建方法，包括:1)對在活動范圍內移動的目標進行即時定位；2)根據(jù)對所述移動目標的定位，即時更新目標所在活動范圍的地磁場地圖；3)根據(jù)所述更新的地磁場地圖，對所述目標移動的下一位置進行定位；重復執(zhí)行步驟2)和3)，直到目標停止移動。
[0005]在上述方法中，步驟I)可包括:初始化目標在活動范圍內的位置和活動范圍內的全局地磁場地圖，所述目標設置有測量目標運行的距離和轉動的角度值和磁場值的傳感器；將目標初始位置的地磁場值通過空間插值法加入所述初始化所述全局地磁場地圖；測量所述目標運行的控制變量和所述目標當前位置的地磁場值；采用粒子濾波算法對隨機移動的目標進行位置估計。
[0006]在一些實施方式中，所述初始化目標在活動范圍內的位置和活動范圍的地磁場地圖的步驟包括:將目標活動的室內范圍抽象化為網(wǎng)格坐標，將網(wǎng)格坐標的大小與活動室的實際大小按比例設置。目標在活動室內的初始位置是由目標的內部傳感器確定的。
[0007]在一些實施方式中，通過空間插值法如克里金插值法將測得的地磁場值加入初始地圖，對目標周圍的測量值進行加權以得出未測量位置的預測，
^ (^0) = 'Σ.為ζ)，其中各參數(shù)的含義是:入i為第i個位置處的

/—I
測量值的未知權重，Stl是預測位置，N是測量值數(shù)目。使用克里金方法時，權重Xi取決于測量點、預測位置的距離和預測位置周圍的測量值之間空間關系的擬合模型。[0008]在一些實施方式中，所述將地磁場值使用插值算法加入地圖的步驟包括:目標位置估計確定后，在地磁網(wǎng)格地圖中抽取位置目標點周圍的第一預定范圍的網(wǎng)格點數(shù)據(jù)以將測得的地磁場值插值，然后將插值后得到的所述第一預定范圍的網(wǎng)格點融合到第二預定范圍的網(wǎng)格地圖中。
[0009]同時，本發(fā)明還提供了目標定位和地圖更新系統(tǒng)，包括:定位模塊和地圖更新模塊，其中所述定位模塊對目標在活動范圍內的移動進行即時定位；所述地圖更新模塊根據(jù)對目標的定位即時更新目標所在活動范圍的地磁場地圖，所述定位模塊進一步根據(jù)所述更新的地磁場地圖，對目標下一位置進行定位校正，直到目標停止移動。
[0010]在一些實施方式中，其中所述定位模塊包括:初始化模塊，配置為對目標初始位置和地磁場網(wǎng)格地圖進行初始化；位置估計模塊，配置為使用粒子濾波算法和運動學模型對目標初始位置和運動中的位置進行估計，所述地圖更新模塊包括:插值模塊，配置為利用空間插值算法將測得的所述目標活動范圍的地磁場值進行插值，和將局部地圖融合到全局地圖。
[0011 ] 所述位置估計模塊可以基于粒子濾波實現(xiàn)，包括:粒子濾波算法模塊，配置為對目標的位置估計進行校正；運動模型模塊，配置為對目標在活動范圍內的移動通過預定數(shù)學模型進行分析。
[0012]在一些實施方式中，在所述權重的確定中，采用與磁場的方向和強度向量<HX，Hy,Hz>相應的網(wǎng)格地圖構成數(shù)據(jù)庫，分別表示對應的向量分布。由此，可以增加地圖的特征，有利于定位。
[0013]在一些實施方式中，所述構建地圖模塊中包括:插值模塊，在目標位置估計確定后，在地磁網(wǎng)格地圖中抽取位置目標點周圍的預定范圍的網(wǎng)格點數(shù)據(jù)將所測得的地磁場值插值，和融合模塊，將插得的預定范圍網(wǎng)格點融合到預定范圍的網(wǎng)格地圖中。
[0014]與現(xiàn)有技術相比，本發(fā)明的上述技術方案具有以下優(yōu)點:由于采用對目標的即時定位和根據(jù)即時定位的結果更新磁場地圖，又利用更新的磁場地圖對目標移動進一步定位，將定位與地圖更新結合來準確快速地對目標進行定位和構建連續(xù)一致的地圖。
[0015]進一步，本發(fā)明人通過深入和廣泛的研究，面對眾多的算法，選擇采用校正產(chǎn)生誤差的粒子濾波算法對室內地磁場地圖的更新，并采用空間協(xié)方差最佳插值方法-克里金算法插值，結果可信度高，這兩種方法的結合能夠更加準確快速地對機器人進行定位和構建連續(xù)一致的地圖。
[0016]并且，在現(xiàn)在的大多數(shù)建筑物內，由于鋼筋混泥土結構，家具等能影響磁場波動的物件存在，使室內磁場波動穩(wěn)定存在，而本發(fā)明正是利用室內的磁場波動來實現(xiàn)定位，進而實現(xiàn)即時定位和構建地圖。所以，本發(fā)明的實現(xiàn)不需要額外布置定位所需的參考點，由此可以減少花費。
【專利附圖】

【附圖說明】
[0017]圖1為本發(fā)明一種實施方式中目標定位方法的步驟示意圖；
[0018]圖2為本發(fā)明一種實施方式中初始化目標在活動范圍內的初始位置的示意圖；
[0019]圖3為本發(fā)明一種實施方式中活動范圍的地磁場網(wǎng)格初始地圖的示意圖；
[0020]圖4為本發(fā)明一種實施方式中將目標初始位置的地磁場值通過克里金插值法加入初始地圖的示意圖；
[0021]圖5為本發(fā)明一種實施方式中對移動的目標進行位置矯正的示意圖；
[0022]圖6為本發(fā)明一種實施方式中測量目標當前位置的地磁場值示意圖；
[0023]圖7為本發(fā)明一種實施方式中將地磁場值使用插值算法加入局部地圖的示意圖；
[0024]圖8為本發(fā)明一種實施方式中將局部地圖融合到全局地圖的示意圖；
[0025]圖9為本發(fā)明一種實施方式中地磁場的目標即時定位和地圖構建系統(tǒng)的組成示意圖。
【具體實施方式】
[0026]下面結合附圖對發(fā)明作進一步詳細的說明。
[0027]圖1為本發(fā)明一種實施方式中的地磁場的即時目標定位和地圖構建方法的步驟示意圖。如圖1所示，本發(fā)明一實施方式的目標即時定位和地圖構建方法的步驟包括:
[0028]步驟SlOl:對待定位的目標所處的活動區(qū)域(本例中以一個活動室為例)進行實地測量，獲得室內平面圖，并對平面圖進行模擬網(wǎng)格地圖初始化。對待定位的目標，結合目標內部傳感器的數(shù)據(jù)(Ii, θ i)其中，Ii為第i次移動的距離，Θ i為第i次移動的偏向角。確定其在模擬的初始化網(wǎng)格地圖中的位置。圖2中A所指位置是目標的初始位置，旁邊的黑點是粒子濾波算法中的重要性采樣算法對位置空間的采樣點，這些采樣點的均值表示對目標位置的估計。
[0029]步驟S102:若目標處于初始位置，則粒子均勻的分布在A的周圍。A的位置是由目標的內部傳感器測得的。隨著目標的移動，基于運動模型Xk = f (xk-1? Vk_1； wk),即通過xk_i和控制輸入量Vlri能求xk，其中，Xk為k時刻的狀態(tài)，Vk^1為k_l時刻的控制輸入量(即目標運行的距離和轉動的角度)，wk為k-Ι時刻到k時刻的相關誤差，傳感器本身以及周圍的環(huán)境會使測量帶有一定的誤差，所以機器人想要依據(jù)這些不確切的控制信息來實現(xiàn)精確地定位是很難的，本發(fā)明中的誤差為隨機噪聲和高斯白噪聲的累加和。本發(fā)明中所使用的運動模型為
【權利要求】
1.目標即時定位與構建地圖的方法，包括: 1)對在活動范圍內移動的目標進行即時定位； 2)根據(jù)對所述移動目標的定位，即時更新目標所在活動范圍的地磁場地圖； 3)根據(jù)所述更新的地磁場地圖，對所述目標移動的下一位置進行定位； 重復執(zhí)行步驟2)和3)，直到目標停止移動。
2.根據(jù)權利要求1所述的方法，其中所述對目標在活動范圍內的移動進行即時定位包括: 初始化目標在活動范圍內的位置和活動范圍內的全局地磁場地圖，所述目標設置有測量目標運行的距離和 轉動的角度值和磁場值的傳感器； 將目標初始位置的地磁場值通過空間插值法加入所述初始化所述全局地磁場地圖； 測量所述目標運行的控制變量和所述目標當前位置的地磁場值； 采用粒子濾波算法對隨機移動的目標進行位置估計。
3.根據(jù)權利要求2所述的方法，其中所述根據(jù)對目標的定位即時更新目標所在活動范圍的地磁場地圖包括: 在對目標的位置估計確定后，在所述地磁網(wǎng)格地圖中抽取位置目標點周圍的預定范圍的網(wǎng)格點數(shù)據(jù)將所述測得的地磁場值插值，然后將所得到的預定范圍網(wǎng)格點融合到預定范圍的網(wǎng)格地圖中。
4.根據(jù)權利要求1所述的方法，其中，所述初始化目標在活動范圍內的位置和活動范圍的地磁場地圖包括: 將目標活動的室內范圍抽象化為網(wǎng)格坐標，所述網(wǎng)格坐標的大小與活動室的實際大小按比例設置。
5.根據(jù)權利要求1-4任一項所述的方法，其中，通過克里金插值法將目標初始位置的地磁場值插入初始地磁場地圖中，包括: 對所述目標周圍的測量值按如下方式進行加權以得出未測量位置的預測
6.根據(jù)權利要求5所述的方法，其中，所述擬合模型為圓形，其變差函數(shù)表達式如下:
7.根據(jù)權利要求2所述的方法，其中所述采用粒子濾波算法對隨機移動的目標進行定位估計包括: 測量目標前一時刻到當前時刻移動的距離和偏移的角度，設粒子初始為均勻分布，隨著目標的移動，所述粒子按與所述目標相同的運動模型移動，基于非線性動態(tài)模型
8.根據(jù)權利要求7所述的方法，其中所述運動模型為:
9.目標即時定位和地圖構建系統(tǒng)，包括定位模塊和地圖更新模塊，其中 所述定位模塊對目標在活動范圍內的移動進行即時定位； 所述地圖更新模塊根據(jù)對目標的定位即時更新目標所在活動范圍的地磁場地圖，所述定位模塊進一步根據(jù)所述更新的地磁場地圖，對目標的下一移動位置進行定位，直到目標停止移動。
10.根據(jù)權利要求9所述的系統(tǒng)，其中 所述定位模塊包括: 初始化模塊，配置為對目標初始位置和地磁場網(wǎng)格地圖進行初始化； 位置估計模塊，配置為使用粒子濾波算法和運動學模型對目標初始位置和運動中的位置進行估計， 所述地圖更新模塊包括: 插值模塊，配置為利用空間插值算法將測得的所述目標活動范圍的地磁場值插值，和 地圖融合模塊，將局部地圖融合到全局地圖。
11.根據(jù)權利要求10所述的系統(tǒng)，其中 所述位置估計模塊包括: 粒子濾波算法模塊，配置為對目標的位置估計進行校正； 運動模型單元，配置為對目標在活動范圍內的移動通過預定數(shù)學模型進行分析。
12.根據(jù)權利要求11所述的系統(tǒng)，其中 所述粒子濾波算法模塊包括: 初始化粒子分布模塊，配置為后面粒子集的移動提供原始的參考； 重要性重采樣模塊，配置為產(chǎn)生基于目標移動過程中的粒子估計集合； 權重計算模塊，配置為對所述粒子賦予相關權重大小，其中與目標特性越接近則權重越大，反之越小； 歸一化權重模塊，配置為對所述粒子的權重進行歸一化處理；有效粒子數(shù)確定模塊，配置為找出與目標特性接近的粒子數(shù)；和 位置估計校正模塊，配置為對目標移動位置的估計進行校正。
13.根據(jù)權利要求12所述的系統(tǒng)，其中，所述權重計算模塊采用的數(shù)據(jù)庫是基于磁場的方向和強度由向量<HX，Hy, Hz>表示的三個網(wǎng)格地圖數(shù)據(jù)，各所述向量分別表示Hx，Hy, Hz的分布，所述三個網(wǎng)格地圖的網(wǎng)格大小相同。
14.根據(jù)權利要求13所述的系統(tǒng)，其中所述插值模塊配置為在目標位置估計確定后，在地磁網(wǎng)格地圖中抽取該位置處所述目標周圍的預定范圍的網(wǎng)格點數(shù)據(jù)，對所述測得的地磁場值進行插值，并且還包括: 融合模塊，將所述插值得到的預定范圍的網(wǎng)格點融合到預定范圍的網(wǎng)格地圖中。
【文檔編號】G01C21/08GK104019813SQ201410277471
【公開日】2014年9月3日 申請日期:2014年6月19日 優(yōu)先權日:2014年6月19日
【發(fā)明者】王新珩, 張聰聰, 賈尚杰 申請人:無錫知谷網(wǎng)絡科技有限公司