本帖最后由 zing 于 2019-7-8 22:53 编辑
[飞控]姿态误差(一)-欧拉角做差
[飞控]姿态误差(二)-旋转矩阵做差
上回我们说到旋转矩阵其实是非常适合用来描述姿态的，无奈设计控制器比较困难，通常还有加上轨迹生成器的设计一起使用，就目前来说，我功力不够驾驭不了这么强大的招式呀！难受
就目前的控制器思路来说，我们已经将控制器分解成了三通道，所以我们需要一种三个元素描述姿态的方法来计算姿态误差，最熟悉的三个元素的描述方法欧拉角，有诸多不便，还有没有其他描述三个元素的旋转描述方法可以解决旋转误差的问题呢？
1.不要有旋转顺序
其实有一种比欧拉角更加直观的旋转描述，初始坐标系A 在空间中找任意一个轴(单位向量)，然后绕着这个轴转一个角度得到目标坐标系B，就是我们说的轴角。
如果我们使用这种方式来描述旋转只需要两个要素：单位向量(转轴)和旋转角度:
就可以把它们写成轴角形式:
是不是很直观很好理解，一个轴一个角就描述了一个旋转，而且因为只有一次旋转，也就自然没有什么旋转顺序的问题了，当然也不用什么小角度假设了(这里可以清晰的感受到轴角与欧拉角的不同，欧拉角是一定顺序的三个角度组成的向量，而轴角是一个方向向量的三个坐标，没有顺序)
2.如何计算旋转之间的误差
现在还需要解决的就是轴角怎么做差？如果想使用轴角做旋转之间的误差，可能又只能做减法了，我们反复强调旋转之间千万不要随便使用减法！(好像三个元素的旋转描述方法都没法直接参与运算)。
虽然轴角之间不能直接进行运算，那它能不能转换成方便做计算的旋转描述方法呢？我们之前才讲到旋转矩阵超级适合做计算。它能不能转换成旋转矩阵呢？可以！使用大名鼎鼎的罗德里格斯公式:
这个公式与 轴角的转轴u 在哪个参考系下没有关系,因为转轴经过旋转之后是不变的:
轴角这么厉害有它和旋转矩阵不就够了吗？为啥还老是要提四元数呢？
因为四元数简直就是可以参与计算的轴角呀！四元数也是存储了一个旋转轴和一个旋转角度。
让我们先看一下轴角如何转换成四元数(修改为国内标准)
对应的数学公式为:
令:
这就是轴角转换成四元数的公式，你看四元数就是用另一种方法来保存转轴和角度，其中i,j,k为坐标系B的x,y,z轴的单位向量(描述旋转的四元数一定是单位四元数)。
而且四元数可以直接参与运算！
如何把空间中的向量通过四元数来进行旋转呢(修改为国内标准,参考严恭敏老师的《捷联惯导算法与组合导航原理讲义》)？
这就是四元数的向量旋转公式，这里的乘法是 四元数乘法(*虽然四元数只有4个数但是参与运算时是一个44的矩阵，并没有减轻计算量)，因为必须有四个数才能运算，而空间中的向量只有三个元素，所以会把向量补上0，来构造 0标量四元数** 参与运算。
计算结果实部为0，为纯虚四元数，其虚部3个量表示旋转后的3D坐标。
如果我们令右下角的3*3矩阵为：
右下角的3*3矩阵，就是四元数转换成旋转矩阵的公式，所以这个算法可以写成:
所以：
即：
但是如果不是0标量四元数呢？如果就是个普通的单位四元数呢？
标量不变，但是后面的三个元素进行坐标系变换，又因为是第一个元素是角度，后三个元素是转轴，所以直接使用四元数向量旋转公式来操作四元数做旋转也是可以的(APM使用了这种形式，但是不建议这样做，显的思路很不清晰)。
总结一下：
-
欧拉角和轴角都是三个数,轴角有点像欧拉角的升级版，解决了旋转顺序，万向节锁死等问题。
-
但是三个数描述旋转的方法都不可以直接参与运算。
-
所以需要把轴角转换成旋转矩阵或者四元数然后进行运算，PX4旋转转换成旋转矩阵，APM选择用四元数。
-
计算完旋转之间的误差后，将误差转换成轴角，就可以直接给控制器使用了，解决了欧拉角的先后顺序问题，做差之后还是个旋转以及没有小角度假设，所以虽然开源算法把这个叫做四元数控制器，其实是轴角控制器，因为给控制器输入的是轴角。
-
一定要注意，轴角转换成四元数的方向以及四元数在转换成旋转矩阵时的方向。
四元数和旋转矩阵，目前就我的感觉在计算姿态间的误差的能力上区别不大，计算量也差不多，不过四元数描述的是个旋转过程，方便插值，旋转矩阵描述的是旋转完成的状态不能插值，所以其实没有最好的方法，只最适合的方法，只是一种选择而已。
补充:
连续旋转公式(在统一使用国内标准描述四元数时，四元数与旋转矩阵也可以像旋转矩阵一样上下标约去)：
参考：
《惯性导航》(秦永元)-9.2姿态更新计算的四元数四元数算法
《捷联惯导算法与组合导航原理讲义》(严恭敏)2.4.2四元数微分方程
《多旋翼飞行器设计与控制》(全权)-5.4四元数和旋转矩阵
关注微信公众号【无人机干货铺】，持续分享飞控干货。
发表于 2019-7-8 22:53:59
3846 浏览 1 回复
