IMU学习记录

目录

储备知识:

1. 坐标系基本知识:

2. 四元数与欧拉角的转换:

 3. 欧拉角表示的坐标变换矩阵:

 4. 四元数表示的坐标变换矩阵:

 5. 欧拉角和四元数表示的坐标变换之间的转换关系:

IMU6轴融合算法

IMU9轴融合算法

卡尔曼滤波


储备知识

1. 坐标系基本知识:

(参考文章:https://blog.csdn.net/weixin_43032593/article/details/103877536?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166847655916782390513060%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166847655916782390513060&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-2-103877536-null-null.142^v63^control,201^v3^add_ask,213^v2^t3_control1&utm_term=%E5%AF%BC%E8%88%AA%E5%9D%90%E6%A0%87%E5%92%8C%E8%BD%BD%E4%BD%93%E5%9D%90%E6%A0%87%E7%B3%BB&spm=1018.2226.3001.4187icon-default.png?t=M85Bhttps://blog.csdn.net/weixin_43032593/article/details/103877536?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166847655916782390513060%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166847655916782390513060&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~rank_v31_ecpm-2-103877536-null-null.142^v63^control,201^v3^add_ask,213^v2^t3_control1&utm_term=%E5%AF%BC%E8%88%AA%E5%9D%90%E6%A0%87%E5%92%8C%E8%BD%BD%E4%BD%93%E5%9D%90%E6%A0%87%E7%B3%BB&spm=1018.2226.3001.4187

图中,红色坐标系为惯性坐标系(i系):原点是地球中心,x与y轴在地球赤道平面内相互垂直,分别指向相应的恒星,z轴是地球的自转轴。

绿色坐标系为地球坐标系(e系):原点是地球中心,x与y在地球赤道平面内相互垂直,z值指向本初子午线。e系与地球固连,随着地球自转相对于i系旋转。

蓝色坐标系为地理坐标系(g系):又称为当地水平坐标系,通常使用的是“东北天”坐标系和“北东地”坐标系。“东北天”坐标系的原点是站心(一般初始位置),x轴指向东方,y轴指向北方,z指向是铅锤方向向天。横滚角,俯仰角,偏航角均定义在地理坐标系下。

在这里插入图片描述

载体坐标系(b系):载体坐标系与载体固连,坐标原点是载体中心,x轴沿载体横轴向右,y轴沿载体纵轴向前,z轴沿载体立轴向上。

在这里插入图片描述

 导航坐标系(n系):在惯导和组合导航中,导航坐标系通常选用地理坐标系,例如“东北天”坐标系。

2. 四元数与欧拉角的转换:

欧拉角转四元数:

struct Quaternion
{
    double w,x,y,z;
}q;

//欧拉角转四元数
void Euler_to_Quaternion(double yaw,double pitch,double roll)
{
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;
}

四元数转欧拉角:

其中φ为roll角度  θ为pitch角度 \psi为yaw角度

#define M_PI 3.141592f
struct Quaternion
{
    double w,x,y,z;
};

struct EulerAngles
{
    double roll,pitch,yaw;
}angles;

void Quaternion_to_Euler(struct Quaternion q)
{
    //roll
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = atan2(sinr_cosp, cosr_cosp);
 
    // pitch
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (fabs(sinp) >= 1)
        angles.pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = asin(sinp);
 
    // yaw 
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = atan2(siny_cosp, cosy_cosp);

}

 3. 欧拉角表示的坐标变换矩阵:

b系转到n系:

 4. 四元数表示的坐标变换矩阵:

b系转到n系:

 5. 欧拉角和四元数表示的坐标变换之间的转换关系:

欧拉角表示四元数的公式:

 四元数表示欧拉角的公式:

 

了解了上述基础知识,就来看IMU如何获取角度的吧!!!!

IMU6轴融合算法:

基础理论和原理在这里不再赘述,不懂的友友可以看这篇文章https://blog.csdn.net/shao15232/article/details/101777722icon-default.png?t=M85Bhttps://blog.csdn.net/shao15232/article/details/101777722代码部分:

#define  Kp  10.0f   //PI控制器的比例系数
#define  Ki  0.008f  //PI控制器的积分系数
#define halfT  0.001f //采样周期的一半

float q0=1,q1=1,q2=0,q3=0;  //四元数初始化
float exlnt=0,eylnt=0,eylnt = 0;  //向量积误差初始化

struct Q_angle
{
    float roll;
    float pitch;
    float yaw;
}; 

void IMUupdate(float gx,float gy,float gz,float ax,float ay,float az)
{
    float norm;
    float vx,vy,vz;
    float ex,ey,ez;

    float q0q0 = q0*q0;
    float q0q1 = q0*q1;
    float q0q2 = q0*q2;
    float q0q3 = q0*q3;
    float q1q1 = q1*q1;
    float q1q2 = q1*q2;
    float q1q3 = q1*q3;
    float q2q2 = q2*q2;
    float q2q3 = q2*q3;
    float q3q3 = q3*q3;

    if(ax*ay*az ==0)
        return;
    
    norm = sqrt(ax*ax+ay*ay+az*az);
    
    //b系的加速度值单位化
    ax = ax/norm;
    ay = ay/norm;
    az = az/norm;

    //n系中的g理论输出转至b系的值
    vx = 2*(q1q3-q0q2);
    vy = 2*(q0q1+q2q3);
    vz = q0q0-q1q1-q2q2+q3q3;

    //将n系中的g理论输出转至b系的值和b系中的加速度计值进行向量叉乘 得到向量积误差
    ex = (ay*vz - az*vy) ;                                          
    ey = (az*vx - ax*vz) ;
    ez = (ax*vy - ay*vx) ;

    //对误差进行积分
    exlnt = exlnt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    //计算负补偿后b系中陀螺仪的输出
    gx = gx + Kp*ex + exlnt;
    gy = gy + Kp*ey + eylnt;
    gz = gz + Kp*ez + ezlnt;

    //四元素的微分方程
    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;

    //四元数转欧拉角(角度制)
    Q_ANGLE.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
    Q_ANGLE.pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
    Q_ANGLE.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

}

IMU9轴融合算法:

基础理论和原理在这里不再赘述,不懂的友友可以看这篇文章https://blog.csdn.net/shao15232/article/details/101777819?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166847909316800180687821%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=166847909316800180687821&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~first_rank_ecpm_v1~rank_v31_ecpm-4-101777819-null-null.nonecase&utm_term=MPU&spm=1018.2226.3001.4450icon-default.png?t=M85Bhttps://blog.csdn.net/shao15232/article/details/101777819?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166847909316800180687821%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=166847909316800180687821&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~first_rank_ecpm_v1~rank_v31_ecpm-4-101777819-null-null.nonecase&utm_term=MPU&spm=1018.2226.3001.4450

#define  Kp  10.0f   //PI控制器的比例系数
#define  Ki  0.008f  //PI控制器的积分系数
#define halfT  0.001f //采样周期的一半

float q0=1,q1=1,q2=0,q3=0;  //四元数初始化
float exlnt=0,eylnt=0,eylnt = 0;  //向量积误差初始化

struct Q_angle
{
    float roll;
    float pitch;
    float yaw;
}; 

void IMUupdate(float gx,float gy,float gz,float ax,float ay,float az,float mx,float my,float mz)
{
    float norm;
    float hx,hy,hz,bx,bz;
    float wx,wy,wz;
    float vx,vy,vz;
    float ex,ey,ez;

    float q0q0 = q0*q0;
    float q0q1 = q0*q1;
    float q0q2 = q0*q2;
    float q0q3 = q0*q3;
    float q1q1 = q1*q1;
    float q1q2 = q1*q2;
    float q1q3 = q1*q3;
    float q2q2 = q2*q2;
    float q2q3 = q2*q3;
    float q3q3 = q3*q3;

    if(ax*ay*az ==0)
        return;
    
    if(mx*my*mz==0)
        return;

    //b系的加速度值单位化
    norm = sqrt(ax*ax+ay*ay+az*az);
    ax = ax/norm;
    ay = ay/norm;
    az = az/norm;

    //b系磁力计数据归一化
    norm = sqrt(mx*mx+my*my+mz*mz);
    mx = mx/norm;
    my = my/norm;
    mz = mz/norm;

    //计算b系转到n系的磁力计数据
    hx = 2 * mx * (0.5 - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2); 
    hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5 - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1); 
    hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 -q2q2);       

    //将原始n系的磁力计数据转换到新的n系 其中x轴指向正北方向
    bx = sqrt((hx*hx)+(hy*hy));
    by = 0;
    bz = hz;

    //将新的n系下的磁力计数据转化为b系的数据
    wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2); 
    wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3); 
    wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);


    //n系中的重力加速度理论输出转至b系的值
    vx = 2*(q1q3-q0q2);
    vy = 2*(q0q1+q2q3);
    vz = q0q0-q1q1-q2q2+q3q3;

    //将n系中的g理论输出转至b系的值和b系中的加速度计值进行向量叉乘 得到向量积误差
    //将最新的b系的数据叉乘原始b系中磁力计数据
    //误差由上面两部分组成
    ex = (ay*vz - az*vy) + (my*wz - mz*wy); 
    ey = (az*vx - ax*vz) + (mz*wx - mx*wz); 
    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

    //对误差进行积分
    exlnt = exlnt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    //计算负补偿后b系中陀螺仪的输出
    gx = gx + Kp*ex + exlnt;
    gy = gy + Kp*ey + eylnt;
    gz = gz + Kp*ez + ezlnt;

    //四元素的微分方程
    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;

    //四元数转欧拉角(角度制)
    Q_ANGLE.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
    Q_ANGLE.pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
    Q_ANGLE.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

}

 

卡尔曼滤波:

原理不再赘述,不懂的友友请看:

https://blog.csdn.net/u010720661/article/details/63253509?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166834740016800184197243%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=166834740016800184197243&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-63253509-null-null.142^v63^control,201^v3^add_ask,213^v2^t3_control1&utm_term=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2&spm=1018.2226.3001.4187icon-default.png?t=M85Bhttps://blog.csdn.net/u010720661/article/details/63253509?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166834740016800184197243%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=166834740016800184197243&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-63253509-null-null.142^v63^control,201^v3^add_ask,213^v2^t3_control1&utm_term=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2&spm=1018.2226.3001.4187

/*  Q_angle 加速度计的过程噪声方差
    Q_bias  陀螺仪漂移的过程噪声方差
    R_measure  测量噪声的方差

    angle  通过卡尔曼滤波器计算出来的角度 是2*1矩阵的一部分
    bias   通过卡尔曼滤波器计算出来的陀螺仪角速度偏差 是2*1矩阵的一部分

    P[2][2]  误差协方差矩阵 是个2*2的矩阵
*/

float Q_angle = 0.001f;
float Q_bias = 0.003f;
float R_measure = 0.03f;

float angle = 0.0f;
float bias = 0.0f;

float P[2][2];

P[0][0] = 0.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.0f;

float Kalmen_getAngle(float newAngle,float newRate,float dt)
{
    //角速度 = 当前陀螺仪测得的角速度-陀螺仪角速度偏差量
    rate = newRate - bias;
    //角度 += 时间*角速度
    angle += dt * rate;

    //计算当前误差协方差矩阵 
    /*  P[0][0]     P[0][1]
        P[1][0]     P[1][1]
    */
    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;

    //计算残差的协方差
    float S = P[0][0] + R_measure;

    //计算卡尔曼增益
    float K[2];
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    //计算残差
    float y = newAngle - angle;
    
    //进行迭代预测
    angle += K[0] * y;
    bias += K[1] * y;

    //计算后验估计对应的误差协方差矩阵
    float P00_temp = P[0][0];
    float P01_temp = P[0][1];

    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;

    return angle;
}

 

你可能感兴趣的:(四足机器人,算法,人工智能)