机器人动力学--双连杆力驱动控制

在simulink机器人双连杆仿真中，通过设计function函数将角度生成机器人关节力矩输入进去；系统报错，但功能函数在matlab中可以正常使用；q1=sin(pi/3)*t;  q2=0;

function [t11, t22 ] =fcn(q1,q2)
T = 10;  % 总仿真时间
dt = 1;  % 时间步长
t = 0:dt:T;  % 时间向量

% 将t1和t2初始化为数组
t1 = zeros(size(t));
t2 = zeros(size(t));

% 定义关节角度 q1 和 q2
%q1 = sin((pi/3)*t);
%q2 = zeros(size(t));

% 初始化速度和加速度
for j=1:length(t)

dq1 = diff(q1) / dt;
dq2 = diff(q2) / dt;
ddq1 = diff(dq1) / dt;
ddq2 = diff(dq2) / dt;

end
% 惯性张量参数
L_1xx = 4.71239e-06;
L_1xy = 0;
L_1xz = 0;
L_1yy = 0.000709215;
L_1yz = 0;
L_1zz = 0.000709215;

L_2xx = 6.28319e-06;
L_2xy = 0;
L_2xz = 0;
L_2yy = 0.000422021;
L_2yz = 0;
L_2zz = 0.000422021;

% 连杆参数：质量和长度
m_1 = 0.0942478;
m_2 = 0.125664;
l1 = 0.3;
l2 = 0.2;

% 连杆质心位置
l_1x = l1 / 2;
l_1y = 0;
l_1z = 0;
l_2x = l2 / 2;
l_2y = 0;
l_2z = 0;

% 初始化 DH 参数和关节信息的单元数组
dh = cell(1, length(t));
q = cell(1, length(t));
dq = cell(1, length(t));
ddq = cell(1, length(t));

% 初始化外部细胞数组
w = cell(1, length(t)); % 创建每个连杆的角速度
v = cell(1, length(t)); % 创建每个连杆的线速度
alpha = cell(1, length(t)); % 创建每个两岸的角加速度
a = cell(1, length(t)); % 创建每个连杆的线加速度
ac = cell(1, length(t)); % 创建每个连杆的质心加速度
l = cell(1, length(t)); % 创建每个连杆的质心
T = cell(1, length(t)); % 存储每个连杆坐标变换矩阵
R = cell(1, length(t)); % 存储每个连杆的旋转矩阵
r = cell(1, length(t)); % 存储每个连杆的偏移量
I = cell(1, length(t)); % 存储每个连杆的惯性张量
T_e = cell(1, length(t)); % 存储末端执行器相对于基坐标的变换矩阵
t_e_pre=cell(1,length(t));%存储末端执行器相对于基座标系的变换矩阵
w_0_0 = cell(1,length(t));%存储初始角速度 (w)
v_0_0 =cell(1,length(t));%存储初始线速度
alpha_0_0 = cell(1,length(t));%存储初始角加速度
a_0_0 =cell(1,length(t));%存储初始线加速度
tr = cell(1,length(t));
l=cell(1,length(t));%存储连杆的质心
tau = cell(1,length(t));%存储所需的力矩
m = cell(1,length(t)); %存储连杆的质量
t1=cell(length(t),1);
t2 = cell(length(t),1);
% 遍历时间步
for j = 1:length(t)-2
    % 为每个时间步填充 DH 参数和关节信息 
    dh{j} = [0, 0, 0, q1(j);
             0, l1, 0, q2(j)];

    q{j} = [q1(j), q2(j)];
    dq{j} = [dq1(j), dq2(j)];
    ddq{j} = [ddq1(j), ddq2(j)];

% 初始化内部细胞数组
    w{j} = cell(1, 2);
    v{j} = cell(1, 2);
    alpha{j} = cell(1, 2);
    a{j} = cell(1, 2);
    ac{j} = cell(1, 2);
    l{j} = cell(1, 2);
    T{j} = cell(1, 2);
    R{j} = cell(1, 2);
    r{j} = cell(1, 2);
    I{j} = cell(1, 2);
    T_e{j} =cell(1, 2);
    t_e_pre{j}=cell(1,2);
    w_0_0{j} = [0; 0; 0];
    v_0_0{j} = [0; 0; 0]; 
    alpha_0_0{j} = [0; 0 ; 0];
    a_0_0 {j}= [0; 9.81; 0];
    tr{j}=zeros(3,3);
    tau{j} = cell(1,2);%存储每个连杆上的扭矩
    m{j}=cell(1,2);

% 为其他细胞数组元素分配值
    for k = 1:2
        w{j}{k} = zeros(3, 1);  % 初始化角速度
        v{j}{k} = zeros(3, 1);  % 初始化线速度
        alpha{j}{k} = zeros(3, 1);  % 初始化角加速度
        a{j}{k} = zeros(3, 1);  % 初始化线加速度
        ac{j}{k} = zeros(3, 1);  % 初始化质心加速度
        l{j}{k} = zeros(3, 1);  % 初始化质心
        T{j}{k} = eye(4);  % 初始化坐标变换矩阵
        R{j}{k} = eye(3);  % 初始化旋转矩阵
        r{j}{k} = zeros(3, 1);  % 初始化偏移量
        I{j}{k} = zeros(3, 3);  % 初始化惯性张量
        T_e{j}{k} = eye(4);  % 初始化末端执行器的坐标变换矩阵
        t_e_pre{j}{k}= eye(4);%初始化末端执行器相对于基座标系的变换矩阵
    end
     %tr=zeros(3,3);

     
     
end

for j = 1:length(t)-2
    for i = 1:2
        % 获取 DH 参数
        theta = dh{j}(i, 1);
        d = dh{j}(i, 2);
        aa = dh{j}(i, 3);
        alph = dh{j}(i, 4);

        trx = [1 0 0 0; 
           0 cos(theta) -sin(theta) 0;
           0 sin(theta) cos(theta)  0;
           0 0 0 1];
        tl = [1 0 0 d ;
          0 1 0 0;
          0 0 1 aa;
          0 0 0 1];
        trz = [cos(alph) -sin(alph) 0 0;
           sin(alph) cos(alph)  0 0;
           0 0 1 0;
           0 0 0 1];
    %trx、tl 和 trz：这些矩阵分别表示绕X轴、平移和绕Z轴的变换矩阵。它们根据 DH 参数矩阵 dh 中的参数构建，

        T{j}{i} = trx * tl * trz;  %trotx(dh(i, 1))*transl([dh(i, 2), 0, dh(i, 3)])*trotz(dh(i, 4));

      %  T{j}{i} = trotx(theta) * transl([aa, 0, d]) * trotz(alph); % 计算齐次变换矩阵T
        R{j}{i} = T{j}{i}(1:3, 1:3);%提取出当前旋转矩阵
        r{j}{i} = T{j}{i}(1:3, 4);%提取当前平移变量
        T_e{j}{i} = t_e_pre{j}{i} * T{j}{i};%计算了末端执行器坐标系相对于基坐标系的坐标变换矩阵
        t_e_pre{j}{i} = T_e{j}{i};%在每次迭代中，将 t_e_pre 更新为当前的末端执行器坐标系相对于基坐标系的变换矩阵，以备下一次迭代使用

    
    end
  
    i=3;
    T{j}{i} = eye(4);%将i+1连杆的变换矩阵初始化为单位矩阵
    R{j}{i} = eye(3);%将i+1连杆的旋转矩阵初始化为单位矩阵
    r{j}{i} = [l2, 0, 0];%下一个连杆在x轴偏移12
    %T_e{j}{i} = T_e{j}{i-1} * transl([l2, 0, 0]);%更新机器人的姿态信息
    T_e{j}{i} = T_e{j}{i-1} * [1 0 0 l2; 0 1 0 0; 0 0 1 0; 0 0 0 1];

    
end

pre_w = w_0_0; %将 pre_w 初始化为初始角速度 w_0_0。
pre_v = v_0_0; %将 pre_v 初始化为初始线速度 v_0_0
pre_alpha = alpha_0_0; %将 pre_alpha 初始化为初始角加速度 alpha_0_0
pre_a = a_0_0; %将 pre_a 初始化为初始线加速度 a_0_0

%forward iteration 正向迭代计算出速度、加速度信息
for j = 1:length(t)-2
    for i = 1:2
        tr{j}= (R{j}{i})';% 获取当前连杆的旋转矩阵 R{i} 描述了当前连杆坐标系相对于前一个连杆坐标系的旋转。
         w_i_i{j} = tr{j} * pre_w{j} + dq{j}(i) * [0;0;1];%计算当前连杆的角速度，是由前一连杆的角速度 pre_w 经过旋转变换并加上当前连杆的关节速度 dq(i) 乘以 [0;0;1] 得到
         v_i_i{j} = tr{j} * (pre_v{j} + cross(pre_w{j}, r{j}{i}));%计算当前连杆内的线速度 v_i_i。它由前一个连杆的线速度 pre_v 经过旋转变换并加上关节速度引起的线速度变化 cross(pre_w, r{i}) 得到。
         
         alpha_i_i{j}= tr{j} * pre_alpha{j} + cross((tr{j} * pre_w{j}), dq{j}(i) * [0;0;1]) + ddq{j}(i) * [0;0;1];
         %计算当前连杆内的角加速度alpha_i_i。它由前一个连杆的角加速度pre_alpha 经过旋转变换，加上角速度引起的
         %角加速度变化 cross((t * pre_w), dq(i) * [0;0;1])，再加上当前连杆的关节加速度 ddq(i) * [0;0;1] 得到。

         a_i_i {j}= tr{j} * (pre_a{j} + cross(pre_alpha{j}, r{j}{i}) + cross(pre_w{j}, cross(pre_w{j}, r{j}{i})));
         % 计算当前连杆内的线加速度 a_i_i。它由前一个连杆的线加速度 pre_a 经过旋转变换，加上角加速度引起的
         % 线加速度变化 cross(pre_alpha, r{i})，以及角速度引起的线加速度变化 cross(pre_w, cross(pre_w, r{i})) 得到。
        
         ac_i_i{j}= a_i_i{j} + cross(alpha_i_i{j}, l{j}{i}) + cross(w_i_i{j}, cross(w_i_i{j}, l{j}{i}));
         %计算当前连杆内的质心加速度 ac_i_i。它由线加速度 a_i_i 加上角加速度引起的加速度变化 cross(alpha_i_i, l{i})
         % 以及角速度引起的加速度变化 cross(w_i_i, cross(w_i_i, l{i})) 得到

        w{j}{i} = w_i_i{j}; %将当前连杆的角速度存储在 w 单元格数组中。
        v{j}{i} = v_i_i{j}; %将当前连杆的线速度存储在 v 单元格数组中。
        alpha{j}{i} = alpha_i_i{j}; %将当前连杆的角加速度存储在 alpha 单元格数组中。
        a{j}{i} = a_i_i{j}; % 将当前连杆的线加速度存储在 a 单元格数组中。
        ac{j}{i} = ac_i_i{j}; %将当前连杆的质心加速度存储在 ac 单元格数组中。

        pre_w{j} = w_i_i{j};
        pre_v{j}= v_i_i{j};
        pre_alpha{j} = alpha_i_i{j};
        pre_a{j}= a_i_i{j};
        %将当前连杆的速度和加速度信息更新为前一个连杆的速度和加速度，以便在下一次迭代中使用
    end
end

%m = [m_1; m_2];  %质量矩阵  m_1 第一个连杆的质量，m_2 第二个连杆的质量。

% backward iteration %反向迭代
for j = 1:length(t)-2
    f_3_3 {j}= [0;0;0]; %表示最后一个连杆受到的力，并将其初始化
    n_3_3{j} = [0;0;0]; %表示最后一个连杆受到的扭矩，并将其初始化
    next_f_i_i{j} = f_3_3{j}; %表示下一连杆受到的力，并将其初始化
    next_n_i_i{j} = n_3_3{j}; %表示下一连杆受到的扭矩，并将其初始化
    I{j}{1} = [L_1xx L_1xy L_1xz; L_1xy L_1yy L_1yz; L_1xz L_1yz L_1zz];%第一个连杆的惯性张量矩阵
    I{j}{2} = [L_2xx L_2xy L_2xz; L_2xy L_2yy L_2yz; L_2xz L_2yz L_2zz];%第二个连杆的惯性张量矩阵
    m{j}{1}=m_1;
    m{j}{2}=m_2;
    for i = 2:-1:1   %从第二个连杆开始向第一个连杆逆向迭代。从末端向基座开始计算。
        rr{j}= R{j}{i+1}; %从第 i+1 个连杆到第 i 个连杆的旋转矩阵
         f_i_i{j} = m{j}{i}*ac{j}{i} + rr{j} * next_f_i_i{j};%f_i_i是当前连杆上的合力，它是质点力乘以加速度的总和，加上来自下一个连杆的力通过旋转矩阵变换过来
         TT{j}= T_e{j}{i};%TT 是用于将外部力和扭矩从下一个连杆坐标系转换到当前连杆坐标系的转换矩阵。
         TT{j}(1:3,1:3) = (T_e{j}{i}(1:3,1:3))';%将转换矩阵 TT 的旋转部分取转置，以实现从下一个连杆到当前连杆的转换。
         TT{j}(1:3, 4) = -(T_e{j}{i}(1:3,1:3))' * T_e{j}{i}(1:3,4);%计算了位移矩阵，以实现从下一个连杆坐标系到当前连杆坐标系的转换。
         xx{j} = TT{j}* T_e{j}{i+1}(1:4, 4);%使用转换矩阵 TT 来计算从下一个连杆坐标系到当前连杆坐标系的位移。
         r_ip1_ci{j} = l{j}{i} - xx{j}(1:3);%r_ip1_ci 表示当前连杆上质心到下一个连杆坐标系原点的位置矢量。
         torque{j}{i} = I{j}{i}*alpha{j}{i} + cross(w{j}{i}, I{j}{i}*w{j}{i}) + rr{j} * next_n_i_i{j} + cross(l{j}{i}, f_i_i{j}) - cross(r_ip1_ci{j}, rr{j} * next_f_i_i{j});
         %计算了当前连杆上的扭矩。它包括惯性扭矩、Coriolis扭矩、外部扭矩、惯性力矩和反作用力矩等各个部分。
         tau{j}{i} = torque{j}{i}(3);
         next_f_i_i{j}= f_i_i{j};
         next_n_i_i{j}= torque{j}{i};%更新了下一个连杆上的力和扭矩，以便在下一次迭代中使用。

    end
    t1{j} = tau{j}{1};
    t2{j}= tau{j}{2};
end

for j = 1:length(t)-2
    t11 = t1{j}
    t22 = t2{j}
end
系统运行之后报错

这种情况怎么进行处理？有没有大佬帮忙解决一下