所谓姿态角,就是机体飞行时相对于世界坐标系下的姿态角,也即是三方向欧拉角的统称,包含:滚转角roll、俯仰角pitch、偏航角yaw。
姿态角的定义:
2. 俯仰角θ(pitch):机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上(抬头)时,俯仰角为正,否则为负。 3. 偏航角ψ(yaw): 机体坐标系xb轴在水平面上投影与地面坐标系xg轴(在水平面上,指向目标为正)之间的夹角,由xg轴逆时针转至机体xb的投影线时,偏航角为正,即机头右偏航为正,反之为负。
这里面,每一个姿态角都是在描述,在一次旋转变换中,坐标系以自身某一坐标轴为周线旋转相应的角度。因此,当发生一次旋转后,之后的旋转变化都是相对于当下的坐标系而言,与初始坐标系无关,因此旋转变换与旋转轴选取顺序有关,也进而可知任意两互为可相互旋转的坐标系,可通过多种旋转变换转换。
为了使分析统一化,我们采用如下图所示的形式来解释欧拉角: 如图所示:我们采取欧拉角变换的步骤为:
其实,我并不觉得这样的欧拉角描述合适,我们不妨换一种形式思考(其实还有很多种,可以思考):
我们假设坐标系$S_2$是由坐标系$S_1$经过x、y、z逐次旋转相应的角度得到,那么我们自然可以逆向逐次还原:
- 首先,我们取坐标系$S_2$的平面OXY与坐标系$S_1$的平面OYZ的交线,记作N
- 其次,我们得到坐标系$S_2$的y轴与N线的夹角,记作$\psi$,并转动z轴$\psi$角度,使得y轴与N轴重合,此时y轴便回归原位
- 然后,我们取此时的坐标系$S_2$的x轴与$S_1$坐标系的x轴的夹角,记作$\theta$,并转动y轴,使得$S_2$坐标系的x轴与$S_1$坐标系的x轴重合
- 最后,我们取此时的坐标系$S_2$的y轴与$S_1$坐标系的y轴的夹角,记作$\phi$,并转动x轴,使得$S_2$坐标系的z轴与$S_1$坐标系的z轴重合
此时,三个角度$\phi、\theta、\psi$分别就是roll、pitch、yaw。
什么是四元数?
四元数就是由四种不同维度的量来描述一个状态量,这种状态量可以描述一种变化,也可以描述一个位置。
- 当四元数的模为1时,也即是单位四元数,可以表达一种旋转变化;
- 当四元数模不为1时,四元数可分为模×单位四元数,可以表达一种伸长且旋转的变化;
- 当四元数为纯四元数时,也即是纯量为0时,可以描述三维空间中的一个笛卡尔坐标点。
四元数基本形态为:q=a+bi+cj+dk 其中,a、b、c、d为实数,i、j、k为虚数表示三个正交方向
四元数的一些性质
参见Wikipedia
要注意,数乘部分写错了,看的时候要多小心。
主要思想
四元数法,主要是依靠不断的更新四元数,从而使四元数构建的坐标系,更加逼近于当下实际机体坐标系,从而得知Roll、Pitch、Yaw的角度,注意,这里可以求得三个角度,其他方法一般只可以求得两个,第三个还需要另加磁力计来融合求解。
可以说这个方法类似于GHKFilter滤波器中的GFilter,其基本思想就是,既然加速度计和陀螺仪都可以得到Pitch、Roll角,那不妨将其加权融合。
具体操作呢,主要是由于不能传感器的测量准确度不同。
三轴加速度在计算角度过程中不存在积累误差,可以直接通过atan()求出,但它包含了太多的噪声,比如机体在做加速运动时引入的加速度、电机运行时产生的震动等等。
陀螺仪呢,它可以直接获得机体三轴的角加速度,而且不易受到外界的干扰,所以精度较高,不过我们需要对他进行离散求和求积分,容易引入离散误差。
因此,综上,我们可以全用一个定值作为权重附加给两个传感器的输出值上,一般情况下,显然在动态机体中,陀螺仪精度要远比三轴加速度计高精度。
//1:一阶卡尔曼双测量滤波器
float angle_m_x=angleAx,gyro_m_x=gx;
angle_x = angle_x + angle_x_dot*dt;
P_x[0][0] += (P_x[1][0]+P_x[0][1])*dt + P_x[1][1]*dt*dt + Q_angle;
P_x[0][1] += P_x[1][1]*dt;
P_x[1][0] += P_x[0][1];
P_x[1][1] += Q_velo;
E = (P_x[0][0]+R_angle)*(P_x[1][1]+R_velo)-P_x[0][1]*P_x[1][0];
K[0][0]= ((P_x[1][1]+R_velo)*P_x[0][0]-P_x[0][1]*P_x[1][0])/E;
K[0][1]= ((P_x[0][0]+R_angle)*P_x[0][1]-P_x[0][0]*P_x[0][1])/E;
K[1][0]= (P_x[1][0]*(P_x[1][1]+R_velo)-P_x[1][1]*P_x[1][0])/E;
K[1][1]= (P_x[1][1]*(P_x[0][0]+R_angle)-P_x[1][0]*P_x[0][1])/E;
P_x[0][0] += -K[0][0]*P_x[0][0] - K[0][1]*P_x[1][0];
P_x[0][1] += -K[0][0]*P_x[0][1] - K[0][1]*P_x[1][1];
P_x[1][0] += -K[1][1]*P_x[1][0] - K[1][0]*P_x[0][0];
P_x[1][1] += -K[1][0]*P_x[0][1] - K[1][1]*P_x[1][1];
angle_x += K[0][0]*(angle_m_x-angle_x) + K[0][1]*(gyro_m_x-angle_x_dot);
angle_x_dot += K[1][0]*(angle_m_x-angle_x) + K[1][1]*(gyro_m_x-angle_x_dot);
//0:二阶卡尔曼双参滤波器
float angle_m_x=angleAx,gyro_m_x=gx;
angle_x = angle_x + angle_x_dot*dt + 1/2*angle_x_dot2*dt*dt;
angle_x_dot = angle_x_dot + angle_x_dot2*dt;
angle_x_dot2 = angle_x_dot2;
P_x[0][0] += (P_x[0][1]+P_x[1][0])*dt + (1/2*P_x[2][0]+P_x[1][1]+1/2*P_x[0][2])*dt*dt + (1/2*P_x[2][1]+1/2*P_x[1][2])*dt*dt*dt + 1/4*P_x[2][2]*dt*dt*dt*dt + Q_angle;
P_x[0][1] = (P_x[0][1]+P_x[1][1]*dt+1/2*P_x[2][1]*dt*dt) + (P_x[0][2]+P_x[1][2]*dt+1/2*P_x[2][2]*dt*dt)*dt;
P_x[0][2] = (P_x[0][2]+P_x[1][2]*dt+1/2*P_x[2][2]*dt*dt);
P_x[1][0] += (P_x[2][0]+P_x[1][1])*dt + (P_x[2][1]+1/2*P_x[1][2])*dt*dt + 1/2*P_x[2][2]*dt*dt*dt;
P_x[1][1] = P_x[1][1]+P_x[2][1]*dt + (P_x[1][2]+P_x[2][2]*dt)*dt + Q_velo;
P_x[1][2] = P_x[2][2]*dt;
P_x[2][0] += P_x[2][1]*dt + 1/2*P_x[2][2]*dt*dt;
P_x[2][1] += P_x[2][2]*dt;
P_x[2][2] += Q_acce;
error_angle = angle_m_x - angle_x;
error_velo = gyro_m_x-angle_x_dot;
E = (P_x[0][0]+R_angle)*(P_x[1][1]+R_velo)-P_x[0][1]*P_x[1][0];
K[0][0]= ((P_x[1][1]+R_velo)*P_x[0][0]-P_x[0][1]*P_x[1][0])/E;
K[0][1]= ((P_x[0][0]+R_angle)*P_x[0][1]-P_x[0][0]*P_x[0][1])/E;
K[1][0]= (P_x[1][0]*(P_x[1][1]+R_velo)-P_x[1][1]*P_x[1][0])/E;
K[1][1]= (P_x[1][1]*(P_x[0][0]+R_angle)-P_x[1][0]*P_x[0][1])/E;
K[2][0]= (P_x[2][0]*(P_x[1][1]+R_velo)-P_x[2][1]*P_x[1][0])/E;
K[2][1]= (P_x[2][1]*(P_x[0][0]+R_angle)-P_x[2][0]*P_x[0][1])/E;
P_x[2][0] -= K[2][0]*P_x[0][0]+K[2][1]*P_x[1][0];
P_x[2][1] -= K[2][0]*P_x[0][1]+K[2][1]*P_x[1][1];
P_x[2][2] -= K[2][0]*P_x[0][2]+K[2][1]*P_x[1][2];
static float p0=P_x[0][0],p1=P_x[0][1],p2=P_x[0][2];
P_x[0][0] -= K[0][0]*P_x[0][0]+K[0][1]*P_x[1][0];
P_x[0][1] -= K[0][0]*P_x[0][1]+K[0][1]*P_x[1][1];
P_x[0][2] -= K[0][0]*P_x[0][2]+K[0][1]*P_x[1][2];
P_x[1][0] -= K[1][0]*p0+K[1][1]*P_x[1][0];
P_x[1][1] -= K[1][0]*p1+K[1][1]*P_x[1][1];
P_x[1][2] -= K[1][0]*p2+K[1][1]*P_x[1][2];
angle_x += K[0][0]*error_angle+K[0][1]*error_velo;
angle_x_dot += K[1][0]*error_angle+K[1][1]*error_velo;
angle_x_dot2 += K[2][0]*error_angle+K[2][1]*error_velo;
如果不想深入研究KalmanFilter算法的,尤其不对非线性KalmanFilter感兴趣的,可以直接调用库函数程序