有大神可以帮忙看看姿态解算的程序吗？延迟太高了

周亚龙

//以下源码为移植无穷四轴的姿态解算代码。分别为头文件与.c文件，原始数据读取我是直接读取的，但是数据融合的结果很不好，延迟很高，竖直90度，基本上要5秒钟才能稳定，仔细看了Mahony的算法介绍，我猜测是由于角速度融合的结果不正确，由于加速度一直在矫正累加所以产生了延迟很高的情况，把加速度矫正那一部分去掉，果然只用角速度得到的结果完全不对，仔细看所有的参数，没有发现我的角速度的积分参数错误（角速度积分这一块的算法我不太懂），mpu6050读取也正常。有大神可以帮忙看看问题到底出在哪里了吗？或者哪里有可能会有错误？小弟在这里谢谢了

/*********BlackHole1 For Study**************
*        文件名                                        :                           ahrs.c
*        描述                                        :   航姿数据处理文件
*        版本                                        :        BlackHole1_For_Study_V1
*        作者                                        :                 【INF】无穷创新               
*        技术论坛                                :                 www.infcre.com
*        QQ群                                        :             168590475
*  源码状态                                :         开源
*                        For infinite creativity
*********(C) COPYRIGHT 2015 INF ************/

#include         "ahrs.h"
#include         "stdio.h"
#include  "math.h"   
#include         "mpu6050.h"
#include  "Control.h"
#include  "oled.h"
typedef struct{        s16 X;        s16 Y;        s16 Z;}S_INT16_XYZ;
typedef struct
{
        s32 X;
        s32 Y;
        s32 Z;
}S_INT32_XYZ;

extern S_INT16_XYZ MPU6050_Gyro;//原始加速度值
extern S_INT16_XYZ MPU6050_Acc; //角速度

//全局变量

volatile S_INT16_XYZ Acc_Avg;                         //滑动滤波后加速度计值
volatile S_FLOAT_XYZ Acc_Angle;    //加速度计计算出的角度
volatile S_FLOAT_XYZ Now_Angle;        //数据融合计算出的角度
volatile S_INT16_XYZ Acc_Buf[FILTER_LENGTH] = {0};

/*
* 函数名：Acc_Flitter
* 描述  ：加速度计预滤波
*                     递推最小二乘估计/滑动滤波
*                        1：无需存储全部数据，取得一组观测数据便可以估计一次参数
*                                且都能够在一个采样周期内完成，计算量小，占用存储空间小
*                        2：具有一定的实时处理能力
* 输入  ：无
* 输出  ：无
*/
void        Acc_Flitter()
{
        static u8 filter_cnt        =        0;
        static u8 cnt;
        S_INT32_XYZ        Temp;
       
        Temp.X = 0;
        Temp.Y = 0;
        Temp.Z = 0;
       
        Acc_Buf[filter_cnt].X = MPU6050_Acc.X;
        Acc_Buf[filter_cnt].Y        = MPU6050_Acc.Y;
        Acc_Buf[filter_cnt].Z = MPU6050_Acc.Z;
       
        filter_cnt++;
       
        for(cnt        =        0;cnt <        FILTER_LENGTH;cnt++)
        {
                Temp.X += Acc_Buf[cnt].X;
                Temp.Y += Acc_Buf[cnt].Y;
                Temp.Z += Acc_Buf[cnt].Z;
        }
        Acc_Avg.X = Temp.X / FILTER_LENGTH;
        Acc_Avg.Y = Temp.Y / FILTER_LENGTH;
        Acc_Avg.Z = Temp.Z / FILTER_LENGTH;
       
        if(filter_cnt        ==        FILTER_LENGTH)        filter_cnt = 0;
}

/*
* 函数名：Prepare_Data
* 描述  ：读取MPU6500原始数据；加速度计预滤波
* 输入  ：Now_Angle：当前姿态；Exp_Angle：期望姿态
* 输出  ：无
*/
void Prepare_Data(void)
{
        MPU6050_ReadValue(); //读取MPU6500原始值
        Acc_Flitter();                          //加速度计预滤波
}

float ex_int = 0, ey_int = 0, ez_int = 0;   //X、Y、Z轴的比例误差
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    //定义四元素
float q0_yaw = 1, q1_yaw = 0, q2_yaw = 0, q3_yaw = 0;    //弥补Mahony算法在无地磁情况解算Yaw轴满足不了大扰动要求的现象

/*
* 函数名：IMU_Update
* 描述  ：姿态数据更新
* 输入  ：无
* 输出  ：无
*/
void IMU_Update(void)
{
  float norm;       
        float gx = MPU6050_Gyro.X        *        GYRO_GR,gy = MPU6050_Gyro.Y        *        GYRO_GR,gz = MPU6050_Gyro.Z        *        GYRO_GR;
        float ax = Acc_Avg.X,ay = Acc_Avg.Y,az = Acc_Avg.Z;
       
        float q0q0 = q0 * q0;
  float q0q1 = q0 * q1;
  float q0q2 = q0 * q2;
  float q1q1 = q1 * q1;
  float q1q3 = q1 * q3;
  float q2q2 = q2        * q2;
  float q2q3 = q2        *        q3;
  float q3q3 = q3        *        q3;       
        float vx, vy, vz;
  float ex, ey, ez;
       
        float        q0_yawq0_yaw = q0_yaw * q0_yaw;
        float        q1_yawq1_yaw = q1_yaw * q1_yaw;
        float        q2_yawq2_yaw = q2_yaw * q2_yaw;
        float        q3_yawq3_yaw = q3_yaw * q3_yaw;
        float        q1_yawq2_yaw = q1_yaw * q2_yaw;
        float        q0_yawq3_yaw = q0_yaw * q3_yaw;
       
//**************************Yaw轴计算******************************
       
        //Yaw轴四元素的微分方程
  q0_yaw = q0_yaw + (-q1_yaw * gx - q2_yaw * gy - q3_yaw * gz) * SAMPLE_HALF_T;
  q1_yaw = q1_yaw + (q0_yaw * gx + q2_yaw * gz - q3_yaw * gy) * SAMPLE_HALF_T;
  q2_yaw = q2_yaw + (q0_yaw * gy - q1_yaw * gz + q3_yaw * gx) * SAMPLE_HALF_T;
  q3_yaw = q3_yaw + (q0_yaw * gz + q1_yaw * gy - q2_yaw * gx) * SAMPLE_HALF_T;
       
        //规范化Yaw轴四元数
  norm = sqrt(q0_yawq0_yaw + q1_yawq1_yaw + q2_yawq2_yaw + q3_yawq3_yaw);
  q0_yaw = q0_yaw / norm;
  q1_yaw = q1_yaw / norm;
  q2_yaw = q2_yaw / norm;
  q3_yaw = q3_yaw / norm;
       

        if(ax * ay * az        == 0)
        return ;
       
        //规范化加速度计值
  norm = sqrt(ax * ax + ay * ay + az * az);
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;
       
        //估计重力方向和流量/变迁
  vx = 2 * (q1q3 - q0q2);                                                                                       
  vy = 2 * (q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;
       
  //向量外积再相减得到差分就是误差
  ex = (ay * vz - az * vy) ;      
  ey = (az * vx - ax * vz) ;
  ez = (ax * vy - ay * vx) ;

        //对误差进行PI计算
  ex_int = ex_int + ex * IMU_KI;                       
  ey_int = ey_int + ey * IMU_KI;
  ez_int = ez_int + ez * IMU_KI;

  //校正陀螺仪
  gx = gx + IMU_KP * ex + ex_int;                                       
  gy = gy + IMU_KP * ey + ey_int;
  gz = gz + IMU_KP * ez + ez_int;
       
               
                       
        //四元素的微分方程
  q0 = q0 + (-q1 * gx - q2        *        gy - q3        *        gz)        *        SAMPLE_HALF_T;
  q1 = q1 + (q0        *        gx + q2        *        gz - q3        *        gy)        *        SAMPLE_HALF_T;
  q2 = q2 + (q0        *        gy - q1        *        gz + q3        *        gx)        *        SAMPLE_HALF_T;
  q3 = q3 + (q0        *        gz + q1        *        gy - q2        *        gx)        *        SAMPLE_HALF_T;

  //规范化Pitch、Roll轴四元数
  norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;
       
        //求解欧拉角
        Now_Angle.X = atan2(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * 57.3f;
        Now_Angle.Y = asin(-2 * q1q3 + 2 * q0q2) * 57.3f;
        Now_Angle.Z = atan2(2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw, -2 * q2_yawq2_yaw - 2 * q3_yawq3_yaw + 1)        * 57.3f;

}

extern S_FLOAT_XYZ Exp_Angle;

/*
* 函数名：AHRS_Date_Init
* 描述  ：航姿数据初始化
* 输入  ：无
* 输出  ：无
*/
void AHRS_Date_Init(void)
{       
        int cnt;
       
        for(cnt        =        0;cnt        <        FILTER_LENGTH;cnt++)
        {
                Acc_Buf[cnt].X = 0;
                Acc_Buf[cnt].Y = 0;
                Acc_Buf[cnt].Z = 0;
        }
         
        q0 = 1;
        q1 = 0;
        q2 = 0;
        q3 = 0;
       
        q0_yaw = 1;
        q1_yaw = 0;
        q2_yaw = 0;
        q3_yaw = 0;
       
        Now_Angle.X = 0;
        Now_Angle.Y = 0;
        Now_Angle.Z = 0;
       
        Exp_Angle.X = 0;
        Exp_Angle.Y = 0;
        Exp_Angle.Z = 0;
}
#ifndef __AHRS_H
#define __AHRS_H

#include "stm32f10x.h"

#define SAMPLE_HALF_T 0.0005f     //采样周期的一半，单位：s
#define FILTER_LENGTH 20                                        //滑动滤波窗口长度

#define GYRO_G         0.0610351f                                //角速度变成度/S   此参数对应陀螺2000度每秒  Gyro_G=1/16.375=0.0610687
#define GYRO_GR        0.0010653f                                //角速度变成弧度/S        此参数对应陀螺2000度每秒

#define IMU_KP 10//1.5f                                             //比例
#define IMU_KI  0.008//0.0005f                                                 //积分

void Geographic_Coordinate_ReadValue(void);
void Prepare_Data(void);
void IMU_Update(void);
void AHRS_Date_Init(void);

#endif

正点原子

帮顶

周亚龙

姿态解算已经把我弄炸了，回头要仔细研究DMP了