【文章內(nèi)容簡(jiǎn)介】 進(jìn)行簡(jiǎn)單的比例控制就可以完成車(chē)模方向控制。 車(chē)模傾角測(cè)量 在上節(jié)車(chē)模直立控制中介紹了控制車(chē)模直立的算法，通過(guò)測(cè)量車(chē)模的傾角和傾角加速度控制車(chē)模車(chē)輪的加速度來(lái)消除車(chē)模的傾角。因此車(chē)模傾角以及傾角加速度的測(cè)量成為控制車(chē)模直立的關(guān)鍵。測(cè)量車(chē)模傾角和傾角加速 度可以通過(guò)加速度傳感器和陀螺儀實(shí)現(xiàn)。 加速度傳感器可以測(cè)量由地球引力作用或者物體運(yùn)動(dòng)所產(chǎn)生的加速度。競(jìng)賽規(guī)則規(guī)定如果車(chē)模使用加速度傳感器必須使用飛思卡爾公司產(chǎn)生的加速度傳感器。該系列的傳感器采用了半導(dǎo)體表面微機(jī)械加工和集成電路技術(shù)，傳感器體積小，重量輕。它的基本原理如圖 所示。 圖 加速度傳感器 通過(guò)微機(jī)械加工技術(shù)在硅片上加工形成了一個(gè)機(jī)械懸臂。它與相鄰的電極形成了兩個(gè)電容。西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 10 頁(yè) 由于加速度使得機(jī)械懸臂與兩個(gè)電極之間的距離發(fā)生變化，從而改變了兩個(gè)電容的參數(shù)。通過(guò)集成的開(kāi)關(guān)電容放大電路量測(cè)電容參 數(shù)的變化，形成了與加速度成正比的電壓輸出。 MMA7260 是一款三軸低 g 半導(dǎo)體加速度計(jì)，可以同時(shí)輸出三個(gè)方向上的加速度模擬信號(hào)，如圖 所示。 圖 三軸加速度傳感器 通過(guò)設(shè)置可以使得 MMA7260 最大輸出靈敏度為 800mV/g 。 只需要測(cè)量其中一個(gè)方向上的加速度值，就可以計(jì)算出車(chē)模傾角，比如使用 Z 軸方向上的加速度信號(hào)。車(chē)模直立時(shí)，固定加速度器在 Z 軸水平方向，此時(shí)輸出信號(hào)為零偏電壓信號(hào)。當(dāng)車(chē)模發(fā)生傾斜時(shí)，重力加速度 g 便會(huì)在 Z 軸方向形成加速度分量，從而引起該軸輸出電壓變化。變化 的規(guī)律為 式中， g 為重力加速度； ? 為車(chē)模傾角； k 為比例系數(shù)。當(dāng)傾角 ? 比較小的時(shí)候，輸出電壓的變化可以近似與傾角成正比。 似乎只需要加速度就可以獲得車(chē)模的傾角，再對(duì)此信號(hào)進(jìn)行微分便可以獲得傾角加速度。但在實(shí)際車(chē)模運(yùn)行過(guò)程中，由于車(chē)模本身的運(yùn)動(dòng)所產(chǎn)生的加速度會(huì)產(chǎn)生很大的干擾信號(hào)疊加在上述測(cè)量信號(hào)上，使得輸出信號(hào)無(wú)法準(zhǔn)確反映車(chē)模的傾角，如圖 所示。 圖 車(chē)模運(yùn) 動(dòng)引起加速度計(jì)信號(hào)波動(dòng) 西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 11 頁(yè) 車(chē)模運(yùn)動(dòng)產(chǎn)生的振動(dòng)加速度使得輸出電壓在實(shí)際傾角電壓附近波動(dòng)，可以通過(guò)數(shù)據(jù)平滑濾波將其濾除。但是平滑濾波也會(huì)使得信號(hào)無(wú)法實(shí)時(shí)反映車(chē)模傾角的變化，從而減緩對(duì)于車(chē)模車(chē)輪控制，使得車(chē)模無(wú)法保持平衡。因此對(duì)于車(chē)模直立控制所需要的傾角信息需要通過(guò)另外一種器件獲得，那就是角速度傳感器 陀螺儀，如圖 所示。 （ 2）角速度傳感器 陀螺儀 陀螺儀可以用來(lái)測(cè)量物體的旋轉(zhuǎn)角速度。競(jìng)賽允許選用村田公司出品的 EN03 系列的加速度傳感器。它利用了旋轉(zhuǎn)坐標(biāo)系中的物體會(huì)受到克里利奧力的原理，在器件中利 用壓電陶瓷做成振動(dòng)單元。當(dāng)旋轉(zhuǎn)器件時(shí)會(huì)改變振動(dòng)頻率從而反映出物體旋轉(zhuǎn)的角速度。 圖 角速度傳感器 在車(chē)模上安裝陀螺儀，可以測(cè)量車(chē)模傾斜的角速度，將角速度信號(hào)進(jìn)行積分便可以得到車(chē)模的傾角。如圖 所示。 圖 測(cè)量車(chē)模角速度和角度 由于陀螺儀輸出的是車(chē)模的角速度，不會(huì)受到車(chē)體振動(dòng)影響。因此該信號(hào)中噪聲很小。車(chē)模的角度又是通過(guò)對(duì)角速度積分而得，這可進(jìn)一步平滑信號(hào)，從而使得角度信號(hào)更加穩(wěn)定。因此車(chē)?？刂扑枰慕嵌群徒撬俣瓤梢允褂猛勇輧x所得到的信號(hào)。 由于從陀螺儀的角速度獲得角度信 息，需要經(jīng)過(guò)積分運(yùn)算。如果角速度信號(hào)存在微小的偏差，經(jīng)過(guò)積分運(yùn)算之后，變化形成積累誤差。這個(gè)誤差會(huì)隨著時(shí)間延長(zhǎng)逐步增加，最終導(dǎo)致電路飽和，無(wú)法形成正確的角度信號(hào)，如圖西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 12 頁(yè) 所示。 圖 角度積分漂移 如何消除這個(gè)累積誤差呢？ 可以通過(guò)上面的加速度傳感器獲得的角度信息對(duì)此進(jìn)行校正，如圖 所示。 圖 角度積分漂移校正 利用加速度計(jì)所獲得的角度信息 g? 與陀螺儀積分后的角度 ? 進(jìn) 行比較，將比較的誤差信號(hào)經(jīng)過(guò)比例 gT 放大之后與陀螺儀輸出的角速度信號(hào)疊加之后再進(jìn)行積分。從圖 中的框圖可以看出，對(duì)于加速度計(jì)給定的角度 g? ， 經(jīng)過(guò)比例、積分環(huán)節(jié)之后產(chǎn)生的角度 ? 必然最終等于 g? 。由于加速度計(jì)獲得的角度信息不會(huì)存在積累誤差，所以最終將輸 出 角度 ? 中的積累誤差消除了。 加速度計(jì)所產(chǎn)生的角度信息 g? 中會(huì)疊加很強(qiáng)的有車(chē)模運(yùn)動(dòng)加速度噪聲信號(hào)。為了避免該信號(hào)對(duì)西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 13 頁(yè) 于角度 ? 的影響，因此比例系數(shù) gT 應(yīng)該非常小。這樣，加速度的噪聲信號(hào)經(jīng)過(guò)比例、積分后，在輸出角度信息中就會(huì)非常小了。由于存在積分環(huán)節(jié)，所以無(wú)論比例 gT 多么小，最終輸出角度 ?必然與加速度計(jì)測(cè)量的角度 g? 相等，只是這個(gè)調(diào)節(jié)過(guò)程會(huì)隨著 gT 的減小而延長(zhǎng)。 為了避免輸出角度 ? 跟著 g? 過(guò)長(zhǎng)，可以采取以下兩個(gè)方面的措施： （ 1）仔細(xì)調(diào)整陀螺儀的放大電路，使得它的零點(diǎn)偏置盡量接近于設(shè)定值，并且穩(wěn)定。 （ 2）在控制電路和程序運(yùn)行的開(kāi)始，盡量保持車(chē)模處于直立狀態(tài)，這樣一開(kāi) 始就 使得輸出角度 ? 與 g? 相等。此后，加速度計(jì)的輸出只是消除積分的偏移，輸出角度不會(huì)出現(xiàn)很大的偏差。 西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 14 頁(yè) 第 3 章 車(chē)?？刂扑惴?設(shè)計(jì) 控制算法 智能車(chē)的控制包主要是電機(jī)控制。具體的控制算法有 PID 控制和模糊控制 等控制算法 。 控制 PID 控制是工業(yè)過(guò)程控制中歷史最悠久，生命力最強(qiáng)的控制方式。這主要是因?yàn)檫@種控制方式具有直觀(guān)、實(shí)現(xiàn)簡(jiǎn)單和魯棒性能好等一系列的優(yōu)點(diǎn)。 PID 控制主要有三部 分組成，比例、積分、微分。 比例控制是一種最簡(jiǎn)單的控制方式。其控制器的輸出與輸入誤差信號(hào)成比例關(guān)系。 偏差一旦產(chǎn)生，調(diào)節(jié)器立即產(chǎn)生控制作用使被控量朝著減小偏差的方向變化，控制作用的強(qiáng)弱取決于 KP。 當(dāng)僅有比例控制時(shí)系統(tǒng)輸出存在穩(wěn)態(tài)誤差（ Steadystate error） 。 為了消除穩(wěn)態(tài)誤差，引入 積分控制 。積分項(xiàng)對(duì)誤差取決于時(shí)間的積分，隨著時(shí)間的增加，積分項(xiàng)會(huì)增大。這樣，即便誤差很小，積分項(xiàng)也會(huì)隨著時(shí)間的增加而加大，它推動(dòng)控制器的輸出增大使穩(wěn)態(tài)誤差進(jìn)一步減小，直到等于零。 為了預(yù)測(cè) 預(yù)測(cè)誤差變化的趨勢(shì) ， 引入 微分的控制器， 這樣 就能夠提前使抑制誤差的控制作用等于零，甚至為負(fù)值，從而避免了被控量的嚴(yán)重超調(diào)。 PID 控制框圖如下圖所示 對(duì)應(yīng)的誤差傳遞函數(shù)為： U（ s） /E(s)=Kp(1+1/Ti+Td) 西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 15 頁(yè) 式中， Kp 為比例增益； Ti 為積分時(shí)間常數(shù)； Td 為微分時(shí)間常數(shù)； U(s)為控制量； E(s) 為被控量與設(shè)定值 R(s)的偏差。 時(shí)域表達(dá)式為 pd0i1 d ( )( ) ( ) ( ) d dt etu t K e t e t t TTt??? ? ??????（式一） 在單片機(jī)中，我們僅能對(duì)數(shù)字信號(hào)處理，即數(shù)字 PID 控制。將上式離散化，得錯(cuò)誤 !未找到引用源。 （式二 ） PID 算法 直接利用上述離散化公式計(jì)算，框圖如右圖所示。由于積分項(xiàng)（ Pi）是將所有采集值偏差相加，在一段時(shí)間后會(huì)很浪費(fèi)單片機(jī)資源。對(duì)其稍加改進(jìn)，得到增量型 PID算法。 PID 算法 根據(jù)式二得第 k1 個(gè)采樣周期的控制量為 錯(cuò)誤 !未找到引用源。 （式三） 式二減式三得 錯(cuò)誤 !未找到引用源。 （式四 ) 由此，第 k 個(gè)采樣時(shí)刻實(shí)際控制量為 錯(cuò)誤 !未找到引用源。 ,為方便書(shū)寫(xiě)，寫(xiě)為 錯(cuò)誤 !未找到引用源。 （式五） 其中， 錯(cuò)誤 !未找到引用源。 由上可知，利用三個(gè)歷史數(shù)據(jù)，遞推使用，即可完成PID 控制量?？驁D如右圖所示 : 模糊控制 一般控制系統(tǒng)包含了五個(gè)主要部分，即 :定義變量、模糊化、知識(shí)庫(kù)、邏輯判斷及反模糊化，底下將就每一部分做簡(jiǎn)單的說(shuō)明： 初始化返回計(jì)算 ）（）（1kkdDee*KkP ??計(jì)算 并輸出)(P)(P)(PP ( k )DIPkkk ???? ? )1()(),1( ???? kPkPkekeII計(jì)算 ? ? ? ?1kPje*KkPIk0jiI??? ??）（計(jì)算 ? ? kpP e*KP ?k計(jì)算kkkyre ??西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 16 頁(yè) (1) 定義變量：也就是決定程序被觀(guān)察的狀況及考慮控制的動(dòng)作，例如在一般控制問(wèn)題上，輸入變量有輸出誤差 E 與輸出誤差之變化率 CE，而控制變量則為下一個(gè)狀態(tài)之輸入 U。其中 E、 CE、 U 統(tǒng)稱(chēng)為模糊變量。 (2) 模糊化（ fuzzify）：將輸入值以適當(dāng)?shù)谋壤D(zhuǎn)換到論域的數(shù)值，利用口語(yǔ)化變量來(lái)描述測(cè)量物理量的過(guò)程，依適合的語(yǔ)言值（ linguisitc value）求該值相對(duì)之隸屬度，此口語(yǔ)化變量我們稱(chēng)之為模糊子集合（ fuzzy subsets）。 (3) 知識(shí)庫(kù)：包括數(shù)據(jù)庫(kù)（ data base）與規(guī)則庫(kù)（ rule base）兩部分，其中數(shù)據(jù)庫(kù)是提供處理模糊 數(shù)據(jù)之相關(guān)定義；而規(guī)則庫(kù)則藉由一群語(yǔ)言控制規(guī)則描述控制目標(biāo)和策略。 (4) 邏輯判斷：模仿人類(lèi)下判斷時(shí)的模糊概念，運(yùn)用模糊邏輯和模糊推論法進(jìn)行推論，而得到模糊控制訊號(hào)。此部分是 模糊控制器的精髓所在。 (5) 解模糊化（ defuzzify）：將推論所得到的模糊值轉(zhuǎn)換為明確的控制訊號(hào)，做為系統(tǒng)的輸入值。 模糊算法可以解決一些非線(xiàn)性問(wèn)題，將賽道分為直線(xiàn)、入大小彎、出大小彎、蛇形彎道，對(duì)應(yīng)的直線(xiàn)加速、入大彎減速轉(zhuǎn)方向、入小彎制動(dòng)轉(zhuǎn)方向、出彎加速、蛇形彎道直接通過(guò)（若可以達(dá)到這種前瞻性）。要達(dá)到這種控制要通過(guò)實(shí)際檢測(cè)，分析大量賽道磁場(chǎng)信息，找出它們的特征。 總結(jié) 雖然模糊控制可以較好解決一些非線(xiàn)性問(wèn)題，但控制復(fù)雜，實(shí)際調(diào)試中較 PID控制無(wú)明顯優(yōu)勢(shì)，所以我們采用 PID 控制。電機(jī)控制為 增量式 PID，由于給定速度頻繁變化，采用微分先行 PID，使電機(jī)能夠快速響應(yīng)。 車(chē)模直立 PD 控制 車(chē)模直立控制是關(guān)鍵子程序。其中涉及到兩個(gè)關(guān)鍵控制系數(shù)： 1k :CAR_AA_P_INT ：傾角比例，（ *0x7fff ） 2k :CAR_AA_D_INT ：角速度比例，（ *0x7fff） 上面的乘以 0x7fff 是為了將小數(shù)化成定點(diǎn)小數(shù)表示形式。 這兩個(gè)系數(shù)是通過(guò)實(shí)驗(yàn)調(diào) 試逐步確定的。具體確定的方式如下： 首先將 CAR_AA_D_INT 置為 0 。逐步加大 CAR_AA_P_INT 數(shù)值，直到車(chē)模開(kāi)始前后震蕩。此時(shí)再逐步增大 CAR_AA_D_INT，消除震蕩。然后逐步增加這兩個(gè)數(shù)值，直到車(chē)模開(kāi)始抖動(dòng)，然后略減少這兩個(gè)數(shù)值即可。 這個(gè)過(guò)程需要反復(fù)調(diào)整，直到車(chē)?？梢员容^穩(wěn)定的達(dá)到直立平衡。由于現(xiàn)在還沒(méi)有加入速度西安文理學(xué)院本科畢業(yè)設(shè)計(jì)（論文） 第 17 頁(yè) 閉環(huán)，所以由于加速度傳感器零偏的誤差，會(huì)導(dǎo)致 有時(shí) 車(chē)模在直立的時(shí)候會(huì)往一個(gè)方向加速行駛。 如下是車(chē)模直立控制子程序代碼： void CarAngleAdjust(void) { int nLeft, nRight。 int nSpeed。 int nP, nD。 nP = g_nCarAngle。 nP = (int)mult_r(nP, CAR_AA_P_INT)。 nD = g_nCarGyroVal 5。 nD = (int)mult_r(nD, CAR_AA_D_INT)。 nSpeed = nD + nP。 if(nSpeed MOTOR_SPEED_SET_MAX) nSpeed = MOTOR_SPEED_SET_MAX。 else if(nSpeed MOTOR_SPEED_SET_MIN) nSpeed = MOTOR_SPEED_SET_MIN。 nLeft = nSpeed + g_nLeftMotorOutSpeed g_nMotorLeftRightDiff。 nRight = nSpeed + g_nRightMotorOutSpeed + g_nMotorLeftRightDiff。 g_nLeftMotorOut = nLeft 6。 g_nRightMotorOut = nRight 6。 if(g_nLeftMotorOut MOTOR_OUT_MAX) g_nLeftMotorOut = MOTOR_OUT_MAX。 if(g_nLeftMotorOut MOTOR_OUT_MIN) g_nLeftMotorOut = MOTOR_OUT_MIN。 if(g_nRightMotorOut MOTOR_OUT_MAX) g_nRightMotorOut = MOTOR_OUT_MAX。 if(g_nRightMotorOut MOTOR_OUT_MIN) g_nRightMotorOut = MOTOR_OUT_MIN。 MotorSpeedOut()。 車(chē)模