[發明專利]一種運動載體姿態實時測量方法、裝置

在審

• 申請號: 202111569311.6
• 申請日: 2021-12-21
• 公開（公告）號: CN114234970A
• 公開（公告）日: 2022-03-25
• 發明（設計）人: 趙祚喜;林旭;曹陽陽;邱志;張壯壯;黃淵;項波瑞;楊厚城;羅舒元;袁凱;米亞龍;夏丹燕;魏洪飛;王乾;陸俊君;廖志輝;賴仕俊
• 申請（專利權）人: 華南農業大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 北京京華知聯專利代理事務所(普通合伙) 11991
• 代理人: 李姣姣

本申請涉及一種運動載體姿態角實時測量方法、裝置。所述方法包括：獲取慣性傳感器實時采集的運動載體的角速度和加速度；根據所述角速度和加速度，通過滿足下降條件的共軛梯度法對姿態四元數進行尋優估計，獲得尋優估計值；分別建立角速度與姿態角的系統模型、加速度與姿態角的觀測模型，并根據系統模型和觀測模型建立無跡卡爾曼濾波遞推方程；將尋優估計值導入無跡卡爾曼濾波遞推方程中，計算得到運動載體姿態角。采用本方法能夠提高運動載體姿態角計算的準確度。

技術領域

本申請涉及姿態測量技術領域，特別是涉及一種運動載體姿態實時計算方法、裝置。

背景技術

運動載體包括飛行器、機器人、兩輪平衡車等技術項目的研究取得了巨大的進步，這些項目能夠完成偵察、監控以及危險環境下的搜救等任務。而在這些項目中，每一個都需要對載體或部件的姿態進行實時測量，這是實現姿態精確控制的基礎。

傳統測量運動載體姿態是采用兩臺高速攝像機進行測量，不僅設備價格昂貴，而且不能實現實時測量，只能進行后處理，而且操作繁瑣，不易快速得到結果；或單獨使用陀螺儀完成姿態角測量，誤差會隨時間積累，產生較大的誤差，短時間結果可靠長時間不可靠；或單獨使用加速度計完成姿態角測量在非平穩狀態下(靜止或勻速運動)結果有干擾，長時間結果可靠短時間不可靠。

總之，現有的運動載體姿態測量方式，不能準確的獲得實時姿態角。

發明內容

基于此，有必要針對上述技術問題，提供一種能夠提高運動載體實時姿態角計算的準確度的運動載體姿態角實時測量方法、裝置。

一種運動載體姿態角實時測量方法，所述方法包括：

獲取慣性傳感器實時采集的運動載體的角速度和加速度；

根據所述角速度和加速度，通過滿足下降條件的共軛梯度法對姿態四元數進行尋優估計，獲得尋優估計值；

分別建立角速度與姿態角的系統模型、加速度與姿態角的觀測模型，并根據系統模型和觀測模型建立無跡卡爾曼濾波遞推方程；

將尋優估計值導入無跡卡爾曼濾波遞推方程中，計算得到運動載體姿態角。

在其中一個實施例中，所述根據所述角速度和加速度，通過滿足下降條件的共軛梯度法對姿態四元數進行尋優估計，獲得尋優估計值，包括：

將運動載體的姿態角采用四元數表示，根據四元數與姿態矩陣的關系和歸一化重力加速度矢量，計算歸一化重力加速度矢量在k次采樣時刻載體坐標系下的投影值；

將第k次采樣時刻的加速度計在x、y、z軸上采集的加速度進行歸一化處理，得到加速度向量；

將投影值與加速度向量相減，得到k次采樣時刻測量誤差；

根據測量誤差構建關于姿態角的誤差函數，獲得在誤差函數最小值時的尋優估計值；

其中，在對四元數進行更新迭代時，根據最優步長和搜索方向作為迭代參數，修改搜索方向的方向調控參數，使得共軛梯度法滿足下降條件。

在其中一個實施例中，所述將運動載體的姿態角采用四元數表示，根據四元數與姿態矩陣的關系和歸一化重力加速度矢量，計算歸一化重力加速度矢量在k次采樣時刻載體坐標系下的投影值，具體為：

采用四元數表示姿態角用矩陣形式表達： q_k＝[q_k0 q_k1 q_k2 q_k4]^T；

其中，q_k表示k次采樣時刻的四元數，根據四元數q_k與姿態矩陣的關系得到姿態矩陣