初级会员
- 积分
- 60
- 金钱
- 60
- 注册时间
- 2019-4-2
- 在线时间
- 33 小时
|
发表于 2019-4-4 08:26:51
|
显示全部楼层
本帖最后由 openhui 于 2019-4-4 08:43 编辑
非常感谢楼主 可以成功的读出九个轴的数据了 有几点疑问麻烦还请指教一下
1.看到您说的姿态解算的流程，我在对数据 进行了均值滤波，去除一下噪声。在MATLAB上看下曲线变化，当变化的幅度不大时候，还是可以取得不错的效果，但是变化幅度大了后，有时候会把数据峰值直接减少很多。您是用的那种滤波方法呢？
中间那块 就是动作幅度很快
2.加速度数据的计算
mpu_value.Accel[1]/=164;
这个164是怎么得来的呢？如果量程选择是16g 是不是 分辨率等于32768/16？
但是 我用mpu_value.Accel[1]/=31768/16; 得出的加速度有感觉不是很对.....
3.最重要的一点
我试着用Mahony融合算法 对换算后数据 进行了下姿态解算
得到的三个角度度数的值 特别小
下面是某一组角度值
roll=0.109
pitch=0.063
yaw=-0.165
劳烦您可以看一下吗 非常 非常感谢
[mw_shl_code=c,true]#include "MahonyAHRS.h"
#include "math.h"
float Angle[3];
//-------------------------------------------------------------------------------------------
// Definitions
#define sampleFreq 125.0f // sample frequency in Hz
#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
//============================================================================================
// Functions
//-------------------------------------------------------------------------------------------
// AHRS algorithm update
//Mahony::Mahony() {
float invSampleFreq = 1.0f/sampleFreq;
float twoKp = twoKpDef;
float twoKi = twoKiDef;
float q0 = 1.0f;
float q1 = 0.0f;
float q2 = 0.0f;
float q3 = 0.0f;
float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
float integralFBx = 0.0f;
float integralFBy = 0.0f;
float integralFBz = 0.0f;
// anglesComputed = 0;
//}
void Mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
Mahony_updateIMU(gx, gy, gz, ax, ay, az);
return;
}
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * invSampleFreq; // integral error scaled by Ki
integralFBy += twoKi * halfey * invSampleFreq;
integralFBz += twoKi * halfez * invSampleFreq;
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 * invSampleFreq); // pre-multiply common factors
gy *= (0.5f * invSampleFreq);
gz *= (0.5f * invSampleFreq);
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;
Mahony_computeAngles(q0,q1,q2,q3);
}
//-------------------------------------------------------------------------------------------
// IMU algorithm update
void Mahony_updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Convert gyroscope degrees/sec to radians/sec 将陀螺仪 度数/秒转换为弧度/秒
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 仅当加速度计测量有效时才计算反馈（避免加速度计归一化中的NaN）
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az); // invSqrt(value) =1.0/sqrt(value) 归一化
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux 垂直于磁通量的重力和矢量的估计方向 这里有疑问 用四元数表示的旋转矩阵和重力【 0 0 1】相乘后
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
//halfvz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
// 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);
// Compute and apply integral feedback if enabled 计算并应用积分反馈（如果启用）
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * invSampleFreq; // integral error scaled by Ki 用ki表示的积分误差
integralFBy += twoKi * halfey * invSampleFreq;
integralFBz += twoKi * halfez * invSampleFreq;
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 * invSampleFreq); // pre-multiply common factors 预乘公因式
gy *= (0.5f * invSampleFreq);
gz *= (0.5f * invSampleFreq);
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;
Mahony_computeAngles(q0,q1,q2,q3);
//anglesComputed = 1;
}
//-------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
//-------------------------------------------------------------------------------------------
void Mahony_computeAngles(float q0,float q1,float q2,float q3)
{
roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*57.29578f;
pitch = asinf(-2.0f * (q1*q3 - q0*q2))*57.29578f;
yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3)*57.29578f;
//anglesComputed = 1;
Angle[0]=roll;
Angle[1]=pitch;
Angle[2]=yaw;
}
[/mw_shl_code] |
|
[复制链接]
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
楼主
233366
