// // Created by SJQ on 2023/12/30. // #include "balance_old.h" static void mylqr_k(float L0, float K_mat[12]); void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig) { balanceControl->target_L0 = 0.15f; 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); estimator_init(&balanceControl->v_estimator,1,0.5); Observer_Init(&balanceControl->state_observer); memset(balanceControl->state,0,6); memset(balanceControl->target_state,0,6); } //解算地面支持力F float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last); // 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,const float MotionAccel[3]) { LegInstance *leg_r = &balanceControl->leg_right; LegInstance *leg_l = &balanceControl->leg_left; Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]); Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]); float K_matrix[12]={0}; float L_average = leg_r->legState.L0; //(leg_r->legState.L0 + leg_l->legState.L0)/2; //(leg_r->legState.L0 + leg_l->legState.L0)/2; float alpha = PI/2 - leg_r->legState.phi0; float theta = alpha - body_phi; float theta_dot = - leg_r->legState.phi0_dot - body_phi_dot; static uint32_t dot_cnt = 0; float dot_dt = DWT_GetDeltaT(&dot_cnt); float theta_dot2 = (balanceControl->state[1] - theta_dot) / dot_dt; //计算机体速度 float xb = x + 2 * leg_r->legState.L0 * arm_sin_f32(theta); float xb_dot = x_dot + 2*(leg_r->legState.L0_dot* arm_sin_f32(theta) + leg_r->legState.L0 * theta_dot * arm_cos_f32(theta)); estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[0]); balanceControl->state[0] = theta; balanceControl->state[1] = (1.0f - 0.75f) * balanceControl->state[1] + 0.75f * theta_dot; //balanceControl->state[1] = theta_dot; balanceControl->state[2] = x; balanceControl->state[3] = x_dot; balanceControl->state[4] = body_phi; balanceControl->state[5] = (1.0f - 0.75f) * balanceControl->state[5] + 0.75f * body_phi_dot; // balanceControl->state[5] = body_phi_dot; //Observer_Estimate(&balanceControl->state_observer,balanceControl->state,balanceControl->balance_LQR.control_vector,L_average); //地面支持力解算 //balanceControl->FN_right = calculate_F(MotionAccel[2],theta,theta_dot,leg_r->legState.L0,leg_r->legState.L0_dot,leg_r->legFeedback.F,leg_r->legFeedback.Tp); //balanceControl->FN_left = calculate_F(MotionAccel[2],theta,theta_dot,leg_l->legState.L0,leg_l->legState.L0_dot,leg_l->legFeedback.F,leg_l->legFeedback.Tp); float theta_state[3] = {theta,theta_dot,theta_dot2}; static uint32_t right_cnt = 0,left_cnt = 0; balanceControl->FN_right = calculate_F(MotionAccel[2],theta_state,leg_r,&right_cnt); balanceControl->FN_left = calculate_F(MotionAccel[2],theta_state,leg_l,&left_cnt); mylqr_k(L_average,K_matrix); //离地情况 if(balanceControl->FN_right <= 20.0f && balanceControl->FN_left <= 20.0f) { float lqr_K[12] = {0}; lqr_K[6] = K_matrix[6]; lqr_K[7] = K_matrix[7]; LQR_set_K(&balanceControl->balance_LQR,lqr_K); } else { 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,balanceControl->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 + 30,Tp + cor_Tp); Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 30,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 add_x) { balanceControl->target_state [2] += add_x; } void state_clear(Balance_Control_t* balanceControl) { //清除位移状态量 balanceControl->target_state [2] = 0; balanceControl->state[2] = 0; } void target_L_set(Balance_Control_t* balanceControl,float add_L) { balanceControl->target_L0 += add_L; float_constrain(balanceControl->target_L0,0.15f,0.30f); } /* * MYLQR_K MATLAB生成 * K = MYLQR_K(L0) * * Arguments : float L0 * float K[12] * Return Type : void */ void mylqr_k(float L0, float K_mat[12]) { float t2; float t3; float K[12]; /* This function was generated by the Symbolic Math Toolbox version 9.2. */ t2 = L0 * L0; t3 = t2 * L0; /* 11-Mar-2024 19:12:17 */ K[0] = ((L0 * -201.705292F + t2 * 289.584625F) - t3 * 247.715317F) - 14.6884327F; K[1] = ((L0 * 133.66568F - t2 * 384.819702F) + t3 * 354.437653F) + 9.50502F; K[2] = ((L0 * -20.6946182F - t2 * 2.9124589F) - t3 * 2.76665568F) - 3.57213783F; K[3] = ((L0 * 2.46232843F - t2 * 18.8637314F) + t3 * 18.9820461F) + 4.76390266F; K[4] = ((L0 * 16.4841461F - t2 * 49.2097549F) + t3 * 45.4498405F) - 23.5533714F; K[5] = ((L0 * 182.920456F - t2 * 598.236755F) + t3 * 622.566F) - 10.9050007F; K[6] = ((L0 * 56.6551895F - t2 * 155.341293F) + t3 * 140.137665F) - 27.346283F; K[7] = ((L0 * 165.648911F - t2 * 564.416138F) + t3 * 604.892395F) - 9.52724552F; K[8] = ((L0 * -97.1309662F + t2 * 154.570053F) - t3 * 113.610588F) + 42.7205772F; K[9] = ((L0 * -20.722168F + t2 * 194.949692F) - t3 * 261.115692F) + 32.8661461F; K[10] = ((L0 * -16.3245182F + t2 * 33.2480202F) - t3 * 28.061224F) + 7.589993F; K[11] = ((L0 * -17.9091167F + t2 * 72.7530289F) - t3 * 82.8507385F) + 4.82622719F; //matlab赋值顺序不同 for (int i = 0; i < 6; ++i) { K_mat[i] = K[2*i]; } for (int i = 0; i < 6; ++i) { K_mat[6+i] = K[2*i+1]; } } /* * File trailer for mylqr_k.c * * [EOF] */ /** * @brief 解算地面支持力 F * @param float theta[3] = {theta,theta_dot,theta_dot2} * @retval none * @attention */ float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last) { //求导所用时间 //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 L = leg->legState.L0; float L_dot = leg->legState.L0_dot; float L_dot2 = leg->legState.L0_dot2; float cos_theta = arm_cos_f32(theta[0]); float sin_theta = arm_sin_f32(theta[0]); float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta[1] * sin_theta + L * theta[2] * sin_theta + L * theta[1] * theta[1] * cos_theta; float P = leg->legFeedback.F * cos_theta + leg->legFeedback.Tp * sin_theta/L; float F_N = P + 0.8f * (zw_dot2 + 9.8f); return F_N; }