【技術實現步驟摘要】
本公開涉及姿態估計，尤其涉及一種基于多傳感器融合的姿態估計方法及系統。
技術介紹
1、動作捕捉技術的發展，穿戴式設備在體育訓練、康復醫療、虛擬現實等領域得到廣泛應用。這些設備通常集成有慣性測量單元(imu)，包括加速度計、陀螺儀和磁力計，用于獲取人體運動數據。然而，在復雜的室內環境中，由于電磁設備、金屬結構等因素導致的磁場干擾會對imu中的磁力計讀數產生影響，進而影響姿態估計的準確性和穩定性。
2、目前，雖然有多種姿態估計算法，如擴展卡爾曼濾波(ekf)、互補濾波器(cf)等，但這些算法在磁場干擾環境下表現不佳。
技術實現思路
1、本公開提出了一種基于多傳感器融合的姿態估計方法及系統技術方案。
2、根據本公開的一方面，提供了一種基于多傳感器融合的姿態估計方法，包括：
3、獲取磁力計測量值smm、第一預設閾值及小于所述第一預設閾值的二預設閾值；
4、根據所述磁力計測量值確定指示磁場變化方向的序列，基于所述指示磁場變化方向的序列確定設定范圍對應的序列和；
5、基于所述序列和對應的數值與所述第一預設閾值和/或所述二預設閾值的大小關系，利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計或利用加速度矢量測量值sam、角速度矢量測量值sωm及磁力計測量值smm進行姿態估計或維持上一時刻的傳感器融合模式進行姿態估計。
6、優選地，所述基于所述序列和對應的數值與所述第一預設閾值和/或所述二預設閾值的大小關系，利用加
7、優選地，所述利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計的方法，包括：獲取角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率所述加速度矢量測量值sam在k時刻對應的第二四元數姿態估計值的第二變化率所述k時刻的上一k-1時刻對應的四元數姿態最優估計值所述k時刻與所述k-1時刻之間的時間增量δk；對所述第一變化率及所述第二變化率做差后，乘以所述時間增量δk，得到所述時間增量δk內對應的變化率差值；對所述時間增量δk內對應的變化率差值與所述四元數姿態最優估計值進行求和，得到所述k時刻對應的姿態。
8、優選地，所述利用加速度矢量測量值sam、角速度矢量測量值sωm及磁力計測量值smm進行姿態估計的方法，包括：獲取角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率所述加速度矢量測量值sam及所磁力計測量值smm在k時刻對應的第二四元數姿態估計值的第二變化率所述k時刻的上一k-1時刻對應的四元數姿態最優估計值所述k時刻與所述k-1時刻之間的時間增量δk；對所述第一變化率及所述第二變化率做差后，乘以所述時間增量δk，得到所述時間增量δk內對應的變化率差值；對所述時間增量δk內對應的變化率差值與所述四元數姿態最優估計值進行求和，得到所述k時刻對應的姿態。
9、優選地，在所述對所述第一變化率及所述第二變化率做差之前，對所述第二變化率進行歸一化處理，得到歸一化的第二變化率對所述第一變化率及所述歸一化的第二變化率做差。
10、優選地，所述對所述第一變化率及所述第二變化率做差后，乘以所述時間增量δk，得到所述時間增量δk內對應的變化率差值之前，還包括：獲取陀螺儀對應的發散率β；利用所述發散率β對所述第二變化率進行調整，得到調整后的第二變化率；對所述第一變化率及所述調整后的第二變化率做差后，乘以所述時間增量δk，得到所述時間增量δk內對應的變化率差值。
11、優選地，所述利用所述發散率β對所述第二變化率進行調整，得到調整后的第二變化率的方法，包括：所述發散率β乘以所述第二變化率得到調整后的第二變化率
12、優選地，所述角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率確定方法，包括：獲取第一變化率預設計算公式及其相應的第一組計算參數，將所述第一組計算參數帶入所述第一變化率預設計算公式，確定所述角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率其中，所述第一組計算參數，包括：所述k時刻的上一k-1時刻對應的四元數姿態最優估計值陀螺儀在傳感器坐標系s中的偏對稱矩陣、所述角速度矢量測量值sωm。
13、優選地，在所述將所述第一組計算參數帶入所述第一變化率預設計算公式，確定所述角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率之前，對所述第一組計算參數中的所述角速度矢量測量值sωm進行歸一化處理。
14、優選地，確定所述第一變化率預設計算公式的方法，包括：在計算k+1時刻對應的四元數q(k+δk)及k時刻對應的四元數q(k)的差值后，除以所述k+1時刻與所述k時刻之間的時間增量對應的時間增量δk，并在δk→0的條件下求取極限。
15、優選地，所述計算k+1時刻對應的四元數及k時刻對應的四元數的差值的方法，包括：獲取在時間區間[k,k+δk]內對應的四元數變化量δq，利用所述k時刻對應的四元數q(k)及所述四元數變化量δq確定所述k+1時刻對應的四元數q(k+δk)；計算所述k+1時刻對應的四元數q(k+δk)與所述k時刻對應的四元數q(k)的差值。
16、優選地，所述利用所述k時刻對應的四元數q(k)及所述四元數變化量δq確定所述k+1時刻對應的四元數q(k+δk)的方法，包括：按照設定階數對所述四元數變化量δq進行泰勒展開，得到泰勒展開多項式；忽略所述泰勒展開多項式中所述設定階數對應的高階項，得到設定階數多項式；利用所述設定階數多項式乘以所述k時刻對應的四元數q(k)，確定所述k+1時刻對應的四元數q(k+δk)。
17、優選地，所述在時間區間[k,k+δk]內對應的四元數變化量δq的確定方法，包括：獲取陀螺儀的單位旋轉軸向量及時間增量δk下陀螺儀的旋轉角度；分別對所述旋轉角度機械能余弦計算及正弦計算，得到對應的余弦分量及正弦分量；所述單位旋轉軸向量乘以所述正弦分量后，加上所述余弦分量，得到在時間區間[k,k+δk]內對應的四元數變化量δq。
18、優選地，所述加速度矢量測量值sam在k時刻對應的第二四元數姿態估計值的第二變化率的確定方法，包括：獲取求解所述k時刻四元數的第一預設迭代公式及對應的第二組計算參數；將所述第二組計算參數帶入所述第一預設迭代公式，得到求解所述k時刻四元數其中，所述第二組本文檔來自技高網...
【技術保護點】
1.一種基于多傳感器融合的姿態估計方法，其特征在于，包括：
2.根據權利要求1所述的姿態估計方法，其特征在于，所述基于所述序列和對應的數值與所述第一預設閾值和/或所述二預設閾值的大小關系，利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計或利用加速度矢量測量值sam、角速度矢量測量值sωm及磁力計測量值smm進行姿態估計或維持上一時刻的融合模式的方法，包括：若所述序列和對應的數值大于或等于所述第一預設閾值，利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計；若所述序列和對應的數值小于或等于所述第二預設閾值，利用加速度矢量測量值sam、角速度矢量測量值sωm及磁力計測量值smm進行姿態估計；若所述序列和對應的數值在所述第二預設閾值及所述第一預設閾值之間，維持上一時刻的融合模式進行姿態估計。
3.根據權利要求1-2任一項所述的姿態估計方法，其特征在于，所述利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計的方法，包括：獲取角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率所述加速度矢量測量值sam在k時刻對
4.根據權利要求3所述的姿態估計方法，其特征在于，所述角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率確定方法，包括：獲取第一變化率預設計算公式及其相應的第一組計算參數，將所述第一組計算參數帶入所述第一變化率預設計算公式，確定所述角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率其中，所述第一組計算參數，包括：所述k時刻的上一k-1時刻對應的四元數姿態最優估計值陀螺儀在傳感器坐標系s中的偏對稱矩陣、所述角速度矢量測量值sωm；和/或，
5.根據權利要求3-4任一項所述的姿態估計方法，其特征在于，所述加速度矢量測量值sam在k時刻對應的第二四元數姿態估計值的第二變化率的確定方法，包括：獲取求解所述k時刻四元數的第一預設迭代公式及對應的第二組計算參數；將所述第二組計算參數帶入所述第一預設迭代公式，得到求解所述k時刻四元數其中，所述第二組計算參數，包括：調節參數λ、設定階數的單位矩陣I、目標函數及其對應的雅可比矩陣所述k-1時刻對應的四元數姿態最優估計值其中，所述目標函數描述通過所述k-1時刻對應的四元數姿態最優估計值旋轉地球坐標系e下的理論場向量使其匹配到所述k時刻的加速度計在傳感器坐標系s中的測量向量ss對應的加速度矢量測量值其中，所述調節參數λ與所述設定階數的單位矩陣I的乘積λI用于實現步長動態調節；其中，所述理論場向量配置為加速度計在地球坐標系e下測量的重力向量基于所述求解所述k時刻四元數及所述k時刻上一k-1時刻四元數確定所述加速度矢量測量值sam在k時刻對應的第二四元數姿態估計值的第二變化率和/或，
6.根據權利要求1-5所述的姿態估計方法，其特征在于，所述根據所述磁力計測量值確定指示磁場變化方向的序列的方法，包括：根據所述磁力計測量值確定磁場強度的瞬時變化趨勢；利用符號函數對所述磁場強度的瞬時變化趨勢進行符號化，得到指示磁場變化方向的序列；和/或，
7.一種姿態估計系統，其特征在于，包括：
8.一種姿態估計系統，其特征在于，包括：
9.一種姿態估計系統，其特征在于，包括：
10.一種姿態估計系統，其特征在于，包括：
...【技術特征摘要】
1.一種基于多傳感器融合的姿態估計方法，其特征在于，包括：
2.根據權利要求1所述的姿態估計方法，其特征在于，所述基于所述序列和對應的數值與所述第一預設閾值和/或所述二預設閾值的大小關系，利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計或利用加速度矢量測量值sam、角速度矢量測量值sωm及磁力計測量值smm進行姿態估計或維持上一時刻的融合模式的方法，包括：若所述序列和對應的數值大于或等于所述第一預設閾值，利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計；若所述序列和對應的數值小于或等于所述第二預設閾值，利用加速度矢量測量值sam、角速度矢量測量值sωm及磁力計測量值smm進行姿態估計；若所述序列和對應的數值在所述第二預設閾值及所述第一預設閾值之間，維持上一時刻的融合模式進行姿態估計。
3.根據權利要求1-2任一項所述的姿態估計方法，其特征在于，所述利用加速度矢量測量值sam及角速度矢量測量值sωm進行姿態估計的方法，包括：獲取角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率所述加速度矢量測量值sam在k時刻對應的第二四元數姿態估計值的第二變化率所述k時刻的上一k-1時刻對應的四元數姿態最優估計值所述k時刻與所述k-1時刻之間的時間增量δk；對所述第一變化率及所述第二變化率做差后，乘以所述時間增量δk，得到所述時間增量δk內對應的變化率差值；對所述時間增量δk內對應的變化率差值與所述四元數姿態最優估計值進行求和，得到所述k時刻對應的姿態；和/或，
4.根據權利要求3所述的姿態估計方法，其特征在于，所述角速度矢量測量值sωm在k時刻對應的第一四元數姿態估計值的第一變化率確定方法，包括：獲取第一變化率預設計算公式及其相應的第一組計算參數，將所述第一組計算參數帶入所述第一變化率預設計算公式，確定所述角速度...
【專利技術屬性】
技術研發人員：魏明生，孫大龍，李世黨，朱士虎，李光潤，
申請(專利權)人：江蘇師范大學，
類型：發明
國別省市：
還沒有人留言評論。發表了對其他瀏覽者有用的留言會獲得科技券。