[發(fā)明專利]自主移動(dòng)機(jī)器人視覺SLAM的不變性中心差分濾波器方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201910879673.1
• 申請(qǐng)日: 2019-09-18
• 公開（公告）號(hào): CN110610513B
• 公開（公告）日: 2022-02-08
• 發(fā)明（設(shè)計(jì)）人: 吳艷敏;丁國(guó)強(qiáng);田英楠;婁泰山;張鐸;方潔
• 申請(qǐng)（專利權(quán)）人: 鄭州輕工業(yè)學(xué)院
• 主分類號(hào): G06T7/277
• 分類號(hào): G06T7/277
• 代理公司: 鄭州優(yōu)盾知識(shí)產(chǎn)權(quán)代理有限公司 41125
• 代理人: 栗改

本發(fā)明提出了一種自主移動(dòng)機(jī)器人視覺SLAM的不變性中心差分濾波器方法，用以解決自主移動(dòng)機(jī)器人視覺SLAM系統(tǒng)運(yùn)動(dòng)模型狀態(tài)變量濾波計(jì)算度復(fù)雜度高問(wèn)題。本發(fā)明設(shè)計(jì)了面向矩陣?yán)钊合蛄靠臻g和不變Kalman濾波器的中心差分濾波器計(jì)算方法，濾波器狀態(tài)變量由SE(3)李群向量表示的機(jī)器人位姿、速度和3D路標(biāo)位置向量及加速度計(jì)與陀螺儀偏差向量組成，在矩陣?yán)钊嚎臻g中設(shè)計(jì)CDKF濾波器的Sigma采樣點(diǎn)均值及誤差量，利用單目視覺相機(jī)構(gòu)造運(yùn)動(dòng)圖像特征點(diǎn)的逆深度觀測(cè)模型，設(shè)計(jì)CDKF中心差分濾波器的預(yù)測(cè)與更新迭代計(jì)算，開展機(jī)器人定位與地圖構(gòu)建計(jì)算任務(wù)。相比于常規(guī)的機(jī)器人系統(tǒng)模型的EKF算法，本發(fā)明計(jì)算效能高，計(jì)算速度快，具備較好的實(shí)際應(yīng)用價(jià)值。

技術(shù)領(lǐng)域

本發(fā)明涉及航空航天系統(tǒng)信息處理的技術(shù)領(lǐng)域，尤其涉及一種自主移動(dòng)機(jī)器人視覺SLAM的不變性中心差分濾波器方法，即自主移動(dòng)機(jī)器人即時(shí)定位與地圖構(gòu)建系統(tǒng)(Visual Simultaneous Localization And Mapping,VSLAM)問(wèn)題的一種新型的視覺里程計(jì)不變性中心差分濾波器模型方法。

背景技術(shù)

最近幾年自主移動(dòng)機(jī)器人的視覺即時(shí)定位與地圖構(gòu)建(VSLAM)技術(shù)獲得較快發(fā)展，作為多傳感系統(tǒng)配置的自主移動(dòng)機(jī)器人系統(tǒng)數(shù)據(jù)融合方法有濾波器方法、梯度下降優(yōu)化方法以及光束平差(bundle Adjustment,BA)優(yōu)化技術(shù)等，其中梯度下降優(yōu)化法計(jì)算效能很好，但是相比濾波法計(jì)算量很大，濾波器法在實(shí)際應(yīng)用中使用的比較多。

在機(jī)器人系統(tǒng)模型研究中應(yīng)用最多的是位姿空間的SE(3)李群結(jié)構(gòu)，特別是近年來(lái)基于機(jī)器人系統(tǒng)狀態(tài)估計(jì)和控制應(yīng)用，SE(3)結(jié)構(gòu)體中的概率分布特性得到較好的研究分析。在SLAM系統(tǒng)中利用幾何保持特性的右不變擴(kuò)展Kalman濾波(Right InvariantExtended Kalman filter,RIEKF)算法中，把輸出函數(shù)投影到相機(jī)坐標(biāo)系中，RIEKF算法具有較好的誤差一致性特性，在2D視覺慣性導(dǎo)航系統(tǒng)(Visual Inertial NavigationSystem,VINS)中獲得較好的計(jì)算效能，但是多狀態(tài)約束RIEKF算法中強(qiáng)調(diào)的是圖像特征狀態(tài)變量的幾何約束特性來(lái)達(dá)到視覺里程計(jì)計(jì)算性能目的，沒有考慮到視覺SLAM問(wèn)題中的路標(biāo)狀態(tài)剔除操作，從而會(huì)導(dǎo)致系統(tǒng)狀態(tài)變量逐步增長(zhǎng)，計(jì)算量增加，對(duì)于視覺慣性SLAM系統(tǒng)導(dǎo)航計(jì)算性能造成不利影響。特別是針對(duì)3D場(chǎng)景應(yīng)用，RIEKF算法還沒有獲得較好的驗(yàn)證與應(yīng)用。

發(fā)明內(nèi)容

針對(duì)配置慣性測(cè)量組件IMU和單目視覺傳感器的自主移動(dòng)機(jī)器人的視覺SLAM中計(jì)算復(fù)雜度高的技術(shù)問(wèn)題，本發(fā)明提出一種自主移動(dòng)機(jī)器人視覺SLAM的不變性中心差分濾波器(CDKF)方法，面向SLAM矩陣?yán)钊合蛄康牟蛔冃灾行牟罘譃V波器計(jì)算，開展自主移動(dòng)機(jī)器人視覺SLAM系統(tǒng)運(yùn)動(dòng)模型狀態(tài)變量最優(yōu)濾波計(jì)算，計(jì)算效能高，計(jì)算速度快，具備較好的實(shí)際應(yīng)用價(jià)值。

為了達(dá)到上述目的，本發(fā)明的技術(shù)方案是這樣實(shí)現(xiàn)的：

本發(fā)明的有益效果：采用RIEKF算法的計(jì)算架構(gòu)，把SLAM問(wèn)題的路標(biāo)變量添加為系統(tǒng)狀態(tài)變量參與計(jì)算過(guò)程，基于不變性理論提出一種右不變中心差分矩陣?yán)钊核惴?，?yīng)用于自主移動(dòng)機(jī)器人視覺SLAM系統(tǒng)運(yùn)動(dòng)模型狀態(tài)變量最優(yōu)濾波計(jì)算過(guò)程，實(shí)現(xiàn)自主移動(dòng)機(jī)器人VSLAM系統(tǒng)模型狀態(tài)參數(shù)最優(yōu)濾波計(jì)算；本發(fā)明基于中心差分變換而不是傳統(tǒng)Taylor級(jí)數(shù)擴(kuò)展逼近變換，基于中心差分變換操作能夠有效改善算法的計(jì)算精度，相比于常規(guī)的機(jī)器人系統(tǒng)模型的EKF算法，中心差分變換的固定插值步長(zhǎng)保證可控制的計(jì)算穩(wěn)定性，計(jì)算效能高，計(jì)算速度快，具備較好的實(shí)際應(yīng)用價(jià)值，可應(yīng)用于無(wú)人機(jī)器人定位導(dǎo)航與控制領(lǐng)域。

附圖說(shuō)明

為了更清楚地說(shuō)明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1為本發(fā)明的流程圖。

圖2為自主移動(dòng)機(jī)器人運(yùn)動(dòng)軌跡圖。