// // Created by SJQ on 2023/12/30. // #include "balance.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->LQR_l,InitConfig.LQR_state_num,InitConfig.LQR_control_num); LQR_Init(&balanceControl->LQR_r,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,0.001f,10.0f); Observer_Init(&balanceControl->state_observer); memset(balanceControl->state_r,0,6); memset(balanceControl->target_state_r,0,6); memset(balanceControl->state_l,0,6); memset(balanceControl->target_state_l,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,const float x[2],const float x_dot[2],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_r[12]={0}; float K_matrix_l[12]={0}; //右腿 float alpha_r = PI/2 - leg_r->legState.phi0; float theta_r = alpha_r - body_phi; float theta_dot_r = - leg_r->legState.phi0_dot - body_phi_dot; static uint32_t dot_cnt = 0; float dot_dt = DWT_GetDeltaT(&dot_cnt); float theta_dot2_r = (balanceControl->state_r[1] - theta_dot_r) / dot_dt; //左腿 float alpha_l = PI/2 - leg_l->legState.phi0; float theta_l = alpha_l - body_phi; float theta_dot_l = - leg_l->legState.phi0_dot - body_phi_dot; float theta_dot2_l = (balanceControl->state_l[1] - theta_dot_l) / dot_dt; //计算机体速度 float xb_r = x[0] + leg_r->legState.L0 * arm_sin_f32(theta_r); float xb_dot_r = x_dot[0] + (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r)); float xb_l = x[1] + leg_l->legState.L0 * arm_sin_f32(theta_l); float xb_dot_l = x_dot[1] + (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_l->legState.L0 * theta_dot_l * arm_cos_f32(theta_l)); float xb = (xb_r + xb_l)/2; float xb_dot = (xb_dot_r + xb_dot_l)/2; estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[1]); float x_est_r = balanceControl->v_estimator.Estimate_X_[0] - leg_r->legState.L0 * arm_sin_f32(theta_r); float x_dot_est_r = balanceControl->v_estimator.Estimate_X_[1] - (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r)); float x_est_l = balanceControl->v_estimator.Estimate_X_[0] - leg_l->legState.L0 * arm_sin_f32(theta_l); float x_dot_est_l = balanceControl->v_estimator.Estimate_X_[1] - (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_r->legState.L0 * theta_dot_l * arm_cos_f32(theta_l)); balanceControl->v_est[0] = x_est_r; balanceControl->v_est[1] = x_dot_est_r; balanceControl->v_est[2] = x_est_l; balanceControl->v_est[3] = x_dot_est_l; balanceControl->state_r[0] = theta_r; balanceControl->state_r[1] = theta_dot_r;//(1.0f - 0.75f) * balanceControl->state_r[1] + 0.75f * theta_dot_r; balanceControl->state_r[2] = x_est_r;//x[0];//balanceControl->v_estimator.Estimate_X_[0]; balanceControl->state_r[3] = x_dot_est_r;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1]; balanceControl->state_r[4] = body_phi; balanceControl->state_r[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_r[5] + 0.75f * body_phi_dot; balanceControl->state_l[0] = theta_l; balanceControl->state_l[1] = theta_dot_l;//(1.0f - 0.75f) * balanceControl->state_l[1] + 0.75f * theta_dot_l; balanceControl->state_l[2] = x_est_l;//x[0];//balanceControl->v_estimator.Estimate_X_[0]; balanceControl->state_l[3] = x_dot_est_l;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1]; balanceControl->state_l[4] = body_phi; balanceControl->state_l[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_l[5] + 0.75f * body_phi_dot; //地面支持力解算 float theta_state_r[3] = {theta_r,theta_dot_r,theta_dot2_r}; static uint32_t right_cnt = 0,left_cnt = 0; balanceControl->FN_right = calculate_F(MotionAccel[2],theta_state_r,leg_r,&right_cnt); float theta_state_l[3] = {theta_l,theta_dot_l,theta_dot2_l}; balanceControl->FN_left = calculate_F(MotionAccel[2],theta_state_l,leg_l,&left_cnt); mylqr_k(leg_r->legState.L0,K_matrix_r); mylqr_k(leg_l->legState.L0,K_matrix_l); //离地情况 //注意电机输出反向力矩也会导致判断离地 if((balanceControl->FN_right <= 10.0f || leg_r->legFeedback.F <= 10.0f) || (balanceControl->jump_flag == 1 ||balanceControl->jump_flag == 2) )//&& leg_r->legState.L0>=0.15f { balanceControl->fly_flag_r = TRUE; float lqr_K[12] = {0}; lqr_K[6] = K_matrix_r[6]; lqr_K[7] = K_matrix_r[7]; LQR_set_K(&balanceControl->LQR_r,lqr_K); } else { balanceControl->fly_flag_r = FALSE; LQR_set_K(&balanceControl->LQR_r,K_matrix_r); } if((balanceControl->FN_left <= 10.0f || leg_l->legFeedback.F <= 10.0f)|| (balanceControl->jump_flag == 1 || balanceControl->jump_flag == 2) )//&& leg_l->legState.L0>=0.15f { balanceControl->fly_flag_l = TRUE; float lqr_K[12] = {0}; lqr_K[6] = K_matrix_l[6]; lqr_K[7] = K_matrix_l[7]; LQR_set_K(&balanceControl->LQR_l,lqr_K); } else { balanceControl->fly_flag_l = FALSE; LQR_set_K(&balanceControl->LQR_l,K_matrix_l); } } #define F_FEEDFORWARD 30.0f //腿部支持力前馈 #define L_MAX 0.40f #define L_MIN 0.13f //腿长最大最小值 void Balance_Control_Loop(Balance_Control_t* balanceControl,uint8_t jump_flag) { LQR_update(&balanceControl->LQR_r,balanceControl->state_r,balanceControl->target_state_r); LQR_update(&balanceControl->LQR_l,balanceControl->state_l,balanceControl->target_state_l); balanceControl->LQR_r.control_vector[1] = - balanceControl->LQR_r.control_vector[1]; float Tp_r = balanceControl->LQR_r.control_vector[1]; //髋关节力矩 float T_r = balanceControl->LQR_r.control_vector[0]; //驱动轮力矩 balanceControl->LQR_l.control_vector[1] = - balanceControl->LQR_l.control_vector[1]; float Tp_l = balanceControl->LQR_l.control_vector[1]; //髋关节力矩 float T_l = balanceControl->LQR_l.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); //跳跃控制 static float F_jump_l,F_jump_r; float max_scale = 10.0f; float K = (max_scale - 1) * F_FEEDFORWARD / (L_MIN - L_MAX); F_jump_l = K*(leg_l->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD; F_jump_r = K*(leg_r->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD; //跳跃支持力 F_jump_l = float_constrain(F_jump_l,F_FEEDFORWARD,max_scale * F_FEEDFORWARD); F_jump_r = float_constrain(F_jump_r,F_FEEDFORWARD,max_scale * F_FEEDFORWARD); 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 = 0;//PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0); if(jump_flag == 1)//起跳阶段 { Leg_Input_update(&leg_r->legInput,F_jump_r,Tp_r + cor_Tp); Leg_Input_update(&leg_l->legInput,F_jump_l,Tp_l - cor_Tp); } else if(jump_flag == 2) //收腿阶段 { Leg_Input_update(&leg_r->legInput,-100,Tp_r + cor_Tp); Leg_Input_update(&leg_l->legInput,-100,Tp_l - cor_Tp); } else //非跳跃阶段 { Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 40,Tp_r + cor_Tp); Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 40,Tp_l - 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_r[3] = add_x; balanceControl->target_state_l[3] = add_x; if( fabsf(add_x) >= 0.01f) { balanceControl->target_state_r[2] = balanceControl->state_r[2]; balanceControl->target_state_l[2] = balanceControl->state_l[2]; } } void state_clear(Balance_Control_t* balanceControl) { //清除位移状态量 balanceControl->target_state_r[2] = 0; balanceControl->state_r[2] = 0; balanceControl->target_state_l[2] = 0; balanceControl->state_l[2] = 0; //estimator_init(&balanceControl->v_estimator,0.001f,10.0f); } void target_L_set(Balance_Control_t* balanceControl,float add_L) { balanceControl->target_L0 += add_L; balanceControl->target_L0 = float_constrain(balanceControl->target_L0,L_MIN,L_MAX); } /* * 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. */ /* 06-May-2024 17:35:50 */ t2 = L0*L0; t3 = t2*L0; K[0] = ((L0 * -282.545074F + t2 * 330.464752F) - t3 * 225.209259F) - 21.1259251F; K[1] = ((L0 * 191.247147F - t2 * 904.015686F) + t3 * 1025.96033F) + 44.8702393F; K[2] = ((L0 * -38.6851654F + t2 * 7.79979753F) - t3 * 7.8749814F) - 5.59480667F; K[3] = ((L0 * -10.0553722F - t2 * 60.9720421F) + t3 * 94.1053925F) + 17.016264F; K[4] = ((L0 * 8.41862774F - t2 * 39.7602615F) + t3 * 49.5394897F) - 9.96936703F; K[5] = ((L0 * 51.7300377F - t2 * 238.936829F) + t3 * 284.121246F) + 3.30276108F; K[6] = ((L0 * 58.7921486F - t2 * 213.884598F) + t3 * 239.798187F) - 32.5602646F; K[7] = ((L0 * 112.231422F - t2 * 583.832947F) + t3 * 718.246582F) + 13.6089172F; K[8] = ((L0 * -198.74147F + t2 * 196.315338F) - t3 * 42.9540405F) + 89.0673752F; K[9] = ((L0 * 348.149323F - t2 * 531.783142F) + t3 * 252.826599F) + 70.4726F; K[10] = ((L0 * -30.6359673F + t2 * 40.8268318F) - t3 * 21.4240017F) + 15.1511059F; K[11] = ((L0 * 40.7163315F - t2 * 35.2042122F) - t3 * 12.030282F) + 9.66719341F; //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; }