215 lines
8.0 KiB
C
215 lines
8.0 KiB
C
//
|
|
// Created by SJQ on 2023/12/30.
|
|
//
|
|
|
|
#include "balance_old.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->balance_LQR,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,100,1);
|
|
Observer_Init(&balanceControl->state_observer);
|
|
memset(balanceControl->state,0,6);
|
|
memset(balanceControl->target_state,0,6);
|
|
}
|
|
|
|
//解算地面支持力F
|
|
float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb);
|
|
|
|
// 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,float x,float x_dot,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[12]={0};
|
|
float L_average = leg_r->legState.L0; //(leg_r->legState.L0 + leg_l->legState.L0)/2;
|
|
//(leg_r->legState.L0 + leg_l->legState.L0)/2;
|
|
float alpha = PI/2 - leg_r->legState.phi0;
|
|
float theta = alpha - body_phi;
|
|
// static uint32_t dot_cnt = 0;
|
|
// float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
|
// float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
|
|
float theta_dot = - leg_r->legState.phi0_dot - body_phi_dot;
|
|
|
|
//计算机体速度
|
|
float xb = x + 2 * leg_r->legState.L0 * arm_sin_f32(theta);
|
|
float xb_dot = x_dot + 2*(leg_r->legState.L0_dot* arm_sin_f32(theta) + leg_r->legState.L0 * theta_dot * arm_cos_f32(theta));
|
|
|
|
estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[0]);
|
|
|
|
balanceControl->state[0] = theta;
|
|
balanceControl->state[1] = (1.0f - 0.75f) * balanceControl->state[1] +
|
|
0.75f * theta_dot;
|
|
//balanceControl->state[1] = theta_dot;
|
|
balanceControl->state[2] = x;
|
|
balanceControl->state[3] = x_dot;
|
|
|
|
balanceControl->state[4] = body_phi;
|
|
balanceControl->state[5] = (1.0f - 0.75f) * balanceControl->state[5] +
|
|
0.75f * body_phi_dot;
|
|
// balanceControl->state[5] = body_phi_dot;
|
|
|
|
//Observer_Estimate(&balanceControl->state_observer,balanceControl->state,balanceControl->balance_LQR.control_vector,L_average);
|
|
|
|
//地面支持力解算
|
|
//balanceControl->FN_right = calculate_F(MotionAccel[2],theta,theta_dot,leg_r->legState.L0,leg_r->legState.L0_dot,leg_r->legFeedback.F,leg_r->legFeedback.Tp);
|
|
//balanceControl->FN_left = calculate_F(MotionAccel[2],theta,theta_dot,leg_l->legState.L0,leg_l->legState.L0_dot,leg_l->legFeedback.F,leg_l->legFeedback.Tp);
|
|
|
|
mylqr_k(L_average,K_matrix);
|
|
LQR_set_K(&balanceControl->balance_LQR,K_matrix);
|
|
}
|
|
|
|
|
|
|
|
void Balance_Control_Loop(Balance_Control_t* balanceControl)
|
|
{
|
|
LQR_update(&balanceControl->balance_LQR,balanceControl->state,balanceControl->target_state);
|
|
|
|
balanceControl->balance_LQR.control_vector[1] = - balanceControl->balance_LQR.control_vector[1];
|
|
float Tp = balanceControl->balance_LQR.control_vector[1]; //髋关节力矩
|
|
float T = balanceControl->balance_LQR.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);
|
|
|
|
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 = PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);
|
|
|
|
Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 30,Tp + cor_Tp);
|
|
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 30,Tp - 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 [2] += add_x;
|
|
}
|
|
|
|
void state_clear(Balance_Control_t* balanceControl)
|
|
{
|
|
//清除位移状态量
|
|
balanceControl->target_state [2] = 0;
|
|
balanceControl->state[2] = 0;
|
|
}
|
|
|
|
void target_L_set(Balance_Control_t* balanceControl,float add_L)
|
|
{
|
|
balanceControl->target_L0 += add_L;
|
|
float_constrain(balanceControl->target_L0,0.15f,0.30f);
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
* 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.
|
|
*/
|
|
t2 = L0 * L0;
|
|
t3 = t2 * L0;
|
|
/* 11-Mar-2024 19:12:17 */
|
|
K[0] =
|
|
((L0 * -173.845428F + t2 * 178.098602F) - t3 * 104.773026F) - 8.81736755F;
|
|
K[1] =
|
|
((L0 * 150.104736F - t2 * 511.085876F) + t3 * 523.35144F) + 10.6000547F;
|
|
K[2] =
|
|
((L0 * -16.623167F - t2 * 12.9591064F) + t3 * 11.4631195F) - 2.62765169F;
|
|
K[3] =
|
|
((L0 * -1.31775725F - t2 * 24.5691776F) + t3 * 32.5003357F) + 5.97731924F;
|
|
K[4] =
|
|
((L0 * 4.03619576F - t2 * 33.4860535F) + t3 * 47.0306854F) - 11.8519392F;
|
|
K[5] =
|
|
((L0 * 23.7967739F - t2 * 163.993057F) + t3 * 207.196823F) + 8.34590435F;
|
|
K[6] =
|
|
((L0 * 29.8367863F - t2 * 116.608421F) + t3 * 131.16246F) - 16.6295223F;
|
|
K[7] =
|
|
((L0 * -16.2756023F - t2 * 56.7522507F) + t3 * 104.046684F) + 14.3186169F;
|
|
K[8] =
|
|
((L0 * -104.948708F + t2 * 87.532341F) - t3 * 7.19816732F) + 51.9084816F;
|
|
K[9] =
|
|
((L0 * 172.631012F - t2 * 274.727661F) + t3 * 150.935028F) + 42.625248F;
|
|
K[10] =
|
|
((L0 * -11.3688688F + t2 * 19.2530308F) - t3 * 15.6853495F) + 6.75911F;
|
|
K[11] =
|
|
((L0 * 10.4498978F - t2 * 4.97098875F) - t3 * 7.18331099F) + 2.7609446F;
|
|
//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]
|
|
*/
|
|
|
|
//解算地面支持力F
|
|
float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb)
|
|
{
|
|
//求导所用时间
|
|
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 cos_theta = arm_cos_f32(theta);
|
|
float sin_theta = arm_sin_f32(theta);
|
|
float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta_dot * sin_theta
|
|
+ L * theta_dot2 * sin_theta + L * theta_dot * theta_dot * cos_theta;
|
|
|
|
float P = F_fb*cos_theta + Tp_fb*sin_theta/L;
|
|
|
|
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
|
|
|
return F_N;
|
|
|
|
}
|