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-01-27 19:38:00 +08:00
parent 72b50e8a65
commit c9bba0bea2
4 changed files with 197 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
application/chassis/balance.c
View File

@@ -17,6 +17,7 @@
#include "bsp_log.h"
#include "linkNleg.h"
#include "speed_estimation.h"
#include "lqr_calc.h"
static uint32_t balance_dwt_cnt;
@@ -231,6 +232,168 @@ static void ControlSwitch()
}
/* 腿缩回复位,只允许驱动轮电机移动 */
static void ResetChassis()
{
EnableAllMotor(); // 打开全部电机,关节复位到起始角度,驱动电机响应速度输入以从墙角或固连中脱身
// 复位时清空距离和腿长积累量,保证顺利站起
chassis.dist = chassis.target_dist = 0;
l_side.target_len = r_side.target_len = 0.24;
// 撞墙时前后移动保证能重新站立,执行速度输入
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.02 &&
abs(lb->measure.total_angle) < 0.05 && abs(lb->measure.total_angle) > 0.02 &&
abs(rf->measure.total_angle) < 0.05 && abs(rf->measure.total_angle) > 0.02 &&
abs(rb->measure.total_angle) < 0.05 && abs(rb->measure.total_angle) > 0.02)
{
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
}
else if (abs(lf->measure.total_angle) <= 0.02 &&
abs(lb->measure.total_angle) <= 0.02 &&
abs(rf->measure.total_angle) <= 0.02 &&
abs(rb->measure.total_angle) <= 0.02)
{ // 双阈值保证关节能够复位而不会进入死区
chassis_status = ROBOT_READY; // 底盘已经准备好重新站立
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorOuterLoop(joint[i], OPEN_LOOP); // 改回直接开环扭矩输入,让电调对扭矩闭环
return; // 退出函数不再执行关节指令
}
else
chassis_status = ROBOT_STOP;
// 还在复位中,关节改为位置环,执行复位
for (uint8_t i = 0; i < JOINT_CNT; i++)
{
HTMotorOuterLoop(joint[i], ANGLE_LOOP);
HTMotorSetRef(joint[i], 0);
}
}
/* 工作状态设定 */
static void WokingStateSet()
{
if (chassis_cmd_recv.chassis_mode == CHASSIS_RESET) // 复位模式
{
ResetChassis();
return;
}
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) // 未收到遥控器和云台指令底盘进入急停
{
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorStop(joint[i]);
for (uint8_t i = 0; i < DRIVEN_CNT; i++)
LKMotorStop(driven[i]);
return; // 关闭所有电机,发送的指令为零
}
// 运动模式
EnableAllMotor();
// 设置目标速度/腿长/距离
l_side.target_len += chassis_cmd_recv.delta_leglen;
r_side.target_len += chassis_cmd_recv.delta_leglen;
VAL_LIMIT(l_side.target_len, 0.13, 0.3); // 腿长限幅
VAL_LIMIT(r_side.target_len, 0.13, 0.3);
// 加速度限幅,防止键盘控制摔倒
if (abs(chassis_cmd_recv.vx - chassis.target_v) / del_t < MAX_ACC_REF)
chassis.target_v = chassis_cmd_recv.vx;
else
chassis.target_v += sign(chassis_cmd_recv.vx - chassis.target_v) * MAX_ACC_REF * del_t;
// 模型距离参考输入
chassis.target_dist += chassis.target_v * del_t;
chassis.target_yaw = chassis_cmd_recv.offset_angle; // 云台和底盘对齐时电机编码器的单圈反馈角度
}
/**
* @brief 将电机和imu的数据组装为LinkNPodParam结构体和chassisParam结构体
*
* @note HT04电机上电的编码器位置为零(校准过),请看Link2Pod()的note,以及HT04.c中的电机解码部分
* @note 海泰04电机顺时针旋转为正; LK9025电机逆时针旋转为正,此处皆需要转换为模型中给定的正方向
*
*/
static void ParamAssemble()
{
// 机体参数,视为平面刚体
chassis.pitch = (-imu_data->Pitch + BALANCE_GRAVITY_BIAS) * DEGREE_2_RAD;
chassis.pitch_w = -imu_data->Gyro[0];
chassis.yaw = imu_data->YawTotalAngle * DEGREE_2_RAD;
chassis.wz = imu_data->Gyro[2];
chassis.roll = imu_data->Roll * DEGREE_2_RAD + ROLL_GRAVITY_BIAS;
chassis.roll_w = imu_data->Gyro[1];
// HT04电机的角度是顺时针为正,LK9025电机的角度是逆时针为正
l_side.phi1 = PI + LIMIT_LINK_RAD - lb->measure.total_angle;
l_side.phi4 = -lf->measure.total_angle - LIMIT_LINK_RAD;
l_side.phi1_w = -lb->measure.speed_rads;
l_side.phi4_w = -lf->measure.speed_rads;
l_side.w_ecd = l_driven->measure.speed_rads;
r_side.phi1 = PI + LIMIT_LINK_RAD + rb->measure.total_angle;
r_side.phi4 = rf->measure.total_angle - LIMIT_LINK_RAD;
r_side.phi1_w = rb->measure.speed_rads;
r_side.phi4_w = rf->measure.speed_rads;
r_side.w_ecd = -r_driven->measure.speed_rads;
}
/* 腿部控制:抗劈叉; 轮子控制:转向 */
static void SynthesizeMotion()
{
// 跟随云台yaw
if (chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW ||
chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG) // 角度环
{
float p_ref = PIDCalculate(&steer_p_pid, chassis_cmd_recv.offset_angle, 0);
PIDCalculate(&steer_v_pid, chassis.wz, p_ref); // 双环
}
else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 速度环
PIDCalculate(&steer_v_pid, chassis.wz, 4);
l_side.T_wheel -= steer_v_pid.Output;
r_side.T_wheel += steer_v_pid.Output;
// 抗劈叉
volatile static float swerving_speed_ff, ff_coef = 3;
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;
}
/* 腿长控制和Roll补偿 */
static void LegControl()
{
PIDCalculate(&roll_compensate_pid, chassis.roll, 0);
l_side.target_len -= roll_compensate_pid.Output;
r_side.target_len += roll_compensate_pid.Output;
static float gravity_comp = 0;
static float roll_extra_comp_p = 0;
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;
// @todo: 还需要加和roll的纯Kp项
}
/* 设定运动模态的输出 */
static void WattLimitSet()
{
HTMotorSetRef(lf, 0.285f * -l_side.T_front); // 根据扭矩常数计算得到的系数
HTMotorSetRef(lb, 0.285f * -l_side.T_back);
HTMotorSetRef(rf, 0.285f * r_side.T_front);
HTMotorSetRef(rb, 0.285f * r_side.T_back);
LKMotorSetRef(l_driven, 274.348 * l_side.T_wheel);
LKMotorSetRef(r_driven, 274.348 * -r_side.T_wheel);
}
void BalanceTask()
{
del_t = DWT_GetDeltaT(&balance_dwt_cnt);

2
application/chassis/linkNleg.h
View File

@@ -13,7 +13,7 @@ void VMCProject(LinkNPodParam *p)
float phi52 = p->phi5 - p->phi2;
float F_m_L = p->F_leg * p->leg_len;
p->T_back = (THIGH_LEN * msin(phi12) * (F_m_L * msin(phi53) + p->T_hip * mcos(phi53))) / (p->leg_len * msin(phi32));
p->T_front = (CALF_LEN * msin(phi34) * (F_m_L * msin(phi52) + p->T_hip * mcos(phi52))) / (p->leg_len * msin(phi32));
p->T_front = (THIGH_LEN * msin(phi34) * (F_m_L * msin(phi52) + p->T_hip * mcos(phi52))) / (p->leg_len * msin(phi32));
}
/**

32
application/chassis/lqr_calc.h Normal file
View File

@@ -0,0 +1,32 @@
#include "balance.h"
#include "stdint.h"
#include "arm_math.h"
/**
* @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
* @note 得到的腿部力矩输出还要经过综合运动控制系统补偿后映射为两个关节电机输出
*
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
static float k[12][3] = {};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;
// float dist_limit = abs(chassis->target_dist - chassis->dist) > MAX_DIST_TRACK ? sign(chassis->target_dist - chassis->dist) * MAX_DIST_TRACK : (chassis->target_dist - chassis->dist); // todo设置值
// float vel_limit = abs(chassis->target_v - chassis->vel) > MAX_VEL_TRACK ? sign(chassis->target_v - chassis->vel) * MAX_VEL_TRACK : (chassis->target_v - chassis->vel);
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;
}
p->T_wheel = T[0];
p->T_hip = T[1];
}