二连杆与三连杆系统运动学解算详解

二连杆与三连杆系统运动学解算详解

引用

CSDN

1.

https://blog.csdn.net/2301_79886074/article/details/142180636

本文介绍了二连杆和三连杆系统的正解算、微分运动学（雅可比矩阵）和逆运动学解算。通过MATLAB函数代码详细解释了这些概念，并配以必要的数学公式和图像说明。内容涉及机器人学中的基本理论和实践应用，具有较高的专业性和实用性。

01-二连杆

1-正解算

通过两个连杆的长度，角度，解算出最终终点的位置

function [x,y] = kin2(q1,q2,A)
a = A(1);
b = A(2);
x = a*cos(q1)+b*cos(q1+q2);
y = a*sin(q1)+b*sin(q1+q2);
end  

这个函数 kin2(q1, q2, A) 实现了二连杆系统的正运动学。它的输入是关节角度 q1 和 q2，以及连杆长度数组 A（其中 A(1) 是第一连杆的长度，A(2) 是第二连杆的长度）。通过这些参数，函数计算并返回末端点的笛卡尔坐标 x 和 y。

具体来说，函数利用三角函数 cos 和 sin 来计算末端点的水平（x）和垂直（y）位置。

通过简单的三角函数，解算出最终x，y的坐标值

2-微分运动学（雅可比）

function [J2] = dKin2(q1,q2,A)
a = A(1);
b = A(2);
J2 = zeros(2)
J2(1,1) = -a*sin(q1)-b*sin(q1+q2);
J2(1,2) = -b*sin(q1+q2);
J2(2,1) = a*cos(q1)+b*cos(q1+q2);
J2(2,2) = b*cos(q1+q2);
end  

雅可比矩阵（Jacobian Matrix）是用于描述一个多变量函数的微分矩阵，在机器人学中，它用于表示机器人末端速度与关节速度之间的关系。通过雅可比矩阵，可以计算关节空间的微小变化如何影响末端执行器的位置和方向。它在运动学、动力学和控制算法中非常重要，特别是在求解机械臂的速度、加速度以及力矩控制等问题时，雅可比矩阵提供了一种有效的数学工具。

在第一步的基础上，添加了对x，y对q1,q2做了全微分，表示角度的微小变动对坐标的影响

3-逆运动学解算

function [q1,q2] = iKin2(x,y,A)
a = A(1);
b = A(2);
q0 = atan2(y./x);
a0 = (x.^2+y.^2).^0.5;
temp = (x.^2+y.^2+a.^2-b.^2)./(2*a0.*a);
q1   = -acos(temp)+q0;
temp = (x.^2+y.^2-a.^2+b.^2)./(2*a0.*b);
q2   = acos(temp)+q0-q1;
end  

通过公式计算出q1与q2

02-三连杆

1-正解算

function [x,y,theta] = kin3(q1,q2,q3,A)
a = A(1);
b = A(2);
c = A(3);
x = a*cos(q1)+b*cos(q1+q2)+c*cos(q1+q2+q3);
y = a*sin(q1)+b*sin(q1+q2)+c*sin(q1+q2+q3);
theta = q1+q2+q3;
![](https://wy-static.wenxiaobai.com/chat-rag-image/9571863720505824974)
end  

也是通过基本的三角函数，解得最终的x,y,和朝向

2-微分运动学

function [J3] = dKin3(q1,q2,q3,A)
a = A(1);
b = A(2);
c = A(3);
J3 = zeros(3)
J3(1,1) = -a*sin(q1)-b*sin(q1+q2)-c*sin(q1+q2+q3);
J3(1,2) = -b*sin(q1+q2)-c*sin(q1+q2+q3);
J3(1,3) = -c*sin(q1+q2+q3);
J3(2,1) = a*cos(q1)+b*cos(q1+q2)+c*cos(q1+q2+q3);
J3(2,2) = b*cos(q1+q2)+c*cos(q1+q2+q3);
J3(2,3) = c*cos(q1+q2+q3);
J3(3,3) = 1;
J3(3,3) = 1;
J3(3,3) = 1;
end  

3-逆运动学解算

function [q1,q2,q3] = iKin3(x,y,theta,A)
a = A(1);
b = A(2);
c = A(3);
xx = x-c*cos(theta);
yy = y-c*sin(theta);
AA = [a,b];
[q1,q2] = iKin2(xx,yy,AA);
q3 = theta - q1 -q2;
end