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

离地时进行速度闭环，基本实现稳定飞坡

Browse Source
This commit is contained in:
kai
2024-05-03 23:22:05 +08:00
parent 69e228ace0
commit 7fee0c15c3
4 changed files with 50 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
application/chassis/balance.c
View File

@@ -113,7 +113,7 @@ void BalanceInit()
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
.Kp = 1200,
.Kd = 150,
.Kd = 200,
.Ki = 0,
.MaxOut = 60,
.DeadBand = 0.0001f,
@@ -173,6 +173,7 @@ void BalanceInit()
// 状态初始化
l_side.target_len = r_side.target_len = 0.12;
l_side.gravity_ff = r_side.gravity_ff = 60.0f;
chassis.vel_cov = 100; // 速度协方差初始化
chassis_status = ROBOT_READY;
DWT_GetDeltaT(&balance_dwt_cnt);
@@ -231,8 +232,6 @@ static void ResetChassis()
l_side.target_len = r_side.target_len = 0.12;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 驱动轮支持力为定值
l_side.normal_force = r_side.normal_force = 100.0f;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
@@ -285,8 +284,6 @@ static void WokingStateSet()
chassis.dist = chassis.target_dist = 0;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 驱动轮支持力为定值
l_side.normal_force = r_side.normal_force = 100.0f;
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorStop(joint[i]);
@@ -383,11 +380,10 @@ static void LegControl() /* 腿长控制和Roll补偿 */
l_side.target_len += roll_compensate_pid.Output;
r_side.target_len -= roll_compensate_pid.Output;
static float gravity_comp = 60;
static float roll_extra_comp_p = 400;
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;
l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + l_side.gravity_ff - roll_comp;
r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + r_side.gravity_ff + roll_comp;
}
static void WattLimitSet() /* 设定运动模态的输出 */
@@ -425,18 +421,15 @@ void BalanceTask()
// VMC映射成关节输出
VMCProject(&l_side);
VMCProject(&r_side);
// 驱动轮支持力解算
NormalForceSolve(&l_side, Chassis_IMU_data);
NormalForceSolve(&r_side, Chassis_IMU_data);
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
else
{
// 正常模式下再进行驱动轮支持力解算
NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
}
// 运动模态,电机输出映射和限幅
WattLimitSet();

4
application/chassis/balance.h
View File

@@ -1,5 +1,7 @@
#pragma once
#include "stdint.h"
// 底盘参数
#define CALF_LEN 0.24f // 小腿
#define THIGH_LEN 0.14f // 大腿
@@ -55,6 +57,8 @@ typedef struct
float T_wheel;
float zw_ddot; // 驱动轮竖直方向加速度
float normal_force; // 支持力
float gravity_ff; // 重力前馈
uint8_t fly_flag; // 离地标志位
// pod
float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一

10
application/chassis/fly_detection.h
View File

@@ -4,7 +4,7 @@
#include "user_lib.h"
// 驱动轮支持力解算
void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
void NormalForceSolve(LinkNPodParam *p, INS_t *imu)
{
static float accx, accy, accz;
accx = imu->MotionAccel_b[X];
@@ -15,11 +15,17 @@ void NormalForceSolve(LinkNPodParam *p, INS_t *imu, float dt)
pitch = imu->Pitch;
roll = imu->Roll;
// 驱动轮竖直方向加速度
// 机体竖直方向加速度
p->zw_ddot = -msin(roll) * accx + mcos(roll) * msin(pitch) * accy + mcos(pitch) * mcos(roll) * accz;
// 驱动轮支持力解算
static float P;
P = p->F_leg * mcos(p->theta) + p->T_hip * msin(p->theta) / p->leg_len;
p->normal_force = P + WHEEL_MASS * (p->zw_ddot + 9.81f);
// 离地检测
if(p->normal_force < 20.0f)
p->fly_flag = 1;
else
p->fly_flag = 0;
}

66
application/chassis/lqr_calc.h
View File

@@ -9,48 +9,44 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
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};
static 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;
// 离地检测
if (p->normal_force < 20.0f)
{
for (size_t i = 0; i < 12; i++)
{
// 除 theta 和 theta_dot 的关节输出外，其余增益全部置0
if(i != 6 && i != 7)
{
for (size_t j = 0; j < 3; j++)
{
k[i][j] = 0;
}
}
}
}
// 计算增益
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;
if(i == 0) // 离地时仅对速度闭环，保证落地时轮速与机体速度一致
{
T[i] = (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) + (p->fly_flag ? 0 :
( (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 + 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 ));
}
else if(i == 1) // 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直
{
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 + (p->fly_flag ? 0 :
( (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];