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