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

        ?

        蛇形機(jī)器人互補(bǔ)濾波和四元數(shù)的姿態(tài)解算*

        2014-07-07 09:14:43王超杰時(shí)佳斌
        傳感器與微系統(tǒng) 2014年10期
        關(guān)鍵詞:磁強(qiáng)計(jì)蛇形陀螺儀

        王超杰,蘇 中,時(shí)佳斌,陳 庚

        (北京信息科技大學(xué) 高動態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室,北京100101)

        蛇形機(jī)器人互補(bǔ)濾波和四元數(shù)的姿態(tài)解算*

        王超杰,蘇 中,時(shí)佳斌,陳 庚

        (北京信息科技大學(xué) 高動態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室,北京100101)

        針對蛇形機(jī)器人姿態(tài)解算問題,陀螺存在漂移特性,加速度計(jì)的測量值包含重力加速度和運(yùn)動加速度,磁強(qiáng)計(jì)易受周圍環(huán)境地磁干擾,并且蛇形機(jī)器人采用嵌入式微處理器,需要減少計(jì)算量。設(shè)計(jì)了用互補(bǔ)濾波器來實(shí)現(xiàn)慣性傳感器的數(shù)據(jù)融合,用四元數(shù)進(jìn)行姿態(tài)解算的方法。經(jīng)過實(shí)驗(yàn)驗(yàn)證表明:采用互補(bǔ)濾波和四元數(shù)進(jìn)行姿態(tài)解算能有效融合各個(gè)慣性傳感器的數(shù)據(jù),計(jì)算量小,能夠滿足蛇形機(jī)器人對精度和實(shí)時(shí)性的要求。

        蛇形機(jī)器人;姿態(tài)解算;互補(bǔ)濾波器;慣性傳感器;四元數(shù)

        0 引 言

        蛇形機(jī)器人由于具有廣闊的應(yīng)用前景,使其成為了當(dāng)前的研究熱點(diǎn)。其中,姿態(tài)檢測是控制機(jī)器人在未知環(huán)境空間運(yùn)動的關(guān)鍵問題之一,通過融合各種傳感器的數(shù)據(jù)來獲取機(jī)器人的運(yùn)動姿態(tài),進(jìn)而實(shí)現(xiàn)對機(jī)器人的運(yùn)動控制與路徑規(guī)劃。姿態(tài)解算的精度和速度將直接影響對蛇形機(jī)器人的控制的穩(wěn)定性和可靠性。隨著MEMS技術(shù)的發(fā)展與對成本因素的考慮,低成本的捷聯(lián)慣性測量單元越來越多的被應(yīng)用到移動機(jī)器人中,其主要包括陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)[1]。MEMS陀螺儀具有漂移特性,加速度計(jì)的測量值中同時(shí)包含了重力加速度和運(yùn)動加速度,同時(shí)磁強(qiáng)計(jì)容易受到周圍環(huán)境中外部磁場的干擾。怎樣有效地融合這些慣性傳感器的數(shù)據(jù),濾除外部干擾,得到高精度的姿態(tài)數(shù)據(jù),是一項(xiàng)非常具有挑戰(zhàn)性的工作。同時(shí),蛇形機(jī)器人采用STM32F103微處理器,不適用于計(jì)算量大的姿態(tài)解算算法。

        本文設(shè)計(jì)了用互補(bǔ)濾波器實(shí)現(xiàn)慣性傳感器的數(shù)據(jù)融合、用四元數(shù)法進(jìn)行姿態(tài)解算的方法能夠滿足蛇形機(jī)器人對精度和實(shí)時(shí)性的要求。

        1 蛇形機(jī)器人姿態(tài)描述

        為了描述蛇形機(jī)器人的俯仰、偏航、橫滾的姿態(tài)信息,需引入空間三維坐標(biāo)系。現(xiàn)選擇北東地作為導(dǎo)航坐標(biāo)系(n系),蛇形機(jī)器人的縱軸、橫軸和豎軸構(gòu)成的右手坐標(biāo)系作為載體坐標(biāo)系(b系)。

        (1)

        其中,φ為偏航角(yaw),θ為俯仰角(pitch),γ為橫滾角(roll),蛇形機(jī)器人運(yùn)動過程中以這3個(gè)角作為姿態(tài)角。

        2 姿態(tài)解算算法原理

        姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在求姿態(tài)時(shí)存在奇點(diǎn),不能用于全姿態(tài)的解算;方向余弦法可用于全姿態(tài)解算但計(jì)算量大,不能滿足實(shí)時(shí)性的要求。本文采用四元數(shù)法[3],其計(jì)算量小、無奇點(diǎn)且可以滿足蛇形機(jī)器人運(yùn)動過程中姿態(tài)的實(shí)時(shí)解算。

        1)初始對準(zhǔn)

        姿態(tài)計(jì)算初始,將已知載體初始姿態(tài)角φ0,θ0,γ0代入下式,求出初始時(shí)刻的四元數(shù),完成初始對準(zhǔn)

        (2)

        2)四元數(shù)隨時(shí)間的更新與求解

        寫成矩陣形式為

        (3)

        式中ωgx,ωgy,ωgz分別為陀螺儀在載體坐標(biāo)系下測得的蛇形機(jī)器人x,y,z軸的角速度。設(shè)置采樣周期T,每隔一段時(shí)間讀取陀螺儀的數(shù)據(jù),通過上式得到更新后的四元數(shù),進(jìn)而求得更新后的方向余弦矩陣。然而,MEMS陀螺儀存在溫度漂移,積分運(yùn)算也有一定的累積誤差,所以,必須采用一定的補(bǔ)償算法,融合加速度計(jì)和磁強(qiáng)計(jì)的數(shù)據(jù),進(jìn)行誤差補(bǔ)償以實(shí)現(xiàn)精確的姿態(tài)解算。

        考慮到蛇形機(jī)器人姿態(tài)解算的實(shí)時(shí)性和精度要求,四元數(shù)更新采用一階龍格—庫塔法,利用傳感器數(shù)據(jù)融合后得到的角速度ωx,ωy,ωz更新四元數(shù)

        (4)

        3)姿態(tài)角求解

        每個(gè)周期更新完四元數(shù),需要對四元數(shù)做歸一化處理

        (5)

        (6)

        對照公式(1)和公式(6)可得姿態(tài)角

        (7)

        3 互補(bǔ)濾波器的設(shè)計(jì)[4]

        3.1 設(shè)計(jì)原理

        設(shè)計(jì)互補(bǔ)濾波器的目的是綜合加速度計(jì)、陀螺儀、磁強(qiáng)計(jì)各自的頻率響應(yīng)特性,從頻域角度對3只傳感器的數(shù)據(jù)進(jìn)行融合,以減小姿態(tài)解算的誤差。陀螺儀通過積分得到姿態(tài)角,能在較短的時(shí)間內(nèi)提供準(zhǔn)確的姿態(tài)數(shù)據(jù),但長期精度較差;加速度計(jì)和磁強(qiáng)計(jì)的測量誤差不隨時(shí)間積累,但是短期內(nèi)精度較差,動態(tài)響應(yīng)較慢。利用其頻率上的互補(bǔ)特性,通過互補(bǔ)濾波能很好地結(jié)合陀螺儀角速度的動態(tài)性能和加速度計(jì)、磁強(qiáng)計(jì)的靜態(tài)精度。

        設(shè)置互補(bǔ)濾波器的傳遞函數(shù)[5]為

        G1(s)具有一階低通濾波特性,G2(s)具有一階高通特性,可以構(gòu)造估計(jì)量

        因此,通過互補(bǔ)濾波器能夠消除高頻噪聲和低頻誤差的積累,能夠很好地融合各傳感器的數(shù)據(jù)?;パa(bǔ)濾波器的原理框圖如圖1所示。

        圖1 互補(bǔ)濾波器原理框圖Fig 1 Principle block diagram of complementary filter

        互補(bǔ)濾波器在頻域上具有互補(bǔ)性,設(shè)計(jì)的濾波器傳遞函數(shù)滿足G1(s)+G2(s)=1,因此,角度的估計(jì)值是陀螺儀高通濾波和加速度計(jì)、磁強(qiáng)計(jì)低通濾波后的數(shù)據(jù)融合。

        3.2 基于互補(bǔ)濾波和四元數(shù)的姿態(tài)求解[6,7]

        圖2 基于互補(bǔ)濾波器和四元數(shù)的姿態(tài)解算方框圖Fig 2 Block diagram of attitude calculation based on complementary filter and quaternion

        (8)

        其中,(mx,my,mz)為磁強(qiáng)計(jì)測量的地磁場的矢量歸一化后的值,將其轉(zhuǎn)到導(dǎo)航坐標(biāo)系中得到(hx,hy,hz),由于導(dǎo)航坐標(biāo)系為北東地,在y軸方向地磁場矢量應(yīng)該為0,對(hx,hy,hz)進(jìn)行一次校準(zhǔn)得導(dǎo)航坐標(biāo)系下的地磁場矢量(bx,by,bz)。

        (9)

        (10)

        (11)

        ω=ωg+δ.

        (12)

        其中,δ為誤差e通過一個(gè)比例積分模塊后生成的對陀螺漂移的補(bǔ)償量,ωg為陀螺儀測量得到的角速度,ω為融合加速度計(jì)和磁強(qiáng)計(jì)數(shù)據(jù)校正后的角速度,將ω代入公式(4)就可得到隨時(shí)間傳遞的準(zhǔn)確的四元數(shù),進(jìn)而求得姿態(tài)矩陣和姿態(tài)角。

        4 實(shí)驗(yàn)驗(yàn)證

        蛇形機(jī)器人所用的MEMS慣性器件包括MPU6050和HMC5883,MPU6050集成3軸陀螺儀和3軸加速度計(jì),HMC5883為3軸磁強(qiáng)計(jì)。為了驗(yàn)證本文設(shè)計(jì)的姿態(tài)解算算法的性能,用日本北陽公司生產(chǎn)的高精度的LPMS—B模塊解算的角度作為參考角度,其姿態(tài)解算精度為動態(tài)情況下小于2°,靜態(tài)情況下小于0.5°。

        將蛇形機(jī)器人所用的MEMS慣性器件與LPMS—B模塊一同固定在平整的電路板上,調(diào)整其基準(zhǔn)一致,手動控制電路板模仿蛇形機(jī)器人的步態(tài)做各種動作,實(shí)驗(yàn)環(huán)境如圖3所示。通過電腦的兩路USB串口分別接收2個(gè)慣導(dǎo)模塊姿態(tài)解算的數(shù)據(jù),并Matlab繪圖如圖4~圖6所示。

        圖3 實(shí)驗(yàn)環(huán)境Fig 3 Experimental environment

        圖4 橫滾角Fig 4 Roll angle

        圖5 俯仰角Fig 5 Pitch angle

        圖6 偏航角Fig 6 Yaw angle

        通過實(shí)驗(yàn)可以看出:通過手動控制電路板模擬蛇形機(jī)器人做運(yùn)動時(shí),俯仰角最大誤差約為±2°,橫滾角最大誤差約為±1.5°,偏航角最大誤差約為±3°,數(shù)據(jù)更新率為100 Hz,能夠滿足蛇形機(jī)器人對精度和實(shí)時(shí)性的要求。

        5 結(jié) 論

        本文針對蛇形機(jī)器人姿態(tài)解算的難點(diǎn),設(shè)計(jì)了用互補(bǔ)濾波器來融合陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)的測量數(shù)據(jù),用四元數(shù)法進(jìn)行姿態(tài)解算。

        實(shí)驗(yàn)結(jié)果表明:采用互補(bǔ)濾波器進(jìn)行數(shù)據(jù)融合、四元數(shù)法進(jìn)行姿態(tài)求解的方法能有效融合陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)的數(shù)據(jù),輸出的姿態(tài)角動態(tài)誤差小,計(jì)算量小,能夠滿足蛇形機(jī)器人在運(yùn)動過程中對精度和實(shí)時(shí)性的要求。

        [1] 王玲玲,富 立,陳 三.基于低成本IMU的捷聯(lián)航姿系統(tǒng)軟件的設(shè)計(jì)與實(shí)現(xiàn)[J].中國慣性技術(shù)學(xué)報(bào),2009,17(4):379-382.

        [2] 趙 欣,王仕成,楊東方,等.一種改進(jìn)的高動態(tài)捷聯(lián)慣導(dǎo)解算算法[J].中國慣性技術(shù)學(xué)報(bào),2011,19(2):163-169.

        [3] 梁延德,程 敏,何福本,等.基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng),2011,30(11):56-61.

        [4] 丁 君,趙忠華.AHRS航姿解算中的兩種濾波方法的比較研究[J].計(jì)算機(jī)仿真,2013,30(9):60-65.

        [5] 蘭建軍,譚力弓,樸 亨,等.慣性傳感器和互補(bǔ)濾波器在姿態(tài)估計(jì)中的應(yīng)用[J].制造業(yè)自動化,2013,17:65-67.

        [6] 閻世梁,王銀玲,張 華.基于改進(jìn)互補(bǔ)濾波器的低成本微小飛行器姿態(tài)估計(jì)方法[J].計(jì)算機(jī)應(yīng)用,2013,33(7):2078-2082.

        [7] Euston M,Coote P,Mahony R,et al.A complementary filter for attitude estimation of a fixed-wing UAV[C]∥IEEE/RSJ 2008 International Conference on Intelligent Robots and Systems,Nice,F(xiàn)rance,2008:340-345.

        [8] Carminati M,Ferrari G,Sampietro M,et al.Fault detection and isolation enhancement of an aircraft attitude and heading reference system based on MEMS inertial sensors[J].Procedia Chemistry,2009,1(1):509-512.

        [9] Brás Sérgio,Rosa Paulo,Silvestre Carlos,et al. Global attitude and gyro bias estimation based on set-valued observers[J].Systems & Control Letters,2013,62(10):937-942.

        Attitude calculation of snake-like robot using complementary filtering and quaternion*

        WANG Chao-jie, SU Zhong, SHI Jia-bin, CHEN Geng

        (Beijing Key Laboratory of High Dynamic Navigation Technology,University of Beijing Information Science & Technology,Beijing 100101,China)

        Aiming at problem of attitude calculation of snake-like robot,gyro exists drift characteristics,measured value of accelerometer includes gravity and motion acceleration,magnetometer vulnerable to ambient geographic magnetic interference,and snake-like robot use embedded microcontrollers,which reduce the need of computation amount.Therefore,design inertial sensor data fusion is achieved with complementary filtering and attitude calculation is carried out with quaternion method.Experimental verification show that using complementary filtering and quaternion method can effectively fuse sensor datas,amount of calculation is small, and can meet requirements of precision and real-time for snake-like robot.

        snake-like robot; attitude calculation; complementary filter; inertial sensor; quaternion

        10.13873/J.1000—9787(2014)10—0137—04

        2014—03—12

        國家自然科學(xué)基金資助項(xiàng)目(61261160497);北京市科技計(jì)劃資助項(xiàng)目(Z121100001612007);北京市教委科技成果轉(zhuǎn)化和產(chǎn)業(yè)化項(xiàng)目(PXM2013—014224—000074)

        TP 212

        A

        1000—9787(2014)10—0137—04

        王超杰(1988-),男,山東煙臺人,碩士研究生,主要從事蛇形機(jī)器人多元信息融合的研究。

        猜你喜歡
        磁強(qiáng)計(jì)蛇形陀螺儀
        磁強(qiáng)計(jì)陣列測量一致性校正
        能鉆過柔軟沙地的蛇形機(jī)器人
        軍事文摘(2021年18期)2021-12-02 01:27:58
        基于EMD的MEMS陀螺儀隨機(jī)漂移分析方法
        基于矢量磁強(qiáng)計(jì)的磁場梯度張量儀誤差校正方法
        組合導(dǎo)航中磁強(qiáng)計(jì)干擾估計(jì)與補(bǔ)償方法
        TP347不銹鋼蛇形管制造工藝
        基于LabVIEW的微型磁通門磁強(qiáng)計(jì)測試系統(tǒng)搭建
        蛇形管高壓加熱器在高效寬負(fù)荷機(jī)組中的應(yīng)用
        我國著名陀螺儀專家——林士諤
        古希臘的符號蛇形珠寶的誘惑
        金色年華(2017年13期)2017-04-04 02:55:52
        国产成人亚洲不卡在线观看| 免费午夜爽爽爽www视频十八禁| 国产精品无码一区二区三区 | 欧美日韩亚洲国内综合网| 视频一区精品自拍| 国产亚洲精品一区二区在线播放| 亚洲av久播在线一区二区| 免费国产黄网站在线观看可以下载 | 日本aⅴ大伊香蕉精品视频| 中文字幕Aⅴ人妻一区二区苍井空 亚洲中文字幕久久精品蜜桃 | 欧美一欧美一区二三区性| 日韩丝袜人妻中文字幕| 蜜臀av毛片一区二区三区| 精品久久久久成人码免费动漫 | 国产精品亚洲综合色区韩国| 中文字幕人妻av四季| 国产精品久久久久一区二区三区 | 无码人妻精品一区二区在线视频| 精品国产亚洲一区二区在线3d| 亚洲精品国产精品系列| 亚洲中文字幕av天堂自拍| 国产suv精品一区二区883| 亚洲高潮喷水中文字幕| 一区二区三区在线乱码| 中文字幕人妻在线中字| 天堂在线www中文| 国产亚洲曝欧美不卡精品| 午夜国产精品视频在线观看| 国模丽丽啪啪一区二区| 欧美亚洲国产日韩一区二区三区| 久久亚洲精精品中文字幕早川悠里| 免费的日本一区二区三区视频| 欧美 变态 另类 人妖| 亚洲av影院一区二区三区四区| sm免费人成虐漫画网站| 国产两女互慰高潮视频在线观看| 国产成人无码一二三区视频| 久久伊人中文字幕有码久久国产| 亚洲国产高清精品在线| 国产呦系列呦交| 美女熟妇67194免费入口|