机器人刚体动力学由以下方程控制！！！
startup_rvc
mdl_puma560
p560.dyn
提前计算出来这些“disturbance”，然后在控制环路中将它“抵消”（有时候也叫前馈控制）
求出所需要的力矩，其中M项代表克服了机械臂的加速度惯量以及不同连杆之间的惯量影响所需力矩、C项代表了克服科里奥利力和离心力所需力矩、G项代表了克服地球引力力矩。
Inverse Dynamics叫做反向动力学，它的作用是输入想要的关节速度(qd)、关节加速度(qdd)、关节角度(q)，输出为每个关节所需要的力矩（u）。当然也有正向动力学（forward dynamics）,它的作用和反向动力学相反，输入关节角度（q）、关节速度（qd）、每个关节的力矩（u），输出为每个关节的加速度（qdd）
“前馈控制”的例子，它的作用是预先计算出所需的力矩，输入给控制器，“反馈”的存在是为了消除一些误差，例如摩擦力
J=[-sin(q1)-sin(q1+q2) -sin(q1+q2);
cos(q1)+cos(q1+q2) cos(q1+q2)];
d_J=[-dq1*cos(q1)-(dq1+dq2)*cos(q1+q2) -(dq1+dq2)*cos(q1+q2);
-dq1*sin(q1)-(dq1+dq2)*sin(q1+q2) -(dq1+dq2)*sin(q1+q2)];
function torque_out = fcn(u_matrix,q1,q2)
%% Robot Joint and mass parameters
g=9.81;
m1=0.3;
m2=1;
l1=0.2;
l2=0.2;
%% B = M Matrix
B=[ (m1+m2)*l1^2+m2*l2^2+2*m2*l2*l1*cos(q2) (m2*l2^2+m2*l1*l2*cos(q2)) ; (m2*l2^2+m2*l1*l2*cos(q2)) m2*l2^2];
torque_out= B* u_matrix ;
end
C=[-(m2*l1*l2*sin(q2)*(2*q1dot*q2dot+q2dot^2)) ; -m2*l1*l2*sin(q2)*q1dot^2];
G=[(m1+m2)*g*l1*cos(q1)+m2*l2*g*cos(q1+q2) ; +m2*g*l2*cos(q1+q2)];
Minv=[ (m2*l2^2)/(((m1+m2)*l1^2*l2^2*m2)-(m2^2*l1^2*l2^2*cos(q2))) -(m2*l2^2+m2*l1*l2*cos(q2))/(((m1+m2)*l1^2*l2^2*m2)-(m2^2*l1^2*l2^2*cos(q2))) ; -(m2*l2^2+m2*l1*l2*cos(q2))/(((m1+m2)*l1^2*l2^2*m2)-(m2^2*l1^2*l2^2*cos(q2))) ((m1+m2)*l1^2+m2*l2^2+2*m2*l2*l1*cos(q2))/(((m1+m2)*l1^2*l2^2*m2)-(m2^2*l1^2*l2^2*cos(q2)))];
alfa = 6.7;beta = 3.4;
epc = 3.0;eta = 0;
m1 = 1;l1 = 1;
lc1 = 1/2;I1 = 1/12;
g = 9.8;
e1 = m1*l1*lc1-I1-m1*l1^2;
e2 = g/l1;
M = [alfa+2*epc*cos(q2)*eta*sin(q2),beta+epc*cos(q2)+eta*sin(q2);beta+epc*cos(q2)+eta*sin(q2),beta];
C = [(-2*epc*sin(q2)+2*eta*cos(q2))*dq2,(-epc*sin(q2)+eta*cos(q2)*dq2;(epc*sin(q2)-eta*cos(q2)*dq1,0];
G = [epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)+(alfa-beta+e1)*e2*cos(q1);epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)];
J=[-sin(q1)-sin(q1+q2) -sin(q1+q2);
cos(q1)+cos(q1+q2) cos(q1+q2)];
画圆轨迹：
xc=[1.0-0.2*cos(pi*t) 1.0+0.2*sin(pi*t)]';
dxc=[0.2*pi*sin(pi*t) 0.2*pi*cos(pi*t)]';
ddxc=[0.2*pi^2*cos(pi*t) -0.2*pi^2*sin(pi*t)]';
function [xd, yd] = fcn(r, t, w, x, y)
alpha = t*w;
xd = x + r*cos(alpha);
yd = y + r*sin(alpha);
end
基于力矩的阻抗控制
动力学模型如下
阻抗控制模型如下
把这个阻抗模型得到的加速度带入到机器人动力学模型中，消除加速度ddx非线性项，得到控制律
基于模型的阻抗控制
笛卡尔空间的机器人动力学模型可描述为：
阻抗控制器可设计为：
————————————————————————————————
BA的存在导致系统耦合，如果希望在与环境交互过程中保持线性并解耦，需要测量接触力：
因此，
正向动力学
正运动学：
雅克比矩阵：
笛卡尔速度与及速度：
定义末端实际位置与虚拟平衡位置的偏差：
阻抗控制目标为：实现末端执行器标架相对于柔顺标架的偏差与末端执行器作用于环境的力旋量之间的二阶期望动态关系！
刚性机器人在末端空间的动力学方程为：、
其中：
% 假设的二自由度机器人参数
L1 = 1; % 连杆1的长度
L2 = 1; % 连杆2的长度
% 假设的关节角度（需要实时更新）
q = [pi/4; pi/6]; % 初始关节角度（弧度制）
% 计算末端执行器的位置（在笛卡尔空间中）
% 这是一个简化的例子，实际中可能需要使用更复杂的正向运动学方程
x = L1*cos(q(1)) + L2*cos(q(1) + q(2));
y = L1*sin(q(1)) + L2*sin(q(1) + q(2));
z = 0; % 假设机器人在二维平面上运动
X = [x; y; z];
% 计算雅可比矩阵（简化的例子）
% 这里只考虑了平面情况，并且假设了简化的雅可比矩阵
% 在实际应用中，雅可比矩阵需要根据机器人的实际结构来计算
J = [-L1*sin(q(1)) - L2*sin(q(1) + q(2)), -L2*sin(q(1) + q(2));
L1*cos(q(1)) + L2*cos(q(1) + q(2)), L2*cos(q(1) + q(2))];
% 阻抗控制参数
M_d = diag([1, 1, 1]); % 期望的惯性矩阵
B_d = diag([2, 2, 2]); % 期望的阻尼矩阵
K_d = diag([100, 100, 100]); % 期望的刚度矩阵
% 期望的末端执行器位置、速度和加速度（需要实时更新）
Xd = [1; 1; 0]; % 期望位置
Xdot_d = [0; 0; 0]; % 期望速度
Xddot_d = [0; 0; 0]; % 期望加速度（通常不需要直接指定）
% 外部力（假设为0或根据传感器数据获取）
F_ext = [0; 0; 0];
% 阻抗控制律
% 计算位置误差
e = Xd - X;
% 计算阻抗力
F_impedance = -M_d*Xddot_d - B_d*Xdot_d - K_d*e;
% 将阻抗力从笛卡尔空间转换到关节空间
tau = J' * F_impedance;
% 如果需要添加重力补偿或其他动力学项，可以在这里添加
% 例如：tau = tau + g(q) + ...;function tau = impedance_control(q, qd, xd_desired, xdot_desired, xddot_desired)
% 机械臂参数
m1 = 1; m2 = 1; % 连杆质量
l1 = 1; l2 = 1; % 连杆长度
% 控制参数
K_d = diag([100, 100]); % 阻抗控制中的刚度系数
D_d = diag([20, 20]); % 阻抗控制中的阻尼系数
% 运动学和动力学矩阵计算
x = forward_kinematics(q, l1, l2);
J = jacobian(q, l1, l2);
M = inertia_matrix(q, m1, m2, l1, l2);
C = coriolis_matrix(q, qd, m1, m2, l1, l2);
g = gravity_vector(q, m1, m2, l1, l2);
% Lambda和mu的计算
Lambda = inv(J' * inv(M) * J);
mu = inv(J') * (C - M * inv(J) * jacobian_dot(q, qd, l1, l2)) * inv(J);
% 误差计算
x_tilde = x - xd_desired;
xdot_tilde = J * qd - xdot_desired;
% 控制律计算
tau = g + J' * (Lambda * xddot_desired + mu * (J * qd)) - J' * Lambda * (K_d * x_tilde + D_d * xdot_tilde);
end
function x = forward_kinematics(q, l1, l2)
x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
end
function J = jacobian(q, l1, l2)
J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
l1 * cos(q(1)) + l2 * cos(q(1) + q(2)), l2 * cos(q(1) + q(2))];
end
function J_dot = jacobian_dot(q, qd, l1, l2)
J_dot = [-l1 * cos(q(1)) * qd(1) - l2 * cos(q(1) + q(2)) * (qd(1) + qd(2)), -l2 * cos(q(1) + q(2)) * (qd(1) + qd(2));
-l1 * sin(q(1)) * qd(1) - l2 * sin(q(1) + q(2)) * (qd(1) + q(2)), -l2 * sin(q(1) + q(2)) * (qd(1) + q(2))];
end
function M = inertia_matrix(q, m1, m2, l1, l2)
M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
M21 = M12;
M22 = m2 * l2^2;
M = [M11, M12; M21, M22];
end
function C = coriolis_matrix(q, qd, m1, m2, l1, l2)
C11 = -m2 * l1 * l2 * sin(q(2)) * qd(2);
C12 = -m2 * l1 * l2 * sin(q(2)) * (qd(1) + qd(2));
C21 = m2 * l1 * l2 * sin(q(2)) * qd(1);
C22 = 0;
C = [C11, C12; C21, C22];
end
function g = gravity_vector(q, m1, m2, l1, l2)
g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
g = [g1; g2];
end
function [q_dot, q_ddot] = robot_dynamics(q, qd, tau)
% 机械臂参数
m1 = 1; m2 = 1; % 连杆质量
l1 = 1; l2 = 1; % 连杆长度
% 计算动力学矩阵
M = inertia_matrix(q, m1, m2, l1, l2);
C = coriolis_matrix(q, qd, m1, m2, l1, l2);
g = gravity_vector(q, m1, m2, l1, l2);
% 动力学方程求解
q_ddot = inv(M) * (tau - C * qd - g);
% 更新关节速度
q_dot = qd;
end
function M = inertia_matrix(q, m1, m2, l1, l2)
M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
M21 = M12;
M22 = m2 * l2^2;
M = [M11, M12; M21, M22];
end
function C = coriolis_matrix(q, qd, m1, m2, l1, l2)
C11 = -m2 * l1 * l2 * sin(q(2)) * qd(2);
C12 = -m2 * l1 * l2 * sin(q(2)) * (qd(1) + qd(2));
C21 = m2 * l1 * l2 * sin(q(2)) * qd(1);
C22 = 0;
C = [C11, C12; C21, C22];
end
function g = gravity_vector(q, m1, m2, l1, l2)
g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
g = [g1; g2];
end
% 二自由度机器人末端空间阻抗控制仿真程序
% 机械臂参数
m1 = 1; m2 = 1; % 连杆质量
l1 = 1; l2 = 1; % 连杆长度
% 初始状态
q = [pi/4; pi/4]; % 关节角度
qd = [0; 0]; % 关节角速度
x = forward_kinematics(q); % 末端位置
xd = [0; 0]; % 末端速度
% 控制参数
K_d = diag([100, 100]); % 阻抗控制中的刚度系数
D_d = diag([20, 20]); % 阻抗控制中的阻尼系数
% 仿真参数
dt = 0.01; % 仿真时间步长
t_final = 10; % 仿真总时间
time = 0:dt:t_final;
% 期望轨迹
xd_desired = [0.5*sin(0.1*time); 0.5*cos(0.1*time)];
xdot_desired = [0.05*cos(0.1*time); -0.05*sin(0.1*time)];
xddot_desired = [-0.005*sin(0.1*time); -0.005*cos(0.1*time)];
% 数据存储
q_history = zeros(2, length(time));
x_history = zeros(2, length(time));
for i = 1:length(time)
% 前向运动学
x = forward_kinematics(q);
J = jacobian(q);
% 计算Lambda, mu
M = inertia_matrix(q, m1, m2, l1, l2);
C = coriolis_matrix(q, qd, m1, m2, l1, l2);
Lambda = inv(J' * inv(M) * J);
mu = inv(J') * (C - M * inv(J) * jacobian_dot(q, qd)) * inv(J);
% 误差计算
x_tilde = x - xd_desired(:, i);
xdot_tilde = J * qd - xdot_desired(:, i);
% 控制律计算
g = gravity_vector(q, m1, m2, l1, l2);
tau = g + J' * (Lambda * xddot_desired(:, i) + mu * (J * qd)) ...
- J' * Lambda * (K_d * x_tilde + D_d * xdot_tilde);
% 动力学方程求解
qdd = inv(M) * (tau - C * qd - g);
% 状态更新
qd = qd + qdd * dt;
q = q + qd * dt;
% 数据存储
q_history(:, i) = q;
x_history(:, i) = x;
end
% 绘图
figure;
subplot(2, 1, 1);
plot(time, q_history);
title('关节角度随时间变化');
xlabel('时间 (s)');
ylabel('关节角度 (rad)');
legend('q1', 'q2');
subplot(2, 1, 2);
plot(time, x_history);
title('末端位置随时间变化');
xlabel('时间 (s)');
ylabel('末端位置 (m)');
legend('x', 'y');
% 函数定义
function x = forward_kinematics(q)
l1 = 1; l2 = 1;
x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
end
function J = jacobian(q)
l1 = 1; l2 = 1;
J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
l1 * cos(q(1)) + l2 * cos(q(1) + q(2)), l2 * cos(q(1) + q(2))];
end
function J_dot = jacobian_dot(q, qd)
l1 = 1; l2 = 1;
J_dot = [-l1 * cos(q(1)) * qd(1) - l2 * cos(q(1) + q(2)) * (qd(1) + qd(2)), -l2 * cos(q(1) + q(2)) * (qd(1) + qd(2));
-l1 * sin(q(1)) * qd(1) - l2 * sin(q(1) + q(2)) * (qd(1) + qd(2)), -l2 * sin(q(1) + q(2)) * (qd(1) + qd(2))];
end
function M = inertia_matrix(q, m1, m2, l1, l2)
M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
M21 = M12;
M22 = m2 * l2^2;
M = [M11, M12; M21, M22];
end
function C = coriolis_matrix(q, qd, m1, m2, l1, l2)
C11 = -m2 * l1 * l2 * sin(q(2)) * qd(2);
C12 = -m2 * l1 * l2 * sin(q(2)) * (qd(1) + qd(2));
C21 = m2 * l1 * l2 * sin(q(2)) * qd(1);
C22 = 0;
C = [C11, C12; C21, C22];
end
function g = gravity_vector(q, m1, m2, l1, l2)
g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
g = [g1; g2];
end
function impedance_control_simulation()
% 机器人参数
m1 = 1; m2 = 1; % 连杆质量
l1 = 1; l2 = 1; % 连杆长度
% 阻抗控制参数
Kd = diag([10, 10]); % 刚度矩阵
Dd = diag([5, 5]); % 阻尼矩阵
% 期望轨迹：圆形运动
t = linspace(0, 10, 1000); % 时间范围
x_d = [0.5*cos(t); 0.5*sin(t)]; % 期望末端位置
dx_d = [-0.5*sin(t); 0.5*cos(t)]; % 期望末端速度
ddx_d = [-0.5*cos(t); -0.5*sin(t)]; % 期望末端加速度
% 初始化机器人状态
q = [pi/4; pi/4]; % 关节角度
dq = [0; 0]; % 关节速度
% 仿真参数
dt = t(2) - t(1); % 时间步长
% 存储数据用于绘图
q_history = zeros(2, length(t));
dq_history = zeros(2, length(t));
x_history = zeros(2, length(t));
% 开始仿真
for i = 1:length(t)
% 计算当前机器人状态
x = forward_kinematics(q, l1, l2); % 计算当前末端位置
J = jacobian(q, l1, l2); % 计算当前雅可比矩阵
M = inertia_matrix(q, m1, m2, l1, l2); % 计算当前动力学矩阵
C = coriolis_matrix(q, dq, m1, m2, l1, l2); % 计算当前科氏力矩
g = gravity_vector(q, m1, m2, l1, l2); % 计算当前重力矢量
% 计算控制律中的各项
Lambda = inv(J' * inv(M) * J); % 阻抗矩阵
mu = J' * (C - M * inv(J) * (J' * inv(M) * J)) * inv(J); % 阻抗补偿项
Lambda_d = eye(2); % 期望阻抗矩阵（假设为单位矩阵）
F_ext = [0; 0]; % 外部力（暂时设为零向量）
tilde_x = x - x_d(:, i); % 位置误差
dot_tilde_x = J * dq - dx_d(:, i); % 速度误差
% 计算控制力
tau = g + J' * (Lambda * ddx_d(:, i) + mu * dx_d(:, i)) - ...
J' * Lambda * Lambda_d * (Kd * tilde_x + Dd * dot_tilde_x) + ...
J' * (Lambda * Lambda_d^(-1) - eye(2)) * F_ext;
% 使用机器人动力学方程求解关节加速度
q_ddot = inv(M) * (tau - C * dq - g);
% 更新机器人状态
dq = dq + q_ddot * dt; % 更新关节速度
q = q + dq * dt; % 更新关节角度
% 存储数据用于绘图
q_history(:, i) = q;
dq_history(:, i) = dq;
x_history(:, i) = x;
end
% 绘制结果
figure;
subplot(3, 1, 1);
plot(t, q_history);
title('Joint Angles');
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('q1', 'q2');
subplot(3, 1, 2);
plot(t, dq_history);
title('Joint Velocities');
xlabel('Time (s)');
ylabel('Velocity (rad/s)');
legend('dq1', 'dq2');
subplot(3, 1, 3);
plot(x_history(1, :), x_history(2, :), 'b', x_d(1, :), x_d(2, :), 'r--');
title('End Effector Path');
xlabel('X Position (m)');
ylabel('Y Position (m)');
legend('Actual Path', 'Desired Path');
end
function x = forward_kinematics(q, l1, l2)
x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
end
function J = jacobian(q, l1, l2)
J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
l1 * cos(q(1)) + l2 * cos(q(1) + q(2)), l2 * cos(q(1) + q(2))];
end
function M = inertia_matrix(q, m1, m2, l1, l2)
M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
M21 = M12;
M22 = m2 * l2^2;
M = [M11, M12; M21, M22];
end
function C = coriolis_matrix(q, dq, m1, m2, l1, l2)
C11 = -m2 * l1 * l2 * sin(q(2)) * dq(2);
C12 = -m2 * l1 * l2 * sin(q(2)) * (dq(1) + dq(2));
C21 = m2 * l1 * l2 * sin(q(2)) * dq(1);
C22 = 0;
C = [C11, C12; C21, C22];
end
function g = gravity_vector(q, m1, m2, l1, l2)
g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
g = [g1; g2];
end
function impedance_control_simulation()
% 机器人参数
m1 = 1; m2 = 1; % 连杆质量
l1 = 1; l2 = 1; % 连杆长度
% 阻抗控制参数
Kd = [10, 0.2; 0.3, 5]; % 阻抗控制中的期望刚度矩阵
Dd = [5, 0.1; 0.2, 5]; % 阻抗控制中的期望阻尼矩阵
Lambda_d = [0.8, 0.001; 0.002, 0.6]; % 阻抗控制中的期望惯量矩阵
% 期望轨迹：圆形运动
t = linspace(0, 10, 1000); % 时间范围
x_d = [0.5*cos(t); 0.5*sin(t)]; % 期望末端位置
dx_d = [-0.5*sin(t); 0.5*cos(t)]; % 期望末端速度
ddx_d = [-0.5*cos(t); -0.5*sin(t)]; % 期望末端加速度
% 障碍物位置
obstacle_pos = [0.2; 0.2];
obstacle_radius = 0.15; % 障碍物半径
k_spring = 1000; % 弹簧刚度
b_damper = 10; % 阻尼系数
% 初始化机器人状态
q = [pi/4; pi/4]; % 关节角度
dq = [0; 0]; % 关节速度
% 仿真参数
dt = t(2) - t(1); % 时间步长
% 存储数据用于绘图
q_history = zeros(2, length(t));
dq_history = zeros(2, length(t));
x_history = zeros(2, length(t));
F_ext_history = zeros(2, length(t));
% 开始仿真
for i = 1:length(t)
% 计算当前机器人状态
x = forward_kinematics(q, l1, l2); % 计算当前末端位置
J = jacobian(q, l1, l2); % 计算当前雅可比矩阵
M = inertia_matrix(q, m1, m2, l1, l2); % 计算当前动力学矩阵
C = coriolis_matrix(q, dq, m1, m2, l1, l2); % 计算当前科氏力矩
g = gravity_vector(q, m1, m2, l1, l2); % 计算当前重力矢量
% 计算接触力
dist_to_obstacle = norm(x - obstacle_pos);
if dist_to_obstacle < obstacle_radius
penetration_depth = obstacle_radius - dist_to_obstacle;
contact_normal = (x - obstacle_pos) / dist_to_obstacle;
F_ext = k_spring * penetration_depth * contact_normal - b_damper * J * dq;
else
F_ext = [0; 0];
end
% 计算阻抗控制律中的各项
Lambda = inv(J' * inv(M) * J); % 阻抗矩阵
mu = J' * (C - M * inv(J) * (jacobian_dot(q, dq, l1, l2))) * inv(J); % 阻抗补偿项
tilde_x = x - x_d(:, i); % 位置误差
dot_tilde_x = J * dq - dx_d(:, i); % 速度误差
% 计算控制力
tau = g + J' * (Lambda * ddx_d(:, i) + mu * dx_d(:, i)) - ...
J' * Lambda * Lambda_d * (Kd * tilde_x + Dd * dot_tilde_x) + ...
J' * (Lambda / Lambda_d - eye(2)) * F_ext;
% 使用机器人动力学方程求解关节加速度
q_ddot = inv(M) * (tau - C * dq - g + J' * F_ext);
% 更新机器人状态
dq = dq + q_ddot * dt; % 更新关节速度
q = q + dq * dt; % 更新关节角度
% 存储数据用于绘图
q_history(:, i) = q;
dq_history(:, i) = dq;
x_history(:, i) = x;
dx_history(:, i) = J * dq;
F_ext_history(:, i) = F_ext;
% 调试输出
if mod(i, 100) == 0
disp(['Time: ', num2str(t(i)), ' s']);
disp(['End effector position: [', num2str(x(1)), ', ', num2str(x(2)), ']']);
disp(['Distance to obstacle: ', num2str(dist_to_obstacle)]);
disp(['Contact force: [', num2str(F_ext(1)), ', ', num2str(F_ext(2)), ']']);
end
end
% 绘制结果
figure;
subplot(3, 1, 1);
plot(t, x_history);
hold on;
plot(t, x_d, '--');
title('End Effector Position');
xlabel('Time (s)');
ylabel('Position (m)');
legend('x', 'y', 'x_d', 'y_d');
subplot(3, 1, 2);
plot(t, dx_history);
hold on;
plot(t, dx_d, '--');
title('End Effector Velocity');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('dx', 'dy', 'dx_d', 'dy_d');
subplot(3, 1, 3);
plot(x_history(1, :), x_history(2, :), 'b', x_d(1, :), x_d(2, :), 'r--');
hold on;
viscircles(obstacle_pos', obstacle_radius, 'LineStyle', '--');
title('End Effector Path');
xlabel('X Position (m)');
ylabel('Y Position (m)');
legend('Actual Path', 'Desired Path', 'Obstacle');
figure;
plot(t, F_ext_history);
title('Contact Force');
xlabel('Time (s)');
ylabel('Force (N)');
legend('F_x', 'F_y');
end
function x = forward_kinematics(q, l1, l2)
x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
end
function J = jacobian(q, l1, l2)
J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
l1 * cos(q(1)) + l2 * cos(q(1) + q(2)), l2 * cos(q(1) + q(2))];
end
function J_dot = jacobian_dot(q, dq, l1, l2)
J_dot = [-l1 * cos(q(1)) * dq(1) - l2 * cos(q(1) + q(2)) * (dq(1) + dq(2)), -l2 * cos(q(1) + q(2)) * (dq(1) + dq(2));
-l1 * sin(q(1)) * dq(1) - l2 * sin(q(1) + q(2)) * (dq(1) + dq(2)), -l2 * sin(q(1) + q(2)) * (dq(1) + dq(2))];
end
function M = inertia_matrix(q, m1, m2, l1, l2)
M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
M21 = M12;
M22 = m2 * l2^2;
M = [M11, M12; M21, M22];
end
function C = coriolis_matrix(q, dq, m1, m2, l1, l2)
C11 = -m2 * l1 * l2 * sin(q(2)) * dq(2);
C12 = -m2 * l1 * l2 * sin(q(2)) * (dq(1) + dq(2));
C21 = m2 * l1 * l2 * sin(q(2)) * dq(1);
C22 = 0;
C = [C11, C12; C21, C22];
end
function g = gravity_vector(q, m1, m2, l1, l2)
g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
g = [g1; g2];
end
参考文献：
平面2R机器人运动学与动力学建模基于平面2R机器人谈谈运动学和动力学建模的应用
https://mp.weixin.qq.com/s/1gQTDHpyUkTMst_OdVFlnw
MATLAB 中的机械臂算法——动力学系列技术文章https://mp.weixin.qq.com/s/opQcpv02sysIcz8seg3lkQ
https://zhuanlan.zhihu.com/p/586836394https://zhuanlan.zhihu.com/p/586836394
扫一扫
340
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
