采用粒子群算法的AGV路徑跟蹤控制研究

彭慕蓉，肖本賢

（1.合肥工業(yè)大學(xué)工業(yè)與裝備技術(shù)研究院，安徽 合肥 230009；2.合肥工業(yè)大學(xué)電氣與自動(dòng)化工程學(xué)院，安徽 合肥 230009）

1 引言

近年來(lái)，隨著自動(dòng)化智能化技術(shù)的提高，自動(dòng)導(dǎo)引小車(chē)（AutomatedGuided Vehicle，AGV）在各個(gè)領(lǐng)域得到了廣泛應(yīng)用。例如在倉(cāng)庫(kù)和碼頭等場(chǎng)所，用自動(dòng)導(dǎo)引叉車(chē)代替?zhèn)鹘y(tǒng)的人工駕駛叉車(chē)，不但節(jié)約了人力和成本，而且提高了運(yùn)輸效率[1]。常見(jiàn)的導(dǎo)引方式有激光導(dǎo)引，磁帶導(dǎo)引[2]等，其中自由路徑導(dǎo)引方式，定位精確，路徑可變換，靈活性高，能適用各種場(chǎng)合。由于這種導(dǎo)引方式下的指令路徑是一條虛擬路徑，車(chē)輛在行駛中容易受到外界因素干擾而與理想路徑之間出現(xiàn)偏差，嚴(yán)重時(shí)可能導(dǎo)致車(chē)輛偏離航線。當(dāng)車(chē)輛偏離路徑時(shí)如何快速穩(wěn)定的回歸航線，以及在正常行駛時(shí)保持對(duì)指定路徑的跟蹤具有重要的意義。

目前大多數(shù)自動(dòng)導(dǎo)引車(chē)輛均采用PID控制策略，PID控制具有控制方法簡(jiǎn)單易操作的特點(diǎn)，但是PID控制算法依賴于參數(shù)的精確性，在復(fù)雜情況下參數(shù)難以整定，存在適應(yīng)性差和精確度低等缺點(diǎn)。文獻(xiàn)[3]設(shè)計(jì)了基于線性二次型最的最優(yōu)控制器，該控制器需要把控制對(duì)象簡(jiǎn)化為線性系統(tǒng)，在控制模型精確的情況下控制精度較高，但在外接干擾情況下魯棒性較差，穩(wěn)定度不高。文獻(xiàn)[4]采用遺傳算法實(shí)現(xiàn)了路徑跟蹤的效果，具有抗干擾強(qiáng)，檢測(cè)速度快等特點(diǎn)，但遺傳算法編程實(shí)現(xiàn)比較復(fù)雜，計(jì)算量大，算法搜索速度慢等缺點(diǎn)。粒子群算法與遺傳算法相似，和遺傳算法相比[5]，沒(méi)有交叉和變異等步驟，參數(shù)較少，計(jì)算速度快，易于實(shí)現(xiàn)。

文獻(xiàn)[6]采用粒子群算法實(shí)現(xiàn)了AGV的路徑跟蹤，但穩(wěn)定性差，收斂速度慢，誤差較大，沒(méi)有考慮到粒子迭代過(guò)程中搜索能力強(qiáng)弱的問(wèn)題。文獻(xiàn)[7]采用粒子群算法與模糊控制相結(jié)合的方法提高了收斂速度，根據(jù)時(shí)間乘誤差絕對(duì)值積分準(zhǔn)則（Integral time absolute error，ITAE）建立了適應(yīng)度函數(shù)，但忽略了車(chē)輛行駛中角度偏差與橫向偏差的比例大小，只能在偏差小的情況下消除誤差，無(wú)法應(yīng)對(duì)各種工況，路徑跟蹤能力不足。

針對(duì)上述問(wèn)題，建立了AGV行駛?cè)肿鴺?biāo)系和局部坐標(biāo)系，在簡(jiǎn)化車(chē)輛運(yùn)動(dòng)模型的基礎(chǔ)上，以消除角度偏差和橫向距離偏差為目的，采用粒子群算法設(shè)計(jì)控制系統(tǒng)。考慮到粒子搜索能力的問(wèn)題和適應(yīng)度函數(shù)對(duì)算法的影響，采用線性遞減慣性權(quán)重粒子群算法，能在更新過(guò)程中改變粒子搜索能力，能有效加強(qiáng)粒子搜索能力，提高收斂速度，設(shè)計(jì)一種隨角度偏差與橫向距離偏差所占比重變化而變化的適應(yīng)度函數(shù)，提高車(chē)輛回歸理想路徑的速度和穩(wěn)定度。

2 AGV路徑跟蹤系統(tǒng)

研究的AGV共有四個(gè)車(chē)輪，其中前輪分別由兩個(gè)電機(jī)驅(qū)動(dòng)控制，后輪為萬(wàn)向輪。AGV通過(guò)改變速度與方向角即可實(shí)現(xiàn)轉(zhuǎn)彎等運(yùn)動(dòng)，對(duì)于采用差速驅(qū)動(dòng)的AGV，通過(guò)調(diào)節(jié)左右驅(qū)動(dòng)輪的輪速便可實(shí)現(xiàn)車(chē)輛各種運(yùn)動(dòng)方式。

AGV路徑跟蹤系統(tǒng)結(jié)構(gòu)，如圖1所示。該系統(tǒng)包括粒子群算法控制模塊，左右輪驅(qū)動(dòng)模塊，AGV運(yùn)動(dòng)模塊。對(duì)指定路徑進(jìn)行跟蹤，實(shí)質(zhì)是消除小車(chē)在行駛過(guò)程中與指定路徑的角度偏差和橫向距離偏差，所以將這兩者作為控制系統(tǒng)的輸入，將能夠驅(qū)動(dòng)電機(jī)消除偏差的電壓模擬量作為系統(tǒng)輸出；經(jīng)過(guò)驅(qū)動(dòng)系統(tǒng)得到AGV的輪速，AGV在輪速調(diào)節(jié)下向目標(biāo)路徑行駛。

圖1 AGV控制系統(tǒng)框圖Fig.1 AGV Control System Block Diagram

3 車(chē)輛運(yùn)動(dòng)系統(tǒng)模型

為了確定AGV的位姿，建立車(chē)輛與全局平面之間的聯(lián)系，簡(jiǎn)化運(yùn)動(dòng)模型，如圖2所示。

圖2 差速驅(qū)動(dòng)AGV簡(jiǎn)化運(yùn)動(dòng)模型Fig.2 Simplified Motion Model of Differential Drive AGV

建立全局坐標(biāo)系XOY，以車(chē)輛中軸線為x軸，驅(qū)動(dòng)輪中點(diǎn)C為原點(diǎn)，建立局部坐標(biāo)系xCy。車(chē)輛在全局坐標(biāo)系的位姿為（Xc，Yc，θ），圖中曲線為跟蹤的路徑，P（XP，YP）點(diǎn)為車(chē)輛跟蹤目標(biāo)點(diǎn)，為x軸與P點(diǎn)切線方向的夾角，d為C點(diǎn)到切線的距離。P點(diǎn)在xCy坐標(biāo)系中的坐標(biāo)表示為：

由圖2可知，路徑跟蹤實(shí)際上是車(chē)輛追趕目標(biāo)點(diǎn)的過(guò)程，就是是要消除與目標(biāo)點(diǎn)之間的角度偏差α與橫向距離偏差d。在坐標(biāo)系xCy中分析AGV行駛偏差，根據(jù)車(chē)輛結(jié)構(gòu)和運(yùn)動(dòng)學(xué)可知：

式中：L—左右輪距；ωr—車(chē)輛角速度；VL和VR—左右驅(qū)動(dòng)輪速度。

經(jīng)過(guò)時(shí)間Δt，車(chē)輛的角度改變量Δα和橫向距離偏差改變量Δd為：

設(shè)兩驅(qū)動(dòng)輪速度差為ΔV，得到：

要實(shí)現(xiàn)車(chē)輛自動(dòng)行駛，還應(yīng)知道AGV驅(qū)動(dòng)模塊如何運(yùn)轉(zhuǎn)，并建立能反映系統(tǒng)驅(qū)動(dòng)特性的模型，但AGV驅(qū)動(dòng)環(huán)節(jié)較多，各部件的性能參數(shù)難以確定，所以難以用解析法推導(dǎo)出其數(shù)學(xué)模型。采用系統(tǒng)辨識(shí)的方法，即通過(guò)測(cè)量系統(tǒng)輸入輸出量，根據(jù)相應(yīng)規(guī)則，估算出能反映系統(tǒng)屬性的數(shù)學(xué)模型?，F(xiàn)引入控制速度的糾偏電壓ΔU作為辨識(shí)系統(tǒng)的輸入，根據(jù)文獻(xiàn)[8]，選擇調(diào)整時(shí)間和周期信號(hào)，采用最小二乘法進(jìn)行系統(tǒng)辨識(shí)，通過(guò)Matlab軟件編程并進(jìn)行降階處理后得到傳遞函。

微分方程為：

式中：ΔV—糾偏速度。

將電壓模擬量作為輸入量，驅(qū)動(dòng)輪速度做狀態(tài)變量，輸出為角度偏差和橫向距離偏差，車(chē)輛控制框圖，如圖3所示。

圖3 運(yùn)動(dòng)控制系統(tǒng)框圖Fig.3 Motion Control System Block Diagram

整個(gè)系統(tǒng)狀態(tài)方程為：

4 改進(jìn)粒子群算法優(yōu)化的控制系統(tǒng)

現(xiàn)階段對(duì)車(chē)輛行駛穩(wěn)定性，安全性，快速響應(yīng)提出了更高的要求，用傳統(tǒng)的控制方法進(jìn)行軌跡跟蹤，往往具有較大的誤差，無(wú)法滿足性能要求，粒子群算法（Particle Swarm Optimization）是一種基于種群的智能優(yōu)化算法[9]，具有快速收斂，計(jì)算簡(jiǎn)單，精度高等優(yōu)點(diǎn)，應(yīng)用于軌跡跟蹤時(shí)能快速有效的達(dá)到減小誤差的目的。

4.1 改進(jìn)的粒子群位置更新方法

AGV行駛環(huán)境復(fù)雜，穩(wěn)定性要求高，在軌跡跟蹤中需要有快速響應(yīng)的性能?；玖Ｗ尤核惴ㄖ械牧Ｗ痈聲r(shí)，其慣性權(quán)重系數(shù)ω是一個(gè)常數(shù)，在面對(duì)復(fù)雜問(wèn)題時(shí)，后期容易陷入局部極值，無(wú)法滿足快速收斂的要求，采用線性遞減的慣性權(quán)重系數(shù)對(duì)上述缺點(diǎn)進(jìn)行改進(jìn)。慣性權(quán)重ω是粒子群算法中的重要參數(shù)，可以用來(lái)控制算法的探索能力。有研究表明，較大的ω能有效提高全局搜索能力，增加種群多樣性，能夠跳出局部極值，較小的ω能提高算法的局部搜索能力，加快收斂速度。線性遞減的慣性權(quán)重系數(shù)，根據(jù)種群更新次數(shù)對(duì)ω進(jìn)行動(dòng)態(tài)調(diào)節(jié)，在算法前期ω取較大值，確保種群粒子更加豐富，搜索區(qū)域更加廣泛，隨著迭代次數(shù)增加，逐漸減小ω值，使算法后期具有較強(qiáng)局部搜索能力，能夠快速的找到最優(yōu)解。

式中：ωmax—最大慣性權(quán)重；ωmin—最小慣性權(quán)重；Tmax—種群最大更新次數(shù)；t—當(dāng)前種群更新次數(shù)。

設(shè)計(jì)的路徑跟蹤粒子群控制方案中，求解問(wèn)題為消除行駛偏差，問(wèn)題的解為能夠驅(qū)動(dòng)左右輪消除偏差的電壓。將問(wèn)題的解集抽象為一個(gè)種群［x1，x2，…，xm］，種群中的每個(gè)粒子代表一組電壓值。每個(gè)粒子的位置用矢量 xi=（xi1，xi2，…，xiD）表示，每個(gè)粒子的速度用矢量 vi=（vi1，vi2，…，viD）表示。粒子 i到目前經(jīng)歷的最好位置為 pbesti=（pbesti1，pbesti2，…，pbestiD），種群目前搜索到的最優(yōu)位置為 gbest=（gbest1，gbest2，…，gbestD）。粒子的速度和位置更新公式如下：

式中：i=1，2，…，m；ω—慣性權(quán)重；c1和 c2—學(xué)習(xí)因子；r1和 r2—［0，1］之間的隨機(jī)數(shù)；D—粒子的維數(shù)；k—當(dāng)前迭代次數(shù)。

4.2 適應(yīng)度函數(shù)的建立

為了判斷粒子每次迭代后的位置優(yōu)劣，需要一個(gè)評(píng)判標(biāo)準(zhǔn)，故建立適應(yīng)度函數(shù)。由于ITAE準(zhǔn)則[10]設(shè)計(jì)的系統(tǒng)具有超調(diào)量小，動(dòng)態(tài)性能好等特點(diǎn)，所以采用角度偏差和橫向距離偏差的ITAE之和作為目標(biāo)函數(shù)。

式中：ω1和ω2—比例系數(shù)，決定角度偏差和距離偏差在控制過(guò)程中所占的比重；α（t）和 d（t）—t時(shí)刻的航向偏差和距離偏差。

在車(chē)輛行駛過(guò)程中，橫向偏差和航向角度偏差不斷地發(fā)生變化，常規(guī)的適應(yīng)度函數(shù)中兩種偏差采用固定比重，這種調(diào)節(jié)方式無(wú)法適應(yīng)各種工況，針對(duì)這一問(wèn)題，對(duì)目標(biāo)函數(shù)進(jìn)行改進(jìn)，設(shè)計(jì)一種根據(jù)兩種行駛偏差實(shí)時(shí)改變比重系數(shù)的動(dòng)態(tài)目標(biāo)函數(shù)。在t時(shí)刻車(chē)輛的橫向偏差為dt，角度偏差為αt，車(chē)輛行駛允許的橫向偏差和角度偏差為 αC和αC，設(shè) k1=dt/dC，k2=αt/αC，分別表示 t時(shí)刻的位置偏差dt和αt角度偏差與允許的位置偏差與角度偏差的比，則該時(shí)刻的目標(biāo)函數(shù)中加權(quán)系數(shù)為：

由于f反應(yīng)的是車(chē)輛與理想路徑的偏差，所以f越小，則說(shuō)明該粒子越優(yōu)。

5 系統(tǒng)仿真及結(jié)果

為了驗(yàn)證上文提出的改進(jìn)粒子群算法對(duì)路徑跟蹤的糾偏效果，在Matlab/simulink模塊進(jìn)行仿真實(shí)驗(yàn)。選取粒子種群規(guī)模m=30，粒子最大迭代次數(shù)T=100，學(xué)習(xí)因子C1=C2=2，車(chē)輛輪距L=1.8m，車(chē)輛行駛速度為1m/s。首先進(jìn)行慣性權(quán)重對(duì)算法的影響對(duì)比，車(chē)輛與理想路徑的初始角度偏差為10°，初始橫向偏差距離為10cm。設(shè)標(biāo)準(zhǔn)粒子群算法的慣性權(quán)重系數(shù)為ω=0.9，改進(jìn)后慣性權(quán)重最大值為ωmax=0.9，慣性權(quán)重最小值為ωmin=0.4，在不同的慣性權(quán)重影響下，目標(biāo)函數(shù)的適應(yīng)度值變化曲線，如圖4所示。

圖4 適應(yīng)度值曲線Fig.4 Fitness Curve

圖中：ω=0.9的情況下，種群迭代第17次開(kāi)始，適應(yīng)度值長(zhǎng)時(shí)間沒(méi)有變化，隨著迭代次數(shù)的增加，遲遲不能跳出局部極值，直到迭代第60次時(shí)才擺脫這個(gè)困境而收斂。在ω隨迭代次數(shù)線性變化的情況下，種群迭代至20次時(shí)收斂，大大減少了種群迭代次數(shù)。仿真結(jié)果表明，變化的慣性權(quán)重系數(shù)可以提高粒子的搜索能力，防止粒子陷入早熟。其次在上文的基礎(chǔ)上，對(duì)采用固定適應(yīng)度函數(shù)和改進(jìn)適應(yīng)度函數(shù)時(shí)，車(chē)輛消除橫向偏差與航向偏差的能力進(jìn)行仿真實(shí)驗(yàn)。設(shè)適應(yīng)度函數(shù)比重系數(shù)ω1=0.7，ω2=0.3。車(chē)輛與理想路徑的初始偏差為［10°，10cm］。兩種適應(yīng)度函數(shù)下車(chē)輛行駛橫向距離偏差與角度偏差仿真圖形，如圖5所示。

圖5 距離偏差與航向偏差響應(yīng)曲線Fig.5 Distance Deviation and Angle Deviation Response Curve

圖5 （a）顯示，在固定適應(yīng)度函數(shù)調(diào)節(jié)下，橫向偏差在［-7.2cm，10.4cm］范圍內(nèi)變化，車(chē)輛在回歸航線的過(guò)程中出現(xiàn)了多次過(guò)度行駛，這使車(chē)輛在指定路徑兩側(cè)進(jìn)行反復(fù)調(diào)節(jié)，增加了車(chē)輛行駛路程，產(chǎn)生較大波動(dòng)，消除橫向偏差總共需要1.51s。在改進(jìn)適應(yīng)度函數(shù)后，橫向偏差在［0cm，10.4cm］范圍內(nèi)變化，車(chē)輛消除橫向偏差總共需要0.8s。雖然兩種控制方法最終都達(dá)到了路徑跟蹤的目的，但是后者收斂時(shí)間更短，超調(diào)小，穩(wěn)定性高，具有更好的響應(yīng)特性。

圖5（b）反映了車(chē)輛回歸期望路徑過(guò)程中航向角的變化情況，在固定適應(yīng)度函數(shù)調(diào)節(jié)下，車(chē)輛的航向角在［-1rad，0.85rad］范圍內(nèi)變化，航向經(jīng)歷了9次左偏和右偏的改變，最大改變量為1.85rad，這會(huì)導(dǎo)致車(chē)輛左右擺動(dòng)，行駛穩(wěn)定性差，增加不安全的因素。在改進(jìn)適應(yīng)度函數(shù)后，航向角在［-0.47rad，0.17rad］范圍內(nèi)變化，并保持航向基本左偏，跟前者相比，具有調(diào)節(jié)時(shí)間短，收斂速度快，精度高的優(yōu)點(diǎn)，保證車(chē)輛安全穩(wěn)定行駛。

最后對(duì)AGV跟蹤路徑過(guò)程進(jìn)行模擬，建立環(huán)境坐標(biāo)系XOY，設(shè)理想路徑為一條直線，車(chē)輛行駛起點(diǎn)為（0，0），現(xiàn)在以初始偏差為［10cm，10°］，車(chē)速1m/s跟蹤理想路徑，仿真結(jié)果，如圖6所示。

將標(biāo)準(zhǔn)粒子群算法調(diào)節(jié)下和改進(jìn)粒子群算法調(diào)節(jié)下AGV消除偏差的能力進(jìn)行對(duì)比，通過(guò)改進(jìn)的控制方案能夠快速且穩(wěn)定的消除行駛中的誤差，具有較強(qiáng)的路徑跟蹤能力。

圖6 模擬軌跡跟蹤路徑圖Fig.6 Simulated Path Tracking Diagram

6 結(jié)論

對(duì)雙輪驅(qū)動(dòng)差速控制的AGV路徑跟蹤問(wèn)題進(jìn)行研究，基于橫向偏差和航向偏差兩者與速度關(guān)系建立運(yùn)動(dòng)模型，采用線性遞減慣性權(quán)重系數(shù)對(duì)粒子群算法進(jìn)行改進(jìn)，并結(jié)合AGV運(yùn)動(dòng)特點(diǎn)，提出變化的適應(yīng)度函數(shù)調(diào)節(jié)策略。仿真結(jié)果表明，采用粒子速度更新公式中慣性權(quán)重系數(shù)線性遞減的方法，可以提高粒子搜索速度，避免陷入局部極值；采用隨橫向偏差和航向偏差動(dòng)態(tài)變化的適應(yīng)度函數(shù)調(diào)節(jié)方法，與固定比例系數(shù)適應(yīng)度函數(shù)調(diào)節(jié)方法相比，調(diào)節(jié)時(shí)間段，超調(diào)量小，車(chē)輛行駛更加穩(wěn)定，安全，對(duì)路徑的跟蹤能力更強(qiáng)。