兩輪自平衡小車論文

1、2013年全國大學(xué)生電子設(shè)計競賽 兩輪自平衡小車設(shè)計 作者：楊魏，黃敏杰，夏俊逸 2015.7.17 摘 要 本文采用自制的兩輪簡易小車作為試驗(yàn)平臺，以MEMS傳感MPU6050為傳感器的姿態(tài)感知系統(tǒng)，通過離散卡爾曼濾波器對兩種傳感器的數(shù)據(jù)進(jìn)行濾波融合，選用32位單片機(jī) STM32F103RB為控制核心處理器，完成對數(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控制。 目 錄1系統(tǒng)方案11.1 姿態(tài)檢測模塊的論證與選擇11.2 電機(jī)驅(qū)動模塊的論

2、證與選擇12 系統(tǒng)硬件設(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軸MEMS陀螺儀，3軸MEMS加速度計，以及一個可擴(kuò)展的

4、數(shù)字運(yùn)動處理器DMP（Digital Motion Processor），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴(kuò)展之后就可以通過其I2C或SPI接口輸出一個9軸的信號（SPI 接口僅在MPU-6000可用）。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)傳感器（MPU6050）監(jiān)測車身所處的俯仰狀態(tài)和狀態(tài)變化率，通過高速微控制器STM32F103RB 完成數(shù)據(jù)融合處理，得到平滑而穩(wěn)定車體姿態(tài)信息，然后驅(qū)動電動機(jī)產(chǎn)生前進(jìn)或后退的加速度來控制車體保持平衡，紅外線檢測障礙物，藍(lán)牙控制前進(jìn)后退。結(jié)構(gòu)框圖如圖。 圖 1 系統(tǒng)設(shè)計總體結(jié)構(gòu)框圖 系統(tǒng)硬件主要由STM32F103RB單片機(jī)系統(tǒng)、電源模塊、姿態(tài)檢測模塊、串口通訊模塊、電機(jī)驅(qū)動模塊等組成。 2.1 STM32F103RB 單片機(jī)系統(tǒng) 本設(shè)計中控制平臺采用ST 意法半導(dǎo)體的STM32F103RB微控制器，STM32系列基于專為要求高性能、低成本、低功耗的嵌入式應(yīng)用專門設(shè)計的

6、ARM CORTEX-M3內(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、3個通用16位定時器和1

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

8、微控制器適合于多種應(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è)計采用STM32F103RB單片機(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振蕩器，如果使能了中斷，軟件可以接收到相應(yīng)的中

9、斷。同樣，在需要時可以采取對PLL時鐘完全的中斷管理(如當(dāng)一個間接使用的外部振蕩器失效時)。多個預(yù)分頻器用于配置AHB的頻率、高速APB(APB2)和低速APB(APB1)區(qū)域。AHB和高速APB的最高頻率是72MHz，低速APB的最高頻率為36MHz。圖3-3 單片機(jī)最小系統(tǒng)及時鐘電路原理圖 單片機(jī)的外部復(fù)位電路可以使用按鈕和電容構(gòu)成，也可以使用專門的復(fù)位芯片。考慮到產(chǎn)品經(jīng)濟(jì)型，本設(shè)計中采用了由電阻電容構(gòu)成的簡易復(fù)位電路，如圖3-5所示。加電后，由于電容的充電時間，RESET保持低電平，單片機(jī)復(fù)位；一段時間后，電容電量充滿，RESET端輸出高電平，此時單片機(jī)運(yùn)行。手動復(fù)位時，按下手動復(fù)位按鈕

10、，RESET端保持低電平，單片機(jī)復(fù)位；釋放手動復(fù)位按鈕后，RESET端輸出高電平，單片機(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引腳和內(nèi)部調(diào)壓器供電。 VSSA，VDDA = 2.03.6V：為ADC、復(fù)位模塊、RC振蕩器和PLL的模擬部分提供供電。使用ADC時，VDDA不得小于2.4V。VDDA和VSSA必須分別連接到VDD和VSS。 VBAT = 1.83.6V：當(dāng)關(guān)閉VDD

11、時，(通過內(nèi)部電源切換器)為RTC、外部32kHz振蕩器和后備寄存器供電。圖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-6050實(shí)物連接圖2.2.1 MPU-6050簡介MPU-60X0 是全球首例9軸運(yùn)動處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計，以及一個可擴(kuò)展的數(shù)字運(yùn)動處理器DMP（Digital Motion Processor），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴(kuò)展之后就可

12、以通過其I2C或SPI接口輸出一個9軸的信號（SPI 接口僅在MPU-6000可用）。MPU-60X0也可以通過其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。 MPU-60X0 對陀螺儀和加速度計分別用了三個16 位的ADC，將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動，傳感器的測量范圍都是用戶可控的，陀螺儀可測范圍為±250，±500，±1000，±2000°/秒（dps），加速度計可測范圍為±2，±4，±8，±16g。一個片上1024 字節(jié)的FIFO，有助于降低系統(tǒng)功耗。和

13、所有設(shè)備寄存器之間的通信采用400kHz 的I2C 接口或1MHz 的SPI 接口（SPI 僅MPU-6000 可用）。對于需要高速傳輸?shù)膽?yīng)用，對寄存器的讀取和中斷可用20MHz 的SPI。 另外，片上還內(nèi)嵌了一個溫度傳感器和在工作環(huán)境下僅有±1%變動的振蕩器。芯片尺寸4×4×0.9mm，采用QFN 封裝（無引線方形封裝），可承受最大10000g 的沖擊，并有可編程的低通濾波器。 關(guān)于電源，MPU-60X0 可支持VDD 范圍2.5V±5%，3.0V±5%，或3.3V±5%。另外MPU-6050 還有一個VLOGIC 引腳，用來為I2

14、C 輸出提供邏輯電平。VLOGIC 電壓可取1.8±5%或者VDD。 MPU6050電路圖2.2.3數(shù)字運(yùn)動處理器（DMP）DMP就是指MPU6050內(nèi)部集成的處理單元，可以直接運(yùn)算出四元數(shù)和姿態(tài)，而不再需要另外進(jìn)行數(shù)學(xué)運(yùn)算。DMP的使用大大簡化了四軸的代碼設(shè)計。DMP是數(shù)字運(yùn)動處理器的縮寫，顧名思義mpu6050并不單單是一款傳感器，其內(nèi)部還包含了可以獨(dú)立完成姿態(tài)解算算法的處理單元。如在設(shè)計中使用DMP來實(shí)現(xiàn)傳感器融合算法優(yōu)勢很明顯。首先invensense官方提供的姿態(tài)解算算法應(yīng)該比像樓主這樣的小白要可靠的多。其次，由DMP實(shí)現(xiàn)姿態(tài)解算算法將單片機(jī)從算法處理的壓力中解放出來，單片

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

16、圖3-11。2.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。圖3-12 編碼器A、B相輸出波形本系統(tǒng)用OMRON旋轉(zhuǎn)編碼器相關(guān)參數(shù)如表3-1所示：表3-1 編碼器參數(shù)表項(xiàng)目參數(shù)電源電壓DC 5V12V，紋波的峰峰值小

17、于5%消耗電流30mA以下分辨率500 脈沖/轉(zhuǎn)輸出相A、B、Z 三相輸出狀態(tài)集電極開路輸出最高輸出響應(yīng)42kHz輸出上升或下降時間1us 以下起動轉(zhuǎn)矩1mN·m以下軸允許力徑向 10N、軸向 5N允許最高轉(zhuǎn)速5000r/min質(zhì)量35g2.3.2 編碼器電路設(shè)計由于編碼器采用集電極開路輸出，輸出波形為矩形波，因此編碼器外圍電路較為簡單。需要在信號輸出端接入一個上拉電阻，即可將信號提供給單片機(jī)采集數(shù)據(jù)。如圖3-13所示，PULSE引腳為編碼器A相，接單片機(jī)的脈沖計數(shù)口，通過單片機(jī)的PACNT模塊對輸入脈沖進(jìn)行計數(shù)從而獲取電機(jī)轉(zhuǎn)速。DIR為編碼器B相輸出，接單片機(jī)I/O口，通過A、B相

18、位差進(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ū)動信號由STM32F103RB提供，信號經(jīng)過光耦隔離后，傳至控制芯片L298N，通過L298N的輸出腳與兩個電機(jī)相連，控制電機(jī)運(yùn)轉(zhuǎn)。L298N的連接方法如圖6所示 圖6理論分析與計算2.4.2 L298N特點(diǎn)基本特點(diǎn)如下： （1）

19、 具有信號指示。（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é)模型 已知自平衡車高度為，質(zhì)量為，將其抽象為一級倒立擺，并將倒立擺至于可水平移動的小車上。假設(shè)其受外力干擾引起的車體角加速度為，沿垂直于車體方向進(jìn)行受力分析如圖2-7，可以得到自平衡車傾角與車輪移動加速度為以及外力干擾帶來的加速度之間的運(yùn)動方程。圖2-7 外力干擾條件下的小車受力分析小車運(yùn)動微分方程表達(dá)式如式2-8： （式2-8）當(dāng)

20、傾角很小的時候，可以進(jìn)行線性化處理： ，運(yùn)動微分方程可簡化為： （式2-9）自平衡車靜止時,其運(yùn)動微分方程為： （式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)用最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡稱PID控制，又稱PID調(diào)節(jié)。PID控制器問世至今已有近70年歷史，以其結(jié)構(gòu)簡單、穩(wěn)定性好、工作可靠、調(diào)整方便而成為工業(yè)控制的主要技術(shù)之一。PID控制器由比例單元（P）、積分單元（I）和微分單元（

21、D）組成。其輸入e (t）與輸出u (t）的關(guān)系為： (式2-11)其中為比例系數(shù)；為積分時間常數(shù)；為微分時間常數(shù)。PID控制器具有原理簡單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對模型依賴少等特點(diǎn)，因此使用PID控制器實(shí)現(xiàn)兩輪自平衡車的控制是完全可行的。3.2.2 PID控制器設(shè)計 由小車靜止時其運(yùn)動方程可得到系統(tǒng)輸入輸出傳遞函數(shù)： (式2-12) 此時系統(tǒng)具有兩個極點(diǎn)：。其中一個極點(diǎn)位于s平面的右半平面。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知系統(tǒng)不穩(wěn)定，因此小車在靜止?fàn)顟B(tài)不能保持平衡56。由小車受力分析可知小車平衡的條件是提供額外的回復(fù)力及阻尼，其來源為車輪與地面的摩擦力。由式2-7可知，車輪提供的加速度的大小

22、是根據(jù)角度及角速度的反饋得出，因此需要在控制系統(tǒng)中引入角度及角速度構(gòu)成比例(P)微分(D)反饋環(huán)節(jié)，如圖2-8所示。圖2-8 加入比例微分環(huán)節(jié)后的控制系統(tǒng)結(jié)構(gòu)圖加入比例微分反饋后的系統(tǒng)傳遞函數(shù)為： (式2-13)此時，系統(tǒng)的兩個極點(diǎn)為。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知，系統(tǒng)穩(wěn)定需要兩個極點(diǎn)都位于s平面的左半平面。要滿足這一點(diǎn)，需要>g, >0。由此可得出結(jié)論，但>g, >0時，小車可以保持平衡，這也與上文中小車受力分析的結(jié)果相符。在反饋環(huán)節(jié)中，與角度成比例的控制量稱為比例控制；與角速度成比例的控制量稱為微分控制（角速度是角度的微分）。因此上面系數(shù)，分別稱為比例和微分控制參數(shù)。其

23、中微分參數(shù)相當(dāng)于阻尼力，可以有效抑制自平衡車振蕩?？刂葡到y(tǒng)的輸出量為電機(jī)控制量，因而小車平衡控制的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程序#include <stm32f10x.h>#include "math.h"#include <pwm.h>float PID_realize();void pwm(void);e

24、xtern float Pitch;float SetSpeed=-1.04;/定義目標(biāo)值float ActualSpeed=0.0;/反饋值float err=0.0;/偏差值float err_last=0.0;/上次偏差值float Kp=135;float Ki=2.7;float Kd=135;float output=0.0;/電壓值控制執(zhí)行器的變量float integral=0.0;/積分量float PID_realize()ActualSpeed=Pitch;err=SetSpeed-ActualSpeed;integral+=err;output=Kp*err+Kd*(e

25、rr-err_last)+Ki*integral;err_last=err;return output;3.3 基于卡爾曼濾波的數(shù)據(jù)融合 卡爾曼濾波器是一種高效率的遞歸濾波器(自回歸濾波器)，能夠從一系列的不完全及包含噪聲的測量中，估計動態(tài)系統(tǒng)的狀態(tài)。卡爾曼濾波器不僅能估計信號的過去和當(dāng)前狀態(tài)，甚至能估計將來的狀態(tài)?？柭鼮V波器解決離散時間控制過程的一般方法，首先定義模型線性隨機(jī)微分方程。假設(shè)卡爾曼濾波模型k時刻真實(shí)狀態(tài)是從（k-1）時刻推算出來，如下式 (式2-15)式2-15中，是k時刻狀態(tài)；A是k-1時刻狀態(tài)變換模型；B是作用在控制器向量上的輸入控制模型；是過程噪聲，假設(shè)其均值為零，協(xié)方

26、差矩陣符合多元正態(tài)分布： (式2-16)k時刻對應(yīng)真實(shí)狀態(tài)的測量滿足下式： (式2-17)式2-17中是觀測模型，將真實(shí)控制映射為觀測空間；為觀測噪聲，其均值為零，協(xié)方差矩陣符合正態(tài)分布： (式2-18)初始狀態(tài)以及每一時刻的噪聲都認(rèn)為是互相獨(dú)立的?？柭鼮V波器的操作主要包括兩個階段：預(yù)估與更新。在預(yù)估階段，濾波器根據(jù)上一時刻狀態(tài)，估算出當(dāng)前時刻狀態(tài)；在更新階段，濾波器利用當(dāng)前時刻觀測值優(yōu)化在預(yù)估階段獲得的測量值，以獲得一個更準(zhǔn)確的新估計值111213?？柭鼮V波器迭代過程如下：1. 先驗(yàn)狀態(tài)估計：2. 先驗(yàn)估計誤差協(xié)方差 3. 卡爾曼增益 (式2-21)4. 后驗(yàn)狀態(tài)估計 (式2-22)5. 后驗(yàn)誤差協(xié)方差 (式2-23)在上面各式中：A：作用在上的n階矩陣；B：作用在控制向量上的n×1輸入控制矩陣；H：m×n觀測模型矩陣，將真實(shí)狀態(tài)空間映射為觀測空間；：n×n先驗(yàn)估計誤差協(xié)方差矩陣；：n×n后驗(yàn)估計誤差協(xié)方差矩陣；Q：n×n過程噪聲協(xié)方差矩陣；R：m×m過程噪聲協(xié)方差矩陣；I：n階單位矩陣；：n×m矩陣，稱之為卡爾曼增益。4 系統(tǒng)軟件設(shè)計 4.1 系統(tǒng)軟件設(shè)計框架系統(tǒng)軟件設(shè)計框架主要