|
使用四元数进行姿态解算 先定义Kp，Ki，以及halfT。 Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度。 halfT，姿态解算时间的一半。此处解算姿态的速度是200Hz，因此halfT为0.0025 #define Kp 2.0f #define Ki 0.002f #define halfT 0.0025f 初始化四元数 float q0=1,q1=0,q2=0,q3=0; 定义姿态解算误差的积分 float exInt=0,eyInt=0,ezInt=0; 以下为姿态解算函数 参数gx，gy，gz分别对应三个轴的角速度，单位为弧度/秒。 参数ax，ay，az分别对应三个轴的加速度原始数据。 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; 将加速度的原始数据，归一化，得到单位加速度 Norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; 把四元数换算成“方向余弦矩阵”中第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx,vy,vz，其实是当前的机体坐标参考系，换算出来的重力单位向量。(用表示机体姿态的四元数进行换算) vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; 这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是分散的，长时间的积分出现漂移的情况，因此需要用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。 在机体坐标系下，加速度计测出来的重力向量是ax，ay，az，陀螺仪积分后的姿态来推算出的重力向量是vx，vy，vz，它们之间的误差向量，就是陀螺仪积分后的姿态和加速度计测出的姿态之间的误差。 向量间的误差，可以用向量积(叉积)来表示，ex，ey，ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，二陀螺仪积分误差也是机体坐标系，而且叉积的大小与陀螺仪积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。 向量积，数学中又称外积、叉积，物理中称矢积、叉乘，是一种在向量空间中向量的二元运算。与点积不同，它的运算结果是一个向量而不是一个标量。并且两个向量的叉积与这两个向量的和垂直。 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; 用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度 gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; 四元数微分方程，就是把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。 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; 四元数单位化 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; } 姿态解算后，就得到了表示姿态的四元数。但四元数不够直观，一般将其转化为欧拉角。转化时根据旋转的次序不同，公式也不同。
| 
STMCU
5038 人阅读
|
3 人回复
|
2015-07-27
/3 
