上一篇介绍了加速度计陀螺仪LSM6DS3 这次介绍姿态融合算法 在介绍算法时，先了解几个概念 Yaw为航向角，表示机头偏离正北方多少度，范围-180到+180; Pitch为俯仰角，表示机头正方向与水平线的夹角，范围-90到+90; Roll为翻滚角，表示机翼与水平线的夹角，范围：-180到+180; 并没有与 XYZ轴之间关联，这是需要人为的确定 常用的 Roll滚动角，绕x轴转动 Pitch 俯仰角，绕y轴转动 Yaw偏航角，绕z轴转 同时注意要满足右手定则 LSM6DS3 是满足的右手定则 但还要注意PCB的安装与实际的问题，如果不满足就在输入时旋转坐标轴，旋转后一定要满足右手定则 其算法如下 我认为这算法包含以下几部分
1: #define Kp 2.0f // 比例增益支配率收敛到加速度计/磁强计 2: #define Ki 0.005f // 积分增益支配率的陀螺仪偏见的衔接3: #define halfT 0.5f // 采样周期的一半4: float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向5: float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差6: float gx=0,gy=0,gz=0,ax=0,ay=0,az=0; //全局变量7: //互补滤波8: //时间常数 t=a/(1-a)*dt a=t/(t+dt) t 截至频率 dt计算间隔时间9: #define kfa 0.9810: #define kfan 1.0-kfa11: //ang= kfa*ang+kfgn*acc;12: #define kfg 0.8013: #define kfgn 1.0-kfg14: float Yaw,Pitch,Rool; //偏航角，俯仰角，翻滚角15: //ang= kfa*g+kfgn*acc;16://====================================================================================================17: // Function18://====================================================================================================19: //gx 采样时间弧度增量20: //加数度为3轴采样数据，做归一化21: //输出为4元数22: void IMUupdate(float gxi, float gyi, float gzi, float axi, float ayi, float azi) {23: float norm;24: float vx, vy, vz;25: float ex, ey, ez;26:27: float q0q0 = q0*q0;28: float q0q1 = q0*q1;29: float q0q2 = q0*q2;30: float q0q3 = q0*q3;31: float q1q1 = q1*q1;32: float q1q2 = q1*q2;33: float q1q3 = q1*q3;34: float q2q2 = q2*q2;35: float q2q3 = q2*q3;36: float q3q3 = q3*q3;37: //增加互补滤波38: ax=ax*kfa+kfan*axi;39: ay=ay*kfa+kfan*ayi;40: az=az*kfa+kfan*azi;41: gx=gx*kfg+kfgn*gxi;42: gy=gy*kfg+kfgn*gyi;43: gz=gz*kfg+kfgn*gzi;44: // 测量正常化45: norm = sqrt(ax*ax + ay*ay + az*az);46: ax = ax / norm;47: ay = ay / norm;48: az = az / norm;49:50: // 估计方向的重力51: vx = 2*(q1*q3 - q0*q2);52: vy = 2*(q0*q1 + q2*q3);53: vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;54:55: // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和56: ex = (ay*vz - az*vy);57: ey = (az*vx - ax*vz);58: ez = (ax*vy - ay*vx);59:60: // 积分误差比例积分增益61: exInt = exInt + ex*Ki;62: eyInt = eyInt + ey*Ki;63: ezInt = ezInt + ez*Ki;64:65: // 调整后的陀螺仪测量66: gx = gx + Kp*ex + exInt;67: gy = gy + Kp*ey + eyInt;68: gz = gz + Kp*ez + ezInt;69:70: // 整合四元数率和正常化71: q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;72: q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;73: q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;74: q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;75:76: // 正常化四元77: norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);78: q0 = q0 / norm;79: q1 = q1 / norm;80: q2 = q2 / norm;81: q3 = q3 / norm;82: Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch83: Rool = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv84: }
| 
楼主
标题置顶
标题高亮
