[发明专利]使用加速度传感器和地磁传感器计算角速度的方法和设备

权利要求书

1.一种基于加速度传感器和地磁传感器确定对象的角速度的角速度计算方法，包括：

基于误差四元数导出状态变量和状态变量的方差，并且将基于四元数的旋转矩阵转换为基于误差四元数的旋转矩阵；

在使用所述基于误差四元数的旋转矩阵重新定义所述加速度传感器的输出和所述地磁传感器的输出之后，基于所述加速度传感器的所述输出和所述地磁传感器的所述输出计算卡尔曼滤波器的观测矩阵和输出矩阵；

在通过离散变换矩阵变换所述状态变量和所述状态变量的方差之后，基于所变换的状态变量的方差和所述卡尔曼滤波器的观测矩阵来计算所述卡尔曼滤波器的增益；

在通过所述卡尔曼滤波器的增益校准所述状态变量和所述状态变量的方差之后，基于校准的状态变量和估计的四元数来计算四元数；以及

基于所述四元数来计算角速度。

2.根据权利要求1所述的方法，其中，在将所述基于四元数的旋转矩阵转换为所述基于误差四元数的旋转矩阵中，表示参考坐标系和传感器坐标系之间的相对角度变化的四元数q＝[q₀,q_x,q_y,q_z]’的误差四元数[q_ex,q_ey,q_ez]’被设定为状态变量x，其中q₀、q_x、q_y和q_z是构成所述四元数的四个因子，并且q_ex、q_ey和q_ez是所述误差四元数的向量分量因子。

3.根据权利要求2所述的方法，其中，在将所述基于四元数的旋转矩阵转换为所述基于误差四元数的旋转矩阵中，在当前状态和先前状态具有仅产生时间延迟τ的统计相关性时，由方程式表示状态变量x，F是由表示的变换矩阵，并且τ是表示状态之间的关联度的时间因子。

4.根据权利要求1所述的方法，其中，在将所述基于四元数的旋转矩阵转换为所述基于误差四元数的旋转矩阵中，所述状态变量的方差p通过方程式来计算，其中，x是状态变量，是估计的状态变量，是状态变量的估计的方差，并且'是转置矩阵。

5.根据权利要求1所述的方法，其中，在将所述基于四元数的旋转矩阵转换为所述基于误差四元数的旋转矩阵中，所述基于四元数的旋转矩阵基于方程式被转换为所述基于误差四元数的旋转矩阵，所述基于四元数的旋转矩阵为所述基于误差四元数的旋转矩阵为其中，是基于估计的四元数的旋转矩阵，q₀、q_x、q_y和q_z是构成所述四元数的四个因子，q_ex、q_ey和q_ez是所述误差四元数的向量分量因子，I_3x3是3×3单位矩阵，q_e×表示q₀是与所述四元数中的标量对应的因子，并且θ表示根据查尔斯的定义的欧拉角或旋转角。

6.根据权利要求5所述的方法，其中，在将所述基于四元数的旋转矩阵转换为所述基于误差四元数的旋转矩阵中，当q_ex、q_ey和q_ez是微小量时，所述基于误差四元数的旋转矩阵表示为并且被简化为