` 本帖最后由 dvd1478 于 2016-3-1 22:45 编辑
【CANNON试用体验】传感器——LSM6DS3 加速度计 陀螺仪
https://bbs.elecfans.com/jishu_550022_1_1.html
(出处: 中国电子技术论坛)
上一篇介绍了加速度计陀螺仪LSM6DS3 这次介绍姿态融合算法
在介绍算法时，先了解几个概念
Yaw为航向角，表示机头偏离正北方多少度，范围-180到+180;
Pitch为俯仰角，表示机头正方向与水平线的夹角，范围-90到+90;
Roll为翻滚角，表示机翼与水平线的夹角，范围：-180到+180;
并没有与 XYZ轴之间关联，这是需要人为的确定
常用的
Roll滚动角，绕x轴转动
Pitch 俯仰角，绕y轴转动
Yaw偏航角，绕z轴转
同时注意要满足右手定则
LSM6DS3 是满足的右手定则
但还要注意 PCB的安装与实际的问题，如果不满足就在输入时旋转坐标轴，旋转后一定要满足右手定则
其算法如下
我认为这算法包含以下几部分
四元数与欧拉角之间的转换、 互被滤波（低通）与卡尔曼滤波
- #define Kp 2.0f // 比例增益支配率收敛到加速度计/磁强计
- #define Ki 0.005f // 积分增益支配率的陀螺仪偏见的衔接
- #define halfT 0.5f // 采样周期的一半
- float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
- float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
- float gx=0,gy=0,gz=0,ax=0,ay=0,az=0; //全局变量
- //互补滤波
- //时间常数 t=a/(1-a)*dt a=t/(t+dt) t 截至频率 dt计算间隔时间
- #define kfa 0.98
- #define kfan 1.0-kfa
- //ang= kfa*ang+kfgn*acc;
- #define kfg 0.80
- #define kfgn 1.0-kfg
- float Yaw,Pitch,Rool; //偏航角，俯仰角，翻滚角
- //ang= kfa*g+kfgn*acc;
- //====================================================================================================
- // Function
- //====================================================================================================
- //gx 采样时间弧度增量
- //加数度为3轴采样数据，做归一化
- //输出为4元数
- void IMUupdate(float gxi, float gyi, float gzi, float axi, float ayi, float azi) {
- float norm;
- float vx, vy, vz;
- float ex, ey, ez;
-
- float q0q0 = q0*q0;
- float q0q1 = q0*q1;
- float q0q2 = q0*q2;
- float q0q3 = q0*q3;
- float q1q1 = q1*q1;
- float q1q2 = q1*q2;
- float q1q3 = q1*q3;
- float q2q2 = q2*q2;
- float q2q3 = q2*q3;
- float q3q3 = q3*q3;
- //增加互补滤波
- ax=ax*kfa+kfan*axi;
- ay=ay*kfa+kfan*ayi;
- az=az*kfa+kfan*azi;
- gx=gx*kfg+kfgn*gxi;
- gy=gy*kfg+kfgn*gyi;
- gz=gz*kfg+kfgn*gzi;
- // 测量正常化
- norm = sqrt(ax*ax + ay*ay + az*az);
- ax = ax / norm;
- ay = ay / norm;
- az = az / norm;
-
- // 估计方向的重力
- vx = 2*(q1*q3 - q0*q2);
- vy = 2*(q0*q1 + q2*q3);
- vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
-
- // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
- 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;
-
- // 调整后的陀螺仪测量
- 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;
- Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
- Rool = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
- }
Yaw 角度的求取还有些问题，不过一般情况下用roll pitch的多
原地址http://blog.sina.com.cn/s/blog_7e7fa4c80102wal9.html
`
|
电子发烧友论坛
2
/6 
工商网监
湘ICP备2023018690号
8767
淘帖
11
赞
回复
显身卡
