[發明專利]隔離線運動擾動的四元數優化初始靜基座對準方法

在審

• 申請號: 202210287945.0
• 申請日: 2022-03-22
• 公開（公告）號: CN114754793A
• 公開（公告）日: 2022-07-15
• 發明（設計）人: 李志波;趙代弟;李明錦
• 申請（專利權）人: 北京數字綠土科技股份有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 北京繪聚高科知識產權代理事務所(普通合伙) 11832
• 代理人: 羅碩

1.一種隔離線運動擾動的四元數優化初始靜基座對準方法，其特征在于，所述方法包括：

步驟S100，通過基于四元數優化的初始對準方法計算所求初始姿態四元數；

步驟S200，對準結果線運動干擾的影響程度進行判定；

步驟S300，當判定結果為影響程度超過誤差上限數值時，則進一步定位對準過程中擾動的發生時間，用以對該點進行剔除消除擾動影響提高所述初始靜基座的對準精度。

2.根據權利要求1所述的隔離線運動擾動的四元數優化初始靜基座對準方法，其特征在于，在步驟S100過程中，通過基于四元數優化的初始對準方法計算所求初始姿態四元數，具體包括如下操作：

步驟S101，在位置姿態系統測量過程中，選取本地地理系作為導航系，同時在導航系下，獲取姿態更新微分方程和速度微分方程；采用四元數姿態更新方法分別對姿態更新微分方程進行解算，得到載體坐標系相對于慣性坐標系的初始姿態對所述速度微分方程進行解算，得到中間變量β(t)和α(t)；

步驟S102，將中間變量β(t)和α(t)進行四元數運算轉化，并同時定義K陣進行公式重新整理可得：L(q)＝q^TKq-λ(q^Tq-1)；

其中，L(q)表示導航中每時刻加速度與零加速度差值的均方差統計，而K陣的最小特征值λ_min即表示L(q)所能達到的最小值；當q取對應于K陣最小特征值λ_min的特征向量時，L(q)取得最小值λ_min，此時，即為所求初始姿態四元數；

具體操作如下：

在位置姿態系統測量過程中，選取本地地理系作為導航系。在導航系下，姿態更新微分方程如下：

其中，公式1為姿態更新微分方程，i是慣性坐標系，n是導航坐標系，b是載體坐標系，是載體坐標系相對于導航坐標系的姿態矩陣，為的轉置；是陀螺輸出角速率，是向量的叉乘矩陣；是表示地球自轉角速率ω_ie在導航坐標系中的投影；示導航坐標系相對于地球坐標系的角速率；表示載體坐標系相對導航坐標系的旋轉角速度在載體坐標系上的投影；

其中，公式2為速度微分方程；其中，是在慣性空間中表示的導航坐標系相對于地球坐標系的運動速度；f^b是在慣性空間中表示的慣性測量單元敏感的比力信息；是在慣性空間中表示的載體相對于地球的運動速度；gⁿ是在慣性空間中表示的地球重力矢量；

經姿態更新微分方程的解算，為如下形式：

其中，x_n(0,t)是利用更新解算得到的函數，更新方法采用四元數通用姿態更新方法，t是時間，是載體坐標系相對于慣性坐標系的初始姿態；

因此，根據上述公式3，和可以表示為如下形式：

其中，χ_b(0,t)為的函數，χ_n(0,t)為的函數。為i系相對于b系的姿態轉換矩陣。為i系相對于n系的姿態轉換矩陣。

根據姿態陣計算關系可以表示為代入式可得：

將上式代入速度微分方程即公式2中，可得

其中，β(t)和α(t)分別定義為

其中，公式6中的β(t)和α(t)均為中間變量；即設置上述公式6為觀察公式；

將上式改為四元數運算的形式為

其中，表示四元數相乘運算，q為四元數形式的即初始姿態四元數。

同時，定義為q＝[s η]，q由標量部分s和矢量部分η組成，q^*為q的共軛四元數。

因此表示如下

根據四元數相乘法則如下

其中，定義四元數的兩種運算如下：

公式可以轉化為

繼而轉化為

按照最小二乘法則求解公式，初始姿態四元數q滿足

這里，定義K陣如下：

因此，公式可以表示為

同時，q滿足

q^Tq＝1； 公式14；

為了將條件融入運算中，使解算方便，應用拉格朗日乘子法定義L(q)如下：

L(q)＝q^TKq-λ(q^Tq-1)； 公式15；

因此，當L(q)取得最小值時，應符合如下條件

由于式是四元數自然屬性，只需考慮式，將式重新整理可得

(K+λI)q＝0； 公式18；

其中，L(q)表示導航中每時刻加速度與零加速度差值的均方差統計，而K陣的最小特征值λ_min即表示L(q)所能達到的最小值；

因此，當q取對應于K陣最小特征值λ_min的特征向量時，L(q)取得最小值λ_min，此時，即為所求初始姿態四元數，即可求得初始對準結果。