https://blog.csdn.net/super_mice/article/details/45619945
这篇博客应该在一年前,做四轴毕设的时候写的.结果一直拖到现在...
有问题请回复评论，然后邮箱提醒我回复，[email protected] 私信不回。。。
一年前的时候关于四元数和姿态解算的中文资料还不多,至少我是找了很久才有那么点真正有价值的资料.
今年又很多学生做四轴相关的毕业论文,所以我希望这篇文章能够整合一些现有的资料并且加上一些自己的理解,方便后来的同学少走些弯路.由于时间久远,很多细节会记不清,还请多包涵.其实简单的飞控已经非常成熟而便宜了,各位技术大神们可以把你们的精力放在更有价值的问题上.比如飞行器的避障,导航,多机协作等.不要为了学技术而学技术,什么事情都想自己DIY,别人做过的事情就没有必要自己过一遍,你一个人做的东西也不大可能比很多牛人一起维护的开源项目来得优秀.可以做的事情太多,我们把精力集中起来做些更需要人去做的工作是比较高效和有价值的,至于基础的东西尽量借助于别人肩膀吗，比如一些开源项目就好.
另外一篇关于四轴双闭环PID(串级PID)的帖子请移步:四轴PID调试
言归正传,当初困扰我的主要是这么几个问题:
[1]四元数是什么鬼?她和欧拉角又是什么暧昧关系?
[2]陀螺仪和加速度计的读数和四元数怎么建立关系?
[3]欧拉角的"gimal lock"?她和欧拉角不合适表示姿态有什么关系?
[4]姿态解算的方法:互补滤波,卡尔曼滤波,梯度下降,DCM都是啥?
[5]如何真正通过代码实现姿态结算?
下面逐个讲讲
[1]四元数是什么鬼?她和欧拉角又是什么暧昧关系?
四元数,百度搜一下一大片,可是我就是不理解到底和姿态有什么关系?
可以把四元数理解为旋转轴+旋转角的描述方法的优化版.用一个3维向量表示转轴(暂时理解为四元数的x,y,z,表示的向量),一个角度表示绕此转轴的旋转角度(暂时理解为四元数的w).
下面举例子说明下，有点长,配了些图片方便理解,跟着文章一步步来,就能理解
假设你一个人在野外，饿了几天饥肠辘辘。突然有只野鸡飞出来，一头撞在了你身边的大树上，挂了。于是你感谢上苍的恩赐。。。有点扯远了。之后你生起了火要烤了那只野鸡。可是怎么烤呢？你找了根木棒，削尖了想插进野鸡里，首先为了转起来顺畅棒子要穿过鸡的重心。那么问题来了。这个棒要从哪个角度插进去呢？，为了方便描述，我们把鸡按照活着时候的样子摆好，并且鸡头对着正东方向。我们把重心当做坐标原点，重心到鸡头方向当做x正方向（正东），重心到左翅膀方向为y正方向（正北），重心到鸡背方向为z正方向。这个样子就是我们的初始姿态（和世界坐标系重合）。如下图所示：绿色箭头为y轴正方向，红色箭头为x轴正方向，蓝色为z轴正方向。（之前写的帖子有错误，现已改正，详见9楼的回复）大的坐标为世界坐标系。绿色指向正北，红色指向正东，蓝色指向天空。小的坐标为野鸡的姿态，红色是鸡头，绿色是左翅膀方向，蓝色是鸡背方向。本文只需要关注两个坐标系之间的旋转变化即可，请无视两个坐标系之间的距离变化。
不过一般我们都喜欢从菊花方向插入，那这个方向单位化后就是（-1,0,0）。然后我们把棒子和鸡放上烤架，准备转起来烤鸡。那么问题又来了，往哪个方向转呢？如果从鸡屁股的方向看，你按逆时针方向把鸡转动了90度。现在鸡背朝北，鸡头仍旧朝东。如下图所示：大的坐标（世界坐标）不变。小坐标系和上图相比绕着红轴的负方向旋转了90度。
刚才我们对鸡的这个转动可以用四元数来表示，
w=cos（90/2) ,
x=sin(90/2)*-1,
y=sin(90/2)*0,
z=sin(90/2)*0;
w=0.707, x=-0.707, y=0. z=0(如果是顺时针转90度那么w=cos（-90/2)=0.707，x=-0.707，y=0,z=0),旋转角度的正负由右手定则确定。
现在对四元数表示旋转大概明白了吧？当然转轴可以沿任何方向，不一定沿着坐标轴。旋转角度为theta，（ax,ay,az）代表单位化的转轴方向。那么
w = cos(theta/2)
x = ax * sin(theta/2)
y = ay * sin(theta/2)
z = az * sin(theta/2)
这个就是表示旋转的四元数。
其实四元数的定义是q=w+xi+yj+zk
其中w是实数，x,y,z是虚数，其中:
i*i=-1
j*j=-1
k*k=-1
当然，只有单位化的四元数才可以用来描述旋转。下面看下如何单位化
||q|| = Norm(q)=sqrt(w2 + x2 + y2 + z2)
Normlize(q)=q/Norm(q)=q / sqrt(w2 + x2 + y2 + z2)
现在我们再次回到烤野鸡的问题。经过刚才的那次旋转后，你突然想给野鸡换个姿势烤了。你不想按之前的转轴方向转了。于是你把鸡的位置和姿态固定不动（鸡头朝东，鸡背朝北），把棒子拔出来，换了个方向从鸡背上插进去，烤架按照棒子的方向架好。现在我们绕着新的转轴转动从鸡背方向看过去沿顺时针转动90度。
现在鸡的姿态应该是鸡头朝上，鸡背朝北。刚才这次的旋转用四元数表示（注意，这次旋转的基准坐标系是上次旋转完后的坐标系，不是和世界坐标系重合的初始姿态坐标系）。所以我们的转轴方向是（0,0,1），角度是 -90 度。所以
w=cos(-90/2)=0.707,
x=sin(-90/2)*0=0 ,
y=sin(-90/2)*0=0,
z=sin(-90/2)*1=-0.707
问题又又来了，你想知道野鸡相对于初始位置的姿态。这时候只要把刚才得到的两个四元数相乘就可以把两次旋转合并为一次旋转，得到一个相对于初始位置的四元数（四元数方便插值就是这个意思），四元数的乘法公式如下，其中q1是后来旋转的四元数，q2是上次旋转的四元数，q3是两次旋转合并后得到的四元数。
q3=q1 * q2
w3=(w1*w2 - x1*x2 - y1*y2 - z1*z2)
x3=(w1*x2 + x1*w2 + y1*z2 - z1*y2)
y3=(w1*y2 - x1*z2 + y1*w2 + z1*x2)
z3=(w1*z2 + x1*y2 - y1*x2 + z1*w2)
计算后可得 q3为
w3=0.5
x3=-0.5
y3=-0.5
z3=-0.5
可以看做是沿着相对于绕着初始姿态坐标中的( -1/sqrt(3) , -1/sqrt(3) , -1/sqrt(3) )（sqrt是开根号的意思）这个方向的转轴，做了从转轴正方向看逆时针转动120度的旋转。启动你的空间想象能力，你应该可想象到野鸡从初始姿态经过这样的一次旋转就变成了现在的姿态。其实这和绕着( 1/sqrt(3) , 1/sqrt(3) , 1/sqrt(3) )方向的转轴，从转轴正方向看顺时针转动120度是一样的效果。
这就是四元数表示姿态最有利的地方，可以将多次的旋转通过简单的运算变为一次旋转。