From f2e3b63eac0c4aacc7104f88097c785fec562f2c Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Wed, 6 Mar 2024 19:44:56 +0800 Subject: [PATCH] =?UTF-8?q?cpp=E9=87=8D=E6=9E=84=E5=B9=B3=E8=A1=A1?= =?UTF-8?q?=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 6 +- application/balance/balance.cpp | 106 +++++++ application/balance/balance.h | 38 +++ application/balance/leg.cpp | 156 +++++++++++ application/balance/leg.h | 69 +++++ .../chassis/{balance.c => balance_old.c} | 49 ++-- .../chassis/{balance.h => balance_old.h} | 10 +- application/chassis/chassis.c | 2 +- application/chassis/{leg.c => leg_old.c} | 52 +--- application/chassis/{leg.h => leg_old.h} | 6 +- application/gimbal/gimbal.c | 2 +- application/robot_def.h | 2 +- modules/algorithm/LQR.cpp | 24 ++ modules/algorithm/LQR.h | 34 +-- modules/algorithm/{LQR.c => LQR_old.c} | 2 +- modules/algorithm/LQR_old.h | 30 ++ modules/matrix/matrix.cpp | 24 ++ modules/matrix/matrix.h | 259 ++++++++++++++++++ modules/matrix/utils.cpp | 56 ++++ modules/matrix/utils.h | 27 ++ 20 files changed, 845 insertions(+), 109 deletions(-) create mode 100644 application/balance/balance.cpp create mode 100644 application/balance/balance.h create mode 100644 application/balance/leg.cpp create mode 100644 application/balance/leg.h rename application/chassis/{balance.c => balance_old.c} (91%) rename application/chassis/{balance.h => balance_old.h} (87%) rename application/chassis/{leg.c => leg_old.c} (83%) rename application/chassis/{leg.h => leg_old.h} (94%) create mode 100644 modules/algorithm/LQR.cpp rename modules/algorithm/{LQR.c => LQR_old.c} (98%) create mode 100644 modules/algorithm/LQR_old.h create mode 100644 modules/matrix/matrix.cpp create mode 100644 modules/matrix/matrix.h create mode 100644 modules/matrix/utils.cpp create mode 100644 modules/matrix/utils.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ae9f64..d48aef4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) # project settings project(wheel_legged C CXX ASM) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 11) set(CMAKE_C_STANDARD 11) #Uncomment for hardware floating point @@ -53,9 +53,9 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310 modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd - modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/power_meter + modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/power_meter modules/matrix modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor - application application/chassis application/cmd application/gimbal application/shoot + application application/chassis application/cmd application/gimbal application/shoot application/balance Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) include_directories(Middlewares/ST/ARM/DSP/Inc) diff --git a/application/balance/balance.cpp b/application/balance/balance.cpp new file mode 100644 index 0000000..be07ae2 --- /dev/null +++ b/application/balance/balance.cpp @@ -0,0 +1,106 @@ +// +// Created by SJQ on 2024/3/2. +// + +#include "balance.h" +void get_lqr_k(float L0, float K[12]); + +balance::balance(Balance_Init_config_s *InitConfig): +leg_r_(&InitConfig->legInitConfig),leg_l_(&InitConfig->legInitConfig) +{ + PIDInit(&leg_coordination_PID_,&InitConfig->leg_cor_PID_config); + PIDInit(&leg_length_PID_,&InitConfig->legInitConfig.length_PID_conf); + memset(state_,0,6); + memset(target_state_,0,6); +} + +void balance::feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z) +{ + leg_r_.state_update(leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]); + leg_l_.state_update(leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]); + + Matrixf<4,1> leg_r_state = leg_r_.get_state(); + Matrixf<4,1> leg_l_state = leg_l_.get_state(); + float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2; + float phi_average = (leg_r_state[1][0] + leg_l_state[1][0])/2; + float phi_dot_average = (leg_r_state[3][0] + leg_l_state[3][0])/2; + + float theta = PI/2 - phi_average - body_phi; + float theta_dot = -phi_dot_average - body_phi_dot; + state_[0] = theta; + state_[1] = theta_dot; + state_[2] = x; state_[3] = x_dot; + state_[4] = body_phi; state_[5] = body_phi_dot; + + float K_mat[12]; + get_lqr_k(L_average,K_mat); + balance_LQR_.set_k(K_mat); +} + +void balance::control_loop() { + balance_LQR_.update(state_,target_state_); + + Matrixf<2,1> control = balance_LQR_.get_control(); + Matrixf<4,1> leg_r_state = leg_r_.get_state(); + Matrixf<4,1> leg_l_state = leg_l_.get_state(); + + float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2; + float F_pid_out= PIDCalculate(&leg_length_PID_,L_average,target_length_); + float coordination_err = leg_r_state[1][0] - leg_l_state[1][0]; + float cor_Tp = PIDCalculate(&leg_coordination_PID_,coordination_err,0); + + leg_r_.input_update(F_pid_out + leg_r_.F_feedforward_ + 20,control[1][0] + cor_Tp); + leg_l_.input_update(F_pid_out + leg_l_.F_feedforward_ + 20,control[1][0] - cor_Tp); +} + +void balance::target_set(float x) { + target_state_[2] = x; +} + +Matrixf<2,1> balance::get_control() { + return balance_LQR_.get_control(); +} + +void get_lqr_k(float L0, float K[12]) +{ + float t2; + float t3; + + float K_temp[12]; + /* This function was generated by the Symbolic Math Toolbox version 9.2. + */ + 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; + K_temp[1] = + ((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F; + K_temp[2] = + ((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F; + 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; + 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; + K_temp[7] = + ((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F; + K_temp[8] = + ((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F; + 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; + K_temp[11] = + ((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F; + + //matlab赋值顺序不同 + for (int i = 0; i < 6; ++i) { + K[i] = K_temp[2*i]; + } + for (int i = 0; i < 6; ++i) { + K[6+i] = K_temp[2*i+1]; + } +} \ No newline at end of file diff --git a/application/balance/balance.h b/application/balance/balance.h new file mode 100644 index 0000000..f448e58 --- /dev/null +++ b/application/balance/balance.h @@ -0,0 +1,38 @@ +// +// Created by SJQ on 2024/3/2. +// + +#ifndef WHEEL_LEGGED_BALANCE_H +#define WHEEL_LEGGED_BALANCE_H + +#include "leg.h" +#include "LQR.h" + +typedef struct +{ + Leg_init_config_s legInitConfig; + PID_Init_Config_s leg_cor_PID_config; +}Balance_Init_config_s; + +class balance { +public: + balance(Balance_Init_config_s* InitConfig); + void feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z); + void control_loop(); + void target_set(float x); + Matrixf<2,1> get_control(); + +private: + leg leg_r_; + leg leg_l_; + float target_length_; //平均腿长目标值 + PIDInstance leg_coordination_PID_; //双腿协调PD控制 + PIDInstance leg_length_PID_; //平均腿长控制 + + float state_[6]; + float target_state_[6]; + LQR<6,2> balance_LQR_; +}; + + +#endif //WHEEL_LEGGED_BALANCE_H diff --git a/application/balance/leg.cpp b/application/balance/leg.cpp new file mode 100644 index 0000000..34c98d8 --- /dev/null +++ b/application/balance/leg.cpp @@ -0,0 +1,156 @@ +// +// Created by SJQ on 2024/3/1. +// + + +#include "leg.h" +#include + + +static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f}; + +leg::leg(Leg_init_config_s* legInitConfig) +{ + PIDInit(&Length_PID_,&legInitConfig->length_PID_conf); + PIDInit(&Phi0_PID_,&legInitConfig->phi0_PID_conf); + + target_L0_ = legInitConfig->init_target_L0; + F_feedforward_ = legInitConfig->F_feedforward; +} + +void leg::input_update(float F, float Tp) +{ + legInput_ = Matrixf<2,1>(new (float[2]){F,Tp}); +} + +void leg::state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot) +{ + if(phi1<=PI/2) + phi1 = PI/2; + if(phi4>=PI/2) + phi4 = PI/2; + + legState_.phi1 = phi1; + legState_.phi4 = phi4; + + //以机体中点建立坐标系 + float xb = -l[4]/2+l[0]* arm_cos_f32(legState_.phi1); + float yb = l[0]* arm_sin_f32(legState_.phi1); + + float xd = l[4]/2+l[3]* arm_cos_f32(legState_.phi4); + float yd = l[3]* arm_sin_f32(legState_.phi4); + + float A0 = 2*l[1]*(xd-xb); + float B0 = 2*l[1]*(yd-yb); + + float lbd2 = (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb); + float C0 = l[1]*l[1]+lbd2-l[2]*l[2]; + + float tanPhi = 0; + + arm_sqrt_f32((A0*A0 + B0*B0 - C0*C0),&tanPhi); + //tanPhi = (B0+tanPhi)/(A0+C0); + + float phi2 = 2* atan2f(B0+tanPhi,A0+C0); + float xc = xb + l[1] * arm_cos_f32(phi2); + float yc = yb + l[1] * arm_sin_f32(phi2); + + float phi3 = acosf(xc-xd/l[2]); + float L0 = 0; + arm_sqrt_f32((xc*xc + yc*yc),&L0); + float phi0 = atan2f(yc,xc); + + //赋值 + legState_.L0 = L0; + legState_.phi0 = phi0; + legState_.phi2 = phi2; + legState_.phi3 = phi3; + + get_dot(phi1_dot,phi4_dot); +} + +void leg::calculate_output() { + float sinPhi23 = arm_sin_f32(legState_.phi2 - legState_.phi3); + float sinPhi12 = arm_sin_f32(legState_.phi1 - legState_.phi2); + float sinPhi34 = arm_sin_f32(legState_.phi3 - legState_.phi4); + + float sinPhi03 = arm_sin_f32(legState_.phi0 - legState_.phi3); + float sinPhi02 = arm_sin_f32(legState_.phi0 - legState_.phi2); + float cosPhi03 = arm_cos_f32(legState_.phi0 - legState_.phi3); + float cosPhi02 = arm_cos_f32(legState_.phi0 - legState_.phi2); + + float Jacobi_data[4] = {0}; + Jacobi_data[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23; + Jacobi_data[1] = -(l[0]* cosPhi03*sinPhi12)/(legState_.L0 * sinPhi23); + + Jacobi_data[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23; + Jacobi_data[3] = -(l[3]* cosPhi02*sinPhi34)/(legState_.L0 * sinPhi23); + + VMC_Jacobi_ = Matrixf<2,2>(Jacobi_data); + legOutput_ = VMC_Jacobi_ * legInput_; +} + +void leg::feedback_update(float T1_fb, float T2_fb) { + Matrixf<2,1> T12(new (float[2]){T1_fb,T2_fb}); + legFeedback_ = matrixf::inv(VMC_Jacobi_) * T12; +} + +void leg::get_dot(float phi1_dot,float phi4_dot) +{ + //多项式拟合方法获得雅可比矩阵 待测试 + float phi_1 = legState_.phi1; + float phi_4 = legState_.phi4; + + float phi1_sq = phi_1*phi_1; + float phi4_sq = phi_4*phi_4; + float pose_Jacobi[4] = {0}; + float temp = sqrtf(powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2)); + + pose_Jacobi[0] = + (0.5f*(2.0f*(0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - 2.0f*(0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp; + pose_Jacobi[1] = + (0.5f*(2.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + 2.0f*(0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp; + + float temp2 = (powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2)/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + 1.0f); + + pose_Jacobi[2] = + ((0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + ((0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2))/temp2; + pose_Jacobi[3] = + -(1.0f*((0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - (1.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2)))/temp2; + Matrixf<2,2> pose_J(pose_Jacobi); + + State_dot_ = pose_J * Matrixf<2,1>(new (float[2]){phi1_dot,phi4_dot}); +} + +Matrixf<4, 1> leg::get_state() { + return Matrixf<4, 1>(new (float[4]) {legState_.L0,legState_.phi0,*State_dot_[0],*State_dot_[1]}); +} + +float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) { + //求导所用时间 + static uint32_t dot_cnt = 0; + static float last_theta_dot,last_L_dot; + float L = legState_.L0; + float L_dot = State_dot_[0][0]; + + float dot_dt = DWT_GetDeltaT(&dot_cnt); + float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt; + float L_dot2 = (last_L_dot - L_dot) / dot_dt; + + float cos_theta = arm_cos_f32(theta); + float sin_theta = arm_sin_f32(theta); + float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta_dot * sin_theta + + L * theta_dot2 * sin_theta + L * theta_dot * theta_dot * cos_theta; + + float P = legFeedback_[0][0]*cos_theta + legFeedback_[1][0]*sin_theta/L; + + float F_N = P + 0.8f * (zw_dot2 + 9.8f); + + return F_N; + return 0; +} + + + + + diff --git a/application/balance/leg.h b/application/balance/leg.h new file mode 100644 index 0000000..ab3aa96 --- /dev/null +++ b/application/balance/leg.h @@ -0,0 +1,69 @@ +// +// Created by SJQ on 2024/3/1. +// + +#ifndef WHEEL_LEGGED_LEG_H +#define WHEEL_LEGGED_LEG_H + +#include +#include "controller.h" + +typedef struct +{ + float L0; + float phi0; + + float phi1; + float phi4; + + float phi2; + float phi3; + + uint32_t DWT_CNT; + float dt; //用于计算时间并求导 + float last_phi0; + float phi0_dot; + + float L0_dot; + +}Leg_State_t; + +typedef struct +{ + PID_Init_Config_s length_PID_conf; + PID_Init_Config_s phi0_PID_conf; + float init_target_L0; //初始腿长目标值 + float F_feedforward; +}Leg_init_config_s; + +class leg +{ +public: + leg(Leg_init_config_s* legInitConfig); + void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot); + void input_update(float F,float Tp); + void feedback_update(float T1_fb,float T2_fb); + void calculate_output(); + Matrixf<4,1> get_state(); + float calculate_FN(float MotionAccel_z,float theta,float theta_dot); + + float F_feedforward_; //支持力前馈补偿 +private: + Leg_State_t legState_; + Matrixf<2,1> legInput_; + Matrixf<2,1> legOutput_; + Matrixf<2,1> legFeedback_; + + Matrixf<2,1> State_dot_; // L0 和 phi0 的导数 + + float target_L0_; + float target_phi0_; + PIDInstance Length_PID_; + PIDInstance Phi0_PID_; + + Matrixf<2,2> VMC_Jacobi_; + + void get_dot(float phi1_dot,float phi4_dot); +}; + +#endif //WHEEL_LEGGED_LEG_H diff --git a/application/chassis/balance.c b/application/chassis/balance_old.c similarity index 91% rename from application/chassis/balance.c rename to application/chassis/balance_old.c index 35629ac..997453c 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance_old.c @@ -2,7 +2,7 @@ // Created by SJQ on 2023/12/30. // -#include "balance.h" +#include "balance_old.h" static void mylqr_k(float L0, float K[12]); @@ -47,16 +47,7 @@ void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4], balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot; mylqr_k(L_average,K_matrix); - float K_matrix_fix[12]; - for (int i = 0; i < 6; ++i) { - K_matrix_fix[i] = K_matrix[2*i]; - } - for (int i = 0; i < 6; ++i) { - K_matrix_fix[6+i] = K_matrix[2*i+1]; - } - - - LQR_set_K(&balanceControl->balance_LQR,K_matrix_fix); + LQR_set_K(&balanceControl->balance_LQR,K_matrix); } @@ -116,34 +107,44 @@ void mylqr_k(float L0, float K[12]) { float t2; float t3; + + float K_temp[12]; /* This function was generated by the Symbolic Math Toolbox version 9.2. */ t2 = L0 * L0; t3 = t2 * L0; /* 24-Jan-2024 14:05:09 */ - K[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F; - K[1] = + K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F; + K_temp[1] = ((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F; - K[2] = + K_temp[2] = ((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F; - K[3] = + K_temp[3] = ((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F; - K[4] = + K_temp[4] = ((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F; - K[5] = + K_temp[5] = ((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F; - K[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) - + K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) - 10.4576302F; - K[7] = + K_temp[7] = ((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F; - K[8] = + K_temp[8] = ((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F; - K[9] = + K_temp[9] = ((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F; - K[10] = + K_temp[10] = ((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F; - K[11] = + K_temp[11] = ((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F; + + //matlab赋值顺序不同 + for (int i = 0; i < 6; ++i) { + K[i] = K_temp[2*i]; + } + for (int i = 0; i < 6; ++i) { + K[6+i] = K_temp[2*i+1]; + } } /* @@ -169,7 +170,7 @@ float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float float P = F_fb*cos_theta + Tp_fb*sin_theta/L; - float F_N = P + 0.8 * (zw_dot2 + 9.8); + float F_N = P + 0.8f * (zw_dot2 + 9.8f); return F_N; diff --git a/application/chassis/balance.h b/application/chassis/balance_old.h similarity index 87% rename from application/chassis/balance.h rename to application/chassis/balance_old.h index bb12e40..28e39c4 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance_old.h @@ -2,11 +2,11 @@ // Created by SJQ on 2023/12/30. // -#ifndef WHEEL_LEGGED_BALANCE_H -#define WHEEL_LEGGED_BALANCE_H +#ifndef WHEEL_LEGGED_BALANCE_OLD_H +#define WHEEL_LEGGED_BALANCE_OLD_H -#include "leg.h" -#include "LQR.h" +#include "leg_old.h" +#include "LQR_old.h" #include "controller.h" #include "kalman_filter.h" typedef struct @@ -40,4 +40,4 @@ void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4], void Balance_Control_Loop(Balance_Control_t* balanceControl); void target_x_set(Balance_Control_t* balanceControl,float x); -#endif //WHEEL_LEGGED_BALANCE_H +#endif //WHEEL_LEGGED_BALANCE_OLD_H diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 028f7b8..dfedf8e 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -22,7 +22,7 @@ #include "power_meter.h" #include "ins_task.h" -#include "balance.h" +#include "balance_old.h" #include "observer.h" #include "general_def.h" diff --git a/application/chassis/leg.c b/application/chassis/leg_old.c similarity index 83% rename from application/chassis/leg.c rename to application/chassis/leg_old.c index 29ed3cf..e76af67 100644 --- a/application/chassis/leg.c +++ b/application/chassis/leg_old.c @@ -2,7 +2,7 @@ // Created by SJQ on 2023/12/25. // -#include "leg.h" +#include "leg_old.h" #include "math.h" #include "main.h" #include "arm_math.h" @@ -179,56 +179,6 @@ void Leg_feedback_update(LegInstance* legInstance,float T1_fb,float T2_fb) legInstance->legFeedback.Tp = F_Tp_array[1]; } - -////此段代码为matlab生成 -//static void calculate_leg_pos(const float phi1, const float phi4, float leg_pos[4]) -//{ -// float A0; -// float B0; -// float C0; -// float L0; -// float phi2; -// float scale; -// float xc; -// float xd; -// float yb_tmp; -// /* 以机体中点建立坐标系 */ -// L0 = 0.15F * cosf(phi1); -// yb_tmp = 0.15F * sinf(phi1); -// xd = 0.15F * cosf(phi4) + 0.075F; -// A0 = 0.54F * (xd - (L0 - 0.075F)); -// C0 = 0.15F * sinf(phi4) - yb_tmp; -// B0 = 0.54F * C0; -// scale = (0.15F * cosf(phi4) + 0.075F) - (L0 - 0.075F); -// C0 = ((scale * scale + C0 * C0) + 0.0729000047F) - 0.0729000047F; -// phi2 = 2.0F * atanf((B0 + sqrtf((A0 * A0 + B0 * B0) - C0 * C0)) / (A0 + C0)); -// xc = L0 + 0.27F * cosf(phi2); -// C0 = yb_tmp + 0.27F * sinf(phi2); -// scale = 1.29246971E-26F; -// A0 = fabsf(xc); -// if (A0 > 1.29246971E-26F) { -// L0 = 1.0F; -// scale = A0; -// } else { -// B0 = A0 / 1.29246971E-26F; -// L0 = B0 * B0; -// } -// A0 = fabsf(C0); -// if (A0 > scale) { -// B0 = scale / A0; -// L0 = L0 * B0 * B0 + 1.0F; -// scale = A0; -// } else { -// B0 = A0 / scale; -// L0 += B0 * B0; -// } -// L0 = scale * sqrtf(L0); -// leg_pos[0] = L0; -// leg_pos[1] = atanf(C0 / xc); -// leg_pos[2] = phi2; -// leg_pos[3] = acosf(xc - xd / 0.27F); -//} -// void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi3, float phi4, float F, float Tp, float T12[2]) { diff --git a/application/chassis/leg.h b/application/chassis/leg_old.h similarity index 94% rename from application/chassis/leg.h rename to application/chassis/leg_old.h index 9ce18a5..54d97ac 100644 --- a/application/chassis/leg.h +++ b/application/chassis/leg_old.h @@ -2,8 +2,8 @@ // Created by SJQ on 2023/12/25. // -#ifndef WHEEL_LEGGED_LEG_H -#define WHEEL_LEGGED_LEG_H +#ifndef WHEEL_LEGGED_LEG_OLD_H +#define WHEEL_LEGGED_LEG_OLD_H #include "controller.h" typedef struct @@ -78,4 +78,4 @@ void Leg_length_control_loop(LegInstance* legInstance); void VMC_getT(LegInstance* legInstance); -#endif //WHEEL_LEGGED_LEG_H +#endif //WHEEL_LEGGED_LEG_OLD_H diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 8035d58..93bed3b 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -9,7 +9,7 @@ #include "bmi088.h" #include "vofa.h" -static attitude_t *gimba_IMU_data; // 云台IMU数据 +static INS_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor, *pitch_motor; static ECMotorInstance *big_yaw_motor; diff --git a/application/robot_def.h b/application/robot_def.h index 915136d..8b8bec9 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -196,7 +196,7 @@ typedef struct typedef struct { - attitude_t gimbal_imu_data; + INS_t gimbal_imu_data; uint16_t yaw_motor_single_round_angle; } Gimbal_Upload_Data_s; diff --git a/modules/algorithm/LQR.cpp b/modules/algorithm/LQR.cpp new file mode 100644 index 0000000..8db5a5c --- /dev/null +++ b/modules/algorithm/LQR.cpp @@ -0,0 +1,24 @@ +// +// Created by SJQ on 2024/3/2. +// + +#include "LQR.h" + +template +void LQR<_state,_control>::set_k(float *K_value) +{ + K_ = Matrixf<_state,_control>((float[_state * _control])K_value); +} +template +void LQR<_state,_control>::update(float* new_state,float* target_state) +{ + state_vec_ = Matrixf<_state,1>((float[_state])new_state); + state_cmd_ = Matrixf<_state,1>((float[_state])target_state); + + control_vec_ = K_ * (state_cmd_ - state_vec_); +} +template +Matrixf<_control,1> LQR<_state,_control>::get_control() +{ + return control_vec_; +} \ No newline at end of file diff --git a/modules/algorithm/LQR.h b/modules/algorithm/LQR.h index 5dacaab..eb8be79 100644 --- a/modules/algorithm/LQR.h +++ b/modules/algorithm/LQR.h @@ -1,30 +1,26 @@ // -// Created by SJQ on 2023/12/29. +// Created by SJQ on 2024/3/2. // #ifndef WHEEL_LEGGED_LQR_H #define WHEEL_LEGGED_LQR_H -#include "user_lib.h" -typedef struct -{ - float* state_vector; - float* control_vector; - float* state_cmd; +#include +template +class LQR { +public: + LQR(){}; + void update(float* new_state,float* target_state); + void set_k(float *K_value); + Matrixf<_control,1> get_control(); +private: + Matrixf<_state,1> state_vec_; - int state_num; //状态量维数 - int control_num; //控制量维数 + Matrixf<_state,1> state_cmd_; + Matrixf<_control,1> control_vec_; - mat control_matrix; //其实是向量 定义成矩阵方便计算 @todo：更新DSP库 新版本库支持矩阵直接乘数组 - mat state_matrix; - mat state_cmd_matrix; + Matrixf<_control,_state> K_; +}; - mat state_err; - mat K_matrix; -}LQRInstance; - -void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt); -void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state); -void LQR_set_K(LQRInstance* lqrInstance, float *K_value); #endif //WHEEL_LEGGED_LQR_H diff --git a/modules/algorithm/LQR.c b/modules/algorithm/LQR_old.c similarity index 98% rename from modules/algorithm/LQR.c rename to modules/algorithm/LQR_old.c index cdda5f2..6d84ec5 100644 --- a/modules/algorithm/LQR.c +++ b/modules/algorithm/LQR_old.c @@ -2,7 +2,7 @@ // Created by SJQ on 2023/12/29. // -#include "LQR.h" +#include "LQR_old.h" void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt) { diff --git a/modules/algorithm/LQR_old.h b/modules/algorithm/LQR_old.h new file mode 100644 index 0000000..a1ee372 --- /dev/null +++ b/modules/algorithm/LQR_old.h @@ -0,0 +1,30 @@ +// +// Created by SJQ on 2023/12/29. +// + +#ifndef WHEEL_LEGGED_LQR_OLD_H +#define WHEEL_LEGGED_LQR_OLD_H +#include "user_lib.h" +typedef struct +{ + float* state_vector; + float* control_vector; + + float* state_cmd; + + int state_num; //状态量维数 + int control_num; //控制量维数 + + mat control_matrix; //其实是向量 定义成矩阵方便计算 @todo：更新DSP库 新版本库支持矩阵直接乘数组 + mat state_matrix; + mat state_cmd_matrix; + + mat state_err; + mat K_matrix; +}LQRInstance; + +void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt); +void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state); +void LQR_set_K(LQRInstance* lqrInstance, float *K_value); + +#endif //WHEEL_LEGGED_LQR_OLD_H diff --git a/modules/matrix/matrix.cpp b/modules/matrix/matrix.cpp new file mode 100644 index 0000000..51fc95a --- /dev/null +++ b/modules/matrix/matrix.cpp @@ -0,0 +1,24 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "matrix.h" + +// hat of vector +Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) { + float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0, + -vec[0][0], -vec[1][0], vec[0][0], 0}; + return Matrixf<3, 3>(hat); +} + +// cross product +Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) { + return vector3f::hat(vec1) * vec2; +} diff --git a/modules/matrix/matrix.h b/modules/matrix/matrix.h new file mode 100644 index 0000000..9acb070 --- /dev/null +++ b/modules/matrix/matrix.h @@ -0,0 +1,259 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef MATRIX_H +#define MATRIX_H +#include "stm32f4xx.h" +#include "arm_math.h" + +// Matrix class +template +class Matrixf { + public: + // Constructor without input data + Matrixf(void) : rows_(_rows), cols_(_cols) { + arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_); + } + // Constructor with input data + Matrixf(float data[_rows * _cols]) : Matrixf() { + memcpy(this->data_, data, _rows * _cols * sizeof(float)); + } + // Copy constructor + Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + } + // Destructor + ~Matrixf(void) {} + + // Row size + int rows(void) { return _rows; } + // Column size + int cols(void) { return _cols; } + + // Element + float* operator[](const int& row) { return &this->data_[row * _cols]; } + + // Operators + Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + return *this; + } + Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator*=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator/=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator*(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_); + return res; + } + friend Matrixf<_rows, _cols> operator*(const float& val, + const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator/(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_); + return res; + } + // Matrix multiplication + template + friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1, + const Matrixf<_cols, cols>& mat2) { + arm_status s; + Matrixf<_rows, cols> res; + s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_); + return res; + } + + // Submatrix + template + Matrixf block(const int& start_row, const int& start_col) { + Matrixf res; + for (int row = start_row; row < start_row + rows; row++) { + memcpy((float*)res[0] + (row - start_row) * cols, + (float*)this->data_ + row * _cols + start_col, + cols * sizeof(float)); + } + return res; + } + // Specific row + Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); } + // Specific column + Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); } + + // Transpose + Matrixf<_cols, _rows> trans(void) { + Matrixf<_cols, _rows> res; + arm_mat_trans_f32(&arm_mat_, &res.arm_mat_); + return res; + } + // Trace + float trace(void) { + float res = 0; + for (int i = 0; i < fmin(_rows, _cols); i++) { + res += (*this)[i][i]; + } + return res; + } + // Norm + float norm(void) { return sqrtf((this->trans() * *this)[0][0]); } + + public: + // arm matrix instance + arm_matrix_instance_f32 arm_mat_; + + protected: + // size + int rows_, cols_; + // data + float data_[_rows * _cols]; +}; + +// Matrix funtions +namespace matrixf { + +// Special Matrices +// Zero matrix +template +Matrixf<_rows, _cols> zeros(void) { + float data[_rows * _cols] = {0}; + return Matrixf<_rows, _cols>(data); +} +// Ones matrix +template +Matrixf<_rows, _cols> ones(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < _rows * _cols; i++) { + data[i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Identity matrix +template +Matrixf<_rows, _cols> eye(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < fmin(_rows, _cols); i++) { + data[i * _cols + i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Diagonal matrix +template +Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) { + Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>(); + for (int i = 0; i < fmin(_rows, _cols); i++) { + res[i][i] = vec[i][0]; + } + return res; +} + +// Inverse +template +Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) { + arm_status s; + // extended matrix [A|I] + Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>(); + for (int i = 0; i < _dim; i++) { + memcpy(ext_mat[i], mat[i], _dim * sizeof(float)); + ext_mat[i][_dim + i] = 1; + } + // elimination + for (int i = 0; i < _dim; i++) { + // find maximum absolute value in the first column in lower right block + float abs_max = fabs(ext_mat[i][i]); + int abs_max_row = i; + for (int row = i; row < _dim; row++) { + if (abs_max < fabs(ext_mat[row][i])) { + abs_max = fabs(ext_mat[row][i]); + abs_max_row = row; + } + } + if (abs_max < 1e-12f) { // singular + return matrixf::zeros<_dim, _dim>(); + s = ARM_MATH_SINGULAR; + } + if (abs_max_row != i) { // row exchange + float tmp; + Matrixf<1, 2 * _dim> row_i = ext_mat.row(i); + Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row); + memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float)); + memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float)); + } + float k = 1.f / ext_mat[i][i]; + for (int col = i; col < 2 * _dim; col++) { + ext_mat[i][col] *= k; + } + for (int row = 0; row < _dim; row++) { + if (row == i) { + continue; + } + k = ext_mat[row][i]; + for (int j = i; j < 2 * _dim; j++) { + ext_mat[row][j] -= k * ext_mat[i][j]; + } + } + } + // inv = ext_mat(:,n+1:2n) + s = ARM_MATH_SUCCESS; + Matrixf<_dim, _dim> res; + for (int i = 0; i < _dim; i++) { + memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float)); + } + return res; +} + +} // namespace matrixf + +namespace vector3f { + +// hat of vector +Matrixf<3, 3> hat(Matrixf<3, 1> vec); + +// cross product +Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2); + +} // namespace vector3f + +#endif // MATRIX_H diff --git a/modules/matrix/utils.cpp b/modules/matrix/utils.cpp new file mode 100644 index 0000000..1c4b42b --- /dev/null +++ b/modules/matrix/utils.cpp @@ -0,0 +1,56 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "utils.h" + +float math::limit(float val, const float& min, const float& max) { + if (min > max) + return val; + else if (val < min) + val = min; + else if (val > max) + val = max; + return val; +} + +float math::limitMin(float val, const float& min) { + if (val < min) + val = min; + return val; +} + +float math::limitMax(float val, const float& max) { + if (val > max) + val = max; + return val; +} + +float math::loopLimit(float val, const float& min, const float& max) { + if (min >= max) + return val; + if (val > max) { + while (val > max) + val -= (max - min); + } else if (val < min) { + while (val < min) + val += (max - min); + } + return val; +} + +float math::sign(const float& val) { + if (val > 0) + return 1; + else if (val < 0) + return -1; + return 0; +} + diff --git a/modules/matrix/utils.h b/modules/matrix/utils.h new file mode 100644 index 0000000..1515eb6 --- /dev/null +++ b/modules/matrix/utils.h @@ -0,0 +1,27 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef UTILS_H +#define UTILS_H + +#include + +namespace math { + +float limit(float val, const float& min, const float& max); +float limitMin(float val, const float& min); +float limitMax(float val, const float& max); +float loopLimit(float val, const float& min, const float& max); +float sign(const float& val); + +} // namespace math + +#endif // UTILS_H