我是新手，请各位大侠解答呀！
在网上找到了一篇关于mpu6050的姿态解算的**，**中姿态结算的原理是利用加计测量的实际重力加速度分量和利用陀螺仪解算出的重力加速度分量做矢量叉积pi控制，融合成一个四元数，然后进行解算。这里的重要一点是加计的测量值。。
里面的一段程序如下：void MPU6050_Dataanl(void){MPU6050_ACC_LAST.X=((((int16_t)mpu6050_buffer[0]) << 8) | mpu6050_buffer[1]) - ACC_OFFSET.X; MPU6050_ACC_LAST.Y=((((int16_t)mpu6050_buffer[2]) << 8) | mpu6050_buffer[3]) - ACC_OFFSET.Y; MPU6050_ACC_LAST.Z=((((int16_t)mpu6050_buffer[4]) << 8) | mpu6050_buffer[5]) - ACC_OFFSET.Z; MPU6050_GYRO_LAST.X=((((int16_t)mpu6050_buffer[8]) << 8) | mpu6050_buffer[9]) - GYRO_OFFSET.X; MPU6050_GYRO_LAST.Y=((((int16_t)mpu6050_buffer[10]) << 8) | mpu6050_buffer[11]) - GYRO_OFFSET.Y; MPU6050_GYRO_LAST.Z=((((int16_t)mpu6050_buffer[12]) << 8) | mpu6050_buffer[13]) - GYRO_OFFSET.Z;问题是在这里的ACC_OFFSET.x代表的是静止时的重力加速度值吧？读到的原始ad值减去它不就会把重力加速度的分量效果抵消了吗？那后面的解算程序中带入的加计测量的重力分量还是实际的重力分量吗？？这样解算带来的误差能接受吗？求各位大侠解答，小弟不胜感激！！！！这是解算程序//=====================================================================================================// IMU.c// S.O.H. Madgwick// 25th September 2010//=====================================================================================================// Description://// Quaternion implementation of the 'DCM filter' [Mayhony et al].//// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.//// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated// orientation. See my report for an overview of the use of quaternions in this application.//// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer// units are irrelevant as the vector is normalised.////=====================================================================================================//----------------------------------------------------------------------------------------------------// Header files#include "IMU.h"#include //----------------------------------------------------------------------------------------------------// Definitions#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases#define halfT 0.5f // half the sample period//---------------------------------------------------------------------------------------------------// Variable definitionsfloat q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientationfloat exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error//====================================================================================================// Function//====================================================================================================void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; // normalise the measurements norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; 把加计的三维向量转成单位向量。 // estimated direction of gravity vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以这里的vx\y\z，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的重力单位向量。 // error is sum of cross product between reference direction of field and direction measured by sensor ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx);axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。 // integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt;用叉积误差来做PI修正陀螺零偏 // integrate quaternion rate and normalise 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; 四元数微分方程 // normalise quaternion norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm;四元数规范化}
|
楼主
标题置顶
标题高亮
