基于慣性傳感器的機(jī)器人姿態(tài)監(jiān)測系統(tǒng)設(shè)計(jì)

1、基于慣性傳感器的機(jī)器人姿態(tài)監(jiān)測系統(tǒng)設(shè)計(jì)一、設(shè)計(jì)背景空間飛行器的慣性測量系統(tǒng)、機(jī)器人的平衡姿態(tài)檢測、機(jī)械臂伸展確定等許多方面都需要測量物體的傾斜和方向等姿態(tài)參數(shù).機(jī)器人的運(yùn)動過程中要不斷的檢測機(jī)器人的運(yùn)動狀態(tài),以實(shí)現(xiàn)對機(jī)器人的精確限制.本文研究的基于MEMS慣性傳感器姿態(tài)檢測系統(tǒng)用于檢測自平衡機(jī)器人運(yùn)動時(shí)姿態(tài),以限制機(jī)器人的平衡.隨著微機(jī)電系統(tǒng)MEMS技術(shù)的開展,采用傳感器應(yīng)用到姿態(tài)檢測系統(tǒng)上的條件變得成熟.基于MEMS技術(shù)的加速度傳感器和陀螺儀具有抗沖擊水平強(qiáng)、可靠性高、壽命長、本錢低等優(yōu)點(diǎn),是適用于構(gòu)建姿態(tài)檢測系統(tǒng)的慣性傳感器.利用MEMS陀螺儀和加速度傳感器等慣性傳感器組成的姿態(tài)檢測系統(tǒng),

2、能夠通過對重力矢量夾角和系統(tǒng)轉(zhuǎn)動角速度進(jìn)行測量,從而實(shí)時(shí)、準(zhǔn)確地檢測系統(tǒng)的偏轉(zhuǎn)角度.由于慣性傳感器隨著時(shí)間、溫度的外界變化,會產(chǎn)生不同程度的漂移.通過對陀螺儀和加速度計(jì)的采集數(shù)據(jù)進(jìn)行數(shù)據(jù)融合,測量的角度與實(shí)際的角度相吻合,取得了良好的限制效果.同時(shí)該系統(tǒng)具有獨(dú)立,易用的特點(diǎn),其應(yīng)用前景廣泛.二、根本原理在地球上任何位置的物體都受到重力的作用而產(chǎn)生一個(gè)加速度,加速度傳感器可以用來測定變化或恒定的加速度.把三軸加速度傳感器固定在物體上,在相對靜止?fàn)顟B(tài)下,當(dāng)物體姿態(tài)改變時(shí),加速度傳感器的敏感軸相對于重力場發(fā)生變化,加速度傳感器的三個(gè)敏感軸分別輸出重力在其相應(yīng)方向產(chǎn)生的分量信號.當(dāng)系統(tǒng)處于變速運(yùn)動狀態(tài)

3、時(shí),由于加速度傳感器同時(shí)受到重力加速度和系統(tǒng)自身加速度的影響,其返回值是重力加速度同系統(tǒng)自身加速度的矢量和.對加速度傳感器溫度漂移及系統(tǒng)振動和機(jī)械噪聲等方面的考慮,加速度傳感器不能獨(dú)立運(yùn)用測量系統(tǒng)的姿態(tài).陀螺儀能夠提供瞬間的動態(tài)角度變化,由于其本身的固有特性、溫度及積分過程的影響,它會隨著工作時(shí)間的延長產(chǎn)生漂移誤差.因此對于姿態(tài)檢測系統(tǒng)而言,單獨(dú)使用陀螺儀或加速度計(jì),都不能提供系統(tǒng)姿態(tài)的可靠估計(jì).為了克服這些問題,數(shù)據(jù)融合算法需使用加速度傳感器的測量值并使用陀螺儀測得的角速度數(shù)據(jù)對加速度傳感器數(shù)據(jù)進(jìn)行融合和矯正.Xa初始狀態(tài)倩加逋腹傳感器細(xì)標(biāo)系c坐標(biāo)系在AZ平面投影圖1加速度傳感器系統(tǒng)依據(jù)上一

4、時(shí)刻的重力矢量方向的估計(jì)值,結(jié)合陀螺儀測得的角度值計(jì)算出當(dāng)前時(shí)刻的重力矢量方向,再與當(dāng)前時(shí)刻加速度傳感器返回的矢量方向進(jìn)行加權(quán)平均,得到當(dāng)前矢量方向的最優(yōu)估計(jì)值.三、系統(tǒng)框架姿態(tài)平衡檢測系統(tǒng)中,限制單元采用單片機(jī)來完成限制,數(shù)據(jù)采集與處理,數(shù)據(jù)通訊等功能.根據(jù)對資料的分析,同時(shí)對性能價(jià)格比的衡量,慣性測量單元采用AnalogDevice公司的ADXRS150陀螺儀和ADXL202加速度計(jì).其根本性能指標(biāo)如下.ADXRS150其輸出電壓與偏航角速度成正比,電壓的極性那么代表轉(zhuǎn)動方向順時(shí)針轉(zhuǎn)動或逆時(shí)針轉(zhuǎn)動.其測量偏航角速度以下簡稱為角速度的范圍是由50rad/s,靈敏度為12.5mV/rad/s,

5、零位輸出電壓為2.50V,非線性誤差為由1%F.S.穩(wěn)定度為W.03rad/s,-3dB帶寬為40Hz,固有頻率為14kHz,角速-1度噪聲密度為0.05°?s?HZ.ADXL202是一款雙軸的加速度傳感器,可測量正負(fù)加速度,其最大測量范圍為改gn.靈敏度12.5%/gn,-3dB帶寬為6kHz.從技術(shù)指標(biāo)可以知道能滿足在測量角度±0.26,但是單純的使用,由于積分計(jì)算及噪聲影響會使得角度測量誤差超出允許的測量范圍,所以從硬件和濾波算法上進(jìn)行校正和數(shù)據(jù)融合,以完成機(jī)器人偏轉(zhuǎn)角度的精確測量.系統(tǒng)框架圖如圖2所示.圖2姿態(tài)檢測系統(tǒng)框架圖四、電路設(shè)計(jì)對于這個(gè)姿態(tài)檢測系統(tǒng),具檢測電

6、路分為兩個(gè)局部：陀螺儀信號采集和加速度計(jì)信號采集.陀螺儀輸出模擬信號,加速度計(jì)輸出的是脈沖信號.1陀螺儀的電路設(shè)計(jì)1.1濾波電路ADXRS150型微機(jī)械陀螺屬于芯片級微機(jī)械陀螺,陀螺儀本身容易受到高頻信號及其他外在因素的影響,導(dǎo)致其信號輸出的不穩(wěn)定.為了有效濾除陀螺儀的高頻信號,在陀螺儀的輸出上增加But-terworth低通濾波電路.經(jīng)過濾波電路再連接到單片機(jī)的A/D端,從而減少了數(shù)據(jù)處理的干擾,提升了檢測精度,濾波電路如圖3所示.5V87.fMCU6T5V:OJuV_LVT200pF13kQ6圖3陀螺儀的濾波電路1.2陀螺儀的根本電路圖4陀螺儀模塊陀螺儀的根本電路主要由ADXRS150組成

7、,為提升可移植性,將它與必要的外圍電阻,電容集成在同一模塊,可以直接應(yīng)用于其他的系統(tǒng)中,如圖4所示,實(shí)際完成后的陀螺儀模塊.2加速度計(jì)的電路設(shè)計(jì)2.1 加速度的根本電路ADXL202是一個(gè)雙軸的加速度計(jì),可以測量運(yùn)動和靜態(tài)的加速度.靜態(tài)加速度的一個(gè)特殊例子是重力加速度.當(dāng)加速度傳感器靜止時(shí)也就是側(cè)面和垂直重力垂直和加速方向沒有加速度作用,那么作用在它上面的只有重力加速度度傳感器靈敏軸之間的夾角就是傾斜角.就是說可將加速度計(jì)用作傾角計(jì).由于角度由靈敏軸和重力矢量組成的垂直平面決定,傾斜可以從各種初始的加速度傳感器位置測得.在大多數(shù)設(shè)計(jì)中,加速度傳感器的位置由水平或者垂直的PCB(印刷電路板)決定

8、.這里選擇了垂直的放置方法,如圖5所示.A炮尸3£-0.5)/12.5%0g=50%l)utyCyclerRset/l25M圖5加速度計(jì)的垂直放置及加速度的計(jì)算方法基于垂直位置的傾斜角,可以測量大于90°的傾斜角時(shí).通過加速度傳感器的x和y軸的結(jié)合起來得到在360°范圍內(nèi)都有比擬好分辨率.同時(shí)執(zhí)行了這個(gè)轉(zhuǎn)換過程后就不用對加速度傳感器進(jìn)行溫度補(bǔ)償了,由于兩個(gè)軸的輸出都是相同的變化幅度,所以靈敏度隨溫度的變化比照值的計(jì)算沒有影響.加速度計(jì)電路主要由ADXL202構(gòu)成,并輔助以一些濾涉及調(diào)節(jié)電路；通過系統(tǒng)限制電路處理ADXL202產(chǎn)生的占空比調(diào)制信號；采樣電路中濾波電容

9、選擇Cx,Cy為0.1F,濾波帶寬為50Hz,選擇的周期T2的電阻為130kQ,T2=1.04ms.利用加速度計(jì)可以實(shí)現(xiàn)傾角傳感器.ADXL202的原理圖如圖6所示：5V圖6ADXL202的測量原理圖輸出信號Xout,Yout連接到單片機(jī),對與加速度成正比的占空比的方波進(jìn)行處理,通過下面的公式得到偏轉(zhuǎn)的角度.Ax=g?sinaAy=g?sinBa+0=909=tan-1(Ax?Ay)2.2 參數(shù)標(biāo)定由于器件參數(shù)的差異,芯片的根本參數(shù)0gn,1gn也不完全相同.因此,當(dāng)要求測量精度較高時(shí)仍使用參數(shù)典型值就會引起誤差.要提升測量的精度,就需要在測量前對相關(guān)參數(shù)認(rèn)真標(biāo)定.采用“1gn標(biāo)定方法來對加速

10、度計(jì)的參數(shù)進(jìn)行標(biāo)定,標(biāo)定后的參數(shù)如下：Ax=(T1/T2-0.46327)/12.5%Ay=(T1/T2-0.52941)/12.5%以上就是機(jī)器人的姿態(tài)平衡限制系統(tǒng),其相應(yīng)的限制板如圖7所示.限制系統(tǒng)由Mi-crochip公司的PIC18F458進(jìn)行數(shù)據(jù)處理,完成數(shù)據(jù)處理后通過串口通信發(fā)送到電機(jī)限制系統(tǒng)板,以驅(qū)動電機(jī)來完成機(jī)器人的姿態(tài)平衡動作.加出性訐五、基于卡爾曼濾波的數(shù)據(jù)融合對于姿態(tài)檢測系統(tǒng)而言,單獨(dú)使用陀螺儀或者加速度計(jì),都不能提供有效的而且可靠的信息來保持機(jī)器人的平衡.陀螺儀能夠提供瞬間的動態(tài)角度變化,但是由于其本身的固有特性、溫度及積分過程的影響,它會隨著工作時(shí)間的延長產(chǎn)生漂移誤差

11、,加速度計(jì)能夠準(zhǔn)確地提供靜態(tài)的角度,但是它容易受到噪聲的干擾,使得數(shù)據(jù)變化較大,為了克服這些問題,利用卡爾曼濾波來對信號進(jìn)行數(shù)據(jù)融合.經(jīng)過卡爾曼濾波的處理,用加速度計(jì)的實(shí)現(xiàn)測量的傾斜角度來消除陀螺儀的漂移,在這個(gè)過程中,有害的噪聲也被最小化了,從而得到精確的角度估計(jì),通過卡爾曼濾波方法去跟蹤機(jī)器人的傾斜角度與陀螺儀的偏差,來完成機(jī)器人的姿態(tài)檢測工作.六、軟件設(shè)計(jì)對于姿態(tài)檢測系統(tǒng)的軟件設(shè)計(jì)包括以下幾個(gè)局部：系統(tǒng)的初始化與自校準(zhǔn),系統(tǒng)的中斷處理程序,系統(tǒng)數(shù)據(jù)處理程序和數(shù)據(jù)通訊模塊的設(shè)計(jì).程序設(shè)計(jì)采用模塊化思想,以便以后的功能擴(kuò)展.對于姿態(tài)檢測每次系統(tǒng)開啟的時(shí)候都有一個(gè)校準(zhǔn)的過程,這樣是為了能夠提升

12、限制的精度.而在數(shù)據(jù)處理程序模塊中,包括了數(shù)據(jù)的分類,濾波處理和自校準(zhǔn).計(jì)算得到的角度值通過數(shù)據(jù)通訊模塊發(fā)送到相關(guān)的系統(tǒng)中.同時(shí)也可以接受命令來對姿態(tài)檢測系統(tǒng)進(jìn)行限制.七、實(shí)驗(yàn)結(jié)果通過卡爾曼濾波的方法,對采集的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合,使采集的數(shù)據(jù)更接近真實(shí)的值.圖8所示是對采集到的數(shù)據(jù),應(yīng)用卡爾曼濾波前后得到的仿真曲線.濾泡前782101-<lmo0123_-F尊73923456時(shí)間(t/s)In口用安Lltf-15Q時(shí)間CtZ>>¥吊物幅7W2345G時(shí)間(t/s)濾破后圖8濾波效果及實(shí)時(shí)數(shù)據(jù)從圖8可以看到,在沒有使用卡爾曼濾波的情況下,角度的所受到的干擾噪聲較大,誤差在1.5.左右；使用了卡爾曼濾波后,角度的誤差限制在0.25.以內(nèi)通過卡爾曼濾波之后的慣性傳感器設(shè)計(jì)姿態(tài)檢測系統(tǒng),誤差范圍在允許的范圍內(nèi)±0.25,完全能夠滿足檢測機(jī)器人的傾斜角度的限制要求.機(jī)器人的靜態(tài)階段中輸出的由加速度信號處理所得的靜態(tài)歐拉姿態(tài)角僅在1.范圍內(nèi)波動,有效提供了機(jī)器人在靜態(tài)時(shí)的姿態(tài)數(shù)據(jù),在動態(tài)階段中,由角速度信號得出的動態(tài)歐拉姿態(tài)角大體在15.以內(nèi)波動,橫滾角因機(jī)器人行進(jìn)中的橫向擺動較大導(dǎo)致其輸出較大,在20.以內(nèi)波動,實(shí)驗(yàn)說明該傳感器模塊較為準(zhǔn)確地獲得了機(jī)器人自身的運(yùn)動姿