基于MEMS傳感器的集裝箱吊具位姿檢測應用

謝雨端,宋亞男,徐榮華

(廣東工業(yè)大學 自動化學院,廣東 廣州　510006)

?

基于MEMS傳感器的集裝箱吊具位姿檢測應用

謝雨端,宋亞男,徐榮華

(廣東工業(yè)大學 自動化學院,廣東 廣州510006)

摘要針對現(xiàn)有的集裝箱吊具位姿檢測中存在的裝卸效率低、成本高的問題,研究了基于MEMS傳感器的集裝箱吊具位姿檢測系統(tǒng)。系統(tǒng)以LSM303DLHC、L3GD20傳感器為數據采集模塊,以STM32F303為微控制器,以上位機界面為實時動態(tài)監(jiān)控,重點分析了基于梯度下降法的姿態(tài)融合算法。通過實驗數據對比和監(jiān)控跟蹤界面分析,文中所研究的系統(tǒng)實時性好、精度較高、界面友好,并在相關課題中得到了充分驗證,是一款值得推廣的集裝箱吊具位姿檢測系統(tǒng)。

關鍵詞MEMS;集裝箱吊具;位姿檢測;姿態(tài)解算

船舶集裝箱的發(fā)展,對港口岸邊的起重機裝卸效率、精準度的要求也隨著提升。在起重機小車平移的過程中產生的振動、形變及編碼器定位誤差等都會影響其定位的準確性,由此可導致其在裝卸箱時,吊具無法對準集裝箱,從而存在安全隱患[1]。

目前,吊具檢測系統(tǒng)主要分為兩大類:視覺傳感器檢測系統(tǒng)和基于慣性傳感器的非視覺傳感檢測系統(tǒng)?；跈C器視覺的吊具位姿檢測方法,能為吊具控制系統(tǒng)提供可靠的反饋數據[2]。MEMS慣性傳感器抗沖擊能力強、制造成本低、可靠性高,此外還具有體積小、重量輕、易集成、適合大批量生產的優(yōu)勢,是一種適于構建姿態(tài)檢測系統(tǒng)的慣性傳感器[3]。

利用MEMS 陀螺儀、加速度計和磁傳感器等慣性傳感器組成的姿態(tài)檢測系統(tǒng),能通過對重力矢量夾角和系統(tǒng)轉動角速度進行測量,從而實時、準確地檢測系統(tǒng)的偏轉角度。隨著科技的發(fā)展,基于MEMS傳感器的位姿檢測系統(tǒng)的功能會更加完善,系統(tǒng)精確性會不斷提高。

1系統(tǒng)架構

基于MEMS傳感器的集裝箱吊具位姿檢測系統(tǒng)由LSM303DLHC、L3GD20數據采集模塊,STM32F303微控制器,上位機實時動態(tài)監(jiān)控界面3個核心部分組成。微控制器實時讀取傳感器數據,并對數據進行姿態(tài)解算,同時將得到的姿態(tài)信息通過USB通訊接口傳送到上位機應用程序進行姿態(tài)實時顯示。

圖1　系統(tǒng)架構框圖

圖2　系統(tǒng)硬件結構圖

L3GD20三軸數字陀螺儀,用于檢測吊具的轉動角速度分量,LSM303DLHC模塊內集成了三軸數字加速度計和三軸磁傳感器,加速度計用于檢測重力加速度在坐標軸上的分量,磁傳感器用于檢測地球上的磁場強度分量的方向;復位按鍵用于系統(tǒng)的復位;LED指示用于模擬在吊具旋轉過程中的姿態(tài)狀況;上位機界面用于吊具姿態(tài)的實時監(jiān)控。

在基于MEMS傳感器的集裝箱吊具位姿檢測系統(tǒng)中,測量數據實時性與可靠性決定了吊具控制的準確性,姿態(tài)解算的速度和精度將直接影響姿態(tài)控制的穩(wěn)定性和實時性,而界面監(jiān)控部分更加直觀地顯示了系統(tǒng)性能。

陀螺儀測量到的各軸角速度通過累計積分可得到導航角、俯仰角和橫滾角,但陀螺儀具有溫度漂移特性,且存在長期積分誤差累計的問題,短期數據精度高,加速度計噪聲比較大,且在運動過程中,受吊具振動影響,短期內的可靠性不高。同時由于磁傳感器是一種磁阻傳感器,很容易受到外部磁場的干擾,不適合短期測量,可用來長時間姿態(tài)角的采集[4]。加速度計在地球上測量的是重力加速度,若吊具沿著豎直Z軸旋轉,加速度計傳感器無法感知吊具的運動的;磁傳感器測量的是地球上的磁場方向,若載體沿著Y軸旋轉,磁傳感器同樣無法感知吊具運動的。基于上述問題,需要通過算法的融合,采用加速度計來修正陀螺儀的垂直誤差。采用磁羅盤來修正陀螺儀的水平誤差,從而得到準確可靠的吊具三維姿態(tài)信息。

2姿態(tài)解算與修正

2.1　吊具姿態(tài)描述

在基于MEMS傳感器的集裝箱吊具位姿檢測系統(tǒng)中,定義兩個基本坐標系:地面坐標系(R系)和載體坐標系(b系)。R系指的是地球上的“東北天”坐標系,b系是吊具自身所形成的坐標系[5]。吊具姿態(tài)計算,需要在b系中完成,坐標系之間的轉換有3種描述方式:四元數、歐拉角、方向余弦矩陣[6]。

歐拉角簡單、直觀,但存在奇異問題;四元素計算簡便,不存在奇異問題,但表示旋轉困難。所以在表示旋轉姿態(tài)時采用歐拉角,在進行空間旋轉的具體計算和插值時,將歐拉角轉化為四元素。

R系與b系之間的旋轉矩陣(方向余弦矩陣)存在誤差,將加速度計的測量數據或者理論數據分別經過旋轉矩陣轉換在同一坐標系下,利用測量值與理論值的誤差來修正旋轉矩陣[7]。

2.2　加速度計修正原理

四元素是一個四元矢量,用來描述坐標旋轉,可表示為:q=[q0,q1,q2,q3]T,初始值為[1,0,0,0]T,導航角φ,俯仰角θ,橫滾角γ分別表示吊具繞Z軸、Y軸和X軸的旋轉角度[5]。陀螺儀的測量值[gx,gy,gz]T,單位是rad/s。

(1)

(2)

利用向量積,得到誤差向量[eaccx,eaccy,eaccz]T,如式(3)所示

(3)

2.3　磁傳感器修正原理

(4)

(5)

該值再和b系中的磁傳感器的輸出[mx,my,mz]T做向量積得到誤差函數如式(6)所示

(6)

故多元向量誤差函數為

(7)

對q0、q1、q2、q3求偏導,得到雅可比矩陣為

(8)

基于加速度計、磁傳感器融合的梯度計算式

(9)

3姿態(tài)融合

利用梯度下降法可求得一組基于加速度計和磁傳感器表征的姿態(tài)四元數,q結合陀螺儀的數據進行融合[9]。假設由微分方程求解的姿態(tài)四元數為q,有

q=α·q+(1-α)qg

(10)

本文討論直接由反梯度方向的迭代值就直接可以達到目標姿態(tài)q[10]

q=-λ·

(11)

在這里梯度下降算法的步長λ與吊具運動的角速度、采樣時間相關[7]。當q的收斂速度等于qg的收斂速度時取得的α為最優(yōu)值[10],即

(12)

式中,β是四元數微分方程求解的姿態(tài)算法的收斂速度,該值就是陀螺儀的測量誤差,本文取β=1.2。等式右邊λ/Vt就是梯度下降算法的收斂速度,進而有

(13)

跟蹤物體高速運動時,上式近似為

(14)

上式中,β和Δt較小,α近似為0。

將式(11)和式(14)代入式(10)得

(15)

每個采樣周期微處理器讀取陀螺儀、加速度計和磁傳感器的數據,采樣誤差和qg,q對上式進行迭代運算,即可實現(xiàn)四元數隨時間的更新。最終梯度下降法的姿態(tài)融合算法公式為

q(t)=q(t-1)+(qg-β·

(16)

姿態(tài)融合算法流程如圖3所示。

姿態(tài)融合算法修正四元素后,根據四元素到歐拉角的轉換式可計算出吊具的姿態(tài)歐拉角數據。

(17)

圖3　基于梯度下降法的姿態(tài)融合算法流程圖

圖中qg是一個qt相關的表達式,而facc和fmag也是與qt相關的表達式,圖中表達的是對qt的迭代運算。

4實驗結果分析

基于Python語言實現(xiàn)上位機界面的顯示,Python語言語法簡捷,具有豐富的模塊,可移植性高。本文在界面設計中基于VPython三維圖形庫,制作實時、交互式的三維動畫。通過VPython提供的鍵盤、鼠標響應事件,實現(xiàn)可手動設置初始吊具坐標系,鍵盤的space按鍵或鼠標實現(xiàn)3D模型初始狀態(tài)的矯正,方便直觀。微控制器通過USB接口和主機連接,基于PyUSB模塊實現(xiàn)在Windows系統(tǒng)下對USB設備的操作,USBHID設備是通過報告來給傳送數據,實時顯示吊具姿態(tài)和歐拉角,從理論上驗證了算法的可行性與跟蹤的實時性。

實時界面監(jiān)控窗口主要由3部分組成:(1)姿態(tài)3D實時顯示。(2)吊具數據四元素輸出和姿態(tài)角兩種輸出。(3)吊具的擺角輸出。在此以φ=60°,θ=60°,γ=0°,為實驗測試數據,得到數據修正前后的圖形。圖中heading表示導航角,pitch表示俯仰角,roll表示橫滾角,angular表示吊具的擺角。

圖4　修正前圖形

圖5　修正后圖形

實驗測試中,將基于MEMS傳感器的集裝箱吊具位姿檢測系統(tǒng)在三維空間下旋轉,觀察監(jiān)控界面能否跟蹤旋轉的吊具。同時將未經姿態(tài)修正的數據和修正后的數據進行對比,設定值由量角器設定,量角器的精度為分(″),實驗數據如表1所示。

表1　實驗數據對比表

5結束語

本文介紹了基于MEMS傳感器的集裝箱吊具位姿檢測系統(tǒng),通過分析姿態(tài)測量原理,采用基于梯度下降算法的姿態(tài)補償算法修正姿態(tài),從而得到準確可靠的姿態(tài),最終通過實驗數據對比測試,該系統(tǒng)的姿態(tài)測量精度在±3°內,該系統(tǒng)硬件部分數據更新率高、短期精度和穩(wěn)定性好,抗沖擊能力強、制造成本低、可靠性高,體積小、易集成,且軟件設計部分實時跟蹤性與界面友好,是一款值得推廣的吊具位姿檢測系統(tǒng)。

參考文獻

[1]鐵道部標準計量研究所.GB1413-85集裝箱外部尺寸和額定重量[S].北京:中國標準出版社,1985.

[2]韓云生.基于雙目立體視覺的移動機器人目標定位[D].無錫:江南大學,2009.

[3]盧秀和,李建波.基于STM32的四旋翼飛行器姿態(tài)控制系統(tǒng)[J].計算機測量與控制,2014,22(3):761-763.

[4]Wang Yangzhu,Li Ning,Chen Xi,et al.Design and implementation of an AHRS based on MEMS sensors and complementary filtering[C].London:Advances in Mechanical Engineering,2014.

[5]秦永元.慣性導航[M].北京:科學出版社,2006.

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

[7]朱文杰,王廣龍,高鳳岐,等.基于MIMU和磁強計的在線實時定姿方法[J].傳感技術學報,2013,26(4):536-540.

[8]晁敏,蔣東方,文彩虹.磁羅盤誤差分析與校準[J].傳感技術學報,2010,23(4):525-531.

[9]Wang Shaohua,Yang Ying.Quadrotor aircraft attitude estimation and control based on Kalman filter[J].Control Theory & Applications,2013,30(9):536-540.

[10]Sebastian O H Madgwick,Andrew J L,Harrison Ravi Vaidyanathan.Estimation of IMU and MARG orientation using gradient descent algorithm[C].NZ:IEEE International Conference on Rehabilitation Robotics,2011.

Position Detection of Container Spreader Based on MEMS Sensors

XIE Yuduan,SONG Yanan,XU Ronghua

(School of Automation,Guangdong University of Technology,Guangzhou 510006,China)

AbstractAccording to the existing problem in position detection system of container spreader,including low handling efficiency highly cost,this paper investigates a position detection system of container spreader based on MEMS sensors.The system consists of the sensor data acquisition modules,the micro-controller and real-time dynamic monitoring.Sensor data acquisition modules contain the LSM303DLHC sensor and the L3GD20 sensor;micro-controller is STM32F303;the real-time dynamic monitoring is the PC interface.The gesture fusion algorithm based on gradient descent is analyzed.Comparison of the experimental data and analysis of the tracking monitoring interface show that the proposed system has good real-time performance,high accuracy and friendly interface.The system has been fully validated in related subjects.

KeywordsMEMS;container spreader;position detection;attitude algorithm

中圖分類號TP212

文獻標識碼A

文章編號1007-7820(2015)10-061-05

doi:10.16180/j.cnki.issn1007-7820.2015.10.017

作者簡介:謝雨端(1990—),男,碩士研究生。研究方向:嵌入式軟件。E-mail:547604932@qq.com。宋亞男(1976—),女,博士,副教授。研究方向:船舶運動建模與控制。徐榮華(1978—),男,博士,講師。研究方向:自動化裝備。

基金項目:廣東省自然科學基金博士啟動基金資助項目(S2012040007949);廣東工業(yè)大學博士啟動基金資助項目(12ZK0147);全國工程專業(yè)學位研究生教育自選課題資助項目(2014-JY-122);廣東工業(yè)大學教育教學改革基金資助項目(2013ZY011)

收稿日期:2015-03-12