【技術實現步驟摘要】
本專利技術涉及無人機控制，特別涉及一種新型的多旋翼無人機姿態解算濾波方法。
技術介紹
1、姿態信息是導航系統中最關鍵的參數之一，姿態測量系統在航空航天、無人機、機器人和運動跟蹤等領域具有廣泛的應用。姿態解算是飛行器進行導航定位的基礎，要想實現穩定飛行和控制，必須保證姿態解算的精度。
2、多旋翼無人機的姿態解算通過搭載加速度計、陀螺儀以及磁力計等多種傳感器，構建一個姿態解算平臺，求解出無人機的實時姿態參數。但通過陀螺儀測量會產生漂移，長期靜態性差，短期漂移誤差不明顯；加速度計測量會產生高頻噪聲，導致短時精度差；磁力計受外界干擾較大，穩定性較差。隨著時間的累積系統自身的誤差較大，嚴重影響解算精度。
3、針對傳感器存在的噪聲問題，通常采用濾波算法求解姿態信息，降低噪聲誤差對多旋翼無人飛行器姿態解算的干擾。因此國內外的學者研究了許多的濾波算法。
4、浙江大學楊清泉等人于2018年在journal?of?navigation期刊發布的名為《afast?adaptive-gain?complementary?filter?algorithm?for?attitude?estimation?ofan?unmanned?aerial?vehicle》的論文，提出一種用于無人機?（uav）?姿態估計的新型快速自適應增益互補濾波算法，減少了姿態誤差和計算量，但互補濾波算法對于高通濾波和低通濾波的頻率選取較為困難，因此姿態解算精度不高。
5、加拿大約克大學daero?lee等人于2017年在journa
6、合肥賽為智能有限公司辛艷峰等人公布的《一種基于最小二乘法與ekf的無人機姿態解算方法及系統》專利技術專利（專利號：cn202010982173.3），將經過最小二乘法計算處理的imu數據導入擴展卡爾曼濾波算法模塊中，對無人機的姿態進行解算。但ekf忽略了泰勒展開后非線性函數的高階項，算法精度有限。
7、西安建筑科技大學蔡安江等人的公布的《一種小型多旋翼無人機姿態解算方法》（專利號：cn201810275931.0），使用改進的ukf濾波算法將加速度數據和磁力計數據分為兩個階段進行處理，降低量測向量的維度，減少計算量，但此方法存在傳感器噪聲干擾的問題。
8、綜上所述，現有技術仍存在解算精度低和適應性差的問題。由于系統噪聲和量測噪聲的統計特性未知且時變，使常規濾波算法精度降低，失去算法最優性，甚至導致濾波發散。
技術實現思路
1、針對上述提出的現有技術存在的缺陷與不足，本專利技術提出了一種迭代姿態解算濾波方法。主要包括以下步驟：
2、第一，選用北東地右手坐標系構建無人機姿態解算系統模型，根據陀螺儀數據的四元數姿態矩陣q作為系統的狀態向量?？紤]到環境因素引起的陀螺儀偏移，建立狀態方程，表示為：
3、(1)
4、(2)
5、式中，是k時刻的狀態估計值，a是狀態轉移矩陣，?和分別是陀螺儀數據、、和陀螺儀偏移組成的矩陣。是服從高斯分布的過程噪聲向量。
6、基于三軸加速度計和三軸磁力計數據建立觀測方程，校正由陀螺儀漂移造成的姿態角累積誤差。使用三軸加速度計測量運動狀態下載體的三軸加速度a，在地理坐標系下重力加速度；使用三軸磁力計可測得地磁場強度在機體坐標系下的三個分量。加速度計和磁力計的輸出分別表示為：
7、(3)
8、其中，?是地理坐標系旋轉到機體坐標系的旋轉矩陣，選取觀測量建立za表示加速度計的輸出，an為重力分量，na為加速度計的零均值白噪聲；zm表示磁力計的輸出，mn為磁場強度分量，nm為磁力計的零均值白噪聲。
9、系統的觀測方程，具體表示為：
10、(4)
11、式中，?h為量測系數矩陣，?是服從高斯分布的觀測噪聲向量；
12、其中，和為互不相關的零均值高斯白噪聲，其噪聲協方差矩陣分別為?，。
13、第二，針對我們需要優化的兩個噪聲協方差=q和=r，進行小批量梯度下降算法取最優值。首先選擇姿態的參考數據與通過實驗得到的解算數據比較獲得的均方根誤差（rmse）作為目標函數：
14、(5)
15、式中，?為采樣點數，為解算的姿態角，為參考的姿態角。
16、該目標函數對我們需要求解的兩個參數分別求其偏導得到梯度分別為：
17、(6)
18、式中，為采樣點數，、是分別對兩個參數求偏導時的第i個輸入x值。
19、利用得到的梯度對參數q、r進行迭代更新，則有：
20、(7)
21、當目標函數收斂至最小值便可以得到最優值和。
22、第三，通過參考無跡卡爾曼濾波中的無跡變換原理，傳播迭代出的最優協方差。
23、?(1)根據第一個步驟設定的狀態方程，確定初始狀態估計值及其協方差。根據狀態向量的維數，通過ut變換獲取一組sigma采樣點及其對應的權重。
24、(8)
25、式中，是縮放比例參數，、、是待選參數。表示矩陣方根第i列。
26、(9)
27、式中，采樣點權值，采樣點方差權值。
28、(2)計算2n+1個sigma點集的一步預測，得到（i=1,2,...,2n+1）；根據sigma點集的預測值加權求和得到系統狀態量的一步預測及協方差矩陣：
29、(10)
30、(3)根據系統狀態量的一步預測值，再次使用ut變換，產生新的sigma點集。
31、(11)
32、(4)將預測的sigma點集帶入觀測方程，得到預測的觀測量：
33、(12)
34、(5)通過加權求和得到觀測變量的預測值和協方差矩陣：
35、(13)
36、(6)計算卡爾曼濾波增益：
37、(14)
38、(7)最后計算狀態更新和協方差更新：
39、(15)
40、式中，?為加速度計真實測量值，，只更新四元數，為濾波后的最優姿態角。
41、本專利技術的有益之處在于創新性的提出了一種對濾波參數噪聲協方差進行自適應調整的方法，對本不確定的兩個噪聲協方差q和r進行自適應估計。通過將小批量梯度下降法原理引入到ukf算法中，使得ukf算法中的q和r從經驗估計值變為最優值，噪聲的實時估計精度和穩定性得以提升，提高了傳統ukf算法的姿態解算精度和魯棒性。
本文檔來自技高網...【技術保護點】
1.一種多旋翼無人機的迭代姿態解算濾波方法，采用四元數作為狀態向量，使用陀螺儀數據建立狀態方程對狀態向量進行更新，利用加速度數據和磁力計數據立觀測方程，校正由陀螺儀漂移造成的姿態角累積誤差；利用小批量梯度下降法，對本不確定的兩個噪聲協方差Q和R進行自適應估計，使其從經驗估計值變為最優值；利用改進的無跡卡爾曼濾波方法對無人機姿態進行解算，傳播迭代出的最優協方差，進行四元數姿態更新，得到四旋翼無人機的解算姿態角。
2.根據權利要求1所述的一種多旋翼無人機的迭代姿態解算濾波方法，其特征在于：以姿態四元數為狀態向量，結合狀態方程和觀測方程建立一個狀態空間模型，利用加速度計和磁力計得到的橫滾角、俯仰角以及偏航角來修正由陀螺儀更新后的姿態角；該模型可揭示系統狀態的變化規律；
3.根據權利要求1所述的一種多旋翼無人機的迭代姿態解算濾波方法，其特征在于：選擇姿態的參考數據與通過實驗得到的解算數據比較獲得的均方根誤差（RMSE）作為目標函數，對噪聲協方差Q和R進行小批量梯度下降算法取最優值
4.根據權利要求1所述的一種多旋翼無人機的迭代姿態解算濾波方法，其特征在
...【技術特征摘要】
1.一種多旋翼無人機的迭代姿態解算濾波方法，采用四元數作為狀態向量，使用陀螺儀數據建立狀態方程對狀態向量進行更新，利用加速度數據和磁力計數據立觀測方程，校正由陀螺儀漂移造成的姿態角累積誤差；利用小批量梯度下降法，對本不確定的兩個噪聲協方差q和r進行自適應估計，使其從經驗估計值變為最優值；利用改進的無跡卡爾曼濾波方法對無人機姿態進行解算，傳播迭代出的最優協方差，進行四元數姿態更新，得到四旋翼無人機的解算姿態角。
2.根據權利要求1所述的一種多旋翼無人機的迭代姿態解算濾波方法，其特征在于：以姿態四元數為狀態向量，結合狀態方程和觀測方程建立一個狀...
【專利技術屬性】
技術研發人員：周國清，周瀟，梅樹紅，謝宗音，羅恒，雷江濤，李彬，
申請(專利權)人：桂林理工大學，
類型：發明
國別省市：
還沒有人留言評論。發表了對其他瀏覽者有用的留言會獲得科技券。