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

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

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

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

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

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

6、：陀螺儀信號(hào)采集和加速度計(jì)信號(hào)采集。陀螺儀輸出模擬信號(hào)，加速度計(jì)輸出的是脈沖信號(hào)。1 陀螺儀的電路設(shè)計(jì)1.1 濾波電路ADXRS150型微機(jī)械陀螺屬于芯片級(jí)微機(jī)械陀螺，陀螺儀本身容易受到高頻信號(hào)及其他外在因素的影響，導(dǎo)致其信號(hào)輸出的不穩(wěn)定。為了有效濾除陀螺儀的高頻信號(hào)，在陀螺儀的輸出上增加But-terworth低通濾波電路。經(jīng)過濾波電路再連接到單片機(jī)的A/D端，從而減少了數(shù)據(jù)處理的干擾，提高了檢測(cè)精度，濾波電路如圖3所示。圖3 陀螺儀的濾波電路1.2 陀螺儀的基本電路陀螺儀的基本電路主要由ADXRS150組成，為提高可移植性，將它與必要的外圍電阻，電容集成在同一模塊，可以直接應(yīng)用于其他的系統(tǒng)

7、中，如圖4所示，實(shí)際完成后的陀螺儀模塊。圖4陀螺儀模塊2 加速度計(jì)的電路設(shè)計(jì)2.1 加速度的基本電路ADXL202是一個(gè)雙軸的加速度計(jì)，可以測(cè)量運(yùn)動(dòng)和靜態(tài)的加速度。靜態(tài)加速度的一個(gè)特殊例子是重力加速度。當(dāng)加速度傳感器靜止時(shí)(也就是側(cè)面和垂直方向沒有加速度作用)，那么作用在它上面的只有重力加速度。重力(垂直)和加速度傳感器靈敏軸之間的夾角就是傾斜角。就是說可將加速度計(jì)用作傾角計(jì)。因?yàn)榻嵌扔伸`敏軸和重力矢量組成的垂直平面決定，傾斜可以從各種初始的加速度傳感器位置測(cè)得。在大多數(shù)設(shè)計(jì)中，加速度傳感器的位置由水平或者垂直的PCB(印刷電路板)決定。這里選擇了垂直的放置方法，如圖5所示。圖5 加速度計(jì)的垂

8、直放置及加速度的計(jì)算方法基于垂直位置的傾斜角，可以測(cè)量大于90的傾斜角時(shí)。通過加速度傳感器的x和y軸的結(jié)合起來得到在360范圍內(nèi)都有比較好分辨率。同時(shí)執(zhí)行了這個(gè)轉(zhuǎn)換過程后就不用對(duì)加速度傳感器進(jìn)行溫度補(bǔ)償了，因?yàn)閮蓚€(gè)軸的輸出都是相同的變化幅度，所以靈敏度隨溫度的變化對(duì)比值的計(jì)算沒有影響。加速度計(jì)電路主要由ADXL202構(gòu)成，并輔助以一些濾波及調(diào)節(jié)電路；通過系統(tǒng)控制電路處理ADXL202產(chǎn)生的占空比調(diào)制信號(hào)；采樣電路中濾波電容選擇Cx，Cy為0.1F，濾波帶寬為50 Hz，選擇的周期T2的電阻為130k，T2=1.04 ms。利用加速度計(jì)可以實(shí)現(xiàn)傾角傳感器。ADXL202的原理圖如圖6所示：圖6

9、ADXL202的測(cè)量原理圖輸出信號(hào)Xout，Yout連接到單片機(jī)，對(duì)與加速度成正比的占空比的方波進(jìn)行處理，通過下面的公式得到偏轉(zhuǎn)的角度。Ax=g*sin Ay=g*sin +=90 =tan-1(AxAy)2.2 參數(shù)標(biāo)定由于器件參數(shù)的差異，芯片的基本參數(shù)(0gn,1gn)也不完全相同。因此，當(dāng)要求測(cè)量精度較高時(shí)仍使用參數(shù)典型值就會(huì)引起誤差。要提高測(cè)量的精度，就需要在測(cè)量前對(duì)相關(guān)參數(shù)認(rèn)真標(biāo)定。采用“1gn標(biāo)定方法”來對(duì)加速度計(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)，其

10、相應(yīng)的控制板如圖7所示.控制系統(tǒng)由Mi-crochip公司的PIC18F458進(jìn)行數(shù)據(jù)處理，完成數(shù)據(jù)處理后通過串口通信發(fā)送到電機(jī)控制系統(tǒng)板，以驅(qū)動(dòng)電機(jī)來完成機(jī)器人的姿態(tài)平衡動(dòng)作。圖7 姿態(tài)檢測(cè)系統(tǒng)電路板五、 基于卡爾曼濾波的數(shù)據(jù)融合對(duì)于姿態(tài)檢測(cè)系統(tǒng)而言，單獨(dú)使用陀螺儀或者加速度計(jì)，都不能提供有效的而且可靠的信息來保持機(jī)器人的平衡。陀螺儀能夠提供瞬間的動(dòng)態(tài)角度變化，但是由于其本身的固有特性、溫度及積分過程的影響，它會(huì)隨著工作時(shí)間的延長(zhǎng)產(chǎn)生漂移誤差，加速度計(jì)能夠準(zhǔn)確地提供靜態(tài)的角度，但是它容易受到噪聲的干擾，使得數(shù)據(jù)變化較大，為了克服這些問題，利用卡爾曼濾波來對(duì)信號(hào)進(jìn)行數(shù)據(jù)融合。 經(jīng)過卡爾曼濾波的

11、處理，用加速度計(jì)的實(shí)現(xiàn)測(cè)量的傾斜角度來消除陀螺儀的漂移,在這個(gè)過程中，有害的噪聲也被最小化了，從而得到精確的角度估計(jì)，通過卡爾曼濾波方法去跟蹤機(jī)器人的傾斜角度與陀螺儀的偏差，來完成機(jī)器人的姿態(tài)檢測(cè)工作。六、 軟件設(shè)計(jì)對(duì)于姿態(tài)檢測(cè)系統(tǒng)的軟件設(shè)計(jì)包括以下幾個(gè)部分：系統(tǒng)的初始化與自校準(zhǔn)，系統(tǒng)的中斷處理程序，系統(tǒng)數(shù)據(jù)處理程序和數(shù)據(jù)通訊模塊的設(shè)計(jì)。程序設(shè)計(jì)采用模塊化思想，以便以后的功能擴(kuò)展。對(duì)于姿態(tài)檢測(cè)每次系統(tǒng)開啟的時(shí)候都有一個(gè)校準(zhǔn)的過程，這樣是為了能夠提高控制的精度。而在數(shù)據(jù)處理程序模塊中，包括了數(shù)據(jù)的分類，濾波處理和自校準(zhǔn)。計(jì)算得到的角度值通過數(shù)據(jù)通訊模塊發(fā)送到相關(guān)的系統(tǒng)中。同時(shí)也可以接受命令來對(duì)姿

12、態(tài)檢測(cè)系統(tǒng)進(jìn)行控制。七、實(shí)驗(yàn)結(jié)果通過卡爾曼濾波的方法，對(duì)采集的數(shù)據(jù)進(jìn)行數(shù)據(jù)融合，使采集的數(shù)據(jù)更接近真實(shí)的值。圖8所示是對(duì)采集到的數(shù)據(jù)，應(yīng)用卡爾曼濾波前后得到的仿真曲線。圖8 濾波效果及實(shí)時(shí)數(shù)據(jù)從圖8可以看到,在沒有使用卡爾曼濾波的情況下，角度的所受到的干擾噪聲較大，誤差在1.5左右；使用了卡爾曼濾波后，角度的誤差控制在0.25以內(nèi)。通過卡爾曼濾波之后的慣性傳感器設(shè)計(jì)姿態(tài)檢測(cè)系統(tǒng)，誤差范圍在允許的范圍內(nèi)0.25，完全能夠滿足檢測(cè)機(jī)器人的傾斜角度的控制要求。機(jī)器人的靜態(tài)階段中輸出的由加速度信號(hào)處理所得的靜態(tài)歐拉姿態(tài)角僅在 1范圍內(nèi)波動(dòng)，有效提供了機(jī)器人在靜態(tài)時(shí)的姿態(tài)數(shù)據(jù)，在動(dòng)態(tài)階段中，由角速度信號(hào)得出的動(dòng)態(tài)歐拉姿態(tài)角大體在 15以內(nèi)波動(dòng)，橫滾角因機(jī)器人行進(jìn)中的橫向擺動(dòng)較大導(dǎo)致其輸出較大，在 20以內(nèi)波動(dòng)，實(shí)驗(yàn)表明該傳感器模塊較為準(zhǔn)確地獲得了機(jī)器人自身的運(yùn)動(dòng)姿態(tài)。八、結(jié) 論本文