【正文】 2 系統(tǒng)設(shè)計(jì) 1 研究背景與意義 Kalman filter。本文提出了一種兩輪自平衡小車的設(shè)計(jì)方案， 采用陀螺儀 ENC03以及 MEMS加速度傳感器 MMA7260構(gòu)成小車姿態(tài)檢測裝置， 使用 卡爾曼 濾波完成陀螺儀數(shù)據(jù)與加速度計(jì)數(shù)據(jù)的數(shù)據(jù)融合。 關(guān)鍵詞： 兩輪自平衡 陀螺儀 姿態(tài)檢測 卡爾曼 濾波 數(shù)據(jù)融合 II Design of TwoWheel SelfBalance Vehicle Abstract In recent years, the research and application of twowheel selfbalanced vehicle have obtained rapid development. This paper presents a design scheme of twowheel selfbalanced vehicle. Gyroscope ENC03 and MEMS accelerometer MMA7260 constitute vehicle posture detection device. System adopts Kalman filter to plete the gyroscope data and accelerometer data fusion.， and adopts freescale16bit microcontrollerMC9S12XS128 as controller core. The center controller realizes the sensor signal processing the sensor signal processing, filtering algorithm and body control, humanmachine interaction and so on. Upon pletion of the entire system, each module can be normal and to coordinate work. The vehicle can keep balancing in unmanned condition. At the same time, the vehicle can be adjusted independently then quickly restore stability when there is a moderate amount of interference. In addition, the vehicle also can achieve forward, backward, left and right turn and other basic movements. Key Words: TwoWheel SelfBalance。 2 數(shù)學(xué)建模 6 兩輪自平衡小車受力 分析 37 本章小結(jié) 40 系統(tǒng)調(diào)試工具 早在 1987 年，日本電信大學(xué)教授山藤一雄就提出了兩輪自平衡機(jī)器人的概念。因此常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 2 卡門嘗試使用精密的陀螺儀來代替人類的前庭與耳蝸等平衡器官，以電動(dòng)馬達(dá)與車輪代替人類的雙腳，發(fā)展出所謂的 “ 動(dòng)態(tài)穩(wěn)定 概念 [3]。因此，對于兩輪自 平衡車來說，能夠精確并穩(wěn)定的檢測當(dāng)前車身傾角，是實(shí)現(xiàn)有效控制的關(guān)鍵所在。 本文主要研究目標(biāo)與內(nèi)容 本課題設(shè)計(jì)了一款兩輪自平衡小車，研究了車身姿態(tài)檢測中陀螺儀與加速 度傳感器的互補(bǔ)特性，并根據(jù)其特性比較并設(shè)計(jì)濾波算法，包括 卡爾曼 濾波，互補(bǔ)濾波等常用濾波算法。 第二章：系統(tǒng) 原理闡述與分析，對小車的運(yùn)動(dòng)進(jìn)行建模， 分析陀螺儀與加速度計(jì)的特性并對濾波算法 進(jìn)行 闡述 ， 介紹 PID 控制器的設(shè)計(jì)。整個(gè)控制系統(tǒng)可以分為三個(gè)子系統(tǒng)： (1)小車 平衡控制：以小車傾角為輸入量，通過控制兩個(gè)電機(jī)的正反轉(zhuǎn)保持小車 衡 。比如在速度控制時(shí)，需要小車已經(jīng)能夠保持直立控制；在方向控制 時(shí)，需要小車能夠保持平衡和速度恒定；同樣，在小車平衡控制時(shí)，也需要速度和方向控制已經(jīng)達(dá)到平穩(wěn)。如人類身體擁有豐富的感知器官，通過大腦調(diào)節(jié)便可以控制腰部及腿部肌肉保持人體的直立。 為了更加直觀的分析系統(tǒng)受力情況，下面將直立小車與單擺模型進(jìn)行對比說明小車的受力情況。阻尼力越大，單擺會(huì)越快停止在平衡位置。 經(jīng)分析可知，要控制倒立擺使其能夠與單擺一樣能夠回復(fù)至平衡位置并保持穩(wěn)定有兩種方案。 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 9 lm?mg ?sinmg ?cosma? 圖 26 非慣性系中的倒立擺受力分析 由于車輪做加速 運(yùn)動(dòng)，倒立擺會(huì)受額外的慣性力作用。 2k 為阻尼系數(shù)，合適的阻尼系數(shù)可以使倒立擺盡快回復(fù) 至 穩(wěn)定位置。 PID 控制器由比例單元（ P）、積分單元（ I）和微分單元（ D）組成。 圖 28 加入比例微分環(huán)節(jié)后的 控制 系統(tǒng) 結(jié)構(gòu)圖 加入比例微分 反饋后的系統(tǒng)傳遞函數(shù)為： ? ? ? ?? ?2 211sHsk k gXs ssll?????? (式 213) 此時(shí)，系統(tǒng)的兩個(gè)極點(diǎn)為 ? ?22 2 142p k k l k gs l? ? ? ??。 姿態(tài)檢測系統(tǒng) 兩輪自平衡車不同于普通傳統(tǒng)結(jié)構(gòu)的小車， 是一種本質(zhì)不穩(wěn)定非線性系統(tǒng)。本系統(tǒng)使用的陀螺儀 是日本村田公司基于壓電陶瓷技術(shù)的單軸陀螺儀 ENC03，其實(shí)物如圖 29 所示。由于陀螺儀及其放大電路 存在溫漂 ，且需要經(jīng)過積分運(yùn)算，最終會(huì)導(dǎo)致誤差 累積，致使檢測結(jié)果出錯(cuò)。 圖 211 陀螺儀與加速度計(jì)輸出波形（ 1 為陀螺儀輸出， 2 為加速度計(jì)輸出） 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 14 基于 卡爾曼 濾波的數(shù)據(jù)融合 雖然單一慣性傳感器就可以單獨(dú)進(jìn)行姿態(tài)角度檢測，但是其準(zhǔn)確性主要取決于慣性 器件的精度，單從改善硬件結(jié)構(gòu)和生產(chǎn)工 藝方面難以有很大幅度的提高， 并且系統(tǒng)誤差會(huì)隨時(shí)間累積，不適用于長時(shí)間姿態(tài)檢測。隨著數(shù)字計(jì)算技術(shù)的進(jìn)步，卡爾曼濾波器得到了越來越廣泛的應(yīng)用和推廣，尤其是在自主或協(xié)助導(dǎo)航領(lǐng)域。假設(shè) 卡爾曼 濾波模型 k時(shí)刻真實(shí)狀態(tài)是從（ k1）時(shí)刻推算出來，如下式 k k k kx A x B u w? ? ? (式 215) 式 215中，kx是 k時(shí)刻狀態(tài)； A是 k1時(shí)刻狀態(tài)變換模型； B是作用在控制器向量 ku 上的輸入控制模型；kw是過程 噪聲，假設(shè)其均值為零，協(xié)方差矩陣 kQ 符合多元正態(tài) 分布： kw～ (0, )kNQ (式 216) 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 15 k時(shí)刻對應(yīng)真實(shí)狀態(tài)kx的測量 kz 滿足下式： k k k kz H x v?? (式 217) 式 217中 kH 是觀測模型，將真實(shí)控制映射為觀測空間； kv 為觀測噪聲，其均值為零，協(xié)方差矩陣 kR 符合正態(tài) 分布： kv～ (0, )kNR (式 218) 初始狀 態(tài)以及每一時(shí)刻的噪聲都認(rèn)為是互相獨(dú)立的。簡述了 卡爾曼 濾波器原理及其設(shè)計(jì)流程。 S12X 系列增加了 172 條額外指令，可以執(zhí)行 32 位計(jì)算（共 280 條指令），總線頻率最高可以達(dá)到 40MHz，改進(jìn)了中斷處理能力。 CPU12X 的累加器 D 是 16 位的，但是可分別看成兩個(gè) 8 位累加器 A 和 B。 當(dāng) 外部時(shí)鐘缺失時(shí)，內(nèi)部提供自時(shí)鐘方式，直到外部時(shí)鐘恢復(fù)為止。圖 34 為單片機(jī)最小系統(tǒng)時(shí)鐘 電路原理 圖。這 樣做可以減少各模塊之間的相互干擾，另外為了進(jìn)一步減小單片機(jī)的5V電源噪聲，可以單獨(dú)使用一個(gè) 5V的穩(wěn)壓芯片，與其它接口電路分開。 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 22 IN3OUT41GND+5V100uFC9C11GND10uFC10C12C13C143V3220R5D110uHC15IN3OUT41GNDLM2940470ufC16C17C18GND+5V 圖 36 電 源 模塊 電路圖 傾角傳感器信號(hào)調(diào)理電路 加速度計(jì)電路設(shè)計(jì) 本系統(tǒng)采用的加速度計(jì)是飛思卡爾公司三軸加速度計(jì) MMA7260。本系統(tǒng)中利用LM358(圖 38 U1A)構(gòu)成電壓跟隨器，輸出電壓通過電位器 調(diào)節(jié)，使零點(diǎn)偏置電壓保持在陀常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 23 螺儀工作電壓 的一半（ ），這樣可以有效抑制陀螺儀的溫漂，放大電路如圖 38。由于上橋臂采用的是 P 通道開關(guān)，對于電荷泵的需求也就不復(fù)存在，因此電磁干擾減至了最小。 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 25 INH3IN2IS6SR5GND1OUT8OUT4VS7BTS7960INH3IN2IS6SR5GND1OUT8OUT4VS7BTS7960R1R3R8R9GNDPWM1C1C3C4C5PWM2GNDMOTOR1MOTOR2GNDGNDGND 圖 310 全橋電機(jī)驅(qū)動(dòng)電路 速度檢測模塊設(shè)計(jì) 兩輪自平衡小車的原理是利用地面對車輪的摩擦力抵消車受到的重力，在本系統(tǒng)的控制環(huán)節(jié)中有兩路閉環(huán)控制，即傾角閉環(huán)控制以及速度閉環(huán)控制。 基本特點(diǎn)如下： （ 1） 在 25C? 時(shí)導(dǎo)通電阻的典型值為 16m? ； （ 2） 低靜態(tài)電流，在 25C? 時(shí)的典型值僅為 7A? ； （ 3） 與主動(dòng)續(xù)流相結(jié)合的脈寬調(diào)制能力高達(dá) 25kHz； （ 4） 開關(guān)電流限制降低功耗的過流保護(hù)； （ 5） 最大驅(qū)動(dòng)電流為 43A； （ 6） 具有電流檢測能力的狀態(tài)標(biāo)志診斷 ； （ 7） 具有鎖定行為的過熱關(guān)斷； （ 8） 過壓鎖定； （ 9） 欠壓關(guān)斷； （ 10） 帶有邏輯電平輸入的驅(qū)動(dòng)電路； （ 11） 用于優(yōu)化電磁干擾的可調(diào)節(jié)轉(zhuǎn)換率。 圖 39 姿態(tài)檢測模塊實(shí)物圖 電機(jī)驅(qū)動(dòng)電路設(shè)計(jì) 為降低機(jī)械結(jié)構(gòu)對系統(tǒng)設(shè)計(jì)帶來的難度，本系統(tǒng)采用傳統(tǒng)四驅(qū)車模加以改 裝作為本設(shè)計(jì)的硬件平臺(tái)。通過 GSEL1 和 GSEL2 腳選擇靈敏度