// // Created by SJQ on 2023/12/30. // #include "balance_old.h" static void mylqr_k(float L0, float K[12]); void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig) { Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig); Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig); LQR_Init(&balanceControl->balance_LQR,InitConfig.LQR_state_num,InitConfig.LQR_control_num); PIDInit(&balanceControl->leg_coordination_PID,&InitConfig.leg_cor_PID_config); PIDInit(&balanceControl->leg_length_PID,&InitConfig.legInitConfig.length_PID_conf); memset(balanceControl->state,0,6); memset(balanceControl->target_state,0,6); } //解算地面支持力F float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb); // leg_phi[4] = {phi1_r,phi4_r,phi1_l,phi4_l} void Balance_feedback_update(Balance_Control_t* balanceControl,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_State_update(&balanceControl->leg_right,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]); Leg_State_update(&balanceControl->leg_left,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]); float K_matrix[12]={0}; float L_average = (balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2; //(balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2; float alpha = PI/2 - balanceControl->leg_right.legState.phi0; float theta = alpha - body_phi; // static uint32_t dot_cnt = 0; // float dot_dt = DWT_GetDeltaT(&dot_cnt); // float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt; float theta_dot = - balanceControl->leg_right.legState.phi0_dot - body_phi_dot; balanceControl->state[0] = theta; balanceControl->state[1] = theta_dot; balanceControl->state[2] = x; balanceControl->state[3] = x_dot; balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot; mylqr_k(L_average,K_matrix); LQR_set_K(&balanceControl->balance_LQR,K_matrix); } void Balance_Control_Loop(Balance_Control_t* balanceControl) { LQR_update(&balanceControl->balance_LQR,balanceControl->state,balanceControl->target_state); balanceControl->balance_LQR.control_vector[1] = - balanceControl->balance_LQR.control_vector[1]; float Tp = balanceControl->balance_LQR.control_vector[1]; //髋关节力矩 float T = balanceControl->balance_LQR.control_vector[0]; //驱动轮力矩 LegInstance *leg_r,*leg_l; leg_r = &balanceControl->leg_right; leg_l = &balanceControl->leg_left; //两条腿独立控制腿长 float F_pid_out_r = PIDCalculate(&leg_r->Length_PID,leg_r->legState.L0,leg_r->target_L0); float F_pid_out_l = PIDCalculate(&leg_l->Length_PID,leg_l->legState.L0,leg_l->target_L0); //控制平均腿长 float average_length = (leg_r->legState.L0 + leg_l->legState.L0)/2; float F_pid_out= PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0); //float F_pid_out_l = PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0); float Tp_pid_out_r = PIDCalculate(&leg_r->Phi0_PID,leg_r->legState.phi0,leg_r->target_phi0); float Tp_pid_out_l = PIDCalculate(&leg_l->Phi0_PID,leg_l->legState.phi0,leg_l->target_phi0); float coordination_err = leg_r->legState.phi0 - leg_l->legState.phi0; float cor_Tp = PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0); Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 20,Tp + cor_Tp); Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 20,Tp - cor_Tp); // Leg_Input_update(&leg_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp); // Leg_Input_update(&leg_l->legInput,F_pid_out_l + 20 ,Tp - cor_Tp); VMC_getT(leg_r); VMC_getT(leg_l); } void target_x_set(Balance_Control_t* balanceControl,float x) { balanceControl->target_state [2] = x; } /* * MYLQR_K MATLAB生成 * K = MYLQR_K(L0) * * Arguments : float L0 * float K[12] * Return Type : void */ 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_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]; } } /* * File trailer for mylqr_k.c * * [EOF] */ //解算地面支持力F float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb) { //求导所用时间 static uint32_t dot_cnt = 0; static float last_theta_dot,last_L_dot; 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 = F_fb*cos_theta + Tp_fb*sin_theta/L; float F_N = P + 0.8f * (zw_dot2 + 9.8f); return F_N; }