[发明专利]基于加速度计和陀螺仪的载体转速与转角补偿测量方法

权利要求书

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θacc(n)=ax(n)ay(n),]]>

继而得到：

θacc(n)=arctanax(n)ay(n);]]>

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

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

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

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

τ=W×T1-W]]>

得到：

W=ττ+T;]]>

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

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

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

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

ω(n)=θest(n)-θest(n-1)T.]]>