使用LSM6DS0得出的数据如何进行姿态解算？求助！

zhen8838 · 发表于 2018-4-22 13:57:44

我手头有一块st官方的板子，X-NUCLEO-IKS01A1，板载的LSM6DS0,6轴传感器，我把这个板子的驱动程序移植到了F767上面，可以正确读取6轴数据了。我将得到数据乘上了官方给的灵敏度值，官方手册如下：红色的是我选择的量程。

TIM截图20180422133812.png

以下是我得到数据（已经乘上了敏感度）：我知道现在加速度数据的单位是mg 陀螺仪数据的单位是 mdps。

ACC_X[-22.143], ACC_Y[-4.514], ACC_Z[984.662]
GYR_X[490.000], GYR_Y[-4900.000], GYR_Z[-490.000]
ACC_X[-19.825], ACC_Y[-2.989], ACC_Z[985.394]
GYR_X[490.000], GYR_Y[-4900.000], GYR_Z[-560.000]
ACC_X[-21.594], ACC_Y[-4.087], ACC_Z[983.564]
GYR_X[280.000], GYR_Y[-4900.000], GYR_Z[-770.000]
ACC_X[-20.740], ACC_Y[-3.477], ACC_Z[983.564]
GYR_X[490.000], GYR_Y[-4970.000], GYR_Z[-630.000]
ACC_X[-23.546], ACC_Y[-5.978], ACC_Z[984.357]
GYR_X[350.000], GYR_Y[-5110.000], GYR_Z[-630.000]
ACC_X[-22.021], ACC_Y[-1.708], ACC_Z[984.296]
GYR_X[420.000], GYR_Y[-4690.000], GYR_Z[-630.000]

我现在想用匿名的姿态解算函数：
[mw_shl_code=cpp,true]//四元数更新姿态
void ANO_IMU:

uaternion_CF(Vector3f gyro,Vector3f acc, float deltaT)
{
Vector3f V_gravity, V_error, V_error_I;

//重力加速度归一化
acc.normalize();

//提取四元数的等效余弦矩阵中的重力分量
Q.vector_gravity(V_gravity);

//向量叉积得出姿态误差
V_error = acc % V_gravity;

//对误差进行积分
V_error_I += V_error * Ki;

//互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移
Gyro += V_error * Kp + V_error_I;

//一阶龙格库塔法更新四元数
Q.Runge_Kutta_1st(Gyro, deltaT);

//四元数归一化
Q.normalize();

//四元数转欧拉角
Q.to_euler(&angle.x, &angle.y, &angle.z);
}[/mw_shl_code]

我把我角度值除了1000然后再转换成了弧度值，，加速度值没有做处理，这样传入不能计算吗？
下面是6050姿态解算是板子上传入的数据：
ACC_X[1.51714635], ACC_Y[4.13073063], ACC_Z[4123.26318]
GYR_X[-0.00212844973], GYR_Y[-0.00106422487], GYR_Z[-0.00851379894]
ACC_X[1.57104981], ACC_Y[4.45104218], ACC_Z[4124.9082]
GYR_X[-0.0031926746], GYR_Y[-0.00212844973], GYR_Z[-0.00957802311]

数据的单位不一样无法计算吗？求助大家了！