姿态的表示方法
载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数。
欧拉角的物理意义比较直观,即航向角
ψ,俯仰角
θ,横滚角
γ,分别是导航系到载体系的三个旋转角度。
姿态矩阵可以由欧拉角直接计算得到,即三个角度对应的转换矩阵依相乘。
注意的是导航系到载体系是按照航向角,俯仰角,横滚角的顺序变换的所以导航系到载体系变换时,三个矩阵相乘的顺序应该是
C(γ)∗(θ)∗C(ψ).
四元数表示方法较为抽象,直接从四元数“很难”看出载体姿态(这个姿态是对人的想象来说的姿态,因为四元数和欧拉角本无本质区别,只是一种表示方法),那么他的物理意义是什么呢?
四元数表示姿态的物理意义
四元数表示姿态时有多个写法,其中一种表示方法如下:
q=cos(2θ)+unsin(2θ)
欧拉角表示姿态变换时,表征的是分别绕着
(i,j,k)三个坐标轴的三次旋转。根据欧拉定理,这三次旋转可以等效成绕着某轴一次旋转而成。这个某轴就是公式中的
un=(u1,u2,u3),旋转角度就是公式中的
θ。
四元数通常写为
q=q0+q1+q2+q3
那么用于姿态表示时,
q0=cos(2θ)
qi=ui∗sin(2θ),i=1,2,3
使用四元数进行载体姿态更新方程
q(t+δt)=q(t)+dtdq×δt
四元数的更新可通过上式完成。因此只要知道初始四元数(载体的初始姿态)和四元数的导数就可以完成任意时刻四元数的解算。
例如px4中姿态更新的代码如下
_q += _q.derivative(corr) * dt;
四元数微分方程
dtdq 需要求解四元数微分方程,基本推导过程如下:
dtdq=−2θ˙sin(2θ)+un2θ˙cos(2θ)+sin(2θ)dtdun
其中,
dtdun=0,因此:
dtdq=−2θ˙sin(2θ)+un2θ˙cos(2θ)
=un×un2θ˙sin(2θ)+un2θ˙cos(2θ)
=un2θ˙×(unsin(2θ)+uncos(2θ))
=un2θ×q=2ωnbn×q
结果中的角速度为导航系到机体系在导航系下的旋转角速度矢量,但是我们通过陀螺仪测得的是载体系下的角速度。因此还需要一部变换:
ωnbn=q×ωnbb×q∗
由此可得到,
dtdq=q×2ωnbb
我们得到了四元数导数和载体系角速度的数学关系,展开上式得
dtdq:
⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0ωx−ωz−ωyωx0⎦⎥⎥⎤×⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
也可以写成,
⎣⎢⎢⎡q0q1q2q3−q10−q3q2−q2q30q1−q3−q2q1q0⎦⎥⎥⎤×⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
四元数初始值确定
px4 中的初始四元数计算。
bool AttitudeEstimatorQ::init()
四元数初始值的确实可以看做是一个初始对准的过程。
而上述模型仅仅是理想状态下的数学模型,实际角速度中必然包含零偏、噪声等误差,更新过程中又不可避免的有飘漂移发生。所以仅用上式是无法给出可以使用的姿态的。
实际使用中常常用kalman滤波或者补偿滤波的方法来“校正”这些误差。因此初始值可以不必那么精确。给出一个概略初始值即可。
例如可以通过加速度z轴方向和磁罗盘的输出向量,来完成对准。两者从物理概念上实际上分别完成了调平和指北的任务。