一種針對星敏感器和陀螺聯合濾波的改進方法與流程
2023-10-09 01:37:54 4
本發明涉及一種針對星敏感器與陀螺的擴展卡爾曼濾波的改進方法，具體地說是在濾波算法的四元數測量值發生跳變時重置四元數參考值的方法，屬於衛星姿態確定技術領域。
背景技術：
星敏感器和陀螺均為衛星姿控系統最為主要的敏感器單機。星敏感器的測量值為星敏感器測量坐標系相對於慣性坐標系的四元數，由星敏感器安裝矩陣可得到衛星本體坐標系相對於慣性坐標系的四元數；陀螺的測量值為陀螺測量坐標系相對於慣性坐標系的角速度在陀螺測量坐標系內的投影，由陀螺安裝矩陣可得到衛星本體坐標系相對於慣性坐標系的角速度在衛星本體坐標系內的投影。由於星敏感器存在高頻的測量白噪聲，陀螺存在常值漂移等誤差，因此衛星姿控系統中通常會將星敏感器與陀螺進行聯合濾波，算法為擴展卡爾曼濾波。濾波得到的四元數估計值將顯著的抑制星敏感器的高頻噪聲。目前大多數星敏感器在單機內部將測量四元數的標部處理為始終為正，即當星敏感器測量四元數標部過零時，四元數的四個數全部取反。此時若擴展卡爾曼濾波算法的測量值參考坐標係為慣性坐標系，則該濾波算法的四元數測量值也會發生跳變，因此濾波算法給出的四元數估計值逐漸發散，較長時間後再重新收斂。衛星在軌時，星敏感器測量四元數以軌道周期為變化周期，即每軌會經過一次零位，因此濾波得到四元數估計值也會每軌出現一次發散-重新收斂的過程，這對於衛星在軌穩態控制是不可接受的。
根據國內外已發表的相關文獻可知，對於三軸對地衛星，衛星控制的參考坐標係為軌道坐標系，在該坐標系下濾波算法的四元數測量值的參考坐標系也為軌道坐標系，因此該濾波算法的四元數測量值不存在跳變的現象，相應地的四元數估計值可以始終穩定地收斂；對於慣性定向衛星或目標姿態的參考坐標係為慣性坐標系，則常見的做法是令星敏感器單機給出的測量四元數連續變化，即濾波算法的四元數測量值標部有正有負，為完整的正弦或者餘弦曲線。而針對四元數測量值跳變時的算法處理未見有效方法。
技術實現要素：
本發明的目的在於提供一種針對星敏感器和陀螺聯合濾波的改進方法，解決現有擴展卡爾曼濾波算法的四元數測量值發生跳變時四元數估計值需重新收斂的問題。
為了達到上述發明目的，本發明提供了一種針對星敏感器和陀螺聯合濾波的改進方法，該方法包含如下步驟：
步驟1，構造擴展卡爾曼濾波器，進行擴展卡爾曼濾波算法；
步驟2，確定出擴展卡爾曼濾波算法的輸入量四元數測量值qs的跳變時刻，該跳變時刻的判斷需同時滿足：四元數測量值的標部小於閾值，且，四元數測量值矢量部分中絕對值最大的數的當前拍與前一拍異號；
步驟3，在跳變時刻，重置擴展卡爾曼濾波算法的四元數參考值以獲得跳變時刻與四元數測量值相適應的四元數參考值
步驟4，以步驟3重置後的四元數參考值繼續進行步驟1所述的擴展卡爾曼濾波算法，以獲得連續且平穩的四元數估計值。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，步驟1中，所述的擴展卡爾曼濾波器的狀態量為偏差四元數的矢量部分以及陀螺常值漂移的估計誤差，取狀態變量為陀螺常值漂移值b與其估計值之間的差值，則可得擴展卡爾曼濾波器的一步預測x(k/k-1)為：
其中，
其中，δt為濾波算法的迭代周期，i3×3為3維的單位矩陣，為的斜對稱矩陣：
其中，為姿態角速度估計值，為的三軸角速度。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，擴展卡爾曼濾波器的狀態更新公式為：
其中，kk為卡爾曼濾波器的增益係數，δq為偏差四元數量測值δq的矢量部分，偏差四元數量測值δq為四元數測量值qs與四元數參考值之間的差值，k代表當前時間點，k-1代表k時間點的前一時間點。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，四元數參考值的計算公式為：
其中，為姿態角速度估計值，為的2-範數，φ為歐拉軸轉角，為的函數。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，k時刻的計算公式為：
其中，為陀螺常值漂移的估計值；ωs為根據陀螺測量值解算出的衛星本體坐標系相對於慣性坐標系的三軸角速度在衛星本體坐標系內的投影。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，ωs的計算公式如下：
ωs＝ω+b+vg
其中，b、vg分別為陀螺的常值漂移與白噪聲，ω為真實的三軸角速度。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，的計算公式如下：
其中，為的三軸角速度，δt為濾波算法的迭代周期。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，步驟3中，重置四元數參考值的方法為將四元數參考值的四個數全部取反。
上述的針對星敏感器和陀螺聯合濾波的改進方法，其中，步驟4中的算法公式包含：及其中，k代表當前時間點，代表四元數參考值，為偏差四元數估計值的矢量部分。
本發明的針對星敏感器和陀螺聯合濾波的改進方法，與現有技術相比，其優點和有益效果是：
1)濾波算法的四元數測量值發生跳變後，濾波算法的四元數估計值仍然連續且平穩，為姿態控制系統提供了穩定的高精度姿態；
2)跳變時刻的判斷對閾值選取的要求低，具備工程可操作性；
3)跳變時刻的判斷方法嚴謹可靠，誤判及漏判的概率極低，具備工程實用性；
4)對擴展卡爾曼濾波算法的改動較小，原理清晰，算法簡單，星載軟體容易實現。
具體實施方式
以下結合實施例對本發明的技術方案做進一步的說明。
1.構造擴展卡爾曼濾波器
根據陀螺測量值解算出的衛星本體坐標系相對於慣性坐標系的三軸角速度在衛星本體坐標系內的投影ωs可表示如下：
ωs＝ω+b+vg
上式中，b、vg分別為陀螺的常值漂移與白噪聲，ω為真實的三軸角速度。
根據星敏感器測量值解算出慣性坐標系相對於衛星本體坐標系的測量四元數qs，該數值即為擴展卡爾曼濾波算法的四元數測量值。該qs與ωs為擴展卡爾曼濾波器的兩個輸入量。
姿態角速度估計值為
上式中，為陀螺常值漂移的估計值。
四元數參考值的計算為
上式中，為的2-範數，φ為歐拉軸轉角，為的函數，分別等於
上式中，為的三軸角速度，δt為濾波算法的迭代周期。
定義偏差四元數量測值δq為四元數測量值qs與四元數參考值之間的差值，則有：
定義為偏差四元數估計值的矢量部分，為陀螺常值漂移b與其估計值之間的差值。
取狀態變量則可得擴展卡爾曼濾波器的一步預測x(k/k-1)為：
上式中f(k)為
上式中，i3×3為3維的單位矩陣，為的斜對稱矩陣：
狀態更新：
上式中，δq為偏差四元數量測值δq的矢量部分，kk為卡爾曼濾波器的增益係數。
陀螺常值漂移的估計值與四元數估計值的更新：
2.確定四元數測量值的跳變時刻
設四元數測量值qs為
上式中，qs0為標部，qs1、qs2、qs3為矢量部分。
當qs0小於閾值ε(閾值通常是根據經驗選擇，本發明所涉及的閾值無特別限定，一般選擇在0-1之間趨近於0的值，如取0.1或0.01等)時，令qm為qs1、qs2、qs3三個數中絕對值最大的數，若qm的當前拍qm(k)與前一拍qm(k-1)異號，即
qm(k)·qm(k-1)＜0
則認為當前拍四元數測量值發生跳變。
3.重置濾波算法的四元數參考值
擴展卡爾曼濾波算法給出的四元數估計值在四元數測量值跳變時刻需要重新收斂的原因是此時四元數測量值發生了反號，從而導致偏差四元數量測值δq發生了跳變。
處理的方法是將當前拍四元數參考值的計算結果取反，即
根據取反後的四元數參考值再去進行後續計算(即，繼續擴展卡爾曼濾波算法)，則可使四元數估計值連續平穩。
綜上所述，本發明通過構造擴展卡爾曼濾波，重置跳變時刻四元數參考值的方法解決了四元數測量值跳變導致四元數估計值需重新收斂的問題，為姿態確定系統提供了穩定地連續地高精度姿態信息；該方法算法簡單，星載軟體容易實現，可靠性高。
儘管本發明的內容已經通過上述優選實施例作了詳細介紹，但應當認識到上述的描述不應被認為是對本發明的限制。在本領域技術人員閱讀了上述內容後，對於本發明的多種修改和替代都將是顯而易見的。因此，本發明的保護範圍應由所附的權利要求來限定。