專利名稱：位移計算方法和設備及即時定位與地圖構建方法和設備的制作方法
技術領域：
本發明涉及位移計算方法和設備以及即時定位與地圖構建方法和設備。更具體地說，涉及能夠提高精度和處理速度的位移計算方法和設備以及即時定位與地圖構建方法和設備。
背景技術：
即時定位與地圖構建(SimultaneousLocalization and Mapping, SLAM)是目前在機器人定位方面的熱門研究課題。SLAM指的是機器人在自身位置不確定的條件下，在完全未知環境中創建地圖，同時利用地圖進行自主定位和導航。SLAM問題可以描述為:機器人在未知環境中從一個未知位置開始移動，在移動過程中根據位置估計和傳感器數據進行自身定位，同時建造增量式地圖。到目前為止，也取得了不少進展。由于高計算復雜度，SLAM通常是非常耗時的處理。通過采用其他裝置(如，相機和里程表)可以提高輸出精度和處理速率(幀每秒，FPS)。例如，在根據現有技術的SLAM中，可以不采用任何里程表而只采用單目相機或立體相機來跟蹤路標(landmark),并每隔一時間步長估計位移。這種方法的缺點在于:首先，由于高計算負荷使得這樣的處理是非常耗時的；另外，精度也不高；最后，該方法還會受到背景光的限制，例如，在背景光較暗的情況下，效果不佳。又如，在根據現有技術的SLAM中，可以使用電動機械的里程表來估計位移。然而，這種方法的缺點在于:首先，電動機械的設計和生產是非常復雜和昂貴的；其次，隨著電動機械的里程表的使用和老化，穩定性降低；第三，這種電動機械的里程表僅適用于二維測量，不能用于三維測量；最后，電動機械的里程表的維護費用很高且很復雜。
發明內容
鑒于以上情形，期望提供一種方法以及基于該方法的設備，其能夠通過采用慣導系統(Inertial Navigation System, INS)來改善在未知環境中對于移動對象(如,機器人或手持設備)的SLAM應用的性能。根據本發明實施例的一個方面，提供了一種用于在即時定位與地圖構建(SLAM)中計算一對象在空間未知環境中的位移的方法，包括如下步驟:基于固定時間間隔δ t重復地獲得所述對象的加速度矢量α、姿態矢量β ;在時間t，根據測量的數據以及之前估計的瞬時速度V’(t_ At)，獲得該對象在從時間t-At到時間t的時間間隔At期間的位移AD(t)以及該對象在時間t的姿態矢量β ⑴，其中 Δ = η.δ t, n e N ；基于由該對象上安裝的圖像采集單元拍攝的環境圖像和姿態矢量β (t)，對于所述位移AD(t)進行濾波以獲得AD’ (t)；根據濾波獲得的AD’ (t)更新瞬時速度V’(t)，并將所述瞬時速度V’ (t)用于下一次位移計算。
優選地，在根據本發明實施例的方法中，根據如下公式計算位移ADU):
權利要求
1.一種用于即時定位與地圖構建SLAM中計算一對象在空間未知環境中的位移的方法，包括如下步驟: 基于固定時間間隔St重復地獲得所述對象的加速度矢量α、姿態矢量β ； 在時間t，根據測量的數據以及之前估計的瞬時速度V’(t_ At)，獲得該對象在從時間t-At到時間t的時間間隔At期間的位移AD(t)以及該對象在時間t的姿態矢量β (t)，其中 Δ = η.δ t, n e N ； 基于由該對象上安裝的圖像采集單元拍攝的環境圖像和姿態矢量β (t)，對于所述位移AD(t)進行濾波以獲得AD’ (t)； 根據濾波獲得的AD’ (t)更新瞬時速度V’(t)，并將所述瞬時速度V’ (t)用于下一次位移計算。
2.根據權利要求1所述的方法,其中根據如下公式計算位移ADU):
3.根據權利要求1所述的方法，其中基于由該對象上安裝的圖像采集單元拍攝的環境圖像和姿態矢量β (t)，對于所述位移AD(t)進行濾波以獲得AD’ (t)包括: 首先基于所述圖像采集單元拍攝的環境圖像以及該對象在時間t的姿態矢量β (t)，通過SLAM方法計算時間間隔At期間的位移ADSUM(t)； 然后將ADSLM(t)與AD(t)進行加權平均，以獲得AD’(t)。
4.一種即時定位與地圖構建SLAM方法，其中通過權利要求1所述的方法計算一對象在空間未知環境中的位移。
5.—種應用于即時定位與地圖構建SLAM的、計算一對象在空間未知環境中的位移的設備，包括: 慣導系統INS傳感器，用于基于固定時間間隔St重復地測量該對象的加速度矢量α、姿態矢量β ; INS處理器，用于在時間t，根據由INS傳感器測量的數據以及之前估計的瞬時速度，計算該對象在從時間t-At到時間t的時間間隔At期間的位移AD(t)以及該對象在時間t 的姿態 β( ),其中 Δ = η.δ t, n e N ； 圖像采集單元，安裝在與所述對象上，用于以該對象的視角拍攝周圍環境的圖像； SLAM應用和濾波器，用于基于所述圖像采集單元拍攝的環境圖像和姿態矢量β (t)，對于由INS處理器計算出的位移AD(t)進行濾波以獲得AD’ (t)； INS代理，用于將由所述INS處理器計算出的AD(t)和β (t)傳送到所述SLAM應用和濾波器，并將由所述SLAM應用和濾波器輸出的AD’ (t)傳送到INS處理器； 其中，所述INS處理器根據從所述SLAM應用和濾波器獲得的AD’(t)估計更新的瞬時速度V’(t)，并用于下一次位移計算。
6.根據權利要求5所述的設備，其中根據如下公式計算位移AD(t):
7.根據權利要求5所述的設備，其中所述SLAM應用和濾波器首先基于所述圖像采集單元拍攝的環境圖像以及該對象在時間t的姿態矢量β (t)，通過SLAM方法計算時間間隔At期間的位移ADSUM(t)，然后將ADSLM(t)與AD(t)進行加權平均，以獲得Λ ，⑴。
8.根據權利要求5所述的設備，其中所述慣導系統INS傳感器包括陀螺儀、地磁儀和加速度計。
9.一種即時定位與地圖構建SLAM設備，包括如權利要求5所述的計算一對象在空間未知環境中的位移的設備 。
全文摘要
公開了一種同步定位與地圖構建(Simultaneous Localization and Mapping，SLAM)方法和設備。所述位移方法包括如下步驟基于固定時間間隔δt重復地獲得所述移動物體(如機器人或手持設備)的加速度矢量α、姿態矢量β；在時間t，根據測量的數據以及之前估計的瞬時速度V’(t-Δt)，獲得該對象在從時間t-Δt到時間t的時間間隔Δt期間的位移ΔD(t)以及該移動物體在時間t的姿態矢量β(t)，其中Δt＝n·δt，n∈N；基于由該移動物體上安裝的三維攝像機采集的數據和姿態矢量β(t)估算位移，并對于所述位移ΔD(t)進行濾波以獲得ΔD’(t)，進而完成三維地圖的構建；根據濾波獲得的ΔD’(t)更新瞬時速度V’(t)，并將所述瞬時速度V’(t)用于下一次同步位移與地圖構建計算。
文檔編號G06F17/16GK103105852SQ201110360180
公開日2013年5月15日 申請日期2011年11月14日 優先權日2011年11月14日
發明者李南君 申請人:聯想(北京)有限公司