CN103913171A - 基于加速度计和陀螺仪的载体转速与转角补偿测量方法 - Google Patents

Abstract

本发明是一种基于加速度计和陀螺仪的载体转速与转角补偿测量方法,属于惯性传感器与测量技术领域。本发明方法同时采用三轴加速度计和单轴陀螺仪对载体转动的转角进行估计，充分利用其在测量频域上的互补性，得到准确的载体转角估计结果。同时，通过该结果计算得到的载体转速，相比于直接利用陀螺仪测量得到的载体转速，可以保持与载体转角持续的一致性。本方法的传感器可以安装于载体旋转平面上的任意位置，不依赖于载体确定的旋转中心和旋转半径，因此不需要对载体进行物理尺寸、旋转中心、旋转半径的测量，并可对旋转中心和旋转半径变化的运动载体进行测量，这极大地简化了测量的复杂性，提高了该方法的适用范围。

Description

基于加速度计和陀螺仪的载体转速与转角补偿测量方法

技术领域

本发明是一种基于加速度计和陀螺仪的载体转速与转角补偿测量方法,属于惯性传感器与测量技术领域。

背景技术

对运动载体转速和转角测量方法的研究一直是自动化、测量控制领域当中的基础研究，对载体转速与转角的测量可广泛见于多种不同领域的不同应用场合。转动角速度和转动角度是描述一个旋转运动物体运动状态的重要参数，它表征了被测物体在旋转运动中的角位移及角位移的变化率。

传统的角速度和角位移测量方法有光电编码器测量、电磁感应测量等方法，这几种方法均需要在载体和参照物上精确安装光电传感器或者霍尔传感器，并且其测量对象只能是以固定圆心固定半径转动的载体，无法对非固定圆心或者非固定半径的转动载体进行测量；另有一种基于加速度计的转角测量方法，测量传感器安装简单，但该方法下加速度计测量值由于外力等因素干扰噪声较大，并且需要测量载体等效旋转半径，在无法得知、准确测量旋转半径或旋转半径变化的情况下，这种测量方法并不适用；还有一种基于陀螺仪的测量方法，该方法利用陀螺仪直接测量载体旋转角速度，短时间内具有较高的精度，但在求解角位移时由于其使用的方法是对角速度进行积分，长期积分之后积分结果漂移严重，所得角位移具有很大的累积误差。

发明内容

本发明的目的是为解决现有测量技术依赖于载体的物理结构和运动方式，实施复杂和误差较大的问题，提供一种基于加速度计和陀螺仪的载体转速与转角补偿测量方法，本发明方法中传感器安装简单易行，测量结果精确可靠，长时间内不漂移发散。

为了达到上述目的，本发明提供一种基于加速度计和陀螺仪的载体转速与转角补偿测量方法。所述载体为任意做旋转运动的载体，所述测量的内容为在任意非水平面内做旋转运动的载体的角速度与角位移。所述的测量方法包含以下步骤：

第一步：将传感器和相关测量电路安装于载体任意位置上，使得加速度计x、y轴平面与载体转动平面平行即可，确定载体的初始转角并记为θ_est(0)；

所述传感器包括三轴加速度计、单轴陀螺仪。

所述相关测量电路包括信号采集电路和处理电路，采集电路分别与三轴加速度计和单轴陀螺仪连接，用于采集数据，处理电路与采集电路连接，用于进行数据处理。

第二步：确定传感器数据采样周期T和互补滤波器数字滤波方法中的互补滤波器时间常数τ，令载体开始转动，测量电路以采样周期T读取三轴加速度计和单轴陀螺仪的测量值，当前时刻n的测量值包括加速度计x轴方向上的加速度值a_x(n)、y轴方向上的加速度值a_y(n)、陀螺仪角速度测量值rate(n)，采用移动平均方法对采集到的加速度计和陀螺仪测量值进行平稳去噪；

所述采样周期T根据实际测量需求确定。

所述互补滤波器时间常数τ根据传感器的频率特性决定，通常取值在0.5～5之间。

第三步：利用加速度计测量值a_x(n)和a_y(n)计算加速度计所测载体转角θ_acc(n)，利用陀螺仪测量值rate(n)计算陀螺仪所测载体转角θ_gyro(n)。具体方法为：

所述的利用加速度计测量值计算载体转角是利用重力加速度在x轴和y轴上分量的三角函数关系求得，当载体在非水平面内做旋转运动时，重力加速度在加速度计的x轴和y轴上产生分量，在不考虑其他外力情况下，利用该x轴和y轴上的加速度分量根据三角关系求得：

${\tan \mathrm{\θ}}_{\mathrm{acc}}\left(n\right)=\frac{a_x\left(n\right)}{a_y\left(n\right)},$

继而得到：

${\mathrm{\θ}}_{\mathrm{acc}}\left(n\right)=\arctan \frac{a_x\left(n\right)}{a_y\left(n\right)}.$

所述的利用陀螺仪测量值计算载体转角是利用转动角速度对时间积分求得。利用上一时刻转角估计值以及当前时刻角速度测量值，并将积分运算离散化为求和运算以后可得：

θ_gyro(n)＝θ_est(n-1)+rate(n)×T；

第四步：根据传感器数据采样周期T和互补滤波器时间常数τ计算滤波器权重系数W，采用互补滤波器数字滤波方法计算载体转角估计值θ_est(n)。互补滤波器数字滤波方法的具体过程为：

根据传感器数据采样周期和互补滤波器时间常数τ计算滤波器权重系数W，由该滤波器时间常数

$\mathrm{\τ}=\frac{W\×T}{1-W}$

可得：

$W=\frac{\mathrm{\τ}}{\mathrm{\τ}+T};$

再利用互补滤波器将加速度计测量计算所得转角θ_acc(n)和陀螺仪测量计算所得转角θ_gyro(n)权重相加，得到载体转角估计值：

θ_est(n)＝W×θ_gyro(n)+(1-W)×θ_acc(n)。

第五步：利用当前时刻载体转角估计值θ_est(n)、上一时刻载体转角估计值θ_est(n-1)和采样周期T计算载体转动角速度ω(n)；

所述的载体转动角速度ω(n)的求解是利用载体转角的估计值对时间微分得到，将微分运算离散化后得到：

$\mathrm{\ω}\left(n\right)=\frac{{\mathrm{\θ}}_{\mathrm{est}}\left(n\right)-{\mathrm{\θ}}_{\mathrm{est}}(n-1)}{T}.$

有益效果

本发明的基于加速度计和陀螺仪的载体转速与转角补偿测量方法，同时采用三轴加速度计和单轴陀螺仪对载体转动的转角进行估计。加速度计测量值在短期内噪声较大，但在进行转角估计时经过长时间测量运算之后不会有累积性偏移，低频测量性能好；而陀螺仪长时间积分之后偏移很大，但短期内测量精度很高，高频测量性能较好；本发明方法可以很好地结合这二者各自的优势，充分利用其在测量频域上的互补性，从而得到准确的载体转角估计结果。同时，通过该结果计算得到的载体转速，相比于直接利用陀螺仪测量得到的载体转速，可以保持与载体转角持续的一致性。因此，相比于单独使用加速度计或陀螺仪，该发明方法在转角和转速的测量精度上都具有很大的优势。

另一方面，本发明所述测量方法中，其传感器可以安装于载体旋转平面上的任意位置，不依赖于载体确定的旋转中心和旋转半径，因此不需要对载体进行物理尺寸、旋转中心、旋转半径的测量，并可对旋转中心和旋转半径变化的运动载体进行测量，这极大地简化了测量的复杂性,提高了该方法的适用范围。

因此，基于加速度计和陀螺仪的载体转速和转角补偿测量方法可以实现一种能够不依赖于载体物理结构、载体转动方式、传感器安装方式的转速转角测量方法，在实际应用中具有显著的意义。

附图说明

图1是本发明中利用加速度计测量载体转角原理示意图；

图2是本发明提供的一种基于加速度计和陀螺仪的载体转速与转角补偿测量方法的原理示意图；

图3是具体实施例中三轴加速度计x轴和y轴测量值曲线；

图4是具体实施例中分别由加速度计、陀螺仪和互补滤波器求得的载体转角曲线；

图5是具体实施例中分别由陀螺仪测量得到和利用互补滤波器计算得到的载体转速曲线。

具体实施方式

本发明已应用于油田抽油机曲柄转动的测量和两轮平衡车的平衡控制中。在抽油机曲柄转动的测量应用当中，曲柄转动发生在竖直平面内，是周期性非匀速圆周运动，其转动半径不易测量且振动剧烈；在两轮平衡车的平衡控制应用中，车体在竖直平面内做一定范围内的往复转动，其转动半径不易准确测量，且转动圆心随着车体运动时刻变化。故以上两种场景下均适合使用本发明提出的补偿测量方法。

以下根据图1和图2具体说明本发明的较佳实施例。如图1所示，本发明提供一种基于加速度计和陀螺仪的载体转速与转角补偿测量方法，该方法按以下步骤实施：

1.将传感器和测量电路安装于载体旋转平面内任意位置，将载体此时刻转角记为转角初始估计值θ_est(0)＝0，启动载体使其以某种方式做旋转运动。传感器包括三轴加速度计、单轴陀螺仪，其中三轴加速度计的x、y轴平面应与陀螺仪测量轴线垂直，这种结构位置关系符合常见的传感器测量坐标设计，可以通过元器件在PCB电路板上的正常焊接实现。

2.确定传感器数据采样周期T(在本实施例中取T＝0.00125s)和互补滤波器时间常数τ(在本实施例中取τ＝1s)，根据T和τ计算滤波器系数

$W=\frac{\mathrm{\τ}}{\mathrm{\τ}+\mathrm{dt}}=\frac{1}{1+0.00125}=0.99875$

3.间隔固定周期T读取三轴加速度计x轴和y轴测量值以及陀螺仪的测量值。在本实施例中，利用测量电路内的信号采集电路设置定时器频率为800Hz，以该固定频率读取传感器测量值。加速度计实际测量值如图3所示。

4.根据x轴加速度测量值和y轴加速度测量值计算载体当前时刻转角θ_acc(n)；如图2，转角θ_acc(n)的计算是利用重力加速度在x、y轴上的分量实现的，遵循三角法则数学关系，由正切关系得：

${\tan \mathrm{\θ}}_{\mathrm{acc}}\left(n\right)=\frac{a_x\left(n\right)}{a_y\left(n\right)}$

再反解得到当前时刻载体转角：

${\mathrm{\θ}}_{\mathrm{acc}}\left(n\right)=\arctan \frac{a_x\left(n\right)}{a_y\left(n\right)}$

由实测数据计算所得结果如图4中θ_acc所示。

5.根据陀螺仪当前时刻测量值rate(n)以及上一时刻载体转角估计值θ_est(n-1)计算当前时刻载体转角θ_gyro(n)。该计算是利用陀螺仪测得的角速度对时间积分求得，在本实施例当中，将积分运算离散化，利用求和运算得到当前时刻载体转角，计算过程如下：

θ_gyro(n)＝θ_est(n-1)+rate(n)×T

＝θ_est(n-1)+rate(n)×0.00125

其中当n＝1时，初始值θ_est(0)为初始时刻载体转角，T为传感器数据采样周期。

由实测数据计算所得结果如图4中θ_gyro所示。

6.根据加速度计求得的当前时刻载体转角和陀螺仪求得的当前时刻载体转角，利用互补滤波器以及已经确定的滤波器系数将二者融合，计算得到当前时刻载体转角的滤波器估计值

θ_est(n)＝W×θ_gyro(n)+(1-W)×θ_acc(n)

＝0.99875×θ_gyro(n)+0.00125×θ_acc(n)

由实测数据计算所得结果如图4中θ_est所示。同时，作为对比，将单独使用陀螺仪计算得到的载体转角结果用θ_gyro-only表示。通过图4中曲线的比较可以看出，由加速度计得到的载体转角θ_acc波动较大，但长时间计算后仍不会有漂移；单独由陀螺仪计算得到的载体转角θ_gyro-only短时间内发展趋势与θ_acc一致，但随时间推移其积分漂移越来越严重；由互补滤波器计算得到的转角估计θ_est在短时间内能够与θ_gyro-only保持一致的发展趋势，保持较高的精度，同时长时间后仍然不会发生大的漂移，具有较高的稳定性。而θ_gyro作为中间结果，与θ_est基本保持一致。

7.根据载体转角估计值对时间微分求得载体转动角速率。在本实施例中，将微分运算离散化，利用当前时刻载体转角估计值、上一时刻载体转角估计值以及固定的采样周期T，可以求得载体当前时刻转动角速率

$\begin{array}{c}\mathrm{\ω}\left(n\right)=\frac{{\mathrm{\θ}}_{\mathrm{est}}\left(n\right)-{\mathrm{\θ}}_{\mathrm{est}}(n-1)}{T}\\ =\frac{{\mathrm{\θ}}_{\mathrm{est}}\left(n\right)-{\mathrm{\θ}}_{\mathrm{est}}(n-1)}{0.00125}\end{array}$

由实测数据计算所得结果如图5中ω_est所示。同时，作为对比，将陀螺仪测量角速率原始数据用ω_gyro表示。通过对比可以看出，ω_est与陀螺仪原始测量数据ω_gyro保持高度的一致性，这反过来说明了使用互补滤波器所得到的转角估计和转速估计都具有较高的精准度。

由实际应用的测试结果可见，本发明所述方法能够方便有效地广泛应用于转角转速测量场合当中，同时保持其较高的测量精确性和准确性。

以上所述仅是本发明的一个具体实施例，应当认识到上述对实施例的描述不应被认为是对本发明的限制。本领域的普通技术人员能够从本发明公开的内容直接推导或联想到的所有变形，都将是显而易见的。因此，本发明的保护范围应当依据所附的权利要求来限定。

Claims (4)

1.基于加速度计和陀螺仪的载体转速与转角补偿测量方法，其特征在于：具体包括如下步骤：

第一步：将传感器和相关测量电路安装于载体任意位置上，使得加速度计x、y轴平面与载体转动平面平行即可，确定载体的初始转角并记为θ_est(0)；

所述传感器包括三轴加速度计、单轴陀螺仪；

所述相关测量电路包括信号采集电路和处理电路，采集电路与传感器连接，处理电路与采集电路连接；

第二步：确定传感器数据采样周期T和互补滤波器数字滤波方法中的互补滤波器时间常数τ，令载体开始转动，测量电路以采样周期T读取三轴加速度计和单轴陀螺仪的测量值，当前时刻n的测量值包括加速度计x轴方向上的加速度值a_x(n)、y轴方向上的加速度值a_y(n)、陀螺仪角速度测量值rate(n)，采用移动平均方法对采集到的加速度计和陀螺仪测量值进行平稳去噪；

第三步：利用加速度计测量值a_x(n)和a_y(n)计算加速度计所测载体转角θ_acc(n)，利用陀螺仪测量值rate(n)计算陀螺仪所测载体转角θ_gyro(n)；具体方法为：

所述的利用加速度计测量值计算载体转角是利用重力加速度在x轴和y轴上分量的三角函数关系求得，当载体在非水平面内做旋转运动时，重力加速度在加速度计的x轴和y轴上产生分量，在不考虑其他外力情况下，利用该x轴和y轴上的加速度分量根据三角关系求得：

${\tan \mathrm{\θ}}_{\mathrm{acc}}\left(n\right)=\frac{a_x\left(n\right)}{a_y\left(n\right)},$

继而得到：

${\mathrm{\θ}}_{\mathrm{acc}}\left(n\right)=\arctan \frac{a_x\left(n\right)}{a_y\left(n\right)};$

所述的利用陀螺仪测量值计算载体转角是利用转动角速度对时间积分求得；利用上一时刻转角估计值以及当前时刻角速度测量值，并将积分运算离散化为求和运算以后可得：

θ_gyro(n)＝θ_est(n-1)+rate(n)×T；

第四步：根据传感器数据采样周期T和互补滤波器时间常数τ计算滤波器权重系数W，采用互补滤波器数字滤波方法计算载体转角估计值θ_est(n)；互补滤波器数字滤波方法的具体过程为：

根据传感器数据采样周期和互补滤波器时间常数τ计算滤波器权重系数W，由该滤波器时间常数

$\mathrm{\τ}=\frac{W\×T}{1-W}$

得到：

$W=\frac{\mathrm{\τ}}{\mathrm{\τ}+T};$

再利用互补滤波器将加速度计测量计算所得转角θ_acc(n)和陀螺仪测量计算所得转角θ_gyro(n)权重相加，得到载体转角估计值：

θ_est(n)＝W×θ_gyro(n)+(1-W)×θ_acc(n)；

第五步：利用当前时刻载体转角估计值θ_est(n)、上一时刻载体转角估计值θ_est(n-1)和采样周期T计算载体转动角速度ω(n)；

所述的载体转动角速度ω(n)的求解是利用载体转角的估计值对时间微分得到，将微分运算离散化后得到：

$\mathrm{\ω}\left(n\right)=\frac{{\mathrm{\θ}}_{\mathrm{est}}\left(n\right)-{\mathrm{\θ}}_{\mathrm{est}}(n-1)}{T}.$

2.根据权利要求1所述的基于加速度计和陀螺仪的载体转速与转角补偿测量方法，其特征在于：所述采样周期T根据实际测量需求确定。

3.根据权利要求1所述的基于加速度计和陀螺仪的载体转速与转角补偿测量方法，其特征在于：所述互补滤波器时间常数τ根据传感器的频率特性决定，取值在0.5～5之间。

4.根据权利要求1所述的基于加速度计和陀螺仪的载体转速与转角补偿测量方法，其特征在于：所述载体为任意做旋转运动的载体，所述测量的内容为在任意非水平面内做旋转运动的载体的角速度与角位移。