【飞控】姿态误差 (一)-欧拉角做差

在设计控制器之前,有个重要的问题要解决,就是如何计算期望姿态的误差?因为大部分控制器都需要你告诉我这个误差,然后才可以去调整,去减小这个误差直到没有误差。

求误差可太简单了,不就是个减法吗?

$$\left[ \begin{array} { c } { \Delta\phi } \\ { \Delta\theta } \\ { \Delta\psi } \end{array} \right]=\left[ \begin{array} { c } { \phi _ { 1 } } \\ { \theta _ { 1 } } \\ { \psi _ { 1 } } \end{array} \right]-\left[ \begin{array} { c } { \phi _ { 2 } } \\ { \theta _ { 2 } } \\ { \psi _ { 2 } } \end{array} \right]$$

看起来很有道理,但是骗骗小孩子可以,骗我和我的读者朋友们还差一点。

我有几个问题代表大家问一下,首先姿态本质是旋转,姿态的误差其实是旋转之间的误差,那必须还是个旋转,那么直接相减还能表示旋转吗?也就是说相减得到的角度转换成旋转矩阵还是个正交矩阵吗?

而且欧拉角是有隐藏条件的,就是旋转顺序,同样是三个角 (10,20.30) ,x-y-z,y-x-z顺序可是两个不同的旋转,如果我不告诉你它的旋转顺序,那其实他就是没用的三个数,因为它根本就没法还原成旋转。但是我就没见过有对控制器做先后顺序处理的,roll,pitch,yaw的控制器都是同时启动,没有顺序之分。

所以,凭什么姿态误差可以用欧拉角相减!!!

我仔细想了一下,估摸着又跟小角度假设有关。

欧拉角的旋转顺序有6种:
x-y-z,y-z-x,z-x-y,x-z-y,z-y-x,y-x-z

我们先看一下我们最最最熟悉的z-y-x顺序的欧拉角对应的旋转矩阵吧

$$\begin{aligned}&[ \mathbf { R_{zyx} } ] =\left[ \begin{array} { c c c } { 1 } & { 0 } & { 0 } \\ { 0 } & { \cos \phi } & { \sin \phi } \\ { 0 } & { - \sin \phi } & { \cos \phi } \end{array} \right] \left[ \begin{array} { c c c } { \cos \theta } & { 0 } & { - \sin \theta } \\ { 0 } & { 1 } & { 0 } \\ { \sin \theta } & { 0 } & { \cos \theta } \end{array} \right] \left[ \begin{array} { c c c } { \cos \psi } & { \sin \psi } & { 0 } \\ { - \sin \psi } & { \cos \psi } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{array} \right] \\& =\left[\begin{matrix}\cos\theta\cos\psi & \cos\theta\sin\psi & -\sin\theta & \\ -\cos\phi\sin\psi+\sin\phi\sin\theta\cos\psi & \cos\phi\cos\psi+\sin\phi\sin\theta\sin\psi & \sin\phi\cos\theta & \\ \sin\phi\sin\psi+\cos\phi\sin\theta\cos\psi & -\sin\phi\cos\psi+\cos\phi\sin\theta\sin\psi & \cos\phi\cos\theta & \\ \end{matrix}\right]\end{aligned}$$$$[\mathbf{ R^T_ {zyx} }]=\left[\begin{matrix}\\ \cos\theta\cos\psi & -\cos\phi\sin\psi+\sin\phi\sin\theta\cos\psi & \sin\phi\sin\psi+\cos\phi\sin\theta\cos\psi & \\ \cos\theta\sin\psi & \cos\phi\cos\psi+\sin\phi\sin\theta\sin\psi & -\sin\phi\cos\psi+\cos\phi\sin\theta\sin\psi & \\ -\sin\theta & \sin\phi\cos\theta & \cos\phi\cos\theta & \\ \end{matrix}\right]$$

这就是我们最熟悉的机体转地理坐标系的旋转矩阵

$$C_b^n$$

再看一下z-x-y顺序的旋转矩阵

$$\begin{aligned}&[ \mathbf { R_{zxy} } ] = \left[ \begin{array} { c c c } { \cos \theta } & { 0 } & { - \sin \theta } \\ { 0 } & { 1 } & { 0 } \\ { \sin \theta } & { 0 } & { \cos \theta } \end{array} \right] \left[ \begin{array} { c c c } { 1 } & { 0 } & { 0 } \\ { 0 } & { \cos \phi } & { \sin \phi } \\ { 0 } & { - \sin \phi } & { \cos \phi } \end{array} \right] \left[ \begin{array} { c c c } { \cos \psi } & { \sin \psi } & { 0 } \\ { - \sin \psi } & { \cos \psi } & { 0 } \\ { 0 } & { 0 } & { 1 } \end{array} \right] \\&=\left[\begin{matrix}\\ \cos\theta\cos\psi-\sin\theta\sin\phi\sin\psi & \cos\theta\sin\psi+\sin\theta\sin\phi\cos\psi & -\sin\theta\cos\phi & \\ -\cos\phi\sin\psi & \cos\phi\cos\psi & \sin\phi & \\ \sin\theta\cos\psi+\cos\theta\sin\phi\sin\psi & \sin\theta\sin\psi-\cos\theta\sin\phi\cos\psi & \cos\theta\cos\phi & \\ \end{matrix}\right]\end{aligned}$$$$[\mathbf{R^T_{zxy } }]=\left[\begin{matrix}\\ \cos\theta\cos\psi-\sin\theta\sin\phi\sin\psi & -\cos\phi\sin\psi & \sin\theta\cos\psi+\cos\theta\sin\phi\sin\psi & \\ \cos\theta\sin\psi+\sin\theta\sin\phi\cos\psi & \cos\phi\cos\psi & \sin\theta\sin\psi-\cos\theta\sin\phi\cos\psi & \\ -\sin\theta\cos\phi & \sin\phi & \cos\theta\cos\phi & \\ \end{matrix}\right]$$

你会发现每个顺序的旋转矩阵都不一样,所以带入同样的三个角度,得到的矩阵是不一样的,同样一个向量左乘一个不同的矩阵,得到的向量肯定是不一样的。

什么是小角度假设呢
就是角度很小时cos很接近1,sinx=x,两个很小的数相乘为0,即:

$$cosx \approx 1 , \quad sinx \approx x , \quad x _ { 1 } x _ { 2 } \approx 0$$

一旦我们加上小角度假设后,你会发现所有不同顺序的欧拉角对应的旋转矩阵都可以变成以下的形式 :

$$R_{all} \approx \left[\begin{matrix} 1 & -\psi & \theta \\ \psi & 1 & -\phi \\ -\theta & \phi & 1 \end{matrix} \right]=\mathbf { 1 } \mathbf + { {\theta}^\land}$$

也就是说,在小角度的假设下,旋转矩阵变的与旋转顺序无关了,所以控制器先后的问题解决了。
而且,这个矩阵一定是个正交矩阵。

所以如果当角度足够小,你可以把这个误差转换成小角度下的旋转矩阵,这个矩阵是个正交矩阵,而且与顺序无关

$$\left[ \begin{array} { c } { \Delta\phi } \\ { \Delta\theta } \\ { \Delta\psi } \end{array} \right]\Rightarrow \left[\begin{matrix} 1 & -\Delta\psi & \Delta\theta \\ \Delta\psi & 1 & -\Delta\phi \\ -\Delta\theta & \Delta\phi & 1 \end{matrix} \right]$$

我就发现这帮人就不讲道理,一说不通就给你上小角度假设,也不说多小的角度叫小角度,难道就没人管这个事了吗?就没有其他跟有说服力的方法吗?作为一个飞控工程师这问题必须死磕,下期我们接着聊。

欢迎加我的个人微信交流,共同进步。

关注微信公众号【zinghd的思考】,持续分享飞控干货,回复【姿态】为您提供生成旋转矩阵的小程序和PDF版。

附录:不同旋转顺序欧拉角对应的旋转矩阵,R_xyz表示先绕x旋转再绕y轴最后绕z轴旋转,也就是说对应的矩阵的乘法是R=[z][y][x]。

旋转矩阵的转置:

$$[\mathbf{R^T_{xzy } }]=\left[\begin{matrix}\\ \cos\theta\cos\psi & -\sin\psi & \sin\theta\cos\psi & \\ \cos\theta\sin\psi\cos\phi+\sin\theta\sin\phi & \cos\psi\cos\phi & \sin\theta\sin\psi\cos\phi-\cos\theta\sin\phi & \\ \cos\theta\sin\psi\sin\phi-\sin\theta\cos\phi & \cos\psi\sin\phi & \sin\theta\sin\psi\sin\phi+\cos\theta\cos\phi & \\ \end{matrix}\right]$$$$[\mathbf{R^T_{xyz } }]=\left[\begin{matrix}\\ \cos\psi\cos\theta & -\sin\psi\cos\theta & \sin\theta & \\ \cos\psi\sin\theta\sin\phi+\sin\psi\cos\phi & -\sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & -\cos\theta\sin\phi & \\ -\cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi & \sin\psi\sin\theta\cos\phi+\cos\psi\sin\phi & \cos\theta\cos\phi & \\ \end{matrix}\right]$$$$[\mathbf{R^T_{yzx } }]=\left[\begin{matrix}\\ \cos\psi\cos\theta & -\cos\phi\sin\psi\cos\theta+\sin\phi\sin\theta & \sin\phi\sin\psi\cos\theta+\cos\phi\sin\theta & \\ \sin\psi & \cos\phi\cos\psi & -\sin\phi\cos\psi & \\ -\cos\psi\sin\theta & \cos\phi\sin\psi\sin\theta+\sin\phi\cos\theta & -\sin\phi\sin\psi\sin\theta+\cos\phi\cos\theta & \\ \end{matrix}\right]$$$$[\mathbf{R^T_{yxz } }]=\left[\begin{matrix}\\ \cos\psi\cos\theta+\sin\psi\sin\phi\sin\theta & -\sin\psi\cos\theta+\cos\psi\sin\phi\sin\theta & \cos\phi\sin\theta & \\ \sin\psi\cos\phi & \cos\psi\cos\phi & -\sin\phi & \\ -\cos\psi\sin\theta+\sin\psi\sin\phi\cos\theta & \sin\psi\sin\theta+\cos\psi\sin\phi\cos\theta & \cos\phi\cos\theta & \\ \end{matrix}\right]$$$$[\mathbf{R^T_{zyx } }]=\left[\begin{matrix}\\ \cos\theta\cos\psi & -\cos\phi\sin\psi+\sin\phi\sin\theta\cos\psi & \sin\phi\sin\psi+\cos\phi\sin\theta\cos\psi & \\ \cos\theta\sin\psi & \cos\phi\cos\psi+\sin\phi\sin\theta\sin\psi & -\sin\phi\cos\psi+\cos\phi\sin\theta\sin\psi & \\ -\sin\theta & \sin\phi\cos\theta & \cos\phi\cos\theta & \\ \end{matrix}\right]$$$$[\mathbf{R^T_{zxy } }]=\left[\begin{matrix}\\ \cos\theta\cos\psi-\sin\theta\sin\phi\sin\psi & -\cos\phi\sin\psi & \sin\theta\cos\psi+\cos\theta\sin\phi\sin\psi & \\ \cos\theta\sin\psi+\sin\theta\sin\phi\cos\psi & \cos\phi\cos\psi & \sin\theta\sin\psi-\cos\theta\sin\phi\cos\psi & \\ -\sin\theta\cos\phi & \sin\phi & \cos\theta\cos\phi & \\ \end{matrix}\right]$$

旋转矩阵:

$$[\mathbf{R_{xzy } }]=\left[\begin{matrix}\\ \cos\theta\cos\psi & \cos\theta\sin\psi\cos\phi+\sin\theta\sin\phi & \cos\theta\sin\psi\sin\phi-\sin\theta\cos\phi & \\ -\sin\psi & \cos\psi\cos\phi & \cos\psi\sin\phi & \\ \sin\theta\cos\psi & \sin\theta\sin\psi\cos\phi-\cos\theta\sin\phi & \sin\theta\sin\psi\sin\phi+\cos\theta\cos\phi & \\ \end{matrix}\right]$$$$[\mathbf{R_{xyz } }]=\left[\begin{matrix}\\ \cos\psi\cos\theta & \cos\psi\sin\theta\sin\phi+\sin\psi\cos\phi & -\cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi & \\ -\sin\psi\cos\theta & -\sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi & \sin\psi\sin\theta\cos\phi+\cos\psi\sin\phi & \\ \sin\theta & -\cos\theta\sin\phi & \cos\theta\cos\phi & \\ \end{matrix}\right]$$$$[\mathbf{R_{yzx } }]=\left[\begin{matrix}\\ \cos\psi\cos\theta & \sin\psi & -\cos\psi\sin\theta & \\ -\cos\phi\sin\psi\cos\theta+\sin\phi\sin\theta & \cos\phi\cos\psi & \cos\phi\sin\psi\sin\theta+\sin\phi\cos\theta & \\ \sin\phi\sin\psi\cos\theta+\cos\phi\sin\theta & -\sin\phi\cos\psi & -\sin\phi\sin\psi\sin\theta+\cos\phi\cos\theta & \\ \end{matrix}\right]$$$$[\mathbf{R_{yxz } }]=\left[\begin{matrix}\\ \cos\psi\cos\theta+\sin\psi\sin\phi\sin\theta & \sin\psi\cos\phi & -\cos\psi\sin\theta+\sin\psi\sin\phi\cos\theta & \\ -\sin\psi\cos\theta+\cos\psi\sin\phi\sin\theta & \cos\psi\cos\phi & \sin\psi\sin\theta+\cos\psi\sin\phi\cos\theta & \\ \cos\phi\sin\theta & -\sin\phi & \cos\phi\cos\theta & \\ \end{matrix}\right]$$$$[\mathbf{R_{zyx } }]=\left[\begin{matrix}\\ \cos\theta\cos\psi & \cos\theta\sin\psi & -\sin\theta & \\ -\cos\phi\sin\psi+\sin\phi\sin\theta\cos\psi & \cos\phi\cos\psi+\sin\phi\sin\theta\sin\psi & \sin\phi\cos\theta & \\ \sin\phi\sin\psi+\cos\phi\sin\theta\cos\psi & -\sin\phi\cos\psi+\cos\phi\sin\theta\sin\psi & \cos\phi\cos\theta & \\ \end{matrix}\right]$$$$[\mathbf{R_{zxy } }]=\left[\begin{matrix}\\ \cos\theta\cos\psi-\sin\theta\sin\phi\sin\psi & \cos\theta\sin\psi+\sin\theta\sin\phi\cos\psi & -\sin\theta\cos\phi & \\ -\cos\phi\sin\psi & \cos\phi\cos\psi & \sin\phi & \\ \sin\theta\cos\psi+\cos\theta\sin\phi\sin\psi & \sin\theta\sin\psi-\cos\theta\sin\phi\cos\psi & \cos\theta\cos\phi & \\ \end{matrix}\right]$$