TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 11:37:45 +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);