精华内容
下载资源
问答
  • 四元数姿态解算

    2015-07-31 17:32:37
    四元数姿态解算的一些推导过程,以及C语言解算代码
  • AHRS四元数姿态解算

    2017-02-15 13:54:38
    AHRS四元数姿态解算
  • BMX055九轴姿态传感器模块 包含调试通过的K60 KEA128硬件软件IIC 四元数姿态解算程序,包含调试上位机以及说明文档。做恩智浦和飞思卡尔平衡车的可以参考。大自然的搬运工。
  • 最近结合惯性导航这本书,详细看了四元数姿态解算的代码,然后对这部分代码进行了详细的分析,分享给大家,如果分析有误请大家留言不吝赐教!!

    代码路径ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

    最近结合惯性导航这本书,详细看了四元数姿态解算的代码,然后对这部分代码进行了详细的分析,分享给大家,如果分析有误请大家留言不吝赐教!!

    1. void AHRS_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {  
    2.     float recipNorm;  
    3.     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;    
    4.     float hx, hy, bx, bz; /*地理坐标系下的磁强度值*/
    5.     float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;/*根据陀螺仪更新的四元数估计出的机体坐标系下的加速度值和磁传感器值*/
    6.     float halfex, halfey, halfez;  /*根据实际测得的加速度值和磁传感器值和估计得出的上述值叉乘得到的估计误差*/
    7.     float qa, qb, qc;  /*四元数*/
    8.   
    9.     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)  
    10.     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {  
    11.         MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);  /*如果测量的磁传感器值为0,则只需要用加速度值对陀螺数据进行补偿*/
    12.         return;  
    13.     }  
    14.   
    15.     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)  
    16.     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {  
    17.   
    18.         /* 对重力加速度进行归一化*/
    19.         recipNorm = invSqrt(ax * ax + ay * ay + az * az);  
    20.         ax *= recipNorm;  
    21.         ay *= recipNorm;  
    22.         az *= recipNorm;       
    23.          /*归一化结束*/
    24.  
    25.         /* 对磁传感器进行归一化*/
    26.         recipNorm = invSqrt(mx * mx + my * my + mz * mz);  
    27.         mx *= recipNorm;  
    28.         my *= recipNorm;  
    29.         mz *= recipNorm;     
    30.          /*归一化结束*/
    31.  
    32.       /*提前进行四元数的相关运算,为后面的矩阵旋转做准备,更便于与推导矩阵风格统一
    33.         q0q0 = q0 * q0;  
    34.         q0q1 = q0 * q1;  
    35.         q0q2 = q0 * q2;  
    36.         q0q3 = q0 * q3;  
    37.         q1q1 = q1 * q1;  
    38.         q1q2 = q1 * q2;  
    39.         q1q3 = q1 * q3;  
    40.         q2q2 = q2 * q2;  
    41.         q2q3 = q2 * q3;  
    42.         q3q3 = q3 * q3;     
    43.   

            /*根据机体坐标系下实际测量得到磁传感器值左乘四元数旋转矩阵得到地理坐标系下的磁传感器值*/  

    现在我们假设CbR旋转矩阵是经过加速度计校正后的矩阵,当某个确定的向量(机体系中)经过这个矩阵旋转之后(到地理坐标系),这两个坐标系在XOY平面上重合,只是在z轴旋转上会存在一个偏航角的误差。下图表示的是经过CbR旋转之后的机体坐标系b和地理坐标系n的相对关系。可以明显发现加速度计可以把机体坐标系b通过四元数法从任意角度拉到与地理坐标系n水平的位置上,这时,只剩下一个偏航角误差。这也是为什么加速度计误差修正偏航的原因。

                                                  

    hx,hy,hz是地理坐标系下的磁传感器值,可以有机体坐标系下的mx,my,mz左乘CbR得到,假设理想情况下的机体能够和当地的地理坐标系处于同一XOY平面,且机头指北,那么此时的磁传感器值应该为bx,0,bz,此时我们很方便的可以得到bx²=hx²+hy²,bz=hz,当时理想必定是理想,飞机的姿态不可能达到这种状态,所以我们再根据bx,0,bz(地理坐标系)右乘CbR得到估计后的磁传感器值halfwx,halfwy,halfwz(这部分解说间黄色底色部分)

    1.         hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));  
    2.         hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));  
    3.         bx = sqrt(hx * hx + hy * hy);  
    4.         bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));  

    对于重力加速度的补偿相比于磁传感器要简单的多,我们认为理想情况下的飞机状态能够达到和当地地理坐标系XOY水平的状态,那么此时的重力加速度值应该为0,0,1(归一化后),那么根据此地理坐标系下的重力加速度值,右乘CbR即可得到此时机体坐标系下的重力加速度估计值(见红色底色部分)


    1.         // Estimated direction of gravity and magnetic field  
    2.         halfvx = q1q3 - q0q2;  
    3.         halfvy = q0q1 + q2q3;  
    4.         halfvz = q0q0 - 0.5f + q3q3;  
    5.         halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);  
    6.         halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);  
    7.         halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);    
    8.       
    9.         /*然后根据上述得到估计值和实测值做叉乘,即可得到陀螺仪的漂移误差,从而使用PI对陀螺仪的结果进行补偿(见绿色部分)*/ 
    10.         halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);  
    11.         halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);  
    12.         halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);  
    13.   
    14.         // Compute and apply integral feedback if enabled  
    15.         if(twoKi > 0.0f) {  
    16.             integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki  
    17.             integralFBy += twoKi * halfey * (1.0f / sampleFreq);  
    18.             integralFBz += twoKi * halfez * (1.0f / sampleFreq);  
    19.             gx += integralFBx;  // apply integral feedback  
    20.             gy += integralFBy;  
    21.             gz += integralFBz;  
    22.         }  
    23.         else {  
    24.             integralFBx = 0.0f; // prevent integral windup  
    25.             integralFBy = 0.0f;  
    26.             integralFBz = 0.0f;  
    27.         }  
    28.   
    29.         // Apply proportional feedback  
    30.         gx += twoKp * halfex;  
    31.         gy += twoKp * halfey;  
    32.         gz += twoKp * halfez;  
    33.     }  
    34.       /*至此我们认为得到了准确的gx,gy,gz,然后根据得到的值跟新四元数,更新的依据是四元数的一阶龙格库塔法*/

     

    1.     // Integrate rate of change of quaternion  
    2.     gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors  
    3.     gy *= (0.5f * (1.0f / sampleFreq));  
    4.     gz *= (0.5f * (1.0f / sampleFreq));  
    5.     qa = q0;  
    6.     qb = q1;  
    7.     qc = q2;  
    8.     q0 += (-qb * gx - qc * gy - q3 * gz);  
    9.     q1 += (qa * gx + qc * gz - q3 * gy);  
    10.     q2 += (qa * gy - qb * gz + q3 * gx);  
    11.     q3 += (qa * gz + qb * gy - qc * gx);   

    /*四元数更新完毕*/

    1.     // Normalise quaternion  
    2.     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);  
    3.     q0 *= recipNorm;  
    4.     q1 *= recipNorm;  
    5.     q2 *= recipNorm;  
    6.     q3 *= recipNorm; 

         /*将更新后的四元数进行归一化,并根据与余弦矩阵的关系可以得到yaw,pitch,roll角度,进而进行下,一次的数据融合 

    根据余弦矩阵和四元数旋转矩阵的关系可以得到角度关系


    roll,pitch,yaw

    展开全文
  • 三维空间中的旋转可以用四元数来表达,用四元数表达三维的旋转与使用矩阵相比具有两个优点:第一,几何意义明确;第二,计算简单.因此,四元数在数学、物理学和计算机图形学中具有很高的应用价值.本文详细叙述了四元数的...
  • 互补滤波器四元数姿态解算算法

    热门讨论 2014-05-19 10:31:52
    基于互补滤波器的姿态解算算法,用的四元数表示的姿态,最后输出欧拉角度,适合于做自平衡小车等
  • 四元数姿态解算算法基础

    千次阅读 热门讨论 2020-04-19 14:25:26
    文章目录姿态的表示方法四元数表示姿态的物理意义使用四元数进行载体姿态更新方程四元数微分方程四元数初始值确定 姿态的表示方法 载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数。 欧拉角的物理...

    姿态的表示方法

    载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数。
    欧拉角的物理意义比较直观,即航向角 ψ \psi ψ,俯仰角 θ \theta θ,横滚角 γ \gamma γ,分别是导航系到载体系的三个旋转角度。
    姿态矩阵可以由欧拉角直接计算得到,即三个角度对应的转换矩阵依相乘。

    注意的是导航系到载体系是按照航向角,俯仰角,横滚角的顺序变换的所以导航系到载体系变换时,三个矩阵相乘的顺序应该是 C ( γ ) ∗ ( θ ) ∗ C ( ψ ) C(\gamma)*(\theta)*C(\psi) C(γ)(θ)C(ψ).

    四元数表示方法较为抽象,直接从四元数“很难”看出载体姿态(这个姿态是对人的想象来说的姿态,因为四元数和欧拉角本无本质区别,只是一种表示方法),那么他的物理意义是什么呢?

    四元数表示姿态的物理意义

    四元数表示姿态时有多个写法,其中一种表示方法如下:
    q = c o s ( θ 2 ) + u n s i n ( θ 2 ) q=cos(\frac{\theta}{2})+u^nsin(\frac{\theta}{2}) q=cos(2θ)+unsin(2θ)

    欧拉角表示姿态变换时,表征的是分别绕着 ( i , j , k ) (i,j,k) (i,j,k)三个坐标轴的三次旋转。根据欧拉定理,这三次旋转可以等效成绕着某轴一次旋转而成。这个某轴就是公式中的 u n = ( u 1 , u 2 , u 3 ) u^n=(u_1,u_2,u_3) un=(u1,u2,u3),旋转角度就是公式中的 θ \theta θ
    四元数通常写为 q = q 0 + q 1 + q 2 + q 3 q=q_0+q_1+q_2+q_3 q=q0+q1+q2+q3
    那么用于姿态表示时,
    q 0 = c o s ( θ 2 ) q_0=cos(\frac{\theta}{2}) q0=cos(2θ)
    q i = u i ∗ s i n ( θ 2 ) , i = 1 , 2 , 3 q_i=u_i*sin(\frac{\theta}{2}),i=1,2,3 qi=uisin(2θ),i=1,2,3

    使用四元数进行载体姿态更新方程

    q ( t + δ t ) = q ( t ) + d q d t × δ t q(t+\delta t) = q(t)+\frac{dq}{dt} \times \delta t q(t+δt)=q(t)+dtdq×δt
    四元数的更新可通过上式完成。因此只要知道初始四元数(载体的初始姿态)和四元数的导数就可以完成任意时刻四元数的解算。
    例如px4中姿态更新的代码如下

    // Apply correction to state
    	_q += _q.derivative(corr) * dt;
    

    四元数微分方程

    d q d t \frac{dq}{dt} dtdq 需要求解四元数微分方程,基本推导过程如下:

    d q d t = − θ ˙ 2 s i n ( θ 2 ) + u n θ ˙ 2 c o s ( θ 2 ) + s i n ( θ 2 ) d u n d t \frac{dq}{dt}=-\frac{\dot{\theta}}{2}sin(\frac{\theta}{2})+u^n \frac{\dot{\theta}}{2}cos(\frac{\theta}{2})+sin(\frac{\theta}{2}) \frac{du^n}{dt} dtdq=2θ˙sin(2θ)+un2θ˙cos(2θ)+sin(2θ)dtdun
    其中, d u n d t = 0 \frac{du^n}{dt}=0 dtdun=0,因此:

    d q d t = − θ ˙ 2 s i n ( θ 2 ) + u n θ ˙ 2 c o s ( θ 2 ) \frac{dq}{dt}=-\frac{\dot{\theta}}{2}sin(\frac{\theta}{2})+u^n \frac{\dot{\theta}}{2}cos(\frac{\theta}{2}) dtdq=2θ˙sin(2θ)+un2θ˙cos(2θ)

    = u n × u n θ ˙ 2 s i n ( θ 2 ) + u n θ ˙ 2 c o s ( θ 2 ) =u^n \times u^n \frac{\dot{\theta}}{2}sin(\frac{\theta}{2})+u^n \frac{\dot{\theta}}{2}cos(\frac{\theta}{2}) =un×un2θ˙sin(2θ)+un2θ˙cos(2θ)

    = u n θ ˙ 2 × ( u n s i n ( θ 2 ) + c o s ( θ 2 ) ) =u^n \frac{\dot{\theta}}{2} \times (u^n sin(\frac{\theta}{2})+cos(\frac{\theta}{2}) ) =un2θ˙×(unsin(2θ)+cos(2θ))

    = u n θ 2 × q = ω n b n × q 2 =u^n \frac{\theta}{2} \times q =\frac{\omega_{nb}^n \times q}{2} =un2θ×q=2ωnbn×q

    结果中的角速度为导航系到机体系在导航系下的旋转角速度矢量,但是我们通过陀螺仪测得的是载体系下的角速度。因此还需要一部变换:

    ω n b n = q × ω n b b × q ∗ \omega_{nb}^n=q \times \omega_{nb}^b \times q^* ωnbn=q×ωnbb×q
    由此可得到,
    d q d t = q × ω n b b 2 \frac{dq}{dt} =q \times \frac{\omega_{nb}^b}{2} dtdq=q×2ωnbb

    我们得到了四元数导数和载体系角速度的数学关系,展开上式得 d q d t \frac{dq}{dt} dtdq
    [ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y ω x 0 ] × [ q 0 q 1 q 2 q 3 ] \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y\\ \omega_y & -\omega_z & 0 & \omega_x\\ \omega_z & \omega_y & \omega_x & 0 \end{bmatrix} \times \begin{bmatrix} q_0 \\ q_1 \\q_2\\q_3 \end{bmatrix} 0ωxωyωzωx0ωzωyωyωz0ωxωzωyωx0×q0q1q2q3
    也可以写成,
    [ q 0 − q 1 − q 2 − q 3 q 1 0 q 3 − q 2 q 2 − q 3 0 q 1 q 3 q 2 q 1 q 0 ] × [ q 0 q 1 q 2 q 3 ] \begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ q_1 & 0 & q_3 & -q_2 \\ q_2 & -q_3 & 0 & q_1 \\ q_3 & q_2 & q_1 & q_0 \end{bmatrix} \times \begin{bmatrix} q_0 \\ q_1 \\q_2\\q_3 \end{bmatrix} q0q1q2q3q10q3q2q2q30q1q3q2q1q0×q0q1q2q3

    四元数初始值确定

    px4 中的初始四元数计算。

    bool AttitudeEstimatorQ::init()
    

    四元数初始值的确实可以看做是一个初始对准的过程。
    而上述模型仅仅是理想状态下的数学模型,实际角速度中必然包含零偏、噪声等误差,更新过程中又不可避免的有飘漂移发生。所以仅用上式是无法给出可以使用的姿态的。
    实际使用中常常用kalman滤波或者补偿滤波的方法来“校正”这些误差。因此初始值可以不必那么精确。给出一个概略初始值即可。
    例如可以通过加速度z轴方向和磁罗盘的输出向量,来完成对准。两者从物理概念上实际上分别完成了调平和指北的任务。

    展开全文
  • 由本人精心原创收集整理,绝对原创!收集了大牛的精华,加之以自己的思路进行理解,认真看完觉对能对四元数有更深刻的认识!
  • 获取陀螺仪和加速度计原始数据 short gryox,gyroy,gyroz; short accx,accy,accz; void IMU_GetData(void) { MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); MPU_Get_Accelerometer(&...#define.
    1. 获取陀螺仪和加速度计原始数据
    short gryox,gyroy,gyroz;
    short accx,accy,accz;
    void IMU_GetData(void)
    {
    	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
    	MPU_Get_Accelerometer(&accx,&accy,&accz);
    
    }
    
    1. 定义变量
    #define RtA 		57.295779f	//弧度->角度			
    #define AtR    		0.0174533f	//角度->弧度
    #define Acc_G 		0.0011963f	
    #define Gyro_G 		0.0610351f				
    #define Gyro_Gr		0.0010653f	
    
    #define Kp 18.0f                        
    #define Ki 0.008f                         
    #define halfT 0.008f     
            
    float q0 = 1, q1 = 0, q2 = 0, q3 = 0;   
    float exInt = 0, eyInt = 0, ezInt = 0;  
    float Angle[3]={0};//最终角度
    
    1. 函数声明
    /*************************
    *函数名:IMU_Update
    *输入:陀螺仪,加速度计原始数据(short类型)
    *************************/
    void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);
    
    
    1. 函数实现
    void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
    {
    	float ax=accx,ay=accy,az=accz;
    	float gx=gyrox,gy=gyroy,gz=gyroz;
      	float norm;
      	float vx, vy, vz;
      	float ex, ey, ez;
      	float q0q0 = q0*q0;
      	float q0q1 = q0*q1;
      	float q0q2 = q0*q2;
      	float q1q1 = q1*q1;
      	float q1q3 = q1*q3;
      	float q2q2 = q2*q2;
      	float q2q3 = q2*q3;
      	float q3q3 = q3*q3;
    	if(ax*ay*az==0)//此时任意方向角加速度为0
     		return; 
     	gx *= Gyro_Gr;
    	gy *= Gyro_Gr;
    	gz *= Gyro_Gr;
    	
      	norm = sqrt(ax*ax + ay*ay + az*az);       
      	ax = ax /norm;
      	ay = ay / norm;
      	az = az / norm;
    
     	 // estimated direction of gravity and flux (v and w)            
      	vx = 2*(q1q3 - q0q2);												
      	vy = 2*(q0q1 + q2q3);
      	vz = q0q0 - q1q1 - q2q2 + q3q3 ;
    
      	// error is sum of cross product between reference direction of fields and direction 		measured by sensors
      	ex = (ay*vz - az*vy) ;                           					
      	ey = (az*vx - ax*vz) ;
      	ez = (ax*vy - ay*vx) ;
    
      	exInt = exInt + ex * Ki;								  
      	eyInt = eyInt + ey * Ki;
      	ezInt = ezInt + ez * Ki;
    
      // adjusted gyroscope measurements
      	gx = gx + Kp*ex + exInt;					   							
      	gy = gy + Kp*ey + eyInt;
      	gz = gz + Kp*ez + ezInt;				   							
    
      	// integrate quaternion rate and normalise						   
      	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
      	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
      	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
      	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
    
      	// normalise quaternion
      	norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      	
      	q0 = q0 / norm;
      	q1 = q1 / norm;
      	q2 = q2 / norm;
      	q3 = q3 / norm;
      	
    	//	angle->yaw  = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; 			// yaw
      	ANgle[0]	= asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitch
      	ANgle[1]	= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA;  	//roll
    	
    
    }
    
    
    
    1. IMU.h
    //IMU.h
    #ifndef _IMU_H
    #define _IMU_H
    /*************************
    *函数名:IMU_Update
    *输入:陀螺仪,加速度计原始数据(short类型)
    *************************/
    void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz);
    
    #endif
    
    1. IMU.c
    
    //IMU.c
    #include "IMU.h"
    short gryox,gyroy,gyroz;
    short accx,accy,accz;
    void IMU_GetData(void)
    {
    	MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
    	MPU_Get_Accelerometer(&accx,&accy,&accz);
    
    }
    
    #define RtA 		57.295779f	//弧度->角度			
    #define AtR    		0.0174533f	//角度->弧度
    #define Acc_G 		0.0011963f	
    #define Gyro_G 		0.0610351f				
    #define Gyro_Gr		0.0010653f	
    
    #define Kp 18.0f                        
    #define Ki 0.008f                         
    #define halfT 0.008f     
            
    float q0 = 1, q1 = 0, q2 = 0, q3 = 0;   
    float exInt = 0, eyInt = 0, ezInt = 0;  
    float Angle[3]={0};//最终角度
    
    
    void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
    {
    	float ax=accx,ay=accy,az=accz;
    	float gx=gyrox,gy=gyroy,gz=gyroz;
      	float norm;
      	float vx, vy, vz;
      	float ex, ey, ez;
      	float q0q0 = q0*q0;
      	float q0q1 = q0*q1;
      	float q0q2 = q0*q2;
      	float q1q1 = q1*q1;
      	float q1q3 = q1*q3;
      	float q2q2 = q2*q2;
      	float q2q3 = q2*q3;
      	float q3q3 = q3*q3;
    	if(ax*ay*az==0)//此时任意方向角加速度为0
     		return; 
     	gx *= Gyro_Gr;
    	gy *= Gyro_Gr;
    	gz *= Gyro_Gr;
    	
      	norm = sqrt(ax*ax + ay*ay + az*az);       
      	ax = ax /norm;
      	ay = ay / norm;
      	az = az / norm;
    
     	 // estimated direction of gravity and flux (v and w)            
      	vx = 2*(q1q3 - q0q2);												
      	vy = 2*(q0q1 + q2q3);
      	vz = q0q0 - q1q1 - q2q2 + q3q3 ;
    
      	// error is sum of cross product between reference direction of fields and direction 		measured by sensors
      	ex = (ay*vz - az*vy) ;                           					
      	ey = (az*vx - ax*vz) ;
      	ez = (ax*vy - ay*vx) ;
    
      	exInt = exInt + ex * Ki;								  
      	eyInt = eyInt + ey * Ki;
      	ezInt = ezInt + ez * Ki;
    
      // adjusted gyroscope measurements
      	gx = gx + Kp*ex + exInt;					   							
      	gy = gy + Kp*ey + eyInt;
      	gz = gz + Kp*ez + ezInt;				   							
    
      	// integrate quaternion rate and normalise						   
      	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
      	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
      	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
      	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
    
      	// normalise quaternion
      	norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      	
      	q0 = q0 / norm;
      	q1 = q1 / norm;
      	q2 = q2 / norm;
      	q3 = q3 / norm;
      	
    	//	angle->yaw  = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; 			// yaw
      	ANgle[0]	= asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitch
      	ANgle[1]	= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA;  	//roll
    	
    
    }
    
    

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    展开全文
  • 这一段是网上比较流行的四元数解算程序 shuju=load('data.txt') ax=shuju(:,1)/32768*16*9.8 ay=shuju(:,2)/32768*16*9.8 az=shuju(:,3)/32768*16*9.8 gx=shuju(:,4)/32768*2000 gy=shuju(:,5)/32768*2000 gz...
  • 四元数姿态解算基础及数学模型

    万次阅读 2017-12-18 15:40:40
    姿态的表示方法载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数欧拉角的物理意义比较直观,即航向角,俯仰角,横滚角,分别是导航系到载体系的三个旋转角度姿态矩阵可以由欧拉角直接计算得到,即三个...

    本文已搬迁到此处

     

     

     

     

     

     

     

     

     

     

     

     

     


    姿态的表示方法

    载体姿态有多种表示方法,常见的三种:欧拉角,姿态矩阵,四元数
    欧拉角的物理意义比较直观,即航向角,俯仰角,横滚角,分别是导航系到载体系的三个旋转角度
    姿态矩阵可以由欧拉角直接计算得到,即三个角度对应的转换矩阵相乘(注意顺序,导航系到载体系是按照航向角,俯仰角,横滚角的顺序变换的,所以导航系到载体系变换时,三个矩阵相乘的顺序,应该是横滚*俯仰*航向*导航系坐标矢量),因此结果较之欧拉角稍微抽象一点点
    四元数则是一个说来话长的姿态表示方法,直接从四元数“很难”看出载体姿态(这个姿态是对人的想象来说的姿态,因为四元数和欧拉角本无本质区别,只是一种表示方法),那么他的物理意义是什么呢?

    四元数表示姿态的物理意义

    四元数表示姿态时有多个写法,如下:
     
     
    欧拉角表示姿态变换时,表征的是分别绕着i,j,k三个坐标轴的三次旋转。根据欧拉定理,这三次旋转可以等效成绕着某轴一次旋转而成。这个某轴就是公式中的u=(u1,u2,u3),旋转角度就是公式中的θ。
    数学中四元数通常写为
    那么用于姿态表示时q0=cos( θ/2),qi=ui*sin(θ/2),i=1,2,3

    使用四元数进行载体姿态更新方程

     
    四元数的更新可通过上式完成。因此只要知道初始四元数(载体的初始姿态)和四元数的导数就可以完成任意时刻四元数的解算

    四元数求导

     
    结果中的角速度为导航系到机体系在导航系下的旋转角速度矢量,但是我们通过陀螺仪测得的是载体系下的角速度。不过通过下边的变换:
     
    我们得到了四元数导数和载体系角速度的数学关系,展开上式:
     

    四元数初始值确定

    四元数初始值的确实可以看做是一个初始对准的过程。
    而上述模型仅仅是理想状态下的数学模型,实际角速度中必然包含零偏等误差,更新过程中又不可避免的有飘漂移发生。所以仅适用上式是无法给出可以使用的姿态的。
    实际使用中常常用kalman滤波或者补偿滤波的方法来“校正”这些误差。因此初始值可以不必那么精确。给出一个概略初始值即可。
    例如可以通过加速度z轴方向和磁罗盘的输出向量,来完成对准。两者从物理概念上实际上分别完成了调平和指北的任务。
     
     
     
    展开全文
  • 四元数姿态解算中的地磁计融合解读

    万次阅读 多人点赞 2014-03-23 14:55:12
    我们的姿态解算求的是四元数,我们是通过修正旋转矩阵中的四元数来达到姿态解算的目的,而不要以为通过加速度计和地磁计来修正姿态,加速度计和地磁计只是测量工具和载体,通过这两个器件表征旋转矩阵的误差存在,...
  • 通过四元素解算姿态,对四元数进行解算,处理相应的四元数
  • 姿态解算(四)四元数 - 姿态解算步骤

    万次阅读 多人点赞 2016-11-20 22:40:21
    四元数姿态解算在前面我们介绍了旋转的表示方法:http://blog.csdn.net/qq_27114397/article/details/53164389 以及,四元数的定义与几何意义:http://blog.csdn.net/qq_27114397/article/details/53165289  ...
  • 四元数AHRS姿态解算和IMU姿态解算分析 AHRS是自动航向基准系统(Automatic Heading Reference System)的简称。目前,使用四元数来进行AHRS姿态解算的算法被广泛采用于四轴飞行器上。该算法源自英国Bristol大学的Ph.D ...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,017
精华内容 406
关键字:

四元数姿态解算