飛思卡爾智能車大獎賽（電磁組2）軟件控制系統(tǒng)設(shè)計與開發(fā)

飛思卡爾智能車大獎賽（電磁組2）軟件控制系統(tǒng)設(shè)計與開發(fā),卡爾,智能,大獎賽,電磁,軟件,控制系統(tǒng),設(shè)計,開發(fā) 飛思卡爾智能車大獎賽（電磁組 2）軟件控制系統(tǒng)設(shè)計與開發(fā)摘要“飛思卡爾”杯智能車競賽從 2016 即第十一屆開始改名為恩智浦智能車競賽。在規(guī)定了車模的基礎(chǔ)上，使用恩智浦半導(dǎo)體公司指定的單片機作為控制主芯片，加上傳感器采集模塊、電源模塊、電機驅(qū)動等模塊，結(jié)合與之對應(yīng)的控制程序，完成能自動循跡的智能車。 本課題以 32 位 arm 芯片——MK60DN512ZVLQ10 為控制主芯片。利用陀螺儀與加速度計的特點，通過互補濾波，便可獲得兩輪直立小車偏離重心的真實角度，利用 PD 控制算法，控制電機來保持兩輪小車的平衡；用光電編碼器測得速度脈沖，經(jīng)過正交解碼使芯片識別小車速度，然后通過 PI 控制構(gòu)成小車的速度閉環(huán)控制；利用電感檢測位于賽道下方導(dǎo)線激發(fā)的磁場，通過歸一化并根據(jù)電感值的變化來確定小車所處位置，加上陀螺儀的轉(zhuǎn)向分量校正，利用動態(tài) PD 控制算法，來實現(xiàn)對小車的方向控制，讓小車自動循跡行駛。其中所使用到的編譯軟件是 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 國內(nèi)外發(fā)展 21.2.1 國外智能車發(fā)展現(xiàn)狀 21.2.2 國內(nèi)智能車發(fā)展現(xiàn)狀 21.3 研究意義 .31.4 課題主要內(nèi)容 32 智能車主控系統(tǒng)方案設(shè)計 52.1 整個系統(tǒng)方案的設(shè)計 52.2 主控系統(tǒng)結(jié)構(gòu) 62.3 本章小結(jié) 73 控制算法分析 83.1 PID 相關(guān)算法簡介 83.1.1 數(shù)字 PID 簡介 83.1.2 模糊控制方法簡介 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è)計 .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)試與開發(fā) .285.1 編譯開發(fā)工具 .285.2 調(diào)試工具 .285.3 本章小結(jié) .306 總結(jié)與展望 .316.1 總結(jié) .316.2 展望 .32參考文獻(xiàn) .33致 謝 36附錄 I .37融合角度程序 .37平衡控制電機程序 .38速度控制輸出程序 .39轉(zhuǎn)向控制程序 .40AD 采集程序 41歸一化程序 .41轉(zhuǎn)向控制部分程序 .41藍(lán)牙聯(lián)調(diào)部分程序 .4411 緒論1.1 引言隨著智能概念被提出來，越來越多的智能相關(guān)產(chǎn)品也隨之被生產(chǎn)。例如，近來倍受青睞的智能家居，雖然是還未實現(xiàn)像宣傳片里的那種智能控制效果，但是這種觀念一經(jīng)提出，就受到了眾多人的狂熱追捧。它以住宅為集聚平臺，結(jié)合學(xué)校、商店等社區(qū)，為人們更便利、快捷、高效率的生活提供保障。這一旦實現(xiàn)，人類的生活將提升至另一個維度。智能汽車，身為這智能大家族的一員當(dāng)然不能停滯不前，它的成熟也必將為人類的智能生活添上濃重的一筆。概念上智能汽車要實現(xiàn)的不僅僅是自動駕駛功能，而且還擁有舒適良好的用戶體驗。但是，完全的自動駕駛卻是智能汽車成功的第一步。能為智能汽車提供自動駕駛功能的傳感器種類繁多，最常用的有攝像頭，紅外，GPS，聲吶，電磁等。但是，以電磁導(dǎo)航為主要循跡的智能小車，才是智能車舞臺的贏家。首先，電磁導(dǎo)航相比于其他諸多能實現(xiàn)導(dǎo)航的傳感器，有著天然的優(yōu)勢，不易受外界環(huán)境干擾，比如溫度、光照和聲音等，這對智能汽車所要求的安全，穩(wěn)定導(dǎo)航理念正好相符。其次，采用電磁導(dǎo)航成本低，相比于其他傳感器更具有推廣前景。因此，采用電磁循跡，在智能車領(lǐng)域具有很大的發(fā)展空 [1]。為了進(jìn)一步開發(fā)磁導(dǎo)航技術(shù)，吸引眾多的年輕大學(xué)生去接觸、熟悉、發(fā)展這樣的一個技術(shù)，飛思卡爾智車比賽才增加了以電磁導(dǎo)航為主的智能車競賽。比賽時，在 PVC 賽道上鋪設(shè)一根直徑 0.1-0.8mm 漆包線，同時在漆包線中通入20kHz，100mA 交流電，從而產(chǎn)生一個交變的磁場，電磁傳感器可以通過檢測磁場有效值的大小來斷定自己所處的位置從而完成自主循跡 [2]。本次比賽電磁組采用兩輪直立的形式，兩輪平衡車有著諸多優(yōu)點；零半徑轉(zhuǎn)彎；可以通過自身的調(diào)節(jié)找到重心；減小驅(qū)動功率，為電池長時間供電提供了可能；避免因為重心過高在汽車啟動或急停時發(fā)生傾倒 [3]。雖然是簡易的智能車模型，但是它的開發(fā)設(shè)計同樣要運用諸多學(xué)科的知識，例如電路，控制，機械等領(lǐng)域知識。還得運用一些常見的模塊去實現(xiàn)電磁組小2車自動循跡的能力。同時通過這樣的比賽，雖然是三人隊，但是成員之間的溝通、合作，共同進(jìn)步同樣重要。1.2 國內(nèi)外發(fā)展1.2.1 國外智能車發(fā)展現(xiàn)狀國外對智能車相關(guān)領(lǐng)域的涉足比較早。初期階段主要是用于軍事領(lǐng)域，以美國在 20 世紀(jì)研究 Autonomous LandVehicle 為標(biāo)志 [4]。進(jìn)入 21 世紀(jì)，歐美國家利用舉辦類似的機器人小車比賽來推動智能車的發(fā)展。通過這些比賽，智能車的觀念深入人心，世界上汽車巨頭，看到了這其中富含的商機，爭先恐后地拉起研究組研究智能汽車，并且已經(jīng)取得了一些成就。其中最具代表性的就是Google 公司，目前谷歌公司已經(jīng)成為智能車領(lǐng)域的代表之一，其研發(fā)的自動駕駛汽車在電腦的控制下安全行駛了 30 萬英里，它主要依靠激光測距儀、視頻攝像頭、車載雷達(dá)、傳感器等來識別道路，確保行駛路徑的正確性。這一跨越性的成果，將智能車技術(shù)的發(fā)展空間再一次提升。1.2.2 國內(nèi)智能車發(fā)展現(xiàn)狀相比于國外，國內(nèi)接觸智能車的時間比較晚，雖然取得了一些階段性的研究成果，但是相對于國外還是有一定的差距，只適用于一些輕場合，比如校園等。對于一些長距離、復(fù)雜環(huán)境中的自動駕駛，還是有一定程度上的完善。我國智能車的最新研究成果是 2011 年成功的研發(fā)出紅旗 HQ3，它的出現(xiàn)標(biāo)志著我國的智能車水平接近了發(fā)達(dá)國家水平 [5]。自從教育部與飛思卡爾公司簽了相關(guān)智能車競賽合約，此項比賽在國內(nèi)已經(jīng)舉辦了十屆，2016 年第十一屆比賽將在中南大學(xué)舉辦。越來越多高校大學(xué)生通過參加比賽，了解智能汽車技術(shù)并進(jìn)行相關(guān)課題的研究，為我國智能車技術(shù)的再次提升積蓄力量。該課題主要是在飛思卡爾智能車硬件系統(tǒng)良好的情況下給出相應(yīng)的控制算法，增加小車整體的協(xié)調(diào)性，優(yōu)化系統(tǒng)其穩(wěn)定性和適應(yīng)性，然后去比賽。31.3 研究意義（1）通過參加比賽，將課堂上學(xué)到的理論知識運用到實際中，更有利于學(xué)生對知識的更高層次的掌握。飛思卡爾是一項綜合性的比賽，除了一些與機械相關(guān)的知識之外，還學(xué)到了很多與計算機、信息有關(guān)專業(yè)知識，提高了自己綜合實踐能力。（2）飛思卡爾一個以團(tuán)隊為單位參加的競賽，要想取得好的成績，缺不了隊員之間的密切合作與溝通交流。通過對小車的制作，隊員們提高了團(tuán)隊意識，為日后的學(xué)習(xí)工作打下基礎(chǔ)。（3）通過競賽讓更多大學(xué)生了解智能車，熟悉智能車，對于智能車的普及、發(fā)展起到積極作用。而且該項比賽具有傳承性，隨著學(xué)生參加比賽的次數(shù)增多，學(xué)生對于智能控制技術(shù)越加成熟，對于促進(jìn)智能車整體水平的而發(fā)展起到重要作用。1.4 課題主要內(nèi)容學(xué)習(xí)飛思卡爾公司規(guī)定的 32 位比賽芯片，完成對硬件系統(tǒng)所用到各個芯片驅(qū)動程序設(shè)計，實現(xiàn)電磁組控制軟件系統(tǒng)設(shè)計。首先，單獨對單一的模塊進(jìn)行獨立調(diào)試，接著再結(jié)合各個模塊，進(jìn)行系統(tǒng)的聯(lián)合調(diào)試。在完成智能車的硬件系統(tǒng)前提下，開始編寫與該硬件相對應(yīng)的算法并調(diào)試，通過調(diào)試選擇其中最好的算法，完善其中各個參數(shù)，使系統(tǒng)在硬件良好的情況下，讓控制系統(tǒng)達(dá)到最大的穩(wěn)定，為比賽做好準(zhǔn)備。研究內(nèi)容包括：(1)平衡控制——通過電機的正反轉(zhuǎn)保持車模直立狀態(tài) [6]。由于車模采用兩輪直立的形式，這種情況下車模的傾倒只會發(fā)生在輪子滾動方向上，這時就可以通過控制輪子電機的轉(zhuǎn)動產(chǎn)生與傾倒方向相同的加速度，抵消在一個維度傾斜的趨勢，從而達(dá)到平衡要求 [7]。(2)速度控制——通過安裝著的編碼器來測量車輪轉(zhuǎn)速 [8].速度的控制通過改變車模的傾角實現(xiàn)。首先給車模預(yù)設(shè)一個傾角，在車模角度的反饋調(diào)節(jié)下，車模會自動維持在一個角度，從而產(chǎn)生傾角方向的加速度，通過一段時間的積累，繼而在此方向產(chǎn)生速度。4(3)方向控制——根據(jù)兩車輪的轉(zhuǎn)動差速實現(xiàn)車模轉(zhuǎn)速控制 [8]。小車沿磁場運動時，利用各個電磁傳感器的差異，對小車速度控制信號進(jìn)行加減運算，形成左、右輪電壓差，從而控制小車的沿磁場行進(jìn)。(4)人機交互控制——智能車的實時狀態(tài)需要被上位機及時反映，提高調(diào)試效率，因此十分必要對智能車進(jìn)行實時監(jiān)控 [9]，本課題通過 12864 液晶屏顯示小車的實時狀態(tài)。利用無線藍(lán)牙模塊與上位機取得聯(lián)系，另外，我們還可以利用藍(lán)牙對小車進(jìn)行實時調(diào)試，在一定程度上極大提高調(diào)試效率。52 智能車主控系統(tǒng)方案設(shè)計電磁直立平衡車系統(tǒng)由三個子部分構(gòu)成；機械系統(tǒng)、硬件系統(tǒng)、算法程序控制。其中機械部分主要任務(wù)是確定小車重心，這其中的工作包括安裝電磁傳感器、編碼器、控制 PCB 板以及為找到合適重心位置而用到的配重物等；硬件部分由主控芯片 MK60、電源模塊、電磁采集模塊、驅(qū)動電機模塊等構(gòu)成；同時，程序控制部分由 AD 處理以及采集后的數(shù)據(jù)處理算法、直立小車的平衡 PD 控制算法、速度閉環(huán) PI 算法、轉(zhuǎn)向 PD 算法、人機交互模塊等六個部分組成。2.1 整個系統(tǒng)方案的設(shè)計電磁組直立平衡小車主控芯片為飛思卡爾智能車比賽官網(wǎng)提供的 32 位單片機 MK60DN512ZVLQ10 與 E 型車模。在此基礎(chǔ)上通過設(shè)計算法去控制一些模塊，完成小車的設(shè)計，實現(xiàn)小車的自動循跡 [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。芯片資源如此豐富，功能足夠強大。同時擁有兩組各 8 路 16 位 A/D 采集與 12 路 32 位 PWM 輸出。對于繁多的數(shù)據(jù)采集，復(fù)雜電磁 AD 的處理過程，毫無壓力。對于所剩下的內(nèi)部資源應(yīng)付小車控制，也是十分輕松。它幾乎可以在不超頻的狀態(tài)下，輕易實現(xiàn) 3ms 內(nèi)執(zhí)行完所有控制程序，并且隨意選中其中四路輸出 PWM 控制電機正反轉(zhuǎn)。芯片內(nèi)還包含了豐富的外圍模塊 I2C、SPI、UART、USB 等，為取得芯片與外界通訊做出了巨大貢獻(xiàn)。由此，對于控制一個性能要求穩(wěn)定、快捷的系統(tǒng)來說，一款功能相當(dāng)?shù)男酒喈?dāng)重要。（2） 藍(lán)牙模塊藍(lán)牙模塊用于單片機與上位機之間的數(shù)據(jù)傳輸。當(dāng)處于這種工作模式時的時候分為一對藍(lán)牙模塊就被分為主機和從機。 [11]其中以偶數(shù)命名的無線藍(lán)牙模塊型號無法更改主從機；而以奇數(shù)命名的型號可以通過 AT 指令修改模塊，任意指定為主機或者從機。本課題使用的是 HC-05 為奇數(shù)型號方式，可以通過 AT 指令指定完主從機后，利用串口就可以接入電腦，配對完畢即可通信。（3） 光電編碼器光電編碼器利用光電效應(yīng)原理，將角度、轉(zhuǎn)速、位置等量轉(zhuǎn)換為電信號并加以輸出的一種傳感器 [12]。（4） 陀螺儀加速度計陀螺儀是利用壓電陶瓷做震動單元，當(dāng)陀螺儀旋轉(zhuǎn)時，震動頻率會隨著變化，根據(jù)這些震動就可以得到旋轉(zhuǎn)角速度。加速度計是利用微機械在兩個電極之間加入一個懸臂梁，與相鄰的電極構(gòu)成兩個電容 [13]。（5） 電磁傳感器7交變磁場產(chǎn)生交變的電場，電磁傳感器是利用纏繞的線圈去切割磁場，獲得交流電，產(chǎn)生信號。2.3 本章小結(jié)本章主要講述了直立兩輪小車實現(xiàn)控制的總體模塊，為控制智能車做出了一定的指引。課題可以沿著各個模塊開始，實現(xiàn)各個模塊的控制，從而進(jìn)一步實現(xiàn)對整體的控制。83 控制算法分析3.1 PID 相關(guān)算法簡介3.1.1 數(shù)字 PID 簡介在眾多控制算法中，PID控制是較常見與經(jīng)典的控制算法 [1]。其算法形式簡單、基本不要控制對象復(fù)雜而繁瑣的動態(tài)模型，加上在線調(diào)試方便，因此被廣泛應(yīng)用于過程控制和運動控制中。數(shù)字PID算法主要有三個參數(shù)，它們分別為P表示比例控制器，I代表積分控制器，D代表微分控制器。3.1.2 模糊控制方法簡介模糊控制算法理論基礎(chǔ)模糊理論，是一種通過對傳統(tǒng)PID算法的改進(jìn)而獲得的控制算法，這樣的作法簡化了系統(tǒng)的繁雜性。通過劃分模糊集，使控制系統(tǒng)不過分依賴于控制對象的精確數(shù)學(xué)模型，但如何確定模糊規(guī)則和系統(tǒng)設(shè)計方法，卻是過分依賴經(jīng)驗。因此，模糊控制適用于非線性、滯后、時變和模型不完全系統(tǒng)的控制 [10]。3.2 PID 算法與其改進(jìn)形式的應(yīng)用3.2.1 PID 控制算法飛思卡爾智能車系統(tǒng)為了實現(xiàn)較高精度的控制，基本上實行閉環(huán)控制。閉環(huán)控制系統(tǒng)利用輸出量與輸入量之間的反饋來增加系統(tǒng)的精度，通過減少系統(tǒng)的不確定性，來提高系統(tǒng)的抗干擾能力。通過比較系統(tǒng)當(dāng)前輸出量與輸入量期望值的偏差，利用此偏差來糾正系統(tǒng)，從而獲得預(yù)期輸出控制量。其中最簡單、最通用的閉環(huán)校正裝置就是 PID 校正裝置或 PID 控制器。其中 PID 控制器又被分為位置式與增量式兩種。9圖 3-1 PID 控制示意圖為了提高控制精度，在位置式 PID 算式中添上 （控制補償量）后，算式??0如下；（3-1）??00])(1)([xdteTeTteKutip ?????式中： ——比例增益p——積分時間常數(shù)????——微分時間常數(shù) ????——控制輸出量()ut——輸出量與輸入量的偏差 e——控制補償量0x設(shè)裝置采樣周期是 T，上式 PID 算式可做離散差分方程變化；（3-2）001])()([xeTeteKxkjkdip ??????因此，對于增量式 PID 算式，可根據(jù)式（3-2）得到（3-3）002111 ])([ xeTeeKxkj kdjikpk ???????則有10（3-4）)]2([2111???????kk kdkipkk eeTKxx正因為式中用到第 k 次、第 k－1 次兩次輸出結(jié)果的變化量，所以稱為增量式 PID 算式。利用這個變化量，控制系統(tǒng)完成相應(yīng)的輸出控制量。上式還可改寫成；（3-5）21?????kkkkCeBAex其中：（3-6）)1(TKAdip??（3-7）)2(Bdp（3-8）TKCdp?增量式與位置式 PID 的比較：（1） 增量式 PID 無須經(jīng)過累加處理，所以比位置式 PID 的累積誤差小。即增量式 PID 精度較高（2） 增量式算法只需輸出限幅，而位置式還要設(shè)置積分限幅。因此，增量式編程方便。（3） 增量式算法，通過與前一次做偏差即可得出結(jié)果，避免存儲每一次的偏差，對芯片的內(nèi)存占據(jù)較小。（4） 位置式算式簡單明了，結(jié)構(gòu)清晰，參數(shù)容易調(diào)整。3.2.3 平衡、轉(zhuǎn)向 PD 控制在小車的平衡與轉(zhuǎn)向控制中采用比例微分控制（PD）。因為小車實現(xiàn)自動循跡，最重要的兩個控制就是平衡與轉(zhuǎn)向控制，通過比例微分控制，會令輸出11信號 Y(S)具有超前于輸入信號 R(S)的相位的特性，最大超前相位角度 90°，幅值改變了。但是對于頻率卻是完完整整保存了下來。通過 PD 控制器可以改善系統(tǒng)的動態(tài)特性性能。圖 3-2 PD 控制示意圖對于小車的直立平衡控制，對系統(tǒng)的響應(yīng)速度有較高的要求，因此 PD 控制可以保證對系統(tǒng)的穩(wěn)定性、快速性的要求。相反加入了積分控制，使系統(tǒng)的穩(wěn)定裕量、快速性降低。因此，直立平衡控制用 PD 控制。具體的調(diào)試方法是先調(diào)節(jié) P，然后調(diào)節(jié) D。P 值從小到大緩慢增加，直道小車出現(xiàn)抖動，無法直立為止，這時取此時 P 值的 75%左右值。然后再緩慢增加D，直至小車又出現(xiàn)顫動為止。這時再反過來再增加 P 值，反復(fù)調(diào)節(jié) PD 值，直至最后小車直立反應(yīng)速度夠快，看干擾能力較大為止。這時，直立調(diào)節(jié)的 PD 值就算完成了。對于轉(zhuǎn)向調(diào)試 PD 值的方法與直立類似，只是注意彎道，直道 PD 值的過渡不可太大，否則容易因為漂移而使小車傾倒。3.2.4 速度 PI 控制速度控制使用了 PI 控制。對于速度閉環(huán)就不需要那樣快速性，需要的是速度變化的平滑性更重要，因此加入 I 滯后調(diào)節(jié)，增加了平穩(wěn)性?？梢员ＷC小車在入彎和出彎時速度改變量較大，小車系統(tǒng)對速度的控制大，減緩過彎道時的速度，使小車順利過彎；同時在小車啟動時 PI 控制表現(xiàn)了良好的控制要求，在啟動時刻，速度會以比較平穩(wěn)的速度緩慢增加，對直立這個基本閉環(huán)起到很大的作用，增加了小車的穩(wěn)定性。12圖 3-3 PI 控制示意圖具體的調(diào)試方法是 I 值，用手推小車，因為小車偏離了原始位置，產(chǎn)生了誤差，這時積分開始作用，小車會在積分的作用下，在原點附近反復(fù)擺動，擺動幅度越來越大。這時添加 P 環(huán)節(jié)，會看到小車擺動幅度變小，隨著 P 的增大，用手推小車，小車會一次停在原點，甚至是推不動小車。這時直立閉環(huán)就算調(diào)試完成。3.3 本章總結(jié)本章介紹了小車控制中算法選取以及調(diào)試方法，小車能夠行走，三個閉環(huán)之間的聯(lián)系仍是十分重要，為了小車能達(dá)到能以穩(wěn)定速度循跡的目的要反復(fù)調(diào)節(jié)這些參數(shù)，并不是毫無聯(lián)系。134 控制系統(tǒng)設(shè)計相對于四輪行走，直立狀態(tài)下的控制更加復(fù)雜。從控制角度來說，車模為一個控制對象，根據(jù)后輪的調(diào)節(jié)實現(xiàn)車模的平衡控制，速度控制，方向控制。其中直立平衡控制為最基本控制，只有在小車直立平衡的基礎(chǔ)上，小車才能繼而完成剩余的兩個控制。而速度控制，建立在平衡的基礎(chǔ)上，利用速度閉環(huán)調(diào)節(jié)，可以控制小車按著預(yù)定速度勻速前進(jìn)；轉(zhuǎn)向閉環(huán)控制使小車實現(xiàn)自動循跡。總系統(tǒng)控制框圖見下；圖 4-1 總控制系統(tǒng)框圖4.1 平衡控制當(dāng)?shù)沽[物體偏離自身的平衡位置時，就會在重力的作用下，倒向偏離的那邊。若欲令倒立擺垂直穩(wěn)定在垂直位置，只能通過向另外方向增加與重力作用相抵消的力，即可以通過控制倒立擺，使它往傾倒方向做加速運動，增加一個額外與重力作用相互抵消的力，這樣就可以使倒立擺穩(wěn)定平衡在垂直位置；14圖 4-2 倒立擺控制原理所需要力 F 大?。海?-1 ）??=????sin???????cos??≈?????????????存在空氣阻尼等，5-1 式便可以簡化成（4-2 ）??=???????????1???????2??'其中 為車輪加速度 a 與偏角 的比值，則可簡化加速度 a；??1 ??（4-3 ）??=??1??+??2??'上式中 g， ， 均為比例系數(shù)。因此可以得出小車平衡??1 ??20的量 ??1、 ??2與真實偏角 有關(guān)。??欲求得平衡控制的傳遞函數(shù)可建立類似，倒立擺的模型。車模在運動時的方程：????2??(??)??2(??)=??sin?????(??)cos??+????(??) (4?4)加上附加條件，小車處于靜止?fàn)顟B(tài)且擺動 ? 很小時，(4-4)中可簡化成：????2??(??)??2(??)=g??(??)+????(??) (4?5)由此得到傳遞函數(shù)：??(??)=??(??)??(??)= 1??2????? (4?6)15該傳遞函數(shù)含有兩個極點 S= ，S= ，其中一個位于右半平面。因????? +????此，系統(tǒng)不穩(wěn)定。引入比例—微分反饋之后，可改觀這情況。L + ?(S)+X(s)圖 4-3 直立平衡控制框圖由程序框圖可得：??(??)=??(??)??(??)= 1??2+??2????+(??1???)?? (4?7)求得極點 ,當(dāng)極點全位于左半平面時，有????=???2±√（ ??22?4??（ ??1???）2????20,??1,即此時系統(tǒng)穩(wěn)定。在這樣的反饋中， 相當(dāng)于比例，而 相當(dāng)于微分控制?? ??1 ??2器，具有阻尼作用。比例值的大小表示了系統(tǒng)調(diào)控作用的強弱，而微分可以減少小車的震蕩。如果想準(zhǔn)確反映小車實時的真實角度，必須要陀螺儀與加速度計相互結(jié)合。MPU6050 + 積 分比 例K2K2角 速 度 ? 角 速 度 ? +++ 加 速 度 計電 機驅(qū) 動陀 螺 儀圖 4-4 直立平衡控制框圖因為加速度計雖然測量的是小車的真實角度，但是它易受外部干擾，所以只利用加速度計測小車角度，會得到毛刺十分明顯的曲線。相反的，陀螺儀測量的是小車的角速度，它可以通過積分獲得真實角度，因此角度曲線光滑，但g)1(????2???1+??2??16是它的測量值隨時間的變化相對較大，而且受溫度的影響大。它們優(yōu)缺點互補，利用加速度計每過 5ms 就去糾正陀螺儀的值，這樣就可以獲得一條即光滑又準(zhǔn)確的角度曲線。融合效果圖見下：圖 4-4 融合角度后的真實角度融合角度程序見附錄 I：得到能真實反映小車傾角的融合曲線之后，將實際的角度值與預(yù)設(shè)角度值相減，得出它們的變化量，預(yù)設(shè)角度值即為小車靜止時平衡位置的角度值。關(guān)于預(yù)設(shè)角度值的產(chǎn)生主要是因為陀螺儀安裝位置與小車靜止重心發(fā)生偏離，預(yù)設(shè)角度值越大代表陀螺儀與重心位置偏離越多；反之，值越小，表示偏離越小。同時，陀螺儀采集到的為偏差的微分。通過 PD 控制輸出相應(yīng)的 PWM，控制電機輸出，對小車進(jìn)行直立平衡控制。 通過 FTM 通道將直立控制的 PWM 波輸入驅(qū)動模塊，來實現(xiàn)電機的正反轉(zhuǎn)，給將要傾倒的方向，提供平衡所需要的加速度，令小車實現(xiàn)垂直平衡控制。電機程序見附錄 I：4.2 速度控制由于小車采用兩輪直立的形式，所以不能通過控制電機轉(zhuǎn)速來調(diào)節(jié)小車的速度。在平衡控制的基礎(chǔ)上，給車模預(yù)設(shè)一個傾角，加上車模角度的反饋調(diào)節(jié)，車模會自動維持在這個角度，從而產(chǎn)生速度。換一句話說，即可以通過改變小車的傾角來產(chǎn)生加速度，從而實現(xiàn)速度控制。17簡化小車平衡控制模型，在速度控制系統(tǒng)中，把小車平衡控制化簡為一個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 方程中具有一個極點 S= ，系統(tǒng)若要保持穩(wěn)定，則必須保證 ，?1??2??? ??2??為系統(tǒng)時間常數(shù)，??2由于小車速度是通過傾角的加速度積分獲得，由此可得速度控制框圖；+K圖 4-6 加積分后速度控制框圖(4-11)??(??)= ??(??)??2??2+(????+1)??+?? 由此得出兩個方程的極點 ,小車系統(tǒng)保持穩(wěn)定則??= 221)4dKT????應(yīng)盡量大。然而， 過大會導(dǎo)致兩個極點實部太小，易引起速度的震蕩。要2T2T??(??) 1??2??+1 ??(??)??(??) 1??2??+11??18消除這種震蕩，可以通過加入微分調(diào)節(jié)器,令極點遠(yuǎn)離虛軸，增加系統(tǒng)的穩(wěn)定性。+KKdS圖 4-7 加積分、微分速度控制框圖實際中，編碼器傳遞回來的信號中，由于受到電機影響，信號中附帶干擾信號，干擾信號會隨著微分運算，會被放大，造成系統(tǒng)的不精確。因此，可以將小車系統(tǒng)看作一個積分環(huán)節(jié)，將最初的積分環(huán)節(jié)與微分環(huán)節(jié)融合，看作是一個比例環(huán)節(jié)。這樣既能保證系統(tǒng)傳遞函數(shù)不變，又能避免因微分運算帶來的附加干擾。具體控制框圖；MPU6050 + 積 分比 例K2K2角 速 度 ? 角 速 度 ? +++加 速 度 計驅(qū) 動 電 機車 模 保 持 給定 的 傾 角陀 螺 儀+速 度 控 制 D速 度 控 制P1/S+-+速 度 反 饋給 定 速 度 速 度 檢 測圖 4-8 角度、速度閉環(huán)控制框圖因此，對小車的控制使用的是 PI 控制中，其中 P 包括了微分效果，所以相當(dāng)于具有阻尼與系統(tǒng)控制強弱作用。當(dāng)在彎道上行駛時，p 值越大，則引起的速度變化越大，從而容易引起系統(tǒng)的震蕩。I 代表系統(tǒng)的精度，即是否回到原點相關(guān)，當(dāng)產(chǎn)生速度偏差時，I 就開始積累，為了避免 I 積累越來越大，可以通過對 I 的適當(dāng)限幅處理，使 I 穩(wěn)定在某一范圍，從而保證小車的穩(wěn)定。??(??) 1??2??+11??19同時為了保證小車速度輸出具有平滑性，以增加小車運動時的穩(wěn)定性，對速度輸出進(jìn)行低通濾波處理與速度平滑輸出處理。通過低通濾波處理，速度的最終值會顯得更加平滑，速度值的抖動不會太大。小車不會因為一時的錯誤數(shù)據(jù)，比如光電編碼器輸出異常，電磁傳感器檢測失誤或者程序執(zhí)行中的問題，而造成對速度控制太大影響；當(dāng)小車過彎道的時候，可以減少因為輪子不小心漂移而產(chǎn)生的某個瞬間值太大，對小車的平衡、速度控制造成的影響。速度平滑輸出處理，芯片每過 5ms 逐步給電機增加轉(zhuǎn)速，讓預(yù)設(shè)速度與實際速度逐漸接近，避免了小車因為一下給予過大速度而出現(xiàn)嚴(yán)重抖動，致使平衡破壞的情況。速度控制輸出程序見附錄 I：4.3 方向控制為了實現(xiàn)小車的自動循跡，小車的方向控制尤其重要。然而，方向控制必須在平衡控制與速度控制比較良好的前提下，換一句話說就是在小車能夠直立，并且能夠以一定的速度前進(jìn)的條件下，實現(xiàn)方向控制。根據(jù)兩個電機的轉(zhuǎn)動差速實現(xiàn)車模轉(zhuǎn)向控制。小車沿磁場運動，利用電磁信號的偏差小車速度控制信號進(jìn)行加減運算，形成左、右輪電壓差，再通過芯片把電壓差反應(yīng)到左右輪電機，從而控制小車的轉(zhuǎn)向。轉(zhuǎn)向控制程序框圖：圖 4-9 方向控制框圖為增加方向輸出的平滑性，采用相類似于與速度閉環(huán)的平滑輸出做法，這樣同樣可以增加小車在轉(zhuǎn)向過程中的穩(wěn)定性。轉(zhuǎn)向控制程序見附錄 I;204.4 電磁傳感器值數(shù)字濾波要實現(xiàn)小車的自動循跡，在裝好電磁傳感器的前提下，對電磁信號的處理、利用相當(dāng)重要。由于不同位置，不同電感，所檢測到的磁場都是不一樣的，為了做到小車對賽道的準(zhǔn)確識別，需要不斷地采集賽道的磁場信息。通過利用K60 芯片中有現(xiàn)成的 AD 模塊，采用數(shù)組來存放采集到的 AD 值，來實現(xiàn)對電磁信號的相關(guān)處理。采集程序見附錄 I：僅僅傳感器采集的 AD 值有著相當(dāng)大的干擾信息，因此需要對 AD 值進(jìn)行濾波處理以降低干擾信息，提高小車的識別度。此次，使用的是數(shù)字濾波方式，相比于模擬濾波裝置，數(shù)字濾波有以下幾個優(yōu)點：（1） 通過程序的形式來到達(dá)濾波效果，很大程度上避免了硬件濾波的不穩(wěn)定性，提高了 AD 采集的可靠性。（2） 數(shù)字濾波含有多種不同方式的濾波算法，更加靈活，適用性更好。（3） 數(shù)字濾波可實現(xiàn)多通道共用，提高 AD 采集的效率。4.4.1 AD 采集后的限幅濾波限幅濾波又被稱為程序判斷濾波法，就是求出前后兩次 AD 值的變化量，然后與兩次采樣允許的最大偏差值 作對比。如果小于等于 ，則取本次△ ?? △ ??采樣值有效；如果大于 ，則此次采樣值無效，取上一次采樣值 [25]?！???即：, 則 ,取本次采樣值|??????????1|≤Δ?? ????=???? ????, 則 ，取上次采樣值|??????????1|Δ?? ????=?????1 ?????1使用限幅濾波對采集回來的 AD 值進(jìn)行處理之后，能有效減少因為偶然因素而引起的 AD 值突變。但是，它卻無法消除系統(tǒng)干擾，特別是周期性的干擾，容易引起突變，不利用小車數(shù)據(jù) AD 采集的平滑。4.4.2 AD 采集后的中值濾波21個 個個 個個 個個 個 個 個 個 個個 個 個 個 個 個 個 個個 個 個個 個 個 個個 個 個 個 個 個個 個個個圖 4-10 AD 采集后的中值濾波示意圖AD 采集后的中值濾波法就是使用芯片 AD 模塊功能，首先設(shè)置連續(xù)采樣次數(shù) n（一般為奇數(shù)，本課題中對賽道同一點 AD 采集 27 次），然后把采集到的AD 值按順序排列，取這組數(shù)的中間值當(dāng)作本次采集的有效值。利用中值濾波取 AD 采集有效值，能有效克服因為偶然因素而引起的有效值干擾，特別是對溫度等變化緩慢的被測量，有良好的濾波效果，但是對流量等快速變化的被測量不宜采用。此次課題中，結(jié)合 AD 數(shù)值的歸一化，可以把電磁的快速變化，穩(wěn)定限制在[0,1]之間，相當(dāng)于把被測量的變化率變緩，正好符合中值濾波的條件。因此，此次課題采用中值濾波對 AD 進(jìn)行處理，實現(xiàn)表明小車能夠有效識別賽道，完成循跡。224.4.3 AD 采集后的算術(shù)平均濾波算術(shù)平均濾波就是連續(xù)進(jìn)行 AD 采集，采樣次數(shù)不作規(guī)定設(shè)為 n，然后對所有采樣值進(jìn)行算術(shù)平均運算，取其平均值作為有效值。此種方法的特點；當(dāng) N 值較大時：AD 值連續(xù)性平滑度較高，偶然的電磁干擾對小車的控制影響較小，同時也會導(dǎo)致小車靈敏度降低。當(dāng) N 值較?。盒盘柶交扰c連續(xù)性差，干擾對小車系統(tǒng)的影響非常大，很容易出現(xiàn)誤判的現(xiàn)象。所以，雖然靈敏度好，卻會出現(xiàn)小車因為尋找合適的區(qū)間值，而不停擺動傳感器，使系統(tǒng)穩(wěn)定性變差，無法完成循跡。此種濾波比較適用于具有一般隨機干擾的信號，比如信號圍繞平均值范圍附近上下浮動。但是，對于要求測量、計算速度快的電磁小車控制不合適，而且大大浪費芯片 RAM 空間。 4.5 電磁 AD 采集處理為了保證小車能比較準(zhǔn)確的識別賽道，較好的適應(yīng)環(huán)境，可以通過歸一化，把電磁的采集值限定在[0,1]之間，有利于單片機找到控制的最優(yōu)解，增加程序運行時的收斂性，提高程序運行效率，降低芯片的利用率。另一方面，也能提高小車的適應(yīng)性，去除由于信號源的不穩(wěn)定而引起的干擾。數(shù)據(jù)歸一化公式為（測試值-最小值）/（最大值-最小值），這樣就可以把電磁傳感器的采集值限定在[0,1]之間。在處理時會出現(xiàn)負(fù)數(shù)或者大于 1 的情況，這時可以通過設(shè)定閾值，把歸一化后的值強制限制在[0,1]之間，具體做法是：出現(xiàn)負(fù)數(shù)時就把這時的歸一化值用 0 來計算，當(dāng)出現(xiàn)大于 1 時，把大于 1 的值認(rèn)為是 1。歸一化過程圖如圖 4-823開 始0?1? = 0AD有 效 值結(jié) 束=1YNYN歸 一 化圖 4-11 歸一化過程圖為了能夠有明顯的區(qū)分性，可以將[0,1]區(qū)間放大，即將歸一化后的值都乘以 100，這樣就把區(qū)間設(shè)置成[0,100]。經(jīng)過這樣的處理后，AD 采集到的最大值和最小值，可以較好地避免因信號源不同或者因不同傳感器之間的輕微誤差，而導(dǎo)致的小車適應(yīng)性差問題，同時也優(yōu)化尋找最優(yōu)解的途徑。在進(jìn)行電磁組的歸一化時，利用也就是 AD 值。這里需要說明的是，關(guān)于最大值與最小值的設(shè)定，每一組都不是固定的，只要找到適合利于調(diào)試的值就可以。部分歸一化程序見附錄 I：采集后的有效值，經(jīng)過歸一化后，克服了電磁數(shù)據(jù)快速變化的缺點。再利用中值濾波，就能使小車以較好的狀態(tài)去識別賽道，為循跡打下良好的基礎(chǔ)。244.6 電磁 AD 值利用對于磁場數(shù)據(jù)的采集其最終目的是實現(xiàn)小車的自動循跡，因此，對于歸一化后 AD 值與小車方向控制尤其重要。電感分布圖：大 S 位置一 小 S 位置二 直道 位置三 小 S 位置四 大 S電感 1 電感 2AD0 AD3圖 4-12 電感分布圖方案中利用電感 1、電感 2，以實現(xiàn)對小車的循跡控制。利用公式：error12=（(AD[0]-AD[3])*1000）/ ((AD[0]+AD[3])*10) （4-12）作為小車循跡行駛的判斷依據(jù)，這樣即可以避免角度的影響，又可以減少小車循跡時順時針與逆時針的一致性，適應(yīng)性更大。利用處于不同位置時兩個傳感器的值不同，而使 error12 值的不同，同時給 error 值的變化劃定四個位置狀態(tài)，通過位置狀態(tài)的不同，來定義小車所處于的不同賽道位置。不同賽道位置所用到的轉(zhuǎn)向 PD 不同，同時對于轉(zhuǎn)向時 error 所起到的轉(zhuǎn)向權(quán)重也是不同，即權(quán)重越大，轉(zhuǎn)向趨勢越大。位置二、位置三定義為直道。此時，小車不轉(zhuǎn)向，是勻速運動。 圖 4-13 直道示意圖位置一或位置四之外定義為大 S 彎道，這時轉(zhuǎn)向的 PD 值最大。25圖 4-14 大 S 彎道示意圖位置一、位置二與位置三、位置四定義為小 S 彎道，但是由于這時的范圍相對于較大，因此具體做法是通過細(xì)分把小 S 彎道又分為小 S1，小 S2,小 S3 分別進(jìn)行處理。同時，又因為對于大 S 彎道的判斷，單片機是先判斷出小 S，進(jìn)而判斷大 S，因此有必要稍微擴大對小 S 的判定范圍，提高電磁傳感器的預(yù)判性，實現(xiàn)對大 S 彎道完全通過。圖 4-15 小 S 彎道示意圖利用對小 S 的細(xì)分還可以順利實現(xiàn)直道轉(zhuǎn)彎道的 PD 轉(zhuǎn)向過渡，不會輕易由于轉(zhuǎn)向 PD 變化大而導(dǎo)致轉(zhuǎn)彎過大，引起漂移，實現(xiàn)平滑循跡的目的。轉(zhuǎn)向控制流程圖見圖 4-10；轉(zhuǎn)向控制部分程序見附錄 I：