[發明專利]姿態估計方法和裝置

有效

• 申請號: 201310526022.7
• 申請日: 2013-10-30
• 公開（公告）號: CN103791905A
• 公開（公告）日: 2014-05-14
• 發明（設計）人: 半田伊吹
• 申請（專利權）人: 雅馬哈株式會社
• 主分類號: G01C21/12
• 分類號: G01C21/12;G01C21/20
• 代理公司: 北京天昊聯合知識產權代理有限公司 11112
• 代理人: 陳源;李銘

1.一種對具有多個傳感器的設備的姿態進行估計的方法，所述多個傳感器包括加速度傳感器，所述方法包括步驟：

產生所述設備在第一時刻的姿態的第一估計值；

根據從所述加速度傳感器輸出的加速度數據來確定所述設備的姿態是否穩定；

通過將所述第一估計值應用于表示所述設備的姿態的變換的狀態變換模型，來預測所述設備在從所述第一時刻開始過去預定時間段之后的第二時刻的姿態，從而產生姿態預測值；

根據從所述多個傳感器中所包含的至少一個傳感器輸出的數據來估計所述設備的所述姿態預測值與姿態的真實值之差，從而產生所述設備的預測姿態與真實姿態之間的估計姿態誤差；

當所述設備的姿態被確定為穩定時，根據所述姿態預測值和所述估計姿態誤差來計算所述設備在所述第二時刻的姿態的第二估計值；

當所述設備的姿態被確定為不穩定時，從所述估計姿態誤差的多個姿態分量中提取特定姿態分量；以及

當所述設備的姿態被確定為不穩定時，根據所述姿態預測值和從所述估計姿態誤差中提取出來的所述特定姿態分量，來計算所述設備在所述第二時刻的姿態的第二估計值。

2.根據權利要求1所述的方法，其中所述估計姿態誤差的多個姿態分量包括表示相對于沿重力加速度的方向延伸的軸的傾斜的傾斜分量以及圍繞該軸的旋轉分量，并且所述特定姿態分量是通過從所述估計姿態誤差中去除所述傾斜分量而得到的所述旋轉分量。

3.根據權利要求1所述的方法，其中所述多個姿態分量是通過將所述設備的基準姿態改變所述估計姿態誤差而得到的。

4.根據權利要求1所述的方法，其中所述設備的姿態由四元數表示。

5.根據權利要求1所述的方法，

其中所述多個傳感器還包括磁傳感器和角速度傳感器，

其中所述方法還包括步驟:在所述設備的姿態被確定為穩定時，產生包括從所述磁傳感器輸出的磁數據以及從所述加速度傳感器輸出的加速度數據作為其元素的輸入觀測值;以及在所述設備的姿態被確定為不穩定時，產生包括從所述磁傳感器輸出的磁數據作為其元素而不包括從所述加速度傳感器輸出的加速度數據的輸入觀測值，

其中通過將所述第一時刻的第一估計值和從所述角速度傳感器輸出的角速度數據應用于所述狀態變換模型來預測所述設備在所述第二時刻的姿態，以及

其中通過以下步驟來產生所述估計姿態誤差:將所述第二時刻的姿態預測值應用于表示觀測值的元素與姿態的值之間的關系的觀測模型，以便對所述第二時刻時的觀測值的元素進行估計，從而產生具有所估計的元素的估計觀測值;計算所述估計觀測值與所述輸入觀測值之差以產生表示所計算出的差的觀測殘差;并且根據所述觀測殘差以及所述設備的姿態預測值來產生所述估計姿態誤差。

6.根據權利要求1所述的方法，其中在從所述加速度傳感器輸出的加速度數據的幅值與重力加速度的幅值之差的絕對值不大于預定值的情況下，所述設備的姿態被確定為穩定。

7.一種對具有傳感器的設備的姿態進行估計的方法，所述方法包括步驟：

產生所述設備在第一時刻的姿態的第一估計值；

通過將所述第一估計值應用于表示所述設備的姿態的變換的狀態變換模型，來預測所述設備在從所述第一時刻開始過去預定時間段之后的第二時刻的姿態，從而產生姿態預測值；

根據從所述傳感器輸出的數據來估計所述設備的所述姿態預測值與姿態的真實值之差，從而產生所述設備的預測姿態與真實姿態之間的估計姿態誤差；

從所述估計姿態誤差的多個姿態分量中提取特定姿態分量；以及

根據所述姿態預測值和從所述估計姿態誤差中提取出來的所述特定姿態分量，來計算所述設備在所述第二時刻的姿態的第二估計值。

8.根據權利要求7所述的方法，其中所述傳感器包括用于輸出表示地磁的磁數據的磁傳感器。

9.根據權利要求7所述的方法，其中所述估計姿態誤差的多個姿態分量包括表示相對于沿重力加速度的方向延伸的軸的傾斜的傾斜分量以及圍繞該軸的旋轉分量，并且所述特定姿態分量是通過從所述估計姿態誤差中去除所述傾斜分量而得到的所述旋轉分量。

10.根據權利要求7所述的方法，其中所述多個姿態分量是通過將所述設備的基準姿態改變所述估計姿態誤差而得到的。