本發明涉及增穩設備的控制與姿態解算方法，常見應用與手持三軸增穩云臺、飛行器增穩云臺航向角控制領域中。

背景技術：

目前的增穩設備中航向角往往因為電機磁場的影響，或者加速度計解算不出航向角的原因，往往只采用陀螺儀來進行增穩設備的航向軸的姿態解算，其結果是可以滿足動態響應的要求，但是在靜止情況下由于陀螺儀的零漂往往會造成航向軸嚴重漂移的問題，因此需要一個既可以消除電機磁場影響又可以解決陀螺儀零漂的方法。

技術實現要素：

本發明的目的是針對上述現有技術中的不足，提出了一種增穩設備中航向角姿態解算的方法。

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

本發明中的轉子位置檢測傳感器可以有多種方式檢測：霍爾傳感器，電位器，磁性編碼器等等。

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

本發明中的陀螺儀放置在轉子位置，磁力計加速度計放置在電機底座連接柄的末端遠離了電機磁場，有與傳統方式不一樣的機械結構和傳感器放置位置。

本發明中與只用磁力計加速度計陀螺儀來獲取航向角的傳統方法不一樣，引入了電機轉子位置傳感器，再結合磁力計加速度計進行間接的角度轉換再融合陀螺儀角速度得到航向角。

附圖說明

圖1是本發明的機械電子結構結構示意圖，系統中包括無刷電機，轉子位置傳感器，電機驅動，控制器，處理器，三軸陀螺儀，三軸加速度計，三軸磁力計。圖 2是本發明的一個實例應用系統結構圖。

具體實施方式

下面結合附圖及實施例對本發明作進一步說明附圖1為系統結構圖:，圖2為具體實現時用的具體器件。圖2中控制器1為Ti公司的數字信號處理器DSP，電機驅動的控制器2為ST公司的32位單片機，電機轉子位置檢測傳感器3為磁性旋轉編碼器或者旋轉電位器，無刷電機4，電機驅動電路5為以MOS管搭建的三路全橋驅動，放置在轉子上的三軸陀螺儀6，放置在電機底座連接柄末端的三軸加速度計三軸磁力計7。

本實例中的控制器是采用DSP，具有數字信號處理能力強特點，驅動電路是采用常見的經典無刷電機驅動電路，驅動電路的控制器采用ST公司的STM32處理器，磁性旋轉編碼器可以實時檢測電機轉子角度，有效數據輸出頻率可達10kHZ，能完全達到實時性的要求，分辨率為12位可達0.08度，MPU6050采用MEMS技術可以采集三軸加速度數據三軸陀螺儀數據，輸出速率可達1KHZ滿足控制頻率，三軸磁力計輸出可達10HZ。控制器DSP28335通過I2C協議與處理器連接傳輸數據，磁性編碼器通過SPI協議與處理器通信，三軸加速度數據三軸陀螺儀通過I2C協議與DSP通信，最終各方數據在DSP里融合計算輸出給STM32，STM32將驅動信號傳輸至電機驅動實現控制系統的正常運行。

使用該方案時將磁性編碼器放置在電機軸中央檢測轉子，或者轉子連接旋轉電位器進行轉子位置檢測。

該方案的機械結構與傳統方式不同，磁力計加速度計放置在電機底座連接柄的末端，原理電機磁場干擾，并且航向角的解算又引入了轉子位置數據，通過間接的運算測量，得到轉子的航向角。

以上是本發明的較佳實施例子，凡依本發明技術方案所作的改變，所產生的功能作用未超出本發明技術方案的范圍時，均屬于本發明的保護范圍。