本發明涉及陸地、航空和航海等多種模式下的高精度定位定姿技術領域，尤其涉及一種適用于陸地/航空/航海等多種模式下的高精度定位定姿系統(POS)動態快速對準技術。
背景技術：
高精度定位定姿系統是集GPS定位和慣性導航技術于一體的系統，通過采用卡爾曼濾波技術實現數據的融合，POS系統將充分發揮GPS和INS各自的優點，能夠為處于陸地/航空/航海模式下的載荷提供實時、高精度的位置、航向、姿態和速度等信息，能為運動載荷的導航和制導提供重要的信息。
隨著科技的進步和社會的發展，人們對地理空間信息的獲取和更新速度提出了更高的需求，傳統的測繪方式已經不能滿足這一需求，測繪科技工作者必須尋求更高效、可靠的空間信息獲取技術。在這種需求下，移動測量系統應運而生，能夠實現陸地、航空、航海等多種模式下的地理空間信息獲取，給測繪行業帶來了一場信息技術革命。
移動測量系統的運用需要高精度的POS系統提供高精度、可靠的導航信息，目前國外一些公司，如加拿大的Applanix、美國Z/I Imaging和德國IGI等已經開發出相應的機載和車載POS產品并應用于航空攝影和地面空間數據的采集，但其對華實施產品禁運和相關技術封鎖。隨著我國對慣性測量技術、高精度GPS接收機研究的不斷深入，特別是隨著我國北斗衛星定位技術的不斷發展，研究具有自主知識產權的高精度定位定姿測量系統，并將其用于城市空間地理信息的獲取和國土測量等領域中，不但為我國大規模生產實踐、國民經濟建設、社會發展、人們生活提供必要的基礎地理信息，還可使我國在導航技術中有所突破，擺脫對國外技術的依賴。
技術實現要素：
POS系統的動態快速對準技術是保證系統可靠、高效運轉的重要技術，針對這一技術難題，本發明在捷聯慣導誤差模型研究的基礎上，提出了一種適用于陸地/航空/航海/多種模式下的高精度POS動態快速對準技術。
本發明采用的技術方案提供一種適用于多種模式下的高精度POS快速對準方法，包括以下步驟：
步驟1，判斷POS系統處于靜止狀態或運動狀態，相應選擇對準模式為靜對準或者動對準；步驟2，根據步驟1中選擇結果，相應進行靜對準或者動對準，同時繼續檢測系統的運動狀態；
設慣性坐標系標記為i，地球坐標系標記為e，導航坐標系標記為n，載體坐標系標記為b，
靜對準時，采用基于慣性系的對準方式，包括將姿態矩陣分解如下式：
式中，ib0指初始對準t0時刻的載體坐標系，為t時刻慣性系與n系之間的變換矩陣，由載體的地理位置及粗對準時間t確定；為t時刻b系與ib0系之間的變換矩陣，利用陀螺輸出的b系相對ib0系的角運動信息確定；為ib0系與i系之間的變換矩陣，由重力加速度與加速度計輸出之間的轉換關系求得；
動對準時，采用基于快速卡爾曼濾波的對準方式；
步驟3，判斷對準是否成功，若對準成功則反饋對準結果，若對準失敗則根據檢測到的載體運動狀態調整對準模式，重新對準。
而且，采用基于快速卡爾曼濾波的對準方式時，在基于SINS/GPS離散模型轉移矩陣Φk/k-1和Φk/k-1和系統狀態向量誤差方差矩陣Pk-1計算一步預測誤差方差矩陣時，先計算矩陣B＝Φk/k-1Pk-1，再計算矩陣計算B＝Φk/k-1Pk-1時，通過直接展開Φk/k-1Pk-1來表示B的每一個元素。
而且，當POS系統在靜態過程中上電啟動，若在步驟3中判斷對準失敗，判定是否執行靜態對準的時間已超過預設的時間閾值，若是則轉入動對準模式。
而且，變換矩陣求取如下，
其中，t表示對準時間，ωie表示地球自轉角速度矢量，λ表示載體所在位置的經度，L表示載體所在位置的緯度，表示e系相對于i系的旋轉矩陣，表示n系相對于e系的位置矩陣。
而且，變換矩陣通過捷聯慣導姿態更新算法求得。
而且，變換矩陣求取如下，
取t0＜tu≤tv＜td，其中t0為粗對準開始時刻、td為粗對準結束時刻，
其中，表示時刻t0到tu加速度計輸出的比力增量在ib0系的投影、表示時刻tv到td加速度計輸出的比力增量在ib0系的投影、表示時刻t0到tu加速度計輸出的比力增量在i系的投影、表示時刻tv到td加速度計輸出的比力增量在i系的投影。
本發明具有下列優點和積極效果：
1)本發明適用于陸地、航空、航海等多種工作模式下的高精度定位定姿系統的初始對準，
使得系統能夠快速有效的獲取準確的初始航向和姿態信息，具有重要的市場價值；
2)對準時間短，精度高，系統計算量小；
3)可根據系統運動狀態自主選擇最優的對準方式，自主性好。
附圖說明
圖1是本發明實施例的流程圖。
具體實施方式
以下結合附圖和實施例對本發明技術方案進行具體描述。
本發明所提供技術方案，是根據POS系統上電時的運動狀態，自動選取對準的方式，以適應陸地/航空/航海等多種模式的應用需求，具體實施時POS系統通過工裝與相應載體固連。
當POS系統在靜態過程中上電啟動時，系統在傳統解析式粗對準的基礎上，將初始對準姿態矩陣進行分解，使得粗對準具有跟蹤載體角運動的功能(具體可參見后續公式(3)，(6)，(15))，降低晃動對系統對準的影響，獲得的是粗對準結束時刻的姿態矩陣。
當POS系統在運動過程中開機或者重新啟動時，需要利用動基座對準技術，本方案利用GPS軌跡的航跡角對慣導初始方位進行粗對準，一般誤差能夠控制在10度以內，由于粗對準對水平姿態角的初始值精度要求不高，而載體的水平姿態角一般不大，不妨將其初始化為零，慣導系統的速度初值由GPS提供，這樣構成一個小失準角的SINS/GPS對準系統。在利用卡爾曼濾波進行組合導航解算是為了提高解算速度。本方案采用直接展開法，摒棄了程序循環控制計算方法，直接展開法雖然增加了少許程序長度和降低了通用性，但是極大減少了乘法運算次數和程序循環控制開銷，有利于減少數值舍入累積誤差，直接展開法還考慮了協方差陣的對稱性，提高了數值穩定性。
進一步，特殊地，可以當POS系統在靜態過程中上電啟動，靜態對準時間不滿足系統需求時，系統將在靜態粗對準的基礎上迅速轉入動基座對準模式，提高對準精度，減少對準時間。
因此，本發明的基本工作原理是：
本發明首先根據POS系統上電前幾秒采集的IMU和GNSS數據判斷系統的初始狀態，并根據相應的狀態選擇對準模式進行初始對準，并在對準的過程中實時監測系統的運動狀態，并根據檢測結果對系統的對準模式進行相應的調整。系統在靜態模式時，采用基于慣性坐標系的對準技術，快速準確獲取載體的初始姿態信息；系統在動態模式時，采用基于快速卡爾曼濾波的動對準技術，快速有效地獲取載體的精確姿態角信息。
參見圖1，實施例包括以下步驟：
1)判斷POS系統處于靜止狀態或運動狀態，相應選擇對準模式為靜對準或者動對準；POS系統通過工裝與載體固連，運動狀態一致；
2)根據步驟1)中選擇結果，相應進行靜對準或者動對準，同時繼續檢測系統的運動狀態；
A.靜對準時，采用基于慣性系的對準方式，具體步驟如下：
傳統解析式粗對準中，姿態矩陣的計算如下式：
式中，下標中的“i”和“e”分別表示慣性坐標系(i系)和地球坐標系(e系)，gn和為重力加速度矢量和地球旋轉矢量在導航坐標系(n系)中的投影，gb和為重力加速度矢量和地球旋轉矢量在載體坐標系(b系)中的投影。
如果載體出現較大幅值的晃動干擾，上述粗對準方法可能產生很大誤差，為減小晃動對對準的影響，將姿態矩陣分解如下式：
式中，ib0指初始對準t0時刻的載體坐標系，為t時刻慣性系(i系)與n系之間的變換矩陣，可由載體的地理位置及粗對準時間t確定；為t時刻b系與ib0系之間的變換矩陣，利用陀螺輸出的b系相對ib0系的角運動信息，通過捷聯慣導姿態更新算法可以求得該矩陣；為ib0系與i系之間的變換矩陣，該矩陣是個常值陣，可由重力加速度與加速度計輸出之間的轉換關系求得。
a)求解變換矩陣
變換矩陣可由n系相對于e系的位置矩陣和e系相對于i系轉過的角度ωiet求得，即
式中，t表示對準時間，ωie表示地球自轉角速度矢量，λ表示載體所在位置的經度，L表示載體所在位置的緯度，表示e系相對于i系的旋轉矩陣。
b)求解變換矩陣
變換矩陣可通過陀螺采樣輸出，利用現有技術的捷聯慣導姿態更新算法可求得。假設tk時刻和tk+1時刻的變換矩陣分別為和其對應的變換四元數分別為和顯然有和其中I表示單位矩陣。再假設tk時刻至tk+1時刻陀螺輸出的角增量為Δθk，則利用等效旋轉矢量單子樣算法可得b系相對于ib0系從tk時刻至tk+1時刻的變換四元數為：
從而可進行四元數更新解算
最后，假設四元數矢量其中q0、q1、q2和q3表示四元數，是表示兩個3維空間矢量轉換關系的一種方式。則由變換四元數和變換矩陣之間的關系可計算得tk時刻的變換矩陣
變換矩陣的更新解算使得粗對準具有了跟蹤載體角運動的功能，獲得粗對準結束時刻的姿態矩陣。
c)求解變換矩陣
變換矩陣是個常值矩陣，當載車靜止時，該矩陣可由重力加速度在i系的投影gi與加速度計輸出在ib0系的投影之間的轉換關系求得，即
其中，表示初始對準t0時刻的載體坐標系相對于慣性系(i系)的轉換矩陣，另外有：
公式(8)和公式(9)中，表示加速度計輸出量在載體系(b系)中的投影，gn表示重力加速度在導航系(n系)中的投影，表示慣性系相對于載體系的轉換矩陣。
公式(7)中取兩個不同時刻tl時刻和tm時刻(tl<tm)的變換方程，類似于(1)式的方法可得：
當tl時刻與tm時刻很接近時，上式中tl和tm時刻慣性系下的重力加速度分量和幾乎平行，為了降低計算誤差應使tl適當遠離tm。公式(10)中，和表示tl時刻與tm時刻加速度輸出量在系中的分量。
由于慣性器件噪聲等隨機干擾的存在，應對(7)式在tl時刻和tm時刻附近積分，以降低干擾的影響。設tk時刻至tk+1時刻陀螺輸出的角增量為Δθk，加速度計輸出的比力速度增量為Δvk，類似于捷聯慣導速度更新算法，利用速度單子樣算法由(8)式可得在ib0系上投影的速度更新算法：
其中，表示時刻tr到tk加速度計輸出的比力增量在ib0系的投影，tr≤tk；表示時刻tr到tk加速度計輸出的比力增量在ib0系的投影；另外，將(3)式代入(9)式并從tr時刻至tk時刻積分可得
公式(12)中，g表示重力加速度，表示時刻tr到tk加速度計輸出的比力增量在i系的投影，表示導航系(n系)相對于慣性系(i系)的轉換矩陣。取t0＜tu≤tv＜td，其中t0為粗對準開始時刻、td為粗對準結束時刻，tu、tv為期間的兩個時刻，則將(7)式兩邊都積分可得
式中表示時刻t0到tu加速度計輸出的比力增量在ib0系的投影、表示時刻tv到td加速度計輸出的比力增量在ib0系的投影、表示時刻t0到tu加速度計輸出的比力增量在i系的投影、表示時刻tv到td加速度計輸出的比力增量在i系的投影，并類似于(1)式的方法可得
d)求解粗對準姿態矩陣
將(3)式、(6)式和(15)式代入(2)式即可求得粗對準姿態矩陣
B.動對準時，采用基于快速卡爾曼濾波的對準方式，具體步驟如下：
a)建立空中動基座對準狀態方程如下：
式中，x表示狀態向量，表示x的一階微分，系統狀態矩陣Ff
公式(17)中，和表示在導航坐標系(n系)中加速度計輸出的比力在x、y、z三個軸向的分量。
b)選擇航向、姿態和速度6維狀態向量,建立空中動基座對準量測方程如下：
公式(18)中，表示航向、俯仰和橫滾，δνn表示速度矢量，T表示向量或者矩陣的轉置。
空中動基座對準速度量測方程為
其中，
c)對POS系統進行快速卡爾曼濾波，具體過程如下：
在SINS/GPS離散模型轉移矩陣Φk/k-1的225個元素中，僅有71個非零元素(包含13個1元素)，因此在利用轉移矩陣Φk/k-1和系統狀態向量誤差方差矩陣Pk-1計算時存在大量的乘0和部分的乘1過程，然而這兩種無實質意義的乘法操作都是可以省略的。
SINS/GPS離散模型轉移矩陣Φk/k-1可參見相關文獻：嚴恭敏.車載自主定位定向系統研究[D].西安：西北工業大學博士學位論文,2006,5.
改進計算方法的思路是將一步預測誤差方差矩陣的計算分為兩步，先計算前兩矩陣相乘且記為矩陣B＝Φk/k-1Pk-1，再計算矩陣計算B＝Φk/k-1Pk-1時，通過直接展開Φk/k-1Pk-1來表示B的每一個元素，例如，在SINS/GPS中實際編程時B(1,1)直接寫成：
式中，B(1,1)表示矩陣元素的臨時變量，Φk/k-1(i,j)表示矩陣Φk/k-1第i行第j列元素，Pk-1(i,j)表示矩陣Pk-1第i行第j列元素。計算時，考慮到C是對稱矩陣，則只需先展開上三角矩陣(或下三角矩陣)，再利用對稱性給另一半矩陣直接賦值即可。
3)判斷對準是否成功，若對準成功則反饋對準結果，若對準失敗則根據檢測到的載體運動狀態調整對準模式，重新對準：
由于POS系統內部反饋機制，對準過程完成后，對準函數會反饋對準狀態，POS系統會根據對準狀態判斷對準成功與否。實施例根據POS系統輸出的對準狀態標志(對準狀態字)判斷系統對準是否成功，若對準成功則將對準結果反饋給POS系統，若對準失敗則根據檢測到的載體運動狀態調整對準模式，重新對準。此時可以仍然是靜止狀態則相應選擇對準模式為靜對準，運動狀態則相應選擇對準模式為動對準。進一步地，當POS系統在靜態過程中上電啟動，此時也可以判定是否執行靜態對準的時間已超過系統需求(超過預設的時間閾值)，若是則可直接轉入動基座對準模式。
以上內容是結合實施例對本發明所做的進一步詳細說明，不能認定本發明的具體實施只限于這些說明。本領域的技術人員應該理解，在不脫離由所附權利要求書限定的情況下，可以在細節上進行各種修改，都應當視為屬于本發明的保護范圍。