姿态误差(二)-旋转矩阵做差

zing

本帖最后由 zing 于 2019-7-2 23:55 编辑

通过上次的研究: [飞控]姿态误差(一)-欧拉角做差 我们发现欧拉角计算姿态间的误差不是很好用。既然欧拉角做差不行，那就试试别的方法吧.

同样是要解决困扰我们的两个问题，首先旋转的误差还得是个旋转，第二不要有像欧拉角一样有先后顺序。当然能别用小角度假设就更好了。

旋转矩阵好像可以！

不行不行，上次你不是说了吗？欧拉角转换成旋转矩阵，欧拉角有这么多顺序，那对应的旋转矩阵不就有很多顺序吗？第二条不满足，换一个。

其实同一个姿态(旋转)可以使用不同顺序的欧拉角表示，但是对应的旋转矩阵是唯一的。

举个栗子： zyx顺序的欧拉角 [-1.04537914205813,-0.0795839823878026,0.909278065160895] xyz顺序的欧拉角 [0.523636366815730,-0.785388573397448,-0.785398163397448] 对应的都是旋转矩阵 [0.5000 -0.8624 0.0795; 0.5000 0.3624 0.7866; -0.7071 -0.3536 0.6124]

所以旋转矩阵本身是没有顺序的，而且所有姿态(旋转)描述方法中只有旋转矩阵一一对应于姿态(旋转)。 这是一个非常棒的优势，既然没有顺序之分，那么也就是说旋转矩阵描述的是一次完整的旋转，欧拉角是将这次完整的旋转拆分成了三次旋转。 (旋转矩阵描述的是个状态，它无法描述运动过程。)

这么一看旋转矩阵还挺适合用来描述姿态的，那旋转矩阵该如何做差呢？做差之后还是不是个旋转呢？

我知道你在想什么！这可是个3*3的旋转矩阵呀，可再别做减法了！

我们联想一下，刚刚才说到旋转矩阵将欧拉角的三次旋转转换成一次完整的旋转！这个不就是旋转矩阵的加法吗？三个旋转矩阵加起来等于一个旋转矩阵！

在讨论旋转矩阵之前，先介绍一下旋转矩阵的描述形式，因为旋转矩阵描述的是一个向量在不同坐标系下的旋转，所以我们可以将矩阵简写为

$$ C_a^b $$

表示从坐标系a 到坐标系b之间的旋转，也就是说如果有个向量A，在a坐标系中的坐标为A_a,在b坐标系下的坐标为A_b。 那么就可以写成:

$$ A_b=C_a^bA_a $$

这个写法非常直观简洁便于理解矩阵间的运算，目前大部分教材都使用类似的写法。

我们就使用这种描述方法来证明一下我们刚才说的旋转矩阵加法：

我们假设有个向量在1坐标系下叫A_1,在坐标系2下叫A_2，那么：

$$ A_2=C_1^2A_1 $$

同理有：

$$ A_3=C_2^3A_2 $$

那么我们就得到了

$$ A_3=C_2^3C_1^2A_1 $$

所以有：

这个公式就是描述了两次旋转是可以转换成一次旋转的，也就是说两次旋转相加等于一个大的旋转，这不就是旋转的加法嘛！(使用这种方法描述矩阵间的运算，可以直接通过上下标约去，得到最终的旋转矩阵非常方便)

那机智的你立马想到

$$ {C_2^3}^{-1}C_1^3=C_1^2 $$

这样不就可以表示一个大的旋转减去一个小的旋转等于另一个旋转了嘛！这不就是旋转之间的减法吗！

但是我们都知道，在矩阵的运算中「矩阵的逆」是比较不好求的，矩阵最好求的是「转置」，做这个减法每次都要求「逆」也是挺麻烦的。

但是旋转矩阵是个「正交阵」，所以它的「转置」=「逆」，我的天呀，这样一来不仅旋转矩阵的误差一定是个旋转矩阵，而且计算过程也变的非常简单，旋转矩阵简直就是完美的姿态描述方法呀！

$$ {C_2^3}^{T}C_1^3=C_1^2 $$

而且，我们甚至得到了反向旋转的方法!旋转矩阵的转置表示的就是反方向的旋转！

$$ \begin{aligned} {C_2^3}^{T}C_1^3&=C_1^2 \ {C_3^2}C_1^3&=C_1^2 \ C_3^2&={C_2^3}^{T} \end{aligned} $$

总结一下旋转矩阵的优点： 1.描述与顺序无关的一次旋转。 2.可以描述空间中所有的姿态。 2.做差之后依然是个旋转. 3.做差的运算非常简单，只涉及矩阵乘法和矩阵转置。

旋转矩阵简直是完美的描述方法呀！

PS: *不是所有的33矩阵都是旋转矩阵，旋转矩阵有两个约束：正交且行列式=1**，即：

$$ CC^T=1, \det C=1 $$

看到这里我有个最大的疑惑，如此完美的方法，可以描述空间中所有的姿态，没有小角度假设之类的说辞，做差也非常简单，为啥就是没人用呢？

书上经常说因为它有9个参数计算量大？可是开源的代码不也大量使用欧拉角转旋转矩阵，旋转矩阵转欧拉角的代码吗？

我倒觉得这些不是核心原因，我觉得核心原因是如果要使用它我们得为他重新设计控制器。

通常我们设计都是三通道姿态控制器，x轴通道，y轴通道，z轴通道。

如果我们使用欧拉角恰好误差是三个数字，可以直接给三个通道的控制器，现在我们得到的误差是个矩阵，这个矩阵要如何给三通道控制器使用呢？

有两个办法 1: 是将旋转矩阵转换成一个三个参数的描述方法(当然不是欧拉角)。 2: 设计一个基于旋转矩阵的控制器。尤其是位置外环直接得到期望旋转矩阵，相当于由里到外重新写一套代码了！

当然显而易见大部分人选择了第一种方法，简单有效。

第二种方法叫做「几何控制」，如果你需要全方位的姿态控制，实现超强的机动性，赶紧去查资料吧。

下一篇我们讲讲大家最最最喜欢的轴角和四元数。

个人微信公众号：【无人机干货铺】

---