[發(fā)明專利]基于機器視覺和慣導融合的移動機器人運動姿態(tài)估計方法

有效

• 申請?zhí)枺?/td>: 201110417967.6
• 申請日: 2011-12-14
• 公開（公告）號: CN102538781A
• 公開（公告）日: 2012-07-04
• 發(fā)明（設計）人: 路丹暉;馬麗莎;楊飛;劉濟林
• 申請（專利權）人: 浙江大學
• 主分類號: G01C21/00
• 分類號: G01C21/00;G01C21/16
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 張法高

技術領域

本發(fā)明涉及一種基于機器視覺和慣導融合的移動機器人運動姿態(tài)估計方法，適用于自主機器人移動姿態(tài)估計及定位。

背景技術

精確的陸地定位系統(tǒng)在移動機器人的自主導航，路徑規(guī)劃，地形重建等方向都具有很重要的應用。傳統(tǒng)的機器人定位方法包括GPS，慣性導航定位等。GPS廣泛應用于車輛定位，但遮擋區(qū)域和室內無法應用；慣導(inertial?measurement?unit，IMU)通過積分角速度和線加速度實現(xiàn)航位推算，卻易受到噪聲影響導致定位結果“漂移”。此外，越來越多的研究將視覺定位方法作為傳統(tǒng)定位方法的補充。例如視覺里程計(visual?odometry，VO)，通過跟蹤前后幀圖像上的相同特征點，估計車體運動姿態(tài)的六個自由度。它適用于室內外、陸地及星際等多種環(huán)境，且定位精度高。但對圖像序列質量要求高，圖像中的移動物體、陰影都會影響其運動估計的準確性。

發(fā)明內容

本發(fā)明的目的是提供一種精度高、魯棒，適用性廣的基于機器視覺和慣導融合的移動機器人運動姿態(tài)估計方法。克服了傳統(tǒng)航位推算由于累積誤差造成的精度問題。

基于機器視覺和慣導融合的移動機器人運動姿態(tài)估計方法的步驟如下：

(1)同步采集移動機器人雙目相機圖像及三軸慣導數(shù)據(jù)；

(2)提取前后幀圖像特征并匹配估計運動姿態(tài)；

(3)利用慣導計算俯仰角和橫滾角；

(4)建立卡爾曼濾波器模型融合視覺和慣導姿態(tài)估計；

(5)根據(jù)估計方差自適應調整濾波器參數(shù)；

(6)姿態(tài)修正的累積航位推算。

所述步驟(1)為：機器人為通用輪式移動機器人，采用捷聯(lián)式慣性導航儀，其輸出為三維線加速度和角速度，采樣頻率100Hz，置于機器人幾何中心位置，慣性導航儀坐標系以垂直地面向下為Z軸方向，機器人行進正前方為X軸方向，機器人正右側且同時垂直于X軸方向和Z軸方向為Y軸方向，雙目立體相機置于機器人正前方中心且無遮擋，俯仰角為30-45度，相機采樣頻率為1Hz。

所述步驟(2)為：對雙目相機采集的左、右圖，提取尺度不變性轉換特征，對特征向量匹配后利用視差計算特征點的三維坐標，再通過匹配前后幀的尺度不變性轉換特征得到同組特征點幀間的運動三維坐標變化，去除離群值，用最小二乘法求解這組特征點運動方程，得到旋轉矩陣R和平移向量T。

所述步驟(3)為：利用慣性導航儀測量自身坐標系下3個軸向的加速度a_x，a_y，a_z，當無其他明顯外力作用時，即機器人處于勻速運動或者靜止等穩(wěn)定狀態(tài)時，得到

axayaz=-g-sinθsinφcosθcosφcosθ.]]>

其中g為重力加速度，根據(jù)慣導輸出的加速度a_x，a_y，a_z，可計算機器人實時的俯仰角θ和側傾角φ。

所述步驟(4)為：利用擴展卡爾曼濾波對視覺運動估計和慣導計算的姿態(tài)進行信息融合，定義狀態(tài)向量x為機器人當前的四元數(shù)[e₀，e₁，e₂，e₃]表征的姿態(tài)：