【正文】 好者和研究者開(kāi)始關(guān)注兩輪自平衡機(jī)器人。卡門(mén)觀察人類(lèi)走路的姿勢(shì)特性，領(lǐng)悟到其實(shí)人類(lèi)之所以可以平穩(wěn)地直立行走，是因?yàn)轶w內(nèi)靈敏的平衡器官可以精確地判斷出身體重心的改變量，透過(guò)小腦的即時(shí)反應(yīng)，然后利用腿部的肌肉即時(shí)出力來(lái)平衡傾倒的態(tài)勢(shì)。在機(jī)械結(jié)構(gòu)上保持小車(chē)重心的穩(wěn)定性，才能減少控制系統(tǒng)由于車(chē)身機(jī)械結(jié)構(gòu)的不合理性而造成的控制復(fù)雜化；硬件系 統(tǒng)必須包含自平衡車(chē)所需的所有電子系統(tǒng)與電氣設(shè)備；軟件系統(tǒng)則具體負(fù)責(zé)車(chē)身平衡控制。并且兩輪子平衡車(chē)是本質(zhì)不穩(wěn)定的非線性系統(tǒng)，因此建模必須考慮線性化問(wèn)題。采用 MEMS(MicroElectroMechanical System， 微機(jī)電 系統(tǒng) )陀螺儀和加速度計(jì)等慣性傳感器構(gòu) 成的姿態(tài)檢測(cè)系統(tǒng)可以實(shí)時(shí)、 準(zhǔn)確的檢測(cè)兩輪自平衡車(chē)的傾角。傳統(tǒng)的 PID 控制在各類(lèi)工業(yè)場(chǎng)合有著廣泛的應(yīng)用，完全可以滿足兩輪自平衡車(chē)的控制系統(tǒng)要求。具體包括： (1) 機(jī)器人本體設(shè)計(jì)：包括機(jī)械，重 心調(diào)整，電氣系統(tǒng)設(shè)計(jì)等，為進(jìn)一步研究提供良好的平臺(tái)； (2) 信號(hào)調(diào)理及控制部分電路設(shè)計(jì)：陀螺儀輸出信號(hào)需要經(jīng)過(guò)進(jìn)一步濾波放大 ，因此需要設(shè)計(jì)信號(hào)調(diào)理電路，同時(shí)控制核心需要構(gòu)建相關(guān)輸入輸出模塊及人際交互設(shè)備，因此需要對(duì)主控單元電路進(jìn)行設(shè)計(jì)。小車(chē)的傾角閉環(huán)控制以及直流電機(jī)的閉環(huán)速度控制。 第四章：系統(tǒng)軟件設(shè)計(jì)，介紹單片機(jī)初始化 ，濾波算法及控制算法，闡述各 模塊 軟件常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 4 設(shè)計(jì) 方法。分析系統(tǒng)要 求可知，保持小車(chē)直立和運(yùn)動(dòng)的動(dòng)力都來(lái)自于小車(chē)的兩只車(chē)輪，車(chē)輪 由兩只直流 電機(jī)驅(qū)動(dòng)。 (3)小車(chē)方向控制：通過(guò)控制兩 個(gè)電機(jī)間的轉(zhuǎn)速不同 實(shí)現(xiàn)轉(zhuǎn)向。由于最終都是對(duì)同一個(gè)控制對(duì)象（小車(chē)的電機(jī)）進(jìn)行控制，所以各個(gè)子系統(tǒng) 之間存在著耦合。由于小車(chē)同時(shí)受到 三種控制的影響，從小車(chē)平衡控制的角度來(lái)看，其它兩個(gè)控制就成為干擾。其中平衡控制是系統(tǒng)的最基本要求 ， 也是整個(gè)控制系統(tǒng)的難點(diǎn)。這需要兩個(gè)條件：一個(gè)是托著木棍的手指可以自由移動(dòng)；另一個(gè)是人的眼睛 可以觀察木棍的傾斜角度與傾斜趨勢(shì) (角速度 )。而小車(chē)上裝載的姿態(tài)檢測(cè)系統(tǒng)能夠?qū)π≤?chē)的傾斜狀況進(jìn)行實(shí)時(shí)檢測(cè)，通過(guò)控 制器控制車(chē)輪轉(zhuǎn)動(dòng)，抵消在這個(gè)維度上的傾斜力矩便可以保持小車(chē)平衡， 如圖 22 所示 。 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 7 θlmmglm單 擺 模 型一 級(jí) 倒 立 擺 模 型 圖 23 小車(chē)抽象為一級(jí)倒立擺模型 對(duì)普通單擺進(jìn)行受力分析如圖 24 所示。由于空氣阻力的存在，單擺最終會(huì)停止在平衡位置。 如果沒(méi)有阻尼力，單擺會(huì)在平衡位置左右晃動(dòng)而無(wú)法停止。 對(duì)靜止的一級(jí)倒立擺模型進(jìn)行受力分析（不考慮車(chē)輪與地面的滾動(dòng)摩擦力），如圖 25所示。顯然，只能通過(guò)第二種方法實(shí)現(xiàn)倒立擺的平衡，即在系統(tǒng)中額外增加一種力使合回復(fù)力與偏移方向相反。 通過(guò)控制倒立擺底部車(chē)輪 ，使其做加速運(yùn)動(dòng)。選取地面為參考的慣性系，根據(jù)牛頓第二定律可知倒立擺受到的慣性力為： g cosF ma ?? （式 23） 這樣，倒立擺所受到的合回復(fù)力為： si n c osF m g m a???? （式 24） 在平衡控制系統(tǒng)中，可控偏移角θ較小，對(duì)其進(jìn)行線性化。增加的阻尼力與偏移角速度成正比，方向相反， 因此式（ 25）可變?yōu)椋? 12F m g m k m k? ? ? ?? ? ? （式 26） 這樣車(chē)輪需要提供的加速度即為： 12a mk mk????? （式 27） 式中 ? 為傾角， ?? 為傾角速度， 1k 、 2k 為比例系數(shù)。假設(shè)其受外力干擾引起的車(chē)體角加速度為 ()xt ， 沿垂直于車(chē)體方向進(jìn)行受力分析如圖 27，可以得到自平衡車(chē)傾角與車(chē)輪移動(dòng)加速度為 ()at 以及外力干擾帶來(lái)的加速常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 10 度 ()xt 之間的運(yùn)動(dòng)方程。在工程實(shí)際中，應(yīng)用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡(jiǎn)稱(chēng) PID控制，又稱(chēng) PID 調(diào)節(jié)。 PID 控制器具有原理簡(jiǎn)單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對(duì)模型依賴 少等特點(diǎn)，因此 使用 PID 控制器實(shí)現(xiàn) 兩輪自平衡車(chē) 的控制 是完全可行的。 由小車(chē)受力分析可知小車(chē)平衡的條件是提供額外的回復(fù) 力及阻尼，其來(lái)源為車(chē)輪與地面的摩擦力。要滿足這一點(diǎn)，需要 1k g, 2k 0。其中微分參數(shù)相當(dāng)于阻尼力，可以有效抑制自平衡車(chē) 振 蕩。因此需要實(shí)時(shí)檢測(cè)自身傾角，再 進(jìn)行合理調(diào)整，就可以實(shí)現(xiàn)動(dòng)態(tài)平衡， 因而姿態(tài)檢測(cè)成為控制小車(chē)直立平衡的關(guān)鍵。本系統(tǒng)采用 MEMS 加速度計(jì)和陀螺儀構(gòu)成自平衡車(chē)的姿態(tài)檢測(cè)系統(tǒng)。當(dāng)旋轉(zhuǎn)器件時(shí)會(huì)改變振動(dòng)頻率 ， 從而反映出物體旋轉(zhuǎn)的角速度。 圖 29 陀螺儀 ENC03 陀螺儀直接輸出角速度，將角速度進(jìn)行積分便可以得到角度。 加速度計(jì) 加速度計(jì)是一種利用檢測(cè)質(zhì)量塊的慣性力來(lái)測(cè)量載體加速度的敏感裝置， 分為線加速度計(jì)和角加速度計(jì)。因此不能直接使用。 多傳感器數(shù)據(jù)融合是一個(gè)非常重要的研究?jī)?nèi)容，只有采用最適合的融合方法才能獲得最佳的效果。考慮到本系統(tǒng)使用的慣性器件特性較差，互補(bǔ)濾波在本質(zhì)原理上不能彌補(bǔ)器件特性缺陷， 故 本系統(tǒng)采用 卡爾曼 濾波算法作為數(shù)據(jù)融合方法。對(duì)于解決大部分的問(wèn)題， 是最優(yōu)，效率 最高甚至是最有用的?？柭鼮V波器不僅能估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)。在預(yù)估 階段，濾波器根據(jù)上一時(shí)刻狀態(tài)，估算出當(dāng)前時(shí)刻狀態(tài)；在更新階段，濾波器利用當(dāng)前時(shí)刻觀測(cè)值優(yōu)化在預(yù)估階段獲得的測(cè)量值，以獲得一個(gè)更準(zhǔn)確的新估計(jì)值 [11][12][13]。解算出小車(chē)運(yùn)動(dòng)控制的傳遞函數(shù)并利用自動(dòng)控制理論進(jìn)行了分析，設(shè)計(jì)了兩輪自平衡車(chē)的 PID 控制器。可靠性是系統(tǒng)設(shè)計(jì)的第一要求，因此對(duì)電路設(shè)計(jì)的所有環(huán)節(jié)都進(jìn)行了電磁兼容性設(shè)計(jì)，做好各部分的接地、屏蔽、濾波等工作，將高速數(shù)字電路與模擬電路分 開(kāi)，從而大大提高本系統(tǒng)工作的可靠性。MC9S12XS128 是 HCS12 系列的增強(qiáng)型產(chǎn)品。中斷有 7 個(gè)優(yōu)先級(jí)并且內(nèi)核支持優(yōu)先級(jí)的調(diào)度，最多可有 117 個(gè)中斷源， S12X 可訪問(wèn)最多 8M 的全部存儲(chǔ)空間 (包括片內(nèi)和片外資源 )[14][15]。通常外部采用 8MHz 或 16MHz 石英晶體振蕩器，可通過(guò)內(nèi)部鎖相環(huán)使片內(nèi)總線速度提升到最高 120MHz， 尋址方式有 16 種。 （ 1） 16 為累加器 D 或 8 位累加器 A 和 B； （ 2） 16 位變址寄存器 X 和 Y，用來(lái)處理地址，可分別用于源地址和目的地址指針型常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 19 變量運(yùn)算； （ 3） 16 位堆棧指針寄存器 SP； （ 4） 16 位程序計(jì)數(shù)器 PC，運(yùn)行時(shí)指向下一條指令的地址； （ 5） 16 位條 件碼寄存器 CCR，在這一點(diǎn)上和 CPU12 不同，要特別注意。 MC9S12XS128 的 J、 H 和 P 口有位輸入信號(hào)跳變沿產(chǎn)生中斷、喚醒 CPU 功能，根據(jù)封裝，最多可有 20 個(gè)帶位中斷的引腳。由于單片機(jī)內(nèi)部集成了 PIM、 TIM、 PWM、 SPI、 SCI、 ECT、 CAN、 AD、 PIT 等模塊，因此使用方便 。本設(shè)計(jì)中采用的外部晶振為 16MHz，電容 C11 和 C12為外部時(shí)鐘的起振電容。考慮到產(chǎn)品經(jīng)濟(jì)型，本設(shè)計(jì)中采用了由電阻電容構(gòu)成的簡(jiǎn)易復(fù)位電路，如圖 35 所示。 電源管理模塊設(shè)計(jì) 可靠的電源方案是整個(gè)硬件電路穩(wěn)定可靠運(yùn)行的基礎(chǔ)。 (2) 5V電源， 為單片機(jī)及相關(guān)外設(shè)供電。 電源采用 AMS1117。通過(guò) GSEL1 和 GSEL2 腳選擇靈敏度，本系統(tǒng)設(shè)置其靈敏度為 800mv/g。系統(tǒng)采用 LM358（圖 38 U1B）設(shè)計(jì)負(fù)反饋放大電路，放大倍數(shù)為4 13RR? ，即放大 10 倍。 圖 39 姿態(tài)檢測(cè)模塊實(shí)物圖 電機(jī)驅(qū)動(dòng)電路設(shè)計(jì) 為降低機(jī)械結(jié)構(gòu)對(duì)系統(tǒng)設(shè)計(jì)帶來(lái)的難度，本系統(tǒng)采用傳統(tǒng)四驅(qū)車(chē)模加以改 裝作為本設(shè)計(jì)的硬件平臺(tái)。由單片機(jī)的 PWM 模塊產(chǎn)生驅(qū)動(dòng)波形，通過(guò)改變 PWM 占空比實(shí)現(xiàn)直流電機(jī)的調(diào)速功能。 基本特點(diǎn)如下： （ 1） 在 25C? 時(shí)導(dǎo)通電阻的典型值為 16m? ； （ 2） 低靜態(tài)電流，在 25C? 時(shí)的典型值僅為 7A? ； （ 3） 與主動(dòng)續(xù)流相結(jié)合的脈寬調(diào)制能力高達(dá) 25kHz； （ 4） 開(kāi)關(guān)電流限制降低功耗的過(guò)流保護(hù)； （ 5） 最大驅(qū)動(dòng)電流為 43A； （ 6） 具有電流檢測(cè)能力的狀態(tài)標(biāo)志診斷； （ 7） 具有鎖定行為的過(guò)熱關(guān)斷； （ 8） 過(guò)壓鎖定； （ 9） 欠壓關(guān)斷； （ 10） 帶有邏輯電平輸入的驅(qū)動(dòng)電路； （ 11） 用于優(yōu)化電磁干擾的可調(diào)節(jié)轉(zhuǎn)換率。當(dāng) PWM1 為高電平，PWM2 為低電平時(shí) ， MOTOR1 口即輸出高電壓， MOTOR2 輸出低電壓，此時(shí)電機(jī)正轉(zhuǎn)；當(dāng) PWM1 為低電平而 PWM2 為高電平時(shí) ， MOTOR1 口 即輸出低電壓， MOTOR2 輸出高電壓，此時(shí)電機(jī)反轉(zhuǎn)。本系統(tǒng)測(cè)速模塊采用 OMRON(歐姆龍 )公司 500 線增量式旋轉(zhuǎn)編碼器如圖 311。 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 25 INH3IN2IS6SR5GND1OUT8OUT4VS7BTS7960INH3IN2IS6SR5GND1OUT8OUT4VS7BTS7960R1R3R8R9GNDPWM1C1C3C4C5PWM2GNDMOTOR1MOTOR2GNDGNDGND 圖 310 全橋電機(jī)驅(qū)動(dòng)電路 速度檢測(cè)模塊設(shè)計(jì) 兩輪自平衡小車(chē)的原理是利用地面對(duì)車(chē)輪的摩擦力抵消車(chē)受到的重力，在本系統(tǒng)的控制環(huán)節(jié)中有兩路閉環(huán)控制，即傾角閉環(huán)控制以及速度閉環(huán)控制。 由于電機(jī)啟動(dòng)瞬間電流很大，會(huì)將整個(gè)系統(tǒng)電壓拉低，造成其他設(shè)備如單片機(jī)的工作不正常，因此要在電池電源輸入側(cè)加上較大濾波電容。由于上橋臂采用的是 P 通道開(kāi)關(guān)，對(duì)于電荷泵的需求也就不復(fù)存在，因此電磁干擾減至了最小。由于兩輪自 平衡車(chē)在平衡過(guò)程中需要不斷前后運(yùn)動(dòng)調(diào)整車(chē)身姿態(tài)，因此需要電機(jī)能夠?qū)崿F(xiàn)雙向轉(zhuǎn)動(dòng)。本系統(tǒng)中利用LM358(圖 38 U1A)構(gòu)成電壓跟隨器，輸出電壓通過(guò)電位器 調(diào)節(jié)，使零點(diǎn)偏置電壓保持在陀常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 23 螺儀工作電壓的一半（ ），這樣可以有效抑制陀螺儀的溫漂，放大電路如圖 38。 XOUT15YOUT14ZOUT13SLEEP12gs_11g_s22VCC3GND4MM