[發明專利]一種云臺姿態調整方法及裝置

在審

• 申請號: 201911152827.3
• 申請日: 2019-11-20
• 公開（公告）號: CN112824829A
• 公開（公告）日: 2021-05-21
• 發明（設計）人: 王潔
• 申請（專利權）人: 杭州海康機器人技術有限公司
• 主分類號: G01C21/00
• 分類號: G01C21/00;G06F17/16;G06F17/18
• 代理公司: 北京柏杉松知識產權代理事務所(普通合伙) 11413
• 代理人: 馬敬;李欣

本發明實施例提供了一種云臺姿態調整方法及裝置，按照預設周期，獲取云臺設備的角速度量測值及姿態量測四元數；計算云臺設備的姿態預測四元數及協方差預測值；判斷姿態量測四元數及姿態預測四元數是否滿足姿態奇異條件；若滿足，將姿態量測四元數轉換為歐拉角，對歐拉角中的航向角取反，對航向角取反后的歐拉角進行轉換，得到更新后的姿態量測四元數，計算更新后的姿態量測四元數與姿態預測四元數之差，得到第二量測新息值；計算得到云臺設備在當前周期的姿態估計四元數；基于云臺設備在當前周期的姿態估計四元數對云臺設備的姿態進行調整。從而減少云臺設備的姿態發散的問題，實現對云臺設備的全姿態調整。

技術領域

本發明涉及云臺控制技術領域，特別是涉及一種云臺姿態調整方法及裝置。

背景技術

隨著攝像機的應用場景越來越復雜，對搭載攝像機的云臺對各種應用場景的適應性要求也越來越高。在一些場景中，需要云臺能夠不受結構與載體姿態的限制，進行全姿態工作。

而在相關技術中，通常采用歐拉角量測的EKF(Extended Kalman Filter，擴展卡爾曼濾波)模型進行云臺姿態調整，但是，這種模型會存在姿態奇異點，當云臺到達姿態奇異點時，很容易出現發散、亂轉現象，難以實現對云臺姿態的控制。

因此，目前亟需一種能夠實現云臺全姿態調整的云臺姿態調整方法。

發明內容

本發明實施例的目的在于提供一種云臺姿態調整方法及裝置，以實現對云臺的全姿態調整。具體技術方案如下：

本發明實施例提供了一種云臺姿態調整方法，所述方法包括：

按照預設周期，獲取云臺設備的角速度量測值及姿態量測四元數；

根據所述角速度量測值、所述預設周期、所述云臺設備在上一周期的姿態估計四元數、所述云臺設備的角速度偏置及所述云臺設備在上一周期的協方差估計值，計算所述云臺設備的姿態預測四元數及協方差預測值；

判斷所述姿態量測四元數及所述姿態預測四元數是否滿足姿態奇異條件；

若滿足，將所述姿態量測四元數轉換為歐拉角，對所述歐拉角中的航向角取反，對航向角取反后的歐拉角進行轉換，得到更新后的姿態量測四元數，計算更新后的姿態量測四元數與所述姿態預測四元數之差，得到第二量測新息值；

利用擴展卡爾曼濾波算法，基于所述角速度偏置、所述姿態預測四元數、所述協方差預測值及所述第二量測新息值，計算得到所述云臺設備在當前周期的姿態估計四元數；

基于所述云臺設備在當前周期的姿態估計四元數，對所述云臺設備的姿態進行調整。

可選的，在所述姿態量測四元數及所述姿態預測四元數不滿足姿態奇異條件的情況下，所述方法還包括：

利用所述擴展卡爾曼濾波算法，基于所述角速度偏置、所述姿態預測四元數、所述協方差預測值及第一量測新息值，計算得到所述云臺設備的當前姿態估計四元數，所述第一量測新息值為所述姿態量測四元數及所述姿態預測四元數之差。

可選的，所述角速度量測值為三軸角速度量測值；所述根據所述角速度量測值、所述預設周期、所述云臺設備在上一周期的姿態估計四元數、所述云臺設備的角速度偏置及所述云臺設備在上一周期的協方差估計值，計算所述云臺設備的姿態預測四元數及協方差預測值，包括：

根據所述三軸角速度量測值、所述預設周期及所述云臺設備的角速度偏置，計算所述云臺設備的三軸角度增量；

將所述三軸角度增量轉換為旋轉角度；

根據所述三軸角度增量及所述旋轉角度，計算所述云臺設備的旋轉軸；

根據所述旋轉角度及所述旋轉軸，計算所述云臺設備的姿態變化量；