修好了
This commit is contained in:
parent
ea97df03a8
commit
221673f702
1358
Chassis_Task.c
1358
Chassis_Task.c
File diff suppressed because it is too large
Load Diff
299
Chassis_Task.h
299
Chassis_Task.h
@ -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
|
||||
@ -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
|
||||
|
||||
185
LQR_README.md
185
LQR_README.md
@ -1,185 +0,0 @@
|
||||
# LQR控制器设计文档
|
||||
|
||||
本文档描述了为平衡步兵轮腿机器人设计的LQR（线性二次调节器）控制器。
|
||||
|
||||
## 1. 系统建模
|
||||
|
||||
### 1.1 状态变量定义
|
||||
|
||||
系统状态向量为6维：
|
||||
```
|
||||
x = [theta, d_theta, x, d_x, phi, d_phi]^T
|
||||
```
|
||||
|
||||
其中：
|
||||
- `theta`: 等效摆杆与竖直方向夹角 (rad)
|
||||
- `d_theta`: 等效摆杆角速度 (rad/s)
|
||||
- `x`: 驱动轮位移 (m)
|
||||
- `d_x`: 驱动轮速度 (m/s)
|
||||
- `phi`: 机体与水平夹角（俯仰角） (rad)
|
||||
- `d_phi`: 机体俯仰角速度 (rad/s)
|
||||
|
||||
### 1.2 控制输入
|
||||
|
||||
控制输入为2维：
|
||||
```
|
||||
u = [T, Tp]^T
|
||||
```
|
||||
|
||||
其中：
|
||||
- `T`: 驱动轮输出力矩 (N·m)
|
||||
- `Tp`: 髋关节输出力矩 (N·m)
|
||||
|
||||
### 1.3 系统动力学方程
|
||||
|
||||
基于拉格朗日方程建立的系统动力学方程组：
|
||||
|
||||
1. **驱动轮动力学**：
|
||||
```
|
||||
d²x/dt² = (T - N*R)/(Iw/R + mw*R)
|
||||
```
|
||||
|
||||
2. **摆杆动力学**：
|
||||
```
|
||||
Ip*d²theta/dt² = (P*L + PM*LM)*sin(theta) - (N*L + NM*LM)*cos(theta) - T + Tp
|
||||
```
|
||||
|
||||
3. **机体动力学**：
|
||||
```
|
||||
IM*d²phi/dt² = Tp + NM*l*cos(phi) + PM*l*sin(phi)
|
||||
```
|
||||
|
||||
### 1.4 线性化
|
||||
|
||||
在平衡点附近进行线性化，得到状态空间模型：
|
||||
```
|
||||
dx/dt = A*x + B*u
|
||||
```
|
||||
|
||||
其中A和B矩阵通过雅可比矩阵计算得到。
|
||||
|
||||
## 2. LQR控制器设计
|
||||
|
||||
### 2.1 代价函数
|
||||
|
||||
LQR控制器最小化以下二次代价函数：
|
||||
```
|
||||
J = ∫[x^T*Q*x + u^T*R*u]dt
|
||||
```
|
||||
|
||||
权重矩阵选择：
|
||||
- `Q = diag([100, 1, 500, 100, 5000, 1])` (状态权重)
|
||||
- `R = diag([240, 25])` (控制权重)
|
||||
|
||||
### 2.2 增益矩阵计算
|
||||
|
||||
由于系统参数随腿长变化，采用多项式拟合方法：
|
||||
|
||||
1. **离线计算**：对不同腿长（0.1-0.4m），使用MATLAB计算最优LQR增益
|
||||
2. **多项式拟合**：对每个增益元素进行3阶多项式拟合
|
||||
3. **在线计算**：根据当前腿长实时计算增益矩阵
|
||||
|
||||
### 2.3 控制律
|
||||
|
||||
```
|
||||
u = -K(L) * (x - x_ref)
|
||||
```
|
||||
|
||||
其中：
|
||||
- `K(L)`: 与腿长L相关的增益矩阵
|
||||
- `x_ref`: 参考状态
|
||||
|
||||
## 3. 实现架构
|
||||
|
||||
### 3.1 文件结构
|
||||
|
||||
```
|
||||
User/component/
|
||||
├── lqr.h # LQR控制器头文件
|
||||
├── lqr.c # LQR控制器实现
|
||||
└── lqr_config_example.c # 配置示例
|
||||
```
|
||||
|
||||
### 3.2 主要功能模块
|
||||
|
||||
1. **LQR_Init()**: 初始化控制器
|
||||
2. **LQR_UpdateState()**: 更新系统状态
|
||||
3. **LQR_CalculateGainMatrix()**: 计算增益矩阵
|
||||
4. **LQR_Control()**: 执行控制计算
|
||||
5. **LQR_GetOutput()**: 获取控制输出
|
||||
|
||||
### 3.3 与VMC的集成
|
||||
|
||||
LQR控制器输出虚拟力，通过VMC转换为关节力矩：
|
||||
|
||||
```c
|
||||
// LQR计算虚拟力和力矩
|
||||
LQR_Control(&chassis->lqr);
|
||||
LQR_GetOutput(&chassis->lqr, &lqr_output);
|
||||
|
||||
// VMC将虚拟力矩转换为关节力矩
|
||||
VMC_InverseSolve(&chassis->vmc[0], virtual_force, lqr_output.Tp);
|
||||
VMC_GetJointTorques(&chassis->vmc[0], &tau_front, &tau_rear);
|
||||
```
|
||||
|
||||
## 4. 调试和优化
|
||||
|
||||
### 4.1 参数调优
|
||||
|
||||
1. **Q矩阵调优**：
|
||||
- 增大theta权重提高平衡性能
|
||||
- 增大phi权重减少机体摆动
|
||||
- 增大x,d_x权重提高位置跟踪精度
|
||||
|
||||
2. **R矩阵调优**：
|
||||
- 增大R减少控制输出，提高稳定性
|
||||
- 减小R提高响应速度
|
||||
|
||||
### 4.2 常见问题
|
||||
|
||||
1. **增益过大**：导致系统震荡，需减小Q或增大R
|
||||
2. **响应迟缓**：增大Q权重或减小R权重
|
||||
3. **静态误差**：检查目标状态设置是否合理
|
||||
|
||||
### 4.3 监控指标
|
||||
|
||||
建议监控以下指标：
|
||||
- 状态误差大小
|
||||
- 控制输出饱和情况
|
||||
- 系统稳定性指标
|
||||
|
||||
## 5. 使用示例
|
||||
|
||||
```c
|
||||
// 1. 配置LQR参数
|
||||
Chassis_Params_t chassis_params = {
|
||||
.lqr_param = {
|
||||
.max_wheel_torque = 10.0f,
|
||||
.max_joint_torque = 5.0f,
|
||||
},
|
||||
.lqr_gains = example_lqr_gains,
|
||||
};
|
||||
|
||||
// 2. 初始化底盘（自动初始化LQR）
|
||||
Chassis_Init(&chassis, &chassis_params, 1000.0f);
|
||||
|
||||
// 3. 控制循环
|
||||
while(1) {
|
||||
// 更新传感器数据
|
||||
Chassis_UpdateFeedback(&chassis);
|
||||
Chassis_UpdateIMU(&chassis, imu_data);
|
||||
|
||||
// 执行LQR控制
|
||||
Chassis_Control(&chassis, &cmd);
|
||||
|
||||
// 延时
|
||||
osDelay(1);
|
||||
}
|
||||
```
|
||||
|
||||
## 6. 参考资料
|
||||
|
||||
1. MATLAB仿真代码：`utils/Simulation-master/balance/series_legs/`
|
||||
2. C++参考实现：`User/module/mod_wheelleg_chassis.cpp`
|
||||
3. VMC控制器：`User/component/vmc.c`
|
||||
4. 理论参考：Modern Control Engineering, Ogata
|
||||
67
LQR_修正建议.md
67
LQR_修正建议.md
@ -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模型进行修正。
|
||||
主要是要确保状态向量定义、增益矩阵维度和控制律计算都与理论模型一致。
|
||||
@ -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
297
User/README.md
@ -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参数设计工具
|
||||
- 完善的安全保护机制
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
@ -3,32 +3,49 @@
|
||||
#include "bsp/can.h"
|
||||
#include "component/user_math.h"
|
||||
#include "component/kinematics.h"
|
||||
#include "component/limiter.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @param c
|
||||
* @return
|
||||
*/
|
||||
static int8_t Chassis_MotorEnable(Chassis_t *c) {
|
||||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||||
|
||||
float fn=0.0f;
|
||||
float tp=0.0f;
|
||||
|
||||
float t1=0.0f;
|
||||
float t2=0.0f;
|
||||
|
||||
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;
|
||||
@ -91,6 +108,8 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
|
||||
PID_Init(&c->pid.leglength[1], KPID_MODE_CALC_D, target_freq, ¶m->leglength);
|
||||
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw);
|
||||
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll);
|
||||
PID_Init(&c->pid.tp_pid[0], KPID_MODE_CALC_D, target_freq, ¶m->tp_pid);
|
||||
PID_Init(&c->pid.tp_pid[1], KPID_MODE_CALC_D, target_freq, ¶m->tp_pid);
|
||||
|
||||
/*初始化LQR控制器*/
|
||||
LQR_Init(&c->lqr[0], param->lqr_param.max_wheel_torque, param->lqr_param.max_joint_torque);
|
||||
@ -273,6 +292,7 @@ void Chassis_Output(Chassis_t *c) {
|
||||
}
|
||||
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);
|
||||
}
|
||||
// 这个函数已经在各个模式中直接调用了电机输出函数
|
||||
// 如果需要统一输出，可以在这里实现
|
||||
@ -310,7 +330,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
xhat = c->chassis_state.position_x;
|
||||
|
||||
// 目标设定
|
||||
target_dot_x = c_cmd->move_vec.vx;
|
||||
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控制 */
|
||||
@ -331,14 +352,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
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.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 - 0.56f; // 目标位置
|
||||
// target_state.d_x = target_dot_x; // 目标速度
|
||||
target_state.x = target_x; // 目标位置
|
||||
target_state.d_x = target_dot_x; // 目标速度
|
||||
// target_state.phi = 0.16f; // 目标俯仰角
|
||||
// target_state.d_phi = 0.0f; // 目标俯仰角速度
|
||||
|
||||
@ -356,6 +377,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
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;
|
||||
@ -374,47 +397,16 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
}
|
||||
}
|
||||
|
||||
/* 转向控制（参考C++版本） */
|
||||
float yaw_force = 0.0f;
|
||||
|
||||
// 更新当前yaw角度
|
||||
c->yaw_control.current_yaw = c->feedback.imu.euler.yaw;
|
||||
|
||||
// 更新目标yaw角度（基于遥控器输入）
|
||||
if (fabsf(c_cmd->move_vec.wz) > 0.01f) {
|
||||
// 有转向指令时，更新目标角度
|
||||
c->yaw_control.target_yaw += c_cmd->move_vec.wz * c->dt;
|
||||
// 角度归一化到 [-π, π]
|
||||
while (c->yaw_control.target_yaw > M_PI) {
|
||||
c->yaw_control.target_yaw -= M_2PI;
|
||||
}
|
||||
while (c->yaw_control.target_yaw < -M_PI) {
|
||||
c->yaw_control.target_yaw += M_2PI;
|
||||
}
|
||||
}
|
||||
c->yaw_control.target_yaw -= c_cmd->move_vec.vy * 0.002f; // 目标偏航角，假设遥控器输入范围为[-10, 10]，映射到[-0.02, 0.02] rad/s
|
||||
|
||||
if (onground_flag[0] && onground_flag[1]) {
|
||||
// 使用PID控制yaw角度
|
||||
float yaw_error = c->yaw_control.target_yaw - c->yaw_control.current_yaw;
|
||||
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);
|
||||
|
||||
// 处理角度循环误差（选择最短路径）
|
||||
if (yaw_error > M_PI) {
|
||||
yaw_error -= M_2PI;
|
||||
} else if (yaw_error < -M_PI) {
|
||||
yaw_error += M_2PI;
|
||||
}
|
||||
|
||||
// 使用yaw轴陀螺仪作为微分项
|
||||
float yaw_d_feedback = c->feedback.imu.gyro.z;
|
||||
|
||||
yaw_force = PID_Calc(&c->pid.yaw, 0.0f, yaw_error, yaw_d_feedback, c->dt);
|
||||
c->yaw_control.yaw_force = yaw_force;
|
||||
}
|
||||
|
||||
/* 轮毂力矩输出（参考C++版本的减速比） */
|
||||
c->output.wheel[0] = Tw[0] / 4.9256f - yaw_force; // 左轮
|
||||
c->output.wheel[1] = Tw[1] / 4.9256f + yaw_force; // 右轮
|
||||
|
||||
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];
|
||||
@ -424,24 +416,36 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
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.12f + c_cmd->height * 0.1f + roll_compensation; // 左腿：基础腿长 + 高度调节 + 横滚角补偿
|
||||
target_L0[1] = 0.12f + c_cmd->height * 0.1f - roll_compensation; // 右腿：基础腿长 + 高度调节 - 横滚角补偿
|
||||
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] * sinf(leg_theta[leg]) +
|
||||
pid_output + // PID腿长控制输出
|
||||
45.0f; // 基础支撑力
|
||||
virtual_force[leg] = (Tp[leg] + Delta_Tp[leg]) * sinf(leg_theta[leg]) +
|
||||
pid_output + 20.0f;
|
||||
// + // PID腿长控制输出
|
||||
// 45.0f; // 基础支撑力
|
||||
|
||||
// VMC逆解算
|
||||
if (VMC_InverseSolve(&c->vmc_[leg], virtual_force[leg], Tp[leg]) == 0) {
|
||||
// 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 {
|
||||
|
||||
@ -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;
|
||||
@ -73,6 +73,7 @@ typedef struct {
|
||||
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]; /* 四个关节电机参数 */
|
||||
@ -151,9 +152,8 @@ typedef struct {
|
||||
KPID_t balance; /* 平衡用的PID */
|
||||
KPID_t yaw; /* yaw轴控制PID */
|
||||
KPID_t roll; /* 横滚角控制PID */
|
||||
|
||||
KPID_t tp_pid[2];
|
||||
KPID_t leglength[2]; /* 两条腿的腿长PID */
|
||||
|
||||
} pid;
|
||||
|
||||
/* 滤波器 */
|
||||
@ -186,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 运行底盘控制逻辑
|
||||
|
||||
@ -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
|
||||
@ -69,22 +69,22 @@ Config_RobotParam_t robot_config = {
|
||||
|
||||
.chassis_param = {
|
||||
.leglength={
|
||||
.k = 5.0f,
|
||||
.p = 15.0f,
|
||||
.k = 20.0f,
|
||||
.p = 1.0f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
.out_limit = 10.0f,
|
||||
.out_limit = 100.0f,
|
||||
.d_cutoff_freq = -1.0f,
|
||||
.range = -1.0f,
|
||||
},
|
||||
.yaw={
|
||||
.k=0.01f,
|
||||
.p=0.1f,
|
||||
.k=1.0f,
|
||||
.p=1.0f,
|
||||
.i=0.0f,
|
||||
.d=0.0f,
|
||||
.i_limit=0.0f,
|
||||
.out_limit=0.2f,
|
||||
.out_limit=1.0f,
|
||||
.d_cutoff_freq=30.0f,
|
||||
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
|
||||
},
|
||||
@ -99,6 +99,17 @@ Config_RobotParam_t robot_config = {
|
||||
.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,
|
||||
@ -171,18 +182,58 @@ Config_RobotParam_t robot_config = {
|
||||
}
|
||||
},
|
||||
.lqr_gains ={
|
||||
.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 = { -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
244
User/整合方案说明.md
@ -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. **回滚机制**：确保可以快速回到原有系统
|
||||
|
||||
这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？
|
||||
171
some_functions.c
171
some_functions.c
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
66
test_vmc.c
66
test_vmc.c
@ -1,66 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include "User/component/vmc.h"
|
||||
|
||||
int main() {
|
||||
// 测试VMC计算
|
||||
VMC_t vmc;
|
||||
VMC_Param_t param = {
|
||||
.hip_length = 0.1f, // 10cm髋关节间距
|
||||
.leg_1 = 0.15f, // 15cm大腿前端
|
||||
.leg_2 = 0.15f, // 15cm大腿后端
|
||||
.leg_3 = 0.15f, // 15cm小腿
|
||||
.leg_4 = 0.15f, // 15cm小腿前端
|
||||
.wheel_radius = 0.05f, // 5cm轮子半径
|
||||
.wheel_mass = 1.0f // 1kg轮子质量
|
||||
};
|
||||
|
||||
if (VMC_Init(&vmc, ¶m, 1000.0f) != 0) {
|
||||
printf("VMC初始化失败\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
printf("测试VMC腿长计算\n");
|
||||
printf("髋关节间距: %.3f\n", param.hip_length);
|
||||
printf("腿段长度: L1=%.3f, L2=%.3f, L3=%.3f, L4=%.3f\n",
|
||||
param.leg_1, param.leg_2, param.leg_3, param.leg_4);
|
||||
printf("\n");
|
||||
|
||||
// 测试不同的关节角度配置
|
||||
float test_angles[][2] = {
|
||||
{0.0f, 0.0f}, // 水平伸展
|
||||
{0.5f, -0.5f}, // 轻微弯曲
|
||||
{1.0f, -1.0f}, // 中等弯曲
|
||||
{1.5f, -1.5f}, // 较大弯曲
|
||||
{-0.5f, 0.5f}, // 反向弯曲
|
||||
};
|
||||
|
||||
int num_tests = sizeof(test_angles) / sizeof(test_angles[0]);
|
||||
|
||||
for (int i = 0; i < num_tests; i++) {
|
||||
float phi1 = test_angles[i][0];
|
||||
float phi4 = test_angles[i][1];
|
||||
float body_pitch = 0.0f;
|
||||
float body_pitch_rate = 0.0f;
|
||||
|
||||
if (VMC_ForwardSolve(&vmc, phi1, phi4, body_pitch, body_pitch_rate) == 0) {
|
||||
float length, angle, d_length, d_angle;
|
||||
VMC_GetVirtualLegState(&vmc, &length, &angle, &d_length, &d_angle);
|
||||
|
||||
float foot_x, foot_y;
|
||||
VMC_GetFootPosition(&vmc, &foot_x, &foot_y);
|
||||
|
||||
printf("测试 %d: phi1=%.2f, phi4=%.2f\n", i+1, phi1, phi4);
|
||||
printf(" 足端位置: (%.3f, %.3f)\n", foot_x, foot_y);
|
||||
printf(" 虚拟腿长: %.3f\n", length);
|
||||
printf(" 虚拟摆角: %.3f (%.1f度)\n", angle, angle * 180.0f / M_PI);
|
||||
printf(" 内部变量: XC=%.3f, YC=%.3f, phi2=%.3f, phi3=%.3f\n",
|
||||
vmc.leg.XC, vmc.leg.YC, vmc.leg.phi2, vmc.leg.phi3);
|
||||
printf("\n");
|
||||
} else {
|
||||
printf("测试 %d: 求解失败 (phi1=%.2f, phi4=%.2f)\n", i+1, phi1, phi4);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -37,46 +37,60 @@ 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));
|
||||
% 打印格式化的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
|
||||
|
||||
@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
|
||||
R1=0.072; %驱动轮半径
|
||||
L1=leg_length/2; %摆杆重心到驱动轮轴距离
|
||||
LM1=leg_length/2; %摆杆重心到其转轴距离
|
||||
l1=0.03; %机体质心距离转轴距离
|
||||
l1=-0.03; %机体质心距离转轴距离
|
||||
mw1=0.60; %驱动轮质量
|
||||
mp1=1.8; %杆质量
|
||||
M1=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; %机体绕质心转动惯量
|
||||
@ -48,7 +48,7 @@ function K = get_k_length(leg_length)
|
||||
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
|
||||
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);
|
||||
|
||||
321
utils/lqr.asv
321
utils/lqr.asv
@ -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
321
utils/lqr.m
@ -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
|
||||
|
||||
173
utils/vmc.m
173
utils/vmc.m
@ -1,173 +0,0 @@
|
||||
%% 五连杆机构腿长计算脚本
|
||||
clear; clc;
|
||||
|
||||
%% 定义输入参数
|
||||
% 连杆长度 (m)
|
||||
l1 = 0.215; % 连杆1长度
|
||||
l2 = 0.258; % 连杆2长度
|
||||
l3 = 0.258; % 连杆3长度
|
||||
l4 = 0.215; % 连杆4长度
|
||||
l5 = 0.0; % 连杆5长度
|
||||
|
||||
% 关节角度 (弧度)
|
||||
phi1 = 0.10; % 30度
|
||||
phi4 = 2.96; % 45度
|
||||
|
||||
%% 调用腿长计算函数
|
||||
fprintf('五连杆机构腿长计算\n');
|
||||
fprintf('输入参数:\n');
|
||||
fprintf(' 连杆长度: l1=%.3f, l2=%.3f, l3=%.3f, l4=%.3f, l5=%.3f (m)\n', l1, l2, l3, l4, l5);
|
||||
fprintf(' 关节角度: phi1=%.3f (%.1f°), phi4=%.3f (%.1f°)\n', phi1, rad2deg(phi1), phi4, rad2deg(phi4));
|
||||
|
||||
try
|
||||
% 计算各关键点位置
|
||||
% B点 (连杆1末端)
|
||||
x_B = l1 * cos(phi1);
|
||||
y_B = l1 * sin(phi1);
|
||||
|
||||
% D点 (连杆4末端)
|
||||
x_D = l5 + l4 * cos(phi4);
|
||||
y_D = l4 * sin(phi4);
|
||||
|
||||
% 计算BD距离
|
||||
BD_dist = sqrt((x_D - x_B)^2 + (y_D - y_B)^2);
|
||||
|
||||
% 检查三角形约束条件
|
||||
if BD_dist > (l2 + l3) || BD_dist < abs(l2 - l3)
|
||||
warning('无法形成闭合五连杆机构！BD距离=%.3f, 需要满足: %.3f < BD < %.3f', ...
|
||||
BD_dist, abs(l2-l3), l2+l3);
|
||||
leg_length = NaN;
|
||||
foot_pos = [NaN, NaN];
|
||||
return;
|
||||
end
|
||||
|
||||
% 使用余弦定理求解phi2
|
||||
cos_phi2_BD = (l2^2 + BD_dist^2 - l3^2) / (2 * l2 * BD_dist);
|
||||
|
||||
% 检查余弦值是否在有效范围内
|
||||
if abs(cos_phi2_BD) > 1
|
||||
warning('余弦值超出范围: %.3f', cos_phi2_BD);
|
||||
leg_length = NaN;
|
||||
foot_pos = [NaN, NaN];
|
||||
return;
|
||||
end
|
||||
|
||||
% 计算BD方向角
|
||||
alpha_BD = atan2(y_D - y_B, x_D - x_B);
|
||||
|
||||
% 计算phi2 (两个解，选择合理的一个)
|
||||
angle_offset = acos(cos_phi2_BD);
|
||||
phi2_solution1 = alpha_BD + angle_offset;
|
||||
phi2_solution2 = alpha_BD - angle_offset;
|
||||
|
||||
% 选择合理的解 (通常选择使机构在合理配置的解)
|
||||
phi2 = phi2_solution1; % 可以根据实际需要调整选择逻辑
|
||||
|
||||
% 计算C点位置 (足端位置)
|
||||
x_C = x_B + l2 * cos(phi2);
|
||||
y_C = y_B + l2 * sin(phi2);
|
||||
|
||||
% 验证C点到D点距离是否等于l3
|
||||
CD_dist = sqrt((x_D - x_C)^2 + (y_D - y_C)^2);
|
||||
error_CD = abs(CD_dist - l3);
|
||||
|
||||
if error_CD > 1e-6
|
||||
fprintf('警告: CD距离误差较大 (%.6f)\n', error_CD);
|
||||
end
|
||||
|
||||
% 计算腿长 (假设为原点到C点的距离)
|
||||
leg_length = sqrt(x_C^2 + y_C^2);
|
||||
foot_pos = [x_C, y_C];
|
||||
|
||||
% 输出结果
|
||||
fprintf('\n计算结果:\n');
|
||||
fprintf(' B点位置: (%.3f, %.3f)\n', x_B, y_B);
|
||||
fprintf(' C点位置: (%.3f, %.3f)\n', x_C, y_C);
|
||||
fprintf(' D点位置: (%.3f, %.3f)\n', x_D, y_D);
|
||||
fprintf(' phi2角度: %.3f rad (%.1f°)\n', phi2, rad2deg(phi2));
|
||||
fprintf(' 腿长: %.3f m\n', leg_length);
|
||||
fprintf(' BD距离: %.3f m\n', BD_dist);
|
||||
fprintf(' CD验证: %.6f m (应该等于l3=%.3f)\n', CD_dist, l3);
|
||||
|
||||
%% 可视化结果
|
||||
figure;
|
||||
plot([0, x_B], [0, y_B], 'b-', 'LineWidth', 2); % 连杆1
|
||||
hold on;
|
||||
plot([x_B, x_C], [y_B, y_C], 'r-', 'LineWidth', 2); % 连杆2
|
||||
plot([x_C, x_D], [y_C, y_D], 'g-', 'LineWidth', 2); % 连杆3
|
||||
plot([l5, x_D], [0, y_D], 'm-', 'LineWidth', 2); % 连杆4
|
||||
plot([0, l5], [0, 0], 'k-', 'LineWidth', 3); % 连杆5 (基座)
|
||||
|
||||
% 标记关键点
|
||||
plot(0, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % A点
|
||||
plot(x_B, y_B, 'bo', 'MarkerSize', 8, 'MarkerFaceColor', 'b'); % B点
|
||||
plot(x_C, y_C, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % C点
|
||||
plot(x_D, y_D, 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % D点
|
||||
plot(l5, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % E点
|
||||
|
||||
% 标记腿长
|
||||
plot([0, x_C], [0, y_C], 'k--', 'LineWidth', 1); % 腿长线
|
||||
|
||||
% 图形设置
|
||||
grid on;
|
||||
axis equal;
|
||||
xlabel('X (m)');
|
||||
ylabel('Y (m)');
|
||||
title('五连杆机构示意图');
|
||||
legend('连杆1', '连杆2', '连杆3', '连杆4', '基座', 'Location', 'best');
|
||||
|
||||
% 添加文本标注
|
||||
text(0, 0-0.02, 'A', 'HorizontalAlignment', 'center');
|
||||
text(x_B, y_B+0.02, 'B', 'HorizontalAlignment', 'center');
|
||||
text(x_C, y_C+0.02, 'C(足端)', 'HorizontalAlignment', 'center');
|
||||
text(x_D, y_D+0.02, 'D', 'HorizontalAlignment', 'center');
|
||||
text(l5, 0-0.02, 'E', 'HorizontalAlignment', 'center');
|
||||
text(x_C/2, y_C/2, sprintf('腿长=%.3fm', leg_length), 'HorizontalAlignment', 'center');
|
||||
|
||||
catch ME
|
||||
fprintf('计算过程中出现错误: %s\n', ME.message);
|
||||
leg_length = NaN;
|
||||
foot_pos = [NaN, NaN];
|
||||
end
|
||||
|
||||
%% 多组参数测试
|
||||
fprintf('\n\n=== 多组参数测试 ===\n');
|
||||
test_cases = [
|
||||
pi/6, pi/4; % 30°, 45°
|
||||
pi/4, pi/3; % 45°, 60°
|
||||
-pi/6, pi/2; % -30°, 90°
|
||||
0, pi/6; % 0°, 30°
|
||||
];
|
||||
|
||||
for i = 1:size(test_cases, 1)
|
||||
phi1_test = test_cases(i, 1);
|
||||
phi4_test = test_cases(i, 2);
|
||||
|
||||
fprintf('\n测试 %d: phi1=%.1f°, phi4=%.1f°\n', i, rad2deg(phi1_test), rad2deg(phi4_test));
|
||||
|
||||
% 重复计算过程（简化版）
|
||||
x_B_test = l1 * cos(phi1_test);
|
||||
y_B_test = l1 * sin(phi1_test);
|
||||
x_D_test = l5 + l4 * cos(phi4_test);
|
||||
y_D_test = l4 * sin(phi4_test);
|
||||
BD_dist_test = sqrt((x_D_test - x_B_test)^2 + (y_D_test - y_B_test)^2);
|
||||
|
||||
if BD_dist_test <= (l2 + l3) && BD_dist_test >= abs(l2 - l3)
|
||||
cos_phi2_BD_test = (l2^2 + BD_dist_test^2 - l3^2) / (2 * l2 * BD_dist_test);
|
||||
if abs(cos_phi2_BD_test) <= 1
|
||||
alpha_BD_test = atan2(y_D_test - y_B_test, x_D_test - x_B_test);
|
||||
angle_offset_test = acos(cos_phi2_BD_test);
|
||||
phi2_test = alpha_BD_test + angle_offset_test;
|
||||
x_C_test = x_B_test + l2 * cos(phi2_test);
|
||||
y_C_test = y_B_test + l2 * sin(phi2_test);
|
||||
leg_length_test = sqrt(x_C_test^2 + y_C_test^2);
|
||||
fprintf(' 结果: 腿长=%.3f m, 足端位置=(%.3f, %.3f)\n', leg_length_test, x_C_test, y_C_test);
|
||||
else
|
||||
fprintf(' 结果: 余弦值超出范围\n');
|
||||
end
|
||||
else
|
||||
fprintf(' 结果: 无法形成闭合机构\n');
|
||||
end
|
||||
end
|
||||
|
||||
fprintf('\n计算完成！\n');
|
||||
Loading…
Reference in New Issue
Block a user