From eccdd5be2e77cdef038a41ec598c7c9b755ca6e9 Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Sat, 9 Mar 2024 15:53:34 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8D=A2cpp=E6=9E=B6=E6=9E=84=E5=89=8D?= =?UTF-8?q?=E5=A4=87=E4=BB=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/balance/estimator.cpp | 62 +++++++++++++++++++++++++++++++ application/balance/estimator.h | 24 ++++++++++++ application/balance/leg.cpp | 1 - application/chassis/balance_old.c | 32 ++++++++-------- application/chassis/chassis.c | 20 +++++++--- application/robot_def.h | 4 +- 6 files changed, 119 insertions(+), 24 deletions(-) create mode 100644 application/balance/estimator.cpp create mode 100644 application/balance/estimator.h diff --git a/application/balance/estimator.cpp b/application/balance/estimator.cpp new file mode 100644 index 0000000..4268870 --- /dev/null +++ b/application/balance/estimator.cpp @@ -0,0 +1,62 @@ +// +// Created by SJQ on 2024/3/7. +// + +#include "estimator.h" +static const float dt = 1e-3; +//状态转移矩阵 +static float F[4] = {0,1, + 1,0}; +//控制矩阵 +static float B[2] = {dt, + 1}; +//观测矩阵 +static float H[4] = {1,0, + 0,1}; +//后验估计协方差初始值 +static float P[4] = {100, 0.1, + 0.1, 100}; +// P Q矩阵初始值（其实这里设置多少都无所谓） +static float Q[4] = {0.01, 0, + 0, 0.01}; +static float R[4] = {100000, 0, + 0, 100000,}; +estimator::estimator(float process_noise, float measure_noise) { + Kalman_Filter_Init(&EstimateKF_,2,1,2); + EstimateKF_.UseAutoAdjustment = 1; + + for (uint8_t i = 0; i < 4; i += 3) + { + // 初始化过程噪声与量测噪声 + Q[i] = process_noise; + R[i] = measure_noise; + } + memcpy(EstimateKF_.F_data,F,sizeof(F)); + memcpy(EstimateKF_.B_data,B,sizeof(B)); + memcpy(EstimateKF_.H_data,H,sizeof(H)); + memcpy(EstimateKF_.Q_data,Q,sizeof(Q)); + memcpy(EstimateKF_.R_data,R,sizeof(R)); + + DWT_GetDeltaT(&DWT_CNT_); +} + +void estimator::update(float x,float x_dot,float ax) { + EstimateKF_.MeasuredVector[0] = x; + EstimateKF_.MeasuredVector[1] = x_dot; + + EstimateKF_.ControlVector[0] = ax; + + Kalman_Filter_Update(&EstimateKF_); + + // 提取估计值 + for (uint8_t i = 0; i < 2; i++) + { + Estimate_X_[i] = EstimateKF_.FilteredValue[i]; + } +} + +void estimator::get_result(float state[2]) +{ + state[0] = Estimate_X_[0]; + state[1] = Estimate_X_[1]; +} \ No newline at end of file diff --git a/application/balance/estimator.h b/application/balance/estimator.h new file mode 100644 index 0000000..bbd81dc --- /dev/null +++ b/application/balance/estimator.h @@ -0,0 +1,24 @@ +// +// Created by SJQ on 2024/3/7. +// +#include "kalman_filter.h" +#include "bsp_dwt.h" +#ifndef WHEEL_LEGGED_ESTIMATOR_H +#define WHEEL_LEGGED_ESTIMATOR_H + + +class estimator { +public: + estimator(float process_noise, float measure_noise); + void get_result(float state[2]); + void update(float x,float x_dot,float ax); + +private: + KalmanFilter_t EstimateKF_; //使用KF作为状态观测器 + float Estimate_X_[2]; //观测器估计的状态量 + uint32_t DWT_CNT_; //计时用 + float dt_; +}; + + +#endif //WHEEL_LEGGED_ESTIMATOR_H diff --git a/application/balance/leg.cpp b/application/balance/leg.cpp index 34c98d8..411d519 100644 --- a/application/balance/leg.cpp +++ b/application/balance/leg.cpp @@ -147,7 +147,6 @@ float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) { float F_N = P + 0.8f * (zw_dot2 + 9.8f); return F_N; - return 0; } diff --git a/application/chassis/balance_old.c b/application/chassis/balance_old.c index 997453c..cd9b5f6 100644 --- a/application/chassis/balance_old.c +++ b/application/chassis/balance_old.c @@ -113,30 +113,30 @@ void mylqr_k(float L0, float K[12]) */ t2 = L0 * L0; t3 = t2 * L0; - /* 24-Jan-2024 14:05:09 */ - K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F; + /* 06-Mar-2024 21:40:29 */ + K_temp[0] = + ((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F; K_temp[1] = - ((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F; + ((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F; K_temp[2] = - ((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F; + ((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F; K_temp[3] = - ((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F; - K_temp[4] = - ((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F; + ((L0 * 5.66845322F - t2 * 16.5131245F) + t3 * 14.656208F) + 0.677895188F; + K_temp[4] = ((L0 * -7.06628323F + t2 * 13.4040155F) - t3 * 8.83661747F) - 8.61461F; K_temp[5] = - ((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F; - K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) - - 10.4576302F; + ((L0 * -28.798315F + t2 * 17.1446743F) + t3 * 15.3877935F) + 11.4884624F; + K_temp[6] = + ((L0 * -5.75727034F - t2 * 3.09837651F) + t3 * 10.1307449F) - 9.7942028F; K_temp[7] = - ((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F; + ((L0 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F; K_temp[8] = - ((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F; + ((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F; K_temp[9] = - ((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F; - K_temp[10] = - ((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F; + ((L0 * 124.852882F - t2 * 253.387085F) + t3 * 193.295883F) + 55.7350769F; + K_temp[10] = ((L0 * -6.44499636F + t2 * 5.48124027F) - t3 * 0.516800761F) + + 5.52106953F; K_temp[11] = - ((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F; + ((L0 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F; //matlab赋值顺序不同 for (int i = 0; i < 6; ++i) { diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index dfedf8e..5cddf98 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -46,6 +46,7 @@ INS_t *Chassis_IMU_data; #ifdef ONE_BOARD static Publisher_t *chassis_pub; // 用于发布底盘的数据 static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令 +INS_t *Chassis_IMU_data; #endif // !ONE_BOARD static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 @@ -120,12 +121,13 @@ void ChassisInit() //@todo:瓴控驱动代码有些问题 1号电机必须先初始化 wheel_motor_config.can_init_config.tx_id = 1; - wheel_motor_l = LKMotorInit(&wheel_motor_config); - - wheel_motor_config.can_init_config.tx_id = 2; wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; wheel_motor_r = LKMotorInit(&wheel_motor_config); + wheel_motor_config.can_init_config.tx_id = 2; + wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; + wheel_motor_l = LKMotorInit(&wheel_motor_config); + referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI @@ -191,7 +193,8 @@ void ChassisInit() .Ki = 0, .Kd = 0.0f, .MaxOut = 2, - .Improve = PID_Derivative_On_Measurement, + .Improve = PID_Derivative_On_Measurement|PID_DerivativeFilter, + .Derivative_LPF_RC = 0.001f }; PIDInit(&Turn_Loop_PID,&turn_pid_cfg); @@ -404,7 +407,7 @@ void ChassisTask() - target_yaw = chassis_cmd_recv.wz * 0.001f;//Chassis_IMU_data->Gyro[Z] + target_yaw = 0.0f;//chassis_cmd_recv.wz * 0.001f;//Chassis_IMU_data->Gyro[Z] //float turn_T = -PIDCalculate(&Turn_Loop_PID,chassis_cmd_recv.offset_angle,0); //float turn_T = -1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); @@ -438,10 +441,17 @@ void ChassisTask() HTMotorSetRef(motor_lf,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF); HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF); +// HTMotorSetRef(motor_rf,0); +// HTMotorSetRef(motor_rb,0); +// +// HTMotorSetRef(motor_lf,0); +// HTMotorSetRef(motor_lb,0); } LKMotorSetRef(wheel_motor_r,wheel_current_r); LKMotorSetRef(wheel_motor_l,wheel_current_l); +// LKMotorSetRef(wheel_motor_r,100); +// LKMotorSetRef(wheel_motor_l,100); Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0); Leg_feedback_update(&BalanceControl.leg_right,motor_rf->measure.real_current/HT_CMD_COEF,motor_rb->measure.real_current/HT_CMD_COEF); diff --git a/application/robot_def.h b/application/robot_def.h index 8b8bec9..8fca5ec 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -17,8 +17,8 @@ #include "stdint.h" /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ -//#define ONE_BOARD // 单板控制整车 -#define CHASSIS_BOARD //底盘板 +#define ONE_BOARD // 单板控制整车 +//#define CHASSIS_BOARD //底盘板 // #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据