概念——二连杆与三连杆解算

https://zhuanlan.zhihu.com/p/161348413

跟着这位大佬学的，侵权删除

计划：先行理解概念以及推演过程，然后自行推导一遍（确认理解），之后思考如何实际调用，目前在第一步

目录

01-二连杆

1-正解算

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

3-逆运动学解算

02-三连杆

1-正解算

2-微分运动学

3-逆运动学解算

---

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;

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

明天进行解算的想法思考