三維姿態測控裝置、機械設備及三維姿態的測控方法與流程

2024-04-12 03:32:05

本發明涉及工程機械的三維姿態測量領域，特別是涉及一種三維姿態測控裝置、機械設備及三維姿態的測試方法。

背景技術：

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

高精度的載體三維姿態的控制通常是工程機械智能控制系統中最重要的一環，其需要對載體的三維姿態進行準確的測量。

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

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

技術實現要素：

基於此，有必要提供一種三維姿態測控裝置、機械設備及三維姿態測控方法，提高對載體三維姿態的測控精度。

一種三維姿態測控裝置，包括：

電路板；

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

安裝於所述電路板另一側的處理控制模塊；

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

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

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

在其中一個實施例中，所述轉換後的線加速度與對應重力矢量的差值為：加速度計輸出的線加速度投影到重力矢量所在三維坐標系的加速度值與所述重力矢量的差值；所述轉換後的磁場強度與對應地球磁場強度的差值為：磁力計輸出的磁場強度投影到地球磁場所在的三維坐標系的磁場強度值與所述地球磁場強度的差值。

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

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

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

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

一種機械設備，所述機械設備安裝有如以上所述的三維姿態測控裝置。

所述機械設備安裝有以上所述三維姿態測控裝置，可有效提升測量和調控載體三維姿態的精確性。

一種三維姿態的測控方法，基於以上所述的三維姿態測控裝置，所述方法包括：

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

對所述三軸加速度計輸出的線加速度和三軸磁力計輸出的磁場強度進行預置偏差補償；

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

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

根據所述俯仰角、橫滾角、航向角及所述三個單軸陀螺儀輸出的旋轉角度的誤差值調整載體的三維姿態。

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

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

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

附圖說明

圖1為一實施例的三維姿態測控裝置的結構示意圖；

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

具體實施方式

為了使本發明的目的、技術方案及優點更加清楚明白，以下結合附圖及實施例，對本發明進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本發明，並不用於限定本發明。

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

電路板150；

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

安裝於電路板150另一側的處理控制模塊140；

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

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

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

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

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

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

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

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

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

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

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

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

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

本實施例還提供了一種機械設備，機械設備安裝有如以上所述的三維姿態測控裝置。

根據以上可知，機械設備安裝有以上所述三維姿態測控裝置，，可有效提升測量和調控載體三維姿態的精確性。

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

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

步驟S120，對三軸加速度計輸出的線加速度和三軸磁力計輸出的磁場強度進行預置偏差補償；

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

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

步驟S150，根據俯仰角、橫滾角、航向角及三個單軸陀螺儀輸出的旋轉角度的誤差值調整載體的三維姿態。

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

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

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

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

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

以上所述實施例的各技術特徵可以進行任意的組合，為使描述簡潔，未對上述實施例中的各個技術特徵所有可能的組合都進行描述，然而，只要這些技術特徵的組合不存在矛盾，都應當認為是本說明書記載的範圍。

以上所述實施例僅表達了本發明的幾種實施方式，其描述較為具體和詳細，但並不能因此而理解為對發明專利範圍的限制。應當指出的是，對於本領域的普通技術人員來說，在不脫離本發明構思的前提下，還可以做出若干變形和改進，這些都屬於本發明的保護範圍。因此，本發明專利的保護範圍應以所附權利要求為準。