本發(fā)明涉及增穩(wěn)設(shè)備的控制與姿態(tài)解算方法，常見(jiàn)應(yīng)用與手持三軸增穩(wěn)云臺(tái)、飛行器增穩(wěn)云臺(tái)航向角控制領(lǐng)域中。

背景技術(shù)：

目前的增穩(wěn)設(shè)備中航向角往往因?yàn)殡姍C(jī)磁場(chǎng)的影響，或者加速度計(jì)解算不出航向角的原因，往往只采用陀螺儀來(lái)進(jìn)行增穩(wěn)設(shè)備的航向軸的姿態(tài)解算，其結(jié)果是可以滿足動(dòng)態(tài)響應(yīng)的要求，但是在靜止情況下由于陀螺儀的零漂往往會(huì)造成航向軸嚴(yán)重漂移的問(wèn)題，因此需要一個(gè)既可以消除電機(jī)磁場(chǎng)影響又可以解決陀螺儀零漂的方法。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是針對(duì)上述現(xiàn)有技術(shù)中的不足，提出了一種增穩(wěn)設(shè)備中航向角姿態(tài)解算的方法。

本發(fā)明是通過(guò)以下技術(shù)方案實(shí)現(xiàn)的：陀螺儀檢測(cè)轉(zhuǎn)子的角速度，加速度計(jì)檢測(cè)電機(jī)底座連接柄所處的加速度參數(shù)，磁力計(jì)檢測(cè)電機(jī)底座連接柄所處的磁場(chǎng)參數(shù)，加速度計(jì)參數(shù)和磁力計(jì)參數(shù)融合計(jì)算之后可以得到準(zhǔn)確的電機(jī)底座連接柄航向角數(shù)據(jù)MagAngle，由于電機(jī)底座與轉(zhuǎn)子并不同軸，所以只靠電機(jī)底座連接柄的航向角數(shù)據(jù)MagAngle并不能得到電機(jī)轉(zhuǎn)子的航向角數(shù)據(jù)YAW，引入電機(jī)轉(zhuǎn)子位置傳感器檢測(cè)電機(jī)轉(zhuǎn)子相對(duì)于電機(jī)底座的位置MotorAngle，電機(jī)底座連接柄航向角數(shù)據(jù)MagAngle與電機(jī)轉(zhuǎn)子位置MotorAngle相結(jié)合以后就可以得到電機(jī)轉(zhuǎn)子的航向角YAW，由于MagAngle和MotorAngle融合得到的航向角YAW存在噪聲所以引入了電機(jī)轉(zhuǎn)子的角速度Gyro,YAW再與Gyro進(jìn)行角度融合濾波即可以得到準(zhǔn)確的航向軸數(shù)據(jù)RealYAW，然后參與控制器的計(jì)算。計(jì)算邏輯如下：增穩(wěn)設(shè)備啟動(dòng)時(shí)記錄下來(lái)此時(shí)磁力計(jì)與加速度計(jì)解算出來(lái)的角度MagAngle和電機(jī)轉(zhuǎn)子初始位置MotorAngle，并將MotorAngle角度數(shù)據(jù)歸零，接著根據(jù)電機(jī)轉(zhuǎn)子位置MotorAngle的增量和MagAngle的值解算出電機(jī)轉(zhuǎn)子相對(duì)地球磁場(chǎng)的真正航向角RealYAW，再結(jié)合轉(zhuǎn)子角速度Gyro進(jìn)行一階濾波或者卡爾曼濾波得到比較準(zhǔn)確無(wú)噪聲的航向角。

本發(fā)明中的轉(zhuǎn)子位置檢測(cè)傳感器可以有多種方式檢測(cè)：霍爾傳感器，電位器，磁性編碼器等等。

本方案中的原始航向角和陀螺儀角速度的角度融合算法可以用一階互補(bǔ)濾波，卡爾曼濾波，或者AHRS融合算法。

本發(fā)明中的陀螺儀放置在轉(zhuǎn)子位置，磁力計(jì)加速度計(jì)放置在電機(jī)底座連接柄的末端遠(yuǎn)離了電機(jī)磁場(chǎng)，有與傳統(tǒng)方式不一樣的機(jī)械結(jié)構(gòu)和傳感器放置位置。

本發(fā)明中與只用磁力計(jì)加速度計(jì)陀螺儀來(lái)獲取航向角的傳統(tǒng)方法不一樣，引入了電機(jī)轉(zhuǎn)子位置傳感器，再結(jié)合磁力計(jì)加速度計(jì)進(jìn)行間接的角度轉(zhuǎn)換再融合陀螺儀角速度得到航向角。

附圖說(shuō)明

圖1是本發(fā)明的機(jī)械電子結(jié)構(gòu)結(jié)構(gòu)示意圖，系統(tǒng)中包括無(wú)刷電機(jī)，轉(zhuǎn)子位置傳感器，電機(jī)驅(qū)動(dòng)，控制器，處理器，三軸陀螺儀，三軸加速度計(jì)，三軸磁力計(jì)。圖 2是本發(fā)明的一個(gè)實(shí)例應(yīng)用系統(tǒng)結(jié)構(gòu)圖。

具體實(shí)施方式

下面結(jié)合附圖及實(shí)施例對(duì)本發(fā)明作進(jìn)一步說(shuō)明附圖1為系統(tǒng)結(jié)構(gòu)圖:，圖2為具體實(shí)現(xiàn)時(shí)用的具體器件。圖2中控制器1為Ti公司的數(shù)字信號(hào)處理器DSP，電機(jī)驅(qū)動(dòng)的控制器2為ST公司的32位單片機(jī)，電機(jī)轉(zhuǎn)子位置檢測(cè)傳感器3為磁性旋轉(zhuǎn)編碼器或者旋轉(zhuǎn)電位器，無(wú)刷電機(jī)4，電機(jī)驅(qū)動(dòng)電路5為以MOS管搭建的三路全橋驅(qū)動(dòng)，放置在轉(zhuǎn)子上的三軸陀螺儀6，放置在電機(jī)底座連接柄末端的三軸加速度計(jì)三軸磁力計(jì)7。

本實(shí)例中的控制器是采用DSP，具有數(shù)字信號(hào)處理能力強(qiáng)特點(diǎn)，驅(qū)動(dòng)電路是采用常見(jiàn)的經(jīng)典無(wú)刷電機(jī)驅(qū)動(dòng)電路，驅(qū)動(dòng)電路的控制器采用ST公司的STM32處理器，磁性旋轉(zhuǎn)編碼器可以實(shí)時(shí)檢測(cè)電機(jī)轉(zhuǎn)子角度，有效數(shù)據(jù)輸出頻率可達(dá)10kHZ，能完全達(dá)到實(shí)時(shí)性的要求，分辨率為12位可達(dá)0.08度，MPU6050采用MEMS技術(shù)可以采集三軸加速度數(shù)據(jù)三軸陀螺儀數(shù)據(jù)，輸出速率可達(dá)1KHZ滿足控制頻率，三軸磁力計(jì)輸出可達(dá)10HZ。控制器DSP28335通過(guò)I2C協(xié)議與處理器連接傳輸數(shù)據(jù)，磁性編碼器通過(guò)SPI協(xié)議與處理器通信，三軸加速度數(shù)據(jù)三軸陀螺儀通過(guò)I2C協(xié)議與DSP通信，最終各方數(shù)據(jù)在DSP里融合計(jì)算輸出給STM32，STM32將驅(qū)動(dòng)信號(hào)傳輸至電機(jī)驅(qū)動(dòng)實(shí)現(xiàn)控制系統(tǒng)的正常運(yùn)行。

使用該方案時(shí)將磁性編碼器放置在電機(jī)軸中央檢測(cè)轉(zhuǎn)子，或者轉(zhuǎn)子連接旋轉(zhuǎn)電位器進(jìn)行轉(zhuǎn)子位置檢測(cè)。

該方案的機(jī)械結(jié)構(gòu)與傳統(tǒng)方式不同，磁力計(jì)加速度計(jì)放置在電機(jī)底座連接柄的末端，原理電機(jī)磁場(chǎng)干擾，并且航向角的解算又引入了轉(zhuǎn)子位置數(shù)據(jù)，通過(guò)間接的運(yùn)算測(cè)量，得到轉(zhuǎn)子的航向角。

以上是本發(fā)明的較佳實(shí)施例子，凡依本發(fā)明技術(shù)方案所作的改變，所產(chǎn)生的功能作用未超出本發(fā)明技術(shù)方案的范圍時(shí)，均屬于本發(fā)明的保護(hù)范圍。