智能車PID 算法實現(xiàn)原理講解

1、為了實現(xiàn)PID控制所需要的等間隔采樣，我們使用了一個定時中斷，每2ms進行一次數(shù)據(jù)采樣和PID計算。與此并行，系統(tǒng)中還設(shè)計了一個轉(zhuǎn)速脈沖檢測中斷，從而實現(xiàn)了轉(zhuǎn)速檢測。為了調(diào)試的需要，程序中還在main函數(shù)中加入了相關(guān)的調(diào)試代碼，這部分代碼有最低的優(yōu)先級，可以在保證不影響控制策略的情況下實現(xiàn)發(fā)送調(diào)試數(shù)據(jù)等功能。檢測環(huán)節(jié)對整個控制系統(tǒng)的質(zhì)量起到至關(guān)重要的作用4.3.2 PID控制調(diào)整速度 本系統(tǒng)采用的是增量式數(shù)字PID控制，通過每一控制周期（10ms）讀入脈沖數(shù)間接測得小車當(dāng)前轉(zhuǎn)速vi_FeedBack，將vi_FeedBack與模糊推理得到的小車期望速度vi_Ref比較，由以下公式求得速度偏差e

2、rror1與速度偏差率d_error。 error1 = vi_Ref vi_FeedBack; （公式3） d_error = error1 vi_PreError; （公式4）公式4中, vi_PreError為上次的速度偏差?？紤]到控制周期較長，假設(shè)按2.5m/s的平均速度計算，則一個控制周期小車大概可以跑過2.5cm，如果按這種周期用上述PID調(diào)節(jié)速度，則會導(dǎo)致加速減速均過長的后果，嚴重的影響小車的快速性和穩(wěn)定性。為了解決這個問題，可以在PID調(diào)速控制中加入BANG-BANG控制思想：根據(jù)error1的大小，如果正大，則正轉(zhuǎn)給全額占空比；如果負大，則自由停車或給一個反轉(zhuǎn)占空比；否則就采

3、用PID計算的占空比。 PID控制算法為了使賽車平滑得保持在黑線中央，即使賽車的偏移量平滑地保持在0，實用了PID控制算法。P為比例參數(shù)，D為微分參數(shù)?；鶞手禐?，PID輸入為水平偏移量X0，PID輸出為轉(zhuǎn)角，轉(zhuǎn)角方向：向左轉(zhuǎn)為正，向右轉(zhuǎn)為負。P參數(shù)在智能車控制器中表示水平偏差量的權(quán)，D參數(shù)在智能車控制器中表示水平偏差速度的權(quán)。水平偏差量直接反映了賽車偏離黑線的程度，例如賽車偏向黑線的左邊越厲害，則賽車的右轉(zhuǎn)角度將越大。水平偏差量，是PID控制器的P部分。水平偏差速度則直接反映了賽車的運動傾向，因為有了賽車的水平偏差速度，對賽車的掌握，將更加精確。例如賽車偏向黑線左邊，然而它的運動方向是向右的

4、，那么，他的轉(zhuǎn)角將比向左運動時的轉(zhuǎn)角要小，因為，我知道賽車已經(jīng)開始朝正確的方向調(diào)整了。水平偏差速度，是PID控制器的D部分。通過兩個相隔一定采樣時間的水平偏差量的差，來得到賽車的水平偏差速度。然而，這個時間間隔多少比較合適呢？ 圖3.3 上述函數(shù)圖像的橫坐標為采樣時間t，每小格為4ms。第1幅圖像的縱坐標為賽車水平偏差量；第2幅圖像的縱坐標為間隔為1的時候水平偏差速度；第3幅圖像的縱坐標為間隔為3時候水平偏差速度；第3幅圖像的縱坐標為間隔為5的時候的水平偏差速度。由上圖可知：相鄰采樣點越遠，數(shù)據(jù)的值域越大，更有利于描述車輛的偏差程度，但是，會降低賽車判斷的響應(yīng)度；相鄰采樣點越近，數(shù)據(jù)的值域越小

5、，0狀態(tài)越多，不利于描述賽車的偏差程度，但是有利于響應(yīng)賽車的偏差程度，所以，這個相鄰的數(shù)量要適中。上圖中，間隔3比較合適。Proportion為的P參數(shù)，Derivative為的D參數(shù)，LastError為的水平偏差量隊列，水平偏差速度即隊尾-隊頭，隊列長度為LAST_ERROR_NUM，即表示間間隔。通過實驗，長度20比較合適。有了偏差Error，有了dError=LastError0- LastErrorLAST_ERROR_NUM，輸出值為 PID_Output = Proportion * Error + Derivative * dError。3.6 PID控制算法的改進通過實驗和軟

6、件仿真，發(fā)現(xiàn)，PID控制器并不是在任何情況下，都是最優(yōu)的選擇，比如，在直線上，PID控制器的調(diào)整時間，遠大于枚舉調(diào)整法，而在嚴重偏離的時候，由于賽車的水平偏移量已經(jīng)固定不變，所以，也沒有必要使用PID控制了。所以，將賽車的狀態(tài)分成了3種，下面逐一介紹并說明智能車在這一狀態(tài)下的控制算法。1：賽車處于直線狀態(tài)：如果賽車居中，則轉(zhuǎn)角=0。如果賽車在右邊，則轉(zhuǎn)角=2。如果賽車在左邊，則轉(zhuǎn)角=-2。電機占空比均為最高。2：賽車處于嚴重偏離狀態(tài)：如果賽車嚴重偏左，則賽車右轉(zhuǎn)最大角度。如果賽車嚴重偏右，則賽車左轉(zhuǎn)最大角度。電機占空比均反向，表示剎車。3：賽車處于一般彎道狀態(tài)（即賽車有偏移，但能檢測到黑線）：

7、則將賽車的水平偏移量作為PID控制器的輸入，然后經(jīng)過PID運算，得出的值為轉(zhuǎn)角的映射。最后，通過一個一次函數(shù)，將輸出映射成轉(zhuǎn)角的值即可。對于電機的控制，抽象出3種情況。第1種，是穩(wěn)定過彎，即水平偏移速度很小，這時候，采用加速過完的策略。第2種，是極不穩(wěn)定的情況，即水平偏移速度很大，這時候，采用剎車策略。第3種，是一般情況，此時，采用勻速通過轉(zhuǎn)彎的策略。實驗證明，這種抽象可以使賽車適應(yīng)幾乎任何曲率半徑小于500mm的彎道。模擬量的采集和PD控制模擬量的采集和位置、角度計模擬量采集傳感器仍然是紅外光耦傳感器，接收管輸出不經(jīng)過施密特觸發(fā)器轉(zhuǎn)化成數(shù)字量，而是接到單片機的A/D轉(zhuǎn)換接口進行轉(zhuǎn)換。位置計算

8、全局求位置法通過各個傳感器的信號可以計算出連續(xù)的黑線位置，較簡單的辦法是用類似于長桿求重心的方法，將各個傳感器輸出分別乘以傳感器位置再求和，除以所有傳感器輸出之和即可得到連續(xù)的位置信息。這之前需要做一些準備工作，由于不同傳感器具有差異，直接用A/D轉(zhuǎn)換的結(jié)果計算位置誤差較大。為了避免傳感器差異造成的影響，我們采用了先定標再用相對值進行加權(quán)計算的辦法，具體過程為：程序開始前讓每個傳感器在賽道上進行掃描，分別記錄每個傳感器輸出信號的最大值max（對應(yīng)讀到黑線中心的情況）和最小值min（對應(yīng)遠離黑線讀到白色賽道的情況），用最大值減去最小值得到每個傳感器在賽道上的輸出范圍，小車行使過程中，將每個傳感器

9、輸出的信號減去最小值，再除以該傳感器的輸出范圍即可得到其相對輸出值，再用每個傳感器的相對值與傳感器位置作加權(quán)平均得到的結(jié)果即為黑線位置。公式為，其中pos為位置，為第n個傳感器的相對輸出值，為第n個傳感器的位置。這種方法消除了傳感器差異造成的影響，并能夠得到相對十分連續(xù)的位置信息，相鄰兩次位置間隔小于0.5mm。然而，全局求位置法在某些入彎情況會造成計算錯誤，黑線與傳感器排列方向夾角越小錯誤越大。這是由于此時多個傳感器離黑線較近，因此輸出值較大，原本黑線位置較大時經(jīng)過全局加權(quán)平均計算結(jié)果將偏小，用LCD（自制的調(diào)試手段，用LCD實時顯示程序變量等信息）顯示測量結(jié)果，誤差可達36%（正確位置為7

10、cm，顯示值為4.5cm），這種情況將造成小車轉(zhuǎn)向不足，沖出賽道。解決辦法是采用下面介紹的對稱求位置法。1.1.1.1. 對稱求位置法對稱求位置法的準備工作與全局求位置法相同，使用的是傳感器的相對輸出值，先判斷出輸出值最大的傳感器，也就是離黑線最近的傳感器，然后根據(jù)該傳感器進行對稱加權(quán)平均求黑線位置。對稱求位置又分為奇數(shù)對稱求位置和偶數(shù)對稱求位置，參與計算傳感器個數(shù)在傳感器總個數(shù)的約束下應(yīng)盡可能大，即兩端的傳感器總是至少有一個參與計算。在前面所述情況下，誤差為7%（正確位置為7cm，顯示值為6.5cm）。對稱求位置法與全局求位置法相比穩(wěn)定性強，但由于有公式的切換，位置的連續(xù)性不如全局求位置法。

11、但當(dāng)傳感器調(diào)節(jié)得較好時，二者可以有相同的連續(xù)性，因為公式切換時被舍棄的傳感器和新加入計算的傳感器輸出均為0，這一點可以通過后面介紹的去除定標誤差辦法實現(xiàn)。 1.1.2. 角度計算1.1.2.1. 多項式逼近雙排直線傳感器分別計算黑線位置，將結(jié)果之差除以傳感器間距再進行反正切運算即可得到角度值。反正切運算通過多項式逼近用多項式計算實現(xiàn)，我們的傳感器測量角度范圍約為，與舵機轉(zhuǎn)向范圍相同，在這個范圍內(nèi)，一次多項式對反正切函數(shù)的逼近效果很好。1.1.3. 去除干擾1.1.3.1. 去除定標誤差由于程序開始前確定每個傳感器的最大值和最小值過程存在一定誤差，即該最大值和最小值并不分別等于小車運行時傳感器讀

12、到黑線中心和讀到遠離黑線的白色賽道時的輸出值，這就造成很可能所有傳感器相對輸出均不為0，在用對稱求位置法計算時，切換公式時會使計算結(jié)果產(chǎn)生跳動，連續(xù)性變差，影響微分控制的效果。解決這一問題的辦法是：掃描求出最大值和最小值后，將最小值加上輸出范圍的a%（如10%）得到新的最小值，再計算每個傳感器的輸出范圍。通過調(diào)整傳感器角度、位置，調(diào)節(jié)接收管上拉電阻和a的值，用LCD顯示每個傳感器的相對輸出值，得到以下效果為最好：相對輸出不為0的傳感器個數(shù)只為2或3，若最邊上的傳感器相對輸出不為0，則該個數(shù)只能為2。這個條件不難滿足，滿足條件后（實際條件要更寬松一些）對稱求位置法的連續(xù)性將與全局求位置法的連續(xù)性

13、相同。1.1.3.2. 去除地圖干擾由于地圖中的十字線、起點線和地圖外的場地會導(dǎo)致計算出錯誤的黑線位置，使小車控制出錯，通過設(shè)定以下約束可以解決這些問題：a.所有傳感器輸出都低于一個閾值時不計算新位置b.輸出超過閾值的所有傳感器并非連續(xù)安放的傳感器時不計算新位置c.輸出最大且超過閾值的傳感器與上一個滿足該要求的傳感器不相鄰時不計算新位置。1.1.4. 連續(xù)性檢測用LCD輸出位置計算結(jié)果，相鄰兩次位置間隔小于0.5mm，相鄰兩次角度間隔小于。1.2. PD控制1.2.1. 比例控制位置控制中的比例控制采用了分段比例控制，位置較小時與位置較大時設(shè)置不同的比例帶，原因是傳感器寬度有限，檢測的位置范圍也就有限，統(tǒng)一的比例帶過大會導(dǎo)致小車振蕩，過小導(dǎo)致最大控制量偏小，小車轉(zhuǎn)向不足，過彎時沖出賽道。使用分段比例控制既方便又可以解決以上兩種問題。角度控制設(shè)置了死區(qū)，由于檢測角度為0