【正文】 波形（ 1 為陀螺儀輸出， 2 為加速度計輸出） 常熟理工學(xué)院畢業(yè)設(shè)計（論文） 14 基于 卡爾曼 濾波的數(shù)據(jù)融合 雖然單一慣性傳感器就可以單獨進(jìn)行姿態(tài)角度檢測，但是其準(zhǔn)確性主要取決于慣性 器件的精度，單從改善硬件結(jié)構(gòu)和生產(chǎn)工 藝方面難以有很大幅度的提高， 并且系統(tǒng)誤差會隨時間累積，不適用于長時間姿態(tài)檢測。因此不能直接使用。 MMA7260 是一種低 g 值 、小量程線性加速度傳感器， 在不運動或不受重力作用（ 0g）條件下輸出為 ，最大測量范圍 06g，靈敏度最高可到 800mV/g，MMA7260 實物如圖 210 所示。 加速度計 加速度計是一種利用檢測質(zhì)量塊的慣性力來測量載體加速度的敏感裝置， 分為線加速度計和角加速度計。由于陀螺儀及其放大電路 存在溫漂 ，且需要經(jīng)過積分運算，最終會導(dǎo)致誤差 累積，致使檢測結(jié)果出錯。 圖 29 陀螺儀 ENC03 陀螺儀直接輸出角速度，將角速度進(jìn)行積分便可以得到角度。采用模擬量輸出，檢測范圍可達(dá)177。當(dāng)旋轉(zhuǎn)器件時會改變振動頻率 ， 從而反映出物體旋轉(zhuǎn)的角速度。本系統(tǒng)使用的陀螺儀 是日本村田公司基于壓電陶瓷技術(shù)的單軸陀螺儀 ENC03，其實物如圖 29 所示。本系統(tǒng)采用 MEMS 加速度計和陀螺儀構(gòu)成自平衡車的姿態(tài)檢測系統(tǒng)。其工作時不依賴外界信息，也不向外界輻射能量，不易受到干擾，是一種自主式導(dǎo)航系統(tǒng) [7][8]。因此需要實時檢測自身傾角，再 進(jìn)行合理調(diào)整，就可以實現(xiàn)動態(tài)平衡， 因而姿態(tài)檢測成為控制小車直立平衡的關(guān)鍵。 姿態(tài)檢測系統(tǒng) 兩輪自平衡車不同于普通傳統(tǒng)結(jié)構(gòu)的小車， 是一種本質(zhì)不穩(wěn)定非線性系統(tǒng)。其中微分參數(shù)相當(dāng)于阻尼力，可以有效抑制自平衡車 振 蕩。 在反饋環(huán)節(jié)中，與角度 ? 成比例的控制量 稱為比例控制；與角速度成比例的控制量稱為微分控制（角速度是角度的微分）。要滿足這一點，需要 1k g, 2k 0。 圖 28 加入比例微分環(huán)節(jié)后的 控制 系統(tǒng) 結(jié)構(gòu)圖 加入比例微分 反饋后的系統(tǒng)傳遞函數(shù)為： ? ? ? ?? ?2 211sHsk k gXs ssll?????? (式 213) 此時，系統(tǒng)的兩個極點為 ? ?22 2 142p k k l k gs l? ? ? ??。 由小車受力分析可知小車平衡的條件是提供額外的回復(fù)力及阻尼，其來源為車輪與地面的摩擦力。其中一個極點位于 s 平面的右半平面。 PID 控制器具有原理簡單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對模型依賴 少等特點，因此 使用 PID 控制器實現(xiàn) 兩輪自平衡車 的控制 是完全可行的。 PID 控制器由比例單元（ P）、積分單元（ I）和微分單元（ D）組成。在工程實際中，應(yīng)用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡稱 PID控制，又稱 PID 調(diào)節(jié)。反饋理論的要素包括三個部分：測量、比較和執(zhí)行。假設(shè)其受外力干擾引起的車體角加速度為 ()xt ， 沿垂直于車體方向進(jìn)行受力分析如圖 27，可以得到自平衡車傾角與車輪移動加速度為 ()at 以及外力干擾帶來的加速常熟理工學(xué)院畢業(yè)設(shè)計（論文） 10 度 ()xt 之間的運動方程。 2k 為阻尼系數(shù)，合適的阻尼系數(shù)可以使倒立擺盡快回復(fù) 至 穩(wěn)定位置。增加的阻尼力與偏移角速度成正比，方向相反， 因此式（ 25）可變?yōu)椋? 12F m g m k m k? ? ? ?? ? ? （式 26） 這樣車輪需要提供的加速度即為： 12a mk mk????? （式 27） 式中 ? 為傾角， ?? 為傾角速度， 1k 、 2k 為比例系數(shù)。這樣， 倒立擺便可以回復(fù)平衡位置，但是其調(diào)整時間過長。選取地面為參考的慣性系，根據(jù)牛頓第二定律可知倒立擺受到的慣性力為： g cosF ma ?? （式 23） 這樣，倒立擺所受到 的合回復(fù)力為： si n c osF m g m a???? （式 24） 在平衡控制系統(tǒng)中，可控偏移角θ較小，對其進(jìn)行線性化。 常熟理工學(xué)院畢業(yè)設(shè)計（論文） 9 lm?mg ?sinmg ?cosma? 圖 26 非慣性系中的倒立擺受力分析 由于車輪做加速 運動，倒立擺會受額外的慣性力作用。 通過控制 倒立擺底部車輪 ，使其做加速運動。牛頓運動定律只在慣性參考系中成立。顯然，只能通過第二種方法實現(xiàn)倒立擺的平衡，即在系統(tǒng)中額外增加一種力使合回復(fù)力與偏移方向相反。 經(jīng)分析可知，要控制倒立擺使其能夠與單擺一樣能夠回復(fù)至平衡位置并保持穩(wěn)定有兩種方案。 對靜止的一級倒立擺模型進(jìn)行受力分析（不考慮車輪與地面 的滾動摩擦力），如圖 25所示。阻尼力過大（過阻尼）則導(dǎo)致單擺恢復(fù)平衡時間加長。 如果沒有阻尼力，單擺會在平衡位置左右晃動而無法停止。阻尼力越大，單擺會越快停止在平衡位置。由于空氣阻力的存在，單擺最終會停止在平衡位置。這個力稱 為回復(fù)力，其大小為： s inF mg ?? （式 21） 在偏移角很小情況下，回復(fù)力與偏移角之間的大小成正比，方向相反。 常熟理工學(xué)院畢業(yè)設(shè)計（論文） 7 θlmmglm單 擺 模 型一 級 倒 立 擺 模 型 圖 23 小車抽象為一級倒立擺模型 對普通單擺進(jìn)行受力分析如圖 24 所示。 為了更加直觀的分析系統(tǒng)受力情況，下面將直立小車與單擺模型進(jìn)行對比說明小車的受力情況。而小車上裝載的姿態(tài)檢測系統(tǒng)能夠?qū)π≤嚨膬A斜狀況進(jìn)行實時檢測，通過控 制器控制車輪轉(zhuǎn)動，抵消在這個維度上的傾斜力矩便可以 保持小車平衡， 如圖 22 所示 。 常熟理工學(xué)院畢業(yè)設(shè)計（論文） 6 圖 21 保持木棍直立的反饋控制系統(tǒng) 自平衡車的控制也是通過負(fù)反饋來實現(xiàn)的，與在指尖保持木棍直立比較則相對簡單。這需要兩個條件：一個是托著木棍的手指可以自由移動；另一個是人的眼睛可以觀察木棍的傾斜角度與傾斜趨勢 (角速度 )。如人類身體擁有豐富的感知器官，通過大腦調(diào)節(jié)便可以控制腰部及腿部肌肉保持人體的直立。其中平衡控制是系統(tǒng)的最基本要求 ， 也是整個控制系統(tǒng)的難點。以速度調(diào)節(jié)為例，需要通過改變車模平衡控制中小車傾角設(shè)定值，從而改變車模實際傾斜角度，達(dá)到速度控制的要求。由于小車同時受到 三種控制的影響，從小車平衡控 制的角度來看，其它兩個控制就成為干擾。比如在速度控制時，需要小車已經(jīng)能夠保持直立控制；在方向控制 時，需要小車能夠保持平衡和速度恒定；同樣，在小車平衡控制時，也需要速度和方向控制已經(jīng)達(dá)到平穩(wěn)。由于最終都是對同一個控制對象（小車的電機(jī)）進(jìn)行控制，所以各個子系統(tǒng) 之間存在著耦合。小車不同的傾角會引起車模的加減速，從而達(dá)到對小車速度的控制。 (3)小車方向控制：通過控制兩 個電機(jī)間的轉(zhuǎn)速不同 實現(xiàn)轉(zhuǎn)向。整個控制系統(tǒng)可以分為三個子系統(tǒng)： (1)小車 平衡控制：以小車傾角為輸入量，通過控制兩個電機(jī)的正反轉(zhuǎn)保持小車 衡 。分析系統(tǒng)要 求 可知，保持小車直立和運動的動力都來自于小車的兩只車輪，車輪 由兩只直流 電機(jī)驅(qū)動。 第六章：總結(jié)與展望， 總結(jié)本設(shè)計各模塊，并對兩輪自平衡小車的優(yōu)化方向進(jìn)行了簡要闡述。 第四章：系統(tǒng)軟件設(shè)計，介紹單片機(jī)初始化，濾波算法及控制算法，闡述各 模塊 軟件常熟理工學(xué)院畢業(yè)設(shè)計（論文） 4 設(shè)計 方法。 第二章：系統(tǒng) 原理闡述與分析，對小車的運動進(jìn)行建模， 分析陀螺儀與加速度計的特性并對濾波算法 進(jìn)行 闡述 ， 介紹 PID 控制器的設(shè)計。小車的傾角閉環(huán)控制以及直流電機(jī)的閉環(huán)速度控制。 (3) 基于卡爾曼 濾波 的數(shù)據(jù)融合 ：由于陀螺儀測量的角速度 只在短時間 內(nèi)穩(wěn)定而加速度傳感器的自身白噪聲很嚴(yán)重，因此根據(jù)其互補(bǔ)特性設(shè)計 卡爾曼 濾波器以得到準(zhǔn)確穩(wěn)定的角度和角速度。具體包括： (1) 機(jī)器人本體設(shè)計：包括機(jī)械，重 心調(diào)整，電氣系統(tǒng)設(shè)計等，為進(jìn)一步研究提供良好的平臺； (2) 信號調(diào)理及控制部分電路設(shè)計：陀螺儀輸出信號需要經(jīng)過進(jìn)一步濾波放大 ，因此需要設(shè)計信號調(diào)理電路，同時控制核心需要構(gòu)建相關(guān)輸入輸出模塊及人際交互設(shè)備，因此需要對主控單元電路進(jìn)行設(shè)計。 本文主要研究目標(biāo)與內(nèi)容 本課題設(shè)計了一款兩輪自平衡小車，研究了車身姿態(tài)檢測中陀螺儀與加速 度傳感器的互補(bǔ)特性，并根據(jù)其特性比較并設(shè)計濾波算法，包括 卡爾曼 濾波，互補(bǔ)濾波等常用濾波算法。傳統(tǒng)的 PID 控制在各類工業(yè)場合有著廣泛的應(yīng)用，完全可以滿足兩輪自平衡車的控制系統(tǒng)要求。 控制算法 兩輪自平衡車屬于本質(zhì)不穩(wěn)定系統(tǒng)，因此其實現(xiàn)的平衡是一種動態(tài)平衡。采用 MEMS(MicroElectroMechanical System， 微機(jī)電 系統(tǒng) )陀螺儀和加速度計等慣性傳感器構(gòu) 成的姿態(tài)檢測系統(tǒng)可以實時、 準(zhǔn)確的檢測兩輪自平衡車的傾角。因此，對于兩輪自 平衡車來說，能夠精確并穩(wěn)定的檢測當(dāng)前車身傾角，是實現(xiàn)有效控制的關(guān)鍵所在。并且兩輪子平衡車是本質(zhì)不穩(wěn)定的非線性系統(tǒng)，因此建模必須考慮線性化問題。系統(tǒng)數(shù)學(xué)模型建立的重點在于動力學(xué)方面，主要采用拉格朗日動力學(xué)方程以及牛頓力學(xué)定律的方法。在機(jī)械 結(jié)構(gòu)上保持小車重心的穩(wěn)定性，才能減少控制系統(tǒng)由于車身機(jī)械結(jié)構(gòu)的不合理性而造成的控制復(fù)雜化；硬件系統(tǒng)必須包含自平衡車所需的所有電子系統(tǒng)與電氣設(shè)備；軟件系統(tǒng)則具體負(fù)責(zé)車身平衡控制。因此常熟理工學(xué)院畢業(yè)設(shè)計（論文） 2 卡門嘗試使用精密的陀螺儀來代替人類的前庭與耳蝸等平衡器官，以電動馬達(dá)與車輪代替人類的雙腳，發(fā)展出所謂的 “ 動態(tài)穩(wěn)定 概念 [3]。卡門觀察人類走路的姿勢特性，領(lǐng)悟到其實人類之所以可以平穩(wěn)地直立行走，是因為體內(nèi)靈敏的平衡器官可以精確地判斷出身體重心的改變量，透過小腦的即時反應(yīng) ，然后利用腿部的肌肉即時出力來平衡傾倒的態(tài)勢。由于兩輪自平衡車有著活動靈便，環(huán)境無害等優(yōu)點，其被廣泛應(yīng)用于各類高規(guī)格社會活動，目前該車已用于奧運會以及世博會等大型場合。 本世紀(jì)初瑞士聯(lián)邦工業(yè)大學(xué)的 Joe、美國的 SegwayN 等兩輪自平衡機(jī)器人相繼問世，世界各國越來 越多的機(jī)器人愛好者和研究者開始關(guān)注兩輪自平衡機(jī)器人。 早在 1987 年，日本電信大學(xué)教授山藤一雄就提出了兩輪自平衡機(jī)器人的概念。由于特殊的結(jié)構(gòu)，其適應(yīng)地形變化能力強(qiáng)，運動靈活，可以勝任一些 復(fù)雜環(huán)境里的工作。如何解決機(jī)器人在這些環(huán)境中運行的問題，逐漸成為研究者關(guān)心的 問題。 52 常熟理工學(xué)院畢業(yè)設(shè)計（論文） 1 研究背景與意義 近年來，隨著電子技術(shù)的發(fā)展與進(jìn)步，移動機(jī)器人的研究不斷深入，成為目前科學(xué)研究最活躍的領(lǐng)域之一，移動機(jī)器人的應(yīng)