兩輪機(jī)器人自平衡研究

1、兩輪機(jī)器人自平衡控制兩輪機(jī)器人自平衡控制 一一、自平衡電動(dòng)車(chē)、自平衡電動(dòng)車(chē) 二二、兩輪自平衡機(jī)器人、兩輪自平衡機(jī)器人 三三、加速度計(jì)、加速度計(jì) 四四、陀螺儀、陀螺儀 五五、兩、兩輪機(jī)器人姿態(tài)檢測(cè)輪機(jī)器人姿態(tài)檢測(cè) 六六、卡爾曼濾波、卡爾曼濾波 七、兩輪自平衡機(jī)器人發(fā)展前景七、兩輪自平衡機(jī)器人發(fā)展前景 一、自平衡車(chē)電動(dòng)車(chē)一、自平衡車(chē)電動(dòng)車(chē) 自平衡電動(dòng)車(chē)是一種電力驅(qū)動(dòng)、 具有自我平衡能力的交通工具。 在社會(huì)飛速發(fā)展的今天，交通 擁堵也成了最終現(xiàn)象，一款時(shí) 尚的電動(dòng)車(chē)，讓您享受穿梭于 鬧市的輕松與快樂(lè)。自平衡電 動(dòng)車(chē)代替自行車(chē)和電動(dòng)車(chē)作為 交通工具是時(shí)尚潮流的發(fā)展。 自平衡電動(dòng)車(chē)的興起，即將引 發(fā)一場(chǎng)

2、新的交通革命。 1、兩輪自平衡電動(dòng)車(chē)組成兩輪自平衡電動(dòng)車(chē)組成 自平衡電動(dòng)也叫電動(dòng)平衡車(chē)、體感車(chē) 等，自動(dòng)平衡運(yùn)作原理主要是建立在 一種被稱(chēng)為“動(dòng)態(tài)穩(wěn)定”（Dynamic Stabilization）的基本原理上，也就是 車(chē)輛本身的自動(dòng)平衡能力。以?xún)?nèi)置的 精密固態(tài)陀螺儀（Solid-State Gyroscopes）來(lái)判斷車(chē)身所處的姿勢(shì) 狀態(tài)，透過(guò)精密且高速的中央微處理 器計(jì)算出適當(dāng)?shù)闹噶詈?，?qū)動(dòng)馬達(dá)來(lái) 做到平衡的效果。如右圖所示： 2、兩輪自平衡車(chē)的運(yùn)動(dòng)、兩輪自平衡車(chē)的運(yùn)動(dòng) 由于兩輪自平衡電動(dòng)車(chē)的兩輪結(jié)構(gòu)，使得它的重心 在上、支點(diǎn)在下，故在非控制狀態(tài)（或靜態(tài)）下為 一不穩(wěn)定系統(tǒng)。然而，可以利用

3、倒立擺系統(tǒng)倒立擺系統(tǒng)的控制 原理，通過(guò)微處理器的控制使它能夠如倒立擺一樣 穩(wěn)定在一個(gè)平衡位置處，并能在保持平衡的狀態(tài)下 按照使用者的指令要求正常運(yùn)行。兩輪自平衡電動(dòng) 車(chē)實(shí)際上是一級(jí)直線(xiàn)式倒立擺和旋轉(zhuǎn)式倒立擺的結(jié) 合體，它的控制原理與倒立擺系統(tǒng)的基本一致。更 形象地說(shuō)，自平衡電動(dòng)車(chē)的工作原理更像人行走的 過(guò)程。 3、倒立、倒立擺擺系統(tǒng)系統(tǒng)Inverted Pendulum System 倒立擺系統(tǒng)是控制系統(tǒng)的一個(gè)重要的分支和典型的 應(yīng)用，實(shí)際上它可以理解成在計(jì)算機(jī)的控制下，通 過(guò)對(duì)系統(tǒng)各種狀態(tài)參數(shù)的實(shí)時(shí)分析，使系統(tǒng)在水平 方向或垂直方向上的 位移和角度（角速度 ）的偏移量控制在允 許的范圍以?xún)?nèi)，

4、從而 使系統(tǒng)保持平衡。右 圖為倒立擺模型： 4、自平衡電動(dòng)車(chē)工作原理、自平衡電動(dòng)車(chē)工作原理 自平衡獨(dú)輪車(chē)在平衡點(diǎn)附近的穩(wěn)定控制與其它倒立 擺系統(tǒng)的特點(diǎn)基本相似。當(dāng)在大范圍內(nèi)保持穩(wěn)定時(shí)， 其由于控制角度變大而引起的非線(xiàn)性和負(fù)載變化造 成的不確定性變得尤其突出，必須施加一定的控制 手段才能使之穩(wěn)定，因此自平衡車(chē)比其它倒立擺控 制系統(tǒng)的研究?jī)?nèi)容更為廣泛，需要采取其他的控制 方法共同完成對(duì)自平衡車(chē)的良好控制。 二、兩輪二、兩輪自平衡自平衡機(jī)器人機(jī)器人 兩輪自平衡機(jī)器人作為一種特殊的倒立擺式的移動(dòng) 機(jī)器人，具有非完整、非線(xiàn)性、欠驅(qū)動(dòng)和不穩(wěn)定等 特點(diǎn)，這使它能夠成為驗(yàn)證各種控制算法的理想平 臺(tái)。同時(shí)它具有

5、運(yùn)動(dòng)靈活、結(jié)構(gòu)簡(jiǎn)單，容易控制的 特點(diǎn)，具有廣泛的應(yīng)用前景?？捎糜诮煌?、教育、 服務(wù)機(jī)器人和玩具等領(lǐng)域。所以開(kāi)展兩輪自平衡機(jī) 器人方面的研究工作對(duì)提高我國(guó)在該領(lǐng)域的科研水 平、擴(kuò)展機(jī)器人的應(yīng)用背景等具有重要的理論與現(xiàn) 實(shí)的意義。 兩兩輪自平衡機(jī)器人控制輪自平衡機(jī)器人控制 （1）機(jī)器人速度控制： 車(chē)模運(yùn)行速度是通過(guò)控制車(chē)輪速度實(shí)現(xiàn)的 （2）機(jī)器人方向控制 道路電磁中心線(xiàn)的偏差檢測(cè) 電機(jī)差動(dòng)控制 （3）車(chē)模傾角測(cè)量 加速度傳感器 角速度傳感器-陀螺儀 三、加速度計(jì)三、加速度計(jì)（accelerometer） 測(cè)量運(yùn)載體線(xiàn)加速度的儀表。測(cè)量飛機(jī)過(guò)載的加速 度計(jì)是最早獲得應(yīng)用的飛機(jī)儀表之一。飛機(jī)上還常 用

6、加速度計(jì)來(lái)監(jiān)控發(fā)動(dòng)機(jī)故障和飛機(jī)結(jié)構(gòu)的疲勞損 傷情況。在各類(lèi)飛行器的飛行試驗(yàn)中，加速度計(jì)是 研究飛行器顫振和疲勞壽命的重要工具。在飛行控 制系統(tǒng)中，加速度計(jì)是重要的動(dòng)態(tài)特性校正元件。 在慣性導(dǎo)航系統(tǒng)中，高精度的加速度計(jì)是最基本的 敏感元件之一。 1、加速度計(jì)基本部件、加速度計(jì)基本部件 加速度計(jì)由檢測(cè)質(zhì)量（也稱(chēng)敏感質(zhì)量）、支承、電 位器、彈簧、阻尼器和殼體組成。檢測(cè)質(zhì)量受支承 的約束只能沿一條軸線(xiàn)移動(dòng)，這個(gè)軸常稱(chēng)為輸入軸 或敏感軸。如下圖所示： 2、加速度計(jì)基本原理、加速度計(jì)基本原理 當(dāng)儀表殼體隨著運(yùn)載體沿敏感軸方向作加速運(yùn)動(dòng)時(shí)， 根據(jù)牛頓定律，具有一定慣性的檢測(cè)質(zhì)量力圖保持 其原來(lái)的運(yùn)動(dòng)狀態(tài)不變

7、。它與殼體之間將產(chǎn)生相對(duì) 運(yùn)動(dòng)，使彈簧變形，于是檢測(cè)質(zhì)量在彈簧力的作用 下隨之加速運(yùn)動(dòng)。當(dāng)彈簧力與檢測(cè)質(zhì)量加速運(yùn)動(dòng)時(shí) 產(chǎn)生的慣性力相平衡時(shí)，檢測(cè)質(zhì)量與殼體之間便不 再有相對(duì)運(yùn)動(dòng)，這時(shí)彈簧的變形反映被測(cè)加速度的 大小。電位器作為位移傳感元件把加速度信號(hào)轉(zhuǎn)換 為電信號(hào)，以供輸出。加速度計(jì)本質(zhì)上是一個(gè)一自 由度的振蕩系統(tǒng)，須采用阻尼器來(lái)改善系統(tǒng)的動(dòng)態(tài) 品質(zhì)。 四、陀螺儀四、陀螺儀（gyroscope） 陀螺儀（gyroscope），是一種用來(lái)傳感與維持方向 的裝置，基于角動(dòng)量守恒的理論設(shè)計(jì)出來(lái)的。陀螺 儀主要是由一個(gè)位于軸心且可旋轉(zhuǎn)的輪子構(gòu)成。 陀 螺儀一旦開(kāi)始旋轉(zhuǎn)，由于輪子的角動(dòng)量，陀螺儀有 抗拒

8、方向改變的趨向。陀螺儀多用于導(dǎo)航、定位等 系統(tǒng)。 1、陀螺儀的原理、陀螺儀的原理 陀螺儀的原理就是，一個(gè)旋轉(zhuǎn)物體的旋轉(zhuǎn)軸所指的 方向在不受外力影響時(shí)，是不會(huì)改變的。人們根據(jù) 這個(gè)道理，用它來(lái)保持方向，制造出來(lái)的東西就叫 陀螺儀。陀螺儀在工作時(shí)要給它一個(gè)力，使它快速 旋轉(zhuǎn)起來(lái)，一般能達(dá)到每分鐘幾十萬(wàn)轉(zhuǎn)，可以工作 很長(zhǎng)時(shí)間。然后用多種方法讀取軸所指示的方向， 并自動(dòng)將數(shù)據(jù)信號(hào)傳給控制系統(tǒng)。 在現(xiàn)實(shí)生活中，陀螺儀發(fā)生的進(jìn)給運(yùn)動(dòng)是在重 力力矩的作用下發(fā)生的。 2、陀螺儀、陀螺儀的基本部件的基本部件 陀螺轉(zhuǎn)子(常采用同步電機(jī)、磁滯電機(jī)、三相交流 電機(jī)等拖動(dòng)方法來(lái)使陀螺轉(zhuǎn)子繞自轉(zhuǎn)軸高速旋轉(zhuǎn)， 并見(jiàn)其轉(zhuǎn)速近

9、似為常值) 內(nèi)、外框架(或稱(chēng)內(nèi)、外環(huán)，它是使陀螺自轉(zhuǎn)軸獲 得所需角轉(zhuǎn)動(dòng)自由度的結(jié)構(gòu)) 附件(是指力矩馬達(dá)、信號(hào)傳感器等) 3、陀螺儀的特性、陀螺儀的特性 定軸性：定軸性：當(dāng)陀螺轉(zhuǎn)子以高速旋轉(zhuǎn)時(shí)，在沒(méi)有任何 外力矩作用在陀螺儀上時(shí)，陀螺儀的自轉(zhuǎn)軸在慣 性空間中的指向保持穩(wěn)定不變，即指向一個(gè)固定 的方向；同時(shí)反抗任何改變轉(zhuǎn)子軸向的力量。 進(jìn)動(dòng)進(jìn)動(dòng)性：性：轉(zhuǎn)子高速旋轉(zhuǎn)時(shí)，若外力矩作用于外環(huán) 軸，陀螺儀將繞內(nèi)環(huán)軸轉(zhuǎn)動(dòng)；若外力矩作用于內(nèi) 環(huán)軸，陀螺儀將繞外環(huán)軸轉(zhuǎn)動(dòng)。其轉(zhuǎn)動(dòng)角速度方 向與外力矩作用方向互相垂直。這種特性，叫做 陀螺儀的進(jìn)動(dòng)性。 五、兩五、兩輪機(jī)器人姿態(tài)檢測(cè)輪機(jī)器人姿態(tài)檢測(cè) 兩輪自平衡機(jī)器人

10、所有的運(yùn)動(dòng)控制方式都以平衡控 制為前提。平衡控制是兩輪自平衡機(jī)器人運(yùn)動(dòng)的關(guān) 鍵。兩輪自平衡機(jī)器人在平衡控制的基礎(chǔ)上，又對(duì) 機(jī)器人的軌跡跟蹤控制進(jìn)行了研究。提出了預(yù)測(cè)控 制的軌跡跟蹤控制方法，對(duì)非完整輪式移動(dòng)機(jī)器人 的軌跡跟蹤問(wèn)題進(jìn)行了研究。預(yù)測(cè)控制在系統(tǒng)模型 的基礎(chǔ)上采用先預(yù)測(cè)后控制，滾動(dòng)優(yōu)化，反饋校正 的方式進(jìn)行控制，對(duì)位姿誤差與軌跡誤差進(jìn)行估計(jì)， 實(shí)現(xiàn)了對(duì)預(yù)定軌跡的準(zhǔn)確跟蹤。 六、卡爾曼濾波六、卡爾曼濾波 卡爾曼濾波是美國(guó)工程師Kalman 在線(xiàn)性最小方差估 計(jì)的基礎(chǔ)上，提出的在數(shù)學(xué)結(jié)構(gòu)上比較簡(jiǎn)單的而且 是最優(yōu)線(xiàn)性遞推濾波方法，具有計(jì)算量小、存儲(chǔ)量 低，實(shí)時(shí)性高的優(yōu)點(diǎn)。特別是對(duì)經(jīng)歷了初始濾

11、波后 的過(guò)渡狀態(tài)，濾波效果非常好。 1、背景介紹、背景介紹 魯?shù)婪蚩柭≧udolf Emil Kalman），匈牙利 裔美國(guó)數(shù)學(xué)家.卡爾曼濾波器源于 匈牙利數(shù)學(xué)家Kalman的博士論文 和1960年發(fā)表的論文 A New Approach to Linear Filtering and Prediction Problems 。 2、什么是卡爾曼濾波器、什么是卡爾曼濾波器 卡爾曼濾波是一種遞歸的估計(jì)，即只要獲知上一 時(shí)刻狀態(tài)的估計(jì)值以及當(dāng)前狀態(tài)的觀測(cè)值就可以 計(jì)算出當(dāng)前狀態(tài)的估計(jì)值，因此不需要記錄觀測(cè) 或者估計(jì)的歷史信息?？柭鼮V波器與大多數(shù)濾 波器不同之處，在于它是一種純粹的時(shí)域?yàn)V波器，

12、 它不需要像低通濾波器等頻域?yàn)V波器那樣，需要 在頻域設(shè)計(jì)再轉(zhuǎn)換到時(shí)域?qū)崿F(xiàn)。 卡爾曼濾波器的操作包括兩個(gè)階段：預(yù)測(cè)與更新。 在預(yù)測(cè)階段，濾波器使用上一狀態(tài)的估計(jì)，做出 對(duì)當(dāng)前狀態(tài)的估計(jì)。在更新階段，濾波器利用對(duì) 當(dāng)前狀態(tài)的觀測(cè)值優(yōu)化在預(yù)測(cè)階段獲得的預(yù)測(cè)值， 以獲得一個(gè)更精確的新估計(jì)值。 3、溫度測(cè)量問(wèn)題看卡爾曼濾波器、溫度測(cè)量問(wèn)題看卡爾曼濾波器 假設(shè)我們要研究的對(duì)象是一個(gè)房間的溫度。根據(jù)你 的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的，也就是下 一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們 用一分鐘來(lái)做時(shí)間單位）。假設(shè)你對(duì)你的經(jīng)驗(yàn)不是 100%的相信，可能會(huì)有上下偏差幾度。我們把這些 偏差看成是高斯白噪聲

13、（White Gaussian Noise）， 也就是這些偏差跟前后時(shí)間是沒(méi)有關(guān)系的而且符合 高斯分配（Gaussian Distribution）。另外，我們?cè)?房間里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的， 測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是 高斯白噪聲。 現(xiàn)在對(duì)于某一分鐘我們有兩個(gè)有關(guān)于該房間的溫 度值：你根據(jù)經(jīng)驗(yàn)的預(yù)測(cè)值（系統(tǒng)的預(yù)測(cè)值）和 溫度計(jì)的值（測(cè)量值）。下面我們要用這兩個(gè)值 結(jié)合他們各自的噪聲來(lái)估算出房間的實(shí)際溫度值。 假如我們要估算k時(shí)刻的是實(shí)際溫度值。首先你要 根據(jù)k-1時(shí)刻的溫度值，來(lái)預(yù)測(cè)k時(shí)刻的溫度。因 為你相信溫度是恒定的，所以你會(huì)得到k時(shí)刻的溫 度預(yù)測(cè)值是

14、跟k-1時(shí)刻一樣的，假設(shè)是23度，同時(shí) 該值的高斯噪聲的偏差是5度（5是這樣得到的： 如果k-1時(shí)刻估算出的最優(yōu)溫度值的偏差是3，你 對(duì)自己預(yù)測(cè)的不確定度是4度，他們平方相加再開(kāi) 方，就是5）。然后，你從溫度計(jì)那里得到了k時(shí) 刻的溫度值，假設(shè)是25度，同時(shí)該值的偏差是4度。 由于我們用于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值， 分別是23度和25度。究竟實(shí)際溫度是多少呢？相信 自己還是相信溫度計(jì)呢？究竟相信誰(shuí)多一點(diǎn)，我們 可以用他們的covariance來(lái)判斷。因?yàn)?Kg2=52/(52+42)，所以Kg=0.78，我們可以估算 出k時(shí)刻的實(shí)際溫度值是：23+0.78*(25-23)=24.56度。

15、 可以看出，因?yàn)闇囟扔?jì)的covariance比較?。ū容^相 信溫度計(jì)），所以估算出的最優(yōu)溫度值偏向溫度計(jì) 的值。 現(xiàn)在我們已經(jīng)得到k時(shí)刻的最優(yōu)溫度值了，下一步就 是要進(jìn)入k+1時(shí)刻，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止， 好像還沒(méi)看到什么自回歸的東西出現(xiàn)。對(duì)了，在進(jìn) 入k+1時(shí)刻之前，我們還要算出k時(shí)刻那個(gè)最優(yōu)值 （24.56度）的偏差。算法如下：(1- Kg)*52)0.5=2.35。這里的5就是上面的k時(shí)刻你預(yù)測(cè) 的那個(gè)23度溫度值的偏差，得出的2.35就是進(jìn)入k+1 時(shí)刻以后k時(shí)刻估算出的最優(yōu)溫度值的偏差（對(duì)應(yīng)于 上面的3）。 就是這樣，卡爾曼濾波器就不斷的把covariance遞歸， 從而估算

16、出最優(yōu)的溫度值。他運(yùn)行的很快，而且它 只保留了上一時(shí)刻的covariance。上面的Kg，就是卡 爾曼增益（Kalman Gain）。他可以隨不同的時(shí)刻而 改變他自己的值，是不是很神奇！ 4、卡爾曼濾波器的五大公式、卡爾曼濾波器的五大公式 X(k|k-1)=A X(k-1|k-1)+B U(k) . (1) P(k|k-1)=A P(k-1|k-1) A+Q (2) X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1) (3) Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (4) P(k|k)=（I-Kg(k) H）P(k|k-1) (

17、5) 5、卡爾曼濾波、卡爾曼濾波控制系統(tǒng)結(jié)構(gòu)圖控制系統(tǒng)結(jié)構(gòu)圖 6、卡爾曼濾波、卡爾曼濾波特點(diǎn)特點(diǎn) 卡爾曼濾波是解決狀態(tài)空間模型估計(jì)與預(yù)測(cè)的 有力工具之一，它不需存儲(chǔ)歷史數(shù)據(jù)，就能夠從一 系列的不完全以及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系 統(tǒng)的狀態(tài)?？柭鼮V波是一種遞歸的估計(jì)，即只要 獲知上一時(shí)刻狀態(tài)的估計(jì)值以及當(dāng)前狀態(tài)的觀測(cè)值 就可以計(jì)算出當(dāng)前狀態(tài)的估計(jì)值，因此不需要記錄 觀測(cè)或者估計(jì)的歷史信息。 7、卡爾曼濾波器卡爾曼濾波器的的軟件軟件實(shí)現(xiàn)實(shí)現(xiàn) 8、卡爾曼濾波器的硬件、卡爾曼濾波器的硬件實(shí)現(xiàn)實(shí)現(xiàn) 卡爾曼濾波器有良好的濾波效果，但由于其計(jì)算量 大，當(dāng)采樣率高時(shí)，一個(gè)采樣周期內(nèi)難以完成計(jì)算， 且計(jì)算機(jī)

18、的字長(zhǎng)有限，使計(jì)算中舍入誤差和截?cái)嗾` 差積累、傳遞，造成數(shù)值不穩(wěn)定，因此用MCU和DSP 難以實(shí)現(xiàn)。FPGA可以實(shí)現(xiàn)并行計(jì)算，它有多個(gè)乘法 器和累加器并行處理數(shù)據(jù)，采用FPGA實(shí)現(xiàn)的卡爾曼 濾波器，由于輸入和輸出數(shù)據(jù)計(jì)算同時(shí)進(jìn)行，因此 可以大大提高濾波速度。 在上述方程中，A、B和C均為矩陣，k是時(shí)間系數(shù)， x稱(chēng)為系統(tǒng)狀態(tài)，u是系統(tǒng)的已知輸入，y是所測(cè)量 的輸出。w和z表示噪音，其中變量w稱(chēng)為進(jìn)程噪 音，z稱(chēng)為測(cè)量噪音，它們都是向量。 則卡爾曼濾波的算法流程為： 9、兩種卡爾曼濾波、兩種卡爾曼濾波 UKF：抽樣卡爾曼濾波器抽樣卡爾曼濾波器 EKF：擴(kuò)展卡爾曼濾波器擴(kuò)展卡爾曼濾波器 擴(kuò)展卡爾曼濾波器（擴(kuò)展卡爾曼濾波器（EKF） 卡爾曼濾波器是一個(gè)“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算 法）”。對(duì)于解決很大部分的問(wèn)題，他是最優(yōu)，效 率最高甚至是最有用的。他的廣泛應(yīng)用已經(jīng)超過(guò)30 年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至 在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。近年來(lái) 更被應(yīng)用于計(jì)算機(jī)圖像處理，例如頭臉識(shí)別，圖像 分割，圖像邊緣檢測(cè)等等。 抽樣卡爾曼濾波器（抽