基于mems慣導的雙四元數動中通天線控制方法及系統的制作方法
【專利摘要】基于MEMS慣導的雙四元數動中通天線控制方法，在導航姿態四元數基礎上引入天線控制四元數。在導航計算機的每個中斷周期，都用陀螺測得的載體系相對于理想平臺坐標系的旋轉矢量更新兩種四元數。在每個濾波周期都用卡爾曼濾波修正導航姿態四元數的誤差。根據由兩種姿態四元數所確定的姿態之間的關系，確定天線控制指令角速度。最后由天線控制四元數姿態換算出的天線伺服控制角驅動伺服系統轉動。基于MEMS慣導的雙四元數動中通天線控制系統，以所述控制方法為控制流程，可以實現對動中通天線指向的精確控制。本發明可以有效避免卡爾曼濾波暫態過程中導航姿態四元數不穩定對動中通天線伺服系統帶來的沖擊，有效縮短動中通系統的對星時間。
【專利說明】基于MEMS慣導的雙四元數動中通天線控制方法及系統

【技術領域】
[0001] 本發明涉及一種動中通天線控制方法。

【背景技術】
[0002] 同步衛星的移動通信應用系統俗稱"動中通"，是當前衛星通信領域需求旺盛、發 展迅速的應用。"動中通"除了具有衛星通信覆蓋區域廣、不受地形地域限制、傳輸線路穩定 可靠的優點外，真正實現了寬帶、移動通信的目的。
[0003] 目前，國內基于MEMS慣性導航（簡稱為MEMS慣導）的動中通系統尚且沒有較為 成熟的方案。隨著MEMS慣性技術的發展，MEMS慣導的精度穩步提升。目前國內動中通系 統應用越來越廣泛，市場對降低動中通系統的成本提出了迫切需求。因此研究MEMS慣導對 動中通天線伺服系統的控制方法具有十分重要的意義。
[0004] 通常的做法，是利用由捷聯慣導解算的導航姿態四元數直接控制天線，在卡爾曼 濾波組合導航過程中，狀態估計若得到較大的誤差估計量，那么在修正估計量的同時，會通 過給伺服系統瞬間輸入非常大的電流來驅動伺服系統瞬間轉動一個較大的角度（角度大 小為卡爾曼濾波估計得到的誤差估計量），這樣勢必會對天線伺服系統造成較大的電學和 力學沖擊。為了避免這種沖擊，只能花費大量時間等待卡爾曼濾波穩定，直到伺服系統能夠 承受由卡爾曼濾波估計的到的誤差修正量所帶來的沖擊時再啟動動中通伺服系統，顯然這 是有悖于市場對動中通的快速對星需求的。


【發明內容】

[0005] 本發明的技術解決問題是：克服現有技術的不足，提供了一種基于MEMS慣導的雙 四元數動中通天線控制方法及系統，通過在傳統導航姿態四元數的基礎上引入了新的天線 控制四元數，從而隔離了卡爾曼濾波修正姿態誤差時對動中通天線伺服系統帶來的沖擊， 可以顯著縮短動中通系統的對星時間。
[0006] 本發明的技術解決方案是：一種基于MEMS慣導的雙四元數動中通天線控制方法， 包括如下步驟：
[0007] (1)在載體上同時安裝MEMS慣導、GPS和動中通，其中MEMS慣導和GPS構成組合 導航系統；
[0008] (2)設定天線控制四元數，天線控制四元數的形式為[q' ^ q' i q' 2 q' 3]，天線控制四 元數中每個參數的含義與捷聯慣導解算中獲取的導航姿態四元數[% qi q2 q3]對應一致，天 線控制四元數的初值與導航姿態四元數相同；
[0009] (3)在捷聯慣導導航計算機的每個中斷周期里，用載體系相對于理想平臺坐標系 的旋轉矢量，分別更新導航姿態四元數和天線控制四元數；
[0010] (4)在所述組合導航系統的每個濾波周期內，利用卡爾曼濾波組合導航算法修正 MEMS慣導的導航姿態中的水平姿態誤差，從而修正導航姿態四元數；
[0011] (5)在捷聯慣導導航計算機的每個中斷周期里，將由導航姿態四元數確定的載體 姿態角與由天線控制四元數確定的載體姿態角對應相減，得到姿態角差值，并根據姿態角 差值產生用于校正天線控制四元數的三軸指令角速度旋轉矢量，具體為：
[0012] a.若由天線控制四元數確定的航向角大于由導航姿態四元數確定的航向角，則三 軸指令角速度旋轉矢量的第三個元素取正的修正指令角速度；
[0013] b.若由天線控制四元數確定的航向角小于由導航姿態四元數確定的航向角，則三 軸指令角速度旋轉矢量的第三個元素取負的修正指令角速度；
[0014] c.若由天線控制四元數確定的俯仰角大于由導航姿態四元數確定的俯仰角，則三 軸指令角速度旋轉矢量的第一個元素取正的修正指令角速度；
[0015] d.若由天線控制四元數確定的俯仰角小于由導航姿態四元數確定的俯仰角，則三 軸指令角速度旋轉矢量的第一個元素取負的修正指令角速度；
[0016] e.若由天線控制四元數確定的橫滾角大于由導航姿態四元數確定的橫滾角，則三 軸指令角速度旋轉矢量的第二個元素取正的修正指令角速度；
[0017] f.若由天線控制四元數確定的橫滾角小于由導航姿態四元數確定的橫滾角，則三 軸指令角速度旋轉矢量的第二個元素取負的修正指令角速度；
[0018] (6)利用三軸指令角速度旋轉矢量校正天線控制四元數，并在校正以后的下一個 捷聯慣導導航計算機的中斷周期，利用校正后的天線控制四元數，解算得到動中通天線的 伺服方位角、伺服仰角和伺服極化角，由此獲得三個姿態方向所對應的控制量控制動中通 天線轉動。
[0019] GPS:測量獲取載體的速度和位置信息并送至動中通天線控制器中的濾波單元；
[0020] MEMS陀螺：測量獲取載體在三維空間內的角速度信息并送至動中通天線控制器 中的慣導解算單元和天線控制四元數計算單元；
[0021] MEMS加速度計：測量獲取載體在三維空間內的比力信息并送至動中通天線控制 器中的慣導解算單元；
[0022] 動中通天線控制器：包括慣導解算單元、濾波單元、天線控制四元數計算單元、天 線控制指令生成單元、天線控制四元數校正指令角速度生成單元，其中 ：
[0023] 慣導解算單元：將MEMS陀螺測量獲取的載體在三維空間內的角速度信息，扣除由 地球自轉、載體沿地球表面運動帶來的角速度后，得到載體坐標系相對于地理坐標系的三 軸旋轉矢量 ;將MEMS加速度計測量獲取的載體在三維空間內的比力信息，扣除重力加速 度、哥氏加速度后，得到載體的對地加速度；利用載體坐標系相對于地理坐標系的三軸旋轉 角速度和載體的對地加速度，經過慣導解算得到載體的姿態、位置和速度信息并送至濾波 單元；將載體坐標系相對于地理坐標系的三軸旋轉矢量ΛΛ,.以及首次慣導解算直接得到的 載體姿態所對應的姿態四元數[q0 q2 q3]送至天線控制四元數計算單元；從濾波單元獲取 修正后的載體姿態信息，利用載體坐標系相對于地理坐標系的三軸旋轉矢量更新與修 正后的載體姿態信息所對應的姿態四元數作為導航姿態四元數送至天線控制四元數校正 指令角速度生成單元；
[0024] 濾波單元：利用GPS輸出的載體速度和位置信息，以及慣導解算單元輸出的載體 速度和位置信息，通過卡爾曼濾波組合導航算法，以固定的濾波周期修正慣導解算單元輸 出載體姿態中的水平姿態誤差并將修正后的結果送至慣導解算單元；
[0025] 天線控制四元數計算單元：生成天線控制四元數，所述的天線控制四元數的形式 為[q' ^ q' i q' 2 q' 3]，天線控制四元數中每個參數的含義與慣導解算單元獲取的姿態四元數 [qQ qi q2 q3]對應一致，且[q' Q q' i q' 2 q' 3]的初值為[qQ qi q2 q3];從慣導解算單元每接收到 一次載體坐標系相對于地理坐標系的三軸旋轉矢量狀，就利用載體坐標系相對于地理坐 標系的三軸旋轉矢量以更新天線控制四元數[q' 0 q' i q' 2 q' 3]并送至天線控制四元數校正 指令角速度生成單元；從天線控制四元數校正指令角速度生成單元獲取三軸指令角速度旋 轉矢量，并用所述三軸指令角速度旋轉矢量再次更新天線控制四元數[q' 0 q' i q' 2 3]并送 至天線控制指令生成單元；
[0026] 天線控制四元數校正指令角速度生成單元：分別從慣導解算單元和天線控制四元 數計算單元獲取導航姿態四元數和天線控制四元數，將由導航姿態四元數確定的載體姿態 角與由天線控制四元數確定的載體姿態角對應相減，得到姿態角差值，并根據姿態角差值 生成用于校正天線控制四元數的三軸指令角速度旋轉矢量并送至天線控制四元數計算單 元，三軸指令角速度旋轉矢量中各元素的取值方法如下：
[0027] a.若由天線控制四元數確定的航向角大于由導航姿態四元數確定的航向角，則三 軸指令角速度旋轉矢量的第三個元素取正的修正指令角速度；
[0028] b.若由天線控制四元數確定的航向角小于由導航姿態四元數確定的航向角，則三 軸指令角速度旋轉矢量的第三個元素取負的修正指令角速度；
[0029] c.若由天線控制四元數確定的俯仰角大于由導航姿態四元數確定的俯仰角，則三 軸指令角速度旋轉矢量的第一個元素取正的修正指令角速度；
[0030] d.若由天線控制四元數確定的俯仰角小于由導航姿態四元數確定的俯仰角，則三 軸指令角速度旋轉矢量的第一個元素取負的修正指令角速度；
[0031] e.若由天線控制四元數確定的橫滾角大于由導航姿態四元數確定的橫滾角，則三 軸指令角速度旋轉矢量的第二個元素取正的修正指令角速度；
[0032] f.若由天線控制四元數確定的橫滾角小于由導航姿態四元數確定的橫滾角，則三 軸指令角速度旋轉矢量的第二個元素取負的修正指令角速度；
[0033] 天線控制指令生成單元：從天線控制四元數計算單元接收最新的天線控制四元 數，根據天線控制四元數解算得到動中通天線的伺服方位角、伺服仰角和伺服極化角，送至 動中通天線伺服機構；
[0034] 動中通天線伺服機構：包括方位向、俯仰向和極化向的電機驅動器和相應的電機， 三個方向的電機驅動器根據天線控制指令生成單元傳來的伺服方位角、伺服仰角和伺服極 化角分別驅動相應方向的電機，由此控制動中通天線的三軸轉動。
[0035] 所述的修正指令角速度，在a和b兩種情況下，大小至少是天線控制四元數確定的 航向角與導航姿態四元數確定的航向角之差再除以組合導航濾波周期，并且不大于動中通 天線每秒所允許的最大對星角度誤差；在c和d兩種情況下，大小至少是天線控制四元數確 定的俯仰角與導航姿態四元數確定的俯仰角之差再除以組合導航濾波周期，并且不大于動 中通天線每秒所允許的最大對星角度誤差；在e和f兩種情況下，大小至少是天線控制四元 數確定的橫滾角與導航姿態四元數確定的橫滾角之差再除以組合導航濾波周期，并且不大 于動中通天線每秒所允許的最大對星角度誤差。
[0036] 本發明與現有技術相比的優點在于：
[0037] (1)本發明方法中引入了天線控制四元數。由于組合導航算法對導航姿態四元數 的誤差修正使得由導航姿態四元數解算得到的姿態產生跳躍，這種跳躍將會對天線伺服系 統帶來沖擊。獨立于導航姿態四元數的天線控制四元數隔離了組合導航誤差修正時導航姿 態四元數震蕩所導致的天線伺服系統震蕩，從而保證了在組合導航算法對慣導誤差進行修 正時，天線伺服系統的平穩運行；
[0038] (2)本發明方法中，通過利用天線控制四元數以較小的指令角速度逼近導航姿態 四元數，初始時刻可用導航姿態四元數初值直接初始化天線控制四元數，天線控制四元數 被初始化后可立刻驅動天線完成對星功能；天線控制四元數初始化完成后，即可由天線控 制四元數獨立控制天線伺服系統平滑的轉動，隔離導航姿態誤差修正對天線伺服系統帶來 的沖擊，從而使天線始終穩定的對準衛星；
[0039] (3)本發明系統中，慣導解算單元采集MEMS陀螺和MEMS加表測量的載體轉動角速 率信息和加速度信息，完成慣導解算；濾波單元通過采集GPS的速度信息和位置信息完成 對慣導解算的誤差修正；天線控制四元數計算單元隔離了濾波單元對慣導解算進行誤差修 正時對天線伺服系統帶來的沖擊；天線控制四元數校正指令角速度生成單元通過對導航姿 態四元數對應的姿態和天線控制四元數對應的姿態進行比較，產生了用于修正天線控制四 元數的三軸指令角速度旋轉矢量；天線控制指令生成單元通過實時解算天線控制四元數對 應的姿態來進一步計算動中通天線伺服系統所需要的伺服方位角、伺服仰角、伺服極化角， 進而通過動中通天線伺服機構完成對動中通天線的實時控制；實現了動中通天線的實時、 平滑、穩定控制，提高了對星精度。

【專利附圖】

【附圖說明】
[0040] 圖1為本發明方法的原理框圖；
[0041] 圖2為天線指向偏差隨時間變化的曲線示意圖；
[0042] 圖3為本發明系統的組成原理框圖。

【具體實施方式】
[0043] 如圖1所示，為本發明方法的原理圖。本發明方法采用導航姿態四元數和天線控 制四元數協同控制載體上的動中通的天線伺服系統。
[0044] 本發明方法中引入天線控制四元數的概念。天線控制四元數的形式為[q' ^ q' i q' 2 q' 3]，每個參數的含義與捷聯慣導解算中的四元數[q0 q2 q3]對應一致。在導航初始時刻， 天線控制四元數與導航姿態四元數相等。天線控制四元數在每個導航周期需要經歷兩次更 新，一次由載體系b相對于理想平臺系T的旋轉矢量在載體系b下旋轉矢量更新，一次由恒 定的三軸小指令角速度更新。第一次更新用于跟蹤載體姿態變化。第二次更新的目的是使 天線控制四元數虛擬的數學平臺以很小的角速度追趕導航姿態四元數虛擬的數學平臺，而 不因瞬間較大幅度地修正導航姿態四元數使得天線伺服姿態角發生劇烈變化。
[0045] 天線控制四元數和導航姿態四元數的相同之處是它們都用同樣的旋轉矢量更新 來跟蹤載體姿態變化。它們的不同之處是導航姿態四元數用卡爾曼濾波在到達濾波時間時 一次性修正估計誤差（會沖擊天線伺服系統），而天線控制四元數則以較小的指令角速度 緩慢逼近導航姿態四元數（不會沖擊伺服系統）。
[0046] 如圖2所示，是天線指向偏差隨時間變化的曲線示意圖。實線AEBGCID表示假設 由導航姿態四元數控制天線轉動時，天線指向誤差曲線。虛線AEFGHI表示由天線控制四元 數控制天線轉動時，天線的指向偏差曲線。AB、BC、CD分別表示組合導航算法對導航姿態四 元數的誤差修正周期。在A點處，天線控制四元數和導航姿態四元數同時初始化。AE段實 線虛線重合表示用相同的旋轉矢量同時更新導航姿態四元數和天線控制四元數，由于 導航姿態四元數和天線控制四元數初值相同，因此在ΑΒ段內，二者始終相等，所以ΑΕ段實 線和虛線重合。EB(GC、ID)段實線表示組合導航算法對導航姿態四元數進行修正，導航姿 態誤差歸零，由導航姿態四元數控制的天線指向誤差由非零值瞬間變為0,對天線伺服系統 帶來震蕩。BG(CI)段實線表示隨著時間推移和慣性器件誤差累積，天線指向偏差逐漸增大。 EF(GH)虛線表示天線控制四元數姿態以較小的指令角速度緩慢追趕導航姿態四元數的過 程。FG(HI)虛線表示天線控制四元數姿態與導航姿態四元數相等后，二者以相同的旋轉矢 量更新，二者對應的天線指向誤差發展狀況相同。
[0047] 本發明方法的主要步驟如下：
[0048] (1)設置天線控制四元數的初值。因獨立設計天線控制四元數的目的是隔離載體 姿態誤差修正時對動中通伺服系統帶來的力學和電學沖擊，天線控制四元數實際上和導航 姿態四元數一樣描述了載體姿態，因此在系統啟動時刻，可使其與捷聯慣導解算四元數法 中獲取的導航姿態四元數相同。
[0049] (2)在捷聯慣導的導航計算機的每個中斷周期里，用載體系相對于理想平臺坐標 系的旋轉矢量，分別更新導航姿態四元數和天線控制四元數。
[0050] (3)在每個濾波周期內，利用卡爾曼濾波修正MEMS慣導的導航姿態四元數，速度 和位置，從而保證MEMS慣導的長時間導航精度。
[0051] (4)在每個中斷周期，將由導航姿態四元數確定的載體姿態角與由天線控制四元 數確定的載體姿態角對應相減，得到姿態角的差值；
[0052] (5)在每個中斷周期，根據姿態角的差值確定天線控制方式；基本原則是以天線 控制四元數逼近導航姿態四元數，在此引入用于校正天線控制四元數的三軸指令角速度旋 轉矢量，分為如下幾種情況 ：
[0053] a.若由天線控制四元數確定的航向角大于由導航姿態四元數確定的航向角，則三 軸指令角速度旋轉矢量的第三個元素取正的修正指令角速度；
[0054] b.若由天線控制四元數確定的航向角小于由導航姿態四元數確定的航向角，則三 軸指令角速度旋轉矢量的第三個元素取負的修正指令角速度；
[0055] 修正指令角速度的大小取值方法為：為了使修正天線控制四元數帶給天線伺服系 統的沖擊量達到最小，三軸指令角速度旋轉矢量的第三個元素的修正指令角速度的大小為 天線控制四元數航向角與導航姿態四元數航向角之差除以組合導航濾波周期，當該指令角 速度的大小為天線控制四元數航向角與導航姿態四元數航向角之差除以組合導航濾波周 期時，該指令角速度恰好使天線控制四元數的航向角誤差與導航姿態四元數的航向角誤差 最大值相等。考慮到系統運行中的不確定因素，應使該指令角速度大于天線控制四元數航 向角與導航姿態四元數航向角之差除以組合導航濾波周期，但其最大值不應超過每秒動中 通天線允許的最大對星角度誤差。
[0056] c.若由天線控制四元數確定的俯仰角大于由導航姿態四元數確定的俯仰角，則三 軸指令角速度旋轉矢量的第一個元素取正的修正指令角速度；
[0057] d.若由天線控制四元數確定的俯仰角小于由導航姿態四元數確定的俯仰角，則三 軸指令角速度旋轉矢量的第一個元素取負的修正指令角速度；
[0058] 上述兩種情況下修正指令角速度的大小取值方法與a，b兩種情況原理相同，不同 的是由于是第一個元素，與其對應的為俯仰角，因此應將a，b兩種情況對應的航向角替換 為俯仰角進行計算即可。
[0059] e.若由天線控制四元數確定的橫滾角大于由導航姿態四元數確定的橫滾角，則三 軸指令角速度旋轉矢量的第二個元素取正的修正指令角速度；
[0060] f.若由天線控制四元數確定的橫滾角小于由導航姿態四元數確定的橫滾角，則三 軸指令角速度旋轉矢量的第二個元素取負的修正指令角速度；
[0061] 上述兩種情況下修正指令角速度的大小取值方法與a，b兩種情況原理相同，不同 的是由于是第二個元素，與其對應的為橫滾角，因此應將a，b兩種情況對應的航向角替換 為橫滾角進行計算即可。
[0062] (6)根據天線控制四元數對應的姿態角計算動中通天線的伺服方位角、伺服仰角、 伺服極化角，驅動動中通伺服系統對天線進行控制。
[0063] 如圖3所示，為本發明基于MEMS慣導的雙四元數動中通天線控制系統的組成原理 圖。主要包括：動中通天線控制器、GPS、MEMS陀螺、MEMS加速度計和動中通天線伺服機構。
[0064] GPS主要是測量獲取載體的速度和位置信息并送至動中通天線控制器中的濾波單 J Li 〇
[0065] MEMS陀螺主要是測量獲取載體在三維空間內的角速度信息并送至動中通天線控 制器中的慣導解算單元和天線控制四元數計算單元。
[0066] MEMS加速度計主要是測量獲取載體在三維空間內的比力信息并送至動中通天線 控制器中的慣導解算單元。
[0067] 動中通天線伺服機構：包括方位向、俯仰向和極化向的電機驅動器和相應的電機， 三個方向的電機驅動器根據天線控制指令生成單元傳來的伺服方位角、伺服仰角和伺服極 化角分別驅動相應方向的電機，由此控制動中通天線的三軸轉動。
[0068] 動中通天線控制器是本發明系統的核心部分，主要包括慣導解算單元、濾波單元、 天線控制四元數計算單元、天線控制指令生成單元、天線控制四元數校正指令角速度生成 單元，其中：
[0069] 慣導解算單元：將MEMS陀螺測量獲取的載體在三維空間內的角速度信息，扣除由 地球自轉、載體沿地球表面運動帶來的角速度后，得到載體坐標系相對于地理坐標系的三 軸旋轉矢量將MEMS加速度計測量獲取的載體在三維空間內的比力信息，扣除重力加速 度、哥氏加速度后，得到載體的對地加速度；利用載體坐標系相對于地理坐標系的三軸旋轉 角速度和載體的對地加速度，經過慣導解算得到載體的姿態、位置和速度信息并送至濾波 單元；將載體坐標系相對于地理坐標系的三軸旋轉矢量以及首次慣導解算直接得到的 載體姿態所對應的姿態四元數[q0 q2 q3]送至天線控制四元數計算單元；從濾波單元獲取 修正后的載體姿態信息，利用載體坐標系相對于地理坐標系的三軸旋轉矢量ΛΛ更新與修 正后的載體姿態信息所對應的姿態四元數作為導航姿態四元數送至天線控制四元數校正 指令角速度生成單元。
[0070] 濾波單元：將GPS輸出的載體速度和位置信息，以及慣導解算單元輸出的載體速 度和位置信息進行組合，將GPS獲取的載體的東向速度和北向速度，以及慣導解算單元輸 出的載體的東向速度和北向速度分別做差，將兩個差值構成卡爾曼濾波的量測量，通過卡 爾曼濾波組合導航算法，以固定的濾波周期修正慣導解算單元輸出的載體姿態信息并將修 正后的結果送至慣導解算單元。卡爾曼濾波組合導航算法具體可參見2012年西北工業 大學出版社出版的，由秦永元、張洪鉞、王淑華編著的《卡爾曼濾波與組合導航原理（第二 版）》一書。本發明中，選取其中東向和北向速度誤差、東向和北向失準角、右向和前向陀螺 的漂移，以及右向和前向加速度計的偏置量這八個量作為狀態變量。
[0071] 天線控制四元數計算單元：生成天線控制四元數，所述的天線控制四元數的形式 為[q' ^ q' i q' 2 q' 3]，天線控制四元數中每個參數的含義與慣導解算單元獲取的姿態四元數 [qQ qi q2 q3]對應一致，且[q' Q q' i q' 2 q' 3]的初值為[qQ qi q2 q3];從慣導解算單元每接收到 一次載體坐標系相對于地理坐標系的三軸旋轉矢量就利用載體坐標系相對于地理坐 標系的三軸旋轉矢量更新天線控制四元數[q' 0 q' i q' 2 q' 3]并送至天線控制四元數校正 指令角速度生成單元；從天線控制四元數校正指令角速度生成單元獲取三軸指令角速度旋 轉矢量，并用所述三軸指令角速度旋轉矢量再次更新天線控制四元數[q' 0 q' i q' 2 3]并送 至天線控制指令生成單元。
[0072] 天線控制四元數校正指令角速度生成單元：分別從慣導解算單元和天線控制四元 數計算單元獲取導航姿態四元數和天線控制四元數，將由導航姿態四元數確定的載體姿態 角與由天線控制四元數確定的載體姿態角對應相減，得到姿態角差值，并根據姿態角差值 生成用于校正天線控制四元數的三軸指令角速度旋轉矢量并送至天線控制四元數計算單 J Li 〇
[0073] 天線控制指令生成單元：從天線控制四元數計算單元接收最新的天線控制四元 數，根據天線控制四元數解算得到動中通天線的伺服方位角、伺服仰角和伺服極化角，送至 動中通天線伺服機構。
[0074] 在慣導解算單元中運行的主要是捷聯慣導算法。在捷聯慣導解算算法中，分別進 行了姿態解算、速度解算、位置解算。其中，姿態信息的具體數學載體是導航姿態四元數；導 航姿態四元數是天線控制四元數變化的參考量；速度信息用于與GPS得到的速度信息構成 濾波單元的濾波量測量；通過位置信息和天線控制四元數對應的姿態信息來計算動中通天 線的伺服方位角、伺服仰角和伺服極化角。
[0075] 伺服方位角、伺服仰角和伺服極化角的計算方法如下：
[0076] 計算中有導航系n，載體坐標系b，天線坐標系v和地球坐標系e。其中導航系η取 地理坐標系（X-東，y-北，Ζ-天）；載體坐標系的X軸、y軸、ζ軸分別指向載體的右、前、上； 天線坐標系v中y軸與天線指向一致，Z軸指向天線方位軸向上，X軸與另外兩軸構成右手 系；地球坐標系e，原點位于地心，x軸穿越本初子午線與赤道的交點，ζ軸穿越地球北極點， y軸穿越東經90°子午線與赤道的交點，該坐標系與地球固連。
[0077] 根據上述坐標系的定義，可以方便的計算出各坐標系之間的轉換矩陣：天線坐標 系至載體坐標系的轉換矩陣為 <，載體坐標系至導航坐標系的轉換矩陣為?：，導航坐標系 至載體坐標系的轉換矩陣為，天線坐標系至導航坐標系的轉換矩陣為，地球坐標系至 導航坐標系的轉換矩陣為c：。
[0078] 對于天線伺服方位角和伺服仰角，可由衛星經度λ s得到衛星在地球直角坐標系 下的坐標(I： f <)，同時易得載體在地球直角坐標系下的坐標為ρ? κ z〗），則載 體到衛星的矢量為

【權利要求】
1. 基于MEMS慣導的雙四元數動中通天線控制方法，其特征在于包括如下步驟： ⑴在載體上同時安裝MEMS慣導、GPS和動中通，其中MEMS慣導和GPS構成組合導航 系統； ⑵設定天線控制四元數，天線控制四元數的形式為[q' ^ q' i q' 2 q' 3]，天線控制四元數 中每個參數的含義與捷聯慣導解算中獲取的導航姿態四元數[% q2 q3]對應一致，天線控 制四元數的初值與導航姿態四元數相同； (3) 在捷聯慣導導航計算機的每個中斷周期里，用載體系相對于理想平臺坐標系的旋 轉矢量，分別更新導航姿態四元數和天線控制四元數； (4) 在所述組合導航系統的每個濾波周期內，利用卡爾曼濾波組合導航算法修正MEMS 慣導的導航姿態中的水平姿態誤差，從而修正導航姿態四元數； (5) 在捷聯慣導導航計算機的每個中斷周期里，將由導航姿態四元數確定的載體姿態 角與由天線控制四元數確定的載體姿態角對應相減，得到姿態角差值，并根據姿態角差值 產生用于校正天線控制四元數的三軸指令角速度旋轉矢量，具體為： a. 若由天線控制四元數確定的航向角大于由導航姿態四元數確定的航向角，則三軸指 令角速度旋轉矢量的第三個元素取正的修正指令角速度； b. 若由天線控制四元數確定的航向角小于由導航姿態四元數確定的航向角，則三軸指 令角速度旋轉矢量的第三個元素取負的修正指令角速度； c. 若由天線控制四元數確定的俯仰角大于由導航姿態四元數確定的俯仰角，則三軸指 令角速度旋轉矢量的第一個元素取正的修正指令角速度； d. 若由天線控制四元數確定的俯仰角小于由導航姿態四元數確定的俯仰角，則三軸指 令角速度旋轉矢量的第一個元素取負的修正指令角速度； e. 若由天線控制四元數確定的橫滾角大于由導航姿態四元數確定的橫滾角，則三軸指 令角速度旋轉矢量的第二個元素取正的修正指令角速度； f. 若由天線控制四元數確定的橫滾角小于由導航姿態四元數確定的橫滾角，則三軸指 令角速度旋轉矢量的第二個元素取負的修正指令角速度； (6) 利用三軸指令角速度旋轉矢量校正天線控制四元數，并在校正以后的下一個捷聯 慣導導航計算機的中斷周期，利用校正后的天線控制四元數，解算得到動中通天線的伺服 方位角、伺服仰角和伺服極化角，由此獲得三個姿態方向所對應的控制量控制動中通天線 轉動。
2. 根據權利要求1所述的基于MEMS慣導的雙四元數動中通天線控制方法，其特征在 于：所述步驟（5)中的修正指令角速度，在a和b兩種情況下，大小至少是天線控制四元數 確定的航向角與導航姿態四元數確定的航向角之差再除以組合導航濾波周期，并且不大于 動中通天線每秒所允許的最大對星角度誤差；在c和d兩種情況下，大小至少是天線控制四 元數確定的俯仰角與導航姿態四元數確定的俯仰角之差再除以組合導航濾波周期，并且不 大于動中通天線每秒所允許的最大對星角度誤差；在e和f兩種情況下，大小至少是天線控 制四元數確定的橫滾角與導航姿態四元數確定的橫滾角之差再除以組合導航濾波周期，并 且不大于動中通天線每秒所允許的最大對星角度誤差。
3. 基于MEMS慣導的雙四元數動中通天線控制系統，其特征在于包括：動中通天線控制 器、GPS、MEMS陀螺、MEMS加速度計和動中通天線伺服機構，其中： GPS:測量獲取載體的速度和位置信息并送至動中通天線控制器中的濾波單元； MEMS陀螺：測量獲取載體在三維空間內的角速度信息并送至動中通天線控制器中的 慣導解算單元和天線控制四元數計算單元； MEMS加速度計：測量獲取載體在三維空間內的比力信息并送至動中通天線控制器中 的慣導解算單元； 動中通天線控制器：包括慣導解算單元、濾波單元、天線控制四元數計算單元、天線控 制指令生成單元、天線控制四元數校正指令角速度生成單元，其中： 慣導解算單元：將MEMS陀螺測量獲取的載體在三維空間內的角速度信息，扣除由地球 自轉、載體沿地球表面運動帶來的角速度后，得到載體坐標系相對于地理坐標系的三軸旋 轉矢量;將MEMS加速度計測量獲取的載體在三維空間內的比力信息，扣除重力加速度、 哥氏加速度后，得到載體的對地加速度；利用載體坐標系相對于地理坐標系的三軸旋轉角 速度和載體的對地加速度，經過慣導解算得到載體的姿態、位置和速度信息并送至濾波單 元；將載體坐標系相對于地理坐標系的三軸旋轉矢量以及首次慣導解算直接得到的載 體姿態所對應的姿態四元數[q〇 q2 q3]送至天線控制四元數計算單元；從濾波單元獲取修 正后的載體姿態信息，利用載體坐標系相對于地理坐標系的三軸旋轉矢量ΛΛ更新與修正 后的載體姿態信息所對應的姿態四元數作為導航姿態四元數送至天線控制四元數校正指 令角速度生成單元； 濾波單元：利用GPS輸出的載體速度和位置信息，以及慣導解算單元輸出的載體速度 和位置信息，通過卡爾曼濾波組合導航算法，以固定的濾波周期修正慣導解算單元輸出載 體姿態中的水平姿態誤差并將修正后的結果送至慣導解算單元； 天線控制四元數計算單元：生成天線控制四元數，所述的天線控制四元數的形式為 [q' ^ q' i q' 2 q' 3]，天線控制四元數中每個參數的含義與慣導解算單元獲取的姿態四元數[(? qi % (?]對應一致，且[q' cl q' 1 q' 2 q' 3]的初值為[%ι I % ;從慣導解算單元每接收到一 次載體坐標系相對于地理坐標系的三軸旋轉矢量β/?就利用載體坐標系相對于地理坐標 系的三軸旋轉矢量0Λ,更新天線控制四元數[q' 〇 q' i q' 2 q' 3]并送至天線控制四元數校正指 令角速度生成單元；從天線控制四元數校正指令角速度生成單元獲取三軸指令角速度旋轉 矢量，并用所述三軸指令角速度旋轉矢量再次更新天線控制四元數[q' 〇 q' i q' 2 q' 3]并送至 天線控制指令生成單元； 天線控制四元數校正指令角速度生成單元：分別從慣導解算單元和天線控制四元數計 算單元獲取導航姿態四元數和天線控制四元數，將由導航姿態四元數確定的載體姿態角與 由天線控制四元數確定的載體姿態角對應相減，得到姿態角差值，并根據姿態角差值生成 用于校正天線控制四元數的三軸指令角速度旋轉矢量并送至天線控制四元數計算單元，三 軸指令角速度旋轉矢量中各元素的取值方法如下： a. 若由天線控制四元數確定的航向角大于由導航姿態四元數確定的航向角，則三軸指 令角速度旋轉矢量的第三個元素取正的修正指令角速度； b. 若由天線控制四元數確定的航向角小于由導航姿態四元數確定的航向角，則三軸指 令角速度旋轉矢量的第三個元素取負的修正指令角速度； C.若由天線控制四元數確定的俯仰角大于由導航姿態四元數確定的俯仰角，則三軸指 令角速度旋轉矢量的第一個元素取正的修正指令角速度； d. 若由天線控制四元數確定的俯仰角小于由導航姿態四元數確定的俯仰角，則三軸指 令角速度旋轉矢量的第一個元素取負的修正指令角速度； e. 若由天線控制四元數確定的橫滾角大于由導航姿態四元數確定的橫滾角，則三軸指 令角速度旋轉矢量的第二個元素取正的修正指令角速度； f. 若由天線控制四元數確定的橫滾角小于由導航姿態四元數確定的橫滾角，則三軸指 令角速度旋轉矢量的第二個元素取負的修正指令角速度； 天線控制指令生成單元：從天線控制四元數計算單元接收最新的天線控制四元數，根 據天線控制四元數解算得到動中通天線的伺服方位角、伺服仰角和伺服極化角，送至動中 通天線伺服機構； 動中通天線伺服機構：包括方位向、俯仰向和極化向的電機驅動器和相應的電機，三個 方向的電機驅動器根據天線控制指令生成單元傳來的伺服方位角、伺服仰角和伺服極化角 分別驅動相應方向的電機，由此控制動中通天線的三軸轉動。
4.根據權利要求3所述的基于MEMS慣導的雙四元數動中通天線控制系統，其特征在 于：所述的天線控制四元數校正指令角速度生成單元生成的修正指令角速度，在a和b兩種 情況下，大小至少是天線控制四元數確定的航向角與導航姿態四元數確定的航向角之差再 除以組合導航濾波周期，并且不大于動中通天線每秒所允許的最大對星角度誤差；在c和d 兩種情況下，大小至少是天線控制四元數確定的俯仰角與導航姿態四元數確定的俯仰角之 差再除以組合導航濾波周期，并且不大于動中通天線每秒所允許的最大對星角度誤差；在 e和f兩種情況下，大小至少是天線控制四元數確定的橫滾角與導航姿態四元數確定的橫 滾角之差再除以組合導航濾波周期，并且不大于動中通天線每秒所允許的最大對星角度誤 差。
【文檔編號】H01Q3/02GK104064869SQ201410265808
【公開日】2014年9月24日 申請日期:2014年6月13日 優先權日:2014年6月13日
【發明者】于清波, 門吉卓, 趙書倫, 郎嶸, 劉曉濱, 楊春香 申請人:北京航天控制儀器研究所