[發明專利]一種基于智能機器人地面特征的PTAM改進方法

有效

• 申請號: 201510025810.7
• 申請日: 2015-01-19
• 公開（公告）號: CN104732518B
• 公開（公告）日: 2017-09-01
• 發明（設計）人: 賈松敏;王可;宣璇;張鵬;董政胤
• 申請（專利權）人: 北京工業大學
• 主分類號: G06T7/80
• 分類號: G06T7/80;G01C21/00
• 代理公司: 北京思海天達知識產權代理有限公司11203
• 代理人: 沈波

1.一種基于智能機器人地面特征的PTAM改進方法，其特征在于：

首先，完成參數校正，這包括參數定義與相機校正；接著對關鍵幀中的特征進行跟蹤，再實現位姿的初始化，再此基礎上對平面參數進行估計；然后利用相機獲取當前環境紋理信息，并構建四層高斯圖像金字塔，運用FAST角點檢測算法提取當前圖像中的特征信息，建立角點特征間的數據關聯，得到位姿估計模型；在地圖的初始繪制階段，獲取兩個關鍵幀將相機架設在移動機器人上，通過相機與地面標定后開始進行初始化過程；在初始化過程中，移動機器人開始移動，同時相機捕獲當前場景中角點信息并建立關聯，由位姿估計模型獲取當前位姿初始估計，再運用基于地面特征的位姿估計方法獲取當前位姿的準確估計；實現三維稀疏地圖的初始化后，更新關鍵幀并利用極線搜索與塊匹配方法建立特征點亞像素精度映射關系，結合位姿估計模型實現相機精確重定位；最后將匹配點投影到空間，完成當前全局環境三維地圖創建；同時利用全局與局部集束調整完成算法的一致性優化；

具體包括以下步驟：

步驟1，參數校正

步驟1.1，參數定義

(1)機器人位姿

在移動機器人起始處建立世界坐標系，在機器人移動過程中，其位姿表示為：

Tr=Rrtr01Rr=cosθsinθ0-sinθcosθ0001,tr=xy0---(1)]]>

由上式可知移動機器人位姿由x＝(x，y,θ)描述，其中，x，y分別表示移動機器人在x軸，y軸方向的平移量，θ為移動機器人繞z軸的旋轉角度；

在世界坐標系下，相機的位姿表示為:

Tc=Rctc01---(2)]]>

其中，T_c∈SE(3)，SE(3)為特殊剛體變換群，R_c為3×3旋轉矩陣，t_c為3×1平移矩陣；該位姿矩陣建立了世界坐標系與相機坐標系下點p_c，p_w的對應關系，即p_c＝T_cp_w；本方法中相機固定在移動機器人上，相機與機器人間存在固定的旋轉平移關系T_cr；在機器人移動過程中，若某一時間間隔機器人位姿增量為T_rs，與之對應，相機的位姿增量為T_cs，則：

T_rs＝T_rcT_csT_cr (3)

其中，

(2)地平面標定參數

將相機固定在移動機器人上，并將標定靶標平放在地面；相機通過獲取具有標定靶標的圖像信息建立相機與機器人間位姿關系；T_cp∈SE(3)為相機與標定靶標間的變換矩陣，R_cp，t_cp分別表示T_cp的旋轉與平移分量，其逆變換假定機器人的坐標原點與相機保持一致，且機器人的x軸與相機的z軸朝向相同；在靶標坐標系下，機器人的x方向向量由T_cp的第3行向量的前兩個分量表示；此時，機器人坐標系與靶標坐標系的變換關系表示為：

pr=TrpppTrp=cosγsinγ0xr-sinγcosγ0yr00100001---(4)]]>

其中，γ為機器人坐標系與靶標坐標系的旋轉角度，(x_r,y_r)為機器人原點在靶標坐標系下的坐標，該坐標由相機與靶標的平移向量確定；因此機器人與相機間的變換關系表示為T_cr＝T_cpT_pr，根據相機與靶標平面間的位姿關系，獲取在相機坐標系下的地平面方程：

(0,0,1,0)T_pcp_c＝0(5)

其中，p_c為相機坐標系下三維點，地平面參數表示為(n^T,d)，且n＝(0,0,1)R_pc，d＝t_p(3)；

步驟1.2，相機校正

由于透鏡在制造上的原因會存在畸變，畸變主要分為由透鏡形狀引起的鏡向畸變和由攝像機組裝過程引起的切向畸變；本方法采用FOV模型實現對單目相機的矯正；該模型是由Devernay和Faugeras提出的一種針對廣角相機去畸變方法，其數學描述如下：

ud=u0v0+fu00fvrdruxurd=1ωarctan(2rutanω2)ru=tan(rdω)2tanω2---(6)]]>

式中，x_u是像素歸一化坐標，u_d是畸變后像素坐標，ω為畸變系數，r_d,r_u分別為矯正前后歸一化坐標到坐標系中心的距離；

利用相機模型，將圖像像素坐標映射到歸一化坐標平面，同時結合相機內參數矩陣K，實現圖像畸變矯正，即：

u＝Kx_u(7)

步驟2，基于地面特征的初始化

步驟2.1，特征跟蹤

FAST角點檢測是由Edward Rosten和Tom Drummond提出的一種簡單快速的角點檢測算法；該算法利用像素點某個鄰域內相鄰像素點灰度值的差異，判斷該像素點是否為角點，如下式：

N=Σs∈c(uf)|I(s)-I(uf)|---(8)]]>

其中，u_f為候選角點，s為候選角點鄰域內任意一點，c(u_f)表示u_f的鄰域，I(s)為在s處圖像灰度值，若N超出閾值條件，s處的候選角點即為角點；

在相機跟蹤線程前，系統利用相機獲取當前環境紋理信息，并構建四層高斯圖像金字塔，采用FAST的角點檢測算法，結合塊搜索的方式建立角點特征間的數據關聯，利用投影關系預測三維點與當前幀中特征點的對應關系，并在位置附近固定區域進行搜索，以獲取準確的匹配關系；

步驟2.2，位姿初始化

在機器人移動過程中，通過相機獲取地面信息，并利用FAST算法實現角點特征的跟蹤；根據上述數據關聯信息，建立平面點的單應性關系，即：

π(Hu_r)＝u_p (9)

其中，π(p)為投影函數，p＝(x,y,z),π(p)＝(x/z,y/z,1)，u_r，u_p分別為參考幀與當前幀下對應特征點；根據相機平移量T_c以及地平面方程參數(n^T,d)，建立平面點的單應性關系：

H＝KT_c(I|n_d)^TK^-1 (10)

其中，n_d＝n/d，K為相機內參數矩陣；將式(3)代入式(10)，此時機器人的位姿與地面特征單應性關系描述為：

H＝KT_crT_rsT_rc(I|n_d)TK^-1(11)

利用上述圖像特征點的單應性關系，建立位姿估計模型：

x=argminxf(x)f(x)=Σp||r(x)||2---(12)]]>

其中，r(x)＝π(H(x)u_ref)-u_p為單應性關系誤差函數，即投影誤差函數，H(x)為單應性矩陣；根據小位移假設，誤差函數r的一階泰勒展開表示為：

r(ξ)=r(0)+∂r∂ξ|ξ=0ξ---(13)]]>

此時，能量函數的極值條件為：

∂f∂ξ=ΣJTr=0---(14)]]>

即：

ΣpJT(r(0)+Jξ)=0ξ=-(ΣpJTJ)-1ΣpJTr(0)---(15)]]>

其中，通過迭代求解上式獲取移動機器人的位姿估計；為了進一步提高定位精度，引入魯棒性權值構建基于M-estimation的位姿估計模型，此時能量函數表示為：

f(x)=Σpw(r)||r||2---(16)]]>

其中，w(r)為魯棒性Tukey加權函數，此時，能量函數的極值條件為：

ξ=-(ΣpJTw(r)J)-1ΣpJTw(r)r(0)---(17)]]>

步驟2.3，平面參數估計

機器人利用基于地面特征的單應性關系實現機器人定位，但由于非地面特征的影響，難以保證定位精度，因此采用一種快速的非地面特征去除方法；根據平行向量叉乘方式構建法向估計模型；由于角點檢測存在誤差，使得理想叉乘條件無法滿足，法向估計模型適應度差；利用投影誤差r作為判定依據，從而確定非地面特征信息；但隨著非地面特征與觀察點間距離減小，該方法區分度明顯降低；相比上述方法，在位姿初始估計的基礎上，利用單應性矩陣直接推導法向估計模型，通過獲取其與地平面方程參數的差異，判別該角點是否為地面特征信息；根據平面單應性關系式，建立平面參數n_d的估計模型：

f(nd)=Σp||r(nd)||2---(18)]]>

其中，r(n_d)＝π(H(n_d)u_ref)-u_p為相應投影誤差函數；與位姿估計求解過程類似，在小位移假設的基礎上，根據誤差函數的一階泰勒公式建立式(18)的求解模型；

此時，平面參數通過迭代式(18)獲取：

nd=nd+n^n^=-(ΣpJnTJn)-1ΣpJnTr(nd)---(19)]]>

其中，

步驟3，位姿估計

位姿估計模塊的主要功能是完成相機位姿的實時更新；其具體實施過程如下；在完成初始化工作后，根據投影誤差，建立位姿估計模型，其數學描述如下：

ξ=argminξΣObj(|ej|σj,σT)---(20)]]>

其中，e_j是投影誤差，且為Tukey雙權目標函數，σ_T為特征點匹配標準差的無偏估計值，μ為相機位姿六元組描述向量；

對于跟蹤中存在的失敗情況，提出特征觀察比值用以評估每幀圖像的跟蹤質量；若比值低于設定閾值，為跟蹤效果較差，則繼續跟蹤，但是系統不會將該幀更新到地圖中；若在多幀圖像的跟蹤中都出現了特征觀察比值低于設定閾值的情況時，即認為屬于“跟丟”的情況，那么就初始化跟蹤過程；通過以上步驟實現了位姿的精確估計，跟蹤過程得以正常進行；

步驟4，地圖創建

步驟4.1，地圖初始化

系統利用利用地面特征與標準立體相機算法模型建立當前環境初始化地圖；在地圖的初始化過程中，利用圖像中FAST角點匹配關系，結合RANSAC算法，采用基于地面特征的初始位姿估計方法，運用三角測量法計算當前特征點處的三維坐標，并確定起始點為全局世界坐標系原點，完成三維稀疏地圖的初始化；

在地圖的初始繪制階段，獲取兩個關鍵幀的具體過程如下，將相機架設在移動機器人上，通過相機與地面標定后開始進行初始化過程；在初始化過程中，移動機器人開始移動，同時相機捕獲當前場景中角點信息并建立關聯，通過求解式(12)獲取當前位姿初始估計；當該初始估計超過設定閾值d₀時，運用2.2中基于地面特征的位姿估計方法獲取當前位姿的準確估計；

步驟4.2，關鍵幀更新與極線搜索

地圖初始化時，僅僅包含兩個關鍵幀，并且只能對鄰近較小的體積空間進行描述，在相機離開初始位置時，需要添加新的關鍵幀以及地圖特征；若相機與當前關鍵幀間圖像幀數超出閾值條件，相機跟蹤效果為最佳，且相機與最近的地圖關鍵點距離盡量小時，將自動執行添加關鍵幀過程；由于實時性的約束，跟蹤系統可能只是測度到了幀中可視特征的子集，因此映射線程需要重新計劃和測量余下的地圖特征；

在關鍵幀的更新中，系統首先將會對新增加的關鍵幀中所有角點進行Shi-Tomas評估，獲取當前具有顯著特征的角點信息，成功觀測的特征點附近的顯著點將會被丟棄，保留下來的顯著性點將作為候選的地圖節點；由于新的地圖節點需要已知深度信息，僅靠單幀圖像無法獲取，因此需要選取與之最近的關鍵幀作為第二視圖，利用極線搜索與塊匹配方法建立特征點亞像素精度映射關系；在第二視圖中，采用零均值SSD法，結合位姿估計模型實現相機精確重定位；最后將匹配點投影到空間，以生成當前全局環境三維地圖；

步驟4.3，優化

利用最優的局部與全局LM集束調整算法實現當前地圖全局一致性優化以提高魯棒性；該集束調整算法的數學描述為：

{{μ2...μN},{p′1...p′M}}=argmin{{μ},{p}}Σi∈X∈YΣj∈SiObj(|eji|σji,σT)---(21)]]>

其中，σ_ji為在第i個關鍵幀中，FAST特征點匹配標準差的無偏估計，μ_i表示第i個關鍵幀的位姿六元組描述向量，p_i為全局地圖中的點；

通過不斷的集束調整，使得式(21)中的穩定代價函數極小；全局集束調整所有關鍵幀的位姿以及地圖點的位置，同時利用結構-運動問題中的內在稀疏性使得整體矩陣分解的復雜度呈現立方級別的下降，由O((N+M)³)降至O(N³)，但在多數的估算情況中又受到生成的外點矢量積的制約，隨著地圖的生長，這個運算量會顯著增加；

針對以上問題，引入局部集束調整，這里只對關鍵幀的子集位姿進行調整，此時的目標函數變為：

{{μχ∈X},{p′z∈Z}}=argmin{{μ},{p}}Σi∈X∈YΣj∈Z∩SiObj(i,j)---(22)]]>

X為系列關鍵幀的待調整集合，Y為固定幀集，Z為地圖點子集；集合X由地圖中一個最新的關鍵幀以及4個最接近的關鍵幀構成，而關鍵幀的所有地圖中的可視點又組成了集合Z，最后Y包含了所有由Z中測量數據所獲取的關鍵幀；由此，局部集束調整優化了最新、最近的關鍵幀的位姿，更新了地圖中的所有點，同時復雜度也顯著降低，不超過O(NM)。