專利名稱：一種適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)的制作方法
技術領域：
本發(fā)明涉及一種適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)，適用于微小型 系統(tǒng)任意運動狀態(tài)下航向的確定。
背景技術：
任何運動體都需要高精度導航系統(tǒng)和控制系統(tǒng)，高精度導航系統(tǒng)為運動
體提供當前的位置、速度和姿態(tài)等運動信息；高精度控制系統(tǒng)依據(jù)導航系統(tǒng) 提供的運動信息控制運動體按照要求的方式運動，其中姿態(tài)信息的精度對控 制系統(tǒng)性能有決定性的影響。
大型運動體利用高精度、大體積、高成本慣性導航系統(tǒng)或者星敏感器已 經(jīng)實現(xiàn)了任意運動情況下的姿態(tài)確定。但大型運動體所用的定姿設備由于體 積龐大、成本極高而無法應用于微小型系統(tǒng)任意運動情況下的姿態(tài)確定。微 小型系統(tǒng)(微小飛行器、微小水下航行器、微小地面機器人)機動性大且隨 機性強，姿態(tài)確定的難度很大。
加速度計和傾角傳感器是目前測量微小型系統(tǒng)橫滾角和俯仰角最常用 的方式，但由于兩種傳感器均無法很好的區(qū)別微小型系統(tǒng)運動加速度和重力 加速度，因此不能給出微小型系統(tǒng)任意運動情況下的橫滾角和俯仰角，只適 合于靜止和勻速直線運動情況。而且上述兩種傳感器均不能給出航向角。
慣性導航系統(tǒng)原理上能夠測量微小型系統(tǒng)在任意運動狀態(tài)下的橫滾角、 俯仰角、航向角等三維姿態(tài)信息，但它需要初始姿態(tài)基準作為積分的初值， 而且微小型慣性導航系統(tǒng)的誤差隨時間急劇增加， 一般不能獨立工作。
磁傳感器能夠測量三個正交方向的磁場級度.，,為了換算為三維姿態(tài)信
息，還必須首先依賴其它手段，在微小型系統(tǒng)任意運動狀態(tài)下確定三維磁傳 感器測量方向與當?shù)厮矫嬷g的坐標轉換關系，即必須預先確定橫滾角和 俯仰角。目前多數(shù)磁傳感器定姿系統(tǒng)都依賴加速度計或傾角傳感器給出靜態(tài) 和勻速直線運動狀態(tài)下的4t滾角和俯仰角，而在其它才幾動運動狀態(tài)下^f茲傳感 器基本不可用。
可見，任何現(xiàn)有的姿態(tài)測量方式均無法準確確定微小型系統(tǒng)在任意運動 狀態(tài)下的三維姿態(tài)信息。本發(fā)明提出一種適用于任意運動微小型系統(tǒng)的
MIMU ( Micro Inertial Measure - ment Unit，微慣性測量單元)和GPS輔助 MMS (Micro Magnetic Sensors,微磁傳感器)定姿系統(tǒng)，利用GPS、 MIMU、 MMS之間迭代解算關系，不斷修正MIMU誤差并給出微小型系統(tǒng)任意運動 狀態(tài)下的三維姿態(tài)信息，具有廣闊的應用前景。

發(fā)明內(nèi)容
本發(fā)明的技術解決問題是克服現(xiàn)有技術的不足，提供一種適用于任意 運動微小型系統(tǒng)的定姿系統(tǒng)，該系統(tǒng)利用MIMU和GPS輔助的MMS定姿， 為任意運動狀態(tài)下的微小型系統(tǒng)提供高精度三維姿態(tài)信息。
本發(fā)明的技術解決方案是 一種適用于任意運動微小型系統(tǒng)的定姿系 統(tǒng)，其特征在于包括微磁傳感器MMS模塊、微慣性測量單元MIMU模塊、 GPS模塊和Kalman濾波模塊，在微小型系統(tǒng)任意運動狀態(tài)下，MIMU模塊 根據(jù)輸出數(shù)據(jù)通過捷聯(lián)導航算法分解出微小型系統(tǒng)重力加速度；MMS模塊 利用所述的重力加速度通過定姿算法實時解算微小型系統(tǒng)的橫滾角、俯仰角 和航向角三維姿態(tài)信息；同時，利用恒虛警CFAR濾波器平滑掉GPS輸出 速度和加速度信息中的噪聲；三維姿悉信息一方面送至Kalman濾波模塊， 與GPS給出的位置和速度信息一道通過信息融合算法間歇校正MIMU誤差， 另一方面利用三維姿態(tài)信息實時更新C/坐標轉換矩陣；坐標轉換矩陣 將被送給MIMU模塊和GPS模塊，將水平坐標系的信息轉換到載體坐標系
下,
本發(fā)明的原理是 一般的載體坐標系見圖2所示，定義為微小型系統(tǒng)縱 軸前進方向為Y軸，向上為Z軸，X軸依據(jù)右手定則確定。MIMU、 GPS、 MMS各自的三維正交測量方向均與載體坐標系三軸重合，見圖3所示，GPS 位于載體坐標系的原點，MMS、 MIMU、三個正交測量軸分別與載體坐標系 三軸重合。
MIMU沖莫塊由三維正交安裝的硅MEMS陀螺和加速度計構成，共六個 慣性器件。硅MEMS陀螺測量微小型系統(tǒng)角速度，硅MEMS加速度計測量 微小型系統(tǒng)線加速度。在靜止狀態(tài)下硅MEMS加速度計敏感重力加速度在 載體坐標系的投影，從而給出載體坐標系與當?shù)厮阶鴺讼档淖鴺宿D換矩陣 以及橫滾角、俯仰角；在其它任意運動狀態(tài)下，MIMU模塊的測量值經(jīng)過捷 聯(lián)導航算法，利用微小型系統(tǒng)運動線速度、微小型系統(tǒng)運動角速度以及地球 自轉角速度，實時分解加速度計測量值中微小型系統(tǒng)運動線加速度、哥式加 速度、向心加速度、重力加速度等不同成分。利用重力加速度在載體坐標系
<formula>formula see original document page 6</formula>以及重力加速度在當?shù)厮阶鴺讼低队癧O 0 g]之間
的關系，計算微小型系統(tǒng)的橫滾角y和俯仰角^，
<formula>formula see original document page 6</formula>MMS模塊由安裝于三個正交方向的微小型磁傳感器組成，測量當前位 置三個正交方向的磁場強度，從而給出當前位置磁場矢量的方向和大小。依 據(jù)橫滾角y和俯仰角P計算地磁場矢量在當?shù)厮阶鴺讼迪碌耐队?br> <formula>formula see original document page 6</formula>
信息融合算法利用當前時刻GPS給出的三維位置及速度倌息、MMS給 出的三維姿態(tài)信息，通過Kalman濾波器校正MIMU的導航誤差和慣性器件 誤差，提高重力加速度在載體坐標系的計算精度。
本發(fā)明與現(xiàn)有技術相比的優(yōu)點在于(1 ) MIMU模塊結合捷聯(lián)導航算 法不斷實時分解出加速度計測量值中重力加速度的成分，并給出重力加速度 在載體坐標系的投影，解決了由于加速度計測量值無法分辨運動加速度和重 力加速度的問題，從而得到載體坐標系與當?shù)厮阶鴺讼抵g的轉換關系， 即橫滾角和俯仰角；(2)采用CFAR濾波器抑制GPS輸出數(shù)據(jù)的噪聲，提 高了微小型系統(tǒng)位置、速度的測量精度。(3)本發(fā)明采用GPS三維位置、 速度信息以及MMS給出的三維姿態(tài)信息不斷校正MIMU的誤差，保證了長 時間工作情況下MIMU測量精度以及分解重力加速度在載體坐標系投影的 精度；(4)MMS模塊結合定姿算法能夠測量載體坐標系三維方向磁場強度， 利用已獲得的橫滾角和俯仰角，循環(huán)迭代計算微小型系統(tǒng)任意運動狀態(tài)下的 三維姿態(tài)信息。


圖1為本發(fā)明的適用于任意運動微小型系統(tǒng)的MIMU和GPS輔助MMS 定姿系統(tǒng)組成框圖2為載體坐標系示意圖3為本發(fā)明的適用于任意運動微小型系統(tǒng)的MIMU和GPS輔助MMS 定姿系統(tǒng)在載體坐標系的安裝方法；
圖4為本發(fā)明的適用于任意運動微小型系統(tǒng)的MIMU和GPS輔助MMS 定姿系統(tǒng)工作流程圖。 '
具體實施例方式
圖1為適用于任意運動微小型系統(tǒng)的MIMU (微慣性測量單元)和GPS
輔助MMS (微磁傳感器)定姿系統(tǒng)組成框圖，包括硬件和軟件兩部分，硬 件部分由MMS模塊、MIMU模塊、GPS模塊組成；軟件算法部分包括捷聯(lián) 導航、信息融合以及定姿算法。在微小型系統(tǒng)任意運動狀態(tài)下，利用MIMU 模塊的數(shù)據(jù)和捷聯(lián)導航算法分解出微小型系統(tǒng)重力加速度，MMS模塊利用 重力加速度和定姿算法實時解算微小型系統(tǒng)的橫滾角、俯仰角和航向角，采 用CFAR (恒虛警)濾波器平滑GPS輸出噪聲，由GPS模塊、三維姿態(tài)信 息以及信息融合算法間歇性的校正MIMU誤差，提高重力加速度和姿態(tài)解 算精度。圖4描述了圖1中捷聯(lián)導航算法、定姿算法、信息融合算法基于硬 件輸出的數(shù)據(jù)確定姿態(tài)的工作流程。
首先，MIMU和GPS輔助的MMS定姿系統(tǒng)在靜止狀態(tài)下開始工作，由
MIMU模塊中加速度計給出重力加速度在載體坐標系的投影<formula>formula see original document page 8</formula>
已知重力加速度在當?shù)厮阶鴺讼档耐队盀?lt;formula>formula see original document page 8</formula>
<formula>formula see original document page 8</formula>地磁場在水平坐標系下的投影<formula>formula see original document page 8</formula>
由于地磁場的磁力線方向總是指向磁北的，即/C/Zf的合向量指向磁北，
》茲;杭向為
再用f減去地磁偏角/1,就是數(shù)字磁羅盤與真北的夾角即航向角
被發(fā)起.這樣的接口有利地獨立于要傳遞的工件的位置的數(shù)量.所
述接口可以是串行打印機接口 /特別是RS-232/422接口 ，或是并行打 印機接口 ，特別是Centronics接口 .
關于通過機床機械加工的狀態(tài)的信息有利地包含用于工件和/或 工具的傳遞指令.這樣的傳遞指令優(yōu)選地包含工件和/或工具的箱位 置號和定位位置、和/或傳遞裝置的回轉(pivoting)動作，這種信息 是對于在機床處交換部件所必需的信息.
尤其，在根據(jù)本發(fā)明的系統(tǒng)中，同步化裝置附加地被提供用于 對機床和傳遞裝置協(xié)調和步進計時.在所有情況下，所述同步化裝 置優(yōu)選地均包含機床和傳遞裝置的數(shù)字輸入/輸出端。尤其，所述同 步化裝置也被用于分別響應于所發(fā)出的信息和所發(fā)出的傳遞指令而 反饋握手(handshaking);這是必需的，因為打印機接口是單向的。 類似于打印機接口 ，數(shù)字輸入/輸出端通常已經(jīng)存在于機床中并且允 許來自控制單元的信號被送出或者被讀入到所述控制單元中.
機床的打印機接口的初始化優(yōu)選地通過加栽用于發(fā)布信息到傳 遞裝置的宏來執(zhí)行； 一旦接收到信息，傳遞裝置的控制單元就通過 激活相應的協(xié)議來起作用.正如先前所提到的，如栽宏可以簡單地 由機床的操作員通過對適當?shù)腎SO代碼編程來執(zhí)行.
尤其，所述傳遞裝置作為對其控制單元已經(jīng)從機床的打印機接 口接收到了交換指令的響應而執(zhí)行對工件或工具的交換.
此外，可以規(guī)定，所述機床通過發(fā)送相應的信號到其數(shù)字輸出 端來用信號通知工件和/或工具的交換準備就緒；所述信號被饋入 傳遞裝置的數(shù)字輸入端并且一旦所述機床不再準備交換就立即被重 置。然而，也可以規(guī)定所述機床的控制單元生成請求交換工件或工 具的循環(huán)重復信號，所述信號被發(fā)送到打印機接口.通過這些特征， 可以保證， 一旦所述機床不再準備用于交換工件或工具，所述交換 指令就被清除，因此，提高了所述機床以及所述傳遞裝置的安全性.
優(yōu)選地，所述機床的控制單元與校驗和一起發(fā)出交換信號到所 述打印機接口.通過所述校驗和，保證所述所傳輸?shù)男畔⒌臄?shù)據(jù)完 整性.
在所述機床側，提供包舍所述機床的控制單元、打印機接口和 數(shù)字輸入端/輸出端的控制架，而在所述傳遞裝置側，提供包含所述
—w —"<formula>formula see original document page 10</formula>
MIMU模塊實時連續(xù)輸出陀螺、加速度計測量值，并將其輸入捷聯(lián)導 航解算，求取載體坐標系下的重力加速度<formula>formula see original document page 10</formula>
上式各項均為矢量，/6為加速度計直接測量值(直接測量值在載體坐標系 內(nèi))；假設￡為GPS給出的煒度，當?shù)氐乩碜鴺讼祪?nèi)地球自轉角速度cw:可 分解為
<formula>formula see original document page 10</formula>當?shù)氐乩碜鴺讼祪?nèi)微小型系統(tǒng)運動產(chǎn)生的相對地球坐標系的角速度為
<formula>formula see original document page 10</formula>為利用航向角信息分解GPS速度獲得的微小型系統(tǒng)在當?shù)氐乩碜鴺?系內(nèi)的北向速度和東向速度信息，,及力分別為地球的子午面半徑和卯酉面 半徑。
得到后，即可采用與靜止狀態(tài)相同的算法計算微小
型系統(tǒng)橫滾角、俯仰角和航向角。
信息融合利用當前獲得的三維速度、三維位置和三維姿態(tài)信息，采用標
準Kalman濾波器校正MIMU的誤差。Kalman濾波器狀態(tài)變量
<formula>formula see original document page 11</formula>
Kalman濾波器觀測變量是上文獲得的三維速度、三維位置和三維姿態(tài)信息 與MIMU輸出的三維速度、三維位置和三維姿態(tài)信息的差值。<formula>formula see original document page 11</formula>根據(jù)慣性導航標準誤差方程建立Kalman濾波器的狀態(tài)方程與觀測方程，其 余計算與標準Kalman濾波器相同。
權利要求
1、一種適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)，其特征在于包括微磁傳感器MMS模塊、微慣性測量單元MIMU模塊、GPS模塊和Kalman濾波模塊，在微小型系統(tǒng)任意運動狀態(tài)下，MIMU模塊根據(jù)輸出數(shù)據(jù)通過捷聯(lián)導航算法分解出微小型系統(tǒng)重力加速度；MMS模塊利用所述的重力加速度通過定姿算法實時解算微小型系統(tǒng)的橫滾角、俯仰角和航向角三維姿態(tài)信息；同時，利用恒虛警CFAR濾波器平滑掉GPS輸出速度和加速度信息中的噪聲；三維姿態(tài)信息一方面送至Kalman濾波模塊，與GPS給出的位置和速度信息一道通過信息融合算法間歇校正MIMU誤差，另一方面利用三維姿態(tài)信息實時更新Cnb坐標轉換矩陣；坐標轉換矩陣Cnb將被送給MIMU模塊和GPS模塊，將水平坐標系的信息轉換到載體坐標系下。
2、 根據(jù)權利要求1所述的適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)，其 特征在于所述的MMS模塊由安裝于三個正交方向的微小型磁傳感器組成， 測量當前位置三個正交方向的地球石茲場強度，從而給出當前位置地球石茲場矢 量的方向和大小。
3、 根據(jù)權利要求1所述的適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)，其 特征在于所述的MIMU模塊中的硅MEMS陀螺測量微小型系統(tǒng)角速度， 硅MEMS加速度計測量微小型系統(tǒng)線加速度；在靜止狀態(tài)下，硅MEMS加 速度計敏感重力加速度在三維正交方向的投影，給出載體坐標系與當?shù)厮?坐標系的坐標轉換矩陣以及橫滾角、俯仰角。
4、 根據(jù)權利要求1所述的適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)，其 特征在于所述的CFAR濾波器釆用滑動平均的方法抑制GPS輸出數(shù)據(jù)的 噪聲，根據(jù)機動性設置滑動窗的時間長度r-MAf ， M為滑動個數(shù)，At為時 間間隙，采用先進先出的方式確定滑動窗內(nèi)參與平均的數(shù)據(jù)，利用當前時刻 之前r時段內(nèi)數(shù)據(jù)經(jīng)過CFAR濾波器抑制GPS數(shù)據(jù)的噪聲。
5、 根據(jù)權利要求I所述的適用于任意運動微小型系統(tǒng)的定姿泉統(tǒng)，其 特征在于所述的捷聯(lián)導航算法在載體坐標系下建立基本力學編排方程，從 而從加速度計測量值中分解出重力加速度在載體坐標系三軸的投影。6、 根據(jù)權利要求1所述的適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)，其 特征在于所述的MMS模塊利用所述的重力加速度通過定姿算法實時解算 微小型系統(tǒng)的橫滾角、俯仰角和航向角三維姿態(tài)信息的過程如下利用重力加速度在載體坐標系的投影n尸Sjc gy Sz以及重力加速度在當?shù)厮阶鴺讼档耐队癧O 0 gf之間的關系，計算微小型系統(tǒng)的橫滾角z和俯仰角P,<formula>formula see original document page 3</formula>依據(jù)y和P計算當前地磁場矢量在水平坐標系下的投影:《=sin 6> sin y +《cos 6 -《sin 0 cos y 考慮磁偏角丄的情況下計算航向角- :
全文摘要
一種適用于任意運動微小型系統(tǒng)的定姿系統(tǒng)，其特征在于包括微磁傳感器MMS模塊、微慣性測量單元MIMU模塊、GPS模塊和Kalman濾波模塊，在微小型系統(tǒng)任意運動狀態(tài)下，MIMU模塊根據(jù)輸出數(shù)據(jù)通過捷聯(lián)導航算法分解出微小型系統(tǒng)重力加速度；MMS模塊利用所述的重力加速度通過定姿算法實時解算微小型系統(tǒng)的橫滾角、俯仰角和航向角三維姿態(tài)信息；同時，利用恒虛警CFAR濾波器平滑掉GPS輸出速度和加速度信息中的噪聲；三維姿態(tài)信息一方面送至Kalman濾波模塊，與GPS給出的位置和速度信息一道通過信息融合算法間歇校正MIMU誤差，另一方面利用三維姿態(tài)信息實時更新C_n^b坐標轉換矩陣；坐標轉換矩陣C_n^b將被送給MIMU模塊和GPS模塊，將水平坐標系的信息轉換到載體坐標系下。本發(fā)明利用了GPS、MIMU、MMS之間的迭代解算關系，解決了任意運動狀態(tài)下確定運動體航向的難題，具有廣闊的應用前景。
文檔編號G05D1/08GK101109959SQ200710119968
公開日2008年1月23日 申請日期2007年8月6日 優(yōu)先權日2007年8月6日
發(fā)明者房建成, 曹娟娟, 蔚 盛, 杰 秦, 馬珍珍, 馬艷武 申請人:北京航空航天大學