概念——二连杆与三连杆解算
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;
end3-逆运动学解算
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明天进行解算的想法思考