TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

修改与轮电机固连杆，使用机体速度计算LQR增益

Browse Source
This commit is contained in:
kai
2024-04-28 16:14:10 +08:00
parent 41e13aee44
commit b7e4c80cb4
8 changed files with 58 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
application/chassis/balance.c
View File

@@ -58,7 +58,7 @@ void BalanceInit()
.can_handle = &hcan1},
.controller_param_init_config = {
.angle_PID = {
.Kp = 0.3,
.Kp = 0.1,
.Kd = 0,
.Ki = 0,
.DeadBand = 0.0001,
@@ -102,9 +102,9 @@ void BalanceInit()
.motor_type = LK9025,
};
driven_conf.can_init_config.tx_id = 1;
driven[RD] = r_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[LD] = l_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[RD] = r_driven = LKMotorInit(&driven_conf);
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
@@ -219,21 +219,21 @@ static void ResetChassis()
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
LKMotorSetRef(l_driven, chassis_cmd_recv.vx * 2);
LKMotorSetRef(r_driven, -chassis_cmd_recv.vx * 2);
// 若关节完成复位,进入ready态
if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.025 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.025 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.025 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.025)
if (abs(lf->measure.total_angle) < 0.05 && abs(lf->measure.total_angle) > 0.03 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.03 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.03 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.03)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
else if (abs(lf->measure.total_angle) <= 0.025 &&
abs(lb->measure.total_angle) <= 0.025 &&
abs(rf->measure.total_angle) <= 0.025 &&
abs(rb->measure.total_angle) <= 0.025)
else if (abs(lf->measure.total_angle) <= 0.03 &&
abs(lb->measure.total_angle) <= 0.03 &&
abs(rf->measure.total_angle) <= 0.03 &&
abs(rb->measure.total_angle) <= 0.03)
{ // 双阈值保证关节能够复位而不会进入死区
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
@@ -279,6 +279,9 @@ static void WokingStateSet()
// 运动模式
EnableAllMotor();
// 保证关节电机为开环扭矩控制
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP);
// 设置目标速度/腿长/距离
l_side.target_len += chassis_cmd_recv.delta_leglen;
@@ -297,6 +300,13 @@ static void WokingStateSet()
// 角度输入
chassis.target_yaw = chassis_cmd_recv.offset_angle;
// TODO 转向速度限幅
// TODO 最大dist误差限幅
// TODO 最大速度误差限幅
}
@@ -344,11 +354,12 @@ static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output;
// 抗劈叉
static float swerving_speed_ff, ff_coef = 0;
swerving_speed_ff = ff_coef * steer_v_pid.Output; // 用于抗劈叉的前馈
PIDCalculate(&anti_crash_pid, l_side.phi5 - r_side.phi5, 0);
l_side.T_hip += anti_crash_pid.Output + swerving_speed_ff;
r_side.T_hip -= anti_crash_pid.Output + swerving_speed_ff;
l_side.T_hip += anti_crash_pid.Output - swerving_speed_ff;
r_side.T_hip -= anti_crash_pid.Output - swerving_speed_ff;
}
@@ -358,8 +369,8 @@ static void LegControl() /* 腿长控制和Roll补偿 */
l_side.target_len += roll_compensate_pid.Output;
r_side.target_len -= roll_compensate_pid.Output;
static float gravity_comp = 57.63;
static float roll_extra_comp_p = 400;
static float gravity_comp = 60;
static float roll_extra_comp_p = 300;
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;
@@ -385,13 +396,6 @@ void BalanceTask()
WokingStateSet();
// 参数组装
ParamAssemble();
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
// 将五连杆映射成单杆
Link2Leg(&l_side, &chassis);
Link2Leg(&r_side, &chassis);
@@ -407,6 +411,13 @@ void BalanceTask()
// VMC映射成关节输出
VMCProject(&l_side);
VMCProject(&r_side);
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
// 运动模态,电机输出映射和限幅
WattLimitSet();
}

6
application/chassis/balance.h
View File

@@ -32,13 +32,13 @@
#define RB 3u
#define DRIVEN_CNT 2u
#define RD 0u
#define LD 1u
#define LD 0u
#define RD 1u
typedef struct
{
// joint
float phi1_w, phi4_w, phi2_w, phi5_w; // phi2_w used for calc real wheel speed
float phi1_w, phi4_w, phi2_w, phi3_w, phi5_w; // phi2_w or phi3_w used for calc real wheel speed
float T_back, T_front;
// link angle, phi1-ph5, phi5 is pod angle

4
application/chassis/linkNleg.h
View File

@@ -68,10 +68,12 @@ void Link2Leg(LinkNPodParam *p, ChassisParam *chassis)
float phi2_pred = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
xC = xB + CALF_LEN * mcos(phi2_pred);
yC = yB + CALF_LEN * msin(phi2_pred);
float phi3_pred = atan2f(yC - yD, xC - xD);
float phi5_pred = atan2f(yC, xC - JOINT_DISTANCE / 2);
// 差分计算腿长变化率和腿角速度
p->phi2_w = (phi2_pred - p->phi2) / predict_dt; // 稍后用于修正轮速
p->phi2_w = (phi2_pred - p->phi2) / predict_dt;
p->phi3_w = (phi3_pred - p->phi3) / predict_dt; // 稍后用于修正轮速
p->phi5_w = (phi5_pred - p->phi5) / predict_dt;
p->legd = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->leg_len) / predict_dt;
p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w * predict_dt) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w

24
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,18 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
float k[12][3] = {155.616146,-200.327241,-0.278791,
-12.564474,-24.018026,0.674594,
145.503085,-87.742553,-5.916048,
97.588867,-70.361944,-4.007305,
303.894243,-240.356086,68.074439,
18.743007,-15.738160,4.718314,
-170.799127,75.225969,18.998160,
-29.312006,20.892957,1.529644,
122.553208,-114.023608,38.147691,
63.561616,-64.916409,25.861226,
-657.233777,402.487049,66.281303,
-40.107302,25.117848,1.705173};
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;

4
application/chassis/speed_estimation.h
View File

@@ -17,8 +17,8 @@
void SpeedEstimation(LinkNPodParam *lp, LinkNPodParam *rp, ChassisParam *cp, INS_t *imu, float delta_t)
{
// 修正轮速和距离
lp->wheel_w = lp->w_ecd + lp->phi2_w - cp->pitch_w; // 减去和定子固连的phi2_w
rp->wheel_w = rp->w_ecd + rp->phi2_w - cp->pitch_w;
lp->wheel_w = lp->w_ecd + lp->phi3_w - cp->pitch_w; // 减去和定子固连的phi2_w
rp->wheel_w = rp->w_ecd + rp->phi3_w - cp->pitch_w;
// 以轮子为基点,计算机体两侧髋关节处的速度
lp->body_v = lp->wheel_w * WHEEL_RADIUS + lp->leg_len * lp->theta_w + lp->legd * msin(lp->theta);

3
application/robot_def.h
View File

@@ -141,14 +141,13 @@ typedef struct
{
// 控制部分
float vx; // 前进方向速度
float rotate_w; // 旋转速度, 目前仅在复位模式下使用
float rotate_w; // 旋转速度, 目前仅在复位模式下使用
float delta_leglen; // 腿长
float offset_angle; // 底盘和归中位置的夹角
chassis_mode_e chassis_mode;
chassis_direction_e direction;
// UI部分
lid_mode_e lid_mode;
friction_mode_e friction_mode;
Target_State_e target_state;
loader_mode_e loader_mode;