飛思卡爾__所有的算法

1、第1章 電機(jī)控制1.1. 直流電機(jī)控制策略：針對(duì)本文所研究的智能車來(lái)說(shuō)，車體速度是大慣性的被控對(duì)象。算法輸出的控制量只能對(duì)電機(jī)輸出力進(jìn)行控制。而有一定負(fù)載時(shí)電機(jī)的輸出力無(wú)論對(duì)車輪的轉(zhuǎn)速還是車體的形式速度都是不成正比的，車在剛開(kāi)始啟動(dòng)的時(shí)候速度是零，而電機(jī)的輸出可能很大；車在勻速行駛的時(shí)候速度很快，而電機(jī)的輸出可能并不是很大。而且電池電量、車體重量都會(huì)對(duì)車速造成影響。因此只有用閉環(huán)才能對(duì)車速進(jìn)行良好的控制。在車輪對(duì)地面不打滑的情況下車體的速度和后輪的轉(zhuǎn)動(dòng)速度是成正比的。因此我們可以直接用光電碼盤對(duì)后輪的轉(zhuǎn)速進(jìn)行控制。對(duì)于這樣一個(gè)大慣性系統(tǒng)，我們選用PID和魯棒相結(jié)合的辦法進(jìn)行速度控制。回路的設(shè)定

2、值由經(jīng)驗(yàn)值確定?？紤]到速度控制通道的時(shí)間滯后比較小，因此采用PID控制方案，并在進(jìn)行加減速控制時(shí)，引入了“棒棒控制”。U(k + 1) = U(K) + P1 * e(k) + P2 * (e(k) e(k - 1) + P3 * (e(k) e(k-1) (e(k-1) e(k - 2);公式 11 PID的公式其中第一項(xiàng)為積分項(xiàng)；第二項(xiàng)為比例項(xiàng)；第三項(xiàng)為微分項(xiàng)?？紤]到被控對(duì)象（車體速度）本身是一個(gè)大的積分環(huán)節(jié)，公式中可以將第一項(xiàng)省略，即采用PD控制。E為誤差。同時(shí)設(shè)定誤差門限，在誤差比較大的時(shí)候采用大輸出控制電機(jī)，將誤差在最短時(shí)間內(nèi)減小到所要求的范圍，這就是魯棒控制的思想。電機(jī)控制策略，其

3、實(shí)就是對(duì)模型車速度的控制策略，它是繼路徑識(shí)別之后非常重要的策略，直接關(guān)系到整個(gè)模型車比賽的性能。速度控制得當(dāng)，小車才能以最好的狀態(tài)，在最短的時(shí)間完成比賽路程。1.1.1. 彎道速度控制 在模型車入彎時(shí)刻出于穩(wěn)定性考慮做減速控制。減速原則是在原來(lái)直道速度設(shè)定值的基礎(chǔ)上，減小速度設(shè)定值到低速擋，保證模型車可以安全入彎。另一方面，入彎之后，為了過(guò)彎時(shí)模型車能夠不明顯的左右擺動(dòng)并采用較好的姿態(tài)通過(guò)彎路，令車速與偏差成線性關(guān)系，偏差越大速度設(shè)定值越小。速度設(shè)定方法如Error! Reference source not found.。.公式 12其中：為速度閉環(huán)設(shè)定值； 模型車全程運(yùn)動(dòng)平均速度設(shè)定值；

4、車體偏離理想軌跡的當(dāng)前偏差值；為減速控制比例系數(shù)。 同時(shí)，通過(guò)實(shí)驗(yàn)發(fā)現(xiàn)模型車入彎之后，令模型車以某一線性規(guī)律加速運(yùn)行可以使車在不沖出軌道的前提下以更短的時(shí)間通過(guò)彎道。控制規(guī)律 如Error! Reference source not found.。.公式 13其中：為彎道加速系數(shù)； 彎道加速變量；為常數(shù)，初始化時(shí)設(shè)定。入彎時(shí)刻初始化為0，每個(gè)控制周期累加1。1.1.2. 直道速度控制直道采用勻速控制方案，速度設(shè)定在此控制系統(tǒng)下可以控制的最大入彎速度。在從彎道駛出進(jìn)入直道后須進(jìn)行加速控制，增大速度設(shè)定值。因速度控制回路有一定的慣性，所以在提速時(shí)為了使速度迅速改變，加入了“棒棒控制”?？刂埔?guī)律如E

5、rror! Reference source not found.。公式 14其中： 補(bǔ)償效果控制系數(shù)； 調(diào)速補(bǔ)償變量。為常數(shù)，初始化時(shí)設(shè)定。出彎時(shí)刻初始化為，每個(gè)控制周期累加。1.1.3. PID閉環(huán)控制算法 根據(jù)路徑識(shí)別的情況，如果當(dāng)前路徑為直道，則需要加速；若是彎道，則需要降速，而且根據(jù)不同的彎道速度也是有所區(qū)別。系統(tǒng)利用測(cè)速模塊反饋的當(dāng)前速度值，通過(guò)增量式PID算法進(jìn)行調(diào)節(jié)，從而控制直流電機(jī)對(duì)當(dāng)前路徑進(jìn)行快速反應(yīng)。增量式PID的算式為：在增量式PID處理的過(guò)程中，有一個(gè)步驟需要注意，即在算完u (k)后，需要把它賦值給電機(jī)控制對(duì)應(yīng)的PWM通道信號(hào)，這時(shí)要判斷該u (k)的值，如果它小于

6、0，則把PWM信號(hào)賦值為0，如果它大于PWM信號(hào)的最大值，即大于PWM整個(gè)周期所對(duì)應(yīng)的數(shù)值時(shí)，要把PWM信號(hào)賦值為該最大值。然后，再存儲(chǔ)本次u (k)，和上次u (k)。error=speed_v-infrared_value7;pwmtemp=PWMDTY23+PID_P*(error-last_error)+PID_I*(error)+PID_D*(error+pre_error-2*last_error);由上述代碼中，speed_v表示標(biāo)準(zhǔn)速度，infrared_value7表示ATD1轉(zhuǎn)換的經(jīng)公式4.5計(jì)算出的即時(shí)速度值，通過(guò)計(jì)算它們的差值error，再利用增量PID控制算法計(jì)算出p

7、wmtemp，再對(duì)pwmtemp進(jìn)行處理，最后PID程序處理的結(jié)果是給出一個(gè)PWM信號(hào)，而這個(gè)信號(hào)就是驅(qū)動(dòng)電機(jī)的，當(dāng)error大，即標(biāo)準(zhǔn)速度和即時(shí)速度相差的比較多時(shí)，pwmtemp的絕對(duì)值相對(duì)比較大，則給電機(jī)的PWM信號(hào)相對(duì)較大，這樣電機(jī)轉(zhuǎn)速相對(duì)較快；反之，電機(jī)轉(zhuǎn)速則相對(duì)較慢。由此可以看出，PID算法主要的功能是，在閉環(huán)系統(tǒng)中，利用即時(shí)速度的反饋，使得即時(shí)速度逼近標(biāo)準(zhǔn)速度的時(shí)間盡量變短，這樣小車就可以根據(jù)路徑識(shí)別得出的速度標(biāo)準(zhǔn)值，及時(shí)調(diào)整自己的速度，以適應(yīng)各種路況。例如，小車正在直道上行進(jìn)，而且直道的時(shí)速應(yīng)該是各種賽道中最快的，當(dāng)系統(tǒng)的路徑識(shí)別算法察覺(jué)到前方出現(xiàn)弧度大的彎道后，系統(tǒng)會(huì)根據(jù)事先調(diào)

8、試的結(jié)果，給出大弧度彎道的速度標(biāo)準(zhǔn)值，然后PID算法根據(jù)速度標(biāo)準(zhǔn)值speed_v和即時(shí)速度反饋值infrared_value7對(duì)電機(jī)的輸入信號(hào)PWM進(jìn)行及時(shí)調(diào)整，反映到實(shí)際中，就是小車及時(shí)減速并順利通過(guò)彎道。在增量式PID處理的過(guò)程中，有一個(gè)步驟需要注意，即在算完u (k)后，需要把它賦值給電機(jī)控制對(duì)應(yīng)的PWM通道信號(hào)，這時(shí)要判斷該u (k)的值，如果它小于0，則把PWM信號(hào)賦值為0，如果它大于PWM信號(hào)的最大值，即大于PWM整個(gè)周期所對(duì)應(yīng)的數(shù)值時(shí)，要把PWM信號(hào)賦值為該最大值。1.2. 反向制動(dòng)算法要使賽車在較短的時(shí)間完成比賽，速度自然越高越好，顯然速度太高彎道是過(guò)不去的，如果以彎道的極限速

9、度勻速跑，又浪費(fèi)了直道的時(shí)間。所以最佳的策略是直道以較高的速度跑，到彎道時(shí)再盡快將速度降下來(lái)。在入彎減速時(shí)如果只靠賽道的摩擦阻力效果顯然是不夠理想的。為此我們引入反向制動(dòng)算法。由于MC33886芯片集成的H橋驅(qū)動(dòng)電路本身就具有反向制動(dòng)功能，所以不需再外加硬件電路。反向制動(dòng)流程圖如圖：在制動(dòng)時(shí)，電機(jī)反向電動(dòng)勢(shì)對(duì)整個(gè)系統(tǒng)電路造成的沖擊，從而引起單片機(jī)工作不穩(wěn)定，電流過(guò)大導(dǎo)致電機(jī)過(guò)熱、電機(jī)頻繁換向?qū)е码娝⒋蚧饎×覍?dǎo)致電機(jī)壽命縮短等一系列問(wèn)題都是需要考慮的。1.3. 速度控制策略1、為了達(dá)到好的速度控制效果，對(duì)速度進(jìn)行閉環(huán)控制是必須的。這里所說(shuō)的速度控制策略是指設(shè)定速度的確定方法設(shè)定速度主要由道路與直

10、道的偏差來(lái)決定，道路越接近直道，設(shè)定速度越高，反之越低。車模行駛中的最低速度是這樣確定的：令車模以較低的速度勻速行使，在保證無(wú)犯規(guī)行為出現(xiàn)的前提下，逐漸提高勻速行使的速度，直到車模出現(xiàn)犯規(guī)行為，此速度再減去一個(gè)安全量，即為所需的最低速度。簡(jiǎn)單的說(shuō)，變速行使的最低速度等于勻速行使的最高速度。車模行駛中的最高速度是這樣確定的：在確定最低速度以后，加入變速策略，不斷提高最高速度的設(shè)定值，直到模型車出現(xiàn)犯規(guī)行為，此速度再減去一個(gè)安全量，即為所需的最高速度。車模行駛過(guò)程中難免出現(xiàn)“失去道路”的情況，對(duì)此需要采取一定的安全策略，防止賽車“盲跑”而導(dǎo)致犯規(guī)。2、針對(duì)本文所研究的智能車來(lái)說(shuō)，車體速度是大慣性的

11、被控對(duì)象。算法輸出的控制量只能對(duì)電機(jī)輸出力進(jìn)行控制。而有一定負(fù)載時(shí)電機(jī)的輸出力無(wú)論對(duì)車輪的轉(zhuǎn)速還是車體的形式速度都是不成正比的，車在剛開(kāi)始啟動(dòng)的時(shí)候速度是零，而電機(jī)的輸出可能很大；車在勻速行駛的時(shí)候速度很快，而電機(jī)的輸出可能并不是很大。而且電池電量、車體重量都會(huì)對(duì)車速造成影響。因此只有用閉環(huán)才能對(duì)車速進(jìn)行良好的控制。在車輪對(duì)地面不打滑的情況下車體的速度和后輪的轉(zhuǎn)動(dòng)速度是成正比的。因此我們可以直接用光電碼盤對(duì)后輪的轉(zhuǎn)速進(jìn)行控制。對(duì)于這樣一個(gè)大慣性系統(tǒng)，我們選用PID和棒棒相結(jié)合的辦法進(jìn)行速度控制?；芈返脑O(shè)定值由經(jīng)驗(yàn)值確定?？紤]到速度控制通道的時(shí)間滯后比較小，因此采用PID控制方案，并在進(jìn)行加減速

12、控制時(shí)，引入了“棒棒控制”。U(k + 1) = U(K) + P1 * e(k) + P2 * (e(k) e(k - 1) + P3 * (e(k) e(k-1) (e(k-1) e(k - 2);公式 15 PID的公式其中第一項(xiàng)為積分項(xiàng)；第二項(xiàng)為比例項(xiàng)；第三項(xiàng)為微分項(xiàng)?？紤]到被控對(duì)象（車體速度）本身是一個(gè)大的積分環(huán)節(jié)，公式中可以將第一項(xiàng)省略，即采用PD控制。E為誤差。同時(shí)設(shè)定誤差門限，在誤差比較大的時(shí)候采用大輸出控制電機(jī)，將誤差在最短時(shí)間內(nèi)減小到所要求的范圍，這就是棒棒控制的思想。電機(jī)控制策略，其實(shí)就是對(duì)模型車速度的控制策略，它是繼路徑識(shí)別之后非常重要的策略，直接關(guān)系到整個(gè)模型車比賽的

13、性能。速度控制得當(dāng)，小車才能以最好的狀態(tài)，在最短的時(shí)間完成比賽路程。1.3.1. 彎道速度控制在模型車入彎時(shí)刻出于穩(wěn)定性考慮做減速控制。減速原則是在原來(lái)直道速度設(shè)定值的基礎(chǔ)上，減小速度設(shè)定值到低速擋，保證模型車可以安全入彎。另一方面，入彎之后，為了過(guò)彎時(shí)模型車能夠不明顯的左右擺動(dòng)并采用較好的姿態(tài)通過(guò)彎路，令車速與偏差成線性關(guān)系，偏差越大速度設(shè)定值越小。速度設(shè)定方法如Error! Reference source not found.。.公式 16其中：為速度閉環(huán)設(shè)定值； 模型車全程運(yùn)動(dòng)平均速度設(shè)定值； 車體偏離理想軌跡的當(dāng)前偏差值；為減速控制比例系數(shù)同時(shí)，通過(guò)實(shí)驗(yàn)發(fā)現(xiàn)模型車入彎之后，令模型車以

14、某一線性規(guī)律加速運(yùn)行可以使車在不沖出軌道的前提下以更短的時(shí)間通過(guò)彎道。控制規(guī)律如Error! Reference source not found.。.公式 17其中：為彎道加速系數(shù)； 彎道加速變量；為常數(shù)，初始化時(shí)設(shè)定。入彎時(shí)刻初始化為0，每個(gè)控制周期累加1。1.3.2. 直道速度控制直道采用勻速控制方案，速度設(shè)定在此控制系統(tǒng)下可以控制的最大入彎速度。在從彎道駛出進(jìn)入直道后須進(jìn)行加速控制，增大速度設(shè)定值。因速度控制回路有一定的慣性，所以在提速時(shí)為了使速度迅速改變，加入了“棒棒控制”?？刂埔?guī)律如Error! Reference source not found.。公式 18其中： 補(bǔ)償效果控制

15、系數(shù)； 調(diào)速補(bǔ)償變量。為常數(shù)，初始化時(shí)設(shè)定。出彎時(shí)刻初始化為，每個(gè)控制周期累加。 1.3.3. 速度及加速度信號(hào)采集子程序當(dāng)圖像數(shù)據(jù)采集處理完后，我們讀取速度傳感器和加速度傳感器的值，采集這兩個(gè)傳感器信號(hào)的頻率也為60Hz。流程圖如圖4.3所示。子程序入口讀速度傳感器的值讀加速度傳感器的值讀兩個(gè)16位脈沖計(jì)數(shù)器的值返回主程序圖4.3速度及加速度信號(hào)采集子程序流程圖1.4. 速度PID算法根據(jù)偏差的比例（Proportional）、積分（Integral）、微分（Derivative）的線性組合進(jìn)行反饋控制（簡(jiǎn)稱PID控制），數(shù)字PID控制算法是電機(jī)微機(jī)控制中常用的一種基本控制算法9。在連續(xù)系統(tǒng)

16、中，模擬PID調(diào)節(jié)器是一種線性調(diào)節(jié)器，控制系統(tǒng)原理框圖如圖5.10。圖5.10 PID控制系統(tǒng)原理框圖控制規(guī)律為： （5.1）式中：比例增益，的倒數(shù)稱為比例帶；：積分時(shí)間常數(shù)；：微分時(shí)間常數(shù)；：控制量；：偏差，等于給定量與反饋量的差。在計(jì)算機(jī)控制系統(tǒng)中，數(shù)字PID控制算法通常又分為位置式PID和增量式PID。本次設(shè)計(jì)中，我們采用增量式PID。增量型算法與位置型算法相比，具有以下優(yōu)點(diǎn)：增量型算法不需要做累加，增量的確定僅與最近幾次偏差采樣值有關(guān)，計(jì)算精度對(duì)控制量的計(jì)算影響較小，而位置型算法要用到過(guò)去偏差的累加值，容易產(chǎn)生大的累加誤差；增量型算法得出的是控制量的增量，而位置型算法的輸出是控制量的全

17、量輸出，誤動(dòng)作影響大；采用增量型算法，易于實(shí)現(xiàn)手動(dòng)到自動(dòng)的無(wú)沖擊切換。在實(shí)際應(yīng)用中，采樣的反饋值即為脈沖累加器中的PACN32中的脈沖數(shù)，預(yù)設(shè)門限值在參數(shù)整定時(shí)根據(jù)實(shí)際情況調(diào)節(jié)，輸出并不能直接用來(lái)控制電機(jī)，需要將其轉(zhuǎn)換為控制PWM占空比，然后用增大或減小PWM占空比的方法來(lái)實(shí)現(xiàn)對(duì)電機(jī)的加減速的控制。換句話說(shuō)，在求偏差量時(shí)，實(shí)際上用的是每20ms電機(jī)轉(zhuǎn)過(guò)的齒輪數(shù)和實(shí)際期望電機(jī)轉(zhuǎn)過(guò)的齒輪數(shù)，通過(guò)二者的差值，再乘以相應(yīng)的系數(shù)，即KP、KI、KD的協(xié)調(diào)控制，計(jì)算出相應(yīng)的PWM占空比，實(shí)際上用的是PWMDTY的值10。本設(shè)計(jì)中綜合考慮各種因素，最后選用的采樣周期為20ms，即每20ms對(duì)電機(jī)進(jìn)行一次PI

18、D調(diào)節(jié)。由于在程序中，對(duì)圖像的采集使用的是PH口的中斷程序，因此，PID采樣周期的選擇實(shí)際上是受限制與圖像采集，因?yàn)槊啃械膾呙柚芷跒?4s，有效掃描時(shí)間為52s，采用的是隔行掃描的方式，即每隔6行采集一行圖像的信息，如果在每行之間加入PID調(diào)節(jié)的話，那么處理PID子程序的時(shí)間必須控制在64*5320s之內(nèi)，另外圖像采集只是采集了奇場(chǎng)中的行數(shù)，在偶場(chǎng)中沒(méi)有采集，因此PID子程序的執(zhí)行是不均勻的，并沒(méi)有達(dá)到預(yù)期的效果，同時(shí)還可能會(huì)影響到視頻采集，因此，經(jīng)過(guò)分析，最終決定將PID的采樣周期定為20ms，即當(dāng)進(jìn)行一次場(chǎng)采集進(jìn)行一次PID調(diào)節(jié)。而且經(jīng)過(guò)最終的檢驗(yàn)，這樣能夠滿足對(duì)速度控制的需要。電機(jī)調(diào)速本

19、作品采用增量式的PI控制。增量式PI調(diào)節(jié)set_value電機(jī)輸出測(cè)速檢測(cè)圖4.12增量式PI算法: pwm=Kp*(errork0-errork1)/100+Ki*errork0/100+pwm;1.5. 直流驅(qū)動(dòng)電機(jī)控制子程序本設(shè)計(jì)通過(guò)調(diào)整方波的占空比來(lái)實(shí)現(xiàn)電機(jī)的調(diào)速。經(jīng)過(guò)初始化子程序的設(shè)置，PWM0和PWM5都能輸出頻率為5KHz的方波。由硬件部分可知，PWM0產(chǎn)生的方波經(jīng)MC33886功率放大后接到電機(jī)的負(fù)極，PWM5產(chǎn)生的方波經(jīng)MC33886功率放大后接到電機(jī)的正極。車模向前行駛時(shí)，PWM0輸出方波的占空比為零，通過(guò)調(diào)整PWM5輸出方波的占空比實(shí)現(xiàn)加速或緩慢減速。當(dāng)需要快速減速時(shí)，P

20、WM5輸出方波的占空比為零，PWM0輸出方波的占空比為合適的值，電機(jī)反轉(zhuǎn)實(shí)現(xiàn)車模制動(dòng)。該部分程序工作流程圖如圖4.7所示。YN圖4.7 驅(qū)動(dòng)電機(jī)控制子程序流程圖是否需要制動(dòng)返回主程序修改PWMDTY5的值獲得速度控制值子程序入口修改PWMDTY0及PWMDTY5值返回主程序1.6. 舵機(jī)控制1.6.1. 舵機(jī)轉(zhuǎn)角的決策由于CCD圖像傳感器的應(yīng)用，大大提高了車的前瞻性。經(jīng)測(cè)量，本系統(tǒng)預(yù)測(cè)距離可達(dá)1米以上。但是，考慮到各部件延時(shí)嚴(yán)重且具有很大不確定性，所以預(yù)測(cè)距離越遠(yuǎn)，控制算法就會(huì)越復(fù)雜。綜合各種因素考慮，我們最終將預(yù)測(cè)距離定在80100cm。黑線中心位置軌跡中包含著兩方面的信息，一方面是反映前方

21、道路趨勢(shì)的信息，另一方面是反映車體與軌道偏離程度的信息。經(jīng)過(guò)反復(fù)試驗(yàn)，我們發(fā)現(xiàn)，前者可以通過(guò)計(jì)算黑線的斜率來(lái)很好的描述。而后者，恰恰就是屏幕最下端黑線的位置。最后用這二者的線性組合得到舵機(jī)轉(zhuǎn)角。有了這兩方面信息，可以使智能車既能充分利用前瞻性，又能不過(guò)分地偏離軌道。從而得到了較理想的轉(zhuǎn)角控制策略解決方案。1.6.2. 偏航距離的計(jì)算由于已經(jīng)獲得了賽道中心線的位置，所以計(jì)算偏航距離的問(wèn)題是選取何處的中心線的距離為當(dāng)前的偏航距離?？刂扑惴ǖ膱?zhí)行周期為40ms，如果賽車的速度為2m/s，則在兩次控制算法的執(zhí)行中間，賽車要前進(jìn)8cm，賽車所處的環(huán)境將發(fā)生比較大的改變，所以賽車的控制只能算是半實(shí)時(shí)控制，

22、這是所有使用攝像頭作為主要尋線傳感器的參賽隊(duì)都避免不了的問(wèn)題。因?yàn)樗惴ǖ臏笮?，賽車需要將“?dāng)前位置”進(jìn)行適當(dāng)前移。前移量應(yīng)該跟賽車當(dāng)前速度成正比，但實(shí)際中我們發(fā)現(xiàn)，適當(dāng)增加一些前移距離是有好處的，因?yàn)榭梢栽谌霃澨幪崆稗D(zhuǎn)彎，使得賽車沿彎道內(nèi)側(cè)行駛，縮短了過(guò)彎距離。1.6.3. 舵機(jī)參數(shù)測(cè)定通過(guò)實(shí)驗(yàn)測(cè)出已安裝在模型車上的舵機(jī)的中位PWM值（Servo_center），模型車轉(zhuǎn)向輪左右極限時(shí)舵機(jī)PWM（Servo_left、Servo_right）值是舵機(jī)控制的基礎(chǔ)。測(cè)試舵機(jī)中位時(shí)，先在舵機(jī)控制模塊中給舵機(jī)PWM設(shè)計(jì)一個(gè)全局變量（Pwm_Servo），將賽車放在長(zhǎng)直道上，保證車身放正并處于賽道正中

23、間，利用BDM查看此時(shí)Pwm_Servo的值，將此值直接賦給舵機(jī)PWM，并將賽車放到直道上跑，看是否跑偏，微調(diào)Pwm_Servo的值，直到賽車在直道上不再跑偏為止。測(cè)定舵機(jī)左右極限時(shí)，可根據(jù)轉(zhuǎn)向輪的左右極限轉(zhuǎn)角，利用BDM查看舵機(jī)Servo_left和Servo_right的值。注意轉(zhuǎn)向輪在左右極限轉(zhuǎn)角時(shí)，轉(zhuǎn)向輪不可被賽車底盤卡住，這樣不但會(huì)增加轉(zhuǎn)向阻力，嚴(yán)重時(shí)還可能使轉(zhuǎn)向輪抱死，失去轉(zhuǎn)向能力。舵機(jī)的PWM計(jì)算公式為：Pwm_Servo=Servo_center+(Servo_left-Servo_right)/Simage_number*offline_one; 公式四式中Simage_nu

24、mber為每行采集的點(diǎn)數(shù)，offline_one為黑線在整幅圖像中的位置。1.6.4. 路徑規(guī)劃要想以最短的時(shí)間完成比賽，并不需要嚴(yán)格按照黑色引導(dǎo)線行駛，我們需要找出相對(duì)較短的行駛路徑，即路徑規(guī)劃。例如 過(guò)普通彎道的時(shí)候，以同樣的速度走內(nèi)彎顯然比走中間或外彎所花時(shí)間少，過(guò)小S形彎時(shí)。直接過(guò)去和嚴(yán)格循跡相比速度快，行駛距離短等等。（圖）如果能提前預(yù)知前方賽道信息，那么我們就有可能對(duì)路徑進(jìn)行規(guī)劃。由于我們采用的是攝像頭，掃描距離較大，這就為我們進(jìn)行路徑規(guī)劃提供了條件，這也是我們選擇攝像頭的原因之一。由于攝像頭看得較遠(yuǎn)，每幅圖像反映的賽道信息量較大，我們需要針對(duì)賽車在不同位置，對(duì)其有側(cè)重的采用。具體

25、就是將采到的15行劃分為幾個(gè)區(qū)域，針對(duì)不同路況對(duì)每個(gè)區(qū)域賦以不同的權(quán)值。其原理圖如圖：圖5.10 路徑規(guī)劃示意圖1.7. 偏航距離的計(jì)算由于已經(jīng)獲得了賽道中心線的位置，所以計(jì)算偏航距離的問(wèn)題是選取何處的中心線的距離為當(dāng)前的偏航距離。控制算法的執(zhí)行周期為40ms，如果賽車的速度為2m/s，則在兩次控制算法的執(zhí)行中間，賽車要前進(jìn)8cm，賽車所處的環(huán)境將發(fā)生比較大的改變，所以賽車的控制只能算是半實(shí)時(shí)控制，這是所有使用攝像頭作為主要尋線傳感器的參賽隊(duì)都避免不了的問(wèn)題。因?yàn)樗惴ǖ臏笮?，賽車需要將“?dāng)前位置”進(jìn)行適當(dāng)前移。前移量應(yīng)該跟賽車當(dāng)前速度成正比，但實(shí)際中我們發(fā)現(xiàn)，適當(dāng)增加一些前移距離是有好處的，

26、因?yàn)榭梢栽谌霃澨幪崆稗D(zhuǎn)彎，使得賽車沿彎道內(nèi)側(cè)行駛，縮短了過(guò)彎距離。1.7.1. 偏航角度的計(jì)算計(jì)算偏航角度的實(shí)質(zhì)是直線擬合問(wèn)題，因?yàn)橘惖乐行木€所在的直線確定了，而直線的斜率與偏航角度一一對(duì)應(yīng)。直線擬合最有效的方法是最小二乘法7，但是直接應(yīng)用存在一個(gè)問(wèn)題，即如何確定進(jìn)行直線擬合的區(qū)間？在整個(gè)成功識(shí)別出賽道的區(qū)間內(nèi)進(jìn)行直線擬合顯然是欠缺考慮的，因?yàn)樵趶澋赖那闆r下，這種方法擬合出的是一條弦線，而不是當(dāng)前該彎道處的切線。攝像頭視野越大，彎道曲率越大，弦線偏離切線的程度也就越大。為了能夠在直道和彎道上都能正確的擬合出正確的直線，我們采用了直線檢測(cè)的方法，即首先根據(jù)殘差的大小確定直線的范圍，然后在這一范圍

27、內(nèi)進(jìn)行直線擬合。1.7.2. 曲率的計(jì)算如果說(shuō)斜率的計(jì)算需要某種技巧的話，計(jì)算曲率則更是一種技巧的應(yīng)用。首屆時(shí)很多參賽隊(duì)針對(duì)各自的實(shí)際需要，提出了自己的方法。其中最普遍的是根據(jù)斜率的導(dǎo)數(shù)來(lái)計(jì)算曲率8。但是斜率的計(jì)算本身就很不準(zhǔn)確，特別是某個(gè)點(diǎn)的斜率，對(duì)斜率求導(dǎo)就更不準(zhǔn)確，所以使用這種方法只能得出一個(gè)大致的結(jié)果。本文作者提出了另外一種方法，首先對(duì)獲得的路徑進(jìn)行濾波，使得路徑盡可能平滑，然后取其兩個(gè)端點(diǎn)和中間點(diǎn)，計(jì)算這3個(gè)點(diǎn)組成的三角形的外接圓的半徑，半徑的倒數(shù)就是這段路徑的曲率。經(jīng)過(guò)多次實(shí)驗(yàn)，這種方法的誤差一般不大于20%，對(duì)智能車的控制來(lái)說(shuō)已經(jīng)足夠了。圖5.8是實(shí)驗(yàn)賽道的照片，讓賽車從起跑線開(kāi)

28、始，在賽道上行駛一圈，記錄下每個(gè)時(shí)刻的曲率，如圖5.9。從圖5.9可以看出，計(jì)算出的曲率能較為正確的反映實(shí)際賽道的彎曲情況。但是這種方法得出的曲率不是攝像頭所看到的當(dāng)前位置的曲率，而是攝像頭所看到的路徑的整體曲率，因?yàn)檫@種算法僅僅與路徑中三個(gè)點(diǎn)的位置有關(guān)。然而實(shí)驗(yàn)中卻發(fā)現(xiàn)，這種特點(diǎn)反而給賽車帶來(lái)了一個(gè)好處：即攝像頭在小S型彎道時(shí)舵機(jī)幾乎不跟隨路徑的擺動(dòng)而擺動(dòng)，而是直線沖過(guò)。這是因?yàn)檩^小的S型路徑其彎曲部分能完整的顯現(xiàn)在攝像頭的視野中，而算法中對(duì)路徑進(jìn)行了濾波，濾除了中間部分的彎曲，使得路徑變直了，賽車因此就直線走過(guò)。1.8. 車輛橫向控制算法本設(shè)計(jì)采用的橫向控制算法是模擬人駕駛汽車的方式控制車

29、模轉(zhuǎn)向。首先，根據(jù)圖像數(shù)據(jù)處理后的數(shù)據(jù)判斷車模偏離黑線的距離。其次，綜合考慮車速傳感器獲得的當(dāng)前速度值，計(jì)算目標(biāo)轉(zhuǎn)角。然后，在下一次采樣時(shí)，與上次采樣值比較，判斷偏離距離的變化趨勢(shì)，再次計(jì)算目標(biāo)轉(zhuǎn)角，當(dāng)車?；氐街芯€位置時(shí)，前輪回正。該部分程序流程圖如圖4.4所示。返回主程序返回主程序Y子程序入口對(duì)近距離光電傳感器編碼，獲得車輛偏離距離是否偏離黑線？前輪回正與上次采樣值比較，判斷偏離距離變化趨勢(shì)結(jié)合車速信號(hào)，得到目標(biāo)轉(zhuǎn)角N 圖4.4 橫向控制算法流程圖1.9. 車輛縱向控制算法本設(shè)計(jì)中模型車的速度控制采用PID控制算法。首先，圖像數(shù)據(jù)處理后的數(shù)據(jù)判斷車模當(dāng)前行駛的路況，得到目標(biāo)速度值。其次，根據(jù)

30、車速度傳感器的值，判斷車速是否達(dá)到目標(biāo)速度，進(jìn)行加減速。第三，結(jié)合加速度傳感器的值判斷模型車是否發(fā)生滑移，如果滑移進(jìn)行防滑處理。該部分程序流程圖如圖4.5所示。N加速控制YNYY子程序入口由前輪速度傳感器的值計(jì)算車速是否達(dá)到目標(biāo)速度？加速控制是否發(fā)生滑轉(zhuǎn)？是否進(jìn)入彎道？防滑處理返回主程序返回主程序制動(dòng)控制N圖4.5 縱向控制算法流程圖1.10. 轉(zhuǎn)向控制子程序由于舵機(jī)反應(yīng)速度的局限，本設(shè)計(jì)每10ms對(duì)舵機(jī)控制一次?？芍狿WM23已經(jīng)在初始化子程序中設(shè)置完畢，其能產(chǎn)生周期為20ms的方波，在此我們只需修改PWMDTY23寄存器即可改變方波的占空比，進(jìn)而控制舵機(jī)轉(zhuǎn)向。本車舵機(jī)的安裝位置和PWM23

31、通道的設(shè)置使得前輪在中間位置時(shí)，PWMDTY23的值為4960，左極限位置時(shí)值為5300，右極限位置時(shí)為4640，即將左極限位置到右極限位置的轉(zhuǎn)向級(jí)別分為660級(jí)。為了防止舵機(jī)轉(zhuǎn)向超過(guò)極限位置，發(fā)生卡死現(xiàn)象，損壞舵機(jī)，本設(shè)計(jì)通過(guò)軟件限定極限轉(zhuǎn)角。該子程序工作過(guò)程如圖4.6所示。N是否超出范圍？返回主程序修改PWMDTY23的值獲得轉(zhuǎn)向角度值子程序入口賦給極限角度值YN圖4.6 舵機(jī)控制子程序流程圖1.11. 車體控制策略：對(duì)于硬件結(jié)果已經(jīng)確定的智能車來(lái)說(shuō)，算法控制的實(shí)際上就是車體的方向和速度，即前輪舵機(jī)的轉(zhuǎn)角和后輪的轉(zhuǎn)速。如果前面圖像識(shí)別和慮噪環(huán)節(jié)做的好的話，對(duì)于控制量的給出的算法就相對(duì)容易，

32、但是需要花很大精力進(jìn)行參數(shù)的調(diào)整，使得車的行駛軌跡更加優(yōu)化，并且讓速度盡可能的提高。對(duì)于前輪的控制來(lái)說(shuō)，由于執(zhí)行機(jī)構(gòu)（舵機(jī)）和被控對(duì)象的慣性相對(duì)較小，換句話說(shuō)算法給出的控制量在誤差允許 的范圍內(nèi)可以認(rèn)為由執(zhí)行機(jī)構(gòu)準(zhǔn)確執(zhí)行，可以進(jìn)行開(kāi)環(huán)的控制2。我們的控制算法是在圖像中分別取上、中、下三個(gè)點(diǎn)，即在23行的數(shù)組中取3、12、21三個(gè)點(diǎn)，計(jì)算出圖像的位置，然后用最上面一點(diǎn)和最下面一點(diǎn)的差做為圖像的斜率。然后用兩者的線性組合算出舵機(jī)的轉(zhuǎn)角。用下面公式給出：Warp = P1 * ( Average2 + Average11 + Average20) + P2 * (Average20 Average2

33、)公式 19 舵機(jī)的轉(zhuǎn)角公式其中，warp 是車體對(duì)于黑線的誤差，也是舵機(jī)應(yīng)該轉(zhuǎn)動(dòng)的角度。同時(shí)，我們發(fā)現(xiàn)在車運(yùn)行過(guò)程當(dāng)中經(jīng)常會(huì)出現(xiàn)圖像中只有半幅圖像有線，或者整幅圖像都沒(méi)有黑線的情況。在半幅圖像有線的情況下，我們就用圖像的在視野中的終點(diǎn)、圖像最下面的點(diǎn)及兩者的中點(diǎn)進(jìn)行控制；如果整幅圖像都沒(méi)有黑線，就讓車的舵機(jī)保持上一次的轉(zhuǎn)角，一直到圖像重新恢復(fù)到視野。對(duì)于前輪的控制來(lái)說(shuō)，由于執(zhí)行機(jī)構(gòu)（舵機(jī)）和被控對(duì)象的慣性相對(duì)較小，換句話說(shuō)算法給出的控制量在誤差允許 的范圍內(nèi)可以認(rèn)為由執(zhí)行機(jī)構(gòu)準(zhǔn)確執(zhí)行，可以進(jìn)行開(kāi)環(huán)的控制。第2章 圖象采集2.1. 采集1、一場(chǎng)圖像采集完成后，得到一幅20*20的原始二值圖像信

34、息，如圖4.3（A）所示，同時(shí)給標(biāo)志變量VideoFinFlag置1。主程序中檢測(cè)到此標(biāo)志的時(shí)候，啟動(dòng)濾噪程序?？紤]到圖像中有效的信息為簡(jiǎn)單的黑色線條，必然具有很強(qiáng)的連續(xù)性。所以，可以采用從圖像一側(cè)到另一側(cè)，逐行鄰域查詢的辦法。具體來(lái)說(shuō)，就是根據(jù)已經(jīng)確定的前行黑線中心位置，在本行的一個(gè)鄰域內(nèi)查找黑點(diǎn)。超出這個(gè)鄰域的黑點(diǎn)都認(rèn)為是無(wú)效的。這樣，只要能夠確定某一行黑線的中心位置，就能逐行確定各行黑線的中心位置，從而，也就提取出了圖像中最基本的信息黑線中心位置軌跡（如圖4.3（B）所示）。原始圖像信息 處理后的信息 221100000-1-1000-1-1-2-2-2-3(A)(B)圖4.3 處理前后

35、的圖像信息2、閾值分割法是一種基于區(qū)域的分割技術(shù)，它對(duì)物體與背景有較強(qiáng)對(duì)比的景物的分割特別有用。它計(jì)算簡(jiǎn)單，而且總能用封閉且連通的邊界定義不交疊的區(qū)域。閾值分割法的關(guān)鍵在于閾值的確定。如果閾值是不隨時(shí)間和空間而變的，稱為靜態(tài)閾值；如果閾值隨時(shí)間或空間而變化，稱為而動(dòng)態(tài)閾值?；陟o態(tài)閾值的分割方法算法簡(jiǎn)單，計(jì)算量小，但是適應(yīng)性差?；趧?dòng)態(tài)閾值的分割方法其復(fù)雜程度取決于動(dòng)態(tài)閾值的計(jì)算方法6。針對(duì)本智能車系統(tǒng)，普通的雙峰法就能適合絕大部分情況，因?yàn)橹悄苘嚨倪\(yùn)行環(huán)境是比賽賽道，背景和前景區(qū)分明顯，且背景簡(jiǎn)約。但是實(shí)驗(yàn)環(huán)境并不理想，由于受到光線斜射的影響，有時(shí)背景和前景的對(duì)比十分不明顯。結(jié)合實(shí)際需要，提

36、出了一種新的計(jì)算閾值的方法，這種方法的思想與迭代法有些相似。首先我們假定，智能車系統(tǒng)運(yùn)行時(shí)，開(kāi)始時(shí)采集的第一幅圖像是良好的。這個(gè)“良好”含義是：第一行也就是最近處的一行，完整的包含了導(dǎo)航線，并且使用“雙峰法”能正確提取。其次，我們對(duì)每一行都定義一個(gè)閾值Tr，每一行都用該行的閾值Tr進(jìn)行分割。在此基礎(chǔ)上按照以下規(guī)則進(jìn)行閾值傳遞：a) 如果第r行分割出來(lái)的黑色區(qū)域（線段）是連續(xù)的，那么計(jì)算目標(biāo)區(qū)域和背景區(qū)域的平均灰度值，并取其中值，做為當(dāng)前行和下一行的Tr。b) 如果第r行分割出來(lái)的黑色區(qū)域是空集或者不連續(xù)，則保持當(dāng)前行和下一行的閾值不變。下濾波算法：當(dāng)某點(diǎn)的灰度值與其左邊和右邊點(diǎn)的灰度值的差同時(shí)

37、大于某個(gè)正閾值或同時(shí)小于某個(gè)負(fù)閾值時(shí)，認(rèn)為該點(diǎn)是干擾點(diǎn)，取其左右兩邊灰度值的平均值作為該點(diǎn)的灰度值。2.2. 圖像抗干擾處理抗干擾處理包括以下方面的內(nèi)容：1）消除信號(hào)產(chǎn)生和傳輸?shù)倪^(guò)程中造成的噪聲；2）消除賽道不理想或光線不均勻形成的干擾；3）消除賽道中的交叉線、斷續(xù)線；4）識(shí)別起跑線。信號(hào)產(chǎn)生和傳輸?shù)倪^(guò)程中造成的噪聲具有普遍性，但是只有當(dāng)其非常嚴(yán)重時(shí)才對(duì)路徑的識(shí)別造成影響。所以采取以下濾波算法：當(dāng)某點(diǎn)的灰度值與其左邊和右邊點(diǎn)的灰度值的差同時(shí)大于某個(gè)正閾值或同時(shí)小于某個(gè)負(fù)閾值時(shí)，認(rèn)為該點(diǎn)是干擾點(diǎn)，取其左右兩邊灰度值的平均值作為該點(diǎn)的灰度值。對(duì)于賽道不理想或光線不均勻形成的干擾，其一般特點(diǎn)是散布于

38、賽道黑線的兩邊，所以采用以下算法處理：步驟1 從最近的第一行開(kāi)始，尋找這樣一個(gè)行：該行只有一段連續(xù)黑色線段，且線段的左右邊均不是圖像的左右邊。標(biāo)記該行為Start行。步驟2 在下一行中尋找這樣一段連續(xù)黑色線段，滿足這樣的條件：其左邊緣不在上一行黑線右邊緣的右邊，且其右邊緣不在上一行黑線左邊緣的左邊。步驟3 如果存在步驟2中的黑色線段，認(rèn)為該段黑線是賽道黑線，其余黑點(diǎn)（線段）均認(rèn)為是干擾，跳步驟2。否則，標(biāo)記該行為End行，并計(jì)算該行與Start行之差，如果大于3，則結(jié)束搜索，如果小于等于3，則返回步驟1。這種算法的實(shí)質(zhì)是搜索連續(xù)的黑線。因?yàn)橘惖朗沁B續(xù)的，而干擾卻大部分是離散分布的。對(duì)于賽道中的

39、交叉線，可以統(tǒng)計(jì)一行中黑點(diǎn)的個(gè)數(shù)，因?yàn)檎５暮诰€只有2.5cm，在圖像中不超過(guò)6個(gè)點(diǎn)。所以把6作為一個(gè)閾值TN，如果一行黑點(diǎn)的個(gè)數(shù)超過(guò)TN個(gè)點(diǎn)，就可以認(rèn)為是交叉干擾。實(shí)際上，我們做了更復(fù)雜的處理，就是根據(jù)當(dāng)前賽道的傾斜程度來(lái)調(diào)節(jié)TN，因?yàn)楫?dāng)賽道傾斜時(shí)其寬度將發(fā)生變化。并且，我們不只是識(shí)別出交叉干擾，而且“砍掉”它的左右兩支，保留一條連續(xù)的賽道黑線。起跑線是在賽道黑線的左右兩邊，對(duì)稱存在兩條長(zhǎng)度各為10cm，相距6cm，且與賽道黑線垂直的黑色線段。正確的識(shí)別起跑線是應(yīng)用更高級(jí)控制算法如記憶算法的基礎(chǔ)。我們使用如下算法識(shí)別起跑線：在已識(shí)別出賽道黑線的行，保證該行不是交叉行（前面已經(jīng)識(shí)別出），然后確

40、定其左右兩邊各一個(gè)矩形區(qū)域，統(tǒng)計(jì)兩個(gè)區(qū)域中黑點(diǎn)的個(gè)數(shù)，如果均大于某一閾值，則判斷該行出現(xiàn)了起跑線。比賽中將設(shè)置限速段，限速段的一種可能的標(biāo)記法就是把賽道黑線做成斷續(xù)的，斷續(xù)寬度2cm，間隔2cm，所以并不是所有不連續(xù)的黑點(diǎn)都是干擾。這給參賽選手帶來(lái)了很大的挑戰(zhàn)。我們采取的應(yīng)對(duì)算法是，在遇到斷續(xù)的行時(shí)，按當(dāng)前已識(shí)別出的黑線的走向趨勢(shì)延伸黑線到該行，如果在3行內(nèi)發(fā)現(xiàn)了可以連接的黑線，則認(rèn)為延伸成功，否則結(jié)束搜索。經(jīng)過(guò)以上處理，我們得到了如下信息：首先是兩個(gè)標(biāo)記Start和End，這兩個(gè)標(biāo)記標(biāo)記了成功識(shí)別出的賽道的最近一行和最遠(yuǎn)一行；其次是在Start和End之間的每一行，記錄了其賽道的左邊緣位置和

41、右邊緣位置，并且通過(guò)坐標(biāo)變換算法，計(jì)算出每行賽道中心線的實(shí)際位置。最后留下了一些其它標(biāo)志，比如每行是否是交叉、起跑線或者斷續(xù)線。這些標(biāo)志在我們調(diào)試的過(guò)程中都用某種方式以圖像的形式反映出來(lái)。整個(gè)處理過(guò)程如圖5.6。如果說(shuō)斜率的計(jì)算需要某種技巧的話，計(jì)算曲率則更是一種技巧的應(yīng)用。首屆時(shí)很多參賽隊(duì)針對(duì)各自的實(shí)際需要，提出了自己的方法。其中最普遍的是根據(jù)斜率的導(dǎo)數(shù)來(lái)計(jì)算曲率8。但是斜率的計(jì)算本身就很不準(zhǔn)確，特別是某個(gè)點(diǎn)的斜率，對(duì)斜率求導(dǎo)就更不準(zhǔn)確，所以使用這種方法只能得出一個(gè)大致的結(jié)果。本文作者提出了另外一種方法，首先對(duì)獲得的路徑進(jìn)行濾波，使得路徑盡可能平滑，然后取其兩個(gè)端點(diǎn)和中間點(diǎn)，計(jì)算這3個(gè)點(diǎn)組成

42、的三角形的外接圓的半徑，半徑的倒數(shù)就是這段路徑的曲率。經(jīng)過(guò)多次實(shí)驗(yàn)，這種方法的誤差一般不大于20%，對(duì)智能車的控制來(lái)說(shuō)已經(jīng)足夠了。圖5.8是實(shí)驗(yàn)賽道的照片，讓賽車從起跑線開(kāi)始，在賽道上行駛一圈，記錄下每個(gè)時(shí)刻的曲率，如圖5.9。從圖5.9可以看出，計(jì)算出的曲率能較為正確的反映實(shí)際賽道的彎曲情況。但是這種方法得出的曲率不是攝像頭所看到的當(dāng)前位置的曲率，而是攝像頭所看到的路徑的整體曲率，因?yàn)檫@種算法僅僅與路徑中三個(gè)點(diǎn)的位置有關(guān)。然而實(shí)驗(yàn)中卻發(fā)現(xiàn)，這種特點(diǎn)反而給賽車帶來(lái)了一個(gè)好處：即攝像頭在小S型彎道時(shí)舵機(jī)幾乎不跟隨路徑的擺動(dòng)而擺動(dòng)，而是直線沖過(guò)。這是因?yàn)檩^小的S型路徑其彎曲部分能完整的顯現(xiàn)在攝像頭

43、的視野中，而算法中對(duì)路徑進(jìn)行了濾波，濾除了中間部分的彎曲，使得路徑變直了，賽車因此就直線走過(guò)。2.3. 圖像濾波找出二值化以后的圖像每一行中的黑色區(qū)域，如果只有一個(gè)黑色區(qū)域，則記下黑色區(qū)域起始點(diǎn)對(duì)應(yīng)的列號(hào)和結(jié)束點(diǎn)對(duì)應(yīng)的列號(hào)，求平均值即為黑線中心，如果黑色區(qū)域大于或者等于兩個(gè)該行圖像信息有錯(cuò)，則利用該行相鄰行找出正確的黑線位置。 如果連續(xù)幾行都為錯(cuò)，則放棄這幾行，直道找到正確行為止，再將剛才放棄的部分黑線補(bǔ)上。2.4. 梯形失真校正梯形失真會(huì)導(dǎo)致路進(jìn)識(shí)別不準(zhǔn)甚至出錯(cuò)，我們對(duì)梯形失真進(jìn)行了簡(jiǎn)單校正。我們采用比例法來(lái)矯正梯形失真。由于攝像頭視野最遠(yuǎn)處的寬度和最近出的寬度顯然是不一樣的，而攝像頭的行分

44、辨率是相同的，這就導(dǎo)致兩個(gè)采樣點(diǎn)之間的實(shí)際距離是不一樣的，而單片機(jī)卻認(rèn)為他們是一樣的，這也是導(dǎo)致梯形失真的原因。我們測(cè)量出攝像頭掃描賽道最近處和最遠(yuǎn)處視野寬度，算出相鄰兩個(gè)采樣點(diǎn)所代表的實(shí)際距離，再量出視野長(zhǎng)度，中間各行相鄰兩采樣點(diǎn)之間的距離根據(jù)其行號(hào)按比例計(jì)算出來(lái)。 1：原始圖片2：閾值分割3：消除干擾4：提取中心線并進(jìn)行坐標(biāo)變換1 23 4圖5.7 賽道抗干擾處理2.5. 路徑識(shí)別路徑識(shí)別是整個(gè)算法的重點(diǎn)，幾乎所有的控制算法都是建立在路徑識(shí)別上的。路徑識(shí)別主要是普通彎道、小S形彎道、大S形彎道、十字交叉道的識(shí)別。普通彎道是最容易識(shí)別的，可以根據(jù)偏移量、曲率等來(lái)識(shí)別。我們采用前一種，這種方法

45、不但算法簡(jiǎn)單，而且還可以用于識(shí)別多種路況。其具體思路就是將整幅圖像的引導(dǎo)線位置相加再求平均值，再根據(jù)我們?cè)囼?yàn)得到的數(shù)據(jù)設(shè)定閾值：直道和小S形彎道判斷的閾值（我們將小S形彎道也處理成直道），只要平均值小于該值就為直道，大于該值則為普通彎道或大S形彎道，再根據(jù)平均值的大小和當(dāng)前車速做加減速控制和舵機(jī)打角控制。十字交叉道識(shí)別也較重要，該處處理不好賽車則不會(huì)按預(yù)定路線行進(jìn)，圖為我們處理前后的交叉道的情況：圖5.7 路徑識(shí)別前后的交叉道2. 圖像平滑（去噪）：用信號(hào)處理的理論來(lái)解釋，我們的做法實(shí)現(xiàn)的是一種簡(jiǎn)單的低通濾波器（low pass filter）。在灰度連續(xù)變化的圖象中，如果出現(xiàn)了與相鄰像素的灰

46、度相差很大的點(diǎn)，比如說(shuō)一片暗區(qū)中突然出現(xiàn)了一個(gè)早點(diǎn)，這種情況被認(rèn)為是一種噪聲?；叶韧蛔?cè)陬l域中代表了一種高頻分量，低通濾波器的作用就是濾掉高頻分量，從而達(dá)到減少圖象噪聲的目的。為了方便的敘述上面所說(shuō)的“將原圖中的每一點(diǎn)的灰度和它周圍八個(gè)點(diǎn)的灰度相加，然后除以9，作為新圖中對(duì)應(yīng)點(diǎn)的灰度”這一操作，我們用如下的表示方法:圖 21模板表示方法這種表示方法有點(diǎn)象矩陣，我們稱其為模板(template)。中間的黑點(diǎn)表示中心元素(此處用*表示)，即，用哪個(gè)元素做為處理后的元素。例如 表示將自身的2倍加上右邊的元素作為新值，而 表示將自身加上左邊元素的2倍作為新值。通常，模板不允許移出邊界，所以結(jié)果圖象會(huì)比

47、原圖小。模板操作實(shí)現(xiàn)了一種鄰域運(yùn)算(Neighborhood Operation)，即，某個(gè)像素點(diǎn)的結(jié)果不僅和本像素灰度有關(guān)，而且和其鄰域點(diǎn)的值有關(guān)。在以后介紹的細(xì)化算法中，我們還將接觸到鄰域運(yùn)算。模板運(yùn)算的數(shù)學(xué)涵義是一種卷積（或互相關(guān)）運(yùn)算。這樣可以濾除圖像中大部分的噪點(diǎn)。同時(shí)，我們根據(jù)賽道的具體信息進(jìn)行慮噪：圖像中黑線必然是連續(xù)的，上一行中黑線位置和本行黑線位置相差太遠(yuǎn)就認(rèn)為本行信息是錯(cuò)誤的，將其保持為上一行的信息。事實(shí)證明這種算法可以比較好的適應(yīng)賽道的具體情況并產(chǎn)生良好的慮噪效果。3. 畸變校正：由于攝像頭看賽道是有一定角度的，實(shí)際上是把賽道上一個(gè)梯形映射為一個(gè)矩形存儲(chǔ)到單片機(jī)中了，畸變

48、校正的目的就是將原來(lái)的梯形還原出來(lái)。畸變校正實(shí)際上是一種坐標(biāo)系的轉(zhuǎn)換?？紤]到圖形從上到下基本是線性的畸變，所以只要每行的點(diǎn)的橫坐標(biāo)乘以不同的系數(shù)就可以達(dá)到畸變校正的目的。如我們的設(shè)想頭看到梯形的上底邊80cm,下底邊35cm只要將最下面的點(diǎn)的橫坐標(biāo) 乘以16，最上面點(diǎn)的橫坐標(biāo)乘以7，并由線性算出中間點(diǎn)的橫坐標(biāo)乘的系數(shù)即可將圖像中的點(diǎn)較好的還原到原來(lái)的位置。當(dāng)然，算法中沒(méi)有考慮縱坐標(biāo)的畸變，是有一定誤差的，但是這種誤差是控制算法可以接受的。4. 圖像邊緣提?。鹤R(shí)別一個(gè)對(duì)象是從其邊緣開(kāi)始的，一幅圖像不同部分的邊緣是模式識(shí)別最重要的特征。在邊沿檢測(cè)中，常用的一種模板是Sobel 算子。Sobel邊緣

49、算子是一種一階差分算子，它可以有效地消除道路圖像中的大部分無(wú)用信息離散Sobel算法的定義如下:公式 210 離散Sobel算法為了編程容易實(shí)現(xiàn)，將Sobel算子轉(zhuǎn)換為兩個(gè)卷積核，分別為水平核和垂直核，圖像中的每個(gè)點(diǎn)都用這兩個(gè)核做卷積，兩處卷積的最大值作為該點(diǎn)的輸出值，運(yùn)算結(jié)果即為經(jīng)過(guò)邊緣增強(qiáng)的圖像。5. 計(jì)算黑線位置：提取出圖像的邊緣以后只要將每行圖像的邊緣進(jìn)行平均就是圖像中黑線的重心位置(Average23)。能夠準(zhǔn)確的辨識(shí)黑線位置對(duì)于下面控制量的給出是至關(guān)重要的。圖 21 采集到的圖像圖 22 經(jīng)過(guò)處理后的圖像1、路徑信息采集處理方法智能車通過(guò)CCD視頻傳感器采集到的路徑信息都是模擬值，

50、然后將一場(chǎng)數(shù)據(jù)的模擬值通過(guò)MC9S12DG128的ATD0通道轉(zhuǎn)換成數(shù)字量，這樣，這一場(chǎng)的數(shù)據(jù)就變成了一個(gè)260*33的二維矩陣，其中的每個(gè)數(shù)字代表著一場(chǎng)圖像中對(duì)應(yīng)位置點(diǎn)的象素灰度值。但是，單片機(jī)處理這么多的數(shù)據(jù)會(huì)十分消耗資源，而且單片機(jī)的內(nèi)存也不可能給這個(gè)存放圖像數(shù)據(jù)的二維矩陣分配太多的存儲(chǔ)空間。所以，為了節(jié)省單片機(jī)的資源和減少處理時(shí)間，就必須對(duì)每場(chǎng)的圖像做一些處理，本隊(duì)采取的方法是，每隔10行進(jìn)行一次處理，這樣，該場(chǎng)圖像的數(shù)據(jù)就變成了一個(gè)26*33二維矩陣。圖5.3就是一條直線的二維矩陣圖。圖5.3 直線賽道圖像數(shù)據(jù)圖接著，就要對(duì)CCD視頻傳感器采集的視頻數(shù)據(jù)進(jìn)行路徑判斷處理，這時(shí)，需要確

51、定視頻閾值的大小。如果象素點(diǎn)灰度值小于該閾值，則確定該點(diǎn)為黑點(diǎn)，否則，確定其為白點(diǎn)。這樣，就可以在一場(chǎng)數(shù)據(jù)中分辨出象素點(diǎn)是黑色還是白色。處理后的結(jié)果如圖5.4所示。圖5.4 處理后的直線賽道圖像數(shù)據(jù)圖 由圖5.3可知，圖像近處的灰度閾值為31，所以，灰度值小于31的點(diǎn)都被判定為黑點(diǎn)，為了調(diào)試方便，把黑點(diǎn)對(duì)應(yīng)的灰度值都置為0。但是，在上數(shù)第8行時(shí)出現(xiàn)了31的灰度值，如果按照近處的灰度閾值來(lái)判斷的話，該點(diǎn)會(huì)被判定為白點(diǎn)，而實(shí)際圖像中該點(diǎn)的位置卻為黑點(diǎn)。因此，為了得到正確的判定結(jié)果，可以將遠(yuǎn)處（圖5.3中為第8行以上）的數(shù)據(jù)的灰度閾值稍作調(diào)整，將其加1，變?yōu)?2，這樣，遠(yuǎn)處的點(diǎn)也可以被判定為黑點(diǎn)，也

52、就符合實(shí)際圖像中的情況了。這里提到的“遠(yuǎn)處”開(kāi)始的位置是通過(guò)實(shí)踐測(cè)定的結(jié)果，是一個(gè)經(jīng)驗(yàn)值。這也 是灰度閾值自適應(yīng)的一種實(shí)現(xiàn)方法。在確定某點(diǎn)是黑（白）點(diǎn)之后，就可以確定黑線的位置了，下面介紹一下具體的實(shí)現(xiàn)方法。因?yàn)槊啃杏?3個(gè)象素點(diǎn)，所以把這33個(gè)點(diǎn)分別標(biāo)號(hào)，記為032。第一次處理的時(shí)候，先按照從左向右的順序，進(jìn)行比較，當(dāng)遇到第一個(gè)黑點(diǎn)的時(shí)候，記下該點(diǎn)的序號(hào)A，接著繼續(xù)比較，當(dāng)遇到第一個(gè)白點(diǎn)的時(shí)候，也記下該點(diǎn)的序號(hào)B，其后的數(shù)據(jù)可以不處理。這樣，將序號(hào)（BA）/2，就可以得到該行黑點(diǎn)的中心位置坐標(biāo)C，（BA）即為該行的黑點(diǎn)個(gè)數(shù)，記為D。以上就是第一行的數(shù)據(jù)，為了提高系統(tǒng)處理的速度，降低系統(tǒng)資源的

53、消耗，以后各行數(shù)據(jù)的處理方式與第一行有所區(qū)別。在處理后面的行時(shí)，可以根據(jù)上一行得出的黑點(diǎn)中心坐標(biāo)C決定比較的順序。例如，如果上一行的黑點(diǎn)中心坐標(biāo)C大于行中心坐標(biāo)16，那么就從右向左開(kāi)始比較。再者，通過(guò)實(shí)踐測(cè)試觀察，攝像頭采集的數(shù)據(jù)中，近處的數(shù)據(jù)一直是比較準(zhǔn)確的，因此，處理的順序是從近向遠(yuǎn)。通過(guò)實(shí)踐測(cè)試，近7行（對(duì)應(yīng)原始圖像的前70行）作為近處數(shù)據(jù)，其它的數(shù)據(jù)作為遠(yuǎn)處數(shù)據(jù)。對(duì)處理過(guò)程再作進(jìn)一步的優(yōu)化方法為，遠(yuǎn)處數(shù)據(jù)的處理范圍為，第8行數(shù)據(jù)參照第7行數(shù)據(jù)得出的黑點(diǎn)中心坐標(biāo)C的位置，將其左右各5個(gè)點(diǎn)作為第8行的待處理數(shù)據(jù)，然后在遵照上面的方法進(jìn)行比較。第9行再參照第8行得出的黑點(diǎn)中心坐標(biāo)這樣，就可以

54、大大減少待處理的數(shù)據(jù)，從而降低了數(shù)據(jù)處理的時(shí)間，加快了系統(tǒng)處理數(shù)據(jù)的速度。經(jīng)過(guò)上述圖像處理后，可以獲得兩個(gè)主要的參數(shù)：每行黑點(diǎn)的中心位置坐標(biāo)和每行的黑點(diǎn)總數(shù)。依據(jù)這兩個(gè)參數(shù)，可以判斷當(dāng)前的路徑信息。經(jīng)過(guò)條件判斷，確定出可靠的最遠(yuǎn)處的一行數(shù)據(jù)作為路徑判斷的依據(jù)，即該行的黑點(diǎn)中心位置坐標(biāo)。2、舵機(jī)轉(zhuǎn)向控制算法經(jīng)過(guò)上面的圖像處理方法，得到了路徑識(shí)別的結(jié)果，記為可靠的最遠(yuǎn)處的一行數(shù)據(jù)黑點(diǎn)中心位置坐標(biāo)。根據(jù)此坐標(biāo)，來(lái)控制舵機(jī)的轉(zhuǎn)向。因?yàn)檫@個(gè)數(shù)據(jù)是遠(yuǎn)處的，所以有一些前瞻性，因此，可以提前做出轉(zhuǎn)向的動(dòng)作，這樣，在急轉(zhuǎn)彎的時(shí)候?qū)?huì)有很大的優(yōu)勢(shì)。為了更好的解決轉(zhuǎn)向的問(wèn)題，本隊(duì)采用查表的方法進(jìn)行控制。設(shè)置一個(gè)一維

55、數(shù)組，其中有33個(gè)元素，對(duì)應(yīng)每行的33行坐標(biāo)位置，不同的坐標(biāo)對(duì)應(yīng)不同的轉(zhuǎn)角。而這33個(gè)轉(zhuǎn)角的大小都是通過(guò)實(shí)際測(cè)量實(shí)驗(yàn)得到的。2.6. 有一定抗干擾和抗反光能力的黑線提取算法(1)原始圖像特點(diǎn)：原始圖像是一個(gè)將模擬圖像經(jīng)A/D轉(zhuǎn)換得到的12030的二位數(shù)據(jù)矩陣，矩陣的每一個(gè)元素對(duì)應(yīng)一個(gè)像素點(diǎn)的灰度值，黑色的灰度值低，白色的灰度值高。圖像的第一行對(duì)應(yīng)最遠(yuǎn)處，大約150cm ,圖像的最低部一行對(duì)應(yīng)最近處，大約20cm。遠(yuǎn)處的圖像小，近處的圖像大，黑線為梯形狀。圖4.4直道圖4.5 彎道圖4.6 S彎圖4.7 手(2)基本思想：a) 直接利用原始圖像，因?yàn)槿绻捎?3均值濾波或中值濾波會(huì)占用大量的計(jì)算時(shí)間，況且在閾值選擇合理的情況下原始圖像黑線比較清晰，容易提取。b) 逐行掃描，小于閾值的點(diǎn)判斷為黑點(diǎn)。c) 黑線寬度有一個(gè)范圍，在確定的黑線寬度范圍內(nèi)才認(rèn)為黑線有效，這樣可以濾除不在寬度范圍內(nèi)的黑點(diǎn)干擾。d) 利用黑線的連續(xù)性來(lái)確定求出的黑線中心