飞行仿真--3.刚体六自由度方程、变换矩阵与四元数

B787Dream

以下文章来源：微信公众号    无风夜雪
1.刚体六自由度方程
飞行仿真归根结底是通过飞行力学分析建立数学模型仿真分析飞机的各项性能。这里的数学建模依然采用质点假设，采用经典力学中的牛顿第二运动定律，即物体加速度的大小跟作用力成正比，跟物体的质量成反比，为F=ma的形式，力矩也同理。三维空间中描述刚体一共有六个自由度，即分别沿三个轴方向的平动和转动。如果已知当前合力和合力距就可以解算出下一时刻的加速度和角加速度量。按照微积分的定义，位移的变化量（微分）即为速度，速度的变化量（微分）即为加速度，如此便可通过列出微分方程积分得到速度、位移、姿态角、操纵性能值等状态量，最终将这些状态量通过虚拟仪表显示出来即完成一次飞行仿真的解算。


飞行仿真有一套特有的符号表示状态量，力采用F（Force）表示，位移用P（Position）表示，机体坐标系下三个轴方向的速度（Velocity）采用Vb表示，角加速度采用ωb表示，写成行向量的形式如下图。机体系下三个欧拉角即滚转角为Φ，偏航角为Ψ，俯仰角θ，个人感觉这三个符号与中国的象形文字相似，专用于表示飞机姿态很容易识别。


推荐采用线性代数表示比较简洁，实际使用中也常采用三维列向量的形式，这在计算机编程求解中很方便索引。
有了合力与合力距，根据牛顿第二定律就可以列出机体质心的六自由度运动方程。
平动方程：


角运动方程：


参考Dreier M E. Introduction to Helicopter and Tiltrotor Flight Simulation[M]. AIAA (American Institute of Aeronautics & Ast, 2007.一书还可以写成更紧凑的矩阵形式，此不列出。
关于六自由度方程求解，想说明的是一定要明确未知和已知，比如平动方程中，我们首先已知的是左侧的合力，未知的是右侧的速度导数，因此通过这个方程解算出来的实际是加速度。三个方程三个未知数的求解并没有太大难度。
2.变换矩阵与四元数
设惯性坐标系下的欧拉角速度为：


机体坐标系下的角速度为：


惯性坐标系下的欧拉角速度与机体坐标系下的角速度还有如下图的转换关系：


为了从机体坐标系速度得到地球坐标系下的欧拉角速度，就必须求变换矩阵的逆矩阵。但是的逆矩阵不是其转置矩阵，且其逆矩阵在俯仰角为±90°时为奇异矩阵，即：


针对这一情况，虽然许多实例和文献认为直升机的欧拉角几乎不可能到±90°，可以放心使用该方法，但实际只是回避并没有解决根本问题，比如应用到固定翼飞机或游戏开发中就无可回避了，当俯仰角达到90°就会出现除0现象，无法计算，计算机程序界称之为『溢出』。这一问题的核心解决方法就是采用更普适的四元数法。四元数在飞行飞行仿真中得到了广泛的应用，它的变换矩阵不存在奇异点。其思路是，先将上一时刻的欧拉角（前提不是上述奇异点）转化为四元数形式的表示形式，进而将变换矩阵也可换成四元数的形式参与微分方程解算，待解算完成后再将四元数转化为欧拉角。
四元数的起源
四元数的起源于寻找复数的三维对应物，复数可以表达一个二维矢量，当处理不共面的多个矢量时，需要用新的数来表达一个三维矢量。1843年哈密顿（Hamilton）发明了四元数，这是一种形如A=a0+a1i+a2j+a3k的数。历史上矢量是从四元数中分离出来的，并由此发展为广泛使用的矢量代数和矢量分析，但矢量的点乘和叉乘不是可结合的，也不是可除的。现在反过来把矢量代数和矢量分析的内容纳入到四元数的体系中，这使得矢量仍可用四元数的方法来处理。同时四元数还具有处理旋转的优势，是一个封闭的数系。现已广泛应用于力学、磁学、晶体学。（参考https://www.cnblogs.com/gary-guo/p/5097008.html）
将四元数用行向量来表示：


将欧拉角转换为四元数的公式为：


利用该转换式可以实现任意欧拉角与四元数的变换，一般用来初始化四元数。但实际，动力学运动方程解算出来的是机体角速度p、q、r，因此还需要其微分形式用于连续求解。而四元数角速度与欧拉角速度又存在如下关系：


其中c为误差修正系数，根据Dreier M E. Introduction to Helicopter and Tiltrotor Flight Simulation[M]. AIAA (American Institute of Aeronautics & Ast, 2007.一书，c取32为最佳修正系数。
为了说明四元数到欧拉角的变换，有必要先说明常用的变换矩阵。在任何一本计算机图形学书籍中都会有这方面介绍，因为比较常用，在此简述。
常用的变换矩阵
(1) 基元旋转矩阵
基元旋转矩阵是旋转变换的基本矩阵，复杂多变的矩阵都可以由这些基元旋转变换矩阵组合而来。所有旋转变换矩阵都满足右手定则。
绕X轴的旋转变换矩阵为：



绕Y轴的旋转变换矩阵设为：


绕Z轴的旋转变换矩阵为：


观察可以发现，基元旋转矩阵比较简单，主要有如下识记规律：
首先，主对角线上的1的位子决定了变换的轴；
其次，主对角线除了1之外都是余弦项，正弦项对角线与余弦项对角线交叉垂直，正弦项的正负按X-Y-Z有“下-上-下”的关系；
最后，其余元素填充为0，这些矩阵皆为正交阵，其转置矩阵就等于其逆矩阵，很方便使用。
(2) 地面坐标系到机体坐标系的变换矩阵Tbe（earth to body）
按照NASA的约定，变换矩阵的是有固定变换顺序的，顺序为先绕Z轴的偏航角，再绕Y轴的俯仰角，最后绕X轴的滚转角。实际矩阵相乘的顺序为左乘，即排在最右端的首先被执行，所以Tbe=Rx.Ry.Rz。为化繁为简，以C代表正弦项，S代表余弦项，Tbe得到的具体项如下图，地面惯性坐标系到机体坐标系的变换矩阵也为正交阵，其转置矩阵就等于其逆矩阵。矩阵论中关于逆的实际工程意义正在于此，例如已知Tbe（大地坐标系到机体坐标系的变换矩阵）,要计算Teb（机体坐标系到大地坐标系的变换矩阵），则求Tbe的逆即可，而Tbe转置矩阵就等于其逆矩阵，极其方便。


(3)四元数形式的地面坐标系到机体坐标系的变换矩阵Tbe(earth to body)
为了充分利用四元数，在汇总计算中，从大地坐标系（机场相对坐标系）到机体坐标系的变换矩阵也可以直接写成四元数的形式，与原形式是等价的：


采用四元数完成解算后，由于四元素形式的Tbe与直接采用欧拉角是等效的，按对应项等效列方程就可以实现四元数到欧拉角的变换，再次转换成我们熟悉的欧拉角。
四元数转换为欧拉角的公式为（其中sign为数学中使用的符号函数）：


注：本文采用了大量线性代数、矩阵论、计算机图形学、飞行仿真等数学定义，如果想进一步了解更多的细节您可参考Dreier M E. Introduction to Helicopter and Tiltrotor Flight Simulation[M]. AIAA (American Institute of Aeronautics & Ast, 2007
或其它更专业的书籍了解基本概念。
更多文章 欢迎关注微信公号：无风夜雪

jasonhsu

楼主，飞行仿真系列的第二篇不见了……

嗯?

四元数运动学方程里面的修正系数什么时候取0⃣️什么时候非0⃣️呀？