diff --git a/Chassis_Task.c b/Chassis_Task.c deleted file mode 100644 index 101c9d9..0000000 --- a/Chassis_Task.c +++ /dev/null @@ -1,1358 +0,0 @@ -#include "Chassis_Task.h" -#include "bsp_buzzer.h" -#include "FreeRTOS.h" -#include "task.h" -#include "detect_task.h" -#include "remote_control.h" -#include "referee.h" -#include "can.h" -#include "bsp_can.h" -#include "arm_math.h" -#include "string.h" -#include "Kalman_Mix.h" -#include "INS_Task.h" -#include "math.h" -#include "VMC.h" -#include "mpc.h" -#include "lqr.h" -#include "user_lib.h" - - -#ifndef PI -#define PI 3.14159265358979f -#endif -#ifndef FOLLOW_DEADBAND -#define FOLLOW_DEADBAND 0.05f -#endif -#ifndef min -#define min(a,b) ((a)<(b)?(a):(b)) -#endif -#ifndef square -#define square(x) ((x)*(x)) -#endif - -#define ABS(x) (((x) > 0) ? (x) : (-(x))) -#define LimitMax(input, max) \ -{ \ - if ((input) > max) \ - { \ - input = max; \ - } \ - else if ((input) < -max) \ - { \ - input = -max; \ - } \ -} -#define LimitOutput(input, min, max ) \ -{ \ - if( input < min ) \ - input = min; \ - else if( input > max ) \ - input = max; \ -} - -#define SIGN(x) ((x)>0 ? 1 : ((x)<0? -1: 0)) - - -uint8_t use_mpc = 0, abnormal_debug = 0; -chassis_control_t chassis_control; -kalman2_state Body_Speed_KF; -// ------------------ PID info ------------------ -fp32 roll_PD[2] = {100.0, 20.0}; // 200, 45 -fp32 coordinate_PD[2] = { 10.0f, 0.5f }; //15.0f,1.0f -fp32 yaw_PD_test[2] = { 20.0f, 180.0f }; -fp32 stand_PD[2] = { 200.0f, 50.0f }; -fp32 jump_stand_PD[2] = { 10000.0f, 150.0f }; -fp32 suspend_stand_PD[2] = { 100.0f, 30.0f }; -// fp32 stand_PD[2]={150.0f, 20.0f};//1N/cm -fp32 suspend_foot_speed_p = 10.0f; -// fp32 High_Offset = 0.20; -fp32 High_Offset = 0.06; -fp32 Moving_High_Offset = 0.0f; -uint8_t last_is_update; -fp32 IDEAL_PREPARING_STAND_JUMPING_ANGLE = 0.6f; - -extern fp32 LQR[4][10]; - -fp32 suspend_LQR[2][6] = { - 20.0f,5.0f, 0.0f, 0.0f, 0.0f, 0.0f, - 0, 0, 0, 0, 0, 0 -}; -fp32 x_set[10]; -fp32 debug_2 = 1,upper_bound, stepp = 0.2; -fp32 debug_flag, last_debug_flag; -fp32 HIGH_HIGH = 0.35f; - -fp32 SIT_HIGH = 0.11f; -fp32 NORMAL_HIGH = 0.18f; //16 - -fp32 lower_dec_speed = 1.0f; -uint8_t mpc_cnt = 0, mpc_period = 9; -float alpha_dx = 0.95f; -fp32 rollP, rollD, roll_angle_deadband = 0.0f, roll_gyro_deadband = 0.0f, leg_dlength_deadband = 0.0f; -fp32 rotate_move_threshold = 45.0f, rotate_speed = 12.0f, fake_sign = 1.0f, rc_sign = 1.0f, rotate_move_scale = 1.0f,normal_move_scale = 2.0f, offset_k = 0.31f, fly_speed = 2.5f; -fp32 rc_angle, rc_angle_temp, X_speed, Y_speed, delta_theta, delta_theta_temp, rotate_move_offset, normalized_speed, temp_max_spd, acc_step = 1.3f, dec_step = 0.1f; -fp32 move_scale_list[11] = { 0.0, 1.0, 1.5, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0}; -fp32 cap_move_scale_list[11] = { 0.0, 1.5, 2.0, 2.5, 2.5, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0}; -fp32 rotate_speed_list[11] = {0.0, 5.0, 5.3, 5.6, 6.0, 6.0, 7.0, 8.0, 9.0, 10.0, 12.0}; -fp32 rotate_move_scale_list[11] = {0.0, 1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8}; -int speed_sign = 0, target_speed_sign = 0; -uint8_t robot_level = 1; -ext_robot_status_t *robot; -fp32 TK_x_p = -10.0f, TK_y_p = 10.0f, TK_y_d = 3.0f, reducing_p = 120.0f; -fp32 stablize_foot_speed_threshold = 1.2f, stablize_yaw_speed_threshold = 1.5f; -uint8_t no_follow_flag, follow_angle; - -enum Chassis_Mode chassis_mode; - - -// ------------------ function definition ------------------ -void Chassis_Init( chassis_control_t *init ); -void Chassis_Data_Update( chassis_control_t *fdb ); -void Chassis_Status_Detect( chassis_control_t *detect ); -void Chassis_Mode_Set( chassis_control_t *mode_set ); -void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change ); -void Target_Value_Set( chassis_control_t *target_value_set ); -void Chassis_Torque_Calculation( chassis_control_t *bl_ctrl ); -void Chassis_Torque_Combine( chassis_control_t *bl_ctrl ); -void Motor_CMD_Send(chassis_control_t *CMD_Send); -void Joint_Motor_to_Init_Pos(void); -void Motor_Zero_CMD_Send(void); -void HT_Motor_zero_set(void); - -int16_t set1, set2; -uint16_t launching_time = 0; - -void Chassis_Task(void const * argument) -{ - vTaskDelay(CHASSIS_TASK_INIT_TIME); - Chassis_Init(&chassis_control); - vTaskDelay(100); - while(1) - { - // ------------------ Update Data ------------------ - Chassis_Data_Update(&chassis_control); - - // ------------------ Chassis Control ------------------ - Chassis_Status_Detect(&chassis_control); - - // ------------------ Mode Set ------------------ - Chassis_Mode_Set(&chassis_control); - - Chassis_Mode_Change_Control_Transit(&chassis_control); - - Target_Value_Set(&chassis_control); - - Chassis_Torque_Calculation(&chassis_control); - - Chassis_Torque_Combine(&chassis_control); - - Motor_CMD_Send(&chassis_control); - - vTaskDelay(1); - } -} - - -void Chassis_Init( chassis_control_t *init ) -{ - // ------------------ Set HT Zero Point ------------------ - buzzer_on(75, 60); - vTaskDelay(100); - HT_Motor_zero_set(); - buzzer_off(); - - Motor_Zero_CMD_Send(); - - kalman_second_order_init(&Body_Speed_KF); - - // ------------------ Ptr Set ------------------ - init->joint_motor_1.motor_measure = get_HT_motor_measure_point(0); - init->joint_motor_2.motor_measure = get_HT_motor_measure_point(1); - init->joint_motor_3.motor_measure = get_HT_motor_measure_point(2); - init->joint_motor_4.motor_measure = get_HT_motor_measure_point(3); - // init->foot_motor_L.motor_measure = get_BMmotor_measure_point(0); - // init->foot_motor_R.motor_measure = get_BMmotor_measure_point(1); - init->chassis_rc_ctrl = get_Gimabl_control_point(); - init->chassis_posture_info.chassis_INS_angle_point = get_INS_angle_point(); - init->chassis_posture_info.chassis_INS_gyro_point = get_gyro_data_point(); - init->chassis_posture_info.chassis_INS_accel_point = get_acc_data_point(); - - /* - first_order_filter_init(&rc_filter, 0.002f, rc_filt_numb); - first_order_filter_init(&tl_filter, 0.002f, tl_numb); - first_order_filter_init(&tl_run_filter, 0.002f, tl_numb); - OLS_Init(&OLS_S0_L,2); - OLS_Init(&OLS_S0_R,2); - */ - - // ------------------ Mode Set ------------------ - init->mode.chassis_mode = init->mode.last_chassis_mode = DISABLE_CHASSIS; - init->mode.chassis_balancing_mode = init->mode.last_chassis_balancing_mode = NO_FORCE; - init->mode.sport_mode = init->mode.last_sport_mode = NORMAL_MOVING_MODE; - init->joint_motor_1.motor_mode = init->joint_motor_1.last_motor_mode = MOTOR_NO_FORCE; - init->joint_motor_2.motor_mode = init->joint_motor_2.last_motor_mode = MOTOR_NO_FORCE; - init->joint_motor_3.motor_mode = init->joint_motor_3.last_motor_mode = MOTOR_NO_FORCE; - init->joint_motor_4.motor_mode = init->joint_motor_4.last_motor_mode = MOTOR_NO_FORCE; - init->foot_motor_L.motor_mode = init->foot_motor_L.last_motor_mode = MOTOR_NO_FORCE; - init->foot_motor_R.motor_mode = init->foot_motor_R.last_motor_mode = MOTOR_NO_FORCE; - - // ------------------ Initialize HT Motor ------------------ - init->joint_motor_1.position_offset = init->joint_motor_1.motor_measure->ecd; - init->joint_motor_2.position_offset = init->joint_motor_2.motor_measure->ecd; - init->joint_motor_3.position_offset = init->joint_motor_3.motor_measure->ecd; - init->joint_motor_4.position_offset = init->joint_motor_4.motor_measure->ecd; - init->joint_motor_1.position = (init->joint_motor_1.motor_measure->ecd -init->joint_motor_1.position_offset) + LEG_OFFSET; - init->joint_motor_2.position = (init->joint_motor_2.motor_measure->ecd -init->joint_motor_2.position_offset) - LEG_OFFSET; - init->joint_motor_3.position = (init->joint_motor_3.motor_measure->ecd -init->joint_motor_3.position_offset) + LEG_OFFSET; - init->joint_motor_4.position = (init->joint_motor_4.motor_measure->ecd -init->joint_motor_4.position_offset) - LEG_OFFSET; - - init->flag_info.init_flag = 1; - Chassis_Data_Update(init); - init->flag_info.init_flag = 0; - - init->foot_motor_L.distance_offset = init->foot_motor_L.distance; - init->foot_motor_R.distance_offset = init->foot_motor_R.distance; - - Power_Init(init); - - // ------------------ MPC init ------------------ - MPC_Init(); -} - -void Chassis_Data_Update( chassis_control_t *fdb ) -{ - robot = get_robot_status_point(); - robot_level = robot->robot_level; - // robot_level = 1; - // -------------------- Update foot measure -------------------- - fdb->foot_motor_L.motor_measure.ecd = (uint16_t)motor_BM[0].ecd; - fdb->foot_motor_L.motor_measure.last_ecd = (uint16_t)motor_BM[0].last_ecd; - fdb->foot_motor_L.motor_measure.speed_rpm = (int16_t)motor_BM[0].speed_rpm; - fdb->foot_motor_L.torque_get = motor_BM[0].given_current*55.0f/32767.0f; - - fdb->foot_motor_R.motor_measure.ecd = (uint16_t)motor_BM[1].ecd; - fdb->foot_motor_R.motor_measure.last_ecd = (uint16_t)motor_BM[1].last_ecd; - fdb->foot_motor_R.motor_measure.speed_rpm = (int16_t)motor_BM[1].speed_rpm; - fdb->foot_motor_R.torque_get = motor_BM[1].given_current*55.0f/32767.0f; - - // ------------------ Update mode info ------------------ - fdb->mode.last_chassis_mode = fdb->mode.chassis_mode; - fdb->mode.last_chassis_balancing_mode = fdb->mode.chassis_balancing_mode; - fdb->mode.last_sport_mode = fdb->mode.sport_mode; - fdb->mode.last_jumping_mode = fdb->mode.jumping_mode; - fdb->mode.last_jumping_stage = fdb->mode.jumping_stage; - fdb->mode.last_chassis_high_mode = fdb->mode.chassis_high_mode; - fdb->flag_info.last_static_flag = fdb->flag_info.static_flag; - fdb->flag_info.last_moving_flag = fdb->flag_info.moving_flag; - last_debug_flag = debug_flag; - fdb->flag_info.last_overpower_warning_flag = fdb->flag_info.overpower_warning_flag; - fdb->flag_info.last_stablize_high_flag = fdb->flag_info.stablize_high_flag; - - fdb->flag_info.last_suspend_flag_L = fdb->flag_info.suspend_flag_L; - fdb->flag_info.last_suspend_flag_R = fdb->flag_info.suspend_flag_R; - - fdb->joint_motor_1.last_motor_mode = fdb->joint_motor_1.motor_mode; - fdb->joint_motor_2.last_motor_mode = fdb->joint_motor_2.motor_mode; - fdb->joint_motor_3.last_motor_mode = fdb->joint_motor_3.motor_mode; - fdb->joint_motor_4.last_motor_mode = fdb->joint_motor_4.motor_mode; - fdb->foot_motor_L.last_motor_mode = fdb->foot_motor_L.motor_mode; - fdb->foot_motor_R.last_motor_mode = fdb->foot_motor_R.motor_mode; - - // ------------------ Update IMU info ------------------ - fdb->chassis_posture_info.pitch_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_PITCH_ADDRESS_OFFSET); - fdb->chassis_posture_info.roll_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_ROLL_ADDRESS_OFFSET); - fdb->chassis_posture_info.yaw_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_YAW_ADDRESS_OFFSET) + yaw_count * PI2; - fdb->chassis_posture_info.pitch_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_PITCH_ADDRESS_OFFSET); - fdb->chassis_posture_info.roll_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_ROLL_ADDRESS_OFFSET); - fdb->chassis_posture_info.yaw_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_YAW_ADDRESS_OFFSET); - - // ------------------ Update HT Motor info ------------------ - fdb->joint_motor_1.position = (fdb->joint_motor_1.motor_measure->ecd - fdb->joint_motor_1.position_offset) + LEG_OFFSET; - fdb->joint_motor_2.position = (fdb->joint_motor_2.motor_measure->ecd - fdb->joint_motor_2.position_offset) - LEG_OFFSET; - fdb->joint_motor_3.position = (fdb->joint_motor_3.motor_measure->ecd - fdb->joint_motor_3.position_offset) + LEG_OFFSET; - fdb->joint_motor_4.position = (fdb->joint_motor_4.motor_measure->ecd - fdb->joint_motor_4.position_offset) - LEG_OFFSET; - - fdb->joint_motor_1.velocity = fdb->joint_motor_1.motor_measure->speed_rpm; - fdb->joint_motor_2.velocity = fdb->joint_motor_2.motor_measure->speed_rpm; - fdb->joint_motor_3.velocity = fdb->joint_motor_3.motor_measure->speed_rpm; - fdb->joint_motor_4.velocity = fdb->joint_motor_4.motor_measure->speed_rpm; - - fdb->joint_motor_1.torque_get = 0.95f * fdb->joint_motor_1.torque_get + 0.05f * fdb->joint_motor_1.motor_measure->real_torque; - fdb->joint_motor_2.torque_get = 0.95f * fdb->joint_motor_2.torque_get + 0.05f * fdb->joint_motor_2.motor_measure->real_torque; - fdb->joint_motor_3.torque_get = 0.95f * fdb->joint_motor_3.torque_get + 0.05f * fdb->joint_motor_3.motor_measure->real_torque; - fdb->joint_motor_4.torque_get = 0.95f * fdb->joint_motor_4.torque_get + 0.05f * fdb->joint_motor_4.motor_measure->real_torque; - - // ------------------ Update LK Motor info ------------------ - fdb->foot_motor_L.last_position = fdb->foot_motor_L.position; - fdb->foot_motor_R.last_position = fdb->foot_motor_R.position; - fdb->foot_motor_L.position = fdb->foot_motor_L.motor_measure.ecd * MOTOR_ECD_TO_RAD;// Angle (rad) - fdb->foot_motor_R.position = fdb->foot_motor_R.motor_measure.ecd * MOTOR_ECD_TO_RAD; - - if( fdb->flag_info.init_flag != 1 ) // Init_flag = 0: after Init fdb 如果不是在init里面调用就�?�圈 - { - if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) > HALF_POSITION_RANGE ) - fdb->foot_motor_L.turns++; - else if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) < -HALF_POSITION_RANGE ) - fdb->foot_motor_L.turns--; - if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) > HALF_POSITION_RANGE ) - fdb->foot_motor_R.turns--; - else if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) < -HALF_POSITION_RANGE ) - fdb->foot_motor_R.turns++; - } - fdb->foot_motor_L.distance = ( fdb->foot_motor_L.position/PI2 + fdb->foot_motor_L.turns ) * WHEEL_PERIMETER - fdb->foot_motor_L.distance_offset; - fdb->foot_motor_R.distance = ( -fdb->foot_motor_R.position/PI2 + fdb->foot_motor_R.turns ) * WHEEL_PERIMETER - fdb->foot_motor_R.distance_offset; - fdb->chassis_posture_info.foot_distance = ( fdb->foot_motor_L.distance + fdb->foot_motor_R.distance ) /2.0f; - - fdb->foot_motor_L.speed = fdb->foot_motor_L.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; // rpm -> m/s - fdb->foot_motor_R.speed = -fdb->foot_motor_R.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; - fdb->chassis_posture_info.foot_speed = ( fdb->foot_motor_L.speed + fdb->foot_motor_R.speed ) / 2.0f; - - // fdb->foot_motor_L.torque_get = fdb->foot_motor_L.motor_measure.given_current; - // fdb->foot_motor_R.torque_get = fdb->foot_motor_R.motor_measure.given_current; - - // ------------------ Kalman Filter ------------------ - - kalman_second_order_update(&Body_Speed_KF, fdb->chassis_posture_info.foot_distance, fdb->chassis_posture_info.foot_speed, temp_a); - fdb->chassis_posture_info.foot_distance_K = Body_Speed_KF.x[0]; - fdb->chassis_posture_info.foot_speed_K = Body_Speed_KF.x[1]; - - // ------------------ Five_Bars_to_Pendulum ------------------ - Forward_kinematic_solution(fdb,-fdb->joint_motor_2.position,-fdb->joint_motor_2.velocity, - -fdb->joint_motor_1.position,-fdb->joint_motor_1.velocity,1); - Forward_kinematic_solution(fdb,fdb->joint_motor_3.position,fdb->joint_motor_3.velocity, - fdb->joint_motor_4.position, fdb->joint_motor_4.velocity,0); - - // ------------------ Huanzhi information update ------------------ - fdb->chassis_posture_info.leg_angle_L -= PI_2; - fdb->chassis_posture_info.leg_angle_L -= fdb->chassis_posture_info.pitch_angle; - // fdb->chassis_posture_info.leg_gyro_L -= fdb->chassis_posture_info.pitch_gyro; - fp32 temp_v_L = ( fdb->chassis_posture_info.leg_angle_L - fdb->chassis_posture_info.last_leg_angle_L ) / TASK_RUN_TIME; - fdb->chassis_posture_info.leg_gyro_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_L; - fdb->chassis_posture_info .last_leg_angle_L = fdb->chassis_posture_info .leg_angle_L; - - fdb->chassis_posture_info.leg_angle_R -= PI_2; - fdb->chassis_posture_info.leg_angle_R -= fdb->chassis_posture_info.pitch_angle; - // fdb->chassis_posture_info.leg_gyro_R -= fdb->chassis_posture_info.pitch_gyro; - fp32 temp_v_R = ( fdb->chassis_posture_info.leg_angle_R - fdb->chassis_posture_info.last_leg_angle_R ) / TASK_RUN_TIME; - fdb->chassis_posture_info.leg_gyro_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_R; - fdb->chassis_posture_info.last_leg_angle_R = fdb->chassis_posture_info .leg_angle_R; - - temp_v_L = ( fdb->chassis_posture_info.leg_length_L - fdb->chassis_posture_info.last_leg_length_L ) / TASK_RUN_TIME; - fdb->chassis_posture_info.leg_dlength_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_L; - fdb->chassis_posture_info.last_leg_length_L = fdb->chassis_posture_info.leg_length_L; - - temp_v_R = ( fdb->chassis_posture_info.leg_length_R - fdb->chassis_posture_info.last_leg_length_R ) / TASK_RUN_TIME; - fdb->chassis_posture_info.leg_dlength_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_R; - fdb->chassis_posture_info.last_leg_length_R = fdb->chassis_posture_info.leg_length_R; - - // ---------- Rotate and Move Info update ------------ - rotate_move_offset = offset_k * rotate_speed_list[robot_level]; - rc_sign = 1.0f; - X_speed = fdb->chassis_rc_ctrl->X_speed; - Y_speed = fdb->chassis_rc_ctrl->Y_speed; - // Original rc angle - if (X_speed == 0){ - if (Y_speed > 0) - rc_angle_temp = PI/2; - else if (Y_speed < 0) - rc_angle_temp = -PI/2; - else - rc_angle_temp = 0; - } - else{ - Y_speed = fdb->chassis_rc_ctrl->Y_speed; - rc_angle_temp = atan_tl(Y_speed/X_speed); - } - // Normalized speed - if (abs(X_speed) < 0.1 ||abs(Y_speed) <0.1) - temp_max_spd = 1.0f; - else if (X_speedPI/2||rc_angle_temp<-PI/2){ - rc_sign *= -1.0f; - } - while (rc_angle_temp>PI/2) { - rc_angle_temp -= PI; - } - while (rc_angle_temp<-PI/2){ - rc_angle_temp += PI; - } - // if (X_speed < 0) rc_sign = -1.0f; - rc_angle = rc_angle_temp * 180.0f / PI; - if (fdb->chassis_rc_ctrl->fake_flag == 0) fake_sign = -1.0f; - else fake_sign = 1.0f; -} - -void Chassis_Status_Detect( chassis_control_t *detect ) -{ - // ------------------ Off Ground Detect ------------------ - - - if( detect->mode.chassis_balancing_mode == BALANCING_READY && detect->mode.sport_mode!=TK_MODE) - { - if( ABS(detect->chassis_posture_info.pitch_angle) >= DANGER_PITCH_ANGLE ) - detect->flag_info.abnormal_flag = 1; - else if( ABS(detect->chassis_posture_info.foot_speed_K) < MOVE_LOWER_BOUND && - ABS(detect->chassis_posture_info.pitch_angle) < 0.1f && - detect->flag_info.abnormal_flag ) - detect->flag_info.abnormal_flag = 0; - } - if (abnormal_debug){ - detect->flag_info.abnormal_flag = 0; - } - - Supportive_Force_Cal(detect, detect->joint_motor_1.position, detect->joint_motor_2.position, 1.0f ); - Supportive_Force_Cal(detect, detect->joint_motor_3.position, detect->joint_motor_4.position, 0.0f ); - - if( detect->mode.jumping_stage == CONSTACTING_LEGS ) - detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND; - else - { - if( detect->mode.sport_mode == JUMPING_MODE ) - { - if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE_FOR_JUMP && - detect->chassis_posture_info.leg_length_R > 0.13f ) - detect->flag_info.suspend_flag_R = OFF_GROUND; - else - detect->flag_info.suspend_flag_L = ON_GROUND; - if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE_FOR_JUMP && - detect->chassis_posture_info.leg_length_L > 0.13f ) - detect->flag_info.suspend_flag_L = OFF_GROUND; - else - detect->flag_info.suspend_flag_R = ON_GROUND; - } - else - { - if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE && - detect->chassis_posture_info.leg_length_R > 0.13f ) - detect->flag_info.suspend_flag_R = OFF_GROUND; - else - detect->flag_info.suspend_flag_L = ON_GROUND; - if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE && - detect->chassis_posture_info.leg_length_L > 0.13f ) - detect->flag_info.suspend_flag_L = OFF_GROUND; - else - detect->flag_info.suspend_flag_R = ON_GROUND; - } - } - - - if( detect->flag_info.abnormal_flag == 1 && - ( detect->flag_info.last_suspend_flag_L == ON_GROUND || detect->flag_info.last_suspend_flag_R == ON_GROUND ) && - ( detect->flag_info.suspend_flag_L == OFF_GROUND || detect->flag_info.suspend_flag_R == OFF_GROUND ) ) - detect->flag_info.Ignore_Off_Ground = 1; - else if( detect->flag_info.abnormal_flag != 1 ) - detect->flag_info.Ignore_Off_Ground = 0; - if( detect->flag_info.Ignore_Off_Ground ) - { - detect->flag_info.suspend_flag_R = ON_GROUND; - detect->flag_info.suspend_flag_L = ON_GROUND; - } - // detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND; - //Moving_High_Offset - - if( ABS(detect->chassis_posture_info.foot_speed_K) > stablize_foot_speed_threshold && - ABS(detect->chassis_posture_info.yaw_gyro ) > stablize_yaw_speed_threshold ) - detect->flag_info.stablize_high_flag = 1; - -} - -void Chassis_Mode_Set( chassis_control_t *mode_set ) -{ - // ----------------- Chassis Mode Update ----------------- - if( mode_set->chassis_rc_ctrl->mode_R == 0 || toe_is_error(GIMBAL_TOE)|| toe_is_error(BM1_TOE)||toe_is_error(BM2_TOE)) - mode_set->mode.chassis_mode = DISABLE_CHASSIS; - else - mode_set->mode.chassis_mode = ENABLE_CHASSIS; - - if( mode_set->mode.chassis_mode == ENABLE_CHASSIS ) - { - mode_set->joint_motor_1.motor_mode = MOTOR_FORCE; - mode_set->joint_motor_2.motor_mode = MOTOR_FORCE; - mode_set->joint_motor_3.motor_mode = MOTOR_FORCE; - mode_set->joint_motor_4.motor_mode = MOTOR_FORCE; - mode_set->foot_motor_L.motor_mode = MOTOR_FORCE; - mode_set->foot_motor_R.motor_mode = MOTOR_FORCE; - } - else - { - if( mode_set->mode.chassis_balancing_mode == JOINT_REDUCING ) - { - mode_set->joint_motor_1.motor_mode = MOTOR_FORCE; - mode_set->joint_motor_2.motor_mode = MOTOR_FORCE; - mode_set->joint_motor_3.motor_mode = MOTOR_FORCE; - mode_set->joint_motor_4.motor_mode = MOTOR_FORCE; - mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE; - mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE; - } - else - { - mode_set->joint_motor_1.motor_mode = MOTOR_NO_FORCE; - mode_set->joint_motor_2.motor_mode = MOTOR_NO_FORCE; - mode_set->joint_motor_3.motor_mode = MOTOR_NO_FORCE; - mode_set->joint_motor_4.motor_mode = MOTOR_NO_FORCE; - mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE; - mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE; - } - - } - - // ----------------- Sport Mode Update ----------------- - if( mode_set->mode.chassis_balancing_mode == BALANCING_READY ) - { - if( mode_set->chassis_rc_ctrl->tk_flag ) - mode_set->mode.sport_mode = TK_MODE; - else if( mode_set->flag_info.abnormal_flag ) - mode_set->mode.sport_mode = ABNORMAL_MOVING_MODE; - else if( mode_set->mode.sport_mode == JUMPING_MODE && mode_set->mode.jumping_stage != FINISHED ) - mode_set->mode.sport_mode = JUMPING_MODE; - else if( mode_set->chassis_rc_ctrl->jump_flag ) - mode_set->mode.sport_mode = JUMPING_MODE; - else if( mode_set->chassis_rc_ctrl->cap_flag ) - mode_set->mode.sport_mode = CAP_MODE; - else if( mode_set->chassis_rc_ctrl->fly_flag ) - mode_set->mode.sport_mode = FLY_MODE; - else - mode_set->mode.sport_mode = NORMAL_MOVING_MODE; - } - else - mode_set->mode.sport_mode = NONE; - - // ----------------- Rotation Flag -------------------- - static uint8_t last_rotation_flag = 0; - last_rotation_flag = mode_set->flag_info.rotation_flag; - mode_set->flag_info.rotation_flag = mode_set->chassis_rc_ctrl->mode_R == 4; //Rotate - if (!last_rotation_flag && mode_set->flag_info.rotation_flag){ - for (int i = 0; i < 11; ++i) - rotate_speed_list[i] = - rotate_speed_list[i]; - } - - // ----------------- No Follow Flag ---------------------- - if (toe_is_error(GIMBAL_TOE)) no_follow_flag = 1; - else no_follow_flag = mode_set->chassis_rc_ctrl->mode_R == 3; //No Follow - - // ---------------- Moving Flag ---------------------------- - if( mode_set->chassis_rc_ctrl->X_speed != 0.0f ) - { - mode_set->flag_info.controlling_flag = 1; - mode_set->flag_info.set_pos_after_moving = 1; - } - else - mode_set->flag_info.controlling_flag = 0; - - if( ABS(mode_set->chassis_posture_info.foot_speed_K) <= 0.05f && - !mode_set->flag_info.controlling_flag && - mode_set->flag_info.set_pos_after_moving ) // maybe bug - { - mode_set->flag_info.moving_flag = 0; - mode_set->flag_info.set_pos_after_moving = 0; - } - else - mode_set->flag_info.moving_flag = 1; - - -} - -uint8_t reduce_flag = 0; -fp32 reduce_high, high_offset = 0.2f; -fp32 debug_1 = 0.995; -void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change ) -{ - if( chassis_mode_change->mode.chassis_mode == ENABLE_CHASSIS && chassis_mode_change->mode.last_chassis_mode == DISABLE_CHASSIS ) - chassis_mode_change->mode.chassis_balancing_mode = FOOT_LAUNCHING; - - if( chassis_mode_change->mode.chassis_balancing_mode == FOOT_LAUNCHING && - ABS(chassis_mode_change->chassis_posture_info.pitch_angle) < EXIT_PITCH_ANGLE ) - chassis_mode_change->mode.chassis_balancing_mode = JOINT_LAUNCHING; - else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_LAUNCHING && ( - (NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_L) < 0.03f || - (NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_R) < 0.03f) ) - chassis_mode_change->mode.chassis_balancing_mode = BALANCING_READY; - else if( chassis_mode_change->mode.chassis_balancing_mode == BALANCING_READY && - chassis_mode_change->mode.chassis_mode == DISABLE_CHASSIS ){ - if (!reduce_flag) { - reduce_high = chassis_mode_change->chassis_posture_info.ideal_high + high_offset; - reduce_flag = 1; - } - chassis_mode_change->mode.chassis_balancing_mode = JOINT_REDUCING; - } - else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_REDUCING && ( - ABS( chassis_mode_change->chassis_posture_info.leg_length_L - SIT_HIGH ) < 0.001f || - ABS( chassis_mode_change->chassis_posture_info.leg_length_R - SIT_HIGH ) < 0.001f ) ){ - chassis_mode_change->mode.chassis_balancing_mode = NO_FORCE; - reduce_flag = 0; - } - - if( chassis_mode_change->mode.sport_mode == JUMPING_MODE && chassis_mode_change->mode.last_sport_mode != JUMPING_MODE ) - { - chassis_mode_change->mode.jumping_mode = STANDING_JUMP; - } - else if( chassis_mode_change->mode.sport_mode != JUMPING_MODE ) - chassis_mode_change->mode.jumping_mode = NOT_DEFINE; - - - if( chassis_mode_change->mode.jumping_mode == MOVING_JUMP ) - { - if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP ) - chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS; - else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS && - chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) - chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS; - else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS && - chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f ) - chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2; - else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 && - chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) - chassis_mode_change->mode.jumping_stage = PREPARING_LANDING; - else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && - chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND && - chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND ) - chassis_mode_change->mode.jumping_stage = FINISHED; - else if( chassis_mode_change->mode.jumping_stage == FINISHED ) - chassis_mode_change->mode.jumping_stage = READY_TO_JUMP; - } - else if( chassis_mode_change->mode.jumping_mode == STANDING_JUMP ) - { - if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP ) - chassis_mode_change->mode.jumping_stage = PREPARING_STAND_JUMPING; - else if(chassis_mode_change->mode.jumping_stage == PREPARING_STAND_JUMPING && - ABS(chassis_mode_change->chassis_posture_info.leg_angle_L + IDEAL_PREPARING_STAND_JUMPING_ANGLE ) < stepp ) - chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS; - else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS && - chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f ) - chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2; - else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 && - chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f ) - chassis_mode_change->mode.jumping_stage = PREPARING_LANDING; - else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING && - chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND && - chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND ) - chassis_mode_change->mode.jumping_stage = FINISHED; - else if( chassis_mode_change->mode.jumping_stage == FINISHED ) - chassis_mode_change->mode.jumping_stage = READY_TO_JUMP; - } - else - chassis_mode_change->mode.jumping_stage = FINISHED; - - - if( chassis_mode_change->flag_info.stablize_high_flag == 1 && - Moving_High_Offset >= 0.2 && - chassis_mode_change->chassis_posture_info.yaw_gyro <= ( stablize_yaw_speed_threshold - 0.5f ) ) - chassis_mode_change->flag_info.stablize_high_flag = 0; -} - -fp32 HIGH_SWITCH = 36.0f; - -void Target_Value_Set( chassis_control_t *target_value_set ) -{ - // ------------------------------------ - if( target_value_set->flag_info.stablize_high_flag == 1 ) - if( Moving_High_Offset < 0.2 ) - Moving_High_Offset += 0.001f; - - if( target_value_set->flag_info.stablize_high_flag == 0 ) - Moving_High_Offset = 0.0f; - - // --------- X Speed Set ---------- - if( target_value_set->mode.sport_mode != NONE && - target_value_set->flag_info.suspend_flag_L == ON_GROUND && - target_value_set->flag_info.suspend_flag_R == ON_GROUND ) - { - if (!target_value_set->flag_info.rotation_flag) {// Normal move - if(toe_is_error(REFEREE_TOE)){ - target_value_set->chassis_posture_info.foot_speed_set = fake_sign * target_value_set->chassis_rc_ctrl->X_speed * normal_move_scale ; - - } else { - // target_speed_sign = (fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]) > 0 ? 1: -1; - target_speed_sign = SIGN(fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]); - target_value_set->chassis_posture_info.foot_speed_set = target_value_set->chassis_posture_info.foot_speed_K + target_speed_sign * acc_step; - - if (target_value_set->mode.sport_mode == FLY_MODE) - target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * fly_speed),ABS(target_value_set->chassis_posture_info.foot_speed_set)); - else if (target_value_set->mode.sport_mode == CAP_MODE) - target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * cap_move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set)); - else - target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set)); - - // if( target_value_set->chassis_posture_info.leg_length_L > 0.22f || target_value_set->chassis_posture_info.leg_length_R > 0.22f ) - // { - // target_value_set->chassis_posture_info.foot_speed_set /= 2.0f; - // debug_2++; - // } - } - - } - else // Rotate and move - { - delta_theta_temp = target_value_set->chassis_rc_ctrl->W_angle - rc_angle; - if (delta_theta_temp>PI/2) delta_theta_temp -= PI; - else if (delta_theta_temp<-PI/2) delta_theta_temp += PI; - delta_theta = abs(delta_theta_temp); - if (delta_thetachassis_posture_info.foot_speed_set - = ((rotate_move_threshold-delta_theta)/rotate_move_threshold) - * rc_sign * fake_sign * normalized_speed * rotate_move_scale_list[robot_level]; - else target_value_set->chassis_posture_info.foot_speed_set = 0; - } - - } - else - target_value_set->chassis_posture_info.foot_speed_set = 0; - - // --------- Distance Set --------- - if( target_value_set->mode.chassis_balancing_mode == NO_FORCE || target_value_set->mode.sport_mode == TK_MODE) - target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; - else if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE ) - target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_set; - else if( target_value_set->flag_info.suspend_flag_R == OFF_GROUND || - target_value_set->flag_info.suspend_flag_L == OFF_GROUND ) - target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; - else if( target_value_set->flag_info.controlling_flag ) - target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; - else if( !target_value_set->flag_info.moving_flag && - target_value_set->mode.chassis_balancing_mode == BALANCING_READY && - target_value_set->flag_info.last_moving_flag ) - target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K; - - - - // --------- Y Speed & Angle Set --------- - if( target_value_set->mode.sport_mode != NONE && - target_value_set->flag_info.suspend_flag_L == ON_GROUND && - target_value_set->flag_info.suspend_flag_R == ON_GROUND ) - { - if( target_value_set->flag_info.rotation_flag == 1 ) - { - target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; - target_value_set->chassis_posture_info.yaw_gyro_set = rotate_speed_list[robot_level]; - } - else - { - if (target_value_set->chassis_rc_ctrl->W_angle<-FOLLOW_DEADBAND) - target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle+FOLLOW_DEADBAND) / 180.0f * PI; - else if (target_value_set->chassis_rc_ctrl->W_angle>FOLLOW_DEADBAND) - target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle-FOLLOW_DEADBAND) / 180.0f * PI; - else - target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; - target_value_set->chassis_posture_info.yaw_gyro_set = 0.0f; - } - } - else - { - target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle; - target_value_set->chassis_posture_info.yaw_gyro_set = target_value_set->chassis_posture_info.yaw_gyro; - } - - - - // --------- Side Angle Set --------- - // if( target_value_set->mode.sport_mode != NONE && - // target_value_set->mode.sport_mode != ABNORMAL_MOVING_MODE && - // target_value_set->flag_info.suspend_flag_R == ON_GROUND && - // target_value_set->flag_info.suspend_flag_L == ON_GROUND ) - // { - // if( target_value_set->mode.sport_mode == SIDE_MODE ) - // target_value_set->chassis_posture_info.roll_angle_set = (fp32)(5.0 / 180.0 * PI) * 50.0f; - // // else if( target_value_set->chassis_rc_ctrl->side_flag == -1 ) - // // target_value_set->chassis_posture_info.roll_angle_set = -(fp32)(5.0 / 180.0 * PI) * 50.0f; - // else - // target_value_set->chassis_posture_info.roll_angle_set = 0.0f; - // } - // else - target_value_set->chassis_posture_info.roll_angle_set = 0.0f; - - // ---------------- Leg Angle Set ---------- - if( target_value_set->mode.jumping_stage == PREPARING_STAND_JUMPING ) - { - target_value_set->chassis_posture_info.leg_angle_L_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE; - target_value_set->chassis_posture_info.leg_angle_R_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE; - } - else - { - target_value_set->chassis_posture_info.leg_angle_L_set = 0.0f; - target_value_set->chassis_posture_info.leg_angle_R_set = 0.0f; - } - - - // ----------------- Chassis High Mode Update ----------------- - if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || target_value_set->mode.sport_mode == TK_MODE) - target_value_set->mode.chassis_high_mode = SIT_MODE; - else if( target_value_set->mode.chassis_balancing_mode == FOOT_LAUNCHING ) - target_value_set->mode.chassis_high_mode = SIT_MODE; - else if( target_value_set->mode.chassis_balancing_mode == JOINT_LAUNCHING ) - target_value_set->mode.chassis_high_mode = NORMAL_MODE; - else if( target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING ) - target_value_set->mode.chassis_high_mode = CHANGING_HIGH; - else if( target_value_set->chassis_rc_ctrl->sit_flag ) - target_value_set->mode.chassis_high_mode = SIT_MODE; - else if( target_value_set->chassis_rc_ctrl->high_flag ) - target_value_set->mode.chassis_high_mode = HIGH_MODE; - else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS ) - target_value_set->mode.chassis_high_mode = SIT_MODE; - else if( target_value_set->mode.jumping_stage == EXTENDING_LEGS ) - target_value_set->mode.chassis_high_mode = HIGH_MODE; - else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS_2 ) - target_value_set->mode.chassis_high_mode = SIT_MODE; - else if( target_value_set->mode.jumping_stage == PREPARING_LANDING ) - target_value_set->mode.chassis_high_mode = NORMAL_MODE; - else - target_value_set->mode.chassis_high_mode = NORMAL_MODE; - - // --------- Leg Length Set --------- - if( target_value_set->mode.chassis_high_mode == SIT_MODE ) - target_value_set->chassis_posture_info.ideal_high = SIT_HIGH; - else if( target_value_set->mode.chassis_high_mode == NORMAL_MODE ) - target_value_set->chassis_posture_info.ideal_high = NORMAL_HIGH + High_Offset - Moving_High_Offset; - else if( target_value_set->mode.chassis_high_mode == HIGH_MODE ) - target_value_set->chassis_posture_info.ideal_high = HIGH_HIGH + High_Offset - Moving_High_Offset; - else if( target_value_set->mode.chassis_high_mode == CHANGING_HIGH ) - { - reduce_high = reduce_high * debug_1; - target_value_set->chassis_posture_info.ideal_high = min(reduce_high,target_value_set->chassis_posture_info.ideal_high); - // target_value_set->chassis_posture_info.ideal_high = - // debug_1 * target_value_set->chassis_posture_info.ideal_high; - } - - if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || - ( target_value_set->flag_info.suspend_flag_L == 1 && target_value_set->flag_info.suspend_flag_R == 1 ) || - target_value_set->chassis_posture_info.ideal_high == SIT_HIGH || - target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING ) - { - target_value_set->chassis_posture_info.leg_length_L_set = target_value_set->chassis_posture_info.ideal_high; - target_value_set->chassis_posture_info.leg_length_R_set = target_value_set->chassis_posture_info.ideal_high; - } - else - { - target_value_set->chassis_posture_info.foot_roll_angle = - target_value_set->chassis_posture_info.roll_angle + - atan(( target_value_set->chassis_posture_info.leg_length_L - target_value_set->chassis_posture_info.leg_length_R ) / 0.50f ); - - target_value_set->chassis_posture_info.leg_length_L_set = - target_value_set->chassis_posture_info .ideal_high - + 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle ); - target_value_set->chassis_posture_info.leg_length_R_set = - target_value_set->chassis_posture_info .ideal_high - - 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle ); - } - -} - -void Chassis_Torque_Calculation(chassis_control_t *bl_ctrl) -{ - - LQR_Data_Update(bl_ctrl); - // -----Roll Balance----- - if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 || - bl_ctrl->mode.chassis_high_mode == SIT_MODE ) - { - bl_ctrl->torque_info.joint_roll_torque_R = 0.0f; - bl_ctrl->torque_info.joint_roll_torque_L = 0.0f; - } - else - { - if (bl_ctrl->chassis_posture_info.roll_angle < -roll_angle_deadband) { - rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle+roll_angle_deadband)); - } - else if (bl_ctrl->chassis_posture_info.roll_angle > roll_angle_deadband){ - rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle-roll_angle_deadband)); - } - else { - rollP = 0.0f; - } - - if (bl_ctrl->chassis_posture_info.roll_gyro < -roll_gyro_deadband) { - rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro + roll_gyro_deadband); - } - else if (bl_ctrl->chassis_posture_info.roll_gyro > roll_gyro_deadband){ - rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro - roll_gyro_deadband); - } - else { - rollD = 0.0f; - } - bl_ctrl->torque_info.joint_roll_torque_R = rollP + rollD; - bl_ctrl->torque_info.joint_roll_torque_L = -bl_ctrl->torque_info.joint_roll_torque_R; - } - - - // -----During turns: prevent displacement of two legs----- - bl_ctrl->torque_info.joint_prevent_splits_torque_L = - coordinate_PD[0] * (bl_ctrl->chassis_posture_info.leg_angle_L - bl_ctrl->chassis_posture_info.leg_angle_R) - + coordinate_PD[1] * (bl_ctrl->chassis_posture_info.leg_gyro_L - bl_ctrl->chassis_posture_info.leg_gyro_R); - - bl_ctrl->torque_info.joint_prevent_splits_torque_R = -bl_ctrl->torque_info.joint_prevent_splits_torque_L; - - if( bl_ctrl->mode.jumping_stage == EXTENDING_LEGS || - bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 ) - { - bl_ctrl->torque_info.joint_stand_torque_L = - + jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) - + jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L ); - - bl_ctrl->torque_info.joint_stand_torque_R = - + jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) - + jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R ); - } - else if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE ) - { - bl_ctrl->torque_info.joint_stand_torque_L = - + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) - + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); - bl_ctrl->torque_info.joint_stand_torque_R = - + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) - + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); - } - else if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 ) - { - if( bl_ctrl->flag_info.suspend_flag_L == 1 ) - { - bl_ctrl->torque_info.joint_stand_torque_L = - + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) - + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); - } - else{ - bl_ctrl->torque_info.joint_stand_torque_L = - FEED_f - + stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) - + stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L ); - } - - if( bl_ctrl->flag_info.suspend_flag_R == 1 ) - { - bl_ctrl->torque_info.joint_stand_torque_R = - + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) - + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); - } else { - bl_ctrl->torque_info.joint_stand_torque_R = - FEED_f - + stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) - + stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R ); - } - } - else if( bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING ) - { - - bl_ctrl->torque_info.joint_stand_torque_L = - + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ) - + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L ); - - bl_ctrl->torque_info.joint_stand_torque_R = - + suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ) - + suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R ); - } - else - { - bl_ctrl->torque_info.joint_stand_torque_L = FEED_f; - bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L ); - if (bl_ctrl->chassis_posture_info.leg_dlength_L > leg_dlength_deadband){ - bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L-leg_dlength_deadband) ); - } - else if (bl_ctrl->chassis_posture_info.leg_dlength_L < -leg_dlength_deadband){ - bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L+leg_dlength_deadband)); - } - else { - bl_ctrl->torque_info.joint_stand_torque_L += 0.0f; - } - bl_ctrl->torque_info.joint_stand_torque_R = FEED_f; - bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R ); - if (bl_ctrl->chassis_posture_info.leg_dlength_R > leg_dlength_deadband){ - bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R-leg_dlength_deadband) ); - } - else if (bl_ctrl->chassis_posture_info.leg_dlength_R < -leg_dlength_deadband){ - bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R+leg_dlength_deadband)); - } - else { - bl_ctrl->torque_info.joint_stand_torque_R += 0.0f; - } - - } - // --------------------- joint motor calucaltion --------------------- - - - // TODDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDO - if( bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 ) - { - bl_ctrl->torque_info.joint_balancing_torque_L = - -suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_L ) - -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L ); - bl_ctrl->torque_info.joint_balancing_torque_R = - -suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_R ) - -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R ); - - bl_ctrl->torque_info.joint_moving_torque_L = 0.0f; - bl_ctrl->torque_info.joint_moving_torque_R = 0.0f; - } - else - { - if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE || bl_ctrl->mode.sport_mode == TK_MODE ) - { - bl_ctrl->torque_info.joint_balancing_torque_L = - bl_ctrl->torque_info.joint_moving_torque_L = - bl_ctrl->torque_info.joint_balancing_torque_R = - bl_ctrl->torque_info.joint_moving_torque_R = 0; - } - else - { - if( bl_ctrl->mode.chassis_high_mode == SIT_MODE || - bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING ) - { - bl_ctrl->torque_info.joint_balancing_torque_L = - bl_ctrl->torque_info.joint_moving_torque_L = - bl_ctrl->torque_info.joint_balancing_torque_R = - bl_ctrl->torque_info.joint_moving_torque_R = 0; - } - else - { - - bl_ctrl->torque_info.joint_balancing_torque_L = ( - -LQR[2][4] * ( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L ) - -LQR[2][5] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L ) - -LQR[2][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) - -LQR[2][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) ); - bl_ctrl->torque_info.joint_moving_torque_L = ( - +LQR[2][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) - +LQR[2][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K ) - +LQR[2][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle ) - +LQR[2][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) - ); - - bl_ctrl->torque_info.joint_balancing_torque_R = ( - -LQR[3][6] * ( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R ) - -LQR[3][7] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R ) - -LQR[3][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle ) - -LQR[3][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) ); - bl_ctrl->torque_info.joint_moving_torque_R = ( - +LQR[3][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) - +LQR[3][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K ) - +LQR[3][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle ) - +LQR[3][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) - ); - } - } - } - - // --------------------- Foot motor LQR --------------------- - if ( bl_ctrl->mode.sport_mode == TK_MODE ) { - bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f; - bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f; - bl_ctrl->torque_info.foot_moving_torque_L = (int) ( - -TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) - -TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle ) - -TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro ) - )* TORQ_K; - bl_ctrl->torque_info.foot_moving_torque_R = (int)-( - -TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K ) - +TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle ) - +TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro ) - )* TORQ_K; - } - else { - bl_ctrl->torque_info.foot_balancing_torque_L = (int) ( - -LQR[0][4]*( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L) - -LQR[0][5]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L) - +LQR[0][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle) - +LQR[0][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) - )* TORQ_K; - - bl_ctrl->torque_info.foot_moving_torque_L = (int) ( - -LQR[0][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) - -LQR[0][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K) - -LQR[0][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle) - -LQR[0][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) - )* TORQ_K; - - bl_ctrl->torque_info.foot_balancing_torque_R = (int) -( - -LQR[1][6]*( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R) - -LQR[1][7]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R) - +LQR[1][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle) - +LQR[1][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro) - )* TORQ_K; - - bl_ctrl->torque_info.foot_moving_torque_R = (int) -( - -LQR[1][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET) - -LQR[1][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K) - +LQR[1][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle) - +LQR[1][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro ) - )* TORQ_K; - } - - if( bl_ctrl->flag_info.suspend_flag_R == 1 ) - { - bl_ctrl->torque_info.joint_balancing_torque_R = - -suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_R ) - -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R ); - - bl_ctrl->torque_info.foot_moving_torque_R = - -suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_R.speed ); - - bl_ctrl->torque_info.joint_moving_torque_R = 0.0f; - bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f; - } - if( bl_ctrl->flag_info.suspend_flag_L == 1 ) - { - bl_ctrl->torque_info.joint_balancing_torque_L = - -suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_L ) - -suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L ); - bl_ctrl->torque_info.foot_moving_torque_L = - +suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_L.speed ); - - bl_ctrl->torque_info.joint_moving_torque_L = 0.0f; - bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f; - } - - - // LimitMax( bl_ctrl->torque_info.foot_moving_torque_L, MAX_ACCL ); - // LimitMax( bl_ctrl->torque_info.foot_moving_torque_R, MAX_ACCL ); - LimitMax( bl_ctrl->torque_info.joint_moving_torque_L, MAX_ACCL_JOINT ); - LimitMax( bl_ctrl->torque_info.joint_moving_torque_R, MAX_ACCL_JOINT ); - -} - -void Chassis_Torque_Combine(chassis_control_t *bl_ctrl) -{ - bl_ctrl->mapping_info .J1_L = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L ); - bl_ctrl->mapping_info .J2_L = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L); - bl_ctrl->mapping_info .J3_L = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L); - bl_ctrl->mapping_info .J4_L = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L ); - bl_ctrl->mapping_info .J1_R = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R ); - bl_ctrl->mapping_info .J2_R = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R); - bl_ctrl->mapping_info .J3_R = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R); - bl_ctrl->mapping_info .J4_R = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R ); - - bl_ctrl->torque_info.foot_horizontal_torque_L = - bl_ctrl->torque_info.foot_balancing_torque_L + bl_ctrl->torque_info.foot_moving_torque_L; - bl_ctrl->torque_info.foot_horizontal_torque_R = - bl_ctrl->torque_info.foot_balancing_torque_R + bl_ctrl->torque_info.foot_moving_torque_R; - - bl_ctrl->foot_motor_L.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_L; - bl_ctrl->foot_motor_R.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_R; - - LimitMax(bl_ctrl->foot_motor_L.torque_out,16383); - LimitMax(bl_ctrl->foot_motor_R.torque_out,16383); - - bl_ctrl->torque_info.joint_horizontal_torque_L = - bl_ctrl->torque_info.joint_balancing_torque_L + bl_ctrl->torque_info.joint_moving_torque_L + bl_ctrl->torque_info.joint_prevent_splits_torque_L; - bl_ctrl->torque_info.joint_horizontal_torque_R = - bl_ctrl->torque_info.joint_balancing_torque_R + bl_ctrl->torque_info.joint_moving_torque_R + bl_ctrl->torque_info.joint_prevent_splits_torque_R; - - bl_ctrl->torque_info.joint_vertical_torque_L = - bl_ctrl->torque_info.joint_stand_torque_L + bl_ctrl->torque_info.joint_roll_torque_L; - bl_ctrl->torque_info.joint_vertical_torque_R = - bl_ctrl->torque_info.joint_stand_torque_R + bl_ctrl->torque_info.joint_roll_torque_R; - - bl_ctrl->torque_info.joint_horizontal_torque_temp1_L = - (bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J3_L ; - bl_ctrl->torque_info.joint_horizontal_torque_temp2_L = - (bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J4_L ; - bl_ctrl->torque_info.joint_horizontal_torque_temp1_R = - (bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J3_R ; - bl_ctrl->torque_info.joint_horizontal_torque_temp2_R = - (bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J4_R ; - - bl_ctrl->torque_info.joint_vertical_torque_temp1_L = - (bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J1_L; - bl_ctrl->torque_info.joint_vertical_torque_temp2_L = - (bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J2_L; - bl_ctrl->torque_info.joint_vertical_torque_temp1_R = - (bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J1_R; - bl_ctrl->torque_info.joint_vertical_torque_temp2_R = - (bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J2_R; - - /****************************************/ - - fp32 MAX_balance = 2000.0f; - - LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_L,MAX_balance); - LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_L,MAX_balance); - LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_R,MAX_balance); - LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_R,MAX_balance); - LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_L,15); - LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_L,15); - LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_R,15); - LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_R,15); - - // 分配到四�?关节电机 - bl_ctrl->joint_motor_1.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp1_L + bl_ctrl->torque_info.joint_vertical_torque_temp1_L; - bl_ctrl->joint_motor_2.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp2_L + bl_ctrl->torque_info.joint_vertical_torque_temp2_L; - bl_ctrl->joint_motor_3.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp1_R + bl_ctrl->torque_info.joint_vertical_torque_temp1_R; - bl_ctrl->joint_motor_4.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp2_R + bl_ctrl->torque_info.joint_vertical_torque_temp2_R; - - if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) - { - bl_ctrl->joint_motor_1.max_torque = LIMITED_TORQUE; - bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE; - } - else if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) - { - bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_1.min_torque = -1.0f * LIMITED_TORQUE; - } - else - { - bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE; - } - - if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) - { - bl_ctrl->joint_motor_3.max_torque = LIMITED_TORQUE; - bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE; - } - else if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) - { - bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_3.min_torque = -1.0f * LIMITED_TORQUE; - } - else - { - bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE; - } - - if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) - { - bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_2.min_torque = -1.0f * LIMITED_TORQUE; - } - else if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) - { - bl_ctrl->joint_motor_2.max_torque = LIMITED_TORQUE; - bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE; - } - else - { - bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE; - } - - if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND ) - { - bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_4.min_torque = -1.0f * LIMITED_TORQUE; - } - else if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND ) - { - bl_ctrl->joint_motor_4.max_torque = LIMITED_TORQUE; - bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE; - } - else - { - bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE; - bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE; - } - - LimitOutput( bl_ctrl->joint_motor_1.torque_out, bl_ctrl->joint_motor_1.min_torque, bl_ctrl->joint_motor_1.max_torque); - LimitOutput( bl_ctrl->joint_motor_2.torque_out, bl_ctrl->joint_motor_2.min_torque, bl_ctrl->joint_motor_2.max_torque); - LimitOutput( bl_ctrl->joint_motor_3.torque_out, bl_ctrl->joint_motor_3.min_torque, bl_ctrl->joint_motor_3.max_torque); - LimitOutput( bl_ctrl->joint_motor_4.torque_out, bl_ctrl->joint_motor_4.min_torque, bl_ctrl->joint_motor_4.max_torque); -} - -void Motor_CMD_Send(chassis_control_t *CMD_Send) -{ - if( CMD_Send->joint_motor_1.motor_mode == MOTOR_FORCE ) - CAN_HT_CMD( 0x01, CMD_Send->joint_motor_1.torque_out ); - else - CAN_HT_CMD( 0x01, 0.0 ); - if( CMD_Send->joint_motor_2.motor_mode == MOTOR_FORCE ) - CAN_HT_CMD( 0x02, CMD_Send->joint_motor_2.torque_out ); - else - CAN_HT_CMD( 0x02, 0.0 ); - - - if( CMD_Send->foot_motor_R.motor_mode != MOTOR_FORCE ) - CMD_Send->foot_motor_R .torque_out = 0.0f; - if( CMD_Send->foot_motor_L.motor_mode != MOTOR_FORCE ) - CMD_Send->foot_motor_L .torque_out = 0.0f; - CAN_BM_CONTROL_CMD( CMD_Send->foot_motor_L.torque_out, CMD_Send->foot_motor_R.torque_out ); - - vTaskDelay(1); - - if( CMD_Send->joint_motor_3.motor_mode == MOTOR_FORCE ) - CAN_HT_CMD( 0x03, CMD_Send->joint_motor_3.torque_out ); - else - CAN_HT_CMD( 0x03, 0.0 ); - - - if( CMD_Send->joint_motor_4.motor_mode == MOTOR_FORCE ) - CAN_HT_CMD( 0x04, CMD_Send->joint_motor_4.torque_out ); - else - CAN_HT_CMD( 0x04, 0.0 ); -} - -void Joint_Motor_to_Init_Pos() -{ - int Init_Time = 0; - while( Init_Time < 300 ) - { - CAN_HT_CMD( 0x01, 1.0 ); - CAN_HT_CMD( 0x02, -1.0 ); - CAN_HT_CMD( 0x03, 1.0 ); - vTaskDelay(1); - CAN_HT_CMD( 0x04, -1.0 ); - vTaskDelay(1); - Init_Time++; - } -} - -void HT_Motor_zero_set(void) -{ - uint8_t tx_buff[8]; - for( int i = 0; i < 7; i++ ) - tx_buff[i] = 0xFF; - tx_buff[7] = 0xfc; - - // ENABLE_CHASSIS - CAN_CMD_HT_Enable( 0x01, tx_buff ); - vTaskDelay(50); - CAN_CMD_HT_Enable( 0x02, tx_buff ); - vTaskDelay(50); - CAN_CMD_HT_Enable( 0x03, tx_buff ); - vTaskDelay(50); - CAN_CMD_HT_Enable( 0x04, tx_buff ); - vTaskDelay(50); - // �?到限�? - Joint_Motor_to_Init_Pos(); - // Set zero init point - tx_buff[7] = 0xfe; - - CAN_CMD_HT_Enable( 0x01, tx_buff ); - vTaskDelay(50); - CAN_CMD_HT_Enable( 0x02, tx_buff ); - vTaskDelay(50); - CAN_CMD_HT_Enable( 0x03, tx_buff ); - vTaskDelay(50); - CAN_CMD_HT_Enable( 0x04, tx_buff ); - - vTaskDelay(50); - -} - -void Motor_Zero_CMD_Send(void) -{ - CAN_HT_CMD( 0x01, 0.0 ); - vTaskDelay(1); - CAN_HT_CMD( 0x02, 0.0 ); - vTaskDelay(1); - CAN_HT_CMD( 0x03, 0.0 ); - vTaskDelay(1); - CAN_HT_CMD( 0x04, 0.0 ); - vTaskDelay(1); - - // CAN_BM_ENABLE_CMD(); - // vTaskDelay(1); - - // CAN_BM_MODE_SET_CMD(); - // vTaskDelay(2); - - // CAN_BM_CURRENT_MODE_CMD(); - // vTaskDelay(1); - -} diff --git a/Chassis_Task.h b/Chassis_Task.h deleted file mode 100644 index 2d33123..0000000 --- a/Chassis_Task.h +++ /dev/null @@ -1,299 +0,0 @@ -#ifndef _CHASSIS_TASK -#define _CHASSIS_TASK - -#include "main.h" -#include "struct_typedef.h" -#include "pid.h" -#include "bsp_can.h" -// ------------- Limit info ------------- -#define MAX_ACCL 13000.0f -#define MAX_ACCL_JOINT 15.0f -#define MAX_FOOT_OUTPUT 2048 - -// ------------- Target value info ------------- -#define SIT_MODE_HEIGHT_SET 0.18f -#define NORMAL_MODE_HEIGHT_SET 0.10f -#define HIGH_MODE_HEIGHT_SET 0.27f -#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f - -// ------------- Mech info ------------- -#define L1 0.15f -#define L2 0.25f -#define L3 0.25f -#define L4 0.15f -#define L5 0.1f - -#define WHEEL_PERIMETER 0.56547 -#define WHEEL_RADIUS 0.09f -#define LEG_OFFSET 0.3790855135f // 水平位置到上限位的夹角 -#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f -#define LOWER_SUPPORT_FORCE 0.0f -#define MOVE_LOWER_BOUND 0.3f -#define EXIT_PITCH_ANGLE 0.1f -#define DANGER_PITCH_ANGLE 0.25f - -#define FEED_f 7.5f -#define FEED_f_1 3.5f - -#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f - -#define MOTOR_POS_UPPER_BOUND 0.05f -#define MOTOR_POS_LOWER_BOUND 1.4f -#define LIMITED_TORQUE 0.5f -#define UNLIMITED_TORQUE 200.0f - -// ------------- Time info ------------- -#define CHASSIS_TASK_INIT_TIME 500 -#define TASK_RUN_TIME 0.002f - -// ------------- Transfer info ------------- -#define MOTOR_ECD_TO_RAD 0.00019174779 // 2*PI / 32767 -#define HALF_ECD_RANGE 14383 -#define HALF_POSITION_RANGE 3.0f -// #define CC 0.00512f -// #define CC 1/494.0f -#define TORQ_K 494.483818182 -// ------------- Math info ------------- -#define PI2 6.28318530717959f -#define PI 3.14159265358979f -#define PI_2 1.57079632679489f -#define PI_4 0.78539816339744f - - - -typedef enum -{ - ENABLE_CHASSIS = 0, - DISABLE_CHASSIS, -} chassis_mode_e; - -typedef enum -{ - NO_FORCE, - FOOT_LAUNCHING, - JOINT_LAUNCHING, - BALANCING_READY, - JOINT_REDUCING, -} chassis_balancing_mode_e; - -typedef enum -{ - NONE, - NORMAL_MOVING_MODE, - ABNORMAL_MOVING_MODE, - JUMPING_MODE, - CAP_MODE, - FLY_MODE, - TK_MODE, -} sport_mode_e; - -typedef enum -{ - READY_TO_JUMP, - PREPARING_LANDING, - PREPARING_STAND_JUMPING, - CONSTACTING_LEGS, - EXTENDING_LEGS, - CONSTACTING_LEGS_2, - FINISHED, -} jumping_stage_e; - -typedef enum -{ - NOT_DEFINE, - STANDING_JUMP, - MOVING_JUMP, -} jumping_mode_e; - -typedef enum -{ - SIT_MODE = 0, - NORMAL_MODE, - HIGH_MODE, - EXTREMELY_HIGH_MODE, - CHANGING_HIGH, -} chassis_high_mode_e; - -typedef enum -{ - MOTOR_NO_FORCE = 0, - MOTOR_FORCE, -} chassis_motor_mode_e; - -typedef enum -{ - ON_GROUND = 0, - OFF_GROUND = 1, -} suspend_flag_e; - -typedef struct -{ - chassis_mode_e chassis_mode, last_chassis_mode; - chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode; - sport_mode_e sport_mode, last_sport_mode; - - jumping_mode_e jumping_mode, last_jumping_mode; - jumping_stage_e jumping_stage, last_jumping_stage; - - chassis_high_mode_e chassis_high_mode, last_chassis_high_mode; - -} mode_t; - -typedef struct -{ - const fp32 *chassis_INS_angle_point; - const fp32 *chassis_INS_gyro_point; - const fp32 *chassis_INS_accel_point; - fp32 yaw_angle, pitch_angle, roll_angle; - fp32 yaw_gyro, pitch_gyro, roll_gyro; - fp32 yaw_accel, pitch_accel, roll_accel; - - fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set; - fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set; - - fp32 ideal_high; - fp32 leg_length_L, last_leg_length_L, leg_length_L_set; - fp32 leg_length_R, last_leg_length_R, leg_length_R_set; - fp32 leg_dlength_L; - fp32 leg_dlength_R; - - fp32 foot_roll_angle; - fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set; - fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set; - fp32 leg_gyro_L, leg_gyro_R; - - fp32 foot_distance, foot_distance_K, foot_distance_set; - fp32 foot_speed, foot_speed_K, foot_speed_set; - - fp32 supportive_force_L, supportive_force_R; - -} chassis_posture_info_t; - -typedef struct -{ - // -------- horizontal force -------- - fp32 joint_balancing_torque_L, joint_balancing_torque_R; - fp32 foot_balancing_torque_L, foot_balancing_torque_R; - - fp32 foot_moving_torque_L, foot_moving_torque_R; - fp32 joint_moving_torque_L, joint_moving_torque_R; - - fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R; - - fp32 joint_horizontal_torque_L, joint_horizontal_torque_R; - fp32 foot_horizontal_torque_L, foot_horizontal_torque_R; - - fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R; - fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L; - - fp32 yaw_torque; - - // -------- vertical force -------- - fp32 joint_roll_torque_L, joint_roll_torque_R; - fp32 joint_stand_torque_L, joint_stand_torque_R; - - fp32 joint_vertical_torque_L, joint_vertical_torque_R; - fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R; - - fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R; - fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L; - -} torque_info_t; - -typedef struct -{ - fp32 J1_L,J2_L; - fp32 J3_L,J4_L; - fp32 J1_R,J2_R; - fp32 J3_R,J4_R; - fp32 invJ1_L,invJ2_L; - fp32 invJ3_L,invJ4_L; - fp32 invJ1_R,invJ2_R; - fp32 invJ3_R,invJ4_R; - -} mapping_info_t; - -typedef struct -{ - const HT_motor_measure_t *motor_measure; - chassis_motor_mode_e motor_mode, last_motor_mode; - - bool_t offline_flag; - - fp32 position; - fp32 init_position; - fp32 position_offset; - - fp32 velocity; - fp32 torque_out, torque_get; - fp32 max_torque, min_torque; -} joint_motor_t; - -typedef struct -{ - motor_measure_t motor_measure; - chassis_motor_mode_e motor_mode, last_motor_mode; - - bool_t offline_flag; - - fp32 distance, distance_offset, last_position, position, turns; - fp32 speed; - fp32 torque_out, torque_get; - -} foot_motor_t; - -typedef struct -{ - bool_t init_flag; - suspend_flag_e suspend_flag_L, last_suspend_flag_L; - suspend_flag_e suspend_flag_R, last_suspend_flag_R; - bool_t Ignore_Off_Ground; - bool_t abnormal_flag; - bool_t static_flag, last_static_flag; - bool_t moving_flag, last_moving_flag; - bool_t rotation_flag; - bool_t controlling_flag; - bool_t set_pos_after_moving; - bool_t overpower_warning_flag; - bool_t last_overpower_warning_flag; - bool_t stablize_high_flag; - bool_t last_stablize_high_flag; -} flag_info_t; - -typedef struct -{ - pid_type_def buffer_control_pid; - pid_type_def cap_pid; - pid_type_def scale_down_pid; -} pid_info_t; - -typedef struct -{ - mode_t mode; - chassis_posture_info_t chassis_posture_info; - torque_info_t torque_info; - mapping_info_t mapping_info; - flag_info_t flag_info; - pid_info_t pid_info; - const Gimbal_ctrl_t *chassis_rc_ctrl; - - joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4; - foot_motor_t foot_motor_L, foot_motor_R; - -}chassis_control_t; - -enum Chassis_Mode -{ - Chassis_No_Force = 0, - Follow_Gimbal, - Follow_Gimbal_90Deg, - No_Follow, - Rotate, -// TK_MODE, -}; - -extern enum Chassis_Mode chassis_mode; -extern chassis_control_t chassis_control; -extern fp32 roll_PD[2]; - -#endif diff --git a/HerKules_VOCAL_SJ_LQR_v4_with_data.m b/HerKules_VOCAL_SJ_LQR_v4_with_data.m deleted file mode 100644 index e762696..0000000 --- a/HerKules_VOCAL_SJ_LQR_v4_with_data.m +++ /dev/null @@ -1,321 +0,0 @@ -% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07） -% v2：添加了可以专门调试定腿长的功能（2024/05/08） -% v3：优化部分注释，添加单位说明（2024/05/15） -% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16） - -% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义 - -%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%% -tic -clear all -clc - -% 定义机器人机体参数 -syms R_w % 驱动轮半径 -syms R_l % 驱动轮轮距/2 -syms l_l l_r % 左右腿长 -syms l_wl l_wr % 驱动轮质心到左右腿部质心距离 -syms l_bl l_br % 机体质心到左右腿部质心距离 -syms l_c % 机体质心到腿部关节中心点距离 -syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量 -syms I_w % 驱动轮转动惯量 (自然坐标系法向) -syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化) -syms I_b % 机体转动惯量 (自然坐标系法向) -syms I_z % 机器人z轴转动惯量 (简化为常量) - -% 定义其他独立变量并补充其导数 -syms theta_wl theta_wr % 左右驱动轮转角 -syms dtheta_wl dtheta_wr -syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b - -% 定义状态向量 -syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b - -% 定义控制向量 -syms T_wl T_wr T_bl T_br - -% 输入物理参数：重力加速度 -syms g - - - -%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%% - -% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式 -eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0; -eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0; -eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0; -eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0; -eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0; -[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b); - - -% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数 -J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]); -J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]); - -% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入 -A = sym('A',[10 10]); -B = sym('B',[10 4]); - -% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109 -for p = 5:2:9 - A_index = (p - 3)/2; - A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2; - A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l); - for q = 6:2:10 - A(q,p) = J_A(q/2,A_index); - end -end - -% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0 -for r = 1:10 - if rem(r,2) == 0 - A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0; - else - A(r,:) = zeros(1,10); - A(r,r+1) = 1; - end -end - -% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104, -for h = 1:4 - B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2; - B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l); - for f = 6:2:10 - B(f,h) = J_B(f/2,h); - end -end - -% B的其余数值为0 -for e = 1:2:9 - B(e,:) = zeros(1,4); -end - - - -%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%% - -% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据 -g_ac = 9.81; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 此处可以输入机器人机体基本参数 % -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -R_w_ac = 0.9; % 驱动轮半径 （单位：m） -R_l_ac = 0.25; % 两个驱动轮之间距离/2 （单位：m） -l_c_ac = 0.037; % 机体质心到腿部关节中心点距离 （单位：m） -m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg） -I_w_ac = (3510000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2） -I_b_ac = 0.260; % 机体转动惯量(自然坐标系法向) （单位：kg m^2） -I_z_ac = 0.226; % 机器人z轴转动惯量 （单位：kg m^2） - -%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%% - -% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉 -% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 - -l_l_ac = 0.18; % 左腿摆杆长度 （左腿对应数据） （单位：m） -l_wl_ac = 0.05; % 左驱动轮质心到左腿摆杆质心距离 （单位：m） -l_bl_ac = 0.13; % 机体转轴到左腿摆杆质心距离 （单位：m） -I_ll_ac = 0.02054500; % 左腿摆杆转动惯量 （单位：kg m^2） -l_r_ac = 0.18; % 右腿摆杆长度 （右腿对应数据） （单位：m） -l_wr_ac = 0.05; % 右驱动轮质心到右腿摆杆质心距离 （单位：m） -l_br_ac = 0.13; % 机体转轴到右腿摆杆质心距离 （单位：m） -I_lr_ac = 0.02054500; % 右腿摆杆转动惯量 （单位：kg m^2） - -% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右 -% 两侧两个关节电机之间的中间点相连所形成的轴 -% （如果目的是小板凳，考虑使左右腿相关数据一致） - -%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll -% 通过以下方式记录数据: 矩阵分4列， -% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m -% 第二列为l_wl，单位：m -% 第三列为l_bl，单位：m -% 第四列为I_ll，单位：kg m^2 -% （注意单位别搞错！） -% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整 - -Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599; - 0.12, 0.0470, 0.0730, 0.01862845; - 0.13, 0.0476, 0.0824, 0.01898641; - 0.14, 0.0480, 0.0920, 0.01931342; - 0.15, 0.0490, 0.1010, 0.01962521; - 0.16, 0.0500, 0.1100, 0.01993092; - 0.17, 0.0510, 0.1190, 0.02023626; - 0.18, 0.0525, 0.1275, 0.02054500; - 0.19, 0.0539, 0.1361, 0.02085969; - 0.20, 0.0554, 0.1446, 0.02118212; - 0.21, 0.0570, 0.1530, 0.02151357; - 0.22, 0.0586, 0.1614, 0.02185496; - 0.23, 0.0600, 0.1700, 0.02220695; - 0.24, 0.0621, 0.1779, 0.02256999; - 0.25, 0.0639, 0.1861, 0.02294442; - 0.26, 0.0657, 0.1943, 0.02333041; - 0.27, 0.0676, 0.2024, 0.02372806; - 0.28, 0.0700, 0.2100, 0.02413735; - 0.29, 0.0713, 0.2187, 0.02455817; - 0.30, 0.0733, 0.2267, 0.02499030]; -% 以上数据应通过实际测量或sw图纸获得 - -% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集 -% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr） -Leg_data_r = Leg_data_l; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 此处可以输入QR矩阵 % -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 矩阵Q中，以下列分别对应： -% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b -lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0; - 0, 2, 0, 0, 0, 0, 0, 0, 0, 0; - 0, 0, 12000, 0, 0, 0, 0, 0, 0, 0; - 0, 0, 0, 200, 0, 0, 0, 0, 0, 0; - 0, 0, 0, 0, 1000, 0, 0, 0, 0, 0; - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; - 0, 0, 0, 0, 0, 0, 1000, 0, 0, 0; - 0, 0, 0, 0, 0, 0, 0, 1, 0, 0; - 0, 0, 0, 0, 0, 0, 0, 0, 20000, 0; - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]; -% 其中： -% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数 -% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数 -% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数 -% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数 -% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数 - -% 矩阵中，以下列分别对应： -% T_wl T_wr T_bl T_br -lqr_R = [0.25, 0, 0, 0; - 0, 0.25, 0, 0; - 0, 0, 1.5, 0; - 0, 0, 0, 1.5]; -% 其中： -% T_wl: 左侧驱动轮输出力矩 -% T_wr：右侧驱动轮输出力矩 -% T_bl：左侧髋关节输出力矩 -% T_br：右腿髋关节输出力矩 -% 单位皆为Nm - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - -%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%% - -% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉， -% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 -K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R) -K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.') - - -%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%% - -sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数 - -length = size(Leg_data_l,1); % 测量腿部数据集的行数 - -% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列 -% 是l_r，第三列是对应的K_ij的数值 -K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。 - -for i = 1:length - for j = 1:length - index = (i - 1)*length + j; - l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据 - l_wl_ac = Leg_data_l(i,2); - l_bl_ac = Leg_data_l(i,3); - I_ll_ac = Leg_data_l(i,4); - l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据 - l_wr_ac = Leg_data_r(j,2); - l_br_ac = Leg_data_r(j,3); - I_lr_ac = Leg_data_r(j,4); - for k = 1:40 - K_sample(index,1,k) = l_l_ac; - K_sample(index,2,k) = l_r_ac; - end - K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R); - % 根据指定的l_l,l_r输入对应的K_ij的数值 - for l = 1:4 - for m = 1:10 - K_sample(index,3,(l - 1)*10 + m) = K(l,m); - end - end - end -end - -% 创建收集全部K_ij的多项式拟合的全部系数的集合 -K_Fit_Coefficients = zeros(40,6); -for n = 1:40 - K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22'); - K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果 -end -Polynomial_expression = formula(K_Surface_Fit) - -% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数 -% 每一行和K_ij的对应关系如下： -% - 第1行对应K_1,1 -% - 第14行对应K_2,4 -% - 第22行对应K_3,2 -% - 第37行对应K_4,7 -% ... 其他行对应关系类似 -% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2 -% 其中x对应左腿腿长l_l，y对应右腿腿长l_r -% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数 -% 每一列和系数pij的对应关系如下： -% - 第1列对应p00 -% - 第2列对应p10 -% - 第3列对应p01 -% - 第4列对应p20 -% - 第5列对应p11 -% - 第6列对应p02 -K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.') - -% 正确食用方法： -% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为： -% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2 -% 2.并填入对应系数即可 - -toc - -%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释 -% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新 -% （前面的蛆，以后再来探索吧（bushi - - - -%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%% - -function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R) - % 基于机体以及物理参数，获得控制矩阵A，B的全部数值 - A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... - [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... - m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); - B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... - [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... - m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); - - % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K - % P为Riccati方程的解，矩阵L可以无视 - [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]); -end - diff --git a/LQR_README.md b/LQR_README.md deleted file mode 100644 index 05f4280..0000000 --- a/LQR_README.md +++ /dev/null @@ -1,185 +0,0 @@ -# LQR控制器设计文档 - -本文档描述了为平衡步兵轮腿机器人设计的LQR（线性二次调节器）控制器。 - -## 1. 系统建模 - -### 1.1 状态变量定义 - -系统状态向量为6维： -``` -x = [theta, d_theta, x, d_x, phi, d_phi]^T -``` - -其中： -- `theta`: 等效摆杆与竖直方向夹角 (rad) -- `d_theta`: 等效摆杆角速度 (rad/s) -- `x`: 驱动轮位移 (m) -- `d_x`: 驱动轮速度 (m/s) -- `phi`: 机体与水平夹角（俯仰角） (rad) -- `d_phi`: 机体俯仰角速度 (rad/s) - -### 1.2 控制输入 - -控制输入为2维： -``` -u = [T, Tp]^T -``` - -其中： -- `T`: 驱动轮输出力矩 (N·m) -- `Tp`: 髋关节输出力矩 (N·m) - -### 1.3 系统动力学方程 - -基于拉格朗日方程建立的系统动力学方程组： - -1. **驱动轮动力学**： - ``` - d²x/dt² = (T - N*R)/(Iw/R + mw*R) - ``` - -2. **摆杆动力学**： - ``` - Ip*d²theta/dt² = (P*L + PM*LM)*sin(theta) - (N*L + NM*LM)*cos(theta) - T + Tp - ``` - -3. **机体动力学**： - ``` - IM*d²phi/dt² = Tp + NM*l*cos(phi) + PM*l*sin(phi) - ``` - -### 1.4 线性化 - -在平衡点附近进行线性化，得到状态空间模型： -``` -dx/dt = A*x + B*u -``` - -其中A和B矩阵通过雅可比矩阵计算得到。 - -## 2. LQR控制器设计 - -### 2.1 代价函数 - -LQR控制器最小化以下二次代价函数： -``` -J = ∫[x^T*Q*x + u^T*R*u]dt -``` - -权重矩阵选择： -- `Q = diag([100, 1, 500, 100, 5000, 1])` (状态权重) -- `R = diag([240, 25])` (控制权重) - -### 2.2 增益矩阵计算 - -由于系统参数随腿长变化，采用多项式拟合方法： - -1. **离线计算**：对不同腿长（0.1-0.4m），使用MATLAB计算最优LQR增益 -2. **多项式拟合**：对每个增益元素进行3阶多项式拟合 -3. **在线计算**：根据当前腿长实时计算增益矩阵 - -### 2.3 控制律 - -``` -u = -K(L) * (x - x_ref) -``` - -其中： -- `K(L)`: 与腿长L相关的增益矩阵 -- `x_ref`: 参考状态 - -## 3. 实现架构 - -### 3.1 文件结构 - -``` -User/component/ -├── lqr.h # LQR控制器头文件 -├── lqr.c # LQR控制器实现 -└── lqr_config_example.c # 配置示例 -``` - -### 3.2 主要功能模块 - -1. **LQR_Init()**: 初始化控制器 -2. **LQR_UpdateState()**: 更新系统状态 -3. **LQR_CalculateGainMatrix()**: 计算增益矩阵 -4. **LQR_Control()**: 执行控制计算 -5. **LQR_GetOutput()**: 获取控制输出 - -### 3.3 与VMC的集成 - -LQR控制器输出虚拟力，通过VMC转换为关节力矩： - -```c -// LQR计算虚拟力和力矩 -LQR_Control(&chassis->lqr); -LQR_GetOutput(&chassis->lqr, &lqr_output); - -// VMC将虚拟力矩转换为关节力矩 -VMC_InverseSolve(&chassis->vmc[0], virtual_force, lqr_output.Tp); -VMC_GetJointTorques(&chassis->vmc[0], &tau_front, &tau_rear); -``` - -## 4. 调试和优化 - -### 4.1 参数调优 - -1. **Q矩阵调优**： - - 增大theta权重提高平衡性能 - - 增大phi权重减少机体摆动 - - 增大x,d_x权重提高位置跟踪精度 - -2. **R矩阵调优**： - - 增大R减少控制输出，提高稳定性 - - 减小R提高响应速度 - -### 4.2 常见问题 - -1. **增益过大**：导致系统震荡，需减小Q或增大R -2. **响应迟缓**：增大Q权重或减小R权重 -3. **静态误差**：检查目标状态设置是否合理 - -### 4.3 监控指标 - -建议监控以下指标： -- 状态误差大小 -- 控制输出饱和情况 -- 系统稳定性指标 - -## 5. 使用示例 - -```c -// 1. 配置LQR参数 -Chassis_Params_t chassis_params = { - .lqr_param = { - .max_wheel_torque = 10.0f, - .max_joint_torque = 5.0f, - }, - .lqr_gains = example_lqr_gains, -}; - -// 2. 初始化底盘（自动初始化LQR） -Chassis_Init(&chassis, &chassis_params, 1000.0f); - -// 3. 控制循环 -while(1) { - // 更新传感器数据 - Chassis_UpdateFeedback(&chassis); - Chassis_UpdateIMU(&chassis, imu_data); - - // 执行LQR控制 - Chassis_Control(&chassis, &cmd); - - // 延时 - osDelay(1); -} -``` - -## 6. 参考资料 - -1. MATLAB仿真代码：`utils/Simulation-master/balance/series_legs/` -2. C++参考实现：`User/module/mod_wheelleg_chassis.cpp` -3. VMC控制器：`User/component/vmc.c` -4. 理论参考：Modern Control Engineering, Ogata diff --git a/LQR_修正建议.md b/LQR_修正建议.md deleted file mode 100644 index 64bc7da..0000000 --- a/LQR_修正建议.md +++ /dev/null @@ -1,67 +0,0 @@ -## LQR控制器修正建议 - -### 问题总结： -1. LQR增益计算方式不一致（3次多项式 vs 2次多项式） -2. 状态向量维度错误（12维 vs 10维） -3. 控制律映射不正确 -4. 状态定义与MATLAB模型不匹配 - -### 修正建议： - -#### 1. 修正LQR_K_calc函数 -应该使用2次多项式而不是3次： -```cpp -float VMC::LQR_K_calc(float *coe, float l_l, float l_r) { - // 使用MATLAB中定义的2次多项式 - // p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2 - return coe[0] + coe[1]*l_l + coe[2]*l_r + coe[3]*l_l*l_l + coe[4]*l_l*l_r + coe[5]*l_r*l_r; -} -``` - -#### 2. 修正增益矩阵维度 -LQR增益矩阵应该是4×10，总共40个系数： -```cpp -// 为每条腿分配40个LQR系数，而不是12个 -float LQR_K[40]; // 4×10矩阵，展开为一维数组 -``` - -#### 3. 修正状态向量定义 -确保状态向量与MATLAB模型一致： -```cpp -// 状态向量：[s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b] -float state_error[10] = { - move_argu_.xhat - move_argu_.target_x, // s误差 - move_argu_.x_dot_hat - move_argu_.target_dot_x, // ds误差 - this->yaw_ - 0.0f, // phi误差 - this->gyro_.z - 0.0f, // dphi误差 - leg_argu_[0].theta - 平衡角度, // theta_ll误差 - leg_argu_[0].d_theta - 0.0f, // dtheta_ll误差 - leg_argu_[1].theta - 平衡角度, // theta_lr误差 - leg_argu_[1].d_theta - 0.0f, // dtheta_lr误差 - this->pit_ - 0.0f, // theta_b误差 - this->gyro_.x - 0.0f // dtheta_b误差 -}; -``` - -#### 4. 修正控制律计算 -使用标准的LQR控制律： -```cpp -// 计算控制输出：u = -K * (x - x_ref) -float control[4] = {0}; // [T_wl, T_wr, T_bl, T_br] - -for(int i = 0; i < 4; i++) { - for(int j = 0; j < 10; j++) { - control[i] += LQR_K[i*10 + j] * state_error[j]; - } - control[i] = -control[i]; // 负反馈 -} - -leg_argu_[0].Tw = control[0]; // T_wl -leg_argu_[1].Tw = control[1]; // T_wr -leg_argu_[0].Tp = control[2]; // T_bl -leg_argu_[1].Tp = control[3]; // T_br -``` - -### 总结： -当前的LQR实现在数学原理上有偏差，需要按照标准的LQR控制理论和MATLAB模型进行修正。 -主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。 diff --git a/MDK-ARM/DevC.uvprojx b/MDK-ARM/DevC.uvprojx index abe2d18..27e7b83 100644 --- a/MDK-ARM/DevC.uvprojx +++ b/MDK-ARM/DevC.uvprojx @@ -139,7 +139,7 @@ 1 BIN\UL2V8M.DLL - + "" () @@ -314,7 +314,7 @@ 1 - 2 + 4 0 0 1 diff --git a/User/README.md b/User/README.md deleted file mode 100644 index 0636689..0000000 --- a/User/README.md +++ /dev/null @@ -1,297 +0,0 @@ -# 轮腿机器人LQR平衡控制系统 - -这是一个完整的轮腿机器人LQR+VMC平衡控制系统，支持6个电机（2个轮电机+4个关节电机）的协调控制。 - -## 系统架构 - -``` -┌─────────────────┐ ┌──────────────────┐ ┌─────────────────┐ -│ 传感器输入 │ │ 平衡控制器 │ │ 电机控制输出 │ -│ │ │ │ │ │ -│ • IMU姿态数据 │────▶│ LQR控制器 │────▶│ • 轮电机力矩 │ -│ • 电机反馈数据 │ │ VMC控制器 │ │ • 关节电机力矩 │ -│ • 遥控器命令 │ │ 状态估计器 │ │ • MIT控制参数 │ -└─────────────────┘ └──────────────────┘ └─────────────────┘ -``` - -## 文件结构 - -### 核心控制文件 -- `balance_control.h/c` - 主控制器，整合LQR和VMC -- `lqr.h/c` - LQR控制器实现 -- `vmc.h/c` - 虚拟模型控制器 -- `kinematics.h/c` - 运动学计算 - -### 使用示例 -- `balance_control_example.c` - 完整的使用示例 - -### MATLAB工具 -- `lqr_design_optimized.m` - LQR参数设计和系数生成 -- `vmc.m` - VMC控制器设计参考 - -## 快速开始 - -### 1. 硬件连接 -确保以下硬件正确连接： -- IMU传感器（提供机体姿态） -- 2个轮电机（支持转矩电流控制） -- 4个关节电机（支持MIT控制模式） -- CAN总线通信 - -### 2. 参数配置 - -#### 2.1 运行MATLAB脚本生成LQR参数 -```matlab -% 在MATLAB中运行 -run('utils/lqr_design_optimized.m'); -``` -这将生成K矩阵的拟合系数，复制到`lqr.c`中的`K_fit_coefficients`数组。 - -#### 2.2 修改机器人物理参数 -在`lqr.h`中修改您的机器人参数： -```c -#define R_W 0.09f // 驱动轮半径 (m) -#define R_L 0.25f // 两个驱动轮之间距离/2 (m) -#define M_W 0.8f // 驱动轮质量 (kg) -#define M_L 1.6183599f // 腿部质量 (kg) -#define M_B 11.542f // 机体质量 (kg) -// ... 其他参数 -``` - -#### 2.3 配置电机ID和CAN接口 -在`balance_control_example.c`中修改电机ID： -```c -#define WHEEL_LEFT_ID 0x01 -#define WHEEL_RIGHT_ID 0x02 -#define HIP_LEFT_ID 0x03 -#define HIP_RIGHT_ID 0x04 -#define KNEE_LEFT_ID 0x05 -#define KNEE_RIGHT_ID 0x06 -``` - -### 3. 代码集成 - -#### 3.1 在main函数中初始化 -```c -#include "balance_control.h" - -int main(void) -{ - // HAL初始化 - HAL_Init(); - SystemClock_Config(); - - // 初始化外设 - MX_CAN1_Init(); - MX_TIM1_Init(); - - // 初始化平衡控制系统 - balance_system_init(); - - // 启动控制循环 - HAL_TIM_Base_Start_IT(&htim1); // 1kHz控制频率 - - while(1) - { - // 主循环处理其他任务 - HAL_Delay(100); - } -} -``` - -#### 3.2 定时器中断处理 -```c -void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) -{ - if (htim->Instance == TIM1) { - balance_control_task(); // 1kHz控制任务 - } -} -``` - -### 4. 传感器数据接口 - -#### 4.1 实现IMU数据读取 -在`balance_control_example.c`中实现`read_imu_data()`函数： -```c -void read_imu_data(imu_data_t* imu) -{ - // 从您的IMU传感器读取数据 - imu->pitch = get_imu_pitch(); - imu->roll = get_imu_roll(); - imu->yaw = get_imu_yaw(); - imu->pitch_rate = get_imu_pitch_rate(); - imu->roll_rate = get_imu_roll_rate(); - imu->yaw_rate = get_imu_yaw_rate(); - // ... 其他数据 -} -``` - -#### 4.2 实现电机反馈读取 -```c -void read_motor_feedback(motor_feedback_t* motor_fb) -{ - // 从CAN总线读取电机反馈 - motor_fb->wheel_left_angle = get_motor_angle(WHEEL_LEFT_ID); - motor_fb->wheel_right_angle = get_motor_angle(WHEEL_RIGHT_ID); - motor_fb->hip_left_angle = get_motor_angle(HIP_LEFT_ID); - // ... 其他电机数据 -} -``` - -#### 4.3 实现电机控制输出 -```c -void send_motor_control(const motor_control_t* motor_ctrl) -{ - // 发送轮电机转矩控制指令 - send_wheel_torque_cmd(WHEEL_LEFT_ID, motor_ctrl->wheel_left_torque_cmd); - send_wheel_torque_cmd(WHEEL_RIGHT_ID, motor_ctrl->wheel_right_torque_cmd); - - // 发送关节电机MIT控制指令 - send_mit_control_cmd(HIP_LEFT_ID, - 0, // 位置指令 - 0, // 速度指令 - motor_ctrl->hip_left_kp, - motor_ctrl->hip_left_kd, - motor_ctrl->hip_left_torque_cmd); - // ... 其他关节电机 -} -``` - -## 控制模式 - -### 1. 平衡模式 (ROBOT_STATE_BALANCE) -- 自动保持机体平衡 -- 响应遥控器前进/转向命令 -- 自动调节腿长以适应地形 - -### 2. 运动模式 (ROBOT_STATE_MOVE) -- 在平衡基础上执行运动控制 -- 支持前进、后退、转向 -- 支持高度调节 - -### 3. 紧急停止 (ROBOT_STATE_EMERGENCY) -- 立即停止所有电机输出 -- 安全保护机制 - -## 控制参数调节 - -### LQR参数调节 -在MATLAB脚本`lqr_design_optimized.m`中修改Q和R矩阵： -```matlab -% 状态权重矩阵Q - 增大数值提高控制精度 -Q = diag([ - 10, % s - 水平位移 - 5, % ds - 水平速度 - 20, % phi - 偏航角 - 10, % dphi - 偏航角速度 - 100, % theta_ll - 左腿角 - 50, % dtheta_ll - 左腿角速度 - 100, % theta_lr - 右腿角 - 50, % dtheta_lr - 右腿角速度 - 200, % theta_b - 机体倾斜角 (最重要) - 100 % dtheta_b - 机体角速度 -]); - -% 控制权重矩阵R - 增大数值降低控制量 -R = diag([ - 0.1, % T_wl - 左轮力矩 - 0.1, % T_wr - 右轮力矩 - 1.0, % T_bl - 左腿力矩 - 1.0 % T_br - 右腿力矩 -]); -``` - -### VMC参数调节 -在`balance_control.c`中修改VMC参数： -```c -vmc_params_t vmc_params = { - .K_spring = 800.0f, // 径向弹簧刚度 - 控制腿长刚度 - .K_damper = 80.0f, // 径向阻尼系数 - 控制腿长阻尼 - .K_theta = 150.0f, // 角度刚度 - 控制腿角刚度 - .K_dtheta = 15.0f, // 角速度阻尼 - 控制腿角阻尼 - .max_radial_force = 1000.0f, // 最大径向力 - .max_tangential_force = 500.0f, // 最大切向力 -}; -``` - -## 安全机制 - -### 1. 倾斜角保护 -```c -#define MAX_TILT_ANGLE 0.5f // 最大倾斜角 (rad) -``` - -### 2. 腿长保护 -```c -#define LEG_MIN_REACH 0.05f // 最小腿长 (m) -#define LEG_MAX_REACH 0.28f // 最大腿长 (m) -``` - -### 3. 力矩限制 -```c -#define MAX_WHEEL_TORQUE 50.0f // 最大轮子力矩 (N*m) -#define MAX_JOINT_TORQUE 100.0f // 最大关节力矩 (N*m) -``` - -## 调试和监控 - -### 1. 串口调试输出 -系统会定期输出关键状态信息： -``` -=== Balance Control Status === -Task Count: 1000 -Robot State: 1 -Safety Flag: 1 -IMU Pitch: 0.050 rad (2.9 deg) -Left Leg Length: 0.200 m -Right Leg Length: 0.200 m -============================== -``` - -### 2. 状态监控 -可以通过以下变量监控系统状态： -- `g_balance_ctrl.state` - 机器人状态 -- `g_balance_ctrl.safety_flag` - 安全标志 -- `g_balance_ctrl.robot_state` - 完整机器人状态 - -## 常见问题 - -### Q: 机器人启动后不稳定？ -A: -1. 检查IMU标定是否正确 -2. 调整LQR的Q矩阵，增大机体角度权重 -3. 检查电机正负方向是否正确 - -### Q: 腿部抖动？ -A: -1. 降低VMC的刚度参数 -2. 增加VMC的阻尼参数 -3. 检查关节电机的刚度设置 - -### Q: 轮子打滑？ -A: -1. 降低LQR的轮子力矩输出 -2. 增加R矩阵中轮子力矩的权重 -3. 检查地面摩擦条件 - -### Q: 系统响应慢？ -A: -1. 增加控制频率（目前为1kHz） -2. 调整LQR的Q矩阵权重 -3. 检查CAN通信延迟 - -## 技术支持 - -如需技术支持或有疑问，请： -1. 检查上述常见问题 -2. 确认硬件连接正确 -3. 验证参数配置无误 -4. 查看串口调试输出 - -## 更新日志 - -- v1.0 (2025-08-30): 初始版本，支持LQR+VMC协调控制 -- 完整的6电机控制接口 -- MATLAB参数设计工具 -- 完善的安全保护机制 diff --git a/User/component/limiter.c b/User/component/limiter.c index 71e4bf1..7c4d7ab 100644 --- a/User/component/limiter.c +++ b/User/component/limiter.c @@ -105,3 +105,26 @@ float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, else return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq; } + +/** + * @brief 速度限制器，计算目标速度 + * @param accl_limit 加速度限制 + * @param speed_now 当前速度 + * @param speed_target 目标速度 + * @param dt 时间间隔 + * @return float 计算得到的目标速度 + */ +float SpeedLimit_TargetSpeed(float accl_limit, float speed_now, float speed_target, float dt){ + float speed_diff = speed_target - speed_now; + float max_speed_change = accl_limit * dt; + + if (fabsf(speed_diff) > max_speed_change) { + if (speed_diff > 0) { + return speed_now + max_speed_change; + } else { + return speed_now - max_speed_change; + } + } else { + return speed_target; + } +} \ No newline at end of file diff --git a/User/component/limiter.h b/User/component/limiter.h index 7c24cb5..93499ab 100644 --- a/User/component/limiter.h +++ b/User/component/limiter.h @@ -53,3 +53,13 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer); */ float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, float heat_increase, bool is_big); + +/** + * @brief 速度限制器，计算目标速度 + * @param accl_limit 加速度限制 + * @param speed_now 当前速度 + * @param speed_target 目标速度 + * @param dt 时间间隔 + * @return float 计算得到的目标速度 + */ +float SpeedLimit_TargetSpeed(float accl_limit, float speed_now, float speed_target, float dt); \ No newline at end of file diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 1a34274..08c2051 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -3,32 +3,49 @@ #include "bsp/can.h" #include "component/user_math.h" #include "component/kinematics.h" +#include "component/limiter.h" #include #include - -float fn=0.0f; -float tp=0.0f; - -float t1=0.0f; -float t2=0.0f; - - +/** + * @brief + * @param c + * @return + */ +static int8_t Chassis_MotorEnable(Chassis_t *c) { + if (c == NULL) return CHASSIS_ERR_NULL; + + for (int i = 0; i < 2; i++) { + MOTOR_LK_MotorOn(&c->param->wheel_motors[i]); + } + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&c->param->joint_motors[i]); + } + + return CHASSIS_OK; +} static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) { if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */ if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */ - MOTOR_LK_MotorOn(&c->param->wheel_motors[0]); - MOTOR_LK_MotorOn(&c->param->wheel_motors[1]); - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&c->param->joint_motors[i]); - } + Chassis_MotorEnable(c); // 清空pid积分 PID_Reset(&c->pid.leglength[0]); PID_Reset(&c->pid.leglength[1]); PID_Reset(&c->pid.yaw); PID_Reset(&c->pid.roll); + PID_Reset(&c->pid.tp_pid[0]); + PID_Reset(&c->pid.tp_pid[1]); + + c->yaw_control.target_yaw = c->feedback.imu.euler.yaw; + + //清空位移 + c->chassis_state.position_x = 0.0f; + c->chassis_state.velocity_x = 0.0f; + c->chassis_state.last_velocity_x = 0.0f; + + LQR_Reset(&c->lqr[0]); LQR_Reset(&c->lqr[1]); c->mode = mode; @@ -91,7 +108,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){ PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, ¶m->leglength); PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw); PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll); - + PID_Init(&c->pid.tp_pid[0], KPID_MODE_CALC_D, target_freq, ¶m->tp_pid); + PID_Init(&c->pid.tp_pid[1], KPID_MODE_CALC_D, target_freq, ¶m->tp_pid); + /*初始化LQR控制器*/ LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque); LQR_SetGainMatrix(&c->lqr[0], ¶m->lqr_gains); @@ -273,6 +292,7 @@ void Chassis_Output(Chassis_t *c) { } for (int i = 0; i < 2; i++) { MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]); + // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); } // 这个函数已经在各个模式中直接调用了电机输出函数 // 如果需要统一输出，可以在这里实现 @@ -310,7 +330,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { xhat = c->chassis_state.position_x; // 目标设定 - target_dot_x = c_cmd->move_vec.vx; + target_dot_x = c_cmd->move_vec.vx/3.0f; + target_dot_x = SpeedLimit_TargetSpeed(1000.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s² target_x += target_dot_x * c->dt; /* 分别计算左右腿的LQR控制 */ @@ -331,14 +352,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_State_t target_state = {0}; target_state.theta = 0.0f; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.x = 0; // 目标位置 - target_state.d_x = 0.0f; // 目标速度 + // target_state.x = 0; // 目标位置 + // target_state.d_x = 0.0f; // 目标速度 target_state.phi = 0.0f; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 // target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度 // target_state.d_theta = 0.0f; // 目标摆杆角速度 - // target_state.x = target_x - 0.56f; // 目标位置 - // target_state.d_x = target_dot_x; // 目标速度 + target_state.x = target_x; // 目标位置 + target_state.d_x = target_dot_x; // 目标速度 // target_state.phi = 0.16f; // 目标俯仰角 // target_state.d_phi = 0.0f; // 目标俯仰角速度 @@ -356,6 +377,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) { Tw[leg] = lqr_output.T; Tp[leg] = lqr_output.Tp; + // Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出 + // Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出 } else { Tw[leg] = 0.0f; Tp[leg] = 0.0f; @@ -373,48 +396,17 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + c->lqr[leg].K_matrix[1][1] * d_theta_error); } } - - /* 转向控制（参考C++版本） */ - float yaw_force = 0.0f; - - // 更新当前yaw角度 + c->yaw_control.current_yaw = c->feedback.imu.euler.yaw; + + c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.002f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s - // 更新目标yaw角度（基于遥控器输入） - if (fabsf(c_cmd->move_vec.wz) > 0.01f) { - // 有转向指令时，更新目标角度 - c->yaw_control.target_yaw += c_cmd->move_vec.wz * c->dt; - // 角度归一化到 [-π, π] - while (c->yaw_control.target_yaw > M_PI) { - c->yaw_control.target_yaw -= M_2PI; - } - while (c->yaw_control.target_yaw < -M_PI) { - c->yaw_control.target_yaw += M_2PI; - } - } + c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt); - if (onground_flag[0] && onground_flag[1]) { - // 使用PID控制yaw角度 - float yaw_error = c->yaw_control.target_yaw - c->yaw_control.current_yaw; - - // 处理角度循环误差（选择最短路径） - if (yaw_error > M_PI) { - yaw_error -= M_2PI; - } else if (yaw_error < -M_PI) { - yaw_error += M_2PI; - } - - // 使用yaw轴陀螺仪作为微分项 - float yaw_d_feedback = c->feedback.imu.gyro.z; - - yaw_force = PID_Calc(&c->pid.yaw, 0.0f, yaw_error, yaw_d_feedback, c->dt); - c->yaw_control.yaw_force = yaw_force; - } /* 轮毂力矩输出（参考C++版本的减速比） */ - c->output.wheel[0] = Tw[0] / 4.9256f - yaw_force; // 左轮 - c->output.wheel[1] = Tw[1] / 4.9256f + yaw_force; // 右轮 - + c->output.wheel[0] = Tw[0] / 5.0f - c->yaw_control.yaw_force; + c->output.wheel[1] = Tw[1] / 5.0f + c->yaw_control.yaw_force; /* 腿长控制和VMC逆解算（使用PID控制） */ float virtual_force[2]; float target_L0[2]; @@ -424,24 +416,36 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, c->feedback.imu.gyro.x, c->dt); // 目标腿长设定 - target_L0[0] = 0.12f + c_cmd->height * 0.1f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿 - target_L0[1] = 0.12f + c_cmd->height * 0.1f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿 + target_L0[0] = 0.15f + c_cmd->height * 0.01f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿 + target_L0[1] = 0.15f + c_cmd->height * 0.01f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿 // 获取腿长变化率 VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL); VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL); + /* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */ + float Delta_Tp[2]; + // 使用tp_pid进行左右腿摆角同步控制 + // 左腿补偿力矩：使左腿theta向右腿theta靠拢 + Delta_Tp[0] = -leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp_pid[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt); + // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反） + Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp_pid[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt); + for (int leg = 0; leg < 2; leg++) { // 使用PID进行腿长控制 float pid_output = PID_Calc(&c->pid.leglength[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt); // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - virtual_force[leg] = Tp[leg] * sinf(leg_theta[leg]) + - pid_output + // PID腿长控制输出 - 45.0f; // 基础支撑力 + virtual_force[leg] = (Tp[leg] + Delta_Tp[leg]) * sinf(leg_theta[leg]) + + pid_output + 20.0f; + // + // PID腿长控制输出 + // 45.0f; // 基础支撑力 - // VMC逆解算 - if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg]) == 0) { + // VMC逆解算（包含摆角补偿） + // virtual_force[leg] = 0.0f; // 暂时屏蔽虚拟力输出，避免VMC逆解算失败 + // Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败 + // Delta_Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败 + if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg] + Delta_Tp[leg]) == 0) { if (leg == 0) { VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque); } else { diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 0dbffe9..ffe1d28 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -36,7 +36,7 @@ extern "C" { typedef enum { CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ - CHASSIS_MODE_RECOVER, + CHASSIS_MODE_RECOVER, /* 复位模式 */ CHASSIS_MODE_WHELL_BALANCE, /* 平衡模式，底盘自我平衡 */ CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */ } Chassis_Mode_t; @@ -73,6 +73,7 @@ typedef struct { KPID_Params_t leglength; /* 腿长PID控制参数，用于控制虚拟腿长度 */ KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */ KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */ + KPID_Params_t tp_pid; /* 摆力矩PID控制参数，用于控制摆力矩 */ MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */ @@ -151,9 +152,8 @@ typedef struct { KPID_t balance; /* 平衡用的PID */ KPID_t yaw; /* yaw轴控制PID */ KPID_t roll; /* 横滚角控制PID */ - + KPID_t tp_pid[2]; KPID_t leglength[2]; /* 两条腿的腿长PID */ - } pid; /* 滤波器 */ @@ -186,6 +186,12 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq); int8_t Chassis_UpdateFeedback(Chassis_t *c); +/** + * @brief 更新IMU数据 + * @param c 包含底盘数据的结构体 + * @param imu IMU数据 + * @return + */ int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu); /** * \brief 运行底盘控制逻辑 diff --git a/User/module/chassis.c b/User/module/chassis.c deleted file mode 100644 index e69de29..0000000 diff --git a/User/module/chassis.h b/User/module/chassis.h deleted file mode 100644 index 9b811aa..0000000 --- a/User/module/chassis.h +++ /dev/null @@ -1,180 +0,0 @@ -/* - * 底盘模组 - */ - -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------------- */ -#include "component\cmd.h" -#include "component\filter.h" -#include "component\mixer.h" -#include "component\pid.h" -#include "device\can.h" -#include "device\referee.h" - -/* Exported constants ------------------------------------------------------- */ -#define CHASSIS_OK (0) /* 运行正常 */ -#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */ -#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */ -#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */ -#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */ - -/* Exported macro ----------------------------------------------------------- */ -/* Exported types ----------------------------------------------------------- */ - -/* 底盘类型（底盘的物理设计） */ -typedef enum { - CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */ - CHASSIS_TYPE_PARLFIX4, /* 平行摆设的四个驱动轮 */ - CHASSIS_TYPE_PARLFIX2, /* 平行摆设的两个驱动轮 */ - CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆设的四个全向轮 */ - CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */ - CHASSIS_TYPE_DRONE, /* 底盘为无人机 */ - CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */ -} Chassis_Type_t; - -/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */ -typedef struct { - Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */ - KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */ - KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */ - - /* 低通滤波器截止频率 */ - struct { - float in; /* 输入 */ - float out; /* 输出 */ - } low_pass_cutoff_freq; - - /* 电机反装 应该和云台设置相同*/ - struct { - bool yaw; - } reverse; - -} Chassis_Params_t; - -/* - * 运行的主结构体，所有这个文件里的函数都在操作这个结构体 - * 包含了初始化参数，中间变量，输出变量 - */ -typedef struct { - uint32_t lask_wakeup; - float dt; - - const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */ - AHRS_Eulr_t *mech_zero; - - /* 模块通用 */ - CMD_ChassisMode_t mode; /* 底盘模式 */ - - /* 底盘设计 */ - int8_t num_wheel; /* 底盘轮子数量 */ - Mixer_t mixer; /* 混合器，移动向量->电机目标值 */ - - MoveVector_t move_vec; /* 底盘实际的运动向量 */ - - /* 反馈信息 */ - struct { - float gimbal_yaw_encoder; /* 云台Yaw轴编码器角度 */ - float *motor_rpm; /* 电机转速的动态数组，单位：RPM */ - float *motor_current; /* 转矩电流 单位：A */ - } feedback; - - float wz_multi; /* 小陀螺模式旋转方向 */ - - /* PID计算的目标值 */ - struct { - float *motor_rpm; /* 电机转速的动态数组，单位：RPM */ - } setpoint; - - /* 反馈控制用的PID */ - struct { - KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */ - KPID_t follow; /* 跟随云台用的PID */ - } pid; - - /* 滤波器 */ - struct { - LowPassFilter2p_t *in; /* 反馈值滤波器 */ - LowPassFilter2p_t *out; /* 输出值滤波器 */ - } filter; - - float *out; /* 电机最终的输出值的动态数组 */ -} Chassis_t; - -/* Exported functions prototypes -------------------------------------------- */ - -/** - * \brief 初始化底盘 - * - * \param c 包含底盘数据的结构体 - * \param param 包含底盘参数的结构体指针 - * \param target_freq 任务预期的运行频率 - * - * \return 函数运行结果 - */ -int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param, - AHRS_Eulr_t *mech_zero, float target_freq); - -/** - * \brief 更新底盘的反馈信息 - * - * \param c 包含底盘数据的结构体 - * \param can CAN设备结构体 - * - * \return 函数运行结果 - */ -int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can); - -/** - * \brief 运行底盘控制逻辑 - * - * \param c 包含底盘数据的结构体 - * \param c_cmd 底盘控制指令 - * \param dt_sec 两次调用的时间间隔 - * - * \return 函数运行结果 - */ -int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, - uint32_t now); - -/** - * @brief 底盘功率限制 - * - * @param c 底盘数据 - * @param cap 电容数据 - * @param ref 裁判系统数据 - * @return 函数运行结果 - */ -int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap, - const Referee_ForChassis_t *ref); - -/** - * \brief 复制底盘输出值 - * - * \param s 包含底盘数据的结构体 - * \param out CAN设备底盘输出结构体 - */ -void Chassis_DumpOutput(Chassis_t *c, CAN_ChassisOutput_t *out); - -/** - * \brief 清空Chassis输出数据 - * - * \param out CAN设备底盘输出结构体 - */ -void Chassis_ResetOutput(CAN_ChassisOutput_t *out); - -/** - * @brief 导出底盘数据 - * - * @param chassis 底盘数据结构体 - * @param ui UI数据结构体 - */ -void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui); - -#ifdef __cplusplus -} -#endif diff --git a/User/module/config.c b/User/module/config.c index 4040620..16ba46e 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -69,22 +69,22 @@ Config_RobotParam_t robot_config = { .chassis_param = { .leglength={ - .k = 5.0f, - .p = 15.0f, + .k = 20.0f, + .p = 1.0f, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, - .out_limit = 10.0f, + .out_limit = 100.0f, .d_cutoff_freq = -1.0f, .range = -1.0f, }, .yaw={ - .k=0.01f, - .p=0.1f, + .k=1.0f, + .p=1.0f, .i=0.0f, .d=0.0f, .i_limit=0.0f, - .out_limit=0.2f, + .out_limit=1.0f, .d_cutoff_freq=30.0f, .range=M_2PI, /* 2*PI，用于处理角度循环误差 */ }, @@ -98,6 +98,17 @@ Config_RobotParam_t robot_config = { .d_cutoff_freq=30.0f, /* 微分截止频率 */ .range=-1.0f, /* 不使用循环误差处理 */ }, + + .tp_pid={ + .k=1.0f, + .p=1.0f, /* 俯仰角比例系数 */ + .i=0.0f, /* 俯仰角积分系数 */ + .d=0.0f, /* 俯仰角微分系数 */ + .i_limit=0.0f, /* 积分限幅 */ + .out_limit=1.0f, /* 输出限幅，腿长差值限制 */ + .d_cutoff_freq=30.0f, /* 微分截止频率 */ + .range=-1.0f, /* 不使用循环误差处理 */ + }, .low_pass_cutoff_freq = { @@ -171,18 +182,58 @@ Config_RobotParam_t robot_config = { } }, .lqr_gains ={ - .k11_coeff = { -61.932040681074966f, 70.963671642086396f, -37.730841182566571f, -0.296475458388679f }, // theta - .k12_coeff = { -0.586710094600108f, 0.886736272521581f, -3.626144273475104f, 0.057861910518974f }, // d_theta - .k13_coeff = { -17.297031110481019f, 16.286794934166178f, -5.176451154477850f, -0.867260018374823f }, // x - .k14_coeff = { -14.893387150006664f, 14.357020815277332f, -5.244645181873441f, -0.869862096507486f}, // d_x - .k15_coeff = { -75.327876471378886f, 79.786699741548944f, -31.183500053811208f, 5.450635661115236f}, // phi - .k16_coeff = { -3.572234723237025f, 4.164905011076312f, -1.874828497771750f, 0.477913222933688f}, // d_phi - .k21_coeff = { 9.327090608559319f, 1.362675928111105f, -8.092777598567881f, 4.351387652359104f}, // theta - .k22_coeff = { 3.872517778351810f, -3.344077414726880f, 0.589693555828904f, 0.310036629174646f}, // d_theta - .k23_coeff = { -71.615766251649134f, 74.748309711530837f, -28.370490124006626f, 4.483586941100197f }, // x - .k24_coeff = { -68.751866288568962f, 71.204785013044102f, -26.812636604518396f, 4.238479323700952f }, // d_x - .k25_coeff = { 205.6887659080132f, -195.1219729060621f, 62.890188701113303f, 7.452313695653162f }, // phi - .k26_coeff = { 16.162999842656344f, -15.926932704437410f, 5.474613395300429f, -0.002856083635449f }, // d_phi + // .k11_coeff = { -61.932040681074966f, 70.963671642086396f, -37.730841182566571f, -0.296475458388679f }, // theta + // .k12_coeff = { -0.586710094600108f, 0.886736272521581f, -3.626144273475104f, 0.057861910518974f }, // d_theta + // .k13_coeff = { -17.297031110481019f, 16.286794934166178f, -5.176451154477850f, -0.867260018374823f }, // x + // .k14_coeff = { -14.893387150006664f, 14.357020815277332f, -5.244645181873441f, -0.869862096507486f}, // d_x + // .k15_coeff = { -75.327876471378886f, 79.786699741548944f, -31.183500053811208f, 5.450635661115236f}, // phi + // .k16_coeff = { -3.572234723237025f, 4.164905011076312f, -1.874828497771750f, 0.477913222933688f}, // d_phi + // .k21_coeff = { 9.327090608559319f, 1.362675928111105f, -8.092777598567881f, 4.351387652359104f}, // theta + // .k22_coeff = { 3.872517778351810f, -3.344077414726880f, 0.589693555828904f, 0.310036629174646f}, // d_theta + // .k23_coeff = { -71.615766251649134f, 74.748309711530837f, -28.370490124006626f, 4.483586941100197f }, // x + // .k24_coeff = { -68.751866288568962f, 71.204785013044102f, -26.812636604518396f, 4.238479323700952f }, // d_x + // .k25_coeff = { 205.6887659080132f, -195.1219729060621f, 62.890188701113303f, 7.452313695653162f }, // phi + // .k26_coeff = { 16.162999842656344f, -15.926932704437410f, 5.474613395300429f, -0.002856083635449f }, // d_phi + + + // .k11_coeff = {-143.1550,156.1754,-98.5282,-11.3781}, // theta + // .k12_coeff = {8.3196,-12.4161,-8.3805,-1.6368}, // d_theta + // .k13_coeff = {-69.6189,68.5619,-23.3079,-4.1736}, // x + // .k14_coeff = {-58.4944,58.2204,-22.8021,-5.2361}, // d_x + // .k15_coeff = {-29.6753,40.9947,-20.1188,2.5142}, // phi + // .k16_coeff = {-13.1787,15.8361,-7.4061,1.1193}, // d_phi + // .k21_coeff = {-76.5141,124.3258,-73.4908,22.0942}, // theta + // .k22_coeff = {-9.1845,12.7284,-5.8169,2.8659}, // d_theta + // .k23_coeff = {-157.6244,179.9415,-77.2161,14.9075}, // x + // .k24_coeff = {-180.5666,202.7656,-85.2869,16.4847}, // d_x + // .k25_coeff = {14.3596,-14.2125,4.1210,24.1729}, // phi + // .k26_coeff = {20.5751,-19.8014,6.3128,2.8044}, // d_phi + .k11_coeff = { -2.046396270532116e+02f, 2.283572275397276e+02f, -99.051642997946473f, 2.549835715107331f }, // theta + .k12_coeff = { 0.689999868157742f, 2.004516582917084f, -5.904779142191341f, 0.331840642644740f }, // d_theta + .k13_coeff = { -59.058694050901643f, 59.363082825119605f, -20.603451414220757f, 1.137708603718630f }, // x + .k14_coeff = { -64.283929532305166f, 65.138737687498519f, -23.323482861600581f, 1.257945614978053f }, // d_x + .k15_coeff = { -15.125353856795936f, 34.329224759247211f, -22.500683150450474f, 5.036897782323629f }, // phi + .k16_coeff = { 2.707768649521343f, 0.390393176362524f, -2.018231845635338f, 0.697604163191230f }, // d_phi + .k21_coeff = { 4.135329288854244e+02f, -3.173913574866715e+02f, 50.321199991176265f, 10.217217753280829f }, // theta + .k22_coeff = { 48.042446261620519f, -45.292268634116219f, 13.273044296221686f, 0.006982339078710f }, // d_theta + .k23_coeff = { 14.015246608117772f, 10.813301135732024f, -18.216050987625373f, 6.078912501009629f }, // x + .k24_coeff = { 21.698411755946285f, 5.621435092936538f, -18.016608013978576f, 6.611542756343175f }, // d_x + .k25_coeff = { 4.798120071828828e+02f, -4.728222224549831e+02f, 1.591181202824602e+02f, -6.421997865768036f }, // phi + .k26_coeff = { 59.794709871063546f, -60.418062715734166f, 20.991263455753383f, -1.388418937916963f }, // d_phi + + // .k11_coeff = { -1.996305368054721e+02f, 2.260001266392926e+02f, -1.009632659573521e+02f, 2.651403223110949e+00f }, // theta + // .k12_coeff = { 1.961704292162323e+00f, 8.251913348469651e-01f, -6.073749575127879e+00f, 3.535645826465822e-01f }, // d_theta + // .k13_coeff = { -8.161081261310467e+01f, 8.274264960693915e+01f, -2.904800049859091e+01f, 1.653742354439720e+00f }, // x + // .k14_coeff = { -7.455421501651551e+01f, 7.622235638964226e+01f, -2.773307975245958e+01f, 1.535810571728176e+00f }, // d_x + // .k15_coeff = { -9.027722377713712e+00f, 2.891936105315331e+01f, -2.106785573613789e+01f, 4.948383450314285e+00f }, // phi + // .k16_coeff = { 3.049839709048039e+00f, 4.627952499276505e-02f, -1.922806522134511e+00f, 6.902507101472589e-01f }, // d_phi + // .k21_coeff = { 4.373445515700652e+02f, -3.391497363529634e+02f, 5.569210421652366e+01f, 1.081229141610274e+01f }, // theta + // .k22_coeff = { 5.284103137531328e+01f, -5.028796043573002e+01f, 1.502532265509508e+01f, -3.077550945526411e-02f }, // d_theta + // .k23_coeff = { 3.111787495894651e+01f, 4.872956811853720e+00f, -2.278008499711769e+01f, 8.426190283284837e+00f }, // x + // .k24_coeff = { 3.329235562185018e+01f, -5.071568882184936e-01f, -1.928141689016582e+01f, 7.776043600153916e+00f }, // d_x + // .k25_coeff = { 4.604866240014122e+02f, -4.577537299814802e+02f, 1.557742271762380e+02f, -6.272551369660446e+00f }, // phi + // .k26_coeff = { 5.608646259671771e+01f, -5.728239809509379e+01f, 2.016452460533993e+01f, -1.320817827839268e+00f }, // d_phi + }, .lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm .lqr_param.max_wheel_torque = 2.5f, // 轮毂电机最大力矩 2.5Nm diff --git a/User/整合方案说明.md b/User/整合方案说明.md deleted file mode 100644 index 372fc66..0000000 --- a/User/整合方案说明.md +++ /dev/null @@ -1,244 +0,0 @@ -# 轮腿机器人控制系统整合方案 - -## 📋 现有系统分析 - -### 🏗️ 您的系统架构（优势） -1. **成熟的硬件接口**：已实现HT+BM电机CAN通信 -2. **完整的运动学**：四连杆正/逆运动学 + 雅可比矩阵 -3. **丰富的控制模式**：平衡、运动、跳跃、悬浮检测 -4. **详细的参数数据**：基于实际测量的腿部参数表 -5. **MATLAB设计工具**：完整的LQR参数生成流程 - -### ⚠️ 可以改进的地方 -1. **控制逻辑分散**：LQR、VMC、MPC混合使用，逻辑复杂 -2. **参数管理困难**：大量硬编码参数分散在各处 -3. **缺乏统一接口**：不同控制算法接口不一致 - -## 🚀 整合方案 - -### 方案一：渐进式整合（推荐） - -#### 第一阶段：保持现有系统，添加新接口 -```c -// 在Chassis_Task.c中添加 -#include "integrated_balance_control.h" - -void Chassis_Task(void const * argument) -{ - vTaskDelay(CHASSIS_TASK_INIT_TIME); - Chassis_Init(&chassis_control); - vTaskDelay(100); - - while(1) - { - Chassis_Data_Update(&chassis_control); - Chassis_Status_Detect(&chassis_control); - Chassis_Mode_Set(&chassis_control); - Chassis_Mode_Change_Control_Transit(&chassis_control); - Target_Value_Set(&chassis_control); - - // 新增：整合控制器调用 - integrated_chassis_control_update(); - - // 如果使用新控制器，跳过原有的力矩计算 - if (!get_current_controller_type()) { - Chassis_Torque_Calculation(&chassis_control); - Chassis_Torque_Combine(&chassis_control); - } - - Motor_CMD_Send(&chassis_control); - vTaskDelay(1); - } -} -``` - -#### 第二阶段：参数统一管理 -创建统一的参数配置文件： - -```c -// robot_config.h -#ifndef ROBOT_CONFIG_H -#define ROBOT_CONFIG_H - -// 机器人物理参数（与MATLAB一致） -#define ROBOT_R_W 0.09f // 驱动轮半径 (m) -#define ROBOT_R_L 0.25f // 轮距/2 (m) -#define ROBOT_L_C 0.037f // 机体质心到关节距离 (m) -#define ROBOT_M_W 0.8f // 驱动轮质量 (kg) -#define ROBOT_M_L 1.6183599f // 腿部质量 (kg) -#define ROBOT_M_B 11.542f // 机体质量 (kg) - -// 腿部几何参数（与您的定义一致） -#define LEG_L1 0.15f // 大腿长度 (m) -#define LEG_L2 0.25f // 小腿长度 (m) -#define LEG_L3 0.25f // -#define LEG_L4 0.15f // -#define LEG_L5 0.1f // 髋关节间距/2 (m) - -// 控制参数 -#define CONTROL_FREQUENCY 500.0f // 控制频率 (Hz) -#define MAX_WHEEL_TORQUE 50.0f // 最大轮子力矩 (N*m) -#define MAX_JOINT_TORQUE 18.0f // 最大关节力矩 (N*m) - -// 安全参数 -#define MAX_TILT_ANGLE 0.25f // 最大倾斜角 (rad) -#define LEG_LENGTH_MIN 0.11f // 最小腿长 (m) -#define LEG_LENGTH_MAX 0.30f // 最大腿长 (m) - -#endif -``` - -#### 第三阶段：控制器选择接口 -添加动态切换功能： - -```c -// 在遥控器中添加切换功能 -void handle_controller_switch(const Gimbal_ctrl_t* rc_ctrl) -{ - static uint8_t last_switch_state = 0; - uint8_t current_switch_state = 0; // 从遥控器读取开关状态 - - if (current_switch_state != last_switch_state) { - if (current_switch_state == 1) { - // 切换到新控制器 - set_controller_type(1); - } else { - // 切换到原有控制器 - set_controller_type(0); - } - last_switch_state = current_switch_state; - } -} -``` - -### 方案二：完全替换（激进） - -直接用新的balance_control替换现有的控制逻辑： - -#### 修改Chassis_Task主循环 -```c -void Chassis_Task(void const * argument) -{ - vTaskDelay(CHASSIS_TASK_INIT_TIME); - - // 初始化新的平衡控制器 - balance_controller_t balance_ctrl; - balance_control_init(&balance_ctrl, TASK_RUN_TIME); - - vTaskDelay(100); - - while(1) - { - // 数据更新（保留） - Chassis_Data_Update(&chassis_control); - - // 转换数据格式 - imu_data_t imu_data; - motor_feedback_t motor_fb; - control_command_t cmd; - - convert_chassis_to_balance_imu(&chassis_control, &imu_data); - convert_chassis_to_balance_motor_fb(&chassis_control, &motor_fb); - convert_chassis_to_balance_cmd(&chassis_control, &cmd); - - // 使用新控制器 - balance_control_update(&balance_ctrl, &imu_data, &motor_fb, &cmd); - - // 获取控制输出 - motor_control_t motor_ctrl; - get_motor_control_output(&balance_ctrl, &motor_ctrl); - - // 转换回原有格式并发送 - convert_balance_to_chassis_output(&motor_ctrl, &chassis_control); - Motor_CMD_Send(&chassis_control); - - vTaskDelay(1); - } -} -``` - -## 🔧 具体修改步骤 - -### 1. 保留您的优势代码 -- **保留**：`Forward_kinematic_solution()` - 运动学计算 -- **保留**：`Supportive_Force_Cal()` - 支撑力计算 -- **保留**：`CAN_HT_CMD()` / `CAN_BM_CONTROL_CMD()` - 电机通信 -- **保留**：MATLAB LQR设计流程 - -### 2. 整合控制架构 -- **替换**：分散的LQR/VMC控制逻辑 → 统一的balance_control -- **简化**：复杂的状态机 → 清晰的模式管理 -- **统一**：参数管理 → robot_config.h - -### 3. 数据接口适配 -```c -// 现有的HT电机数据 -typedef struct { - const HT_motor_measure_t *motor_measure; - fp32 position; - fp32 velocity; - fp32 torque_out, torque_get; -} joint_motor_t; - -// 转换为新系统格式 -void convert_ht_motor_to_balance_fb(const joint_motor_t* ht_motor, - float* angle, float* velocity, float* torque) -{ - *angle = ht_motor->position; - *velocity = ht_motor->velocity; - *torque = ht_motor->torque_get; -} -``` - -### 4. 参数迁移 -将您MATLAB中的参数直接用于新系统： - -```c -// 使用您的LQR拟合系数 -const float K_fit_coefficients[40][6] = { - // 复制您MATLAB生成的系数 - // ... -}; - -// 使用您的机器人参数 -#define R_W_ACTUAL 0.09f // 与您的R_w_ac一致 -#define R_L_ACTUAL 0.25f // 与您的R_l_ac一致 -// ... -``` - -## 🎯 推荐的实施步骤 - -### 第1周：准备工作 -1. 备份现有代码 -2. 创建新的git分支 -3. 编译并测试现有系统确保正常 - -### 第2周：集成准备 -1. 添加integrated_balance_control文件 -2. 修改Chassis_Task添加切换接口 -3. 保持原有控制器为默认 - -### 第3周：参数对齐 -1. 创建robot_config.h统一参数 -2. 运行MATLAB脚本生成LQR系数 -3. 对比调试确保参数一致 - -### 第4周：功能测试 -1. 在安全环境下测试新控制器 -2. 对比两套控制器的性能 -3. 调试和优化 - -### 第5周：全面切换 -1. 将新控制器设为默认 -2. 移除冗余的旧代码 -3. 文档整理和代码规范化 - -## 🛡️ 风险控制 - -1. **渐进式切换**：保留原有系统作为备份 -2. **参数验证**：MATLAB仿真验证参数正确性 -3. **安全测试**：在安全环境下测试新控制器 -4. **性能对比**：记录并对比控制效果 -5. **回滚机制**：确保可以快速回到原有系统 - -这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？ diff --git a/d435.py b/d435.py deleted file mode 100644 index e69de29..0000000 diff --git a/some_functions.c b/some_functions.c deleted file mode 100644 index 8af2891..0000000 --- a/some_functions.c +++ /dev/null @@ -1,171 +0,0 @@ -// Front -// | 1 4 | -// (1)Left| |Right(2) -// | 2 3 | - -#define HT_SLAVE_ID1 0x01 -#define HT_SLAVE_ID2 0x02 -#define HT_SLAVE_ID3 0x03 -#define HT_SLAVE_ID4 0x04 - -#define CAN_BM_M1_ID 0x97 -#define CAN_BM_M2_ID 0x98 -#define BM_CAN hcan2 - -#define get_HT_motor_measure(ptr, data) \ -{ \ - (ptr)->last_ecd = (ptr)->ecd; \ - (ptr)->ecd = uint_to_float((uint16_t)((data)[1] << 8 | (data)[2] ),P_MIN, P_MAX, 16); \ - (ptr)->speed_rpm = uint_to_float((uint16_t)(data[3]<<4)|(data[4]>>4), V_MIN, V_MAX, 12);\ - (ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \ -} - - - - - - -#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x))) -#define P_MIN -95.5f// Radians -#define P_MAX 95.5f -#define V_MIN -45.0f// Rad/s -#define V_MAX 45.0f -#define KP_MIN 0.0f// N-m/rad -#define KP_MAX 500.0f -#define KD_MIN 0.0f// N-m/rad/s -#define KD_MAX 5.0f -#define T_MIN -18.0f -#define T_MAX 18.0f - - -void Forward_kinematic_solution(chassis_control_t *feedback_update, - fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce) -{ - fp32 dL0=0,L0=0,Q0=0,S0=0; - fp32 xb,xd,yb,yd,Lbd,xc,yc; - fp32 A0,B0,C0,Q2,Q3,S2,S3; - fp32 vxb,vxd,vyb,vyd,vxc,vyc; - fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4; - fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3; - fp32 axb,ayb,axd,ayd,a2,axc; - /******************************/ - Q1 = PI + Q1; - cos_Q1 = arm_cos_f32(Q1); - sin_Q1 = arm_sin_f32(Q1); - cos_Q4 = arm_cos_f32(Q4); - sin_Q4 = arm_sin_f32(Q4); - xb = -L5/2.0f + L1*cos_Q1; - xd = L5/2.0f + L4*cos_Q4; - yb = L1*sin_Q1; - yd = L4*sin_Q4; - - Lbd=(xd-xb)*(xd-xb)+(yd-yb)*(yd-yb); - A0 = 2.0f*L2*(xd-xb); - B0 = 2.0f*L2*(yd-yb); - C0 = L2*L2+Lbd-L3*L3; - Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0)); - - xc = xb + arm_cos_f32(Q2)*L2; - yc = yb + arm_sin_f32(Q2)*L2; - - L0=Sqrt( xc*xc + yc*yc ); - Q0 = atan(yc/xc); - - vxb = -S1*L1*sin_Q1; - vyb = S1*L1*cos_Q1; - vxd = -S4*L4*sin_Q4; - vyd = S4*L4*cos_Q4; - Q3 = atan_tl((yc-yd)/(xc-xd)); - S2 = ((vxd-vxb)*arm_cos_f32(Q3) + (vyd-vyb)*arm_sin_f32(Q3))/(L2*arm_sin_f32(Q3-Q2)); - S3 = ((vxd-vxb)*arm_cos_f32(Q2) + (vyd-vyb)*arm_sin_f32(Q2))/(L2*arm_sin_f32(Q3-Q2)); - vxc = vxb - S2*L2*arm_sin_f32(Q2); - vyc = vyb + S2*L2*arm_cos_f32(Q2); - S0 = 3*(-arm_sin_f32(ABS(Q0))*vxc-arm_cos_f32(Q0)*vyc); - - if( Q0 < 0 ) - Q0 += PI; - /*******************************/ - if (ce) - { - feedback_update->chassis_posture_info .leg_length_L = L0; - feedback_update->chassis_posture_info .leg_angle_L = Q0; - // feedback_update->chassis_posture_info .leg_gyro_L = S0; - } - else - { - feedback_update->chassis_posture_info .leg_length_R = L0; - feedback_update->chassis_posture_info .leg_angle_R = Q0; - // feedback_update->chassis_posture_info .leg_gyro_R = S0; - } -} - -void Supportive_Force_Cal( chassis_control_t *Suspend_Detect, fp32 Q1, fp32 Q4, fp32 ce ) -{ - fp32 dL0=0,L0=0,Q0=0,S0=0; - fp32 xb,xd,yb,yd,Lbd,xc,yc; - fp32 A0,B0,C0,Q2,Q3,S2,S3; - fp32 vxb,vxd,vyb,vyd,vxc,vyc; - fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4; - fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3; - fp32 axb,ayb,axd,ayd,a2,axc; - /******************************/ - Q1 = PI + Q1; - cos_Q1 = arm_cos_f32(Q1); - sin_Q1 = arm_sin_f32(Q1); - cos_Q4 = arm_cos_f32(Q4); - sin_Q4 = arm_sin_f32(Q4); - xb = -L5/2.0f + L1*cos_Q1; - xd = L5/2.0f + L4*cos_Q4; - yb = L1*sin_Q1; - yd = L4*sin_Q4; - - Lbd=sqrt( (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb) ); - A0 = 2.0f*L2*(xd-xb); - B0 = 2.0f*L2*(yd-yb); - C0 = L2*L2+Lbd*Lbd-L3*L3; - Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0)); - xc = xb + arm_cos_f32(Q2)*L2; - yc = yb + arm_sin_f32(Q2)*L2; - - L0 = Sqrt( xc*xc + yc*yc ); - Q0 = atan_tl(yc/xc); - if( Q0 < 0 ) - Q0 += PI; - - Q3 = atan_tl((yc-yd)/(xc-xd))+PI; - - - - fp32 J1 = ( L1 * arm_sin_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 )); - fp32 J2 = ( L1 * arm_cos_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0); - fp32 J3 = ( L4 * arm_sin_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 )); - fp32 J4 = ( L4 * arm_cos_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0); - - - fp32 dett = J1 * J4 - J2 * J3; - fp32 Inv_J1 = J4 / dett; - fp32 Inv_J2 = -J2 / dett; - fp32 Inv_J3 = -J3 / dett; - fp32 Inv_J4 = J1 / dett; - - ddebug = yc; - - if( ce == 1.0f ) - { - Suspend_Detect->chassis_posture_info.supportive_force_L = - Inv_J1 * Suspend_Detect->joint_motor_1.torque_get + - Inv_J2 * Suspend_Detect->joint_motor_2.torque_get; - // if( Suspend_Detect->chassis_posture_info.supportive_force_L < 0.0f ) - // Suspend_Detect->chassis_posture_info.supportive_force_L += 40.0f; - } - else - { - Suspend_Detect->chassis_posture_info.supportive_force_R = - Inv_J1 * Suspend_Detect->joint_motor_4.torque_get + - Inv_J2 * Suspend_Detect->joint_motor_3.torque_get; - Suspend_Detect->chassis_posture_info.supportive_force_R *= -1.0f; - // if( Suspend_Detect->chassis_posture_info.supportive_force_R < 0.0f ) - // Suspend_Detect->chassis_posture_info.supportive_force_R += 40.0f; - } - -} \ No newline at end of file diff --git a/test_vmc.c b/test_vmc.c deleted file mode 100644 index 0e70c86..0000000 --- a/test_vmc.c +++ /dev/null @@ -1,66 +0,0 @@ -#include -#include -#include "User/component/vmc.h" - -int main() { - // 测试VMC计算 - VMC_t vmc; - VMC_Param_t param = { - .hip_length = 0.1f, // 10cm髋关节间距 - .leg_1 = 0.15f, // 15cm大腿前端 - .leg_2 = 0.15f, // 15cm大腿后端 - .leg_3 = 0.15f, // 15cm小腿 - .leg_4 = 0.15f, // 15cm小腿前端 - .wheel_radius = 0.05f, // 5cm轮子半径 - .wheel_mass = 1.0f // 1kg轮子质量 - }; - - if (VMC_Init(&vmc, ¶m, 1000.0f) != 0) { - printf("VMC初始化失败\n"); - return -1; - } - - printf("测试VMC腿长计算\n"); - printf("髋关节间距: %.3f\n", param.hip_length); - printf("腿段长度: L1=%.3f, L2=%.3f, L3=%.3f, L4=%.3f\n", - param.leg_1, param.leg_2, param.leg_3, param.leg_4); - printf("\n"); - - // 测试不同的关节角度配置 - float test_angles[][2] = { - {0.0f, 0.0f}, // 水平伸展 - {0.5f, -0.5f}, // 轻微弯曲 - {1.0f, -1.0f}, // 中等弯曲 - {1.5f, -1.5f}, // 较大弯曲 - {-0.5f, 0.5f}, // 反向弯曲 - }; - - int num_tests = sizeof(test_angles) / sizeof(test_angles[0]); - - for (int i = 0; i < num_tests; i++) { - float phi1 = test_angles[i][0]; - float phi4 = test_angles[i][1]; - float body_pitch = 0.0f; - float body_pitch_rate = 0.0f; - - if (VMC_ForwardSolve(&vmc, phi1, phi4, body_pitch, body_pitch_rate) == 0) { - float length, angle, d_length, d_angle; - VMC_GetVirtualLegState(&vmc, &length, &angle, &d_length, &d_angle); - - float foot_x, foot_y; - VMC_GetFootPosition(&vmc, &foot_x, &foot_y); - - printf("测试 %d: phi1=%.2f, phi4=%.2f\n", i+1, phi1, phi4); - printf(" 足端位置: (%.3f, %.3f)\n", foot_x, foot_y); - printf(" 虚拟腿长: %.3f\n", length); - printf(" 虚拟摆角: %.3f (%.1f度)\n", angle, angle * 180.0f / M_PI); - printf(" 内部变量: XC=%.3f, YC=%.3f, phi2=%.3f, phi3=%.3f\n", - vmc.leg.XC, vmc.leg.YC, vmc.leg.phi2, vmc.leg.phi3); - printf("\n"); - } else { - printf("测试 %d: 求解失败 (phi1=%.2f, phi4=%.2f)\n", i+1, phi1, phi4); - } - } - - return 0; -} diff --git a/utils/Simulation-master/balance/series_legs/get_k.m b/utils/Simulation-master/balance/series_legs/get_k.m index 46e1806..62754ff 100644 --- a/utils/Simulation-master/balance/series_legs/get_k.m +++ b/utils/Simulation-master/balance/series_legs/get_k.m @@ -37,46 +37,60 @@ a26=polyfit(leg,k26,3); toc -% x0=leg; %步长为0.1 -% y11=polyval(a11,x0); %返回值y0是对应于x0的函数值 -% y12=polyval(a12,x0); %返回值y0是对应于x0的函数值 -% y13=polyval(a13,x0); %返回值y0是对应于x0的函数值 -% y14=polyval(a14,x0); %返回值y0是对应于x0的函数值 -% y15=polyval(a15,x0); %返回值y0是对应于x0的函数值 -% y16=polyval(a16,x0); %返回值y0是对应于x0的函数值 -% -% y21=polyval(a21,x0); %返回值y0是对应于x0的函数值 -% y22=polyval(a22,x0); %返回值y0是对应于x0的函数值 -% y23=polyval(a23,x0); %返回值y0是对应于x0的函数值 -% y24=polyval(a24,x0); %返回值y0是对应于x0的函数值 -% y25=polyval(a25,x0); %返回值y0是对应于x0的函数值 -% y26=polyval(a26,x0); %返回值y0是对应于x0的函数值 -% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11'); -% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12'); -% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13'); -% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14'); -% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15'); -% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16'); -% -% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21'); -% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22'); -% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23'); -% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24'); -% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25'); -% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26'); -% grid on; %添加网格线 -% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1); %将网格线变成虚线 -% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4)); -% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4)); -% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4)); -% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4)); -% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4)); -% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4)); -% -% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4)); -% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4)); -% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4)); -% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4)); -% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4)); -% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4)); +% 打印格式化的C代码结构，可以直接复制到config.c中 +fprintf('\n=========== 可直接复制的C代码 ===========\n'); +fprintf('.lqr_gains = {\n'); +fprintf(' .k11_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a11(1), a11(2), a11(3), a11(4)); +fprintf(' .k12_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_theta\n', a12(1), a12(2), a12(3), a12(4)); +fprintf(' .k13_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // x\n', a13(1), a13(2), a13(3), a13(4)); +fprintf(' .k14_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_x\n', a14(1), a14(2), a14(3), a14(4)); +fprintf(' .k15_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a15(1), a15(2), a15(3), a15(4)); +fprintf(' .k16_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_phi\n', a16(1), a16(2), a16(3), a16(4)); +fprintf(' .k21_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a21(1), a21(2), a21(3), a21(4)); +fprintf(' .k22_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_theta\n', a22(1), a22(2), a22(3), a22(4)); +fprintf(' .k23_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // x\n', a23(1), a23(2), a23(3), a23(4)); +fprintf(' .k24_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_x\n', a24(1), a24(2), a24(3), a24(4)); +fprintf(' .k25_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a25(1), a25(2), a25(3), a25(4)); +fprintf(' .k26_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_phi\n', a26(1), a26(2), a26(3), a26(4)); +fprintf('},\n'); +fprintf('========================================\n\n'); + +% 可选：显示拟合曲线图 +x0=leg; %步长为0.1 +y11=polyval(a11,x0); %返回值y0是对应于x0的函数值 +y12=polyval(a12,x0); %返回值y0是对应于x0的函数值 +y13=polyval(a13,x0); %返回值y0是对应于x0的函数值 +y14=polyval(a14,x0); %返回值y0是对应于x0的函数值 +y15=polyval(a15,x0); %返回值y0是对应于x0的函数值 +y16=polyval(a16,x0); %返回值y0是对应于x0的函数值 + +y21=polyval(a21,x0); %返回值y0是对应于x0的函数值 +y22=polyval(a22,x0); %返回值y0是对应于x0的函数值 +y23=polyval(a23,x0); %返回值y0是对应于x0的函数值 +y24=polyval(a24,x0); %返回值y0是对应于x0的函数值 +y25=polyval(a25,x0); %返回值y0是对应于x0的函数值 +y26=polyval(a26,x0); %返回值y0是对应于x0的函数值 + +figure; +subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('leg length');ylabel('k11');title('k11拟合'); +subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('leg length');ylabel('k12');title('k12拟合'); +subplot(3,4,3);plot(leg,k13,'o',x0,y13,'r');xlabel('leg length');ylabel('k13');title('k13拟合'); +subplot(3,4,4);plot(leg,k14,'o',x0,y14,'r');xlabel('leg length');ylabel('k14');title('k14拟合'); +subplot(3,4,5);plot(leg,k15,'o',x0,y15,'r');xlabel('leg length');ylabel('k15');title('k15拟合'); +subplot(3,4,6);plot(leg,k16,'o',x0,y16,'r');xlabel('leg length');ylabel('k16');title('k16拟合'); + +subplot(3,4,7);plot(leg,k21,'o',x0,y21,'r');xlabel('leg length');ylabel('k21');title('k21拟合'); +subplot(3,4,8);plot(leg,k22,'o',x0,y22,'r');xlabel('leg length');ylabel('k22');title('k22拟合'); +subplot(3,4,9);plot(leg,k23,'o',x0,y23,'r');xlabel('leg length');ylabel('k23');title('k23拟合'); +subplot(3,4,10);plot(leg,k24,'o',x0,y24,'r');xlabel('leg length');ylabel('k24');title('k24拟合'); +subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('leg length');ylabel('k25');title('k25拟合'); +subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('leg length');ylabel('k26');title('k26拟合'); + +for i = 1:12 + subplot(3,4,i); + grid on; %添加网格线 + set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',0.5); %将网格线变成虚线 + legend('原始数据','拟合曲线','Location','best'); +end + toc diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index f5cc9e7..bce2ebb 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -17,10 +17,10 @@ function K = get_k_length(leg_length) R1=0.072; %驱动轮半径 L1=leg_length/2; %摆杆重心到驱动轮轴距离 LM1=leg_length/2; %摆杆重心到其转轴距离 - l1=0.03; %机体质心距离转轴距离 + l1=-0.03; %机体质心距离转轴距离 mw1=0.60; %驱动轮质量 mp1=1.8; %杆质量 - M1=1.8; %机体质量 + M1=6.0; %机体质量 Iw1=mw1*R1^2; %驱动轮转动惯量 Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量 IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量 @@ -48,7 +48,7 @@ function K = get_k_length(leg_length) B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]); B=double(B); - Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + Q=diag([500 1 1000 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 R=[240 0;0 25]; %T Tp K=lqr(A,B,Q,R); diff --git a/utils/lqr.asv b/utils/lqr.asv deleted file mode 100644 index 811ff95..0000000 --- a/utils/lqr.asv +++ /dev/null @@ -1,321 +0,0 @@ -% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07） -% v2：添加了可以专门调试定腿长的功能（2024/05/08） -% v3：优化部分注释，添加单位说明（2024/05/15） -% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16） - -% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义 - -%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%% -tic -clear all -clc - -% 定义机器人机体参数 -syms R_w % 驱动轮半径 -syms R_l % 驱动轮轮距/2 -syms l_l l_r % 左右腿长 -syms l_wl l_wr % 驱动轮质心到左右腿部质心距离 -syms l_bl l_br % 机体质心到左右腿部质心距离 -syms l_c % 机体质心到腿部关节中心点距离 -syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量 -syms I_w % 驱动轮转动惯量 (自然坐标系法向) -syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化) -syms I_b % 机体转动惯量 (自然坐标系法向) -syms I_z % 机器人z轴转动惯量 (简化为常量) - -% 定义其他独立变量并补充其导数 -syms theta_wl theta_wr % 左右驱动轮转角 -syms dtheta_wl dtheta_wr -syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b - -% 定义状态向量 -syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b - -% 定义控制向量 -syms T_wl T_wr T_bl T_br - -% 输入物理参数：重力加速度 -syms g - - - -%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%% - -% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式 -eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0; -eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0; -eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0; -eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0; -eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0; -[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b); - - -% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数 -J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]); -J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]); - -% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入 -A = sym('A',[10 10]); -B = sym('B',[10 4]); - -% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109 -for p = 5:2:9 - A_index = (p - 3)/2; - A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2; - A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l); - for q = 6:2:10 - A(q,p) = J_A(q/2,A_index); - end -end - -% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0 -for r = 1:10 - if rem(r,2) == 0 - A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0; - else - A(r,:) = zeros(1,10); - A(r,r+1) = 1; - end -end - -% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104, -for h = 1:4 - B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2; - B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l); - for f = 6:2:10 - B(f,h) = J_B(f/2,h); - end -end - -% B的其余数值为0 -for e = 1:2:9 - B(e,:) = zeros(1,4); -end - - - -%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%% - -% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据 -g_ac = 9.81; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 此处可以输入机器人机体基本参数 % -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -R_w_ac = 0.77; % 驱动轮半径 （单位：m） -R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m） -l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m） -m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg） -I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2） -I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2） -I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2） - -%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%% - -% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉 -% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 - -l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m） -l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m） -l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m） -I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2） -l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m） -l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m） -l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m） -I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：kg m^2） - -% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右 -% 两侧两个关节电机之间的中间点相连所形成的轴 -% （如果目的是小板凳，考虑使左右腿相关数据一致） - -%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll -% 通过以下方式记录数据: 矩阵分4列， -% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m -% 第二列为l_wl，单位：m -% 第三列为l_bl，单位：m -% 第四列为I_ll，单位：kg m^2 -% （注意单位别搞错！） -% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整 - -Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599; - 0.12, 0.0470, 0.0730, 0.01862845; - 0.13, 0.0476, 0.0824, 0.01898641; - 0.14, 0.0480, 0.0920, 0.01931342; - 0.15, 0.0490, 0.1010, 0.01962521; - 0.16, 0.0500, 0.1100, 0.01993092; - 0.17, 0.0510, 0.1190, 0.02023626; - 0.18, 0.0525, 0.1275, 0.02054500; - 0.19, 0.0539, 0.1361, 0.02085969; - 0.20, 0.0554, 0.1446, 0.02118212; - 0.21, 0.0570, 0.1530, 0.02151357; - 0.22, 0.0586, 0.1614, 0.02185496; - 0.23, 0.0600, 0.1700, 0.02220695; - 0.24, 0.0621, 0.1779, 0.02256999; - 0.25, 0.0639, 0.1861, 0.02294442; - 0.26, 0.0657, 0.1943, 0.02333041; - 0.27, 0.0676, 0.2024, 0.02372806; - 0.28, 0.0700, 0.2100, 0.02413735; - 0.29, 0.0713, 0.2187, 0.02455817; - 0.30, 0.0733, 0.2267, 0.02499030]; -% 以上数据应通过实际测量或sw图纸获得 - -% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集 -% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr） -Leg_data_r = Leg_data_l; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 此处可以输入QR矩阵 % -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 矩阵Q中，以下列分别对应： -% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b -lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0; - 0, 2, 0, 0, 0, 0, 0, 0, 0, 0; - 0, 0, 12000, 0, 0, 0, 0, 0, 0, 0; - 0, 0, 0, 200, 0, 0, 0, 0, 0, 0; - 0, 0, 0, 0, 1000, 0, 0, 0, 0, 0; - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; - 0, 0, 0, 0, 0, 0, 1000, 0, 0, 0; - 0, 0, 0, 0, 0, 0, 0, 1, 0, 0; - 0, 0, 0, 0, 0, 0, 0, 0, 20000, 0; - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]; -% 其中： -% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数 -% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数 -% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数 -% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数 -% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数 - -% 矩阵中，以下列分别对应： -% T_wl T_wr T_bl T_br -lqr_R = [0.25, 0, 0, 0; - 0, 0.25, 0, 0; - 0, 0, 1.5, 0; - 0, 0, 0, 1.5]; -% 其中： -% T_wl: 左侧驱动轮输出力矩 -% T_wr：右侧驱动轮输出力矩 -% T_bl：左侧髋关节输出力矩 -% T_br：右腿髋关节输出力矩 -% 单位皆为Nm - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - -%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%% - -% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉， -% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 -K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R) -K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.') - - -%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%% - -sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数 - -length = size(Leg_data_l,1); % 测量腿部数据集的行数 - -% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列 -% 是l_r，第三列是对应的K_ij的数值 -K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。 - -for i = 1:length - for j = 1:length - index = (i - 1)*length + j; - l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据 - l_wl_ac = Leg_data_l(i,2); - l_bl_ac = Leg_data_l(i,3); - I_ll_ac = Leg_data_l(i,4); - l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据 - l_wr_ac = Leg_data_r(j,2); - l_br_ac = Leg_data_r(j,3); - I_lr_ac = Leg_data_r(j,4); - for k = 1:40 - K_sample(index,1,k) = l_l_ac; - K_sample(index,2,k) = l_r_ac; - end - K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R); - % 根据指定的l_l,l_r输入对应的K_ij的数值 - for l = 1:4 - for m = 1:10 - K_sample(index,3,(l - 1)*10 + m) = K(l,m); - end - end - end -end - -% 创建收集全部K_ij的多项式拟合的全部系数的集合 -K_Fit_Coefficients = zeros(40,6); -for n = 1:40 - K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22'); - K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果 -end -Polynomial_expression = formula(K_Surface_Fit) - -% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数 -% 每一行和K_ij的对应关系如下： -% - 第1行对应K_1,1 -% - 第14行对应K_2,4 -% - 第22行对应K_3,2 -% - 第37行对应K_4,7 -% ... 其他行对应关系类似 -% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2 -% 其中x对应左腿腿长l_l，y对应右腿腿长l_r -% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数 -% 每一列和系数pij的对应关系如下： -% - 第1列对应p00 -% - 第2列对应p10 -% - 第3列对应p01 -% - 第4列对应p20 -% - 第5列对应p11 -% - 第6列对应p02 -K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.') - -% 正确食用方法： -% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为： -% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2 -% 2.并填入对应系数即可 - -toc - -%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释 -% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新 -% （前面的蛆，以后再来探索吧（bushi - - - -%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%% - -function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R) - % 基于机体以及物理参数，获得控制矩阵A，B的全部数值 - A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... - [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... - m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); - B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... - [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... - m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); - - % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K - % P为Riccati方程的解，矩阵L可以无视 - [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]); -end - diff --git a/utils/lqr.m b/utils/lqr.m deleted file mode 100644 index 811ff95..0000000 --- a/utils/lqr.m +++ /dev/null @@ -1,321 +0,0 @@ -% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07） -% v2：添加了可以专门调试定腿长的功能（2024/05/08） -% v3：优化部分注释，添加单位说明（2024/05/15） -% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16） - -% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义 - -%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%% -tic -clear all -clc - -% 定义机器人机体参数 -syms R_w % 驱动轮半径 -syms R_l % 驱动轮轮距/2 -syms l_l l_r % 左右腿长 -syms l_wl l_wr % 驱动轮质心到左右腿部质心距离 -syms l_bl l_br % 机体质心到左右腿部质心距离 -syms l_c % 机体质心到腿部关节中心点距离 -syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量 -syms I_w % 驱动轮转动惯量 (自然坐标系法向) -syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化) -syms I_b % 机体转动惯量 (自然坐标系法向) -syms I_z % 机器人z轴转动惯量 (简化为常量) - -% 定义其他独立变量并补充其导数 -syms theta_wl theta_wr % 左右驱动轮转角 -syms dtheta_wl dtheta_wr -syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b - -% 定义状态向量 -syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b - -% 定义控制向量 -syms T_wl T_wr T_bl T_br - -% 输入物理参数：重力加速度 -syms g - - - -%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%% - -% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式 -eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0; -eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0; -eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0; -eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0; -eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0; -[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b); - - -% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数 -J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]); -J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]); - -% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入 -A = sym('A',[10 10]); -B = sym('B',[10 4]); - -% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109 -for p = 5:2:9 - A_index = (p - 3)/2; - A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2; - A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l); - for q = 6:2:10 - A(q,p) = J_A(q/2,A_index); - end -end - -% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0 -for r = 1:10 - if rem(r,2) == 0 - A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0; - else - A(r,:) = zeros(1,10); - A(r,r+1) = 1; - end -end - -% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104, -for h = 1:4 - B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2; - B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l); - for f = 6:2:10 - B(f,h) = J_B(f/2,h); - end -end - -% B的其余数值为0 -for e = 1:2:9 - B(e,:) = zeros(1,4); -end - - - -%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%% - -% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据 -g_ac = 9.81; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 此处可以输入机器人机体基本参数 % -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -R_w_ac = 0.77; % 驱动轮半径 （单位：m） -R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m） -l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m） -m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg） -I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2） -I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2） -I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2） - -%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%% - -% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉 -% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 - -l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m） -l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m） -l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m） -I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2） -l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m） -l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m） -l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m） -I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：kg m^2） - -% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右 -% 两侧两个关节电机之间的中间点相连所形成的轴 -% （如果目的是小板凳，考虑使左右腿相关数据一致） - -%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll -% 通过以下方式记录数据: 矩阵分4列， -% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m -% 第二列为l_wl，单位：m -% 第三列为l_bl，单位：m -% 第四列为I_ll，单位：kg m^2 -% （注意单位别搞错！） -% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整 - -Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599; - 0.12, 0.0470, 0.0730, 0.01862845; - 0.13, 0.0476, 0.0824, 0.01898641; - 0.14, 0.0480, 0.0920, 0.01931342; - 0.15, 0.0490, 0.1010, 0.01962521; - 0.16, 0.0500, 0.1100, 0.01993092; - 0.17, 0.0510, 0.1190, 0.02023626; - 0.18, 0.0525, 0.1275, 0.02054500; - 0.19, 0.0539, 0.1361, 0.02085969; - 0.20, 0.0554, 0.1446, 0.02118212; - 0.21, 0.0570, 0.1530, 0.02151357; - 0.22, 0.0586, 0.1614, 0.02185496; - 0.23, 0.0600, 0.1700, 0.02220695; - 0.24, 0.0621, 0.1779, 0.02256999; - 0.25, 0.0639, 0.1861, 0.02294442; - 0.26, 0.0657, 0.1943, 0.02333041; - 0.27, 0.0676, 0.2024, 0.02372806; - 0.28, 0.0700, 0.2100, 0.02413735; - 0.29, 0.0713, 0.2187, 0.02455817; - 0.30, 0.0733, 0.2267, 0.02499030]; -% 以上数据应通过实际测量或sw图纸获得 - -% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集 -% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr） -Leg_data_r = Leg_data_l; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 此处可以输入QR矩阵 % -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 矩阵Q中，以下列分别对应： -% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b -lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0; - 0, 2, 0, 0, 0, 0, 0, 0, 0, 0; - 0, 0, 12000, 0, 0, 0, 0, 0, 0, 0; - 0, 0, 0, 200, 0, 0, 0, 0, 0, 0; - 0, 0, 0, 0, 1000, 0, 0, 0, 0, 0; - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; - 0, 0, 0, 0, 0, 0, 1000, 0, 0, 0; - 0, 0, 0, 0, 0, 0, 0, 1, 0, 0; - 0, 0, 0, 0, 0, 0, 0, 0, 20000, 0; - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]; -% 其中： -% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数 -% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数 -% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数 -% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数 -% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数 - -% 矩阵中，以下列分别对应： -% T_wl T_wr T_bl T_br -lqr_R = [0.25, 0, 0, 0; - 0, 0.25, 0, 0; - 0, 0, 1.5, 0; - 0, 0, 0, 1.5]; -% 其中： -% T_wl: 左侧驱动轮输出力矩 -% T_wr：右侧驱动轮输出力矩 -% T_bl：左侧髋关节输出力矩 -% T_br：右腿髋关节输出力矩 -% 单位皆为Nm - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - -%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%% - -% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉， -% 或者点击左侧数字"224"让程序只运行行之前的内容并停止 -K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R) -K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.') - - -%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%% - -sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数 - -length = size(Leg_data_l,1); % 测量腿部数据集的行数 - -% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列 -% 是l_r，第三列是对应的K_ij的数值 -K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。 - -for i = 1:length - for j = 1:length - index = (i - 1)*length + j; - l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据 - l_wl_ac = Leg_data_l(i,2); - l_bl_ac = Leg_data_l(i,3); - I_ll_ac = Leg_data_l(i,4); - l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据 - l_wr_ac = Leg_data_r(j,2); - l_br_ac = Leg_data_r(j,3); - I_lr_ac = Leg_data_r(j,4); - for k = 1:40 - K_sample(index,1,k) = l_l_ac; - K_sample(index,2,k) = l_r_ac; - end - K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R); - % 根据指定的l_l,l_r输入对应的K_ij的数值 - for l = 1:4 - for m = 1:10 - K_sample(index,3,(l - 1)*10 + m) = K(l,m); - end - end - end -end - -% 创建收集全部K_ij的多项式拟合的全部系数的集合 -K_Fit_Coefficients = zeros(40,6); -for n = 1:40 - K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22'); - K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果 -end -Polynomial_expression = formula(K_Surface_Fit) - -% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数 -% 每一行和K_ij的对应关系如下： -% - 第1行对应K_1,1 -% - 第14行对应K_2,4 -% - 第22行对应K_3,2 -% - 第37行对应K_4,7 -% ... 其他行对应关系类似 -% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2 -% 其中x对应左腿腿长l_l，y对应右腿腿长l_r -% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数 -% 每一列和系数pij的对应关系如下： -% - 第1列对应p00 -% - 第2列对应p10 -% - 第3列对应p01 -% - 第4列对应p20 -% - 第5列对应p11 -% - 第6列对应p02 -K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.') - -% 正确食用方法： -% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为： -% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2 -% 2.并填入对应系数即可 - -toc - -%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%% - -% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释 -% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新 -% （前面的蛆，以后再来探索吧（bushi - - - -%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%% - -function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ... - R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ... - l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ... - A,B,lqr_Q,lqr_R) - % 基于机体以及物理参数，获得控制矩阵A，B的全部数值 - A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... - [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... - m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); - B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ... - [R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ... - m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]); - - % 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K - % P为Riccati方程的解，矩阵L可以无视 - [P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]); -end - diff --git a/utils/vmc.m b/utils/vmc.m deleted file mode 100644 index 507f912..0000000 --- a/utils/vmc.m +++ /dev/null @@ -1,173 +0,0 @@ -%% 五连杆机构腿长计算脚本 -clear; clc; - -%% 定义输入参数 -% 连杆长度 (m) -l1 = 0.215; % 连杆1长度 -l2 = 0.258; % 连杆2长度 -l3 = 0.258; % 连杆3长度 -l4 = 0.215; % 连杆4长度 -l5 = 0.0; % 连杆5长度 - -% 关节角度 (弧度) -phi1 = 0.10; % 30度 -phi4 = 2.96; % 45度 - -%% 调用腿长计算函数 -fprintf('五连杆机构腿长计算\n'); -fprintf('输入参数:\n'); -fprintf(' 连杆长度: l1=%.3f, l2=%.3f, l3=%.3f, l4=%.3f, l5=%.3f (m)\n', l1, l2, l3, l4, l5); -fprintf(' 关节角度: phi1=%.3f (%.1f°), phi4=%.3f (%.1f°)\n', phi1, rad2deg(phi1), phi4, rad2deg(phi4)); - -try - % 计算各关键点位置 - % B点 (连杆1末端) - x_B = l1 * cos(phi1); - y_B = l1 * sin(phi1); - - % D点 (连杆4末端) - x_D = l5 + l4 * cos(phi4); - y_D = l4 * sin(phi4); - - % 计算BD距离 - BD_dist = sqrt((x_D - x_B)^2 + (y_D - y_B)^2); - - % 检查三角形约束条件 - if BD_dist > (l2 + l3) || BD_dist < abs(l2 - l3) - warning('无法形成闭合五连杆机构！BD距离=%.3f, 需要满足: %.3f < BD < %.3f', ... - BD_dist, abs(l2-l3), l2+l3); - leg_length = NaN; - foot_pos = [NaN, NaN]; - return; - end - - % 使用余弦定理求解phi2 - cos_phi2_BD = (l2^2 + BD_dist^2 - l3^2) / (2 * l2 * BD_dist); - - % 检查余弦值是否在有效范围内 - if abs(cos_phi2_BD) > 1 - warning('余弦值超出范围: %.3f', cos_phi2_BD); - leg_length = NaN; - foot_pos = [NaN, NaN]; - return; - end - - % 计算BD方向角 - alpha_BD = atan2(y_D - y_B, x_D - x_B); - - % 计算phi2 (两个解，选择合理的一个) - angle_offset = acos(cos_phi2_BD); - phi2_solution1 = alpha_BD + angle_offset; - phi2_solution2 = alpha_BD - angle_offset; - - % 选择合理的解 (通常选择使机构在合理配置的解) - phi2 = phi2_solution1; % 可以根据实际需要调整选择逻辑 - - % 计算C点位置 (足端位置) - x_C = x_B + l2 * cos(phi2); - y_C = y_B + l2 * sin(phi2); - - % 验证C点到D点距离是否等于l3 - CD_dist = sqrt((x_D - x_C)^2 + (y_D - y_C)^2); - error_CD = abs(CD_dist - l3); - - if error_CD > 1e-6 - fprintf('警告: CD距离误差较大 (%.6f)\n', error_CD); - end - - % 计算腿长 (假设为原点到C点的距离) - leg_length = sqrt(x_C^2 + y_C^2); - foot_pos = [x_C, y_C]; - - % 输出结果 - fprintf('\n计算结果:\n'); - fprintf(' B点位置: (%.3f, %.3f)\n', x_B, y_B); - fprintf(' C点位置: (%.3f, %.3f)\n', x_C, y_C); - fprintf(' D点位置: (%.3f, %.3f)\n', x_D, y_D); - fprintf(' phi2角度: %.3f rad (%.1f°)\n', phi2, rad2deg(phi2)); - fprintf(' 腿长: %.3f m\n', leg_length); - fprintf(' BD距离: %.3f m\n', BD_dist); - fprintf(' CD验证: %.6f m (应该等于l3=%.3f)\n', CD_dist, l3); - - %% 可视化结果 - figure; - plot([0, x_B], [0, y_B], 'b-', 'LineWidth', 2); % 连杆1 - hold on; - plot([x_B, x_C], [y_B, y_C], 'r-', 'LineWidth', 2); % 连杆2 - plot([x_C, x_D], [y_C, y_D], 'g-', 'LineWidth', 2); % 连杆3 - plot([l5, x_D], [0, y_D], 'm-', 'LineWidth', 2); % 连杆4 - plot([0, l5], [0, 0], 'k-', 'LineWidth', 3); % 连杆5 (基座) - - % 标记关键点 - plot(0, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % A点 - plot(x_B, y_B, 'bo', 'MarkerSize', 8, 'MarkerFaceColor', 'b'); % B点 - plot(x_C, y_C, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % C点 - plot(x_D, y_D, 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % D点 - plot(l5, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % E点 - - % 标记腿长 - plot([0, x_C], [0, y_C], 'k--', 'LineWidth', 1); % 腿长线 - - % 图形设置 - grid on; - axis equal; - xlabel('X (m)'); - ylabel('Y (m)'); - title('五连杆机构示意图'); - legend('连杆1', '连杆2', '连杆3', '连杆4', '基座', 'Location', 'best'); - - % 添加文本标注 - text(0, 0-0.02, 'A', 'HorizontalAlignment', 'center'); - text(x_B, y_B+0.02, 'B', 'HorizontalAlignment', 'center'); - text(x_C, y_C+0.02, 'C(足端)', 'HorizontalAlignment', 'center'); - text(x_D, y_D+0.02, 'D', 'HorizontalAlignment', 'center'); - text(l5, 0-0.02, 'E', 'HorizontalAlignment', 'center'); - text(x_C/2, y_C/2, sprintf('腿长=%.3fm', leg_length), 'HorizontalAlignment', 'center'); - -catch ME - fprintf('计算过程中出现错误: %s\n', ME.message); - leg_length = NaN; - foot_pos = [NaN, NaN]; -end - -%% 多组参数测试 -fprintf('\n\n=== 多组参数测试 ===\n'); -test_cases = [ - pi/6, pi/4; % 30°, 45° - pi/4, pi/3; % 45°, 60° - -pi/6, pi/2; % -30°, 90° - 0, pi/6; % 0°, 30° -]; - -for i = 1:size(test_cases, 1) - phi1_test = test_cases(i, 1); - phi4_test = test_cases(i, 2); - - fprintf('\n测试 %d: phi1=%.1f°, phi4=%.1f°\n', i, rad2deg(phi1_test), rad2deg(phi4_test)); - - % 重复计算过程（简化版） - x_B_test = l1 * cos(phi1_test); - y_B_test = l1 * sin(phi1_test); - x_D_test = l5 + l4 * cos(phi4_test); - y_D_test = l4 * sin(phi4_test); - BD_dist_test = sqrt((x_D_test - x_B_test)^2 + (y_D_test - y_B_test)^2); - - if BD_dist_test <= (l2 + l3) && BD_dist_test >= abs(l2 - l3) - cos_phi2_BD_test = (l2^2 + BD_dist_test^2 - l3^2) / (2 * l2 * BD_dist_test); - if abs(cos_phi2_BD_test) <= 1 - alpha_BD_test = atan2(y_D_test - y_B_test, x_D_test - x_B_test); - angle_offset_test = acos(cos_phi2_BD_test); - phi2_test = alpha_BD_test + angle_offset_test; - x_C_test = x_B_test + l2 * cos(phi2_test); - y_C_test = y_B_test + l2 * sin(phi2_test); - leg_length_test = sqrt(x_C_test^2 + y_C_test^2); - fprintf(' 结果: 腿长=%.3f m, 足端位置=(%.3f, %.3f)\n', leg_length_test, x_C_test, y_C_test); - else - fprintf(' 结果: 余弦值超出范围\n'); - end - else - fprintf(' 结果: 无法形成闭合机构\n'); - end -end - -fprintf('\n计算完成！\n'); \ No newline at end of file