• 机器人动力学模型与MATLAB仿真

    机器人刚体动力学由以下方程控制！！！

    1. startup_rvc
    2. mdl_puma560
    3. p560.dyn

    提前计算出来这些“disturbance”，然后在控制环路中将它“抵消”（有时候也叫前馈控制）

    求出所需要的力矩，其中M项代表克服了机械臂的加速度惯量以及不同连杆之间的惯量影响所需力矩、C项代表了克服科里奥利力和离心力所需力矩、G项代表了克服地球引力力矩。

    Inverse Dynamics叫做反向动力学，它的作用是输入想要的关节速度(qd)、关节加速度(qdd)、关节角度(q)，输出为每个关节所需要的力矩（u）。当然也有正向动力学（forward dynamics）,它的作用和反向动力学相反，输入关节角度（q）、关节速度（qd）、每个关节的力矩（u），输出为每个关节的加速度（qdd）

    “前馈控制”的例子，它的作用是预先计算出所需的力矩，输入给控制器，“反馈”的存在是为了消除一些误差，例如摩擦力

    1. J=[-sin(q1)-sin(q1+q2) -sin(q1+q2);
    2.     cos(q1)+cos(q1+q2) cos(q1+q2)];
    3. d_J=[-dq1*cos(q1)-(dq1+dq2)*cos(q1+q2) -(dq1+dq2)*cos(q1+q2);
    4.     -dq1*sin(q1)-(dq1+dq2)*sin(q1+q2) -(dq1+dq2)*sin(q1+q2)];

    1. function torque_out    = fcn(u_matrix,q1,q2)
    2.  
    3. %% Robot Joint and mass parameters
    4. g=9.81;
    5. m1=0.3;
    6. m2=1;
    7. l1=0.2;
    8. l2=0.2;
    9.  
    10.  
    11.  
    12. %% B = M Matrix 
    13. 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];
    14.  
    15. torque_out= B* u_matrix ;
    16.  
    17. 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)))];

    1. alfa = 6.7;beta = 3.4;
    2. epc = 3.0;eta = 0;
    3. m1 = 1;l1 = 1;
    4. lc1 = 1/2;I1 = 1/12;
    5. g = 9.8;
    6. e1 = m1*l1*lc1-I1-m1*l1^2;
    7. e2 = g/l1;
    8.  
    9.  
    10.  
    11. M = [alfa+2*epc*cos(q2)*eta*sin(q2),beta+epc*cos(q2)+eta*sin(q2);beta+epc*cos(q2)+eta*sin(q2),beta];
    12. 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];
    13. 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)];

    1. J=[-sin(q1)-sin(q1+q2) -sin(q1+q2);
    2.     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)]';

    1. function [xd, yd] = fcn(r, t, w, x, y)
    2.  
    3. alpha = t*w;
    4.  
    5. xd = x + r*cos(alpha);
    6. yd = y + r*sin(alpha);
    7.  
    8.  
    9. end

    基于力矩的阻抗控制

    动力学模型如下

    阻抗控制模型如下

    把这个阻抗模型得到的加速度带入到机器人动力学模型中，消除加速度ddx非线性项，得到控制律

    基于模型的阻抗控制

    笛卡尔空间的机器人动力学模型可描述为：

    阻抗控制器可设计为：

    ————————————————————————————————

    BA的存在导致系统耦合，如果希望在与环境交互过程中保持线性并解耦，需要测量接触力：

    因此，

    正向动力学

    正运动学：

    雅克比矩阵：

    笛卡尔速度与及速度：

    定义末端实际位置与虚拟平衡位置的偏差：

    阻抗控制目标为：实现末端执行器标架相对于柔顺标架的偏差与末端执行器作用于环境的力旋量之间的二阶期望动态关系！

    刚性机器人在末端空间的动力学方程为：、

    其中：

    1. % 假设的二自由度机器人参数  
    2. L1 = 1; % 连杆1的长度  
    3. L2 = 1; % 连杆2的长度  
    4.   
    5. % 假设的关节角度（需要实时更新）  
    6. q = [pi/4; pi/6]; % 初始关节角度（弧度制）  
    7.   
    8. % 计算末端执行器的位置（在笛卡尔空间中）  
    9. % 这是一个简化的例子，实际中可能需要使用更复杂的正向运动学方程  
    10. x = L1*cos(q(1)) + L2*cos(q(1) + q(2));  
    11. y = L1*sin(q(1)) + L2*sin(q(1) + q(2));  
    12. z = 0; % 假设机器人在二维平面上运动  
    13. X = [x; y; z];  
    14.   
    15. % 计算雅可比矩阵（简化的例子）  
    16. % 这里只考虑了平面情况，并且假设了简化的雅可比矩阵  
    17. % 在实际应用中，雅可比矩阵需要根据机器人的实际结构来计算  
    18. J = [-L1*sin(q(1)) - L2*sin(q(1) + q(2)), -L2*sin(q(1) + q(2));  
    19.      L1*cos(q(1)) + L2*cos(q(1) + q(2)),  L2*cos(q(1) + q(2))];  
    20.   
    21. % 阻抗控制参数  
    22. M_d = diag([1, 1, 1]); % 期望的惯性矩阵  
    23. B_d = diag([2, 2, 2]); % 期望的阻尼矩阵  
    24. K_d = diag([100, 100, 100]); % 期望的刚度矩阵  
    25.   
    26. % 期望的末端执行器位置、速度和加速度（需要实时更新）  
    27. Xd = [1; 1; 0]; % 期望位置  
    28. Xdot_d = [0; 0; 0]; % 期望速度  
    29. Xddot_d = [0; 0; 0]; % 期望加速度（通常不需要直接指定）  
    30.   
    31. % 外部力（假设为0或根据传感器数据获取）  
    32. F_ext = [0; 0; 0];  
    33.   
    34. % 阻抗控制律  
    35. % 计算位置误差  
    36. e = Xd - X;  
    37.   
    38. % 计算阻抗力  
    39. F_impedance = -M_d*Xddot_d - B_d*Xdot_d - K_d*e;  
    40.   
    41. % 将阻抗力从笛卡尔空间转换到关节空间  
    42. tau = J' * F_impedance;  
    43.   
    44. % 如果需要添加重力补偿或其他动力学项，可以在这里添加  
    45. % 例如：tau = tau + g(q) + ...;
    1. function tau = impedance_control(q, qd, xd_desired, xdot_desired, xddot_desired)
    2.     % 机械臂参数
    3.     m1 = 1; m2 = 1; % 连杆质量
    4.     l1 = 1; l2 = 1; % 连杆长度
    5.     
    6.     % 控制参数
    7.     K_d = diag([100, 100]); % 阻抗控制中的刚度系数
    8.     D_d = diag([20, 20]);   % 阻抗控制中的阻尼系数
    9.     
    10.     % 运动学和动力学矩阵计算
    11.     x = forward_kinematics(q, l1, l2);
    12.     J = jacobian(q, l1, l2);
    13.     M = inertia_matrix(q, m1, m2, l1, l2);
    14.     C = coriolis_matrix(q, qd, m1, m2, l1, l2);
    15.     g = gravity_vector(q, m1, m2, l1, l2);
    16.  
    17.     % Lambda和mu的计算
    18.     Lambda = inv(J' * inv(M) * J);
    19.     mu = inv(J') * (C - M * inv(J) * jacobian_dot(q, qd, l1, l2)) * inv(J);
    20.  
    21.     % 误差计算
    22.     x_tilde = x - xd_desired;
    23.     xdot_tilde = J * qd - xdot_desired;
    24.  
    25.     % 控制律计算
    26.     tau = g + J' * (Lambda * xddot_desired + mu * (J * qd)) - J' * Lambda * (K_d * x_tilde + D_d * xdot_tilde);
    27. end
    28.  
    29. function x = forward_kinematics(q, l1, l2)
    30.     x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
    31.          l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
    32. end
    33.  
    34. function J = jacobian(q, l1, l2)
    35.     J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
    36.           l1 * cos(q(1)) + l2 * cos(q(1) + q(2)),  l2 * cos(q(1) + q(2))];
    37. end
    38.  
    39. function J_dot = jacobian_dot(q, qd, l1, l2)
    40.     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));
    41.              -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))];
    42. end
    43.  
    44. function M = inertia_matrix(q, m1, m2, l1, l2)
    45.     M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
    46.     M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
    47.     M21 = M12;
    48.     M22 = m2 * l2^2;
    49.     M = [M11, M12; M21, M22];
    50. end
    51.  
    52. function C = coriolis_matrix(q, qd, m1, m2, l1, l2)
    53.     C11 = -m2 * l1 * l2 * sin(q(2)) * qd(2);
    54.     C12 = -m2 * l1 * l2 * sin(q(2)) * (qd(1) + qd(2));
    55.     C21 = m2 * l1 * l2 * sin(q(2)) * qd(1);
    56.     C22 = 0;
    57.     C = [C11, C12; C21, C22];
    58. end
    59.  
    60. function g = gravity_vector(q, m1, m2, l1, l2)
    61.     g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
    62.     g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
    63.     g = [g1; g2];
    64. end

    1. function [q_dot, q_ddot] = robot_dynamics(q, qd, tau)
    2.     % 机械臂参数
    3.     m1 = 1; m2 = 1; % 连杆质量
    4.     l1 = 1; l2 = 1; % 连杆长度
    5.  
    6.     % 计算动力学矩阵
    7.     M = inertia_matrix(q, m1, m2, l1, l2);
    8.     C = coriolis_matrix(q, qd, m1, m2, l1, l2);
    9.     g = gravity_vector(q, m1, m2, l1, l2);
    10.  
    11.     % 动力学方程求解
    12.     q_ddot = inv(M) * (tau - C * qd - g);
    13.  
    14.     % 更新关节速度
    15.     q_dot = qd;
    16. end
    17.  
    18. function M = inertia_matrix(q, m1, m2, l1, l2)
    19.     M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
    20.     M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
    21.     M21 = M12;
    22.     M22 = m2 * l2^2;
    23.     M = [M11, M12; M21, M22];
    24. end
    25.  
    26. function C = coriolis_matrix(q, qd, m1, m2, l1, l2)
    27.     C11 = -m2 * l1 * l2 * sin(q(2)) * qd(2);
    28.     C12 = -m2 * l1 * l2 * sin(q(2)) * (qd(1) + qd(2));
    29.     C21 = m2 * l1 * l2 * sin(q(2)) * qd(1);
    30.     C22 = 0;
    31.     C = [C11, C12; C21, C22];
    32. end
    33.  
    34. function g = gravity_vector(q, m1, m2, l1, l2)
    35.     g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
    36.     g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
    37.     g = [g1; g2];
    38. end
    39.  

    1. % 二自由度机器人末端空间阻抗控制仿真程序
    2.  
    3. % 机械臂参数
    4. m1 = 1; m2 = 1; % 连杆质量
    5. l1 = 1; l2 = 1; % 连杆长度
    6.  
    7. % 初始状态
    8. q = [pi/4; pi/4]; % 关节角度
    9. qd = [0; 0];      % 关节角速度
    10. x = forward_kinematics(q); % 末端位置
    11. xd = [0; 0];      % 末端速度
    12.  
    13. % 控制参数
    14. K_d = diag([100, 100]); % 阻抗控制中的刚度系数
    15. D_d = diag([20, 20]);   % 阻抗控制中的阻尼系数
    16.  
    17. % 仿真参数
    18. dt = 0.01; % 仿真时间步长
    19. t_final = 10; % 仿真总时间
    20. time = 0:dt:t_final;
    21.  
    22. % 期望轨迹
    23. xd_desired = [0.5*sin(0.1*time); 0.5*cos(0.1*time)];
    24. xdot_desired = [0.05*cos(0.1*time); -0.05*sin(0.1*time)];
    25. xddot_desired = [-0.005*sin(0.1*time); -0.005*cos(0.1*time)];
    26.  
    27. % 数据存储
    28. q_history = zeros(2, length(time));
    29. x_history = zeros(2, length(time));
    30.  
    31. for i = 1:length(time)
    32.     % 前向运动学
    33.     x = forward_kinematics(q);
    34.     J = jacobian(q);
    35.     
    36.     % 计算Lambda, mu
    37.     M = inertia_matrix(q, m1, m2, l1, l2);
    38.     C = coriolis_matrix(q, qd, m1, m2, l1, l2);
    39.     Lambda = inv(J' * inv(M) * J);
    40.     mu = inv(J') * (C - M * inv(J) * jacobian_dot(q, qd)) * inv(J);
    41.     
    42.     % 误差计算
    43.     x_tilde = x - xd_desired(:, i);
    44.     xdot_tilde = J * qd - xdot_desired(:, i);
    45.     
    46.     % 控制律计算
    47.     g = gravity_vector(q, m1, m2, l1, l2);
    48.     tau = g + J' * (Lambda * xddot_desired(:, i) + mu * (J * qd)) ...
    49.           - J' * Lambda * (K_d * x_tilde + D_d * xdot_tilde);
    50.     
    51.     % 动力学方程求解
    52.     qdd = inv(M) * (tau - C * qd - g);
    53.     
    54.     % 状态更新
    55.     qd = qd + qdd * dt;
    56.     q = q + qd * dt;
    57.     
    58.     % 数据存储
    59.     q_history(:, i) = q;
    60.     x_history(:, i) = x;
    61. end
    62.  
    63. % 绘图
    64. figure;
    65. subplot(2, 1, 1);
    66. plot(time, q_history);
    67. title('关节角度随时间变化');
    68. xlabel('时间 (s)');
    69. ylabel('关节角度 (rad)');
    70. legend('q1', 'q2');
    71.  
    72. subplot(2, 1, 2);
    73. plot(time, x_history);
    74. title('末端位置随时间变化');
    75. xlabel('时间 (s)');
    76. ylabel('末端位置 (m)');
    77. legend('x', 'y');
    78.  
    79. % 函数定义
    80. function x = forward_kinematics(q)
    81.     l1 = 1; l2 = 1;
    82.     x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
    83.          l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
    84. end
    85.  
    86. function J = jacobian(q)
    87.     l1 = 1; l2 = 1;
    88.     J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
    89.           l1 * cos(q(1)) + l2 * cos(q(1) + q(2)),  l2 * cos(q(1) + q(2))];
    90. end
    91.  
    92. function J_dot = jacobian_dot(q, qd)
    93.     l1 = 1; l2 = 1;
    94.     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));
    95.              -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))];
    96. end
    97.  
    98. function M = inertia_matrix(q, m1, m2, l1, l2)
    99.     M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
    100.     M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
    101.     M21 = M12;
    102.     M22 = m2 * l2^2;
    103.     M = [M11, M12; M21, M22];
    104. end
    105.  
    106. function C = coriolis_matrix(q, qd, m1, m2, l1, l2)
    107.     C11 = -m2 * l1 * l2 * sin(q(2)) * qd(2);
    108.     C12 = -m2 * l1 * l2 * sin(q(2)) * (qd(1) + qd(2));
    109.     C21 = m2 * l1 * l2 * sin(q(2)) * qd(1);
    110.     C22 = 0;
    111.     C = [C11, C12; C21, C22];
    112. end
    113.  
    114. function g = gravity_vector(q, m1, m2, l1, l2)
    115.     g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
    116.     g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
    117.     g = [g1; g2];
    118. end
    1. function impedance_control_simulation()
    2.     % 机器人参数
    3.     m1 = 1; m2 = 1; % 连杆质量
    4.     l1 = 1; l2 = 1; % 连杆长度
    5.     
    6.     % 阻抗控制参数
    7.     Kd = diag([10, 10]); % 刚度矩阵
    8.     Dd = diag([5, 5]); % 阻尼矩阵
    9.     
    10.     % 期望轨迹：圆形运动
    11.     t = linspace(0, 10, 1000); % 时间范围
    12.     x_d = [0.5*cos(t); 0.5*sin(t)]; % 期望末端位置
    13.     dx_d = [-0.5*sin(t); 0.5*cos(t)]; % 期望末端速度
    14.     ddx_d = [-0.5*cos(t); -0.5*sin(t)]; % 期望末端加速度
    15.     
    16.     % 初始化机器人状态
    17.     q = [pi/4; pi/4]; % 关节角度
    18.     dq = [0; 0]; % 关节速度
    19.     
    20.     % 仿真参数
    21.     dt = t(2) - t(1); % 时间步长
    22.     
    23.     % 存储数据用于绘图
    24.     q_history = zeros(2, length(t));
    25.     dq_history = zeros(2, length(t));
    26.     x_history = zeros(2, length(t));
    27.     
    28.     % 开始仿真
    29.     for i = 1:length(t)
    30.         % 计算当前机器人状态
    31.         x = forward_kinematics(q, l1, l2); % 计算当前末端位置
    32.         J = jacobian(q, l1, l2); % 计算当前雅可比矩阵
    33.         M = inertia_matrix(q, m1, m2, l1, l2); % 计算当前动力学矩阵
    34.         C = coriolis_matrix(q, dq, m1, m2, l1, l2); % 计算当前科氏力矩
    35.         g = gravity_vector(q, m1, m2, l1, l2); % 计算当前重力矢量
    36.         
    37.         % 计算控制律中的各项
    38.         Lambda = inv(J' * inv(M) * J); % 阻抗矩阵
    39.         mu = J' * (C - M * inv(J) * (J' * inv(M) * J)) * inv(J); % 阻抗补偿项
    40.         Lambda_d = eye(2); % 期望阻抗矩阵（假设为单位矩阵）
    41.         F_ext = [0; 0]; % 外部力（暂时设为零向量）
    42.         tilde_x = x - x_d(:, i); % 位置误差
    43.         dot_tilde_x = J * dq - dx_d(:, i); % 速度误差
    44.         
    45.         % 计算控制力
    46.         tau = g + J' * (Lambda * ddx_d(:, i) + mu * dx_d(:, i)) - ...
    47.               J' * Lambda * Lambda_d * (Kd * tilde_x + Dd * dot_tilde_x) + ...
    48.               J' * (Lambda * Lambda_d^(-1) - eye(2)) * F_ext;
    49.         
    50.         % 使用机器人动力学方程求解关节加速度
    51.         q_ddot = inv(M) * (tau - C * dq - g);
    52.         
    53.         % 更新机器人状态
    54.         dq = dq + q_ddot * dt; % 更新关节速度
    55.         q = q + dq * dt; % 更新关节角度
    56.         
    57.         % 存储数据用于绘图
    58.         q_history(:, i) = q;
    59.         dq_history(:, i) = dq;
    60.         x_history(:, i) = x;
    61.     end
    62.     
    63.     % 绘制结果
    64.     figure;
    65.     subplot(3, 1, 1);
    66.     plot(t, q_history);
    67.     title('Joint Angles');
    68.     xlabel('Time (s)');
    69.     ylabel('Angle (rad)');
    70.     legend('q1', 'q2');
    71.     
    72.     subplot(3, 1, 2);
    73.     plot(t, dq_history);
    74.     title('Joint Velocities');
    75.     xlabel('Time (s)');
    76.     ylabel('Velocity (rad/s)');
    77.     legend('dq1', 'dq2');
    78.     
    79.     subplot(3, 1, 3);
    80.     plot(x_history(1, :), x_history(2, :), 'b', x_d(1, :), x_d(2, :), 'r--');
    81.     title('End Effector Path');
    82.     xlabel('X Position (m)');
    83.     ylabel('Y Position (m)');
    84.     legend('Actual Path', 'Desired Path');
    85. end
    86.  
    87. function x = forward_kinematics(q, l1, l2)
    88.     x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
    89.          l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
    90. end
    91.  
    92. function J = jacobian(q, l1, l2)
    93.     J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
    94.           l1 * cos(q(1)) + l2 * cos(q(1) + q(2)),  l2 * cos(q(1) + q(2))];
    95. end
    96.  
    97. function M = inertia_matrix(q, m1, m2, l1, l2)
    98.     M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
    99.     M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
    100.     M21 = M12;
    101.     M22 = m2 * l2^2;
    102.     M = [M11, M12; M21, M22];
    103. end
    104.  
    105. function C = coriolis_matrix(q, dq, m1, m2, l1, l2)
    106.     C11 = -m2 * l1 * l2 * sin(q(2)) * dq(2);
    107.     C12 = -m2 * l1 * l2 * sin(q(2)) * (dq(1) + dq(2));
    108.     C21 = m2 * l1 * l2 * sin(q(2)) * dq(1);
    109.     C22 = 0;
    110.     C = [C11, C12; C21, C22];
    111. end
    112.  
    113. function g = gravity_vector(q, m1, m2, l1, l2)
    114.     g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
    115.     g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
    116.     g = [g1; g2];
    117. end
    1. function impedance_control_simulation()
    2.     % 机器人参数
    3.     m1 = 1; m2 = 1; % 连杆质量
    4.     l1 = 1; l2 = 1; % 连杆长度
    5.     
    6.     % 阻抗控制参数
    7.     Kd = [10, 0.2; 0.3, 5]; % 阻抗控制中的期望刚度矩阵
    8.     Dd = [5, 0.1; 0.2, 5]; % 阻抗控制中的期望阻尼矩阵
    9.     Lambda_d = [0.8, 0.001; 0.002, 0.6]; % 阻抗控制中的期望惯量矩阵
    10.     
    11.     % 期望轨迹：圆形运动
    12.     t = linspace(0, 10, 1000); % 时间范围
    13.     x_d = [0.5*cos(t); 0.5*sin(t)]; % 期望末端位置
    14.     dx_d = [-0.5*sin(t); 0.5*cos(t)]; % 期望末端速度
    15.     ddx_d = [-0.5*cos(t); -0.5*sin(t)]; % 期望末端加速度
    16.     
    17.     % 障碍物位置
    18.     obstacle_pos = [0.2; 0.2];
    19.     obstacle_radius = 0.15; % 障碍物半径
    20.     k_spring = 1000; % 弹簧刚度
    21.     b_damper = 10; % 阻尼系数
    22.     
    23.     % 初始化机器人状态
    24.     q = [pi/4; pi/4]; % 关节角度
    25.     dq = [0; 0]; % 关节速度
    26.     
    27.     % 仿真参数
    28.     dt = t(2) - t(1); % 时间步长
    29.     
    30.     % 存储数据用于绘图
    31.     q_history = zeros(2, length(t));
    32.     dq_history = zeros(2, length(t));
    33.     x_history = zeros(2, length(t));
    34.     F_ext_history = zeros(2, length(t));
    35.     
    36.     % 开始仿真
    37.     for i = 1:length(t)
    38.         % 计算当前机器人状态
    39.         x = forward_kinematics(q, l1, l2); % 计算当前末端位置
    40.         J = jacobian(q, l1, l2); % 计算当前雅可比矩阵
    41.         M = inertia_matrix(q, m1, m2, l1, l2); % 计算当前动力学矩阵
    42.         C = coriolis_matrix(q, dq, m1, m2, l1, l2); % 计算当前科氏力矩
    43.         g = gravity_vector(q, m1, m2, l1, l2); % 计算当前重力矢量
    44.         
    45.         % 计算接触力
    46.         dist_to_obstacle = norm(x - obstacle_pos);
    47.         if dist_to_obstacle < obstacle_radius
    48.             penetration_depth = obstacle_radius - dist_to_obstacle;
    49.             contact_normal = (x - obstacle_pos) / dist_to_obstacle;
    50.             F_ext = k_spring * penetration_depth * contact_normal - b_damper * J * dq;
    51.         else
    52.             F_ext = [0; 0];
    53.         end
    54.         
    55.         % 计算阻抗控制律中的各项
    56.         Lambda = inv(J' * inv(M) * J); % 阻抗矩阵
    57.         mu = J' * (C - M * inv(J) * (jacobian_dot(q, dq, l1, l2))) * inv(J); % 阻抗补偿项
    58.         tilde_x = x - x_d(:, i); % 位置误差
    59.         dot_tilde_x = J * dq - dx_d(:, i); % 速度误差
    60.         
    61.         % 计算控制力
    62.         tau = g + J' * (Lambda * ddx_d(:, i) + mu * dx_d(:, i)) - ...
    63.               J' * Lambda * Lambda_d * (Kd * tilde_x + Dd * dot_tilde_x) + ...
    64.               J' * (Lambda / Lambda_d - eye(2)) * F_ext;
    65.         
    66.         % 使用机器人动力学方程求解关节加速度
    67.         q_ddot = inv(M) * (tau - C * dq - g + J' * F_ext);
    68.         
    69.         % 更新机器人状态
    70.         dq = dq + q_ddot * dt; % 更新关节速度
    71.         q = q + dq * dt; % 更新关节角度
    72.         
    73.         % 存储数据用于绘图
    74.         q_history(:, i) = q;
    75.         dq_history(:, i) = dq;
    76.         x_history(:, i) = x;
    77.         dx_history(:, i) = J * dq;
    78.         F_ext_history(:, i) = F_ext;
    79.         
    80.         % 调试输出
    81.         if mod(i, 100) == 0
    82.             disp(['Time: ', num2str(t(i)), ' s']);
    83.             disp(['End effector position: [', num2str(x(1)), ', ', num2str(x(2)), ']']);
    84.             disp(['Distance to obstacle: ', num2str(dist_to_obstacle)]);
    85.             disp(['Contact force: [', num2str(F_ext(1)), ', ', num2str(F_ext(2)), ']']);
    86.         end
    87.     end
    88.     
    89.     % 绘制结果
    90.     figure;
    91.     subplot(3, 1, 1);
    92.     plot(t, x_history);
    93.     hold on;
    94.     plot(t, x_d, '--');
    95.     title('End Effector Position');
    96.     xlabel('Time (s)');
    97.     ylabel('Position (m)');
    98.     legend('x', 'y', 'x_d', 'y_d');
    99.     
    100.     subplot(3, 1, 2);
    101.     plot(t, dx_history);
    102.     hold on;
    103.     plot(t, dx_d, '--');
    104.     title('End Effector Velocity');
    105.     xlabel('Time (s)');
    106.     ylabel('Velocity (m/s)');
    107.     legend('dx', 'dy', 'dx_d', 'dy_d');
    108.     
    109.     subplot(3, 1, 3);
    110.     plot(x_history(1, :), x_history(2, :), 'b', x_d(1, :), x_d(2, :), 'r--');
    111.     hold on;
    112.     viscircles(obstacle_pos', obstacle_radius, 'LineStyle', '--');
    113.     title('End Effector Path');
    114.     xlabel('X Position (m)');
    115.     ylabel('Y Position (m)');
    116.     legend('Actual Path', 'Desired Path', 'Obstacle');
    117.     
    118.     figure;
    119.     plot(t, F_ext_history);
    120.     title('Contact Force');
    121.     xlabel('Time (s)');
    122.     ylabel('Force (N)');
    123.     legend('F_x', 'F_y');
    124. end
    126. function x = forward_kinematics(q, l1, l2)
    127.     x = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2));
    128.          l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
    129. end
    131. function J = jacobian(q, l1, l2)
    132.     J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
    133.           l1 * cos(q(1)) + l2 * cos(q(1) + q(2)),  l2 * cos(q(1) + q(2))];
    134. end
    136. function J_dot = jacobian_dot(q, dq, l1, l2)
    137.     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));
    138.              -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))];
    139. end
    141. function M = inertia_matrix(q, m1, m2, l1, l2)
    142.     M11 = m1 * l1^2 + m2 * (l1^2 + 2 * l1 * l2 * cos(q(2)) + l2^2);
    143.     M12 = m2 * (l1 * l2 * cos(q(2)) + l2^2);
    144.     M21 = M12;
    145.     M22 = m2 * l2^2;
    146.     M = [M11, M12; M21, M22];
    147. end
    149. function C = coriolis_matrix(q, dq, m1, m2, l1, l2)
    150.     C11 = -m2 * l1 * l2 * sin(q(2)) * dq(2);
    151.     C12 = -m2 * l1 * l2 * sin(q(2)) * (dq(1) + dq(2));
    152.     C21 = m2 * l1 * l2 * sin(q(2)) * dq(1);
    153.     C22 = 0;
    154.     C = [C11, C12; C21, C22];
    155. end
    157. function g = gravity_vector(q, m1, m2, l1, l2)
    158.     g1 = (m1 * l1 + m2 * l1) * 9.81 * cos(q(1)) + m2 * l2 * 9.81 * cos(q(1) + q(2));
    159.     g2 = m2 * l2 * 9.81 * cos(q(1) + q(2));
    160.     g = [g1; g2];
    161. end

    参考文献：

    平面2R机器人运动学与动力学建模基于平面2R机器人谈谈运动学和动力学建模的应用icon-default.png?t=N7T8https://mp.weixin.qq.com/s/1gQTDHpyUkTMst_OdVFlnw

    MATLAB 中的机械臂算法——动力学系列技术文章icon-default.png?t=N7T8https://mp.weixin.qq.com/s/opQcpv02sysIcz8seg3lkQ

    刚性机器人笛卡尔阻抗控制算法-腾讯云开发者社区-腾讯云机器人阻抗控制是机器人力控制中的一种重要方式。了解机器人的阻抗控制需要首先了解刚性机器人的阻抗控制方法，并且首先需要了解刚性机器人的动力学方程和运动学方程。在本栏中给出的机器人为非冗余机器人，即机器人的关节空间自由度与笛卡尔空间运动自由度是相等的。icon-default.png?t=N7T8https://cloud.tencent.com/developer/article/1753836

    https://zhuanlan.zhihu.com/p/586836394icon-default.png?t=N7T8https://zhuanlan.zhihu.com/p/586836394

    【云+社区年度征文】机器人力控制算法与实际应用详解-腾讯云开发者社区-腾讯云机器人力控制应用在机器人控制任务中的方方面面，其也是衡量一个机器人本体控制水平的衡量标准。机器人力控制兼顾机器人本体特性与人机交互等任务，使得机器人在发挥自身运动灵巧性的同时与人进行友好的交互。可以说机器人的力控制是机器人实际走向人类生活的关键技术。 本文详细阐述机器人的力控制的概念和应用，并且简要给出算法实现步骤。icon-default.png?t=N7T8https://cloud.tencent.com/developer/article/1754382

  • 相关阅读:
    Redis桌面管理工具RedisDesktopManager
    YGG 西班牙 subDAO——Ola GG 正式成立
    排序算法-插入排序详解
    帆软报表决策系统 上传excel文件
    解析Flutter应用在iOS环境中的性能优化技巧
    c语言 编程及答案
    Linux系统IO和标准IO的接口函数
    搜题公众号搭建
    详解HTML5新增的多媒体标签
    数据准备之日志采集发展历程
  • 原文地址：https://blog.csdn.net/weixin_51367832/article/details/139333350