電動叉車叉子轉角檢測系統(tǒng)設計

王 磊，李天劍

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

0 引言

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

1 系統(tǒng)結構組成

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

圖1 電動叉車叉子轉角檢測系統(tǒng)結構

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

圖2 MPU6050模塊接線圖

2 軟件設計

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

圖3 叉車叉子檢測流程

求電動叉車叉子傾斜角度時，通過MPU6050讀取叉車叉子在x、y方向的加速度，進一步求解叉子傾角。叉子的力學分析如圖4所示。

圖4 叉子的力學分析

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

(1)

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

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

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

y=πx/180

(2)

y=0.95πx/180

(3)

y=sinx

(4)

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

(5)

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

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

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

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

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

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

Xk=Xk-1+Buk

(6)

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

Pk=APk-1AT+Q

(7)

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

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

X=Xk+K(yk-HXk)

(8)

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

K=PkHT/(HPkHT+R)

(9)

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

P=(I-KH)Pk

(10)

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

4 實驗結果與分析

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

圖7 叉子運動角度突變時，原始波形和整定后的波形

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

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

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

5 結束語

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