[發明專利]一種DVL輔助SINS魯棒行進間初始對準方法

有效

• 申請號: 201811075323.1
• 申請日: 2018-09-14
• 公開（公告）號: CN109141475B
• 公開（公告）日: 2020-06-05
• 發明（設計）人: 徐祥;徐大誠
• 申請（專利權）人: 蘇州大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 蘇州市中南偉業知識產權代理事務所(普通合伙) 32257
• 代理人: 馮瑞;楊慧林

1.一種DVL輔助SINS魯棒行進間初始對準方法，其特征在于，包括以下幾個步驟：

步驟(1)：獲取傳感器實時數據；

步驟(2)：建立矢量視運動參數方程；

步驟(3)：在建立參數方程的基礎上，利用自適應魯棒卡爾曼濾波估計參數矩陣；

步驟(4)：建立基于估計參數的重構矢量；

步驟(5)：利用四元數姿態確定方法，并計算確定姿態與真實姿態之間的誤差角；

步驟(6)：初始對準過程運行時間為M，獲取實時數據的時刻為k，若k＝M，則輸出初始對準結果，完成初始對準過程，若k＜M，表示初始對準過程未完成，則重復上述步驟(1)至步驟(5)，直至初始對準過程結束；

步驟(1)中所述傳感器實時數據包括陀螺儀數據和加速度計數據；

步驟(2)中建立矢量視運動參數方程的方式為，

步驟(2.1)：定義解算所需的參考坐標系，

b—載體坐標系，表示捷聯慣性導航系統三軸正交坐標系，其x軸、y軸和z軸分別指向載體的右-前-上；

n—導航坐標系，表示載體所在位置的地理坐標系，其三軸分別指向當地東向、北向和天向；

e—地球坐標系，表示原點在地心，x軸為地心指向本初子午線與赤道交點，z軸為地心指向北極點，y軸與x軸和z軸構成右手坐標系；

i—慣性坐標系，表示慣性空間非旋轉坐標系；

b0—初始載體坐標系，表示慣導系統開機運行時刻的載體坐標系，并在整個對準過程中相對于慣性空間保持靜止；

n0—初始導航坐標系，表示慣導系統開機運行時刻的導航坐標系，并在整個對準過程中相對于慣性空間保持靜止；

e0—初始地球坐標系，表示慣導系統開機運行時刻的地球坐標系，并在整個對準過程中相對于慣性空間保持靜止；

步驟(2.2)：矢量視運動建模，

由坐標變換關系可知：

式中：表示導航系速度的微分；表示載體系相對于導航系變化的方向余弦矩陣；表示方向余弦矩陣的微分；v^b表示載體系速度；表示載體系速度微分；

由方向余弦矩陣計算關系可知：

式中：表示方向余弦矩陣的微分；表示載體系相對于導航系變化的方向余弦矩陣；表示載體系相對于導航系的轉動角速度在載體系上的投影；×表示矢量叉乘運算；

由慣性導航系統比力方程可知：

式中：表示導航系速度的微分；表示載體系相對于導航系變化的方向余弦矩陣；f^b表示比力；表示地球系相對于慣性系的旋轉角速度在導航系的投影；表示導航系相對于地球系的轉動角速度在導航系的投影；×表示矢量叉乘運算；vⁿ表示導航系速度；gⁿ表示重力加速度在導航系下的投影；

通過推導可知：

式中：表示載體系相對于導航系變化的方向余弦矩陣；表示載體系相對于導航系的轉動角速度在載體系上的投影；×表示矢量叉乘運算；v^b表示載體系速度；表示載體系速度微分；表示初始導航系相對于導航系的方向余弦矩陣；表示初始載體系相對于初始導航系的方向余弦矩陣；表示載體系相對于初始載體系的方向余弦矩陣；f^b表示比力；表示地球系相對于慣性系的旋轉角速度在導航系的投影；表示導航系相對于地球系的轉動角速度在導航系的投影；×表示矢量叉乘運算；vⁿ表示導航系速度；gⁿ表示重力加速度在導航系下的投影；

對上式進行積分運算可得：

式中：表示載體系相對于初始載體系的方向余弦矩陣；表示地球系相對于慣性系的旋轉角速度在載體系的投影；表示導航系相對于地球系的轉動角速度在載體系的投影；×表示矢量叉乘運算；v^b表示載體系速度；表示載體系速度微分；f^b表示比力；表示初始載體系相對于初始導航系的方向余弦矩陣；表示導航系相對于初始導航系的方向余弦矩陣；gⁿ表示重力加速度在導航系下的投影；

定義矢量構成：

式中：β表示觀測矢量；α表示參考矢量；表示載體系相對于初始載體系的方向余弦矩陣；表示地球系相對于慣性系的旋轉角速度在載體系的投影；表示導航系相對于地球系的轉動角速度在載體系的投影；×表示矢量叉乘運算；v^b表示載體系速度；表示載體系速度微分；f^b表示比力；表示導航系相對于初始導航系的方向余弦矩陣；gⁿ表示重力加速度在導航系下的投影；

構造矢量觀測器：

式中：β表示觀測矢量；α表示參考矢量；表示初始載體系相對于初始導航系的方向余弦矩陣；

對參考矢量和觀測矢量進行積分運算并表示為離散形式：

式中：β_k表示k時刻的觀測矢量；表示k時刻的載體系相對于初始載體系的方向余弦矩陣；v^b(k)表示k時刻的載體系速度；v^b(0)表示初始時刻的載體系速度；β′_k表示k時刻的中間變量；β′_k-1表示k-1時刻的中間變量；表示k-1時刻載體系相對于初始載體系的方向余弦矩陣；表示k-1時刻導航系相對于k-1時刻載體系的方向余弦矩陣；Δν^n(k-1)表示k-1時刻一個DVL采樣周期內速度增量的中間變量；Δυ^b(k-1)表示k-1時刻一個DVL采樣周期內比力增量；α_k表示k時刻的參考矢量；α_k-1表示k-1時刻的參考矢量；Δt_d表示DVL采樣周期；I₃表示三維單位矩陣；表示導航系相對于慣性系的旋轉角速度在導航系的投影；×表示矢量叉乘運算；gⁿ表示重力加速度在導航系下的投影；

由重力矢量視運動可知：

式中，α表示參考矢量；表示初始地球系相對于初始導航系的方向余弦矩陣；表示地球系相對于初始地球系的方向余弦矩陣；表示導航系相對于地球系的方向余弦矩陣；gⁿ表示重力加速度在導航系下的投影；

對上式進行積分運算并做處理可得：

α＝ΦΓ(t)

式中：Γ(t)表示隨時間變化的矢量；Φ表示參數矩陣；且可以表示為：

式中：Φ表示參數矩陣；g表示地球重力加速度；ω_ie表示地球自轉角速率；L表示當地緯度；t表示時間；

觀測矢量的真值可以表示為：

β＝ΞΓ(t)

式中：表示初始導航系相對于初始載體系的方向余弦矩陣，Φ表示參數矩陣；β表示觀測矢量；Γ(t)表示隨時間變化的矢量；

當代入DVL量測，并考慮量測矢量含有噪聲時，可以表示為：

式中：表示k時刻的量測觀測矢量；Ξ表示常值參數矩陣；Γ(k)表示k時刻的離散矢量；表示k時刻載體系相對于初始載體系的方向余弦矩陣；δv^b表示量測噪聲；

利用參數估計理論進行參數建模可得：

式中：Ξ_z,k表示k時刻參數矩陣第三行分量；w_z,k-1表示過程噪聲；表示k時刻觀測矢量的第三個元素；Γ(k)表示k時刻時間矢量；表示k時刻量測噪聲的第三個元素；

步驟(4)中建立基于估計參數的重構矢量的方式為，

利用自適應魯棒卡爾曼濾波方程進行矢量重構可得：

式中：表示j時刻重構矢量；表示j時刻后驗估計參數矩陣；Γ(k)表示k時刻時間矢量；

步驟(5)中計算確定姿態與真實姿態之間的誤差角的方式為，

由四元數姿態確定方法可知，姿態K矩陣可以表示為：

式中：K_k表示k時刻的K矩陣；K_k-1表示j-1時刻的K矩陣；表示k時刻重構矢量；α_k表示k時刻參考矢量。