robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: robofish:58f7ba86dbb6a0497aea7442461da28d29b37776
Branches Tags
robofish:main
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
...
compare: robofish:221673f7027578d1a2e64eba72980bf53981ff96
Branches Tags
robofish:神秘小轮腿
robofish:ai
robofish:单独底层纯转发
robofish:main

6 Commits
58f7ba86db ... 221673f702

Author SHA1 Message Date
Robofish
221673f702 修好了 2025-09-18 21:09:22 +08:00
Robofish
ea97df03a8 起来自旋 2025-09-17 08:19:07 +08:00
Robofish
b00ecf1af5 会斜腿 2025-09-17 08:10:09 +08:00
Robofish
eef7001e2b 起飞了旋转了 2025-09-17 07:38:59 +08:00
Robofish
de8bc5ac8c 起不来站不住 2025-09-17 07:10:10 +08:00
Robofish
e7cccbf706 提交 2025-09-17 03:41:35 +08:00
56 changed files with 1931 additions and 4214 deletions
Show Stats Expand all files Collapse all files

1358
Chassis_Task.c
View File

File diff suppressed because it is too large Load Diff

299
Chassis_Task.h
View File

@ -1,299 +0,0 @@
#ifndef _CHASSIS_TASK
#define _CHASSIS_TASK
#include "main.h"
#include "struct_typedef.h"
#include "pid.h"
#include "bsp_can.h"
// ------------- Limit info -------------
#define MAX_ACCL 13000.0f
#define MAX_ACCL_JOINT 15.0f
#define MAX_FOOT_OUTPUT 2048
// ------------- Target value info -------------
#define SIT_MODE_HEIGHT_SET 0.18f
#define NORMAL_MODE_HEIGHT_SET 0.10f
#define HIGH_MODE_HEIGHT_SET 0.27f
#define EXTREMELY_HIGH_MODE_HEIGHT_SET 0.30f
// ------------- Mech info -------------
#define L1 0.15f
#define L2 0.25f
#define L3 0.25f
#define L4 0.15f
#define L5 0.1f
#define WHEEL_PERIMETER 0.56547
#define WHEEL_RADIUS 0.09f
#define LEG_OFFSET 0.3790855135f // 水平位置到上限位的夹角
#define LOWER_SUPPORT_FORCE_FOR_JUMP 5.0f
#define LOWER_SUPPORT_FORCE 0.0f
#define MOVE_LOWER_BOUND 0.3f
#define EXIT_PITCH_ANGLE 0.1f
#define DANGER_PITCH_ANGLE 0.25f
#define FEED_f 7.5f
#define FEED_f_1 3.5f
#define NORMAL_MODE_WEIGHT_DISTANCE_OFFSET -0.0f
#define MOTOR_POS_UPPER_BOUND 0.05f
#define MOTOR_POS_LOWER_BOUND 1.4f
#define LIMITED_TORQUE 0.5f
#define UNLIMITED_TORQUE 200.0f
// ------------- Time info -------------
#define CHASSIS_TASK_INIT_TIME 500
#define TASK_RUN_TIME 0.002f
// ------------- Transfer info -------------
#define MOTOR_ECD_TO_RAD 0.00019174779 // 2*PI / 32767
#define HALF_ECD_RANGE 14383
#define HALF_POSITION_RANGE 3.0f
// #define CC 0.00512f
// #define CC 1/494.0f
#define TORQ_K 494.483818182
// ------------- Math info -------------
#define PI2 6.28318530717959f
#define PI 3.14159265358979f
#define PI_2 1.57079632679489f
#define PI_4 0.78539816339744f
typedef enum
{
ENABLE_CHASSIS = 0,
DISABLE_CHASSIS,
} chassis_mode_e;
typedef enum
{
NO_FORCE,
FOOT_LAUNCHING,
JOINT_LAUNCHING,
BALANCING_READY,
JOINT_REDUCING,
} chassis_balancing_mode_e;
typedef enum
{
NONE,
NORMAL_MOVING_MODE,
ABNORMAL_MOVING_MODE,
JUMPING_MODE,
CAP_MODE,
FLY_MODE,
TK_MODE,
} sport_mode_e;
typedef enum
{
READY_TO_JUMP,
PREPARING_LANDING,
PREPARING_STAND_JUMPING,
CONSTACTING_LEGS,
EXTENDING_LEGS,
CONSTACTING_LEGS_2,
FINISHED,
} jumping_stage_e;
typedef enum
{
NOT_DEFINE,
STANDING_JUMP,
MOVING_JUMP,
} jumping_mode_e;
typedef enum
{
SIT_MODE = 0,
NORMAL_MODE,
HIGH_MODE,
EXTREMELY_HIGH_MODE,
CHANGING_HIGH,
} chassis_high_mode_e;
typedef enum
{
MOTOR_NO_FORCE = 0,
MOTOR_FORCE,
} chassis_motor_mode_e;
typedef enum
{
ON_GROUND = 0,
OFF_GROUND = 1,
} suspend_flag_e;
typedef struct
{
chassis_mode_e chassis_mode, last_chassis_mode;
chassis_balancing_mode_e chassis_balancing_mode, last_chassis_balancing_mode;
sport_mode_e sport_mode, last_sport_mode;
jumping_mode_e jumping_mode, last_jumping_mode;
jumping_stage_e jumping_stage, last_jumping_stage;
chassis_high_mode_e chassis_high_mode, last_chassis_high_mode;
} mode_t;
typedef struct
{
const fp32 *chassis_INS_angle_point;
const fp32 *chassis_INS_gyro_point;
const fp32 *chassis_INS_accel_point;
fp32 yaw_angle, pitch_angle, roll_angle;
fp32 yaw_gyro, pitch_gyro, roll_gyro;
fp32 yaw_accel, pitch_accel, roll_accel;
fp32 yaw_angle_sett, pitch_angle_set, roll_angle_set;
fp32 yaw_gyro_set, pitch_gyro_set, roll_gyro_set;
fp32 ideal_high;
fp32 leg_length_L, last_leg_length_L, leg_length_L_set;
fp32 leg_length_R, last_leg_length_R, leg_length_R_set;
fp32 leg_dlength_L;
fp32 leg_dlength_R;
fp32 foot_roll_angle;
fp32 leg_angle_L, last_leg_angle_L, leg_angle_L_set;
fp32 leg_angle_R, last_leg_angle_R, leg_angle_R_set;
fp32 leg_gyro_L, leg_gyro_R;
fp32 foot_distance, foot_distance_K, foot_distance_set;
fp32 foot_speed, foot_speed_K, foot_speed_set;
fp32 supportive_force_L, supportive_force_R;
} chassis_posture_info_t;
typedef struct
{
// -------- horizontal force --------
fp32 joint_balancing_torque_L, joint_balancing_torque_R;
fp32 foot_balancing_torque_L, foot_balancing_torque_R;
fp32 foot_moving_torque_L, foot_moving_torque_R;
fp32 joint_moving_torque_L, joint_moving_torque_R;
fp32 joint_prevent_splits_torque_L, joint_prevent_splits_torque_R;
fp32 joint_horizontal_torque_L, joint_horizontal_torque_R;
fp32 foot_horizontal_torque_L, foot_horizontal_torque_R;
fp32 joint_horizontal_torque_temp1_R, joint_horizontal_torque_temp2_R;
fp32 joint_horizontal_torque_temp1_L, joint_horizontal_torque_temp2_L;
fp32 yaw_torque;
// -------- vertical force --------
fp32 joint_roll_torque_L, joint_roll_torque_R;
fp32 joint_stand_torque_L, joint_stand_torque_R;
fp32 joint_vertical_torque_L, joint_vertical_torque_R;
fp32 joint_real_vertical_torque_L, joint_real_vertical_torque_R;
fp32 joint_vertical_torque_temp1_R, joint_vertical_torque_temp2_R;
fp32 joint_vertical_torque_temp1_L, joint_vertical_torque_temp2_L;
} torque_info_t;
typedef struct
{
fp32 J1_L,J2_L;
fp32 J3_L,J4_L;
fp32 J1_R,J2_R;
fp32 J3_R,J4_R;
fp32 invJ1_L,invJ2_L;
fp32 invJ3_L,invJ4_L;
fp32 invJ1_R,invJ2_R;
fp32 invJ3_R,invJ4_R;
} mapping_info_t;
typedef struct
{
const HT_motor_measure_t *motor_measure;
chassis_motor_mode_e motor_mode, last_motor_mode;
bool_t offline_flag;
fp32 position;
fp32 init_position;
fp32 position_offset;
fp32 velocity;
fp32 torque_out, torque_get;
fp32 max_torque, min_torque;
} joint_motor_t;
typedef struct
{
motor_measure_t motor_measure;
chassis_motor_mode_e motor_mode, last_motor_mode;
bool_t offline_flag;
fp32 distance, distance_offset, last_position, position, turns;
fp32 speed;
fp32 torque_out, torque_get;
} foot_motor_t;
typedef struct
{
bool_t init_flag;
suspend_flag_e suspend_flag_L, last_suspend_flag_L;
suspend_flag_e suspend_flag_R, last_suspend_flag_R;
bool_t Ignore_Off_Ground;
bool_t abnormal_flag;
bool_t static_flag, last_static_flag;
bool_t moving_flag, last_moving_flag;
bool_t rotation_flag;
bool_t controlling_flag;
bool_t set_pos_after_moving;
bool_t overpower_warning_flag;
bool_t last_overpower_warning_flag;
bool_t stablize_high_flag;
bool_t last_stablize_high_flag;
} flag_info_t;
typedef struct
{
pid_type_def buffer_control_pid;
pid_type_def cap_pid;
pid_type_def scale_down_pid;
} pid_info_t;
typedef struct
{
mode_t mode;
chassis_posture_info_t chassis_posture_info;
torque_info_t torque_info;
mapping_info_t mapping_info;
flag_info_t flag_info;
pid_info_t pid_info;
const Gimbal_ctrl_t *chassis_rc_ctrl;
joint_motor_t joint_motor_1, joint_motor_2, joint_motor_3, joint_motor_4;
foot_motor_t foot_motor_L, foot_motor_R;
}chassis_control_t;
enum Chassis_Mode
{
Chassis_No_Force = 0,
Follow_Gimbal,
Follow_Gimbal_90Deg,
No_Follow,
Rotate,
// TK_MODE,
};
extern enum Chassis_Mode chassis_mode;
extern chassis_control_t chassis_control;
extern fp32 roll_PD[2];
#endif

321
HerKules_VOCAL_SJ_LQR_v4_with_data.m
View File

@ -1,321 +0,0 @@
% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
% v2：添加了可以专门调试定腿长的功能（2024/05/08）
% v3：优化部分注释，添加单位说明（2024/05/15）
% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
tic
clear all
clc
% 定义机器人机体参数
syms R_w % 驱动轮半径
syms R_l % 驱动轮轮距/2
syms l_l l_r % 左右腿长
syms l_wl l_wr % 驱动轮质心到左右腿部质心距离
syms l_bl l_br % 机体质心到左右腿部质心距离
syms l_c % 机体质心到腿部关节中心点距离
syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量
syms I_w % 驱动轮转动惯量 (自然坐标系法向)
syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化)
syms I_b % 机体转动惯量 (自然坐标系法向)
syms I_z % 机器人z轴转动惯量 (简化为常量)
% 定义其他独立变量并补充其导数
syms theta_wl theta_wr % 左右驱动轮转角
syms dtheta_wl dtheta_wr
syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
% 定义状态向量
syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
% 定义控制向量
syms T_wl T_wr T_bl T_br
% 输入物理参数：重力加速度
syms g
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
A = sym('A',[10 10]);
B = sym('B',[10 4]);
% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
for p = 5:2:9
A_index = (p - 3)/2;
A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
for q = 6:2:10
A(q,p) = J_A(q/2,A_index);
end
end
% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
for r = 1:10
if rem(r,2) == 0
A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
else
A(r,:) = zeros(1,10);
A(r,r+1) = 1;
end
end
% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
for h = 1:4
B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
for f = 6:2:10
B(f,h) = J_B(f/2,h);
end
end
% B的其余数值为0
for e = 1:2:9
B(e,:) = zeros(1,4);
end
%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
g_ac = 9.81;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入机器人机体基本参数 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_w_ac = 0.9; % 驱动轮半径 （单位：m）
R_l_ac = 0.25; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.037; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.8; m_l_ac = 1.6183599; m_b_ac = 11.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
I_w_ac = (3510000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
I_b_ac = 0.260; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
I_z_ac = 0.226; % 机器人z轴转动惯量 （单位：kg m^2）
%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
l_l_ac = 0.18; % 左腿摆杆长度 （左腿对应数据） （单位：m）
l_wl_ac = 0.05; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
l_bl_ac = 0.13; % 机体转轴到左腿摆杆质心距离 （单位：m）
I_ll_ac = 0.02054500; % 左腿摆杆转动惯量 （单位：kg m^2）
l_r_ac = 0.18; % 右腿摆杆长度 （右腿对应数据） （单位：m）
l_wr_ac = 0.05; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
l_br_ac = 0.13; % 机体转轴到右腿摆杆质心距离 （单位：m）
I_lr_ac = 0.02054500; % 右腿摆杆转动惯量 （单位：kg m^2）
% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
% 两侧两个关节电机之间的中间点相连所形成的轴
% （如果目的是小板凳，考虑使左右腿相关数据一致）
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
% 通过以下方式记录数据: 矩阵分4列，
% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
% 第二列为l_wl，单位：m
% 第三列为l_bl，单位：m
% 第四列为I_ll，单位：kg m^2
% （注意单位别搞错！）
% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599;
0.12, 0.0470, 0.0730, 0.01862845;
0.13, 0.0476, 0.0824, 0.01898641;
0.14, 0.0480, 0.0920, 0.01931342;
0.15, 0.0490, 0.1010, 0.01962521;
0.16, 0.0500, 0.1100, 0.01993092;
0.17, 0.0510, 0.1190, 0.02023626;
0.18, 0.0525, 0.1275, 0.02054500;
0.19, 0.0539, 0.1361, 0.02085969;
0.20, 0.0554, 0.1446, 0.02118212;
0.21, 0.0570, 0.1530, 0.02151357;
0.22, 0.0586, 0.1614, 0.02185496;
0.23, 0.0600, 0.1700, 0.02220695;
0.24, 0.0621, 0.1779, 0.02256999;
0.25, 0.0639, 0.1861, 0.02294442;
0.26, 0.0657, 0.1943, 0.02333041;
0.27, 0.0676, 0.2024, 0.02372806;
0.28, 0.0700, 0.2100, 0.02413735;
0.29, 0.0713, 0.2187, 0.02455817;
0.30, 0.0733, 0.2267, 0.02499030];
% 以上数据应通过实际测量或sw图纸获得
% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
Leg_data_r = Leg_data_l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入QR矩阵 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 矩阵Q中，以下列分别对应：
% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 2, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 12000, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 200, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 1000, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 1, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 1000, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 20000, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 1];
% 其中：
% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数
% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
% 矩阵中，以下列分别对应：
% T_wl T_wr T_bl T_br
lqr_R = [0.25, 0, 0, 0;
0, 0.25, 0, 0;
0, 0, 1.5, 0;
0, 0, 0, 1.5];
% 其中：
% T_wl: 左侧驱动轮输出力矩
% T_wr：右侧驱动轮输出力矩
% T_bl：左侧髋关节输出力矩
% T_br：右腿髋关节输出力矩
% 单位皆为Nm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.')
%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
length = size(Leg_data_l,1); % 测量腿部数据集的行数
% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
% 是l_r，第三列是对应的K_ij的数值
K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
for i = 1:length
for j = 1:length
index = (i - 1)*length + j;
l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
l_wl_ac = Leg_data_l(i,2);
l_bl_ac = Leg_data_l(i,3);
I_ll_ac = Leg_data_l(i,4);
l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
l_wr_ac = Leg_data_r(j,2);
l_br_ac = Leg_data_r(j,3);
I_lr_ac = Leg_data_r(j,4);
for k = 1:40
K_sample(index,1,k) = l_l_ac;
K_sample(index,2,k) = l_r_ac;
end
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R);
% 根据指定的l_l,l_r输入对应的K_ij的数值
for l = 1:4
for m = 1:10
K_sample(index,3,(l - 1)*10 + m) = K(l,m);
end
end
end
end
% 创建收集全部K_ij的多项式拟合的全部系数的集合
K_Fit_Coefficients = zeros(40,6);
for n = 1:40
K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
end
Polynomial_expression = formula(K_Surface_Fit)
% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
% 每一行和K_ij的对应关系如下：
% - 第1行对应K_1,1
% - 第14行对应K_2,4
% - 第22行对应K_3,2
% - 第37行对应K_4,7
% ... 其他行对应关系类似
% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
% 每一列和系数pij的对应关系如下：
% - 第1列对应p00
% - 第2列对应p10
% - 第3列对应p01
% - 第4列对应p20
% - 第5列对应p11
% - 第6列对应p02
K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.')
% 正确食用方法：
% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
% 2.并填入对应系数即可
toc
%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
% （前面的蛆，以后再来探索吧（bushi
%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
% 基于机体以及物理参数，获得控制矩阵A，B的全部数值
A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
% 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
% P为Riccati方程的解，矩阵L可以无视
[P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
end

67
LQR_修正建议.md
View File

@ -1,67 +0,0 @@
## LQR控制器修正建议
### 问题总结：
1. LQR增益计算方式不一致（3次多项式 vs 2次多项式）
2. 状态向量维度错误（12维 vs 10维）
3. 控制律映射不正确
4. 状态定义与MATLAB模型不匹配
### 修正建议：
#### 1. 修正LQR_K_calc函数
应该使用2次多项式而不是3次：
```cpp
float VMC::LQR_K_calc(float *coe, float l_l, float l_r) {
// 使用MATLAB中定义的2次多项式
// p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
return coe[0] + coe[1]*l_l + coe[2]*l_r + coe[3]*l_l*l_l + coe[4]*l_l*l_r + coe[5]*l_r*l_r;
}
```
#### 2. 修正增益矩阵维度
LQR增益矩阵应该是4×10，总共40个系数：
```cpp
// 为每条腿分配40个LQR系数，而不是12个
float LQR_K[40]; // 4×10矩阵，展开为一维数组
```
#### 3. 修正状态向量定义
确保状态向量与MATLAB模型一致：
```cpp
// 状态向量：[s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]
float state_error[10] = {
move_argu_.xhat - move_argu_.target_x, // s误差
move_argu_.x_dot_hat - move_argu_.target_dot_x, // ds误差
this->yaw_ - 0.0f, // phi误差
this->gyro_.z - 0.0f, // dphi误差
leg_argu_[0].theta - 平衡角度, // theta_ll误差
leg_argu_[0].d_theta - 0.0f, // dtheta_ll误差
leg_argu_[1].theta - 平衡角度, // theta_lr误差
leg_argu_[1].d_theta - 0.0f, // dtheta_lr误差
this->pit_ - 0.0f, // theta_b误差
this->gyro_.x - 0.0f // dtheta_b误差
};
```
#### 4. 修正控制律计算
使用标准的LQR控制律：
```cpp
// 计算控制输出：u = -K * (x - x_ref)
float control[4] = {0}; // [T_wl, T_wr, T_bl, T_br]
for(int i = 0; i < 4; i++) {
for(int j = 0; j < 10; j++) {
control[i] += LQR_K[i*10 + j] * state_error[j];
}
control[i] = -control[i]; // 负反馈
}
leg_argu_[0].Tw = control[0]; // T_wl
leg_argu_[1].Tw = control[1]; // T_wr
leg_argu_[0].Tp = control[2]; // T_bl
leg_argu_[1].Tp = control[3]; // T_br
```
### 总结：
当前的LQR实现在数学原理上有偏差，需要按照标准的LQR控制理论和MATLAB模型进行修正。
主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。

4
MDK-ARM/DevC.uvprojx
View File

@ -139,7 +139,7 @@
</Flash1>
<bUseTDR>1</bUseTDR>
<Flash2>BIN\UL2V8M.DLL</Flash2>
<Flash3></Flash3>
<Flash3>"" ()</Flash3>
<Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
@ -314,7 +314,7 @@
</ArmAdsMisc>
<Cads>
<interw>1</interw>
<Optim>2</Optim>
<Optim>4</Optim>
<oTime>0</oTime>
<SplitLS>0</SplitLS>
<OneElfS>1</OneElfS>

297
User/README.md
View File

@ -1,297 +0,0 @@
# 轮腿机器人LQR平衡控制系统
这是一个完整的轮腿机器人LQR+VMC平衡控制系统，支持6个电机（2个轮电机+4个关节电机）的协调控制。
## 系统架构
```
┌─────────────────┐ ┌──────────────────┐ ┌─────────────────┐
│ 传感器输入 │ │ 平衡控制器 │ │ 电机控制输出 │
│ │ │ │ │ │
│ • IMU姿态数据 │────▶│ LQR控制器 │────▶│ • 轮电机力矩 │
│ • 电机反馈数据 │ │ VMC控制器 │ │ • 关节电机力矩 │
│ • 遥控器命令 │ │ 状态估计器 │ │ • MIT控制参数 │
└─────────────────┘ └──────────────────┘ └─────────────────┘
```
## 文件结构
### 核心控制文件
- `balance_control.h/c` - 主控制器，整合LQR和VMC
- `lqr.h/c` - LQR控制器实现
- `vmc.h/c` - 虚拟模型控制器
- `kinematics.h/c` - 运动学计算
### 使用示例
- `balance_control_example.c` - 完整的使用示例
### MATLAB工具
- `lqr_design_optimized.m` - LQR参数设计和系数生成
- `vmc.m` - VMC控制器设计参考
## 快速开始
### 1. 硬件连接
确保以下硬件正确连接：
- IMU传感器（提供机体姿态）
- 2个轮电机（支持转矩电流控制）
- 4个关节电机（支持MIT控制模式）
- CAN总线通信
### 2. 参数配置
#### 2.1 运行MATLAB脚本生成LQR参数
```matlab
% 在MATLAB中运行
run('utils/lqr_design_optimized.m');
```
这将生成K矩阵的拟合系数，复制到`lqr.c`中的`K_fit_coefficients`数组。
#### 2.2 修改机器人物理参数
在`lqr.h`中修改您的机器人参数：
```c
#define R_W 0.09f // 驱动轮半径 (m)
#define R_L 0.25f // 两个驱动轮之间距离/2 (m)
#define M_W 0.8f // 驱动轮质量 (kg)
#define M_L 1.6183599f // 腿部质量 (kg)
#define M_B 11.542f // 机体质量 (kg)
// ... 其他参数
```
#### 2.3 配置电机ID和CAN接口
在`balance_control_example.c`中修改电机ID：
```c
#define WHEEL_LEFT_ID 0x01
#define WHEEL_RIGHT_ID 0x02
#define HIP_LEFT_ID 0x03
#define HIP_RIGHT_ID 0x04
#define KNEE_LEFT_ID 0x05
#define KNEE_RIGHT_ID 0x06
```
### 3. 代码集成
#### 3.1 在main函数中初始化
```c
#include "balance_control.h"
int main(void)
{
// HAL初始化
HAL_Init();
SystemClock_Config();
// 初始化外设
MX_CAN1_Init();
MX_TIM1_Init();
// 初始化平衡控制系统
balance_system_init();
// 启动控制循环
HAL_TIM_Base_Start_IT(&htim1); // 1kHz控制频率
while(1)
{
// 主循环处理其他任务
HAL_Delay(100);
}
}
```
#### 3.2 定时器中断处理
```c
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == TIM1) {
balance_control_task(); // 1kHz控制任务
}
}
```
### 4. 传感器数据接口
#### 4.1 实现IMU数据读取
在`balance_control_example.c`中实现`read_imu_data()`函数：
```c
void read_imu_data(imu_data_t* imu)
{
// 从您的IMU传感器读取数据
imu->pitch = get_imu_pitch();
imu->roll = get_imu_roll();
imu->yaw = get_imu_yaw();
imu->pitch_rate = get_imu_pitch_rate();
imu->roll_rate = get_imu_roll_rate();
imu->yaw_rate = get_imu_yaw_rate();
// ... 其他数据
}
```
#### 4.2 实现电机反馈读取
```c
void read_motor_feedback(motor_feedback_t* motor_fb)
{
// 从CAN总线读取电机反馈
motor_fb->wheel_left_angle = get_motor_angle(WHEEL_LEFT_ID);
motor_fb->wheel_right_angle = get_motor_angle(WHEEL_RIGHT_ID);
motor_fb->hip_left_angle = get_motor_angle(HIP_LEFT_ID);
// ... 其他电机数据
}
```
#### 4.3 实现电机控制输出
```c
void send_motor_control(const motor_control_t* motor_ctrl)
{
// 发送轮电机转矩控制指令
send_wheel_torque_cmd(WHEEL_LEFT_ID, motor_ctrl->wheel_left_torque_cmd);
send_wheel_torque_cmd(WHEEL_RIGHT_ID, motor_ctrl->wheel_right_torque_cmd);
// 发送关节电机MIT控制指令
send_mit_control_cmd(HIP_LEFT_ID,
0, // 位置指令
0, // 速度指令
motor_ctrl->hip_left_kp,
motor_ctrl->hip_left_kd,
motor_ctrl->hip_left_torque_cmd);
// ... 其他关节电机
}
```
## 控制模式
### 1. 平衡模式 (ROBOT_STATE_BALANCE)
- 自动保持机体平衡
- 响应遥控器前进/转向命令
- 自动调节腿长以适应地形
### 2. 运动模式 (ROBOT_STATE_MOVE)
- 在平衡基础上执行运动控制
- 支持前进、后退、转向
- 支持高度调节
### 3. 紧急停止 (ROBOT_STATE_EMERGENCY)
- 立即停止所有电机输出
- 安全保护机制
## 控制参数调节
### LQR参数调节
在MATLAB脚本`lqr_design_optimized.m`中修改Q和R矩阵：
```matlab
% 状态权重矩阵Q - 增大数值提高控制精度
Q = diag([
10, % s - 水平位移
5, % ds - 水平速度
20, % phi - 偏航角
10, % dphi - 偏航角速度
100, % theta_ll - 左腿角
50, % dtheta_ll - 左腿角速度
100, % theta_lr - 右腿角
50, % dtheta_lr - 右腿角速度
200, % theta_b - 机体倾斜角 (最重要)
100 % dtheta_b - 机体角速度
]);
% 控制权重矩阵R - 增大数值降低控制量
R = diag([
0.1, % T_wl - 左轮力矩
0.1, % T_wr - 右轮力矩
1.0, % T_bl - 左腿力矩
1.0 % T_br - 右腿力矩
]);
```
### VMC参数调节
在`balance_control.c`中修改VMC参数：
```c
vmc_params_t vmc_params = {
.K_spring = 800.0f, // 径向弹簧刚度 - 控制腿长刚度
.K_damper = 80.0f, // 径向阻尼系数 - 控制腿长阻尼
.K_theta = 150.0f, // 角度刚度 - 控制腿角刚度
.K_dtheta = 15.0f, // 角速度阻尼 - 控制腿角阻尼
.max_radial_force = 1000.0f, // 最大径向力
.max_tangential_force = 500.0f, // 最大切向力
};
```
## 安全机制
### 1. 倾斜角保护
```c
#define MAX_TILT_ANGLE 0.5f // 最大倾斜角 (rad)
```
### 2. 腿长保护
```c
#define LEG_MIN_REACH 0.05f // 最小腿长 (m)
#define LEG_MAX_REACH 0.28f // 最大腿长 (m)
```
### 3. 力矩限制
```c
#define MAX_WHEEL_TORQUE 50.0f // 最大轮子力矩 (N*m)
#define MAX_JOINT_TORQUE 100.0f // 最大关节力矩 (N*m)
```
## 调试和监控
### 1. 串口调试输出
系统会定期输出关键状态信息：
```
=== Balance Control Status ===
Task Count: 1000
Robot State: 1
Safety Flag: 1
IMU Pitch: 0.050 rad (2.9 deg)
Left Leg Length: 0.200 m
Right Leg Length: 0.200 m
==============================
```
### 2. 状态监控
可以通过以下变量监控系统状态：
- `g_balance_ctrl.state` - 机器人状态
- `g_balance_ctrl.safety_flag` - 安全标志
- `g_balance_ctrl.robot_state` - 完整机器人状态
## 常见问题
### Q: 机器人启动后不稳定？
A:
1. 检查IMU标定是否正确
2. 调整LQR的Q矩阵，增大机体角度权重
3. 检查电机正负方向是否正确
### Q: 腿部抖动？
A:
1. 降低VMC的刚度参数
2. 增加VMC的阻尼参数
3. 检查关节电机的刚度设置
### Q: 轮子打滑？
A:
1. 降低LQR的轮子力矩输出
2. 增加R矩阵中轮子力矩的权重
3. 检查地面摩擦条件
### Q: 系统响应慢？
A:
1. 增加控制频率（目前为1kHz）
2. 调整LQR的Q矩阵权重
3. 检查CAN通信延迟
## 技术支持
如需技术支持或有疑问，请：
1. 检查上述常见问题
2. 确认硬件连接正确
3. 验证参数配置无误
4. 查看串口调试输出
## 更新日志
- v1.0 (2025-08-30): 初始版本，支持LQR+VMC协调控制
- 完整的6电机控制接口
- MATLAB参数设计工具
- 完善的安全保护机制

23
User/component/limiter.c
View File

@ -105,3 +105,26 @@ float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
else
return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
}
/**
* @brief 速度限制器，计算目标速度
* @param accl_limit 加速度限制
* @param speed_now 当前速度
* @param speed_target 目标速度
* @param dt 时间间隔
* @return float 计算得到的目标速度
*/
float SpeedLimit_TargetSpeed(float accl_limit, float speed_now, float speed_target, float dt){
float speed_diff = speed_target - speed_now;
float max_speed_change = accl_limit * dt;
if (fabsf(speed_diff) > max_speed_change) {
if (speed_diff > 0) {
return speed_now + max_speed_change;
} else {
return speed_now - max_speed_change;
}
} else {
return speed_target;
}
}

10
User/component/limiter.h
View File

@ -53,3 +53,13 @@ float PowerLimit_TargetPower(float power_limit, float power_buffer);
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big);
/**
* @brief 速度限制器，计算目标速度
* @param accl_limit 加速度限制
* @param speed_now 当前速度
* @param speed_target 目标速度
* @param dt 时间间隔
* @return float 计算得到的目标速度
*/
float SpeedLimit_TargetSpeed(float accl_limit, float speed_now, float speed_target, float dt);

358
User/component/lqr.c
View File

@ -1,229 +1,265 @@
/*
* LQR线性二次型最优控制器简化实现
*
* 本文件实现了轮腿机器人的LQR (Linear Quadratic Regulator) 控制算法
* 主要功能包括:
* 1. 状态反馈控制
* 2. 增益矩阵K计算控制输出
* 3. 控制输出限幅
*
* 系统模型:
* u = -K*(x - x_ref) (状态反馈)
* LQR控制器实现
*/
#include "lqr.h"
#include <string.h>
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macros ----------------------------------------------------------- */
#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
#define LQR_EPSILON (1e-6f) // 数值计算精度
#define LQR_DEFAULT_MAX_WHEEL (50.0f) // 默认最大轮毂力矩 (N*m)
#define LQR_DEFAULT_MAX_JOINT (30.0f) // 默认最大关节力矩 (N*m)
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* 从MATLAB仿真get_k.m得到的默认增益矩阵多项式拟合系数 */
/* 这些系数是通过对不同腿长的LQR增益进行3阶多项式拟合得到的 */
static LQR_GainMatrix_t default_gain_matrix = {
/* K矩阵第一行 - 轮毂力矩T的增益系数 */
.k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f}, /* theta */
.k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_theta */
.k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f}, /* x */
.k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f}, /* d_x */
.k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f}, /* phi */
.k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* d_phi */
/* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
.k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f}, /* theta */
.k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_theta */
.k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f}, /* x */
.k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f}, /* d_x */
.k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f}, /* phi */
.k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* d_phi */
};
/* Private function prototypes ---------------------------------------------- */
static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr);
static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr);
static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]);
static float LQR_ComputeStateError(float current, float reference);
/* Public functions --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化LQR控制器
*/
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) {
if (lqr == NULL || max_wheel_torque <= 0 || max_joint_torque <= 0) {
if (lqr == NULL) {
return -1;
}
// 设置力矩限制
lqr->max_wheel_torque = max_wheel_torque;
lqr->max_joint_torque = max_joint_torque;
/* 清零结构体 */
memset(lqr, 0, sizeof(LQR_Controller_t));
// 重置状态
LQR_Reset(lqr);
/* 设置控制限制 */
lqr->param.max_wheel_torque = max_wheel_torque;
lqr->param.max_joint_torque = max_joint_torque;
/* 设置默认系统物理参数（从MATLAB仿真get_k_length.m获取） */
lqr->param.R = 0.072f; /* 驱动轮半径 */
lqr->param.l = 0.03f; /* 机体重心到转轴距离 */
lqr->param.mw = 0.60f; /* 驱动轮质量 */
lqr->param.mp = 1.8f; /* 摆杆质量 */
lqr->param.M = 1.8f; /* 机体质量 */
lqr->param.g = 9.8f; /* 重力加速度 */
/* 计算转动惯量 */
lqr->param.Iw = lqr->param.mw * lqr->param.R * lqr->param.R;
lqr->param.IM = lqr->param.M * (0.3f * 0.3f + 0.12f * 0.12f) / 12.0f;
/* 设置默认增益矩阵 */
lqr->gain_matrix = &default_gain_matrix;
/* 设置默认目标状态（平衡状态） */
memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
/* 初始化当前腿长为中等值 */
lqr->current_leg_length = 0.25f;
/* 计算初始增益矩阵 */
LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
lqr->initialized = true;
return 0;
}
/**
* @brief 设置固定LQR增益矩阵
*/
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K) {
if (lqr == NULL || !lqr->initialized || K == NULL) {
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix) {
if (lqr == NULL || gain_matrix == NULL) {
return -1;
}
// 复制增益矩阵
memcpy(&lqr->K, K, sizeof(LQR_GainMatrix_t));
lqr->gain_matrix = gain_matrix;
return 0;
/* 重新计算增益矩阵 */
return LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
}
/**
* @brief 更新机器人状态
*/
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
if (lqr == NULL || !lqr->initialized || state == NULL) {
if (lqr == NULL || state == NULL) {
return -1;
}
// 复制状态，并对角度进行归一化
lqr->state = *state;
LQR_ANGLE_NORMALIZE(lqr->state.phi);
LQR_ANGLE_NORMALIZE(lqr->state.theta_ll);
LQR_ANGLE_NORMALIZE(lqr->state.theta_lr);
LQR_ANGLE_NORMALIZE(lqr->state.theta_b);
/* 更新当前状态 */
lqr->current_state = *state;
/* 计算状态误差 */
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state) {
if (lqr == NULL || target_state == NULL) {
return -1;
}
lqr->param.target_state = *target_state;
/* 重新计算状态误差 */
return LQR_CalculateErrorState(lqr);
}
int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) {
if (lqr == NULL || lqr->gain_matrix == NULL) {
return -1;
}
/* 限制腿长范围 */
leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
lqr->current_leg_length = leg_length;
/* 更新与腿长相关的物理参数 */
lqr->param.L = leg_length / 2.0f; /* 摆杆重心到驱动轮轴距离 */
lqr->param.LM = leg_length / 2.0f; /* 摆杆重心到其转轴距离 */
/* 计算摆杆转动惯量 */
float leg_total_length = lqr->param.L + lqr->param.LM;
lqr->param.Ip = lqr->param.mp * (leg_total_length * leg_total_length + 0.05f * 0.05f) / 12.0f;
/* 使用多项式拟合计算当前增益矩阵 */
lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length); /* K11: theta */
lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length); /* K12: d_theta */
lqr->K_matrix[0][2] = LQR_PolynomialCalc(lqr->gain_matrix->k13_coeff, leg_length); /* K13: x */
lqr->K_matrix[0][3] = LQR_PolynomialCalc(lqr->gain_matrix->k14_coeff, leg_length); /* K14: d_x */
lqr->K_matrix[0][4] = LQR_PolynomialCalc(lqr->gain_matrix->k15_coeff, leg_length); /* K15: phi */
lqr->K_matrix[0][5] = LQR_PolynomialCalc(lqr->gain_matrix->k16_coeff, leg_length); /* K16: d_phi */
lqr->K_matrix[1][0] = LQR_PolynomialCalc(lqr->gain_matrix->k21_coeff, leg_length); /* K21: theta */
lqr->K_matrix[1][1] = LQR_PolynomialCalc(lqr->gain_matrix->k22_coeff, leg_length); /* K22: d_theta */
lqr->K_matrix[1][2] = LQR_PolynomialCalc(lqr->gain_matrix->k23_coeff, leg_length); /* K23: x */
lqr->K_matrix[1][3] = LQR_PolynomialCalc(lqr->gain_matrix->k24_coeff, leg_length); /* K24: d_x */
lqr->K_matrix[1][4] = LQR_PolynomialCalc(lqr->gain_matrix->k25_coeff, leg_length); /* K25: phi */
lqr->K_matrix[1][5] = LQR_PolynomialCalc(lqr->gain_matrix->k26_coeff, leg_length); /* K26: d_phi */
return 0;
}
/**
* @brief 设置参考状态
*/
int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference) {
if (lqr == NULL || !lqr->initialized || reference == NULL) {
return -1;
}
// 复制参考状态，并对角度进行归一化
lqr->reference = *reference;
LQR_ANGLE_NORMALIZE(lqr->reference.phi);
LQR_ANGLE_NORMALIZE(lqr->reference.theta_ll);
LQR_ANGLE_NORMALIZE(lqr->reference.theta_lr);
LQR_ANGLE_NORMALIZE(lqr->reference.theta_b);
return 0;
}
/**
* @brief 计算LQR控制输出
*
* 实现状态反馈控制律: u = -K*(x - x_ref)
*/
int8_t LQR_ComputeControl(LQR_Controller_t *lqr) {
int8_t LQR_Control(LQR_Controller_t *lqr) {
if (lqr == NULL || !lqr->initialized) {
return -1;
}
// 计算状态误差向量
float state_error[10];
state_error[0] = LQR_ComputeStateError(lqr->state.s, lqr->reference.s);
state_error[1] = LQR_ComputeStateError(lqr->state.ds, lqr->reference.ds);
state_error[2] = LQR_ComputeStateError(lqr->state.phi, lqr->reference.phi);
state_error[3] = LQR_ComputeStateError(lqr->state.dphi, lqr->reference.dphi);
state_error[4] = LQR_ComputeStateError(lqr->state.theta_ll, lqr->reference.theta_ll);
state_error[5] = LQR_ComputeStateError(lqr->state.dtheta_ll, lqr->reference.dtheta_ll);
state_error[6] = LQR_ComputeStateError(lqr->state.theta_lr, lqr->reference.theta_lr);
state_error[7] = LQR_ComputeStateError(lqr->state.dtheta_lr, lqr->reference.dtheta_lr);
state_error[8] = LQR_ComputeStateError(lqr->state.theta_b, lqr->reference.theta_b);
state_error[9] = LQR_ComputeStateError(lqr->state.dtheta_b, lqr->reference.dtheta_b);
/* LQR控制律: u = -K * x_error
* 其中 u = [T; Tp], x_error = x_current - x_target
*/
// 计算控制输出: u = -K * (x - x_ref)
float control_vector[4];
LQR_MatrixMultiply(lqr->K.K, state_error, control_vector);
/* 计算轮毂力矩T */
lqr->control_output.T = -(
lqr->K_matrix[0][0] * lqr->error_state.theta +
lqr->K_matrix[0][1] * lqr->error_state.d_theta +
lqr->K_matrix[0][2] * lqr->error_state.x +
lqr->K_matrix[0][3] * lqr->error_state.d_x +
lqr->K_matrix[0][4] * lqr->error_state.phi +
lqr->K_matrix[0][5] * lqr->error_state.d_phi
);
// 应用负反馈并限幅
lqr->control.T_wl = LQR_CLAMP(-control_vector[0], -lqr->max_wheel_torque, lqr->max_wheel_torque);
lqr->control.T_wr = LQR_CLAMP(-control_vector[1], -lqr->max_wheel_torque, lqr->max_wheel_torque);
lqr->control.T_bl = LQR_CLAMP(-control_vector[2], -lqr->max_joint_torque, lqr->max_joint_torque);
lqr->control.T_br = LQR_CLAMP(-control_vector[3], -lqr->max_joint_torque, lqr->max_joint_torque);
/* 计算髋关节力矩Tp */
lqr->control_output.Tp = -(
lqr->K_matrix[1][0] * lqr->error_state.theta +
lqr->K_matrix[1][1] * lqr->error_state.d_theta +
lqr->K_matrix[1][2] * lqr->error_state.x +
lqr->K_matrix[1][3] * lqr->error_state.d_x +
lqr->K_matrix[1][4] * lqr->error_state.phi +
lqr->K_matrix[1][5] * lqr->error_state.d_phi
);
return 0;
/* 应用控制限制 */
return LQR_ApplyControlLimits(lqr);
}
/**
* @brief 获取控制输出
*/
int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control) {
if (lqr == NULL || !lqr->initialized || control == NULL) {
int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output) {
if (lqr == NULL || output == NULL) {
return -1;
}
*control = lqr->control;
*output = lqr->control_output;
return 0;
}
/**
* @brief 重置LQR控制器状态
*/
void LQR_Reset(LQR_Controller_t *lqr) {
int8_t LQR_Reset(LQR_Controller_t *lqr) {
if (lqr == NULL) {
return;
}
// 清零状态和控制量
memset(&lqr->state, 0, sizeof(LQR_State_t));
memset(&lqr->reference, 0, sizeof(LQR_State_t));
memset(&lqr->control, 0, sizeof(LQR_Control_t));
memset(&lqr->K, 0, sizeof(LQR_GainMatrix_t));
}
/**
* @brief 从轮腿机器人传感器数据构建LQR状态
*/
int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
float yaw_angle, float yaw_rate,
float left_leg_angle, float left_leg_rate,
float right_leg_angle, float right_leg_rate,
float body_pitch, float body_pitch_rate,
LQR_State_t *state) {
if (state == NULL) {
return -1;
}
state->s = position_x;
state->ds = velocity_x;
state->phi = yaw_angle;
state->dphi = yaw_rate;
state->theta_ll = left_leg_angle;
state->dtheta_ll = left_leg_rate;
state->theta_lr = right_leg_angle;
state->dtheta_lr = right_leg_rate;
state->theta_b = body_pitch;
state->dtheta_b = body_pitch_rate;
/* 清零状态和输出 */
memset(&lqr->current_state, 0, sizeof(LQR_State_t));
memset(&lqr->error_state, 0, sizeof(LQR_State_t));
memset(&lqr->control_output, 0, sizeof(LQR_Input_t));
// 角度归一化
LQR_ANGLE_NORMALIZE(state->phi);
LQR_ANGLE_NORMALIZE(state->theta_ll);
LQR_ANGLE_NORMALIZE(state->theta_lr);
LQR_ANGLE_NORMALIZE(state->theta_b);
/* 重置限幅标志 */
lqr->wheel_torque_limited = false;
lqr->joint_torque_limited = false;
return 0;
}
float LQR_PolynomialCalc(const float *coeff, float leg_length) {
if (coeff == NULL) {
return 0.0f;
}
/* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
float L = leg_length;
float L2 = L * L;
float L3 = L2 * L;
return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 矩阵向量乘法: result = K * state_error
*
* K: 4x10矩阵
* state_error: 10x1向量
* result: 4x1向量
*/
static void LQR_MatrixMultiply(const float K[4][10], const float state_error[10], float result[4]) {
for (int i = 0; i < 4; i++) {
result[i] = 0.0f;
for (int j = 0; j < 10; j++) {
result[i] += K[i][j] * state_error[j];
}
static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr) {
if (lqr == NULL) {
return -1;
}
/* 计算状态误差 */
lqr->error_state.theta = lqr->current_state.theta - lqr->param.target_state.theta;
lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->param.target_state.d_theta;
lqr->error_state.x = lqr->current_state.x - lqr->param.target_state.x;
lqr->error_state.d_x = lqr->current_state.d_x - lqr->param.target_state.d_x;
lqr->error_state.phi = lqr->current_state.phi - lqr->param.target_state.phi;
lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->param.target_state.d_phi;
return 0;
}
/**
* @brief 计算状态误差(考虑角度周期性)
*/
static float LQR_ComputeStateError(float current, float reference) {
float error = current - reference;
static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr) {
if (lqr == NULL) {
return -1;
}
// 对于角度状态，需要考虑周期性
// 这里假设大部分状态都是角度，如果需要区分可以添加参数
while (error > M_PI) error -= 2 * M_PI;
while (error < -M_PI) error += 2 * M_PI;
/* 重置限幅标志 */
lqr->wheel_torque_limited = false;
lqr->joint_torque_limited = false;
return error;
/* 限制轮毂力矩 */
if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
lqr->control_output.T = LQR_LIMIT(lqr->control_output.T,
-lqr->param.max_wheel_torque,
lqr->param.max_wheel_torque);
lqr->wheel_torque_limited = true;
}
/* 限制髋关节力矩 */
if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
-lqr->param.max_joint_torque,
lqr->param.max_joint_torque);
lqr->joint_torque_limited = true;
}
return 0;
}

239
User/component/lqr.h
View File

@ -1,3 +1,8 @@
/*
* LQR控制器
* 用于轮腿平衡机器人的线性二次调节器控制
*/
#pragma once
#ifdef __cplusplus
@ -8,160 +13,192 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "component/user_math.h"
/* Exported constants ------------------------------------------------------- */
#define LQR_STATE_DIM 6 /* 状态向量维度: theta, d_theta, x, d_x, phi, d_phi */
#define LQR_INPUT_DIM 2 /* 输入向量维度: T(轮毂力矩), Tp(髋关节力矩) */
#define LQR_POLY_ORDER 4 /* 多项式拟合阶数 */
/* Exported types ----------------------------------------------------------- */
/**
* @brief LQR控制器状态向量定义
*
* 状态向量维度: 10 x 1
* [s, ds, phi, dphi, theta_ll, dtheta_ll, theta_lr, dtheta_lr, theta_b, dtheta_b]^T
* @brief LQR状态向量
* 状态定义：
* theta: 摆杆与竖直方向夹角 (rad)
* d_theta: 摆杆角速度 (rad/s)
* x: 驱动轮位移 (m)
* d_x: 驱动轮速度 (m/s)
* phi: 机体与水平夹角 (rad)
* d_phi: 机体角速度 (rad/s)
*/
typedef struct {
float s; // 机器人水平方向移动距离 (m)
float ds; // 机器人水平方向移动速度 (m/s)
float phi; // 机器人水平方向移动时yaw偏航角度 (rad)
float dphi; // yaw偏航角速度 (rad/s)
float theta_ll; // 左腿摆杆与竖直方向夹角 (rad)
float dtheta_ll; // 左腿摆杆角速度 (rad/s)
float theta_lr; // 右腿摆杆与竖直方向夹角 (rad)
float dtheta_lr; // 右腿摆杆角速度 (rad/s)
float theta_b; // 机体与水平方向夹角 (rad)
float dtheta_b; // 机体角速度 (rad/s)
float theta; /* 摆杆角度 */
float d_theta; /* 摆杆角速度 */
float x; /* 驱动轮位移 */
float d_x; /* 驱动轮速度 */
float phi; /* 机体俯仰角 */
float d_phi; /* 机体俯仰角速度 */
} LQR_State_t;
/**
* @brief LQR控制器控制输入向量定义
*
* 控制向量维度: 4 x 1
* [T_wl, T_wr, T_bl, T_br]^T
* @brief LQR控制输入向量
*/
typedef struct {
float T_wl; // 左侧驱动轮输出力矩 (N*m)
float T_wr; // 右侧驱动轮输出力矩 (N*m)
float T_bl; // 左侧髋关节输出力矩 (N*m)
float T_br; // 右侧髋关节输出力矩 (N*m)
} LQR_Control_t;
float T; /* 轮毂力矩 (N·m) */
float Tp; /* 髋关节力矩 (N·m) */
} LQR_Input_t;
/**
* @brief LQR增益矩阵K (4x10)
* @brief LQR增益矩阵参数
* K矩阵的每个元素的多项式拟合系数
* K(leg_length) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
*/
typedef struct {
float K[4][10]; // LQR反馈增益矩阵
/* K矩阵第一行(轮毂力矩T对应的增益) */
float k11_coeff[LQR_POLY_ORDER]; /* K(1,1): theta */
float k12_coeff[LQR_POLY_ORDER]; /* K(1,2): d_theta */
float k13_coeff[LQR_POLY_ORDER]; /* K(1,3): x */
float k14_coeff[LQR_POLY_ORDER]; /* K(1,4): d_x */
float k15_coeff[LQR_POLY_ORDER]; /* K(1,5): phi */
float k16_coeff[LQR_POLY_ORDER]; /* K(1,6): d_phi */
/* K矩阵第二行(髋关节力矩Tp对应的增益) */
float k21_coeff[LQR_POLY_ORDER]; /* K(2,1): theta */
float k22_coeff[LQR_POLY_ORDER]; /* K(2,2): d_theta */
float k23_coeff[LQR_POLY_ORDER]; /* K(2,3): x */
float k24_coeff[LQR_POLY_ORDER]; /* K(2,4): d_x */
float k25_coeff[LQR_POLY_ORDER]; /* K(2,5): phi */
float k26_coeff[LQR_POLY_ORDER]; /* K(2,6): d_phi */
} LQR_GainMatrix_t;
/**
* @brief 简化的LQR控制器实例结构体
* @brief LQR控制器参数
*/
typedef struct {
LQR_GainMatrix_t K; // 增益矩阵
LQR_State_t state; // 当前状态
LQR_State_t reference; // 参考状态
LQR_Control_t control; // 控制输出
/* 系统物理参数 */
float R; /* 驱动轮半径 (m) */
float L; /* 摆杆重心到驱动轮轴距离 (m) */
float LM; /* 摆杆重心到其转轴距离 (m) */
float l; /* 机体重心到其转轴距离 (m) */
float mw; /* 驱动轮质量 (kg) */
float mp; /* 摆杆质量 (kg) */
float M; /* 机体质量 (kg) */
float Iw; /* 驱动轮转动惯量 (kg·m²) */
float Ip; /* 摆杆绕质心转动惯量 (kg·m²) */
float IM; /* 机体绕质心转动惯量 (kg·m²) */
float g; /* 重力加速度 (m/s²) */
float max_wheel_torque; // 轮毂电机最大力矩限制 (N*m)
float max_joint_torque; // 关节电机最大力矩限制 (N*m)
bool initialized; // 初始化标志
/* 控制限制 */
float max_wheel_torque; /* 轮毂电机最大力矩 (N·m) */
float max_joint_torque; /* 关节电机最大力矩 (N·m) */
/* 目标状态 */
LQR_State_t target_state; /* 目标状态 */
} LQR_Param_t;
/**
* @brief LQR控制器主结构体
*/
typedef struct {
LQR_Param_t param; /* 控制器参数 */
LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */
LQR_State_t current_state; /* 当前状态 */
LQR_State_t error_state; /* 状态误差 */
LQR_Input_t control_output; /* 控制输出 */
/* 运行时变量 */
float current_leg_length; /* 当前腿长 */
float K_matrix[LQR_INPUT_DIM][LQR_STATE_DIM]; /* 当前增益矩阵 */
bool initialized; /* 初始化标志 */
/* 限幅标志 */
bool wheel_torque_limited; /* 轮毂力矩是否被限幅 */
bool joint_torque_limited; /* 关节力矩是否被限幅 */
} LQR_Controller_t;
/* Exported constants ------------------------------------------------------- */
#define LQR_STATE_DIM (10) // 状态向量维度
#define LQR_CONTROL_DIM (4) // 控制向量维度
/* Exported macros ---------------------------------------------------------- */
/**
* @brief 角度归一化到[-PI, PI]
*/
#define LQR_ANGLE_NORMALIZE(angle) do { \
while((angle) > M_PI) (angle) -= 2*M_PI; \
while((angle) < -M_PI) (angle) += 2*M_PI; \
} while(0)
/**
* @brief 数值限幅
*/
#define LQR_CLAMP(val, min, max) ((val) < (min) ? (min) : ((val) > (max) ? (max) : (val)))
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化LQR控制器
* @param lqr LQR控制器实例
* @param max_wheel_torque 轮毂电机最大力矩 (N*m)
* @param max_joint_torque 关节电机最大力矩 (N*m)
* @return 0:成功, -1:失败
*
* @param lqr LQR控制器结构体指针
* @param max_wheel_torque 轮毂电机最大力矩
* @param max_joint_torque 关节电机最大力矩
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque);
/**
* @brief 设置固定LQR增益矩阵
* @param lqr LQR控制器实例
* @param K 增益矩阵
* @return 0:成功, -1:失败
* @brief 设置LQR增益矩阵参数
*
* @param lqr LQR控制器结构体指针
* @param gain_matrix 增益矩阵参数指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, const LQR_GainMatrix_t *K);
int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix);
/**
* @brief 更新机器人状态
* @param lqr LQR控制器实例
* @brief 更新当前状态
*
* @param lqr LQR控制器结构体指针
* @param state 当前状态
* @return 0:成功, -1:失败
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
/**
* @brief 设置参考状态
* @param lqr LQR控制器实例
* @param reference 参考状态
* @return 0:成功, -1:失败
* @brief 设置目标状态
*
* @param lqr LQR控制器结构体指针
* @param target_state 目标状态
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_SetReference(LQR_Controller_t *lqr, const LQR_State_t *reference);
int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state);
/**
* @brief 计算LQR控制输出
* @param lqr LQR控制器实例
* @return 0:成功, -1:失败
* @brief 根据当前腿长计算增益矩阵
*
* @param lqr LQR控制器结构体指针
* @param leg_length 当前腿长 (m)
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_ComputeControl(LQR_Controller_t *lqr);
int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length);
/**
* @brief 执行LQR控制计算
*
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_Control(LQR_Controller_t *lqr);
/**
* @brief 获取控制输出
* @param lqr LQR控制器实例
* @param control 控制输出
* @return 0:成功, -1:失败
*
* @param lqr LQR控制器结构体指针
* @param output 输出控制指令指针
* @return int8_t 0-成功, 其他-失败
*/
int8_t LQR_GetControl(const LQR_Controller_t *lqr, LQR_Control_t *control);
int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output);
/**
* @brief 重置LQR控制器状态
* @param lqr LQR控制器实例
* @brief 重置LQR控制器
*
* @param lqr LQR控制器结构体指针
* @return int8_t 0-成功, 其他-失败
*/
void LQR_Reset(LQR_Controller_t *lqr);
int8_t LQR_Reset(LQR_Controller_t *lqr);
/**
* @brief 从轮腿机器人传感器数据构建LQR状态
* @param position_x 机体x位置 (m)
* @param velocity_x 机体x速度 (m/s)
* @param yaw_angle yaw角度 (rad)
* @param yaw_rate yaw角速度 (rad/s)
* @param left_leg_angle 左腿角度 (rad)
* @param left_leg_rate 左腿角速度 (rad/s)
* @param right_leg_angle 右腿角度 (rad)
* @param right_leg_rate 右腿角速度 (rad/s)
* @param body_pitch 机体pitch角度 (rad)
* @param body_pitch_rate 机体pitch角速度 (rad/s)
* @param state 输出状态
* @return 0:成功, -1:失败
* @brief 根据多项式系数计算增益值
*
* @param coeff 多项式系数数组
* @param leg_length 腿长
* @return float 增益值
*/
int8_t LQR_BuildStateFromSensors(float position_x, float velocity_x,
float yaw_angle, float yaw_rate,
float left_leg_angle, float left_leg_rate,
float right_leg_angle, float right_leg_rate,
float body_pitch, float body_pitch_rate,
LQR_State_t *state);
float LQR_PolynomialCalc(const float *coeff, float leg_length);
#ifdef __cplusplus
}

71
User/component/lqr_config_example.c Normal file
View File

@ -0,0 +1,71 @@
/*
* LQR控制器配置参数示例
* 基于MATLAB仿真get_k.m和get_k_length.m的结果
*/
#include "lqr.h"
/*
* LQR增益矩阵多项式拟合系数
* 从MATLAB仿真get_k.m得到，对应不同腿长下的最优LQR增益
*
* 多项式形式: K(L) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
* 其中 L 为腿长，范围 [0.1, 0.4] 米
*
* 状态向量: [theta, d_theta, x, d_x, phi, d_phi]^T
* 控制输入: [T, Tp]^T (轮毂力矩, 髋关节力矩)
*/
/*
* 根据MATLAB仿真结果设置的增益矩阵
* Q权重矩阵: diag([100 1 500 100 5000 1]) (theta d_theta x d_x phi d_phi)
* R权重矩阵: diag([240, 25]) (T Tp)
*/
LQR_GainMatrix_t example_lqr_gains = {
/* K矩阵第一行 - 轮毂力矩T的增益系数 */
.k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f}, /* K(1,1): theta */
.k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* K(1,2): d_theta */
.k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f}, /* K(1,3): x */
.k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f}, /* K(1,4): d_x */
.k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f}, /* K(1,5): phi */
.k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f}, /* K(1,6): d_phi */
/* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
.k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f}, /* K(2,1): theta */
.k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* K(2,2): d_theta */
.k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f}, /* K(2,3): x */
.k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f}, /* K(2,4): d_x */
.k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f}, /* K(2,5): phi */
.k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f}, /* K(2,6): d_phi */
};
/*
* 使用示例：
*
* 1. 在底盘参数结构体中设置LQR增益：
* Chassis_Params_t chassis_params = {
* .lqr_param = {
* .max_wheel_torque = 10.0f, // 10 N·m
* .max_joint_torque = 5.0f, // 5 N·m
* },
* .lqr_gains = example_lqr_gains,
* // ... 其他参数
* };
*
* 2. 初始化底盘时会自动设置LQR增益矩阵
*
* 3. 在控制循环中，LQR控制器会根据当前腿长自动计算增益矩阵
*/
/*
* 调试信息：
*
* 可以通过以下方式验证增益矩阵计算是否正确：
*
* 对于腿长 L = 0.25m：
* K(1,1) = -2845.3*0.25^2 + 1899.4*0.25 - 123.8 ≈ -123.8
* K(1,2) = -89.7*0.25^2 + 61.2*0.25 - 4.8 ≈ -4.8
* ... 其他系数类似计算
*
* 这些数值应该与MATLAB仿真中对应腿长的LQR增益一致
*/

163
User/component/lqr_test.c Normal file
View File

@ -0,0 +1,163 @@
/*
* LQR控制器测试程序
*/
#include "lqr.h"
#include <stdio.h>
#include <assert.h>
/* 测试用的增益矩阵 */
extern LQR_GainMatrix_t example_lqr_gains;
/* 测试函数 */
void test_lqr_init(void);
void test_lqr_gain_calculation(void);
void test_lqr_control(void);
void test_lqr_polynomial_calc(void);
int main(void) {
printf("开始LQR控制器测试...\n");
test_lqr_polynomial_calc();
test_lqr_init();
test_lqr_gain_calculation();
test_lqr_control();
printf("所有测试通过！\n");
return 0;
}
void test_lqr_polynomial_calc(void) {
printf("测试多项式计算...\n");
/* 测试多项式计算函数 */
float coeff[4] = {1.0f, 2.0f, 3.0f, 4.0f}; // L^3 + 2*L^2 + 3*L + 4
float L = 0.25f;
float expected = 1.0f * 0.25f * 0.25f * 0.25f +
2.0f * 0.25f * 0.25f +
3.0f * 0.25f +
4.0f;
float result = LQR_PolynomialCalc(coeff, L);
printf(" 多项式计算: L=%.2f, 期望=%.4f, 实际=%.4f\n", L, expected, result);
assert(fabsf(result - expected) < 1e-4f);
printf(" 多项式计算测试通过\n");
}
void test_lqr_init(void) {
printf("测试LQR初始化...\n");
LQR_Controller_t lqr;
int8_t result = LQR_Init(&lqr, 10.0f, 5.0f);
assert(result == 0);
assert(lqr.initialized == true);
assert(lqr.param.max_wheel_torque == 10.0f);
assert(lqr.param.max_joint_torque == 5.0f);
assert(lqr.current_leg_length == 0.25f);
printf(" LQR初始化测试通过\n");
}
void test_lqr_gain_calculation(void) {
printf("测试增益矩阵计算...\n");
LQR_Controller_t lqr;
LQR_Init(&lqr, 10.0f, 5.0f);
LQR_SetGainMatrix(&lqr, &example_lqr_gains);
/* 测试不同腿长的增益计算 */
float test_lengths[] = {0.15f, 0.25f, 0.35f};
int num_tests = sizeof(test_lengths) / sizeof(test_lengths[0]);
for (int i = 0; i < num_tests; i++) {
float L = test_lengths[i];
int8_t result = LQR_CalculateGainMatrix(&lqr, L);
assert(result == 0);
assert(lqr.current_leg_length == L);
/* 检查增益矩阵是否合理 */
for (int j = 0; j < LQR_INPUT_DIM; j++) {
for (int k = 0; k < LQR_STATE_DIM; k++) {
assert(!isnan(lqr.K_matrix[j][k]));
assert(!isinf(lqr.K_matrix[j][k]));
}
}
printf(" 腿长%.2fm的增益矩阵计算通过\n", L);
printf(" K11=%.2f, K12=%.2f, K13=%.2f\n",
lqr.K_matrix[0][0], lqr.K_matrix[0][1], lqr.K_matrix[0][2]);
}
printf(" 增益矩阵计算测试通过\n");
}
void test_lqr_control(void) {
printf("测试LQR控制计算...\n");
LQR_Controller_t lqr;
LQR_Init(&lqr, 10.0f, 5.0f);
LQR_SetGainMatrix(&lqr, &example_lqr_gains);
/* 设置测试状态 */
LQR_State_t test_state = {
.theta = 0.1f, /* 摆杆角度 */
.d_theta = 0.05f, /* 摆杆角速度 */
.x = 0.02f, /* 位置 */
.d_x = 0.1f, /* 速度 */
.phi = 0.05f, /* 俯仰角 */
.d_phi = 0.02f /* 俯仰角速度 */
};
LQR_State_t target_state = {0}; /* 平衡状态 */
/* 更新状态 */
int8_t result = LQR_UpdateState(&lqr, &test_state);
assert(result == 0);
result = LQR_SetTargetState(&lqr, &target_state);
assert(result == 0);
/* 执行控制计算 */
result = LQR_Control(&lqr);
assert(result == 0);
/* 获取控制输出 */
LQR_Input_t output;
result = LQR_GetOutput(&lqr, &output);
assert(result == 0);
/* 检查输出是否合理 */
assert(!isnan(output.T));
assert(!isnan(output.Tp));
assert(!isinf(output.T));
assert(!isinf(output.Tp));
assert(fabsf(output.T) <= lqr.param.max_wheel_torque);
assert(fabsf(output.Tp) <= lqr.param.max_joint_torque);
printf(" 控制输出: T=%.3f N·m, Tp=%.3f N·m\n", output.T, output.Tp);
printf(" LQR控制计算测试通过\n");
}
/* 如果没有math.h中的函数，提供简单实现 */
#ifndef __has_include
#define __has_include(x) 0
#endif
#if !__has_include(<math.h>)
float fabsf(float x) {
return x < 0 ? -x : x;
}
int isnan(float x) {
return x != x;
}
int isinf(float x) {
return (x == x) && ((x - x) != 0);
}
#endif

7
User/component/vmc.c
View File

@ -80,6 +80,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
if (vmc == NULL || !vmc->initialized) {
return -1;
}
body_pitch = -body_pitch; // 机体俯仰角取反
VMC_Leg_t *leg = &vmc->leg;
@ -111,7 +112,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
// 计算等效摆动杆角度(相对于机体坐标系)
leg->alpha = VMC_PI_2 - leg->phi0;
leg->theta = -(VMC_PI_2 + body_pitch - leg->phi0);
leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0);
// 角度归一化
VMC_ANGLE_NORMALIZE(leg->theta);
@ -140,8 +141,8 @@ int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
}
// 保存虚拟力和力矩
vmc->leg.F_virtual = -F_virtual;
vmc->leg.T_virtual = T_virtual;
vmc->leg.F_virtual = F_virtual;
vmc->leg.T_virtual = -T_virtual;
// 计算雅可比矩阵
if (VMC_ComputeJacobian(vmc) != 0) {

393
User/module/balance_chassis.c
View File

@ -3,28 +3,51 @@
#include "bsp/can.h"
#include "component/user_math.h"
#include "component/kinematics.h"
#include "component/limiter.h"
#include <math.h>
#include <string.h>
float fn=0.0f;
float tp=0.0f;
float t1=0.0f;
float t2=0.0f;
/**
* @brief
* @param c
* @return
*/
static int8_t Chassis_MotorEnable(Chassis_t *c) {
if (c == NULL) return CHASSIS_ERR_NULL;
for (int i = 0; i < 2; i++) {
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
}
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Enable(&c->param->joint_motors[i]);
}
return CHASSIS_OK;
}
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
MOTOR_LK_MotorOn(&c->param->wheel_motors[0]);
MOTOR_LK_MotorOn(&c->param->wheel_motors[1]);
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Enable(&c->param->joint_motors[i]);
}
Chassis_MotorEnable(c);
// 清空pid积分
PID_Reset(&c->pid.leglength[0]);
PID_Reset(&c->pid.leglength[1]);
PID_Reset(&c->pid.yaw);
PID_Reset(&c->pid.roll);
PID_Reset(&c->pid.tp_pid[0]);
PID_Reset(&c->pid.tp_pid[1]);
c->yaw_control.target_yaw = c->feedback.imu.euler.yaw;
//清空位移
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
LQR_Reset(&c->lqr[0]);
LQR_Reset(&c->lqr[1]);
c->mode = mode;
c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
@ -56,92 +79,6 @@ static void Chassis_UpdateChassisState(Chassis_t *c) {
c->chassis_state.position_x += c->chassis_state.velocity_x * c->dt;
}
/* 执行LQR控制 */
static int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) return -1;
// 构建当前状态
LQR_State_t current_state;
float left_leg_length, left_leg_angle, left_leg_d_length, left_leg_d_angle;
float right_leg_length, right_leg_angle, right_leg_d_length, right_leg_d_angle;
// 获取等效摆动杆状态
VMC_GetVirtualLegState(&c->vmc_[0], &left_leg_length, &left_leg_angle, &left_leg_d_length, &left_leg_d_angle);
VMC_GetVirtualLegState(&c->vmc_[1], &right_leg_length, &right_leg_angle, &right_leg_d_length, &right_leg_d_angle);
LQR_BuildStateFromSensors(
c->chassis_state.position_x,
c->chassis_state.velocity_x,
c->feedback.imu.euler.yaw,
c->feedback.imu.gyro.z,
left_leg_angle,
left_leg_d_angle,
right_leg_angle,
right_leg_d_angle,
c->feedback.imu.euler.pit,
c->feedback.imu.gyro.y,
&current_state
);
// 设置参考状态
LQR_State_t reference_state = {0};
reference_state.s = 0.0f; // 期望位移设为0（相对平衡位置）
reference_state.ds = c_cmd->move_vec.vx; // 期望速度
reference_state.phi = 0.0f; // 期望yaw角度
reference_state.dphi = c_cmd->move_vec.wz; // 期望yaw角速度
// 其他状态保持为0（平衡状态）
// 更新LQR控制器状态
LQR_UpdateState(&c->lqr, &current_state);
LQR_SetReference(&c->lqr, &reference_state);
// 计算控制输出
if (LQR_ComputeControl(&c->lqr) != 0) {
return -1;
}
// 获取控制输出
LQR_Control_t lqr_output;
LQR_GetControl(&c->lqr, &lqr_output);
// 分配力矩到电机
// 轮毂电机 (考虑减速比)
// float wheel_gear_ratio = 19.2f;
// MOTOR_LK_SetTorque(&c->param->wheel_motors[0], lqr_output.T_wl / wheel_gear_ratio);
// MOTOR_LK_SetTorque(&c->param->wheel_motors[1], lqr_output.T_wr / wheel_gear_ratio);
c->output.wheel[0] = lqr_output.T_wl/2.5; // 轮子电机输出
c->output.wheel[1] = lqr_output.T_wr/2.5;
// 通过VMC将虚拟力转换为关节力矩
// 左腿
float F_virtual_left = lqr_output.T_bl; // 简化映射，实际需要更复杂的转换
// float T_virtual_left = 0.0f;
float T_virtual_left = lqr_output.T_wl; // 左腿虚拟摆动力矩
VMC_InverseSolve(&c->vmc_[0], F_virtual_left, T_virtual_left);
float tau_left_front, tau_left_rear;
VMC_GetJointTorques(&c->vmc_[0], &tau_left_front, &tau_left_rear);
// 右腿
float F_virtual_right = lqr_output.T_br;
// float T_virtual_right = 0.0f;
float T_virtual_right = lqr_output.T_wr; // 右腿虚拟摆动力矩
VMC_InverseSolve(&c->vmc_[1], F_virtual_right, T_virtual_right);
float tau_right_front, tau_right_rear;
VMC_GetJointTorques(&c->vmc_[1], &tau_right_front, &tau_right_rear);
// 输出到关节电机
// MOTOR_LZ_SetTorque(&c->param->joint_motors[0], tau_left_rear); // 左后
// MOTOR_LZ_SetTorque(&c->param->joint_motors[1], tau_left_front); // 左前
// MOTOR_LZ_SetTorque(&c->param->joint_motors[2], tau_right_front);// 右前
// MOTOR_LZ_SetTorque(&c->param->joint_motors[3], tau_right_rear); // 右后
c->output.joint[0].torque = tau_left_rear;
c->output.joint[1].torque = tau_left_front;
c->output.joint[2].torque = tau_right_front;
c->output.joint[3].torque = tau_right_rear;
return 0;
}
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
if (c == NULL || param == NULL || target_freq <= 0.0f) {
@ -165,15 +102,30 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
/*初始化VMC控制器*/
VMC_Init(&c->vmc_[0], &param->vmc_param[0], target_freq);
VMC_Init(&c->vmc_[1], &param->vmc_param[1], target_freq);
/*初始化pid*/
PID_Init(&c->pid.leglength[0], KPID_MODE_CALC_D, target_freq, &param->leglength);
PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, &param->leglength);
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.tp_pid[0], KPID_MODE_CALC_D, target_freq, &param->tp_pid);
PID_Init(&c->pid.tp_pid[1], KPID_MODE_CALC_D, target_freq, &param->tp_pid);
/*初始化LQR控制器*/
LQR_Init(&c->lqr, param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr, &param->lqr_gains);
LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr[0], &param->lqr_gains);
LQR_Init(&c->lqr[1], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
LQR_SetGainMatrix(&c->lqr[1], &param->lqr_gains);
/*初始化机体状态*/
c->chassis_state.position_x = 0.0f;
c->chassis_state.velocity_x = 0.0f;
c->chassis_state.last_velocity_x = 0.0f;
/*初始化yaw控制状态*/
c->yaw_control.target_yaw = 0.0f;
c->yaw_control.current_yaw = 0.0f;
c->yaw_control.yaw_force = 0.0f;
return CHASSIS_OK;
}
@ -195,7 +147,11 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c){
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
if (joint_motor != NULL) {
c->feedback.joint[i] = joint_motor->motor.feedback;
c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整
// c->feedback.joint[i].rotor_abs_angle = joint_motor->motor.feedback.rotor_abs_angle - M_PI; // 机械零点调整
if (c->feedback.joint[i].rotor_abs_angle > M_PI ){
c->feedback.joint[i].rotor_abs_angle -= M_2PI;
}
c->feedback.joint[i].rotor_abs_angle = - c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
}
}
@ -251,15 +207,15 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
// VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
// VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
// MOTOR_LZ_MotionControl(&c->param->joint_motors[0], &(MOTOR_LZ_MotionParam_t){.torque = t1});
// Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
if (Chassis_LQRControl(c, c_cmd) != 0) {
// LQR控制失败，切换到安全模式
return CHASSIS_ERR;
}
break;
@ -289,31 +245,34 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_InverseSolve(&c->vmc_[0], fn, tp);
VMC_GetJointTorques(&c->vmc_[0], &t1, &t2);
// VMC_InverseSolve(&c->vmc_[1], fn, tp);
// VMC_GetJointTorques(&c->vmc_[1], &t1, &t2);
c->output.joint[0].torque = t1;
c->output.joint[1].torque = t2;
// Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
// c->output.joint[3].torque = t1;
// c->output.joint[2].torque = t2;
Chassis_LQRControl(c, c_cmd); // 即使在放松模式下也执行LQR以保持状态更新
// c->output.wheel[0] = 0.2f;
// c->output.wheel[1] = 0.2f;
Chassis_Output(c); // 统一输出
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
// 轮腿平衡模式，使用LQR控制
// 轮腿平衡模式，使用LQR控制和PID腿长控制
// // 更新VMC正解算
// VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
// c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
// VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
// c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
// 更新VMC正解算
VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
VMC_ForwardSolve(&c->vmc_[1], c->feedback.joint[3].rotor_abs_angle, c->feedback.joint[2].rotor_abs_angle,
c->feedback.imu.euler.pit, c->feedback.imu.gyro.y);
// // 执行LQR控制
// if (Chassis_LQRControl(c, c_cmd) != 0) {
// // LQR控制失败，切换到安全模式
// return CHASSIS_ERR;
// }
// 执行LQR控制（包含PID腿长控制）
if (Chassis_LQRControl(c, c_cmd) != 0) {
// LQR控制失败，切换到安全模式
return CHASSIS_ERR;
}
Chassis_Output(c); // 统一输出
break;
default:
@ -328,12 +287,194 @@ void Chassis_Output(Chassis_t *c) {
for (int i = 0; i < 4; i++) {
MOTOR_LZ_MotionParam_t param = {0};
param.torque = c->output.joint[i].torque;
MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &param);
}
for (int i = 0; i < 2; i++) {
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
}
// 这个函数已经在各个模式中直接调用了电机输出函数
// 如果需要统一输出，可以在这里实现
// 现在的设计是在控制逻辑中直接输出，所以这里留空
}
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
if (c == NULL || c_cmd == NULL) {
return CHASSIS_ERR_NULL;
}
/* 参考C++版本实现的LQR控制 */
/* 地面接触检测（参考C++版本） */
float leg_fn[2];
bool onground_flag[2];
// 暂时关闭离地监测，强制设置为着地状态
leg_fn[0] = VMC_GroundContactDetection(&c->vmc_[0]);
leg_fn[1] = VMC_GroundContactDetection(&c->vmc_[1]);
onground_flag[0] = true; // 强制设置左腿着地
onground_flag[1] = true; // 强制设置右腿着地
/* 获取VMC状态（等效摆杆参数） */
float leg_L0[2], leg_theta[2], leg_d_theta[2];
VMC_GetVirtualLegState(&c->vmc_[0], &leg_L0[0], &leg_theta[0], NULL, &leg_d_theta[0]);
VMC_GetVirtualLegState(&c->vmc_[1], &leg_L0[1], &leg_theta[1], NULL, &leg_d_theta[1]);
/* 运动参数（参考C++版本的状态估计） */
static float xhat = 0.0f, x_dot_hat = 0.0f;
static float target_x = 0.0f, target_dot_x = 0.0f;
// 简化的速度估计（后续可以改进为C++版本的复杂滤波）
x_dot_hat = c->chassis_state.velocity_x;
xhat = c->chassis_state.position_x;
// 目标设定
target_dot_x = c_cmd->move_vec.vx/3.0f;
target_dot_x = SpeedLimit_TargetSpeed(1000.0f, c->chassis_state.velocity_x, target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
target_x += target_dot_x * c->dt;
/* 分别计算左右腿的LQR控制 */
float Tw[2] = {0.0f, 0.0f}; // 轮毂力矩
float Tp[2] = {0.0f, 0.0f}; // 髋关节力矩
for (int leg = 0; leg < 2; leg++) {
/* 构建当前状态 */
LQR_State_t current_state = {0};
current_state.theta = leg_theta[leg];
current_state.d_theta = leg_d_theta[leg];
current_state.x = xhat;
current_state.d_x = x_dot_hat;
current_state.phi = -c->feedback.imu.euler.pit;
current_state.d_phi = -c->feedback.imu.gyro.y;
/* 构建目标状态 */
LQR_State_t target_state = {0};
target_state.theta = 0.0f; // 目标摆杆角度
target_state.d_theta = 0.0f; // 目标摆杆角速度
// target_state.x = 0; // 目标位置
// target_state.d_x = 0.0f; // 目标速度
target_state.phi = 0.0f; // 目标俯仰角
target_state.d_phi = 0.0f; // 目标俯仰角速度
// target_state.theta = -0.8845f * leg_L0[leg] + 0.40663f; // 目标摆杆角度
// target_state.d_theta = 0.0f; // 目标摆杆角速度
target_state.x = target_x; // 目标位置
target_state.d_x = target_dot_x; // 目标速度
// target_state.phi = 0.16f; // 目标俯仰角
// target_state.d_phi = 0.0f; // 目标俯仰角速度
/* 根据当前腿长更新增益矩阵 */
LQR_CalculateGainMatrix(&c->lqr[leg], leg_L0[leg]);
/* 更新LQR状态 */
LQR_SetTargetState(&c->lqr[leg], &target_state);
LQR_UpdateState(&c->lqr[leg], &current_state);
if (onground_flag[leg]) {
/* 接地状态：使用LQR控制器计算输出 */
if (LQR_Control(&c->lqr[leg]) == 0) {
LQR_Input_t lqr_output = {0};
if (LQR_GetOutput(&c->lqr[leg], &lqr_output) == 0) {
Tw[leg] = lqr_output.T;
Tp[leg] = lqr_output.Tp;
// Tw[leg] = 0.0f; // 暂时屏蔽轮毂力矩输出
// Tp[leg] = -2.5f; // 暂时屏蔽腿部力矩输出
} else {
Tw[leg] = 0.0f;
Tp[leg] = 0.0f;
}
} else {
Tw[leg] = 0.0f;
Tp[leg] = 0.0f;
}
} else {
/* 离地状态：简化控制，只控制腿部摆动 */
Tw[leg] = 0.0f;
// 只控制摆杆角度
float theta_error = current_state.theta - target_state.theta;
float d_theta_error = current_state.d_theta - target_state.d_theta;
Tp[leg] = -(c->lqr[leg].K_matrix[1][0] * theta_error + c->lqr[leg].K_matrix[1][1] * d_theta_error);
}
}
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.002f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt);
/* 轮毂力矩输出（参考C++版本的减速比） */
c->output.wheel[0] = Tw[0] / 5.0f - c->yaw_control.yaw_force;
c->output.wheel[1] = Tw[1] / 5.0f + c->yaw_control.yaw_force;
/* 腿长控制和VMC逆解算（使用PID控制） */
float virtual_force[2];
float target_L0[2];
float leg_d_length[2]; // 腿长变化率
/* 横滚角PID补偿计算 */
float roll_compensation = PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol, c->feedback.imu.gyro.x, c->dt);
// 目标腿长设定
target_L0[0] = 0.15f + c_cmd->height * 0.01f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
target_L0[1] = 0.15f + c_cmd->height * 0.01f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿
// 获取腿长变化率
VMC_GetVirtualLegState(&c->vmc_[0], NULL, NULL, &leg_d_length[0], NULL);
VMC_GetVirtualLegState(&c->vmc_[1], NULL, NULL, &leg_d_length[1], NULL);
/* 左右腿摆角相互补偿（参考C++版本的Delta_Tp机制） */
float Delta_Tp[2];
// 使用tp_pid进行左右腿摆角同步控制
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
Delta_Tp[0] = -leg_L0[0] * 10.0f * PID_Calc(&c->pid.tp_pid[0], leg_theta[1], leg_theta[0], leg_d_theta[0], c->dt);
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
Delta_Tp[1] = leg_L0[1] * 10.0f * PID_Calc(&c->pid.tp_pid[1], leg_theta[0], leg_theta[1], leg_d_theta[1], c->dt);
for (int leg = 0; leg < 2; leg++) {
// 使用PID进行腿长控制
float pid_output = PID_Calc(&c->pid.leglength[leg], target_L0[leg], leg_L0[leg], leg_d_length[leg], c->dt);
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力
virtual_force[leg] = (Tp[leg] + Delta_Tp[leg]) * sinf(leg_theta[leg]) +
pid_output + 20.0f;
// + // PID腿长控制输出
// 45.0f; // 基础支撑力
// VMC逆解算（包含摆角补偿）
// virtual_force[leg] = 0.0f; // 暂时屏蔽虚拟力输出，避免VMC逆解算失败
// Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
// Delta_Tp[leg] = 0.0f; // 暂时屏蔽腿部力矩输出，避免VMC逆解算失败
if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg] + Delta_Tp[leg]) == 0) {
if (leg == 0) {
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
} else {
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
}
} else {
// VMC失败，设为0力矩
if (leg == 0) {
c->output.joint[0].torque = 0.0f;
c->output.joint[1].torque = 0.0f;
} else {
c->output.joint[2].torque = 0.0f;
c->output.joint[3].torque = 0.0f;
}
}
}
/* 安全限制 */
for (int i = 0; i < 2; i++) {
if (fabsf(c->output.wheel[i]) > 0.8f) {
c->output.wheel[i] = (c->output.wheel[i] > 0) ? 0.8f : -0.8f;
}
}
for (int i = 0; i < 4; i++) {
if (fabsf(c->output.joint[i].torque) > 20.0f) {
c->output.joint[i].torque = (c->output.joint[i].torque > 0) ? 20.0f : -20.0f;
}
}
return CHASSIS_OK;
}

39
User/module/balance_chassis.h
View File

@ -36,7 +36,7 @@ extern "C" {
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_RECOVER,
CHASSIS_MODE_RECOVER, /* 复位模式 */
CHASSIS_MODE_WHELL_BALANCE, /* 平衡模式，底盘自我平衡 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
} Chassis_Mode_t;
@ -70,7 +70,11 @@ typedef struct {
typedef struct {
VMC_Param_t vmc_param[2]; /* VMC参数 */
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵 */
KPID_Params_t leglength; /* 腿长PID控制参数，用于控制虚拟腿长度 */
KPID_Params_t yaw; /* yaw轴PID控制参数，用于控制底盘朝向 */
KPID_Params_t roll; /* roll轴PID控制参数，用于横滚角补偿 */
KPID_Params_t tp_pid; /* 摆力矩PID控制参数，用于控制摆力矩 */
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
@ -82,6 +86,8 @@ typedef struct {
float max_wheel_torque; /* 轮毂电机最大力矩限制 */
float max_joint_torque; /* 关节电机最大力矩限制 */
} lqr_param;
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
/* 低通滤波器截止频率 */
struct {
@ -110,7 +116,7 @@ typedef struct {
Chassis_Output_t output;
VMC_t vmc_[2]; /* 两条腿的VMC */
LQR_Controller_t lqr; /* LQR控制器 */
LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */
int8_t state;
@ -124,6 +130,13 @@ typedef struct {
float last_velocity_x; /* 上一次速度用于数值微分 */
} chassis_state;
/* yaw控制相关 */
struct {
float target_yaw; /* 目标yaw角度 */
float current_yaw; /* 当前yaw角度 */
float yaw_force; /* yaw轴控制力矩 */
} yaw_control;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
@ -137,6 +150,10 @@ typedef struct {
KPID_t right_wheel; /* 右轮PID */
KPID_t follow; /* 跟随云台用的PID */
KPID_t balance; /* 平衡用的PID */
KPID_t yaw; /* yaw轴控制PID */
KPID_t roll; /* 横滚角控制PID */
KPID_t tp_pid[2];
KPID_t leglength[2]; /* 两条腿的腿长PID */
} pid;
/* 滤波器 */
@ -169,6 +186,12 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
int8_t Chassis_UpdateFeedback(Chassis_t *c);
/**
* @brief 更新IMU数据
* @param c 包含底盘数据的结构体
* @param imu IMU数据
* @return
*/
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
/**
* \brief 运行底盘控制逻辑
@ -180,6 +203,16 @@ int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu);
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief LQR控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
*
* \return 函数运行结果
*/
int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/**
* \brief 底盘输出值

0
User/module/chassis.c
View File

180
User/module/chassis.h
View File

@ -1,180 +0,0 @@
/*
* 底盘模组
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "component\cmd.h"
#include "component\filter.h"
#include "component\mixer.h"
#include "component\pid.h"
#include "device\can.h"
#include "device\referee.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时发现了其他错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 底盘类型（底盘的物理设计） */
typedef enum {
CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */
CHASSIS_TYPE_PARLFIX4, /* 平行摆设的四个驱动轮 */
CHASSIS_TYPE_PARLFIX2, /* 平行摆设的两个驱动轮 */
CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆设的四个全向轮 */
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */
CHASSIS_TYPE_DRONE, /* 底盘为无人机 */
CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */
} Chassis_Type_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */
KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
/* 电机反装 应该和云台设置相同*/
struct {
bool yaw;
} reverse;
} Chassis_Params_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
uint32_t lask_wakeup;
float dt;
const Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
AHRS_Eulr_t *mech_zero;
/* 模块通用 */
CMD_ChassisMode_t mode; /* 底盘模式 */
/* 底盘设计 */
int8_t num_wheel; /* 底盘轮子数量 */
Mixer_t mixer; /* 混合器，移动向量->电机目标值 */
MoveVector_t move_vec; /* 底盘实际的运动向量 */
/* 反馈信息 */
struct {
float gimbal_yaw_encoder; /* 云台Yaw轴编码器角度 */
float *motor_rpm; /* 电机转速的动态数组，单位：RPM */
float *motor_current; /* 转矩电流 单位：A */
} feedback;
float wz_multi; /* 小陀螺模式旋转方向 */
/* PID计算的目标值 */
struct {
float *motor_rpm; /* 电机转速的动态数组，单位：RPM */
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t *motor; /* 控制轮子电机用的PID的动态数组 */
KPID_t follow; /* 跟随云台用的PID */
} pid;
/* 滤波器 */
struct {
LowPassFilter2p_t *in; /* 反馈值滤波器 */
LowPassFilter2p_t *out; /* 输出值滤波器 */
} filter;
float *out; /* 电机最终的输出值的动态数组 */
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化底盘
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
AHRS_Eulr_t *mech_zero, float target_freq);
/**
* \brief 更新底盘的反馈信息
*
* \param c 包含底盘数据的结构体
* \param can CAN设备结构体
*
* \return 函数运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
* \param dt_sec 两次调用的时间间隔
*
* \return 函数运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd,
uint32_t now);
/**
* @brief 底盘功率限制
*
* @param c 底盘数据
* @param cap 电容数据
* @param ref 裁判系统数据
* @return 函数运行结果
*/
int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
const Referee_ForChassis_t *ref);
/**
* \brief 复制底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_DumpOutput(Chassis_t *c, CAN_ChassisOutput_t *out);
/**
* \brief 清空Chassis输出数据
*
* \param out CAN设备底盘输出结构体
*/
void Chassis_ResetOutput(CAN_ChassisOutput_t *out);
/**
* @brief 导出底盘数据
*
* @param chassis 底盘数据结构体
* @param ui UI数据结构体
*/
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
#ifdef __cplusplus
}
#endif

130
User/module/config.c
View File

@ -68,6 +68,49 @@ Config_RobotParam_t robot_config = {
},
.chassis_param = {
.leglength={
.k = 20.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
.out_limit = 100.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.yaw={
.k=1.0f,
.p=1.0f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=1.0f,
.d_cutoff_freq=30.0f,
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
},
.roll={
.k=1.0f,
.p=5.0f, /* 横滚角比例系数 */
.i=0.0f, /* 横滚角积分系数 */
.d=0.2f, /* 横滚角微分系数 */
.i_limit=0.0f, /* 积分限幅 */
.out_limit=0.05f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */
},
.tp_pid={
.k=1.0f,
.p=1.0f, /* 俯仰角比例系数 */
.i=0.0f, /* 俯仰角积分系数 */
.d=0.0f, /* 俯仰角微分系数 */
.i_limit=0.0f, /* 积分限幅 */
.out_limit=1.0f, /* 输出限幅，腿长差值限制 */
.d_cutoff_freq=30.0f, /* 微分截止频率 */
.range=-1.0f, /* 不使用循环误差处理 */
},
.low_pass_cutoff_freq = {
.in = 30.0f,
.out = 30.0f,
@ -78,7 +121,7 @@ Config_RobotParam_t robot_config = {
.motor_id = 124,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 左膝关节
@ -86,7 +129,7 @@ Config_RobotParam_t robot_config = {
.motor_id = 125,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右膝关节
@ -94,7 +137,7 @@ Config_RobotParam_t robot_config = {
.motor_id = 126,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
{ // 右髋关节
@ -102,7 +145,7 @@ Config_RobotParam_t robot_config = {
.motor_id = 127,
.host_id = 130,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
},
@ -121,29 +164,76 @@ Config_RobotParam_t robot_config = {
},
},
.mech_zero_yaw = 0.0f,
.vmc_param = {
{ // 左腿
.leg_1 = 0.206f, // 前大腿长度 (m)
.leg_2 = 0.258f, // 前小腿长度 (m)
.leg_3 = 0.206f, // 后小腿长度 (m)
.leg_4 = 0.258f, // 后大腿长度 (m)
.hip_length = 0.0f // 髋宽 (m)
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
.leg_3 = 0.258f, // 前膝连杆长度 (L3) (m)
.leg_4 = 0.215f, // 前髋连杆长度 (L4) (m)
.hip_length = 0.0f // 髋宽 (L5) (m)
},
{ // 右腿
.leg_1 = 0.206f, // 前大腿长度 (m)
.leg_2 = 0.258f, // 前小腿长度 (m)
.leg_3 = 0.206f, // 后小腿长度 (m)
.leg_4 = 0.258f, // 后大腿长度 (m)
.hip_length = 0.0f // 髋宽 (m)
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
.leg_3 = 0.258f, // 前膝连杆长度 (L3) (m)
.leg_4 = 0.215f, // 前髋连杆长度 (L4) (m)
.hip_length = 0.0f // 髋宽 (L5) (m)
}
},
.lqr_gains ={
.K = {
{ -1.3677, -12.022, 4.0676, -2.6185, -66.132, -4.2516, -1.4083, -0.051404, -57.561, -5.3641 },
{ -1.3677, -12.022, -4.0676, 2.6185, -1.4083, -0.051404, -66.132, -4.2516, -57.561, -5.3641 },
{ 0.14689, 1.2865, -63.224, -12.495, 6.2265, -0.13959, 1.2635, 0.48938, -78.822, -5.121 },
{ 0.14689, 1.2865, 63.224, 12.495, 1.2635, 0.48938, 6.2265, -0.13959, -78.822, -5.121 }
}
// .k11_coeff = { -61.932040681074966f, 70.963671642086396f, -37.730841182566571f, -0.296475458388679f }, // theta
// .k12_coeff = { -0.586710094600108f, 0.886736272521581f, -3.626144273475104f, 0.057861910518974f }, // d_theta
// .k13_coeff = { -17.297031110481019f, 16.286794934166178f, -5.176451154477850f, -0.867260018374823f }, // x
// .k14_coeff = { -14.893387150006664f, 14.357020815277332f, -5.244645181873441f, -0.869862096507486f}, // d_x
// .k15_coeff = { -75.327876471378886f, 79.786699741548944f, -31.183500053811208f, 5.450635661115236f}, // phi
// .k16_coeff = { -3.572234723237025f, 4.164905011076312f, -1.874828497771750f, 0.477913222933688f}, // d_phi
// .k21_coeff = { 9.327090608559319f, 1.362675928111105f, -8.092777598567881f, 4.351387652359104f}, // theta
// .k22_coeff = { 3.872517778351810f, -3.344077414726880f, 0.589693555828904f, 0.310036629174646f}, // d_theta
// .k23_coeff = { -71.615766251649134f, 74.748309711530837f, -28.370490124006626f, 4.483586941100197f }, // x
// .k24_coeff = { -68.751866288568962f, 71.204785013044102f, -26.812636604518396f, 4.238479323700952f }, // d_x
// .k25_coeff = { 205.6887659080132f, -195.1219729060621f, 62.890188701113303f, 7.452313695653162f }, // phi
// .k26_coeff = { 16.162999842656344f, -15.926932704437410f, 5.474613395300429f, -0.002856083635449f }, // d_phi
// .k11_coeff = {-143.1550,156.1754,-98.5282,-11.3781}, // theta
// .k12_coeff = {8.3196,-12.4161,-8.3805,-1.6368}, // d_theta
// .k13_coeff = {-69.6189,68.5619,-23.3079,-4.1736}, // x
// .k14_coeff = {-58.4944,58.2204,-22.8021,-5.2361}, // d_x
// .k15_coeff = {-29.6753,40.9947,-20.1188,2.5142}, // phi
// .k16_coeff = {-13.1787,15.8361,-7.4061,1.1193}, // d_phi
// .k21_coeff = {-76.5141,124.3258,-73.4908,22.0942}, // theta
// .k22_coeff = {-9.1845,12.7284,-5.8169,2.8659}, // d_theta
// .k23_coeff = {-157.6244,179.9415,-77.2161,14.9075}, // x
// .k24_coeff = {-180.5666,202.7656,-85.2869,16.4847}, // d_x
// .k25_coeff = {14.3596,-14.2125,4.1210,24.1729}, // phi
// .k26_coeff = {20.5751,-19.8014,6.3128,2.8044}, // d_phi
.k11_coeff = { -2.046396270532116e+02f, 2.283572275397276e+02f, -99.051642997946473f, 2.549835715107331f }, // theta
.k12_coeff = { 0.689999868157742f, 2.004516582917084f, -5.904779142191341f, 0.331840642644740f }, // d_theta
.k13_coeff = { -59.058694050901643f, 59.363082825119605f, -20.603451414220757f, 1.137708603718630f }, // x
.k14_coeff = { -64.283929532305166f, 65.138737687498519f, -23.323482861600581f, 1.257945614978053f }, // d_x
.k15_coeff = { -15.125353856795936f, 34.329224759247211f, -22.500683150450474f, 5.036897782323629f }, // phi
.k16_coeff = { 2.707768649521343f, 0.390393176362524f, -2.018231845635338f, 0.697604163191230f }, // d_phi
.k21_coeff = { 4.135329288854244e+02f, -3.173913574866715e+02f, 50.321199991176265f, 10.217217753280829f }, // theta
.k22_coeff = { 48.042446261620519f, -45.292268634116219f, 13.273044296221686f, 0.006982339078710f }, // d_theta
.k23_coeff = { 14.015246608117772f, 10.813301135732024f, -18.216050987625373f, 6.078912501009629f }, // x
.k24_coeff = { 21.698411755946285f, 5.621435092936538f, -18.016608013978576f, 6.611542756343175f }, // d_x
.k25_coeff = { 4.798120071828828e+02f, -4.728222224549831e+02f, 1.591181202824602e+02f, -6.421997865768036f }, // phi
.k26_coeff = { 59.794709871063546f, -60.418062715734166f, 20.991263455753383f, -1.388418937916963f }, // d_phi
// .k11_coeff = { -1.996305368054721e+02f, 2.260001266392926e+02f, -1.009632659573521e+02f, 2.651403223110949e+00f }, // theta
// .k12_coeff = { 1.961704292162323e+00f, 8.251913348469651e-01f, -6.073749575127879e+00f, 3.535645826465822e-01f }, // d_theta
// .k13_coeff = { -8.161081261310467e+01f, 8.274264960693915e+01f, -2.904800049859091e+01f, 1.653742354439720e+00f }, // x
// .k14_coeff = { -7.455421501651551e+01f, 7.622235638964226e+01f, -2.773307975245958e+01f, 1.535810571728176e+00f }, // d_x
// .k15_coeff = { -9.027722377713712e+00f, 2.891936105315331e+01f, -2.106785573613789e+01f, 4.948383450314285e+00f }, // phi
// .k16_coeff = { 3.049839709048039e+00f, 4.627952499276505e-02f, -1.922806522134511e+00f, 6.902507101472589e-01f }, // d_phi
// .k21_coeff = { 4.373445515700652e+02f, -3.391497363529634e+02f, 5.569210421652366e+01f, 1.081229141610274e+01f }, // theta
// .k22_coeff = { 5.284103137531328e+01f, -5.028796043573002e+01f, 1.502532265509508e+01f, -3.077550945526411e-02f }, // d_theta
// .k23_coeff = { 3.111787495894651e+01f, 4.872956811853720e+00f, -2.278008499711769e+01f, 8.426190283284837e+00f }, // x
// .k24_coeff = { 3.329235562185018e+01f, -5.071568882184936e-01f, -1.928141689016582e+01f, 7.776043600153916e+00f }, // d_x
// .k25_coeff = { 4.604866240014122e+02f, -4.577537299814802e+02f, 1.557742271762380e+02f, -6.272551369660446e+00f }, // phi
// .k26_coeff = { 5.608646259671771e+01f, -5.728239809509379e+01f, 2.016452460533993e+01f, -1.320817827839268e+00f }, // d_phi
},
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
.lqr_param.max_wheel_torque = 2.5f, // 轮毂电机最大力矩 2.5Nm

244
User/整合方案说明.md
View File

@ -1,244 +0,0 @@
# 轮腿机器人控制系统整合方案
## 📋 现有系统分析
### 🏗️ 您的系统架构（优势）
1. **成熟的硬件接口**：已实现HT+BM电机CAN通信
2. **完整的运动学**：四连杆正/逆运动学 + 雅可比矩阵
3. **丰富的控制模式**：平衡、运动、跳跃、悬浮检测
4. **详细的参数数据**：基于实际测量的腿部参数表
5. **MATLAB设计工具**：完整的LQR参数生成流程
### ⚠️ 可以改进的地方
1. **控制逻辑分散**：LQR、VMC、MPC混合使用，逻辑复杂
2. **参数管理困难**：大量硬编码参数分散在各处
3. **缺乏统一接口**：不同控制算法接口不一致
## 🚀 整合方案
### 方案一：渐进式整合（推荐）
#### 第一阶段：保持现有系统，添加新接口
```c
// 在Chassis_Task.c中添加
#include "integrated_balance_control.h"
void Chassis_Task(void const * argument)
{
vTaskDelay(CHASSIS_TASK_INIT_TIME);
Chassis_Init(&chassis_control);
vTaskDelay(100);
while(1)
{
Chassis_Data_Update(&chassis_control);
Chassis_Status_Detect(&chassis_control);
Chassis_Mode_Set(&chassis_control);
Chassis_Mode_Change_Control_Transit(&chassis_control);
Target_Value_Set(&chassis_control);
// 新增：整合控制器调用
integrated_chassis_control_update();
// 如果使用新控制器，跳过原有的力矩计算
if (!get_current_controller_type()) {
Chassis_Torque_Calculation(&chassis_control);
Chassis_Torque_Combine(&chassis_control);
}
Motor_CMD_Send(&chassis_control);
vTaskDelay(1);
}
}
```
#### 第二阶段：参数统一管理
创建统一的参数配置文件：
```c
// robot_config.h
#ifndef ROBOT_CONFIG_H
#define ROBOT_CONFIG_H
// 机器人物理参数（与MATLAB一致）
#define ROBOT_R_W 0.09f // 驱动轮半径 (m)
#define ROBOT_R_L 0.25f // 轮距/2 (m)
#define ROBOT_L_C 0.037f // 机体质心到关节距离 (m)
#define ROBOT_M_W 0.8f // 驱动轮质量 (kg)
#define ROBOT_M_L 1.6183599f // 腿部质量 (kg)
#define ROBOT_M_B 11.542f // 机体质量 (kg)
// 腿部几何参数（与您的定义一致）
#define LEG_L1 0.15f // 大腿长度 (m)
#define LEG_L2 0.25f // 小腿长度 (m)
#define LEG_L3 0.25f //
#define LEG_L4 0.15f //
#define LEG_L5 0.1f // 髋关节间距/2 (m)
// 控制参数
#define CONTROL_FREQUENCY 500.0f // 控制频率 (Hz)
#define MAX_WHEEL_TORQUE 50.0f // 最大轮子力矩 (N*m)
#define MAX_JOINT_TORQUE 18.0f // 最大关节力矩 (N*m)
// 安全参数
#define MAX_TILT_ANGLE 0.25f // 最大倾斜角 (rad)
#define LEG_LENGTH_MIN 0.11f // 最小腿长 (m)
#define LEG_LENGTH_MAX 0.30f // 最大腿长 (m)
#endif
```
#### 第三阶段：控制器选择接口
添加动态切换功能：
```c
// 在遥控器中添加切换功能
void handle_controller_switch(const Gimbal_ctrl_t* rc_ctrl)
{
static uint8_t last_switch_state = 0;
uint8_t current_switch_state = 0; // 从遥控器读取开关状态
if (current_switch_state != last_switch_state) {
if (current_switch_state == 1) {
// 切换到新控制器
set_controller_type(1);
} else {
// 切换到原有控制器
set_controller_type(0);
}
last_switch_state = current_switch_state;
}
}
```
### 方案二：完全替换（激进）
直接用新的balance_control替换现有的控制逻辑：
#### 修改Chassis_Task主循环
```c
void Chassis_Task(void const * argument)
{
vTaskDelay(CHASSIS_TASK_INIT_TIME);
// 初始化新的平衡控制器
balance_controller_t balance_ctrl;
balance_control_init(&balance_ctrl, TASK_RUN_TIME);
vTaskDelay(100);
while(1)
{
// 数据更新（保留）
Chassis_Data_Update(&chassis_control);
// 转换数据格式
imu_data_t imu_data;
motor_feedback_t motor_fb;
control_command_t cmd;
convert_chassis_to_balance_imu(&chassis_control, &imu_data);
convert_chassis_to_balance_motor_fb(&chassis_control, &motor_fb);
convert_chassis_to_balance_cmd(&chassis_control, &cmd);
// 使用新控制器
balance_control_update(&balance_ctrl, &imu_data, &motor_fb, &cmd);
// 获取控制输出
motor_control_t motor_ctrl;
get_motor_control_output(&balance_ctrl, &motor_ctrl);
// 转换回原有格式并发送
convert_balance_to_chassis_output(&motor_ctrl, &chassis_control);
Motor_CMD_Send(&chassis_control);
vTaskDelay(1);
}
}
```
## 🔧 具体修改步骤
### 1. 保留您的优势代码
- **保留**：`Forward_kinematic_solution()` - 运动学计算
- **保留**：`Supportive_Force_Cal()` - 支撑力计算
- **保留**：`CAN_HT_CMD()` / `CAN_BM_CONTROL_CMD()` - 电机通信
- **保留**：MATLAB LQR设计流程
### 2. 整合控制架构
- **替换**：分散的LQR/VMC控制逻辑 → 统一的balance_control
- **简化**：复杂的状态机 → 清晰的模式管理
- **统一**：参数管理 → robot_config.h
### 3. 数据接口适配
```c
// 现有的HT电机数据
typedef struct {
const HT_motor_measure_t *motor_measure;
fp32 position;
fp32 velocity;
fp32 torque_out, torque_get;
} joint_motor_t;
// 转换为新系统格式
void convert_ht_motor_to_balance_fb(const joint_motor_t* ht_motor,
float* angle, float* velocity, float* torque)
{
*angle = ht_motor->position;
*velocity = ht_motor->velocity;
*torque = ht_motor->torque_get;
}
```
### 4. 参数迁移
将您MATLAB中的参数直接用于新系统：
```c
// 使用您的LQR拟合系数
const float K_fit_coefficients[40][6] = {
// 复制您MATLAB生成的系数
// ...
};
// 使用您的机器人参数
#define R_W_ACTUAL 0.09f // 与您的R_w_ac一致
#define R_L_ACTUAL 0.25f // 与您的R_l_ac一致
// ...
```
## 🎯 推荐的实施步骤
### 第1周：准备工作
1. 备份现有代码
2. 创建新的git分支
3. 编译并测试现有系统确保正常
### 第2周：集成准备
1. 添加integrated_balance_control文件
2. 修改Chassis_Task添加切换接口
3. 保持原有控制器为默认
### 第3周：参数对齐
1. 创建robot_config.h统一参数
2. 运行MATLAB脚本生成LQR系数
3. 对比调试确保参数一致
### 第4周：功能测试
1. 在安全环境下测试新控制器
2. 对比两套控制器的性能
3. 调试和优化
### 第5周：全面切换
1. 将新控制器设为默认
2. 移除冗余的旧代码
3. 文档整理和代码规范化
## 🛡️ 风险控制
1. **渐进式切换**：保留原有系统作为备份
2. **参数验证**：MATLAB仿真验证参数正确性
3. **安全测试**：在安全环境下测试新控制器
4. **性能对比**：记录并对比控制效果
5. **回滚机制**：确保可以快速回到原有系统
这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？

213
comp_vmc.cpp Normal file
View File

@ -0,0 +1,213 @@
#include "comp_vmc.hpp"
#include <tuple>
#define PI_2 1.571f
using namespace Component;
VMC::VMC(VMC::Param &param, float sample_freq) : param_(param) {
float dt_min = 1.0f / sample_freq;
XB_ASSERT(isfinite(dt_min));
this->Reset();
}
/*
正负极参考韭菜的菜 知乎 平衡步兵控制系统设计
VMC 机体pitch正负极 d_pitch同
/
/ 正+
/
x ---------> 0
\ 负-
\
\
phi角正负极 d_phi同
/
/ 正+
/
x ---------> 0
\ 负-
\
\
后 <---->前
/phi1-----phi4\
/ \
\ /
\ OO /
\O轮O/
OO
*/
/* 两个大腿角度 机体角度 角速度 求出虚拟腿摆角 摆角速度 虚拟腿长
* 虚拟腿长变化速度 */
std::tuple<float, float, float, float> VMC::VMCsolve(float phi1, float phi4,
float eulrPit,
float d_eulrPit,
float dt) {
static float body_pitch = 0.0f;
static float d_body_pitch = 0.0f;
body_pitch = eulrPit;
d_body_pitch = d_eulrPit;
/*点D B x y坐标 */
this->vmc_leg.YD = this->param_.leg_4 * sinf(phi4);
this->vmc_leg.YB = this->param_.leg_1 * sinf(phi1);
this->vmc_leg.XD = this->param_.hip_length + this->param_.leg_4 * cosf(phi4);
this->vmc_leg.XB = this->param_.leg_1 * cosf(phi1);
/*BD长度*/
this->vmc_leg.lBD = sqrtf((this->vmc_leg.XD - this->vmc_leg.XB) *
(this->vmc_leg.XD - this->vmc_leg.XB) +
(this->vmc_leg.YD - this->vmc_leg.YB) *
(this->vmc_leg.YD - this->vmc_leg.YB));
this->vmc_leg.A0 =
2 * this->param_.leg_2 * (this->vmc_leg.XD - this->vmc_leg.XB);
this->vmc_leg.B0 =
2 * this->param_.leg_2 * (this->vmc_leg.YD - this->vmc_leg.YB);
this->vmc_leg.C0 = this->param_.leg_2 * this->param_.leg_2 +
this->vmc_leg.lBD * this->vmc_leg.lBD -
this->param_.leg_3 * this->param_.leg_3;
this->vmc_leg.phi2 =
2 *
atan2f((this->vmc_leg.B0 + sqrtf(this->vmc_leg.A0 * this->vmc_leg.A0 +
this->vmc_leg.B0 * this->vmc_leg.B0 -
this->vmc_leg.C0 * this->vmc_leg.C0)),
this->vmc_leg.A0 + this->vmc_leg.C0);
this->vmc_leg.phi3 =
atan2f(this->vmc_leg.YB - this->vmc_leg.YD +
this->param_.leg_2 * sinf(this->vmc_leg.phi2),
this->vmc_leg.XB - this->vmc_leg.XD +
this->param_.leg_2 * cosf(this->vmc_leg.phi2));
/*点C x y坐标 */
this->vmc_leg.XC = this->param_.leg_1 * cosf(phi1) +
this->param_.leg_2 * cosf(this->vmc_leg.phi2);
this->vmc_leg.YC = this->param_.leg_1 * sinf(phi1) +
this->param_.leg_2 * sinf(this->vmc_leg.phi2);
/*点C 极坐标 */
this->vmc_leg.L0 =
sqrtf((this->vmc_leg.XC - this->param_.hip_length / 2.0f) *
(this->vmc_leg.XC - this->param_.hip_length / 2.0f) +
this->vmc_leg.YC * this->vmc_leg.YC);
this->vmc_leg.phi0 = atan2f(
this->vmc_leg.YC, (this->vmc_leg.XC - this->param_.hip_length / 2.0f));
this->vmc_leg.alpha = PI_2 - this->vmc_leg.phi0;
this->vmc_leg.d_phi0 = (this->vmc_leg.phi0 - this->vmc_leg.last_phi0) / dt;
this->vmc_leg.d_alpha = 0.0f - this->vmc_leg.d_phi0;
/*虚拟腿 摆角theta 摆角速度d_theta */
this->vmc_leg.theta = PI_2 + body_pitch - this->vmc_leg.phi0;
this->vmc_leg.d_theta = (-d_body_pitch - this->vmc_leg.d_phi0);
this->vmc_leg.last_phi0 = this->vmc_leg.phi0;
/*虚拟腿 腿长L0 腿长变化速度d_L0 */
this->vmc_leg.d_L0 = (this->vmc_leg.L0 - this->vmc_leg.last_L0) / dt;
this->vmc_leg.dd_L0 = (this->vmc_leg.d_L0 - this->vmc_leg.last_d_L0) / dt;
this->vmc_leg.last_d_L0 = this->vmc_leg.d_L0;
this->vmc_leg.last_L0 = this->vmc_leg.L0;
this->vmc_leg.dd_theta =
(this->vmc_leg.d_theta - this->vmc_leg.last_d_theta) / dt;
this->vmc_leg.last_d_theta = this->vmc_leg.d_theta;
return std::make_tuple(vmc_leg.L0, vmc_leg.d_L0, vmc_leg.theta,
vmc_leg.d_theta);
}
/* 两个大腿角度 期望腿支持力 期望腿摆力矩 求出两个关节输出力矩 */
std::tuple<float, float> VMC::VMCinserve(float phi1, float phi4, float Tp,
float F0) {
/*jacobian矩阵计算*/
this->vmc_leg.j11 =
(this->param_.leg_1 * sinf(this->vmc_leg.phi0 - this->vmc_leg.phi3) *
sinf(phi1 - this->vmc_leg.phi2)) /
sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2);
this->vmc_leg.j12 =
(this->param_.leg_1 * cosf(this->vmc_leg.phi0 - this->vmc_leg.phi3) *
sinf(phi1 - this->vmc_leg.phi2)) /
(this->vmc_leg.L0 * sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2));
this->vmc_leg.j21 =
(this->param_.leg_4 * sinf(this->vmc_leg.phi0 - this->vmc_leg.phi2) *
sinf(this->vmc_leg.phi3 - phi4)) /
sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2);
this->vmc_leg.j22 =
(this->param_.leg_4 * cosf(this->vmc_leg.phi0 - this->vmc_leg.phi2) *
sinf(this->vmc_leg.phi3 - phi4)) /
(this->vmc_leg.L0 * sinf(this->vmc_leg.phi3 - this->vmc_leg.phi2));
/*得到前髋关节的输出轴期望力矩，F0为五连杆机构末端沿腿的推力*/
this->vmc_leg.torque_set[0] = this->vmc_leg.j11 * F0 + this->vmc_leg.j12 * Tp;
/*得到后髋关节的输出轴期望力矩，Tp为虚拟腿摆力矩的力矩*/
this->vmc_leg.torque_set[1] = this->vmc_leg.j21 * F0 + this->vmc_leg.j22 * Tp;
return std::make_tuple(this->vmc_leg.torque_set[0],
this->vmc_leg.torque_set[1]);
}
/* 用到了前两个函数解算算出来的变量 尽量放在前两个函数之后 */
float VMC::GndDetector(float Tp, float F0) {
vmc_leg.Fn =
F0 * cosf(vmc_leg.theta) + Tp * sinf(vmc_leg.theta) / vmc_leg.L0 + 6; //
// 腿部机构的力+轮子重力，这里忽略了轮子质量*驱动轮竖直方向运动加速度
// vmc_leg.Fn =
// F0 * cosf(vmc_leg.theta) + Tp * sinf(vmc_leg.theta) / vmc_leg.L0 +
// wheel_mess *
// (z_accl - vmc_leg.dd_L0 * cosf(vmc_leg.theta) +
// 2.0f * vmc_leg.d_L0 * vmc_leg.d_theta * sinf(vmc_leg.theta) +
// vmc_leg.L0 * vmc_leg.dd_theta * sinf(vmc_leg.theta) +
// vmc_leg.L0 * vmc_leg.d_theta * vmc_leg.d_theta *
// cosf(vmc_leg.theta));
return vmc_leg.Fn;
}
/* 计算拟合函数结果 */
float VMC::LQR_K_calc(float *coe, float len) {
return coe[0] * len * len * len + coe[1] * len * len + coe[2] * len + coe[3];
}
/* 变量刷新 */
void VMC::Reset() {
vmc_leg.L0 = 0;
vmc_leg.phi0 = 0;
vmc_leg.alpha = 0;
vmc_leg.d_alpha = 0;
vmc_leg.lBD = 0;
vmc_leg.d_phi0 = 0;
vmc_leg.last_phi0 = 0;
vmc_leg.A0 = 0;
vmc_leg.B0 = 0;
vmc_leg.C0 = 0;
vmc_leg.phi2 = 0;
vmc_leg.phi3 = 0;
vmc_leg.j11 = 0;
vmc_leg.j12 = 0;
vmc_leg.j21 = 0;
vmc_leg.j22 = 0;
vmc_leg.torque_set[0] = 0;
vmc_leg.torque_set[1] = 0;
vmc_leg.theta = 0;
vmc_leg.d_theta = 0;
vmc_leg.last_d_theta = 0;
vmc_leg.dd_theta = 0;
vmc_leg.d_L0 = 0;
vmc_leg.dd_L0 = 0;
vmc_leg.last_L0 = 0;
vmc_leg.last_d_L0 = 0;
vmc_leg.first_flag = 0;
vmc_leg.leg_flag = 0;
}

0
d435.py
View File

171
some_functions.c
View File

@ -1,171 +0,0 @@
// Front
// | 1 4 |
// (1)Left| |Right(2)
// | 2 3 |
#define HT_SLAVE_ID1 0x01
#define HT_SLAVE_ID2 0x02
#define HT_SLAVE_ID3 0x03
#define HT_SLAVE_ID4 0x04
#define CAN_BM_M1_ID 0x97
#define CAN_BM_M2_ID 0x98
#define BM_CAN hcan2
#define get_HT_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = uint_to_float((uint16_t)((data)[1] << 8 | (data)[2] ),P_MIN, P_MAX, 16); \
(ptr)->speed_rpm = uint_to_float((uint16_t)(data[3]<<4)|(data[4]>>4), V_MIN, V_MAX, 12);\
(ptr)->real_torque = uint_to_float((uint16_t)(((data[4] & 0x0f) << 8) | (data)[5]), -18, +18, 12); \
}
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
#define P_MIN -95.5f// Radians
#define P_MAX 95.5f
#define V_MIN -45.0f// Rad/s
#define V_MAX 45.0f
#define KP_MIN 0.0f// N-m/rad
#define KP_MAX 500.0f
#define KD_MIN 0.0f// N-m/rad/s
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
void Forward_kinematic_solution(chassis_control_t *feedback_update,
fp32 Q1,fp32 S1,fp32 Q4,fp32 S4, uint8_t ce)
{
fp32 dL0=0,L0=0,Q0=0,S0=0;
fp32 xb,xd,yb,yd,Lbd,xc,yc;
fp32 A0,B0,C0,Q2,Q3,S2,S3;
fp32 vxb,vxd,vyb,vyd,vxc,vyc;
fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
fp32 axb,ayb,axd,ayd,a2,axc;
/******************************/
Q1 = PI + Q1;
cos_Q1 = arm_cos_f32(Q1);
sin_Q1 = arm_sin_f32(Q1);
cos_Q4 = arm_cos_f32(Q4);
sin_Q4 = arm_sin_f32(Q4);
xb = -L5/2.0f + L1*cos_Q1;
xd = L5/2.0f + L4*cos_Q4;
yb = L1*sin_Q1;
yd = L4*sin_Q4;
Lbd=(xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
A0 = 2.0f*L2*(xd-xb);
B0 = 2.0f*L2*(yd-yb);
C0 = L2*L2+Lbd-L3*L3;
Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
xc = xb + arm_cos_f32(Q2)*L2;
yc = yb + arm_sin_f32(Q2)*L2;
L0=Sqrt( xc*xc + yc*yc );
Q0 = atan(yc/xc);
vxb = -S1*L1*sin_Q1;
vyb = S1*L1*cos_Q1;
vxd = -S4*L4*sin_Q4;
vyd = S4*L4*cos_Q4;
Q3 = atan_tl((yc-yd)/(xc-xd));
S2 = ((vxd-vxb)*arm_cos_f32(Q3) + (vyd-vyb)*arm_sin_f32(Q3))/(L2*arm_sin_f32(Q3-Q2));
S3 = ((vxd-vxb)*arm_cos_f32(Q2) + (vyd-vyb)*arm_sin_f32(Q2))/(L2*arm_sin_f32(Q3-Q2));
vxc = vxb - S2*L2*arm_sin_f32(Q2);
vyc = vyb + S2*L2*arm_cos_f32(Q2);
S0 = 3*(-arm_sin_f32(ABS(Q0))*vxc-arm_cos_f32(Q0)*vyc);
if( Q0 < 0 )
Q0 += PI;
/*******************************/
if (ce)
{
feedback_update->chassis_posture_info .leg_length_L = L0;
feedback_update->chassis_posture_info .leg_angle_L = Q0;
// feedback_update->chassis_posture_info .leg_gyro_L = S0;
}
else
{
feedback_update->chassis_posture_info .leg_length_R = L0;
feedback_update->chassis_posture_info .leg_angle_R = Q0;
// feedback_update->chassis_posture_info .leg_gyro_R = S0;
}
}
void Supportive_Force_Cal( chassis_control_t *Suspend_Detect, fp32 Q1, fp32 Q4, fp32 ce )
{
fp32 dL0=0,L0=0,Q0=0,S0=0;
fp32 xb,xd,yb,yd,Lbd,xc,yc;
fp32 A0,B0,C0,Q2,Q3,S2,S3;
fp32 vxb,vxd,vyb,vyd,vxc,vyc;
fp32 cos_Q1,cos_Q4,sin_Q1,sin_Q4;
fp32 sin_Q2,cos_Q2,sin_Q3,cos_Q3;
fp32 axb,ayb,axd,ayd,a2,axc;
/******************************/
Q1 = PI + Q1;
cos_Q1 = arm_cos_f32(Q1);
sin_Q1 = arm_sin_f32(Q1);
cos_Q4 = arm_cos_f32(Q4);
sin_Q4 = arm_sin_f32(Q4);
xb = -L5/2.0f + L1*cos_Q1;
xd = L5/2.0f + L4*cos_Q4;
yb = L1*sin_Q1;
yd = L4*sin_Q4;
Lbd=sqrt( (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb) );
A0 = 2.0f*L2*(xd-xb);
B0 = 2.0f*L2*(yd-yb);
C0 = L2*L2+Lbd*Lbd-L3*L3;
Q2 = 2.0f *atan_tl((B0+Sqrt(A0*A0 + B0*B0 -C0*C0))/(A0+C0));
xc = xb + arm_cos_f32(Q2)*L2;
yc = yb + arm_sin_f32(Q2)*L2;
L0 = Sqrt( xc*xc + yc*yc );
Q0 = atan_tl(yc/xc);
if( Q0 < 0 )
Q0 += PI;
Q3 = atan_tl((yc-yd)/(xc-xd))+PI;
fp32 J1 = ( L1 * arm_sin_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ));
fp32 J2 = ( L1 * arm_cos_f32(Q0 - Q3) * arm_sin_f32( Q1 - Q2 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
fp32 J3 = ( L4 * arm_sin_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ));
fp32 J4 = ( L4 * arm_cos_f32(Q0 - Q2) * arm_sin_f32( Q3 - Q4 ) ) / (arm_sin_f32( Q3 - Q2 ) * L0);
fp32 dett = J1 * J4 - J2 * J3;
fp32 Inv_J1 = J4 / dett;
fp32 Inv_J2 = -J2 / dett;
fp32 Inv_J3 = -J3 / dett;
fp32 Inv_J4 = J1 / dett;
ddebug = yc;
if( ce == 1.0f )
{
Suspend_Detect->chassis_posture_info.supportive_force_L =
Inv_J1 * Suspend_Detect->joint_motor_1.torque_get +
Inv_J2 * Suspend_Detect->joint_motor_2.torque_get;
// if( Suspend_Detect->chassis_posture_info.supportive_force_L < 0.0f )
// Suspend_Detect->chassis_posture_info.supportive_force_L += 40.0f;
}
else
{
Suspend_Detect->chassis_posture_info.supportive_force_R =
Inv_J1 * Suspend_Detect->joint_motor_4.torque_get +
Inv_J2 * Suspend_Detect->joint_motor_3.torque_get;
Suspend_Detect->chassis_posture_info.supportive_force_R *= -1.0f;
// if( Suspend_Detect->chassis_posture_info.supportive_force_R < 0.0f )
// Suspend_Detect->chassis_posture_info.supportive_force_R += 40.0f;
}
}

13
utils/Simulation-master/.gitignore vendored Normal file
View File

@ -0,0 +1,13 @@
# Duplicate files
*(*)*
# MATLAB files
balance/parallel_legs/slprj
balance/series_legs/slprj
balance/series_parallel_legs/slprj
*/slprj/
*/codegen/
*.prj
*.slxc
*.autosave
*.asv

21
utils/Simulation-master/LICENSE Normal file
View File

@ -0,0 +1,21 @@
MIT License
Copyright (c) 2024 是小企鹅呀
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx.original Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2022b Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/parallel_legs/blance_leg.slx.r2023a Normal file
View File

Binary file not shown.

20
utils/Simulation-master/balance/parallel_legs/d_phi0.m Normal file
View File

@ -0,0 +1,20 @@
syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
XD=l5+l4*cos(phi_4);
YD=l4*sin(phi_4);
XB=l1*cos(phi_1);
YB=l1*sin(phi_1);
A0=2*l2*(XD-XB);
B0=2*l2*(YD-YB);
lBD=sqrt((XD-XB)^2+(YD-YB)^2);
C0=l2^2+lBD^2-l3^2;
phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
XC=XB+l2*cos(phi_2);
YC=YB+l2*sin(phi_2);
L0=sqrt((XC-l5/2)^2+YC^2);
phi_0=atan(YC/(XC-l5/2));
J=jacobian([L0;phi_0],[phi_1;phi_4]);
phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4

19
utils/Simulation-master/balance/parallel_legs/display_polynomial.m Normal file
View File

@ -0,0 +1,19 @@
% 定义一个函数来显示多项式方程
function display_polynomial(coefficients, name)
equation = sprintf('%s = ', name);
n = length(coefficients);
for i = 1:n
if coefficients(i) ~= 0
if i == n
term = sprintf('%.4f', coefficients(i));
else
term = sprintf('%.4f*t%d', coefficients(i), n-i);
end
if i > 1 && coefficients(i) > 0
term = [' + ', term];
end
equation = [equation, term];
end
end
fprintf('%s;\n', equation);
end

82
utils/Simulation-master/balance/parallel_legs/get_k.m Normal file
View File

@ -0,0 +1,82 @@
%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
tic
j=1;
leg=0.1:0.01:0.4;
for i=leg
k=get_k_length(i);
k11(j) = k(1,1);
k12(j) = k(1,2);
k13(j) = k(1,3);
k14(j) = k(1,4);
k15(j) = k(1,5);
k16(j) = k(1,6);
k21(j) = k(2,1);
k22(j) = k(2,2);
k23(j) = k(2,3);
k24(j) = k(2,4);
k25(j) = k(2,5);
k26(j) = k(2,6);
j=j+1;
fprintf('leg_length=%d\n', i);
end
a11=polyfit(leg,k11,3);
a12=polyfit(leg,k12,3);
a13=polyfit(leg,k13,3);
a14=polyfit(leg,k14,3);
a15=polyfit(leg,k15,3);
a16=polyfit(leg,k16,3);
a21=polyfit(leg,k21,3);
a22=polyfit(leg,k22,3);
a23=polyfit(leg,k23,3);
a24=polyfit(leg,k24,3);
a25=polyfit(leg,k25,3);
a26=polyfit(leg,k26,3);
toc
% x0=leg; %步长为0.1
% y11=polyval(a11,x0); %返回值y0是对应于x0的函数值
% y12=polyval(a12,x0); %返回值y0是对应于x0的函数值
% y13=polyval(a13,x0); %返回值y0是对应于x0的函数值
% y14=polyval(a14,x0); %返回值y0是对应于x0的函数值
% y15=polyval(a15,x0); %返回值y0是对应于x0的函数值
% y16=polyval(a16,x0); %返回值y0是对应于x0的函数值
%
% y21=polyval(a21,x0); %返回值y0是对应于x0的函数值
% y22=polyval(a22,x0); %返回值y0是对应于x0的函数值
% y23=polyval(a23,x0); %返回值y0是对应于x0的函数值
% y24=polyval(a24,x0); %返回值y0是对应于x0的函数值
% y25=polyval(a25,x0); %返回值y0是对应于x0的函数值
% y26=polyval(a26,x0); %返回值y0是对应于x0的函数值
% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
%
% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
% grid on; %添加网格线
% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1); %将网格线变成虚线
% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
%
% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
toc

56
utils/Simulation-master/balance/parallel_legs/get_k_length.m Normal file
View File

@ -0,0 +1,56 @@
function K = get_k_length(leg_length)
%theta : 摆杆与竖直方向夹角 R ：驱动轮半径
%x : 驱动轮位移 L : 摆杆重心到驱动轮轴距离
%phi : 机体与水平夹角 LM : 摆杆重心到其转轴距离
%T ：驱动轮输出力矩 l : 机体重心到其转轴距离
%Tp : 髋关节输出力矩 mw : 驱动轮转子质量
%N ：驱动轮对摆杆力的水平分量 mp : 摆杆质量
%P ：驱动轮对摆杆力的竖直分量 M : 机体质量
%Nm ：摆杆对机体力水平方向分量 Iw : 驱动轮转子转动惯量
%Pm ：摆杆对机体力竖直方向分量 Ip : 摆杆绕质心转动惯量
%Nf : 地面对驱动轮摩擦力 Im : 机体绕质心转动惯量
syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0
R1=0.072; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=0.03; %机体质心距离转轴距离
mw1=0.80; %驱动轮质量
mp1=1.11; %杆质量
M1=2.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
N = NM + mp*diff(x + L*sin(theta),t,2);
PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
P = PM +mp*g+mp*diff(L*cos(theta),t,2);
eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
[f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
A=double(A);
B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 25]; %T Tp
K=lqr(A,B,Q,R);
end

BIN
utils/Simulation-master/balance/parallel_legs/physical_calc.mlx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_legs/blance_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_legs/calc.mlx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_legs/doc/2_link.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 22 KiB

154
utils/Simulation-master/balance/series_legs/doc/2连杆分析.md Normal file
View File

@ -0,0 +1,154 @@
# 2连杆分析
参考：[五连杆运动学解算与VMC](https://zhuanlan.zhihu.com/p/613007726)
![pic](./2_link.png)
## 1 正运动学解算
$\phi_1$和$\phi_2$可由电机编码器测量得到。
$C$点坐标：
$$
\left \{
\begin{array}{l}
x_C = l_1\cos\phi_1 + l_2\cos\phi_2\\
y_C = l_1\sin\phi_1 + l_2\sin\phi_2
\end{array}
\right .
$$
得：
$$
\left \{
\begin{array}{l}
L0 = \sqrt{x_C^2 + y_C^2} \\
\phi_0 = \arctan{\frac{y_C}{x_C}}
\end{array}
\right .
$$
## 2 逆运动学解算
由余弦定理易得：
$$
\phi_2+\phi_3 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}
$$
又：
$$
\phi_3 = \pi - \phi_1
$$
得：
$$
\phi_2 = \arccos\frac{l_1^2+l_2^2-L_0^2}{2l_1l_2}+\phi_1-\pi
$$
## 3 雅可比矩阵
<!-- $$
\left [
\begin{matrix}
\dot L0 \\
\dot \phi_0
\end{matrix}
\right ]
=
J
\left [
\begin{matrix}
\dot\phi_1 \\
\dot\phi_2
\end{matrix}
\right ]
$$ -->
基于文中描述可得：
$$
\left \{
\begin{array}{l}
\dot x_C = -l_1\dot\phi_1\sin\phi_1 - l_2\dot\phi_2\sin\phi_2\\
\dot y_C = l_1\dot\phi_1\cos\phi_1 + l_2\dot\phi_2\cos\phi_2
\end{array}
\right .
$$
即：
$$
\left [
\begin{matrix}
\dot x_C \\
\dot y_C
\end{matrix}
\right ]
=
\left [
\begin{matrix}
-l_1\sin\phi_1 & -l_2\sin\phi_2 \\
l_1\cos\phi_1 & l_2\cos\phi_2
\end{matrix}
\right ]
\left [
\begin{matrix}
\dot\phi_1 \\
\dot\phi_2
\end{matrix}
\right ]
$$
记作：
$$
\left [
\begin{matrix}
\dot x_C \\
\dot y_C
\end{matrix}
\right ]
=
J_0
\left [
\begin{matrix}
\dot\phi_1 \\
\dot\phi_2
\end{matrix}
\right ]
$$
下面操作与文中相同，可得：
$$
J^T = J_0^TRM =
\left[
\begin{matrix}
l_1 \,\sin \left(\phi_0 -\phi_1 \right) & \frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 }\\
l_2 \,\sin \left(\phi_0 -\phi_2 \right) & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
\end{matrix}
\right]
$$
即：
$$
J =
\left[
\begin{matrix}
l_1 \,\sin \left(\phi_0 -\phi_1 \right) & l_2 \,\sin \left(\phi_0 -\phi_2 \right)\\
\frac{l_1 \,\cos \left(\phi_0 -\phi_1 \right)}{L_0 } & \frac{l_2 \,\cos \left(\phi_0 -\phi_2 \right)}{L_0 }
\end{matrix}
\right]
$$

96
utils/Simulation-master/balance/series_legs/get_k.m Normal file
View File

@ -0,0 +1,96 @@
%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
tic
j=1;
leg=0.1:0.01:0.4;
for i=leg
k=get_k_length(i);
k11(j) = k(1,1);
k12(j) = k(1,2);
k13(j) = k(1,3);
k14(j) = k(1,4);
k15(j) = k(1,5);
k16(j) = k(1,6);
k21(j) = k(2,1);
k22(j) = k(2,2);
k23(j) = k(2,3);
k24(j) = k(2,4);
k25(j) = k(2,5);
k26(j) = k(2,6);
j=j+1;
fprintf('leg_length=%d\n', i);
end
a11=polyfit(leg,k11,3);
a12=polyfit(leg,k12,3);
a13=polyfit(leg,k13,3);
a14=polyfit(leg,k14,3);
a15=polyfit(leg,k15,3);
a16=polyfit(leg,k16,3);
a21=polyfit(leg,k21,3);
a22=polyfit(leg,k22,3);
a23=polyfit(leg,k23,3);
a24=polyfit(leg,k24,3);
a25=polyfit(leg,k25,3);
a26=polyfit(leg,k26,3);
toc
% 打印格式化的C代码结构，可以直接复制到config.c中
fprintf('\n=========== 可直接复制的C代码 ===========\n');
fprintf('.lqr_gains = {\n');
fprintf(' .k11_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a11(1), a11(2), a11(3), a11(4));
fprintf(' .k12_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_theta\n', a12(1), a12(2), a12(3), a12(4));
fprintf(' .k13_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // x\n', a13(1), a13(2), a13(3), a13(4));
fprintf(' .k14_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_x\n', a14(1), a14(2), a14(3), a14(4));
fprintf(' .k15_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a15(1), a15(2), a15(3), a15(4));
fprintf(' .k16_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_phi\n', a16(1), a16(2), a16(3), a16(4));
fprintf(' .k21_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // theta\n', a21(1), a21(2), a21(3), a21(4));
fprintf(' .k22_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_theta\n', a22(1), a22(2), a22(3), a22(4));
fprintf(' .k23_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // x\n', a23(1), a23(2), a23(3), a23(4));
fprintf(' .k24_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_x\n', a24(1), a24(2), a24(3), a24(4));
fprintf(' .k25_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // phi\n', a25(1), a25(2), a25(3), a25(4));
fprintf(' .k26_coeff = { %.15ef, %.15ef, %.15ef, %.15ef }, // d_phi\n', a26(1), a26(2), a26(3), a26(4));
fprintf('},\n');
fprintf('========================================\n\n');
% 可选：显示拟合曲线图
x0=leg; %步长为0.1
y11=polyval(a11,x0); %返回值y0是对应于x0的函数值
y12=polyval(a12,x0); %返回值y0是对应于x0的函数值
y13=polyval(a13,x0); %返回值y0是对应于x0的函数值
y14=polyval(a14,x0); %返回值y0是对应于x0的函数值
y15=polyval(a15,x0); %返回值y0是对应于x0的函数值
y16=polyval(a16,x0); %返回值y0是对应于x0的函数值
y21=polyval(a21,x0); %返回值y0是对应于x0的函数值
y22=polyval(a22,x0); %返回值y0是对应于x0的函数值
y23=polyval(a23,x0); %返回值y0是对应于x0的函数值
y24=polyval(a24,x0); %返回值y0是对应于x0的函数值
y25=polyval(a25,x0); %返回值y0是对应于x0的函数值
y26=polyval(a26,x0); %返回值y0是对应于x0的函数值
figure;
subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('leg length');ylabel('k11');title('k11拟合');
subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('leg length');ylabel('k12');title('k12拟合');
subplot(3,4,3);plot(leg,k13,'o',x0,y13,'r');xlabel('leg length');ylabel('k13');title('k13拟合');
subplot(3,4,4);plot(leg,k14,'o',x0,y14,'r');xlabel('leg length');ylabel('k14');title('k14拟合');
subplot(3,4,5);plot(leg,k15,'o',x0,y15,'r');xlabel('leg length');ylabel('k15');title('k15拟合');
subplot(3,4,6);plot(leg,k16,'o',x0,y16,'r');xlabel('leg length');ylabel('k16');title('k16拟合');
subplot(3,4,7);plot(leg,k21,'o',x0,y21,'r');xlabel('leg length');ylabel('k21');title('k21拟合');
subplot(3,4,8);plot(leg,k22,'o',x0,y22,'r');xlabel('leg length');ylabel('k22');title('k22拟合');
subplot(3,4,9);plot(leg,k23,'o',x0,y23,'r');xlabel('leg length');ylabel('k23');title('k23拟合');
subplot(3,4,10);plot(leg,k24,'o',x0,y24,'r');xlabel('leg length');ylabel('k24');title('k24拟合');
subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('leg length');ylabel('k25');title('k25拟合');
subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('leg length');ylabel('k26');title('k26拟合');
for i = 1:12
subplot(3,4,i);
grid on; %添加网格线
set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',0.5); %将网格线变成虚线
legend('原始数据','拟合曲线','Location','best');
end
toc

56
utils/Simulation-master/balance/series_legs/get_k_length.m Normal file
View File

@ -0,0 +1,56 @@
function K = get_k_length(leg_length)
%theta : 摆杆与竖直方向夹角 R ：驱动轮半径
%x : 驱动轮位移 L : 摆杆重心到驱动轮轴距离
%phi : 机体与水平夹角 LM : 摆杆重心到其转轴距离
%T ：驱动轮输出力矩 l : 机体重心到其转轴距离
%Tp : 髋关节输出力矩 mw : 驱动轮转子质量
%N ：驱动轮对摆杆力的水平分量 mp : 摆杆质量
%P ：驱动轮对摆杆力的竖直分量 M : 机体质量
%Nm ：摆杆对机体力水平方向分量 Iw : 驱动轮转子转动惯量
%Pm ：摆杆对机体力竖直方向分量 Ip : 摆杆绕质心转动惯量
%Nf : 地面对驱动轮摩擦力 Im : 机体绕质心转动惯量
syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0
R1=0.072; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=-0.03; %机体质心距离转轴距离
mw1=0.60; %驱动轮质量
mp1=1.8; %杆质量
M1=6.0; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
N = NM + mp*diff(x + L*sin(theta),t,2);
PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
P = PM +mp*g+mp*diff(L*cos(theta),t,2);
eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
[f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
A=double(A);
B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([500 1 1000 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 25]; %T Tp
K=lqr(A,B,Q,R);
end

BIN
utils/Simulation-master/balance/series_legs/leg_sim.slx Normal file
View File

Binary file not shown.

3
utils/Simulation-master/balance/series_parallel_legs/README.md Normal file
View File

@ -0,0 +1,3 @@
# 串并联腿仿真总结
这次仿真对小企鹅来说最重要的一点就是确定了替换为串联腿时不需要替换模型，直接移植现有并联腿模型即可。

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.original Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2022b Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/blance_leg.slx.r2023a Normal file
View File

Binary file not shown.

20
utils/Simulation-master/balance/series_parallel_legs/d_phi0.m Normal file
View File

@ -0,0 +1,20 @@
syms phi_0 phi_1 phi_2 phi_4 l1 l2 l3 l4 l5 L0 d_phi1 d_phi4
XD=l5+l4*cos(phi_4);
YD=l4*sin(phi_4);
XB=l1*cos(phi_1);
YB=l1*sin(phi_1);
A0=2*l2*(XD-XB);
B0=2*l2*(YD-YB);
lBD=sqrt((XD-XB)^2+(YD-YB)^2);
C0=l2^2+lBD^2-l3^2;
phi_2=2*atan((B0+sqrt(A0^2+B0^2-C0^2))/(A0+C0));
XC=XB+l2*cos(phi_2);
YC=YB+l2*sin(phi_2);
L0=sqrt((XC-l5/2)^2+YC^2);
phi_0=atan(YC/(XC-l5/2));
J=jacobian([L0;phi_0],[phi_1;phi_4]);
phi0_dot = J(2,1)*d_phi1 +J(2,2)*d_phi4

19
utils/Simulation-master/balance/series_parallel_legs/display_polynomial.m Normal file
View File

@ -0,0 +1,19 @@
% 定义一个函数来显示多项式方程
function display_polynomial(coefficients, name)
equation = sprintf('%s = ', name);
n = length(coefficients);
for i = 1:n
if coefficients(i) ~= 0
if i == n
term = sprintf('%.4f', coefficients(i));
else
term = sprintf('%.4f*t%d', coefficients(i), n-i);
end
if i > 1 && coefficients(i) > 0
term = [' + ', term];
end
equation = [equation, term];
end
end
fprintf('%s;\n', equation);
end

82
utils/Simulation-master/balance/series_parallel_legs/get_k.m Normal file
View File

@ -0,0 +1,82 @@
%计算不同腿长下适合的K矩阵，再进行多项式拟合，得到2*6矩阵每个参数对应的多项式参数
tic
j=1;
leg=0.1:0.01:0.4;
for i=leg
k=get_k_length(i);
k11(j) = k(1,1);
k12(j) = k(1,2);
k13(j) = k(1,3);
k14(j) = k(1,4);
k15(j) = k(1,5);
k16(j) = k(1,6);
k21(j) = k(2,1);
k22(j) = k(2,2);
k23(j) = k(2,3);
k24(j) = k(2,4);
k25(j) = k(2,5);
k26(j) = k(2,6);
j=j+1;
fprintf('leg_length=%d\n', i);
end
a11=polyfit(leg,k11,3);
a12=polyfit(leg,k12,3);
a13=polyfit(leg,k13,3);
a14=polyfit(leg,k14,3);
a15=polyfit(leg,k15,3);
a16=polyfit(leg,k16,3);
a21=polyfit(leg,k21,3);
a22=polyfit(leg,k22,3);
a23=polyfit(leg,k23,3);
a24=polyfit(leg,k24,3);
a25=polyfit(leg,k25,3);
a26=polyfit(leg,k26,3);
toc
% x0=leg; %步长为0.1
% y11=polyval(a11,x0); %返回值y0是对应于x0的函数值
% y12=polyval(a12,x0); %返回值y0是对应于x0的函数值
% y13=polyval(a13,x0); %返回值y0是对应于x0的函数值
% y14=polyval(a14,x0); %返回值y0是对应于x0的函数值
% y15=polyval(a15,x0); %返回值y0是对应于x0的函数值
% y16=polyval(a16,x0); %返回值y0是对应于x0的函数值
%
% y21=polyval(a21,x0); %返回值y0是对应于x0的函数值
% y22=polyval(a22,x0); %返回值y0是对应于x0的函数值
% y23=polyval(a23,x0); %返回值y0是对应于x0的函数值
% y24=polyval(a24,x0); %返回值y0是对应于x0的函数值
% y25=polyval(a25,x0); %返回值y0是对应于x0的函数值
% y26=polyval(a26,x0); %返回值y0是对应于x0的函数值
% subplot(3,4,1);plot(leg,k11,'o',x0,y11,'r');xlabel('x');ylabel('y');title('k11');
% subplot(3,4,2);plot(leg,k12,'o',x0,y12,'r');xlabel('x');ylabel('y');title('k12');
% subplot(3,4,5);plot(leg,k13,'o',x0,y13,'r');xlabel('x');ylabel('y');title('k13');
% subplot(3,4,6);plot(leg,k14,'o',x0,y14,'r');xlabel('x');ylabel('y');title('k14');
% subplot(3,4,9);plot(leg,k15,'o',x0,y15,'r');xlabel('x');ylabel('y');title('k15');
% subplot(3,4,10);plot(leg,k16,'o',x0,y16,'r');xlabel('x');ylabel('y');title('k16');
%
% subplot(3,4,3);plot(leg,k21,'o',x0,y21,'r');xlabel('x');ylabel('y');title('k21');
% subplot(3,4,4);plot(leg,k22,'o',x0,y22,'r');xlabel('x');ylabel('y');title('k22');
% subplot(3,4,7);plot(leg,k23,'o',x0,y23,'r');xlabel('x');ylabel('y');title('k23');
% subplot(3,4,8);plot(leg,k24,'o',x0,y24,'r');xlabel('x');ylabel('y');title('k24');
% subplot(3,4,11);plot(leg,k25,'o',x0,y25,'r');xlabel('x');ylabel('y');title('k25');
% subplot(3,4,12);plot(leg,k26,'o',x0,y26,'r');xlabel('x');ylabel('y');title('k26');
% grid on; %添加网格线
% set(gca,'GridLineStyle',':','GridColor','k','GridAlpha',1); %将网格线变成虚线
% fprintf('fp32 a11[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a11(1),a11(2),a11(3),a11(4));
% fprintf('fp32 a12[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a12(1),a12(2),a12(3),a12(4));
% fprintf('fp32 a13[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a13(1),a13(2),a13(3),a13(4));
% fprintf('fp32 a14[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a14(1),a14(2),a14(3),a14(4));
% fprintf('fp32 a15[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a15(1),a15(2),a15(3),a15(4));
% fprintf('fp32 a16[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a16(1),a16(2),a16(3),a16(4));
%
% fprintf('fp32 a21[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a21(1),a21(2),a21(3),a21(4));
% fprintf('fp32 a22[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a22(1),a22(2),a22(3),a22(4));
% fprintf('fp32 a23[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a23(1),a23(2),a23(3),a23(4));
% fprintf('fp32 a24[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a24(1),a24(2),a24(3),a24(4));
% fprintf('fp32 a25[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a25(1),a25(2),a25(3),a25(4));
% fprintf('fp32 a26[6] = {0,%.4f,%.4f,%.4f,%.4f};\n',a26(1),a26(2),a26(3),a26(4));
toc

56
utils/Simulation-master/balance/series_parallel_legs/get_k_length.m Normal file
View File

@ -0,0 +1,56 @@
function K = get_k_length(leg_length)
%theta : 摆杆与竖直方向夹角 R ：驱动轮半径
%x : 驱动轮位移 L : 摆杆重心到驱动轮轴距离
%phi : 机体与水平夹角 LM : 摆杆重心到其转轴距离
%T ：驱动轮输出力矩 l : 机体重心到其转轴距离
%Tp : 髋关节输出力矩 mw : 驱动轮转子质量
%N ：驱动轮对摆杆力的水平分量 mp : 摆杆质量
%P ：驱动轮对摆杆力的竖直分量 M : 机体质量
%Nm ：摆杆对机体力水平方向分量 Iw : 驱动轮转子转动惯量
%Pm ：摆杆对机体力竖直方向分量 Ip : 摆杆绕质心转动惯量
%Nf : 地面对驱动轮摩擦力 Im : 机体绕质心转动惯量
syms x(t) T R Iw mw M L LM theta(t) l phi(t) mp g Tp Ip IM
syms f1 f2 f3 d_theta d_x d_phi theta0 x0 phi0
R1=0.086; %驱动轮半径
L1=leg_length/2; %摆杆重心到驱动轮轴距离
LM1=leg_length/2; %摆杆重心到其转轴距离
l1=0.03; %机体质心距离转轴距离
mw1=1.18; %驱动轮质量
mp1=1.11; %杆质量
M1=10.3; %机体质量
Iw1=mw1*R1^2; %驱动轮转动惯量
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
NM = M*diff(x + (L + LM )*sin(theta)-l*sin(phi),t,2);
N = NM + mp*diff(x + L*sin(theta),t,2);
PM = M*g + M*diff((L+LM)*cos(theta)+l*cos(phi),t,2);
P = PM +mp*g+mp*diff(L*cos(theta),t,2);
eqn1 = diff(x,t,2) == (T -N*R)/(Iw/R + mw*R);
eqn2 = Ip*diff(theta,t,2) == (P*L + PM*LM)*sin(theta)-(N*L+NM*LM)*cos(theta)-T+Tp;
eqn3 = IM*diff(phi,t,2) == Tp +NM*l*cos(phi)+PM*l*sin(phi);
eqn10 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn1,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn20 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn2,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
eqn30 = subs(subs(subs(subs(subs(subs(subs(subs(subs(eqn3,diff(theta,t,2),f1),diff(x,t,2),f2),diff(phi,t,2),f3),diff(theta,t),d_theta),diff(x,t),d_x),diff(phi,t),d_phi),theta,theta0),x,x0),phi,phi0);
[f1,f2,f3] = solve(eqn10,eqn20,eqn30,f1,f2,f3);
A=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[theta0,d_theta,x0,d_x,phi0,d_phi]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
A=subs(A,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
A=double(A);
B=subs(jacobian([d_theta,f1,d_x,f2,d_phi,f3],[T,Tp]),[theta0,d_theta,d_x,phi0,d_phi,T,Tp],[0,0,0,0,0,0,0]);
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
B=double(B);
Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
R=[240 0;0 25]; %T Tp
K=lqr(A,B,Q,R);
end

BIN
utils/Simulation-master/balance/series_parallel_legs/physical_calc.mlx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/balance/series_parallel_legs/save_phy.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/dog_simple/dog.slx Normal file
View File

Binary file not shown.

BIN
utils/Simulation-master/dog_simple/dog.slx.r2023a Normal file
View File

Binary file not shown.

321
utils/lqr.asv
View File

@ -1,321 +0,0 @@
% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
% v2：添加了可以专门调试定腿长的功能（2024/05/08）
% v3：优化部分注释，添加单位说明（2024/05/15）
% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
tic
clear all
clc
% 定义机器人机体参数
syms R_w % 驱动轮半径
syms R_l % 驱动轮轮距/2
syms l_l l_r % 左右腿长
syms l_wl l_wr % 驱动轮质心到左右腿部质心距离
syms l_bl l_br % 机体质心到左右腿部质心距离
syms l_c % 机体质心到腿部关节中心点距离
syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量
syms I_w % 驱动轮转动惯量 (自然坐标系法向)
syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化)
syms I_b % 机体转动惯量 (自然坐标系法向)
syms I_z % 机器人z轴转动惯量 (简化为常量)
% 定义其他独立变量并补充其导数
syms theta_wl theta_wr % 左右驱动轮转角
syms dtheta_wl dtheta_wr
syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
% 定义状态向量
syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
% 定义控制向量
syms T_wl T_wr T_bl T_br
% 输入物理参数：重力加速度
syms g
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
A = sym('A',[10 10]);
B = sym('B',[10 4]);
% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
for p = 5:2:9
A_index = (p - 3)/2;
A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
for q = 6:2:10
A(q,p) = J_A(q/2,A_index);
end
end
% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
for r = 1:10
if rem(r,2) == 0
A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
else
A(r,:) = zeros(1,10);
A(r,r+1) = 1;
end
end
% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
for h = 1:4
B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
for f = 6:2:10
B(f,h) = J_B(f/2,h);
end
end
% B的其余数值为0
for e = 1:2:9
B(e,:) = zeros(1,4);
end
%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
g_ac = 9.81;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入机器人机体基本参数 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_w_ac = 0.77; % 驱动轮半径 （单位：m）
R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2）
%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m）
l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m）
I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2）
l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m）
l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m）
I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：kg m^2）
% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
% 两侧两个关节电机之间的中间点相连所形成的轴
% （如果目的是小板凳，考虑使左右腿相关数据一致）
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
% 通过以下方式记录数据: 矩阵分4列，
% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
% 第二列为l_wl，单位：m
% 第三列为l_bl，单位：m
% 第四列为I_ll，单位：kg m^2
% （注意单位别搞错！）
% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599;
0.12, 0.0470, 0.0730, 0.01862845;
0.13, 0.0476, 0.0824, 0.01898641;
0.14, 0.0480, 0.0920, 0.01931342;
0.15, 0.0490, 0.1010, 0.01962521;
0.16, 0.0500, 0.1100, 0.01993092;
0.17, 0.0510, 0.1190, 0.02023626;
0.18, 0.0525, 0.1275, 0.02054500;
0.19, 0.0539, 0.1361, 0.02085969;
0.20, 0.0554, 0.1446, 0.02118212;
0.21, 0.0570, 0.1530, 0.02151357;
0.22, 0.0586, 0.1614, 0.02185496;
0.23, 0.0600, 0.1700, 0.02220695;
0.24, 0.0621, 0.1779, 0.02256999;
0.25, 0.0639, 0.1861, 0.02294442;
0.26, 0.0657, 0.1943, 0.02333041;
0.27, 0.0676, 0.2024, 0.02372806;
0.28, 0.0700, 0.2100, 0.02413735;
0.29, 0.0713, 0.2187, 0.02455817;
0.30, 0.0733, 0.2267, 0.02499030];
% 以上数据应通过实际测量或sw图纸获得
% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
Leg_data_r = Leg_data_l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入QR矩阵 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 矩阵Q中，以下列分别对应：
% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 2, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 12000, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 200, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 1000, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 1, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 1000, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 20000, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 1];
% 其中：
% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数
% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
% 矩阵中，以下列分别对应：
% T_wl T_wr T_bl T_br
lqr_R = [0.25, 0, 0, 0;
0, 0.25, 0, 0;
0, 0, 1.5, 0;
0, 0, 0, 1.5];
% 其中：
% T_wl: 左侧驱动轮输出力矩
% T_wr：右侧驱动轮输出力矩
% T_bl：左侧髋关节输出力矩
% T_br：右腿髋关节输出力矩
% 单位皆为Nm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.')
%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
length = size(Leg_data_l,1); % 测量腿部数据集的行数
% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
% 是l_r，第三列是对应的K_ij的数值
K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
for i = 1:length
for j = 1:length
index = (i - 1)*length + j;
l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
l_wl_ac = Leg_data_l(i,2);
l_bl_ac = Leg_data_l(i,3);
I_ll_ac = Leg_data_l(i,4);
l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
l_wr_ac = Leg_data_r(j,2);
l_br_ac = Leg_data_r(j,3);
I_lr_ac = Leg_data_r(j,4);
for k = 1:40
K_sample(index,1,k) = l_l_ac;
K_sample(index,2,k) = l_r_ac;
end
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R);
% 根据指定的l_l,l_r输入对应的K_ij的数值
for l = 1:4
for m = 1:10
K_sample(index,3,(l - 1)*10 + m) = K(l,m);
end
end
end
end
% 创建收集全部K_ij的多项式拟合的全部系数的集合
K_Fit_Coefficients = zeros(40,6);
for n = 1:40
K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
end
Polynomial_expression = formula(K_Surface_Fit)
% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
% 每一行和K_ij的对应关系如下：
% - 第1行对应K_1,1
% - 第14行对应K_2,4
% - 第22行对应K_3,2
% - 第37行对应K_4,7
% ... 其他行对应关系类似
% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
% 每一列和系数pij的对应关系如下：
% - 第1列对应p00
% - 第2列对应p10
% - 第3列对应p01
% - 第4列对应p20
% - 第5列对应p11
% - 第6列对应p02
K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.')
% 正确食用方法：
% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
% 2.并填入对应系数即可
toc
%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
% （前面的蛆，以后再来探索吧（bushi
%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
% 基于机体以及物理参数，获得控制矩阵A，B的全部数值
A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
% 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
% P为Riccati方程的解，矩阵L可以无视
[P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
end

321
utils/lqr.m
View File

@ -1,321 +0,0 @@
% v1：这份LQR程序是参考我之前写的哈工程LQR程序以及小周写的AB矩阵求解器优化后写出来的，感谢周神（2024/05/07）
% v2：添加了可以专门调试定腿长的功能（2024/05/08）
% v3：优化部分注释，添加单位说明（2024/05/15）
% v4: 优化了输出，输出矩阵K的系数可以真正的复制到C里（2024/05/16）
% 以下所有变量含义参考2023上交轮腿电控开源（https://bbs.robomaster.com/forum.php?mod=viewthread&tid=22756）所使用符号含义
%%%%%%%%%%%%%%%%%%%%%%%%%Step 0：重置程序，定义变量%%%%%%%%%%%%%%%%%%%%%%%%%
tic
clear all
clc
% 定义机器人机体参数
syms R_w % 驱动轮半径
syms R_l % 驱动轮轮距/2
syms l_l l_r % 左右腿长
syms l_wl l_wr % 驱动轮质心到左右腿部质心距离
syms l_bl l_br % 机体质心到左右腿部质心距离
syms l_c % 机体质心到腿部关节中心点距离
syms m_w m_l m_b % 驱动轮质量 腿部质量 机体质量
syms I_w % 驱动轮转动惯量 (自然坐标系法向)
syms I_ll I_lr % 驱动轮左右腿部转动惯量 (自然坐标系法向，实际上会变化)
syms I_b % 机体转动惯量 (自然坐标系法向)
syms I_z % 机器人z轴转动惯量 (简化为常量)
% 定义其他独立变量并补充其导数
syms theta_wl theta_wr % 左右驱动轮转角
syms dtheta_wl dtheta_wr
syms ddtheta_wl ddtheta_wr ddtheta_ll ddtheta_lr ddtheta_b
% 定义状态向量
syms s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
% 定义控制向量
syms T_wl T_wr T_bl T_br
% 输入物理参数：重力加速度
syms g
%%%%%%%%%%%%%%%%%%%%%%%Step 1：解方程，求控制矩阵A，B%%%%%%%%%%%%%%%%%%%%%%%
% 通过原文方程组(3.11)-(3.15)，求出ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b表达式
eqn1 = (I_w*l_l/R_w+m_w*R_w*l_l+m_l*R_w*l_bl)*ddtheta_wl+(m_l*l_wl*l_bl-I_ll)*ddtheta_ll+(m_l*l_wl+m_b*l_l/2)*g*theta_ll+T_bl-T_wl*(1+l_l/R_w)==0;
eqn2 = (I_w*l_r/R_w+m_w*R_w*l_r+m_l*R_w*l_br)*ddtheta_wr+(m_l*l_wr*l_br-I_lr)*ddtheta_lr+(m_l*l_wr+m_b*l_r/2)*g*theta_lr+T_br-T_wr*(1+l_r/R_w)==0;
eqn3 = -(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wl-(m_w*R_w*R_w+I_w+m_l*R_w*R_w+m_b*R_w*R_w/2)*ddtheta_wr-(m_l*R_w*l_wl+m_b*R_w*l_l/2)*ddtheta_ll-(m_l*R_w*l_wr+m_b*R_w*l_r/2)*ddtheta_lr+T_wl+T_wr==0;
eqn4 = (m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wl+(m_w*R_w*l_c+I_w*l_c/R_w+m_l*R_w*l_c)*ddtheta_wr+m_l*l_wl*l_c*ddtheta_ll+m_l*l_wr*l_c*ddtheta_lr-I_b*ddtheta_b+m_b*g*l_c*theta_b-(T_wl+T_wr)*l_c/R_w-(T_bl+T_br)==0;
eqn5 = ((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wl-((I_z*R_w)/(2*R_l)+I_w*R_l/R_w)*ddtheta_wr+(I_z*l_l)/(2*R_l)*ddtheta_ll-(I_z*l_r)/(2*R_l)*ddtheta_lr-T_wl*R_l/R_w+T_wr*R_l/R_w==0;
[ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b] = solve(eqn1,eqn2,eqn3,eqn4,eqn5,ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b);
% 通过计算雅可比矩阵的方法得出控制矩阵A，B所需要的全部偏导数
J_A = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[theta_ll,theta_lr,theta_b]);
J_B = jacobian([ddtheta_wl,ddtheta_wr,ddtheta_ll,ddtheta_lr,ddtheta_b],[T_wl,T_wr,T_bl,T_br]);
% 定义矩阵A，B，将指定位置的数值根据上述偏导数计算出来并填入
A = sym('A',[10 10]);
B = sym('B',[10 4]);
% 填入A数据：a25,a27,a29,a45,a47,a49,a65,a67,a69,a85,a87,a89,a105,a107,a109
for p = 5:2:9
A_index = (p - 3)/2;
A(2,p) = R_w*(J_A(1,A_index) + J_A(2,A_index))/2;
A(4,p) = (R_w*(- J_A(1,A_index) + J_A(2,A_index)))/(2*R_l) - (l_l*J_A(3,A_index))/(2*R_l) + (l_r*J_A(4,A_index))/(2*R_l);
for q = 6:2:10
A(q,p) = J_A(q/2,A_index);
end
end
% A的以下数值为1：a12,a34,a56,a78,a910，其余数值为0
for r = 1:10
if rem(r,2) == 0
A(r,1) = 0; A(r,2) = 0; A(r,3) = 0; A(r,4) = 0; A(r,6) = 0; A(r,8) = 0; A(r,10) = 0;
else
A(r,:) = zeros(1,10);
A(r,r+1) = 1;
end
end
% 填入B数据：b21,b22,b23,b24,b41,b42,b43,b44,b61,b62,b63,b64,b81,b82,b83,b84,b101,b102,b103,b104,
for h = 1:4
B(2,h) = R_w*(J_B(1,h) + J_B(2,h))/2;
B(4,h) = (R_w*(- J_B(1,h) + J_B(2,h)))/(2*R_l) - (l_l*J_B(3,h))/(2*R_l) + (l_r*J_B(4,h))/(2*R_l);
for f = 6:2:10
B(f,h) = J_B(f/2,h);
end
end
% B的其余数值为0
for e = 1:2:9
B(e,:) = zeros(1,4);
end
%%%%%%%%%%%%%%%%%%%%%Step 2：输入参数（可以修改的部分）%%%%%%%%%%%%%%%%%%%%%
% 物理参数赋值（唯一此处不可改变！），后面的数据通过增加后缀_ac区分模型符号和实际数据
g_ac = 9.81;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入机器人机体基本参数 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人机体与轮部参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_w_ac = 0.77; % 驱动轮半径 （单位：m）
R_l_ac = 0.210; % 两个驱动轮之间距离/2 （单位：m）
l_c_ac = 0.025; % 机体质心到腿部关节中心点距离 （单位：m）
m_w_ac = 0.5; m_l_ac = 2.133; m_b_ac = 4.542; % 驱动轮质量 腿部质量 机体质量 （单位：kg）
I_w_ac = (7630000)*10^(-7); % 驱动轮转动惯量 （单位：kg m^2）
I_b_ac = 0.3470; % 机体转动惯量(自然坐标系法向) （单位：kg m^2）
I_z_ac = 0.322; % 机器人z轴转动惯量 （单位：kg m^2）
%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
l_l_ac = 0.16; % 左腿摆杆长度 （左腿对应数据） （单位：m）
l_wl_ac = 0.10; % 左驱动轮质心到左腿摆杆质心距离 （单位：m）
l_bl_ac = 0.4; % 机体转轴到左腿摆杆质心距离 （单位：m）
I_ll_ac = 0.01886; % 左腿摆杆转动惯量 （单位：kg m^2）
l_r_ac = 0.16; % 右腿摆杆长度 （右腿对应数据） （单位：m）
l_wr_ac = 0.10; % 右驱动轮质心到右腿摆杆质心距离 （单位：m）
l_br_ac = 0.4; % 机体转轴到右腿摆杆质心距离 （单位：m）
I_lr_ac = 0.01886; % 右腿摆杆转动惯量 （单位：kg m^2）
% 机体转轴定义参考哈工程开源（https://zhuanlan.zhihu.com/p/563048952），是左右
% 两侧两个关节电机之间的中间点相连所形成的轴
% （如果目的是小板凳，考虑使左右腿相关数据一致）
%%%%%%%%%%%%%%%%%%%%%%%%%%%机器人腿部参数数据集%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 根据不同腿长长度，先针对左腿测量出对应的l_wl，l_bl，和I_ll
% 通过以下方式记录数据: 矩阵分4列，
% 第一列为左腿腿长范围区间中所有小数点精度0.01的长度，例如：0.09，0.18，单位：m
% 第二列为l_wl，单位：m
% 第三列为l_bl，单位：m
% 第四列为I_ll，单位：kg m^2
% （注意单位别搞错！）
% 行数根据L_0范围区间（，单位cm时）的整数数量进行调整
Leg_data_l = [0.11, 0.0480, 0.0620, 0.01819599;
0.12, 0.0470, 0.0730, 0.01862845;
0.13, 0.0476, 0.0824, 0.01898641;
0.14, 0.0480, 0.0920, 0.01931342;
0.15, 0.0490, 0.1010, 0.01962521;
0.16, 0.0500, 0.1100, 0.01993092;
0.17, 0.0510, 0.1190, 0.02023626;
0.18, 0.0525, 0.1275, 0.02054500;
0.19, 0.0539, 0.1361, 0.02085969;
0.20, 0.0554, 0.1446, 0.02118212;
0.21, 0.0570, 0.1530, 0.02151357;
0.22, 0.0586, 0.1614, 0.02185496;
0.23, 0.0600, 0.1700, 0.02220695;
0.24, 0.0621, 0.1779, 0.02256999;
0.25, 0.0639, 0.1861, 0.02294442;
0.26, 0.0657, 0.1943, 0.02333041;
0.27, 0.0676, 0.2024, 0.02372806;
0.28, 0.0700, 0.2100, 0.02413735;
0.29, 0.0713, 0.2187, 0.02455817;
0.30, 0.0733, 0.2267, 0.02499030];
% 以上数据应通过实际测量或sw图纸获得
% 由于左右腿部数据通常完全相同，我们通过复制的方式直接定义右腿的全部数据集
% 矩阵分4列，第一列为右腿腿长范围区间中（，单位cm时）的整数腿长l_r*0.01，第二列为l_wr，第三列为l_br，第四列为I_lr）
Leg_data_r = Leg_data_l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 此处可以输入QR矩阵 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 矩阵Q中，以下列分别对应：
% s ds phi dphi theta_ll dtheta_ll theta_lr dtheta_lr theta_b dtheta_b
lqr_Q = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 2, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 12000, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 200, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 1000, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 1, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 1000, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 20000, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 1];
% 其中：
% s : 自然坐标系下机器人水平方向移动距离，单位：m，ds为其导数
% phi ：机器人水平方向移动时yaw偏航角度，dphi为其导数
% theta_ll：左腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_ll为其导数
% theta_lr：右腿摆杆与竖直方向（自然坐标系z轴）夹角，dtheta_lr为其导数
% theta_b ：机体与自然坐标系水平夹角，dtheta_b为其导数
% 矩阵中，以下列分别对应：
% T_wl T_wr T_bl T_br
lqr_R = [0.25, 0, 0, 0;
0, 0.25, 0, 0;
0, 0, 1.5, 0;
0, 0, 0, 1.5];
% 其中：
% T_wl: 左侧驱动轮输出力矩
% T_wr：右侧驱动轮输出力矩
% T_bl：左侧髋关节输出力矩
% T_br：右腿髋关节输出力矩
% 单位皆为Nm
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%Step 2.5：求解矩阵（定腿长调试用）%%%%%%%%%%%%%%%%%%%%%
% 如果需要使用此部分，请去掉120-127行以及215-218行注释，然后将224行之后的所有代码注释掉，
% 或者点击左侧数字"224"让程序只运行行之前的内容并停止
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
K = sprintf([strjoin(repmat({'%.5g'},1,size(K,2)),', ') '\n'], K.')
%%%%%%%%%%%%%%%%%%%%%%%%%%Step 3：拟合控制律函数%%%%%%%%%%%%%%%%%%%%%%%%%%
sample_size = size(Leg_data_l,1)^2; % 单个K_ij拟合所需要的样本数
length = size(Leg_data_l,1); % 测量腿部数据集的行数
% 定义所有K_ij依据l_l,l_r变化的表格，每一个表格有3列，第一列是l_l，第二列
% 是l_r，第三列是对应的K_ij的数值
K_sample = zeros(sample_size,3,40); % 40是因为增益矩阵K应该是4行10列。
for i = 1:length
for j = 1:length
index = (i - 1)*length + j;
l_l_ac = Leg_data_l(i,1); % 提取左腿对应的数据
l_wl_ac = Leg_data_l(i,2);
l_bl_ac = Leg_data_l(i,3);
I_ll_ac = Leg_data_l(i,4);
l_r_ac = Leg_data_r(j,1); % 提取右腿对应的数据
l_wr_ac = Leg_data_r(j,2);
l_br_ac = Leg_data_r(j,3);
I_lr_ac = Leg_data_r(j,4);
for k = 1:40
K_sample(index,1,k) = l_l_ac;
K_sample(index,2,k) = l_r_ac;
end
K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R);
% 根据指定的l_l,l_r输入对应的K_ij的数值
for l = 1:4
for m = 1:10
K_sample(index,3,(l - 1)*10 + m) = K(l,m);
end
end
end
end
% 创建收集全部K_ij的多项式拟合的全部系数的集合
K_Fit_Coefficients = zeros(40,6);
for n = 1:40
K_Surface_Fit = fit([K_sample(:,1,n),K_sample(:,2,n)],K_sample(:,3,n),'poly22');
K_Fit_Coefficients(n,:) = coeffvalues(K_Surface_Fit); % 拟合并提取出拟合的系数结果
end
Polynomial_expression = formula(K_Surface_Fit)
% 最终返回的结果K_Fit_Coefficients是一个40行6列矩阵，每一行分别对应一个K_ij的多项式拟合的全部系数
% 每一行和K_ij的对应关系如下：
% - 第1行对应K_1,1
% - 第14行对应K_2,4
% - 第22行对应K_3,2
% - 第37行对应K_4,7
% ... 其他行对应关系类似
% 拟合出的函数表达式为 p(x,y) = p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2
% 其中x对应左腿腿长l_l，y对应右腿腿长l_r
% K_Fit_Coefficients每一列分别对应全部K_ij的多项式拟合的单个系数
% 每一列和系数pij的对应关系如下：
% - 第1列对应p00
% - 第2列对应p10
% - 第3列对应p01
% - 第4列对应p20
% - 第5列对应p11
% - 第6列对应p02
K_Fit_Coefficients = sprintf([strjoin(repmat({'%.5g'},1,size(K_Fit_Coefficients,2)),', ') '\n'], K_Fit_Coefficients.')
% 正确食用方法：
% 1.在C代码中写出控制律K矩阵的全部多项式，其中每一个多项式的表达式为：
% p(l_l,l_r) = p00 + p10*l_l + p01*l_r + p20*l_l^2 + p11*l_l*l_r + p02*l_r^2
% 2.并填入对应系数即可
toc
%%%%%%%%%%%%%%%%%%%%%%%%%以下信息仅供参考，可忽略%%%%%%%%%%%%%%%%%%%%%%%%%%
% 如有需要可以把所有K_ij画出图来参考，可以去掉以下注释
% 此版本只能同时查看其中一个K_ij，同时查看多个的功能下次更新
% （前面的蛆，以后再来探索吧（bushi
%%%%%%%%%%%%%%%%%%%%%%%%%%得出控制矩阵K使用的函数%%%%%%%%%%%%%%%%%%%%%%%%%%
function K = get_K_from_LQR(R_w,R_l,l_l,l_r,l_wl,l_wr,l_bl,l_br,l_c,m_w,m_l,m_b,I_w,I_ll,I_lr,I_b,I_z,g, ...
R_w_ac,R_l_ac,l_l_ac,l_r_ac,l_wl_ac,l_wr_ac,l_bl_ac,l_br_ac, ...
l_c_ac,m_w_ac,m_l_ac,m_b_ac,I_w_ac,I_ll_ac,I_lr_ac,I_b_ac,I_z_ac,g_ac, ...
A,B,lqr_Q,lqr_R)
% 基于机体以及物理参数，获得控制矩阵A，B的全部数值
A_ac = subs(A,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
B_ac = subs(B,[R_w R_l l_l l_r l_wl l_wr l_bl l_br l_c m_w m_l m_b I_w I_ll I_lr I_b I_z g], ...
[R_w_ac R_l_ac l_l_ac l_r_ac l_wl_ac l_wr_ac l_bl_ac l_br_ac l_c_ac ...
m_w_ac m_l_ac m_b_ac I_w_ac I_ll_ac I_lr_ac I_b_ac I_z_ac g_ac]);
% 根据以上信息和提供的矩阵Q和R求解Riccati方程，获得增益矩阵K
% P为Riccati方程的解，矩阵L可以无视
[P,K,L_k] = icare(A_ac,B_ac,lqr_Q,lqr_R,[],[],[]);
end

219
utils/vmc.m
View File

@ -1,219 +0,0 @@
function [tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual)
% VMC (Virtual Model Control) for serial leg mechanism
% 基于虚拟模型控制的串联腿机构力矩计算
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
% F_virtual: 虚拟力大小 (N)
% theta_virtual: 虚拟力方向角度 (rad)
%
% 输出:
% tau_hip: 髋关节所需力矩 (N*m)
% tau_knee: 膝关节所需力矩 (N*m)
% 计算雅可比矩阵
J = compute_jacobian(L1, L2, q1, q2);
% 虚拟力向量 (笛卡尔空间)
F_cartesian = [F_virtual * cos(theta_virtual);
F_virtual * sin(theta_virtual)];
% 通过雅可比转置将笛卡尔力转换为关节力矩
tau = J' * F_cartesian;
tau_hip = tau(1);
tau_knee = tau(2);
end
function J = compute_jacobian(L1, L2, q1, q2)
% 计算串联腿机构的雅可比矩阵
% 从关节空间到足端笛卡尔空间的映射
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% J: 2x2雅可比矩阵
% 足端位置 (相对于髋关节)
% x = L1*cos(q1) + L2*cos(q1+q2)
% y = L1*sin(q1) + L2*sin(q1+q2)
% 雅可比矩阵元素
J11 = -L1*sin(q1) - L2*sin(q1+q2); % dx/dq1
J12 = -L2*sin(q1+q2); % dx/dq2
J21 = L1*cos(q1) + L2*cos(q1+q2); % dy/dq1
J22 = L2*cos(q1+q2); % dy/dq2
J = [J11, J12;
J21, J22];
end
function [x, y] = forward_kinematics(L1, L2, q1, q2)
% 串联腿正运动学 - 足端位置
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% x: 足端x坐标 (m)
% y: 足端y坐标 (m)
x = L1*cos(q1) + L2*cos(q1+q2);
y = L1*sin(q1) + L2*sin(q1+q2);
end
function [q1, q2] = inverse_kinematics(L1, L2, x, y)
% 串联腿逆运动学 - 关节角度
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% x: 足端目标x坐标 (m)
% y: 足端目标y坐标 (m)
%
% 输出:
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
% 计算到足端的距离
r = sqrt(x^2 + y^2);
% 检查工作空间
if r > (L1 + L2) || r < abs(L1 - L2)
error('目标位置超出工作空间范围');
end
% 使用余弦定理计算膝关节角度
cos_q2 = (r^2 - L1^2 - L2^2) / (2*L1*L2);
q2 = acos(cos_q2); % 肘部向上的解
% 计算髋关节角度
alpha = atan2(y, x);
beta = atan2(L2*sin(q2), L1 + L2*cos(q2));
q1 = alpha - beta;
end
function [angle_equiv, length_equiv] = serial_to_virtual_leg(L1, L2, q1, q2)
% 将串联腿转换为等效摆动杆表示 (对应C代码中的正运动学)
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
%
% 输出:
% angle_equiv: 等效摆动杆角度 (rad)
% length_equiv: 等效摆动杆长度 (m)
% 对应C代码中的计算
q4 = (pi - q1 - q2) / 2;
% 使用余弦定理求解等效摆动杆长度
a = 1.0;
b = -2.0 * L1 * cos(q4);
c = L1^2 - L2^2;
discriminant = b^2 - 4*a*c;
if discriminant < 0
error('无实数解，配置不可达');
end
sqrt_discriminant = sqrt(discriminant);
L3_1 = (-b + sqrt_discriminant) / (2*a);
L3_2 = (-b - sqrt_discriminant) / (2*a);
% 选择正的解
if L3_1 > 0 && L3_2 > 0
L3 = min(L3_1, L3_2); % 选择较小的解
elseif L3_1 > 0
L3 = L3_1;
elseif L3_2 > 0
L3 = L3_2;
else
error('无正解');
end
angle_equiv = q1 + q4;
length_equiv = L3;
end
function [q1, q2] = virtual_to_serial_leg(L1, L2, angle_equiv, length_equiv)
% 将等效摆动杆转换为串联腿表示 (对应C代码中的逆运动学)
%
% 输入参数:
% L1: 大腿长度 (m)
% L2: 小腿长度 (m)
% angle_equiv: 等效摆动杆角度 (rad)
% length_equiv: 等效摆动杆长度 (m)
%
% 输出:
% q1: 髋关节角度 (rad)
% q2: 膝关节角度 (rad)
h = length_equiv;
q = angle_equiv;
% 计算cos(q4)
cos_q4 = (L1^2 + h^2 - L2^2) / (2.0 * L1 * h);
% 检查范围
if cos_q4 < -1.0 || cos_q4 > 1.0
error('不可达配置');
end
q4 = acos(cos_q4);
% 计算关节角度
q1 = q - q4;
q2 = pi - 2.0 * q4 - q1;
end
% 示例使用函数
function demo_vmc_control()
% VMC控制示例
% 腿部参数
L1 = 0.15; % 大腿长度 15cm
L2 = 0.15; % 小腿长度 15cm
% 当前关节角度
q1 = pi/6; % 30度
q2 = pi/3; % 60度
% 期望的虚拟力
F_virtual = 50; % 50N
theta_virtual = -pi/2; % 向下的力
% 计算所需关节力矩
[tau_hip, tau_knee] = vmc_control(L1, L2, q1, q2, F_virtual, theta_virtual);
fprintf('髋关节所需力矩: %.3f N*m\n', tau_hip);
fprintf('膝关节所需力矩: %.3f N*m\n', tau_knee);
% 显示雅可比矩阵
J = compute_jacobian(L1, L2, q1, q2);
fprintf('雅可比矩阵:\n');
disp(J);
% 测试等效摆动杆转换
[angle_eq, length_eq] = serial_to_virtual_leg(L1, L2, q1, q2);
fprintf('等效摆动杆角度: %.3f rad (%.1f deg)\n', angle_eq, rad2deg(angle_eq));
fprintf('等效摆动杆长度: %.3f m\n', length_eq);
% 验证逆变换
[q1_back, q2_back] = virtual_to_serial_leg(L1, L2, angle_eq, length_eq);
fprintf('验证 - 原始角度: q1=%.3f, q2=%.3f\n', q1, q2);
fprintf('验证 - 恢复角度: q1=%.3f, q2=%.3f\n', q1_back, q2_back);
end