王 磊，李天劍

(北京信息科技大學(xué) 機(jī)電工程學(xué)院，北京 100192)

0 引言

電商的飛速發(fā)展，引領(lǐng)智能倉儲(chǔ)物流行業(yè)進(jìn)入新的紀(jì)元，倉儲(chǔ)物流機(jī)器人受到越來越多的關(guān)注和重視[1]，在揀選、碼垛以及搬運(yùn)等諸多環(huán)節(jié)得到了廣泛的運(yùn)用。無人駕駛電動(dòng)叉車能夠幫助企業(yè)降低人工成本，提升工作效率，促進(jìn)企業(yè)智能轉(zhuǎn)型，有助于實(shí)現(xiàn)智能化倉儲(chǔ)生產(chǎn)管理新模式[2-3]。無人駕駛電動(dòng)叉車工作時(shí)，叉子的動(dòng)作是一個(gè)不穩(wěn)定的非線性系統(tǒng)[4]。因?yàn)槭菬o人駕駛系統(tǒng)，需要傳感器替代視覺對(duì)貨架及叉車叉子的傾斜角度進(jìn)行感知[5]；由于叉子本身顫動(dòng)加上貨物影響，導(dǎo)致叉子的角度檢測(cè)困難[6]。本文提出一種使用陀螺儀和加速度計(jì)通過卡爾曼濾波融合計(jì)算角度的方法，對(duì)電動(dòng)叉車叉子進(jìn)行狀態(tài)估計(jì)。實(shí)驗(yàn)結(jié)果表明，此方法可以快速準(zhǔn)確地返回叉子傾角。

1 系統(tǒng)結(jié)構(gòu)組成

無人駕駛電動(dòng)叉車叉子的轉(zhuǎn)角檢測(cè)系統(tǒng)主要包括上位機(jī)運(yùn)算模塊、下位機(jī)的控制模塊與檢測(cè)反饋模塊。上位機(jī)模塊主要由倍?？刂破鳌C端和顯示器組成：PC端編寫倍福程序，通過以太網(wǎng)將程序發(fā)送至倍福控制器，通過顯示器觀察程序運(yùn)行情況。下位機(jī)的控制模塊主要由控制系統(tǒng)、液壓系統(tǒng)和電源管理系統(tǒng)組成，控制系統(tǒng)又由松下PLC、數(shù)字步進(jìn)電機(jī)驅(qū)動(dòng)器和JULI電機(jī)構(gòu)成：松下PLC接收倍?？刂破鞯闹噶?，對(duì)數(shù)字步進(jìn)電機(jī)驅(qū)動(dòng)器發(fā)送指令，控制電機(jī)驅(qū)動(dòng)，進(jìn)而控制液壓系統(tǒng)，從而控制叉車叉子的運(yùn)動(dòng)；電源管理系統(tǒng)為各個(gè)模塊提供電能。檢測(cè)反饋模塊主要由加速度計(jì)、陀螺儀構(gòu)成[7-8]，將獲取的數(shù)據(jù)發(fā)送到PC端，進(jìn)而分析數(shù)據(jù)。無人駕駛電動(dòng)叉車叉子的轉(zhuǎn)角檢測(cè)系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 電動(dòng)叉車叉子轉(zhuǎn)角檢測(cè)系統(tǒng)結(jié)構(gòu)

轉(zhuǎn)角檢測(cè)模塊MPU6050[9]用來讀取叉車叉子的姿態(tài)，包括加速度計(jì)和陀螺儀。加速度計(jì)用來檢測(cè)叉子運(yùn)動(dòng)的加速度，陀螺儀檢測(cè)叉子轉(zhuǎn)動(dòng)的角速度。系統(tǒng)采用數(shù)字式MPU6050模塊，其內(nèi)部自帶穩(wěn)壓電路可兼容3.3 V/5 V供電電壓，可以通過USB轉(zhuǎn)TTL電平的串口模塊，將MPU6050連接至上位機(jī)PC端。MPU6050模塊接線圖如圖2所示。

圖2 MPU6050模塊接線圖

2 軟件設(shè)計(jì)

叉車在獲取貨物的同時(shí)，需要反饋系統(tǒng)不斷調(diào)整叉子的轉(zhuǎn)角，從而獲取叉子的最佳傾角，獲取貨物。同時(shí)，叉子的運(yùn)動(dòng)軌跡是一個(gè)實(shí)時(shí)非線性的系統(tǒng)，需要不斷對(duì)叉車叉子的轉(zhuǎn)角進(jìn)行檢測(cè)。系統(tǒng)采用MPU6050獲取電動(dòng)叉車叉子的初始加速度和角速度，通過卡爾曼濾波融合得到精準(zhǔn)的傾角信息。叉車叉子檢測(cè)流程如圖3所示。

圖3 叉車叉子檢測(cè)流程

求電動(dòng)叉車叉子傾斜角度時(shí)，通過MPU6050讀取叉車叉子在x、y方向的加速度，進(jìn)一步求解叉子傾角。叉子的力學(xué)分析如圖4所示。

圖4 叉子的力學(xué)分析

從圖中可以看出，角度a、c同與角度b互為余角，那么有：

(1)

式中：gx為x方向叉車叉子發(fā)生傾角的加速度；g為y方向叉車叉子的加速度。要計(jì)算叉車叉子的傾角，只需要求a的角度。由于反正弦函數(shù)求解a困難，且叉車叉子傾角范圍為[-π/6,π/6]，因此，可以用相近的一次函數(shù)替代正弦函數(shù)，進(jìn)而求得叉車叉子傾角。相近的一次函數(shù)如圖5所示。

圖5 不同角度相對(duì)于不同函數(shù)的曲線

從圖5可以看出，橫坐標(biāo)x軸坐標(biāo)范圍為 [0,π/2]。從上到下，第一條、第二條、第三條曲線的函數(shù)表達(dá)式分別為：

y=πx/180

(2)

y=0.95πx/180

(3)

y=sinx

(4)

圖5中,區(qū)間[0,π/6]的曲線可以滿足要求，選擇式(3)替代正弦曲線，就可以得到角度a：

(5)

3 卡爾曼濾波的實(shí)現(xiàn)

在叉子動(dòng)態(tài)測(cè)量過程中，叉子傾角和角速度與其真實(shí)情況相比，存在一定的偏差。本文采用卡爾曼濾波算法將加速度計(jì)與陀螺儀采集的數(shù)據(jù)融合，克服陀螺儀造成的漂移誤差和加速度計(jì)造成的動(dòng)態(tài)誤差，得出最優(yōu)值消除誤差，有效優(yōu)化叉子姿態(tài)數(shù)據(jù)信息，提高輸出精度。

除招收本科生以外，臺(tái)灣的大部分應(yīng)用型大學(xué)還招收碩士生，有的還招收博士生，人才培養(yǎng)層次較高，招收的碩士生也基本為應(yīng)用型碩士，具有較為完備的應(yīng)用型人才培養(yǎng)體系。以德明財(cái)經(jīng)科技大學(xué)為例，該校除招收本科生以外，還在資訊管理、財(cái)政稅務(wù)、金融服務(wù)管理等領(lǐng)域招收碩士生。

卡爾曼濾波的主要作用是利用信號(hào)、噪聲的狀態(tài)空間模型，以最小方差為最優(yōu)估計(jì)準(zhǔn)則，依據(jù)前一次的狀態(tài)預(yù)測(cè)出當(dāng)前檢測(cè)值，然后與實(shí)測(cè)狀態(tài)比較，來修正估計(jì)出當(dāng)前狀態(tài)?？柭鼮V波的使用，可以有效防止和基本消除噪點(diǎn)，有效地將數(shù)據(jù)融合，來糾正實(shí)測(cè)數(shù)據(jù)，使結(jié)果更準(zhǔn)確、更高效?？柭鼮V波系統(tǒng)流程如圖6所示。

圖6 卡爾曼濾波系統(tǒng)流程

首先利用卡爾曼濾波系統(tǒng)的過程模型，來預(yù)測(cè)下一狀態(tài)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)：

Xk=Xk-1+Buk

(6)

式中：Xk為現(xiàn)在的狀態(tài)預(yù)測(cè)；Xk-1為上次的狀態(tài)預(yù)測(cè)；B為將輸入轉(zhuǎn)為狀態(tài)的矩陣；uk為此時(shí)狀態(tài)的控制量，如果沒有控制量，它可以為0。系統(tǒng)更新后，對(duì)應(yīng)于Xk-1的協(xié)方差還需要更新。用P表示協(xié)方差，有：

Pk=APk-1AT+Q

(7)

式中：Pk為Xk對(duì)應(yīng)的協(xié)方差；Pk-1為Xk-1對(duì)應(yīng)的協(xié)方差；A為狀態(tài)轉(zhuǎn)移矩陣；Q為系統(tǒng)過程的協(xié)方差。式(6)、式(7)是對(duì)系統(tǒng)的狀態(tài)預(yù)測(cè)。

在狀態(tài)預(yù)測(cè)結(jié)果的基礎(chǔ)上，收集現(xiàn)在狀態(tài)的測(cè)量值；結(jié)合預(yù)測(cè)值和測(cè)量值，得到現(xiàn)在狀態(tài)k的最優(yōu)化估算值X：

X=Xk+K(yk-HXk)

(8)

式中：H為狀態(tài)變量到測(cè)量的轉(zhuǎn)換矩陣；K為卡爾曼增益：

K=PkHT/(HPkHT+R)

(9)

其中R為測(cè)量噪聲協(xié)方差。為了卡爾曼濾波器不斷運(yùn)行下去直到系統(tǒng)過程結(jié)束，需要更新k狀態(tài)下X的協(xié)方差：

P=(I-KH)Pk

(10)

其中I為1的矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P就是式(7)的Pk-1。也就是說卡爾曼濾波是時(shí)間更新和狀態(tài)更新兩個(gè)步驟不斷迭代的過程。此外，考慮陀螺儀受外界的干擾比較大，因此在計(jì)算中需要減去零點(diǎn)漂移，這樣可以減少誤差。

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

卡爾曼濾波調(diào)試是將傳感器的返回值，通過卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，讀取處理后的角度數(shù)據(jù)，不斷調(diào)整參數(shù)，使其輸出穩(wěn)定有效的波形。對(duì)傾角數(shù)據(jù)融合時(shí)，需要通過單片機(jī)將波形信號(hào)發(fā)送到電腦上，通過上位機(jī)獲取融合后的信號(hào)。不斷調(diào)整參數(shù)，直到波形滿足融合的精度要求。當(dāng)叉車取貨時(shí)，叉子受到很大的外力，運(yùn)動(dòng)角度突變很大，會(huì)產(chǎn)生相應(yīng)的波形。波形整定后可以大大減少叉子運(yùn)動(dòng)角度突變，原始波形和整定后的波形如圖7所示。

圖7 叉子運(yùn)動(dòng)角度突變時(shí)，原始波形和整定后的波形

在靜止?fàn)顟B(tài)下，輕觸叉車叉子，叉子受到力的作用后，其波形也會(huì)發(fā)生相應(yīng)的變化，原始波形和整定后的波形如圖8所示。

圖8 叉子靜止時(shí)，輕觸叉子，原始波形和整定后的波形

從圖中可以看出，在叉子運(yùn)動(dòng)時(shí)由于運(yùn)動(dòng)角度的突變，產(chǎn)生較為震蕩的波形；經(jīng)過整定之后，濾波更加平穩(wěn)，大大減少了角度突變，基本滿足融合的精度要求。

5 結(jié)束語

本文設(shè)計(jì)了一種無人駕駛電動(dòng)叉車叉子的轉(zhuǎn)角檢測(cè)系統(tǒng)。基于改進(jìn)卡爾曼濾波算法，對(duì)通過加速度求解的角度和y軸的角速度進(jìn)行融合處理。利用位移測(cè)量值與狀態(tài)預(yù)測(cè)值進(jìn)行加權(quán)計(jì)算得到狀態(tài)估計(jì)值，以狀態(tài)估計(jì)值參與后續(xù)迭代，在數(shù)次迭代后能夠消除初始參數(shù)的誤差，讀取處理后的角度數(shù)據(jù)，不斷調(diào)整參數(shù)。通過叉車叉子的故障真實(shí)值與叉車叉子的運(yùn)動(dòng)姿態(tài)的對(duì)比表明，本文方法可以實(shí)現(xiàn)傳感器故障良好地重構(gòu)，明顯提升了叉車叉子的轉(zhuǎn)角檢測(cè)穩(wěn)定性，有較大的實(shí)用價(jià)值。