四轴飞行器姿态融合算法详解

原标题：四轴飞行器姿态融合算法详解

·概念

1.1，加速度计，测量的是"比力"。

根据牛顿惯性定律，当载体相对惯性空间以加速度a运动时，可以用载体中的加速度计测出作用在单位质量上惯性力和引力的矢量和的大小，即比力的大小。

（比力）MSF = ma – mg

式中 ：

SF ——加速度计检测质量受到的比力；

m ——感受加速度的检测质量；

a ——载体的运动加速度；

g ——地球的引力加速度。

加速度计测量的是机体坐标系(b系)相对于惯性空间的加速度在机体坐标系中的投影，该测试量也称为比力。而对于我们四轴姿态计算，要在机体坐标系中完成，测出的加速度就是机体上的，只要取出重力分量。重力分量在世界坐标系中是【0,0,1】.这一转换由姿态矩阵完成，而是利用陀螺仪的输出即载体相对惯性空间转动的角速率在机体坐标系下的投影计算得到。姿态矩阵是随时间的变化而不断变化的。另外，从姿态矩阵中可以单值的确定飞行器的姿态角。捷联式惯导系统中需要实时地求取姿态矩阵，以便提取飞行器姿态角（航向角、横滚角、俯仰角）以及变换比力。机体上测得的重力正好和g对应。

1.2姿态到底是什么

姿态角实际上就是机体坐标系OXbYbZb 相对于 地理坐标系OXtYtZt之间的方位关系.用矩阵表达。也可用四元数表达。四元数是“轴-角”描述。yaw-pitch-roll 3次转动，合并为绕通过顶点"O"的向量n转过θ/2，其中含有不可交换误差。

2.互补滤波实现代码

函数里的积分时间，误差都用了half，因为外层算法中融合是在1/2周期时做。

void sensfusion6UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float dt)

{

float recipNorm;

float halfvx, halfvy, halfvz;

float halfex, halfey, halfez;

float qa, qb, qc;

//角度转换为弧度

gx = gx * M_PI / 180;

gy = gy * M_PI / 180;

gz = gz * M_PI / 180;

//如果加速度计处于自由落体状态，可能会出现这种情况，不进行姿态解算，因为会产生分母无穷大的情况

if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))

{

// 单位化加速度计,意义在于在变更了加速度计的量程之后不需要修改Kp参数,因为这里归一化了

recipNorm = invSqrt(ax * ax + ay * ay + az * az);

ax *= recipNorm;

ay *= recipNorm;

az *= recipNorm;

//将当前姿态的重力在三个轴上的分量分离出来

//就是方向余弦旋转矩阵的第三列,注意是地理坐标系(n系)到载体坐标系(b系)的,不要弄反了.如果书上是b系到n系,转置即可

书上都是从b系机体坐标转移到世界坐标，而我们需要把重力向量转移到机体坐标，所以要求逆矩阵

而又因为是标准正交矩阵，逆等于转置，得到下面的矩阵。

halfvx = q1 * q3 - q0 * q2;

halfvy = q0 * q1 + q2 * q3;

//1==q0^2+q1^2+q3^2+q4^2; 做一个代数运算 目的是减少计算量

halfvz = q0 * q0 + q3 * q3- 0.5f；

// Error is sum of cross product between estimated and measured direction of gravity

//叉乘，得到重力误差 忽略不可交换误差

halfex = (ay * halfvz - az * halfvy);

halfey = (az * halfvx - ax * halfvz);

halfez = (ax * halfvy - ay * halfvx);

//使用PI来纠正误差 叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。

//特殊的互补滤波

// Compute and apply integral feedback if enabled

if(twoKi > 0.0f)

{

integralFBx += twoKi * halfex * dt; // integral error scaled by Ki

integralFBy += twoKi * halfey * dt;

integralFBz += twoKi * halfez * dt;

gx += integralFBx; // apply integral feedback

gy += integralFBy;

gz += integralFBz;

}

else

{

integralFBx = 0.0f; // prevent integral windup

integralFBy = 0.0f;

integralFBz = 0.0f;

}

// Apply proportional feedback

gx += twoKp * halfex;

gy += twoKp * halfey;

gz += twoKp * halfez;

}

//到此就得到了融合过的角速度，纠正了陀螺的零偏，但是没有磁场纠正

// Integrate rate of change of quaternion

//陀螺仪直接积分得到角度,直接测得的角速度乘以变化时间得到角度,这里提前积分了

gx *= (0.5f * dt); // pre-multiply common factors

gy *= (0.5f * dt);

gz *= (0.5f * dt);//现在gx gy gz 表示了旋转角度

qa = q0;

qb = q1;

qc = q2;

//数值积分得到角度，用于解微分方程

q0 += (-qb * gx - qc * gy - q3 * gz);

q1 += (qa * gx + qc * gz - q3 * gy);

q2 += (qa * gy - qb * gz + q3 * gx);

q3 += (qa * gz + qb * gy - qc * gx);

// Normalise quaternion

recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

q0 *= recipNorm;

q1 *= recipNorm;

q2 *= recipNorm;

q3 *= recipNorm;

}

四元数构造方向余弦矩阵的推导：

关于微分方程的说明

【q0,q1,q2,q3】是旧的姿态矩阵，【gx,gy,gz】是融合过的当前角速度，要求出当前的新的【q0,q1,q2,q3】

如用方向余弦矩阵, 微分方程为

C^ = C U（角度向量)

如果用四元数，微分方程为

q^ = 1/2 w （角度向量的四元数表示）*（四元数乘法） q(旧)

设在t时刻动系OXbYbZb以角速度ω相对定系OXtYtZt转动，

则动系b和定系t之间的变换四元数Q同样是时间的函数，

可用Q(t)表示t时刻的四元数，Q(t+Δt)表示t+Δt时刻的四元数，如图3.3所示。

从图3.3可以看出，Q(t)和ΔQ(t)之间和为Q(t+Δt),故有：

Q(t+Δt)=ΔQ(t)Q(t)

图：3.3

如果您觉得文章对您有帮助，请分享给您的朋友

分享知识，分享快乐

关注前沿科技一点通：了解更多前沿科技返回搜狐，查看更多

责任编辑：