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