|
|
本帖最后由 h2o0312 于 2014-5-11 20:18 编辑
导师给了个课题，要用MPU6050和arduino做路径跟踪。在论坛里潜水了一段时间，现在能得到更新四元数、方向余弦矩阵。通过互补滤波能得到还算稳定的Pitch和Roll角。现有如下几个问题：
一、Yaw角是不是必须要磁力计辅助才能得到稳定结果？用四元数算出的Yaw漂的厉害（1-2秒就跳1度，在-180到+180之间来回跳。）
二、重力在加速度三轴的分量滤除和载体坐标系到导航坐标系的转换的思路是不是：重力先转换到载体坐标系，然后载体坐标系测得的加速度减去转换后的重力，最后将去除重力分量的加速度再转换到导航坐标系？还有，下面图片里的转换方式有没问题？
三、因为要做路径跟踪，需要对加速度二次积分到位移。尝试过直接对加速度数据一次积分，可结果分分钟z轴数据就不知道漂哪去了。各位帮忙看看是我的积分程序有问题还是需要其他处理？参考其他大神的程序都是进行互补滤波或卡尔曼滤波，我要得到稳定的加速度积分可以如何处理？
四、用于积分的dt是下面程序中这样取得吗？
本人小白问题有点多，希望各位大神能指点一二
感谢@Lance热心回复。下午反复测试程序修正了些坐标变换程序里的错误，现在发现积分结果xyz任意轴只要不是和重力轴垂直，数据都会不断变大，但是垂直于竖直方向的数据就基本没多大变化（静止时），就像测试结果中xy平面水平时，vx和vy基本没什么变化，但是vz就疯跑。。。考虑可能是重力影响，那么我的重力分量滤除方法不对咯？
- void loop()
- {
- /* Update all the values */
- while (i2cRead(0x3B, i2cData, 14));
- accX = ((i2cData[0] << 8) | i2cData[1]) ;// + 2000.0
- accY = ((i2cData[2] << 8) | i2cData[3]) ;// + 3319.84
- accZ = ((i2cData[4] << 8) | i2cData[5]) + acc_offset_Z;// + 664.48
- // tempRaw = (i2cData[6] << 8) | i2cData[7];
- gyroX = ((i2cData[8] << 8) | i2cData[9]) + gyro_offset_X;// +332.0;
- gyroY = ((i2cData[10] << 8) | i2cData[11]) + gyro_offset_Y;// +96.0;
- gyroZ = ((i2cData[12] << 8) | i2cData[13]) + gyro_offset_Z;// +147.0;
- double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
- timer = micros();
- ………………
- //********************************************************************
- // Function:四元数更新算法
- //********************************************************************
- void IMUupdate(double gx, double gy, double gz, double ax, double ay, double az)
- {
- double norm;
- double vx, vy, vz;
- double ex, ey, ez;
-
- norm = sqrt(ax*ax + ay*ay + az*az);
- ax = ax / norm;
- ay = ay / norm;
- az = az / norm; // acc数据归一化
- vx = 2*(q1*q3 - q0*q2);
- vy = 2*(q0*q1 + q2*q3);
- vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // estimated direction of gravity
- ex = (ay*vz - az*vy);
- ey = (az*vx - ax*vz);
- ez = (ax*vy - ay*vx); // error is sum of cross product between reference direction of field and direction measured by sensor
- exInt = exInt + ex * Ki;
- eyInt = eyInt + ey * Ki;
- ezInt = ezInt + ez * Ki; // integral error scaled integral gain
- gx = gx + Kp * ex + exInt;
- gy = gy + Kp * ey + eyInt;
- gz = gz + Kp * ez + ezInt; // adjusted gyroscope measurements
- 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; // integrate quaternion rate and normalise
- norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
- q0 = q0 / norm;
- q1 = q1 / norm;
- q2 = q2 / norm;
- q3 = q3 / norm; // normalise quaternion
-
- //更新方向余弦矩阵
- double t11 = q0*q0+q1*q1-q2*q2-q3*q3;
- double t12 = 2.0*(q1*q2-q0*q3);
- double t13 = 2.0*(q1*q3+q0*q2);
- double t21 = 2.0*(q1*q2+q0*q3);
- double t22 = q0*q0-q1*q1+q2*q2-q3*q3;
- double t23 = 2.0*(q2*q3-q0*q1);
- double t31 = 2.0*(q1*q3-q0*q2);
- double t32 = 2.0*(q2*q3+q0*q1);
- double t33 = q0*q0-q1*q1-q2*q2+q3*q3;
- //求出欧拉角
- roll = asin(t32) * RAD_TO_DEG;//俯仰角，绕x轴转动
- pitch = -atan2(t31,t33) * RAD_TO_DEG;//横滚角，绕y轴转动
- yaw = atan2(t12,t22) * RAD_TO_DEG;//偏航角，绕z轴转动
-
- }
- //********************************************************************
- // Function: 消除重力加速度影响
- //********************************************************************
- void Sub_g(double aa, double bb, double cc )
- {
- int i, j, k;
- double s[3][1] = {0,0,0};
- double a[3][3] = {t11, t21, t31, t12, t22, t32, t13, t23, t33};
- double b[3][1] = {0, 0, g};
-
- for (i = 0; i < 3; i++)
- {
- for (j = 0; j < 1; j++)
- {
- for (k = 0; k < 3; k++)
- {
- s[i][j] += a[i][k] * b[k][j];
- }
- }
- }
- for ( int l = 0; l < 3; l++)
- {
- c[l][0] = s[l][0];
- }
- aa -= c[0][0];
- bb -= c[1][0];
- cc -= c[2][0];
- }
- //********************************************************************
- // Function: 坐标变换，acc数值由b系变换至n系
- //********************************************************************
- void acc_convert(double x, double y, double z)
- {
- double w[3][1] = {0,0,0};
- double f[3][1];
- double d[3][3] = {t11, t12, t13, t21, t22, t23, t31, t32, t33};
- double e[3][1] = {x, y, z};
-
- for (int i = 0; i < 3; i++)
- {
- for (int j = 0; j < 1; j++)
- {
- for (int k = 0 ; k < 3; k++)
- {
- w[i][j] += d[i][k] * e[k][j];
- }
- }
- }
- for ( int l = 0; l < 3; l++)
- {
- f[l][0] = w[l][0];
- }
- x = f[0][0];
- y = f[1][0];
- z = f[2][0];
- }
- //********************************************************************
- // Function: 加速度一重积分
- // V(n) = V(n-1) + 0.5 * (a(n) + a(n-1)) * dt
- //********************************************************************
- void acc_to_vel(int n)
- {
- double result[3][1] = {vel_x, vel_y, vel_z}; //计算结果Vn
- double previous[3][1]; //前一结果Vn-1
- double al[3][1] = {init_ax/16384.0, init_ay/16384.0, init_az/16384.0}; //当前结果a(n)
- double ap[3][1] = {0,0,0}; //前一结果a(n-1)
- while (n >= 1)
- {
- n--;
- for (int i = 0; i < 3; i++)
- {
- previous[i][0] = result[i][0];
- }
- for (int j = 0; j < 3; j++)
- {
- result[j][0] = previous[j][0] + 0.5 * (al[j][0] + ap[j][0]) * dt; //V(n)=V(n-1)+0.5*(a(n)+a(n-1))*dt
- }
- for (int k = 0; k < 3; k++)
- {
- ap[k][0] = al[k][0];
- }
- }
- vel_x = result[0][0];
- vel_y = result[1][0];
- vel_z = result[2][0];
- velocity = sqrt(vel_x * vel_x + vel_y * vel_y + vel_z * vel_z);
- Serial.print("vx =");
- Serial.print(vel_x);Serial.print("\t");
- Serial.print("vy =");
- Serial.print(vel_y);Serial.print("\t");
- Serial.print("vz =");
- Serial.print(vel_z);Serial.print("\t");
- Serial.print("velocity =");
- Serial.print(velocity);Serial.print("\t");
- Serial.print("\t");
- }
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有帐号？注册
x
|
发表于 2014-5-11 12:12:26
楼主
