亚洲免费av电影一区二区三区,日韩爱爱视频,51精品视频一区二区三区,91视频爱爱,日韩欧美在线播放视频,中文字幕少妇AV,亚洲电影中文字幕,久久久久亚洲av成人网址,久久综合视频网站,国产在线不卡免费播放

        ?

        基于磁力計(jì)、IMU和單目視覺(jué)的自主定位方法

        2019-12-05 05:10:06樊建彪樊歡歡
        宇航總體技術(shù) 2019年6期
        關(guān)鍵詞:移動(dòng)機(jī)器人測(cè)量

        樊建彪,孫 劍,樊歡歡,陳 偉

        (1.西安交通大學(xué)航天航空學(xué)院,西安 710049;2.西安交通大學(xué)機(jī)械結(jié)構(gòu)強(qiáng)度與振動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室,西安 710049)

        0 引言

        近年來(lái),隨著社會(huì)發(fā)展和科技技術(shù)的進(jìn)步,移動(dòng)機(jī)器人越來(lái)越多地應(yīng)用于日常生活中。導(dǎo)航技術(shù)不僅是移動(dòng)機(jī)器人的核心技術(shù),同時(shí)也是移動(dòng)機(jī)器人自主完成任務(wù)的基礎(chǔ)。目前移動(dòng)機(jī)器人的導(dǎo)航系統(tǒng)主要依靠GPS(全球定位系統(tǒng))與 IMU進(jìn)行融合解算位姿信息,而對(duì)于復(fù)雜環(huán)境此方案無(wú)法保證GPS信號(hào)的實(shí)時(shí)性,同時(shí)IMU模塊的零偏會(huì)造成解算的位姿漂移,因此應(yīng)用場(chǎng)景有限。

        SLAM(simultaneous localization and mapping)技術(shù)依靠移動(dòng)機(jī)器人自身攜帶的傳感器對(duì)周圍環(huán)境進(jìn)行觀測(cè),同時(shí)通過(guò)融合算法對(duì)各傳感器的數(shù)據(jù)進(jìn)行處理,可以實(shí)時(shí)得到移動(dòng)機(jī)器人在空間中的位姿,不依賴于外部傳感器信息,是實(shí)現(xiàn)機(jī)器人在復(fù)雜環(huán)境下自主定位導(dǎo)航的關(guān)鍵技術(shù)。SLAM技術(shù)于20世紀(jì)80年代被提出,經(jīng)過(guò)幾十年的發(fā)展,已經(jīng)產(chǎn)生多種成熟的SLAM算法。

        目前主要的SLAM算法包括單目視覺(jué)SLAM[1-5]、雙目視覺(jué)SLAM[6-8]、RGB-D視覺(jué)SLAM[9]和激光雷達(dá)SLAM[10]。單目視覺(jué)SLAM系統(tǒng)僅需一臺(tái)攝像頭和一套低成本IMU,其體積小、成本低、功耗小等優(yōu)點(diǎn)引起了學(xué)術(shù)界和工業(yè)界的極大興趣。自從基于擴(kuò)展卡爾曼濾波的Mono-SLAM[11]算法被提出以后,單目視覺(jué)逐漸流行起來(lái)。目前有眾多的單目視覺(jué)解決方案,包括PTAM[12]、SVO[13]、ORB-SLAM2[14]、MSCKF[15]和VINS-SLAM[16]等。這些方案主要是基于濾波器或者優(yōu)化器來(lái)融合IMU和視覺(jué)傳感器數(shù)據(jù)對(duì)位姿進(jìn)行解算。而目前的單目視覺(jué)SLAM算法主要是依靠IMU數(shù)據(jù)來(lái)解決單目視覺(jué)解算過(guò)程中的尺度問(wèn)題,但I(xiàn)MU存在漂移,其尺度解算精度不高。同時(shí)單目視覺(jué)SLAM算法主要依靠單目視覺(jué)數(shù)據(jù)進(jìn)行優(yōu)化得到位姿信息,但是當(dāng)運(yùn)動(dòng)為純旋轉(zhuǎn)時(shí),則無(wú)法通過(guò)求解對(duì)極幾何來(lái)求解機(jī)器人的姿態(tài)角。該算法在實(shí)際應(yīng)用中存在位姿解算漂移的問(wèn)題,同時(shí)解算精度不高。

        通過(guò)將三維磁阻傳感器按照載體坐標(biāo)系安裝,磁力計(jì)可測(cè)量載體空間磁場(chǎng)的三維磁感應(yīng)強(qiáng)度,據(jù)此可以計(jì)算出載體的姿態(tài)角。因此本文首次提出了基于磁力計(jì)、IMU和單目視覺(jué)傳感器融合的位姿解算方法,用來(lái)對(duì)視覺(jué)尺度因子、重力、速度、IMU參數(shù)和載體姿態(tài)進(jìn)行估計(jì),解決單目視覺(jué)在解算位姿時(shí)由于純旋轉(zhuǎn)情況而出現(xiàn)的不穩(wěn)定情況,同時(shí)本方案具有高精確和強(qiáng)魯棒的特性。

        1 算法設(shè)計(jì)

        本文通過(guò)融合磁力計(jì)、IMU和單目視覺(jué)傳感器數(shù)據(jù),設(shè)計(jì)能夠在三維空間中實(shí)時(shí)估計(jì)移動(dòng)機(jī)器人位姿的算法。所設(shè)計(jì)的擴(kuò)展卡爾曼濾波器將磁力計(jì)和IMU數(shù)據(jù)進(jìn)行融合,解算出機(jī)器人的姿態(tài)并對(duì)陀螺儀零偏進(jìn)行了估計(jì),之后用陀螺儀的零偏估計(jì)量修正陀螺儀數(shù)據(jù),同時(shí)將估計(jì)的姿態(tài)量用于加速度計(jì)的積分,從而得到機(jī)器人的位置量。通過(guò)單目視覺(jué)傳感器解算對(duì)極幾何和PnP計(jì)算位置信息。通過(guò)圖優(yōu)化算法融合IMU計(jì)算的位置殘差和單目視覺(jué)計(jì)算的殘差,得到移動(dòng)機(jī)器人的位置狀態(tài)和加速度計(jì)的零偏量。用計(jì)算得到的加速度計(jì)的零偏修正加速度計(jì)數(shù)據(jù)。至此就可以估計(jì)出移動(dòng)機(jī)器人的位置和姿態(tài)狀態(tài)。算法流程如圖1所示。

        圖1 算法流程圖

        圖1中,bw和ba分別表示通過(guò)算法估計(jì)出來(lái)的陀螺儀零偏量和加速度計(jì)零偏量。通過(guò)對(duì)IMU中各傳感器的零偏進(jìn)行修正,可以得到更加精確的位姿信息并提高系統(tǒng)的魯棒性。

        1.1 磁力計(jì)與IMU卡爾曼姿態(tài)估計(jì)器

        1.1.1 IMU傳感器特性

        IMU傳感器包括三軸陀螺儀和三軸加速度計(jì),可以測(cè)量高頻的角速度和線加速度數(shù)據(jù)。但是,IMU的測(cè)量數(shù)據(jù)中包含兩種測(cè)量誤差,即測(cè)量噪聲和零偏。測(cè)量噪聲是高斯白噪聲,零偏服從高斯隨機(jī)游走。因此IMU的測(cè)量模型如式(1):

        (1)

        1.1.2 基于卡爾曼濾波器的姿態(tài)解算算法

        建立基于非線性卡爾曼濾波器的姿態(tài)解算算法,其狀態(tài)方程和量測(cè)方程[17]如下

        Z=h(X)+V

        (2)

        式中,狀態(tài)向量X=[q0,q1,q2,q3,bwx,bwy,bwz],分別表示四元數(shù)和陀螺儀的偏差,W為系統(tǒng)噪聲,服從高斯分布;V為測(cè)量噪聲,服從高斯分布,四元數(shù)微分方程為

        對(duì)上式進(jìn)行離散化可以得到如下?tīng)顟B(tài)轉(zhuǎn)移方程

        XK+1=AKXK

        (3)

        式中

        AK=(I+TF)=I+

        XK(i)=qi,i=0,1,2,3。T為IMU傳感器數(shù)據(jù)采集時(shí)間間隔。

        對(duì)于加速度的測(cè)量數(shù)據(jù)可以通過(guò)方向余弦矩陣進(jìn)行估計(jì),其中假設(shè)重力加速度為常數(shù)|g|,可以根據(jù)式(4)得到加速度計(jì)在載體坐標(biāo)下的測(cè)量方程。

        (4)

        式中

        (5)

        通過(guò)視覺(jué)和IMU優(yōu)化模塊中式(17)可以得到優(yōu)化后加速度計(jì)的零偏估計(jì)并對(duì)加速度計(jì)進(jìn)行修正,因此修正后的測(cè)量值為

        磁力計(jì)的測(cè)量方程與加速度計(jì)測(cè)量方程相同。對(duì)磁場(chǎng)進(jìn)行歸一化,并假設(shè)其指向y軸。磁力計(jì)的測(cè)量方程見(jiàn)式(6)

        (6)

        根據(jù)式(2)~(6)所得到的狀態(tài)方程和測(cè)量方程進(jìn)行卡爾曼更新,求解后驗(yàn)狀態(tài)估計(jì)和后驗(yàn)狀態(tài)協(xié)方差矩陣見(jiàn)式(7):

        (7)

        1.2 基于視覺(jué)傳感器和IMU的非線性圖優(yōu)化

        1.2.1 IMU測(cè)量殘差

        首先對(duì)IMU的測(cè)量數(shù)據(jù)進(jìn)行積分,為IMU的殘差計(jì)算提供約束關(guān)系。在給定的兩幀連續(xù)圖像bk和bk+1之間,可以通過(guò)加速度計(jì)、陀螺儀的測(cè)量數(shù)據(jù)和式(7)中解算的姿態(tài)角和陀螺儀偏差估計(jì)值計(jì)算bk+1幀時(shí)刻的位置,速度和姿態(tài)角信息,見(jiàn)式(8)[16]:

        (8)

        (9)

        式中

        (10)

        對(duì)于式(10)采用歐拉法進(jìn)行離散處理求解積分值,同時(shí)進(jìn)行泰勒一階近似。就可以計(jì)算得到bk和bk+1之間的位移、速度和姿態(tài)角的增量,如式(11)所示:

        (11)

        對(duì)于攝像機(jī)視頻窗口內(nèi)兩個(gè)連續(xù)幀圖像bk和bk+1之間的IMU測(cè)量數(shù)據(jù),根據(jù)式(9)~式(11),IMU積分過(guò)程的測(cè)量殘差為[16]

        (12)

        1.2.2 視覺(jué)測(cè)量殘差

        (13)

        式中,s為待求的尺度因子,表示與實(shí)際移動(dòng)距離尺寸相關(guān)的縮放比例。

        定義單目視覺(jué)在圖像平面上的重投影誤差,通過(guò)將第i幀圖像中觀測(cè)到的特征點(diǎn)n重投影到第j幀圖像中,則視覺(jué)測(cè)量殘差為

        (14)

        (15)

        1.2.3 圖優(yōu)化狀態(tài)方程

        定義需要優(yōu)化的狀態(tài)向量如下

        (16)

        由式(12)和(14)定義代價(jià)函數(shù)為

        (17)

        式中,B為所有IMU測(cè)量數(shù)據(jù)的集合,C為解算關(guān)鍵幀內(nèi)所有圖像特征點(diǎn)的集合。通過(guò)Ceres Solver優(yōu)化器,對(duì)式(17)求解最小二乘問(wèn)題,就可以得到位置估計(jì)量、陀螺儀和加速度計(jì)零偏。至此,完成了由視覺(jué)和IMU傳感器求解位置的過(guò)程。

        2 實(shí)驗(yàn)結(jié)果

        2.1 實(shí)驗(yàn)平臺(tái)

        實(shí)驗(yàn)平臺(tái)采用optor慣性相機(jī)、Pixhawk飛控硬件和深圳玩智商科技有限公司生產(chǎn)的Dashgo D1移動(dòng)機(jī)器人平臺(tái)。optor慣性相機(jī)帶有一個(gè)MPU-6050模塊,集成了三軸陀螺儀和三軸加速度計(jì),還帶有兩個(gè)30fps、分辨率為752×480的攝像頭。用Pixhawk飛控中的HMC5883L三軸磁力計(jì)采集地磁場(chǎng)數(shù)據(jù)。Dashgo D1移動(dòng)機(jī)器人的移動(dòng)精度低于1%,作為移動(dòng)平臺(tái)驗(yàn)證算法的移動(dòng)精度和旋轉(zhuǎn)精度。所有傳感器都通過(guò)USB連接到NUC機(jī)載電腦上,進(jìn)行數(shù)據(jù)采集,并在機(jī)載電腦上運(yùn)行算法進(jìn)行位姿估計(jì),如圖1所示。

        圖2 實(shí)驗(yàn)平臺(tái)

        2.2 實(shí)驗(yàn)結(jié)果

        通過(guò)Dashgo D1移動(dòng)機(jī)器人搭載實(shí)驗(yàn)設(shè)備在室內(nèi)分別沿著XY軸直線運(yùn)動(dòng)15m和繞z軸旋轉(zhuǎn)360°,分別運(yùn)行本文算法和VINS-SLAM算法,進(jìn)行實(shí)驗(yàn)并記錄實(shí)驗(yàn)數(shù)據(jù)。將本文算法解算結(jié)果和當(dāng)前解算精度高的VINS-SLAM算法解算結(jié)果進(jìn)行對(duì)比。

        首先進(jìn)行沿XY軸直線運(yùn)動(dòng)的實(shí)驗(yàn),使移動(dòng)機(jī)器人以0.05m/s的速度分別沿著XY軸直線勻速移動(dòng)15m。記錄并對(duì)比本文算法和VINS-SLAM算法解算結(jié)果的誤差。實(shí)驗(yàn)結(jié)果如圖3~圖6所示,橫軸表示時(shí)間,縱軸表示移動(dòng)距離。

        圖3 X軸實(shí)驗(yàn)結(jié)果

        圖4 X軸實(shí)驗(yàn)誤差

        圖5 Y軸實(shí)驗(yàn)結(jié)果

        圖6 Y軸實(shí)驗(yàn)誤差

        將Dashgo D1移動(dòng)機(jī)器人繞z軸按2°/s勻速旋轉(zhuǎn)360°進(jìn)行Yaw姿態(tài)角解算實(shí)驗(yàn)。記錄并對(duì)比本文算法和VINS-SLAM算法解算結(jié)果的誤差,如圖7、圖8所示。

        圖7 Yaw角實(shí)驗(yàn)結(jié)果

        圖8 Yaw角實(shí)驗(yàn)誤差

        從圖(8)可以看出,由于VINS-SLAM算法只用IMU和單目視覺(jué)進(jìn)行位姿解算,在純旋轉(zhuǎn)的情況下,當(dāng)旋轉(zhuǎn)180°后由于此時(shí)視覺(jué)傳感器無(wú)法解算對(duì)極幾何而解算失效,此時(shí)只依賴陀螺儀積分進(jìn)行姿態(tài)解算,就會(huì)出現(xiàn)姿態(tài)解算漂移。而本文算法主要依靠磁力計(jì)和IMU進(jìn)行姿態(tài)解算,有效避免了單目視覺(jué)SLAM算法中存在的姿態(tài)解算漂移的問(wèn)題。

        表1 實(shí)驗(yàn)誤差結(jié)果

        通過(guò)表1對(duì)比本文算法與不使用磁力計(jì)的VINS-SLAM算法可以看出,本文算法通過(guò)使用磁力計(jì)使位置解算誤差降低到0.4m以內(nèi)。而解算的姿態(tài)角克服了VINS-SLAM算法中由于純旋轉(zhuǎn)情況而產(chǎn)生的漂移問(wèn)題,并且解算精度在5°以內(nèi)。

        3 結(jié)論

        本文首次將磁力計(jì)、單目視覺(jué)和 IMU傳感器數(shù)據(jù)進(jìn)行融合,解算移動(dòng)機(jī)器人位姿狀態(tài),并通過(guò)實(shí)驗(yàn)對(duì)設(shè)計(jì)的算法進(jìn)行驗(yàn)證,與VINS-SLAM單目視覺(jué)算法進(jìn)行對(duì)比。實(shí)驗(yàn)結(jié)果表明,所設(shè)計(jì)的基于磁力計(jì)、IMU和單目視覺(jué)的自主定位算法,不但提高了移動(dòng)機(jī)器人的位姿解算精度,同時(shí)也克服了現(xiàn)有的單目視覺(jué)SLAM算法在純旋轉(zhuǎn)情況下姿態(tài)角解算漂移的問(wèn)題。本算法由于只使用磁力計(jì)、IMU和單目攝像頭,具有質(zhì)量小、成本低、消耗計(jì)算資源少等優(yōu)點(diǎn),因此可廣泛應(yīng)用于小型移動(dòng)機(jī)器人的自主導(dǎo)航系統(tǒng)中。

        猜你喜歡
        移動(dòng)機(jī)器人測(cè)量
        移動(dòng)機(jī)器人自主動(dòng)態(tài)避障方法
        移動(dòng)機(jī)器人VSLAM和VISLAM技術(shù)綜述
        把握四個(gè)“三” 測(cè)量變簡(jiǎn)單
        滑動(dòng)摩擦力的測(cè)量和計(jì)算
        滑動(dòng)摩擦力的測(cè)量與計(jì)算
        測(cè)量的樂(lè)趣
        基于Twincat的移動(dòng)機(jī)器人制孔系統(tǒng)
        測(cè)量
        室內(nèi)環(huán)境下移動(dòng)機(jī)器人三維視覺(jué)SLAM
        極坐標(biāo)系下移動(dòng)機(jī)器人的點(diǎn)鎮(zhèn)定
        欧美日韩性高爱潮视频| 亚洲国产天堂久久综合| 无码成人aaaaa毛片| 狠狠色综合播放一区二区| 精品蜜桃av一区二区三区| 19款日产奇骏车怎么样| 亚洲av永久无码精品网址| 欧美人成人亚洲专区中文字幕| 精品人妻av区乱码| 国产成人一区二区三区影院| 欧美又粗又长又爽做受| 内射后入在线观看一区| 久久亚洲精彩无码天堂| 久久日本视频在线观看| 国产动作大片中文字幕| 波多野结衣aⅴ在线| 久久婷婷免费综合色啪| 久久精品女人av一区二区| 国产三级久久久精品麻豆三级| 人妻无码中文人妻有码| 精品少妇后入一区二区三区| 媚药丝袜美女高清一二区| 鸭子tv国产在线永久播放| 精品视频入口| av在线不卡一区二区三区| 少妇性l交大片7724com| 99精品免费久久久久久久久日本| 久久精品性无码一区二区爱爱| 亚洲偷自拍国综合第一页国模| 亚洲精品一区二区三区麻豆| 亚洲人成亚洲精品| 日本一本草久国产欧美日韩| 国产白色视频在线观看| 国产精品久久久久9999赢消| 亚洲天堂资源网| 日韩人妻大奶子生活片| 精品人妻一区二区三区四区在线| 久久久久久久性潮| 成人国产在线播放自拍| 久久红精品一区二区三区| 天天做天天爱天天爽综合网|