前段时间搞了个平衡车,涉及stm32F3 步进电机驱动 陀螺仪mpu3050 加速度计adxl345(也可以用6轴mpu6050) 无线NRF24L01
当初最大问题是卡尔曼滤波(进行陀螺仪与加速度计的数据融合)和pid调节
对于卡尔曼滤波,经过自己不断深究,其实也不是很复杂,核心是五大公式,涉及矩阵运算,思想是预测值 最优估计值 噪声 协方差的概念,难点:一些参数选择
说下用卡尔曼滤波的出发点,陀螺仪 加速度计都可以得到角度,而陀螺仪是先得到角速度再经积分才得到角度, 陀螺仪相比加速度计短时间内动态性能好,得到角度精准,但本身有小漂移,随着时间变长,不断积分,误差会越来越大,那就需要用加速度计进行校正
对于pid算法,里面涉及二级pid,首先要明白小车速度跟给步进电机的频率是成正比的,就把频率等效为速度
第一个pid,角度pid,通过测角度反馈给stm32f3产生频率(速度)来进行平衡调节(即调节角度)
第二个pid,速度pid,由于角度调节产生了速度变化,而为了不改变设定的速度,需要进行速度调节,它的反馈来自不断角度pid的结果(由于速度跟频率成正比,不需要测速反馈)
难点:pid整定参数
代码资料(完美)见下
balance car nrf24l01程序 完美.zip(6.43 MB, 下载次数: 242)
当初最大问题是卡尔曼滤波(进行陀螺仪与加速度计的数据融合)和pid调节
对于卡尔曼滤波,经过自己不断深究,其实也不是很复杂,核心是五大公式,涉及矩阵运算,思想是预测值 最优估计值 噪声 协方差的概念,难点:一些参数选择
说下用卡尔曼滤波的出发点,陀螺仪 加速度计都可以得到角度,而陀螺仪是先得到角速度再经积分才得到角度, 陀螺仪相比加速度计短时间内动态性能好,得到角度精准,但本身有小漂移,随着时间变长,不断积分,误差会越来越大,那就需要用加速度计进行校正
对于pid算法,里面涉及二级pid,首先要明白小车速度跟给步进电机的频率是成正比的,就把频率等效为速度
第一个pid,角度pid,通过测角度反馈给stm32f3产生频率(速度)来进行平衡调节(即调节角度)
第二个pid,速度pid,由于角度调节产生了速度变化,而为了不改变设定的速度,需要进行速度调节,它的反馈来自不断角度pid的结果(由于速度跟频率成正比,不需要测速反馈)
难点:pid整定参数
代码资料(完美)见下
balance car nrf24l01程序 完美.zip(6.43 MB, 下载次数: 242)
评分
我用printf("%0.2f %0.2f %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度,角速度和倾角,我发现当我快速的改变板子的倾角的时候,比如快速变化10度,Angle(卡尔曼滤波后的倾角)瞬时变化非常快,可能会瞬间变化几十度然后回到正常角度,而当我缓慢变化10度的时候,Angle变化是正常线性变化到10度,在这两种变化中,Angle_ax(从MPU6050读取的值经过处理后的陀螺仪的Y轴数据)的变化一直都是线性正常的,并且Angle的值特别接近Angle_ax的值 问题:1,我快速改变板子倾角时Angle的变化正常吗? 2,Angle正常变化的时候是应该与Angle_ax的值相近吗? 现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大 *************读取数据******************** //定义MPU6050内部地址 #define SMPLRT_DIV 0x19 //陀螺仪采样率 典型值 0X07 125Hz #define CONFIG 0x1A //低通滤波频率 典型值 0x00 #define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围 典型值 0x18 不自检 2000deg/s #define ACCEL_CONFIG 0x1C //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz #define INT_PIN_CFG 0x37 #define INT_ENABLE 0x38 #define INT_STATUS 0x3A //只读 #define ACCEL_XOUT_H 0x3B #define ACCEL_XOUT_L 0x3C #define ACCEL_YOUT_H 0x3D #define ACCEL_YOUT_L 0x3E #define ACCEL_ZOUT_H 0x3F #define ACCEL_ZOUT_L 0x40 #define TEMP_OUT_H 0x41 #define TEMP_OUT_L 0x42 #define GYRO_XOUT_H 0x43 #define GYRO_XOUT_L 0x44 #define GYRO_YOUT_H 0x45 #define GYRO_YOUT_L 0x46 #define GYRO_ZOUT_H 0x47 #define GYRO_ZOUT_L 0x48 //读取寄存器原生数据 MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]); MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]); MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]); MPU6050_Raw_Data.Temp = (buf[6]<<8 | buf[7]); MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]); MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]); MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]); //将原生数据转换为实际值,计算公式跟寄存器的配置有关 MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0; MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0; MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0; MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5; MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5; MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5; } //******卡尔曼参数************ const float Q_angle=0.001; const float Q_gyro=0.003; const float R_angle=0.5; const float dt=0.01; //dt为kalman滤波器采样时间; const char C_0 = 1; float Q_bias, Angle_err; float PCt_0, PCt_1, E; float K_0, K_1, t_0, t_1; float Pdot[4] ={0,0,0,0}; float PP[2][2] = { { 1, 0 },{ 0, 1 } }; /*****************卡尔曼滤波**************************************************/ void Kalman_Filter(float Accel,float Gyro) { Angle+=(Gyro - Q_bias) * dt; //先验估计 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分 Pdot[1]= -PP[1][1]; Pdot[2]= -PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分 PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差 PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; //zk-先验估计 PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; //后验估计误差协方差 PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; //后验估计 Q_bias += K_1 * Angle_err; //后验估计 Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 } ******************倾角计算***************** void Angle_Calculate(void) { /****************************加速度****************************************/ Accel_x = MPU6050_Real_Data.Accel_X; //读取X轴加速度 Angle_ax = Accel_x*1.2*180/3.14; //弧度转换为度 /****************************角速度****************************************/ Gyro_y = MPU6050_Real_Data.Gyro_Y; 时间dt,所以此处不用积分 /***************************卡尔曼融合*************************************/ Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角