本發(fā)明涉及工程機(jī)械的三維姿態(tài)測量領(lǐng)域，特別是涉及一種三維姿態(tài)測控裝置、機(jī)械設(shè)備及三維姿態(tài)的測試方法。

背景技術(shù)：

智能控制系統(tǒng)是工程機(jī)械(如工業(yè)機(jī)器人、壓路機(jī)、播種機(jī)等)生產(chǎn)發(fā)展的新潮流，是以信息技術(shù)為支撐，根據(jù)載體的時空信息，定時、定量地實施一整套現(xiàn)代化操作與管理的控制系統(tǒng)，要求以最經(jīng)濟(jì)的投入取得最佳的生產(chǎn)和環(huán)境效益。

高精度的載體三維姿態(tài)的控制通常是工程機(jī)械智能控制系統(tǒng)中最重要的一環(huán)，其需要對載體的三維姿態(tài)進(jìn)行準(zhǔn)確的測量。

目前工程機(jī)械的三維姿態(tài)測量主要有兩種應(yīng)用場景：一種是姿態(tài)異常監(jiān)測，比如海上浮標(biāo)的傾斜監(jiān)測和渣土車的翻車監(jiān)測；另一種是姿態(tài)跟蹤控制，比如工業(yè)級機(jī)器人機(jī)械臂的控制和農(nóng)機(jī)的農(nóng)具控制。目前三維姿態(tài)測量的方法主要電子羅盤法，電子羅盤一般包含兩個水平方向的加速度計和一個三軸磁力計，通過兩個水平加速度計測量載體傾斜角度，包括載體的俯仰角和橫滾角，再通過俯仰角和橫滾角對三軸磁力計的測量值進(jìn)行調(diào)平，通過三軸磁力計計算出載體的航向角。

由于電子羅盤單純依靠加速度計的測量值計算俯仰角和橫滾角，因此，加速度計的系統(tǒng)誤差和量測噪聲會直接影響俯仰角和橫滾角的計算精度。俯仰角和橫滾角的計算是以重力矢量作為參考，而加速度計并不能區(qū)分重力矢量的分量和載體的線加速度信息，因此，當(dāng)載體存在線加速度時，通過加速度計測量的俯仰角和橫滾角會存在較大的誤差。三軸磁力計對電磁環(huán)境敏感，當(dāng)三軸磁力計周圍的電磁環(huán)境發(fā)生變化時，會導(dǎo)致三軸磁力計的測量值具有較大的誤差，導(dǎo)致航向角計算錯誤。

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

基于此，有必要提供一種三維姿態(tài)測控裝置、機(jī)械設(shè)備及三維姿態(tài)測控方法，提高對載體三維姿態(tài)的測控精度。

一種三維姿態(tài)測控裝置，包括：

電路板；

安裝于所述電路板一側(cè)的三軸加速度計、三軸磁力計和三個互相垂直的單軸陀螺儀；

安裝于所述電路板另一側(cè)的處理控制模塊；

所述處理控制模塊對所述三軸加速度計輸出的線加速度及所述三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償，并以所述三軸加速度計所處位置的重力矢量和所述三軸磁力計所處位置的地球磁場矢量為參考，根據(jù)所述三個單軸陀螺儀輸出的旋轉(zhuǎn)角速度分別計算出載體的俯仰角、橫滾角、航向角，所述處理控制模塊根據(jù)載體的俯仰角、橫滾角和航向角對所述三軸加速度計輸出的線加速度和所述三軸磁力計輸出的磁場強(qiáng)度進(jìn)行坐標(biāo)轉(zhuǎn)換，并在分別獲取轉(zhuǎn)換后的線加速度和磁場強(qiáng)度與對應(yīng)重力矢量和地球磁場強(qiáng)度的差值后，通過數(shù)據(jù)融合算法對所述載體的俯仰角、橫滾角和航向角進(jìn)行修正并對所述三軸陀螺儀輸出的旋轉(zhuǎn)角速度進(jìn)行誤差補償。

在其中一個實施例中，所述處理控制模塊為CPU。

在其中一個實施例中，所述處理控制模塊通過橢球校正算法對所述三軸加速度計輸出的線加速度及所述三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償。

在其中一個實施例中，所述轉(zhuǎn)換后的線加速度與對應(yīng)重力矢量的差值為：加速度計輸出的線加速度投影到重力矢量所在三維坐標(biāo)系的加速度值與所述重力矢量的差值；所述轉(zhuǎn)換后的磁場強(qiáng)度與對應(yīng)地球磁場強(qiáng)度的差值為：磁力計輸出的磁場強(qiáng)度投影到地球磁場所在的三維坐標(biāo)系的磁場強(qiáng)度值與所述地球磁場強(qiáng)度的差值。

在其中一個實施例中，所述處理控制模塊通過數(shù)據(jù)融合算法對所述載體的俯仰角、橫滾角和航向角進(jìn)行修正并對陀螺儀輸出的旋轉(zhuǎn)角速度進(jìn)行誤差補償具體為：

所述處理控制模塊通過數(shù)據(jù)融合算法融合三個單軸陀螺儀、三軸磁力計和三軸加速度計的量測值，通過三軸加速計和三軸磁力計的量測值對三個單軸陀螺儀計算的俯仰角、橫滾角、航向角進(jìn)行修正并對所述三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差進(jìn)行補償。

在其中一個實施例中，所述數(shù)據(jù)融合算法為Kalman濾波數(shù)據(jù)融合算法。

以上所述三維姿態(tài)測控裝置中，電路板的一側(cè)集成有三軸加速度計、三軸磁力計和三個互相垂直的單軸陀螺儀，另一側(cè)集成有處理控制模塊，與將三軸加速度計、三軸磁力計、三個互相垂直的單軸陀螺儀及處理控制模塊集成在電路板的同一側(cè)相比，減小了電路板的面積，進(jìn)而縮小了整個三維姿態(tài)測控裝置的體積；對三軸加速度計輸出的線加速度及三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償，可使三軸加速度計和三軸磁力計輸出的值更加精準(zhǔn)；根據(jù)三軸加速度計輸出的線加速度與重力矢量存在的差值和三軸磁力計輸出的磁場強(qiáng)度與地球磁場存在的差值，分別計算出俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值，可以提升測量的俯仰角、橫滾角、航向角和旋轉(zhuǎn)角度的準(zhǔn)確性，依次對載體的三維姿態(tài)進(jìn)行控制，可有效提升對載體三維姿態(tài)的穩(wěn)定性。

一種機(jī)械設(shè)備，所述機(jī)械設(shè)備安裝有如以上所述的三維姿態(tài)測控裝置。

所述機(jī)械設(shè)備安裝有以上所述三維姿態(tài)測控裝置，可有效提升測量和調(diào)控載體三維姿態(tài)的精確性。

一種三維姿態(tài)的測控方法，基于以上所述的三維姿態(tài)測控裝置，所述方法包括：

獲取所述三軸加速度計靜態(tài)輸出的俯仰角、橫滾角及所述三軸磁力計靜態(tài)輸出的航向角；

對所述三軸加速度計輸出的線加速度和三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償；

分別獲取三軸加速度計輸出的線加速度與對應(yīng)重力矢量的差值和三軸磁力計輸出的磁場強(qiáng)度與對應(yīng)地球磁場強(qiáng)度的差值；

根據(jù)所述差值分別計算所述俯仰角、橫滾角、航向角及所述三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值；

根據(jù)所述俯仰角、橫滾角、航向角及所述三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值調(diào)整載體的三維姿態(tài)。

在其中一個實施例中，通過橢球校正算法對所述三軸加速度計輸出的線加速度和三軸磁力計輸出的磁場強(qiáng)度預(yù)置偏差補償。

在其中一個實施例中，所述三軸加速度計輸出的線加速度與對應(yīng)重力矢量存在的差值為：三軸加速度計輸出的線加速度投影到重力矢量所在三維坐標(biāo)系的加速度值與所述重力矢量的差值；所述三軸磁力計輸出的磁場強(qiáng)度與對應(yīng)地球磁場存在的差值為：三軸磁力計輸出的磁場強(qiáng)度投影到地球磁場所在的三維坐標(biāo)系的磁場強(qiáng)度值與所述地球磁場強(qiáng)度的差值。

以上所述三維姿態(tài)的測控方法中，對三軸加速度計輸出的線加速度及三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償，可使三軸加速度計和三軸磁力計輸出的值更加精準(zhǔn)；根據(jù)三軸加速度計輸出的線加速度與重力矢量存在的差值和三軸磁力計輸出的磁場強(qiáng)度與地球磁場存在的差值，分別計算出俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值，可以提升測量的俯仰角、橫滾角、航向角和旋轉(zhuǎn)角度的準(zhǔn)確性，依次對載體的三維姿態(tài)進(jìn)行控制，可有效提升對載體三維姿態(tài)的穩(wěn)定性。

附圖說明

圖1為一實施例的三維姿態(tài)測控裝置的結(jié)構(gòu)示意圖；

圖2為一實施例的三維姿態(tài)的測控方法的流程示意圖。

具體實施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點更加清楚明白，以下結(jié)合附圖及實施例，對本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

如圖1所示，一實施例的三維姿態(tài)測控裝置包括：

電路板150；

安裝于電路板150一側(cè)的三軸加速度計120、三軸磁力計130和三個互相垂直的單軸陀螺儀110；

安裝于電路板150另一側(cè)的處理控制模塊140；

處理控制模塊140對三軸加速度計120輸出的線加速度及三軸磁力計130輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償，并以三軸加速度120計所處位置的重力矢量和所述三軸磁力計130所處位置的地球磁場矢量為參考，根據(jù)三個單軸陀螺儀110輸出的旋轉(zhuǎn)角速度分別計算出載體的俯仰角、橫滾角、航向角，處理控制模塊140根據(jù)載體的俯仰角、橫滾角和航向角對三軸加速度計120輸出的線加速度和三軸磁力計130輸出的磁場強(qiáng)度進(jìn)行坐標(biāo)轉(zhuǎn)換，并在分別獲取轉(zhuǎn)換后的線加速度和磁場強(qiáng)度與對應(yīng)重力矢量和地球磁場強(qiáng)度的差值后，通過數(shù)據(jù)融合算法對所述載體的俯仰角、橫滾角和航向角進(jìn)行修正并對三軸陀螺儀110輸出的旋轉(zhuǎn)角速度進(jìn)行誤差補償。

以上所述三維姿態(tài)測控裝置中，電路板的一側(cè)集成有三軸加速度計、三軸磁力計和三個互相垂直的單軸陀螺儀，另一側(cè)集成有處理控制模塊，與將三軸加速度計、三軸磁力計、三個互相垂直的單軸陀螺儀及處理控制模塊集成在電路板的同一側(cè)相比，減小了電路板的面積，進(jìn)而縮小了整個三維姿態(tài)測控裝置的體積；對三軸加速度計輸出的線加速度和三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償，可使三軸加速度計和三軸磁力計輸出的值更加精準(zhǔn)；根據(jù)三軸加速度計輸出的線加速度與重力矢量存在的差值和三軸磁力計輸出的磁場強(qiáng)度與地球磁場存在的差值，分別計算出俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值，可以提升測量的俯仰角、橫滾角、航向角和旋轉(zhuǎn)角度的準(zhǔn)確性，依次對載體的三維姿態(tài)進(jìn)行控制，可有效提升對載體三維姿態(tài)的穩(wěn)定性。

如圖1所示，圖1中所顯示的排列方式只是實現(xiàn)本實施例的最優(yōu)選擇，其中，三個互相垂直的單軸陀螺儀110的其中一個設(shè)置為垂直于電路板150，另外兩個與電路板150平行設(shè)置，平行設(shè)置的兩個單軸陀螺儀110的垂直夾角處安裝三軸加速度計120和三軸磁力計130。這樣結(jié)構(gòu)設(shè)計可最大程度地減小電路板150的面積，縮小整個三維姿態(tài)測控裝置的體積。

本實施例中，處理控制模塊140為CPU處理器，其安裝于電路板150的另一側(cè)，與將三軸加速度計、三軸磁力計、三個互相垂直的單軸陀螺儀及處理控制模塊集成在電路板的同一側(cè)相比，減小了電路板的面積，進(jìn)而縮小了整個三維姿態(tài)測控裝置的體積。

可以知道的是，三個單軸陀螺儀110測量的是載體繞三個正交軸(三個單軸陀螺儀的三個相互正交軸)正方向上的旋轉(zhuǎn)角速度，通過角度積分運算可以確定載體繞三個正交軸轉(zhuǎn)過的角度，進(jìn)而測量角度的變化量。但測試時需要載體的初始姿態(tài)，且由于陀螺儀的測量值中含有零偏、交叉耦合和隨機(jī)噪聲等誤差，其角度積分運算的誤差會隨時間累積，所以需要對角度積分運算計算的角度進(jìn)行校正，并對陀螺儀的測量誤差進(jìn)行補償。因此，通常陀螺儀測量姿態(tài)具有短期精度高、動態(tài)性好、誤差長期漂移的特點。

可以知道的是，靜態(tài)情況下，三軸加速度計120可以測量重力的反作用力在載體三個正交坐標(biāo)軸上的分量，從而確定載體的傾斜情況，包括載體的俯仰角(pitch)和橫滾角(roll)。三軸加速度計120是直接通過離散的量測值計算角度，不需要經(jīng)過積分運算，因此，三軸加速度計120計算角度的誤差不會隨時間累積。靜態(tài)情況下，三軸加速度計120計算角度的精度可以達(dá)到0.1°以上。但在動態(tài)情況下，由于三軸加速度計120只能測量載體的比力，即除重力以外的力的合力，而不能從其測量值中辨識載體的運動加速度，載體的運動加速度越大，計算角度的誤差也越大，且三軸加速度計的零偏，交叉耦合和隨機(jī)噪聲等誤差也會影響其計算角度的精度。因此，通常三軸加速度計具有靜態(tài)精度高、動態(tài)精度低、系統(tǒng)偏差、誤差不隨時間累積的特點。

可以知道的是，三軸磁力計130可以測量地球磁場在載體三個正交坐標(biāo)軸系上的分量，水平放置磁力計時可以通過三軸磁力計130的水平軸測得的地球磁場分量計算出航向角，因此，使用三軸磁力計130測量航向角首先需要利用載體的俯仰角和橫滾角對三軸磁力計130的量測值進(jìn)行調(diào)平，理想情況下三軸磁力計計算的航向角的精度可以達(dá)到1°以上，但磁力計的零偏，交叉耦合和隨機(jī)噪聲等誤差會影響航向角的計算精度，尤其當(dāng)磁力計周圍的電磁環(huán)境發(fā)生變化時，磁力計的測量值會受到明顯的干擾從而導(dǎo)致航向角計算錯誤。因此，磁力計測量航向角存在易受電磁環(huán)境干擾的特點。

單軸陀螺儀、三軸加速度計和三軸磁力計各自能夠完成載體三維姿態(tài)測量的部分工作，有各自明顯的優(yōu)點。其中，單軸陀螺儀短期精度高、動態(tài)性好；三軸加速度計誤差不積累、靜態(tài)精度高；三軸磁力計能指北、誤差不累積。同時，單軸陀螺儀、三軸加速度計和三軸磁力計也有各自明顯的缺點，其中，單軸陀螺儀誤差會積累且需要初始姿態(tài)；三軸加速度計會受載體加速度和系統(tǒng)誤差的影；三軸磁力計會受電磁干擾和系統(tǒng)誤差的影響。

本實施例中，綜合設(shè)置有單軸陀螺儀、三軸加速度計和三軸磁力，通過三者的互補性，通過橢球校正算法補償三軸加速度計和三軸磁力計的系統(tǒng)誤差，主要是零偏和交叉耦合誤差，再通過數(shù)據(jù)融合算法將單軸陀螺儀、三軸加速度計和三軸磁力計的量測數(shù)據(jù)進(jìn)行融合從而實現(xiàn)單軸陀螺儀、三軸加速度計和三軸磁力計的優(yōu)勢互補。

具體的，本實施例中，處理控制模塊140通過橢球校正算法對三軸加速度計輸出的線加速度及三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償。橢球校正算法可以對三軸加速度計和三軸磁力計的零偏、交叉耦合等系統(tǒng)偏差進(jìn)行補償，從而極大提高三軸加速度計和三軸磁力計的原始觀測值的精度。進(jìn)一步通過三軸加速度計和三軸磁力計的靜態(tài)輸出的載體的俯仰角、橫滾角和航向角即可作為三個單軸陀螺儀姿態(tài)更新的初始值。

通常三維姿態(tài)的解算存在誤差，因此，將線加速度和磁場強(qiáng)度投影到導(dǎo)航系后其值與重力矢量和地球磁場不符，本實施例利用它們的差值通過Kalman濾波數(shù)據(jù)融合算法估計出三維姿態(tài)和陀螺儀儀器參數(shù)的誤差并對三維姿態(tài)和陀螺儀參數(shù)進(jìn)行校正。因為單軸陀螺儀積分解算的短期精度高，可以利用差值判別三軸加速度計和三軸磁力計是否受到干擾，從而提高整個三維姿態(tài)測控裝置的魯棒性。

其中，將線加速度和磁場矢量投影到導(dǎo)航系后其值與重力矢量和地球磁場的值不符，即三軸加速度計120輸出與重力矢量存在的誤差和三軸磁力計輸出與地球磁場存在的誤差。三軸加速度計存在的重力誤差為三軸加速度計輸出的線加速度投影到重力矢量所在三維坐標(biāo)系的加速度值與所述重力矢量的差值，三維坐標(biāo)系有三個極軸，三軸加速度計輸出的線加速度投影到每一個極軸的值與重力矢量在對應(yīng)極軸分量的差值即為重力誤差值，誤差值具有3個，分別為在三個極軸上的重力誤差。三軸磁力計存在的磁力誤差為三軸磁力計輸出的磁場強(qiáng)度投影到地球磁場所在的三維坐標(biāo)系的磁場強(qiáng)度值與所述地球磁場強(qiáng)度的差值，同理，三維坐標(biāo)系有三個極軸，磁力計輸出的磁場強(qiáng)度投影到每一個極軸的值與地球磁場在對應(yīng)極軸上的分量的差值即為磁力誤差值，誤差值為3個，分別為在三個極軸上的磁力誤差。

本實施例中，處理控制模塊通過Kalman濾波數(shù)據(jù)融合算法分別計算出俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值，并根據(jù)誤差值調(diào)整載體的三維姿態(tài)。由此，本實施例可有效提高載體三維姿態(tài)的測量和調(diào)控精度。

本實施例還提供了一種機(jī)械設(shè)備，機(jī)械設(shè)備安裝有如以上所述的三維姿態(tài)測控裝置。

根據(jù)以上可知，機(jī)械設(shè)備安裝有以上所述三維姿態(tài)測控裝置，，可有效提升測量和調(diào)控載體三維姿態(tài)的精確性。

如圖2所示，為一實施例的三維姿態(tài)的測控方法的流程示意圖，所述測控方法基于以上所述的三維姿態(tài)測控裝置。具體的，所述方法包括步驟S110至步驟S150。

步驟S110，獲取三軸加速度計輸出的俯仰角、橫滾角及三軸磁力計靜態(tài)輸出的航向角；

步驟S120，對三軸加速度計輸出的線加速度和三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償；

步驟S130，分別獲取三軸加速度計輸出的線加速度與對應(yīng)重力矢量的差值和三軸磁力計輸出的磁場強(qiáng)度與對應(yīng)地球磁場強(qiáng)度的差值；

步驟S140，根據(jù)差值分別計算俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值；

步驟S150，根據(jù)俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值調(diào)整載體的三維姿態(tài)。

以上所述三維姿態(tài)的測控方法中，對三軸加速度計輸出的線加速度及三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償，可使三軸加速度計和三軸磁力計輸出的值更加精準(zhǔn)；根據(jù)三軸加速度計輸出的線加速度與與重力矢量存在的差值和三軸磁力計輸出的磁場強(qiáng)度與地球磁場存在的差值，分別計算出俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值，可以提升測量的俯仰角、橫滾角、航向角和旋轉(zhuǎn)角度的準(zhǔn)確性，依次對載體的三維姿態(tài)進(jìn)行控制，可有效提升對載體三維姿態(tài)的穩(wěn)定性。

本實施例中，綜合設(shè)置有單軸陀螺儀、三軸加速度計和三軸磁力，通過三者的互補性，通過橢球校正算法補償三軸加速度計和三軸磁力計的系統(tǒng)誤差，主要是零偏和交叉耦合誤差，再通過數(shù)據(jù)融合算法將單軸陀螺儀、三軸加速度計和三軸磁力計的量測數(shù)據(jù)進(jìn)行融合從而實現(xiàn)單軸陀螺儀、三軸加速度計和三軸磁力計的優(yōu)勢互補。

本實施例中，處理控制模塊140通過橢球校正算法對三軸加速度計輸出的線加速度及三軸磁力計輸出的磁場強(qiáng)度進(jìn)行預(yù)置偏差補償。即步驟S120通過橢球校正算法對三軸加速度計和三軸磁力計進(jìn)行預(yù)置偏差補償。橢球校正算法可以對三軸加速度計和三軸磁力計的零偏、交叉耦合等系統(tǒng)偏差進(jìn)行補償，從而極大提高三軸加速度計和三軸磁力計的原始觀測值的精度。進(jìn)一步通過三軸加速度計和三軸磁力計的靜態(tài)輸出的載體的俯仰角、橫滾角和航向角即可作為三個單軸陀螺儀姿態(tài)更新的初始值。

通常三維姿態(tài)的解算存在誤差，因此，將線加速度和和磁場強(qiáng)度投影到導(dǎo)航系后其值與重力矢量和地球磁場的分量不符，本實施例利用它們的差值通過Kalman濾波數(shù)據(jù)融合算法估計出三維姿態(tài)和陀螺儀儀器參數(shù)的誤差并對三維姿態(tài)和陀螺儀參數(shù)進(jìn)行校正。因為單軸陀螺儀積分解算的短期精度高，可以利用差值判別三軸加速度計和三軸磁力計是否受到干擾，從而提高整個三維姿態(tài)測控裝置的魯棒性。

其中，將線加速度和和磁場強(qiáng)度投影到導(dǎo)航系后其值與重力矢量和地球磁場的分量不符，即三軸加速度計120輸出與重力矢量存在的誤差和三軸磁力計輸出與地球磁場存在的誤差。三軸加速度計存在的重力誤差為加速度計輸出的線加速度投影到重力矢量所在三維坐標(biāo)系的加速度值與所述重力矢量的差值，三維坐標(biāo)系有三個極軸，加速度計輸出的線加速度投影到每一個極軸的值與重力矢量在對應(yīng)極軸分量的差值即為重力誤差值，誤差值具有3個，分別為在三個極軸上的重力誤差。三軸磁力計存在的磁力誤差為磁力計輸出的磁場強(qiáng)度投影到地球磁場所在的三維坐標(biāo)系的磁場強(qiáng)度值與所述地球磁場強(qiáng)度的差值，同理，三維坐標(biāo)系有三個極軸，磁力計輸出的磁場強(qiáng)度投影到每一個極軸的值與地球磁場在對應(yīng)極軸上的分量的差值即為磁力誤差值，誤差值為3個，分別為在三個極軸上的磁力誤差。本實施例中，處理控制模塊通過Kalman濾波數(shù)據(jù)融合算法分別計算出俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉(zhuǎn)角度的誤差值，并根據(jù)誤差值調(diào)整載體的三維姿態(tài)。由此，本實施例可有效提高載體三維姿態(tài)的測量和調(diào)控精度。

以上所述實施例的各技術(shù)特征可以進(jìn)行任意的組合，為使描述簡潔，未對上述實施例中的各個技術(shù)特征所有可能的組合都進(jìn)行描述，然而，只要這些技術(shù)特征的組合不存在矛盾，都應(yīng)當(dāng)認(rèn)為是本說明書記載的范圍。

以上所述實施例僅表達(dá)了本發(fā)明的幾種實施方式，其描述較為具體和詳細(xì)，但并不能因此而理解為對發(fā)明專利范圍的限制。應(yīng)當(dāng)指出的是，對于本領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變形和改進(jìn)，這些都屬于本發(fā)明的保護(hù)范圍。因此，本發(fā)明專利的保護(hù)范圍應(yīng)以所附權(quán)利要求為準(zhǔn)。