STM32F103ZET6單片機(jī)的叉車式AGV的尋跡算法※

石含飛，何勇靈

(北京航空航天大學(xué) 交通科學(xué)與工程學(xué)院，北京100191)

?

STM32F103ZET6單片機(jī)的叉車式AGV的尋跡算法※

石含飛，何勇靈

(北京航空航天大學(xué) 交通科學(xué)與工程學(xué)院，北京100191)

摘要:以單片機(jī)為系統(tǒng)控制器核心，針對(duì)磁敏傳感器組尋跡的叉車式AGV,設(shè)計(jì)了傳感器的陣列布置形式，并根據(jù)此形式以及磁導(dǎo)引叉車式AGV的尋跡約束條件和行走要求提出了路徑的識(shí)別與跟蹤算法。前進(jìn)時(shí)采用分段模糊PID控制，PD系數(shù)被模糊控制器動(dòng)態(tài)改變,以適應(yīng)驅(qū)動(dòng)輪的不同轉(zhuǎn)速和前向位置偏差的大小。后退過程應(yīng)用雙閉環(huán)PID進(jìn)行控制，姿態(tài)角和后向位置分別為內(nèi)環(huán)和外環(huán)。在實(shí)際行走測試中，AGV穩(wěn)定行駛時(shí)路徑偏差可保持在10 mm內(nèi)。

關(guān)鍵詞:STM32F103ZET6；AGV；磁導(dǎo)引；模糊控制；PID控制

引言

綜合考慮AGV車身長度為2 m、穩(wěn)定行駛路徑偏差不超過10 mm等約束條件和行走要求，以磁帶作為導(dǎo)引路徑，以STM32F103ZET6單片機(jī)為核心構(gòu)建控制器，提出了一種基于模糊控制和PID控制的AGV智能尋跡算法，解決了叉車式AGV自動(dòng)行進(jìn)狀態(tài)下路徑的準(zhǔn)確識(shí)別與快速跟蹤問題。

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

文中所討論的為單舵輪叉車式AGV。輪系部分主要有一個(gè)轉(zhuǎn)向驅(qū)動(dòng)輪、兩個(gè)從動(dòng)輪、再輔以兩個(gè)支撐輪，轉(zhuǎn)向驅(qū)動(dòng)輪與兩個(gè)從動(dòng)輪的布局方式和相對(duì)位置決定了AGV的運(yùn)動(dòng)形式[2]。叉車運(yùn)動(dòng)的前進(jìn)與后退的方向定義如圖1(a)所示。

e1為前向16位磁敏傳感器感知的路徑中心相對(duì)預(yù)設(shè)路徑中心的偏移量，即前向位置偏差；e2為后向8位磁敏傳感器感知的路徑中心相對(duì)預(yù)設(shè)路徑中心的偏移量，即后向位置偏差；α為兩磁敏傳感器感知的路徑中心線相對(duì)于預(yù)設(shè)路徑中心線的偏轉(zhuǎn)角，即姿態(tài)角。前向與后向磁敏傳感器的布置陣列如圖1(b)所示，前向磁敏傳感器為YF-9016(16路開關(guān)量傳感器)。后向磁敏傳感器為YF-9006(8路開關(guān)量傳感器)，當(dāng)磁敏傳感器相應(yīng)的位檢測到磁條的有效信號(hào)時(shí)，該位輸出高電平，位與位之間的距離L=10 mm，虛線上的7和4分別為磁條中心預(yù)設(shè)在前向磁敏傳感器和后向磁敏傳感器上的位置，即預(yù)設(shè)路徑中心對(duì)應(yīng)于傳感器上的位置。

圖1　叉車式AGV結(jié)構(gòu)與磁敏傳感器布置陣列

2路徑識(shí)別

2.1磁條中心位置檢測

AGV在運(yùn)動(dòng)過程中，磁條與傳感器的相對(duì)位置會(huì)出現(xiàn)3種情形：①磁條被傳感器完全覆蓋，此時(shí)磁條中心位置是傳感器連續(xù)獲取信號(hào)的中心；②磁條處于傳感器邊緣，若此時(shí)傳感器上檢測到磁信號(hào)的位數(shù)少于3，則自動(dòng)補(bǔ)齊傳感器外面的位，使有效位數(shù)為5，然后再計(jì)算磁條中心位置；③磁條超出了傳感器的感應(yīng)距離，傳感器丟線，此時(shí)沿用丟線之前的磁條中心位置作為當(dāng)前采樣周期的磁條中心位置，若連續(xù)丟線達(dá)到一定次數(shù)，則認(rèn)為AGV失去自導(dǎo)引能力。

2.2有效磁條選擇

AGV在行進(jìn)過程中，同一時(shí)刻可能會(huì)檢測到多塊磁條的存在：①車子處在無分叉路段，則會(huì)比較當(dāng)前所有磁條的中心位置，從多塊磁條中選擇中心位置最接近歷史位置的一塊磁條作為有效磁條；②AGV正在進(jìn)入分岔路段，那么其會(huì)根據(jù)RFID標(biāo)簽信號(hào)選擇最靠近分岔方向的那塊磁條作為有效磁條，并將其中心位置視為新檢測到的路徑中心位置。

2.3磁條中心位置均值濾波平滑

經(jīng)傳感器信號(hào)計(jì)算出來的磁條中心位置，需進(jìn)行平滑濾波，如此處理可屏蔽某些干擾信號(hào)，避免檢測到的路徑中心位置存在較大波動(dòng)，降低轉(zhuǎn)向輪的擺動(dòng)頻率與幅值，使車子行進(jìn)更加平穩(wěn)。具體方法是將當(dāng)前采樣周期檢測到的磁條中心位置與前幾次的采樣值取均值作為當(dāng)前磁條中心位置，即均值濾波。

3路徑跟蹤

路徑跟蹤過程是單片機(jī)接收RFID天線發(fā)送過來的RFID標(biāo)簽號(hào)，并據(jù)此判斷AGV當(dāng)前的位置，將要執(zhí)行的動(dòng)作(直行、后退、轉(zhuǎn)彎、加速、加速、負(fù)載、空載)，以及何種路徑跟蹤模式，并可依據(jù)橫向位置偏差e1和e2計(jì)算出轉(zhuǎn)向輪的轉(zhuǎn)向角。RFID即射頻識(shí)別，亦稱射頻識(shí)別技術(shù)或無線識(shí)別技術(shù)，可通過射頻信號(hào)獲取目標(biāo)信息，實(shí)現(xiàn)自動(dòng)識(shí)別[3]，無需人工接觸及光學(xué)可視即可完成信息輸入和處理[4]。轉(zhuǎn)向角的單位是°，取值區(qū)間為[-90°,90°]，轉(zhuǎn)向角以逆時(shí)針方向?yàn)檎D(zhuǎn)向輪軸線垂直于AGV縱向軸線時(shí)轉(zhuǎn)向角定為0°。AGV前進(jìn)或者后退等最基本的控制算法都是基于位置式數(shù)字PID控制，其輸出如下：

(1)

式中，Kp、Ki和Kd分別為比例系數(shù)、積分系數(shù)、微分系數(shù)，ej(0≤ej≤n)為第j次采樣所計(jì)算出的偏差，Un為第n次采樣偏差輸入后的控制器輸出。

3.1前進(jìn)路徑跟蹤

前進(jìn)路徑跟蹤采用的是分段模糊PID控制，在標(biāo)準(zhǔn)位置式數(shù)字PID的基礎(chǔ)上，以驅(qū)動(dòng)輪的轉(zhuǎn)速ω=2 000rpm為邊界，進(jìn)行高速段和低速段的劃分，高速PID作用于直道行走，低速PID控制AGV轉(zhuǎn)彎，以下所述ω的單位皆為rpm(轉(zhuǎn)/分鐘)。此外，基于前向位置偏差對(duì)PID中的比例、微分系數(shù)進(jìn)行模糊控制，實(shí)時(shí)動(dòng)態(tài)調(diào)節(jié)這二者的值[5]，使PID控制適應(yīng)不同的速度段與偏差范圍，基于經(jīng)驗(yàn)知識(shí)的模糊控制器不需要精確的數(shù)學(xué)模型且控制的魯棒性較好[6]。轉(zhuǎn)彎過程中，后向磁敏傳感器一直處于丟線狀態(tài)，而在走直道過程中依靠前向磁敏傳感器的信號(hào)就可以穩(wěn)定地控制AGV，因此前進(jìn)過程中只用到了前向16位磁敏傳感器。

3.1.1前進(jìn)模糊控制器

由表1可知，比例作用強(qiáng)度模糊量化為三級(jí)：強(qiáng)、中、弱。微分作用強(qiáng)度模糊量化為二級(jí)：強(qiáng)、弱。強(qiáng)度等級(jí)可用數(shù)值進(jìn)行表征，數(shù)值越大，相應(yīng)的作用強(qiáng)度越大。

表1　比例、微分作用強(qiáng)度模糊等級(jí)量化

圖2為比例作用強(qiáng)度隸屬度曲線圖，A(x)、B(x)、C(x)分別表示比例強(qiáng)作用、比例中作用、比例弱作用的隸屬度函數(shù)。圖3為微分作用強(qiáng)度隸屬度曲線圖，D(z)、E(z)分別表示微分強(qiáng)作用、微分弱作用的隸屬度函數(shù)。

圖2　比例作用強(qiáng)度隸屬度

圖3　微分作用強(qiáng)度隸屬度

模糊決策采用“均值評(píng)分原則”，即各級(jí)作用強(qiáng)度值與相應(yīng)的隸屬度的積的累加和，代入表1中高速段和低速段PID的系數(shù)，得到模糊控制器的最終輸出：

(2)

(3)

3.1.2前進(jìn)PID控制器

低速下，AGV的實(shí)際速度和電機(jī)輸出轉(zhuǎn)速之間偏差較大，尤其是在電機(jī)有輸出但是AGV未動(dòng)時(shí)，轉(zhuǎn)向角的積分值會(huì)迅速達(dá)到飽和狀態(tài)。而AGV一旦動(dòng)起來，轉(zhuǎn)向

輪就會(huì)往積分飽和的方向轉(zhuǎn)一個(gè)很大的角度，路徑偏差變大，并出現(xiàn)較大超調(diào)，系統(tǒng)穩(wěn)定性下降[7]。因此，低速時(shí)，一般通過減小積分系數(shù)(Ki)的方式來避免過度積分；而高速時(shí)，為了追求AGV行駛的平穩(wěn)性，積分項(xiàng)的變化率也不宜過大。本文不采取調(diào)整積分系數(shù)的方式來改變積分項(xiàng)的大小和變化速率，而是根據(jù)驅(qū)動(dòng)電機(jī)轉(zhuǎn)速輸出值的大小，調(diào)節(jié)前向位置累計(jì)偏差的衰減程度，前向位置累計(jì)偏差的計(jì)算式如下：

(4)

其中，k為衰減系數(shù)，sumn_e為從開始采樣到第n個(gè)采樣周期時(shí)的前向位置偏差累計(jì)值，sum(n-1)_e為從開始采樣到第(n-1)個(gè)采樣周期時(shí)的前向位置偏差累計(jì)值，k的取值如下：

(5)

其中，800rpm為電機(jī)的死區(qū)轉(zhuǎn)速，即當(dāng)電機(jī)在轉(zhuǎn)而AGV未行走時(shí)電機(jī)輸出的最大轉(zhuǎn)速。

上述方法可以有效減小前向位置累計(jì)偏差的變化速率，但是并不能防止因?yàn)榉e分項(xiàng)而出現(xiàn)的超調(diào)，而且AGV在行進(jìn)過程中，車輪與地面的滑動(dòng)以及重心偏移產(chǎn)生的慣性負(fù)載，都會(huì)導(dǎo)致前向位置累計(jì)偏差的變化，可能會(huì)引起瞬時(shí)超調(diào)[8]，因此，對(duì)積分項(xiàng)的輸出要進(jìn)行限幅:

(6)

前進(jìn)控制PID的積分系數(shù)恒定且Ki=0.023，結(jié)合式(2)～(6)的結(jié)果，可得PID控制器的輸出如下：

3.2后退路徑跟蹤

后退路徑跟蹤使用的是固定系數(shù)的雙閉環(huán)PID控制器，內(nèi)環(huán)為AGV姿態(tài)角控制環(huán)，外環(huán)為AGV后向8位磁敏傳感器的后向位置控制環(huán)。叉車式AGV的后退是為了取貨和放貨，其路徑是直道，因?yàn)锳GV轉(zhuǎn)向輪在前，叉車車身長2 m，僅靠后向8位磁敏傳感器采集的磁條信息無法控制叉車直線行走，故需要前向磁敏傳感器配合進(jìn)行姿態(tài)角調(diào)節(jié)。如圖4所示，后退過程中后向位置控制環(huán)基于后向位置偏差的大小，輸出預(yù)設(shè)姿態(tài)角，預(yù)設(shè)姿態(tài)角的值不一定為0(兩磁敏傳感器的中心線平行于AGV縱軸線)。如果偏差較大，如AGV過于靠近磁條右邊時(shí)，需要修正，則給予轉(zhuǎn)向輪一個(gè)正的轉(zhuǎn)向角，即視輸出的預(yù)設(shè)姿態(tài)角為正，而姿態(tài)角控制環(huán)則根據(jù)前向位置偏差和后向位置偏差，計(jì)算出AGV當(dāng)前的姿態(tài)角，根據(jù)預(yù)設(shè)姿態(tài)角與當(dāng)前姿態(tài)角的偏差給出轉(zhuǎn)向輪的轉(zhuǎn)向角。姿態(tài)角的表征方程如下：

(8)

x1和x2分別為前向和后向磁敏傳感器在后退過程中經(jīng)采樣和濾波后所得的磁條中心位置，X和Y分別為在后退過程中預(yù)設(shè)路徑中心位置對(duì)應(yīng)于前、后向磁敏傳感器上的位置，D表示兩磁敏傳感器的中心距離。

圖4　后退路徑跟蹤雙閉環(huán)PID控制框圖

4試驗(yàn)結(jié)果

為直觀呈現(xiàn)AGV實(shí)際行走路徑與預(yù)設(shè)路徑的吻合度，在布置好的磁條線路上進(jìn)行測試。所用磁條寬度為3 cm，磁條被磁敏傳感器完全覆蓋時(shí)，磁敏傳感器感應(yīng)到磁信號(hào)的位數(shù)恒為5。小車走直道、小車過彎道的示意圖略——編者注。

前進(jìn)過程中預(yù)設(shè)路徑中心對(duì)應(yīng)于前向磁敏傳感器上的位置為7，圖5為前進(jìn)過程中，前向磁敏傳感器采集磁條信息后，經(jīng)計(jì)算所得的實(shí)際路徑中心與時(shí)間的關(guān)系圖。

圖5　AGV前進(jìn)路徑中心關(guān)系圖

由圖5可知，實(shí)際路徑中心與預(yù)設(shè)路徑中心在大部分時(shí)間內(nèi)基本吻合，誤差在10 mm以內(nèi)，折線密集的地方是AGV處于轉(zhuǎn)彎狀態(tài)，在轉(zhuǎn)彎的過程中軌跡方向時(shí)刻變化，導(dǎo)致AGV的前向位置偏差一直存在且處于變化狀態(tài)[9]。圖5中初始階段偏差較大，這是由于外力把AGV移動(dòng)到導(dǎo)引路徑上時(shí)，磁條中心位置和前向磁敏傳感器的上的位置7相距較遠(yuǎn)，經(jīng)過一段時(shí)間調(diào)整后，前向磁敏傳感器感知的路徑中心位置穩(wěn)定在預(yù)設(shè)路徑中心附近。

圖6為AGV后退走直道的過程，預(yù)設(shè)路徑中心對(duì)應(yīng)于后向磁敏傳感器上的位置為4，對(duì)應(yīng)于前向磁敏傳感器上的位置為7。由圖6可知，AGV尋跡效果良好，誤差在10 mm以內(nèi),折線密集的地方是因?yàn)榇艞l沒有貼直，導(dǎo)致AGV轉(zhuǎn)向輪擺動(dòng)頻率過高。

圖6　AGV后退路徑中心關(guān)系圖

結(jié)語

總體上，在走直道過程中，AGV會(huì)有輕微震蕩，橫向位置偏差在10 mm以內(nèi)。過彎道時(shí)，通過AGV減速，并增大PID系數(shù)，可使橫向位置偏差穩(wěn)定在10 mm以內(nèi)，但震蕩頻率相對(duì)走直道略高。控制器以STM32F103ZET6單片機(jī)為核心，相對(duì)于工業(yè)控制器具有低功耗的優(yōu)勢[10]，同時(shí)充分利用了單片機(jī)方便靈活、編程方式容易實(shí)現(xiàn)智能控制算法的優(yōu)點(diǎn)。從現(xiàn)場試驗(yàn)的結(jié)果來看，本文設(shè)計(jì)的集模糊控制與PID控制于一體的算法尋跡效果良好，為磁導(dǎo)引叉車式AGV的自動(dòng)控制提供了一種可供參考的方法。

參考文獻(xiàn)

[1] 關(guān)宏.AGV整體集成系統(tǒng)結(jié)構(gòu)設(shè)計(jì)[J] .物流技術(shù)，2004(4)：37-38.

[2] 耿牛牛.單舵輪AGV路徑跟蹤方法的研究[J] .制造業(yè)自動(dòng)化，2011,33(4)：81-84.

[3] 游戰(zhàn)清，李蘇劍.無線射頻識(shí)別技術(shù)(RFID)理論與應(yīng)用[M] .北京：電子工業(yè)出版社，2004.

[4] 周曉光，王曉華，王偉.射頻識(shí)別(RFID)系統(tǒng)設(shè)計(jì)、仿真與應(yīng)用[M] .北京：人民郵電出版社，2008.

[5] 宋長會(huì).移動(dòng)機(jī)器人控制系統(tǒng)設(shè)計(jì)與研究[D] .哈爾濱:哈爾濱工程大學(xué),2006.

[6] 廖華麗，周祥，董豐，等.基于模糊控制的AGV循跡算法[J] .哈爾濱工業(yè)大學(xué)學(xué)報(bào).2005,37(7)：896-898.

[7] 陳盈.磁導(dǎo)引自動(dòng)導(dǎo)航小車控制系統(tǒng)的設(shè)計(jì)[D] .武漢：湖北工業(yè)大學(xué)，2013.

[8] 程遠(yuǎn).基于模糊控制的技術(shù)的AGV運(yùn)動(dòng)控制器設(shè)計(jì)[J] .無錫職業(yè)技術(shù)學(xué)院學(xué)報(bào),2012,11(5)：41-44.

[9] 黃胄.基于PLC控制的AGV技術(shù)研究及其應(yīng)用[D] .上海：華東理工大學(xué)，2013.

[10] 鄭炳坤，賴乙宗，葉峰.磁導(dǎo)航AGV控制的設(shè)計(jì)與實(shí)現(xiàn)[J] .自動(dòng)化與儀表,2014(3)：6-10.

石含飛(碩士研究生)，研究方向?yàn)榍度胧较到y(tǒng)應(yīng)用、AGV控制系統(tǒng)技術(shù)開發(fā)；何勇靈(教授)，研究方向?yàn)槠囯娮榆浻布刂萍夹g(shù)、工業(yè)機(jī)器人技術(shù)。

Navigation Algorithm of Automated Guided Forklift Based on STM32F103ZET6※

Shi Hanfei,He Yongling

(School of Transportation Science and Engineering,Beihang University,Beijing 100191,China)

Abstract：Taking the STM32F103ZET6 MCU controller as the core of system,the sensor array design is layout for the magnetic sensor group tracking forklift AGV.According to the layout of sensors,tracing constraints and walking requirements,the identification path and tracking algorithm are proposed.AGV is controlled by segmentation fuzzy PID controller during the forward process.PD coefficient is dynamically adjusted by the fuzzy controller to adapt to different speeds of driving wheel and different size of forward position deviation.Double-loop PID controller is applied during the reverse process.The gesture angle and backward position are inner ring and outer ring respectively.In actual walking tests,the path deviation maintains less than 10 mm when AGV walks stably.

Key words:STM32F103ZET6;AGV;magnetic navigation;fuzzy control;PID control

收稿日期：(責(zé)任編輯：楊迪娜2015-07-06)

中圖分類號(hào):TP24

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