飞控：姿态解算

坐标系

导航坐标系：地球为坐标系，常常采用北东地构成xyz轴

机体坐标系：固定在物体上，x指向机头，y指向飞机右侧，z垂直于xy平面向下

姿态

飞行器的俯仰（pitch）\横滚（roll）\航向(yaw)

数学模型：飞行器的机体坐标系（r）和导航坐标系（R）之间的角位置关系（姿态角）。常常用欧拉角，四元数等数学方法来描述r和R的关系

姿态表示方式

1.欧拉角

用机体坐标系和静止不动的参考坐标系之间的几个夹角进行表述

2、四元数表示：

四元数表示着一个四维空间相对于复数表示一个二维空间

四

元数是一个由四个元表示的数

q为实数

i,j,k向量互相正交且又是虚数单位

表示为三角形式

u为虚数

表示为指数形式

姿态解算的方法

同理可以推出

所得的C即为方向余弦矩阵，也称为旋转矩阵，描述了b系相对于n系的旋转，里面的元素都是由欧拉角来进行表示的。将旋转矩阵的元素视为常数

得到n系向b系转换的旋转矩阵

如果用四元数表示旋转矩阵，结果为

想要得到飞行姿态，需要实时更新四元数，如果用四元数的三角形式建立微分方程，如果解出就可以解出四元数

用一阶龙格库塔方法求解得

通过上个周期的四元数和所获得的加速度数据就可以实时解算出四元数，从而解出当前姿态