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

背景技術(shù)：

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

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

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

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

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

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

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

電路板；

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

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

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

在其中一個(gè)實(shí)施例中，所述處理控制模塊為CPU。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

在其中一個(gè)實(shí)施例中，通過(guò)橢球校正算法對(duì)所述三軸加速度計(jì)輸出的線加速度和三軸磁力計(jì)輸出的磁場(chǎng)強(qiáng)度預(yù)置偏差補(bǔ)償。

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

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

附圖說(shuō)明

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

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

具體實(shí)施方式

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

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

電路板150；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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