本發(fā)明涉及一種低成本的物體姿態(tài)監(jiān)測(cè)系統(tǒng)及方法,屬于姿態(tài)監(jiān)測(cè)技術(shù)領(lǐng)域。
背景技術(shù):
姿態(tài)監(jiān)測(cè)(Attitude monitor)是一種應(yīng)用廣泛的技術(shù),以為物體的運(yùn)動(dòng)姿態(tài)為研究對(duì)象,通過采集物體姿態(tài)數(shù)據(jù)信息以監(jiān)測(cè)、分析物體的運(yùn)動(dòng)狀態(tài)。姿態(tài)監(jiān)測(cè)是運(yùn)動(dòng)機(jī)械控制、狀態(tài)預(yù)警的核心技術(shù),在工業(yè)領(lǐng)域有重要的地位,近年來已成為機(jī)器人、無人機(jī)、安全領(lǐng)域的熱點(diǎn)研究問題,收到了國內(nèi)外眾多領(lǐng)域的學(xué)者和工程師的關(guān)注。
但現(xiàn)有姿態(tài)監(jiān)測(cè)技術(shù)中姿態(tài)測(cè)量魯棒性不高、監(jiān)控系統(tǒng)復(fù)雜、成本高。
技術(shù)實(shí)現(xiàn)要素:
本發(fā)明技術(shù)解決問題:克服了現(xiàn)有技術(shù)中的不足,提供一種低成本的物體姿態(tài)監(jiān)測(cè)系統(tǒng)及方法,具有姿態(tài)測(cè)量魯棒性高、監(jiān)控簡(jiǎn)單、成本低的特點(diǎn)。
本發(fā)明技術(shù)解決方案:一種低成本的物體姿態(tài)監(jiān)測(cè)系統(tǒng),包括:姿態(tài)測(cè)量裝置和上位機(jī);姿態(tài)測(cè)量裝置由測(cè)量模塊、輸出模塊和存儲(chǔ)模塊組成;測(cè)量模塊包括主測(cè)量系統(tǒng)和輔助測(cè)量系統(tǒng),主測(cè)量系統(tǒng)由三軸陀螺儀構(gòu)成,輔助測(cè)量系統(tǒng)由三軸加速度計(jì)和電子羅盤構(gòu)成;對(duì)三軸陀螺儀、三軸加速度計(jì)的和電子羅盤的初始誤差進(jìn)行校準(zhǔn)及濾波,其中保證三軸陀螺儀、三軸加速度計(jì)和電子羅盤測(cè)量精度,使輸出的測(cè)量值具有良好魯棒性,得到準(zhǔn)確角速度和加速度;再將主測(cè)量系統(tǒng)和輔助測(cè)量系統(tǒng)的測(cè)量值經(jīng)過四元數(shù)法姿態(tài)解算和卡爾曼濾波的信息融合,使輔助測(cè)量系統(tǒng)實(shí)時(shí)校準(zhǔn)主測(cè)量系統(tǒng),獲得了較好的測(cè)量結(jié)果,從而得到準(zhǔn)確的姿態(tài)角;輸出模塊實(shí)現(xiàn)姿態(tài)角、角速度、加速度的測(cè)量數(shù)據(jù)無線傳輸;存儲(chǔ)模塊將測(cè)量數(shù)據(jù)以文本方式存儲(chǔ),測(cè)量數(shù)據(jù)包括運(yùn)行時(shí)間、角速度、加速度和姿態(tài)角;上位機(jī)無線接收姿態(tài)測(cè)量裝置輸出模塊的測(cè)量數(shù)據(jù),實(shí)時(shí)渲染輸出物體3D模擬運(yùn)動(dòng)和測(cè)量數(shù)據(jù),通過設(shè)置自定義閾值實(shí)現(xiàn)物體危險(xiǎn)狀態(tài)判斷,完成預(yù)警監(jiān)控。
所述三軸陀螺儀和三軸加速度計(jì)初始誤差的校準(zhǔn)采用線性偏差模型補(bǔ)償,并通過最小二乘法進(jìn)行參數(shù)辨識(shí)的校準(zhǔn)算法,具體如下:采用轉(zhuǎn)臺(tái)實(shí)驗(yàn)進(jìn)行校準(zhǔn),三軸加速度計(jì)的X軸在轉(zhuǎn)臺(tái)上分別沿X、Y、Z正負(fù)半軸放置,可得到三軸加速度計(jì)的6組理論值與測(cè)量值,即得到6組獨(dú)立方程,通過最小二乘法進(jìn)行參數(shù)辨識(shí)得到零偏誤差和非正交誤差矩陣各參數(shù)的值,已知測(cè)量值,求解矩陣方程即可得準(zhǔn)確測(cè)量值。
所述電子羅盤測(cè)量誤差的校準(zhǔn)采用基于磁場(chǎng)補(bǔ)償模型的校準(zhǔn)算法,具體如下:電子羅盤與三軸加速度計(jì)、三軸陀螺儀正交安裝,分別繞三軸旋轉(zhuǎn)電子羅盤,最大與最小值的均值即為線性偏移誤差,電子羅盤測(cè)量值減去所述線性偏移誤差即為準(zhǔn)確測(cè)量值。
所述三軸陀螺儀和三軸加速度計(jì)的測(cè)量值濾波時(shí)采用滑動(dòng)均值濾波器,特別是五階滑動(dòng)均值濾波器,使三軸陀螺儀和三軸加速度計(jì)的輸出具有了抗干擾和振動(dòng)的魯棒性。
所述電子羅盤測(cè)量值濾波采用中值濾波器,特別是三階中值濾波器,能夠有效去除了突變值。
所述信息融合采用卡爾曼濾波,使用輔助測(cè)量系統(tǒng)實(shí)時(shí)校準(zhǔn)主測(cè)量系統(tǒng),獲得了較好的測(cè)量結(jié)果。
所述上位機(jī)基于Processing設(shè)計(jì),具有數(shù)據(jù)輸出、信息校核、3D姿態(tài)可視化顯示、自定義預(yù)警閾值、危險(xiǎn)狀態(tài)記錄及狀態(tài)3D模擬功能,包括示波器模塊、預(yù)警模塊、數(shù)據(jù)記錄與3D模擬仿真模塊。
所述姿態(tài)測(cè)量裝置的嵌入式開發(fā)載體是開源硬件Arduino平臺(tái)。Arduino使用低價(jià)微處理器(ATmega8或ATmega128),集成了類似于Java、C語言的開發(fā)環(huán)境。本發(fā)明上位機(jī)使用基于Java的開源平臺(tái)Processing庫進(jìn)行開發(fā),使用了OpenGL渲染技術(shù),代碼簡(jiǎn)單,開發(fā)周期短,成本較低。
一種低成本的物體姿態(tài)監(jiān)測(cè)方法,包括以下步驟:
(1)啟動(dòng)姿態(tài)測(cè)量系統(tǒng),設(shè)定危險(xiǎn)閾值,閾值的選取范圍為姿態(tài)角、角速度、加速度;
(2)將三軸加速度計(jì)固定在轉(zhuǎn)臺(tái)上、X軸分別沿X、Y、Z正負(fù)半軸放置,采集數(shù)據(jù)均值,使用最小二乘法辨識(shí)誤差參數(shù)并補(bǔ)償,完成加速度計(jì)初始誤差校準(zhǔn);
(3)將電子羅盤繞X,Y,Z軸旋轉(zhuǎn)一周,采集電子羅盤三軸測(cè)量值并通過遍歷得到最大最小值,求取均值對(duì)電子羅盤測(cè)量值進(jìn)行補(bǔ)償,完成電子羅盤初始誤差校準(zhǔn);
(3)三軸加速度計(jì)、三軸陀螺儀和電子羅盤測(cè)量值經(jīng)校準(zhǔn)補(bǔ)償后進(jìn)行濾波,三軸加速度計(jì)和三軸陀螺儀采用滑動(dòng)均值濾波,特別是五階滑動(dòng)均值濾波,電子羅盤測(cè)量值使用中值濾波,特別是三階中值濾波;
(4)對(duì)三軸陀螺儀測(cè)量值進(jìn)行四元數(shù)法姿態(tài)解算,得到主測(cè)量系統(tǒng)的姿態(tài)角,對(duì)三軸加速度計(jì)和電子羅盤測(cè)量值由幾何關(guān)系得到輔助測(cè)量系統(tǒng)的姿態(tài)角;
(5)對(duì)主測(cè)量系統(tǒng)和輔助測(cè)量系統(tǒng)得到的姿態(tài)角使用卡爾曼濾波進(jìn)行信息融合,得到最優(yōu)估計(jì)的姿態(tài)角,將姿態(tài)角、角速度、加速度的測(cè)量數(shù)據(jù)無線傳輸至上位機(jī);
(6)上位機(jī)接收測(cè)量數(shù)據(jù),通過系統(tǒng)通信協(xié)議進(jìn)行識(shí)別,當(dāng)三軸加速度計(jì)、三軸陀螺儀、電子羅盤未連接,處于無響應(yīng)時(shí)進(jìn)行預(yù)警提示;
(7)上位機(jī)將識(shí)別后的測(cè)量數(shù)據(jù)解析,實(shí)時(shí)顯示測(cè)量數(shù)據(jù)和運(yùn)行時(shí)間,測(cè)量數(shù)據(jù)包括角速度、加速度、姿態(tài)角,并在模擬示波器上繪制三個(gè)姿態(tài)角的波形,根據(jù)姿態(tài)角變化渲染模擬圖形旋轉(zhuǎn)實(shí)現(xiàn)3D仿真;
(8)上位機(jī)根據(jù)設(shè)定的閾值與接收到的測(cè)量數(shù)據(jù)判斷物體是否處于危險(xiǎn)狀態(tài),如處于危險(xiǎn)狀態(tài),則顯示文字語音警示進(jìn)行預(yù)警,并自動(dòng)記錄并存儲(chǔ)危險(xiǎn)信息,測(cè)量結(jié)束;否則,進(jìn)入步驟(4)繼續(xù)。
本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于:
(1)本發(fā)明使用轉(zhuǎn)臺(tái)實(shí)驗(yàn)進(jìn)行誤差校準(zhǔn)、濾波器聯(lián)合濾波、信息融合,提高姿態(tài)測(cè)量系統(tǒng)的精度和適用性,通過3D仿真、自定義危險(xiǎn)閾值、數(shù)據(jù)仿真再現(xiàn)等方式實(shí)現(xiàn)監(jiān)測(cè)預(yù)警??朔爽F(xiàn)有姿態(tài)監(jiān)測(cè)技術(shù)中姿態(tài)測(cè)量魯棒性不高、監(jiān)控系統(tǒng)復(fù)雜、成本高等特點(diǎn),具有姿態(tài)測(cè)量魯棒性高、監(jiān)控簡(jiǎn)單、成本低的優(yōu)點(diǎn),對(duì)姿態(tài)監(jiān)測(cè)、運(yùn)動(dòng)測(cè)量、安全防護(hù)等領(lǐng)域的工程實(shí)踐具有一定的參考指導(dǎo)意義。
(2)本發(fā)明通過最小二乘法辨識(shí)校準(zhǔn)了三軸加速計(jì)、三軸陀螺儀初始誤差,使用中值濾波器和滑動(dòng)均值濾波器增強(qiáng)魯棒性,將主測(cè)量系統(tǒng)和輔助測(cè)量系統(tǒng)信息融合得到準(zhǔn)確姿態(tài)角,具有姿態(tài)測(cè)量魯棒性高的優(yōu)點(diǎn)。同時(shí),上位機(jī)可以進(jìn)行測(cè)量結(jié)果的實(shí)時(shí)仿真與輸出,通過自定義閾值實(shí)現(xiàn)危險(xiǎn)預(yù)警并記錄信息。對(duì)于記錄的危險(xiǎn)狀況信息,上位機(jī)能夠?qū)崿F(xiàn)3D仿真再現(xiàn),有助于工程分析。
(3)本發(fā)明的姿態(tài)測(cè)量裝置及上位機(jī)擴(kuò)展性好,成本較低,應(yīng)用范圍較廣。
附圖說明
圖1為本發(fā)明姿態(tài)檢測(cè)系統(tǒng)的組成框圖;
圖2為本發(fā)明姿態(tài)監(jiān)測(cè)系統(tǒng)的工作流程圖;
圖3為給系統(tǒng)通電,打開操作系統(tǒng)時(shí),LCD屏幕上顯示“Welcome,Please Wait”,上位機(jī)等待接收數(shù)據(jù);
圖4為三軸加速度計(jì)、三軸陀螺儀校準(zhǔn)完成后,LCD和上位機(jī)顯示“ACCCHECKED”,并提示進(jìn)行電子羅盤校準(zhǔn);
圖5為電子羅盤校準(zhǔn)完成后,LCD和上位機(jī)顯示“MAG CHECKED”,開始進(jìn)入姿態(tài)測(cè)量主循環(huán);
圖6為姿態(tài)測(cè)量裝置進(jìn)入主循環(huán)正常工作時(shí),LCD屏幕顯示實(shí)時(shí)的姿態(tài)角、工作狀態(tài)、運(yùn)行時(shí)間,上位機(jī)顯示姿態(tài)角并做出20s內(nèi)的波形圖,同時(shí)進(jìn)行可視化3D運(yùn)動(dòng)仿真;
圖7為傳感器(即三軸加速度計(jì)、三軸陀螺儀、電子羅盤)未連接時(shí),LCD屏幕顯示“CONNECTED ERROR”,上位機(jī)顯示“傳感器未連接”,提示用戶進(jìn)行檢查。
圖8為系統(tǒng)無響應(yīng)時(shí),LCD屏幕和上位機(jī)顯示“TIMEOUT ERROR”,提示用戶進(jìn)行線路檢查。
圖9為測(cè)量數(shù)據(jù)超過閾值后,LCD顯示“DANGER ERROR”,上位機(jī)將進(jìn)行預(yù)警提示,顯示“XX(自定義的值,可以是姿態(tài)角、角速度、加速度超過閾值”,進(jìn)行危險(xiǎn)預(yù)警。
圖10至圖12為本發(fā)明監(jiān)測(cè)測(cè)量結(jié)果與傳統(tǒng)姿態(tài)測(cè)量方案結(jié)果對(duì)比圖,其中:
圖10為經(jīng)轉(zhuǎn)臺(tái)實(shí)驗(yàn)、最小二乘法校準(zhǔn)前后三軸陀螺儀的測(cè)量值,其中a是校準(zhǔn)前三軸陀螺儀靜置狀態(tài)下的測(cè)量輸出值,b為校準(zhǔn)后陀三軸螺儀靜置狀態(tài)下的測(cè)量輸出值;經(jīng)校準(zhǔn)后靜止?fàn)顟B(tài)下,三軸陀螺儀的測(cè)量值回歸0軸波動(dòng),達(dá)到了校準(zhǔn)效果;
圖11為本發(fā)明經(jīng)過濾波后,三軸加速度計(jì)、三軸陀螺儀在濾波后的測(cè)量輸出值。其中a為擾動(dòng)情況下使用五階滑動(dòng)均值濾波器前后三軸加速度計(jì)的測(cè)量輸出值,b為擾動(dòng)情況下使用五階滑動(dòng)均值濾波器前后三軸陀螺儀的測(cè)量輸出值,c為使用三階中值濾波器前后電子羅盤的測(cè)量輸出值。由圖11可以看出,經(jīng)濾波器濾波后,在擾動(dòng)情況下三軸加速度計(jì)和三軸陀螺儀測(cè)量有一定的魯棒性,減小了測(cè)量偏差;經(jīng)中值濾波后,電子羅盤輸出排除了突變值干擾;
圖12為本發(fā)明的采用信息融合與未采用信息融合的對(duì)比圖,Pitch1、Roll1、Yaw1表示實(shí)際姿態(tài)角度值,以Pitch2、Roll2、Yaw2作為信息融合后的角度測(cè)量值,Pitch3、Roll3、Yaw3表示主測(cè)量系統(tǒng)測(cè)量值,Pitch4、Roll4、Yaw4表示輔助測(cè)量系統(tǒng)測(cè)量值;其中a為本發(fā)明的采用信息融合與未采用信息融合的俯仰角輸出值對(duì)比圖,b為滾轉(zhuǎn)角輸出值對(duì)比圖,c為偏航角輸出值對(duì)比圖。由圖12可以看出,信息融合得到的角度測(cè)量值準(zhǔn)確性最高。
具體實(shí)施方式
如圖1所示,本發(fā)明物體姿態(tài)監(jiān)測(cè)系統(tǒng)的具體實(shí)現(xiàn)分為軟件和硬件兩個(gè)方面。
(1)軟件方面
姿態(tài)測(cè)量系統(tǒng)的軟件準(zhǔn)備主要包括LCD2004顯示驅(qū)動(dòng)、傳感器驅(qū)動(dòng)、SD存儲(chǔ)模塊驅(qū)動(dòng)、串口通訊協(xié)議和姿態(tài)測(cè)量算法。其中,傳感器驅(qū)動(dòng)含三軸陀螺儀和三軸加速度計(jì)(六軸融合式傳感器MPU6050驅(qū)動(dòng)以及電子羅盤(HMC5883L)驅(qū)動(dòng),SD存儲(chǔ)模塊驅(qū)動(dòng)為存儲(chǔ)卡驅(qū)動(dòng)(MMC/SD)協(xié)議,通訊協(xié)議含集成電路總線(IIC)通訊協(xié)議、藍(lán)牙通信(Bluetooth)協(xié)議及串行外設(shè)接口(SPI)通訊協(xié)議,姿態(tài)測(cè)量算法含三軸加速度計(jì)、三軸陀螺儀、電子羅盤校準(zhǔn)算法、數(shù)據(jù)濾波算法、四元數(shù)姿態(tài)解算算法及卡爾曼濾波融合算法。其中數(shù)據(jù)濾波算法包括三軸加速度計(jì)和三軸陀螺儀的五階滑動(dòng)均值濾波算法和電子羅盤的三階中值濾波算法。
上位機(jī)的軟件準(zhǔn)備主要包括Java開發(fā)環(huán)境搭建,Processing環(huán)境設(shè)置,藍(lán)牙驅(qū)動(dòng),上位機(jī)程序設(shè)計(jì)。上位機(jī)程序包括通信協(xié)議校驗(yàn)程序、3D模擬仿真程序、示波器程序算法,危險(xiǎn)預(yù)警算法程序。
(2)硬件方面
硬件測(cè)量裝置由傳感器數(shù)據(jù)采集模塊、處理器、輸出顯示模塊、數(shù)據(jù)存儲(chǔ)模塊組成。其中傳感器采集模塊由三軸加速度計(jì)、三軸陀螺儀(MPU6050)與電子羅盤(HMC5883L)組成,實(shí)現(xiàn)加速度、角速度、磁場(chǎng)強(qiáng)度的信息采集。處理器由Arduino UNO控制器組成,可使用USB接口為系統(tǒng)供電。輸出顯示模塊包括LCD2004顯示屏和藍(lán)牙模塊,能夠?qū)y(cè)量得到的姿態(tài)信息實(shí)時(shí)傳輸給上位機(jī)并顯示在LCD屏幕上。數(shù)據(jù)存儲(chǔ)模塊由SD讀卡裝置和SD卡組成,能夠?qū)崟r(shí)存儲(chǔ)姿態(tài)信息。
(3)操作流程
A.給硬件測(cè)量裝置供電,打開上位機(jī)和測(cè)量裝置,修改上位機(jī)程序設(shè)置,自定義設(shè)置危險(xiǎn)閾值。若正常啟動(dòng),LCD屏幕顯示“Welcome,Please wait”,如圖3所示。否則將不顯示,LCD屏幕提示開機(jī)異常。
B.將硬件測(cè)量裝置三軸加速度計(jì)X軸指向沿依次沿轉(zhuǎn)軸X、Y、Z方向放置在轉(zhuǎn)臺(tái)上5s,此時(shí)三軸加速度計(jì)X、Y、Z軸指向如表1所示:
表1硬件測(cè)量裝置位置圖
硬件測(cè)量裝置可得到三軸加速度計(jì)的6組理論值與測(cè)量值,即得到6組獨(dú)立方程,通過最小二乘法進(jìn)行參數(shù)辨識(shí)得到零偏誤差和非正交誤差矩陣各參數(shù)的值,已知測(cè)量值,求解矩陣方程即可得準(zhǔn)確測(cè)量值。硬件測(cè)量裝置進(jìn)行三軸加速度計(jì)和陀螺儀傳感器誤差校準(zhǔn),如果校準(zhǔn)成功,LCD顯示“ACC Checked,Please_wait”,上位機(jī)顯示“ACC CHECKED SUCCEED”,如圖4所示。
C.將硬件測(cè)量裝置繞X、Y、Z軸旋轉(zhuǎn),放置測(cè)量系統(tǒng),系統(tǒng)自動(dòng)分別采集繞三軸旋轉(zhuǎn)過程中500次數(shù)據(jù),通過遍歷得到最大最小值,最大與最小值的均值即為線性偏移誤差。LCD顯示“ACC Checked,Please_wait”,上位機(jī)顯示“MAG CHECKED SUCCEED”,如圖5所示。之后將測(cè)量系統(tǒng)固定在待監(jiān)測(cè)物體正上方平面處。
D.校準(zhǔn)成功后,進(jìn)入姿態(tài)測(cè)量程序主循環(huán)。主循環(huán)首先對(duì)三軸加速度計(jì)、三軸陀螺儀的數(shù)據(jù),使用校準(zhǔn)環(huán)節(jié)得到的誤差矩陣參數(shù),求解矩陣方程即可得準(zhǔn)確測(cè)量值。如圖10所示。主程序?qū)﹄娮恿_盤三軸測(cè)量值分別減去校準(zhǔn)環(huán)節(jié)得到的三軸線性偏差得到準(zhǔn)確測(cè)量值。主程序?qū)θS加速度計(jì)和三軸陀螺儀進(jìn)行五階滑動(dòng)均值濾波,對(duì)電子羅盤進(jìn)行三階中值濾波,減小了干擾影響、增強(qiáng)了測(cè)量結(jié)果的魯棒性,得到較為穩(wěn)定的測(cè)量值,如圖11所示。
E.對(duì)濾波校準(zhǔn)后的三軸陀螺儀測(cè)量值使用四元數(shù)法進(jìn)行姿態(tài)解算得到主測(cè)量系統(tǒng)測(cè)量值。由濾波校準(zhǔn)后的三軸加速度計(jì)和電子羅盤由幾何關(guān)系得到輔助測(cè)量系統(tǒng)測(cè)量值。對(duì)主測(cè)量系統(tǒng)測(cè)量值和輔助測(cè)量系統(tǒng)測(cè)量值進(jìn)行卡爾曼濾波融合,得到更為準(zhǔn)確的硬件系統(tǒng)的最終測(cè)量值,如圖12所示。
F.測(cè)量裝置將實(shí)時(shí)輸出數(shù)據(jù)給LCD屏幕和上位機(jī)。LCD屏幕實(shí)時(shí)顯示角速度、加速度、姿態(tài)角等信息,以一定的頻率刷新。上位機(jī)接收測(cè)量數(shù)據(jù)后,根據(jù)通信協(xié)議進(jìn)行解析。通信協(xié)議如表2所示:
上位機(jī)根據(jù)測(cè)量數(shù)據(jù)識(shí)別出“傳感器未連接”、“傳感器無響應(yīng)狀態(tài)”等狀態(tài)并進(jìn)行提示,如圖6、圖7、圖8所示。
G.上位機(jī)將測(cè)量得到的數(shù)據(jù)實(shí)時(shí)顯示并實(shí)現(xiàn)3D模擬仿真,同時(shí)模擬示波器模塊記錄20s內(nèi)姿態(tài)角的變化并做出波形圖。若姿態(tài)參數(shù)超出設(shè)定閾值,上位機(jī)輸出“系統(tǒng)處于危險(xiǎn)狀態(tài)”“XX超出閾值”進(jìn)行警示,并發(fā)出警報(bào)聲,如圖9所示。
H.若姿態(tài)變化參數(shù)超出設(shè)定閾值,觸發(fā)測(cè)量裝置的SD存儲(chǔ)程序,將超出閾值階段的姿態(tài)信息記錄到SD卡中。
I.如果系統(tǒng)關(guān)機(jī),則系統(tǒng)停止工作。否則,姿態(tài)測(cè)量系統(tǒng)進(jìn)行下一次循環(huán)至D步驟。
提供以上實(shí)施例僅僅是為了描述本發(fā)明的目的,而并非要限制本發(fā)明的范圍。本發(fā)明的范圍由所附權(quán)利要求限定。不脫離本發(fā)明的精神和原理而做出的各種等同替換和修改,均應(yīng)涵蓋在本發(fā)明的范圍之內(nèi)。