diff --git a/User/lib/filter.c b/User/lib/filter.c new file mode 100644 index 0000000..ed0bbca --- /dev/null +++ b/User/lib/filter.c @@ -0,0 +1,184 @@ +/* + 各类滤波器。 +*/ + +#include "filter.h" + +#include "user_math.h" + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq) { + if (f == NULL) return; + + f->cutoff_freq = cutoff_freq; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (f->cutoff_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + const float fr = sample_freq / f->cutoff_freq; + const float ohm = tanf(M_PI / fr); + const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm; + + f->b0 = ohm * ohm / c; + f->b1 = 2.0f * f->b0; + f->b2 = f->b0; + + f->a1 = 2.0f * (ohm * ohm - 1.0f) / c; + f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* do the filtering */ + float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + + if (isinf(delay_element_0)) { + /* don't allow bad values to propagate via the filter */ + delay_element_0 = sample; + } + + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + /* return the value. Should be no need to check limits */ + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + const float dval = sample / (f->b0 + f->b1 + f->b2); + + if (isfinite(dval)) { + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + } else { + f->delay_element_1 = sample; + f->delay_element_2 = sample; + } + + return LowPassFilter2p_Apply(f, sample); +} + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,float bandwidth) +{ + if (f == NULL) return; + + f->notch_freq = notch_freq; + f->bandwidth = bandwidth; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (notch_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + + const float alpha = tanf(M_PI * bandwidth / sample_freq); + const float beta = -cosf(M_2PI * notch_freq / sample_freq); + const float a0_inv = 1.0f / (alpha + 1.0f); + + f->b0 = a0_inv; + f->b1 = 2.0f * beta * a0_inv; + f->b2 = a0_inv; + + f->a1 = f->b1; + f->a2 = (1.0f - alpha) * a0_inv; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +inline float NotchFilter_Apply(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* Direct Form II implementation */ + const float delay_element_0 = sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + float dval = sample; + + if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) { + dval = dval / (f->b0 + f->b1 + f->b2); + } + + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + return NotchFilter_Apply(f, sample); +} diff --git a/User/lib/filter.h b/User/lib/filter.h new file mode 100644 index 0000000..c075946 --- /dev/null +++ b/User/lib/filter.h @@ -0,0 +1,104 @@ +/* + 各类滤波器。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* 二阶低通滤波器 */ +typedef struct { + float cutoff_freq; /* 截止频率 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + + float delay_element_1; + float delay_element_2; + +} LowPassFilter2p_t; + +/* 带阻滤波器 */ +typedef struct { + float notch_freq; /* 阻止频率 */ + float bandwidth; /* 带宽 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + float delay_element_1; + float delay_element_2; + +} NotchFilter_t; + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample); + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Apply(NotchFilter_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample); + +#ifdef __cplusplus +} +#endif diff --git a/User/lib/kalman.c b/User/lib/kalman.c new file mode 100644 index 0000000..52bca43 --- /dev/null +++ b/User/lib/kalman.c @@ -0,0 +1,65 @@ +/** + * @author Liu heng + * 一阶卡尔曼滤波器来自RoboMaster论坛 + * 一维卡尔曼滤波器 + * 使用时先定义一个kalman指针，然后调用kalmanCreate()创建一个滤波器 + * 每次读取到传感器数据后即可调用KalmanFilter()来对数据进行滤波 + * 使用示例 + * extKalman p; //定义一个卡尔曼滤波器结构体 + * float SersorData; //需要进行滤波的数据 + * KalmanCreate(&p,20,200); //初始化该滤波器的Q=20 R=200参数 + * while(1) + * { + * SersorData = sersor(); //获取数据 + * SersorData = KalmanFilter(&p,SersorData); //对数据进行滤波 + * } + */ + +#include "kalman.h" + +/** + * @name kalmanCreate + * @brief 创建一个卡尔曼滤波器 + * @param p: 滤波器 + * T_Q:系统噪声协方差 + * T_R:测量噪声协方差 + * + * @retval none + * @attention R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值 + * 反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测 + */ +void KalmanCreate(extKalman_t *p,float T_Q,float T_R) +{ + p->X_last = (float)0; + p->P_last = 0; + p->Q = T_Q; + p->R = T_R; + p->A = 1; + p->B = 0; + p->H = 1; + p->X_mid = p->X_last; +} + +/** + * @name KalmanFilter + * @brief 卡尔曼滤波器 + * @param p: 滤波器 + * dat:待滤波数据 + * @retval 滤波后的数据 + * @attention Z(k)是系统输入,即测量值 X(k|k)是卡尔曼滤波后的值,即最终输出 + * A=1 B=0 H=1 I=1 W(K) V(k)是高斯白噪声,叠加在测量值上了,可以不用管 + * 以下是卡尔曼的5个核心公式 + * 一阶H'即为它本身,否则为转置矩阵 + */ + +float KalmanFilter(extKalman_t* p,float dat) +{ + p->X_mid =p->A*p->X_last; //百度对应公式(1) x(k|k-1) = A*X(k-1|k-1)+B*U(k)+W(K) + p->P_mid = p->A*p->P_last+p->Q; //百度对应公式(2) p(k|k-1) = A*p(k-1|k-1)*A'+Q + p->kg = p->P_mid/(p->P_mid+p->R); //百度对应公式(4) kg(k) = p(k|k-1)*H'/(H*p(k|k-1)*H'+R) + p->X_now = p->X_mid+p->kg*(dat-p->X_mid); //百度对应公式(3) x(k|k) = X(k|k-1)+kg(k)*(Z(k)-H*X(k|k-1)) + p->P_now = (1-p->kg)*p->P_mid; //百度对应公式(5) p(k|k) = (I-kg(k)*H)*P(k|k-1) + p->P_last = p->P_now; //状态更新 + p->X_last = p->X_now; + return p->X_now; //输出预测结果x(k|k) +} diff --git a/User/lib/kalman.h b/User/lib/kalman.h new file mode 100644 index 0000000..72ddb58 --- /dev/null +++ b/User/lib/kalman.h @@ -0,0 +1,28 @@ +/** + * @author Liu heng + * ˲RoboMaster̳ + */ + +#ifndef _KALMAN_H +#define _KALMAN_H + + +typedef struct { + float X_last; //һʱ̵Ž X(k-|k-1) + float X_mid; //ǰʱ̵Ԥ X(k|k-1) + float X_now; //ǰʱ̵Ž X(k|k) + float P_mid; //ǰʱԤЭ P(k|k-1) + float P_now; //ǰʱŽЭ P(k|k) + float P_last; //һʱŽЭ P(k-1|k-1) + float kg; //kalman + float A; //ϵͳ + float B; + float Q; + float R; + float H; +}extKalman_t; + +void KalmanCreate(extKalman_t *p,float T_Q,float T_R); +float KalmanFilter(extKalman_t* p,float dat); + +#endif diff --git a/User/module/ball.cpp b/User/module/ball.cpp index bb6772e..1d41eb6 100644 --- a/User/module/ball.cpp +++ b/User/module/ball.cpp @@ -32,8 +32,8 @@ Ball ::Ball() PID_init(&extend_speed_pid,PID_POSITION,Extend_speed_PID,3000, 200); PID_init(&extend_angle_pid,PID_POSITION,Extend_angle_PID,3000, 200); -// e_result = 0; -// angleSet = 0; + e_result = 0; + angleSet = 0; //三摩擦轮 for(int i = 0;i < MOTOR_NUM;i ++) { @@ -47,9 +47,22 @@ Ball ::Ball() speedSet[i] = 0; } + //状态机状态初始化 currentState1= BALL_IDLE; - + +} + +void Ball :: Filter_init(float target_freq) +{ + for(int i = 0;i < MOTOR_NUM;i ++) + { + //摩擦轮滤波器初始化 + LowPassFilter2p_Init(filter.in + i , target_freq, + -1.0f); + LowPassFilter2p_Init(filter.out + i, target_freq, + -1.0f); + } } void Ball ::Extend_control(int angle) @@ -72,6 +85,11 @@ void Ball ::Extend_control(int angle) void Ball ::Spin(float speed,float speed2) { + for(int i = 0;i < MOTOR_NUM;i ++) + { + hand_Motor[i]->speed_rpm=LowPassFilter2p_Apply(filter.in + i, hand_Motor[i]->speed_rpm); + } + speedSet[MOTOR_1] = -speed; result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]); @@ -81,6 +99,11 @@ void Ball ::Spin(float speed,float speed2) speedSet[MOTOR_3] = speed2; result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]); + for(int i = 0;i < MOTOR_NUM;i ++) + { + result[i]=LowPassFilter2p_Apply(filter.out + i, result[i]); + } + } int flag =0; diff --git a/User/module/ball.hpp b/User/module/ball.hpp index a6d8093..94a8e5a 100644 --- a/User/module/ball.hpp +++ b/User/module/ball.hpp @@ -8,6 +8,7 @@ #include "bsp_delay.h" #include "djiMotor.h" #include "pid.h" +#include "filter.h" // 定义状态枚举 typedef enum { @@ -33,6 +34,12 @@ typedef enum MOTOR_NUM }motorhangd_e; + /* 滤波器 */ + typedef struct { + LowPassFilter2p_t *in; /* 反馈值滤波器 */ + LowPassFilter2p_t *out; /* 输出值滤波器 */ + } Filter; + // 定义光电传感器检测宏 #define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET) @@ -40,6 +47,7 @@ class Ball { public: Ball(); + void Filter_init(float target_freq); void Spin(float speed,float speed2); void ballHadling(void); void Send_control(void); @@ -59,6 +67,9 @@ public: volatile uint32_t flag_thread;//接收传回的线程通知 private: + //滤波一下 + Filter filter; + //三个摩擦轮 static const float M3508_speed_PID[3]; static const float M3508_angle_PID[3];