1. 俯仰角与互补滤波1.1 加速度计测量俯仰角可以将加速度计的值视作如下模型考虑XOZ平面当小车前后倾斜静止时重力分量作用于X轴和Z轴采样得到X轴加速度axa_xax​Z轴加速度aza_zaz​则俯仰角θ\thetaθ满足tan⁡(θ)axaz \tan(\theta) \frac{a_x}{a_z}tan(θ)az​ax​​如下图所示就是下图所示的theta角1.2 陀螺仪测量俯仰角如果单单使用加速度计测量俯仰角系统太容易受到运动的影响加速度计测量只在静止的时候有作用此时需要引入陀螺仪测量考虑XOZ平面当小车前后倾斜运动时产生绕Y轴旋转的角速度在离散系统中每隔TTT时间采样一次绕Y轴旋转的角速度gy(k)gy(k)gy(k)初始俯仰角为θ(0)\theta(0)θ(0)则当前时刻俯仰角θ(k)\theta(k)θ(k)满足θ(k)θ(0)∑gy(k)∗T \theta(k) \theta(0) \sum gy(k) * Tθ(k)θ(0)∑gy(k)∗Tθ(k)θ(k−1)gy(k)∗T \theta(k) \theta(k - 1) gy(k) * Tθ(k)θ(k−1)gy(k)∗T积分部分如下图所示第二个公式表示每次的角度值为上次的角度值加上角速度乘时间陀螺仪内部可以简化为如下的模型当旋转的时候对应的加速度增大此时可以对应加速度与角速度的关系从而可以得到角速度相比与加速度计陀螺仪得到的角度不受加速度的影响陀螺仪直接测量的是「角速度」积分得到角度加速度计测量的是「比力加速度 重力」靠重力投影算角度。加速度会干扰加速度计但加速度本身不会在陀螺仪测量值里引入误差理想陀螺仪。1.2.1 陀螺仪漂移陀螺仪漂移的原因陀螺仪静止时输出并不严格等于 0存在一个微小的零偏bias我们对角速度做积分算角度这个微小的零偏误差会随着采样不断累加角度就一点点飘走了。1.3 加速度计与陀螺仪对比对比如下表所示加速度计陀螺仪角速度计测量原理测量重力分量通过三角函数计算得到角度测量角速度通过角速度累加得到角度静态稳定性重力分量测量准确角度误差小长时间不漂移由于陀螺仪零漂和误差长时间累加会导致角度逐渐漂移动态稳定性受运动加速度影响加减速时角度偏移受震动、颠簸等冲击力影响冲击时角度可能瞬间偏移很大不依赖重力分量运动加速度不影响测量受冲击力影响小绝对与相对参考重力方向可以测量绝对角度只能测量相对角度变化需要明确初值才能测量绝对角度1.4 互补滤波互补滤波以陀螺仪为主加速度计为辅通过参考加速度计角度来修正陀螺仪角度的漂移同时为陀螺仪提供绝对参考加速度计角度和陀螺仪角度取加权平均值得到互补滤波后的陀螺仪角度θgyro_fα∗θacc(1−α)∗θgyro \theta_{gyro\_f} \alpha * \theta_{acc} (1 - \alpha) * \theta_{gyro}θgyro_f​α∗θacc​(1−α)∗θgyro​注意滤波后下次θgyro\theta_{gyro}θgyro​的累加要在θgyro_f\theta_{gyro\_f}θgyro_f​的基础上进行上式也可变换为如下形式θgyro_fθgyroα∗(θacc−θgyro) \theta_{gyro\_f} \theta_{gyro} \alpha * (\theta_{acc} - \theta_{gyro})θgyro_f​θgyro​α∗(θacc​−θgyro​)可以看出互补滤波会给θgyro\theta_{gyro}θgyro​添加一个偏移方向朝向θacc\theta_{acc}θacc​大小正比于θacc\theta_{acc}θacc​和θgyro\theta_{gyro}θgyro​的距离比例系数为α\alphaα原理可以参照公式如果陀螺仪飘高了θgyroθacc\theta_{gyro}\theta_{acc}θgyro​θacc​差值(θacc−θgyro)(\theta_{acc}-\theta_{gyro})(θacc​−θgyro​)为负 → 减去一小部分把陀螺仪角度往下拉如果陀螺仪飘低了θgyroθacc\theta_{gyro}\theta_{acc}θgyro​θacc​差值(θacc−θgyro)(\theta_{acc}-\theta_{gyro})(θacc​−θgyro​)为正 → 加上一小部分把陀螺仪角度往上拉α\boldsymbol{\alpha}α取值范围0α10\alpha10α1小车IMUMPU6050这类工程常用取值0.01 ~ 0.05α\alphaα偏小如0.01修正力度弱优先信任陀螺仪动态性能好漂移校正慢α\alphaα偏大如0.05修正力度强漂移抑制更快但小车加减速时更容易被加速度计的错误角度带偏。θgyro_fθgyroα∗(θacc−θgyro) \theta_{gyro\_f} \theta_{gyro} \alpha * (\theta_{acc} - \theta_{gyro})θgyro_f​θgyro​α∗(θacc​−θgyro​)1.4.1 假想模型想象在一个数轴上有两个小球它们的位置分别表示θacc\theta_{acc}θacc​和θgyro\theta_{gyro}θgyro​的值两个小球之间用一个弹簧连接弹簧的劲度系数为α\alphaαθacc\theta_{acc}θacc​值由加速度计直接确定作为绝对位置参考是一个锚点θgyro\theta_{gyro}θgyro​值通过陀螺仪累加得到同时它还受到弹簧的牵引力当陀螺仪累加得到的θgyro\theta_{gyro}θgyro​逐渐漂移远离θacc\theta_{acc}θacc​时弹簧牵引力会逐渐增强直到弹簧牵引力与漂移“力”相等此时θgyro\theta_{gyro}θgyro​将不会再漂移虽然漂移被抑制但是θgyro\theta_{gyro}θgyro​与θacc\theta_{acc}θacc​之间会存在稳态误差稳态误差大小与漂移值大小有关增大α\alphaα和校准陀螺仪可减小此误差在能抑制漂移到合理范围的情况下α\alphaα要尽可能小这样才能保证θgyro\theta_{gyro}θgyro​的独立响应同时也能降低运动和受到冲击时被θacc\theta_{acc}θacc​的影响程度滤波后的角度也无法完全消除加速度计尖峰的影响