pid控制器实现互补滤波原理

转载自：https://blog.csdn.net/wxc971231/article/details/97449026

一、姿态解算原理相关

1、简介

• 对于四旋翼无人机来说，有两个相关坐标系，即地理坐标系$OnXnYnZn{O}_n{X}_n{Y}_n{Z}_{n}OnXnYnZn$和机体坐标系$ObXbYbZb{O}_b{X}_b{Y}_b{Z}_{b}ObXbYbZb$。
• 姿态角的检测对于四旋翼控制是至关重要的。比如在做飞行器姿态控制的时候，通常使用串级pid的方法，需要得到飞行器姿态角（即欧拉角）来做角度位置环反馈。再如利用装置在机腹垂直向下拍摄的摄像头，进行机体视觉定位时，也需要靠姿态角进行视觉反馈补偿。
• 要解算姿态角，就要研究从地理坐标系到机体坐标系的转换过程。这个转换过程不是唯一的，比如可以先绕X轴旋转$\theta \theta \theta$，再绕Y轴旋转$\gamma \gamma \gamma$，最后绕Z轴旋转$\psi \psi \psi$，这样得到的一组姿态角称为卡尔丹角；也可以按ZYX的顺序旋转，这样得到的一组姿态角称为欧拉角（这是最常用的，pitch,roll,yaw）。不管按照什么顺序，得到的角度都可以称作广义欧拉角。实际理论分析时，旋转顺序不是很重要，这个顺序会影响四元数与欧拉角的关系，但是都可以进行解算。

2、坐标变换和旋转矩阵

下面从最基本的坐标变换入手开始讲解，请看如下坐标系旋转：$OAXAYAZA{O}_A{X}_A{Y}_A{Z}_{A}OAXAYAZA$绕OX旋转$\theta \theta \theta$到$OBXBYBZB{O}_B{X}_B{Y}_B{Z}_{B}OBXBYBZB$

对于原系$OAXAYAZA{O}_A{X}_A{Y}_A{Z}_{A}OAXAYAZA$中的一个向量$[rxA,ryA,rzA][r_{x_A},r_{y_A},r_{z_A}][rxA,ryA,rzA]$，转换到新系$OBXBYBZB{O}_B{X}_B{Y}_B{Z}_{B}OBXBYBZB$中的向量$[rxB,ryB,rzB][r_{x_B},r_{y_B},r_{z_B}][rxB,ryB,rzB]$，有：

$$\begin{gathered} rxB=rxAryB=cos(\theta )ryA+sin(\theta )rzArzB=-sin(\theta )ryA+cos(\theta )rzA{r}_{x_B}=r_{x_A} \\ {r}_{y_B}=cos(\theta )r_{y_A}+sin(\theta )r_{z_A} \\ {r}_{z_B}=-sin(\theta )r_{y_A}+cos(\theta )r_{z_A}rxB=rxAryB=cos(\theta )ryA+sin(\theta )rzArzB=-sin(\theta )ryA+cos(\theta )rzA \end{gathered}$$

可以用旋转矩阵表示

$$[rxBryBrzB]=[1000cos(\theta )sin(\theta )0-sin(\theta )cos(\theta )][rxAryArzA]\left[\begin{array}{c}{r}_{x_B}\\ r_{y_B}\\ r_{z_B}\end{array}\right]=\left[\begin{array}{ccc}1& amp;0& amp;0\\ 0& amp;cos(\theta )& amp;sin(\theta )\\ 0& amp;-sin(\theta )& amp;cos(\theta )\end{array}\right]\left[\begin{array}{c}{r}_{x_A}\\ r_{y_A}\\ r_{z_A}\end{array}\right]⎣⎡rxBryBrzB⎦⎤=⎣⎡1000cos(\theta )-sin(\theta )0sin(\theta )cos(\theta )⎦⎤⎣⎡rxAryArzA⎦⎤$$

• 中间的就是 绕x轴旋转$\theta \theta \theta$ 的旋转矩阵，记作
  $$Cx(\theta )=[1000cos(\theta )sin(\theta )0-sin(\theta )cos(\theta )]C_{x_{(}\theta )}=\left[\begin{array}{ccc}1& amp;0& amp;0\\ 0& amp;cos(\theta )& amp;sin(\theta )\\ 0& amp;-sin(\theta )& amp;cos(\theta )\end{array}\right]Cx(\theta )=⎣⎡1000cos(\theta )-sin(\theta )0sin(\theta )cos(\theta )⎦⎤$$

• 同样的，还有 绕y轴旋转$\gamma \gamma \gamma$ 的旋转矩阵，记作
  $$Cy(\gamma )=[cos(\gamma )0-sin(\gamma )010sin(\gamma )0cos(\gamma )]C_{y_{(}\gamma )}=\left[\begin{array}{ccc}cos(\gamma )& amp;0& amp;-sin(\gamma )\\ 0& amp;1& amp;0\\ sin(\gamma )& amp;0& amp;cos(\gamma )\end{array}\right]Cy(\gamma )=⎣⎡cos(\gamma )0sin(\gamma )010-sin(\gamma )0cos(\gamma )⎦⎤$$

• 同样的，还有 绕z轴旋转$\psi \psi \psi$ 的旋转矩阵，记作
  $$Cz(\psi )=[cos(\psi )-sin(\psi )0sin(\psi )cos(\psi )0001]C_{z_{(}\psi )}=\left[\begin{array}{ccc}cos(\psi )& amp;-sin(\psi )& amp;0\\ sin(\psi )& amp;cos(\psi )& amp;0\\ 0& amp;0& amp;1\end{array}\right]Cz(\psi )=⎣⎡cos(\psi )sin(\psi )0-sin(\psi )cos(\psi )0001⎦⎤$$

• 假如现在我们按ZXY的顺序，从地理坐标系$OnXnYnZn{O}_n{X}_n{Y}_n{Z}_{n}OnXnYnZn$旋转到机体坐标系$ObXbYbZb{O}_b{X}_b{Y}_b{Z}_{b}ObXbYbZb$，整体旋转矩阵为
  $$Cnb=Cy(\gamma )Cx(\theta )Cz(\psi )=[cos(\gamma )cos(\psi )+sin(\gamma )sin(\psi )sin(\theta )-cos(\gamma )sin(\psi )+sin(\gamma )cos(\psi )cos(\theta )-sin(\psi )cos(\theta )sin(\psi )cos(\theta )cos(\psi )cos(\theta )sin(\theta )sin(\gamma )cos(\psi )-cos(\gamma )sin(\psi )sin(\theta )-sin(\gamma )sin(\psi )-cos(\gamma )cos(\psi )sin(\theta )cos(\gamma )cos(\theta )]C_n^b=C_{y_{(}\gamma )}{C}_{x_{(}\theta )}{C}_{z_{(}\psi )}=\left[\begin{array}{ccc}cos(\gamma )cos(\psi )+sin(\gamma )sin(\psi )sin(\theta )& amp;-cos(\gamma )sin(\psi )+sin(\gamma )cos(\psi )cos(\theta )& amp;-sin(\psi )cos(\theta )\\ sin(\psi )cos(\theta )& amp;cos(\psi )cos(\theta )& amp;sin(\theta )\\ sin(\gamma )cos(\psi )-cos(\gamma )sin(\psi )sin(\theta )& amp;-sin(\gamma )sin(\psi )-cos(\gamma )cos(\psi )sin(\theta )& amp;cos(\gamma )cos(\theta )\end{array}\right]Cnb=Cy(\gamma )Cx(\theta )Cz(\psi )=⎣⎡cos(\gamma )cos(\psi )+sin(\gamma )sin(\psi )sin(\theta )sin(\psi )cos(\theta )sin(\gamma )cos(\psi )-cos(\gamma )sin(\psi )sin(\theta )-cos(\gamma )sin(\psi )+sin(\gamma )cos(\psi )cos(\theta )cos(\psi )cos(\theta )-sin(\gamma )sin(\psi )-cos(\gamma )cos(\psi )sin(\theta )-sin(\psi )cos(\theta )sin(\theta )cos(\gamma )cos(\theta )⎦⎤$$

• 注意一点，如果从n系到b系的旋转矩阵为$Cnb{C}_n^{b}Cnb$，则从b系到n系的旋转矩阵为$CnbT{C_n^b}^{T}CnbT$，显然有$Cnb\ast CnbT=E{C}_n^b\ast {C_n^b}^T=ECnb\ast CnbT=E$，所以旋转矩阵是正交矩阵

• 这个结论不能直接使用，一方面这个直接是不可解的，另一方面大量三角函数运算会导致大量资源占用。

3、四元数

• 四元数是一种超复数，由四个元构成：$Q(q0,q1,q2,q3)=q0+q1i+q2i+q3kQ(q_0,q_1,q_2,q_3)=q_0+q_{1}i+q_{2}i+q_{3}kQ(q0,q1,q2,q3)=q0+q1i+q2i+q3k$
• 关于四元数更详细的介绍可以参考四元数
• 利用四元数我们可以提出另一种描述空间矩阵的方法，具体可以参考秦永元的《惯性导航》和【教程】四旋翼飞行器姿态解算算法入门学习-Rick Grimes

（1）四元数和欧拉角的关系

• 注意这三个式子中，$\theta \theta \theta$是绕Y轴转角，$\varphi \varphi \varphi$是绕X轴转角，$\psi \psi \psi$是绕Z轴转角，和前文有所不同

1. 按ZYX的顺序从地理坐标系$OeXeYeZe{O}_e{X}_e{Y}_e{Z}_{e}OeXeYeZe$旋转到机体坐标系$ObXbYbZb{O}_b{X}_b{Y}_b{Z}_{b}ObXbYbZb$，整体旋转矩阵为
   
2. 对应的四元数表示法为：
   
3. 解算出的欧拉角为
   

（2）四元数的求解

• 飞行器姿态的改变，可以对应到旋转矩阵的改变，进一步对应到四元数的改变。实时姿态计算，实际上也就是实时更新四元数。
• 我们可以构建四元数关于时间的微分方程，来研究四元数关于时间的变化规律，求解该微分方程，即可解出四元数

1、建立微分方程

• 从四元数的三角表示法入手推到微分方程，具体推到过程可以参考秦永元的《惯性导航》
  $$Q=cos(\theta )2+\lambda sin(\theta )2Q=\frac{cos(\theta )}{2}+\frac{\lambda sin(\theta )}{2}Q=2cos(\theta )+2\lambda sin(\theta )$$$$dQdt=1/2[0-\omega x-\omega y-\omega x\omega x0\omega z-\omega y\omega y-\omega z0\omega x\omega z\omega y-\omega x0]\cdot [q0q1q2q3]\frac{dQ}{dt}=1/2\left[\begin{array}{cccc}0& amp;-{\omega }_x& amp;-{\omega }_y& amp;-{\omega }_x\\ {\omega }_x& amp;0& amp;{\omega }_z& amp;-{\omega }_y\\ {\omega }_y& amp;-{\omega }_z& amp;0& amp;{\omega }_x\\ {\omega }_z& amp;{\omega }_y& amp;-{\omega }_x& amp;0\end{array}\right]\cdot \left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]dtdQ=1/2⎣⎢⎢⎡0\omega x\omega y\omega z-\omega x0-\omega z\omega y-\omega y\omega z0-\omega x-\omega x-\omega y\omega x0⎦⎥⎥⎤\cdot ⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤$$$$记为dQdt=\Phi \cdot Q记为\frac{dQ}{dt}=\Phi \cdot Q记为dtdQ=\Phi \cdot Q$$

2、一阶龙格库塔法求解微分方程

一阶龙格库塔法是求解微分方程常用的工程方法，原理是把微分转化为微元增量，利用递推迭代的方法求解

• 设有微分方程$dydx=f(x,y)\frac{dy}{dx}=f(x,y)dxdy=f(x,y)$

  求解y的迭代公式为$y(\lambda +\Delta \lambda )=y(\lambda )+\Delta \lambda \cdot f(x(\lambda ),y(\lambda ))y(\lambda +\Delta \lambda )=y(\lambda )+\Delta \lambda \cdot f(x(\lambda ),y(\lambda ))y(\lambda +\Delta \lambda )=y(\lambda )+\Delta \lambda \cdot f(x(\lambda ),y(\lambda ))$

• 对应到四元数微分方程：
  $$Q(t-\Delta t)=Q(t)+\Delta t\cdot \Phi (t)\cdot Q(t)Q(t-\Delta t)=Q(t)+\Delta t\cdot \Phi (t)\cdot Q(t)Q(t-\Delta t)=Q(t)+\Delta t\cdot \Phi (t)\cdot Q(t)$$
  整理得
  $$[q0q1q2q3]t+\Delta t=[q0q1q2q3]t+12\cdot \Delta t\cdot [-\omega x\cdot q1-\omega y\cdot q2-\omega z\cdot q3\omega x\cdot q0-\omega y\cdot q3+\omega z\cdot q2\omega x\cdot q3+\omega y\cdot q0-\omega z\cdot q1-\omega x\cdot q2+\omega y\cdot q1+\omega z\cdot q0]{\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_{t+\Delta t}={\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]}_t+\frac{1}{2}\cdot \Delta t\cdot \left[\begin{array}{c}-{\omega }_x\cdot q_1-{\omega }_y\cdot q_2-{\omega }_z\cdot q_3\\ {\omega }_x\cdot q_0-{\omega }_y\cdot q_3+{\omega }_z\cdot q_2\\ {\omega }_x\cdot q_3+{\omega }_y\cdot q_0-{\omega }_z\cdot q_1\\ -{\omega }_x\cdot q_2+{\omega }_y\cdot q_1+{\omega }_z\cdot q_0\end{array}\right]⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+\Delta t=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤t+21\cdot \Delta t\cdot ⎣⎢⎢⎡-\omega x\cdot q1-\omega y\cdot q2-\omega z\cdot q3\omega x\cdot q0-\omega y\cdot q3+\omega z\cdot q2\omega x\cdot q3+\omega y\cdot q0-\omega z\cdot q1-\omega x\cdot q2+\omega y\cdot q1+\omega z\cdot q0⎦⎥⎥⎤$$
  这里$\omega \omega \omega$就是三轴角速度，可以用陀螺仪直接测得

4、旋转矩阵中的一列

• 旋转矩阵中的一列有特殊意义，它代表原坐标系中一个方向的单位向量（X/Y/Z），在新坐标系中对应的向量值
• 用方向向量（如X方向$[1,0,0]T[1,0,0{]}^T[1,0,0]T$）左乘旋转矩阵可以轻易看出，这里不写了

二、匿名四轴姿态解算分析

按Ano_Imu.c文件顺序进行分析

1、坐标系定义

• 匿名四轴姿态解算部分主要写在Ano_Imu.c中
• 匿名四轴程序中一共有三个坐标系：

1. 地理坐标系，标记为w（官方定义如下）
2. 机体坐标系，标记为a（官方定义如下）
   
3. 航向坐标系，标记为h（专门解释一下，四旋翼飞行时，姿态角pitch和roll一般很小，在任意时刻，如果我们粗略地认为飞行器始终与底面平行（把飞机摆正，使pitch=roll=0），此时的机体坐标系即为航向坐标系）

2、坐标系转换

（1）匿名旋转坐标系

• 匿名的坐标系变换，是按是按ZYX顺序从地理坐标系转到机体坐标系的，这个旋转矩阵是前面提过的 (注意这三个式子中，$\theta \theta \theta$是绕Y轴转角，$\varphi \varphi \varphi$是绕X轴转角，$\psi \psi \psi$是绕Z轴转角)

$$Cwa=[cos(\theta )cos(\psi )sin(\psi )cos(\theta )-sin(\theta )sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )sin(\varphi )cos(\theta )cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )cos(\varphi )cos(\theta )]C_w^a=\left[\begin{array}{ccc}cos(\theta )cos(\psi )& amp;sin(\psi )cos(\theta )& amp;-sin(\theta )\\ sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )& amp;sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )& amp;sin(\varphi )cos(\theta )\\ cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )& amp;cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )& amp;cos(\varphi )cos(\theta )\end{array}\right]Cwa=⎣⎡cos(\theta )cos(\psi )sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )sin(\psi )cos(\theta )sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )-sin(\theta )sin(\varphi )cos(\theta )cos(\varphi )cos(\theta )⎦⎤$$

• 程序中用二维数组att_matrix表示从机体坐标系转到地理坐标系的旋转矩阵，是上面那个的转置
  $$at{t}_{m}atrix=Caw=(Cwa)T=[cos(\theta )cos(\psi )sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )sin(\psi )cos(\theta )sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )-sin(\theta )sin(\varphi )cos(\theta )cos(\varphi )cos(\theta )]att\_matrix=C_a^w=(C_w^a{)}^T=\left[\begin{array}{ccc}cos(\theta )cos(\psi )& amp;sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )& amp;cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )\\ sin(\psi )cos(\theta )& amp;sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )& amp;cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )\\ -sin(\theta )& amp;sin(\varphi )cos(\theta )& amp;cos(\varphi )cos(\theta )\end{array}\right]at{t}_{m}atrix=Caw=(Cwa)T=⎣⎡cos(\theta )cos(\psi )sin(\psi )cos(\theta )-sin(\theta )sin(\varphi )sin(\theta )cos(\psi )-cos(\varphi )sin(\psi )sin(\varphi )sin(\theta )sin(\psi )+cos(\varphi )cos(\psi )sin(\varphi )cos(\theta )cos(\varphi )sin(\theta )cos(\psi )+sin(\varphi )sin(\psi )cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi )cos(\varphi )cos(\theta )⎦⎤$$
• 注意这里绕Y轴转角$\theta \theta \theta$ (pitch)，绕X轴转角$\varphi \varphi \varphi$ (roll)都是很小的角

（2）相关代码分析

#