雙輪自平衡小車項目設(shè)計報告電子與信息工程學院項目設(shè)計報告項目名稱 雙輪自平衡小車設(shè)計 學生姓名 戴磊103621015 廖崎107221046 李旭103621045 王思然103522024 專業(yè) 電子信息科學與技術(shù)班級 103622 指導教師 李東京萬青趙東目錄一自平衡小車的總體方案設(shè)計 31、自平衡小車的設(shè)計方案 32、自平衡小車的總體框圖 3二系統(tǒng)的具體設(shè)計與實現(xiàn) 41、單片機控制模塊 42、陀螺儀加速度計模塊 43、光碼盤測速模塊 64、穩(wěn)壓模塊 75、電機驅(qū)動模塊 86、LCD1602顯示模塊 11三軟件系統(tǒng)設(shè)計 161、設(shè)計思想 16（1）PID技術(shù) 16（2）應(yīng)用現(xiàn)狀 16（3）PID調(diào)節(jié)規(guī)律 17（4）極點配置 18（5）極點配置條件 18（6）極點配置控制器 212、程序流程圖 223、程序代碼 23摘要隨著科技進步，生活水平的提高，人們追求智能與舒適的愿望也日益強烈。從而催生了許多智能化的產(chǎn)品。如智能電視、智能小車等。如何實現(xiàn)小車的小車的自動快捷駕駛，也成為人們心中的向往與疑問，基于這種趨勢與需求，著眼于實際情況。本文介紹了基于STC90C51單片機的自平衡小車系統(tǒng)的設(shè)計。系統(tǒng)基于陀螺儀等傳感器，利用PID平衡算法，對小車的速度傾斜角度平衡狀態(tài)來進行檢測，并通過單片機來控制電機來實現(xiàn)雙輪小車自如平衡地運動。從而實現(xiàn)小車智能自主控制的目的。關(guān)鍵詞：STC90C51自平衡PID算法一自平衡小車的總體方案設(shè)計1、自平衡小車的設(shè)計方案該自平衡小車，采用STC90C51單片機和各種傳感器的組合，構(gòu)成了自平衡小車系統(tǒng)。其系統(tǒng)主要由以下幾個部分組成：單片機控制系統(tǒng)、陀螺儀加速度檢測模塊、光碼盤測速模塊、穩(wěn)壓模塊、電機驅(qū)動模塊、LCD1602顯示模塊組成。本設(shè)計的自平衡小車工作原理：給小車通電，平衡放在地上，當小車開始傾斜時，陀螺儀及時地采集的小車傾斜角度數(shù)據(jù)傳給單片機，而加速度計將車子傾斜的瞬時加速度采集后也傳給單片機，同時，光碼測速儀也將車子的實時速度采集后傳給單片機。單片機系統(tǒng)收集到以上三組數(shù)據(jù)，對數(shù)據(jù)進行量化處理后，在PID平衡算法的控制下，控制電機及時地做出前進或后退或加速或減速的反應(yīng)，使車子在一個小角度范圍內(nèi)做平衡地來回擺動，以保持車子的不倒。2、自平衡小車的總體框圖自平衡小車主要由以下模塊組成：單片機控制系統(tǒng)、陀螺儀加速度檢測模塊、光碼盤測速模塊、穩(wěn)壓模塊、電機驅(qū)動模塊、LCD1602顯示模塊，以下是自平衡小車系統(tǒng)方框圖。單片機控制模塊陀螺儀加速度模塊單片機控制模塊陀螺儀加速度模塊電機驅(qū)動模塊電機驅(qū)動模塊光碼盤測速模塊光碼盤測速模塊LCD1602顯示模塊LCD1602顯示模塊穩(wěn)壓模塊（電源）穩(wěn)壓模塊（電源）自平衡小車系統(tǒng)框圖二系統(tǒng)的具體設(shè)計與實現(xiàn)1、單片機控制模塊單片機最小系統(tǒng)原理圖如下：單片機最小系統(tǒng)單片機最小系統(tǒng)由復位電路以及晶振電路組成，它是保證單片機能正常工作的最基本條件，在此不作過多介紹。2、陀螺儀加速度計模塊本設(shè)計中所采用的陀螺儀加速度設(shè)模塊為MPU6050，之所以選擇這個模塊，是因為其有以下優(yōu)點：（）集角度測量與加速度測量于一體（2）其那同時測量三軸上的角度與加速度測量（3）其輸出為數(shù)字信號，便于處理于存儲與傳輸（4）測量范圍大，反應(yīng)快。以下是MPU6050相關(guān)資料MPU-6000為全球首例整合性6軸運動處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了大量的包裝空間。MPU-6000整合了3軸陀螺儀、3軸加速器，并含可藉由第二個I2C端口連接其他廠牌之加速器、磁力傳感器、或其他傳感器的數(shù)位運動處理(DMP:DigitalMotionProcessor)硬件加速引擎，由主要I2C端口以單一數(shù)據(jù)流的形式，向應(yīng)用端輸出完整的9軸融合演算技術(shù)InvenSense的運動處理資料庫，可處理運動感測的復雜數(shù)據(jù)，降低了運動處理運算對操作系統(tǒng)的負荷，并為應(yīng)用開發(fā)提供架構(gòu)化的API。MPU-6000的角速度全格感測范圍為±250、±500、±1000與±2000°/sec(dps)，可準確追緃快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g±8g與±16g。產(chǎn)品傳輸可透過最高至400kHz的I2C或最高達20MHz的SPI。MPU-6000可在不同電壓下工作，VDD供電電壓介為2.5V±5%、3.0V±5%或3.3V±5%，邏輯接口VVDIO供電為1.8V±5%。MPU-6000的包裝尺寸4x4x0.9mm(QFN)，在業(yè)界是革命性的尺寸。其他的特征包含內(nèi)建的溫度感測器、包含在運作環(huán)境中僅有±1%變動的振蕩器。引腳圖實物圖3、光碼盤測速模塊此模塊有以下優(yōu)點：測速精度高，反應(yīng)快，因為其一圈有高達60個柵格（2）能測正反轉(zhuǎn)此模塊詳細資料如下：對管廠家：美國惠普HP原裝供電電壓：5V接線方式：紅5V

黑GND輸出信號：5Vp-p

兩相信號輸出（見下圖）。兩根信號線，一根黃色的一根藍色的（由于生產(chǎn)批次不同，有時候信號線可能為綠色，不影響使用）,黃色和藍色分別是2個TTL電平（可以直接接單片機）信號

（注:測正反轉(zhuǎn)就是靠這兩個電平區(qū)分是正轉(zhuǎn)還是反轉(zhuǎn)）（設(shè)A

B

為信號線）A先有脈沖,證明是正轉(zhuǎn)B先有脈沖,證明是反轉(zhuǎn)重要說明：長時間擋住或者不擋均沒有輸出，作為相位檢測，只有在對射的上升沿和下降沿才會有波形輸出，所以轉(zhuǎn)動碼盤或者用不透明物體來回切換狀態(tài)的時候才會有方波輸出，高速狀態(tài)下，類似正弦波或者鋸齒波，詳見下面視頻演示。萬用表測試一直表現(xiàn)為0.1V左右，是無效的！測試請用示波器或者單片機的采集口。測速原理示意圖測速原理演示圖4、穩(wěn)壓模塊之所以要設(shè)計穩(wěn)壓模塊，是因為平衡小車不同模塊所需電壓值不同，比如電機的工作電壓在9V，而單片機控制系統(tǒng)、LCD1602顯示模塊及測速模塊工作電壓在5V；但同時陀螺儀加速度模塊工作電壓卻在3.3V，因此有必要設(shè)計一穩(wěn)壓模塊，能夠同時穩(wěn)定地提供這三組電壓值。穩(wěn)壓模塊的原理圖如下：穩(wěn)壓模塊原理圖其中用到兩個穩(wěn)壓芯片LM7805及LM1117，LM7805能夠?qū)?V到12V之間的輸入電壓轉(zhuǎn)化為5V的輸出輸出電壓，同時LM117以LM7805V的輸出電壓為輸入電壓，將其轉(zhuǎn)化為3.3V輸出以供陀螺儀加速度模塊使用。圖中，LED為測試電路是否正常，正常時燈亮；反之，燈滅。D1為普通二極管，在LM7805的輸入與輸出之間起濾波作用。圖中電容也均為濾波作用。使用時，兩穩(wěn)壓芯片均要用散熱片，否則會導致溫度過高而燒壞。相應(yīng)的PCB布線圖如下：穩(wěn)壓模塊PCB圖5、電機驅(qū)動模塊由于電機工作電壓與單片機的不同，且單片機本身帶負載能力不強，電機工作需要較大電流等因素，無法用單片機直接驅(qū)動電機運轉(zhuǎn)，因此需要電機驅(qū)動模塊來驅(qū)動電機。在此設(shè)計中所選用的電機驅(qū)動為L298N模塊。它具有以下優(yōu)點：（1）負載能力強（2）能實現(xiàn)高電壓強電流的輸出（3）控制簡單方便具體資料如下：L298N是ST公司生產(chǎn)的一種高電壓、大電流電機驅(qū)動芯片。該芯片采用15腳封裝。主要特點是：工作電壓高，最高工作電壓可達46V；輸出電流大，瞬間峰值電流可達3A，持續(xù)工作電流為2A；額定功率25W。內(nèi)含兩個H橋的高電壓大電流全橋式驅(qū)動器，可以用來驅(qū)動直流電動機和步進電動機、繼電器線圈等感性負載；采用標準邏輯電平信號控制；具有兩個使能控制端，在不受輸入信號影響的情況下允許或禁止器件工作有一個邏輯電源輸入端，使內(nèi)部邏輯電路部分在低電壓下工作；可以外接檢測電阻，將變化量反饋給控制電路。使用L298N芯片驅(qū)動電機，該芯片可以驅(qū)動一臺兩相步進電機或四相步進電機，也可以驅(qū)動兩臺直流電機。簡要說明：尺寸：80mmX45mm主要芯片：L298N、光電耦合器工作電壓：控制信號直流5V；電機電壓直流3V~46V（建議使用36伏以下）最大工作電流：2.5A額定功率：25W特點：1、具有信號指示。2、轉(zhuǎn)速可調(diào)3、抗干擾能力強4、具有過電壓和過電流保護5、可單獨控制兩臺直流電機6、可單獨控制一臺步進電機7、PWM脈寬平滑調(diào)速8、可實現(xiàn)正反轉(zhuǎn)9、采用光電隔離使用直流/步進兩用驅(qū)動器可以驅(qū)動兩臺直流電機。分別為M1和M2。引腳A，B可用于輸入PWM脈寬調(diào)制信號對電機進行調(diào)速控制。（如果無須調(diào)速可將兩引腳接5V，使電機工作在最高速狀態(tài)，既將短接帽短接）實現(xiàn)電機正反轉(zhuǎn)就更容易了，輸入信號端IN1接高電平輸入端IN2接低電平，電機M1正轉(zhuǎn)。（如果信號端IN1接低電平，IN2接高電平，電機M1反轉(zhuǎn)。）控制另一臺電機是同樣的方式，輸入信號端IN3接高電平，輸入端IN4接低電平，電機M2正轉(zhuǎn)。（反之則反轉(zhuǎn)），PWM信號端A控制M1調(diào)速，PWM信號端B控制M2調(diào)速?？蓞⒖枷聢D表：電機旋轉(zhuǎn)方式控制端IN1控制端IN2控制端IN3控制端IN4輸入PWM信號改變脈寬可調(diào)速調(diào)速端A調(diào)速端BM1正轉(zhuǎn)高低//高/反轉(zhuǎn)低高//高/停止低低//高/M2正轉(zhuǎn)//高低/高反轉(zhuǎn)//低高/高停止低低///高其實物圖如下：L298N電機驅(qū)動模塊實物圖6、LCD1602顯示模塊此模塊主要用來顯示自平衡小車的各種參數(shù)，如傾斜角、瞬時速度、瞬時加速度等。同時也可以觀察可調(diào)參數(shù)，這些參數(shù)可以用按鍵輸入，通過LCD1602顯示出來，方便自平衡小車參數(shù)的觀察與更改，達到高效調(diào)速的目的。其實物圖如下：LCD1602實物圖其與單片機系統(tǒng)組成的原理圖如下：單片機控制系統(tǒng)及1602顯示模塊相應(yīng)的PCB布線圖如下：其詳細資料如下：1602LCD分為帶背光和不帶背光兩種，基控制器大部分為HD44780，帶背光的比不帶背光的厚，是否帶背光在應(yīng)用中并無差別1602LCD主要技術(shù)參數(shù)：顯示容量:16×2個字符芯片工作電壓:4.5—5.5V工作電流:2.0mA(5.0V)模塊最佳工作電壓:5.0V字符尺寸:2.95×4.35(W×H)mm引腳功能說明1602LCD采用標準的14腳（無背光）或16腳（帶背光）接口，各引腳接口說明如表10-13所示:編號符號引腳說明編號符號引腳說明1VSS電源地9D2數(shù)據(jù)2VDD電源正極10D3數(shù)據(jù)3VL液晶顯示偏壓11D4數(shù)據(jù)4RS數(shù)據(jù)/命令選擇12D5數(shù)據(jù)5R/W讀/寫選擇13D6數(shù)據(jù)6E使能信號14D7數(shù)據(jù)7D0數(shù)據(jù)15BLA背光源正極8D1數(shù)據(jù)16BLK背光源負極表10-13：引腳接口說明表第1腳：VSS為地電源。第2腳：VDD接5V正電源。第3腳：VL為液晶顯示器對比度調(diào)整端，接正電源時對比度最弱，接地時對比度最高，對比度過高時會產(chǎn)生“鬼影”，使用時可以通過一個10K的電位器調(diào)整對比度。第4腳：RS為寄存器選擇，高電平時選擇數(shù)據(jù)寄存器、低電平時選擇指令寄存器。第5腳：R/W為讀寫信號線，高電平時進行讀操作，低電平時進行寫操作。當RS和R/W共同為低電平時可以寫入指令或者顯示地址，當RS為低電平R/W為高電平時可以讀忙信號，當RS為高電平R/W為低電平時可以寫入數(shù)據(jù)。第6腳：E端為使能端，當E端由高電平跳變成低電平時，液晶模塊執(zhí)行命令。第7～14腳：D0～D7為8位雙向數(shù)據(jù)線。第15腳：背光源正極。第16腳：背光源負極。LCD寄存器的選擇ER/WRS功能說明100寫入命令寄存器101寫入數(shù)據(jù)寄存器110讀取忙碌標志及RAM地址111讀取RAM數(shù)據(jù)0X不動作LCD指令表指令功能控制線數(shù)據(jù)線RSR/WD7D6D5D4D3D2D1D0清除屏幕0000000001清除屏幕，并把光標移至左上角光標回到原點000000001x光標移至左上角，顯示內(nèi)容不變設(shè)定進入模式00000001I/DSI/D=1：地址遞增，I/D=0：地址遞減S=1：開啟顯示屏，S=0：關(guān)閉顯示屏顯示器開關(guān)0000001DCBD=1：開啟顯示幕C=1：開啟光標B=1：光標所在位置的字符閃爍移位方式000001S/CR/LxxS/C=0、R/L=0:光標左移；S/C=0、R/L=1：光標右移S/C=1、R/L=0：字符和光標左移；S/C=1、R/L=1：字符和光標右移功能設(shè)定00001DLNFxxDL=1：數(shù)據(jù)長度為8位，DL=0：數(shù)據(jù)長度為4位N=1：雙列字，N=0：單列字；F=1:5x10字形，F(xiàn)=0：5x7字形CGRAM地址設(shè)定0001CGRAM地址將所要操作的CGRAM地址放入地址計數(shù)器DDRAM地址設(shè)定001DDRAM地址將所要操作的DDRAM地址放入地址計數(shù)器忙碌標志位BF01BF地址計數(shù)器內(nèi)容讀取地址計數(shù)器，并查詢LCM是否忙碌，BF表示LCM忙碌寫入數(shù)據(jù)10寫入數(shù)據(jù)將數(shù)據(jù)寫入CGRAM或DDRAM讀取數(shù)據(jù)11讀取數(shù)據(jù)讀取CGRAM或DDRAM的數(shù)據(jù)圖10-571602LCD內(nèi)部顯示地址例如第二行第一個字符的地址是40H，那么是否直接寫入40H就可以將光標定位在第二行，第一個字符的位置呢？這樣不行，因為寫入顯示地址時要求最高位D7恒定為高電平1所以實際寫入的數(shù)據(jù)應(yīng)該是01000000B（40H）+10000000B(80H)=11000000B(C0H)。在對液晶模塊的初始化中要先設(shè)置其顯示模式，在液晶模塊顯示字符時光標是自動右移的，無需人工干預。每次輸入指令前都要判斷液晶模塊是否處于忙的狀態(tài)。1602液晶模塊內(nèi)部的字符發(fā)生存儲器（CGROM）已經(jīng)存儲了160個不同的點陣字符圖形，如圖10-58所示，這些字符有：阿拉伯數(shù)字、英文字母的大小寫、常用的符號、和日文假名等，每一個字符都有一個固定的代碼，比如大寫的英文字母“A”的代碼是01000001B（41H），顯示時模塊把地址41H中的點陣字符圖形顯示出來，我們就能看到字母“A”實物圖如下：三軟件系統(tǒng)設(shè)計1、設(shè)計思想其主要是通過PID技術(shù)對小車的狀態(tài)進行實時地跟蹤及調(diào)整。現(xiàn)對PID算法作些介紹。（1）PID技術(shù)控制技術(shù)是運動控制的核心，各種先進控制技術(shù)的研究不斷推動著運動控制的發(fā)展，比如自適應(yīng)控制技術(shù)和以神經(jīng)網(wǎng)絡(luò)和模糊控制為代表的智能控制技術(shù)，但在實際生產(chǎn)實踐中應(yīng)用最普遍的還是各種以PID為代表的基本控制技術(shù)按照偏差的比例、積分和微分進行控制的調(diào)節(jié)器，簡稱為PD調(diào)節(jié)器，是連續(xù)系統(tǒng)中技術(shù)成熟且應(yīng)用廣泛的一種調(diào)節(jié)器。本節(jié)將對系統(tǒng)用到的PID控制技術(shù)做相應(yīng)的介紹和研究，傳感器將車體的角度和運動速度等信息傳遞給系統(tǒng)控制器，控制器經(jīng)分析處理運用PID控制技術(shù)，將目標命令傳遞給電機驅(qū)動器來完成系統(tǒng)的閉環(huán)控制（2）應(yīng)用現(xiàn)狀在電機伺服系統(tǒng)的控制中，經(jīng)典的PID控制具有其結(jié)構(gòu)簡單、魯棒性強以及現(xiàn)場對Pro的廣泛使用積累了豐富的經(jīng)驗等優(yōu)點，在無刷直流電動機的控制方面一直占有很重要的地位。PID的引入保證了其系統(tǒng)響應(yīng)的快速性，穩(wěn)定了閉環(huán)控制器，補償了由逆變器引起的控制誤差[25][26]。PID控制器就是將偏差的比例(Proportional)、積分(Integral)和微分(Differential)通過線性組合構(gòu)成控制量，用這一控制量對被控對象進行控制。PID算法是目前工業(yè)過程控制中應(yīng)用最廣泛的控制算法。PID算法應(yīng)用如此廣泛，是因為它具有如下優(yōu)點:(1)算法較為簡單，易于實現(xiàn)；(2)基于線性控制理論，具備許多成熟的穩(wěn)定性分析方法，有較高的可靠性；(3)可以在很寬的操作條件內(nèi)保持較好的魯棒性，對于控制對象模型參數(shù)小范變化不敏感；(4)不要求了解控制對象的精確數(shù)學模型。利用許多成熟的參數(shù)整定方法，可以根據(jù)控制對象的實際響應(yīng)曲線來計算PID控制器的參數(shù)；(5)允許工程技術(shù)人員以一種簡單直接的方式來調(diào)節(jié)控制系統(tǒng)，以達到希望得到的控制性能，如上升時間、最大超調(diào)量和穩(wěn)態(tài)誤差等。當被控對象的結(jié)構(gòu)和參數(shù)不能完全掌握，或得不到精確的數(shù)學模型時，系統(tǒng)控制器的結(jié)構(gòu)和參數(shù)必須依靠經(jīng)驗和現(xiàn)場調(diào)試來確定，這時應(yīng)用PID控制技術(shù)最為方便。即當我們不完全了解一個系統(tǒng)和被控對象，或不能通過有效的測量手段來獲得系統(tǒng)參數(shù)時，最適合用PID控制技術(shù)。（3）PID調(diào)節(jié)規(guī)律PID控制分為兩大類，一個是模擬PID控制，一個是數(shù)字PM控制。在模擬控制系統(tǒng)中，PID是最常用的控制方法。圖4.7所示為模擬PID控制系統(tǒng)原理框圖。比例系數(shù)Kp比例系數(shù)Kp積分系數(shù)Ki微分系數(shù)Kd被控對象_ + +u(t)y(t) +圖4.7模擬PID控制系統(tǒng)原理框圖計算機控制系統(tǒng)中，使用的是數(shù)字PID控制器。將連續(xù)的模擬量進行離散化處理，則可得 （4-20）在數(shù)字PID控制中，采樣周期相對于系統(tǒng)的時間常數(shù)來說一般是很短。因此其參數(shù)可按模擬PID控制器中的方法來選擇。由于要保持動態(tài)的平衡，則小車的傾角在一定的范圍內(nèi)要求可控。故本文選擇系統(tǒng)的小車的傾角作為輸出量，利用PID技術(shù)對其進行分析。由上述理論分析可以看出:其比例、積分、微分三者是彼此影響；同時要使其系統(tǒng)保持穩(wěn)定，PID三參數(shù)必須滿足上述關(guān)系；為了讓系統(tǒng)能夠?qū)崿F(xiàn)其動態(tài)的平衡，需要通過反復試湊的方法來解決，而且整定的參數(shù)多導致反復試湊的次數(shù)極大的增加，控制器的參數(shù)較難選取[27]。加之系統(tǒng)經(jīng)過線性化處理，即使所選參數(shù)滿足上述的關(guān)系式，仍需要根據(jù)系統(tǒng)的實際模型進行調(diào)整，這大大增加了調(diào)試的難度。鑒于此我們考慮采用其極點配置的方法來考察分析其系統(tǒng)的穩(wěn)定性。（４）極點配置所謂極點配置就是指利用狀態(tài)反饋或輸出反饋使閉環(huán)系統(tǒng)的極點位于所希望的極點位置。由于系統(tǒng)的性能和它的極點位置密切相關(guān)，因此極點配置問題在系統(tǒng)中是很重要的?？刂葡到y(tǒng)的各種特性及其各種品質(zhì)指標很大程度上由閉環(huán)系統(tǒng)的零點和極點位置決定。零點和極點在復數(shù)平面中的分布狀況決定了相應(yīng)表達式中該函數(shù)前的系數(shù)大小。一組零點和極點的分布就對應(yīng)了一個系統(tǒng)的響應(yīng)。極點配置的問題，就是通過選擇反饋增益矩陣，將閉環(huán)系統(tǒng)的極點恰好配置在根平面上期望的位置，以獲得所希望的動態(tài)性能。極點配置決定了控制系統(tǒng)的動態(tài)性能和穩(wěn)定性[28]。對于系統(tǒng)的極點配置，需要解決兩個問題:一是建立極點可配置的條件；二是確定其反饋增益矩陣。（５）極點配置條件狀態(tài)能控性、狀態(tài)能觀性及其穩(wěn)定性都是控制系統(tǒng)的重要屬性。要設(shè)計相應(yīng)穩(wěn)定的系統(tǒng)，必須先考察其能控性和能觀性。狀態(tài)能控性問題只考察系統(tǒng)在u(t)作用下狀態(tài)的轉(zhuǎn)移情況，與輸出量y(t)無關(guān)。對于線性連續(xù)定常系統(tǒng)，如果存在一個分段連續(xù)的輸入u(t)，能在有限時間區(qū)間[t0，tf]內(nèi)，使系統(tǒng)由某一初始狀態(tài)x(t0)轉(zhuǎn)移到指定的任一終端狀態(tài)x(tf)，則該狀態(tài)就是能控的。若系統(tǒng)的所有狀態(tài)都是能控的，則稱此系統(tǒng)是狀態(tài)完全能控。本文小車參數(shù)參考LEGO公司提供的主要機器人組建參數(shù)如下表：小車的車輪質(zhì)量（Mw）0.03kg小車的車輪半徑（r）0.04m車輪的轉(zhuǎn)動慣量（Iw）車身的質(zhì)量（Mp）0.6kg車輪中心到機器人的質(zhì)心的距離（l）0.072m車身轉(zhuǎn)動慣量（Ip）電機慣量（Jm）電機的電阻（Rm）6.69Ω反電動勢常量（ke）0.468Vsec/rad電機轉(zhuǎn)矩常量（km）0.317Nm/A該系統(tǒng)的能控性矩陣可表示為： （4-21）利用文獻[27]中對系統(tǒng)能控性的判定定理，即線性非時變系統(tǒng)為完全能控系統(tǒng)的充分必要條件是能控判別矩陣滿秩。即若rank(Tc)=4，則系統(tǒng)能控。利用MATLAB仿真工具，調(diào)用其中函數(shù)為Tc=ctrb(A，B)。其代碼及結(jié)果如下所示。>>A=[0100:0-56.5160:0001:0-525239.50]>>B=[0:483.2:0:44.9]>>Ic=ctrb(A,B)%求系統(tǒng)的能控判別矩陣Ic=1.0e+008*00.0000-0.00030.01540.0000-0.00030.0154-0.912500.0000-0.00250.14340.00000.00250.1434-8.7094>>rank(Tc)%求矩陣的值，若此數(shù)值是4，則該系統(tǒng)完全可控ans=4由上可以看出，其rank(Tc)=4，這說明兩輪自平衡小車系統(tǒng)是完全能控的，只有在此基礎(chǔ)才可以設(shè)計控制器，實現(xiàn)平衡控制。狀態(tài)能觀性問題是指對于任意給定的輸入u(t)在有限觀測時間tf>t0，使得根據(jù)[t0，tf]期間的輸出y(t)能唯一的確定系統(tǒng)在初始時刻的狀態(tài)x(t0)，則該狀態(tài)x(t0)是能觀測的。若系統(tǒng)的每一個狀態(tài)都是能觀的，則稱此系統(tǒng)是狀態(tài)完全能觀。該系統(tǒng)狀態(tài)可觀性矩陣為： （4-22）線性非時變系統(tǒng)為完全能觀系統(tǒng)的充分必要條件是能觀判別矩陣滿秩。即若rank(To)=4，則系統(tǒng)能觀。同樣利用MATLAB仿真工具，調(diào)用其中函數(shù)為To=obsv(A，C)；其代碼及結(jié)果如下所示。>>A=[0100:0-56.5160:0001:0-525239.50]:>>C=[1000:0010]:>>To=obsv(A,C)%求系統(tǒng)能觀判別矩陣To=1.0e+004*0.00010000 00.0001000.0001000000.00010-0.00560.001600-0.05250.0239000.3192-0.09040.001602.9663-0.84000.0239>>rank(To)%求矩陣的值，若數(shù)值為4，則該系統(tǒng)完全可觀ans=4從上中可得出：其rank(To)=4，則該系統(tǒng)能觀。在控制系統(tǒng)中，反饋控制信息是由系統(tǒng)的輸出或者狀態(tài)變量組合而成的。但是并非所有系統(tǒng)的狀態(tài)變量在物理上都能測得，于是能否通過對輸出的測量獲得全部的狀態(tài)變量的信息，便是系統(tǒng)的能觀測問題。此時系統(tǒng)能觀，可以設(shè)計控制器對那些不能測量的量進行觀測，觀測系統(tǒng)變動對它們的影響。由上述分析可得該系統(tǒng)能控且能觀，滿足極點配置的條件，因此可以利用狀態(tài)反饋或輸出反饋來配置該系統(tǒng)的閉環(huán)極點。（６）極點配置控制器極點配置的問題，就是通過選擇反饋增益矩陣，將閉環(huán)系統(tǒng)的極點恰好配置在根平面上期望的位置，以獲得所希望的動態(tài)性能。極點配置決定了控制系統(tǒng)的動態(tài)性能和系統(tǒng)的穩(wěn)定性。本文通過狀態(tài)反饋來實現(xiàn)極點配置。系統(tǒng)采用狀態(tài)反饋對系統(tǒng)(ABC)任意配置極點的充要條件是系統(tǒng)能控。而第4.5.1節(jié)已經(jīng)證明該系統(tǒng)滿足其條件，因此可以利用狀態(tài)反饋法來任意配置系統(tǒng)的極點。對于系統(tǒng)(組4-2)引入狀態(tài)反饋，假定考慮進入受控系統(tǒng)的信號為u=v-kx且D=0的情況，其中v為系統(tǒng)的外部參考輸入信號，則可將系統(tǒng)的閉環(huán)狀態(tài)方程寫成(組4-3)。 由于穩(wěn)定是控制系統(tǒng)能夠運行的前提，因此選擇調(diào)節(jié)時間和超調(diào)量作為考察系統(tǒng)動態(tài)性能的指標。為了讓系統(tǒng)滿足如下的性能要求：超調(diào)量：調(diào)節(jié)時間：取ε=0.6、，由可得該閉環(huán)系統(tǒng)的期望主導極點為：s1，2=-3±4i，同時要求另外的兩個極點在主導極點左邊且遠離主導極點，一般取其距離主導極點的4到5倍左右，本設(shè)計選取-15和-20。這樣高階系統(tǒng)可近似為二階系統(tǒng)來處理。所謂極點配置問題是指通過尋找適當?shù)臓顟B(tài)反饋增益矩陣K，使得閉環(huán)系統(tǒng)極點(即矩陣A—BK的特征值)位于預先給定位置的狀態(tài)反饋控制器設(shè)計問題。其加入狀態(tài)反饋后的系統(tǒng)結(jié)構(gòu)圖如圖4.8所示。BB(4×1)C(2×4)A(4×4)K(1×4)圖4.8加入狀態(tài)反饋后的系統(tǒng)結(jié)構(gòu)圖由線性時不變系統(tǒng)的穩(wěn)定性分析可知，當v=0時，閉環(huán)系統(tǒng)方程的穩(wěn)定性由其變化系統(tǒng)矩陣(A-BK)的特征值決定，即其矩陣(A-BK)的所有特征值都具有負實部。而由經(jīng)典控制理論知道，矩陣(A-BK)的特征值也將影響諸如衰減速度、振蕩、超調(diào)等過渡過程特性。因此需要找到一個合適的矩陣K，使得矩陣(A-BK)的特征值位于復平面預先給定的特定位置，同時具有所期望的動態(tài)響應(yīng)特性。２、程序流程圖3、程序代碼#include<REG52.H> #include<math.h>//Keillibrary#include<stdio.h>//Keillibrary #include<INTRINS.H>typedefunsignedcharuchar;typedefunsignedshortushort;typedefunsignedintuint;//****************************************//定義51單片機端口//****************************************#defineDataPortP2 //LCD1602數(shù)據(jù)端口sbitP30=P3^0;sbitP31=P3^1;sbitP32=P3^2;sbitP33=P3^3;sbitP34=P3^4;sbitP35=P3^5;sbitkey0=P1^0;sbitkey1=P1^1;sbitkey2=P1^2;sbitkey3=P1^3;sbitP14=P1^4;sbitP15=P1^5;sbitP16=P1^6;sbitP17=P1^7;sbitP00=P0^0;sbitP01=P0^1;sbitP02=P0^2;sbitP03=P0^3;sbitP04=P0^4;sbitP05=P0^5;sbitP06=P0^6;sbitP07=P0^7;sbitSCL=P3^6; //IIC時鐘引腳定義sbitSDA=P3^7; //IIC數(shù)據(jù)引腳定義sbitLCM_RS=P0^5; //LCD1602命令端口 sbitLCM_RW=P0^6; //LCD1602命令端口 sbitLCM_EN=P0^7; //LCD1602命令端口//****************************************//定義MPU6050內(nèi)部地址//****************************************#define SMPLRT_DIV 0x19 //陀螺儀采樣率，典型值：0x07(125Hz)#define CONFIG 0x1A //低通濾波頻率，典型值：0x06(5Hz)#define GYRO_CONFIG 0x1B //陀螺儀自檢及測量范圍，典型值：0x18(不自檢，2000deg/s)#define ACCEL_CONFIG 0x1C //加速計自檢、測量范圍及高通濾波頻率，典型值：0x01(不自檢，2G，5Hz)#define ACCEL_XOUT_H 0x3B#define ACCEL_XOUT_L 0x3C//#define ACCEL_YOUT_H 0x3D//#define ACCEL_YOUT_L 0x3E//#define ACCEL_ZOUT_H 0x3F//#define ACCEL_ZOUT_L 0x40#define TEMP_OUT_H 0x41#define TEMP_OUT_L 0x42#define GYRO_XOUT_H 0x43#define GYRO_XOUT_L 0x44 //#define GYRO_YOUT_H 0x45//#define GYRO_YOUT_L 0x46//#define GYRO_ZOUT_H 0x47//#define GYRO_ZOUT_L 0x48#define PWR_MGMT_1 0x6B //電源管理，典型值：0x00(正常啟用)#define WHO_AM_I 0x75 //IIC地址寄存器(默認數(shù)值0x68，只讀)#define SlaveAddress 0xD0 //IIC寫入時的地址字節(jié)數(shù)據(jù)，+1為讀取//****************************************//定義類型及變量//****************************************uchardis[4]; //顯示數(shù)字(-511至512)的字符數(shù)組int dis_data; //變量//int Temperature,Temp_h,Temp_l; //溫度及高低位數(shù)據(jù)//****************************************//函數(shù)聲明//****************************************voiddelay(unsignedintk); //延時//LCD相關(guān)函數(shù)voidInitLcd(); //初始化lcd1602voidlcd_printf(uchar*s,inttemp_data);voidWriteDataLCM(uchardataW); //LCD數(shù)據(jù)voidWriteCommandLCM(ucharCMD,ucharAttribc); //LCD指令voidDisplayOneChar(ucharX,ucharY,ucharDData); //顯示一個字符voidDisplayListChar(ucharX,ucharY,uchar*DData,L); //顯示字符串//MPU6050操作函數(shù)voidInitMPU6050(); //初始化MPU6050voidDelay5us();voidI2C_Start();voidI2C_Stop();voidI2C_SendACK(bitack);bitI2C_RecvACK();voidI2C_SendByte(uchardat);ucharI2C_RecvByte();voidI2C_ReadPage();voidI2C_WritePage();voiddisplay_ACCEL_x();voiddisplay_ACCEL_y();voiddisplay_ACCEL_z();ucharSingle_ReadI2C(ucharREG_Address); //讀取I2C數(shù)據(jù)voidSingle_WriteI2C(ucharREG_Address,ucharREG_data); //向I2C寫入數(shù)據(jù)//****************************************//整數(shù)轉(zhuǎn)字符串//****************************************voidlcd_printf(uchar*s,inttemp_data){ if(temp_data<0) { temp_data=-temp_data; *s='-'; } else*s=''; *++s=temp_data/100+0x30; temp_data=temp_data%100;//取余運算 *++s=temp_data/10+0x30; temp_data=temp_data%10;//取余運算 *++s=temp_data+0x30; }//****************************************//延時//****************************************voiddelay(unsignedintk) { unsignedinti,j; for(i=0;i<k;i++) { for(j=0;j<3;j++); } }//****************************************//LCD1602初始化//****************************************voidInitLcd() { WriteCommandLCM(0x38,1); WriteCommandLCM(0x08,1); WriteCommandLCM(0x01,1); WriteCommandLCM(0x06,1); WriteCommandLCM(0x0c,1); DisplayOneChar(0,0,'A'); DisplayOneChar(0,1,'G');} //****************************************//LCD1602寫允許//****************************************voidWaitForEnable(void) { DataPort=0xff; LCM_RS=0;LCM_RW=1;_nop_(); LCM_EN=1;_nop_();_nop_(); while(DataPort&0x01); LCM_EN=0; } //****************************************//LCD1602寫入命令//****************************************voidWriteCommandLCM(ucharCMD,ucharAttribc){ CMD=((CMD&0x01)<<7)|((CMD&0x02)<<5)|((CMD&0x04)<<3)|((CMD&0x08)<<1)|((CMD&0x10)>>1)|((CMD&0x20)>>3)|((CMD&0x40)>>5)|((CMD&0x80)>>7); if(Attribc)WaitForEnable(); LCM_RS=0;LCM_RW=0;_nop_(); DataPort=CMD;_nop_(); LCM_EN=1;_nop_();_nop_();LCM_EN=0;} //****************************************//LCD1602寫入數(shù)據(jù)//****************************************voidWriteDataLCM(uchardataW){ dataW=((dataW&0x01)<<7)|((dataW&0x02)<<5)|((dataW&0x04)<<3)|((dataW&0x08)<<1)|((dataW&0x10)>>1)|((dataW&0x20)>>3)|((dataW&0x40)>>5)|((dataW&0x80)>>7); WaitForEnable(); LCM_RS=1;LCM_RW=0;_nop_(); DataPort=dataW;_nop_(); LCM_EN=1;_nop_();_nop_();LCM_EN=0;} //****************************************//LCD1602寫入一個字符//****************************************voidDisplayOneChar(ucharX,ucharY,ucharDData){ Y&=1; X&=15; if(Y)X|=0x40; X|=0x80; WriteCommandLCM(X,0); WriteDataLCM(DData); } //****************************************//LCD1602顯示字符串//****************************************voidDisplayListChar(ucharX,ucharY,uchar*DData,L){ ucharListLength=0; Y&=0x1; X&=0xF; while(L--) { DisplayOneChar(X,Y,DData[ListLength]); ListLength++; X++; }}//**************************************//延時5微秒(STC90C52RC@12M)//不同的工作環(huán)境,需要調(diào)整此函數(shù)//當改用1T的MCU時,請調(diào)整此延時函數(shù)//**************************************voidDelay5us(){ _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_(); _nop_();_nop_();_nop_();_nop_();}//**************************************//I2C起始信號//**************************************voidI2C_Start(){SDA=1;//拉高數(shù)據(jù)線SCL=1;//拉高時鐘線Delay5us();//延時SDA=0;//產(chǎn)生下降沿Delay5us();//延時SCL=0;//拉低時鐘線}//**************************************//I2C停止信號//**************************************voidI2C_Stop(){SDA=0;//拉低數(shù)據(jù)線SCL=1;//拉高時鐘線Delay5us();//延時SDA=1;//產(chǎn)生上升沿Delay5us();//延時}//**************************************//I2C發(fā)送應(yīng)答信號//入口參數(shù):ack(0:ACK1:NAK)//**************************************voidI2C_SendACK(bitack){SDA=ack;//寫應(yīng)答信號SCL=1;//拉高時鐘線Delay5us();//延時SCL=0;//拉低時鐘線Delay5us();//延時}//**************************************//I2C接收應(yīng)答信號//**************************************bitI2C_RecvACK(){SCL=1;//拉高時鐘線Delay5us();//延時CY=SDA;//讀應(yīng)答信號SCL=0;//拉低時鐘線Delay5us();//延時returnCY;}//**************************************//向I2C總線發(fā)送一個字節(jié)數(shù)據(jù)//**************************************voidI2C_SendByte(uchardat){uchari;for(i=0;i<8;i++)//8位計數(shù)器{dat<<=1;//移出數(shù)據(jù)的最高位SDA=CY;//送數(shù)據(jù)口SCL=1;//拉高時鐘線Delay5us();//延時SCL=0;//拉低時鐘線Delay5us();//延時}I2C_RecvACK();}//**************************************//從I2C總線接收一個字節(jié)數(shù)據(jù)//**************************************ucharI2C_RecvByte(){uchari;uchardat=0;SDA=1;//使能內(nèi)部上拉,準備讀取數(shù)據(jù),for(i=0;i<8;i++)//8位計數(shù)器{dat<<=1;SCL=1;//拉高時鐘線Delay5us();//延時dat|=SDA;//讀數(shù)據(jù)SCL=0;//拉低時鐘線Delay5us();//延時}returndat;}//**************************************//向I2C設(shè)備寫入一個字節(jié)數(shù)據(jù)//**************************************voidSingle_WriteI2C(ucharREG_Address,ucharREG_data){I2C_Start();//起始信號I2C_SendByte(SlaveAddress);//發(fā)送設(shè)備地址+寫信號I2C_SendByte(REG_Address);//內(nèi)部寄存器地址，I2C_SendByte(REG_data);//內(nèi)部寄存器數(shù)據(jù)，I2C_Stop();//發(fā)送停止信號}//**************************************//從I2C設(shè)備讀取一個字節(jié)數(shù)據(jù)//**************************************ucharSingle_ReadI2C(ucharREG_Address){ ucharREG_data; I2C_Start();//起始信號 I2C_SendByte(SlaveAddress);//發(fā)送設(shè)備地址+寫信號 I2C_SendByte(REG_Address);//發(fā)送存儲單元地址，從0開始 I2C_Start();//起始信號 I2C_SendByte(SlaveAddress+1);//發(fā)送設(shè)備地址+讀信號 REG_data=I2C_RecvByte();//讀出寄存器數(shù)據(jù) I2C_SendACK(1);//接收應(yīng)答信號 I2C_Stop();//停止信號 returnREG_data;}//**************************************//初始化MPU6050//**************************************voidInitMPU6050(){ Single_WriteI2C(PWR_MGMT_1,0x00); //解除休眠狀態(tài) Single_WriteI2C(SMPLRT_DIV,0x07); Single_WriteI2C(CONFIG,0x06); Single_WriteI2C(GYRO_CONFIG,0x18); Single_WriteI2C(ACCEL_CONFIG,0x01);}//**************************************//合成數(shù)據(jù)//**************************************intGetData(ucharREG_Address){ charH,L; H=Single_ReadI2C(REG_Address); L=Single_ReadI2C(REG_Address+1); return(H<<8)+L;//合成數(shù)據(jù)}//**************************************//在1602上顯示10位數(shù)據(jù)//**************************************voidDisplay10BitData(intvalue,ucharx,uchary){ value/=19; value-=20; //角度值基準校正 //轉(zhuǎn)換為10位數(shù)據(jù) lcd_printf(dis,value); //轉(zhuǎn)換數(shù)據(jù)顯示 DisplayListChar(x,y,dis,4); //啟始列，行，顯示數(shù)組，顯示長度}voidmain(){inti,j,k,m,n,num0=380,num1,num2=380,num3,num4=380,num5,num6=380,num7,cun1,t1,t2; charc='A'; delay(1000); //上電延時 InitLcd(); //液晶初始化 InitMPU6050(); //初始化MPU6050 delay(500); while(1) { for(i=0;i<50000;i++) { if(key0==0) { delay(4000); if(key0==0) { c++; } } if(key1==0) { delay(4000); if(key1==0) { c--; } } if(key2==0&&c=='A') { delay(4000); if(key2==0) { num0+=19; num1++; } } if(key3==0&&c=='A') { delay(4000); if(key3==0) { num0-=19; num1--; } } if(key2==0&&c=='B') { delay(4000); if(key2==0) { num2+=19; num3++; } } if(key3==0&&c=='B') { delay(4000); if(key3==0) { num2-=19; num3--; } } if(key2==0&&c=='C') { delay(4000); if(key2==0) { num4+=19; num5++; } } if(key3==0&&c=='C') { delay(4000); if(key3==0) { num4-=19; num5--; } } if(key2==0&&c=='D') { delay(4000); if(key2==0) { num6+=19; num7++; } } if(key3==0&&c=='D') { delay(4000); if(key3==0) { num6-=19; num7--; } } m=GetData(ACCEL_XOUT_H)/96; //使m的值介于正負180 n=abs(m); j=m/5; k=n*3/5+num1/2+j*num1/3+j*j*num1/4+num3+num3*num3/2; cun1=n*4/5+num5/2+j*num5/2+j*j*num5/3+num7+num7*num7/2; t1=k; t2=cun1; if(