基于STM32F407四旋翼无人机 | 您所在的位置:网站首页 › mpu6050姿态解算融合 › 基于STM32F407四旋翼无人机 |
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五)
姿态解算姿态解算定义欧拉角四元数四元数性质
方向余弦矩阵四元数方向余弦矩阵
叉积法融合陀螺仪数据和加速度数据叉积运算
一阶龙格库塔法四元数更新获得欧拉角
姿态解算
姿态解算定义
在机器人的姿态中是涉及了两个比较关键的坐标系,一个是机器人本身的坐标系,称为机体坐标系,该坐标系会跟随机器人的姿态变化而变化。另一个坐标系为地理坐标系,在看地图的时候我们只有一个固定的方位是东西南北,即该坐标系为东北天坐标系,X轴指向东方向,Y轴指向北方向,Z轴指向天,地理坐标系也可以称为导航坐标系。为什么要介绍这两个坐标系呢?第一是因为在做姿态解算的时候就是需要将机体坐标系和地理坐标系进行相互联系,计算出机体坐标系对于地理坐标系的变化。第二是做惯性导航需要将机体坐标系转到东北天坐标系(地理坐标系)。 这两个坐标系之间的转换其实就是对一个坐标系绕一个轴做旋转,两个坐标系的原点重合。而坐标系的定点转动可以通过一个矩阵来表示,矩阵中的各个元素包含了坐标系的姿态信息,对于机器人来说可以根据该矩阵中的一些元素计算出,横滚角,俯仰角和偏航角。将机体坐标系中的一个向量,左乘这个矩阵之后就可以推出该向量在地理坐标系中的大小。 我们的目的就是根据惯性元件(加速度计、陀螺仪和磁力计)输出的实时的输出数据来计算姿态矩阵,从而可以不断更新横滚、俯仰和偏航角。陀螺仪输出的为角速度。 欧拉角欧拉角表示法具有简便、几何意义明显的优点,同时姿态敏感器可以直接测出这些参数,但是采用欧拉角的姿态描述方式存在一些问题,且需要好几次的三角函数运算,对于MCU而言,计算很多的三角函数很耗资源,而且计算量很大。而采用四元数表示方法则可以避免这些问题,因此开始采用四元数来描述飞行器的姿态。 根据欧拉定理,坐标系绕固定点的位移也可以是绕该点的若干次有限转动的合成。在欧拉转动中,在三次转动中每次的旋转轴是被转动坐标系的某一坐标轴,每次转动的角为欧拉角。 根据秦永元的《惯性导航》来讲解转换矩阵。
矩阵相乘结果为: (1)四元数定义:四元数是由四个元构成的数Q(q0,q1,q2,q3) = q0 + q1i + q2j + q3k;其中,q0,q1,q2,q3是实数,i,j,k既是互相正交的单位向量,又是虚单位根号-1。四元数即可看作四维空间中的一个向量,又可以看做一个超复数。对于后续有一个重要的变化需要记住: (2)四元数的范数:
由于加速度计短时间内数据不算可靠,但是长时间加速度计的数据是可靠的,而陀螺仪数据是由积分得来的,所以长时间的话积分误差会不断的叠加,不断增大,所以就要通过算法融合加速度计和陀螺仪来不断矫正陀螺仪数据。在此使用叉积法去做融合。 世界坐标系重力分向量是通过方向旋转矩阵的最后一列的三个元素乘上加速度就可以算出机体坐标系中的重力向量。 Gravity.x = 2*(Nq.q1*Nq.q3 - Nq.q0*Nq.q2); //由下向上方向的加速度在加速度计X分量 Gravity.y = 2*(Nq.q0*Nq.q1 + Nq.q2*Nq.q3); //由下向上方向的加速度在加速度计Y分量 Gravity.z = 1 - 2*(Nq.q1*Nq.q1 + Nq.q2*Nq.q2); //由下向上方向的加速度在加速度计Z分量
图1中模型中的小球处于失重状态,坐标系与世界坐标系重合。 图2中坐标系仍与世界坐标系重合,描述为[x,y,z],不同的是小球收到重力作用,模型坐标系中重力向量为[0,0,1],世界坐标系中也是[0,0,1]。 图3中在世界坐标系中与地面夹角为45度,但是坐标系仍是[x,y,z],重力向量变为了[-0.71,0,-0.71],注意这个向量是模型坐标系的向量值,转换到世界坐标系仍是[0,0,1]。 所以无论加计怎么旋转重力向量在世界坐标系中都是[0,0,1],但是我们有加速度计读到的值是基于模型坐标系的,也就是我们所说的机体坐标系。现在就明白了,刚才为什么要用四元数将[0,0,1]乘一个旋转矩阵,我们之后关于加速度计和陀螺仪的计算都是基于机体坐标系的。 叉积运算详细过程可以通过代码来了解: (1)将加计的三维向量转为单位向量 NormQuat = my_sqrt(my_pow(Acc.x) + my_pow(Acc.y) + my_pow(Acc.z)); //把加计的三维向量转成单位向量。 Acc.x = Acc.x / NormQuat; Acc.y = Acc.y / NormQuat; Acc.z = Acc.z / NormQuat;(2)叉积运算开始。 /* 叉乘得到误差 */ why_err_Acc.x = Acc.y*Gravity.z - Acc.z*Gravity.y; why_err_Acc.y = Acc.z*Gravity.x - Acc.x*Gravity.z; why_err_Acc.z = Acc.x * Gravity.y - Acc.y * Gravity.x;(3)在对叉乘得到的误差进行低通滤波,其中的HalfTime 表示在姿态解算中运行周期的一半。 /* 误差低通 */ ref.err_lpf.x += 6.28f *HalfTime *( why_err_Acc.x - ref.err_lpf.x ); ref.err_lpf.y += 6.28f *HalfTime *( why_err_Acc.y - ref.err_lpf.y ); ref.err_lpf.z += 6.28f *HalfTime *( why_err_Acc.z - ref.err_lpf.z );(4)开始对误差进行积分运算,并进行积分限幅,在进行kp控制 #define IMU_INTEGRAL_LIM 0.034906585f #define ANGLE_TO_RADIAN 0.017453293f ref.err_Int.x += ref.err.x *Ki *2 *HalfTime ; ref.err_Int.y += ref.err.y *Ki *2 *HalfTime ; ref.err_Int.z += ref.err.z *Ki *2 *HalfTime ; /* 积分限幅 */ ref.err_Int.x = LIMIT(ref.err_Int.x, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM ); ref.err_Int.y = LIMIT(ref.err_Int.y, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM ); ref.err_Int.z = LIMIT(ref.err_Int.z, - IMU_INTEGRAL_LIM ,IMU_INTEGRAL_LIM ); /***************与上式构成PI控制******************/ ref.g.x = (Gyro.x - Gravity.x * yaw_correct ) *ANGLE_TO_RADIAN + ( Kp*(ref.err.x + ref.err_Int.x) ) ; ref.g.y = (Gyro.y - Gravity.y * yaw_correct ) *ANGLE_TO_RADIAN + ( Kp*(ref.err.y + ref.err_Int.y) ) ; ref.g.z = (Gyro.z - Gravity.z * yaw_correct ) *ANGLE_TO_RADIAN; 一阶龙格库塔法已知一四元数: 四元数转换为方向余弦矩阵的关键元素: /**********四元数的旋转矩阵转换***************/ static void IMU_ComputeRotationMatrix(void) { Eln_angle[0] = 2*(Nq.q0*Nq.q1 + Nq.q2*Nq.q3); Eln_angle[1] = 1 - 2*(Nq.q1*Nq.q1 + Nq.q2*Nq.q2); Eln_angle[2] = 2*(Nq.q1*Nq.q3 - Nq.q0*Nq.q2); Eln_angle[3] = 2*(-Nq.q1*Nq.q2 - Nq.q0*Nq.q3); Eln_angle[4] = 2*(Nq.q0*Nq.q0 + Nq.q1*Nq.q1) - 1; } /***********获得欧拉角**********/ void Get_Angle(void) { IMU_ComputeRotationMatrix(); //四元数转旋转矩阵 IMU_data.Pitch = asin(Eln_angle[2])*DEG_PER_RAD; IMU_data.Roll= fast_atan2(Eln_angle[0],Eln_angle[1])*DEG_PER_RAD; IMU_data.Yaw = fast_atan2(Eln_angle[3],Eln_angle[4])*DEG_PER_RAD; }最终得到横滚、俯仰和偏航角。但是需要注意的有一点,如果单独依靠陀螺仪和加速度计解算出来的姿态横滚和俯仰是没有问题的,但是航向是一直在漂移的,这个是因为解算出来就会漂移的,所以一般都使用磁力计去限航。通过之前的博客我们获取的磁力计数据。 /*磁力计限制偏航函数*/ void Mag_Limit(float DT) { static xyz_t Mag; //计算磁力计向量的模 Mag_norm_xyz = my_sqrt(my_pow(WHY.MAGX) + my_pow(WHY.MAGY) + my_pow(WHY.MAGZ)); if( Mag_norm_xyz != 0) { Mag.x += 20 *(6.28f *DT) *( (float)WHY.MAGX /( Mag_norm_xyz ) - Mag.x); Mag.y += 20 *(6.28f *DT) *( (float)WHY.MAGY /( Mag_norm_xyz ) - Mag.y); Mag.z += 20 *(6.28f *DT) *( (float)WHY.MAGZ /( Mag_norm_xyz ) - Mag.z); } mag_trans(&Gravity,&Mag,&Mag_process); Mag_norm = my_sqrt(Mag_process.x * Mag_process.x + Mag_process.y *Mag_process.y); if( Mag_process.x != 0 && Mag_process.y != 0 && Mag_process.z != 0 && Mag_norm != 0) { yaw_mag = fast_atan2( ( Mag_process.y/Mag_norm ) , ( Mag_process.x/Mag_norm) ) * DEG_PER_RAD; } yaw_correct = Kp *0.8f *To_180_degrees(yaw_mag - IMU_data.Yaw); }有很多的上位机可以通过你传输的欧拉角去看此时机器人的姿态,比如匿名的上位机,有能力的也可以自己写一个上位机,按照通信协议将所需显示的数据在PC端进行显示。 姿态解算C代码 |
CopyRight 2018-2019 实验室设备网 版权所有 |