【正文】 示 。由于小車(chē)只依靠?jī)蓚€(gè)車(chē)輪著地，車(chē)輪與地面會(huì)發(fā)生相對(duì)滾動(dòng)使得小車(chē)傾斜。 圖 22 通過(guò)車(chē)輪轉(zhuǎn)動(dòng)保持小車(chē)平衡 自平衡小車(chē)數(shù)學(xué)模型 兩輪自平衡小車(chē)受力分析 為了準(zhǔn)確控制車(chē)輪轉(zhuǎn)動(dòng)，保持小車(chē)始終穩(wěn)定的直立平衡，需要對(duì)自平衡車(chē)進(jìn)行運(yùn)動(dòng)學(xué)分析并建立其數(shù)學(xué)模型，從而更好的設(shè)計(jì)控制系統(tǒng)。 重力場(chǎng)中使用細(xì)線懸掛的重物經(jīng)抽象化便形成理想化的單擺模型，兩輪自平衡車(chē)可以看作一級(jí)倒立擺模型進(jìn)行分析，如圖 23 所示 。 mmgF??sinll?sinmg? 圖 24 單擺受力分析 當(dāng)物體離開(kāi)平衡位 置后便會(huì)受到重力與線的合作用力，驅(qū)使物體回復(fù)至平衡位置。在此回復(fù)力的作用下，單擺進(jìn)行 周期運(yùn)動(dòng)?？諝庾枘崃εc單擺的速度成正比，方向相反?？傻贸觯瑔螖[保持平衡的條件有兩點(diǎn)： (1) 受 到與偏移相反的回復(fù)力作用； (2) 受 到與運(yùn)動(dòng)速度相反的阻尼力作用。如果阻尼力過(guò)?。ㄇ纷枘幔?，常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 8 單擺會(huì)在平衡位置震蕩。因而存在一個(gè)臨界阻尼系數(shù)，使單擺穩(wěn)定在平衡位置 所需 時(shí)間最短。 lm?mg?sinmg 圖 25 一級(jí)倒立擺模型受力分析圖 由一級(jí)倒立擺模型靜止時(shí)的受力分析可知，其回復(fù)力大小為： sinF mg ?? （式 22） 靜止的倒立擺之所以不能像單擺一樣可以穩(wěn)定在平衡位置，是因?yàn)?在偏離平衡位置時(shí)所受回復(fù)力與其偏移方向相同，而不是相反，因此不能回復(fù)至平衡位置，而是加速偏離垂直位置直至傾倒 。一種是改變重力方向；另一種是在系統(tǒng)中增加另外一種力使合回復(fù)力與偏移方向相反。 根據(jù)牛頓第一運(yùn)動(dòng)定律（即慣性定律），任何一個(gè)物體在不受外力或受平衡力的作用時(shí)，總是保持靜止?fàn)顟B(tài)或勻速直線運(yùn)動(dòng)狀態(tài)，直到有作用在其上面的外力迫使其改變這種狀態(tài)為止。在非慣性參考系中，由于慣性的存在，物體會(huì)受慣性力。在此條件下再次分析倒立擺受力情況，如圖 26 所示 。假設(shè)車(chē)輪運(yùn)動(dòng)使倒立擺具有的加速度為 α 。假設(shè)控制系統(tǒng)中車(chē)輪加速度α與偏移角θ成正比關(guān)系，比例系數(shù)為 1k ，則式 24 可變換為： 1F mg mk???? （式 25） 若比例系數(shù) 1k g（ 重力加速度），則倒立擺所受合回復(fù)力的方向即與偏移方向相反。為提高倒立擺調(diào)整 時(shí)間，需要加入阻尼力。由式 27 可知，只有當(dāng) 1k g時(shí) ，倒立擺才能穩(wěn)定到垂直位置。 自平衡小車(chē)運(yùn)動(dòng)微分方程 已知自平衡車(chē)高度為 l ，質(zhì)量為 m ，將其抽象為一級(jí)倒立擺，并將倒立擺至于可水平移動(dòng)的小車(chē)上。 lm?mg)(tx)(ta 圖 27 外力干擾條件下的小車(chē)受力 分析 小車(chē)運(yùn)動(dòng)微分方程表達(dá)式如式 28： ? ? ? ? ? ? ? ? ? ?2 2 s in c o sdtl g t a t t lx tdt? ??? ? ?? ? ? ?? ? ? ? （式 28） 當(dāng)傾角 ? 很小的時(shí)候，可以進(jìn)行線性化處理： c os ( ) 1 si n( )? ? ???、 ，運(yùn)動(dòng)微分方程可簡(jiǎn)化為： ? ? ? ? ? ? ? ?22dtl g t a t lx tdt? ?? ? ? （式 29） 自平衡車(chē)靜止時(shí) ?? 0at? ,其運(yùn)動(dòng)微分方程為： ? ? ? ? ? ?22dtl g t lx tdt? ??? （式 210） PID 控制器設(shè)計(jì) PID 控制器原理 當(dāng)今的自動(dòng)控制技術(shù)都是基于反 饋的概念。測(cè)量系統(tǒng)需要控制 的變量，與期望值相比較，用這個(gè)誤差糾正調(diào)節(jié)控制系統(tǒng)的響應(yīng)。 PID 控制器問(wèn)世至今已有近 70 年歷史， 以其結(jié)構(gòu)簡(jiǎn)單、穩(wěn)定性好、工作可靠、 調(diào)整方便而成為工業(yè)控制的主要技術(shù)之一。其輸入 e (t）與輸出 u (t）的關(guān)系為： ? ? ? ? ? ? ? ?011 t D d e tu t K p e t e t d t TT d t??? ? ?????? (式 211) 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 11 其中 Kp 為比例系數(shù)； 1T 為積分時(shí)間常數(shù)； DT 為微分時(shí)間常數(shù)。 PID 控制器設(shè)計(jì) 由小車(chē)靜止時(shí)其運(yùn)動(dòng)方程可得到系統(tǒng)輸入輸出傳遞函數(shù)： ? ? ? ?? ?21sHsgXs sl???? (式 212) 此時(shí)系統(tǒng)具有兩個(gè)極點(diǎn)：p gs l??。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知系統(tǒng)不穩(wěn)定 ，因此小車(chē)在靜止?fàn)顟B(tài)不能保持平衡 [5][6]。由式 27 可知，車(chē)輪提供的加速度的大小是根據(jù)角度 ? 及角速度 ?? 的 反饋得出，因此需要在控制系統(tǒng)中引入角度 ? 及角速度 ?? 構(gòu)成比例 (P)微分 (D)反饋環(huán)節(jié)，如圖 28所示。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知，系統(tǒng)穩(wěn)定需要兩個(gè)極點(diǎn)都位于 s 平面的左半平面。由此可得出結(jié)論，但 1k g, 2k 0 時(shí)，小車(chē)可 以保持平衡，這也與 上文中 小車(chē)受力分析的結(jié)果相常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 12 符。因此上面系數(shù) 1k ， 2k 分別 稱(chēng)為比例和微分控制參數(shù)。 控制系統(tǒng)的輸出量為電機(jī)控制量，因而小車(chē)平衡控制的 PID控制器的輸出方程可寫(xiě)為： OUT_Motor=Kp*Angle+Kd*Angle_dot (式 214) 式 214 中， OUT_Motor 為 PID 控制輸出量， Angle 為反饋傾角值， Angle_dot 為反饋角速度值， Kp 和 Kd 分別 為 比例系數(shù)及微分系數(shù)。需要不斷調(diào)整自身角度，以實(shí)現(xiàn)動(dòng)態(tài)平衡。 慣性導(dǎo)航是依據(jù)牛頓慣性原理，利用慣性元件來(lái) 測(cè)量 運(yùn)載體本身的加速度，經(jīng)過(guò)積分和運(yùn)算得到速度和位置，從而達(dá)到角度、角速度、 位置等姿態(tài)檢測(cè)的目的。 MEMS 慣性器件具有體積小，耐沖擊，壽命長(zhǎng)，可靠性高，成本低等特點(diǎn)， 非常適于構(gòu)建微型捷聯(lián)慣性導(dǎo)航系統(tǒng)。 陀螺儀 陀螺儀是一種用高速回轉(zhuǎn)體的動(dòng)量矩敏感殼體相對(duì)慣性空間繞正交于自轉(zhuǎn)軸的一個(gè)或二個(gè)軸的角運(yùn)動(dòng)檢測(cè)裝置，可以用于檢測(cè)角速度。其 利用了旋轉(zhuǎn)坐標(biāo)系中的物體會(huì)受到科里奧利力的原理，在器件中利用壓電陶瓷做成振動(dòng)單元。 ENC03 體積小，響應(yīng)快，功耗低，成本低。 300deg/sec(度每秒 )，靈敏度為 。陀螺儀輸出數(shù)據(jù)噪聲較常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 13 少，短時(shí)誤差較小。因此不能直接利用陀螺儀的積分結(jié)果作為可以直接使用的角度 [7][8]。本系統(tǒng)采用飛思卡爾公司利用 MEMS 微電子技術(shù)開(kāi)發(fā)生產(chǎn)的三軸加速度計(jì) MMA7260。 圖 210 加速度計(jì) MMA7260 加速度計(jì)可以直接通過(guò)反三角函數(shù)計(jì)算出小車(chē)傾斜角度，但是其對(duì)震動(dòng)非常敏感，輸出值中含有大量噪聲，而且其輸出的值是小車(chē)運(yùn)動(dòng)加速度與重力加速度的混合數(shù)據(jù)。 通過(guò)示波器連接陀螺儀與加速度計(jì)可以觀察其 輸出波形，如圖 211。由于利用單一傳感器（陀螺儀或加速度計(jì)）難以獲得相對(duì)真實(shí)的小車(chē)姿態(tài)角度，出于對(duì)系統(tǒng)測(cè)量姿態(tài)角度準(zhǔn)確性的考慮，本系統(tǒng)采用多傳感器信號(hào)進(jìn)行數(shù)據(jù)融合，以獲得最佳姿態(tài)角度 [9][10]。常用 數(shù)據(jù)融合方法有加權(quán)平均法，神經(jīng)網(wǎng)絡(luò)法等。國(guó)外有研究者根據(jù)加速度計(jì)與陀螺儀的互補(bǔ)特點(diǎn)研究出互補(bǔ)濾波算法，其簡(jiǎn)單明了并且具有較好的實(shí)時(shí)性與穩(wěn)定性，能夠較好的融合出姿態(tài)角度。 1960 年 卡爾曼 發(fā)表了著名的用遞歸方法解決離散數(shù)據(jù)線性濾波問(wèn)題的論文。 卡爾曼 濾波器與大多數(shù)濾波器不同之處，在于其是一種純粹的時(shí)域?yàn)V波器， 不需要 像低通濾 波器等頻域?yàn)V波器那樣，需要在頻域設(shè)計(jì)再轉(zhuǎn)換到時(shí)域?qū)崿F(xiàn)。 卡爾曼濾波器 的廣泛應(yīng)用已經(jīng)超過(guò) 30 年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。 卡爾曼 濾波 器 是一種高效率的遞歸濾波器 (自回歸濾波器 )， 能夠從一系列的不完全及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。 卡爾 曼 濾波器解決離散時(shí)間控制過(guò)程的一般方法，首先定義模型線性隨機(jī)微分方程。 卡爾曼 濾波器的操作主要包括兩個(gè)階段：預(yù)估與更新。 卡爾曼 濾波器迭代過(guò)程如下： 1. 先驗(yàn)狀態(tài)估計(jì)： 111? ?kkkkx A x B u??? ?? (式 219) 2. 先驗(yàn)估計(jì)誤差協(xié)方差 11 TkkkkP A P A Q?? ?? (式 220) 3. 卡爾曼 增益 ? ?11TTk k k k kK P H H P H R???? (式 221) 4. 后驗(yàn)狀態(tài)估計(jì) 11? ? ?()k k kk k k kx x K z H x??? ? ? (式 222) 5. 后驗(yàn)誤差協(xié)方差 1()kk kkP I K H P ??? (式 223) 在上面各式中： A：作用在 1?kx? 上的 n階矩陣； B：作用在控制向量 1ku? 上的 n 1 輸入控制矩陣； H： m n觀測(cè)模型矩陣，將真實(shí)狀態(tài)空間映射為觀測(cè)空間； 1kkP ? ： n n先驗(yàn)估計(jì)誤差協(xié)方差矩陣； kP ： n n后驗(yàn)估計(jì)誤差協(xié)方差矩陣； 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 16 Q： n n過(guò)程噪聲協(xié)方差矩陣； R： m m過(guò)程噪聲協(xié)方差矩陣； I： n階單位矩陣； kK ： n m矩陣，稱(chēng)之為 卡爾曼 增益。對(duì)小車(chē)進(jìn)行受力分析，構(gòu)建了小車(chē)的運(yùn)動(dòng)模型并提出了小車(chē)的運(yùn)動(dòng)微分方程。介紹了本系統(tǒng)使用的姿態(tài)檢測(cè)傳感器，分析了其性能特點(diǎn)。 常熟理工學(xué)院畢業(yè)設(shè)計(jì)（論文） 17 本系統(tǒng)硬件電路的設(shè)計(jì)目標(biāo)為：可靠、高效、簡(jiǎn)潔。系統(tǒng)主要由以下幾個(gè)模塊組成： MC9S12XS128 單片機(jī)最小系統(tǒng)硬件設(shè)計(jì)、電源模塊硬件設(shè)計(jì)、傾角傳感器信號(hào)調(diào)理電路設(shè)計(jì)、電機(jī)驅(qū)動(dòng)電路設(shè)計(jì)、速度檢測(cè)電路。該單片機(jī)是飛思卡爾公司的 16 位 HCS12 系列單片機(jī)，簡(jiǎn)稱(chēng) S12 系列。基于 S12 的 CPU 內(nèi)核，可達(dá)到 25MHz 的HCS12 的 2~5 倍性能。 S12X 系列的 CPU采用復(fù)