[发明专利]方向估计方法及装置

权利要求书

1.一种设备的方向估计方法，所述方向估计方法包括：

第一方向估计步骤，用于根据陀螺仪数据估计第一方向；

第二方向估计步骤，用于根据磁力计数据和所述设备的俯仰角及滚转角估计第二方向；

校正增益确定步骤，用于通过将第一校正增益和第二校正增益进行组合来确定校正增益；

校准步骤，用于利用所述校正增益来校准所述第一方向估计和所述第二方向估计，以得到所述设备的方向；

其中，所述校正增益确定步骤还包括以下的步骤：基于磁力计数据和磁场的理论值来计算所述第一校正增益；基于磁力计数据的变化率来计算所述第二校正增益。

2.根据权利要求1所述的方向估计方法，所述方向估计方法还包括以下步骤：基于所述两个方向，根据原始的陀螺仪数据和所校准的方向来调整所述校正增益。

3.根据权利要求1或2所述的方向估计方法，其中，所述方向估计方法还包括以下步骤：

对磁力计数据与地磁场的理论值之差的范数的第一分段函数进行定义，以及

对磁力计数据的范数的变化率的第二分段函数进行定义。

4.根据权利要求3所述的方向估计方法，其中，在磁力计数据的范数位于预定义范围内的情况下，所述第一分段函数是高斯函数或线性函数。

5.根据权利要求3所述的方向估计方法，其中，在磁力计数据与地磁场的理论值在正交轴上的差的范数位于预定义范围内的情况下，所述第一分段函数是单调递减函数。

6.根据权利要求3所述的方向估计方法，其中，在磁力计数据的范数的变化率位于预定义范围内的情况下，所述第二分段函数是高斯函数或线性函数。

7.根据权利要求2所述的方向估计方法，其中，所述方向估计方法还包括以下步骤：

计算所述两个方向的偏差。

8.根据权利要求2所述的方向估计方法，其中，在新的方向偏差与方向偏差的平均值之差大于预定义阈值的情况下，将所述校正增益强制调整为预定义值。

9.根据权利要求2所述的方向估计方法，其中，在新的方向偏差与方向偏差的平均值之差大于预定义阈值乘以该方向的标准方差的情况下，将所述校正增益强制调整为预定义值。

10.一种设备的方向估计装置，所述方向估计装置包括：

输入单元，其被构造为输入来自陀螺仪和磁力计的传感器数据；

方向估计单元，其被构造为根据陀螺仪估计第一方向，以及根据磁力计和所述设备的俯仰角及滚转角估计第二方向；

校正增益确定单元，其被构造为通过将第一校正增益和第二校正增益进行组合来确定校正增益；

方向校准单元，其被构造为利用所述校正增益来校准所述第一方向估计和所述第二方向估计，以得到所述设备的方向；

估计和输出单元，其被构造为根据所校准的数据来估计和输出所述设备的方向；

其中，所述校正增益确定单元还包括：基于磁力计数据和磁场的理论值来计算所述第一校正增益；基于磁力计数据的变化率来计算所述第二校正增益。

11.根据权利要求10所述的方向估计装置，所述校正增益确定单元还被构造为基于所述两个方向，根据原始的陀螺仪数据和所校准的方向来调整所述校正增益。

12.根据权利要求10或11所述的方向估计装置，其中，所述方向估计装置还包括：

对磁力计数据与地磁场的理论值之差的范数的第一分段函数进行定义，以及

对磁力计数据的范数的变化率的第二分段函数进行定义。

13.根据权利要求12所述的方向估计装置，其中，在磁力计数据的范数位于预定义范围内的情况下，所述第一分段函数是高斯函数或线性函数。

14.根据权利要求12所述的方向估计装置，其中，在磁力计数据与地磁场的理论值在正交轴上的差的范数位于预定义范围内的情况下，所述第一分段函数是单调递减函数。

15.根据权利要求12所述的方向估计装置，其中，在磁力计数据的范数的变化率位于预定义范围内的情况下，所述第二分段函数是高斯函数或线性函数。