[发明专利]一种加速度计算方法

权利要求书

1.一种加速度计算方法，其特征在于：所述加速度计算方法选用载体坐标系XbYbZb和地理坐标系NED，其包括以下步骤

步骤一、采集九轴惯性传感器信息；

步骤二、利用四元数姿态解算建立运动学方程；

步骤三、建立九轴惯性传感器误差模型，解算正确的角速度值，并利用互补滤波器校准偏差，得到正确的角速度值；

步骤四、根据四元微分方程和校准偏差后的角速度值ω，得到正确的四元数；

步骤五、通过正确的四元数q₀、q₁、q₂和q₃，解算出刚体姿态角和加速度的真实值。

2.根据权利要求1所述的加速度计算方法，其特征在于：所述步骤一中，九轴惯性传感器采集信息包括：加速度计数据信息、磁力计数据信息、陀螺仪数据信息。

3.根据权利要求1所述的加速度计算方法，其特征在于：所述步骤二中，利用四元数姿态解算法建立四元数的姿态矩阵方程；采用数值积分法来求解矩阵和四元数微分方程；

其中，根据四元数的复数表达式q＝q₀+q₁i+q₂j+q₃k，通过矢量变换四元数姿态矩阵为：

数值积分法选择一阶龙格—库塔法解四元数微分方程：

四元数的微分方程：

一阶龙格—库塔法得到的计算式：

展开成分量表达式，有：

对四元数做归一化处理，得：

式中是载体坐标系相对导航坐标系的角速度，x、y、z代表陀螺仪上的x、y、z轴；通过将修正后得到真实的四个参数q₀、q₁、q₂和q₃值。

4.根据权利要求1所述的加速度计算方法，其特征在于：所述步骤三中，建立互补滤波器,利用陀螺仪输出角速度值和用加速度计和磁力计校准偏差，得到正确的角速度值；

校准偏差方案：加速度计修正两坐标系在X、Y轴的旋转误差；磁力计修正两坐标系在Z轴的旋转误差；结合X、Y、Z三个轴的旋转误差修正值；

具体包括：

确立互补滤波器设互补滤波器的传递函数为：

式中，k_P的大小决定了互补滤波器的截止频率，在大于f_T的高频阶段，陀螺仪数据决定解算结果；在小于f_T的低频阶段，对解算结果起主要作用的是加速度计和磁力计数据，k_I决定消除静态偏差用时；C(s)具有全通特性；

校准偏差后，加速度修正值为：

ω＝ω_g+δ

δ＝k_P·e+k_I·∫edt

e＝e_acc+e_mag

ω表示修正后的角速度值；ω_g表示陀螺仪输出的角速度向量；e表示加速度计和磁力计校准得到的误差值；δ表示角速度修正值，将修正后的角速度值ω代入步骤二，可计算出四元数q₀、q₁、q₂和q₃；

加速度计输出修正误差如下：

加速度计三轴数据为a_x、a_y、a_z，归一化后：

由加速度计校准得到的偏差为e_acc，v表示重力向量在机体坐标系的向量；

e_acc＝a×v

磁力计输出修正误差如下：

磁力计三轴数据为m_x、m_y、m_z，归一化后：

经过坐标变换后，m变为

设计一个理想的地磁场：Y轴指向正北方，此时Y轴的地磁力为1，X轴的地磁力为0；

磁场强度归一化向量为：

理论的地理磁场强度归一化向量经过坐标转换，得到理论的地理磁场强度在芯片上的归一化投影向量为：

e_mag＝m×w。