謝雨端,宋亞男,徐榮華

(廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院,廣東 廣州　510006)

?

基于MEMS傳感器的集裝箱吊具位姿檢測(cè)應(yīng)用

謝雨端,宋亞男,徐榮華

(廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院,廣東 廣州510006)

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

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

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

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

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

1系統(tǒng)架構(gòu)

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

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

圖2　系統(tǒng)硬件結(jié)構(gòu)圖

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

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

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

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

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

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

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

R系與b系之間的旋轉(zhuǎn)矩陣(方向余弦矩陣)存在誤差,將加速度計(jì)的測(cè)量數(shù)據(jù)或者理論數(shù)據(jù)分別經(jīng)過旋轉(zhuǎn)矩陣轉(zhuǎn)換在同一坐標(biāo)系下,利用測(cè)量值與理論值的誤差來(lái)修正旋轉(zhuǎn)矩陣[7]。

2.2　加速度計(jì)修正原理

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

(1)

(2)

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

(3)

2.3　磁傳感器修正原理

(4)

(5)

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

(6)

故多元向量誤差函數(shù)為

(7)

對(duì)q0、q1、q2、q3求偏導(dǎo),得到雅可比矩陣為

(8)

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

(9)

3姿態(tài)融合

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

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

(10)

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

q=-λ·

(11)

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

(12)

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

(13)

跟蹤物體高速運(yùn)動(dòng)時(shí),上式近似為

(14)

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

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

(15)

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

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

(16)

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

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

(17)

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

圖中qg是一個(gè)qt相關(guān)的表達(dá)式,而facc和fmag也是與qt相關(guān)的表達(dá)式,圖中表達(dá)的是對(duì)qt的迭代運(yùn)算。

4實(shí)驗(yàn)結(jié)果分析

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

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

圖4　修正前圖形

圖5　修正后圖形

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

表1　實(shí)驗(yàn)數(shù)據(jù)對(duì)比表

5結(jié)束語(yǔ)

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

參考文獻(xiàn)

[1]鐵道部標(biāo)準(zhǔn)計(jì)量研究所.GB1413-85集裝箱外部尺寸和額定重量[S].北京:中國(guó)標(biāo)準(zhǔn)出版社,1985.

[2]韓云生.基于雙目立體視覺的移動(dòng)機(jī)器人目標(biāo)定位[D].無(wú)錫:江南大學(xué),2009.

[3]盧秀和,李建波.基于STM32的四旋翼飛行器姿態(tài)控制系統(tǒng)[J].計(jì)算機(jī)測(cè)量與控制,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]秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,2006.

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

[7]朱文杰,王廣龍,高鳳岐,等.基于MIMU和磁強(qiáng)計(jì)的在線實(shí)時(shí)定姿方法[J].傳感技術(shù)學(xué)報(bào),2013,26(4):536-540.

[8]晁敏,蔣東方,文彩虹.磁羅盤誤差分析與校準(zhǔn)[J].傳感技術(shù)學(xué)報(bào),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

中圖分類號(hào)TP212

文獻(xiàn)標(biāo)識(shí)碼A

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

doi:10.16180/j.cnki.issn1007-7820.2015.10.017

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

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

收稿日期:2015-03-12