[發(fā)明專利]基于磁梯度的運動速度估計方法及裝置

有效

• 申請?zhí)枺?/td>: 202210069819.8
• 申請日: 2022-01-21
• 公開（公告）號: CN114526730B
• 公開（公告）日: 2023-06-30
• 發(fā)明（設(shè)計）人: 周璇;梁列全;彭宏偉;杜梓鈞
• 申請（專利權(quán)）人: 華南理工大學(xué);廣東財經(jīng)大學(xué);上海創(chuàng)感傳感技術(shù)有限公司;廣州市合天信息科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 廣州市華學(xué)知識產(chǎn)權(quán)代理有限公司 44245
• 代理人: 李斌

1.基于磁梯度的運動速度估計方法，其特征在于，包括以下步驟：

S1、觸發(fā)航姿參考系統(tǒng)并同步引發(fā)磁傳感器陣列模塊對載體進行測量；根據(jù)航姿參考系統(tǒng)返回的姿態(tài)四元數(shù)數(shù)據(jù)計算旋轉(zhuǎn)矩陣，根據(jù)航姿參考系統(tǒng)返回的角速度數(shù)據(jù)計算角速度反對稱陣；步驟S1具體為：

S11、配置MCU控制模塊中的定時器，觸發(fā)各磁傳感器的測量動作，實現(xiàn)航姿參考系統(tǒng)的數(shù)據(jù)以固定的頻率進行輸出；

S12、測量動作觸發(fā)外部中斷引腳電平跳變，引發(fā)航姿參考系統(tǒng)測量動作輸出引腳上的電平進行高低轉(zhuǎn)換；依次觸發(fā)磁傳感器陣列模塊中的各個磁傳感器，使磁傳感器陣列模塊能根據(jù)外部中斷輸入引腳上的電平變換觸發(fā)各個磁傳感器進行測量動作；

S13、根據(jù)航姿參考系統(tǒng)測量返回的姿態(tài)四元數(shù)數(shù)據(jù)，MCU控制模塊計算旋轉(zhuǎn)矩陣，計算公式為：

其中q₀、q₁、q₂、q₃為姿態(tài)四元數(shù)分量；

S14、根據(jù)航姿參考系統(tǒng)測量返回的角速度數(shù)據(jù)，MCU控制模塊計算角速度反對稱陣，計算公式為：

其中ω_x、ω_y、ω_z分別為x、y、z軸的角速度量測；

S2、采集磁傳感器陣列模塊中各磁傳感器數(shù)據(jù)，計算得到磁場梯度，同時根據(jù)航姿參考系統(tǒng)返回的磁場數(shù)據(jù)計算磁場矢量；S2具體為：

S21、在航姿參考系統(tǒng)返回數(shù)據(jù)后，判斷MCU控制模塊與航姿參考系統(tǒng)之間的SPI通信使能引腳的電平狀況，當使能引腳從低電平跳變?yōu)楦唠娖剑幢砻鱉CU控制模塊與航姿參考系統(tǒng)之間的SPI通信數(shù)據(jù)傳輸完成，此時立即采集磁傳感器陣列模塊中各個磁傳感器的測量數(shù)據(jù)；

S22、對各個磁傳感器的測量數(shù)據(jù)進行濾波與補償；整合濾波及補償后的各磁傳感器數(shù)據(jù)，分別表示為B_A、B_B、B_C和B_D：

其中，B_Ax、B_Ay、B_Az分別表示磁傳感器A在x、y、z軸上的測量數(shù)據(jù)；B_Bx、B_By、B_Bz分別表示磁傳感器B在x、y、z軸上的測量數(shù)據(jù)；B_Cx、B_Cy、B_Cz分別表示磁傳感器C在x、y、z軸上的測量數(shù)據(jù)；B_Dx、B_Dy、B_Dz分別表示磁傳感器D在x、y、z軸上的測量數(shù)據(jù)；

S23、計算得到磁梯度G，公式為：

其中，L為十字形傳感器陣列的基線距離；

S24、整合航姿參考系統(tǒng)返回的磁場數(shù)據(jù)，得到裝置中心點附近的磁場矢量B^b；

S3、將計算得到的旋轉(zhuǎn)矩陣、角速度反對稱陣、磁場梯度及磁場矢量數(shù)據(jù)輸入MCU控制模塊中進行計算，得到載體運動速度；步驟S3具體為：

S31、計算磁場矢量對時間的導(dǎo)數(shù)：

其中B_x、B_y、B_z為磁場矢量B^b在x、y、z軸的分量；ΔT為時間步長，由傳感器測量與數(shù)據(jù)刷新頻率確定；

S32、整合旋轉(zhuǎn)矩陣、角速度反對稱陣、磁梯度數(shù)據(jù)、磁場矢量與磁場矢量對時間的導(dǎo)數(shù)，通過克拉默法則求解非齊次線性方程組：

其中V為載體相對慣性空間的運動速度矢量。

2.根據(jù)權(quán)利要求1所述基于磁梯度的運動速度估計方法，其特征在于，所述測量動作輸出引腳是指MCU控制模塊與航姿參考系統(tǒng)之間互聯(lián)的GPIO引腳；所述外部中斷輸入引腳是指MCU控制模塊與航姿參考系統(tǒng)的測量動作引腳相連的引腳。

3.根據(jù)權(quán)利要求1所述基于磁梯度的運動速度估計方法，其特征在于，所述SPI通信使能引腳的電平通過將使能引腳與MCU控制模塊外部中斷輸入進行連接，并將使能引腳的中斷觸發(fā)條件設(shè)置為上升沿觸發(fā)來進行判別；所述磁傳感器陣列模塊上各個磁傳感器數(shù)據(jù)的采集在外部中斷的中斷處理函數(shù)中進行。