本帖最后由 qiao--- 于 2024-3-31 19:41 编辑
前言：上期我们通过移植lsm6dsl的驱动成功读取到了该传感器的原始数据，说是原始数据，其实也不很准确，其实是经过简单处理后的数据，但是还不是加速度。那么本期测评就对这个数据进行处理，算出这个板子的6轴加速度，并进行姿态解算。
我们上期的代码是通过FROM_FS_4g_TO_mg宏定义进行简单处理的，可能大家对这个函数不怎么了解，接下来我就先简单的介绍一下这个函数；我们跳转到这个宏定义代码如下
这个宏定义将原始数据根据我们选择的范围不同进行不同的灵敏度的相乘，这个其实是在数据手册中给出的，但是这个其实我们也可以用手算出来。我们以4G的范围为例子，看看是怎么算出的122。我们读出来的数据是两个字节，范围就是-32768到32768，那我们要算出加速度的话，就应该进行比例兑换，读出来的数据和32768的比例等于实际加速度和4G的比例，算式如下所示：
在这个宏定义中，lsb就是我们读出的寄存器数据，算式中g为10m每秒方，而我们要算加速度的值，则加速度为lsb*40/32768，算出来的结果为lsb*0.0012207，约定于lsb*0.00122，这就是122的由来。但是在代码宏定义中为lsb*0.122，还不能完全转换为m/s2为单位的加速度，我们还需除以100，因此我们在外面的代码中对这个数据再次除以100
代码如下所示
float xl_X,xl_Y,xl_Z;
lsm6dsl_acceleration_raw(&lsm6dsl_ctx, &data_raw);
acceleration_mg.i32bit[0] = FROM_FS_4g_TO_mg( data_raw.i16bit[0] );
acceleration_mg.i32bit[1] = FROM_FS_4g_TO_mg( data_raw.i16bit[1] );
acceleration_mg.i32bit[2] = FROM_FS_4g_TO_mg( data_raw.i16bit[2] );
xl_X = acceleration_mg.i32bit[0] / 100.0;
xl_Y = acceleration_mg.i32bit[1] / 100.0;
xl_Z = acceleration_mg.i32bit[2] / 100.0;
sprintf((char*)USBbuffer, "XL:%.3f\t %.3f\t %.3f\r\n", xl_X,xl_Y, xl_Z);
printf("%s",USBbuffer);
接下来我们看看加速度和实际加速度书否相仿
IMG_8817
可以看到在我变换板子的方向的时候，由于重力的影响，xyz方向总有一个方向的数值为10m/s2左右，所以测出来还是挺精准的。
接下来试试角加速度，我们用上面同样的方法进行计算，只是这次要除以的是1000，至于怎么出来的大家可以按照我上面的方法自行分析。
代码如下：
float gy_X,gy_Y,gy_Z;
lsm6dsl_angular_rate_raw(&lsm6dsl_ctx, &data_raw);
angular_rate_mdps.i32bit[0] = FROM_FS_2000dps_TO_mdps( data_raw.i16bit[0] );
angular_rate_mdps.i32bit[1] = FROM_FS_2000dps_TO_mdps( data_raw.i16bit[1] );
angular_rate_mdps.i32bit[2] = FROM_FS_2000dps_TO_mdps( data_raw.i16bit[2] );
gy_X = angular_rate_mdps.i32bit[0] / 1000.0;
gy_Y = angular_rate_mdps.i32bit[1] / 1000.0;
gy_Z = angular_rate_mdps.i32bit[2] / 1000.0;
sprintf((char*)USBbuffer, "GY:%lf\t%lf\t%lf\r\n", gy_X,gy_Y, gy_Z);
printf("%s",USBbuffer);
我们来测试一下角速度是否和实际相仿
IMG_8818
可以看到，在我板子不动的时候三个方向的角加速度基本为0，当我晃动板子的时候，三个方向的角加速度就会突然变大，这个还是和实际相仿的。
6轴加速度既然都算出来了，接下来就让我们来进行姿态解算吧，我对姿态解算的原理不是很懂，就单纯的吧算法移植过来了，效果其实还行。
姿态解算其实就是算出来板子的俯仰角，偏航角和滚转角。这三个角可以用一个图来很形象的解释，如下
而这个算法的过程由于楼主能力有限就不进行解释了，有兴趣的可以去搜索一下相关的文章。
我从网上找的一个算法如下所示
代码如下：
#define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角
//加速度单位g，陀螺仪rad/s
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 = 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 ,转换为度数
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //偏移太大，等我找一个好用的
}
这个算法唯一的缺点就是偏航角的偏移太大，所以偏移角算出的不是很准，但是俯仰角和滚转角还是很准的。所以这里我就不展示偏航角的数据了。
演示效果如下所示：
IMG_8821
可以看到我在xy轴中将一个轴保持不变，晃动另外一个轴，则板子的俯仰角和滚转角总有一个在变化，另一个保持不变。
总结：通过本期测评我成功的算出了板子的6轴加速度，并根据加速度成功的对板子进行了姿态解算，从解算结果来说效果其实还可以。
参考文章：六轴传感器—姿态检测（MPU6050、LSM6DS3）_六轴传感器rad/s转换成°/s-CSDN博客
提升卡
变色卡
千斤顶
