[發(fā)明專利]一種基于擴展卡爾曼濾波的手持云臺姿態(tài)估計方法有效
| 申請?zhí)枺?/td> | 202010427203.4 | 申請日： | 2020-05-19 |
| 公開（公告）號： | CN111625768B | 公開（公告）日： | 2023-05-30 |
| 發(fā)明（設(shè)計）人： | 陳朋印;李定涌;閆永馳;陳柯柯;馬一鳴;曾幼涵 | 申請（專利權(quán)）人： | 西安因諾航空科技有限公司 |
| 主分類號： | G06F17/16 | 分類號： | G06F17/16;G06F17/11 |
| 代理公司： | 西安通大專利代理有限責任公司 61200 | 代理人： | 賀小停 |
| 地址： | 710119 陜西省西安市*** | 國省代碼： | 陜西;61 |
| 權(quán)利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關(guān)鍵詞： | 一種 基于 擴展 卡爾 濾波 手持 姿態(tài) 估計 方法 | ||
一種基于擴展卡爾曼濾波的手持云臺姿態(tài)估計方法，包括以下步驟：步驟1，選取姿態(tài)估計系統(tǒng)的狀態(tài)向量，構(gòu)建狀態(tài)向量的預(yù)測方程；步驟2，根據(jù)量測預(yù)測方程計算量測預(yù)測值的雅克比矩陣，此矩陣為量測矩陣，完成姿態(tài)估計系統(tǒng)模型的建立；步驟3，將擴展卡爾曼濾波器編寫為序貫濾波的形式，完成手持云臺姿態(tài)估計。本發(fā)明提出的一種基于擴展卡爾曼濾波的手持云臺姿態(tài)估計方法首先建立姿態(tài)估計狀態(tài)模型，然后建立量測模型，最后根據(jù)選取的傳感器設(shè)計濾波參數(shù)，最后進行序貫濾波形式的擴展卡爾曼濾波并輸出手持云臺高精度的姿態(tài)信息與速度信息。
技術(shù)領(lǐng)域
本發(fā)明屬于自動控制技術(shù)領(lǐng)域，特別涉及一種基于擴展卡爾曼濾波的手持云臺姿態(tài)估計方法。
背景技術(shù)
目前，云臺在航拍上應(yīng)用最廣泛，然而在日場生活中，人們應(yīng)用最多的攝影設(shè)備是手機，在手機攝影時面臨著畫面抖動的問題。雖然目前攝影設(shè)備的光學(xué)防抖技術(shù)得到了長足發(fā)展，但相機內(nèi)的鏡頭組只能對微小的運動位移進行補償，為了拍攝到穩(wěn)定的畫面，手必須要長期保持穩(wěn)定的狀態(tài)，所以引入手持云臺來進行更大范圍的運動補償是十分必要的。手持云臺是近兩年才興起的，其原理與機載云臺的原理是相通的，均是通過姿態(tài)傳感器估計云臺的姿態(tài)，向電機反饋當前的姿態(tài)信息，電機輸出轉(zhuǎn)矩進行高效率的運動補償，從而使姿態(tài)始終保持穩(wěn)定，最終獲得穩(wěn)定的拍攝畫面，在手持云臺系統(tǒng)中，姿態(tài)估計是最重要的環(huán)節(jié)。
在姿態(tài)估計技術(shù)上，經(jīng)典的一階互補濾波姿態(tài)估計算法由于算法實現(xiàn)簡單，計算量小，被廣泛地應(yīng)用于各工程項目中，但是，一階互補濾波算法的性能有限，不能很好地適應(yīng)有高頻擾動輸入的環(huán)境；基于歐拉角的姿態(tài)估計，其算法直觀，易于理解，因此被廣泛地應(yīng)用于各種運動控制系統(tǒng)當中，但歐拉角存在萬向節(jié)死鎖問題。
發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種基于擴展卡爾曼濾波的手持云臺姿態(tài)估計方法，以解決上述問題。
為實現(xiàn)上述目的，本發(fā)明采用以下技術(shù)方案：
一種基于擴展卡爾曼濾波的手持云臺姿態(tài)估計方法，包括以下步驟：
步驟1，選取姿態(tài)估計系統(tǒng)的狀態(tài)向量，構(gòu)建狀態(tài)向量的預(yù)測方程，根據(jù)狀態(tài)向量的預(yù)測方程計算狀態(tài)預(yù)測的雅克比矩陣，此矩陣為狀態(tài)轉(zhuǎn)移矩陣；
步驟2，根據(jù)姿態(tài)估計系統(tǒng)中的傳感器選取量測并結(jié)合狀態(tài)向量構(gòu)造量測預(yù)測方程，然后根據(jù)量測預(yù)測方程計算量測預(yù)測值的雅克比矩陣，此矩陣為量測矩陣，完成姿態(tài)估計系統(tǒng)模型的建立；
步驟3，將擴展卡爾曼濾波器編寫為序貫濾波的形式，完成手持云臺姿態(tài)估計。
進一步的，步驟1中，狀態(tài)向量包括手持云臺的姿態(tài)四元數(shù)、速度、陀螺零偏、加速度計零偏、北東地方向地磁場分量和磁力計零偏，狀態(tài)向量表示為：
其中q0為手持云臺姿態(tài)四元數(shù)實部，q1、q2、q3為手持云臺姿態(tài)四元數(shù)虛部，vn為手持云臺北向速度，ve為手持云臺東向速度，vd為手持云臺地向速度，εx為x軸陀螺零偏，εy為y軸陀螺零偏，εz為z軸陀螺零偏，為x軸加速度計零偏，為y軸加速度計零偏，為z軸加速度計零偏，mn為北向地磁場分量，me為東向地磁場分量，md為地向地磁場分量，mbx為x軸磁力計零偏，mby為y軸磁力計零偏，mbz為z軸磁力計零偏；
狀態(tài)方程為：
其中：Xk-1——k-1時刻狀態(tài)向量；Φk/k-1——狀態(tài)轉(zhuǎn)移矩陣；——k時刻狀態(tài)向量的預(yù)測。
該專利技術(shù)資料僅供研究查看技術(shù)是否侵權(quán)等信息，商用須獲得專利權(quán)人授權(quán)。該專利全部權(quán)利屬于西安因諾航空科技有限公司，未經(jīng)西安因諾航空科技有限公司許可，擅自商用是侵權(quán)行為。如果您想購買此專利、獲得商業(yè)授權(quán)和技術(shù)合作，請聯(lián)系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/202010427203.4/2.html，轉(zhuǎn)載請聲明來源鉆瓜專利網(wǎng)。
