Hshine
/
wheel_legged
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
wheel_legged/application/balance/balance.c

337 lines
13 KiB
C
Raw Blame History

//
// Created by SJQ on 2023/12/30.
//
#include "balance.h"
static void mylqr_k(float L0, float K_mat[12]);
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
{
balanceControl->target_L0 = 0.15f;
Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);
LQR_Init(&balanceControl->LQR_l,InitConfig.LQR_state_num,InitConfig.LQR_control_num);
LQR_Init(&balanceControl->LQR_r,InitConfig.LQR_state_num,InitConfig.LQR_control_num);
PIDInit(&balanceControl->leg_coordination_PID,&InitConfig.leg_cor_PID_config);
PIDInit(&balanceControl->leg_length_PID,&InitConfig.legInitConfig.length_PID_conf);
estimator_init(&balanceControl->v_estimator,0.001f,10.0f);
Observer_Init(&balanceControl->state_observer);
memset(balanceControl->state_r,0,6);
memset(balanceControl->target_state_r,0,6);
memset(balanceControl->state_l,0,6);
memset(balanceControl->target_state_l,0,6);
}
//解算地面支持力F
float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last);
// leg_phi[4] = {phi1_r,phi4_r,phi1_l,phi4_l}
void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,const float x[2],const float x_dot[2],const float MotionAccel[3])
{
LegInstance *leg_r = &balanceControl->leg_right;
LegInstance *leg_l = &balanceControl->leg_left;
Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
float K_matrix_r[12]={0};
float K_matrix_l[12]={0};
//右腿
float alpha_r = PI/2 - leg_r->legState.phi0;
float theta_r = alpha_r - body_phi;
float theta_dot_r = - leg_r->legState.phi0_dot - body_phi_dot;
static uint32_t dot_cnt = 0;
float dot_dt = DWT_GetDeltaT(&dot_cnt);
float theta_dot2_r = (balanceControl->state_r[1] - theta_dot_r) / dot_dt;
//左腿
float alpha_l = PI/2 - leg_l->legState.phi0;
float theta_l = alpha_l - body_phi;
float theta_dot_l = - leg_l->legState.phi0_dot - body_phi_dot;
float theta_dot2_l = (balanceControl->state_l[1] - theta_dot_l) / dot_dt;
//计算机体速度
float xb_r = x[0] + leg_r->legState.L0 * arm_sin_f32(theta_r);
float xb_dot_r = x_dot[0] + (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r));
float xb_l = x[1] + leg_l->legState.L0 * arm_sin_f32(theta_l);
float xb_dot_l = x_dot[1] + (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_l->legState.L0 * theta_dot_l * arm_cos_f32(theta_l));
float xb = (xb_r + xb_l)/2;
float xb_dot = (xb_dot_r + xb_dot_l)/2;
estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[1]);
float x_est_r = balanceControl->v_estimator.Estimate_X_[0] - leg_r->legState.L0 * arm_sin_f32(theta_r);
float x_dot_est_r = balanceControl->v_estimator.Estimate_X_[1] - (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r));
float x_est_l = balanceControl->v_estimator.Estimate_X_[0] - leg_l->legState.L0 * arm_sin_f32(theta_l);
float x_dot_est_l = balanceControl->v_estimator.Estimate_X_[1] - (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_r->legState.L0 * theta_dot_l * arm_cos_f32(theta_l));
balanceControl->v_est[0] = x_est_r;
balanceControl->v_est[1] = x_dot_est_r;
balanceControl->v_est[2] = x_est_l;
balanceControl->v_est[3] = x_dot_est_l;
balanceControl->state_r[0] = theta_r;
balanceControl->state_r[1] = theta_dot_r;//(1.0f - 0.75f) * balanceControl->state_r[1] + 0.75f * theta_dot_r;
balanceControl->state_r[2] = x_est_r;//x[0];//balanceControl->v_estimator.Estimate_X_[0];
balanceControl->state_r[3] = x_dot_est_r;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1];
balanceControl->state_r[4] = body_phi;
balanceControl->state_r[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_r[5] + 0.75f * body_phi_dot;
balanceControl->state_l[0] = theta_l;
balanceControl->state_l[1] = theta_dot_l;//(1.0f - 0.75f) * balanceControl->state_l[1] + 0.75f * theta_dot_l;
balanceControl->state_l[2] = x_est_l;//x[0];//balanceControl->v_estimator.Estimate_X_[0];
balanceControl->state_l[3] = x_dot_est_l;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1];
balanceControl->state_l[4] = body_phi;
balanceControl->state_l[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_l[5] + 0.75f * body_phi_dot;
//地面支持力解算
float theta_state_r[3] = {theta_r,theta_dot_r,theta_dot2_r};
static uint32_t right_cnt = 0,left_cnt = 0;
balanceControl->FN_right = calculate_F(MotionAccel[2],theta_state_r,leg_r,&right_cnt);
float theta_state_l[3] = {theta_l,theta_dot_l,theta_dot2_l};
balanceControl->FN_left = calculate_F(MotionAccel[2],theta_state_l,leg_l,&left_cnt);
mylqr_k(leg_r->legState.L0,K_matrix_r);
mylqr_k(leg_l->legState.L0,K_matrix_l);
//离地情况
//注意电机输出反向力矩也会导致判断离地
if((balanceControl->FN_right <= 10.0f || leg_r->legFeedback.F <= 10.0f) ||
(balanceControl->jump_flag == 1 ||balanceControl->jump_flag == 2) )//&& leg_r->legState.L0>=0.15f
{
balanceControl->fly_flag_r = TRUE;
float lqr_K[12] = {0};
lqr_K[6] = K_matrix_r[6];
lqr_K[7] = K_matrix_r[7];
LQR_set_K(&balanceControl->LQR_r,lqr_K);
}
else
{
balanceControl->fly_flag_r = FALSE;
LQR_set_K(&balanceControl->LQR_r,K_matrix_r);
}
if((balanceControl->FN_left <= 10.0f || leg_l->legFeedback.F <= 10.0f)||
(balanceControl->jump_flag == 1 || balanceControl->jump_flag == 2) )//&& leg_l->legState.L0>=0.15f
{
balanceControl->fly_flag_l = TRUE;
float lqr_K[12] = {0};
lqr_K[6] = K_matrix_l[6];
lqr_K[7] = K_matrix_l[7];
LQR_set_K(&balanceControl->LQR_l,lqr_K);
}
else
{
balanceControl->fly_flag_l = FALSE;
LQR_set_K(&balanceControl->LQR_l,K_matrix_l);
}
}
#define F_FEEDFORWARD 30.0f //腿部支持力前馈
#define L_MAX 0.40f
#define L_MIN 0.13f //腿长最大最小值
void Balance_Control_Loop(Balance_Control_t* balanceControl,uint8_t jump_flag)
{
LQR_update(&balanceControl->LQR_r,balanceControl->state_r,balanceControl->target_state_r);
LQR_update(&balanceControl->LQR_l,balanceControl->state_l,balanceControl->target_state_l);
balanceControl->LQR_r.control_vector[1] = - balanceControl->LQR_r.control_vector[1];
float Tp_r = balanceControl->LQR_r.control_vector[1]; //髋关节力矩
float T_r = balanceControl->LQR_r.control_vector[0]; //驱动轮力矩
balanceControl->LQR_l.control_vector[1] = - balanceControl->LQR_l.control_vector[1];
float Tp_l = balanceControl->LQR_l.control_vector[1]; //髋关节力矩
float T_l = balanceControl->LQR_l.control_vector[0]; //驱动轮力矩
LegInstance *leg_r,*leg_l;
leg_r = &balanceControl->leg_right;
leg_l = &balanceControl->leg_left;
//两条腿独立控制腿长
float F_pid_out_r = PIDCalculate(&leg_r->Length_PID,leg_r->legState.L0,leg_r->target_L0);
float F_pid_out_l = PIDCalculate(&leg_l->Length_PID,leg_l->legState.L0,leg_l->target_L0);
//控制平均腿长
float average_length = (leg_r->legState.L0 + leg_l->legState.L0)/2;
float F_pid_out = PIDCalculate(&balanceControl->leg_length_PID,average_length,balanceControl->target_L0);
//float F_pid_out_l = PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
//跳跃控制
static float F_jump_l,F_jump_r;
float max_scale = 10.0f;
float K = (max_scale - 1) * F_FEEDFORWARD / (L_MIN - L_MAX);
F_jump_l = K*(leg_l->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD;
F_jump_r = K*(leg_r->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD;
//跳跃支持力
F_jump_l = float_constrain(F_jump_l,F_FEEDFORWARD,max_scale * F_FEEDFORWARD);
F_jump_r = float_constrain(F_jump_r,F_FEEDFORWARD,max_scale * F_FEEDFORWARD);
float Tp_pid_out_r = PIDCalculate(&leg_r->Phi0_PID,leg_r->legState.phi0,leg_r->target_phi0);
float Tp_pid_out_l = PIDCalculate(&leg_l->Phi0_PID,leg_l->legState.phi0,leg_l->target_phi0);
float coordination_err = leg_r->legState.phi0 - leg_l->legState.phi0;
float cor_Tp = 0;//PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);
if(jump_flag == 1)//起跳阶段
{
Leg_Input_update(&leg_r->legInput,F_jump_r,Tp_r + cor_Tp);
Leg_Input_update(&leg_l->legInput,F_jump_l,Tp_l - cor_Tp);
}
else if(jump_flag == 2) //收腿阶段
{
Leg_Input_update(&leg_r->legInput,-100,Tp_r + cor_Tp);
Leg_Input_update(&leg_l->legInput,-100,Tp_l - cor_Tp);
}
else //非跳跃阶段
{
Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 40,Tp_r + cor_Tp);
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 40,Tp_l - cor_Tp);
}
// Leg_Input_update(&leg_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp);
// Leg_Input_update(&leg_l->legInput,F_pid_out_l + 20 ,Tp - cor_Tp);
VMC_getT(leg_r);
VMC_getT(leg_l);
}
void target_x_set(Balance_Control_t* balanceControl,float add_x)
{
balanceControl->target_state_r[3] = add_x;
balanceControl->target_state_l[3] = add_x;
if( fabsf(add_x) >= 0.01f)
{
balanceControl->target_state_r[2] = balanceControl->state_r[2];
balanceControl->target_state_l[2] = balanceControl->state_l[2];
}
}
void state_clear(Balance_Control_t* balanceControl)
{
//清除位移状态量
balanceControl->target_state_r[2] = 0;
balanceControl->state_r[2] = 0;
balanceControl->target_state_l[2] = 0;
balanceControl->state_l[2] = 0;
//estimator_init(&balanceControl->v_estimator,0.001f,10.0f);
}
void target_L_set(Balance_Control_t* balanceControl,float add_L)
{
balanceControl->target_L0 += add_L;
balanceControl->target_L0 = float_constrain(balanceControl->target_L0,L_MIN,L_MAX);
}
/*
* MYLQR_K MATLAB生成
* K = MYLQR_K(L0)
*
* Arguments : float L0
* float K[12]
* Return Type : void
*/
void mylqr_k(float L0, float K_mat[12])
{
float t2;
float t3;
float K[12];
/* This function was generated by the Symbolic Math Toolbox version 9.2.
*/
/* 06-May-2024 17:35:50 */
t2 = L0*L0;
t3 = t2*L0;
K[0] =
((L0 * -282.545074F + t2 * 330.464752F) - t3 * 225.209259F) - 21.1259251F;
K[1] =
((L0 * 191.247147F - t2 * 904.015686F) + t3 * 1025.96033F) + 44.8702393F;
K[2] =
((L0 * -38.6851654F + t2 * 7.79979753F) - t3 * 7.8749814F) - 5.59480667F;
K[3] =
((L0 * -10.0553722F - t2 * 60.9720421F) + t3 * 94.1053925F) + 17.016264F;
K[4] =
((L0 * 8.41862774F - t2 * 39.7602615F) + t3 * 49.5394897F) - 9.96936703F;
K[5] =
((L0 * 51.7300377F - t2 * 238.936829F) + t3 * 284.121246F) + 3.30276108F;
K[6] =
((L0 * 58.7921486F - t2 * 213.884598F) + t3 * 239.798187F) - 32.5602646F;
K[7] =
((L0 * 112.231422F - t2 * 583.832947F) + t3 * 718.246582F) + 13.6089172F;
K[8] =
((L0 * -198.74147F + t2 * 196.315338F) - t3 * 42.9540405F) + 89.0673752F;
K[9] = ((L0 * 348.149323F - t2 * 531.783142F) + t3 * 252.826599F) + 70.4726F;
K[10] =
((L0 * -30.6359673F + t2 * 40.8268318F) - t3 * 21.4240017F) + 15.1511059F;
K[11] =
((L0 * 40.7163315F - t2 * 35.2042122F) - t3 * 12.030282F) + 9.66719341F;
//matlab赋值顺序不同
for (int i = 0; i < 6; ++i) {
K_mat[i] = K[2*i];
}
for (int i = 0; i < 6; ++i) {
K_mat[6+i] = K[2*i+1];
}
}
/*
* File trailer for mylqr_k.c
*
* [EOF]
*/
/**
* @brief 解算地面支持力 F
* @param float theta[3] = {theta,theta_dot,theta_dot2}
* @retval none
* @attention
*/
float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last)
{
//求导所用时间
//static uint32_t dot_cnt = 0;
// static float last_theta_dot,last_L_dot;
// float dot_dt = DWT_GetDeltaT(&dot_cnt);
// float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt;
// float L_dot2 = (last_L_dot - L_dot) / dot_dt;
float L = leg->legState.L0;
float L_dot = leg->legState.L0_dot;
float L_dot2 = leg->legState.L0_dot2;
float cos_theta = arm_cos_f32(theta[0]);
float sin_theta = arm_sin_f32(theta[0]);
float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta[1] * sin_theta
+ L * theta[2] * sin_theta + L * theta[1] * theta[1] * cos_theta;
float P = leg->legFeedback.F * cos_theta + leg->legFeedback.Tp * sin_theta/L;
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
return F_N;
}
Reference in New Issue View Git Blame Copy Permalink