[發(fā)明專利]基于函數(shù)迭代積分的剛體姿態(tài)解算方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 201810236436.9
• 申請日: 2018-03-21
• 公開（公告）號: CN108534774B
• 公開（公告）日: 2020-02-21
• 發(fā)明（設計）人: 武元新
• 申請（專利權）人: 上海交通大學
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 上海漢聲知識產(chǎn)權代理有限公司 31236
• 代理人: 莊文莉

1.一種基于函數(shù)迭代積分的剛體姿態(tài)解算方法，其特征在于，包括：

擬合步驟：根據(jù)時間區(qū)間上的陀螺測量值，擬合出角速度的切比雪夫多項式函數(shù)；

迭代步驟：利用得到的角速度的切比雪夫多項式系數(shù)以及羅德里格向量積分方程，迭代計算羅德里格向量的切比雪夫多項式系數(shù)，并對每次迭代的結果按照預設的階數(shù)進行多項式截斷；

姿態(tài)解算步驟：根據(jù)得到的羅德里格向量的切比雪夫多項式系數(shù)以及對應的切比雪夫多項式計算得到羅德里格向量，以四元數(shù)的形式給出時間區(qū)間上的姿態(tài)變化；

其中，所述擬合步驟具體包括：

對于t_k時刻的N個角速度測量值或角增量測量值令將原時間區(qū)間映射到[-1 1]上；角速度采用不超過N-1階的切比雪夫多項式進行擬合近似

其中n為角速度的切比雪夫多項式的階數(shù)，c_i為第i階切比雪夫多項式的系數(shù)向量，F(xiàn)_i(τ)為第i階第一類切比雪夫多項式，τ為映射后的時間自變量；

所述迭代步驟具體包括：

在l次迭代時，羅德里格向量的切比雪夫多項式記做：

其中n_T為預設的截斷階數(shù)，b_l,i為l次迭代時第i階切比雪夫多項式的系數(shù)，當l＝0時，g_l＝0；

羅德里格向量的切比雪夫多項式系數(shù)按如下迭代計算：

迭代計算直到滿足收斂條件或達到事先設定的最大迭代次數(shù)，角速度的多項式近似精度不超過N-1階，設置截斷階數(shù)n_T≥N。

2.根據(jù)權利要求1所述的基于函數(shù)迭代積分的剛體姿態(tài)解算方法，其特征在于，所述陀螺測量值包括角速度測量值或者角增量測量值。

3.一種基于函數(shù)迭代積分的剛體姿態(tài)解算系統(tǒng)，其特征在于，包括：

擬合模塊：根據(jù)時間區(qū)間上的陀螺測量值，擬合出角速度的切比雪夫多項式函數(shù)；

迭代模塊：利用得到的角速度的切比雪夫多項式系數(shù)以及羅德里格向量積分方程，迭代計算羅德里格向量的切比雪夫多項式系數(shù)，并對每次迭代的結果按照預設的階數(shù)進行多項式截斷；

姿態(tài)解算模塊：根據(jù)得到的羅德里格向量的切比雪夫多項式系數(shù)以及對應的切比雪夫多項式計算得到羅德里格向量，以四元數(shù)的形式給出時間區(qū)間上的姿態(tài)變化；

其中，所述擬合模塊具體包括：

對于t_k時刻的N個角速度測量值或角增量測量值令將原時間區(qū)間映射到[-1 1]上；角速度采用不超過N-1階的切比雪夫多項式進行擬合近似

其中n為角速度切比雪夫多項式的階數(shù)，c_i為第i階切比雪夫多項式的系數(shù)向量，F(xiàn)_i(τ)為第i階第一類切比雪夫多項式，τ為映射后的時間自變量；

所述迭代模塊具體包括：

在l次迭代時，羅德里格向量的切比雪夫多項式記做：

其中n_T為預設的截斷階數(shù)，b_l,i為l次迭代時第i階切比雪夫多項式的系數(shù)，當l＝0時，g_l＝0；

羅德里格向量的切比雪夫多項式系數(shù)按如下迭代計算：

迭代計算直到滿足收斂條件或達到事先設定的最大迭代次數(shù)，角速度的多項式近似精度不超過N-1階，設置截斷階數(shù)n_T≥N。

4.根據(jù)權利要求3所述的基于函數(shù)迭代積分的剛體姿態(tài)解算系統(tǒng)，其特征在于，所述陀螺測量值包括角速度測量值或者角增量測量值。