基于STM32的AGV分段式PID磁導航方案研究

林義忠，望 翔，張 勤

（廣西大學機械工程學院，廣西 南寧530004）

0 引言

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

PLC因其電氣特性穩定、編程簡單等特性通常作為傳統磁導航AGV的控制器，但相較于STM32單片機，其運算資源較弱，無法實現較為復雜的導航算法。本文從導航效果、成本等方面考慮采用了磁條導航，STM32F103C8T6為AGV控制器及分段式PID導航算法的導航方案。

AGV導航方案主要包括驅動單元和磁導航糾偏單元，所以本文首先闡述AGV的基本結構，其次分析磁導航傳感器在STM32F103上的實現形式。然后分析AGV運動學模型，推導出本文AGV糾偏傳遞函數模型為PID控制算法提供了設計依據。最后在實際磁條軌道上測試本文AGV的實際運行效果，以證明該種導航方案的可行性。

1 AGV本體結構及導航控制系統方案

1.1 AGV本體結構

本文AGV的本體結構示意圖如圖1所示，底盤采用六輪式結構，兩驅動輪位于車體中間，車體前后兩側各有兩個從動輪。AGV動力部分由兩個步進電機驅動單元組成，兩步進電機差速驅動AGV的直行、轉向。

圖1 AGV本體結構圖

1.2 磁導航硬件方案

本文控制器核心為一塊芯片為STM32F103開發板，該芯片是以Cortex-M3為內核基礎改進，相比CM3降低了功耗，中斷響應大幅度提高[3]。該芯片有144的針腳，其中多達80個GPIO口，選擇作為磁導航AGV的主控制器。

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

1.3 驅動單元硬件方案

驅動單元采用了兩個86系列步進電機，具備閉環步進的功能，即在驅動器-電機-驅動器之間構成“小閉環”，由控制器下發給驅動器的多個脈沖能夠由驅動器確保電機轉過脈沖指令對應的實際角度，從而改善了傳統步進電機“丟步”的情形。如圖2所示，STM32的通用GPIO口連接驅動器脈沖輸入口，根據外糾偏信息，通過高級定時器給驅動器發送可變頻率脈沖信號，驅動器驅動電機至目標轉速，從而達到驅動AGV的目的。

圖2 運動控制硬件方案框圖

2 糾偏系統建模及分析

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

2.1 差速運動模型

從動輪對AGV不產生任何實際的運動學約束，因此建立運動學模型時可以將AGV簡化為兩輪式結構，僅考慮兩驅動輪情況。根據圖1結構圖簡化之后的運動學模型如圖3所示，本文選取AGV兩驅動輪連線中點A作為局部坐標系原點，建立X1-Y1坐標系。

圖3 差速運動學模型分析圖

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

推導出AGV差速驅動運動學模型為：

通常易獲取左右驅動輪角速度和，根據剛體平面運動理論可得到將驅動輪角速度作為輸入量的差速運動模型：

從AGV的運動學模型即式（4）可知通過控制兩驅動輪輪速即可改變AGV的整體運行速度和角速度，從而對AGV在全局坐標系中的位姿進行改變，使AGV能夠在二維平面內向任意位置運動。

2.2 偏差模型分析

機器人運動實際是一個離散系統，可認為目標路徑由一系列目標點或目標位姿組成，在一個采樣時刻，實際位姿與目標位姿之差即為AGV的偏差。具有固定路徑的AGV運動模型如圖4所示，VL，VR，VA則代表AGV左驅動輪，右驅動輪以及AGV中心點A點的線速度，R為小車瞬時轉向半徑，同時可得如下運動關系。

圖4 路徑跟蹤糾偏分析圖

當AGV與目標路徑出現位姿偏差時，在A點會出現X和Y方向的分速度：

由圖4所示幾何關系，計算中心點A在時刻的坐標如下：

本文AGV所用磁傳感器內部由間距為10mm的16個霍爾元件組成，其感應到磁信號時都會在對應位置輸出高電平信號，在AGV底部共布置2個距離為D磁傳感器，如圖5所示。

圖5 磁導航傳感器布置圖

在AGV偏差未超過測量范圍時，可以根據圖5所示幾何關系得出中心點的偏距和偏角?？芍爸么艂鞲衅髦悬cB與兩驅動輪中點A點始終相距D/2。在測得AGV的前、后磁傳感器讀數分別為ed1、ed2后，偏轉方向則由圖示前置磁傳感器信號即ed1決定，則可得：

將式（3～11）代入式（12），可得點 B 在時刻的偏差形式如下：

則由系統辨識法可得一般AGV糾偏模型[3]的傳遞函數為：

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

該章節在運動模型的基礎上建立了AGV包含角度、距離的偏差模型，得到AGV較為理想的驅動系統數學模型傳遞函數，為小車的路徑跟蹤算法研究及驗證提供理論基礎。

3 PID糾偏控制器設計

本文AGV的糾偏控制流程如圖6所示，由2部分構成：（1）經典PID控制器：該控制器直接對被控制對象進行閉環控制，并且3個參數KP、KI、KD為在線控制；（2）PID參數選擇模塊。

圖6 糾偏控制流程圖

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

式（16）中，ek為第 k 次采樣值，KP、KI、KD分別為比例、積分、微分環節系數。

實驗測試結果及PID參數整定理論[4]表明傳統PID控制在偏差過大或過小時的控制效果不佳，因此本文采用工程整定法即反復實驗比較不同參數的效果，得到PID參數對控制系統影響效果如表1所示。

表1 PID參數表

AGV在出現偏差較大的情況時，比如在轉彎半徑較小位置過彎，小車左右擺動情況比較明顯，甚至可能出現丟失信號情況。對此本文提出用分段式PID控制算法，對不同范圍偏差PID過程選取對應參數，能夠在快速響應前提下減弱系統震蕩現象，分段式PID控制流程如圖7所示。

圖7 分式PID控制流程圖

在MATLAB中進行仿真，得到階躍響應曲線如圖8所示。與傳統PID控制相比，分段式PID在不同偏差階段采用對應控制參數，比傳統PID控制能更快進入穩態且超調量更低，從而提高了系統的動態和靜態性能。

圖8 兩種PID控制方式階躍響應曲線圖

4 實驗

為測試本文AGV導航方案的實際可行性與效果，進行了現場實驗，在AGV中間安裝偏差記錄裝置，磁條軌道上布置20 cm*3 cm的白色紙條記錄測量實際偏差值，直行軌道每邊布置11個，轉彎處2個，共記52個。實驗過程讓AGV以0.5 m/s速度沿磁條運行5圈共260個數據，統計偏差結果如表2所示。

圖9 AGV實驗測試圖

表2 AGV運行5圈偏差數據統計

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

5 結束語

傳統室內AGV導航方案通常使用PLC配合單磁傳感器，無法獲取運行時的位姿以及不能加入較為復雜糾偏算法，會對糾偏準確性產生影響。本文利用兩個磁傳感器獲取AGV實時位姿，STM32F103C8T6作為控制器，實現了將位姿偏差作為分段式PID控制輸入量的導航算法。實驗結果表明該種控制及導航方案效果較好，且整套控制系統軟硬件成本均較低，對AGV導航控制方案設計有一定參考價值。