【正文】 立小車與單擺模型進(jìn)行對(duì)比說(shuō)明小車的受力情況。重力場(chǎng)中使用細(xì)線懸掛的重物經(jīng)抽象化便形成理想化的單擺模型，兩輪自平衡車可以看作一級(jí)倒立擺模型進(jìn)行分析，如圖23所示。圖23 小車抽象為一級(jí)倒立擺模型對(duì)普通單擺進(jìn)行受力分析如圖24所示。圖24 單擺受力分析當(dāng)物體離開平衡位置后便會(huì)受到重力與線的合作用力，驅(qū)使物體回復(fù)至平衡位置。這個(gè)力稱為回復(fù)力，其大小為： （式21）在偏移角很小情況下，回復(fù)力與偏移角之間的大小成正比，方向相反。在此回復(fù)力的作用下，單擺進(jìn)行周期運(yùn)動(dòng)。由于空氣阻力的存在，單擺最終會(huì)停止在平衡位置?？諝庾枘崃εc單擺的速度成正比，方向相反。阻尼力越大，單擺會(huì)越快停止在平衡位置?？傻贸?，單擺保持平衡的條件有兩點(diǎn)：(1) 受到與偏移相反的回復(fù)力作用；(2) 受到與運(yùn)動(dòng)速度相反的阻尼力作用。如果沒有阻尼力，單擺會(huì)在平衡位置左右晃動(dòng)而無(wú)法停止。如果阻尼力過(guò)?。ㄇ纷枘幔瑔螖[會(huì)在平衡位置震蕩。阻尼力過(guò)大（過(guò)阻尼）則導(dǎo)致單擺恢復(fù)平衡時(shí)間加長(zhǎng)。因而存在一個(gè)臨界阻尼系數(shù)，使單擺穩(wěn)定在平衡位置所需時(shí)間最短。對(duì)靜止的一級(jí)倒立擺模型進(jìn)行受力分析（不考慮車輪與地面的滾動(dòng)摩擦力），如圖25所示。圖25一級(jí)倒立擺模型受力分析圖由一級(jí)倒立擺模型靜止時(shí)的受力分析可知，其回復(fù)力大小為： （式22）靜止的倒立擺之所以不能像單擺一樣可以穩(wěn)定在平衡位置，是因?yàn)樵谄x平衡位置時(shí)所受回復(fù)力與其偏移方向相同，而不是相反，因此不能回復(fù)至平衡位置，而是加速偏離垂直位置直至傾倒。經(jīng)分析可知，要控制倒立擺使其能夠與單擺一樣能夠回復(fù)至平衡位置并保持穩(wěn)定有兩種方案。一種是改變重力方向；另一種是在系統(tǒng)中增加另外一種力使合回復(fù)力與偏移方向相反。顯然，只能通過(guò)第二種方法實(shí)現(xiàn)倒立擺的平衡，即在系統(tǒng)中額外增加一種力使合回復(fù)力與偏移方向相反。根據(jù)牛頓第一運(yùn)動(dòng)定律（即慣性定律），任何一個(gè)物體在不受外力或受平衡力的作用時(shí)，總是保持靜止?fàn)顟B(tài)或勻速直線運(yùn)動(dòng)狀態(tài)，直到有作用在其上面的外力迫使其改變這種狀態(tài)為止。牛頓運(yùn)動(dòng)定律只在慣性參考系中成立。在非慣性參考系中，由于慣性的存在，物體會(huì)受慣性力。通過(guò)控制倒立擺底部車輪，使其做加速運(yùn)動(dòng)。在此條件下再次分析倒立擺受力情況，如圖26所示。圖26 非慣性系中的倒立擺受力分析由于車輪做加速運(yùn)動(dòng)，倒立擺會(huì)受額外的慣性力作用。假設(shè)車輪運(yùn)動(dòng)使倒立擺具有的加速度為α。選取地面為參考的慣性系，根據(jù)牛頓第二定律可知倒立擺受到的慣性力為： （式23）這樣，倒立擺所受到的合回復(fù)力為： （式24）在平衡控制系統(tǒng)中，可控偏移角θ較小，對(duì)其進(jìn)行線性化。假設(shè)控制系統(tǒng)中車輪加速度α與偏移角θ成正比關(guān)系，比例系數(shù)為，則式24可變換為： （式25）若比例系數(shù)g（重力加速度），則倒立擺所受合回復(fù)力的方向即與偏移方向相反。這樣，倒立擺便可以回復(fù)平衡位置，但是其調(diào)整時(shí)間過(guò)長(zhǎng)。為提高倒立擺調(diào)整時(shí)間，需要加入阻尼力。增加的阻尼力與偏移角速度成正比，方向相反，因此式（25）可變?yōu)椋? （式26）這樣車輪需要提供的加速度即為： （式27）式中為傾角，為傾角速度，、為比例系數(shù)。由式27可知，只有當(dāng)g時(shí)，倒立擺才能穩(wěn)定到垂直位置。為阻尼系數(shù)，合適的阻尼系數(shù)可以使倒立擺盡快回復(fù)至穩(wěn)定位置。已知自平衡車高度為，質(zhì)量為，將其抽象為一級(jí)倒立擺，并將倒立擺至于可水平移動(dòng)的小車上。假設(shè)其受外力干擾引起的車體角加速度為，沿垂直于車體方向進(jìn)行受力分析如圖27，可以得到自平衡車傾角與車輪移動(dòng)加速度為以及外力干擾帶來(lái)的加速度之間的運(yùn)動(dòng)方程。圖27 外力干擾條件下的小車受力分析小車運(yùn)動(dòng)微分方程表達(dá)式如式28： （式28）當(dāng)傾角很小的時(shí)候，可以進(jìn)行線性化處理： ，運(yùn)動(dòng)微分方程可簡(jiǎn)化為： （式29）自平衡車靜止時(shí),其運(yùn)動(dòng)微分方程為： （式210） PID控制器設(shè)計(jì) PID控制器原理當(dāng)今的自動(dòng)控制技術(shù)都是基于反饋的概念。反饋理論的要素包括三個(gè)部分：測(cè)量、比較和執(zhí)行。測(cè)量系統(tǒng)需要控制的變量，與期望值相比較，用這個(gè)誤差糾正調(diào)節(jié)控制系統(tǒng)的響應(yīng)。在工程實(shí)際中，應(yīng)用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡(jiǎn)稱PID控制，又稱PID調(diào)節(jié)。PID控制器問世至今已有近70年歷史，以其結(jié)構(gòu)簡(jiǎn)單、穩(wěn)定性好、工作可靠、調(diào)整方便而成為工業(yè)控制的主要技術(shù)之一。PID控制器由比例單元（P）、積分單元（I）和微分單元（D）組成。其輸入e (t）與輸出u (t）的關(guān)系為： (式211)其中為比例系數(shù)；為積分時(shí)間常數(shù)；為微分時(shí)間常數(shù)。PID控制器具有原理簡(jiǎn)單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對(duì)模型依賴少等特點(diǎn)，因此使用PID控制器實(shí)現(xiàn)兩輪自平衡車的控制是完全可行的。 PID控制器設(shè)計(jì) 由小車靜止時(shí)其運(yùn)動(dòng)方程可得到系統(tǒng)輸入輸出傳遞函數(shù)： (式212) 此時(shí)系統(tǒng)具有兩個(gè)極點(diǎn)：。其中一個(gè)極點(diǎn)位于s平面的右半平面。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知系統(tǒng)不穩(wěn)定，因此小車在靜止?fàn)顟B(tài)不能保持平衡[5][6]。由小車受力分析可知小車平衡的條件是提供額外的回復(fù)力及阻尼，其來(lái)源為車輪與地面的摩擦力。由式27可知，車輪提供的加速度的大小是根據(jù)角度及角速度的反饋得出，因此需要在控制系統(tǒng)中引入角度及角速度構(gòu)成比例(P)微分(D)反饋環(huán)節(jié)，如圖28所示。圖28 加入比例微分環(huán)節(jié)后的控制系統(tǒng)結(jié)構(gòu)圖加入比例微分反饋后的系統(tǒng)傳遞函數(shù)為： (式213)此時(shí)，系統(tǒng)的兩個(gè)極點(diǎn)為。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知，系統(tǒng)穩(wěn)定需要兩個(gè)極點(diǎn)都位于s平面的左半平面。要滿足這一點(diǎn)，需要g, 0。由此可得出結(jié)論，但g, 0時(shí)，小車可以保持平衡，這也與上文中小車受力分析的結(jié)果相符。在反饋環(huán)節(jié)中，與角度成比例的控制量稱為比例控制；與角速度成比例的控制量稱為微分控制（角速度是角度的微分）。因此上面系數(shù)，分別稱為比例和微分控制參數(shù)。其中微分參數(shù)相當(dāng)于阻尼力，可以有效抑制自平衡車振蕩?？刂葡到y(tǒng)的輸出量為電機(jī)控制量，因而小車平衡控制的PID控制器的輸出方程可寫為：OUT_Motor=Kp*Angle+Kd*Angle_dot (式214)式214中，OUT_Motor為PID控制輸出量，Angle為反饋傾角值，Angle_dot為反饋角速度值，Kp和Kd分別為比例系數(shù)及微分系數(shù)。兩輪自平衡車不同于普通傳統(tǒng)結(jié)構(gòu)的小車，是一種本質(zhì)不穩(wěn)定非線性系統(tǒng)。需要不斷調(diào)整自身角度，以實(shí)現(xiàn)動(dòng)態(tài)平衡。因此需要實(shí)時(shí)檢測(cè)自身傾角，再進(jìn)行合理調(diào)整，就可以實(shí)現(xiàn)動(dòng)態(tài)平衡，因而姿態(tài)檢測(cè)成為控制小車直立平衡的關(guān)鍵。慣性導(dǎo)航是依據(jù)牛頓慣性原理，利用慣性元件來(lái)測(cè)量運(yùn)載體本身的加速度，經(jīng)過(guò)積分和運(yùn)算得到速度和位置，從而達(dá)到角度、角速度、位置等姿態(tài)檢測(cè)的目的。其工作時(shí)不依賴外界信息，也不向外界輻射能量，不易受到干擾，是一種自主式導(dǎo)航系統(tǒng)[7][8]。MEMS慣性器件具有體積小，耐沖擊，壽命長(zhǎng)，可靠性高，成本低等特點(diǎn)，非常適于構(gòu)建微型捷聯(lián)慣性導(dǎo)航系統(tǒng)。本系統(tǒng)采用MEMS加速度計(jì)和陀螺儀構(gòu)成自平衡車的姿態(tài)檢測(cè)系統(tǒng)。陀螺儀是一種用高速回轉(zhuǎn)體的動(dòng)量矩敏感殼體相對(duì)慣性空間繞正交于自轉(zhuǎn)軸的一個(gè)或二個(gè)軸的角運(yùn)動(dòng)檢測(cè)裝置，可以用于檢測(cè)角速度。本系統(tǒng)使用的陀螺儀是日本村田公司基于壓電陶瓷技術(shù)的單軸陀螺儀ENC03，其實(shí)物如圖29所示。其利用了旋轉(zhuǎn)坐標(biāo)系中的物體會(huì)受到科里奧利力的原理，在器件中利用壓電陶瓷做成振動(dòng)單元。當(dāng)旋轉(zhuǎn)器件時(shí)會(huì)改變振動(dòng)頻率，從而反映出物體旋轉(zhuǎn)的角速度。ENC03體積小，響應(yīng)快，功耗低，成本低。采用模擬量輸出，檢測(cè)范圍可達(dá)177。300deg/sec(度每秒)。圖29 陀螺儀ENC03陀螺儀直接輸出角速度，將角速度進(jìn)行積分便可以得到角度。陀螺儀輸出數(shù)據(jù)噪聲較少，短時(shí)誤差較小。由于陀螺儀及其放大電路存在溫漂，且需要經(jīng)過(guò)積分運(yùn)算，最終會(huì)導(dǎo)致誤差累積，致使檢測(cè)結(jié)果出錯(cuò)。因此不能直接利用陀螺儀的積分結(jié)果作為可以直接使用的角度[7][8]。加速度計(jì)是一種利用檢測(cè)質(zhì)量塊的慣性力來(lái)測(cè)量載體加速度的敏感裝置，分為線加速度計(jì)和角加速度計(jì)。本系統(tǒng)采用飛思卡爾公司利用MEMS微電子技術(shù)開發(fā)生產(chǎn)的三軸加速度計(jì)MMA7260。MMA7260是一種低g值、小量程線性加速度傳感器，在不運(yùn)動(dòng)或不受重力作用（0g），最大測(cè)量范圍06g，靈敏度最高可到800mV/g，MMA7260實(shí)物如圖210所示。圖210 加速度計(jì)MMA7260加速度計(jì)可以直接通過(guò)反三角函數(shù)計(jì)算出小車傾斜角度，但是其對(duì)震動(dòng)非常敏感，輸出值中含有大量噪聲，而且其輸出的值是小車運(yùn)動(dòng)加速度與重力加速度的混合數(shù)據(jù)。因此不能直接使用。通過(guò)示波器連接陀螺儀與加速度計(jì)可以觀察其輸出波形，如圖211。圖211 陀螺儀與加速度計(jì)輸出波形（1為陀螺儀輸出，2為加速度計(jì)輸出）雖然單一慣性傳感器就可以單獨(dú)進(jìn)行姿態(tài)角度檢測(cè)，但是其準(zhǔn)確性主要取決于慣性器件的精度，單從改善硬件結(jié)構(gòu)和生產(chǎn)工藝方面難以有很大幅度的提高，并且系統(tǒng)誤差會(huì)隨時(shí)間累積，不適用于長(zhǎng)時(shí)間姿態(tài)檢測(cè)。由于利用單一傳感器（陀螺儀或加速度計(jì)）難以獲得相對(duì)真實(shí)的小車姿態(tài)角度，出于對(duì)系統(tǒng)測(cè)量姿態(tài)角度準(zhǔn)確性的考慮，本系統(tǒng)采用多傳感器信號(hào)進(jìn)行數(shù)據(jù)融合，以獲得最佳姿態(tài)角度[9][10]。多傳感器數(shù)據(jù)融合是一個(gè)非常重要的研究?jī)?nèi)容，只有采用最適合的融合方法才能獲得最佳的效果。常用數(shù)據(jù)融合方法有加權(quán)平均法，神經(jīng)網(wǎng)絡(luò)法等。加權(quán)平均法是一種簡(jiǎn)單的融合方法，故其運(yùn)算精度很差；神經(jīng)網(wǎng)絡(luò)法具有很好的非線性和有效的自學(xué)能力，但是其涉及的模型構(gòu)建，參數(shù)優(yōu)化非常復(fù)雜，不適用于本系統(tǒng)。國(guó)外有研究者根據(jù)加速度計(jì)與陀螺儀的互補(bǔ)特點(diǎn)研究出互補(bǔ)濾波算法，其簡(jiǎn)單明了并且具有較好的實(shí)時(shí)性與穩(wěn)定性，能夠較好的融合出姿態(tài)角度?？紤]到本系統(tǒng)使用的慣性器件特性較差，互補(bǔ)濾波在本質(zhì)原理上不能彌補(bǔ)器件特性缺陷，故本系統(tǒng)采用卡爾曼濾波算法作為數(shù)據(jù)融合方法。1960年卡爾曼發(fā)表了著名的用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文。隨著數(shù)字計(jì)算技術(shù)的進(jìn)步，卡爾曼濾波器得到了越來(lái)越廣泛的應(yīng)用和推廣，尤其是在自主或協(xié)助導(dǎo)航領(lǐng)域?？柭鼮V波器與大多數(shù)濾波器不同之處，在于其是一種純粹的時(shí)域?yàn)V波器，不需要像低通濾波器等頻域?yàn)V波器那樣，需要在頻域設(shè)計(jì)再轉(zhuǎn)換到時(shí)域?qū)崿F(xiàn)。對(duì)于解決大部分的問題，是最優(yōu)，效率最高甚至是最有用的?？柭鼮V波器的廣泛應(yīng)用已經(jīng)超過(guò)30年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。近年來(lái)更被應(yīng)用于計(jì)算機(jī)圖像處理，例如頭臉識(shí)別，圖像分割，圖像邊緣檢測(cè)等等[11][12]。 卡爾曼濾波器是一種高效率的遞歸濾波器(自回歸濾波器)，能夠從一系列的不完全及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。卡爾曼濾波器不僅能估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)?？柭鼮V波器解決離散時(shí)間控制過(guò)程的一般方法，首先定義模型線性隨機(jī)微分方程。假設(shè)卡爾曼濾波模型k時(shí)刻真實(shí)狀態(tài)是從（k1）時(shí)刻推算出來(lái)，如下式 (式215)式215中，是k時(shí)刻狀態(tài)；A是k1時(shí)刻狀態(tài)變換模型；B是作用在控制器向量上的輸入控制模型；是過(guò)程噪聲，假設(shè)其均值為零，協(xié)方差矩陣符合多元正態(tài)分布：～ (式216)k時(shí)刻對(duì)應(yīng)真實(shí)狀態(tài)的測(cè)量滿足下式： (式217)式217中是觀測(cè)模型，將真實(shí)控制映射為觀測(cè)空間；為觀測(cè)噪聲，其均值為零，協(xié)方差矩陣符合正態(tài)分布：～ (式218)初始狀態(tài)以及每一時(shí)刻的噪聲都認(rèn)為是互相獨(dú)立的。卡爾曼濾波器的操作主要包括兩個(gè)階段：預(yù)估與更新。在預(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]?？柭鼮V波器迭代過(guò)程如下：1. 先驗(yàn)狀態(tài)估計(jì)： (式219)2. 先驗(yàn)估計(jì)誤差協(xié)方差 (式220) 3. 卡爾曼增益 (式221)