在下面的Simulink模型中,我解释的函数输出是一个包含38个元素的向量。我有两个具有相同输出的函数,其中一个完美地工作(desiredtrajectory_sim.m)但另一个没有(desiredtrajectory.m)。有什么建议。谢谢
这是Simulink模型
function [desired_state] = desiredtrajectory_sim(in)
t = in(1);
Sf = [ 1; 2; pi/4];
dSf = [0;0;0];
Pf = [ 0.1*t; 0; 0.5*sin(0.03*pi*t) + 2; 0; 0.01*pi*t ; 0];
dPf = [ 0.1; 0; 0.5*0.03*pi*cos(0.03*pi*t); 0; 0.01*pi; 0];
pf = Sf(1); qf = Sf(2); betaf = Sf(3);
xf = Pf(1); yf = Pf(2); zf = Pf(3);
phif = Pf(4); thetaf = Pf(5); psif = Pf(6);
rf = sqrt(pf^2 + qf^2 - 2*pf*qf*cos(betaf));
h1 = sqrt(0.5*(pf^2 + qf^2 - 0.5*rf^2));
h2 = sqrt(0.5*(rf^2 + pf^2 - 0.5*qf^2));
h3 = sqrt(0.5*(qf^2 + rf^2 - 0.5*pf^2));
alpha1 = acos((4*(h1^2+h2^2)-9*pf^2)/(8*h1*h2));
alpha2 = acos((4*(h1^2+h3^2)-9*qf^2)/(8*h1*h3));
Rot = RPYtoRot_ZXY(phif, thetaf, psif);
r1 = Rot*[2/3*h1;0;0];
r2 = Rot*[2/3*h2*cos(alpha1);2/3*h2*sin(alpha1);0];
r3 = Rot*[2/3*h3*cos(alpha2);-2/3*h3*sin(alpha2);0];
pos_des1 = [xf;yf;zf] + r1;
pos_des2 = [xf;yf;zf] + r2;
pos_des3 = [xf;yf;zf] + r3;
omega = [0 -sin(psif) cos(thetaf)*cos(psif);...
0 -cos(psif) cos(thetaf)*sin(psif);...
1 0 -sin(thetaf)]*dPf(4:6);
vel_des1 = dPf(1:3) + cross(omega, r1);
vel_des2 = dPf(1:3) + cross(omega, r2);
vel_des3 = dPf(1:3) + cross(omega, r3);
acc_des = [0;0;0];
desired_state1 = [pos_des1;vel_des1;acc_des];
desired_state2 = [pos_des2;vel_des2;acc_des];
desired_state3 = [pos_des3;vel_des3;acc_des];
desired_state = [desired_state1;desired_state2;desired_state3; psif; 0; Pf;
Sf]
size(desired_state)
end
这是Simulink块和错误消息你可以注意到总线只提供了一个元素,而前一个元素提供了38个元素,尽管它们具有相同的输出。
function [desired_state] = desiredtrajectory(in)%(t, pos)
tm= in(1)
pos = in(2:10);
syms t xf yf zf phif thetaf psif pf qf betaf
rf = sqrt(pf^2+qf^2-2*pf*qf*cos(betaf));
h1 = sqrt(0.5*(pf^2+qf^2-0.5*rf^2));
h2 = sqrt(0.5*(rf^2+pf^2-0.5*qf^2));
h3 = sqrt(0.5*(qf^2+rf^2-0.5*pf^2));
alf1 = acos((4*(h1^2+h2^2)-9*pf^2)/(8*h1*h2));
alf2 = acos((4*(h1^2+h3^2)-9*qf^2)/(8*h1*h3));
Rot = RPYtoRot_ZXY(phif, thetaf, psif);
eps = [Rot*[2/3;0;0]+[xf;yf;zf]
Rot*[2/3*h2*cos(alf1);2/3*h2*sin(alf1);0]+[xf;yf;zf]
Rot*[2/3*h2*cos(alf2);-2/3*h3*sin(alf2);0]+[xf;yf;zf]];
X = [ xf yf zf phif thetaf psif pf qf betaf];
Sf = [ 1; 2; pi/4];
dSf = [0;0;0];
Pf = [ 0.1*t; 0; 0.5*sin(0.03*pi*t) + 2; 0; 0.01*pi*t ; 0];
dPf = [ 0.1; 0; 0.5*0.03*pi*cos(0.03*pi*t); 0; 0.01*pi; 0];
qd = [Pf; Sf];
qddot = [dPf; dSf];
jac = jacobian(eps,X);
%%%%%%%%%%%%%
pf = Sf(1); qf = Sf(2); betaf = Sf(3);
xf = Pf(1); yf = Pf(2); zf = Pf(3);
phif = Pf(4); thetaf = Pf(5); psif = Pf(6);
x1=pos(1);
y1=pos(2);
z1=pos(3);
x2=pos(4);
y2=pos(5);
z2=pos(6);
x3=pos(7);
y3=pos(8);
z3=pos(9);
qpf=[(x1+x2+x3)/3;...
(y1+y2+y3)/3;...
(z1+z2+z3)/3;...
atan2((2*z1/3-z2/3-z3/3),(2*y1/3-y2/3-y3/3)); ...
-atan2((2*z1/3-z2/3-z3/3),(2*x1/3-x2/3-x3/3)); ...
atan2((2*y1/3-y2/3-y3/3),(2*x1/3-x2/3-x3/3))];
qsf=[sqrt((x1-x2)^2+(y1-y2)^2+(z1-z2)^2); ...
sqrt((x1-x3)^2+(y1-y3)^2+(z1-z3)^2); ...
acos((pf^2+qf^2-rf^2)/(2*pf*qf))];
q = [qpf;qsf];
%%%%%%%%%%%%%
%%%pos_desired%%%%%%%
pos_des = eval(eps);
pos_des =subs(pos_des,t,tm);
jacval = eval(jac);
qd = eval(qd);%subs(qd,t,tm);
q = eval(q);
qe = qd - q;
qddot = eval(qddot);%subs(qddot,t,tm);
kappa=0.2*eye(9);
qrefdot = qddot + kappa*qe;
vel_des = jacval*qrefdot;
vel_des = subs(vel_des,t,tm);
acc_des = zeros(3,1);
yaw = 0;
yawdot = 0;
% =================== Your code ends here ===================
desired_state1 = [pos_des(1:3);vel_des(1:3);acc_des];
desired_state2 = [pos_des(4:6);vel_des(4:6);acc_des];
desired_state3 = [pos_des(7:9);vel_des(7:9);acc_des];
Pf = subs(Pf,t,tm);
Sf = subs(Sf,t,tm);
format short
digits(3);
desired_state = vpa([desired_state1;desired_state2;desired_state3; psif; 0;
Pf; Sf])
size(desired_state)
end
第二个图像显示第二个函数的输出是标量 - 块的输出处的维度是1 - 而不是38,因为您认为它是。
也就是说,您的功能不会提供与您认为相同的输出。
错误是因为Selector
块期望它们的输入是38维,而它们不是。
要确定为什么你认为发生的事实并非实际发生的事情,你可以使用编辑器在m代码中设置一个断点,运行模型,然后逐步执行代码以确定为什么它给你一个标量输出期望它不这样做。
另一种方法是使用假输入数据从MATLAB命令行运行您的函数。就像是
tmp = desiredtrajectory(randn(10,1))
在这里是合适的。
答案是desiredtrajectory
输出desired_state
,这是一个象征性的变量。是的,它包含38个元素向量,但Simulink将对象本身视为标量。
但真正的问题是你无法沿着Simulink信号传播符号变量。您需要输出为数字向量。
解决这个问题的一种方法是放线
desired_state = double(desired_state);
在文件的末尾将符号对象强制转换为double,它将包含38个元素。
(但是,你不清楚为什么你首先使用符号数学,我建议它会更好,如果你不使用它会更有效率。)