以 2-DOF 机械臂模型作为研究对象,模型参数选取为:l1=1.5m,l2=1m,n1=1.2kg,n2=0.8 kg…
- User
以 2-DOF 机械臂模型作为研究对象,模型参数选取为:l1=1.5m,l2=1m,n1=1.2kg,n2=0.8 kg 模型不确定项设定M(n)=0.01M0,C(n)=0.02C0,G(n)=0.01G0,并且让系统的末端状态不违反输出约束,用matlab语言写
- Assistant
法如下:
% 2-DOF机械臂模型仿真
clear all; clc;
%% 参数定义
% 机械臂参数 l1 = 1.5; % m l2 = 1; % m n1 = 1.2; % kg n2 = 0.8; % kg
% 不确定项 M0 = n1l1^2 + n2l2^2; C0 = 0; G0 = 9.81*(n1l1/2 + n2l2);
Mn = 0.01M0; Cn = 0.02C0; Gn = 0.01*G0;
% 控制参数 Kp = 200; Kv = 100;
% 时间参数 dt = 0.01; t = 0:dt:10;
% 初始状态 q0 = [pi/2; -pi/4]; dq0 = [0; 0];
% 目标状态 qd = [pi/3; pi/6]; dqd = [0; 0];
% 输出约束 y_max = 2;
%% 仿真主程序
% 初始化状态 q = q0; dq = dq0; u = [0; 0];
% 记录状态与控制输入变量 Q = q; DQ = dq; U = u;
for i = 1:length(t) % 计算动力学参数 M = [n1l1^2+n2l2^2+2n2l1l2cos(q(2)), n2l2^2+n2l1l2cos(q(2)); n2l2^2+n2l1l2cos(q(2)), n2l2^2]; C = [-n2l1l2sin(q(2))dq(2), -n2l1l2sin(q(2))(dq(1)+dq(2)); n2l1l2sin(q(2))dq(1), 0]; G = [n1l1/2+n2l1cos(q(2))+n2l2/2; n2l2/2]9.81cos(q(1)) + n2l1l2/2dq(1)dq(2)sin(q(2)); dM = [Mn(rand-0.5), 0; 0, Mn(rand-0.5)]; dC = [Cn(rand-0.5), Cn*(rand-0.5); Cn*(rand-0.5), 0]; dG = [Gn*(rand-0.5); Gn*(rand-0.5)];
% 计算控制力 e = qd-q; de = dqd-dq; u = M*(dqd+Kp*e+Kv*de) + C*dq + G + dM*dq + dC*dq + dG; % 更新状态变量 ddq = inv(M)*(u-C*dq-G-dM*dq-dC*dq-dG); q = q + dq*dt + 0.5*ddq*dt^2; dq = dq + ddq*dt; % 记录状态与控制输入变量 Q = [Q, q]; DQ = [DQ, dq]; U = [U, u]; % 输出约束处理 y = l1*sin(q(1))+l2*sin(q(1)+q(2)); if y > y_max q = [pi/2; -pi/4]; dq = [0; 0]; endend
%% 可视化结果
figure(1) plot(t, Q(1,:), 'b--', t, Q(2,:), 'r-') title('Joint angles') xlabel('Time (s)') ylabel('Angle (rad)') legend('q1', 'q2')
figure(2) plot(t, U(1,:), 'b--', t, U(2,:), 'r-') title('Control input') xlabel('Time (s)') ylabel('Torque (Nm)') legend('u1', 'u2')
figure(3) y = l1sin(Q(1,:))+l2sin(Q(1,:)+Q(2,:)); plot(t, y, 'b-') hold on plot([0, t(end)], [y_max, y_max], 'r--') title('End effector position') xlabel('Time (s)') ylabel('Position (m)') legend('y', 'y_{max}')