一種低成本的物體姿態監測系統及方法與流程

2023-12-09 16:59:31 2

本發明涉及一種低成本的物體姿態監測系統及方法，屬於姿態監測技術領域。

背景技術：

姿態監測(Attitude monitor)是一種應用廣泛的技術，以為物體的運動姿態為研究對象，通過採集物體姿態數據信息以監測、分析物體的運動狀態。姿態監測是運動機械控制、狀態預警的核心技術，在工業領域有重要的地位，近年來已成為機器人、無人機、安全領域的熱點研究問題，收到了國內外眾多領域的學者和工程師的關注。

但現有姿態監測技術中姿態測量魯棒性不高、監控系統複雜、成本高。

技術實現要素：

本發明技術解決問題：克服了現有技術中的不足，提供一種低成本的物體姿態監測系統及方法，具有姿態測量魯棒性高、監控簡單、成本低的特點。

本發明技術解決方案：一種低成本的物體姿態監測系統，包括：姿態測量裝置和上位機；姿態測量裝置由測量模塊、輸出模塊和存儲模塊組成；測量模塊包括主測量系統和輔助測量系統，主測量系統由三軸陀螺儀構成，輔助測量系統由三軸加速度計和電子羅盤構成；對三軸陀螺儀、三軸加速度計的和電子羅盤的初始誤差進行校準及濾波，其中保證三軸陀螺儀、三軸加速度計和電子羅盤測量精度，使輸出的測量值具有良好魯棒性，得到準確角速度和加速度；再將主測量系統和輔助測量系統的測量值經過四元數法姿態解算和卡爾曼濾波的信息融合，使輔助測量系統實時校準主測量系統，獲得了較好的測量結果，從而得到準確的姿態角；輸出模塊實現姿態角、角速度、加速度的測量數據無線傳輸；存儲模塊將測量數據以文本方式存儲，測量數據包括運行時間、角速度、加速度和姿態角；上位機無線接收姿態測量裝置輸出模塊的測量數據，實時渲染輸出物體3D模擬運動和測量數據，通過設置自定義閾值實現物體危險狀態判斷，完成預警監控。

所述三軸陀螺儀和三軸加速度計初始誤差的校準採用線性偏差模型補償，並通過最小二乘法進行參數辨識的校準算法，具體如下：採用轉臺實驗進行校準，三軸加速度計的X軸在轉臺上分別沿X、Y、Z正負半軸放置，可得到三軸加速度計的6組理論值與測量值，即得到6組獨立方程，通過最小二乘法進行參數辨識得到零偏誤差和非正交誤差矩陣各參數的值，已知測量值，求解矩陣方程即可得準確測量值。

所述電子羅盤測量誤差的校準採用基於磁場補償模型的校準算法，具體如下：電子羅盤與三軸加速度計、三軸陀螺儀正交安裝，分別繞三軸旋轉電子羅盤，最大與最小值的均值即為線性偏移誤差，電子羅盤測量值減去所述線性偏移誤差即為準確測量值。

所述三軸陀螺儀和三軸加速度計的測量值濾波時採用滑動均值濾波器，特別是五階滑動均值濾波器，使三軸陀螺儀和三軸加速度計的輸出具有了抗幹擾和振動的魯棒性。

所述電子羅盤測量值濾波採用中值濾波器，特別是三階中值濾波器，能夠有效去除了突變值。

所述信息融合採用卡爾曼濾波，使用輔助測量系統實時校準主測量系統，獲得了較好的測量結果。

所述上位機基於Processing設計，具有數據輸出、信息校核、3D姿態可視化顯示、自定義預警閾值、危險狀態記錄及狀態3D模擬功能，包括示波器模塊、預警模塊、數據記錄與3D模擬仿真模塊。

所述姿態測量裝置的嵌入式開發載體是開源硬體Arduino平臺。Arduino使用低價微處理器(ATmega8或ATmega128)，集成了類似於Java、C語言的開發環境。本發明上位機使用基於Java的開源平臺Processing庫進行開發，使用了OpenGL渲染技術，代碼簡單，開發周期短，成本較低。

一種低成本的物體姿態監測方法，包括以下步驟：

(1)啟動姿態測量系統，設定危險閾值，閾值的選取範圍為姿態角、角速度、加速度；

(2)將三軸加速度計固定在轉臺上、X軸分別沿X、Y、Z正負半軸放置，採集數據均值，使用最小二乘法辨識誤差參數並補償，完成加速度計初始誤差校準；

(3)將電子羅盤繞X，Y，Z軸旋轉一周，採集電子羅盤三軸測量值並通過遍歷得到最大最小值，求取均值對電子羅盤測量值進行補償，完成電子羅盤初始誤差校準；

(3)三軸加速度計、三軸陀螺儀和電子羅盤測量值經校準補償後進行濾波，三軸加速度計和三軸陀螺儀採用滑動均值濾波，特別是五階滑動均值濾波，電子羅盤測量值使用中值濾波，特別是三階中值濾波；

(4)對三軸陀螺儀測量值進行四元數法姿態解算，得到主測量系統的姿態角，對三軸加速度計和電子羅盤測量值由幾何關係得到輔助測量系統的姿態角；

(5)對主測量系統和輔助測量系統得到的姿態角使用卡爾曼濾波進行信息融合，得到最優估計的姿態角，將姿態角、角速度、加速度的測量數據無線傳輸至上位機；

(6)上位機接收測量數據，通過系統通信協議進行識別，當三軸加速度計、三軸陀螺儀、電子羅盤未連接，處於無響應時進行預警提示；

(7)上位機將識別後的測量數據解析，實時顯示測量數據和運行時間，測量數據包括角速度、加速度、姿態角，並在模擬示波器上繪製三個姿態角的波形，根據姿態角變化渲染模擬圖形旋轉實現3D仿真；

(8)上位機根據設定的閾值與接收到的測量數據判斷物體是否處於危險狀態，如處於危險狀態，則顯示文字語音警示進行預警，並自動記錄並存儲危險信息，測量結束；否則，進入步驟(4)繼續。

本發明與現有技術相比的優點在於：

(1)本發明使用轉臺實驗進行誤差校準、濾波器聯合濾波、信息融合，提高姿態測量系統的精度和適用性，通過3D仿真、自定義危險閾值、數據仿真再現等方式實現監測預警。克服了現有姿態監測技術中姿態測量魯棒性不高、監控系統複雜、成本高等特點，具有姿態測量魯棒性高、監控簡單、成本低的優點，對姿態監測、運動測量、安全防護等領域的工程實踐具有一定的參考指導意義。

(2)本發明通過最小二乘法辨識校準了三軸加速計、三軸陀螺儀初始誤差，使用中值濾波器和滑動均值濾波器增強魯棒性，將主測量系統和輔助測量系統信息融合得到準確姿態角，具有姿態測量魯棒性高的優點。同時，上位機可以進行測量結果的實時仿真與輸出，通過自定義閾值實現危險預警並記錄信息。對於記錄的危險狀況信息，上位機能夠實現3D仿真再現，有助於工程分析。

(3)本發明的姿態測量裝置及上位機擴展性好，成本較低，應用範圍較廣。

附圖說明

圖1為本發明姿態檢測系統的組成框圖；

圖2為本發明姿態監測系統的工作流程圖；

圖3為給系統通電，打開作業系統時，LCD屏幕上顯示「Welcome，Please Wait」，上位機等待接收數據；

圖4為三軸加速度計、三軸陀螺儀校準完成後，LCD和上位機顯示「ACCCHECKED」，並提示進行電子羅盤校準；

圖5為電子羅盤校準完成後，LCD和上位機顯示「MAG CHECKED」，開始進入姿態測量主循環；

圖6為姿態測量裝置進入主循環正常工作時，LCD屏幕顯示實時的姿態角、工作狀態、運行時間，上位機顯示姿態角並做出20s內的波形圖，同時進行可視化3D運動仿真；

圖7為傳感器(即三軸加速度計、三軸陀螺儀、電子羅盤)未連接時，LCD屏幕顯示「CONNECTED ERROR」，上位機顯示「傳感器未連接」，提示用戶進行檢查。

圖8為系統無響應時，LCD屏幕和上位機顯示「TIMEOUT ERROR」，提示用戶進行線路檢查。

圖9為測量數據超過閾值後，LCD顯示「DANGER ERROR」，上位機將進行預警提示，顯示「XX(自定義的值，可以是姿態角、角速度、加速度超過閾值」，進行危險預警。

圖10至圖12為本發明監測測量結果與傳統姿態測量方案結果對比圖，其中：

圖10為經轉臺實驗、最小二乘法校準前後三軸陀螺儀的測量值，其中a是校準前三軸陀螺儀靜置狀態下的測量輸出值，b為校準後陀三軸螺儀靜置狀態下的測量輸出值；經校準後靜止狀態下，三軸陀螺儀的測量值回歸0軸波動，達到了校準效果；

圖11為本發明經過濾波後，三軸加速度計、三軸陀螺儀在濾波後的測量輸出值。其中a為擾動情況下使用五階滑動均值濾波器前後三軸加速度計的測量輸出值，b為擾動情況下使用五階滑動均值濾波器前後三軸陀螺儀的測量輸出值，c為使用三階中值濾波器前後電子羅盤的測量輸出值。由圖11可以看出，經濾波器濾波後，在擾動情況下三軸加速度計和三軸陀螺儀測量有一定的魯棒性，減小了測量偏差；經中值濾波後，電子羅盤輸出排除了突變值幹擾；

圖12為本發明的採用信息融合與未採用信息融合的對比圖，Pitch1、Roll1、Yaw1表示實際姿態角度值，以Pitch2、Roll2、Yaw2作為信息融合後的角度測量值，Pitch3、Roll3、Yaw3表示主測量系統測量值，Pitch4、Roll4、Yaw4表示輔助測量系統測量值；其中a為本發明的採用信息融合與未採用信息融合的俯仰角輸出值對比圖，b為滾轉角輸出值對比圖，c為偏航角輸出值對比圖。由圖12可以看出，信息融合得到的角度測量值準確性最高。

具體實施方式

如圖1所示，本發明物體姿態監測系統的具體實現分為軟體和硬體兩個方面。

(1)軟體方面

姿態測量系統的軟體準備主要包括LCD2004顯示驅動、傳感器驅動、SD存儲模塊驅動、串口通訊協議和姿態測量算法。其中，傳感器驅動含三軸陀螺儀和三軸加速度計(六軸融合式傳感器MPU6050驅動以及電子羅盤(HMC5883L)驅動，SD存儲模塊驅動為存儲卡驅動(MMC/SD)協議，通訊協議含集成電路總線(IIC)通訊協議、藍牙通信(Bluetooth)協議及串行外設接口(SPI)通訊協議，姿態測量算法含三軸加速度計、三軸陀螺儀、電子羅盤校準算法、數據濾波算法、四元數姿態解算算法及卡爾曼濾波融合算法。其中數據濾波算法包括三軸加速度計和三軸陀螺儀的五階滑動均值濾波算法和電子羅盤的三階中值濾波算法。

上位機的軟體準備主要包括Java開發環境搭建，Processing環境設置，藍牙驅動，上位機程序設計。上位機程序包括通信協議校驗程序、3D模擬仿真程序、示波器程序算法，危險預警算法程序。

(2)硬體方面

硬體測量裝置由傳感器數據採集模塊、處理器、輸出顯示模塊、數據存儲模塊組成。其中傳感器採集模塊由三軸加速度計、三軸陀螺儀(MPU6050)與電子羅盤(HMC5883L)組成，實現加速度、角速度、磁場強度的信息採集。處理器由Arduino UNO控制器組成，可使用USB接口為系統供電。輸出顯示模塊包括LCD2004顯示屏和藍牙模塊，能夠將測量得到的姿態信息實時傳輸給上位機並顯示在LCD屏幕上。數據存儲模塊由SD讀卡裝置和SD卡組成，能夠實時存儲姿態信息。

(3)操作流程

A.給硬體測量裝置供電，打開上位機和測量裝置，修改上位機程序設置，自定義設置危險閾值。若正常啟動，LCD屏幕顯示「Welcome，Please wait」，如圖3所示。否則將不顯示，LCD屏幕提示開機異常。

B.將硬體測量裝置三軸加速度計X軸指向沿依次沿轉軸X、Y、Z方向放置在轉臺上5s，此時三軸加速度計X、Y、Z軸指向如表1所示：

表1硬體測量裝置位置圖

硬體測量裝置可得到三軸加速度計的6組理論值與測量值，即得到6組獨立方程，通過最小二乘法進行參數辨識得到零偏誤差和非正交誤差矩陣各參數的值，已知測量值，求解矩陣方程即可得準確測量值。硬體測量裝置進行三軸加速度計和陀螺儀傳感器誤差校準，如果校準成功，LCD顯示「ACC Checked，Please_wait」，上位機顯示「ACC CHECKED SUCCEED」，如圖4所示。

C.將硬體測量裝置繞X、Y、Z軸旋轉，放置測量系統，系統自動分別採集繞三軸旋轉過程中500次數據，通過遍歷得到最大最小值，最大與最小值的均值即為線性偏移誤差。LCD顯示「ACC Checked，Please_wait」，上位機顯示「MAG CHECKED SUCCEED」，如圖5所示。之後將測量系統固定在待監測物體正上方平面處。

D.校準成功後，進入姿態測量程序主循環。主循環首先對三軸加速度計、三軸陀螺儀的數據，使用校準環節得到的誤差矩陣參數，求解矩陣方程即可得準確測量值。如圖10所示。主程序對電子羅盤三軸測量值分別減去校準環節得到的三軸線性偏差得到準確測量值。主程序對三軸加速度計和三軸陀螺儀進行五階滑動均值濾波，對電子羅盤進行三階中值濾波，減小了幹擾影響、增強了測量結果的魯棒性，得到較為穩定的測量值，如圖11所示。

E.對濾波校準後的三軸陀螺儀測量值使用四元數法進行姿態解算得到主測量系統測量值。由濾波校準後的三軸加速度計和電子羅盤由幾何關係得到輔助測量系統測量值。對主測量系統測量值和輔助測量系統測量值進行卡爾曼濾波融合，得到更為準確的硬體系統的最終測量值，如圖12所示。

F.測量裝置將實時輸出數據給LCD屏幕和上位機。LCD屏幕實時顯示角速度、加速度、姿態角等信息，以一定的頻率刷新。上位機接收測量數據後，根據通信協議進行解析。通信協議如表2所示：

上位機根據測量數據識別出「傳感器未連接」、「傳感器無響應狀態」等狀態並進行提示，如圖6、圖7、圖8所示。

G.上位機將測量得到的數據實時顯示並實現3D模擬仿真，同時模擬示波器模塊記錄20s內姿態角的變化並做出波形圖。若姿態參數超出設定閾值，上位機輸出「系統處於危險狀態」「XX超出閾值」進行警示，並發出警報聲，如圖9所示。

H.若姿態變化參數超出設定閾值，觸發測量裝置的SD存儲程序，將超出閾值階段的姿態信息記錄到SD卡中。

I.如果系統關機，則系統停止工作。否則，姿態測量系統進行下一次循環至D步驟。

提供以上實施例僅僅是為了描述本發明的目的，而並非要限制本發明的範圍。本發明的範圍由所附權利要求限定。不脫離本發明的精神和原理而做出的各種等同替換和修改，均應涵蓋在本發明的範圍之內。