CN115659108A - 一种加速度计算方法 - Google Patents

Abstract

本发明公开了一种加速度计算方法，包括使用互补滤波器融合陀螺仪、加速度计和磁力计。本发明的有益效果是：利用互补滤波器，采集加速度计、陀螺仪、磁力计组成的九轴惯性传感器输出数据，计算出四元数，研究地理坐标系和载体坐标系之间的投影关系，采用四元数姿态解算法解算出刚体的运动方向，去除重力加速度g在加速度计三轴上的投影分量后，最终得到刚体的真实X、Y、Z轴的加速度与合加速度。

Description

一种加速度计算方法

技术领域

本发明涉及一种加速度计算方法，属于运动加速度研究技术领域。

背景技术

目前，在人体运动姿态、运动加速度和速度的研究领域方面，研究者常借助使用互补滤波姿态算法计算刚体的姿态角和使用三轴加速度计来采集加速度数据，进行加速度的计算，如：一种航姿参考系统的互补滤波姿态解算方法(专利申请号：202010464589.6)，一种加速度计的测试方法(专利申请号：201711408774.8)。经研究后发现，通过姿态解算只能得到刚体的运动方向；且采用三轴加速度计来计算加速度，得到的加速度方向与刚体的真实运动的加速度方向任然存在误差，依靠加速度的公式计算合加速度数值在计算计算加速度值时，存在外在因素影响，计算得到的加速度值也存在误差。

造成刚体加速度存在误差的原因主要有：

(1)方向问题：当芯片安置在刚体上，存在安装倾斜、放置位置变化、芯片与刚体接触不平整等诸多因素，刚体运动的三轴方向实际上很难与芯片的三轴方向完全重合，研究者仅凭三轴加速度计提供的各轴原始加速度数值，无法获得刚体真实的运动合加速度方向。

(2)数值问题：三轴加速度计提供的X、Y、Z各轴向的加速度数值含有重力加速度g的分量。

这种由加速度计提供的各轴原始加速度数值包含有重力加速度g的三轴投影分量，由该值计算出的刚体运动合加速度无论是在方向上，还是在数值大小上，都与真实的刚体运动合加速度相差甚远，加速度计输出的三轴向加速度并不等同于刚体的三轴向加速度数据。

因此，仅凭三轴加速度计自身是无法准确计算出刚体的合加速度，但依靠九轴惯性传感器提供的九轴数值，通过互补滤波器利用输出数据计算出四元数的算法，可以去掉重力加速度g的影响，从而计算出刚体真实的合加速度数值。

发明内容

本发明的目的就在于为了解决上述至少一个技术问题而提供一种加速度计算方法。

本发明通过以下技术方案来实现上述目的：一种加速度计算方法，包括选用载体坐标系XbYbZb和地理坐标系NED，包括以下步骤

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

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

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

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

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

作为本发明再进一步的方案：所述步骤一中，九轴惯性传感器采集信息包括：加速度计数据信息、磁力计数据信息、陀螺仪数据信息。

作为本发明再进一步的方案：所述步骤二中，利用四元数姿态解算法建立四元数的姿态矩阵方程；采用数值积分法来求解矩阵和四元数微分方程；

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

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

四元数的微分方程：

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

展开成分量表达式，有：

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

式中

是载体坐标系相对导航坐标系的角速度，x、y、z代表陀螺仪上的x、y、z轴；通过将

修正后得到真实的四个参数q₀、q₁、q₂和q₃值。

作为本发明再进一步的方案：所述步骤三中，建立互补滤波器,利用陀螺仪输出角速度值和用加速度计和磁力计校准偏差，得到正确的角速度值；

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

具体包括：

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

式中，k_P的大小决定了互补滤波器的截止频率，

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

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

ω＝ω_g+δ

δ＝k_P·e+k_I·∫e dt

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

作为本发明再进一步的方案：所述步骤四中，具体包括：

建立姿态矩阵方程，采用数值积分法来求解矩阵和四元数微分方程；利用陀螺仪输出角速度值和用加速度计和磁力计校准偏差，通过步骤三中得到的正确的角速度值ω,代入步骤二中可得真实的四元数数值。

作为本发明再进一步的方案：所述步骤五中，具体包括：

刚体姿态角的计算

通过载体坐标系相对于地理坐标系的关系得载体的姿态，地理坐标系到运载体坐标系的变换矩阵为

姿态矩阵与四元数姿态矩阵等价：

将计算所得四元数带入式中，计算出姿态角为

刚体加速度大小的计算：

用归一化后的四元数表示地理坐标系n和载体坐标系b之间的投影关系，

为姿态矩阵；

在只考虑重力加速度g的影响情况下，可只保留姿态矩阵

的第三列，其它参数可都设置为零；

重力加速度g对载体坐标系b中X_b、Y_b、Z_b三轴上的加速度影响量为：

刚体线加速度为a_b，可得

令加速度计坐标系为设载体坐标系b，载体坐标系b中X_b、Y_b、Z_b三轴上的加速度为

由加速度计采集得到三轴上原始加速度数值为A_x、A_y、A_z。那么

刚体三轴向加速度数值的计算式为：

刚体合加速度数值的计算式为：

本发明的有益效果是：互补滤波器利用三轴加速度计、三轴磁力计与三轴陀螺仪在频域上的互补特性，对陀螺仪(低频噪声)使用高通滤波、对加速度计与磁力计(高频噪声)使用低频滤波，算出实际旋转矩阵与理论值之间的偏差，完成四元数姿态矩阵的修正，得到正确的四元数数值，求得刚体真实的加速度。

附图说明

图1为本发明流程示意图；

图2为本发明地理坐标系n和载体坐标系b之间的投影关系示意图；

图3为本发明重力加速度g对载体坐标系b的三轴影响示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

如图1至图3所示，一种加速度计算方法，包括选用载体坐标系XbYbZb和地理坐标系NED，包括以下步骤

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

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

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

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

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

在本发明实施例中，所述步骤一中，九轴惯性传感器采集信息包括：加速度计数据信息、磁力计数据信息、陀螺仪数据信息。

在本发明实施例中，所述步骤二中，利用四元数姿态解算法建立四元数的姿态矩阵方程；采用数值积分法来求解矩阵和四元数微分方程；

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

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

四元数的微分方程：

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

展开成分量表达式，有：

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

式中

是载体坐标系相对导航坐标系的角速度，x、y、z代表陀螺仪上的x、y、z轴；通过将

修正后得到真实的四个参数q₀、q₁、q₂和q₃值。

在本发明实施例中，所述步骤三中，建立互补滤波器,利用陀螺仪输出角速度值和用加速度计和磁力计校准偏差，得到正确的角速度值；

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

具体包括：

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

式中，k_P的大小决定了互补滤波器的截止频率，

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

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

ω＝ω_g+δ

δ＝k_P·e+k_I·∫e dt

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

在本发明实施例中，所述步骤四中，具体包括：

建立姿态矩阵方程，采用数值积分法来求解矩阵和四元数微分方程；利用陀螺仪输出角速度值和用加速度计和磁力计校准偏差，通过步骤三中得到的正确的角速度值ω,代入步骤二中可得真实的四元数数值。

在本发明实施例中，所述步骤五中，具体包括：

刚体姿态角的计算

通过载体坐标系相对于地理坐标系的关系得载体的姿态，地理坐标系到运载体坐标系的变换矩阵为

姿态矩阵与四元数姿态矩阵等价：

将计算所得四元数带入式中，计算出姿态角为

刚体加速度大小的计算：

用归一化后的四元数表示地理坐标系n和载体坐标系b之间的投影关系，

为姿态矩阵。

在只考虑重力加速度g的影响情况下，可只保留姿态矩阵

的第三列，其它参数可都设置为零。

重力加速度g对载体坐标系b中X_b、Y_b、Z_b三轴上的加速度影响量为：

刚体线加速度为a_b，可得

令加速度计坐标系为设载体坐标系b，载体坐标系b中X_b、Y_b、Z_b三轴上的加速度为

由加速度计采集得到三轴上原始加速度数值为A_x、A_y、A_z。那么

刚体三轴向加速度数值的计算式为：

刚体合加速度数值的计算式为：

实施例二

一种加速度计算方法，包括选用载体坐标系XbYbZb和地理坐标系NED，包括以下步骤划船时，可以通过安装九轴惯性传感器输出数据，通过互补滤波器修正误差，计算出船体的真实加速度，使船手可以明确知晓自己的运动状态，从而做出改变。

工作原理：使用九轴惯性传感器收集数据和互补滤波器修正数据，通过四元数法进行姿态解算，解算出刚体在相对坐标系下的姿态角，和刚体的真实加速度。

利用四元数姿态解算法建立四元数的姿态矩阵方程，采用数值积分法来求解矩阵和四元数微分方程；通过采集九轴惯性传感器信息，使用互补滤波器融合陀螺仪、加速度计、磁力计这三种传感器输出的数据，可以提高测量精度和系统的动态性能，用加速度计与磁力计进行旋转误差修正得到加速度计的修校准偏差值，再使用陀螺仪输出的经过修正的角速度数值ω，得到正确的四元数q₀、q₁、q₂和q₃。

通过姿态解算，得到刚体在相对坐标系下的姿态角，且通过姿态矩阵得到载体坐标系b中X_b、Y_b、Z_b三轴上的加速度为

数值，和加速度计采集得到三轴上原始加速度数值为A_x、A_y、A_z的基础上，计算出刚体真实的三轴向加速度

通过合加速度数值的计算式，得到真实的合加速度a_b。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (6)

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。

5.根据权利要求1所述的加速度计算方法，其特征在于：所述步骤四中，具体包括：

建立姿态矩阵方程，采用数值积分法来求解矩阵和四元数微分方程；利用陀螺仪输出角速度值和用加速度计和磁力计校准偏差，通过步骤三中得到的正确的角速度值ω,代入步骤二中可得真实的四元数数值。

6.根据权利要求1所述的加速度计算方法，其特征在于：所述步骤五中，具体包括：

刚体姿态角的计算

通过载体坐标系相对于地理坐标系的关系得载体的姿态，地理坐标系到运载体坐标系的变换矩阵为

姿态矩阵与四元数姿态矩阵等价：

将计算所得四元数带入式中，计算出姿态角为

刚体加速度大小的计算：

用归一化后的四元数表示地理坐标系n和载体坐标系b之间的投影关系，

为姿态矩阵；

在只考虑重力加速度g的影响情况下，可只保留姿态矩阵

的第三列，其它参数可都设置为零；

重力加速度g对载体坐标系b中X_b、Y_b、Z_b三轴上的加速度影响量为：

刚体线加速度为a_b，可得

令加速度计坐标系为设载体坐标系b，载体坐标系b中X_b、Y_b、Z_b三轴上的加速度为

由加速度计采集得到三轴上原始加速度数值为A_x、A_y、A_z，那么

刚体三轴向加速度数值的计算式为：

刚体合加速度数值的计算式为：

Images