專利名稱：即時定位與地圖構建方法和設備的制作方法
技術領域：
本發明涉及即時定位與地圖構建方法和設備。更具體地說，涉及通過采用陀螺儀作為附加傳感器來加速定位處理的、基于矢量的即時定位與地圖構建方法和設備。
背景技術：
即時定位與地圖構建(SimultaneousLocalization and Mapping, SLAM)是目前在機器人定位方面的熱門研究課題。所謂SLAM就是將移動機器人定位與環境地圖創建融為一體，即機器人在運動過程中根據自身位姿估計和傳感器對環境的感知構建增量式環境地圖，同時利用該地圖實現自身的定位。在SLAM中，存在兩種坐標系:機器人坐標系和全局坐標系。機器人的位姿是指它在全局坐標系下的絕對坐標，例如三維環境中記為X = (X，y，Ζ，Θ )，其中X，y, Z表示在全局坐標系下的坐標，Θ表示在該點的姿態(或，視角)。在初始狀態下，機器人坐標系與全局坐標系重合。移動機器人在場景中運動，不需要任何先驗知識，利用雙目立體視覺來感知周圍環境信息，并提取穩定的圖像特征點來表征3D空間實際物理點，以此作為自然路標，來構建環境的幾何地圖，同時通過與當前時刻之前所創建的環境地圖(自然路標庫)中的路標進行匹配，估計機器人當前位姿并更新自然路標庫，從而實現移動機器人的SLAM過程。在現有的2D或3D SLAM應用中，迭代最近點法(Iterative Closest Point, ICP)是經常使用的算法。該算法通過迭代優化矩陣，在每次迭代過程中，對目標點集上的每個點，在參考點集中尋找最近點，并利用這樣的對應點，計算相應的旋轉矩陣和平移向量，將其用于目標點集上，得到新的目標點集并進入下次迭代過程，最終得到優秀的轉換矩陣，實現兩點集的精確配準。然而，該算法的缺點在于:1)深度數據的噪聲頻繁地導致最近點的不匹配；2)尤其對于大尺度平面的偏移來說，地圖校正能力很差；3)定位結果不精確。另外，基于尺度 不變特征變換(Scale-1nvariantfeature transform, SIFT)的SLAM也是已知的。SIFT是一種基于特征的配準方法。SIFT特征匹配算法是DavidG.Lowe在2004年總結了現有的基于不變量技術的特征檢測方法的基礎上，提出的一種基于尺度空間的、對圖像縮放、旋轉甚至仿射變換保持不變性的特征匹配算法。該算法匹配能力較強，能提取穩定的特征，可以處理兩幅圖像之間發生平移、旋轉、仿射變換、視角變換、光照變換情況下的匹配問題，甚至在某種程度上對任意角度拍攝的圖像也具備較為穩定的特征匹配能力，從而可以實現差異較大的兩幅圖像之間的特征的匹配。但是，這種方法的缺點在于對于環境亮度和隨機出現的路標具有很強的依賴性。可見，在使用通過范圍檢測設備(例如，單視場主動紅外相機或立體視覺相機)產生的深度數據的2D/3D SLAM中，大多通過點匹配來進行特征匹配。但是由于點云數據(Point Cloud Data，PCD)是海量的，從而使得處理非常耗時。此外，在本申請人于之前提交的另一專利申請中，還提出了基于環境矢量的SLAM方法。在所述方法中，以法向矢量唯一地表示與其垂直的平面，然后通過全局坐標系下與機器人坐標系下表示相同或相近平面的面匹配(即，法向矢量與特征矢量的匹配)來確定機器人當前的位姿。與采用點匹配的技術相比，由于基于環境矢量的SLAM方法中采用面匹配且在面匹配之前對相同或相近平面進行合并從而進一步減少了需要進行匹配的平面數量，因此大幅度地減小了待處理的數據量，極大地減小了 SLAM的計算負荷，并且提高了處理速度。另外，還可以提高包括地圖和定位結果在內的輸出精度。然而，在這種方法中，由于需要通過在多個特征矢量中尋求匹配來計算旋轉矩陣，因此定位速度有待進一步提高。
發明內容
鑒于以上情形，本發明在基于環境矢量的SLAM的基礎上，提出了采用陀螺儀作為附加傳感器的一種新的SLAM的算法和工作流程。這些特征矢量表示在待開發的地區或空間中可能頻繁出現的平面。根據本發明實施例的一個方面，提供了一種基于矢量的即時定位與地圖構建方法，其用于未知環境中的3D建模與地圖構建，包括如下步驟:使一設置有陀螺儀的設備以一未知視角獲取一幀點云數據；將所獲取的點云數據進行三角形網格化，以形成多個平面，獲得與多個平面中的每一個對應的法向矢量，并合并接近的法向矢量以獲得法向矢量列表；獲得已知環境地圖的環境特征矢量表；通過讀取所述設備上的陀螺儀的相應參數，獲得所述設備在該未知視角處的姿態，并根據該姿態獲得旋轉矩陣；將旋轉矩陣應用于法向矢量列表中的各法向矢量，從而獲得旋轉后的法向矢量表;將旋轉后的法向矢 量列表與所述環境特征矢量表中的各矢量進行匹配；將未能在環境特征矢量表中找到相匹配的特征矢量的法向矢量列表中的法向矢量添加到環境特征矢量表中以更新環境特征矢量表；利用相互匹配的矢量，計算位移；以及根據所述姿態和位移，完成當前幀與已知環境地圖的合并。優選地，在根據本發明實施例的方法中，如果所述環境特征矢量表不存在，則通過使用該幀的法向矢量表來初始化環境特征矢量表。優選地，在根據本發明實施例的方法中，每一個法向矢量Qi由四個元素(α、，α\, Qi2, Qi3)構成，其中Cii3表示坐標原點到與法向矢量a1垂直的平面的距離，α、、
Qi2分別表示X、y、ζ坐標的歸一化元素。優選地，在根據本發明實施例的方法中，基于最小均方誤差進行所述匹配。優選地，在根據本發明實施例的方法中，通過如下公式計算位移D’:
N-1Dy=-^m1-D1 ,
i=0其中，N表示交叉點的數量，ω 是歸一化的加權因子，Di是第i個交叉點的位移，所述交叉點是指在所述法向矢量表中由法向矢量表示的互不平行且不軸旋轉對稱的三個平面的交點。優選地，在根據本發明實施例的方法中，所述設備為機器人、手機、筆記本電腦或平板電腦。
根據本發明實施例的另一個方面，提供了一種基于矢量的即時定位與地圖構建設備，其用于未知環境中的3D建模與地圖構建,包括:點云數據獲取單元，用于以一未知視角獲取一幀點云數據；法向矢量獲取單元，用于將所獲取的點云數據進行三角形網格化，以形成多個平面，并獲得與多個平面中的每一個對應的法向矢量；合并單元，用于合并接近的法向矢量以獲得法向矢量列表；存儲單元，用于存儲已知環境地圖的環境特征矢量表；環境特征矢量表獲取單元，用于從所述存儲單元獲取所述環境特征矢量表；陀螺儀，用于獲得在該未知視角處的姿態；旋轉單元，用于根據該姿態獲得旋轉矩陣，將旋轉矩陣應用于法向矢量列表中的各法向矢量，從而獲得旋 轉后的法向矢量表；匹配單元，用于將旋轉后的法向矢量列表與所述環境特征矢量表中的各矢量進行匹配；更新單元，用于將未能匹配的法向矢量列表中的法向矢量添加到環境特征矢量表中以更新環境特征矢量表；計算單元，用于利用相互匹配的矢量，計算位移；以及3D定位與地圖構建單元，用于根據旋轉矩陣和位移的計算結果，完成當前幀與已知環境地圖的合并。優選地，根據本發明實施例的設備進一步包括:初始化單元，用于如果所述環境特征矢量表不存在，則通過使用該幀的法向矢量表來初始化環境特征矢量表。優選地，在根據本發明實施例的設備中，每一個法向矢量Cii由四個元素(α1。，α\, Qi2, Qi3)構成，其中Cii3表示坐標原點到與法向矢量a1垂直的平面的距離，α、、
Qi2分別表示X、y、ζ坐標的歸一化元素。優選地，在根據本發明實施例的設備中，所述匹配單元基于最小均方誤差進行所述匹配。優選地，在根據本發明實施例的設備中，所述計算單元通過如下公式計算位移D’:
N-1Dy=-^mrDi ,
i=0其中，N表示交叉點的數量，ω 是歸一化的加權因子，Di是第i個交叉點的位移，所述交叉點是指在所述法向矢量表中由法向矢量表示的互不平行且不軸旋轉對稱的三個平面的交點。在根據本發明實施例的基于矢量的即時定位與地圖構建方法和設備中，通過采用陀螺儀作為附加傳感器，能夠進一步加速定位處理，減小SLAM的計算負荷，并提高包括地圖和定位結果的輸出精度。
圖1示出了根據本發明實施例的基于矢量的即時定位與地圖構建方法的流程圖；圖2A-2C是分別示出了獲取點云數據、獲得法向矢量以及合并接近的法向矢量的示意圖；以及圖3是示出了根據本發明實施例的基于矢量的即時定位與地圖構建設備的配置的框圖。
具體實施例方式下面將參照附圖對本發明的各個優選的實施方式進行描述。提供以下參照附圖的描述，以幫助對由權利要求及其等價物所限定的本發明的示例實施方式的理解。其包括幫助理解的各種具體細節，但它們只能被看作是示例性的。因此，本領域技術人員將認識到，可對這里描述的實施方式進行各種改變和修改，而不脫離本發明的范圍和精神。而且，為了使說明書更加清楚簡潔，將省略對本領域熟知功能和構造的詳細描述。首先，將參照圖1描述根據本發明實施例的基于矢量的即時定位與地圖構建方法，其用于未知環境中的3D建模與地圖構建。圖1示出了根據本發明實施例的基于矢量的即時定位與地圖構建方法的流程圖。假設待測環境包括靜態的、易于識別的多個平面，如墻壁和屋頂。機器人可以頻繁地觀測到這些平面中的一些或全部，并通過利用它們進行快速定位。因此，將加速的SLAM處理設計為包括圖1中所示的如下步驟:首先，在步驟S101，使一設置有陀螺儀的設備以一未知視角獲取一幀點云數據。其中，該幀中至少包括三個屏幕。例如，所述設備可以為機器人、手機、筆記本電腦或平板電腦。該步驟的處理結果如圖2A所示。需要注意的是，在圖2A中，僅示意性地以附圖標記“X”示出了點云數據中的幾個點。然而，實際上，點云數據是海量的。另外，點云數據是三維分布的。這里，為了便于繪制，僅以二維形式示出。然后，在步驟S102，將所獲取的點云數據進行三角形網格化，以形成多個平面，獲得與多個平面中的每一個對應的法向矢量，該步驟的處理結果如圖2B所示。需要注意的是，在圖2B中示出了以兩個“ X ”點連接的線段以及與該線段垂直的法向矢量(其以箭頭表示)，但是該二維線段所要表示的是以多個點(至少三個點)確定的平面，且以箭頭表示的法向矢量垂直于該線段所表示的平面。為了便于繪制，在圖2B中僅以二維形式表示。由于與確定平面垂直的法向矢量是唯一的，因此可以每一個法向矢量均代表一個平面，即與其垂直的那個平面。假設一個平面的方程為Ax+By+Cz+D = 0，與其垂直的法向矢量Qi由四個元素(Qi0^i1, Cii2, Cii3)構成，即通過四個參數來限定該唯一的平面。其中Qi3表示坐標原點到與法向矢量α 1垂直的平面的距離，即
權利要求
1.一種基于矢量的即時定位與地圖構建方法，其用于未知環境中的3D建模與地圖構建，包括如下步驟: 使一設置有陀螺儀的設備以一未知視角獲取一幀點云數據； 將所獲取的點云數據進行三角形網格化，以形成多個平面，獲得與多個平面中的每一個對應的法向矢量，并合并接近的法向矢量以獲得法向矢量列表； 獲得已知環境地圖的環境特征矢量表； 通過讀取所述設備上的陀螺儀的相應參數，獲得所述設備在該未知視角處的姿態，并根據該姿態獲得旋轉矩陣； 將旋轉矩陣應用于法向矢量列表中的各法向矢量，從而獲得旋轉后的法向矢量表； 將旋轉后的法向矢量列表與所述環境特征矢量表中的各矢量進行匹配； 將未能在環境特征矢量表中找到相匹配的特征矢量的法向矢量列表中的法向矢量添加到環境特征矢量表中以更新環境特征矢量表； 利用相互匹配的矢量，計算位移；以及 根據所述姿態和位移，完成當前幀與已知環境地圖的合并。
2.根據權利要求1所述的方法，其中如果所述環境特征矢量表不存在，則通過使用該中貞的法向矢量表來初始化環境特征矢量表。
3.根據權利要求1所述的方法，其中每一個法向矢量α1由四個元素(α α \，aQi3)構成，其中Cii3表示坐標原點到與法向矢量Cii垂直的平面的距離，a; α\、a ^分別表示X、1、ζ坐標的歸一化元素。
4.根據權利要求1所述的方法，其中基于最小均方誤差進行所述匹配。
5.根據權利要求1所述的方法，其中通過如下公式計算位移D’: N-1i=0 其中，N表示交叉點的數量，Oi是歸一化的加權因子，Di是第i個交叉點的位移，所述交叉點是指在所述法向矢量表中由法向矢量表示的互不平行且不軸旋轉對稱的三個平面的交點。
6.根據權利要求1所述的方法，其中所述設備為機器人、手機、筆記本電腦或平板電腦。
7.一種基于矢量的即時定位與地圖構建設備，其用于未知環境中的3D建模與地圖構建，包括: 點云數據獲取單元，用于以一未知視角獲取一幀點云數據； 法向矢量獲取單元，用于將所獲取的點云數據進行三角形網格化，以形成多個平面，并獲得與多個平面中的每一個對應的法向矢量； 合并單元，用于合并接近的法向矢量以獲得法向矢量列表； 存儲單元，用于存儲已知環境地圖的環境特征矢量表； 環境特征矢量表獲取單元，用于從所述存儲單元獲取所述環境特征矢量表； 陀螺儀，用于獲得在該未知視角處的姿態； 旋轉單元，用于根據該姿態獲得旋轉矩陣，將旋轉矩陣應用于法向矢量列表中的各法向矢量，從而獲得旋轉后的法向矢量表；匹配單元，用于將旋轉后的法向矢量列表與所述環境特征矢量表中的各矢量進行匹配； 更新單元，用于將未能匹配的法向矢量列表中的法向矢量添加到環境特征矢量表中以更新環境特征矢量表； 計算單元，用于利用相互匹配的矢量，計算位移；以及 3D定位與地圖構建單元，用于根據旋轉矩陣和位移的計算結果，完成當前幀與已知環境地圖的合并。
8.根據權利要求7所述的設備，進一步包括: 初始化單元，用于如果所述環境特征矢量表不存在，則通過使用該幀的法向矢量表來初始化環境特征矢量表。
9.根據權利要求7所述的設備，其中每一個法向矢量Cii由四個元素(a:Qi1, Qi2,Qi3)構成，其中Cii3表示坐標原點到與法向矢量Cii垂直的平面的距離，a; α\、a ^分別表示X、1、ζ坐標的歸一化元素。
10.根據權利要求7所述的設備，其中所述匹配單元基于最小均方誤差進行所述匹配。
11.根據權利要求7所述的設備，其中所述計算單元通過如下公式計算位移D’:
全文摘要
一種基于矢量的即時定位與地圖構建方法，其用于未知環境中的3D建模與地圖構建，包括如下步驟使一設置有陀螺儀的設備以一未知視角獲取一幀點云數據；將點云數據三角形網格化，形成多個平面，獲得與多個平面中的每一個對應的法向矢量，合并接近的法向矢量以獲得法向矢量列表；獲得已知環境地圖的環境特征矢量表；通過讀取陀螺儀的相應參數，獲得設備在該未知視角處的姿態，并根據該姿態獲得旋轉矩陣；將旋轉矩陣應用于法向矢量列表中的各法向矢量，獲得旋轉后的法向矢量表；將旋轉后的法向矢量列表與環境特征矢量表中的各矢量進行匹配；更新環境特征矢量表；利用相互匹配的矢量，計算位移；以及根據姿態和位移，完成當前幀與已知環境地圖的合并。
文檔編號G06T17/05GK103247225SQ20121003184
公開日2013年8月14日 申請日期2012年2月13日 優先權日2012年2月13日
發明者李南君, 張賀 申請人:聯想(北京)有限公司