小型雙輪自衡車項目報告

1、小型雙輪自衡車項目報告2013.11綜述本報告介紹了有關(guān)該小型自衡小車的完整制作過程，包括硬件使用，簡易模型構(gòu)建及最終的實際工程算法實施。本項目旨在通過此次實踐提高參與人員的工程實踐能力，勾連各科目所學(xué)知識，為可能的后續(xù)研究提供原型參考。該車所占空間體積可近似認(rèn)為是11cm*11cm*11cm的正方體。由于此體積內(nèi)搭載器件性能的有限性，本車的控制策略為簡單的基于傾角誤差的PD控制，其理論基礎(chǔ)來源于單擺的自穩(wěn)定原理，最終可實現(xiàn)基礎(chǔ)的平衡維持。實現(xiàn)速度與轉(zhuǎn)向控制并非不可能，但是需要對底盤進(jìn)行改裝以加裝測速碼盤。本車目前實現(xiàn)的功能為維持平衡直立狀態(tài)，尚無法實現(xiàn)速度控制與轉(zhuǎn)彎控制。然而平衡維持的性能同

2、樣因為尚未對速度進(jìn)行閉環(huán)控制而受到限制，本質(zhì)上也受到了微型電機轉(zhuǎn)矩和調(diào)速范圍的極大限制。在較好的情況下，本車可維持自衡數(shù)分鐘。PS：喜歡DIY的同志們歡迎參考此文檔信息，但由于個人能力有限，未盡之處請查閱其他資料。同時歡迎觀在優(yōu)酷上搜索“小型雙輪自衡車”以觀看本項目演示視頻。目錄綜述21、硬件組成介紹41.1小車框架41.2直流減速電機及車輪41.3陀螺儀和加速度計模塊61.4控制器61.5驅(qū)動板及電源72、模型介紹與基本控制原理82.1 單擺及倒立擺82.2電機的加速度控制113.小車傾角測量124.程序編輯及算法實現(xiàn)13附錄：卡爾曼濾波程序161、硬件組成介紹1.1小車框架本車的框架使用塑

3、料框架，其尺寸長寬均約為11cm，在合適的位置可打孔以通過螺絲安裝需要的器件，包括檢測器件，電機支撐架等等。1.2直流減速電機及車輪此處采用N20直流減速電機。此電機實際上是此底盤的配套電機，屬于微型電機。其大小及性能參數(shù)可參見下圖：（上圖單位為mm）在本項目中選用減速比1:150的型號，工作電壓由充電鋰電池提供約8v。該減速比的選擇的依據(jù)為：在此減速比下給電機施加從0到8V的階躍電壓可以使小車從完全傾倒的狀態(tài)直接打回到直立狀態(tài)。實際上該車裝配完畢后的重量約270g，平衡時重心離地高度約4cm，完全傾倒時的傾角約為15°。從力矩供應(yīng)來講，要滿足上述的挑選條件其減速比不應(yīng)小于100 。

4、然而，當(dāng)減速比過大時電機提供加速度的時間將會降低，故而實際操作時使用1:300的減速比也未能滿足上述條件，使用1:150卻可以。本車使用車輪也為該底盤及電機的配套車輪，其直徑為42mm。最終與電機的裝配圖可見下：1.3陀螺儀和加速度計模塊此處使用的陀螺儀及加速度計模塊為InvenSense公司生產(chǎn)的MPU6050。該芯片內(nèi)同時整合三軸加速度計和三軸陀螺儀模塊，并支持I2C通信協(xié)議的快速模式，即傳輸速率可達(dá)400Kbit/s。陀螺儀模塊使用時配置的量程為±2000/s，最小分辨大小約為0.061/s。加速劑模塊使用時配置的量程為±2g，最小分辨大小約為0.000061g。以上

5、兩個模塊的數(shù)據(jù)更新頻率均為1000Hz，陀螺儀數(shù)據(jù)輸出延遲為0.98ms，加速度計數(shù)據(jù)輸出無延遲。且輸出值均已經(jīng)過帶寬為256Hz的濾波器。1.4控制器本車所使用控制器為STM32最小系統(tǒng)板。即該系統(tǒng)板使用芯片為ST公司生產(chǎn)的STM32f103RBT6芯片。該芯片為32位單片機芯片，其CPU主頻可達(dá)到72MHz。外設(shè)豐富支持I2C通信，UART和USART通信等，程序總空間128K，RAM大小為20K。整個開發(fā)板的大小為64*52mm。1.5驅(qū)動板及電源由于本車所使用的電機功率有限，故而此處使用的驅(qū)動芯片為L293D，也無需加散熱片。該芯片共有4個通道，支持的峰值輸出電流為1.2A，每個通道額

6、定電流600mA，最大輸出電壓36V。由于單片機供電使用5V電壓，故而此驅(qū)動板上還集成一78M05的降壓芯片用以提供5V電壓的輸出。本車使用電池為兩節(jié)神火14500充電電池，單節(jié)提供電壓為3.7V，容量為800mAh，大小與普通5號電池相同。最終的硬件連接可由下圖表示：2、模型介紹與基本控制原理2.1 單擺及倒立擺由于本車尚未實現(xiàn)速度及轉(zhuǎn)向控制，故而此處只對平衡控制進(jìn)行原理分析。小車的平衡控制的原理可從最常見的單擺模型中得到啟發(fā)。對于單擺而言，即便受到外力干擾它也總能趨于豎直下垂的平衡狀態(tài)。從這個現(xiàn)象中分析即可發(fā)現(xiàn)，促使這一現(xiàn)象產(chǎn)生的機理首先需要與偏移平衡位置成正比的回復(fù)力，其次還應(yīng)有與速度成

7、正比的阻尼力。該回復(fù)力指向單擺豎直下垂的平衡位置，故而可以將球拉回平衡位置。但是僅有此力，單擺會處于無限制的震蕩狀態(tài)。實際的單擺還處在阻尼介質(zhì)中，故而會逐漸削弱系統(tǒng)能量從而令球穩(wěn)定在平衡位置。對于小車而言，可以將其看做是底部可以運動的倒立單擺。其簡單的受力分析可見圖2.2 。圖2.1車模簡化為倒立擺但是值得一提的是，運用分析直線運動的思維來進(jìn)行分析是不確切的。右圖的受力分析中明顯缺少了桿子的支撐力。此支撐力由于由剛性的支撐桿提供，故而其方向和大小都是不確定的。不像單擺中的繩子，其提供拉力的方向只能是沿繩子方向。 而且單擺中所定義的回復(fù)力是垂直圖2.2 以車輪為參考系的受力分析 指向平衡位置的力

8、?？偠灾?，該處使用的模型是不確切的。更加確切的模型需要運用有關(guān)剛體轉(zhuǎn)動的知識，涉及力矩和轉(zhuǎn)動慣量等等。此處無意將問題復(fù)雜，況且此模型分析由飛思卡爾比賽委員會（即是指全國大學(xué)生“飛思卡爾”杯智能汽車競賽委員會。該處原理分析可參見該比賽發(fā)布的電磁組直立行車參考設(shè)計方案（版本2.0）給出，后經(jīng)本項目實際驗證可行，故而暫且以這種通俗易懂的方式進(jìn)行分析。從上圖可以看出假如以車輪為參考系，那么當(dāng)車輪向前加速運動時，上半部的小球就會受到與輪子加速方向相反的慣性力，大小即由牛頓第二定律給出。將此力與重力都分解在與桿子垂直的方向上，則認(rèn)為此二分力的合力為該小球最終的回復(fù)力：F=mgsin-macos將用弧度表

9、示，則在小角度（最寬泛的觀點認(rèn)為在0-30°內(nèi)的誤差都是可以接受的）內(nèi)可以認(rèn)為sin，同時將第二項簡化則可得到下面的式子：F=mgsin-macosmg-mk1當(dāng)然有關(guān)此處第二項的化簡是無法用小角度內(nèi)以弧度制角替換余弦值來解釋的。實際上，此處只是進(jìn)行了一個線性化處理，也即該處認(rèn)為acosk1,很明顯該處的近似使得兩項的導(dǎo)數(shù)變化相差很大，前者對求導(dǎo)得到asin而后者即為k1（是個常數(shù)）。而k1實際上不管如何取值，與近似前的誤差也是相當(dāng)大的。總之，此處的近似存在問題，但是由于解釋k1的用處涉及到此后的控制算法，故而此處先暫且認(rèn)為這種近似是合理的。上式中如果k1足夠大，那么回復(fù)力將會為負(fù)（

10、有關(guān)正負(fù)方向的定義可參見圖中正負(fù)的標(biāo)識），換言之，此力的方向即指向了平衡位置，從而滿足了車子保持在平衡位置的基本條件。此外，正如在單擺中分析的那樣，為了使小球盡快的平衡下來擺脫震蕩的狀態(tài)，還需要加入阻尼力，此力的大小應(yīng)該與小球運動的速度成正比，方向與速度方向相反。因而最終的回復(fù)力應(yīng)該具有以下形式：F=mgsin-macosmg-mk1-mk2于是抵抗車子重力的慣性力應(yīng)該是：F抗=mk1+mk2最后，小車的加速度即可由上式推出：a車=k1+k2上式即為基本的控制算法，由得到的小車傾角和小車傾斜角度進(jìn)行比例放大后輸出為電機的加速度。換言之，也就是對小車傾角的PD控制。于是k1、k2也就是PD控制中

11、的比例系數(shù)和微分系數(shù)。得知k1、k2的意義之后，才回過頭來看acosk1。此式的近似雖然大有問題，但是保證了將問題化歸為最簡單的PD控制。如果近一步的，我們認(rèn)為k1可變、不為常數(shù)則可得到如下的變換：F=mgsin-macos=mgsin-mk1'tancos=mgsin-mk1'sinmg-mk1'式中k1'=a/tan加入阻尼之后即為下式：F=mgsin-macosmg-mk1'-mk2于是抵抗車子重力的慣性力應(yīng)該是：F抗=mk1'+mk2最后，小車的加速度即可由上式推出：a車=k1'+k2而k1'=a/tan。同時注意，為了保

12、證此慣性力指向平衡位置，實際最后k1'的數(shù)值應(yīng)該取絕對值，即k1'=atan 。于是最后得到的是比例系數(shù)隨測量角變化的PD控制，待確定的參數(shù)仍然只有兩個：a和k2。經(jīng)本項目驗證，使用比例系數(shù)的不變的PD控制可以達(dá)到平衡效果，使用比例系數(shù)可變的PD控制也可，效果相差不多，但是由于使用到了除法，可能會放大角度測量誤差，實際上車子的控制作用變強了，抖動頻率和幅度都有所增加。2.2電機的加速度控制前一部分提到，最終需要控制的是車子的加速度。然而一般來講，電機的控制都集中在速度控制，如何實現(xiàn)電機的加速度控制是本部分要解決的問題。此處使用的電機為直流減速電機。所謂減速，其實是通過加裝減速箱

13、以實現(xiàn)速度降低從而提高可輸出的扭矩。此處所使用的同一型號電機并不是扭矩越大越好，扭矩大意味著減速比大，即調(diào)速范圍被大大壓縮。調(diào)速范圍的壓縮有時比力矩不足更加致命，因為當(dāng)這個電機很容易就進(jìn)入滿速工作時，那么就再也無法通過提高轉(zhuǎn)速來施加加速度了，于是系統(tǒng)就進(jìn)入了失控狀態(tài)。對于直流電機，其速度變化曲線可參考下圖：圖2.3電機在不同電壓下的速度變化曲線從上圖可以看出，電機的加速度可以通過控制輸出給電機的階躍電壓來控制。而上圖中的加速度段隨電機型號不同而不同，與減速比、電機的轉(zhuǎn)動慣量等因素有關(guān)。本項目中電機的加速度段時間約為60ms，一般電機在十幾到幾百個毫秒之間。只有電機處于加速運行狀態(tài)系統(tǒng)才會受到慣

14、性力借以維持平衡，故而為了達(dá)到這個目的將控制周期壓縮到幾個毫秒的長度。因而可以認(rèn)為在控制周期內(nèi)系統(tǒng)處于加速階段，也就是可控的。當(dāng)然此時的控制器輸出即為電壓值即可，無需階躍電壓：電機得到的電壓=k3*a車其中k3為放大倍數(shù)，a車即為前部分算得的加速度。當(dāng)然，控制周期的縮短只能從一定程度上避免電機調(diào)速范圍不夠帶來的加速度失控狀態(tài)。如果小車不能及時糾正傾斜狀態(tài)，那么電機的電壓會長時間處于某一個較高的數(shù)值，也就是說電機會進(jìn)入較高轉(zhuǎn)速下的勻速狀態(tài)，那么此時就無法再行控制了，因為已經(jīng)無法通過提高較大幅度的轉(zhuǎn)速來獲得所需的加速度了。3.小車傾角測量本項目中小車的傾角測量使用加速度計和陀螺儀的卡爾曼融合濾波。

15、使用融合濾波是因為不論加速度計還是陀螺儀都有各自明顯的優(yōu)缺點：加速度計： 通過測量重力加速度在測量軸上的分量可直接反算獲得絕對傾角。 受芯片的加速運動和其他機械振動影響嚴(yán)重。陀螺儀： 直接測量芯片轉(zhuǎn)動角速度，積分可得傾角偏量。 對機械振動，及加速運動不敏感。 由于需要積分，故而零漂的影響極其嚴(yán)重。實際上，各種方法最終的思路都是以陀螺儀為核心，以加速度計為輔助清除零漂。由于卡爾曼濾波已經(jīng)有了流程化的使用方法，此處僅將所需的系統(tǒng)方程和觀測方程列出：系統(tǒng)方程：angle(k)q_bias(k)=1-dt01angle(k-1)q_bias(k-1)+dt000rate_gyro0+Q_angleQ_

16、gyro觀測方程：Zk=1 0anglekq_bias(k)+R_angle式中q_bias(k)為陀螺儀的零點誤差，angle(k)為測量角，rate_gyro為陀螺儀所測角速度，Q_angle、R_angle均為角度測量的方差，Q_gyro為陀螺儀測量的方差。此外，飛思卡爾提供的方案中給出了另一個更加簡單的角度測量方法：本項目中未使用該種方法，因為由卡爾曼濾波得到的程序在收斂速度和準(zhǔn)確度上都滿足要求?？柭鼮V波的程序由網(wǎng)上的開源代碼獲得，實際運行時做了一些修改，將會在下部分加以說明。4.程序編輯及算法實現(xiàn)控制器中的程序?qū)嶋H上十分簡單：main()系統(tǒng)初始化（）；/時鐘及外設(shè)初始化卡爾曼濾波

17、初始化（）；/先在靜止?fàn)顟B(tài)下運行500次，以進(jìn)行角度的初始收斂while(1) 卡爾曼濾波得到傾角及角速度（）； if (控制周期時間到) 對傾角和角速度進(jìn)行均值濾波（）；計算傾角與設(shè)定角的誤差（）；閾值判定（）；PD計算、輸出電機電壓（）；輸出限幅（）； 在以上的程序中，卡爾曼濾波初始化包括了對陀螺儀零點誤差的測量。雖然在前部分中此零點誤差在系統(tǒng)方程中，按理說可以進(jìn)行自動的追蹤。但是實際操作時由于此追蹤實際上是受到加速度計傾角值的直接影響，故而在程序中屏蔽了有關(guān)此值的更新以避免加速度計對其干擾。陀螺儀的零點誤差對時間并不敏感，對溫度比較敏感，故而實際運行時此零點誤差在初始化時測量一次即可。此

18、外實際操作時，加速度計由于受到小車晃動帶來的加速度過于明顯，故而利用其讀數(shù)校正陀螺儀讀數(shù)時也做了限幅處理。即當(dāng)加速度計和陀螺儀兩個讀出的傾角相差大于0.5度時，一律認(rèn)為是0.5度 。注意此數(shù)的單位不是弧度。在程序中該限幅處理語句已經(jīng)經(jīng)過了加粗處理，有關(guān)該開源程序的具體原理此處不再贅述。有關(guān)PD控制。PD控制毫無疑問是需要給出設(shè)定值的。從前面的原理分析中認(rèn)為直接對角度進(jìn)行PD控制即可，實際上即是認(rèn)為角度設(shè)定值為0.實際操作時，由于小車的對稱性等影響，將此設(shè)定值設(shè)為0并不一定是真正能使小車平衡的位置，需要根據(jù)實際的效果進(jìn)行調(diào)整。有關(guān)PD參數(shù)的整定，不論使用可變比例系數(shù)還是不變比例系數(shù)都需要兩個參數(shù)

19、進(jìn)行整定，除此之外還需要整定一個從小車加速度到最終電壓輸出的比例放大倍數(shù)k3。此k3直接與控制器控制PWM輸出的占空比有關(guān)，也就是在此處使用的調(diào)壓手段為PWM控制，輸出占空比可調(diào)的方波經(jīng)過濾波之后則可實現(xiàn)連續(xù)的變壓。具體到單片機上，PWM占空比設(shè)置是改變單片機計時器的比較值，PWM頻率的調(diào)節(jié)是改變計時器的裝載值。一般而言，當(dāng)比較值等于裝載值時則輸出高電平，即是滿電壓輸出，k3的實際上與計時器裝載值有很大關(guān)系。但是這種明確的數(shù)量關(guān)系無需過分分析。有關(guān)上述三個參數(shù)的整定，本項目使用以下的順序：首先將小車完全傾倒，調(diào)節(jié)比例系數(shù)和k3,保證小車上電后可以從此傾倒?fàn)顟B(tài)恢復(fù)直立。其次，再加入微分系數(shù)調(diào)節(jié)系

20、統(tǒng)的震蕩。最后，對比例和k3進(jìn)行微調(diào)以達(dá)到滿意的效果。最終算法輸出的限幅不是必須的，因為對某些單片機而言比較值高于裝載值不會報錯，當(dāng)做等于處理。附錄：卡爾曼濾波程序/*/* kalman.c */* 1-D Kalman filter Algorithm, using an inclinometer and gyro */* Author: Rich Chi Ooi */* Version: 1.0 */* Date */ /* Adapted from Trammel Hudson(hudson) */ /* - */* Compilation procedure: */* Linux */

21、* gcc68 -c XXXXXX.c (to create object file) */* gcc68 -o XXXXXX.hex XXXXXX.o ppwa.o */*Upload data : */* ul filename.txt */*/* In this version: */*/* This is a free software; you can redistribute it and/or modify */* it under the terms of the GNU General Public License as published */* by the Free S

22、oftware Foundation; either version 2 of the License, */ /* or (at your option) any later version. */* */* this code is distributed in the hope that it will be useful, */* but WITHOUT ANY WARRANTY; without even the implied warranty of */* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *

23、/* GNU General Public License for more details. */* */ /* You should have received a copy of the GNU General Public License */* along with Autopilot; if not, write to the Free Software */* Foundation, Inc., 59 Temple Place, Suite 330, Boston, */* MA 02111-1307 USA */*/#include <math.h>#include

24、 "eyebot.h"/* * The state is updated with gyro rate measurement every 20ms * change this value if you update at a different rate. */static const float dt = 0.02;/* * The covariance matrix.This is updated at every time step to * determine how well the sensors are tracking the actual state.

25、*/static float P22 = 1, 0 , 0, 1 ;/* * Our two states, the angle and the gyro bias.As a byproduct of computing * the angle, we also have an unbiased angular rate available.These are * read-only to the user of the module. */float angle;float q_bias;float rate;/* * The R represents the measurement cov

26、ariance noise.R=EvvT * In this case,it is a 1x1 matrix that says that we expect * 0.1 rad jitter from the inclinometer. * for a 1x1 matrix in this case v = 0.1 */static const float R_angle = 0.001 ;/* * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how

27、much we trust the inclinometer * relative to the gyros. */static const float Q_angle = 0.001;static const float Q_gyro = 0.0015;/* * state_update is called every dt with a biased gyro measurement * by the user of the module. It updates the current angle and * rate estimate. * * The pitch gyro measur

28、ement should be scaled into real units, but * does not need any bias removal. The filter will track the bias. * * A = 0 -1 * 0 0 */void stateUpdate(const float q_m) float q; float Pdot4; /* Unbias our gyro */ q = q_m - q_bias;/當(dāng)前角速度：測量值-估計值 /* * Compute the derivative of the covariance matrix * (equ

29、ation 22-1) * Pdot = A*P + P*A' + Q * */ Pdot0 = Q_angle - P01 - P10; /* 0,0 */ Pdot1 = - P11; /* 0,1 */ Pdot2 = - P11; /* 1,0 */ Pdot3 = Q_gyro; /* 1,1 */ /* Store our unbias gyro estimate */ rate = q; /* * Update our angle estimate * angle += angle_dot * dt * += (gyro - gyro_bias) * dt * += q

30、* dt */ angle += q * dt;/角速度積分累加到 估計角度 /* Update the covariance matrix */ P00 += Pdot0 * dt; P01 += Pdot1 * dt; P10 += Pdot2 * dt; P11 += Pdot3 * dt;/* * kalman_update is called by a user of the module when a new * inclinoometer measurement is available. * * This does not need to be called every tim

31、e step, but can be if * the accelerometer data are available at the same rate as the * rate gyro measurement. * * H = 1 0 * * because the angle measurement directly corresponds to the angle * estimate and the angle measurement has no relation to the gyro * bias. */void kalmanUpdate(const float incAn

32、gle) /* Compute our measured angle and the error in our estimate */ float angle_m = incAngle; float angle_err = angle_m - angle;/1.12 zk-H*xk_dot if(angle_err>0.5) angle_err=0.5; else if(angle_err<-0.5) angle_err=-0.5; /* * h_0 shows how the state measurement directly relates to * the state estimate. * * H = h_0 h_1 * * The h_1 shows that the state measurement does not relate * to the gyro bias estimate.