專利名稱：一種提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法
技術(shù)領(lǐng)域：
本發(fā)明涉及的是ー種基于模糊控制自適應(yīng)Kalman濾波算法來提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法。
背景技術(shù)：
航姿參考系統(tǒng)(Attitudeand Heading Reference System, AHRS)主要由微機(jī)械陀螺儀、微機(jī)械加速度計(jì)和地磁傳感器等器件組成，用來實(shí)時(shí)測量運(yùn)動(dòng)載體的姿態(tài)信息。航姿參考系統(tǒng)依靠慣性元件進(jìn)行測量，無需向外發(fā)射或接受電磁波，可完全自主確定載體的姿態(tài)信息，因此其隱蔽性很好，在軍事上有很廣泛的應(yīng)用，如無人飛機(jī)、導(dǎo)航與制導(dǎo)、航空航天等；同吋，由于航姿參考系統(tǒng)采用微機(jī)械器件，系統(tǒng)的集成度高、體積小巧、成本低，因此其在民用領(lǐng)域也有著廣泛的應(yīng)用，如車輛導(dǎo)航與控制、平臺(tái)控制、船舶姿態(tài)控制、機(jī)器人
坐坐寸寸o在航姿參考系統(tǒng)中，載體的航向信息主要是依靠地磁傳感器來確定，然而，在當(dāng)今的社會(huì)生活中，電器、無線通信設(shè)備等電子產(chǎn)品的廣泛應(yīng)用，使得載體經(jīng)常處于有電磁干擾的環(huán)境之中。航姿參考系統(tǒng)中的kalman濾波模塊，要求對系統(tǒng)噪聲和量測噪聲先驗(yàn)已知，系統(tǒng)噪聲可以經(jīng)過對系統(tǒng)的多次測量而得到，然而，復(fù)雜的電磁環(huán)境使得量測噪聲不確定，這將嚴(yán)重影響常規(guī)kalman濾波結(jié)果的精度，嚴(yán)重時(shí)甚至?xí)?dǎo)致系統(tǒng)濾波發(fā)散。因此，如何實(shí)時(shí)估計(jì)量測噪聲，成為了提高系統(tǒng)抗干擾能力及解算精度的首要問題。J. Z. Sasiadek 在論又〈〈Sensor Fusion Based on Fuzzy Kalman FilteringforAutonomous Robot Vehicle》中提出了基于模糊控制的自適應(yīng)kalman濾波算法，并將其成功的應(yīng)用到了 GPS/INS組合導(dǎo)航系統(tǒng)中。目前，基于模糊控制的自適應(yīng)kalman濾波算法已經(jīng)廣泛的應(yīng)用于GPS/INS組合導(dǎo)航系統(tǒng)中，然而其在AHRS中的應(yīng)用很少，QinWei 社論又《Fuzzy Adaptive Extended Kalman Filter for Miniature Attitude andHeadingReference System》中雖將該算法應(yīng)用于航姿參考系統(tǒng)中，然而其采用擴(kuò)展kalman濾波并同時(shí)對系統(tǒng)噪聲和量測噪聲同時(shí)進(jìn)行實(shí)時(shí)調(diào)整，這無疑大大的増加系統(tǒng)的計(jì)算復(fù)雜度，嚴(yán)重影響了系統(tǒng)的實(shí)時(shí)性。

發(fā)明內(nèi)容
本發(fā)明的目的在于克服現(xiàn)有技術(shù)存在的不足，而提供ー種以常規(guī)Kalman濾波為基礎(chǔ)，結(jié)合模糊控制理論，實(shí)時(shí)估計(jì)量測噪聲，實(shí)現(xiàn)自適應(yīng)濾波，以提高系統(tǒng)的穩(wěn)定性和可靠性的提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法。本發(fā)明提出的方法以常規(guī)kalman濾波為基礎(chǔ)，結(jié)合模糊控制理論，實(shí)時(shí)估計(jì)量測噪聲，實(shí)現(xiàn)自適應(yīng)濾波，提高系統(tǒng)的解算精度和抗干擾能力，所述的航姿參考系統(tǒng)主要由微機(jī)械陀螺儀、微機(jī)械加速度計(jì)和地磁傳感器組成，用來實(shí)時(shí)測量運(yùn)動(dòng)載體的姿態(tài)信息；所述的方法具體包括以下步驟第一根據(jù)加速度計(jì)和磁傳感器的輸出進(jìn)行系統(tǒng)的初始化，完成粗對準(zhǔn)過程；
第二根據(jù)陀螺儀的輸出，確定四元數(shù)的更新方程，實(shí)時(shí)計(jì)算更新系統(tǒng)的四元數(shù)，實(shí)現(xiàn)系統(tǒng)的更新過程；第三以加速度計(jì)和磁傳感器的輸出作為參考，運(yùn)用kalman濾波技術(shù)，實(shí)現(xiàn)系統(tǒng)的修正過程；第四結(jié)合模糊控制理論，在系統(tǒng)中加入模糊控制模塊，實(shí)時(shí)估計(jì)系統(tǒng)的量測噪聲，實(shí)現(xiàn)自適應(yīng)kalman濾波。本發(fā)明在所述的粗對準(zhǔn)過程中，根據(jù)加速度計(jì)的輸出，確定系統(tǒng)的初始俯仰角和橫滾角；結(jié)合磁傳 感器的輸出，確定初始航向角。根據(jù)初始姿態(tài)角，確定初始捷聯(lián)矩陣和初始四元數(shù)；所述的系統(tǒng)更新過程中，四元數(shù)的更新采用龍格-庫塔算法，根據(jù)更新得到的四元數(shù)，更新捷聯(lián)矩陣和姿態(tài)角；所述的系統(tǒng)修正過程中，四元數(shù)誤差矢量和陀螺儀的零偏誤差矢量作為狀態(tài)向量、以加速度誤差矢量和地磁誤差矢量作為觀測向量，確定kalman濾波的狀態(tài)方程和觀測方程，實(shí)時(shí)估計(jì)系統(tǒng)誤差，并將誤差反饋到系統(tǒng)中，修正四元數(shù)、捷聯(lián)矩陣和姿態(tài)角；所述的模糊控制模塊中有兩個(gè)模糊控制器，在kalman濾波過程中，分別實(shí)時(shí)計(jì)算殘差向量中加速度部分的理論、實(shí)際方差和磁部分的理論、實(shí)際方差，分別以二者的理論方差與實(shí)際方差的比值作為兩個(gè)模糊控制器的輸入。本發(fā)明所述的模糊控制器根據(jù)輸入，分別輸出修正系數(shù)修正加速度的量測噪聲和磁的量測噪聲，實(shí)現(xiàn)實(shí)時(shí)估計(jì)量測噪聲的作用。本發(fā)明可以在提高航姿參考系統(tǒng)精度的同時(shí)，大大降低了系統(tǒng)的計(jì)算復(fù)雜度；尤其在系統(tǒng)受較大噪聲干擾時(shí)，該算法能夠正確估計(jì)量測噪聲特性，改善姿態(tài)精度，提高系統(tǒng)的穩(wěn)定性和可靠性。


圖I是本發(fā)明所述的為基于模糊控制自適應(yīng)kalman濾波的航姿參考系統(tǒng)圖。圖2是本發(fā)明所述的為模糊控制模塊的輸入、輸出隸屬度函數(shù)圖。
具體實(shí)施例方式下面將結(jié)合附圖對本發(fā)明作詳細(xì)的介紹本發(fā)明是一種提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法。本發(fā)明提出的方法以常規(guī)kalman濾波為基礎(chǔ)，結(jié)合模糊控制理論，實(shí)時(shí)估計(jì)量測噪聲，實(shí)現(xiàn)自適應(yīng)濾波，提高系統(tǒng)的解算精度和抗干擾能力，所述的航姿參考系統(tǒng)主要由微機(jī)械陀螺儀、微機(jī)械加速度計(jì)和地磁傳感器組成，用來實(shí)時(shí)測量運(yùn)動(dòng)載體的姿態(tài)信息；所述的方法具體包括以下步驟第一、根據(jù)加速度計(jì)和磁傳感器的輸出進(jìn)行系統(tǒng)的初始化，完成粗對準(zhǔn)過程；第二、根據(jù)陀螺儀的輸出，確定四元數(shù)的更新方程，實(shí)時(shí)計(jì)算更新系統(tǒng)的四元數(shù)，實(shí)現(xiàn)系統(tǒng)的更新過程；第三、以加速度計(jì)和磁傳感器的輸出作為參考，運(yùn)用kalman濾波技木，實(shí)現(xiàn)系統(tǒng)的修正過程；
第四、結(jié)合模糊控制理論，在系統(tǒng)中加入模糊控制模塊，實(shí)時(shí)估計(jì)系統(tǒng)的量測噪聲，實(shí)現(xiàn)自適應(yīng)kalman濾波。本發(fā)明在所述的粗對準(zhǔn)過程中，根據(jù)加速度計(jì)的輸出，確定系統(tǒng)的初始俯仰角和橫滾角；結(jié)合磁傳感器的輸出，確定初始航向角。根據(jù)初始姿態(tài)角，確定初始捷聯(lián)矩陣和初始四元數(shù)；由于航向角的改變，并不改變加速度計(jì)的輸出，因此令航向角y = 0，則載體坐標(biāo)系和導(dǎo)航坐標(biāo)系下的加速度轉(zhuǎn)換關(guān)系可表示為
權(quán)利要求
1.ー種提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法，所述的航姿參考系統(tǒng)主要由微機(jī)械陀螺儀、微機(jī)械加速度計(jì)和地磁傳感器組成，用來實(shí)時(shí)測量運(yùn)動(dòng)載體的姿態(tài)信息；其特征在于所述的方法具體包括以下步驟 第一、根據(jù)加速度計(jì)和磁傳感器的輸出進(jìn)行系統(tǒng)的初始化，完成粗對準(zhǔn)過程； 第二、根據(jù)陀螺儀的輸出，確定四元數(shù)的更新方程，實(shí)時(shí)計(jì)算更新系統(tǒng)的四元數(shù)，實(shí)現(xiàn)系統(tǒng)的更新過程； 第三、以加速度計(jì)和磁傳感器的輸出作為參考，運(yùn)用kalman濾波技木，實(shí)現(xiàn)系統(tǒng)的修正過程； 第四、結(jié)合模糊控制理論，在系統(tǒng)中加入模糊控制模塊，實(shí)時(shí)估計(jì)系統(tǒng)的量測噪聲，實(shí)現(xiàn)自適應(yīng)kalman濾波。
2.根據(jù)權(quán)利要求I所述的提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法，其特征在于所述的粗對準(zhǔn)過程中，根據(jù)加速度計(jì)的輸出，確定系統(tǒng)的初始俯仰角和橫滾角；結(jié)合磁傳感器的輸出，確定初始航向角。根據(jù)初始姿態(tài)角，確定初始捷聯(lián)矩陣和初始四元數(shù)； 所述的系統(tǒng)更新過程中，四元數(shù)的更新采用龍格-庫塔算法，根據(jù)更新得到的四元數(shù)，更新捷聯(lián)矩陣和姿態(tài)角； 所述的系統(tǒng)修正過程中，四元數(shù)誤差矢量和陀螺儀的零偏誤差矢量作為狀態(tài)向量、以加速度誤差矢量和地磁誤差矢量作為觀測向量，確定kalman濾波的狀態(tài)方程和觀測方程，實(shí)時(shí)估計(jì)系統(tǒng)誤差，并將誤差反饋到系統(tǒng)中，修正四元數(shù)、捷聯(lián)矩陣和姿態(tài)角； 所述的模糊控制模塊中有兩個(gè)模糊控制器，在kalman濾波過程中，分別實(shí)時(shí)計(jì)算殘差向量中加速度部分的理論、實(shí)際方差和磁部分的理論、實(shí)際方差，分別以二者的理論方差與實(shí)際方差的比值作為兩個(gè)模糊控制器的輸入。
3.根據(jù)權(quán)利要求2所述的提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法，其特征在于所述的模糊控制器根據(jù)輸入，分別輸出修正系數(shù)修正加速度的量測噪聲和磁的量測噪聲，實(shí)現(xiàn)實(shí)時(shí)估計(jì)量測噪聲的作用。
全文摘要
一種提高航姿參考系統(tǒng)解算精度和系統(tǒng)抗干擾能力的方法，所述的航姿參考系統(tǒng)主要由微機(jī)械陀螺儀、微機(jī)械加速度計(jì)和地磁傳感器組成，用來實(shí)時(shí)測量運(yùn)動(dòng)載體的姿態(tài)信息；所述的方法具體包括以下步驟第一、根據(jù)加速度計(jì)和磁傳感器的輸出進(jìn)行系統(tǒng)的初始化，完成粗對準(zhǔn)過程；第二、根據(jù)陀螺儀的輸出，確定四元數(shù)的更新方程，實(shí)時(shí)計(jì)算更新系統(tǒng)的四元數(shù)，實(shí)現(xiàn)系統(tǒng)的更新過程；第三、以加速度計(jì)和磁傳感器的輸出作為參考，運(yùn)用kalman濾波技術(shù)，實(shí)現(xiàn)系統(tǒng)的修正過程；第四、結(jié)合模糊控制理論，在系統(tǒng)中加入模糊控制模塊，實(shí)時(shí)估計(jì)系統(tǒng)的量測噪聲，實(shí)現(xiàn)自適應(yīng)kalman濾波；本發(fā)明可以在提高航姿參考系統(tǒng)精度的同時(shí)，大大降低了系統(tǒng)的計(jì)算復(fù)雜度；尤其在系統(tǒng)受較大噪聲干擾時(shí)，該算法能夠正確估計(jì)量測噪聲特性，改善姿態(tài)精度，提高系統(tǒng)的穩(wěn)定性和可靠性。
文檔編號G01C21/18GK102654404SQ20111005508
公開日2012年9月5日 申請日期2011年3月2日 優(yōu)先權(quán)日2011年3月2日
發(fā)明者田易 申請人:浙江中科無線授時(shí)與定位研發(fā)中心