添加电机
This commit is contained in:
parent
843ca3d931
commit
e75094a41d
321
HerKules_VOCAL_SJ_LQR_v4_with_data.m
Normal file
321
HerKules_VOCAL_SJ_LQR_v4_with_data.m
Normal file
@ -0,0 +1,321 @@
|
||||
% 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
|
||||
|
||||
@ -854,6 +854,11 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\module\config.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>balance_chassis.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\module\balance_chassis.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
@ -889,6 +894,11 @@
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\atti_esti.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ctrl_chassis.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\User\task\ctrl_chassis.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
||||
@ -82,11 +82,13 @@
|
||||
"devc\motor_lk.o"
|
||||
"devc\motor_lz.o"
|
||||
"devc\config.o"
|
||||
"devc\balance_chassis.o"
|
||||
"devc\blink.o"
|
||||
"devc\init.o"
|
||||
"devc\rc.o"
|
||||
"devc\user_task.o"
|
||||
"devc\atti_esti.o"
|
||||
"devc\ctrl_chassis.o"
|
||||
--strict --scatter "DevC\DevC.sct"
|
||||
--summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols
|
||||
--info sizes --info totals --info unused --info veneers
|
||||
|
||||
297
User/README.md
Normal file
297
User/README.md
Normal file
@ -0,0 +1,297 @@
|
||||
# 轮腿机器人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参数设计工具
|
||||
- 完善的安全保护机制
|
||||
@ -17,6 +17,24 @@ typedef struct {
|
||||
float temp; /* 温度 */
|
||||
} MOTOR_Feedback_t;
|
||||
|
||||
/**
|
||||
* @brief mit电机输出参数结构体
|
||||
*/
|
||||
typedef struct {
|
||||
float torque; /* 目标力矩 */
|
||||
float velocity; /* 目标速度 */
|
||||
float angle; /* 目标位置 */
|
||||
float kp; /* 位置环增益 */
|
||||
float kd; /* 速度环增益 */
|
||||
} MOTOR_MIT_Output_t;
|
||||
|
||||
/**
|
||||
* @brief 转矩电流控制模式参数结构体
|
||||
*/
|
||||
typedef struct {
|
||||
float current; /* 目标电流 */
|
||||
} MOTOR_Current_Output_t;
|
||||
|
||||
typedef struct {
|
||||
DEVICE_Header_t header;
|
||||
bool reverse; /* 是否反装 true表示反装 */
|
||||
|
||||
@ -44,8 +44,8 @@ static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
|
||||
/* Private functions -------------------------------------------------------- */
|
||||
static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
|
||||
switch (module) {
|
||||
case MOTOR_MF9025:
|
||||
case MOTOR_MF9035:
|
||||
case MOTOR_LK_MF9025:
|
||||
case MOTOR_LK_MF9035:
|
||||
return LK_CURR_LSB_MF;
|
||||
default:
|
||||
return LK_CURR_LSB_MG; // 默认使用MG的分辨率
|
||||
@ -89,8 +89,8 @@ static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
|
||||
|
||||
// 根据电机类型解析电流或功率
|
||||
switch (motor->param.module) {
|
||||
case MOTOR_MF9025:
|
||||
case MOTOR_MF9035:
|
||||
case MOTOR_LK_MF9025:
|
||||
case MOTOR_LK_MF9035:
|
||||
// MF/MG电机：转矩电流值
|
||||
motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
|
||||
break;
|
||||
|
||||
@ -15,8 +15,8 @@ extern "C" {
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
typedef enum {
|
||||
MOTOR_MF9025,
|
||||
MOTOR_MF9035,
|
||||
MOTOR_LK_MF9025,
|
||||
MOTOR_LK_MF9035,
|
||||
} MOTOR_LK_Module_t;
|
||||
|
||||
|
||||
|
||||
@ -27,10 +27,19 @@
|
||||
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
|
||||
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
|
||||
|
||||
#define LZ_MAX_RECOVER_DIFF_RAD (0.3f)
|
||||
#define MOTOR_TX_BUF_SIZE (8)
|
||||
#define MOTOR_RX_BUF_SIZE (8)
|
||||
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
|
||||
MOTOR_LZ_MotionParam_t lz_recover_param = {
|
||||
.target_angle = 0.0f,
|
||||
.target_velocity = 0.0f,
|
||||
.kp = 10.0f,
|
||||
.kd = 1.0f,
|
||||
.torque = 0.0f,
|
||||
};
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
|
||||
@ -82,7 +91,7 @@ static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2,
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 浮点值转换为原始值
|
||||
* @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
|
||||
*/
|
||||
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
|
||||
// 限制范围
|
||||
@ -93,6 +102,18 @@ static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
|
||||
return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
|
||||
*/
|
||||
static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
|
||||
// 限制范围
|
||||
if (value > max_value) value = max_value;
|
||||
if (value < 0.0f) value = 0.0f;
|
||||
|
||||
// 转换为0~65535范围，对应0~max_value
|
||||
return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 原始值转换为浮点值
|
||||
*/
|
||||
@ -137,7 +158,7 @@ static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t fram
|
||||
// 解析扩展ID各个字段
|
||||
uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型
|
||||
uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2
|
||||
uint8_t target_id = original_id & 0xFF; // Bit7~0: 目标地址
|
||||
uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID
|
||||
|
||||
// 对于反馈数据帧，我们使用特殊的解析规则
|
||||
if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
|
||||
@ -145,11 +166,11 @@ static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t fram
|
||||
// Bit8~15: 当前电机CAN ID
|
||||
// Bit16~21: 故障信息
|
||||
// Bit22~23: 模式状态
|
||||
uint8_t motor_can_id = data2 & 0xFF;
|
||||
uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID
|
||||
|
||||
// 返回格式化的ID，便于匹配
|
||||
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=目标ID)
|
||||
return (0x02000000) | ((data2 & 0xFF00) << 8) | (motor_can_id << 8) | target_id;
|
||||
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
|
||||
return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
|
||||
}
|
||||
|
||||
// 对于其他命令类型，直接返回原始ID
|
||||
@ -163,11 +184,12 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
|
||||
if (motor == NULL || msg == NULL) return;
|
||||
|
||||
// 检查是否为反馈数据帧 (通信类型2)
|
||||
uint8_t cmd_type = (msg->parsed_id >> 24) & 0x1F;
|
||||
// 需要使用原始ID来解析，因为parsed_id已经被IdParser处理过了
|
||||
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
|
||||
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
|
||||
|
||||
// 解析ID中的信息
|
||||
uint16_t id_data2 = (msg->parsed_id >> 8) & 0xFFFF;
|
||||
// 解析原始ID中的数据区2 (bit23~8)
|
||||
uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
|
||||
uint8_t motor_can_id = id_data2 & 0xFF; // Bit8~15: 当前电机CAN ID
|
||||
uint8_t fault_info = (id_data2 >> 8) & 0x3F; // Bit16~21: 故障信息
|
||||
uint8_t mode_state = (id_data2 >> 14) & 0x03; // Bit22~23: 模式状态
|
||||
@ -187,20 +209,20 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
|
||||
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
|
||||
|
||||
// 解析数据区
|
||||
// Byte0~1: 当前角度
|
||||
uint16_t raw_angle = (uint16_t)((msg->data[1] << 8) | msg->data[0]);
|
||||
// Byte0~1: 当前角度 (高字节在前，低字节在后)
|
||||
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
|
||||
motor->lz_feedback.current_angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
|
||||
|
||||
// Byte2~3: 当前角速度
|
||||
uint16_t raw_velocity = (uint16_t)((msg->data[3] << 8) | msg->data[2]);
|
||||
// Byte2~3: 当前角速度 (高字节在前，低字节在后)
|
||||
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
|
||||
motor->lz_feedback.current_velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
|
||||
|
||||
// Byte4~5: 当前力矩
|
||||
uint16_t raw_torque = (uint16_t)((msg->data[5] << 8) | msg->data[4]);
|
||||
// Byte4~5: 当前力矩 (高字节在前，低字节在后)
|
||||
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
|
||||
motor->lz_feedback.current_torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
|
||||
|
||||
// Byte6~7: 当前温度 (温度*10)
|
||||
uint16_t raw_temp = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
|
||||
// Byte6~7: 当前温度 (温度*10) (高字节在前，低字节在后)
|
||||
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
|
||||
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
|
||||
|
||||
// 更新通用电机反馈信息
|
||||
@ -263,8 +285,9 @@ int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
|
||||
new_motor->motor.reverse = param->reverse;
|
||||
|
||||
// 注册CAN接收ID - 使用解析后的反馈数据ID
|
||||
// 构建原始扩展ID
|
||||
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->host_id, param->motor_id);
|
||||
// 构建反馈数据的原始扩展ID
|
||||
// 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
|
||||
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
|
||||
// 通过ID解析器得到解析后的ID
|
||||
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
|
||||
|
||||
@ -288,7 +311,7 @@ int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
|
||||
MOTOR_LZ_t *motor = manager->motors[i];
|
||||
if (motor && motor->param.motor_id == param->motor_id) {
|
||||
// 获取反馈数据 - 使用解析后的ID
|
||||
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->host_id, param->motor_id);
|
||||
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
|
||||
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
|
||||
BSP_CAN_Message_t msg;
|
||||
|
||||
@ -328,31 +351,38 @@ int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *m
|
||||
// 更新运控参数
|
||||
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
|
||||
|
||||
// 构建扩展ID
|
||||
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, 0, param->motor_id);
|
||||
// 根据协议，bit23~8数据区2包含力矩信息
|
||||
// 力矩范围：-60Nm~60Nm 对应 0~65535
|
||||
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(motion_param->torque, LZ_TORQUE_RANGE_NM);
|
||||
|
||||
// 准备数据
|
||||
// 构建扩展ID - 运控模式控制指令
|
||||
// bit28~24: 0x1 (运控模式)
|
||||
// bit23~8: 力矩数据 (0~65535)，协议中描述为"Byte2:力矩"
|
||||
// bit7~0: 目标电机CAN_ID
|
||||
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
|
||||
|
||||
// 准备8字节数据区
|
||||
uint8_t data[8];
|
||||
|
||||
// Byte0~1: 目标角度
|
||||
// Byte0~1: 目标角度 [0~65535] 对应 (-12.57f~12.57f rad) (高字节在前，低字节在后)
|
||||
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(motion_param->target_angle, LZ_ANGLE_RANGE_RAD);
|
||||
data[0] = raw_angle & 0xFF;
|
||||
data[1] = (raw_angle >> 8) & 0xFF;
|
||||
data[0] = (raw_angle >> 8) & 0xFF; // 高字节
|
||||
data[1] = raw_angle & 0xFF; // 低字节
|
||||
|
||||
// Byte2~3: 目标角速度
|
||||
// Byte2~3: 目标角速度 [0~65535] 对应 (-20rad/s~20rad/s) (高字节在前，低字节在后)
|
||||
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(motion_param->target_velocity, LZ_VELOCITY_RANGE_RAD_S);
|
||||
data[2] = raw_velocity & 0xFF;
|
||||
data[3] = (raw_velocity >> 8) & 0xFF;
|
||||
data[2] = (raw_velocity >> 8) & 0xFF; // 高字节
|
||||
data[3] = raw_velocity & 0xFF; // 低字节
|
||||
|
||||
// Byte4~5: Kp
|
||||
uint16_t raw_kp = MOTOR_LZ_FloatToRaw(motion_param->kp, LZ_KP_MAX);
|
||||
data[4] = raw_kp & 0xFF;
|
||||
data[5] = (raw_kp >> 8) & 0xFF;
|
||||
// Byte4~5: Kp [0~65535] 对应 (0.0~5000.0) (高字节在前，低字节在后)
|
||||
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(motion_param->kp, LZ_KP_MAX);
|
||||
data[4] = (raw_kp >> 8) & 0xFF; // 高字节
|
||||
data[5] = raw_kp & 0xFF; // 低字节
|
||||
|
||||
// Byte6~7: Kd
|
||||
uint16_t raw_kd = MOTOR_LZ_FloatToRaw(motion_param->kd, LZ_KD_MAX);
|
||||
data[6] = raw_kd & 0xFF;
|
||||
data[7] = (raw_kd >> 8) & 0xFF;
|
||||
// Byte6~7: Kd [0~65535] 对应 (0.0~100.0) (高字节在前，低字节在后)
|
||||
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(motion_param->kd, LZ_KD_MAX);
|
||||
data[6] = (raw_kd >> 8) & 0xFF; // 高字节
|
||||
data[7] = raw_kd & 0xFF; // 低字节
|
||||
|
||||
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
|
||||
}
|
||||
@ -422,4 +452,81 @@ int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
|
||||
return DEVICE_OK;
|
||||
}
|
||||
return DEVICE_ERR_NO_DEV;
|
||||
}
|
||||
|
||||
static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
|
||||
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
|
||||
if (motor && motor->motor.header.online) {
|
||||
return &motor->lz_feedback;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque) {
|
||||
if (param == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
// 创建运控参数，只设置力矩，其他参数为0
|
||||
MOTOR_LZ_MotionParam_t motion_param = {0};
|
||||
motion_param.torque = torque;
|
||||
motion_param.target_angle = 0.0f;
|
||||
motion_param.target_velocity = 0.0f;
|
||||
motion_param.kp = 0.0f;
|
||||
motion_param.kd = 0.0f;
|
||||
|
||||
return MOTOR_LZ_MotionControl(param, &motion_param);
|
||||
}
|
||||
|
||||
int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity) {
|
||||
if (param == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
// 创建运控参数，设置位置和速度限制
|
||||
MOTOR_LZ_MotionParam_t motion_param = {0};
|
||||
motion_param.target_angle = target_angle;
|
||||
motion_param.target_velocity = max_velocity;
|
||||
motion_param.torque = 0.0f;
|
||||
motion_param.kp = 100.0f; // 默认位置增益
|
||||
motion_param.kd = 5.0f; // 默认微分增益
|
||||
|
||||
return MOTOR_LZ_MotionControl(param, &motion_param);
|
||||
}
|
||||
|
||||
int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity) {
|
||||
if (param == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
// 创建运控参数，只设置速度
|
||||
MOTOR_LZ_MotionParam_t motion_param = {0};
|
||||
motion_param.target_angle = 0.0f;
|
||||
motion_param.target_velocity = target_velocity;
|
||||
motion_param.torque = 0.0f;
|
||||
motion_param.kp = 0.0f;
|
||||
motion_param.kd = 1.0f; // 少量阻尼
|
||||
|
||||
return MOTOR_LZ_MotionControl(param, &motion_param);
|
||||
}
|
||||
|
||||
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param) {
|
||||
if (param == NULL) return DEVICE_ERR_NULL;
|
||||
|
||||
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
|
||||
if (motor == NULL) return DEVICE_ERR_NO_DEV;
|
||||
|
||||
// 获取当前角度
|
||||
MOTOR_LZ_Feedback_t *feedback = MOTOR_LZ_GetFeedback(param);
|
||||
if (feedback == NULL) return DEVICE_ERR_NO_DEV;
|
||||
|
||||
float current_angle = feedback->current_angle;
|
||||
|
||||
// 计算目标角度为0时的最短路径
|
||||
float angle_diff = -current_angle; // 目标是0，所以差值就是-current_angle
|
||||
// 限制最大差值，防止过大跳变
|
||||
if (angle_diff > LZ_MAX_RECOVER_DIFF_RAD) angle_diff = LZ_MAX_RECOVER_DIFF_RAD;
|
||||
if (angle_diff < -LZ_MAX_RECOVER_DIFF_RAD) angle_diff = -LZ_MAX_RECOVER_DIFF_RAD;
|
||||
|
||||
float target_angle = current_angle + angle_diff;
|
||||
|
||||
// 创建运控参数，设置位置和速度限制
|
||||
MOTOR_LZ_MotionParam_t motion_param = lz_recover_param; // 使用预设的恢复参数
|
||||
motion_param.target_angle = target_angle;
|
||||
|
||||
return MOTOR_LZ_MotionControl(param, &motion_param);
|
||||
}
|
||||
@ -145,6 +145,31 @@ int8_t MOTOR_LZ_UpdateAll(void);
|
||||
*/
|
||||
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
|
||||
|
||||
/**
|
||||
* @brief 电流(力矩)模式控制电机
|
||||
* @param param 电机参数
|
||||
* @param torque 目标力矩 (-60~60 Nm)
|
||||
* @return 设备状态码
|
||||
*/
|
||||
int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque);
|
||||
|
||||
/**
|
||||
* @brief 位置模式控制电机
|
||||
* @param param 电机参数
|
||||
* @param target_angle 目标角度 (-12.57~12.57 rad)
|
||||
* @param max_velocity 最大速度 (0~20 rad/s)
|
||||
* @return 设备状态码
|
||||
*/
|
||||
int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity);
|
||||
|
||||
/**
|
||||
* @brief 速度模式控制电机
|
||||
* @param param 电机参数
|
||||
* @param target_velocity 目标速度 (-20~20 rad/s)
|
||||
* @return 设备状态码
|
||||
*/
|
||||
int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity);
|
||||
|
||||
/**
|
||||
* @brief 电机使能运行
|
||||
* @param param 电机参数
|
||||
@ -188,6 +213,8 @@ int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
|
||||
*/
|
||||
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
|
||||
|
||||
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
218
User/module/balance_chassis.c
Normal file
218
User/module/balance_chassis.c
Normal file
@ -0,0 +1,218 @@
|
||||
#include "module/balance_chassis.h"
|
||||
#include "bsp/time.h"
|
||||
#include "bsp/can.h"
|
||||
#include <math.h>
|
||||
|
||||
|
||||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, Chassis_Action_t action) {
|
||||
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
|
||||
if (mode == c->mode && action == c->action) return CHASSIS_OK; /* 模式未改变直接返回 */
|
||||
|
||||
PID_Reset(&c->pid.left_wheel);
|
||||
PID_Reset(&c->pid.right_wheel);
|
||||
PID_Reset(&c->pid.follow);
|
||||
PID_Reset(&c->pid.balance);
|
||||
c->mode = mode;
|
||||
c->action = action;
|
||||
c->state = 0;
|
||||
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
|
||||
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
|
||||
if (c == NULL || param == NULL || target_freq <= 0.0f) {
|
||||
return -1; // 参数错误
|
||||
}
|
||||
c->param = param;
|
||||
/*初始化can*/
|
||||
BSP_CAN_Init();
|
||||
|
||||
/*初始化和注册所有电机*/
|
||||
MOTOR_LZ_Init();
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_Register(&c->param->joint_motors[i]);
|
||||
}
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_Register(&c->param->wheel_motors[i]);
|
||||
}
|
||||
/*初始化pid*/
|
||||
PID_Init(&c->pid.left_wheel, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param);
|
||||
PID_Init(&c->pid.right_wheel, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param);
|
||||
PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, ¶m->follow_pid_param);
|
||||
PID_Init(&c->pid.balance, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param);
|
||||
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
int8_t Chassis_UpdateFeedback(Chassis_t *c){
|
||||
if (c == NULL) {
|
||||
return -1; // 参数错误
|
||||
}
|
||||
/*更新电机反馈*/
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_Update(&c->param->joint_motors[i]);
|
||||
}
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_Update(&c->param->wheel_motors[i]);
|
||||
}
|
||||
|
||||
/*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
|
||||
// 更新关节电机反馈并检查离线状态
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
|
||||
if (joint_motor != NULL) {
|
||||
c->feedback.joint[i] = joint_motor->motor.feedback;
|
||||
// 检查关节电机是否离线
|
||||
if (!joint_motor->motor.header.online) {
|
||||
return CHASSIS_ERR; // 有关节电机离线，返回错误
|
||||
}
|
||||
} else {
|
||||
return CHASSIS_ERR; // 无法获取关节电机实例，返回错误
|
||||
}
|
||||
}
|
||||
|
||||
// 更新轮子电机反馈并检查离线状态
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
|
||||
if (wheel_motor != NULL) {
|
||||
c->feedback.wheel[i] = wheel_motor->motor.feedback;
|
||||
// 检查轮子电机是否离线
|
||||
if (!wheel_motor->motor.header.online) {
|
||||
return CHASSIS_ERR; // 有轮子电机离线，返回错误
|
||||
}
|
||||
} else {
|
||||
return CHASSIS_ERR; // 无法获取轮子电机实例，返回错误
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t Chassis_UpdateIMU(Chassis_t *c, const AHRS_Eulr_t *euler, const AHRS_Gyro_t *gyro, const AHRS_Accl_t *accl){
|
||||
if (c == NULL || euler == NULL || gyro == NULL || accl == NULL) {
|
||||
return -1; // 参数错误
|
||||
}
|
||||
c->feedback.imu_euler = *euler;
|
||||
c->feedback.imu_gyro = *gyro;
|
||||
c->feedback.imu_accl = *accl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
|
||||
if (c == NULL || c_cmd == NULL) {
|
||||
return CHASSIS_ERR_NULL; // 参数错误
|
||||
}
|
||||
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
|
||||
c->lask_wakeup = BSP_TIME_Get_us();
|
||||
|
||||
/*设置底盘模式*/
|
||||
if (Chassis_SetMode(c, c_cmd->mode, c_cmd->action) != CHASSIS_OK) {
|
||||
return CHASSIS_ERR_MODE; // 设置模式失败
|
||||
}
|
||||
|
||||
/*根据底盘模式执行不同的控制逻辑*/
|
||||
switch (c->mode) {
|
||||
case CHASSIS_MODE_RELAX:
|
||||
// 放松模式，电机不输出
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_Relax(&c->param->joint_motors[i]);
|
||||
}
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_MotorOff(&c->param->wheel_motors[i]);
|
||||
}
|
||||
break;
|
||||
|
||||
case CHASSIS_MODE_RECOVER:
|
||||
switch (c->state) {
|
||||
case 0:
|
||||
//使能电机
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_Enable(&c->param->joint_motors[i]);
|
||||
}
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
|
||||
}
|
||||
c->state += 1;
|
||||
break;
|
||||
case 1:
|
||||
// 关节电机复位轮电机输出0；
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 1.0f); // 回到零位，最大速度1.0 rad/s
|
||||
}
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f); // 设置轮子速度为0
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case CHASSIS_MODE_WHELL_BALANCE:
|
||||
// 只靠两轮平衡，关节电机锁死，通过pid实现倒立摆
|
||||
// 锁定关节电机到固定位置（比如直立状态）
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 0.5f); // 目标位置0，最大速度0.5 rad/s
|
||||
}
|
||||
|
||||
// 双轮平衡控制
|
||||
// 获取IMU pitch角度和角速度作为平衡反馈
|
||||
float pitch_angle = c->feedback.imu_euler.pit; // pitch角度
|
||||
float pitch_rate = c->feedback.imu_gyro.y; // pitch角速度
|
||||
|
||||
// 平衡PID计算 - 以直立为目标（0度）
|
||||
float balance_output = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, pitch_rate, c->dt);
|
||||
|
||||
// 前进后退控制（基于控制指令）
|
||||
float forward_cmd = c_cmd->move_vec.vx; // 前进速度指令
|
||||
|
||||
// 转向控制（基于控制指令）
|
||||
float turn_cmd = c_cmd->move_vec.wz; // 转向速度指令
|
||||
|
||||
// 计算左右轮速度设定值
|
||||
c->setpoint.left_wheel = balance_output + forward_cmd - turn_cmd * 0.5f;
|
||||
c->setpoint.right_wheel = balance_output + forward_cmd + turn_cmd * 0.5f;
|
||||
|
||||
// 左轮速度PID控制
|
||||
float left_wheel_speed = c->feedback.wheel[0].rotor_speed;
|
||||
float left_output = PID_Calc(&c->pid.left_wheel,
|
||||
c->setpoint.left_wheel,
|
||||
left_wheel_speed,
|
||||
0.0f,
|
||||
c->dt);
|
||||
|
||||
// 右轮速度PID控制
|
||||
float right_wheel_speed = c->feedback.wheel[1].rotor_speed;
|
||||
float right_output = PID_Calc(&c->pid.right_wheel,
|
||||
c->setpoint.right_wheel,
|
||||
right_wheel_speed,
|
||||
0.0f,
|
||||
c->dt);
|
||||
|
||||
// 限制输出范围
|
||||
left_output = fmaxf(-1.0f, fminf(1.0f, left_output));
|
||||
right_output = fmaxf(-1.0f, fminf(1.0f, right_output));
|
||||
|
||||
// 输出到电机
|
||||
MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_output);
|
||||
MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_output);
|
||||
break;
|
||||
|
||||
case CHASSIS_MODE_WHELL_LEG_BALANCE:
|
||||
// 轮子+腿平衡模式（暂时留空，后续实现）
|
||||
break;
|
||||
|
||||
default:
|
||||
return CHASSIS_ERR_MODE;
|
||||
}
|
||||
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
void Chassis_Output(Chassis_t *c) {
|
||||
if (c == NULL) return;
|
||||
|
||||
// 这个函数已经在各个模式中直接调用了电机输出函数
|
||||
// 如果需要统一输出，可以在这里实现
|
||||
// 现在的设计是在控制逻辑中直接输出，所以这里留空
|
||||
}
|
||||
177
User/module/balance_chassis.h
Normal file
177
User/module/balance_chassis.h
Normal file
@ -0,0 +1,177 @@
|
||||
/*
|
||||
* 平衡底盘模组
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// Front
|
||||
// | 1 4 |
|
||||
// (1)Left| |Right(2)
|
||||
// | 2 3 |
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
// #include "component/cmd.h"
|
||||
#include "component/ahrs.h"
|
||||
#include "component/filter.h"
|
||||
#include "component/pid.h"
|
||||
#include "device/motor.h"
|
||||
#include "device/motor_lk.h"
|
||||
#include "device/motor_lz.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_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
|
||||
CHASSIS_MODE_RECOVER,
|
||||
CHASSIS_MODE_WHELL_BALANCE, /* 平衡模式，底盘自我平衡 */
|
||||
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
|
||||
} Chassis_Mode_t;
|
||||
|
||||
typedef enum {
|
||||
CHASSIS_ACTION_JUMP,
|
||||
CHASSIS_ACTION_STAND,
|
||||
}Chassis_Action_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
Chassis_Mode_t mode; /* 底盘模式 */
|
||||
Chassis_Action_t action; /* 底盘动作 */
|
||||
MoveVector_t move_vec; /* 底盘运动向量 */
|
||||
float height; /* 目标高度 */
|
||||
} Chassis_CMD_t;
|
||||
|
||||
typedef struct {
|
||||
MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
|
||||
MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
|
||||
AHRS_Accl_t imu_accl; /* IMU加速度计 */
|
||||
AHRS_Gyro_t imu_gyro; /* IMU陀螺仪 */
|
||||
AHRS_Eulr_t imu_euler; /* IMU欧拉角 */
|
||||
MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
|
||||
}Chassis_Feedback_t;
|
||||
|
||||
typedef struct {
|
||||
MOTOR_MIT_Output_t joint[4]; /* 四个关节电机输出 */
|
||||
MOTOR_MIT_Output_t wheel[2]; /* 两个轮子电机输出 */
|
||||
}Chassis_Output_t;
|
||||
|
||||
|
||||
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
||||
typedef struct {
|
||||
KPID_Params_t motor_pid_param; /* 轮子控制PID的参数 */
|
||||
KPID_Params_t follow_pid_param; /* 跟随云台PID的参数 */
|
||||
|
||||
MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
|
||||
MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
|
||||
|
||||
float mech_zero_yaw; /* 机械零点 */
|
||||
|
||||
/* 低通滤波器截止频率 */
|
||||
struct {
|
||||
float in; /* 输入 */
|
||||
float out; /* 输出 */
|
||||
} low_pass_cutoff_freq;
|
||||
} Chassis_Params_t;
|
||||
|
||||
/*
|
||||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
|
||||
* 包含了初始化参数，中间变量，输出变量
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t lask_wakeup;
|
||||
float dt;
|
||||
|
||||
Chassis_Params_t *param; /* 底盘的参数，用Chassis_Init设定 */
|
||||
AHRS_Eulr_t *mech_zero;
|
||||
|
||||
/* 模块通用 */
|
||||
Chassis_Mode_t mode; /* 底盘模式 */
|
||||
Chassis_Action_t action; /* 底盘动作 */
|
||||
|
||||
/* 反馈信息 */
|
||||
Chassis_Feedback_t feedback;
|
||||
/* 控制信息*/
|
||||
Chassis_Output_t output;
|
||||
|
||||
int8_t state;
|
||||
float wz_multi; /* 小陀螺模式旋转方向 */
|
||||
|
||||
/* PID计算的目标值 */
|
||||
struct {
|
||||
float left_wheel;
|
||||
float right_wheel;
|
||||
} setpoint;
|
||||
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t left_wheel; /* 左轮PID */
|
||||
KPID_t right_wheel; /* 右轮PID */
|
||||
KPID_t follow; /* 跟随云台用的PID */
|
||||
KPID_t balance; /* 平衡用的PID */
|
||||
} pid;
|
||||
|
||||
/* 滤波器 */
|
||||
struct {
|
||||
LowPassFilter2p_t *in; /* 反馈值滤波器 */
|
||||
LowPassFilter2p_t *out; /* 输出值滤波器 */
|
||||
} filter;
|
||||
|
||||
} Chassis_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
/**
|
||||
* \brief 初始化底盘
|
||||
*
|
||||
* \param c 包含底盘数据的结构体
|
||||
* \param param 包含底盘参数的结构体指针
|
||||
* \param target_freq 任务预期的运行频率
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq);
|
||||
|
||||
/**
|
||||
* \brief 更新底盘的反馈信息
|
||||
*
|
||||
* \param c 包含底盘数据的结构体
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Chassis_UpdateFeedback(Chassis_t *c);
|
||||
|
||||
|
||||
int8_t Chassis_UpdateIMU(Chassis_t *c, const AHRS_Eulr_t *euler, const AHRS_Gyro_t *gyro, const AHRS_Accl_t *accl);
|
||||
/**
|
||||
* \brief 运行底盘控制逻辑
|
||||
*
|
||||
* \param c 包含底盘数据的结构体
|
||||
* \param c_cmd 底盘控制指令
|
||||
*
|
||||
* \return 函数运行结果
|
||||
*/
|
||||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
|
||||
|
||||
|
||||
/**
|
||||
* \brief 底盘输出值
|
||||
*
|
||||
* \param s 包含底盘数据的结构体
|
||||
* \param out CAN设备底盘输出结构体
|
||||
*/
|
||||
void Chassis_Output(Chassis_t *c);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
0
User/module/chassis.c
Normal file
0
User/module/chassis.c
Normal file
180
User/module/chassis.h
Normal file
180
User/module/chassis.h
Normal file
@ -0,0 +1,180 @@
|
||||
/*
|
||||
* 底盘模组
|
||||
*/
|
||||
|
||||
#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
|
||||
@ -21,6 +21,50 @@ Config_RobotParam_t robot_config = {
|
||||
.device_id = 0x42,
|
||||
.master_id = 0x43,
|
||||
},
|
||||
.joint_motors[0] = { // 左膝关节
|
||||
.can = BSP_CAN_1,
|
||||
.motor_id = 124,
|
||||
.host_id = 130,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = false,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
.joint_motors[1] = { // 左髋关节
|
||||
.can = BSP_CAN_1,
|
||||
.motor_id = 125,
|
||||
.host_id = 130,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = false,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
.joint_motors[2] = { // 右髋关节
|
||||
.can = BSP_CAN_1,
|
||||
.motor_id = 126,
|
||||
.host_id = 130,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = false,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
.joint_motors[3] = { // 右膝关节
|
||||
.can = BSP_CAN_1,
|
||||
.motor_id = 127,
|
||||
.host_id = 130,
|
||||
.module = MOTOR_LZ_RSO3,
|
||||
.reverse = false,
|
||||
.mode = MOTOR_LZ_MODE_MOTION,
|
||||
},
|
||||
.wheel_motors[0] = { // 左轮电机
|
||||
.can = BSP_CAN_1,
|
||||
.id = 0x141,
|
||||
.module = MOTOR_LK_MF9025,
|
||||
.reverse = false,
|
||||
},
|
||||
.wheel_motors[1] = { // 右轮电机
|
||||
.can = BSP_CAN_1,
|
||||
.id = 0x142,
|
||||
.module = MOTOR_LK_MF9025,
|
||||
.reverse = true,
|
||||
},
|
||||
};
|
||||
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
|
||||
@ -10,9 +10,14 @@ extern "C" {
|
||||
|
||||
#include <stdint.h>
|
||||
#include "device/dm_imu.h"
|
||||
#include "device/motor_lz.h"
|
||||
#include "device/motor_lk.h"
|
||||
|
||||
|
||||
typedef struct {
|
||||
DM_IMU_Param_t imu_param;
|
||||
MOTOR_LZ_Param_t joint_motors[4];
|
||||
MOTOR_LK_Param_t wheel_motors[2];
|
||||
} Config_RobotParam_t;
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
@ -19,3 +19,10 @@
|
||||
function: Task_atti_esti
|
||||
name: atti_esti
|
||||
stack: 256
|
||||
- delay: 0
|
||||
description: ''
|
||||
freq_control: true
|
||||
frequency: 500.0
|
||||
function: Task_ctrl_chassis
|
||||
name: ctrl_chassis
|
||||
stack: 256
|
||||
|
||||
97
User/task/ctrl_chassis.c
Normal file
97
User/task/ctrl_chassis.c
Normal file
@ -0,0 +1,97 @@
|
||||
/*
|
||||
ctrl_chassis Task
|
||||
|
||||
*/
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/can.h"
|
||||
#include "device/motor_lk.h"
|
||||
#include "device/motor_lz.h"
|
||||
#include "module/config.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
/* Private define ----------------------------------------------------------- */
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
MOTOR_LZ_MotionParam_t joint0_motion_params = {
|
||||
.target_angle = 0.0f,
|
||||
.target_velocity = 0.0f,
|
||||
.kp = 1.0f,
|
||||
.kd = 1.0f,
|
||||
.torque = 0.0f,
|
||||
};
|
||||
|
||||
MOTOR_LZ_MotionParam_t joint1_motion_params = {
|
||||
.target_angle = 0.0f,
|
||||
.target_velocity = 0.0f,
|
||||
.kp = 1.0f,
|
||||
.kd = 1.0f,
|
||||
.torque = 0.0f,
|
||||
};
|
||||
bool motor_free = false;
|
||||
MOTOR_t joint0;
|
||||
MOTOR_t joint1;
|
||||
// MOTOR_LZ_Feedback_t lz_feedback;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
void Task_ctrl_chassis(void *argument) {
|
||||
(void)argument; /* 未使用argument，消除警告 */
|
||||
|
||||
|
||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_CHASSIS_FREQ;
|
||||
|
||||
osDelay(CTRL_CHASSIS_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
BSP_CAN_Init();
|
||||
// 初始化电机
|
||||
MOTOR_LZ_Init();
|
||||
for (int i = 0; i < 4; i++) {
|
||||
MOTOR_LZ_Register(&Config_GetRobotParam()->joint_motors[i]);
|
||||
MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_motors[i]);
|
||||
}
|
||||
for (int i = 0; i < 2; i++) {
|
||||
MOTOR_LK_Register(&Config_GetRobotParam()->wheel_motors[i]);
|
||||
MOTOR_LK_MotorOn(&Config_GetRobotParam()->wheel_motors[i]);
|
||||
}
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
MOTOR_LZ_Update(&Config_GetRobotParam()->joint_motors[0]);
|
||||
MOTOR_LZ_Update(&Config_GetRobotParam()->joint_motors[1]);
|
||||
|
||||
// 实时获取电机数据并更新 joint0 和 joint1
|
||||
MOTOR_LZ_t* joint0_instance = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_motors[0]);
|
||||
MOTOR_LZ_t* joint1_instance = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_motors[1]);
|
||||
if (joint0_instance != NULL) {
|
||||
joint0 = joint0_instance->motor;
|
||||
}
|
||||
if (joint1_instance != NULL) {
|
||||
joint1 = joint1_instance->motor;
|
||||
}
|
||||
|
||||
if (motor_free) {
|
||||
// MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_motors[0]);
|
||||
// MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_motors[1]);
|
||||
MOTOR_LZ_RecoverToZero(&Config_GetRobotParam()->joint_motors[0]);
|
||||
MOTOR_LZ_RecoverToZero(&Config_GetRobotParam()->joint_motors[1]);
|
||||
} else {
|
||||
MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_motors[0], &joint0_motion_params);
|
||||
MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_motors[1], &joint1_motion_params);
|
||||
}
|
||||
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}}
|
||||
@ -33,6 +33,7 @@ void Task_Init(void *argument) {
|
||||
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
|
||||
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
||||
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
|
||||
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
#include "device/dr16.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -14,7 +14,7 @@
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
DR16_t dr16;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -30,13 +30,14 @@ void Task_rc(void *argument) {
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
|
||||
DR16_Init(&dr16);
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
|
||||
DR16_StartDmaRecv(&dr16);
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -23,4 +23,9 @@ const osThreadAttr_t attr_atti_esti = {
|
||||
.name = "atti_esti",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
const osThreadAttr_t attr_ctrl_chassis = {
|
||||
.name = "ctrl_chassis",
|
||||
.priority = osPriorityNormal,
|
||||
.stack_size = 256 * 4,
|
||||
};
|
||||
@ -16,12 +16,14 @@ extern "C" {
|
||||
#define BLINK_FREQ (100.0)
|
||||
#define RC_FREQ (500.0)
|
||||
#define ATTI_ESTI_FREQ (1000.0)
|
||||
#define CTRL_CHASSIS_FREQ (500.0)
|
||||
|
||||
/* 任务初始化延时ms */
|
||||
#define TASK_INIT_DELAY (100u)
|
||||
#define BLINK_INIT_DELAY (0)
|
||||
#define RC_INIT_DELAY (0)
|
||||
#define ATTI_ESTI_INIT_DELAY (0)
|
||||
#define CTRL_CHASSIS_INIT_DELAY (0)
|
||||
|
||||
/* Exported defines --------------------------------------------------------- */
|
||||
/* Exported macro ----------------------------------------------------------- */
|
||||
@ -34,6 +36,7 @@ typedef struct {
|
||||
osThreadId_t blink;
|
||||
osThreadId_t rc;
|
||||
osThreadId_t atti_esti;
|
||||
osThreadId_t ctrl_chassis;
|
||||
} thread;
|
||||
|
||||
/* USER MESSAGE BEGIN */
|
||||
@ -58,6 +61,7 @@ typedef struct {
|
||||
UBaseType_t blink;
|
||||
UBaseType_t rc;
|
||||
UBaseType_t atti_esti;
|
||||
UBaseType_t ctrl_chassis;
|
||||
} stack_water_mark;
|
||||
|
||||
/* 各任务运行频率 */
|
||||
@ -65,6 +69,7 @@ typedef struct {
|
||||
float blink;
|
||||
float rc;
|
||||
float atti_esti;
|
||||
float ctrl_chassis;
|
||||
} freq;
|
||||
|
||||
/* 任务最近运行时间 */
|
||||
@ -72,6 +77,7 @@ typedef struct {
|
||||
float blink;
|
||||
float rc;
|
||||
float atti_esti;
|
||||
float ctrl_chassis;
|
||||
} last_up_time;
|
||||
|
||||
} Task_Runtime_t;
|
||||
@ -84,12 +90,14 @@ extern const osThreadAttr_t attr_init;
|
||||
extern const osThreadAttr_t attr_blink;
|
||||
extern const osThreadAttr_t attr_rc;
|
||||
extern const osThreadAttr_t attr_atti_esti;
|
||||
extern const osThreadAttr_t attr_ctrl_chassis;
|
||||
|
||||
/* 任务函数声明 */
|
||||
void Task_Init(void *argument);
|
||||
void Task_blink(void *argument);
|
||||
void Task_rc(void *argument);
|
||||
void Task_atti_esti(void *argument);
|
||||
void Task_ctrl_chassis(void *argument);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
244
User/整合方案说明.md
Normal file
244
User/整合方案说明.md
Normal file
@ -0,0 +1,244 @@
|
||||
# 轮腿机器人控制系统整合方案
|
||||
|
||||
## 📋 现有系统分析
|
||||
|
||||
### 🏗️ 您的系统架构（优势）
|
||||
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. **回滚机制**：确保可以快速回到原有系统
|
||||
|
||||
这样您既可以利用现有系统的优势，又能获得新架构的清晰性和可维护性。您觉得哪个方案更适合您的需求？
|
||||
304
some_functions.c
Normal file
304
some_functions.c
Normal file
@ -0,0 +1,304 @@
|
||||
// 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 get_BM_motor_measure(ptr, data) \
|
||||
{ \
|
||||
(ptr)->last_ecd = (ptr)->ecd; \
|
||||
(ptr)->ecd = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
||||
(ptr)->speed_rpm = (int16_t)((data)[0] << 8 | (data)[1]); \
|
||||
(ptr)->given_current = (int16_t)((data)[2] << 8 | (data)[3]); \
|
||||
(ptr)->error = (data)[6]; \
|
||||
(ptr)->mode = (data)[7]; \
|
||||
}
|
||||
|
||||
void CAN_BM_ENABLE_CMD(void)
|
||||
{
|
||||
CAN_TxHeaderTypeDef bm_tx_measure;
|
||||
uint8_t bm_can_send_data[8];
|
||||
|
||||
uint32_t send_mail_box;
|
||||
bm_tx_measure.StdId = 0x105; // 120
|
||||
bm_tx_measure.IDE = CAN_ID_STD;
|
||||
bm_tx_measure.RTR = CAN_RTR_DATA;
|
||||
bm_tx_measure.DLC = 0x08;
|
||||
bm_can_send_data[0] = 0x0A;
|
||||
bm_can_send_data[1] = 0x0A;
|
||||
bm_can_send_data[2] = 0x00;
|
||||
bm_can_send_data[3] = 0x00;
|
||||
bm_can_send_data[4] = 0x00;
|
||||
bm_can_send_data[5] = 0x00;
|
||||
bm_can_send_data[6] = 0x00;
|
||||
bm_can_send_data[7] = 0x00;
|
||||
|
||||
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
void CAN_BM_MODE_SET_CMD(void)
|
||||
{
|
||||
CAN_TxHeaderTypeDef bm_tx_measure;
|
||||
uint8_t bm_can_send_data[8];
|
||||
|
||||
uint32_t send_mail_box;
|
||||
bm_tx_measure.StdId = 0x106; // 120
|
||||
bm_tx_measure.IDE = CAN_ID_STD;
|
||||
bm_tx_measure.RTR = CAN_RTR_DATA;
|
||||
bm_tx_measure.DLC = 0x08;
|
||||
bm_can_send_data[0] = 0x01;
|
||||
bm_can_send_data[1] = 0x01;
|
||||
bm_can_send_data[2] = 0x00;
|
||||
bm_can_send_data[3] = 0x00;
|
||||
bm_can_send_data[4] = 0x00;
|
||||
bm_can_send_data[5] = 0x00;
|
||||
bm_can_send_data[6] = 0x00;
|
||||
bm_can_send_data[7] = 0x00;
|
||||
|
||||
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
void CAN_BM_CONTROL_CMD( int16_t motor1, int16_t motor2 )
|
||||
{
|
||||
CAN_TxHeaderTypeDef bm_tx_measure;
|
||||
uint8_t bm_can_send_data[8];
|
||||
|
||||
uint32_t send_mail_box;
|
||||
bm_tx_measure.StdId = 0x32; // 120
|
||||
bm_tx_measure.IDE = CAN_ID_STD;
|
||||
bm_tx_measure.RTR = CAN_RTR_DATA;
|
||||
bm_tx_measure.DLC = 0x08;
|
||||
bm_can_send_data[0] = motor1 >> 8;
|
||||
bm_can_send_data[1] = motor1;
|
||||
bm_can_send_data[2] = motor2 >> 8;
|
||||
bm_can_send_data[3] = motor2;
|
||||
bm_can_send_data[4] = 0x00;
|
||||
bm_can_send_data[5] = 0x00;
|
||||
bm_can_send_data[6] = 0x00;
|
||||
bm_can_send_data[7] = 0x00;
|
||||
|
||||
HAL_CAN_AddTxMessage(&BM_CAN, &bm_tx_measure, bm_can_send_data, &send_mail_box);
|
||||
}
|
||||
|
||||
#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 CAN_HT_CMD( uint8_t id, fp32 f_t )
|
||||
{
|
||||
uint32_t canTxMailbox = CAN_TX_MAILBOX0;
|
||||
|
||||
fp32 f_p = 0.0f, f_v = 0.0f, f_kp = 0.0f, f_kd = 0.0f;
|
||||
uint16_t p, v, kp, kd, t;
|
||||
uint8_t buf[8];
|
||||
LIMIT_MIN_MAX(f_p, P_MIN, P_MAX);
|
||||
LIMIT_MIN_MAX(f_v, V_MIN, V_MAX);
|
||||
LIMIT_MIN_MAX(f_kp, KP_MIN, KP_MAX);
|
||||
LIMIT_MIN_MAX(f_kd, KD_MIN, KD_MAX);
|
||||
LIMIT_MIN_MAX(f_t, T_MIN, T_MAX);
|
||||
|
||||
p = float_to_uint(f_p, P_MIN, P_MAX, 16);
|
||||
v = float_to_uint(f_v, V_MIN, V_MAX, 12);
|
||||
kp = float_to_uint(f_kp, KP_MIN, KP_MAX, 12);
|
||||
kd = float_to_uint(f_kd, KD_MIN, KD_MAX, 12);
|
||||
t = float_to_uint(f_t, T_MIN, T_MAX, 12);
|
||||
|
||||
buf[0] = p>>8;
|
||||
buf[1] = p&0xFF;
|
||||
buf[2] = v>>4;
|
||||
buf[3] = ((v&0xF)<<4)|(kp>>8);
|
||||
buf[4] = kp&0xFF;
|
||||
buf[5] = kd>>4;
|
||||
buf[6] = ((kd&0xF)<<4)|(t>>8);
|
||||
buf[7] = t&0xff;
|
||||
|
||||
chassis_tx_message.StdId = id;
|
||||
chassis_tx_message.IDE = CAN_ID_STD;
|
||||
chassis_tx_message.RTR = CAN_RTR_DATA;
|
||||
chassis_tx_message.DLC = 0x08;
|
||||
|
||||
// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
|
||||
if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET)
|
||||
{canTxMailbox = CAN_TX_MAILBOX0;}
|
||||
else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET)
|
||||
{canTxMailbox = CAN_TX_MAILBOX1;}
|
||||
else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET)
|
||||
{canTxMailbox = CAN_TX_MAILBOX2;}
|
||||
|
||||
if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, buf, (uint32_t *)&canTxMailbox)==HAL_OK){};
|
||||
}
|
||||
void CAN_CMD_HT_Enable(uint8_t id, uint8_t unterleib_motor_send_data[8] )
|
||||
{
|
||||
uint32_t canTxMailbox= CAN_TX_MAILBOX0;
|
||||
|
||||
chassis_tx_message.StdId = id;
|
||||
chassis_tx_message.IDE = CAN_ID_STD;
|
||||
chassis_tx_message.RTR = CAN_RTR_DATA;
|
||||
chassis_tx_message.DLC = 0x08;
|
||||
|
||||
// while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0);
|
||||
if ((hcan1.Instance->TSR & CAN_TSR_TME0) != RESET)
|
||||
{canTxMailbox = CAN_TX_MAILBOX0;}
|
||||
else if ((hcan1.Instance->TSR & CAN_TSR_TME1) != RESET)
|
||||
{canTxMailbox = CAN_TX_MAILBOX1;}
|
||||
else if ((hcan1.Instance->TSR & CAN_TSR_TME2) != RESET)
|
||||
{canTxMailbox = CAN_TX_MAILBOX2;}
|
||||
|
||||
if(HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, unterleib_motor_send_data, (uint32_t *)&canTxMailbox)==HAL_OK){};
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
321
utils/lqr.m
Normal file
321
utils/lqr.m
Normal file
@ -0,0 +1,321 @@
|
||||
% 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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user