文章目录
一、互补滤波
1.陀螺仪和加速计(特性分析)
1)陀螺仪2)加速度计3)磁力计比喻 二、低通滤波
1.一阶低通滤波
一、互补滤波
参考链接
1.陀螺仪和加速计(特性分析) 1)陀螺仪灵敏度
spacespacespacespacespacespacespace
测量角速度,具有高动态特性,它是一个间接测量角度的器件其中一个关键参数就是gyro sensitivity(其单位是millivolts per degree persecond,把转速转换到电压值),测量范围越小气灵敏度越好。也就是说测量的是角度的导数,即角速度,要将角速度对时间积分才能得到角度。陀螺仪就是内部有一个陀螺,它的轴由于陀螺效应始终与初始方向平行,这样就可以通过与初始方向的偏差计算出旋转方向和角度。
偏移
spacespacespacespacespacespacespace
偏移就是在陀螺没有转动的时候却又输出,这个输出量的大小和供电电压以及温度有关,该偏移可以在陀螺仪上电时通过一小段时间的测量来修正。
漂移
spacespacespacespacespacespacespace
它是由于在时间的积累下偏移和噪声相互影响的结果,例如有一个偏置(offset)0.1dps加在上面,于是测量出来是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,还能忍受,一小时之后是360度,转了一圈,也就是说,陀螺仪在短时间内有很大的参考价值。
白噪声
spacespacespacespacespacespacespace
电信号的测量中,一定会带有白噪声,陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声,而且这种白噪声会随着积分而累加。
积分误差
spacespacespacespacespacespacespace
对陀螺仪角速度的积分是离散的,长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。
溢出
spacespacespacespacespacespacespace
就是转速超过了其测量的最大转速范围。
捷联式惯性导航系统传送门
spacespacespacespacespacespacespace
加速度计的低频特性好,可以测量低速的静态加速度。在无人机上就是对重力加速度g的测量和分析。当把加速度计拿在手上随意转动时,看的是重力加速度在三个轴上的分量值。
spacespacespacespacespacespacespace
加速度计在自由落体时,其输出为0。为什么会这样呢?这里涉及到加速度计的设计原理:加速度计测量加速度是通过比力来测量(比力方程,秦永元的书中有介绍),而不是通过加速度。加速度计仅仅测量的是重力加速度,而重力加速度与刚才所说的R坐标系(地理坐标系)(Earthframe)是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系。
spacespacespacespacespacespacespace
加速度计仅仅测量的是重力加速度,3轴加速度计输出重力加速度在加速度计所在机体坐标系3个轴上的分量大小。重力加速度的方向和大小是固定的。通过这种关系,可以得到加速度计所在平面与地面的角度关系。
spacespacespacespacespacespacespace
加速度计若是绕着重力加速度的轴转动,则测量值不会改变,也就是说加速度计无法感知这种水平旋转。
比力
spacespacespacespacespacespacespace
可以测量磁场,在没有其他磁场的情况下,仅仅测量的是地球的磁场,而地磁也是和R坐标系固连的,通过这种关系,可以得到平面A和地平面的关系。(平面A:和磁场方向垂直的平面),同样的,若是沿着磁场方向的轴旋转,测量值不会改变,无法感知这种旋转。
spacespacespacespacespacespacespace
综合考虑,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系,动态性能好,受外部干扰小,但测量值随时间变化比较大。可以看出,它们优缺点互补,结合起来才能有好的效果。
有关陀螺仪和加速度计和关系,姿态解算融合的原理,再把下面这个比喻放到这里一遍。
spacespacespacespacespacespacespace
机体好似一条船,姿态就是航向(船头的方位),重力是灯塔,陀螺(角速度积分)是舵手,加速度计是瞭望手。舵手负责估计和把稳航向,他相信自己,本来船向北开的,就一定会一直往北开,觉得转了90度弯,那就会往东开。当然如果舵手很牛逼,也许能估计很准确,维持很长时间。不过只信任舵手,肯定会迷路,所以一般都有瞭望手来观察误差。
spacespacespacespacespacespacespace
瞭望手根据地图灯塔方位和船的当前航向,算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位,那肯定船的当前航向有偏差了,偏差就是ERR=X-Y。舵手收到瞭望手给的ERR报告,觉得可靠,那就听个90%ERR,觉得天气不好、地图误差大,那就听个10%ERR,根据这个来纠正估算航向。
陀螺仪加速度计MPU6050
#ifdef USE_MAG//(用的这个,在普通互补融合滤波基础上增加了磁力计补偿) d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ; #else //(这个没用,普通的互补融合滤波,用加速度计pi控制器输出补偿陀螺仪) d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use ) * dT / 2 ; #endif } // 计算姿态。 姿态更新(一阶龙格库塔法,wxyz初值为1000) imu->w = imu->w - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z]; imu->x = imu->w*d_angle[X] + imu->x + imu->y*d_angle[Z] - imu->z*d_angle[Y]; imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y + imu->z*d_angle[X]; imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z; q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z); imu->w *= q_norm_l; imu->x *= q_norm_l; imu->y *= q_norm_l; imu->z *= q_norm_l;
这里的d_angle是陀螺仪得到的3轴角加速度且经过PI器进行修正,修正参考方向为磁力计得到的标准方向。
代码看不懂可以参考匿名姿态解算
一阶RC低通滤波
1.一阶低通滤波void LPF_1(float hz,float time,float in,float *out) { *out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out ); } void LPF_1_db(float hz,float time,double in,double *out) { *out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out ); }
这里 A是
1
1
+
1
h
z
∗
6.28
∗
t
i
m
e
=
1
1
+
R
C
T
=
T
T
+
R
C
spacespacespacespacespacespacespacefrac{1}{1+frac{1}{hz*6.28*time}}=frac{1}{1+frac{RC}{T}}=frac{T}{T+RC}
1+hz∗6.28∗time11=1+TRC1=T+RCT
X为输入 Y为输出
欢迎分享,转载请注明来源:内存溢出
评论列表(0条)