電磁智能小車(chē)控制算法研究與編程實(shí)現(xiàn)

李全民 賈林鋒

（廣州理工學(xué)院，廣東 廣州 510540）

1 引言

智能汽車(chē)是未來(lái)汽車(chē)的發(fā)展方向，在減少交通事故、發(fā)展自動(dòng)化技術(shù)、提高舒適性等許多方面發(fā)揮很重要的作用[1]，而電磁導(dǎo)航技術(shù)相對(duì)于其他技術(shù)有穩(wěn)定性強(qiáng)、成本低且原理簡(jiǎn)單等特點(diǎn)。電磁導(dǎo)航是指在道路中線(xiàn)鋪設(shè)有漆包線(xiàn)并通有20kHz、100mA交變電流，因而在導(dǎo)線(xiàn)周?chē)a(chǎn)生磁場(chǎng)，智能小車(chē)上的電感傳感器在磁場(chǎng)中產(chǎn)生感應(yīng)電動(dòng)勢(shì)，靠檢測(cè)電動(dòng)勢(shì)值來(lái)確定小車(chē)在道路上相對(duì)位置。算法和編程是智能汽車(chē)控制核心，決定了小車(chē)的行駛速度和方向。本文在信號(hào)歸一化處理、差比和算法、速度控制等方面對(duì)傳統(tǒng)控制方法進(jìn)行改進(jìn)，使智能小車(chē)控制更加線(xiàn)性化，小車(chē)的運(yùn)行狀態(tài)也更穩(wěn)定和高速，從而優(yōu)化了控制策略，并且實(shí)現(xiàn)了C 語(yǔ)言編程。最后完成的智能小車(chē)實(shí)現(xiàn)了自動(dòng)起跑與停車(chē)、上下坡、沿道路自動(dòng)行駛及轉(zhuǎn)向等多種功能。

2 智能小車(chē)硬件支持

控制算法建立在硬件基礎(chǔ)之上，一般智能小車(chē)硬件包括機(jī)械系統(tǒng)、含有最小系統(tǒng)的主板、電源模塊、電機(jī)控制模塊、信號(hào)檢測(cè)與處理模塊等部分。在信號(hào)檢測(cè)和處理部分，本文采用兩個(gè)10MH工字電感作為傳感器，電感采用對(duì)稱(chēng)于小車(chē)中線(xiàn)的水平放置[2]，且其軸線(xiàn)與漆包線(xiàn)垂直，離地高度15cm，距離中線(xiàn)12cm；采集的感應(yīng)電動(dòng)勢(shì)經(jīng)過(guò)LC選頻和OPA2350運(yùn)算放大后傳給單片機(jī)AD接口；單片機(jī)采用由恩智浦公司生產(chǎn)的S9KEAZ128AMLK，它具有32 位MCU 內(nèi)核、48MHz的CPU 頻率、12 位16 通道的模數(shù)轉(zhuǎn)換（ADC）及一個(gè)脈寬調(diào)制（PWM）定時(shí)器[3]，由輸出的PWM信號(hào)控制電機(jī)轉(zhuǎn)速，進(jìn)而控制智能小車(chē)的方向和速度。

3 控制算法的軟件流程

經(jīng)過(guò)單片機(jī)模數(shù)轉(zhuǎn)換后，兩個(gè)水平電感產(chǎn)生的感應(yīng)電動(dòng)勢(shì)模擬信號(hào)分別轉(zhuǎn)換成數(shù)字信號(hào)進(jìn)行編程計(jì)算。為了防止干擾，首先采用軟件均值濾波進(jìn)行第一步的信號(hào)處理，對(duì)每個(gè)電感產(chǎn)生的感應(yīng)電動(dòng)勢(shì)連續(xù)采集10次并求平均值作為當(dāng)前控制周期的傳感器信號(hào)；其次通過(guò)歸一化算法處理兩電感產(chǎn)生的電動(dòng)勢(shì)值，使其數(shù)字信號(hào)在一定范圍內(nèi)變化，如0～200；再次，對(duì)歸一化處理后數(shù)值進(jìn)行差比和計(jì)算，得到小車(chē)相對(duì)于道路中線(xiàn)的相對(duì)位置和偏差距離；最后將偏差輸入PID控制器，得到當(dāng)前控制周期的輸出。方向控制通常利用單片機(jī)的定時(shí)器接口，輸出PWM 信號(hào)對(duì)電機(jī)轉(zhuǎn)速進(jìn)行控制（本文研究的是三輪小車(chē)，通過(guò)兩后輪的轉(zhuǎn)速差來(lái)控制行駛方向）。

在速度控制方面，首先通過(guò)編碼器采集目前速度信號(hào)，并輸入單片機(jī)后轉(zhuǎn)化為脈沖計(jì)數(shù)；然后判斷直道還是彎道，前者高速后者低速；最后，將設(shè)定速度輸入PID控制器，得到當(dāng)前控制周期的速度輸出，通過(guò)PWM 信號(hào)對(duì)電機(jī)進(jìn)行速度控制。

控制流程如圖1 所示，其中，中斷子程序每1ms 執(zhí)行一次，方向每2.5ms 控制一次，速度控制是每10ms 控制一次，速度控制的時(shí)間相對(duì)長(zhǎng)一點(diǎn)，主要為了降低對(duì)方向控制的影響。因?yàn)閷?duì)于三輪車(chē)來(lái)說(shuō)，前輪是沒(méi)有動(dòng)力的隨動(dòng)輪，不論是方向控制還是速度控制，都是通過(guò)改變兩后輪轉(zhuǎn)速來(lái)實(shí)現(xiàn)的。

圖1 程序控制流程圖

4 歸一化算法設(shè)計(jì)

4.1 傳統(tǒng)歸一化算法

歸一化算法的目的是為了解決電感因加工質(zhì)量不同帶來(lái)的誤差，將電感傳感器輸出的感應(yīng)電動(dòng)勢(shì)的絕對(duì)值（因?yàn)镃 語(yǔ)言定義時(shí)數(shù)據(jù)類(lèi)型為無(wú)符號(hào)型，故這里只討論絕對(duì)值）轉(zhuǎn)換成相對(duì)值，以便于同時(shí)處理多個(gè)電動(dòng)勢(shì)數(shù)據(jù)。傳統(tǒng)的歸一化算法如公式1所示。

其中a表示電感歸一后的值；b表示每次平均濾波后得到的值；c是轉(zhuǎn)換系數(shù)，取決于設(shè)定的電動(dòng)勢(shì)參數(shù)范圍，本文參數(shù)范圍設(shè)在0～200，故c取200；公式中的Max和Min是單個(gè)電感在工作環(huán)境下采集到的最大電動(dòng)勢(shì)值和最小電動(dòng)勢(shì)值。歸一化算法只能對(duì)單個(gè)電感進(jìn)行計(jì)算，每個(gè)電感的Max和Min都不同，這取決于運(yùn)算放大電路的增益倍數(shù)和電感的加工質(zhì)量。一般在使用歸一化算法前，都需要預(yù)先采集每個(gè)電感輸出電動(dòng)勢(shì)的最大值和最小值。在實(shí)際的操作中，往往需要人工移動(dòng)電感遠(yuǎn)離和靠近漆包線(xiàn)來(lái)取值，不僅操作繁瑣，而且預(yù)先采集時(shí)電感高度與小車(chē)行駛時(shí)的實(shí)際高度也有誤差。

4.2 改進(jìn)的歸一化算法

對(duì)傳統(tǒng)歸一化算法進(jìn)行分析可以發(fā)現(xiàn)，預(yù)先采集的最大最小值是感應(yīng)電動(dòng)勢(shì)在當(dāng)前磁場(chǎng)環(huán)境的變化區(qū)間，而每一次采集到的值都與最小值相減并與區(qū)間長(zhǎng)度相除。上述的處理方法實(shí)質(zhì)上是將電動(dòng)勢(shì)實(shí)際變化值轉(zhuǎn)換成相對(duì)于最小值的一個(gè)變化率，利用了變化率來(lái)解決電感加工質(zhì)量帶來(lái)的誤差。

現(xiàn)假設(shè)小車(chē)先直線(xiàn)行駛?cè)缓筠D(zhuǎn)彎，小車(chē)每次轉(zhuǎn)向產(chǎn)生的電動(dòng)勢(shì)變化值都是根據(jù)偏離中線(xiàn)的距離得來(lái)的，因而電動(dòng)勢(shì)的變化值則反映了小車(chē)偏離中線(xiàn)的距離。因此可以把小車(chē)處于直道中心位置時(shí)的電動(dòng)勢(shì)值作為中值，行駛后采集到的值與中值相減并算出相對(duì)誤差，將原先相對(duì)于最小值的變化率轉(zhuǎn)換成了相對(duì)于中值的變化率，歸一化算法如下：

其中△d表示單個(gè)電感產(chǎn)生的電動(dòng)勢(shì)相對(duì)于中值的變化率；Z是歸一化前采集到的小車(chē)位于跑道中間的電感中值；b是每次采集后平均濾波的電感值；c是轉(zhuǎn)換系數(shù)，表示偏差變化的范圍。在實(shí)際操作時(shí)只需要將小車(chē)放在跑道中間就能上電自動(dòng)采集，用單次采集代替多次采集，操作上簡(jiǎn)單了好多，也較好解決了高度誤差帶來(lái)的采集精度問(wèn)題。

4.3 算法改進(jìn)的編程實(shí)現(xiàn)

由于小車(chē)的左右轉(zhuǎn)向，電動(dòng)勢(shì)值會(huì)在中值左右變化，因此△d值也會(huì)出現(xiàn)負(fù)值，可以在歸一后加上一個(gè)起點(diǎn)變化系數(shù)來(lái)避免負(fù)值，便于直接用于計(jì)算。C 語(yǔ)言代碼實(shí)現(xiàn)如圖2所示，110是電感的起點(diǎn)變化系數(shù)，通過(guò)限幅使感應(yīng)電動(dòng)勢(shì)的變化率在0～200范圍內(nèi)。

圖2 改進(jìn)歸一化C語(yǔ)言代碼實(shí)現(xiàn)

5 差比和算法改進(jìn)設(shè)計(jì)

5.1 傳統(tǒng)的差比和控制方法

單個(gè)電感輸出的電動(dòng)勢(shì)變化可以反映小車(chē)偏離中線(xiàn)的距離變化，但不能判斷偏離中線(xiàn)的左右方向。因此在循跡中至少需要分別位于小車(chē)中線(xiàn)左右側(cè)的兩個(gè)電感，利用兩個(gè)感應(yīng)電動(dòng)勢(shì)的差值來(lái)判斷方向。在實(shí)際布置時(shí)，為了對(duì)道路進(jìn)行提前預(yù)測(cè)，電感一般靠支架布置在距小車(chē)頭部前沿還有一段距離的前部。因此當(dāng)小車(chē)轉(zhuǎn)彎時(shí)，有可能兩個(gè)電感都位于道路外面，兩個(gè)電感的感應(yīng)電動(dòng)勢(shì)都同時(shí)變小，其絕對(duì)誤差也變得更小，不能達(dá)到驅(qū)動(dòng)小車(chē)轉(zhuǎn)向的要求，所以引進(jìn)了差比和的概念，其算法如下：

其中e表示計(jì)算后的相對(duì)誤差；L和R分別表示左右電感歸一后的電動(dòng)勢(shì)值；c是系數(shù)，決定相對(duì)誤差的變化范圍。實(shí)際上即使這樣處理，當(dāng)轉(zhuǎn)彎的角度很大時(shí)e還是很小，不能滿(mǎn)足大轉(zhuǎn)向要求，這時(shí)就需要丟線(xiàn)處理（又稱(chēng)“鎖差值”），給小車(chē)一恒定的大偏差轉(zhuǎn)向（舵機(jī)打死）。

這種處理方法比較簡(jiǎn)單，但存在兩個(gè)問(wèn)題：一是控制不線(xiàn)性，在丟線(xiàn)之前，輸入和輸出通過(guò)差比和控制，但丟線(xiàn)后，輸出達(dá)到最大，輸入不對(duì)這個(gè)控制系統(tǒng)有任何影響，所以小車(chē)會(huì)出現(xiàn)出彎抖動(dòng)、入彎反應(yīng)慢等現(xiàn)象；二是當(dāng)小車(chē)直線(xiàn)行駛但稍微偏離中線(xiàn)時(shí)，絕對(duì)誤差有可能比相對(duì)誤差還要大，這時(shí)就應(yīng)用絕對(duì)誤差而不是相對(duì)誤差。

5.2 改進(jìn)后的差比和控制方法

為了解決使用單一差比和及丟線(xiàn)處理存在的問(wèn)題，本文進(jìn)行了兩個(gè)改進(jìn)：一是使公式（3）中的系數(shù)c取值在L與R最大和的范圍內(nèi)，當(dāng)兩電感絕對(duì)誤差的值大于其差比和時(shí)采用絕對(duì)誤差作為小車(chē)的偏差；當(dāng)兩電感絕對(duì)誤差的值小于其差比和時(shí)采用差比和作為小車(chē)的偏差，這樣就解決了單一使用差比和問(wèn)題。二是把絕對(duì)誤差轉(zhuǎn)入差比和時(shí)的絕對(duì)誤差值保存起來(lái)，該值作為后續(xù)差比和公式中的分子并保持不變，每次循環(huán)只改變分母值（即兩電動(dòng)勢(shì)和值），使兩電感在入彎時(shí)的差比和值不斷增大，滿(mǎn)足轉(zhuǎn)彎需要；當(dāng)出彎時(shí)兩電感向中線(xiàn)靠近，其電動(dòng)勢(shì)值變大，作為兩者之和的分母也變大，差比和值減小，達(dá)到判斷條件后再由差比和轉(zhuǎn)入絕對(duì)誤差，這樣就解決了入彎和出彎時(shí)控制不線(xiàn)性問(wèn)題。把改進(jìn)后算法與和PID 控制器的結(jié)合，小車(chē)入彎、彎內(nèi)和出彎姿態(tài)都得到了良好的調(diào)整。

5.3 改進(jìn)后的編程實(shí)現(xiàn)

改進(jìn)后算法的C 語(yǔ)言代碼實(shí)現(xiàn)如圖3 所示，其中為了避免分母為零的情況出現(xiàn)，當(dāng)兩電動(dòng)勢(shì)值之和很小時(shí)，分母自動(dòng)加1。

圖3 改進(jìn)差比和算法的C語(yǔ)言實(shí)現(xiàn)

6 速度控制

6.1 速度控制策略

速度控制重點(diǎn)在彎道，本文采用直道高速、入彎減速、出彎加速的控制策略，并預(yù)先設(shè)置直道、入彎和彎道內(nèi)三種不同速度，作為小車(chē)不同姿態(tài)下速度PID控制器的期望值。由于單靠編碼器無(wú)法判斷三種速度的轉(zhuǎn)換時(shí)機(jī)，所以另外引入D_1 和D_2 兩個(gè)變量：D_1 是當(dāng)前兩電感電動(dòng)勢(shì)差值減去上次相應(yīng)的差值，D_2是D_1的累加并對(duì)其限幅。由差值變化規(guī)律可知，直道時(shí)由于方向PID 控制器的調(diào)整，偏差波動(dòng)范圍小，D_1和D_2均較??；入彎時(shí)差值不斷增大，D_1為正數(shù)，D_2系數(shù)不斷增大至限幅值；在彎道內(nèi)D_2系數(shù)由于入彎前的差值累加，數(shù)值較大，但D_1 在彎道內(nèi)的偏差變化不大數(shù)值較小；小車(chē)出彎時(shí)，差值不斷減小，D_2系數(shù)值降低。上述控制策略結(jié)合了方向的偏差變化，對(duì)小車(chē)整體的速度有極大的提高，C語(yǔ)言程序?qū)崿F(xiàn)如圖4所示，程序中的參數(shù)影響速度轉(zhuǎn)換的靈敏度，可由實(shí)際測(cè)試得到。

圖4 速度控制策略C語(yǔ)言實(shí)現(xiàn)

6.2 Bang-Bang 控制

在速度控制策略基礎(chǔ)上，本文還采用了增量式PID+Bang-Bang 控制，C 語(yǔ)言實(shí)現(xiàn)如圖5 所示。其中KOUPLUSMAX是設(shè)定的最大占空比，PSpeedCut是當(dāng)前檢測(cè)到的速度，ASPeed1是期望速度，程序里Bang-Bang控制區(qū)間為“-3”和“+1”，可由實(shí)際測(cè)試得到。當(dāng)前速度遠(yuǎn)小于期望速度時(shí)，給最大占空比；當(dāng)前速度超調(diào)一段距離時(shí)，給最小占空比；在這個(gè)區(qū)間內(nèi)則使用增量式PID。這樣可以使小車(chē)速度反應(yīng)更靈敏，勻速性能更好。

圖5 增量式PID+Bang-Bang控制C語(yǔ)言實(shí)現(xiàn)

7 結(jié)語(yǔ)

本文涉及的歸一化算法、差比和算法、速度控制及Bang-Bang控制都是電磁智能小車(chē)控制的基礎(chǔ)，結(jié)合PID控制器的原理，將計(jì)算后的偏差作為控制器的輸入使得小車(chē)控制系統(tǒng)得到較大的改善。對(duì)于單根漆包線(xiàn)鋪設(shè)的磁場(chǎng)環(huán)境，該算法的魯棒性好、計(jì)算簡(jiǎn)便。對(duì)于多跟漆包線(xiàn)組成的跑道，如圓環(huán)等，可以增加多個(gè)電感輔助上述算法的路徑判斷。

最后在包括直道、彎道、十字交叉、環(huán)島、坡道和顛簸等元素的道路上實(shí)地測(cè)試表明，改進(jìn)后智能小車(chē)的速度提高了20%，并且在入彎、彎內(nèi)和出彎都顯示出良好的穩(wěn)定姿態(tài)，說(shuō)明該算法能大大提高智能小車(chē)的速度和穩(wěn)定性，可操作性強(qiáng)。