林義忠，望 翔，張 勤

（廣西大學(xué)機(jī)械工程學(xué)院，廣西 南寧530004）

0 引言

自動(dòng)引導(dǎo)小車 AGV（automated guided vehicle），是現(xiàn)代物流系統(tǒng)中的重要成員[1]。而AGV的導(dǎo)航控制是總體設(shè)計(jì)中重要內(nèi)容。目前AGV導(dǎo)航方式主要分為視覺、GPS、激光、磁條導(dǎo)引[2]等幾種方式，視覺、激光導(dǎo)航精度雖然高，但其所依賴的傳感器成本較高，GPS導(dǎo)航方式缺陷在于室內(nèi)接收GPS信號(hào)不穩(wěn)定從而不適合室內(nèi)AGV導(dǎo)航，因此磁導(dǎo)航方式仍是室內(nèi)AGV主流導(dǎo)航方式。

PLC因其電氣特性穩(wěn)定、編程簡單等特性通常作為傳統(tǒng)磁導(dǎo)航AGV的控制器，但相較于STM32單片機(jī)，其運(yùn)算資源較弱，無法實(shí)現(xiàn)較為復(fù)雜的導(dǎo)航算法。本文從導(dǎo)航效果、成本等方面考慮采用了磁條導(dǎo)航，STM32F103C8T6為AGV控制器及分段式PID導(dǎo)航算法的導(dǎo)航方案。

AGV導(dǎo)航方案主要包括驅(qū)動(dòng)單元和磁導(dǎo)航糾偏單元，所以本文首先闡述AGV的基本結(jié)構(gòu)，其次分析磁導(dǎo)航傳感器在STM32F103上的實(shí)現(xiàn)形式。然后分析AGV運(yùn)動(dòng)學(xué)模型，推導(dǎo)出本文AGV糾偏傳遞函數(shù)模型為PID控制算法提供了設(shè)計(jì)依據(jù)。最后在實(shí)際磁條軌道上測試本文AGV的實(shí)際運(yùn)行效果，以證明該種導(dǎo)航方案的可行性。

1 AGV本體結(jié)構(gòu)及導(dǎo)航控制系統(tǒng)方案

1.1 AGV本體結(jié)構(gòu)

本文AGV的本體結(jié)構(gòu)示意圖如圖1所示，底盤采用六輪式結(jié)構(gòu)，兩驅(qū)動(dòng)輪位于車體中間，車體前后兩側(cè)各有兩個(gè)從動(dòng)輪。AGV動(dòng)力部分由兩個(gè)步進(jìn)電機(jī)驅(qū)動(dòng)單元組成，兩步進(jìn)電機(jī)差速驅(qū)動(dòng)AGV的直行、轉(zhuǎn)向。

圖1 AGV本體結(jié)構(gòu)圖

1.2 磁導(dǎo)航硬件方案

本文控制器核心為一塊芯片為STM32F103開發(fā)板，該芯片是以Cortex-M3為內(nèi)核基礎(chǔ)改進(jìn)，相比CM3降低了功耗，中斷響應(yīng)大幅度提高[3]。該芯片有144的針腳，其中多達(dá)80個(gè)GPIO口，選擇作為磁導(dǎo)航AGV的主控制器。

路徑跟蹤信號(hào)通過磁傳感器獲得，磁傳感器能檢測到事先鋪設(shè)的磁條路徑信息。本文磁傳感器由16個(gè)霍爾元件組成，16個(gè)引出針腳與開發(fā)板上的通用GPIO口連接，通過標(biāo)STM32標(biāo)準(zhǔn)開發(fā)庫的GPIO_Read InputDataBith（）函數(shù)獲取磁場信息。

1.3 驅(qū)動(dòng)單元硬件方案

驅(qū)動(dòng)單元采用了兩個(gè)86系列步進(jìn)電機(jī)，具備閉環(huán)步進(jìn)的功能，即在驅(qū)動(dòng)器-電機(jī)-驅(qū)動(dòng)器之間構(gòu)成“小閉環(huán)”，由控制器下發(fā)給驅(qū)動(dòng)器的多個(gè)脈沖能夠由驅(qū)動(dòng)器確保電機(jī)轉(zhuǎn)過脈沖指令對(duì)應(yīng)的實(shí)際角度，從而改善了傳統(tǒng)步進(jìn)電機(jī)“丟步”的情形。如圖2所示，STM32的通用GPIO口連接驅(qū)動(dòng)器脈沖輸入口，根據(jù)外糾偏信息，通過高級(jí)定時(shí)器給驅(qū)動(dòng)器發(fā)送可變頻率脈沖信號(hào)，驅(qū)動(dòng)器驅(qū)動(dòng)電機(jī)至目標(biāo)轉(zhuǎn)速，從而達(dá)到驅(qū)動(dòng)AGV的目的。

圖2 運(yùn)動(dòng)控制硬件方案框圖

2 糾偏系統(tǒng)建模及分析

AGV在實(shí)際工作運(yùn)行當(dāng)中，是一個(gè)非完整約束系統(tǒng)，此外輪胎與地面接觸時(shí)具有非線性特性，要得到其精確運(yùn)動(dòng)學(xué)模型十分困難。為后續(xù)后置算法提供理論基礎(chǔ)，首先構(gòu)建AGV的理想運(yùn)動(dòng)學(xué)模型，為此作如下簡化和假設(shè)：（1）車輪在二維的水平路面上低速運(yùn)行，且車輪不會(huì)變形；（2）車輪不會(huì)發(fā)生與車輪軸方向平行的橫向滑移；（3）車輪與地面只發(fā)生純滾動(dòng)運(yùn)動(dòng)過程；（4）兩驅(qū)動(dòng)輪大小相同并始終處于同一軸線，且軸線同AGV前進(jìn)方向相垂直。

2.1 差速運(yùn)動(dòng)模型

從動(dòng)輪對(duì)AGV不產(chǎn)生任何實(shí)際的運(yùn)動(dòng)學(xué)約束，因此建立運(yùn)動(dòng)學(xué)模型時(shí)可以將AGV簡化為兩輪式結(jié)構(gòu)，僅考慮兩驅(qū)動(dòng)輪情況。根據(jù)圖1結(jié)構(gòu)圖簡化之后的運(yùn)動(dòng)學(xué)模型如圖3所示，本文選取AGV兩驅(qū)動(dòng)輪連線中點(diǎn)A作為局部坐標(biāo)系原點(diǎn)，建立X1-Y1坐標(biāo)系。

圖3 差速運(yùn)動(dòng)學(xué)模型分析圖

已知A（XA，YA）及局部坐標(biāo)系與全局坐標(biāo)系偏轉(zhuǎn)角θ，v、ω分別為C點(diǎn)的速度和自轉(zhuǎn)角速度，L為兩驅(qū)動(dòng)輪輪距。在全局坐標(biāo)系中，以點(diǎn)A為中心的AGV 位姿矩陣為（x，y，θ），左右驅(qū)動(dòng)輪坐標(biāo)為（x1，y1）和（x2，y2），可得如下約束關(guān)系：

推導(dǎo)出AGV差速驅(qū)動(dòng)運(yùn)動(dòng)學(xué)模型為：

通常易獲取左右驅(qū)動(dòng)輪角速度和，根據(jù)剛體平面運(yùn)動(dòng)理論可得到將驅(qū)動(dòng)輪角速度作為輸入量的差速運(yùn)動(dòng)模型：

從AGV的運(yùn)動(dòng)學(xué)模型即式（4）可知通過控制兩驅(qū)動(dòng)輪輪速即可改變AGV的整體運(yùn)行速度和角速度，從而對(duì)AGV在全局坐標(biāo)系中的位姿進(jìn)行改變，使AGV能夠在二維平面內(nèi)向任意位置運(yùn)動(dòng)。

2.2 偏差模型分析

機(jī)器人運(yùn)動(dòng)實(shí)際是一個(gè)離散系統(tǒng)，可認(rèn)為目標(biāo)路徑由一系列目標(biāo)點(diǎn)或目標(biāo)位姿組成，在一個(gè)采樣時(shí)刻，實(shí)際位姿與目標(biāo)位姿之差即為AGV的偏差。具有固定路徑的AGV運(yùn)動(dòng)模型如圖4所示，VL，VR，VA則代表AGV左驅(qū)動(dòng)輪，右驅(qū)動(dòng)輪以及AGV中心點(diǎn)A點(diǎn)的線速度，R為小車瞬時(shí)轉(zhuǎn)向半徑，同時(shí)可得如下運(yùn)動(dòng)關(guān)系。

圖4 路徑跟蹤糾偏分析圖

當(dāng)AGV與目標(biāo)路徑出現(xiàn)位姿偏差時(shí)，在A點(diǎn)會(huì)出現(xiàn)X和Y方向的分速度：

由圖4所示幾何關(guān)系，計(jì)算中心點(diǎn)A在時(shí)刻的坐標(biāo)如下：

本文AGV所用磁傳感器內(nèi)部由間距為10mm的16個(gè)霍爾元件組成，其感應(yīng)到磁信號(hào)時(shí)都會(huì)在對(duì)應(yīng)位置輸出高電平信號(hào)，在AGV底部共布置2個(gè)距離為D磁傳感器，如圖5所示。

圖5 磁導(dǎo)航傳感器布置圖

在AGV偏差未超過測量范圍時(shí)，可以根據(jù)圖5所示幾何關(guān)系得出中心點(diǎn)的偏距和偏角。可知前置磁傳感器中點(diǎn)B與兩驅(qū)動(dòng)輪中點(diǎn)A點(diǎn)始終相距D/2。在測得AGV的前、后磁傳感器讀數(shù)分別為ed1、ed2后，偏轉(zhuǎn)方向則由圖示前置磁傳感器信號(hào)即ed1決定，則可得：

將式（3～11）代入式（12），可得點(diǎn) B 在時(shí)刻的偏差形式如下：

則由系統(tǒng)辨識(shí)法可得一般AGV糾偏模型[3]的傳遞函數(shù)為：

式（14）中，ω為自然頻率，ζ為阻尼比。對(duì)本文AGV，速度0.5 m/s、車輪直徑150 mm，可得ω=6.67，取 ζ=0.5，采樣周期為 1 ms。則可得本文 AGV的糾偏模型傳遞函數(shù)為：

該章節(jié)在運(yùn)動(dòng)模型的基礎(chǔ)上建立了AGV包含角度、距離的偏差模型，得到AGV較為理想的驅(qū)動(dòng)系統(tǒng)數(shù)學(xué)模型傳遞函數(shù)，為小車的路徑跟蹤算法研究及驗(yàn)證提供理論基礎(chǔ)。

3 PID糾偏控制器設(shè)計(jì)

本文AGV的糾偏控制流程如圖6所示，由2部分構(gòu)成：（1）經(jīng)典PID控制器：該控制器直接對(duì)被控制對(duì)象進(jìn)行閉環(huán)控制，并且3個(gè)參數(shù)KP、KI、KD為在線控制；（2）PID參數(shù)選擇模塊。

圖6 糾偏控制流程圖

PID控制增量式離散形式如下：

式（16）中，ek為第 k 次采樣值，KP、KI、KD分別為比例、積分、微分環(huán)節(jié)系數(shù)。

實(shí)驗(yàn)測試結(jié)果及PID參數(shù)整定理論[4]表明傳統(tǒng)PID控制在偏差過大或過小時(shí)的控制效果不佳，因此本文采用工程整定法即反復(fù)實(shí)驗(yàn)比較不同參數(shù)的效果，得到PID參數(shù)對(duì)控制系統(tǒng)影響效果如表1所示。

表1 PID參數(shù)表

AGV在出現(xiàn)偏差較大的情況時(shí)，比如在轉(zhuǎn)彎半徑較小位置過彎，小車左右擺動(dòng)情況比較明顯，甚至可能出現(xiàn)丟失信號(hào)情況。對(duì)此本文提出用分段式PID控制算法，對(duì)不同范圍偏差PID過程選取對(duì)應(yīng)參數(shù)，能夠在快速響應(yīng)前提下減弱系統(tǒng)震蕩現(xiàn)象，分段式PID控制流程如圖7所示。

圖7 分式PID控制流程圖

在MATLAB中進(jìn)行仿真，得到階躍響應(yīng)曲線如圖8所示。與傳統(tǒng)PID控制相比，分段式PID在不同偏差階段采用對(duì)應(yīng)控制參數(shù)，比傳統(tǒng)PID控制能更快進(jìn)入穩(wěn)態(tài)且超調(diào)量更低，從而提高了系統(tǒng)的動(dòng)態(tài)和靜態(tài)性能。

圖8 兩種PID控制方式階躍響應(yīng)曲線圖

4 實(shí)驗(yàn)

為測試本文AGV導(dǎo)航方案的實(shí)際可行性與效果，進(jìn)行了現(xiàn)場實(shí)驗(yàn)，在AGV中間安裝偏差記錄裝置，磁條軌道上布置20 cm*3 cm的白色紙條記錄測量實(shí)際偏差值，直行軌道每邊布置11個(gè)，轉(zhuǎn)彎處2個(gè)，共記52個(gè)。實(shí)驗(yàn)過程讓AGV以0.5 m/s速度沿磁條運(yùn)行5圈共260個(gè)數(shù)據(jù)，統(tǒng)計(jì)偏差結(jié)果如表2所示。

圖9 AGV實(shí)驗(yàn)測試圖

表2 AGV運(yùn)行5圈偏差數(shù)據(jù)統(tǒng)計(jì)

由表2可知AGV的偏差均值和標(biāo)準(zhǔn)差均較低，說明路徑跟蹤精度較好；本文AGV磁傳感器由16個(gè)間距10mm霍爾元件組成，因此單邊最大糾偏距離80mm，且表2中最大值40 mm明顯小于該值，可知AGV未出現(xiàn)脫軌情況，亦表明本文AGV導(dǎo)航方案可行且效果良好。

5 結(jié)束語

傳統(tǒng)室內(nèi)AGV導(dǎo)航方案通常使用PLC配合單磁傳感器，無法獲取運(yùn)行時(shí)的位姿以及不能加入較為復(fù)雜糾偏算法，會(huì)對(duì)糾偏準(zhǔn)確性產(chǎn)生影響。本文利用兩個(gè)磁傳感器獲取AGV實(shí)時(shí)位姿，STM32F103C8T6作為控制器，實(shí)現(xiàn)了將位姿偏差作為分段式PID控制輸入量的導(dǎo)航算法。實(shí)驗(yàn)結(jié)果表明該種控制及導(dǎo)航方案效果較好，且整套控制系統(tǒng)軟硬件成本均較低，對(duì)AGV導(dǎo)航控制方案設(shè)計(jì)有一定參考價(jià)值。