兩輪自平衡小車論文.

1、2013年全國大學(xué)生電子設(shè)計競賽兩輪自平衡小車設(shè)計作者：楊魏，黃敏杰，夏俊逸2015.7.17ii本文采用自制的兩輪簡易小車作為試驗(yàn)平臺，以 MEM傳感MPU605（為傳感器的姿態(tài) 感知系統(tǒng)，通過離散卡爾曼濾波器對兩種傳感器的數(shù)據(jù)進(jìn)行濾波融合，選用32位單片機(jī)STM32F103R為控制核心處理器，完成對數(shù)據(jù)的采集處理和車身控制，采用 PID控制算法 實(shí)現(xiàn)小車兩輪自平衡。用藍(lán)牙控制前后運(yùn)動。實(shí)驗(yàn)結(jié)果驗(yàn)證了該系統(tǒng)的性能滿足設(shè)計要求。關(guān)鍵詞：兩輪自平衡；姿態(tài)感知；STM32F103RB卡爾曼濾波；PID控制41系統(tǒng)方案11.1 姿態(tài)檢測模塊的論證與選擇 11.2 電機(jī)驅(qū)動模塊的論證與選擇 12系統(tǒng)硬

2、件設(shè)計12.1 STM32F103RB 單片機(jī)系統(tǒng)22.1.1 STM32F103RB 單片機(jī)介紹22.1.2單片機(jī)最小系統(tǒng)設(shè)計 32.1.3電源管理模塊設(shè)計 42.2姿態(tài)檢測模塊 MPU-605052.2.1 MPU-6050 簡介52.2.3數(shù)字運(yùn)動處理器（DMP 62.3速度檢測模塊設(shè)計 72.3.1編碼器介紹72.3.2 編碼器電路設(shè)計82.4 電機(jī)驅(qū)動模塊82.4.1 L298N 簡介82.4.2 L298N 特點(diǎn)93理論分析與計算93.1兩輪平衡小車數(shù)學(xué)模型 93.2 PID 控制器設(shè)計 103.2.1 PID 控制器原理103.2.2 PID 控制器設(shè)計 113.2.3 PID

3、程序123.3基于卡爾曼濾波的數(shù)據(jù)融合 134系統(tǒng)軟件設(shè)計 154.1系統(tǒng)軟件設(shè)計框架 154.2 資源模塊初始化 154.3兩輪小車姿態(tài)信息檢測 165測試方案與測試結(jié)果 165.1測試方案165.1.1硬件連接檢測 165.1.2小車功能檢測165.2 測試分析與結(jié)論 161系統(tǒng)方案本系統(tǒng)主要由姿態(tài)檢測模塊、電機(jī)驅(qū)動模塊、藍(lán)牙模塊、紅外對管模塊、電源模塊組成, 下面分別論證這幾個模塊的選擇。1.1姿態(tài)檢測模塊的論證與選擇方案一：ENC-03 種普通陀螺儀。方案二：MPU-6050 MPU-60X0是全球首例9軸運(yùn)動處理傳感器。它集成了 3軸MEM陀螺 儀，3軸MEMS卩速度計，以及一個可擴(kuò)

4、展的數(shù)字運(yùn)動處理器DMP（ Digital MotionProcessor），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴(kuò)展之后就可以通過其I2C或SPI接口輸出一個9軸的信號（SPI接口僅在MPU-600C可用）。MPU-60X0 也可以通過其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。綜合以上兩種種方案，選擇方案二。1.2電機(jī)驅(qū)動模塊的論證與選擇方案一：TB6612FNG體積小，但容易壞。方案二：L298N具有信號指示，轉(zhuǎn)數(shù)可調(diào)，抗干擾能力強(qiáng)，具有過電壓和過電流保護(hù)，可單獨(dú)控制兩臺直流電機(jī)，可單獨(dú)控制一臺步進(jìn)電機(jī)。綜合以上三種方案，選擇方案二。2系統(tǒng)硬件設(shè)計本文設(shè)計的兩輪

5、自平衡小車采用姿態(tài)傳感器（MPU605）監(jiān)測車身所處的俯仰狀態(tài)和狀態(tài)變化率，通過高速微控制器STM32F103R完成數(shù)據(jù)融合處理，得到平滑而穩(wěn)定車體姿 態(tài)信息，然后驅(qū)動電動機(jī)產(chǎn)生前進(jìn)或后退的加速度來控制車體保持平衡，紅外線檢測障礙 物，藍(lán)牙控制前進(jìn)后退。結(jié)構(gòu)框圖如圖。20主控制器STM32F103RBL29SX電機(jī)顋動藍(lán)牙紅外線圖1系統(tǒng)設(shè)計總體結(jié)構(gòu)框圖系統(tǒng)硬件主要由STM32F103R單片機(jī)系統(tǒng)、電源模塊、姿態(tài)檢測模塊、串口通訊模塊、 電機(jī)驅(qū)動模塊等組成。2.1 STM32F103RB單片機(jī)系統(tǒng)本設(shè)計中控制平臺采用 ST意法半導(dǎo)體的STM32F103R微控制器，STM32系列基于專為要 求高性

6、能、低成本、低功耗的嵌入式應(yīng)用專門設(shè)計的ARM CORTEX-M內(nèi)核，其最高主頻72MHz在滿足平衡控制的調(diào)節(jié)頻率的同時有更多的CPU資源，工作電壓為5 V。其豐富的內(nèi)置模塊資源可以為姿態(tài)檢測系統(tǒng)提供 12位精度的A/D轉(zhuǎn)換，同時使用定時器可以產(chǎn) 生PWM&出控制電機(jī)轉(zhuǎn)速和轉(zhuǎn)向。2.1.1 STM32F103RB 單片機(jī)介紹STM32F103xB增強(qiáng)型系列使用高性能的 ARM? Cortex?-M3 32位的RISC內(nèi)核，工作頻 率為72MHz內(nèi)置高速存儲器（高達(dá)128K字節(jié)的閃存和20K字節(jié)的SRAM）豐富的增強(qiáng)I/O 端口和聯(lián)接到兩條APB總線的外設(shè)。所有型號的器件都包含2個12位的ADC

7、 3個通用16 位定時器和1個PWME時器，還包含標(biāo)準(zhǔn)和先進(jìn)的通信接口 ：多達(dá) 2個I2C接口和SPI接 口、3 個 USART接 口、一個 USB接 口和一個 CANS 口。STM32F103xx中等容量增強(qiáng)型系列產(chǎn)品供電電壓為 2.0V至3.6V，包含-40 C至+85 C溫度范圍和-40 C至+105 C的擴(kuò)展溫度范圍。一系列的省電模式保證低功耗應(yīng)用的要 求oSTM32F103x中等容量增強(qiáng)型系列產(chǎn)品提供包括從 36腳至100腳的6種不同封裝形式； 根據(jù)不同的封裝形式，器件中的外設(shè)配置不盡相同。下面給出了該系列產(chǎn)品中所有外設(shè)的 基本介紹。這些豐富的外設(shè)配置，使得 STM32F103X）產(chǎn)

8、品容量增強(qiáng)型系列微控制器適合于 多種應(yīng)用場合：電機(jī)驅(qū)動和應(yīng)用控制醫(yī)療和手持設(shè)備 PC游戲外設(shè)和GPS平臺工業(yè)應(yīng)用：可編程控制器（PLC）、變頻器、打印機(jī)和掃描儀警報系統(tǒng)、視頻對講、和暖氣通風(fēng)空調(diào)系統(tǒng)等2.1.2單片機(jī)最小系統(tǒng)設(shè)計本設(shè)計采用STM32F103R單片機(jī)為控制器，最小系統(tǒng)原理圖如圖 3-3所示，主要包括 單片機(jī)供電、復(fù)位電路、時鐘電路以及 BDM接 口電路。系統(tǒng)時鐘的選擇是在啟動時進(jìn)行，復(fù)位時內(nèi)部 8MHZ的RC振蕩器被選為默認(rèn)的CPU時 鐘，隨后可以選擇外部的、具失效監(jiān)控的 416MHZ寸鐘；當(dāng)檢測到外部時鐘失效時，它將 被隔離，系統(tǒng)將自動地切換到內(nèi)部的 RC振蕩器，如果使能了中斷

9、，軟件可以接收到相應(yīng) 的中斷。同樣，在需要時可以采取對 PLL時鐘完全的中斷管理(如當(dāng)一個間接使用的外部 振蕩器失效時)。多個預(yù)分頻器用于配置 AHB的頻率、高速APB(APB2和低速APB(APB1區(qū) 域。AHB和高速APB的最高頻率是72MHz低速APB的最高頻率為36MHzSBfJJM PA；C4 匹pay1亡2丁M1PA42CarPACPAtPA2PA3PA4 PA! PASPATM PAS 41初札2 貯M PA1CI C：jT3 yiz :J3 30044 jfPCUSTM3：FISJMT6圖3-3單片機(jī)最小系統(tǒng)及時鐘電路原理圖單片機(jī)的外部復(fù)位電路可以使用按鈕和電容構(gòu)成，也可以使用

10、專門的復(fù)位芯片?？紤]到產(chǎn)品經(jīng)濟(jì)型，本設(shè)計中采用了由電阻電容構(gòu)成的簡易復(fù)位電路，如圖3-5所示。加電后，由于電容的充電時間，RESET保持低電平，單片機(jī)復(fù)位；一段時間后，電容電量充滿，RESET端輸出高電平，此時單片機(jī)運(yùn)行。手動復(fù)位時，按下手動復(fù)位按鈕，RESETS保持低電平,單片機(jī)復(fù)位；釋放手動復(fù)位按鈕后，RESETS輸出高電平，單片機(jī)工作。圖3-5復(fù)位及按鍵接口電路2.1.3電源管理模塊設(shè)計可靠的電源方案是整個硬件電路穩(wěn)定可靠運(yùn)行的基礎(chǔ)。電源模塊由若干相互獨(dú)立的穩(wěn)壓電路模塊組成。這樣做可以減少各模塊之間的相互干擾。整個系統(tǒng)需要3種電源： VDD = 2.03.6V: VDD引腳為I/O引腳和

11、內(nèi)部調(diào)壓器供電。 VSSA VDDA = 2.03.6V:為ADC復(fù)位模塊、RC振蕩器和PLL的模擬部分提供供電。 使用ADC時，VDDAF得小于2.4V。 VDDA和VSSA必須分別連接到 VDDffi VSS VBAT = 1.83.6V:當(dāng)關(guān)閉VDD寸，（通過內(nèi)部電源切換器）為RTC外部32kHz振蕩器 和后備寄存器供電。電源SL-5V1jJOCJtST SPST* Cl圖3-6電源模塊電路圖2.2姿態(tài)檢測模塊MPU-6050姿態(tài)檢測模塊能夠精確并穩(wěn)定地檢測出當(dāng)前的車身姿態(tài)，這是本設(shè)計兩輪自平衡小車實(shí)現(xiàn)有效控制的關(guān)鍵。本設(shè)計選用MEMS專感MPU6050作為姿態(tài)檢測傳感器MPU-605C

12、實(shí)物連接圖2.2.1 MPU-6050 簡介MPU-60X0是全球首例9軸運(yùn)動處理傳感器。它集成了 3軸MEM陀螺儀，3軸MEMS 加速度計，以及一個可擴(kuò)展的數(shù)字運(yùn)動處理器DMP（ Digital Motion Processor ），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴(kuò)展之后就可以通過其I2C或SPI接口輸出一個9軸的信號（SPI接口僅在 MPU-600C可用）。MPU-60XC也可以通過 其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。MPU-60X0對陀螺儀和加速度計分別用了三個 16位的ADC將其測量的模擬量 轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動，

13、傳感器的測量范圍都是用戶 可控的，陀螺儀可測范圍為土 250， 500， 1000， 2000 /秒（dps），加速度計可 測范圍為土 2， 4， 8， 16g。一個片上1024字節(jié)的FIFO,有助于降低系統(tǒng)功耗。和所有設(shè)備寄存器之間的通信 采用400kHz的I2C接口或1MHz的SPI接口（ SPI僅MPU-6000可用）。對于需要高 速傳輸?shù)膽?yīng)用，對寄存器的讀取和中斷可用 20MHz的SPI。另外，片上還內(nèi)嵌了一個溫度傳感器和在工作環(huán)境下僅有土 1%變動的振蕩器。芯片尺寸 4X 4X 0.9mm采用QFN封裝（無引線方形封裝），可承受最大 10000g的沖擊，并有可 編程的低通濾波器。關(guān)于

14、電源，MPU-60X0可支持VDD范圍2.5V 5% 3.0V 5% 或3.3V 5% 另外 MPU-6050還有一個VLOGIC引腳，用來為I2C輸出提供邏輯電平。VLOGIC電壓可取 1.8 5%或者 VDDMPU6050電路圖2.2.3數(shù)字運(yùn)動處理器（DMPDMP就是指MPU605C內(nèi)部集成的處理單元，可以直接運(yùn)算出四元數(shù)和姿態(tài)，而不再需要另 外進(jìn)行數(shù)學(xué)運(yùn)算。DM啲使用大大簡化了四軸的代碼設(shè)計。DMP是數(shù)字運(yùn)動處理器的縮寫，顧名思義mpu6050并不單單是一款傳感器，其內(nèi)部還包含了可以獨(dú)立完成姿態(tài)解算算法的 處理單元。如在設(shè)計中使用 DMPin ve nse nse官方提供的姿態(tài)解算算法

15、應(yīng)該比像樓主這樣的小白要可靠的多。其次，由DMP實(shí)現(xiàn)姿態(tài)解算算法將單片機(jī)從算法處理的壓力中解放出來，單片機(jī)所要做的是等待DMP解算完成后產(chǎn)生的外部中斷，在外部中斷里去讀取姿態(tài)解算的結(jié)果。這樣單片機(jī)有大量的時間來處理諸 如電機(jī)調(diào)速等其他任務(wù)，提高了系統(tǒng)的實(shí)時性。DMP從陀螺儀、加速度計以及外接的傳感器接收并處理數(shù)據(jù)，處理結(jié)果可以從DMP寄存器讀出，或通過FIFO緩沖。DMPt權(quán)使用MPU勺一個外部引腳產(chǎn)生中斷。2.3速度檢測模塊設(shè)計兩輪自平衡小車的原理是利用地面對車輪的摩擦力抵消車受到的重力，在本系統(tǒng)的控制環(huán)節(jié)中有兩路閉環(huán)控制，即傾角閉環(huán)控制以及速度閉環(huán)控制。為實(shí)現(xiàn)速度的閉環(huán)控制，必須加入速度檢

16、測裝置實(shí)現(xiàn)速度閉環(huán)控制中的反饋環(huán)節(jié)。本系統(tǒng)測速模塊采用OMRO歐姆龍）公司500線增量式旋轉(zhuǎn)編碼器如圖3-112.3.1編碼器介紹編碼器內(nèi)部為一個中心有軸的光電碼盤，其上有環(huán)形通、暗的刻線，有光電發(fā)射和接 收器件讀取,獲得四組正弦波信號組合成 A B C D,每個正弦波相差90度相位差（相對 于一個周波為360度），將C、D信號反向，疊加在A、B兩相上，可增強(qiáng)穩(wěn)定信號；另每 轉(zhuǎn)輸出一個Z相脈沖以代表零位參考位。由于A B兩相相差90度，可通過比較A相在前還是B相在前，以判別編碼器的正轉(zhuǎn) 與反轉(zhuǎn)，通過零位脈沖，可獲得編碼器的零位參考位如圖3-12。旋轉(zhuǎn)方向：CCW （從軸側(cè)看：左旋轉(zhuǎn)）A柑日相O

17、N： H；*OFFfLJ ONlHJOFFCL)rrLjr-L一H-1 ,;4T1011945圖3-12編碼器A、B相輸出波形本系統(tǒng)用OMRC旋轉(zhuǎn)編碼器相關(guān)參數(shù)如表3-1所示：表3-1編碼器參數(shù)表項(xiàng)目參數(shù)電源電壓DC 5V12V紋波的峰峰值小于5%消耗電流30mA以 下分辨率500脈沖/轉(zhuǎn)輸出相A B、Z三相輸出狀態(tài)集電極開路輸出最高輸出響應(yīng)42kHz輸出上升或下降時間1us以下起動轉(zhuǎn)矩1mN- ml以下軸允許力徑向10N、軸向5N允許最咼轉(zhuǎn)速5000r/mi n質(zhì)量35g2.3.2編碼器電路設(shè)計由于編碼器采用集電極開路輸出，輸出波形為矩形波，因此編碼器外圍電路較為簡單。 需要在信號輸出端接入

18、一個上拉電阻，即可將信號提供給單片機(jī)采集數(shù)據(jù)。如圖3-13所示，PULSE引腳為編碼器A相，接單片機(jī)的脈沖計數(shù)口，通過單片機(jī)的PACN模塊對輸入脈沖進(jìn)行計數(shù)從而獲取電機(jī)轉(zhuǎn)速。DIR為編碼器B相輸出，接單片機(jī)I/O口，通過A、B相位差進(jìn)行軟件鑒相，從而判斷電機(jī)轉(zhuǎn)動方向。圖3-13編碼器接口電路2.4電機(jī)驅(qū)動模塊電機(jī)的驅(qū)動芯片選用L298N作為驅(qū)動芯片。2.4.1 L298N 簡介L298是SGS公司的產(chǎn)品，內(nèi)部包含4通道邏輯驅(qū)動電路，是一種兩相和四相電機(jī)的專用驅(qū)動器，即內(nèi)含兩個H橋的高電壓大電流雙全橋式驅(qū)動器，接收標(biāo)準(zhǔn)的TTL邏輯電平信號，可驅(qū)動46V、2A以下的電機(jī)。本設(shè)計中直流電機(jī)的驅(qū)動信號

19、由STM32F103R提供，信圖6理論分析與計算2.4.2 L298N 特點(diǎn)基本特點(diǎn)如下：（1）具有信號指示。（2）轉(zhuǎn)數(shù)可調(diào)。（3）抗干擾能力強(qiáng)。（4）具有過電壓和過電流保護(hù)（5）可單獨(dú)控制兩臺直流電機(jī)（6）可單獨(dú)控制一臺步進(jìn)電機(jī)（7）PWM脈寬平滑調(diào)速。（8）可實(shí)現(xiàn)正反轉(zhuǎn)。（9）米用光電隔離。3理論分析與計算3.1兩輪平衡小車數(shù)學(xué)模型已知自平衡車高度為I，質(zhì)量為m,將其抽象為一級倒立擺，并將倒立擺至于可水平移動的小車上。假設(shè)其受外力干擾引起的車體角加速度為x（t），沿垂直于車體方向進(jìn)行受力分析如圖2-7，可以得到自平衡車傾角與車輪移動加速度為a（t）以及外力干擾帶來的加速度x(t)之間的運(yùn)動

20、方程圖2-7外力干擾條件下的小車受力分析小車運(yùn)動微分方程表達(dá)式如式2-8 :tdt2= gsinfita t cosFtlx t（式 2-8）當(dāng)傾角二很小的時候，可以進(jìn)行線性化處理：cos(、1、sin(R ：宀，運(yùn)動微分方程可簡化為:d勺tdt2=g t -at lx t（式 2-9）自平衡車靜止時a t =0,其運(yùn)動微分方程為:d2, t dt2（式 2-10）3.2 PID控制器設(shè)計3.2.1 PID控制器原理當(dāng)今的自動控制技術(shù)都是基于反饋的概念。反饋理論的要素包括三個部分：測量、比 較和執(zhí)行。測量系統(tǒng)需要控制的變量，與期望值相比較，用這個誤差糾正調(diào)節(jié)控制系統(tǒng)的 響應(yīng)。在工程實(shí)際中，應(yīng)用

21、最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡稱PID控制，又稱PID調(diào)節(jié)。PID控制器問世至今已有近70年歷史，以其結(jié)構(gòu)簡單、穩(wěn)定性好、 工作可靠、調(diào)整方便而成為工業(yè)控制的主要技術(shù)之一。PID控制器由比例單元（P）、積分單元（I ）和微分單元（D）組成。其輸入e （t） 與輸出u （t ）的關(guān)系為：一1 tde（t ）1u（t ）=Kp t-iQt）dtTD（式-Ti 0dt 一2-11）其中Kp為比例系數(shù)；Ti為積分時間常數(shù)；Td為微分時間常數(shù)。PID控制器具有原理簡單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對模型依賴少等特點(diǎn)， 因此使用PID控制器實(shí)現(xiàn)兩輪自平衡車的控制是完全可行的。3.2.

22、2 PID 控制器設(shè)計由小車靜止時其運(yùn)動方程可得到系統(tǒng)輸入輸出傳遞函數(shù):H（s）=妙=匕（式 212）X（s）二 g（式2-12）s _l此時系統(tǒng)具有兩個極點(diǎn)：Sp = , g。其中一個極點(diǎn)位于s平面的右半平面。根據(jù)奈 奎斯特穩(wěn)定判據(jù)可知系統(tǒng)不穩(wěn)定，因此小車在靜止?fàn)顟B(tài)不能保持平衡0由小車受力分析可知小車平衡的條件是提供額外的回復(fù)力及阻尼，其來源為車輪與地 面的摩擦力。由式2-7可知，車輪提供的加速度的大小是根據(jù)角度 二及角速度J的反饋得 出,因此需要在控制系統(tǒng)中引入角度 二及角速度丁構(gòu)成比例（P）微分（D）反饋環(huán)節(jié)，如圖2-8 所示。圖2-8加入比例微分環(huán)節(jié)后的控制系統(tǒng)結(jié)構(gòu)圖加入比例微分反饋后

23、的系統(tǒng)傳遞函數(shù)為：。（S）1H s（式X（s）孑丄匕才匕謳（式I I2-13）此時，系統(tǒng)的兩個極點(diǎn)為s = 一k2*Jk；4|（kig）。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知，p2I系統(tǒng)穩(wěn)定需要兩個極點(diǎn)都位于 s平面的左半平面。要滿足這一點(diǎn)，需要 &g, k20。由此 可得出結(jié)論，但ki g, k20時，小車可以保持平衡，這也與上文中小車受力分析的結(jié)果相 符。在反饋環(huán)節(jié)中，與角度二成比例的控制量稱為比例控制；與角速度成比例的控制量稱 為微分控制（角速度是角度的微分）。因此上面系數(shù)ki，k2分別稱為比例和微分控制參數(shù)。 其中微分參數(shù)相當(dāng)于阻尼力，可以有效抑制自平衡車振蕩。控制系統(tǒng)的輸出量為電機(jī)控制量，因而小

24、車平衡控制的PID控制器的輸出方程可寫為：OUT_Motor=Kp*Angle+Kd*Angle_dot（式 2-14）式2-14中，OUT_Motor為PID控制輸出量，Angle為反饋傾角值，Angle_dot為反饋 角速度值，Kp和Kd分別為比例系數(shù)及微分系數(shù)。3.2.3 PID 程序#in clude #in clude math.h#i nclude vpwm.hfloat PID_realize（）;void pwm（void）;exter n float Pitch;float SetSpeed=-1.04;定義目標(biāo)值float ActualSpeed=0.0;反饋值float

25、err=0.0;偏差值float err_last=0.0;上次偏差值float Kp=135;float Ki=2.7;float Kd=135;float output=0.0;電壓值控制執(zhí)行器的變量float in tegral=0.0; 積分量float PID_realize()ActualSpeed=Pitch;err=SetSpeed-ActualSpeed;in tegral+=err;output=Kp*err+Kd*(err-err_last)+Ki* in tegral; err_last=err;return output;3.3基于卡爾曼濾波的數(shù)據(jù)融合卡爾曼濾波器是一

26、種高效率的遞歸濾波器（自回歸濾波器），能夠從一系列的不完全及包含噪聲的測量中，估計動態(tài)系統(tǒng)的狀態(tài)?？柭鼮V波器不僅能估計信號的過去和當(dāng)前狀態(tài)，甚至能估計將來的狀態(tài)?？柭鼮V波器解決離散時間控制過程的一般方法，首先定義模型線性隨機(jī)微分方程。假設(shè)卡爾曼濾波模型k時刻真實(shí)狀態(tài)是從（k-1 ）時刻推算出來，如下式人二 AXk Buk Wk（式2-15）式2-15中，xk是k時刻狀態(tài)；A是k-1時刻狀態(tài)變換模型；B是作用在控制器向量 山上 的輸入控制模型；Wk是過程噪聲，假設(shè)其均值為零，協(xié)方差矩陣 Qk符合多元正態(tài)分布： Wk N（0,Qk）（式2-16）k時刻對應(yīng)真實(shí)狀態(tài)Xk的測量Zk滿足下式：Zk

27、= HkXk Vk（式2-17）式2-17中Hk是觀測模型，將真實(shí)控制映射為觀測空間；Vk為觀測噪聲，其均值為零,式 2-18)協(xié)方差矩陣Rk符合正態(tài)分布：VkN(O,R)初始狀態(tài)以及每一時刻的噪聲都認(rèn)為是互相獨(dú)立的卡爾曼濾波器的操作主要包括兩個階段：預(yù)估與更新。在預(yù)估階段，濾波器根據(jù)上 時刻狀態(tài)，估算出當(dāng)前時刻狀態(tài)；在更新階段，濾波器利用當(dāng)前時刻觀測值優(yōu)化在預(yù)估階段獲得的測量值，以獲得一個更準(zhǔn)確的新估計值冋1213 卡爾曼濾波器迭代過程如下：1. 先驗(yàn)狀態(tài)估計：?k|kAXk_i BUk_i2. 先驗(yàn)估計誤差協(xié)方差Pk |k_1 二 AkPk_lAQ3.卡爾曼增益KPkk-iHTHPk k_iHT R2-21)4.后驗(yàn)狀態(tài)估計?k吆|k心夂沖?十)(2-22)5.后驗(yàn)誤差協(xié)方差Pk=(1 - H)Pk k-1(2-23)在上面各式中：A:作用在上的n階矩陣;B:作用在控制向量 山4上的nx 1輸入控制矩陣；H: mx n觀測模型矩陣，將真實(shí)狀態(tài)空間映射為觀測空間;Pk|k4 : nx n先驗(yàn)估計誤差協(xié)方差矩陣；R : n x n后驗(yàn)估計誤差協(xié)方差矩陣;Q: nx n過程噪聲協(xié)方差矩陣；R: mx m過程噪聲協(xié)方差矩陣；I : n階單位矩陣；Kk : nxm矩陣，稱之為卡爾曼增益4系統(tǒng)軟件設(shè)計4.1系統(tǒng)軟件設(shè)計框架系統(tǒng)軟件設(shè)