1.一種依據(jù)陀螺儀數(shù)據(jù)和加速計數(shù)據(jù)的、針對運動物體的姿態(tài)估計方法,所述姿態(tài)估計方法包括:
校正增益確定步驟,其用于基于利用所述陀螺儀數(shù)據(jù)和加速計數(shù)據(jù)而測量的物體運動程度,來確定校正增益;
校正矢量計算步驟,其用于通過基于所述陀螺儀數(shù)據(jù)和加速計數(shù)據(jù),將所述參考矢量從地心坐標系轉換到體坐標系,來計算校正矢量;
傳感器數(shù)據(jù)校準步驟,其用于使用所述校正增益和所述校正矢量,來校準所述陀螺儀數(shù)據(jù);
姿態(tài)估計步驟,其用于利用經(jīng)校準的陀螺儀數(shù)據(jù)來估計所述姿態(tài),
其中,所述校正增益確定步驟還包括以下步驟:基于所述加速計數(shù)據(jù)來估計在體坐標系中的重力分量;通過從所述加速度數(shù)據(jù)中去除重力分量來計算所述物體的運動加速度;基于所述物體的所述運動加速度的預定義函數(shù)來計算所述校正增益。
2.根據(jù)權利要求1所述的姿態(tài)估計方法,所述姿態(tài)估計方法還包括調整步驟,所述調整步驟基于利用當前陀螺儀數(shù)據(jù)和經(jīng)校準的陀螺儀數(shù)據(jù)而計算的姿態(tài)估計偏差,來調整所述校正增益。
3.根據(jù)權利要求1或2所述的姿態(tài)估計方法,其中,所述估計在體坐標系中的重力分量還包括:
基于所述加速計數(shù)據(jù)、陀螺儀數(shù)據(jù)和磁力計數(shù)據(jù)中的至少一個,來確定所述物體處于靜止的狀態(tài);
在所述物體處于靜止狀態(tài),計算所述加速度的范數(shù);
在所述物體處于靜止狀態(tài),計算所述加速度的范數(shù)的平均值;
利用計算出的范數(shù)的平均值,構建在所述地心坐標系中的重力矢量;
基于使用陀螺儀數(shù)據(jù)而計算出的轉換矩陣,將所述重力矢量從地心坐標系轉換到體坐標系。
4.根據(jù)權利要求1或2所述的姿態(tài)估計方法,其中,所述運動加速度的預定義函數(shù)是分段函數(shù),并且如果所述運動加速度的范數(shù)超過預定義閾值,則將所述校正增益設置為預定義常量;否則,使用所述運動加 速度的范數(shù)的連續(xù)函數(shù)來計算所述校正增益。
5.根據(jù)權利要求1或2所述的姿態(tài)估計方法,其中,所述校正矢量計算步驟還包括以下步驟:
基于使用陀螺儀數(shù)據(jù)計算出的轉換矩陣,將所述地心坐標系中的參考矢量轉換到體坐標系;
基于使用加速計數(shù)據(jù)計算出的轉換矩陣,或基于加速計數(shù)據(jù)和磁力計數(shù)據(jù),將地心坐標系中的參考矢量轉換到體坐標系;
執(zhí)行兩個轉換的參考矢量之間的叉積運算,以生成校正矢量。
6.根據(jù)權利要求1所述的姿態(tài)估計方法,其中,所述傳感器數(shù)據(jù)校準步驟還包括以下步驟:
通過將校正矢量乘以校正增益,來計算矢量;
將所述矢量與在體坐標系的不同軸上的當前陀螺儀數(shù)據(jù)相加。
7.根據(jù)權利要求2所述的姿態(tài)估計方法,其中,所述姿態(tài)估計偏差的計算還包括以下步驟:
依據(jù)陀螺儀數(shù)據(jù)分別得到當前姿態(tài)估計和經(jīng)校準的姿態(tài)估計;
計算所述當前姿態(tài)估計與所述經(jīng)校準的姿態(tài)估計之差,以獲得所述姿態(tài)估計偏差。
8.根據(jù)權利要求2所述的姿態(tài)估計方法,其中,如果新姿態(tài)估計偏差與所述姿態(tài)估計偏差的平均值之間的差大于預定義閾值,則將所述校正增益強制調整為預定義值。
9.根據(jù)權利要求2所述的姿態(tài)估計方法,其中,如果新姿態(tài)估計偏差與所述姿態(tài)估計偏差的平均值之間的距離大于所述姿態(tài)估計偏差的標準方差的預定義閾值倍數(shù),則將所述校正增益強制調整為預定義值。
10.根據(jù)權利要求4所述的姿態(tài)估計方法,其中,所述運動加速度的范數(shù)的所述連續(xù)函數(shù)是線性函數(shù)或高斯函數(shù)。
11.根據(jù)權利要求7所述的姿態(tài)估計方法,其中,所述差是以下項目中的一個:
1)所述當前陀螺儀數(shù)據(jù)的范數(shù)減去所述經(jīng)校準的陀螺儀數(shù)據(jù);
2)依據(jù)所述當前陀螺儀數(shù)據(jù)與所述經(jīng)校準的陀螺儀數(shù)據(jù)之間的叉積運算的輸出矢量的范數(shù);
3)相應的歐拉角之間的差的求和。
12.一種針對運動物體的姿態(tài)估計裝置,所述姿態(tài)估計裝置包括:
輸入單元,其被構造為從陀螺儀數(shù)據(jù)和加速計數(shù)據(jù)輸入傳感器數(shù)據(jù);
校正增益確定單元,其被構造為基于利用所述陀螺儀數(shù)據(jù)和加速計數(shù)據(jù)而測量的物體運動程度,來確定校正增益;
校正矢量計算單元,其被構造為通過基于所述陀螺儀數(shù)據(jù)和加速計數(shù)據(jù),將所述參考矢量從地心坐標系轉換到體坐標系,來計算校正矢量;
傳感器數(shù)據(jù)校準單元,其被構造為使用所述校正增益和所述校正矢量,來校準所述陀螺儀數(shù)據(jù);
姿態(tài)估計和輸出單元,其被構造為利用經(jīng)校準的陀螺儀數(shù)據(jù)來估計并輸出所述姿態(tài),
其中,所述校正增益確定單元還基于所述加速計數(shù)據(jù)來估計在體坐標系中的重力分量;通過從所述加速度數(shù)據(jù)中去除重力分量來計算所述物體的運動加速度;基于所述物體的所述運動加速度的預定義函數(shù)來計算所述校正增益。
13.根據(jù)權利要求12所述的姿態(tài)估計裝置,所述校正增益確定單元還基于利用當前陀螺儀數(shù)據(jù)和經(jīng)校準的陀螺儀數(shù)據(jù)而計算的姿態(tài)估計偏差,來調整所述校正增益。
14.根據(jù)權利要求12或13所述的姿態(tài)估計裝置,其中,所述估計在體坐標系中的重力分量還包括:
基于所述加速計數(shù)據(jù)、陀螺儀數(shù)據(jù)和磁力計數(shù)據(jù)中的至少一個,確定所述物體處于靜止的狀態(tài);
在所述物體處于靜止狀態(tài),計算所述加速度的范數(shù);
在所述物體處于靜止狀態(tài),計算所述加速度的范數(shù)的平均值;
利用計算出的范數(shù)的平均值,構建在所述地心坐標系中的重力矢量;
基于使用陀螺儀數(shù)據(jù)而計算出的轉換矩陣,將所述重力矢量從地心 坐標系轉換到體坐標系。
15.根據(jù)權利要求12或13所述的姿態(tài)估計裝置,其中,所述運動加速度的預定義函數(shù)是分段函數(shù),并且如果所述運動加速度的范數(shù)超過預定義閾值,則將所述校正增益設置為預定義常量;否則,使用所述運動加速度的范數(shù)的連續(xù)函數(shù)來計算所述校正增益。
16.根據(jù)權利要求12或13所述的姿態(tài)估計裝置,其中,所述校正矢量計算單元還基于使用陀螺儀數(shù)據(jù)計算出的轉換矩陣,將所述地心坐標系中的參考矢量轉換到體坐標系;基于使用加速計數(shù)據(jù)計算出的轉換矩陣,或基于加速計數(shù)據(jù)和磁力計數(shù)據(jù),將地心坐標系中的參考矢量轉換到體坐標系;執(zhí)行兩個轉換的參考矢量之間的叉積運算,以生成校正矢量。
17.根據(jù)權利要求13所述的姿態(tài)估計裝置,其中,所述姿態(tài)估計偏差還包括以下步驟:
從陀螺儀數(shù)據(jù)分別得到所述當前姿態(tài)估計和經(jīng)校準的姿態(tài)估計;
計算所述當前姿態(tài)估計與所述經(jīng)校準的姿態(tài)估計之差,以獲得所述姿態(tài)估計偏差。
18.根據(jù)權利要求13所述的姿態(tài)估計裝置,其中,如果新姿態(tài)估計偏差與所述姿態(tài)估計偏差的平均值之間的差大于預定義閾值,則將所述校正增益強制調整為預定義值。
19.根據(jù)權利要求13所述的姿態(tài)估計裝置,其中,如果新姿態(tài)估計偏差與所述姿態(tài)估計偏差的平均值之間的距離大于所述姿態(tài)估計偏差的標準方差的預定義閾值倍數(shù),則將所述校正增益強制調整為預定義值。
20.根據(jù)權利要求17所述的姿態(tài)估計裝置,其中,所述差是以下項目中的一個:
1)所述當前陀螺儀數(shù)據(jù)的范數(shù)減去經(jīng)校準的陀螺儀數(shù)據(jù);
2)來自所述當前陀螺儀數(shù)據(jù)與經(jīng)校準的陀螺儀數(shù)據(jù)之間的叉積運算的輸出矢量的范數(shù);
3)相應的歐拉角之間的差的求和。