diff --git a/application/chassis/balance.c b/application/chassis/balance.c index add0669..d8fbb2e 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -113,7 +113,7 @@ void BalanceInit() // 腿长控制 PID_Init_Config_s leg_length_pid_conf = { .Kp = 1200, - .Kd = 150, + .Kd = 200, .Ki = 0, .MaxOut = 60, .DeadBand = 0.0001f, @@ -173,6 +173,7 @@ void BalanceInit() // 状态初始化 l_side.target_len = r_side.target_len = 0.12; + l_side.gravity_ff = r_side.gravity_ff = 60.0f; chassis.vel_cov = 100; // 速度协方差初始化 chassis_status = ROBOT_READY; DWT_GetDeltaT(&balance_dwt_cnt); @@ -231,8 +232,6 @@ static void ResetChassis() l_side.target_len = r_side.target_len = 0.12; // 角度输入为当前角度 chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; - // 驱动轮支持力为定值 - l_side.normal_force = r_side.normal_force = 100.0f; // 撞墙时前后移动保证能重新站立,执行速度输入 LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w); @@ -285,8 +284,6 @@ static void WokingStateSet() chassis.dist = chassis.target_dist = 0; // 角度输入为当前角度 chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; - // 驱动轮支持力为定值 - l_side.normal_force = r_side.normal_force = 100.0f; for (uint8_t i = 0; i < JOINT_CNT; i++) HTMotorStop(joint[i]); @@ -383,11 +380,10 @@ static void LegControl() /* 腿长控制和Roll补偿 */ l_side.target_len += roll_compensate_pid.Output; r_side.target_len -= roll_compensate_pid.Output; - static float gravity_comp = 60; static float roll_extra_comp_p = 400; float roll_comp = roll_extra_comp_p * chassis.roll; - l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp; - r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp; + l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + l_side.gravity_ff - roll_comp; + r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + r_side.gravity_ff + roll_comp; } static void WattLimitSet() /* 设定运动模态的输出 */ @@ -425,18 +421,15 @@ void BalanceTask() // VMC映射成关节输出 VMCProject(&l_side); VMCProject(&r_side); + // 驱动轮支持力解算 + NormalForceSolve(&l_side, Chassis_IMU_data); + NormalForceSolve(&r_side, Chassis_IMU_data); // stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码 if (chassis_status == ROBOT_STOP || chassis_cmd_recv.chassis_mode == CHASSIS_RESET || chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) return; // 复位模态或急停,直接退出 - else - { - // 正常模式下再进行驱动轮支持力解算 - NormalForceSolve(&l_side, Chassis_IMU_data, del_t); - NormalForceSolve(&r_side, Chassis_IMU_data, del_t); - } // 运动模态,电机输出映射和限幅 WattLimitSet(); diff --git a/application/chassis/balance.h b/application/chassis/balance.h index a52719a..da64ab5 100644 --- a/application/chassis/balance.h +++ b/application/chassis/balance.h @@ -1,5 +1,7 @@ #pragma once +#include "stdint.h" + // 底盘参数 #define CALF_LEN 0.24f // 小腿 #define THIGH_LEN 0.14f // 大腿 @@ -55,6 +57,8 @@ typedef struct float T_wheel; float zw_ddot; // 驱动轮竖直方向加速度 float normal_force; // 支持力 + float gravity_ff; // 重力前馈 + uint8_t fly_flag; // 离地标志位 // pod float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一 diff --git a/application/chassis/fly_detection.h b/application/chassis/fly_detection.h index 6cc3e1d..2cf96c0 100644 --- a/application/chassis/fly_detection.h +++ b/application/chassis/fly_detection.h @@ -4,7 +4,7 @@ #include "user_lib.h" // 驱动轮支持力解算 -void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt) +void NormalForceSolve(LinkNPodParam *p, INS_t *imu) { static float accx, accy, accz; accx = imu->MotionAccel_b[X]; @@ -15,11 +15,17 @@ void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt) pitch = imu->Pitch; roll = imu->Roll; - // 驱动轮竖直方向加速度 + // 机体竖直方向加速度 p->zw_ddot = -msin(roll) * accx + mcos(roll) * msin(pitch) * accy + mcos(pitch) * mcos(roll) * accz; // 驱动轮支持力解算 static float P; P = p->F_leg * mcos(p->theta) + p->T_hip * msin(p->theta) / p->leg_len; p->normal_force = P + WHEEL_MASS * (p->zw_ddot + 9.81f); + + // 离地检测 + if(p->normal_force < 20.0f) + p->fly_flag = 1; + else + p->fly_flag = 0; } \ No newline at end of file diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h index 323d725..aad3c90 100644 --- a/application/chassis/lqr_calc.h +++ b/application/chassis/lqr_calc.h @@ -9,48 +9,44 @@ */ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis) { - float k[12][3] = {62.680622,-74.772126,-13.135672, - 1.620796,-4.331826,-0.454705, - 32.093563,-25.558681,-16.605856, - 19.128242,-17.562747,-10.815380, - 225.373594,-201.324771,53.236052, - 13.575849,-13.013546,3.845604, - 76.704297,-72.201666,20.359891, - 4.520141,-4.051799,1.189345, - 139.418565,-125.750074,34.110069, - 88.295852,-79.170124,21.430975, - -163.725633,127.852860,110.004619, - -9.557678,7.476790,4.655220}; + static float k[12][3] = {62.680622,-74.772126,-13.135672, + 1.620796,-4.331826,-0.454705, + 32.093563,-25.558681,-16.605856, + 19.128242,-17.562747,-10.815380, + 225.373594,-201.324771,53.236052, + 13.575849,-13.013546,3.845604, + 76.704297,-72.201666,20.359891, + 4.520141,-4.051799,1.189345, + 139.418565,-125.750074,34.110069, + 88.295852,-79.170124,21.430975, + -163.725633,127.852860,110.004619, + -9.557678,7.476790,4.655220}; float T[2] = {0}; // 0 T_wheel 1 T_hip float l = p->leg_len; float lsqr = l * l; - // 离地检测 - if (p->normal_force < 20.0f) - { - for (size_t i = 0; i < 12; i++) - { - // 除 theta 和 theta_dot 的关节输出外，其余增益全部置0 - if(i != 6 && i != 7) - { - for (size_t j = 0; j < 3; j++) - { - k[i][j] = 0; - } - } - } - } - - // 计算增益 for (uint8_t i = 0; i < 2; ++i) { uint8_t j = i * 6; - T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta + - (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + - (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) + - (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) + - (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch + - (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w; + + if(i == 0) // 离地时仅对速度闭环，保证落地时轮速与机体速度一致 + { + T[i] = (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) + (p->fly_flag ? 0 : + ( (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta + + (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + + (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) + + (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch + + (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w )); + } + else if(i == 1) // 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直 + { + T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta + + (k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + (p->fly_flag ? 0 : + ( (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) + + (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) + + (k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch + + (k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w )); + } } p->T_wheel = T[0]; p->T_hip = T[1];