飛思卡爾智能車大獎(jiǎng)賽（電磁組2）軟件控制系統(tǒng)設(shè)計(jì)與開(kāi)發(fā),卡爾,智能,大獎(jiǎng)賽,電磁,軟件,控制系統(tǒng),設(shè)計(jì),開(kāi)發(fā) 飛思卡爾智能車大獎(jiǎng)賽（電磁組 2）軟件控制系統(tǒng)設(shè)計(jì)與開(kāi)發(fā)摘要“飛思卡爾”杯智能車競(jìng)賽從 2016 即第十一屆開(kāi)始改名為恩智浦智能車競(jìng)賽。在規(guī)定了車模的基礎(chǔ)上，使用恩智浦半導(dǎo)體公司指定的單片機(jī)作為控制主芯片，加上傳感器采集模塊、電源模塊、電機(jī)驅(qū)動(dòng)等模塊，結(jié)合與之對(duì)應(yīng)的控制程序，完成能自動(dòng)循跡的智能車。 本課題以 32 位 arm 芯片——MK60DN512ZVLQ10 為控制主芯片。利用陀螺儀與加速度計(jì)的特點(diǎn)，通過(guò)互補(bǔ)濾波，便可獲得兩輪直立小車偏離重心的真實(shí)角度，利用 PD 控制算法，控制電機(jī)來(lái)保持兩輪小車的平衡；用光電編碼器測(cè)得速度脈沖，經(jīng)過(guò)正交解碼使芯片識(shí)別小車速度，然后通過(guò) PI 控制構(gòu)成小車的速度閉環(huán)控制；利用電感檢測(cè)位于賽道下方導(dǎo)線激發(fā)的磁場(chǎng)，通過(guò)歸一化并根據(jù)電感值的變化來(lái)確定小車所處位置，加上陀螺儀的轉(zhuǎn)向分量校正，利用動(dòng)態(tài) PD 控制算法，來(lái)實(shí)現(xiàn)對(duì)小車的方向控制，讓小車自動(dòng)循跡行駛。其中所使用到的編譯軟件是 IAR，數(shù)據(jù)采集、分析軟件 VisualScope、串口獵人，以及芯片解鎖軟件 Jlink4.36。關(guān)鍵詞：PD 平衡控制，PI 速度控制，電磁歸一化，PD 轉(zhuǎn)向控制 FREESCALE CUP INTELLIGENT CAR COMPETITION (ELECTROMAGNETIC GROUP2) SOFTWARE CONTROL SYSTEM DESIGN AND EMPOLDERABSTRACT“Freescale Cup“ smart car competition renamed as NXP smart car competition from 2016 when it has held its eleventh. Specified on the basis of car models, using the NXP designated microcontroller as the main control chip, adding to sensor acquisition module, power module, motor drive and so on, you should complete an automatically tracking smart car with the procedures.32-bit arm chip --MK60DN512ZVLQ10 is used as the main control chip in this subject. Combining with the characteristics of gyroscope and accelerometer and through the complementary filter, you can get two rounds of upright car real Angle which deviates from the center of gravity, and use PD control algorithm to control the motor to keep the car balance. By Photoelectric encoder measuring speed-pulse and then using orthogonal, you can get the car real speed, through using the PI control algorithm to achieve closed-loop control of the speed of car. By taking advantage of inductances to detect the magnetic fields which excited by circuit conductor under the track. By normalizing the value of inductance to determine the location of the car, adding the gyroscope steering component correction and using Dynamic PID control algorithm to achieve the control of direction, the smart car can achieve automatic tracking with these. The compile software is IAR, data acquisition and analysis software is VisualScope and serial hunters, and chip unlock software is Jlink4.36.Key words: PD balance control , PI speed control, electromagnetic normalization, PD steering control目 錄1 緒論 11.1 引言 .11.2 國(guó)內(nèi)外發(fā)展 21.2.1 國(guó)外智能車發(fā)展現(xiàn)狀 21.2.2 國(guó)內(nèi)智能車發(fā)展現(xiàn)狀 21.3 研究意義 .31.4 課題主要內(nèi)容 32 智能車主控系統(tǒng)方案設(shè)計(jì) 52.1 整個(gè)系統(tǒng)方案的設(shè)計(jì) 52.2 主控系統(tǒng)結(jié)構(gòu) 62.3 本章小結(jié) 73 控制算法分析 83.1 PID 相關(guān)算法簡(jiǎn)介 83.1.1 數(shù)字 PID 簡(jiǎn)介 83.1.2 模糊控制方法簡(jiǎn)介 83.2 PID 算法與其改進(jìn)形式的應(yīng)用 83.2.1 PID 控制算法 .83.2.3 平衡、轉(zhuǎn)向 PD 控制 103.2.4 速度 PI 控制 .113.3 本章總結(jié) .124 控制系統(tǒng)設(shè)計(jì) .134.1 平衡控制 .134.2 速度控制 .164.3 方向控制 .194.4 電磁傳感器值數(shù)字濾波 .204.4.1 AD 采集后的限幅濾波 .204.4.2 AD 采集后的中值濾波 .214.4.3 AD 采集后的算術(shù)平均濾波 224.5 電磁 AD 采集處理 224.6 電磁 AD 值利用 244.7 本章小結(jié) .265 調(diào)試與開(kāi)發(fā) .285.1 編譯開(kāi)發(fā)工具 .285.2 調(diào)試工具 .285.3 本章小結(jié) .306 總結(jié)與展望 .316.1 總結(jié) .316.2 展望 .32參考文獻(xiàn) .33致 謝 36附錄 I .37融合角度程序 .37平衡控制電機(jī)程序 .38速度控制輸出程序 .39轉(zhuǎn)向控制程序 .40AD 采集程序 41歸一化程序 .41轉(zhuǎn)向控制部分程序 .41藍(lán)牙聯(lián)調(diào)部分程序 .4411 緒論1.1 引言隨著智能概念被提出來(lái)，越來(lái)越多的智能相關(guān)產(chǎn)品也隨之被生產(chǎn)。例如，近來(lái)倍受青睞的智能家居，雖然是還未實(shí)現(xiàn)像宣傳片里的那種智能控制效果，但是這種觀念一經(jīng)提出，就受到了眾多人的狂熱追捧。它以住宅為集聚平臺(tái)，結(jié)合學(xué)校、商店等社區(qū)，為人們更便利、快捷、高效率的生活提供保障。這一旦實(shí)現(xiàn)，人類的生活將提升至另一個(gè)維度。智能汽車，身為這智能大家族的一員當(dāng)然不能停滯不前，它的成熟也必將為人類的智能生活添上濃重的一筆。概念上智能汽車要實(shí)現(xiàn)的不僅僅是自動(dòng)駕駛功能，而且還擁有舒適良好的用戶體驗(yàn)。但是，完全的自動(dòng)駕駛卻是智能汽車成功的第一步。能為智能汽車提供自動(dòng)駕駛功能的傳感器種類繁多，最常用的有攝像頭，紅外，GPS，聲吶，電磁等。但是，以電磁導(dǎo)航為主要循跡的智能小車，才是智能車舞臺(tái)的贏家。首先，電磁導(dǎo)航相比于其他諸多能實(shí)現(xiàn)導(dǎo)航的傳感器，有著天然的優(yōu)勢(shì)，不易受外界環(huán)境干擾，比如溫度、光照和聲音等，這對(duì)智能汽車所要求的安全，穩(wěn)定導(dǎo)航理念正好相符。其次，采用電磁導(dǎo)航成本低，相比于其他傳感器更具有推廣前景。因此，采用電磁循跡，在智能車領(lǐng)域具有很大的發(fā)展空 [1]。為了進(jìn)一步開(kāi)發(fā)磁導(dǎo)航技術(shù)，吸引眾多的年輕大學(xué)生去接觸、熟悉、發(fā)展這樣的一個(gè)技術(shù)，飛思卡爾智車比賽才增加了以電磁導(dǎo)航為主的智能車競(jìng)賽。比賽時(shí)，在 PVC 賽道上鋪設(shè)一根直徑 0.1-0.8mm 漆包線，同時(shí)在漆包線中通入20kHz，100mA 交流電，從而產(chǎn)生一個(gè)交變的磁場(chǎng)，電磁傳感器可以通過(guò)檢測(cè)磁場(chǎng)有效值的大小來(lái)斷定自己所處的位置從而完成自主循跡 [2]。本次比賽電磁組采用兩輪直立的形式，兩輪平衡車有著諸多優(yōu)點(diǎn)；零半徑轉(zhuǎn)彎；可以通過(guò)自身的調(diào)節(jié)找到重心；減小驅(qū)動(dòng)功率，為電池長(zhǎng)時(shí)間供電提供了可能；避免因?yàn)橹匦倪^(guò)高在汽車啟動(dòng)或急停時(shí)發(fā)生傾倒 [3]。雖然是簡(jiǎn)易的智能車模型，但是它的開(kāi)發(fā)設(shè)計(jì)同樣要運(yùn)用諸多學(xué)科的知識(shí)，例如電路，控制，機(jī)械等領(lǐng)域知識(shí)。還得運(yùn)用一些常見(jiàn)的模塊去實(shí)現(xiàn)電磁組小2車自動(dòng)循跡的能力。同時(shí)通過(guò)這樣的比賽，雖然是三人隊(duì)，但是成員之間的溝通、合作，共同進(jìn)步同樣重要。1.2 國(guó)內(nèi)外發(fā)展1.2.1 國(guó)外智能車發(fā)展現(xiàn)狀國(guó)外對(duì)智能車相關(guān)領(lǐng)域的涉足比較早。初期階段主要是用于軍事領(lǐng)域，以美國(guó)在 20 世紀(jì)研究 Autonomous LandVehicle 為標(biāo)志 [4]。進(jìn)入 21 世紀(jì)，歐美國(guó)家利用舉辦類似的機(jī)器人小車比賽來(lái)推動(dòng)智能車的發(fā)展。通過(guò)這些比賽，智能車的觀念深入人心，世界上汽車巨頭，看到了這其中富含的商機(jī)，爭(zhēng)先恐后地拉起研究組研究智能汽車，并且已經(jīng)取得了一些成就。其中最具代表性的就是Google 公司，目前谷歌公司已經(jīng)成為智能車領(lǐng)域的代表之一，其研發(fā)的自動(dòng)駕駛汽車在電腦的控制下安全行駛了 30 萬(wàn)英里，它主要依靠激光測(cè)距儀、視頻攝像頭、車載雷達(dá)、傳感器等來(lái)識(shí)別道路，確保行駛路徑的正確性。這一跨越性的成果，將智能車技術(shù)的發(fā)展空間再一次提升。1.2.2 國(guó)內(nèi)智能車發(fā)展現(xiàn)狀相比于國(guó)外，國(guó)內(nèi)接觸智能車的時(shí)間比較晚，雖然取得了一些階段性的研究成果，但是相對(duì)于國(guó)外還是有一定的差距，只適用于一些輕場(chǎng)合，比如校園等。對(duì)于一些長(zhǎng)距離、復(fù)雜環(huán)境中的自動(dòng)駕駛，還是有一定程度上的完善。我國(guó)智能車的最新研究成果是 2011 年成功的研發(fā)出紅旗 HQ3，它的出現(xiàn)標(biāo)志著我國(guó)的智能車水平接近了發(fā)達(dá)國(guó)家水平 [5]。自從教育部與飛思卡爾公司簽了相關(guān)智能車競(jìng)賽合約，此項(xiàng)比賽在國(guó)內(nèi)已經(jīng)舉辦了十屆，2016 年第十一屆比賽將在中南大學(xué)舉辦。越來(lái)越多高校大學(xué)生通過(guò)參加比賽，了解智能汽車技術(shù)并進(jìn)行相關(guān)課題的研究，為我國(guó)智能車技術(shù)的再次提升積蓄力量。該課題主要是在飛思卡爾智能車硬件系統(tǒng)良好的情況下給出相應(yīng)的控制算法，增加小車整體的協(xié)調(diào)性，優(yōu)化系統(tǒng)其穩(wěn)定性和適應(yīng)性，然后去比賽。31.3 研究意義（1）通過(guò)參加比賽，將課堂上學(xué)到的理論知識(shí)運(yùn)用到實(shí)際中，更有利于學(xué)生對(duì)知識(shí)的更高層次的掌握。飛思卡爾是一項(xiàng)綜合性的比賽，除了一些與機(jī)械相關(guān)的知識(shí)之外，還學(xué)到了很多與計(jì)算機(jī)、信息有關(guān)專業(yè)知識(shí)，提高了自己綜合實(shí)踐能力。（2）飛思卡爾一個(gè)以團(tuán)隊(duì)為單位參加的競(jìng)賽，要想取得好的成績(jī)，缺不了隊(duì)員之間的密切合作與溝通交流。通過(guò)對(duì)小車的制作，隊(duì)員們提高了團(tuán)隊(duì)意識(shí)，為日后的學(xué)習(xí)工作打下基礎(chǔ)。（3）通過(guò)競(jìng)賽讓更多大學(xué)生了解智能車，熟悉智能車，對(duì)于智能車的普及、發(fā)展起到積極作用。而且該項(xiàng)比賽具有傳承性，隨著學(xué)生參加比賽的次數(shù)增多，學(xué)生對(duì)于智能控制技術(shù)越加成熟，對(duì)于促進(jìn)智能車整體水平的而發(fā)展起到重要作用。1.4 課題主要內(nèi)容學(xué)習(xí)飛思卡爾公司規(guī)定的 32 位比賽芯片，完成對(duì)硬件系統(tǒng)所用到各個(gè)芯片驅(qū)動(dòng)程序設(shè)計(jì)，實(shí)現(xiàn)電磁組控制軟件系統(tǒng)設(shè)計(jì)。首先，單獨(dú)對(duì)單一的模塊進(jìn)行獨(dú)立調(diào)試，接著再結(jié)合各個(gè)模塊，進(jìn)行系統(tǒng)的聯(lián)合調(diào)試。在完成智能車的硬件系統(tǒng)前提下，開(kāi)始編寫與該硬件相對(duì)應(yīng)的算法并調(diào)試，通過(guò)調(diào)試選擇其中最好的算法，完善其中各個(gè)參數(shù)，使系統(tǒng)在硬件良好的情況下，讓控制系統(tǒng)達(dá)到最大的穩(wěn)定，為比賽做好準(zhǔn)備。研究?jī)?nèi)容包括：(1)平衡控制——通過(guò)電機(jī)的正反轉(zhuǎn)保持車模直立狀態(tài) [6]。由于車模采用兩輪直立的形式，這種情況下車模的傾倒只會(huì)發(fā)生在輪子滾動(dòng)方向上，這時(shí)就可以通過(guò)控制輪子電機(jī)的轉(zhuǎn)動(dòng)產(chǎn)生與傾倒方向相同的加速度，抵消在一個(gè)維度傾斜的趨勢(shì)，從而達(dá)到平衡要求 [7]。(2)速度控制——通過(guò)安裝著的編碼器來(lái)測(cè)量車輪轉(zhuǎn)速 [8].速度的控制通過(guò)改變車模的傾角實(shí)現(xiàn)。首先給車模預(yù)設(shè)一個(gè)傾角，在車模角度的反饋調(diào)節(jié)下，車模會(huì)自動(dòng)維持在一個(gè)角度，從而產(chǎn)生傾角方向的加速度，通過(guò)一段時(shí)間的積累，繼而在此方向產(chǎn)生速度。4(3)方向控制——根據(jù)兩車輪的轉(zhuǎn)動(dòng)差速實(shí)現(xiàn)車模轉(zhuǎn)速控制 [8]。小車沿磁場(chǎng)運(yùn)動(dòng)時(shí)，利用各個(gè)電磁傳感器的差異，對(duì)小車速度控制信號(hào)進(jìn)行加減運(yùn)算，形成左、右輪電壓差，從而控制小車的沿磁場(chǎng)行進(jìn)。(4)人機(jī)交互控制——智能車的實(shí)時(shí)狀態(tài)需要被上位機(jī)及時(shí)反映，提高調(diào)試效率，因此十分必要對(duì)智能車進(jìn)行實(shí)時(shí)監(jiān)控 [9]，本課題通過(guò) 12864 液晶屏顯示小車的實(shí)時(shí)狀態(tài)。利用無(wú)線藍(lán)牙模塊與上位機(jī)取得聯(lián)系，另外，我們還可以利用藍(lán)牙對(duì)小車進(jìn)行實(shí)時(shí)調(diào)試，在一定程度上極大提高調(diào)試效率。52 智能車主控系統(tǒng)方案設(shè)計(jì)電磁直立平衡車系統(tǒng)由三個(gè)子部分構(gòu)成；機(jī)械系統(tǒng)、硬件系統(tǒng)、算法程序控制。其中機(jī)械部分主要任務(wù)是確定小車重心，這其中的工作包括安裝電磁傳感器、編碼器、控制 PCB 板以及為找到合適重心位置而用到的配重物等；硬件部分由主控芯片 MK60、電源模塊、電磁采集模塊、驅(qū)動(dòng)電機(jī)模塊等構(gòu)成；同時(shí)，程序控制部分由 AD 處理以及采集后的數(shù)據(jù)處理算法、直立小車的平衡 PD 控制算法、速度閉環(huán) PI 算法、轉(zhuǎn)向 PD 算法、人機(jī)交互模塊等六個(gè)部分組成。2.1 整個(gè)系統(tǒng)方案的設(shè)計(jì)電磁組直立平衡小車主控芯片為飛思卡爾智能車比賽官網(wǎng)提供的 32 位單片機(jī) MK60DN512ZVLQ10 與 E 型車模。在此基礎(chǔ)上通過(guò)設(shè)計(jì)算法去控制一些模塊，完成小車的設(shè)計(jì)，實(shí)現(xiàn)小車的自動(dòng)循跡 [9]。圖 3-1 小車控制系統(tǒng)結(jié)構(gòu)框圖UART正交解碼J-linkPWMI2CADC 采集SPIIO 口62.2 主控系統(tǒng)結(jié)構(gòu)（1） MK60DN512ZVLQ10 芯片 [10]MK60DN512ZVLQ10 芯片擁有 32 位且其 CPU 主頻率達(dá)到 100MHZ，引腳數(shù)高達(dá) 144，這些引腳資源完全足夠小車使用，而不需要考慮復(fù)雜繁瑣的引腳復(fù)用，擁有 ARM Cortex-M4 內(nèi)核。其中內(nèi)部 Flash 和程序空間高達(dá)512KB，而且還有 4KB 的 EEPROM。芯片資源如此豐富，功能足夠強(qiáng)大。同時(shí)擁有兩組各 8 路 16 位 A/D 采集與 12 路 32 位 PWM 輸出。對(duì)于繁多的數(shù)據(jù)采集，復(fù)雜電磁 AD 的處理過(guò)程，毫無(wú)壓力。對(duì)于所剩下的內(nèi)部資源應(yīng)付小車控制，也是十分輕松。它幾乎可以在不超頻的狀態(tài)下，輕易實(shí)現(xiàn) 3ms 內(nèi)執(zhí)行完所有控制程序，并且隨意選中其中四路輸出 PWM 控制電機(jī)正反轉(zhuǎn)。芯片內(nèi)還包含了豐富的外圍模塊 I2C、SPI、UART、USB 等，為取得芯片與外界通訊做出了巨大貢獻(xiàn)。由此，對(duì)于控制一個(gè)性能要求穩(wěn)定、快捷的系統(tǒng)來(lái)說(shuō)，一款功能相當(dāng)?shù)男酒喈?dāng)重要。（2） 藍(lán)牙模塊藍(lán)牙模塊用于單片機(jī)與上位機(jī)之間的數(shù)據(jù)傳輸。當(dāng)處于這種工作模式時(shí)的時(shí)候分為一對(duì)藍(lán)牙模塊就被分為主機(jī)和從機(jī)。 [11]其中以偶數(shù)命名的無(wú)線藍(lán)牙模塊型號(hào)無(wú)法更改主從機(jī)；而以奇數(shù)命名的型號(hào)可以通過(guò) AT 指令修改模塊，任意指定為主機(jī)或者從機(jī)。本課題使用的是 HC-05 為奇數(shù)型號(hào)方式，可以通過(guò) AT 指令指定完主從機(jī)后，利用串口就可以接入電腦，配對(duì)完畢即可通信。（3） 光電編碼器光電編碼器利用光電效應(yīng)原理，將角度、轉(zhuǎn)速、位置等量轉(zhuǎn)換為電信號(hào)并加以輸出的一種傳感器 [12]。（4） 陀螺儀加速度計(jì)陀螺儀是利用壓電陶瓷做震動(dòng)單元，當(dāng)陀螺儀旋轉(zhuǎn)時(shí)，震動(dòng)頻率會(huì)隨著變化，根據(jù)這些震動(dòng)就可以得到旋轉(zhuǎn)角速度。加速度計(jì)是利用微機(jī)械在兩個(gè)電極之間加入一個(gè)懸臂梁，與相鄰的電極構(gòu)成兩個(gè)電容 [13]。（5） 電磁傳感器7交變磁場(chǎng)產(chǎn)生交變的電場(chǎng)，電磁傳感器是利用纏繞的線圈去切割磁場(chǎng)，獲得交流電，產(chǎn)生信號(hào)。2.3 本章小結(jié)本章主要講述了直立兩輪小車實(shí)現(xiàn)控制的總體模塊，為控制智能車做出了一定的指引。課題可以沿著各個(gè)模塊開(kāi)始，實(shí)現(xiàn)各個(gè)模塊的控制，從而進(jìn)一步實(shí)現(xiàn)對(duì)整體的控制。83 控制算法分析3.1 PID 相關(guān)算法簡(jiǎn)介3.1.1 數(shù)字 PID 簡(jiǎn)介在眾多控制算法中，PID控制是較常見(jiàn)與經(jīng)典的控制算法 [1]。其算法形式簡(jiǎn)單、基本不要控制對(duì)象復(fù)雜而繁瑣的動(dòng)態(tài)模型，加上在線調(diào)試方便，因此被廣泛應(yīng)用于過(guò)程控制和運(yùn)動(dòng)控制中。數(shù)字PID算法主要有三個(gè)參數(shù)，它們分別為P表示比例控制器，I代表積分控制器，D代表微分控制器。3.1.2 模糊控制方法簡(jiǎn)介模糊控制算法理論基礎(chǔ)模糊理論，是一種通過(guò)對(duì)傳統(tǒng)PID算法的改進(jìn)而獲得的控制算法，這樣的作法簡(jiǎn)化了系統(tǒng)的繁雜性。通過(guò)劃分模糊集，使控制系統(tǒng)不過(guò)分依賴于控制對(duì)象的精確數(shù)學(xué)模型，但如何確定模糊規(guī)則和系統(tǒng)設(shè)計(jì)方法，卻是過(guò)分依賴經(jīng)驗(yàn)。因此，模糊控制適用于非線性、滯后、時(shí)變和模型不完全系統(tǒng)的控制 [10]。3.2 PID 算法與其改進(jìn)形式的應(yīng)用3.2.1 PID 控制算法飛思卡爾智能車系統(tǒng)為了實(shí)現(xiàn)較高精度的控制，基本上實(shí)行閉環(huán)控制。閉環(huán)控制系統(tǒng)利用輸出量與輸入量之間的反饋來(lái)增加系統(tǒng)的精度，通過(guò)減少系統(tǒng)的不確定性，來(lái)提高系統(tǒng)的抗干擾能力。通過(guò)比較系統(tǒng)當(dāng)前輸出量與輸入量期望值的偏差，利用此偏差來(lái)糾正系統(tǒng)，從而獲得預(yù)期輸出控制量。其中最簡(jiǎn)單、最通用的閉環(huán)校正裝置就是 PID 校正裝置或 PID 控制器。其中 PID 控制器又被分為位置式與增量式兩種。9圖 3-1 PID 控制示意圖為了提高控制精度，在位置式 PID 算式中添上 （控制補(bǔ)償量）后，算式??0如下；（3-1）??00])(1)([xdteTeTteKutip ?????式中： ——比例增益p——積分時(shí)間常數(shù)????——微分時(shí)間常數(shù) ????——控制輸出量()ut——輸出量與輸入量的偏差 e——控制補(bǔ)償量0x設(shè)裝置采樣周期是 T，上式 PID 算式可做離散差分方程變化；（3-2）001])()([xeTeteKxkjkdip ??????因此，對(duì)于增量式 PID 算式，可根據(jù)式（3-2）得到（3-3）002111 ])([ xeTeeKxkj kdjikpk ???????則有10（3-4）)]2([2111???????kk kdkipkk eeTKxx正因?yàn)槭街杏玫降?k 次、第 k－1 次兩次輸出結(jié)果的變化量，所以稱為增量式 PID 算式。利用這個(gè)變化量，控制系統(tǒng)完成相應(yīng)的輸出控制量。上式還可改寫成；（3-5）21?????kkkkCeBAex其中：（3-6）)1(TKAdip??（3-7）)2(Bdp（3-8）TKCdp?增量式與位置式 PID 的比較：（1） 增量式 PID 無(wú)須經(jīng)過(guò)累加處理，所以比位置式 PID 的累積誤差小。即增量式 PID 精度較高（2） 增量式算法只需輸出限幅，而位置式還要設(shè)置積分限幅。因此，增量式編程方便。（3） 增量式算法，通過(guò)與前一次做偏差即可得出結(jié)果，避免存儲(chǔ)每一次的偏差，對(duì)芯片的內(nèi)存占據(jù)較小。（4） 位置式算式簡(jiǎn)單明了，結(jié)構(gòu)清晰，參數(shù)容易調(diào)整。3.2.3 平衡、轉(zhuǎn)向 PD 控制在小車的平衡與轉(zhuǎn)向控制中采用比例微分控制（PD）。因?yàn)樾≤噷?shí)現(xiàn)自動(dòng)循跡，最重要的兩個(gè)控制就是平衡與轉(zhuǎn)向控制，通過(guò)比例微分控制，會(huì)令輸出11信號(hào) Y(S)具有超前于輸入信號(hào) R(S)的相位的特性，最大超前相位角度 90°，幅值改變了。但是對(duì)于頻率卻是完完整整保存了下來(lái)。通過(guò) PD 控制器可以改善系統(tǒng)的動(dòng)態(tài)特性性能。圖 3-2 PD 控制示意圖對(duì)于小車的直立平衡控制，對(duì)系統(tǒng)的響應(yīng)速度有較高的要求，因此 PD 控制可以保證對(duì)系統(tǒng)的穩(wěn)定性、快速性的要求。相反加入了積分控制，使系統(tǒng)的穩(wěn)定裕量、快速性降低。因此，直立平衡控制用 PD 控制。具體的調(diào)試方法是先調(diào)節(jié) P，然后調(diào)節(jié) D。P 值從小到大緩慢增加，直道小車出現(xiàn)抖動(dòng)，無(wú)法直立為止，這時(shí)取此時(shí) P 值的 75%左右值。然后再緩慢增加D，直至小車又出現(xiàn)顫動(dòng)為止。這時(shí)再反過(guò)來(lái)再增加 P 值，反復(fù)調(diào)節(jié) PD 值，直至最后小車直立反應(yīng)速度夠快，看干擾能力較大為止。這時(shí)，直立調(diào)節(jié)的 PD 值就算完成了。對(duì)于轉(zhuǎn)向調(diào)試 PD 值的方法與直立類似，只是注意彎道，直道 PD 值的過(guò)渡不可太大，否則容易因?yàn)槠贫剐≤噧A倒。3.2.4 速度 PI 控制速度控制使用了 PI 控制。對(duì)于速度閉環(huán)就不需要那樣快速性，需要的是速度變化的平滑性更重要，因此加入 I 滯后調(diào)節(jié)，增加了平穩(wěn)性。可以保證小車在入彎和出彎時(shí)速度改變量較大，小車系統(tǒng)對(duì)速度的控制大，減緩過(guò)彎道時(shí)的速度，使小車順利過(guò)彎；同時(shí)在小車啟動(dòng)時(shí) PI 控制表現(xiàn)了良好的控制要求，在啟動(dòng)時(shí)刻，速度會(huì)以比較平穩(wěn)的速度緩慢增加，對(duì)直立這個(gè)基本閉環(huán)起到很大的作用，增加了小車的穩(wěn)定性。12圖 3-3 PI 控制示意圖具體的調(diào)試方法是 I 值，用手推小車，因?yàn)樾≤嚻x了原始位置，產(chǎn)生了誤差，這時(shí)積分開(kāi)始作用，小車會(huì)在積分的作用下，在原點(diǎn)附近反復(fù)擺動(dòng)，擺動(dòng)幅度越來(lái)越大。這時(shí)添加 P 環(huán)節(jié)，會(huì)看到小車擺動(dòng)幅度變小，隨著 P 的增大，用手推小車，小車會(huì)一次停在原點(diǎn)，甚至是推不動(dòng)小車。這時(shí)直立閉環(huán)就算調(diào)試完成。3.3 本章總結(jié)本章介紹了小車控制中算法選取以及調(diào)試方法，小車能夠行走，三個(gè)閉環(huán)之間的聯(lián)系仍是十分重要，為了小車能達(dá)到能以穩(wěn)定速度循跡的目的要反復(fù)調(diào)節(jié)這些參數(shù)，并不是毫無(wú)聯(lián)系。134 控制系統(tǒng)設(shè)計(jì)相對(duì)于四輪行走，直立狀態(tài)下的控制更加復(fù)雜。從控制角度來(lái)說(shuō)，車模為一個(gè)控制對(duì)象，根據(jù)后輪的調(diào)節(jié)實(shí)現(xiàn)車模的平衡控制，速度控制，方向控制。其中直立平衡控制為最基本控制，只有在小車直立平衡的基礎(chǔ)上，小車才能繼而完成剩余的兩個(gè)控制。而速度控制，建立在平衡的基礎(chǔ)上，利用速度閉環(huán)調(diào)節(jié)，可以控制小車按著預(yù)定速度勻速前進(jìn)；轉(zhuǎn)向閉環(huán)控制使小車實(shí)現(xiàn)自動(dòng)循跡?？傁到y(tǒng)控制框圖見(jiàn)下；圖 4-1 總控制系統(tǒng)框圖4.1 平衡控制當(dāng)?shù)沽[物體偏離自身的平衡位置時(shí)，就會(huì)在重力的作用下，倒向偏離的那邊。若欲令倒立擺垂直穩(wěn)定在垂直位置，只能通過(guò)向另外方向增加與重力作用相抵消的力，即可以通過(guò)控制倒立擺，使它往傾倒方向做加速運(yùn)動(dòng)，增加一個(gè)額外與重力作用相互抵消的力，這樣就可以使倒立擺穩(wěn)定平衡在垂直位置；14圖 4-2 倒立擺控制原理所需要力 F 大?。海?-1 ）??=????sin???????cos??≈?????????????存在空氣阻尼等，5-1 式便可以簡(jiǎn)化成（4-2 ）??=???????????1???????2??'其中 為車輪加速度 a 與偏角 的比值，則可簡(jiǎn)化加速度 a；??1 ??（4-3 ）??=??1??+??2??'上式中 g， ， 均為比例系數(shù)。因此可以得出小車平衡??1 ??20的量 ??1、 ??2與真實(shí)偏角 有關(guān)。??欲求得平衡控制的傳遞函數(shù)可建立類似，倒立擺的模型。車模在運(yùn)動(dòng)時(shí)的方程：????2??(??)??2(??)=??sin?????(??)cos??+????(??) (4?4)加上附加條件，小車處于靜止?fàn)顟B(tài)且擺動(dòng) ? 很小時(shí)，(4-4)中可簡(jiǎn)化成：????2??(??)??2(??)=g??(??)+????(??) (4?5)由此得到傳遞函數(shù)：??(??)=??(??)??(??)= 1??2????? (4?6)15該傳遞函數(shù)含有兩個(gè)極點(diǎn) S= ，S= ，其中一個(gè)位于右半平面。因????? +????此，系統(tǒng)不穩(wěn)定。引入比例—微分反饋之后，可改觀這情況。L + ?(S)+X(s)圖 4-3 直立平衡控制框圖由程序框圖可得：??(??)=??(??)??(??)= 1??2+??2????+(??1???)?? (4?7)求得極點(diǎn) ,當(dāng)極點(diǎn)全位于左半平面時(shí)，有????=???2±√（ ??22?4??（ ??1???）2????20,??1,即此時(shí)系統(tǒng)穩(wěn)定。在這樣的反饋中， 相當(dāng)于比例，而 相當(dāng)于微分控制?? ??1 ??2器，具有阻尼作用。比例值的大小表示了系統(tǒng)調(diào)控作用的強(qiáng)弱，而微分可以減少小車的震蕩。如果想準(zhǔn)確反映小車實(shí)時(shí)的真實(shí)角度，必須要陀螺儀與加速度計(jì)相互結(jié)合。MPU6050 + 積 分比 例K2K2角 速 度 ? 角 速 度 ? +++ 加 速 度 計(jì)電 機(jī)驅(qū) 動(dòng)陀 螺 儀圖 4-4 直立平衡控制框圖因?yàn)榧铀俣扔?jì)雖然測(cè)量的是小車的真實(shí)角度，但是它易受外部干擾，所以只利用加速度計(jì)測(cè)小車角度，會(huì)得到毛刺十分明顯的曲線。相反的，陀螺儀測(cè)量的是小車的角速度，它可以通過(guò)積分獲得真實(shí)角度，因此角度曲線光滑，但g)1(????2???1+??2??16是它的測(cè)量值隨時(shí)間的變化相對(duì)較大，而且受溫度的影響大。它們優(yōu)缺點(diǎn)互補(bǔ)，利用加速度計(jì)每過(guò) 5ms 就去糾正陀螺儀的值，這樣就可以獲得一條即光滑又準(zhǔn)確的角度曲線。融合效果圖見(jiàn)下：圖 4-4 融合角度后的真實(shí)角度融合角度程序見(jiàn)附錄 I：得到能真實(shí)反映小車傾角的融合曲線之后，將實(shí)際的角度值與預(yù)設(shè)角度值相減，得出它們的變化量，預(yù)設(shè)角度值即為小車靜止時(shí)平衡位置的角度值。關(guān)于預(yù)設(shè)角度值的產(chǎn)生主要是因?yàn)橥勇輧x安裝位置與小車靜止重心發(fā)生偏離，預(yù)設(shè)角度值越大代表陀螺儀與重心位置偏離越多；反之，值越小，表示偏離越小。同時(shí)，陀螺儀采集到的為偏差的微分。通過(guò) PD 控制輸出相應(yīng)的 PWM，控制電機(jī)輸出，對(duì)小車進(jìn)行直立平衡控制。 通過(guò) FTM 通道將直立控制的 PWM 波輸入驅(qū)動(dòng)模塊，來(lái)實(shí)現(xiàn)電機(jī)的正反轉(zhuǎn)，給將要傾倒的方向，提供平衡所需要的加速度，令小車實(shí)現(xiàn)垂直平衡控制。電機(jī)程序見(jiàn)附錄 I：4.2 速度控制由于小車采用兩輪直立的形式，所以不能通過(guò)控制電機(jī)轉(zhuǎn)速來(lái)調(diào)節(jié)小車的速度。在平衡控制的基礎(chǔ)上，給車模預(yù)設(shè)一個(gè)傾角，加上車模角度的反饋調(diào)節(jié)，車模會(huì)自動(dòng)維持在這個(gè)角度，從而產(chǎn)生速度。換一句話說(shuō)，即可以通過(guò)改變小車的傾角來(lái)產(chǎn)生加速度，從而實(shí)現(xiàn)速度控制。17簡(jiǎn)化小車平衡控制模型，在速度控制系統(tǒng)中，把小車平衡控制化簡(jiǎn)為一個(gè)V 一階慣性環(huán)節(jié) ，輪子速度為傾角 變化得導(dǎo)數(shù)乘以陀螺儀安裝位置1??2??+1 (??2(??))到輪子的距離，即式子：(4-8)??(??)=?????2(??)????(4-9)??(??)=???2(??)??控制框圖如下：+K-S圖 4-5 速度控制框圖由此可得：(4-10)??(??)=??(??)(??2???)??+1 方程中具有一個(gè)極點(diǎn) S= ，系統(tǒng)若要保持穩(wěn)定，則必須保證 ，?1??2??? ??2??為系統(tǒng)時(shí)間常數(shù)，??2由于小車速度是通過(guò)傾角的加速度積分獲得，由此可得速度控制框圖；+K圖 4-6 加積分后速度控制框圖(4-11)??(??)= ??(??)??2??2+(????+1)??+?? 由此得出兩個(gè)方程的極點(diǎn) ,小車系統(tǒng)保持穩(wěn)定則??= 221)4dKT????應(yīng)盡量大。然而， 過(guò)大會(huì)導(dǎo)致兩個(gè)極點(diǎn)實(shí)部太小，易引起速度的震蕩。要2T2T??(??) 1??2??+1 ??(??)??(??) 1??2??+11??18消除這種震蕩，可以通過(guò)加入微分調(diào)節(jié)器,令極點(diǎn)遠(yuǎn)離虛軸，增加系統(tǒng)的穩(wěn)定性。+KKdS圖 4-7 加積分、微分速度控制框圖實(shí)際中，編碼器傳遞回來(lái)的信號(hào)中，由于受到電機(jī)影響，信號(hào)中附帶干擾信號(hào)，干擾信號(hào)會(huì)隨著微分運(yùn)算，會(huì)被放大，造成系統(tǒng)的不精確。因此，可以將小車系統(tǒng)看作一個(gè)積分環(huán)節(jié)，將最初的積分環(huán)節(jié)與微分環(huán)節(jié)融合，看作是一個(gè)比例環(huán)節(jié)。這樣既能保證系統(tǒng)傳遞函數(shù)不變，又能避免因微分運(yùn)算帶來(lái)的附加干擾。具體控制框圖；MPU6050 + 積 分比 例K2K2角 速 度 ? 角 速 度 ? +++加 速 度 計(jì)驅(qū) 動(dòng) 電 機(jī)車 模 保 持 給定 的 傾 角陀 螺 儀+速 度 控 制 D速 度 控 制P1/S+-+速 度 反 饋給 定 速 度 速 度 檢 測(cè)圖 4-8 角度、速度閉環(huán)控制框圖因此，對(duì)小車的控制使用的是 PI 控制中，其中 P 包括了微分效果，所以相當(dāng)于具有阻尼與系統(tǒng)控制強(qiáng)弱作用。當(dāng)在彎道上行駛時(shí)，p 值越大，則引起的速度變化越大，從而容易引起系統(tǒng)的震蕩。I 代表系統(tǒng)的精度，即是否回到原點(diǎn)相關(guān)，當(dāng)產(chǎn)生速度偏差時(shí)，I 就開(kāi)始積累，為了避免 I 積累越來(lái)越大，可以通過(guò)對(duì) I 的適當(dāng)限幅處理，使 I 穩(wěn)定在某一范圍，從而保證小車的穩(wěn)定。??(??) 1??2??+11??19同時(shí)為了保證小車速度輸出具有平滑性，以增加小車運(yùn)動(dòng)時(shí)的穩(wěn)定性，對(duì)速度輸出進(jìn)行低通濾波處理與速度平滑輸出處理。通過(guò)低通濾波處理，速度的最終值會(huì)顯得更加平滑，速度值的抖動(dòng)不會(huì)太大。小車不會(huì)因?yàn)橐粫r(shí)的錯(cuò)誤數(shù)據(jù)，比如光電編碼器輸出異常，電磁傳感器檢測(cè)失誤或者程序執(zhí)行中的問(wèn)題，而造成對(duì)速度控制太大影響；當(dāng)小車過(guò)彎道的時(shí)候，可以減少因?yàn)檩喿硬恍⌒钠贫a(chǎn)生的某個(gè)瞬間值太大，對(duì)小車的平衡、速度控制造成的影響。速度平滑輸出處理，芯片每過(guò) 5ms 逐步給電機(jī)增加轉(zhuǎn)速，讓預(yù)設(shè)速度與實(shí)際速度逐漸接近，避免了小車因?yàn)橐幌陆o予過(guò)大速度而出現(xiàn)嚴(yán)重抖動(dòng)，致使平衡破壞的情況。速度控制輸出程序見(jiàn)附錄 I：4.3 方向控制為了實(shí)現(xiàn)小車的自動(dòng)循跡，小車的方向控制尤其重要。然而，方向控制必須在平衡控制與速度控制比較良好的前提下，換一句話說(shuō)就是在小車能夠直立，并且能夠以一定的速度前進(jìn)的條件下，實(shí)現(xiàn)方向控制。根據(jù)兩個(gè)電機(jī)的轉(zhuǎn)動(dòng)差速實(shí)現(xiàn)車模轉(zhuǎn)向控制。小車沿磁場(chǎng)運(yùn)動(dòng)，利用電磁信號(hào)的偏差小車速度控制信號(hào)進(jìn)行加減運(yùn)算，形成左、右輪電壓差，再通過(guò)芯片把電壓差反應(yīng)到左右輪電機(jī)，從而控制小車的轉(zhuǎn)向。轉(zhuǎn)向控制程序框圖：圖 4-9 方向控制框圖為增加方向輸出的平滑性，采用相類似于與速度閉環(huán)的平滑輸出做法，這樣同樣可以增加小車在轉(zhuǎn)向過(guò)程中的穩(wěn)定性。轉(zhuǎn)向控制程序見(jiàn)附錄 I;204.4 電磁傳感器值數(shù)字濾波要實(shí)現(xiàn)小車的自動(dòng)循跡，在裝好電磁傳感器的前提下，對(duì)電磁信號(hào)的處理、利用相當(dāng)重要。由于不同位置，不同電感，所檢測(cè)到的磁場(chǎng)都是不一樣的，為了做到小車對(duì)賽道的準(zhǔn)確識(shí)別，需要不斷地采集賽道的磁場(chǎng)信息。通過(guò)利用K60 芯片中有現(xiàn)成的 AD 模塊，采用數(shù)組來(lái)存放采集到的 AD 值，來(lái)實(shí)現(xiàn)對(duì)電磁信號(hào)的相關(guān)處理。采集程序見(jiàn)附錄 I：僅僅傳感器采集的 AD 值有著相當(dāng)大的干擾信息，因此需要對(duì) AD 值進(jìn)行濾波處理以降低干擾信息，提高小車的識(shí)別度。此次，使用的是數(shù)字濾波方式，相比于模擬濾波裝置，數(shù)字濾波有以下幾個(gè)優(yōu)點(diǎn)：（1） 通過(guò)程序的形式來(lái)到達(dá)濾波效果，很大程度上避免了硬件濾波的不穩(wěn)定性，提高了 AD 采集的可靠性。（2） 數(shù)字濾波含有多種不同方式的濾波算法，更加靈活，適用性更好。（3） 數(shù)字濾波可實(shí)現(xiàn)多通道共用，提高 AD 采集的效率。4.4.1 AD 采集后的限幅濾波限幅濾波又被稱為程序判斷濾波法，就是求出前后兩次 AD 值的變化量，然后與兩次采樣允許的最大偏差值 作對(duì)比。如果小于等于 ，則取本次△ ?? △ ??采樣值有效；如果大于 ，則此次采樣值無(wú)效，取上一次采樣值 [25]?！???即：, 則 ,取本次采樣值|??????????1|≤Δ?? ????=???? ????, 則 ，取上次采樣值|??????????1|Δ?? ????=?????1 ?????1使用限幅濾波對(duì)采集回來(lái)的 AD 值進(jìn)行處理之后，能有效減少因?yàn)榕既灰蛩囟鸬?AD 值突變。但是，它卻無(wú)法消除系統(tǒng)干擾，特別是周期性的干擾，容易引起突變，不利用小車數(shù)據(jù) AD 采集的平滑。4.4.2 AD 采集后的中值濾波21個(gè) 個(gè)個(gè) 個(gè)個(gè) 個(gè)個(gè) 個(gè) 個(gè) 個(gè) 個(gè) 個(gè)個(gè) 個(gè) 個(gè) 個(gè) 個(gè) 個(gè) 個(gè) 個(gè)個(gè) 個(gè) 個(gè)個(gè) 個(gè) 個(gè) 個(gè)個(gè) 個(gè) 個(gè) 個(gè) 個(gè) 個(gè)個(gè) 個(gè)個(gè)個(gè)圖 4-10 AD 采集后的中值濾波示意圖AD 采集后的中值濾波法就是使用芯片 AD 模塊功能，首先設(shè)置連續(xù)采樣次數(shù) n（一般為奇數(shù)，本課題中對(duì)賽道同一點(diǎn) AD 采集 27 次），然后把采集到的AD 值按順序排列，取這組數(shù)的中間值當(dāng)作本次采集的有效值。利用中值濾波取 AD 采集有效值，能有效克服因?yàn)榕既灰蛩囟鸬挠行е蹈蓴_，特別是對(duì)溫度等變化緩慢的被測(cè)量，有良好的濾波效果，但是對(duì)流量等快速變化的被測(cè)量不宜采用。此次課題中，結(jié)合 AD 數(shù)值的歸一化，可以把電磁的快速變化，穩(wěn)定限制在[0,1]之間，相當(dāng)于把被測(cè)量的變化率變緩，正好符合中值濾波的條件。因此，此次課題采用中值濾波對(duì) AD 進(jìn)行處理，實(shí)現(xiàn)表明小車能夠有效識(shí)別賽道，完成循跡。224.4.3 AD 采集后的算術(shù)平均濾波算術(shù)平均濾波就是連續(xù)進(jìn)行 AD 采集，采樣次數(shù)不作規(guī)定設(shè)為 n，然后對(duì)所有采樣值進(jìn)行算術(shù)平均運(yùn)算，取其平均值作為有效值。此種方法的特點(diǎn)；當(dāng) N 值較大時(shí)：AD 值連續(xù)性平滑度較高，偶然的電磁干擾對(duì)小車的控制影響較小，同時(shí)也會(huì)導(dǎo)致小車靈敏度降低。當(dāng) N 值較小：信號(hào)平滑度與連續(xù)性差，干擾對(duì)小車系統(tǒng)的影響非常大，很容易出現(xiàn)誤判的現(xiàn)象。所以，雖然靈敏度好，卻會(huì)出現(xiàn)小車因?yàn)閷ふ液线m的區(qū)間值，而不停擺動(dòng)傳感器，使系統(tǒng)穩(wěn)定性變差，無(wú)法完成循跡。此種濾波比較適用于具有一般隨機(jī)干擾的信號(hào)，比如信號(hào)圍繞平均值范圍附近上下浮動(dòng)。但是，對(duì)于要求測(cè)量、計(jì)算速度快的電磁小車控制不合適，而且大大浪費(fèi)芯片 RAM 空間。 4.5 電磁 AD 采集處理為了保證小車能比較準(zhǔn)確的識(shí)別賽道，較好的適應(yīng)環(huán)境，可以通過(guò)歸一化，把電磁的采集值限定在[0,1]之間，有利于單片機(jī)找到控制的最優(yōu)解，增加程序運(yùn)行時(shí)的收斂性，提高程序運(yùn)行效率，降低芯片的利用率。另一方面，也能提高小車的適應(yīng)性，去除由于信號(hào)源的不穩(wěn)定而引起的干擾。數(shù)據(jù)歸一化公式為（測(cè)試值-最小值）/（最大值-最小值），這樣就可以把電磁傳感器的采集值限定在[0,1]之間。在處理時(shí)會(huì)出現(xiàn)負(fù)數(shù)或者大于 1 的情況，這時(shí)可以通過(guò)設(shè)定閾值，把歸一化后的值強(qiáng)制限制在[0,1]之間，具體做法是：出現(xiàn)負(fù)數(shù)時(shí)就把這時(shí)的歸一化值用 0 來(lái)計(jì)算，當(dāng)出現(xiàn)大于 1 時(shí)，把大于 1 的值認(rèn)為是 1。歸一化過(guò)程圖如圖 4-823開(kāi) 始0?1? = 0AD有 效 值結(jié) 束=1YNYN歸 一 化圖 4-11 歸一化過(guò)程圖為了能夠有明顯的區(qū)分性，可以將[0,1]區(qū)間放大，即將歸一化后的值都乘以 100，這樣就把區(qū)間設(shè)置成[0,100]。經(jīng)過(guò)這樣的處理后，AD 采集到的最大值和最小值，可以較好地避免因信號(hào)源不同或者因不同傳感器之間的輕微誤差，而導(dǎo)致的小車適應(yīng)性差問(wèn)題，同時(shí)也優(yōu)化尋找最優(yōu)解的途徑。在進(jìn)行電磁組的歸一化時(shí)，利用也就是 AD 值。這里需要說(shuō)明的是，關(guān)于最大值與最小值的設(shè)定，每一組都不是固定的，只要找到適合利于調(diào)試的值就可以。部分歸一化程序見(jiàn)附錄 I：采集后的有效值，經(jīng)過(guò)歸一化后，克服了電磁數(shù)據(jù)快速變化的缺點(diǎn)。再利用中值濾波，就能使小車以較好的狀態(tài)去識(shí)別賽道，為循跡打下良好的基礎(chǔ)。244.6 電磁 AD 值利用對(duì)于磁場(chǎng)數(shù)據(jù)的采集其最終目的是實(shí)現(xiàn)小車的自動(dòng)循跡，因此，對(duì)于歸一化后 AD 值與小車方向控制尤其重要。電感分布圖：大 S 位置一 小 S 位置二 直道 位置三 小 S 位置四 大 S電感 1 電感 2AD0 AD3圖 4-12 電感分布圖方案中利用電感 1、電感 2，以實(shí)現(xiàn)對(duì)小車的循跡控制。利用公式：error12=（(AD[0]-AD[3])*1000）/ ((AD[0]+AD[3])*10) （4-12）作為小車循跡行駛的判斷依據(jù)，這樣即可以避免角度的影響，又可以減少小車循跡時(shí)順時(shí)針與逆時(shí)針的一致性，適應(yīng)性更大。利用處于不同位置時(shí)兩個(gè)傳感器的值不同，而使 error12 值的不同，同時(shí)給 error 值的變化劃定四個(gè)位置狀態(tài)，通過(guò)位置狀態(tài)的不同，來(lái)定義小車所處于的不同賽道位置。不同賽道位置所用到的轉(zhuǎn)向 PD 不同，同時(shí)對(duì)于轉(zhuǎn)向時(shí) error 所起到的轉(zhuǎn)向權(quán)重也是不同，即權(quán)重越大，轉(zhuǎn)向趨勢(shì)越大。位置二、位置三定義為直道。此時(shí)，小車不轉(zhuǎn)向，是勻速運(yùn)動(dòng)。 圖 4-13 直道示意圖位置一或位置四之外定義為大 S 彎道，這時(shí)轉(zhuǎn)向的 PD 值最大。25圖 4-14 大 S 彎道示意圖位置一、位置二與位置三、位置四定義為小 S 彎道，但是由于這時(shí)的范圍相對(duì)于較大，因此具體做法是通過(guò)細(xì)分把小 S 彎道又分為小 S1，小 S2,小 S3 分別進(jìn)行處理。同時(shí)，又因?yàn)閷?duì)于大 S 彎道的判斷，單片機(jī)是先判斷出小 S，進(jìn)而判斷大 S，因此有必要稍微擴(kuò)大對(duì)小 S 的判定范圍，提高電磁傳感器的預(yù)判性，實(shí)現(xiàn)對(duì)大 S 彎道完全通過(guò)。圖 4-15 小 S 彎道示意圖利用對(duì)小 S 的細(xì)分還可以順利實(shí)現(xiàn)直道轉(zhuǎn)彎道的 PD 轉(zhuǎn)向過(guò)渡，不會(huì)輕易由于轉(zhuǎn)向 PD 變化大而導(dǎo)致轉(zhuǎn)彎過(guò)大，引起漂移，實(shí)現(xiàn)平滑循跡的目的。轉(zhuǎn)向控制流程圖見(jiàn)圖 4-10；轉(zhuǎn)向控制部分程序見(jiàn)附錄 I：