2015電賽控制類風(fēng)力擺論文

2015電賽控制類風(fēng)力擺論文2015電賽控制類風(fēng)力擺論文/2015電賽控制類風(fēng)力擺論文2015年全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽風(fēng)力擺控制系統(tǒng)（B題）【本科組】2015年8月15日摘要本設(shè)計(jì)由MC9S12XS128MAL單片機(jī)控制模塊、角度檢測(cè)模塊、人機(jī)交互系統(tǒng)以與風(fēng)力擺機(jī)械結(jié)構(gòu)組成的閉環(huán)控制系統(tǒng)。由BTN7971驅(qū)動(dòng)12V、1.2A的直流風(fēng)機(jī)作為風(fēng)力擺的動(dòng)力源，MMA7361采集風(fēng)力狀態(tài)角，單片機(jī)處理狀態(tài)角數(shù)據(jù)后通過PID調(diào)節(jié)直流風(fēng)機(jī)控制風(fēng)力擺，實(shí)現(xiàn)在直流風(fēng)機(jī)作為動(dòng)力控制下快速起擺、畫線、恢復(fù)靜止，畫圓的功能。為了使測(cè)控系統(tǒng)控制更為精確，在采集數(shù)據(jù)的過程中采用了非線性誤差校正以與卡爾曼濾波等數(shù)據(jù)處理方法。同時(shí)設(shè)計(jì)中考慮到可能存在的各種干擾因素，采用軟硬件結(jié)合的抗干擾方法提高系統(tǒng)控制的穩(wěn)定性。經(jīng)過反復(fù)的測(cè)驗(yàn)，該方案完全能夠?qū)崿F(xiàn)題目要求！關(guān)鍵字：?jiǎn)纹瑱C(jī)閉環(huán)風(fēng)力擺控制系統(tǒng)卡爾曼濾波目錄TOC\o"1-2"\h\u83461.方案論證與比較 1169281.1.系統(tǒng)主控芯片選擇 1220331.2.風(fēng)力擺動(dòng)力系統(tǒng)方案選擇 1138601.3.角度檢測(cè)方案的選擇與論證 2291541.4.電機(jī)驅(qū)動(dòng)模塊的選擇與論證 2234741.5.算法選擇 2297812.系統(tǒng)設(shè)計(jì) 3247402.1.系統(tǒng)總體設(shè)計(jì) 340372.2.模塊電路設(shè)計(jì) 387423.系統(tǒng)理論分析與計(jì)算 5193183.1.風(fēng)力擺的運(yùn)動(dòng)控制分析 5245813.2.系統(tǒng)算法的分析 529484.軟件設(shè)計(jì) 6268014.1.程序功能描述與設(shè)計(jì)思路 683524.2.程序流程圖 6325455.系統(tǒng)調(diào)試與測(cè)試 7129585.1.系統(tǒng)調(diào)試 7177495.2.系統(tǒng)測(cè)試 7304876.結(jié)束語(yǔ) 840447.附錄 9145107.1.附錄1：參考文獻(xiàn) 9118307.2.附錄2：主要元器件清單 9249637.3.附錄3：電路原理圖 1078377.4.附錄4：部分源程序 11方案論證與比較系統(tǒng)主控芯片選擇方案一：采用AT89C51系列單片機(jī)作為控制的核心。51單片機(jī)價(jià)格便宜，應(yīng)用廣泛，I/O口的設(shè)置和使用操作簡(jiǎn)單。但是51單片機(jī)的運(yùn)行速度過慢，抗靜電抗干擾能力弱，內(nèi)部資源和存儲(chǔ)器功能較少，而且無(wú)ADC，還需要用外接電路實(shí)現(xiàn)AD轉(zhuǎn)換，使硬件電路變得復(fù)雜。另外，51單片機(jī)完成系統(tǒng)功能較為困難。方案二：采用MC9S12XS128MAL作為控制核心MC9S12XS128MAL是Freescale公司生產(chǎn)的16位單片機(jī)，儲(chǔ)存功能強(qiáng)大，具有豐富的輸入/輸出端口資源，具有多種寄存器功能，而且端口引腳大多為復(fù)用口，具有多功能，所有端口都具有通用I/O口功能。內(nèi)部本身自帶PWM、A/D轉(zhuǎn)換功能，可以直接用，省去了單獨(dú)做PWM模塊和A/D轉(zhuǎn)換模塊，節(jié)省大量時(shí)間。完全能夠?qū)崿F(xiàn)本系統(tǒng)的所有功能。綜合考慮，本系統(tǒng)選擇方案二風(fēng)力擺動(dòng)力系統(tǒng)方案選擇根據(jù)題目要求，用2~4只直流風(fēng)機(jī)，共有一下三種方案選擇！方案一：采用兩只風(fēng)機(jī)作為系統(tǒng)動(dòng)力系統(tǒng)。兩只風(fēng)機(jī)并排同向而立，分別固定擺桿兩側(cè)，通過控制兩個(gè)風(fēng)機(jī)轉(zhuǎn)速產(chǎn)生不同風(fēng)力控裝置擺動(dòng)是激光筆劃線畫圓。此方案遂犯負(fù)載親，但是在很難控制風(fēng)力擺的狀態(tài)修正和制動(dòng)。方案二：采用三只風(fēng)機(jī)作為動(dòng)力系統(tǒng)。三只風(fēng)機(jī)成等邊三角形相背而立，互成120度夾角（外角）。此方案相對(duì)于方案一在控制風(fēng)力擺轉(zhuǎn)動(dòng)過程中狀態(tài)修正方面有提升，而且對(duì)于畫圓有一定優(yōu)勢(shì)，但是不好掌握相鄰風(fēng)機(jī)的推力，在控制系統(tǒng)運(yùn)動(dòng)方向難度很大。方案三：采用四只直流風(fēng)機(jī)作為動(dòng)力系統(tǒng)。四只風(fēng)機(jī)兩兩背靠背而立，中間用碳纖維棒將四只風(fēng)機(jī)固定成十字架形狀。綜合上述比較分析，考慮到系統(tǒng)的快速工作以與精確控制，本系統(tǒng)采用方案三。角度檢測(cè)方案的選擇與論證方案一：角位移電位器。角位移電位器屬于二位平面內(nèi)角位移傳感器測(cè)量擺動(dòng)時(shí)關(guān)于靜止?fàn)顟B(tài)時(shí)的偏轉(zhuǎn)角，通過該偏轉(zhuǎn)角控制流風(fēng)機(jī)的狀態(tài)。但是角位移電位器只能檢測(cè)出二維平面內(nèi)的角度變化，不利于檢測(cè)風(fēng)力擺的空間位置。方案二：選用三軸加速度傳感器。三周加速度傳感器具有體積小和重量輕的特點(diǎn)，可以測(cè)得空間加速度，能夠全面準(zhǔn)確反映物體的運(yùn)動(dòng)的空間位置。此方案課精確測(cè)量風(fēng)力擺的當(dāng)前狀態(tài)，實(shí)現(xiàn)對(duì)風(fēng)力擺的精確控制。綜合考慮，本系統(tǒng)選擇方案二。電機(jī)驅(qū)動(dòng)模塊的選擇與論證方案一：使用L298芯片實(shí)現(xiàn)電機(jī)驅(qū)動(dòng)。L298N內(nèi)部包含4通道邏輯驅(qū)動(dòng)電路，與內(nèi)部含兩個(gè)H橋的雙全橋驅(qū)動(dòng)器，接受TTL邏輯電平信號(hào)通過PWM實(shí)現(xiàn)直流風(fēng)機(jī)調(diào)速。但是L298驅(qū)動(dòng)大功率電機(jī)時(shí)發(fā)熱嚴(yán)重易燒毀芯片。方案二：使用英飛凌的BTN7971驅(qū)動(dòng)芯片。BTN7971是大功率集成半橋驅(qū)動(dòng)芯片，在單一封裝中集成了PMOS和NMOS高低側(cè)功率開關(guān)以與門驅(qū)動(dòng)器，還集成了邏輯控制電路和一些保護(hù)檢測(cè)功能，能夠防止過熱、過壓、欠壓、過流和短路。綜合考慮，本系統(tǒng)選擇方案二。算法選擇算法有很多種，它的選擇很重要，它決定處理系統(tǒng)的性能質(zhì)量和可行性。方案一：自適應(yīng)算法。自適應(yīng)過程是一個(gè)不斷逼近目標(biāo)的過程。單片機(jī)讀取傳感器采集來的風(fēng)力擺空間位置來控制風(fēng)力擺的運(yùn)動(dòng)，但是由于該系統(tǒng)變化太快，自適應(yīng)算法調(diào)節(jié)方式難以達(dá)到系統(tǒng)要求。方案二：PID算法。在過程控制中，按比例、積分和微分的函數(shù)關(guān)系，對(duì)系統(tǒng)模型進(jìn)行運(yùn)算分析，將其運(yùn)算結(jié)果用以輸出控制?？刂凭雀?，且算法簡(jiǎn)單明了。系統(tǒng)設(shè)計(jì)系統(tǒng)總體設(shè)計(jì)根據(jù)上述方案論證，我們最終確定了以MC9S12XS128MAL單片機(jī)作為中央處理器，采用型號(hào)為MMA7361的模擬加速度陀螺儀檢測(cè)風(fēng)力擺的空間位置，使用BTN7971芯片驅(qū)動(dòng)4個(gè)大功率直流風(fēng)機(jī)?？傮w框圖如下圖2.1所示。人機(jī)交互S12人機(jī)交互S12XS128單片機(jī)角度傳感器模塊信號(hào)處理角度傳感器模塊信號(hào)處理LED燈光提示直流風(fēng)機(jī)電機(jī)驅(qū)動(dòng)LED燈光提示直流風(fēng)機(jī)電機(jī)驅(qū)動(dòng)圖2.1設(shè)計(jì)總體框圖模塊電路設(shè)計(jì)角度傳感器角度傳感器選用藍(lán)宙電子生產(chǎn)的3軸陀螺儀+3軸模擬加速度傳感器MMA7361模塊。該模塊具有ENC-03M（全新包裝）三軸陀螺儀傳感器、MMA7361三軸模擬量輸出。電機(jī)驅(qū)動(dòng)模塊BTN7971驅(qū)動(dòng)芯片電路圖如圖2.2所示，BTN7971屬于半橋驅(qū)動(dòng)芯片，所以用兩個(gè)芯片組成一個(gè)完整的全橋驅(qū)動(dòng)芯片，如圖2（b）所示。由于直流風(fēng)機(jī)無(wú)法反轉(zhuǎn)，將風(fēng)機(jī)地端連載一起，將電源端接入驅(qū)動(dòng)電路中的OUT1、OUT2、OUT3、OUT4，將驅(qū)動(dòng)電路的輸入口接單片機(jī)PWM1、3、5、7口，通過個(gè)給驅(qū)動(dòng)輸入PWM值臺(tái)階風(fēng)機(jī)速度達(dá)到對(duì)風(fēng)機(jī)控制目的。圖2.2撥碼開關(guān)在單片機(jī)的PB口接一個(gè)撥碼開關(guān)，實(shí)現(xiàn)人機(jī)交互功能。當(dāng)撥動(dòng)一個(gè)值后，系統(tǒng)會(huì)切換一個(gè)系統(tǒng)模式，完成本系統(tǒng)的全部功能操作。該模塊方便選擇各種模式。系統(tǒng)理論分析與計(jì)算風(fēng)力擺的運(yùn)動(dòng)控制分析風(fēng)力擺采用4只功率為12V、1.2A的大功率直流風(fēng)機(jī)為動(dòng)力驅(qū)動(dòng)系統(tǒng)。模擬加速度陀螺儀采集風(fēng)力擺的當(dāng)前的角度，單片機(jī)處理角度信息輸出PWM占空比，控制4只風(fēng)機(jī)的工作狀態(tài)，從而實(shí)現(xiàn)對(duì)風(fēng)力擺的控制。圖3.1風(fēng)力擺結(jié)構(gòu)示意圖系統(tǒng)算法的分析本系統(tǒng)采用PID算法來控制流風(fēng)機(jī)的轉(zhuǎn)動(dòng)速度，流風(fēng)機(jī)開始工作后，姿態(tài)采集模塊不斷采集當(dāng)前風(fēng)力擺的空間位置角度，并與之前的狀態(tài)比較，是的風(fēng)力擺的運(yùn)動(dòng)狀態(tài)逐漸趨向于平穩(wěn)。PID算法由風(fēng)力擺轉(zhuǎn)動(dòng)角度比例P、角度誤差積分I和角度微分D組成。其輸入e（t）與輸出U（t）的關(guān)系為：U（t）=P*［e(t)+1/I∫e(t)dt+D*de(t)/dt］它的傳遞函數(shù)為：G（s）=U(S)/E(S)=P*[1+1/(I*s)+D*s]風(fēng)力擺轉(zhuǎn)動(dòng)角度比例P：對(duì)風(fēng)力擺角速度進(jìn)行比例調(diào)節(jié)，即對(duì)風(fēng)機(jī)轉(zhuǎn)動(dòng)速度調(diào)整。比例越大，調(diào)節(jié)速度越快，但不能過大，過大可能造成四風(fēng)機(jī)因工作狀態(tài)的突變而使擺桿不穩(wěn)定。角度誤差積分I：使系統(tǒng)消除穩(wěn)態(tài)誤差。計(jì)入微分調(diào)節(jié)使系統(tǒng)穩(wěn)定系數(shù)下降，動(dòng)態(tài)相應(yīng)變慢。根據(jù)本系統(tǒng)要更快更穩(wěn)完成風(fēng)力擺的控制，本系統(tǒng)對(duì)積分調(diào)節(jié)的需要就非常弱。即保證在不需要時(shí)系統(tǒng)不會(huì)受到影響。角度微分D：微分反映風(fēng)力擺角度的變化率，具有可預(yù)見性，能預(yù)見偏差變化的趨勢(shì)因此產(chǎn)生超前的控制作用，在偏差還沒有形成之前，已被微分作用調(diào)節(jié)消除。因此，可以改善系統(tǒng)的動(dòng)態(tài)性能。在為分時(shí)間選擇合適情況下，減少調(diào)節(jié)時(shí)間。軟件設(shè)計(jì)程序功能描述與設(shè)計(jì)思路系統(tǒng)軟件實(shí)現(xiàn)的功能如下：1、讀取陀螺儀數(shù)據(jù)，將數(shù)據(jù)反饋給單片機(jī)；2、單片機(jī)將讀取數(shù)據(jù)的模擬量AD轉(zhuǎn)換為數(shù)字量后，將相對(duì)應(yīng)的PWM波發(fā)送給軸流風(fēng)機(jī)；3、控制平衡板；4、撥碼開關(guān)控制；5、數(shù)據(jù)匯總做出判斷。程序流程圖系統(tǒng)主程序流程圖如下圖所示，進(jìn)入主程序并初始化后，等待撥碼開關(guān)的狀態(tài)執(zhí)行相應(yīng)程序，每個(gè)程序能完成一項(xiàng)要求，各子模塊是獨(dú)立模塊，結(jié)構(gòu)清楚。開始開始初始化初始化否功能鍵按下否功能鍵按下是是基本要求1發(fā)揮部分基本要求1發(fā)揮部分2發(fā)揮部分1基本要求3基本要求2基基本要求4 圖4.1系統(tǒng)程序流程圖系統(tǒng)調(diào)試與測(cè)試系統(tǒng)調(diào)試為了便于系統(tǒng)調(diào)試，本系統(tǒng)使用CodeWarriorIDE進(jìn)行原程序的編寫、編譯和鏈接，并最終生成可執(zhí)行文件。該編譯器具有高度可視化界面操作與自動(dòng)創(chuàng)建復(fù)雜嵌入式系統(tǒng)應(yīng)用的功能，包括集成開發(fā)環(huán)境IDE、處理器專家?guī)臁⑷酒抡?、可視化參?shù)顯示工具、項(xiàng)目工程管理器、C交叉編譯器、匯編器、連接器以與調(diào)試器，可支持在線調(diào)試，為使用嵌入式產(chǎn)品開發(fā)提供了便利。系統(tǒng)測(cè)試測(cè)試工具刻度尺、秒表、量角器測(cè)試指標(biāo)與方法基本要求（1）：驅(qū)動(dòng)風(fēng)力擺工作，使激光筆穩(wěn)定地在地面劃出的直線段，來回五次，記錄七由靜止只開始自由擺時(shí)間與最大偏差距離。（15s內(nèi)完成，線性誤差大于±2.5cm），經(jīng)測(cè)驗(yàn)，滿足題目要求。基本要求（2）：設(shè)置風(fēng)力擺劃線長(zhǎng)度，驅(qū)動(dòng)風(fēng)力擺工作，記錄其由靜止開始自由擺時(shí)間與在華不同長(zhǎng)度直線時(shí)的最大偏差距離。（15s內(nèi)完成，線性誤差大于±2.5cm）。經(jīng)測(cè)驗(yàn)，滿足題目要求?；疽螅?）：設(shè)置風(fēng)力擺自由擺時(shí)的角度，驅(qū)動(dòng)風(fēng)力擺工作，記錄其由靜止只開始自由擺時(shí)間與在畫不同角度直線時(shí)的最大偏差距離。經(jīng)測(cè)驗(yàn)，滿足題目要求?；疽螅?）：將風(fēng)力擺拉起任意角度（30度~45度），5s內(nèi)使風(fēng)力擺制動(dòng)達(dá)到靜止。將風(fēng)力擺拉起一定角度放開，測(cè)試風(fēng)力擺制動(dòng)所需的時(shí)間。經(jīng)測(cè)驗(yàn)，滿足規(guī)定的5s內(nèi)靜止。發(fā)揮部分：以風(fēng)力擺靜止時(shí)的光電為圓心，設(shè)置風(fēng)力擺畫圓半徑，區(qū)東風(fēng)里擺動(dòng)激光筆在地面畫圓，記錄其畫三次圓所用的時(shí)間以與最大偏差距離，重復(fù)測(cè)試三次。改變?cè)霃皆俅螠y(cè)驗(yàn)，重復(fù)以上操作四次，經(jīng)測(cè)驗(yàn)，誤差在規(guī)定的30s、±2.5cm的線性誤差內(nèi)。結(jié)束語(yǔ)通過緊張的四天三夜的奮力拼搏，與小組其他成員的通力協(xié)作，團(tuán)結(jié)互助，終于完成了設(shè)計(jì)項(xiàng)目。通過不斷地測(cè)試調(diào)試，本系統(tǒng)達(dá)到了設(shè)計(jì)的四個(gè)基本要求和發(fā)揮部分，在本次設(shè)計(jì)的過程中，我們遇到了很多突發(fā)事件和各種困難。尤其是向如何在擺動(dòng)過程中如何保持激光軌跡的線性度一度陷入困難。但通過團(tuán)隊(duì)的仔細(xì)分析和自我調(diào)整狀態(tài)后我們終于解決了問題，取得了比較滿意的結(jié)果。經(jīng)過本次電子設(shè)計(jì)競(jìng)賽讓我們對(duì)電路的設(shè)計(jì)、調(diào)試有了深刻的印象，加深了對(duì)測(cè)控技術(shù)的理解，同時(shí)也深刻體會(huì)到了共同協(xié)作和團(tuán)隊(duì)精神的重要性，提高了我們動(dòng)手解決問題的能力，設(shè)計(jì)中還有欠缺的地方，今后的學(xué)習(xí)工作中會(huì)加以注意。同時(shí)感謝競(jìng)賽的主辦方讓我們得到了很好的鍛煉機(jī)會(huì)，由于時(shí)間緊迫我們未能將系統(tǒng)方案調(diào)整到最優(yōu)，但是我們會(huì)利用課余時(shí)間繼續(xù)深入研究找出更好的的方案！附錄附錄1：參考文獻(xiàn)[1]黃智偉.大學(xué)生全國(guó)電子設(shè)計(jì)競(jìng)賽訓(xùn)練教程.北京：電子工業(yè)出版社，2005[2]譚浩強(qiáng).C語(yǔ)言程序設(shè)計(jì)(第二版).北京：清華大學(xué)出版社，2000[3]郭天祥.新概念51單片機(jī)C語(yǔ)言教程.入門、提高、開發(fā)[M].北京：電子工業(yè)出版社，2009[4]張陽(yáng)、吳曄、滕勤.MC9S12XS單片機(jī)原理與嵌入式系統(tǒng)開發(fā).北京：電子工業(yè)出版社附錄2：主要元器件清單元件名稱數(shù)量（個(gè)）模擬加速度陀螺儀112V、1.2A軸流風(fēng)機(jī)4MC9S12XS128MAL單片機(jī)1BTN7971驅(qū)動(dòng)芯片412V大功率電源1激光筆1附錄3：電路原理圖圖7.1主板原理圖圖7.2驅(qū)動(dòng)模塊整體電路圖附錄4：部分源程序voidKalman_Filter_Y(floataccel,floatgyro)//range-1000-2g{floatPHt_0,PHt_1,E,K_0,K_1,Y_0,Y_1;//K_0含有卡爾曼增益的另外一個(gè)函數(shù)，用于計(jì)算最優(yōu)估計(jì)值//K_1含有卡爾曼增益的函數(shù)，用于計(jì)算最優(yōu)估計(jì)值的偏差//先驗(yàn)估計(jì)//角度更新angle_y+=(gyro-gyro_bias)*dt;//由角速度積分得到的角度//計(jì)算過程協(xié)方差矩陣的微分矩陣//關(guān)于dt的調(diào)整，增加dt可使kalmanfilter的輸出相位滯后Pdot[0]=Q_acce-PP[0][1]-PP[1][0];//派生協(xié)方差矩陣計(jì)算Pk-'0,0先驗(yàn)估計(jì)誤差協(xié)方差的微分Pdot[1]=-PP[1][1];//0,1Pdot[2]=-PP[1][1];//1,0Pdot[3]=Q_gyro;//1,1Pdot=A*P+P*A導(dǎo)+QPP[0][0]+=Pdot[0]*dt;//計(jì)算協(xié)方差矩陣//協(xié)方差矩陣更新PP[0][1]+=Pdot[1]*dt;//Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分=先驗(yàn)估計(jì)誤差協(xié)方差PP[1][0]+=Pdot[2]*dt;PP[1][1]+=Pdot[3]*dt;PHt_0=H_0*PP[0][0];PHt_1=H_0*PP[1][0];E=R_angle+H_0*PHt_0;//估計(jì)偏差//計(jì)算的過程量//誤差估計(jì)計(jì)算K_0=PHt_0/E;//計(jì)算卡爾曼增益//卡爾曼增益，E越大，K越小K_1=PHt_1/E;//后驗(yàn)估計(jì)//更新狀態(tài)估計(jì)//計(jì)算測(cè)量角度和估計(jì)角度的偏差angle_error=accel-angle_y;//計(jì)算角度偏差//zk-先驗(yàn)估計(jì)angle_y+=K_0*angle_error;//更新角度最優(yōu)估計(jì)值//融合后的角度gyro_bias+=K_1*angle_error;//更新最優(yōu)估計(jì)值偏差rotate_y=gyro-gyro_bias;//更新角速度最優(yōu)估計(jì)值//輸出值（angle_y）的微分=角速度//融合后角速度//更新協(xié)方差矩陣//后驗(yàn)估計(jì)，更新協(xié)方差陣Y_0=PHt_0;Y_1=H_0*PP[0][1];PP[0][0]-=K_0*Y_0;//后驗(yàn)估計(jì)誤差協(xié)方差PP[0][1]-=K_0*Y_1;PP[1][0]-=K_1*Y_0;//P（K|K）E越大，P越大PP[1][1]-=K_1*Y_1;}/*voidKalman_Filter_Y1(floataccel1,floatgyro1)//range-1000-2g{floatPHt_0,PHt_1,E,K_0,K_1,Y_0,Y_1;//K_0含有卡爾曼增益的另外一個(gè)函數(shù)，用于計(jì)算最優(yōu)估計(jì)值//K_1含有卡爾曼增益的函數(shù)，用于計(jì)算最優(yōu)估計(jì)值的偏差//先驗(yàn)估計(jì)//角度更新angle_y1+=(gyro1-gyro_bias1)*dt1;//由角速度積分得到的角度//計(jì)算過程協(xié)方差矩陣的微分矩陣//關(guān)于dt的調(diào)整，增加dt可使kalmanfilter的輸出相位滯后Pdot1[0]=Q_acce1-PP1[0][1]-PP1[1][0];//派生協(xié)方差矩陣計(jì)算Pk-'0,0先驗(yàn)估計(jì)誤差協(xié)方差的微分Pdot1[1]=-PP1[1][1];//0,1Pdot1[2]=-PP1[1][1];//1,0Pdot1[3]=Q_gyro1;//1,1Pdot=A*P+P*A導(dǎo)+QPP1[0][0]+=Pdot1[0]*dt1;//計(jì)算協(xié)方差矩陣//協(xié)方差矩陣更新PP1[0][1]+=Pdot1[1]*dt1;//Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分=先驗(yàn)估計(jì)誤差協(xié)方差PP1[1][0]+=Pdot1[2]*dt1;PP1[1][1]+=Pdot1[3]*dt1;PHt_0=H_1*PP1[0][0];PHt_1=H_1*PP1[1][0];E=R_angle1+H_1*PHt_0;//估計(jì)偏差//計(jì)算的過程量//誤差估計(jì)計(jì)算K_0=PHt_0/E;//計(jì)算卡爾曼增益//卡爾曼增益，E越大，K越小K_1=PHt_1/E;//后驗(yàn)估計(jì)//更新狀態(tài)估計(jì)//計(jì)算測(cè)量角度和估計(jì)角度的偏差angle_error1=accel1-angle_y1;//計(jì)算角度偏差//zk-先驗(yàn)估計(jì)//融合后的角度angle_y1+=K_0*angle_error1;//更新角度最優(yōu)估計(jì)值gyro_bias1+=K_1*angle_error1;//更新最優(yōu)估計(jì)值偏差//融合后角速度