求解系统的jacobian矩阵问题

wxl789

我的程序老是出错，但不知道怎么修改。
请高手指导指导。程序如下：
syms x y z
syms u v w g

l1 = 1;  l2 = 1; l3 = 1;
t = 0:100;
% delta
delta = 0.0024*sin(8.2078*t);
ddelta = 0.02*cos(8.2078*t);
dddelta = -0.1642*sin(8.2078*t);


pd1 = 1.5+0.5*cos(2*pi*t);
pd2 = 1.5+0.5*sin(2*pi*t);
pd = [pd1,pd2]';


dpd1 = -pi*sin(2*pi*t);
dpd2 = pi*cos(2*pi*t);
dpd = [dpd1,dpd2]';


ddpd1 = -2*pi*pi*cos(2*pi*t);
ddpd2 = -2*pi*pi*sin(2*pi*t);
ddpd = [ddpd1,ddpd2]';

p1 = l1*cos(x)+l2*cos(x+y)+l3*cos(x+y+z)-delta*sin(x+y+z);
p2 = l1*sin(x)+l2*sin(x+y)+l3*sin(x+y+z)+delta*cos(x+y+z);
p = [p1,p2]';



dp1 = -l1*u*sin(x)-l2*(u+v)*sin(x+y)-l3*(u+v+w)*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z);
dp2 = l1*u*cos(x)+l2*(u+v)*cos(x+y)+l3*(u+v+w)*cos(x+y+z)-delta*(u+v+w)*sin(x+y+z);
dp = [dp1,dp2]';


ep = pd-p;
dep = dpd-dp;




% kv,kp
kp = 40*eye(2);
kv = 12*eye(2);




Jr = [-l1*sin(x)-l2*sin(x+y)-l3*sin(x+y+z)-delta*cos(x+y+z),-l2*sin(x+y)-l3*sin(x+y+z)-delta*cos(x+y+z),-l3*sin(x+y+z)-delta*cos(x+y+z);
      l1*cos(x)+l2*cos(x+y)+l3*cos(x+y+z)-delta*sin(x+y+z),l2*cos(x+y)+l3*cos(x+y+z)-delta*sin(x+y+z),l3*cos(x+y+z)-delta*sin(x+y+z)];

dJr = [-l1*u*cos(x)-l2*(u+v)*cos(x+y)-l3*(u+v+w)*cos(x+y+z)-ddelta*cos(x+y+z)+delta*(u+v+w)*sin(x+y+z),-l2*(u+v)*cos(x+y)-l3*(u+v+w)*cos(x+y+z)-ddelta*cos(x+y+z)+delta*(u+v+w)*sin(x+y+z),-l3*(u+v+w)*cos(x+y+z)-ddelta*cos(x+y+z)+delta*(u+v+w)*sin(x+y+z);
      -l1*u*sin(x)-l2*(u+v)*sin(x+y)-l3*(u+v+w)*sin(x+y+z)-ddelta*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z),-l2*(u+v)*sin(x+y)-l3*(u+v+w)*sin(x+y+z)-ddelta*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z),-l3*(u+v+w)*sin(x+y+z)-ddelta*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z)];
  


  
Jf = [-sin(x+y+z);cos(x+y+z)];
dJf = [-(u+v+w)*cos(x+y+z);-(u+v+w)*sin(x+y+z)];

Jr1 = pinv(Jr);

% equation
% ddtheata = J
J = Jr1*(ddpd+kv*dep+kp*ep-dJr*[u,v,w]'-Jf*ddelta-dJf*ddelta);


dx = u;
dy = v;
dz = w;
du = J(1);
dv = J(2);
dw = J(3);

du=du(:);
dg=1;       % g = t;

Ja = jacobian([u;v;w;J(1);J(2);J(3);g], [x,y,z,u,v,w,g])

错误提示如下：
??? Error using ==> sym.svd
Too many input arguments.
Error in ==> pinv at 29
   [U,S,V] = svd(A,0);
Error in ==> pinv at 27
   X = pinv(A',varargin{:})';
Error in ==> ex1_jacobian at 65
Jr1 = pinv(Jr);

请教--------------------

wxl789

请高手救命啊。。。。。。

着急的很，急切等待中————————-

wxl789

现在上边的问题已经解决。
到最后发现，matlab算广义逆的时候，如果维数过高或者是矩阵过于复杂，即使是很简单的一中运算，它都求不出来，这时候我们必须按照广义逆的基本定义来求。
这是我做这个的一点发现。
随着这个问题的解决，紧接着又有新的 问题，请各位高手帮忙。
clear
clc
syms x y z u v w g

l1 = 1;  l2 = 1; l3 = 1;
t = 0:3;
% delta
delta = 0.0024*sin(8.2078*t);
ddelta = 0.02*cos(8.2078*t);
dddelta = -0.1642*sin(8.2078*t);


pd1 = 1.5+0.5*cos(2*pi*t);
pd2 = 1.5+0.5*sin(2*pi*t);
pd = [pd1,pd2]';


dpd1 = -pi*sin(2*pi*t);
dpd2 = pi*cos(2*pi*t);
dpd = [dpd1,dpd2]';


ddpd1 = -2*pi*pi*cos(2*pi*t);
ddpd2 = -2*pi*pi*sin(2*pi*t);
ddpd = [ddpd1,ddpd2]';

p1 = l1*cos(x)+l2*cos(x+y)+l3*cos(x+y+z)-delta*sin(x+y+z);
p2 = l1*sin(x)+l2*sin(x+y)+l3*sin(x+y+z)+delta*cos(x+y+z);
p = [p1,p2]';



dp1 = -l1*u*sin(x)-l2*(u+v)*sin(x+y)-l3*(u+v+w)*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z);
dp2 = l1*u*cos(x)+l2*(u+v)*cos(x+y)+l3*(u+v+w)*cos(x+y+z)-delta*(u+v+w)*sin(x+y+z);
dp = [dp1,dp2]';


ep = pd-p;
dep = dpd-dp;




% kv,kp
kp = 40*eye(2);
kv = 12*eye(2);


Jr = [-l1*sin(x)-l2*sin(x+y)-l3*sin(x+y+z)-delta*cos(x+y+z),-l2*sin(x+y)-l3*sin(x+y+z)-delta*cos(x+y+z),-l3*sin(x+y+z)-delta*cos(x+y+z);
      l1*cos(x)+l2*cos(x+y)+l3*cos(x+y+z)-delta*sin(x+y+z),l2*cos(x+y)+l3*cos(x+y+z)-delta*sin(x+y+z),l3*cos(x+y+z)-delta*sin(x+y+z)];

dJr = [-l1*u*cos(x)-l2*(u+v)*cos(x+y)-l3*(u+v+w)*cos(x+y+z)-ddelta*cos(x+y+z)+delta*(u+v+w)*sin(x+y+z),-l2*(u+v)*cos(x+y)-l3*(u+v+w)*cos(x+y+z)-ddelta*cos(x+y+z)+delta*(u+v+w)*sin(x+y+z),-l3*(u+v+w)*cos(x+y+z)-ddelta*cos(x+y+z)+delta*(u+v+w)*sin(x+y+z);
      -l1*u*sin(x)-l2*(u+v)*sin(x+y)-l3*(u+v+w)*sin(x+y+z)-ddelta*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z),-l2*(u+v)*sin(x+y)-l3*(u+v+w)*sin(x+y+z)-ddelta*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z),-l3*(u+v+w)*sin(x+y+z)-ddelta*sin(x+y+z)-delta*(u+v+w)*cos(x+y+z)];
  


  
Jf = [-sin(x+y+z);cos(x+y+z)];
dJf = [-(u+v+w)*cos(x+y+z);-(u+v+w)*sin(x+y+z)];

% Jr1 = pinv(Jr);
% Jr1 = (Jr)'*inv(((Jr)*(Jr)'));（修改部分）
J1 = (Jr)';
J2 = Jr*J1;
J3 = inv(J2);
Jr1 = J1*J3;

% equation
% ddtheata = J
J = Jr1*(ddpd+kv*dep+kp*ep-dJr*[u,v,w]'-Jf*ddelta-dJf*ddelta);


dx = u;
dy = v;
dz = w;
du = J(1);
dv = J(2);
dw = J(3);

du=du(:);
dg=1;       % g = t;

Ja = jacobian([u;v;w;J(1);J(2);J(3);g], [x,y,z,u,v,w,g])

错误信息：
??? Error using ==> sym.mtimes
Inner matrix dimensions must agree.
Error in ==> ex1_jacobian at 75
J = Jr1*(ddpd+kv*dep+kp*ep-dJr*[u,v,w]'-Jf*ddelta-dJf*ddelta);

wxl789

我知道这是一个基本的错误，错在时间t，如果上边的程序中t=1的话，程序即可运行出结果，但是t不可能只取一个值啊。。

请高手教我怎么修改，谢谢！！！

xjzuo

作一个t 的for循环即可解决问题。
存储的话，可以用cell数组.

wxl789

十分感谢你的方法。
热心的sogooda也这么给我讲的，这个问题已经解决。

我这个程序是为了应用在let工具箱的，但是let中的时间应该不是这么取的，所以还是报错：
??? Error using ==> setting('checking')
Error using ==> maple
Error, (in jacobian) wrong number (or type) of parameters in function diff

查了diff函数，没有发现什么不合语法的地方啊。
还请各位继续指导
再次感谢！！！