6 轴惯性传感器与 PID

6 轴惯性传感器与 PID

MPU6050

MPU6050是全球首例整合性6轴运动处理组件，也就是说它是一款测量三轴加速度和三轴角速度传感器。

参考链接：http://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/

加速度计(Accelerometer)

测量加速度大小，原理其实是测量力的大小，然后根据$F=ma$来算出加速度。因此当你把传感器放在桌面上，静止不动，仍会测得Z轴有一个加速度，大致是$a=-9.8N/kg$，实际上就是Z轴力传感器受到了地心引力。当你把传感器扔在空中时，虽然你能看到它正在以$a=-9.8N/kg$的加速度往下落，不过传感器得到的三轴加速度将会是三个0，因为在空中下落的时候失重了，三个方向均不受力。

通过加速度，我们可以算出物体静止时的姿态，举个例子：

这是一个xyz各倾斜30度的方盒子，假设其中放置了一个加速度计，那么加速度计就会去测量三条白线方向上的受力情况。这个盒子在地球上会受到橘黄色方向上的重力，这个力总是竖直向下，那么我们就可以通过下面的公式简单地计算出当前物体的姿态。

$\displaystyle{\theta_x = arctan(\frac{acc_x}{\sqrt{acc_y^2 + acc_z^2}})}$

$\displaystyle{\theta_y = arctan(\frac{acc_y}{\sqrt{acc_x^2 + acc_z^2}})}$

$\displaystyle{\theta_z = arctan(\frac{acc_z}{\sqrt{acc_x^2 + acc_y^2}})}$

其中的$acc_x$,$acc_y$,$acc_z$指的是三轴的加速度，$\theta_x$,$\theta_y$,$\theta_z$指的是三轴的夹角。更专业的算法是通过矩阵变换来计算出四元数。

陀螺仪(Gyroscope)

陀螺仪测量的值是角速度。角速度的定义是$\displaystyle{\omega = \frac{d\theta}{d t}}$，但是我们实际上需要测量的是角度，所以我们要通过积分来得到角度：$\displaystyle{\theta = \int_0^t{\omega dt}}$。

互补滤波

由于我们的传感器精度达不到要求，所以即使是静止不动，仍能从陀螺仪读取到非零的角速度的值，不过加速度计有这样的优点那就是不会随着时间的推移产生误差，这时候我们就需要使用一个能够结合加速度和角速度两者优点的一个算法，这个简单的算法叫互补滤波，公式如下：

$\displaystyle{\theta_x = (\theta_x + \omega_x * \Delta t) * 0.99 + \theta_{ax} * 0.01}$

其中$\theta_x$是积分得到的角度，$\omega_x$是陀螺仪得到的角速度，$\theta_{ax}$是加速度算出来的角度。

其中的0.99是陀螺仪占比，短期来看，陀螺仪的变化决定了角度的变化，长期来看，角度值会趋向于加速度对应的值。由于陀螺仪十分灵敏，加速度计受物体运动影响较大，而且波动也大，所以我们给陀螺仪较高的比例。通过此算法得到的角度，不论短期来看，还是长期来看，都是稳定可靠的，可以用于平衡车和四轴的控制。工业上更多地采用卡尔曼滤波。

PID

PID的公式如下：

$\displaystyle{u(t)=K_p e(t)+K_I\int_0^t{e(t)d\tau}+K_D\frac{d e(t)}{d t}}$

其中$K_p$,$K_I$,$K_D$,分别是比例系数，积分系数，微分系数。$e(t)$为期望值与实际值之差，$u(t)$为输出的控制量。

因此如果我们要控制一辆平衡车保持平衡，那么期望值$\theta=0$，$e(t)=\theta-\theta_x(t)$，假设我们的传感器摆放的位置是x轴竖直向上。那么$\displaystyle{\frac{de(t)}{dt} = -\frac{d\theta_x(t)}{dt} = -\omega_x(t)}$，由于平衡车比较简单，我们不需要积分控制。因此最后的PWM值$u(t)$为：

$\displaystyle{u(t)=-K_p \theta_x(t) - K_D\omega_x(t)}$

我们只需要在程序中调节$K_p$和$K_D$即可让平衡车锁定在竖直方向。

我们单独看$-K_p \theta_x(t)$的时候，可以知道，它是让电机控制平衡车保持在0度，假设$K_p=1$，当车向前倾5度的时候，电机就会输出-5，让车回到0度。不过由于没有阻尼力，它会保持一个振荡的状态，类似于弹簧拉伸后再松手，在没有摩擦力的时候，会一直往复运动。胡克定律：$\displaystyle{F=-kx}$

现在让我们单独看$-K_D\omega_x(t)$，它是一个让车的角速度趋向于0的一个负反馈。有了它，我们的车就不会无限振荡，而是经过一两次振荡以后保持静止。