【正文】 K_0 * t_0。 P[0][1] = K_0 * t_1。P[1][0] = K_1 * t_0。P[1][1] = K_1 * t_1。angle+= K_0 * angle_err。 q_bias+= K_1 * angle_err。 angle_dot = gyro_mq_bias。 }在程序中利用angle+=(gyro_mq_bias) * dt計算出陀螺儀積分出的角度，其中q_bias是陀螺儀偏差。此時利用陀螺儀積分求出的angle相當(dāng)于系統(tǒng)的估計值，得到系統(tǒng)的觀測方程；而加速度計檢測的角度angle_m相當(dāng)于系統(tǒng)中的測量值，得到系統(tǒng)狀態(tài)方程。程序中Q_angle和Q_gyro分別表示系統(tǒng)對加速度計及陀螺儀的信任度。根據(jù)Pdot = A*P + P*A39。 + Q_angle計算出先驗估計協(xié)方差的微分，用于將當(dāng)前估計值進(jìn)行線性化處理。其中A為雅克比矩陣。本系統(tǒng)中雅克比矩陣如式47： (式47)隨后計算系統(tǒng)預(yù)測角度的協(xié)方差矩陣P。計算估計值angle_m與預(yù)測值angle間的誤差angle_err。計算卡爾曼增益K_0,K_1，K_0用于最優(yōu)估計值，K_1用于計算最優(yōu)估計值的偏差并更新協(xié)方差矩陣P。通過卡爾曼增益計算出最優(yōu)估計值angle及預(yù)測值偏差q_bias，此時得到最優(yōu)角度值angle及角速度值。PID控制器主要由比例單元（P）、積分單元（I）和微分單元（D）組成，其輸入與輸出u (t）的關(guān)系如式211所示。在兩輪平衡車系統(tǒng)中，以車身在重力方向的傾角作為輸入量e (t）輸入PID控制器，而輸出u (t）則為電機(jī)控制量，本系統(tǒng)使用的是直流電機(jī)，采用的調(diào)速方法為調(diào)壓調(diào)速，因此輸出量u (t）為PWM驅(qū)動占空比。相應(yīng)公式如下：Uout=Kp*angle+Kd*angle_dot (式48)式中：Kp、Kd為比例系數(shù)和微分系數(shù)，angle為車身傾角，angle_dot為車身傾角的角度。由于小車輪胎與地面存在靜摩擦力，在控制占空比很小的時候，電機(jī)電樞得電，但由于靜摩擦力的存在，轉(zhuǎn)子沒有發(fā)生相對轉(zhuǎn)動。為此需要根據(jù)實際情況加上一個死區(qū)常量以提高系統(tǒng)靜態(tài)穩(wěn)定性。圖44顯示了電機(jī)死區(qū)補(bǔ)償關(guān)系。圖44 電機(jī)死區(qū)補(bǔ)償有時候控制系統(tǒng)輸入量會發(fā)生較大抖動，此時根據(jù)PD計算出來的電機(jī)控制就會大于電機(jī)可控范圍，為此需要在PD計算輸出后，判斷輸出控制量是否大于最高控制量，若大于最高控制量，則將輸出控制量置為最高控制量。圖45 自平衡PD控制軟件流程圖在小車保持平衡基礎(chǔ)上加入前進(jìn)，后退，左右轉(zhuǎn)等動作。當(dāng)小車車身與重力方向不在同一垂直線時，小車重心前傾，其傾角改變，此時為保持小車平衡，控制則會控制電機(jī)轉(zhuǎn)動，使小車向傾角方向運(yùn)動，通過小車的加速運(yùn)動產(chǎn)生慣性力與重力的分量抵消。這時小車在保持平衡的基礎(chǔ)上完成了前進(jìn)動作?？刂菩≤嚽斑M(jìn)與后退就是利用此原理而實現(xiàn)的。為了能夠快速穩(wěn)定的對小車速度進(jìn)行控制，本系統(tǒng)加入了編碼器作為速度反饋環(huán)節(jié)，形成了速度的閉環(huán)控制。系統(tǒng)采用PID控制作為平衡車速度控制策略。其輸入為車速的偏差，而輸出為傾角，通過平衡控制達(dá)到控制車速的目的。由于平衡車的首要任務(wù)是保持平衡，因此其速度控制對平衡控制來說是一種干擾，為了盡量降低速度控制對小車平衡控制的干擾，速度控制應(yīng)盡量平滑。系統(tǒng)平衡控制周期為10ms，為在盡量少干擾平衡控制的基礎(chǔ)上提高速度控制的力度，系統(tǒng)速度控制周期設(shè)定為50ms。在實際速度控制中，采用了帶死區(qū)的PID算法。選擇帶死區(qū)的PID算法是防止調(diào)節(jié)過于頻繁，導(dǎo)致系統(tǒng)振蕩。車速PID控制程序如下：extern void Speed_Control(int Speed_hope){ Speed_error=Speed_hopeSpeed_now。 Speed_diff=Speed_errorSpeed_error_last。 Speed_error_last=Speed_error。 if(Speed_error10 amp。amp。 Speed_error(10)) { Speed_error=0。 } angle_speed=Speed_KP*Speed_error+Speed_KD*Speed_diff。 if(angle_speed15) { angle_speed=15。} else if(angle_speed(15)) { angle_speed=15。 } }當(dāng)期望車速（Speed_hope）與實際車速(Speed_now)偏差不大時將偏差(Speed_error)設(shè)為零，以此保持系統(tǒng)的穩(wěn)定性。而當(dāng)PID控制器是輸出超出保持平衡可控角范圍則將輸出設(shè)為最大可控角。兩輪自平衡車的轉(zhuǎn)彎功能實現(xiàn)較為簡單。在速度控制周期內(nèi)，對兩個電機(jī)實現(xiàn)差速輸出，則可實現(xiàn)在保持平衡的基礎(chǔ)上實現(xiàn)轉(zhuǎn)彎。為降低系統(tǒng)復(fù)雜性，本系統(tǒng)采用的轉(zhuǎn)彎策略為：當(dāng)小車需要左轉(zhuǎn)時，則設(shè)定左輪轉(zhuǎn)速為右輪轉(zhuǎn)速的1/2；同理，當(dāng)小車需要右轉(zhuǎn)時，則設(shè)定小車的右輪轉(zhuǎn)速為左輪的1/2。自平衡運(yùn)動控制程序如下：extern Car_turn(int speed,byte dir) //dir:straight直走。 dir:left左轉(zhuǎn)。 dir:right右轉(zhuǎn){ switch(dir) { case 0:motor(speed,speed)。 break。 case 1:motor((speed=1),speed)。break。 case 2:motor(speed,(speed=1))。break。 } }通過電機(jī)控制子函數(shù)motor（）給電機(jī)設(shè)定速度，而通過dir的設(shè)置選擇是直行，左轉(zhuǎn)，或是右轉(zhuǎn)。本章闡述了兩輪自平衡車的軟件設(shè)計。包括：單片機(jī)各個模塊初始化、姿態(tài)檢測模塊的姿態(tài)轉(zhuǎn)換、卡爾曼濾波器的軟件實現(xiàn)以及PID控制器的軟件實現(xiàn)。并給出了小車速度閉環(huán)控制的PID程序。5. 系統(tǒng)調(diào)試兩輪自平衡車的硬件設(shè)計與軟件開發(fā)為自平衡車的運(yùn)行打下了基礎(chǔ)，是否能運(yùn)行的穩(wěn)定和快速則需要通過精心的調(diào)試才能實現(xiàn)。本系統(tǒng)采用了模塊化的設(shè)計思想，因此可以采用模塊化的調(diào)試方法。將系統(tǒng)分為獨(dú)立的幾個部分分別進(jìn)行調(diào)試。調(diào)試所需儀器儀表和工具如下：1. 計算機(jī)一臺2. 在線調(diào)試器（BDM）一個3. 萬用表一個4. 串口線一根MC9S12XS128的調(diào)試接口為BDM接口。采用開源開發(fā)調(diào)試工具TBDML對其進(jìn)行在線調(diào)試、仿真。IDE采用CodeWarriorIDE 。CodeWarrior系列集成開發(fā)環(huán)境（IDE，Integrated Development Environment）是由Metrowerks公司提供的專門面向 Freescale所有MCU嵌入式應(yīng)用開發(fā)的軟件工具。其中包括集成開發(fā)環(huán)境IDE、處理器專家、全芯片仿真、可視化參數(shù)顯示工具、項目工程管理、C交叉編譯器、匯編器、鏈接器以及調(diào)試器。圖51 BDM調(diào)試界面用萬用表仔細(xì)檢查電路的連接是否正確，是否存在短路，虛焊等情況。尤其是要注意電源極性是否正確，在確認(rèn)硬件電路連接無誤后，上電使用BDM在線調(diào)試，看是否能連接到MC9S12XS128最小系統(tǒng)。檢測正常后，向單片機(jī)下載測試程序。如果能夠正常下載程序，觀察LED是否正常閃動。電機(jī)驅(qū)動電路測試通過軟件控制電機(jī)轉(zhuǎn)速，驗證驅(qū)動電路的正確性。確定兩個電極輸出電壓的極性。保證輸出 PWM 正電壓時，電機(jī)正轉(zhuǎn)，PWM 輸出負(fù)電壓時，電極反轉(zhuǎn)。速度采集模塊測試，通過軟件采集編碼器輸入脈沖并技術(shù)，通過串口在上位機(jī)顯示。姿態(tài)檢測模塊測試則通過串口通訊向上位機(jī)發(fā)送陀螺儀及加速度計信號。若上述測試均能正常通過，則表明系統(tǒng)硬件電路工作正常。圖52 系統(tǒng)硬件測試姿態(tài)檢測模塊作為整個系統(tǒng)的檢測核心，直接關(guān)系到小車能否保持平衡，因此，對姿態(tài)檢測模塊的調(diào)試是調(diào)試工作的重點(diǎn)也是難點(diǎn)。首先，對陀螺儀及加速度計分別進(jìn)行調(diào)試，觀察其是否能正常工作，并對陀螺儀方法電路的偏置電壓進(jìn)行調(diào)整，使整個電路的溫漂最小，保證陀螺儀輸出信號的準(zhǔn)確性。將小車傾角分別置于45176。、90176。及135176。（與水平面的夾角），通過串口將陀螺儀積分出來的角度與加速度計換算出來的角度顯示在上位機(jī)上。通過對比，若計算出來的角度與實際傾角相差較大，則說明硬件電路或者軟件算法出現(xiàn)問題，需要及時調(diào)整。圖53顯示的是利用串口助手顯示的測量傾角，實際傾角為90176。圖53中，g表示陀螺儀積分出來的角度，積分周期5ms，a表示加速度計求出的角度?？梢园l(fā)現(xiàn)陀螺儀積分出來的角度會隨著實際推移而發(fā)現(xiàn)變化，若時間足夠長則陀螺儀積分出的角度很不準(zhǔn)確，這也說明了陀螺儀的輸出數(shù)據(jù)只在短時有效。同時，加速度計測量出來的角度在90176。附近變化，若長期記錄加速度計的測量角度再求其均值則會發(fā)現(xiàn)加速度計的輸出信號長時間較為穩(wěn)定，但其受震動影響較大，測量值在真實值上下波動。圖53陀螺儀及加速度計測量傾角顯示通過對陀螺儀及加速度計單獨(dú)檢測結(jié)果的分析也說明了采用多傳感器數(shù)據(jù)融合的必要性。所以，對卡爾曼濾波器的調(diào)試又是姿態(tài)檢測系統(tǒng)中的重點(diǎn)。卡爾曼濾波器的調(diào)試采用串口波形顯示的方式進(jìn)行。小車通過串口通訊向PC機(jī)發(fā)送濾波前與濾波后的姿態(tài)傾角數(shù)據(jù)，通過上位機(jī)波形顯示軟件，將離散的數(shù)據(jù)點(diǎn)描繪為動態(tài)波形，并在同一坐標(biāo)系中顯示，通過對比可以直觀的看出濾波器濾波效果，便于調(diào)試。由于陀螺儀需要經(jīng)過積分，因為積分周期在濾波器中的設(shè)定也尤為重要，為了與實際應(yīng)用相符，調(diào)試時數(shù)據(jù)發(fā)送周期為10ms，采樣率為100Hz。首先對濾波器的相關(guān)參數(shù)進(jìn)行設(shè)定。通過理論分析，結(jié)合國內(nèi)外研究者的經(jīng)驗，對卡爾曼濾波器參數(shù)進(jìn)行了初步設(shè)定。濾波器中需要整定的參數(shù)主要有三個：陀螺儀權(quán)值Q_gyro、加速度計權(quán)值Q_angle以及卡爾曼增益R_angle。濾波器參數(shù)設(shè)置完畢后，系統(tǒng)向上位機(jī)發(fā)送數(shù)據(jù)。此時，保持小車后輪不動，對小車姿態(tài)傾角進(jìn)行人為改變，通過上位機(jī)軟件顯示波形，如圖54。圖中標(biāo)號為1的波形是加速度計測量傾角波形，標(biāo)號為2的波形為經(jīng)卡爾曼濾波器后的傾角波形?？梢园l(fā)現(xiàn)，未濾波前的波形其噪聲很大，有的地方尖峰噪聲超過了真實值很多。而經(jīng)過濾波后的波形則十分平滑，沒有毛刺，同時也可以精確顯示小車姿態(tài)信息。圖54 卡爾曼濾波器輸出波形（1為加速度計輸出，2為濾波器輸出）將濾波器輸出波形放大后（如圖55）發(fā)現(xiàn)，傾角狀態(tài)改變時濾波器有一定的滯后，這會對小車的平衡控制產(chǎn)生影響導(dǎo)致小車不能穩(wěn)定。圖55放大后的波形存在滯后（1為加速度計輸出，2為濾波器輸出）對濾波器參數(shù)不斷調(diào)整后，濾波器輸出波形達(dá)到很好的效果，如圖56。圖56 輸出波形較為理想（1為加速度計輸出，2為濾波器輸出）在兩輪自平衡車的平衡控制中，采用了PD控制器。P作為比例控制，其主要作用是放大傾角偏差，輸出電機(jī)控制器，對小車來說P控制提供了抵消重力分量的回復(fù)力；D控制其主要作用預(yù)知傾角偏差的變化趨勢，在系統(tǒng)中相當(dāng)于提供了阻尼力。PD在兩輪自平衡小車中的作用如圖57。圖57 兩輪自平衡小車PD控制模型分析可知，本系統(tǒng)只使用P控制就可以使小車達(dá)到穩(wěn)定，但是調(diào)整時間較長，且小車會在平衡位置振蕩。加入D控制可以減小動態(tài)偏差，縮短控制過程時間。對PID參數(shù)的整定可以使用試湊法。首先，調(diào)整Kp大小，讓車站起來，小車在直立的過程中會有振蕩。然后將Kp減少至原值的70%，慢慢提高Kd的大小直到小車的可以穩(wěn)定的平衡。最后將Kp的值加大一點(diǎn)并對Kd的值進(jìn)行微調(diào)，使小車能夠快速穩(wěn)定的保持平衡。根據(jù)系統(tǒng)要求，小車不僅能夠穩(wěn)定的站立還要完成前進(jìn)、后退、轉(zhuǎn)彎等動作。因此還要對小車的動態(tài)性能進(jìn)行調(diào)試。為了更好的測試小車的動態(tài)性能，系統(tǒng)設(shè)計了一種尋線方式：通過自動識別道路中心線位置處由通有100mA 交變電流的導(dǎo)線所產(chǎn)生的電磁場而前進(jìn)。通過人為的設(shè)置一些較為復(fù)雜的道路，觀測小車在自動識別路徑并實現(xiàn)前進(jìn)、后退、轉(zhuǎn)彎等動作時是否能靈活自如，從而對其動態(tài)性能進(jìn)行改進(jìn)。經(jīng)過較長時間的測試與調(diào)整，本系統(tǒng)設(shè)計的兩輪自平衡小車不僅可以實現(xiàn)穩(wěn)定的站立，還能夠?qū)崿F(xiàn)前進(jìn)、后退、轉(zhuǎn)彎等運(yùn)動，其運(yùn)動較為靈活。在未知道路上，小車可以自主檢測路徑并沿路徑前行如圖58。圖58 兩輪自平衡車實現(xiàn)自主識別道路本章主要闡述系統(tǒng)各個模塊的調(diào)試步驟和方法，重點(diǎn)闡述了卡爾曼濾波器的調(diào)試方法，并設(shè)計了一種小車動態(tài)性能的測試方法。最終，小車的動態(tài)性能得到較大提高。6. 總結(jié)與展望 總結(jié)本設(shè)計主要研究兩輪自平衡小車的設(shè)計與實現(xiàn)。通過相應(yīng)硬件與軟件的設(shè)計，實現(xiàn)了兩輪自平衡小車的動態(tài)平衡與運(yùn)動控制。系統(tǒng)硬件結(jié)構(gòu)以飛思卡爾公司16位單片機(jī)MC9S12XS128為控制核心，采用壓電陀螺儀ENC03及MEMS加速度計MMA7260構(gòu)成了慣性姿態(tài)檢測系統(tǒng)，通過H橋電機(jī)驅(qū)動及旋轉(zhuǎn)編碼器實現(xiàn)了直流電機(jī)的閉環(huán)調(diào)速，最終實現(xiàn)了兩輪自平衡車的姿態(tài)檢測與平衡控制。系統(tǒng)軟件設(shè)計上比較了各類濾波器優(yōu)缺點(diǎn)，結(jié)合本系統(tǒng)硬件構(gòu)架設(shè)計了以卡爾曼濾波器為核心的數(shù)據(jù)融合算法。通過卡爾曼濾波器將陀螺儀與加速度計的輸出融合為準(zhǔn)確的傾角與角速度輸出，為系統(tǒng)的控制提供了有力保障。本設(shè)計的控制策略采用PID控制算法。通過對小車的運(yùn)動建模，構(gòu)建了小車運(yùn)動控制的PD控制算法，并對PID參數(shù)進(jìn)行了整定。本設(shè)計最終實現(xiàn)了兩輪自平衡小車的平衡控制及運(yùn)動控制。小車在保持兩輪平衡的基礎(chǔ)上可以實現(xiàn)前進(jìn)、后退、轉(zhuǎn)彎等基本動作。 展望由于時間及個人能力原因，本設(shè)計完成的兩輪自平衡小車只是一種簡單的模型。本設(shè)計還有以下方面可以繼續(xù)研究和