STM32F103ZET6單片機的叉車式AGV的尋跡算法※

石含飛，何勇靈

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

?

STM32F103ZET6單片機的叉車式AGV的尋跡算法※

石含飛，何勇靈

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

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

關鍵詞:STM32F103ZET6；AGV；磁導引；模糊控制；PID控制

引言

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

1AGV基本結構

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

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

圖1　叉車式AGV結構與磁敏傳感器布置陣列

2路徑識別

2.1磁條中心位置檢測

AGV在運動過程中，磁條與傳感器的相對位置會出現3種情形：①磁條被傳感器完全覆蓋，此時磁條中心位置是傳感器連續獲取信號的中心；②磁條處于傳感器邊緣，若此時傳感器上檢測到磁信號的位數少于3，則自動補齊傳感器外面的位，使有效位數為5，然后再計算磁條中心位置；③磁條超出了傳感器的感應距離，傳感器丟線，此時沿用丟線之前的磁條中心位置作為當前采樣周期的磁條中心位置，若連續丟線達到一定次數，則認為AGV失去自導引能力。

2.2有效磁條選擇

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

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

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

3路徑跟蹤

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

(1)

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

3.1前進路徑跟蹤

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

3.1.1前進模糊控制器

由表1可知，比例作用強度模糊量化為三級：強、中、弱。微分作用強度模糊量化為二級：強、弱。強度等級可用數值進行表征，數值越大，相應的作用強度越大。

表1　比例、微分作用強度模糊等級量化

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

圖2　比例作用強度隸屬度

圖3　微分作用強度隸屬度

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

(2)

(3)

3.1.2前進PID控制器

低速下，AGV的實際速度和電機輸出轉速之間偏差較大，尤其是在電機有輸出但是AGV未動時，轉向角的積分值會迅速達到飽和狀態。而AGV一旦動起來，轉向

輪就會往積分飽和的方向轉一個很大的角度，路徑偏差變大，并出現較大超調，系統穩定性下降[7]。因此，低速時，一般通過減小積分系數(Ki)的方式來避免過度積分；而高速時，為了追求AGV行駛的平穩性，積分項的變化率也不宜過大。本文不采取調整積分系數的方式來改變積分項的大小和變化速率，而是根據驅動電機轉速輸出值的大小，調節前向位置累計偏差的衰減程度，前向位置累計偏差的計算式如下：

(4)

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

(5)

其中，800rpm為電機的死區轉速，即當電機在轉而AGV未行走時電機輸出的最大轉速。

上述方法可以有效減小前向位置累計偏差的變化速率，但是并不能防止因為積分項而出現的超調，而且AGV在行進過程中，車輪與地面的滑動以及重心偏移產生的慣性負載，都會導致前向位置累計偏差的變化，可能會引起瞬時超調[8]，因此，對積分項的輸出要進行限幅:

(6)

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

3.2后退路徑跟蹤

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

(8)

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

圖4　后退路徑跟蹤雙閉環PID控制框圖

4試驗結果

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

前進過程中預設路徑中心對應于前向磁敏傳感器上的位置為7，圖5為前進過程中，前向磁敏傳感器采集磁條信息后，經計算所得的實際路徑中心與時間的關系圖。

圖5　AGV前進路徑中心關系圖

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

圖6為AGV后退走直道的過程，預設路徑中心對應于后向磁敏傳感器上的位置為4，對應于前向磁敏傳感器上的位置為7。由圖6可知，AGV尋跡效果良好，誤差在10 mm以內,折線密集的地方是因為磁條沒有貼直，導致AGV轉向輪擺動頻率過高。

圖6　AGV后退路徑中心關系圖

結語

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

參考文獻

[1] 關宏.AGV整體集成系統結構設計[J] .物流技術，2004(4)：37-38.

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

[3] 游戰清，李蘇劍.無線射頻識別技術(RFID)理論與應用[M] .北京：電子工業出版社，2004.

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

[5] 宋長會.移動機器人控制系統設計與研究[D] .哈爾濱:哈爾濱工程大學,2006.

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

[7] 陳盈.磁導引自動導航小車控制系統的設計[D] .武漢：湖北工業大學，2013.

[8] 程遠.基于模糊控制的技術的AGV運動控制器設計[J] .無錫職業技術學院學報,2012,11(5)：41-44.

[9] 黃胄.基于PLC控制的AGV技術研究及其應用[D] .上海：華東理工大學，2013.

[10] 鄭炳坤，賴乙宗，葉峰.磁導航AGV控制的設計與實現[J] .自動化與儀表,2014(3)：6-10.

石含飛(碩士研究生)，研究方向為嵌入式系統應用、AGV控制系統技術開發；何勇靈(教授)，研究方向為汽車電子軟硬件控制技術、工業機器人技術。

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

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

中圖分類號:TP24

文獻標識碼:A