基于模糊自適應(yīng)PI的姿態(tài)解算算法研究

劉佳 謝富強(qiáng)

摘? 要：小型無(wú)人機(jī)姿態(tài)信息獲取的實(shí)時(shí)性與準(zhǔn)確性主要依靠于慣性測(cè)量單元的實(shí)測(cè)數(shù)據(jù)，本文針對(duì)慣性測(cè)量單元中陀螺儀長(zhǎng)時(shí)間工作會(huì)出現(xiàn)數(shù)據(jù)漂移的問(wèn)題，提出了一種使用加速度計(jì)數(shù)據(jù)基于模糊自適應(yīng)PI控制的互補(bǔ)濾波算法。最后，在由STM32F427VIT6所搭建的飛控系統(tǒng)平臺(tái)上將該姿態(tài)解算算法所得的姿態(tài)角與利用四元數(shù)和基于Mahony互補(bǔ)濾波算法解算出的姿態(tài)角進(jìn)行比較。研究結(jié)果表明：此算法能夠準(zhǔn)確獲取小型無(wú)人機(jī)的各項(xiàng)姿態(tài)信息，且響應(yīng)速度更優(yōu)于Mahony互補(bǔ)濾波算法，具有更強(qiáng)的實(shí)時(shí)性，能夠滿足小型無(wú)人機(jī)的控制要求。

關(guān)鍵詞：模糊自適應(yīng);方向余弦矩陣（DCM）;姿態(tài)解算;互補(bǔ)濾波

中圖分類號(hào)：TP241? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：2096-4706（2019）09-0139-03

0? 引? 言

小型無(wú)人機(jī)的飛行控制器上均會(huì)搭載一塊或多塊慣性測(cè)量單元（IMU），來(lái)實(shí)時(shí)獲取更新無(wú)人機(jī)的姿態(tài)數(shù)據(jù)。將傳感器數(shù)據(jù)轉(zhuǎn)換成無(wú)人機(jī)相對(duì)于地球的方位的過(guò)程就被稱為姿態(tài)解算，而此類方位可以表示成一個(gè)旋轉(zhuǎn)。表示旋轉(zhuǎn)的方法目前主要分為兩大類：方向余弦法和四元數(shù)法。雖然方向余弦法相較于四元數(shù)法使用的參數(shù)較多，但在實(shí)際應(yīng)用方面，方向余弦矩陣具有使用方便，計(jì)算量小，編程容易等特點(diǎn)，且二者的運(yùn)算精度與誤差趨勢(shì)基本相當(dāng)。

針對(duì)常用的慣性測(cè)量單元的姿態(tài)解算算法?；パa(bǔ)濾波算法也有了比較成熟的研究與應(yīng)用。互補(bǔ)濾波算法主要利用加速度計(jì)數(shù)據(jù)校準(zhǔn)陀螺儀的誤差，但是由于加速度計(jì)的數(shù)據(jù)易受外界干擾，當(dāng)互補(bǔ)濾波算法中的參數(shù)固定時(shí)，容易使互補(bǔ)濾波出現(xiàn)較大的誤差，造成姿態(tài)解算結(jié)果不理想，實(shí)時(shí)性差的問(wèn)題。對(duì)于這一問(wèn)題，國(guó)內(nèi)外進(jìn)行了大量的研究，出現(xiàn)了利用模糊自適應(yīng)算法優(yōu)化互補(bǔ)濾波算法參數(shù)的方法，該方法利用模糊自適應(yīng)優(yōu)化比例參數(shù)，提高了互補(bǔ)濾波解算的精度，但對(duì)積分參數(shù)的操作僅限于積分分離與積分限幅，解算的響應(yīng)時(shí)間并沒(méi)有提高。

針對(duì)上述問(wèn)題，本文借助MPU6000慣性測(cè)量單元，設(shè)計(jì)了基于模糊自適應(yīng)PI的姿態(tài)解算算法，根據(jù)加速度計(jì)與重力加速度參考向量的誤差同時(shí)優(yōu)化P、I參數(shù)，最終利用方向余弦矩陣求取出小型無(wú)人機(jī)的姿態(tài)角。

1? 平臺(tái)搭建

1.1? 整體結(jié)構(gòu)框圖

本研究以MPU6000為姿態(tài)傳感器，將姿態(tài)傳感器原始數(shù)據(jù)使用SPI通信發(fā)送至主控芯片STM32F427VIT6進(jìn)行姿態(tài)解算得出姿態(tài)角。由傳感器原始數(shù)據(jù)轉(zhuǎn)換為姿態(tài)角數(shù)據(jù)的整體控制框圖如圖1所示。

1.2? 慣性測(cè)量單元MPU6000簡(jiǎn)介

慣性測(cè)量單元（IMU）是測(cè)量物體三軸角速率以及加速度的裝置。一般地，一個(gè)IMU包含了三個(gè)單軸的加速度計(jì)和三個(gè)單軸的陀螺儀，分別測(cè)量物體在三維空間中的角速度和加速度并以此解算出物體的姿態(tài)。

MPU6000是9軸運(yùn)動(dòng)處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP（Digital Motion Processor）。

MPU6000對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動(dòng)，傳感器的測(cè)量范圍都是用戶可控的，其所有設(shè)備寄存器之間的通信均采用400kHz的I2C接口或1MHz的SPI接口。

3? 姿態(tài)解算結(jié)果分析

本研究使用兩塊基于STM32F427VIT6的以MPU6000為姿態(tài)傳感器的飛控板，將兩塊飛控板固定于同一平面上，且兩塊飛控的初始橫滾、俯仰角均為0°。當(dāng)該平面處于靜止?fàn)顟B(tài)時(shí)，使用同一姿態(tài)解算算法的二者姿態(tài)角誤差在0.5°左右。

在此前提下，一塊飛控板使用本文所設(shè)計(jì)的基于模糊自適應(yīng)PI的姿態(tài)解算算法，另一塊飛控板使用基于傳統(tǒng)Mahony互補(bǔ)濾波的姿態(tài)解算算法，并通過(guò)MATLAB分別繪制出二者的橫滾角與俯仰角數(shù)據(jù)，比較結(jié)果如圖3、圖4所示。

測(cè)試結(jié)果表明，隨著時(shí)間的推移，與基于傳統(tǒng)互補(bǔ)濾波的姿態(tài)解算算法相比，本文所設(shè)計(jì)的姿態(tài)解算算法響應(yīng)速度提高了4%～5%，具有更強(qiáng)的實(shí)時(shí)性。

4? 結(jié)? 論

本文介紹了一種基于模糊自適應(yīng)PID的姿態(tài)解算算法，利用姿態(tài)傳感器MPU6000的原始數(shù)據(jù)，根據(jù)加速度計(jì)與陀螺儀的實(shí)測(cè)誤差，使用模糊自適應(yīng)PI算法校正陀螺儀數(shù)據(jù)漂移，最終利用方向余弦矩陣計(jì)算出姿態(tài)角，并與基于Mahony互補(bǔ)濾波的姿態(tài)解算算法計(jì)算出的姿態(tài)角進(jìn)行對(duì)比。

結(jié)果表明，本文所設(shè)計(jì)的姿態(tài)解算算法具有更快的響應(yīng)速度，實(shí)時(shí)性更強(qiáng)。經(jīng)過(guò)實(shí)際的飛行測(cè)試，該算法能夠滿足一般小型無(wú)人機(jī)的飛行控制要求。

參考文獻(xiàn)：

[1] 尹劍，陳紅，楊萌，等.捷聯(lián)姿態(tài)計(jì)算中方向余弦與四元數(shù)法分析比較 [J].四川兵工學(xué)報(bào)，2015，36（9）：106-110.

[2] 陳國(guó)定，周鵬豪，胡朕豪，等.基于MPU6050的四軸硬件姿態(tài)解算研究 [J].機(jī)電工程，2018，35（1）：95-100.

[3] 周湛杰，王新生，王巖.基于模糊自適應(yīng)算法的航天器姿態(tài)控制 [J].電機(jī)與控制學(xué)報(bào)，2019，23（2）：123-128.

[4] 王焱春，張新英，梁芬.多傳感器信息融合的四旋翼姿態(tài)解算研究 [J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用，2018，18（8）：39-42.

作者簡(jiǎn)介：劉佳（1994-），男，漢族，湖南岳陽(yáng)人，本科，碩士，研究方向：智能控制與信息處理;通訊作者：謝富強(qiáng)（1971-），男，漢族，湖南衡山人，副教授，博士，研究方向：智能控制理論及應(yīng)用。