[發明專利]水田拖拉機位姿檢測和偏航角提取的方法及裝置

有效

• 申請號: 201811328662.6
• 申請日: 2018-11-09
• 公開（公告）號: CN109540135B
• 公開（公告）日: 2020-07-31
• 發明（設計）人: 齊龍;李帥;馬旭;龔浩;陶明;劉闖;黃旭楠;林潔平;謝梓釗;陳釗國
• 申請（專利權）人: 華南農業大學
• 主分類號: G01C21/18
• 分類號: G01C21/18;G01C21/08;G01C21/06
• 代理公司: 廣州市華學知識產權代理有限公司 44245
• 代理人: 李君

1.一種水田拖拉機位姿檢測和偏航角提取的方法，其特征在于，所述方法包括：

通過三軸加速度傳感器測量水田拖拉機三個坐標軸上的加速度數據，從而得到水田拖拉機各個坐標軸與重力之間的夾角；

通過三軸陀螺儀傳感器測量水田拖拉機繞坐標軸轉動的角速度數據，從而得到水田拖拉機轉動的角度；

通過三軸地磁傳感器測量水田拖拉機各個方向的磁場數據，從而得到水田拖拉機的偏航角；

采用互補濾波算法將三軸加速度傳感器測量的角度數據和三軸陀螺儀傳感器測量的角度數據進行融合；

采用姿態數據融合算法將三軸陀螺儀傳感器測量的角度數據和三軸地磁傳感器測量的偏航角數據進行融合；

所述采用互補濾波算法將三軸加速度傳感器測量的角度數據和三軸陀螺儀傳感器測量的角度數據進行融合，具體為：

通過三軸加速度傳感器測量的角度數據和上一次融合后的角度數據對三軸陀螺儀傳感器的角速度數據進行修改，然后再進行積分；當水田拖拉機處于靜止狀態時，增大三軸加速度傳感器測量的角度數據權重，當水田拖拉機處于運動狀態時，增大三軸陀螺儀傳感器測量的角度數據的權重；

所述采用姿態數據融合算法將三軸陀螺儀傳感器測量的角度數據和三軸地磁傳感器測量的偏航角數據進行融合，具體為：

將三軸陀螺儀傳感器測量的角度數據和三軸地磁傳感器測量的偏航角數據進行融合；當水田拖拉機處于靜止狀態時，增大三軸地磁傳感器測量的偏航角數據的權重，當水田拖拉機處于運動狀態時，增大三軸陀螺儀傳感器測量的角度數據的權重。

2.根據權利要求1所述的水田拖拉機位姿檢測和偏航角提取的方法，其特征在于，所述方法還包括：

根據陀螺儀傳感器的溫度零點偏移對陀螺儀傳感器的溫度進行補償。

3.根據權利要求1所述的水田拖拉機位姿檢測和偏航角提取的方法，其特征在于，所述方法還包括：

通過三軸加速度傳感器輸入輸出加速度數據，通過三軸陀螺儀傳感器輸入輸出角速度數據，以及通過三軸地磁傳感器輸入輸出磁場數據，利用卡爾曼濾波算法對各傳感器狀態進行最優估計，并通過最優估計去除各傳感器中噪聲和干擾對測量數據的影響。

4.根據權利要求1所述的水田拖拉機位姿檢測和偏航角提取的方法，其特征在于，所述方法還包括：

通過卡爾曼濾波融合算法對采集到的加速度數據、角速度數據和磁場數據進行誤差補償和濾波，將三軸陀螺儀傳感器測量的角度數據作為卡爾曼濾波器的預測值，并估計過程噪聲協方差；將三軸加速度傳感器測量的角度數據和三軸地磁傳感器測量的偏航角作為測量值，并結合三軸陀螺儀傳感器誤差估計測量噪聲協方差矩陣，實現三軸加速度傳感器、三軸陀螺儀傳感器和三軸地磁傳感器的融合。

5.根據權利要求1所述的水田拖拉機位姿檢測和偏航角提取的方法，其特征在于，所述方法還包括：

利用三軸陀螺儀傳感器測量的角速度數據作為四元數的更新，以三軸加速度傳感器測量的重力加速度數據作為四元數的觀測，實時解算姿態角。