feat(FOC): 增加闭环速度控制算法
- 重构setPhaseVoltage函数实现，提高执行效率 - 增加update_power函数，动态更新电源电压 - 优化velocity_open_loop函数，调用、setPhaseVoltage避免重复代码 - 实现angle_control和velocity_control闭环控制函数 - 调整PID控制器参数初始化及增加参数设置接口 - 改进电流和编码器速度卡尔曼滤波器实现 - 更新CAN通信数据打包逻辑，增加测试字段 - 在定时器中断中集成多种控制模式切换逻辑 - 完善角度传感器数据处理，改进速度计算方法 - 减小速度滤波数组大小，提升响应速度
This commit is contained in:
@@ -1,4 +1,5 @@
|
||||
#include "DengFOC.h"
|
||||
#include "Can.h"
|
||||
#include "Filter.h"
|
||||
#include "Timer.h"
|
||||
#include "adc.h"
|
||||
@@ -14,21 +15,22 @@
|
||||
constexpr double PI = 3.1415926535897932384626433832795f;
|
||||
constexpr float DEG2RAD = 1.0f / 360.0f * 6.28318530718f;
|
||||
constexpr float RAD2DEG = 360.0f / 6.28318530718f;
|
||||
constexpr float ANGLE_FACTOR = 25.0f / 360.0f * 2.0f * 3.14159265358979323846f;
|
||||
constexpr float _1_SQRT3 = 0.57735026919f;
|
||||
constexpr float _2_SQRT3 = 1.15470053838f;
|
||||
constexpr float _3PI_2 = 4.71238898038f;
|
||||
constexpr u8 PP = 11;
|
||||
constexpr float DIR = -1.0;
|
||||
constexpr float DIR = 1.0;
|
||||
constexpr float sqrt3 = 1.73205080757f; // sqrt(3)的预计算值
|
||||
constexpr float zero_electric_angle = 0.0f;
|
||||
|
||||
extern sampler sampler;
|
||||
extern angle_sensor KTH7823;
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
|
||||
CurrentKalman curU_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
|
||||
CurrentKalman curV_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
|
||||
CurrentKalman curW_kalman(0.0f, 0.0026f, 4.8f, 0.0001f, 1e-6f, 0.02f);
|
||||
|
||||
// 初始变量及函数定义
|
||||
#define _constrain(amt, low, high) \
|
||||
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
|
||||
@@ -65,15 +67,17 @@ FOC_Controlroller::FOC_Controlroller() { ; }
|
||||
*/
|
||||
inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
|
||||
float angle_el) {
|
||||
angle_el = _normalizeAngle(angle_el);
|
||||
// 帕克逆变换
|
||||
Ualpha = -Uq * sin(angle_el);
|
||||
Ubeta = Uq * cos(angle_el);
|
||||
// 帕克逆变换 (直接计算，避免函数调用开销)
|
||||
const float sin_el = sin(angle_el);
|
||||
const float cos_el = cos(angle_el);
|
||||
Ualpha = -Uq * sin_el;
|
||||
Ubeta = Uq * cos_el;
|
||||
|
||||
// 克拉克逆变换 (预计算常量值)
|
||||
Ua = Ualpha + v_half;
|
||||
Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
|
||||
Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
|
||||
|
||||
// 克拉克逆变换
|
||||
Ua = Ualpha + voltage_power_supply / 2;
|
||||
Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
|
||||
Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
|
||||
// 计算占空比
|
||||
Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
|
||||
Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
|
||||
@@ -84,6 +88,33 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
|
||||
Set_3Pwm(dc_a, dc_b, dc_c);
|
||||
// std::printf("PWM:%f,%f,%f\n", dc_a, dc_b, dc_c);
|
||||
}
|
||||
/**
|
||||
* @brief 电机关机
|
||||
*
|
||||
*/
|
||||
void FOC_Controlroller::shut_down() {
|
||||
is_shut_down = 100;
|
||||
gpio_bit_reset(GPIOB, GPIO_PIN_12);
|
||||
}
|
||||
/**
|
||||
* @brief 电机恢复
|
||||
*
|
||||
*/
|
||||
void FOC_Controlroller::resume() {
|
||||
over_current = 0;
|
||||
temp_error = 0;
|
||||
voltage_error = 0;
|
||||
is_shut_down = 0;
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_12);
|
||||
}
|
||||
/**
|
||||
* @brief 更新电源电压
|
||||
*
|
||||
*/
|
||||
void FOC_Controlroller::update_power(void) {
|
||||
voltage_power_supply = sampler.get_vbus_voltage();
|
||||
v_half = voltage_power_supply * 0.5f;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 速度开环控制
|
||||
@@ -94,36 +125,16 @@ inline void FOC_Controlroller::setPhaseVoltage(float Uq, float Ud,
|
||||
float FOC_Controlroller::velocity_open_loop() {
|
||||
// 通过时间间隔和目标速度计算需要转动的机械角度，存储在shaft_angle中，在此之前，还需要对轴角度归一化
|
||||
// 预计算常量 1e-4 = 100微秒控制周期
|
||||
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * 1e-4f);
|
||||
shaft_angle =
|
||||
_normalizeAngle(shaft_angle + target_velocity * ANGLE_FACTOR * 1e-4f);
|
||||
|
||||
// 直接计算电气角度，避免函数调用开销
|
||||
float angle_el = shaft_angle * PP + zero_electric_angle;
|
||||
angle_el = _normalizeAngle(angle_el);
|
||||
float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
|
||||
|
||||
// 因为在开环控制中，Ud始终为0，只控制Uq
|
||||
const float Uq = voltage_power_supply / 2.0f;
|
||||
const float Ud = 0.0f;
|
||||
|
||||
// 帕克逆变换 (直接计算，避免函数调用开销)
|
||||
const float sin_el = sin(angle_el);
|
||||
const float cos_el = cos(angle_el);
|
||||
Ualpha = -Uq * sin_el;
|
||||
Ubeta = Uq * cos_el;
|
||||
|
||||
// 克拉克逆变换 (预计算常量值)
|
||||
const float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
||||
Ua = Ualpha + v_half;
|
||||
Ub = (sqrt3 * Ubeta - Ualpha) * 0.5f + v_half;
|
||||
Uc = (-Ualpha - sqrt3 * Ubeta) * 0.5f + v_half;
|
||||
|
||||
// 计算占空比
|
||||
Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
|
||||
Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
|
||||
Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage * 2);
|
||||
const float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
|
||||
const float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
|
||||
const float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
|
||||
Set_3Pwm(dc_a, dc_b, dc_c);
|
||||
setPhaseVoltage(Uq, 0.0f, angle_el);
|
||||
|
||||
return limit_voltage;
|
||||
}
|
||||
@@ -142,55 +153,40 @@ float FOC_Controlroller::velocity_open_loop() {
|
||||
// std::printf("0电角度:%f", zero_electric_angle);
|
||||
// }
|
||||
|
||||
void FOC_Controlroller::shut_down() {
|
||||
is_shut_down = 100;
|
||||
gpio_bit_reset(GPIOB, GPIO_PIN_12);
|
||||
}
|
||||
|
||||
void FOC_Controlroller::resume() {
|
||||
over_current = 0;
|
||||
temp_error = 0;
|
||||
voltage_error = 0;
|
||||
is_shut_down = 0;
|
||||
gpio_bit_set(GPIOB, GPIO_PIN_12);
|
||||
}
|
||||
|
||||
void FOC_Controlroller::update_power(void) {
|
||||
voltage_power_supply = sampler.get_vbus_voltage();
|
||||
/**
|
||||
* @brief 角度闭环控制
|
||||
*
|
||||
* @param target_angle: 目标角度
|
||||
* @return float: q轴电压
|
||||
*/
|
||||
float FOC_Controlroller::angle_control(float target_angle) {
|
||||
shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
|
||||
static float Kp = 2.0f;
|
||||
float Uq =
|
||||
1.0f - _constrain(Kp * (target_angle - DIR * shaft_angle) * RAD2DEG,
|
||||
-voltage_power_supply, voltage_power_supply);
|
||||
static float Ud = 0.0f;
|
||||
// 直接计算电气角度，避免函数调用开销
|
||||
float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
|
||||
// 设置相电压
|
||||
setPhaseVoltage(Uq, 0.0f, angle_el);
|
||||
return shaft_angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 角度闭环控制
|
||||
* @brief 速度闭环控制
|
||||
*
|
||||
* @param target_velocity: 目标速度
|
||||
* @return float: q轴电压
|
||||
*/
|
||||
float FOC_Controlroller::angle_control(float target_angle) {
|
||||
float Sensor_angel = KTH7823.getRawAngle() * DEG2RAD;
|
||||
float Kp = 2.0f;
|
||||
float FOC_Controlroller::velocity_control(float target_velocity) {
|
||||
// PID 控制计算
|
||||
float Uq = v_half - vel_loop(target_velocity - rt_velocity);
|
||||
|
||||
float Uq = _constrain(Kp * (target_angle - DIR * Sensor_angel) * RAD2DEG,
|
||||
-voltage_power_supply, voltage_power_supply);
|
||||
shaft_angle = _normalizeAngle(KTH7823.read_sensorAngle() * DEG2RAD);
|
||||
float Ud = 0.0f;
|
||||
shaft_angle = KTH7823.getRawAngle() * DEG2RAD;
|
||||
// 直接计算电气角度，避免函数调用开销
|
||||
float angle_el = _normalizeAngle(shaft_angle * PP + zero_electric_angle);
|
||||
|
||||
// 帕克逆变换
|
||||
Ualpha = -Uq * sin(angle_el);
|
||||
Ubeta = Uq * cos(angle_el);
|
||||
|
||||
// 克拉克逆变换
|
||||
Ua = Ualpha + voltage_power_supply / 2;
|
||||
Ub = (sqrt3 * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
|
||||
Uc = (-Ualpha - sqrt3 * Ubeta) / 2 + voltage_power_supply / 2;
|
||||
// 计算占空比
|
||||
Ua = constrain(Ua, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
|
||||
Ub = constrain(Ub, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
|
||||
Uc = constrain(Uc, 0.0f, voltage_power_supply, 0.0f, limit_voltage);
|
||||
float dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f);
|
||||
float dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f);
|
||||
float dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f);
|
||||
Set_3Pwm(dc_a, dc_b, dc_c);
|
||||
return Sensor_angel;
|
||||
// 设置相电压
|
||||
setPhaseVoltage(Uq, 0.0f, angle_el);
|
||||
return rt_velocity;
|
||||
}
|
||||
@@ -21,8 +21,7 @@ class FOC_Controlroller {
|
||||
|
||||
float velocity_open_loop();
|
||||
float angle_control(float target_angle);
|
||||
// void SET_VEL_PID(float P, float I, float D, float ramp); // 速度环PID设置
|
||||
// void SET_ANGLE_PID(float P, float I, float D, float ramp); // 角度环PID设置
|
||||
float velocity_control(float target_velocity);
|
||||
|
||||
void update_power(void);
|
||||
void DFOC_alignSensor(int _PP);
|
||||
@@ -36,9 +35,10 @@ class FOC_Controlroller {
|
||||
float Ualpha, Ubeta = 0, Ua = 0, Ub = 0, Uc = 0;
|
||||
float zero_electric_angle = 0;
|
||||
float voltage_power_supply = 24.0f; // 电源电压
|
||||
float v_half = voltage_power_supply * 0.5f; // 电源电压的一半
|
||||
|
||||
PIDController vel_loop =
|
||||
PIDController{2, 0, 0, 100000, voltage_power_supply / 2};
|
||||
PIDController{0.8f, 3.0f, 0, 100000, voltage_power_supply};
|
||||
PIDController angle_loop = PIDController{2, 0, 0, 100000, 100};
|
||||
|
||||
void setPhaseVoltage(float Uq, float Ud, float angle_el);
|
||||
|
||||
@@ -1,10 +1,16 @@
|
||||
#include "Filter.h"
|
||||
#include "ang_sensor.h"
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
#include <stdint.h>
|
||||
|
||||
//队列实现
|
||||
|
||||
extern angle_sensor KTH7823;
|
||||
|
||||
EncoderVelocityKalman Vel_kalman(KTH7823.getVelocity(), 1e-5f, 1e-6f, 0.05f);
|
||||
|
||||
// 队列实现
|
||||
void CircularBuffer::enqueue(const float value) {
|
||||
buffer[head] = value;
|
||||
head = (head + 1) % HISTORY_SIZE;
|
||||
@@ -73,54 +79,92 @@ float medium_filter(float values[], u8 size) {
|
||||
*/
|
||||
CurrentKalman::CurrentKalman(float initial_current, float L, float R, float T,
|
||||
float Q, float R_noise)
|
||||
: x(initial_current), P(1.0f), // 初始误差协方差
|
||||
Q(Q), // 过程噪声
|
||||
R(R_noise), // 测量噪声
|
||||
L(L), R_m(R), T(T) {
|
||||
|
||||
CircularBuffer buffer;
|
||||
}
|
||||
: kalman(initial_current, 1.0f, Q, R_noise), L(L), R_m(R), T(T) {}
|
||||
/*!
|
||||
\brief 更新卡尔曼滤波器状态
|
||||
\param[in] measured_current 测量电流值
|
||||
\param[in] measured_value 测量值
|
||||
\param[in] applied_voltage 应用电压值
|
||||
\retval 估计的电流值
|
||||
\note 实现卡尔曼滤波的预测和更新步骤
|
||||
*/
|
||||
float CurrentKalman::update(float measured_current, float applied_voltage) {
|
||||
float CurrentKalman::update(float measured_value, float applied_voltage) {
|
||||
// 1. 预测步骤 - 基于电机模型
|
||||
float predicted_current = x + T * (applied_voltage - R_m * x) / L;
|
||||
|
||||
// 2. 计算预测误差协方差
|
||||
float P_pred = P + Q;
|
||||
|
||||
// 3. 计算卡尔曼增益
|
||||
float K = P_pred / (P_pred + R);
|
||||
|
||||
// 4. 更新状态估计
|
||||
x = predicted_current + K * (measured_current - predicted_current);
|
||||
|
||||
// 5. 更新估计误差协方差
|
||||
P = (1 - K) * P_pred;
|
||||
|
||||
return x;
|
||||
float predicted_current = kalman.getState() + T * (applied_voltage - R_m * kalman.getState()) / L;
|
||||
|
||||
// 2. 预测误差协方差已经在kalman.predict中处理
|
||||
|
||||
// 3. 执行预测
|
||||
kalman.predict(predicted_current);
|
||||
|
||||
// 4. 更新步骤 - 直接使用测量值更新
|
||||
return kalman.update(measured_value);
|
||||
}
|
||||
/*!
|
||||
\brief 获取当前电流估计值
|
||||
\retval 当前电流估计值
|
||||
*/
|
||||
float CurrentKalman::getCurrent() const { return x; }
|
||||
float CurrentKalman::getCurrent() const { return kalman.getState(); }
|
||||
/*!
|
||||
\brief 设置过程噪声协方差
|
||||
\param[in] q 过程噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void CurrentKalman::setProcessNoise(float q) { Q = q; }
|
||||
void CurrentKalman::setProcessNoise(float q) { kalman.setProcessNoise(q); }
|
||||
/*!
|
||||
\brief 设置测量噪声协方差
|
||||
\param[in] r 测量噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void CurrentKalman::setMeasurementNoise(float r) { R = r; }
|
||||
void CurrentKalman::setMeasurementNoise(float r) { kalman.setMeasurementNoise(r); }
|
||||
|
||||
// EncoderVelocityKalman类的实现
|
||||
/*!
|
||||
\brief 构造函数，初始化编码器速度卡尔曼滤波器参数
|
||||
\param[in] initial_velocity 初始速度值 (rad/s)
|
||||
\param[in] T 控制周期(s)
|
||||
\param[in] Q 过程噪声协方差(典型值: 1e-4 ~ 1e-2)
|
||||
\param[in] R 测量噪声协方差(典型值: 速度范围的1%~5%)
|
||||
\retval none
|
||||
*/
|
||||
EncoderVelocityKalman::EncoderVelocityKalman(float initial_velocity, float T,
|
||||
float Q, float R)
|
||||
: kalman(initial_velocity, T, Q, R) {}
|
||||
|
||||
/*!
|
||||
\brief 更新卡尔曼滤波器状态
|
||||
\param[in] measured_velocity 测量的速度值 (rad/s)
|
||||
\retval 估计的速度值 (rad/s)
|
||||
\note 实现卡尔曼滤波的预测和更新步骤，假设速度变化符合随机游走模型
|
||||
*/
|
||||
float EncoderVelocityKalman::update(float measured_velocity) {
|
||||
// 1. 预测步骤 - 假设速度符合随机游走模型（加速度为0）
|
||||
// 由于速度变化缓慢，我们可以简单预测速度保持不变
|
||||
kalman.predict(kalman.getState());
|
||||
|
||||
// 2. 更新步骤 - 直接使用测量值更新
|
||||
return kalman.update(measured_velocity);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 获取当前速度估计值
|
||||
\retval 当前速度估计值 (rad/s)
|
||||
*/
|
||||
float EncoderVelocityKalman::getVelocity() const { return kalman.getState(); }
|
||||
|
||||
/*!
|
||||
\brief 设置过程噪声协方差
|
||||
\param[in] q 过程噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void EncoderVelocityKalman::setProcessNoise(float q) {
|
||||
kalman.setProcessNoise(q);
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 设置测量噪声协方差
|
||||
\param[in] r 测量噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void EncoderVelocityKalman::setMeasurementNoise(float r) {
|
||||
kalman.setMeasurementNoise(r);
|
||||
}
|
||||
+110
-5
@@ -34,6 +34,92 @@ class CircularBuffer {
|
||||
|
||||
float medium_filter(float values[], u8 size);
|
||||
|
||||
/*!
|
||||
\brief 通用卡尔曼滤波器模板类
|
||||
\note 可用于各种基于线性系统的状态估计
|
||||
*/
|
||||
template <typename T> class KalmanFilter {
|
||||
private:
|
||||
T x; // 状态估计
|
||||
T P; // 估计误差协方差
|
||||
T Q; // 过程噪声协方差
|
||||
T R; // 测量噪声协方差
|
||||
|
||||
public:
|
||||
/*!
|
||||
\brief 构造函数
|
||||
\param[in] initial_state 初始状态值
|
||||
\param[in] initial_error_covariance 初始误差协方差
|
||||
\param[in] process_noise 过程噪声协方差
|
||||
\param[in] measurement_noise 测量噪声协方差
|
||||
*/
|
||||
KalmanFilter(T initial_state, T initial_error_covariance, T process_noise,
|
||||
T measurement_noise)
|
||||
: x(initial_state), P(initial_error_covariance), Q(process_noise),
|
||||
R(measurement_noise) {}
|
||||
|
||||
/*!
|
||||
\brief 预测步骤
|
||||
\param[in] state_prediction 状态预测值
|
||||
\retval none
|
||||
*/
|
||||
void predict(T state_prediction) {
|
||||
x = state_prediction;
|
||||
P += Q;
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 更新步骤
|
||||
\param[in] measurement 测量值
|
||||
\param[in] measurement_prediction 测量预测值
|
||||
\retval 估计的状态值
|
||||
*/
|
||||
T update(T measurement, T measurement_prediction) {
|
||||
T y = measurement - measurement_prediction; // 测量残差
|
||||
T S = P + R; // 残差协方差
|
||||
T K = P / S; // 卡尔曼增益
|
||||
|
||||
x += K * y; // 更新状态估计
|
||||
P = (1 - K) * P; // 更新误差协方差
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 直接更新步骤（适用于状态直接可测的情况）
|
||||
\param[in] measurement 测量值
|
||||
\retval 估计的状态值
|
||||
*/
|
||||
T update(T measurement) {
|
||||
T K = P / (P + R); // 卡尔曼增益
|
||||
|
||||
x += K * (measurement - x); // 更新状态估计
|
||||
P = (1 - K) * P; // 更新误差协方差
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 获取当前状态估计值
|
||||
\retval 当前状态估计值
|
||||
*/
|
||||
T getState() const { return x; }
|
||||
|
||||
/*!
|
||||
\brief 设置过程噪声协方差
|
||||
\param[in] process_noise 过程噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void setProcessNoise(T process_noise) { Q = process_noise; }
|
||||
|
||||
/*!
|
||||
\brief 设置测量噪声协方差
|
||||
\param[in] measurement_noise 测量噪声协方差值
|
||||
\retval none
|
||||
*/
|
||||
void setMeasurementNoise(T measurement_noise) { R = measurement_noise; }
|
||||
};
|
||||
|
||||
/*!
|
||||
\brief 电流卡尔曼滤波器类
|
||||
\note 基于电机模型的卡尔曼滤波器实现，用于电流状态估计
|
||||
@@ -45,10 +131,7 @@ float medium_filter(float values[], u8 size);
|
||||
*/
|
||||
class CurrentKalman {
|
||||
private:
|
||||
float x; // 电流状态估计
|
||||
float P; // 估计误差协方差
|
||||
float Q; // 过程噪声协方差
|
||||
float R; // 测量噪声协方差
|
||||
KalmanFilter<float> kalman;
|
||||
float L; // 电机电感(H)
|
||||
float R_m; // 电机电阻(Ω)
|
||||
float T; // 控制周期(s)
|
||||
@@ -66,6 +149,28 @@ class CurrentKalman {
|
||||
void setMeasurementNoise(float r);
|
||||
};
|
||||
|
||||
/*!
|
||||
\brief 编码器速度卡尔曼滤波器类
|
||||
\note 用于滤除编码器速度测量噪声的卡尔曼滤波器实现
|
||||
*/
|
||||
class EncoderVelocityKalman {
|
||||
private:
|
||||
KalmanFilter<float> kalman;
|
||||
|
||||
public:
|
||||
EncoderVelocityKalman(float initial_velocity,
|
||||
float initial_error_covariance, float process_noise,
|
||||
float measurement_noise);
|
||||
|
||||
float update(float measured_velocity);
|
||||
|
||||
float getVelocity() const;
|
||||
|
||||
void setProcessNoise(float q);
|
||||
|
||||
void setMeasurementNoise(float r);
|
||||
};
|
||||
|
||||
#endif // __cplusplus
|
||||
|
||||
#endif // _FILTER_H
|
||||
#endif // _FILTER_H
|
||||
+55
-41
@@ -1,52 +1,66 @@
|
||||
#include "pid.h"
|
||||
#include "Timer.h"
|
||||
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
||||
#define _constrain(amt, low, high) \
|
||||
((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
|
||||
#include "Can.h"
|
||||
|
||||
extern CAN_transmiter CAN0_transmiter;
|
||||
|
||||
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
|
||||
: P(P)
|
||||
, I(I)
|
||||
, D(D)
|
||||
, output_ramp(ramp) // PID控制器加速度限幅
|
||||
, limit(limit) // PID控制器输出限幅
|
||||
, error_prev(0.0f)
|
||||
, output_prev(0.0f)
|
||||
, integral_prev(0.0f)
|
||||
{
|
||||
timestamp_prev = get_system_runtime_us() ;
|
||||
: P(P), I(I), D(D), output_ramp(ramp) // PID控制器加速度限幅
|
||||
,
|
||||
limit(limit) // PID控制器输出限幅
|
||||
,
|
||||
error_prev(0.0f), output_prev(0.0f), integral_prev(0.0f) {
|
||||
timestamp_prev = get_system_runtime_us();
|
||||
}
|
||||
|
||||
// PID 控制器函数
|
||||
float PIDController::operator() (float error){
|
||||
// 计算两次循环中间的间隔时间
|
||||
unsigned long timestamp_now = get_system_runtime_us();
|
||||
float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
|
||||
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
|
||||
|
||||
// P环
|
||||
float proportional = P * error;
|
||||
// Tustin 散点积分（I环）
|
||||
float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
|
||||
integral = _constrain(integral, -limit, limit);
|
||||
// D环（微分环节）
|
||||
float derivative = D*(error - error_prev)/Ts;
|
||||
float PIDController::operator()(float error) {
|
||||
// 计算两次循环中间的间隔时间
|
||||
unsigned long timestamp_now = get_system_runtime_us();
|
||||
float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
|
||||
if (Ts <= 0 || Ts > 0.5f)
|
||||
Ts = 1e-3f;
|
||||
|
||||
// 将P,I,D三环的计算值加起来
|
||||
float output = proportional + integral + derivative;
|
||||
output = _constrain(output, -limit, limit);
|
||||
// P环
|
||||
float proportional = P * error;
|
||||
// Tustin 散点积分（I环）
|
||||
float integral = integral_prev + I * Ts * 0.5f * (error + error_prev);
|
||||
integral = _constrain(integral, -limit, limit);
|
||||
// D环（微分环节）
|
||||
float derivative = D * (error - error_prev) / Ts;
|
||||
// 将P,I,D三环的计算值加起来
|
||||
float output = proportional + integral + derivative;
|
||||
output = _constrain(output, -limit, limit);
|
||||
|
||||
if(output_ramp > 0){
|
||||
// 对PID的变化速率进行限制
|
||||
float output_rate = (output - output_prev)/Ts;
|
||||
if (output_rate > output_ramp)
|
||||
output = output_prev + output_ramp*Ts;
|
||||
else if (output_rate < -output_ramp)
|
||||
output = output_prev - output_ramp*Ts;
|
||||
}
|
||||
// 保存值（为了下一次循环）
|
||||
integral_prev = integral;
|
||||
output_prev = output;
|
||||
error_prev = error;
|
||||
timestamp_prev = timestamp_now;
|
||||
return output;
|
||||
if (output_ramp > 0) {
|
||||
// 对PID的变化速率进行限制
|
||||
float output_rate = (output - output_prev) / Ts;
|
||||
if (output_rate > output_ramp)
|
||||
output = output_prev + output_ramp * Ts;
|
||||
else if (output_rate < -output_ramp)
|
||||
output = output_prev - output_ramp * Ts;
|
||||
}
|
||||
// 保存值（为了下一次循环）
|
||||
integral_prev = integral;
|
||||
output_prev = output;
|
||||
error_prev = error;
|
||||
timestamp_prev = timestamp_now;
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置PID参数
|
||||
*
|
||||
* @param P: P参数
|
||||
* @param I: I参数
|
||||
* @param D: D参数
|
||||
* @return void
|
||||
*/
|
||||
void PIDController::set_PID(float P, float I, float D) {
|
||||
P = P;
|
||||
I = I;
|
||||
D = D;
|
||||
}
|
||||
Reference in New Issue
Block a user