控制算法
電機(jī)控制算法的作用是接受指令速度值，通過運(yùn)算向電機(jī)提供適當(dāng)?shù)尿?qū)動(dòng)電壓，盡快地和盡快平穩(wěn)地使電機(jī)轉(zhuǎn)速達(dá)到指令速度值，并維持這個(gè)速度值。換言之，一旦電機(jī)轉(zhuǎn)速達(dá)到了指令速度值，即使在各種不利因素(如斜坡、碰撞之類等使電機(jī)轉(zhuǎn)速發(fā)生變化的因素)的干擾下也應(yīng)該保持速度值不變。為了提高機(jī)器人小車控制系統(tǒng)的控制精度，選用合適的控制算法顯得十分必要?？刂扑惴ㄊ侨魏伍]環(huán)系統(tǒng)控制方案的核心，然而并非越復(fù)雜、精度越高的算法越好，因?yàn)楸荣愐蠓浅８叩膶?shí)時(shí)性，機(jī)器人必須在非常短的時(shí)間內(nèi)作出靈敏的反應(yīng)，所以現(xiàn)代的一些先進(jìn)控制算法，比如模糊控制、神經(jīng)元網(wǎng)絡(luò)控制等就不能應(yīng)用到小車控制系統(tǒng)里。本系統(tǒng)選用了最常規(guī)、最經(jīng)典的PID控制算法，通過實(shí)際應(yīng)用取得了很好的效果。

1. 比例項(xiàng)
控制回路中的第一個(gè)偏差轉(zhuǎn)換環(huán)節(jié)就是比例項(xiàng)。這一環(huán)節(jié)簡單地將偏差信號乘以常數(shù)K 得到新的CV值(值域?yàn)?100~100)?；镜谋壤刂扑惴ㄈ缦拢?

loop:
PV=ReadMotorSpeed()
Error=SP-PV
CV=Error*Kprop
Setpwm(cv)
Goto loop
上一段程序中的SP表示預(yù)設(shè)目標(biāo)值，SetPWM()函數(shù)并非將CV值作為絕對的PWM占空比來對待。否則，不斷降低的偏差值會使輸出值接近零，而且由于電機(jī)工作時(shí)需要持續(xù)的PWM信號，控制系統(tǒng)將會使電機(jī)穩(wěn)定在低速運(yùn)轉(zhuǎn)狀態(tài)上，從而導(dǎo)致控制系統(tǒng)策略失敗。

相反，CV值一般被取作當(dāng)前PWM占空比的改變量，并被附加到當(dāng)前的PWM占空比上。這也要求SetPWM()函數(shù)必須將相加后得到的PWM占空比限制在0%~100%。正的CV值將使電機(jī)兩端電壓增加。負(fù)的CV值將使電機(jī)兩端電壓降低。如果CV值等于0，則無需改變當(dāng)前占空比。較低的K 值會使電機(jī)的速度響應(yīng)緩慢，但是卻很平穩(wěn)。較高的K 值會使速度響應(yīng)更快，但是卻可能導(dǎo)致超調(diào)，即達(dá)到穩(wěn)定輸出前在期望值附近振蕩。過高的K 值會導(dǎo)致系統(tǒng)的不穩(wěn)定，即輸出不斷震蕩且不會趨于期望值。

2. 微分項(xiàng)
任何變量的微分項(xiàng)被用來描述該變量是如何相對于另一個(gè)變量(多數(shù)是時(shí)間)變化的。換句話說，任何變量的微分項(xiàng)就是它隨時(shí)間的變化率。如位移隨時(shí)間的變化率是速度。速度相對于時(shí)間的微分是加速度。

在PID控制器中，值得關(guān)心的是偏差信號相對于時(shí)間的微分，或稱變化率。絕大多數(shù)控制器將微分項(xiàng)定義為：
R a t e = ( E t ? E t ? 1 ) T Rate=\frac{(E_t-E_{t-1} )}{T}
Rate=
T
(E
t

?E
t?1

)

式中，Et為當(dāng)前偏差，Et-1 為前次偏差值，T為兩次測量的時(shí)間間隔。負(fù)的變化率表明偏差信號的改善。當(dāng)微分項(xiàng)被具體應(yīng)用于控制器中時(shí)，將一個(gè)常數(shù)乘以該微分項(xiàng)，并將它加到比例項(xiàng)上，就可以得到最終的CV值計(jì)算公式：
C V = ( K p ? E ) + ( K d ? R a t e ) CV=( K_p*E)+( K_d*Rate)
CV=(K
p

?E)+(K
d

?Rate)

當(dāng)偏差信號接近零時(shí)，CV值將為負(fù)，所以當(dāng)偏差信號開始改善時(shí)，微分項(xiàng)的作用將逐漸減弱校正輸出量。在某些場合下，微分項(xiàng)還有利于超調(diào)量的消除，并可以允許使用較大的K 值，從而可以改善響應(yīng)的快速性。微分環(huán)節(jié)還預(yù)示了偏差信號的變化趨勢。當(dāng)控制對象對控制器的輸出響應(yīng)遲緩時(shí)，微分環(huán)節(jié)的作用尤為明顯。
含有微分項(xiàng)的控制算法的偽代碼實(shí)現(xiàn)如下：

loop:
PV=ReadMotorSpeed()
LastError=Error
Error=SP-PV
Rate=Error-LastError
CV=Error*Kprop+Krate*Rate
SetPWM(CV)
Goto loop
3. 積分項(xiàng)
積分正好與微分相對。假如有一個(gè)描述變化率(微分)的表達(dá)式，那么對該表達(dá)式的積分就將得到隨時(shí)間變化的原物理量。如加速度的積分是速度，速度的積分是位移。
在PID控制回路中，偏差的積分代表從控制開始時(shí)算起所有偏差積累的總和。該總和被常數(shù)K 所乘后再添加到回路輸出中。在回路中，如果沒有積分環(huán)節(jié)，盡管控制系統(tǒng)也會趨于穩(wěn)定，但是由于某種原因輸出值可能最終也無法達(dá)到SP值。
一個(gè)簡單但完全的PID控制器地偽代碼實(shí)現(xiàn)如下：

loop:
PV=ReadMotorSpeed()
LastError=Error
Isum=Isum+Error
Error=SP-PV
Rate=Error-LastError
CV=Error*Kprop+Krate*Rate+Kint*Isum
SetPWM(CV)
Goto loop
由于積分項(xiàng)會越來越大，這就會使控制回路在SP值的改變時(shí)響應(yīng)變慢，某些應(yīng)用場合在CV值達(dá)到取值邊界(如為：-100~100)時(shí)會停止累加Isum。在SP值改變時(shí)，也可以除去Isum項(xiàng)。