Блог

В модели 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 элементов.

(Однако неясно, почему вы вообще используете символьную математику, и я бы предположил, что это было бы лучше и, безусловно, более эффективно, если бы вы ее не использовали.)