基于模糊PD 算法的競速小車過彎策略研究

謝智陽，龐項文

（1.河源職業(yè)技術(shù)學(xué)院 機(jī)電工程學(xué)院，廣東 河源 517000；2.河源市工業(yè)機(jī)器人技術(shù)應(yīng)用工程技術(shù)研究中心，廣東 河源 517000）

競速小車按尋跡傳感器進(jìn)行分類大致包含光電傳感器導(dǎo)航［1］、電磁導(dǎo)航［2］、CCD 攝像頭導(dǎo)航［3］和激光雷達(dá)導(dǎo)航［4］.光電傳感器導(dǎo)航有成本低廉、易于實現(xiàn)等優(yōu)點，一直以來都是簡易尋跡小車的研究熱點.競速小車的成績除受尋跡傳感器的影響之外，尤為重要的是其控制算法的優(yōu)劣會在競速賽中起到關(guān)鍵性的作用，然而小車在直道行駛中很難體現(xiàn)出各自的差距，因此研究如何快速通過彎道是算法設(shè)計的重點.

文獻(xiàn)［5］研究了在停車過程中采用了梯形規(guī)劃曲線的停車策略，初步解決了停車過程中減速距離短、平穩(wěn)性差的問題；文獻(xiàn)［6］采用強(qiáng)化學(xué)習(xí)的方法在一定程度上解決了智能車路徑認(rèn)知的問題；文獻(xiàn)［7-8］在智能車避障和多工況方面作了較深入的研究.鑒于前人對過彎策略研究的較少，可借鑒的經(jīng)驗很難從文獻(xiàn)中找到，且缺乏系統(tǒng)性的研究策略.筆者以開源的Arduino UNO 單片機(jī)為控制核心構(gòu)建了競速小車，并用固定參數(shù)的PID 算法和模糊PD 算法進(jìn)行了系統(tǒng)性的過彎策略研究.

1 系統(tǒng)總體設(shè)計方案

系統(tǒng)主要由Arduino UNO 單片機(jī)、八路光電傳感器探測模塊、陀螺儀模塊、紅外避障傳感器、電機(jī)驅(qū)動模塊和轉(zhuǎn)向系統(tǒng)組成.Arduino UNO 單片機(jī)為主控模塊，負(fù)責(zé)采集各種傳感器的輸入信號和控制算法的運算；八路光電傳感器探測模塊負(fù)責(zé)探測小車相對于賽道的偏離情況，并將探測結(jié)果反饋至主控單片機(jī)；陀螺儀模塊負(fù)責(zé)探測小車的傾斜角，用以判斷小車是處在平道、上坡和下坡3 種狀態(tài)中的何種狀態(tài)；賽道終點處設(shè)有障礙物，當(dāng)紅外避障傳感器探測到前方有障礙物時，實現(xiàn)停車功能.

尋跡模塊采用TCRC5000 紅外傳感器，紅外傳感器采集到路徑信息通過比較器后直接傳到MCU 進(jìn)行處理，同時JY901 陀螺儀捕捉角度信號輸入MCU，MCU 統(tǒng)一將各傳感器輸入數(shù)據(jù)進(jìn)行綜合判斷、運算，并輸出信號控制舵機(jī)及電機(jī)驅(qū)動模塊BTS7970.

圖1 系統(tǒng)硬件結(jié)構(gòu)

2 硬件電路系統(tǒng)設(shè)計

2.1 電機(jī)驅(qū)動模塊設(shè)計

電機(jī)驅(qū)動模塊選用BTS7970，該電機(jī)驅(qū)動模塊具有功率大、性能穩(wěn)定、體積緊湊等特點，非常適合小車的電機(jī)驅(qū)動，其原理見圖2.

圖2 單路電機(jī)驅(qū)動原理

2.2 尋跡模塊的設(shè)計

八路光電傳感器探測模塊選用TCRC5000 紅外模塊，此模塊集成度高、布局靈活、接線簡易.為保證小車具有較好的前瞻性，尋跡模塊安裝在距小車前車輪9～10 cm 處.安裝位置距離過長會導(dǎo)致小車上坡時與賽道發(fā)生剮蹭，到達(dá)坡頂時易丟失信號；距離過短會導(dǎo)致實時性不強(qiáng)，速度受限.探測模塊共由八組紅外光電傳感器組成，根據(jù)本賽道的實際寬度，每組紅外光電傳感器的間隔為2.5 mm，具體布局見圖3，若改變賽道黑線的實際寬度，則需重新調(diào)整紅外光電傳感器的間距.

圖3 八路光電傳感器探測模塊布局

3 軟件系統(tǒng)的設(shè)計

圖4 軟件系統(tǒng)結(jié)構(gòu)

3.1 軟件總體設(shè)計方案

在結(jié)合文獻(xiàn)［9-12］的基礎(chǔ)上，設(shè)計并優(yōu)化本系統(tǒng)的控制策略，系統(tǒng)總體執(zhí)行流程見圖4.系統(tǒng)軟件流程包含常規(guī)模式和模糊PD 模式，常規(guī)模式執(zhí)行經(jīng)典的固定參數(shù)PID 算法，模糊PD 模式則執(zhí)行參數(shù)根據(jù)賽道實際情況參數(shù)可變的模糊PD 算法，且引入陀螺儀傳感器，用以實現(xiàn)坡度檢測功能，能有效防止小車因車速過快沖出賽道.

3.2 尋跡模塊輸入信號映射

TCRC5000 紅外模塊是開關(guān)量輸入，即壓黑線為低電平，沒壓黑線為高電平，因此只需讀取相關(guān)IO 端口的電平狀態(tài)便可獲知相關(guān)傳感器是否壓到黑線，如想把零亂的8 個傳感器的輸入值反饋至控制算法中計算就必須對傳感器輸入的值進(jìn)行組合量化排序.首先對與傳感器連接的IO 端口進(jìn)行初 始化，把IO 口設(shè)置成輸入模式以讀取傳感器輸入的高低電平信號，用for循環(huán)將8 組傳感器的值存至一個八位數(shù)組里，并按表1 的映射關(guān)系進(jìn)行映射，映射前，尋跡模塊的探測精度為1/8，映射后的探測精度為1/15，在不增加硬件成本的情況下，將探測精度提升一倍，有效保證競速小車行駛的穩(wěn)定性.

3.3 差速策略

小車驅(qū)動電機(jī)采用BTS7970 電機(jī)驅(qū)動進(jìn)行驅(qū)動，該電機(jī)驅(qū)動模塊引出4 個引腳IN1～I(xiàn)N4 供用戶操作，IN1 和IN2 控制左輪，IN3 和IN4 控制右輪.本文重點研究了如何在不脫離賽道的前提下快速通過彎道，處理方式主要是直道以正常速度行駛，在進(jìn)入彎道時進(jìn)行減速同時給予兩個輪相應(yīng)的差速以模擬真車的差速器來達(dá)到過彎時更順暢快速，彎道的減速處理將傳感器位置和電機(jī)進(jìn)行關(guān)聯(lián)，具體策略見公式（1）.

公式（1）中V 為期望速度，即小車在該賽道上的上限速度；Kp為比例參數(shù)；error 為小車的方向偏離值.小車無偏離時，偏離值為0，誤差隨黑線偏離中間傳感器距離增大而增大，一邊誤差為正一邊為負(fù)具體偏離值的映射值見表1.

表1 傳感器映射關(guān)系

表2 有差速的對應(yīng)

表3 無差速的誤差

由表2 和表3 可知，有無差速的各個誤差的左右電機(jī)的PWM 對應(yīng)關(guān)系，其差速與誤差成一定的線性關(guān)系，會根據(jù)賽道的路況的變化而變化，具有一定的適應(yīng)性，性能表現(xiàn)會比一般的等速過彎有顯著的提升.

3.4 固定參數(shù)PID 算法實驗

PID 參數(shù)整定過程是理論與工程實踐經(jīng)驗相互結(jié)合的過程，實驗前需結(jié)合小車模型大致推算出PID 參數(shù)的范圍，工程實踐中需反復(fù)調(diào)試參數(shù)進(jìn)行實驗分析，從而得到最佳的PID 參數(shù)組合.

PID 的經(jīng)典公式為：

由圖5 和PID 的經(jīng)典公式可知，經(jīng)典的PID 控制算法由期望值、反饋值、比例項調(diào)節(jié)參數(shù)、積分項調(diào)節(jié)參數(shù)、微分項調(diào)節(jié)參數(shù)和輸出值構(gòu)成.

由經(jīng)典的PID 公式得出本文的數(shù)字PID 公式（3）.

由公式（3）和圖5 可知，PID 的輸入為誤差error、輸出為舵機(jī)PWM 的關(guān)聯(lián)值out，此值經(jīng)換算后為舵機(jī)的PWM 值.

圖5 本文數(shù)字PID 框架

第一步，整定比例參數(shù)Kp，此時微分參數(shù)和積分參數(shù)為零，即out=Kp*error，實驗結(jié)果見圖6.

由圖6 可知Kp為7 時，整個系統(tǒng)的反應(yīng)速度快，但震蕩也較大，且震蕩頻率會相對較高.

Kp為2 時，整個系統(tǒng)的反應(yīng)速度會變得非常緩慢，但到了引導(dǎo)線后波動不大，該參數(shù)的應(yīng)變能力不強(qiáng)，不適合用于賽道.

本文采用比較折中比例參數(shù)，即Kp為4，從圖6 可看到該參數(shù)在震蕩和系統(tǒng)響應(yīng)速度上都比較理想.

經(jīng)實驗分析，比例參數(shù)相對較大時，系統(tǒng)反應(yīng)會迅速，但系統(tǒng)的震蕩也會很大，誤差會在期望值間來回震蕩，整個系統(tǒng)顯得很不穩(wěn)定且效率低下，并不理想.而把比例值調(diào)到相對較小時，系統(tǒng)的反應(yīng)就會相對緩慢，越小越慢，由于比例值小所以到了期望值后它的震蕩不會很明顯，但響應(yīng)遲鈍.增大Kp則響應(yīng)快但震蕩大，減小Kp則震蕩小但響應(yīng)慢，這是一個矛盾體，因此經(jīng)典的PID 算法在Kp的基礎(chǔ)上引入積分參數(shù)，當(dāng)Kp調(diào)到系統(tǒng)適中的時候，固定Kp，緊接著調(diào)試Ki，具體見公式（4）.

圖6 比例值大小效果對比

公式（4）中的error_sum 是誤差進(jìn)行自我累加的結(jié)果，如果進(jìn)行操作后誤差還沒消除，error_sum 便會愈加愈大，操控的力度也會愈大，直到消除誤差，它便會漸漸減小，直至為0.積分參數(shù)在消除靜態(tài)誤差效果比較好，如果誤差較大且短時間內(nèi)修正不好的話，error_sum 便會累加到系統(tǒng)的極限，會超過相應(yīng)的硬件的執(zhí)行極限，導(dǎo)致系統(tǒng)崩潰，此時要進(jìn)行積分分離處理，操作相對復(fù)雜.對此，選擇屏蔽積分參數(shù)，即Ki=0.

其實只引入比例項參數(shù)Kp時，已經(jīng)得到初步的尋跡效果，但想得到更好的過彎的效果，便要引入微分項參數(shù)Kd.本文重點對引入微分項參數(shù)Kd進(jìn)行實驗分析，見公式（5），通過實驗得到最佳的微分項參數(shù)Kd.

在Kp的基礎(chǔ)上引入微分項參數(shù)Kd會起到預(yù)測變化、抑制誤差增大的作用，彌補(bǔ)Kp比例的缺陷.微分簡要的說是把當(dāng)前誤差與上次誤差進(jìn)行比較，來得出它的變化曲線、變化規(guī)律，再施加一定的阻尼力，讓誤差接近0 時減小調(diào)節(jié)力度，使小車可從震蕩中恢復(fù)過來，增加動態(tài)穩(wěn)定性，提高小車的抗干擾性.

3.5 模糊PD 控制器設(shè)計

由于賽道路況較復(fù)雜，若想獲得極佳的過彎效果，固定參數(shù)的PID 算法還不足以適應(yīng)復(fù)雜的賽道情況，為了得到更快的過彎速度，需對PID 算法參數(shù)進(jìn)行簡單的模糊化.經(jīng)過多次實驗分析，本文重點對PD參數(shù)進(jìn)行實驗整定，模糊控制器總體框架見圖7.

模糊控制器的輸入量為誤差e 與誤差的變化率ec，輸出為PD 控制器的比例參數(shù)Kp和微分參數(shù)Kd的語言變量，本文的Kp、Kd的模糊子集為{負(fù)、零、正}，模糊規(guī)則見表4，Z1 為激進(jìn)PD 參數(shù)，Z2 為中等偏激進(jìn)PD 參數(shù)，Z3 為中等折中PD 參數(shù)，Z4 為保守PD 參數(shù)，參數(shù)再根據(jù)計算和經(jīng)驗進(jìn)行賦值.

圖7 模糊控制器架構(gòu)

4 實驗結(jié)果分析

單獨運用固定參數(shù)的PID 算法并不能很好的適應(yīng)賽道，在長直道入彎的時候由于慣性的作用使小車偏離賽道甚至有沖出賽道的可能，且過彎后恢復(fù)穩(wěn)定的時間過長，并不適合該賽道使用，因此本文在固定參數(shù)的PID 的基礎(chǔ)上加入了模糊規(guī)則，使系統(tǒng)具有一定的適應(yīng)性.根據(jù)圖8 的實驗結(jié)果可知，相比于固定參數(shù)的PID 算法，模糊PD 算法能穩(wěn)定、快速的通過彎道，且小車擺動幅度較小.

表4 模糊規(guī)則表

圖8 模糊后的小車過彎行徑

5 結(jié)論

（1）設(shè)計了由Arduino UNO 單片機(jī)、八路光電傳感器探測模塊、陀螺儀模塊、紅外避障傳感器、電機(jī)驅(qū)動模塊和轉(zhuǎn)向系統(tǒng)組成的競速小車.

（2）筆者分別從比例算法、固定參數(shù)PD 算法和模糊PD 算法進(jìn)行了系統(tǒng)性的實驗研究.實驗結(jié)果表明，固定比例算法、固定參數(shù)的PD 算法在直道上與模糊PD 算法并無明顯差異，而在彎道上模糊PD 算法明顯優(yōu)于固定參數(shù)PD 算法和比例算法.同時，給出了詳細(xì)的調(diào)參過程和實驗數(shù)據(jù)分析，為競速小車過彎策略研究提供了重要的試驗經(jīng)驗.