基于MEMS的吊車(chē)系統(tǒng)擺角無(wú)線測(cè)量*

高丙團(tuán), 張羅馬, 范怡然, 朱振宇, 周 勰

(東南大學(xué) 電氣工程學(xué)院，江蘇 南京 210096)

0 引 言

在碼頭、倉(cāng)庫(kù)、工地等工業(yè)場(chǎng)所廣泛應(yīng)用著吊車(chē)系統(tǒng)，其在吊運(yùn)過(guò)程中不可避免地會(huì)出現(xiàn)“擺動(dòng)”現(xiàn)象，這種擺動(dòng)會(huì)危害吊車(chē)作業(yè)和工人安全，也會(huì)降低吊車(chē)的工作效率。所以，對(duì)吊車(chē)系統(tǒng)進(jìn)行有效的防擺控制非常必要，基于反饋控制進(jìn)行防擺的一個(gè)重要前提是能夠準(zhǔn)確地進(jìn)行擺角檢測(cè)[1]。

吊車(chē)擺角檢測(cè)方法主要包括光電編碼器[2]、電位計(jì)[3]、視覺(jué)傳感器[4]和傾角計(jì)[5]等。光電編碼器和電位計(jì)一般只適用于小型的實(shí)驗(yàn)室實(shí)驗(yàn)裝置，視覺(jué)傳感器成本較高且易受環(huán)境影響；傾角計(jì)在檢測(cè)過(guò)程中具有一定的優(yōu)勢(shì)，特別是基于微機(jī)電系統(tǒng)(MEMS)的傾角計(jì)成本較低。傾角計(jì)檢測(cè)吊車(chē)擺角的缺點(diǎn)是當(dāng)負(fù)載圍繞吊繩轉(zhuǎn)動(dòng)時(shí)刻，則無(wú)法判斷負(fù)載的實(shí)際擺動(dòng)角度。在多數(shù)的吊運(yùn)過(guò)程中，特別是單股吊繩的情況，負(fù)載一般都具有圍繞吊繩轉(zhuǎn)動(dòng)的特征，此刻采用傾角計(jì)則不能準(zhǔn)確檢測(cè)負(fù)載的空間擺角，然而可采用廣泛應(yīng)用于航天航空的航姿參考系統(tǒng)(AHRS)模塊[6]實(shí)現(xiàn)。

基于國(guó)內(nèi)外在吊車(chē)系統(tǒng)和移動(dòng)物體姿態(tài)導(dǎo)航方面的研究成果，本文提出一種吊車(chē)系統(tǒng)擺角檢測(cè)方法：由2套基于MEMS傳感器的AHRS模塊分別檢測(cè)吊車(chē)和負(fù)載的空間姿態(tài)，經(jīng)無(wú)線傳輸后相互校核解算獲得吊車(chē)負(fù)載的擺角。

1 檢測(cè)系統(tǒng)設(shè)計(jì)

吊車(chē)系統(tǒng)負(fù)載空間擺動(dòng)和設(shè)計(jì)的擺角檢測(cè)系統(tǒng)如圖1所示，參考坐標(biāo)XOY固定在吊車(chē)本體上，吊桿的末端A連接吊繩的始端，吊繩末端B連接負(fù)載，負(fù)載在空間擺動(dòng)，o點(diǎn)為A點(diǎn)在水平面oCBD的投影。最終需要測(cè)量的空間擺角為θx和θy，θx為負(fù)載相對(duì)于吊臂的左右方向的擺動(dòng)，θy為負(fù)載相對(duì)于吊臂的前后方向的擺動(dòng)。

圖1 吊車(chē)擺角無(wú)線檢測(cè)示意圖

傳感器1安裝在吊繩末端的吊鉤上，作用是采用基于MEMS技術(shù)的三自由度的磁場(chǎng)計(jì)、三自由度的陀螺儀和三自由度的加速度計(jì)測(cè)量計(jì)算并通過(guò)擴(kuò)展卡爾曼濾波得到吊鉤的空間姿態(tài)參數(shù)；傳感器2安裝在吊車(chē)本體控制箱中，可得到吊車(chē)本體的空間姿態(tài)參數(shù)；本文中采用導(dǎo)航學(xué)中常用航向角ψ、俯仰角θ和橫滾角φ作為空間載體的姿態(tài)角。Zig Bee無(wú)線通信模塊將傳感器1測(cè)得的吊鉤空間姿態(tài)參數(shù)傳送給傳感器2，傳感器2的處理器通過(guò)對(duì)吊鉤和吊車(chē)本體兩部分的姿態(tài)進(jìn)行標(biāo)定解算，得到吊車(chē)負(fù)載相對(duì)于吊車(chē)本體的空間擺角θx和θy并最終將該擺角信息上傳至吊車(chē)控制系統(tǒng)，進(jìn)行防擺控制。

2 擺角的解算

吊車(chē)的擺角解算包括2個(gè)重要步驟：1) 基于MEMS傳感器的AHRS解算；2) 基于AHRS信息的吊車(chē)擺角標(biāo)定解算?；贛EMS傳感器的AHRS解算主要包括：首先，基于三自由度加速度傳感器計(jì)算得到物體俯仰角θ和橫滾角φ；其次，基于三自由度磁場(chǎng)計(jì)計(jì)算得到物體的航向角ψ；最后，結(jié)合三自由度的陀螺儀的數(shù)據(jù)結(jié)合姿態(tài)更新算法(如擴(kuò)展卡爾曼濾波)對(duì)物體的空間姿態(tài)參數(shù)進(jìn)行實(shí)時(shí)更新。對(duì)于基于MEMS傳感器實(shí)現(xiàn)AHRS解算的具體方法不是本文的重點(diǎn)，可參考文獻(xiàn)[6,7]。本文重點(diǎn)在于如何采用基于MEMS技術(shù)的AHRS模塊實(shí)現(xiàn)吊車(chē)擺角的無(wú)線檢測(cè)。

結(jié)合圖1，由于俯仰角與橫滾角都是反映載體坐標(biāo)XOY平面與參考坐標(biāo)XOY平面夾角的姿態(tài)角，假設(shè)吊繩平行于傳感模塊1中MEMS器件的Z軸方向，對(duì)于吊鉤/負(fù)載針對(duì)吊繩沒(méi)有旋轉(zhuǎn)的特殊情況：坐標(biāo)系中某一特定位置只存在兩者中的一個(gè)角，這樣就可以將2個(gè)角簡(jiǎn)化成一個(gè)。設(shè)只有俯仰角，橫滾角為0，則傳感器模塊1檢測(cè)到的俯仰角為θ=∠oAB，傳感模塊1和傳感模塊2檢測(cè)到的偏航角之差為Δψ=∠BoD，根據(jù)三角幾何可知

sin∠oBA=sin∠ABD·sin∠ADo,

cos∠ABD=cos∠oAB·cos∠oBD.

θy=arcsin(sinθ·sin Δψ),

(1)

θx=arccos(cosθ·secθy).

(2)

對(duì)于一般情況下負(fù)載在參考坐標(biāo)系中既有橫滾角又有俯仰角，可利用方向余弦矩陣旋轉(zhuǎn)成上述特殊情況。不妨設(shè)一般情況下負(fù)載AHRS解算得到的空間姿態(tài)為(ψ,θ,φ)，同時(shí)設(shè)特殊情況下的航向角為ψ′,俯仰角為θ′。需要注意的是，負(fù)載在擺動(dòng)時(shí)不僅隨繩子搖擺，還有可能繞著繩子自身旋轉(zhuǎn)，這種情況可以看成負(fù)載在載體坐標(biāo)系中繞Z軸旋轉(zhuǎn)一個(gè)角度α，故還需左乘方向余弦矩陣

(3)

(4)

由式(3)中對(duì)應(yīng)項(xiàng)相等，可解得

θ′=arccos(cosφ·cosθ),

(5)

(6)

θ′為特殊情況下的俯仰角，且θ′范圍為[0°,180°]；ψ′即是相對(duì)于參考坐標(biāo)系的偏航角，此時(shí)即可將它與傳感模塊2檢測(cè)的偏航角相減，得到Δψ。至此已求得轉(zhuǎn)換到特殊情況下的偏航角Δψ和俯仰角θ′，則將式(5)和式(6)代入式(1)和式(2)，可求得一般情況下吊車(chē)擺角擺角θx和θy。

3 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)系統(tǒng)的結(jié)構(gòu)如圖2所示。每一個(gè)傳感模塊由電源模塊、AHRS模塊和Zig Bee模塊組成；AHRS模塊由ARM STM32F103TB嵌入式處理器集成MEMS器件三軸陀螺儀和三軸加速度計(jì)MPU6050，三軸磁力計(jì)HMC5883實(shí)現(xiàn)載體的空間姿態(tài)檢測(cè)；AHRS模塊得到的姿態(tài)信息由串口經(jīng)過(guò)基于CC2530的Zig Bee模塊進(jìn)行無(wú)線通信。傳感數(shù)據(jù)可經(jīng)過(guò)傳感模塊2基于串口上傳給上位機(jī)使用和處理。

圖2 實(shí)驗(yàn)系統(tǒng)結(jié)構(gòu)

基于實(shí)驗(yàn)平臺(tái)進(jìn)行了靜態(tài)擺角和動(dòng)態(tài)擺角的檢測(cè)，靜態(tài)擺角的檢測(cè)結(jié)果如表1所示，動(dòng)態(tài)擺角檢測(cè)的結(jié)果如圖3所示。靜態(tài)檢測(cè)通過(guò)將負(fù)載固定至4個(gè)典型的角度進(jìn)行靜態(tài)檢測(cè)；動(dòng)態(tài)檢測(cè)將負(fù)載拉至任意空間角度后釋放，記錄產(chǎn)生的自然擺動(dòng)角度，上位機(jī)記錄周期為50 ms。表1所示的靜態(tài)檢測(cè)結(jié)果顯示檢測(cè)的誤差隨著角度的增大而增大，負(fù)載在小范圍擺動(dòng)時(shí)候檢測(cè)角度的誤差小于1°，擺動(dòng)角度接近60°時(shí)，檢測(cè)最大誤差接近3°；且由于θx要基于θy進(jìn)行計(jì)算，因此，前者的誤差要大于后者，這也在實(shí)驗(yàn)結(jié)果中得到正確體現(xiàn)。圖3所示的動(dòng)態(tài)檢測(cè)結(jié)果顯示，擺動(dòng)呈現(xiàn)平滑的周期性，且由于阻尼的存在呈現(xiàn)一定的衰減，這與實(shí)際擺動(dòng)情況符合。

根據(jù)上一節(jié)的擺角的理論解算可知，擺角的檢測(cè)精度依賴(lài)于基于MEMS傳感器檢測(cè)的空間姿態(tài)角(ψ，θ，φ)的解算精度，目前較好的基于MEMS的AHRS產(chǎn)品檢測(cè)誤差可小于0.5°[8]，該精度能夠滿足吊車(chē)擺角檢測(cè)與防擺控制的要求。

表1 擺角靜態(tài)檢測(cè)實(shí)驗(yàn)結(jié)果

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

本文設(shè)計(jì)了一種基于MEMS器件和Zig Bee無(wú)線通信的吊車(chē)擺角檢測(cè)系統(tǒng)，給出了吊車(chē)系統(tǒng)基于AHRS模塊的擺角計(jì)算公式，實(shí)驗(yàn)驗(yàn)證了提出方案的正確性和可行性。提出的吊車(chē)擺角檢測(cè)方案具有成本低，檢測(cè)模塊體積小，易于安裝的優(yōu)點(diǎn)，可推廣用于吊車(chē)系統(tǒng)擺角檢測(cè)和防擺控制。

參考文獻(xiàn):

[1] Ngo Q H,Hong K S.Sliding-mode antisway control of an offshore container crane[J].IEEE/ASME Transactions on Mechatronics,2012,17(2):201-209.

圖3 擺角動(dòng)態(tài)檢測(cè)實(shí)驗(yàn)結(jié)果

[2] Liu D,Yi J,Zhao D,et al.Adaptive sliding mode fuzzy control for a two-dimensional overhead crane[J].Mechatronics,2005,15:502-522.

[3] Gao B,Chen H,Zhang X,et al.A practical optimal controller for underactuated gantry crane systems[C]∥Symposium on System and Control of Aerospace and Aeronatics,2006:720-725.

[4] Yoshida Y,Tsuzuki K.Visual tracking and control of a moving overhead crane load[C]∥IEEE International Workshop on Advanced Motion Control,2006:630-635.

[5] Yu Y,Ou J,Zhang C,et al.Wireless inclinometer acquisition system for reducing swing movement control module experiment of hook model[C]∥Proceedings of SPIE,Sensors and Smart Structures Technologies for Civil,Mechanical,and Aerospace Systems,2008:693216—1-693216—9.

[6] Markley F L,Crassidis J L,Cheng Y.Nonlinear attitude filtering methods[J].Journal of Guidance,Control,Dynamics,2007,30(1):12-28.

[7] 張榮輝，賈宏光，陳 濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程，2008，16(10)：1963-1970.

[8] VectorNav Technologies LLC.Specifications of VN—100 IMU/AHRS[EB/OL].[2014—01—15].http:∥www.vectornav.com/products/vn100-smt?id=30.