一、scara机器人运动学正解

末端BBB的xxx坐标为向量OA\bf{OA}OA与向量AB\bf{AB}AB在xxx轴上投影之和，末端BBB的yyy坐标亦然：
{x=L1cosθ1+L2cos(θ1+θ2)y=L1sinθ1+L2sin(θ1+θ2)(1)\left \{ \begin{array}{c} x=L_1cos\theta_1+L_2cos(\theta_1+\theta_2) \\ \tag 1 y=L_1sin\theta_1+L_2sin(\theta_1+\theta_2) \end{array}\right. {x=L1cosθ1+L2cos(θ1+θ2)y=L1sinθ1+L2sin(θ1+θ2)(1)
第三轴为丝杆上下平移运动，设丝杆螺距sss，末端BBB的zzz坐标：
z=θ3s2π(2)z=\frac{\theta_3s}{2\pi} \tag 2 z=2πθ3s(2)
末端BBB的姿态角ccc：
c=θ1+θ2+θ4(3)c=\theta_1+ \theta_2+ \theta_4 \tag 3 c=θ1+θ2+θ4(3)
综上，scara机器人的运动学正解为：
{x=L1cosθ1+L2cos(θ1+θ2)y=L1sinθ1+L2sin(θ1+θ2)z=θ3s2πc=θ1+θ2+θ4(4)\begin{cases} x=L_1cos\theta_1+L_2cos(\theta_1+\theta_2) \\ y=L_1sin\theta_1+L_2sin(\theta_1+\theta_2) \\ z=\frac{\theta_3s}{2\pi} \\ c=\theta_1+ \theta_2+ \theta_4 \tag 4 \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧x=L1cosθ1+L2cos(θ1+θ2)y=L1sinθ1+L2sin(θ1+θ2)z=2πθ3sc=θ1+θ2+θ4(4)

二、scara机器人运动学逆解

1、正装scara机器人运动学逆解

连接OBOBOB，过BBB作BCBCBC垂直于OAOAOA于CCC，在ΔOAB\Delta OABΔOAB中，由余弦定理：
cos(π−θ2)=L12+L22−(x2+y2)2L1L2(5)cos(\pi-\theta_2)=\frac{L_1^2+L_2^2-(x^2+y^2)}{2L_1L_2}\tag 5 cos(π−θ2)=2L1L2L12+L22−(x2+y2)(5)
记c2=cosθ2c_2=cos\theta_2c2=cosθ2，式(5)可写成：
c2=x2+y2−L12−L222L1L2(6)c_2=\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}\tag 6 c2=2L1L2x2+y2−L12−L22(6)
记s2=sinθ2s_2=sin\theta_2s2=sinθ2，则s2s_2s2有两个解：
s2=±1−c22(7)s_2=\pm\sqrt{1-c_2^2}\tag 7 s2=±1−c22(7)
取正数解，机器人处于右手系(right handcoor)；取负数解，机器人处于左手系(left handcoor)；特殊地，若s2=0s_2=0s2=0，机器人处于奇异位置(singular position)，此时θ2=kπ(k∈Z)\theta_2=k\pi(k\in{Z})θ2=kπ(k∈Z)，一般 θ2∈{−2π,−π,0,π,2π}\theta_2\in{\{-2\pi,-\pi,0,\pi,2\pi}\}θ2∈{−2π,−π,0,π,2π}。
至此，可求得θ2\theta_2θ2：
θ2=atan2(s2,c2)(8)\theta_2=atan2(s_2,c_2)\tag 8 θ2=atan2(s2,c2)(8)
如上图，易得：
α=atan2(y,x)(9)\alpha=atan2(y,x)\tag 9 α=atan2(y,x)(9)
在ΔOBC\Delta OBCΔOBC中，记r=∥OB∥r=\|OB\|r=∥OB∥：
sinβ=∥BC∥∥OB∥=L2s2r(10)sin\beta=\frac{\|BC\|}{\|OB\|}=\frac{L_2s_2}{r}\tag {10} sinβ=∥OB∥∥BC∥=rL2s2(10)
cosβ=∥OC∥∥OB∥=∥OA∥+∥AC∥∥OB∥=L1+L2c2r(11)cos\beta=\frac{\|OC\|}{\|OB\|}=\frac{\|OA\|+\|AC\|}{\|OB\|}=\frac{L_1+L_2c_2}{r}\tag {11} cosβ=∥OB∥∥OC∥=∥OB∥∥OA∥+∥AC∥=rL1+L2c2(11)
由于r>0r>0r>0，由式(10)(10)(10)和式(11)(11)(11)得：
β=atan2(L2s2,L1+L2c2)(12)\beta=atan2(L_2s_2,L_1+L_2c_2)\tag {12} β=atan2(L2s2,L1+L2c2)(12)
至此，可求得θ1\theta_1θ1：
θ1=α−β=atan2(y,x)−atan2(L2s2,L1+L2c2)(13)\theta_1=\alpha-\beta=atan2(y,x)-atan2(L_2s_2,L_1+L_2c_2)\tag {13} θ1=α−β=atan2(y,x)−atan2(L2s2,L1+L2c2)(13)
由式(4)(4)(4)容易求得θ3\theta_3θ3和θ4\theta_4θ4。
综上，正装scara机器人的运动学逆解为：
{θ1=atan2(y,x)−atan2(L2s2,L1+L2c2)θ2=atan2(s2,c2)θ3=2πzsθ4=c−θ1−θ2(14)\begin{cases} \theta_1=atan2(y,x)-atan2(L_2s_2,L_1+L_2c_2) \\ \theta_2=atan2(s_2,c_2) \\ \theta_3=\frac{2\pi z}{s} \\ \theta_4=c-\theta_1-\theta_2 \tag {14} \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧θ1=atan2(y,x)−atan2(L2s2,L1+L2c2)θ2=atan2(s2,c2)θ3=s2πzθ4=c−θ1−θ2(14)
在求解θ2\theta_2θ2时，完全可以根据式(6)(6)(6)用反余弦函数acosacosacos求得，但这里采用了双变量反正切函数atan2atan2atan2，至于原因，可以参见这篇博文：为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？
由于atan2atan2atan2的值域为[−π,π][-\pi,\pi][−π,π]，可得关节角度θ1\theta_1θ1在−2π-2\pi−2π和2π2\pi2π之间(区间左端点−2π-2\pi−2π和区间右端点2π2\pi2π不一定能达到)，θ2∈[−π,π]\theta_2\in[-\pi,\pi]θ2∈[−π,π]，θ3\theta_3θ3范围与丝杆螺距sss和zzz轴的行程有关，θ4\theta_4θ4一般范围在[−2π,2π][-2\pi,2\pi][−2π,2π]。对于正装scara机器人，为避免机构干涉，一般θ1∈[−3π4,3π4]\theta_1\in[-\frac{3\pi}{4},\frac{3\pi}{4}]θ1∈[−43π,43π]，θ2∈[−2π3,2π3]\theta_2\in[-\frac{2\pi}{3},\frac{2\pi}{3}]θ2∈[−32π,32π]。式(14)(14)(14)的运动学逆解无法做到使机器人在工作空间360°无死角运动，需要将其推广，使得θ1∈[−2π,2π]\theta_1\in[-2\pi,2\pi]θ1∈[−2π,2π]且θ2∈[−2π,2π]\theta_2\in[-2\pi,2\pi]θ2∈[−2π,2π]，即吊装scara机器人关节1和关节2的角度范围。

2、吊装scara机器人运动学逆解

通过式(14)(14)(14)计算得到θ1\theta_1θ1与θ2\theta_2θ2后，增加关节1与关节2角度标志位flagJ1与flagJ2，利用以下的规则，可以将θ1\theta_1θ1与θ2\theta_2θ2范围限定在[−2π,2π][-2\pi,2\pi][−2π,2π]，实现吊装scara机器人360°无死角运动。
将θ1\theta_1θ1范围变换到[−π,π][-\pi,\pi][−π,π]:
若θ1<−π\theta_1<-\piθ1<−π，则θ1=θ1+2π\theta_1=\theta_1+2\piθ1=θ1+2π；
若θ1>π\theta_1>\piθ1>π，则θ1=θ1−2π\theta_1=\theta_1-2\piθ1=θ1−2π

对于θ1\theta_1θ1，当flagJ1=1flagJ1=1flagJ1=1时：
若θ1≥0\theta_1\geq0θ1≥0，则θ1=θ1−2π\theta_1=\theta_1-2\piθ1=θ1−2π；
若θ1<0\theta_1<0θ1<0，则θ1=θ1+2π\theta_1=\theta_1+2\piθ1=θ1+2π

对于θ2\theta_2θ2，当flagJ2=1flagJ2=1flagJ2=1时：
若θ2≥0\theta_2\geq0θ2≥0，则θ2=θ2−2π\theta_2=\theta_2-2\piθ2=θ2−2π；
若θ2<0\theta_2<0θ2<0，θ2=θ2+2π\theta_2=\theta_2+2\piθ2=θ2+2π

θ4=c−θ1−θ2\theta_4=c-\theta_1-\theta_2θ4=c−θ1−θ2

3、几个值得思考的问题

(1)、手系handcoor的确定

当θ2∈(0,π)⋃(−2π,−π)\theta_2\in(0,\pi) \bigcup(-2\pi,-\pi)θ2∈(0,π)⋃(−2π,−π)时，机器人处于右手系，此时handcoor=1handcoor=1handcoor=1
当θ2∈(−π,0)⋃(π,2π)\theta_2\in(-\pi,0) \bigcup(\pi,2\pi)θ2∈(−π,0)⋃(π,2π)时，机器人处于左手系，此时handcoor=0handcoor=0handcoor=0
特别的，当θ2∈{−2π,−π,0,π,2π}\theta_2\in{\{-2\pi,-\pi,0,\pi,2\pi}\}θ2∈{−2π,−π,0,π,2π}，机器人处于奇异位置。
当机器人示教了一个点位，θ2\theta_2θ2便确定了，因此机器人当前示教点的手系便确定。

(2)、标志位flagJ1与flagJ2的确定

当θ1∈[−π,π]\theta_1\in[-\pi,\pi]θ1∈[−π,π]时，flagJ1=0flagJ1=0flagJ1=0；否则，flagJ1=1flagJ1=1flagJ1=1。
当θ2∈[−π,π]\theta_2\in[-\pi,\pi]θ2∈[−π,π]时，flagJ2=0flagJ2=0flagJ2=0；否则，flagJ2=1flagJ2=1flagJ2=1。
当手系handcoor确定后，便可以根据手系选逆解。当标志位flagJ1与flagJ2确定后，便可以确定关节角度值是否需要±2π\pm 2\pi±2π。

(3)、选取最短关节路径逆解

在做笛卡尔空间插补(如直线、圆弧、样条插补等)时，如果θ1∈[−2π,2π]\theta_1\in[-2\pi,2\pi]θ1∈[−2π,2π]且θ2∈[−2π,2π]\theta_2\in[-2\pi,2\pi]θ2∈[−2π,2π]，求逆解过程可能会有±2π\pm 2\pi±2π的跳变。这时需要通过对比上一次关节位置，选取最短关节路径的解，以达到插补过程关节位置平滑变化，此时不再需要标志位flagJ1与flagJ2。

三、正逆解正确性验证

1、单点验证

(1)在[−2π,2π][-2\pi, 2\pi][−2π,2π]中随机生成4个关节角度（模拟示教过程）
(2)计算标志位flagJ1和flagJ2，手系handcoor
(3)计算正运动学
(4)计算逆运动学
(5)逆运动学结果与随机生成的关节角度作差，验证运动学正逆解的正确性

2、直线验证

(1)在[−2π,2π][-2\pi,2\pi][−2π,2π]中随机生成4个关节角度，计算手系handcoor（模拟示教过程）
(2)运动学正解计算直线起点坐标(x,y,z,c)
(3)在工作空间里随机获取直线的终点x，y坐标，z在[-50mm,50mm]里随机生成,
c在[-30°, 30°]里随机生成，手系与起点的相同（模拟示教过程）
(4)速度规划(采用归一化五次多项式规划，详见博文：归一化5次多项式速度规划)
(5)直线插补
(6)计算每个插补点的逆运动学
(7)计算每个插补点的正运动学
(8)画出直线插补结果，合成位置、速度、加速度，关节位置曲线，验证正逆解的正确性

四、MATLAB代码

%{
Function: find_handcoor
Description: 计算scara机器人当前手系
Input: 关节2角度theta2(rad)
Output: 机器人手系
Author: Marc Pony(marc_pony@163.com)
%}
function handcoor = find_handcoor( theta2 )
if (theta2 > 0.0 && theta2 < pi) || (theta2 > -2.0 * pi && theta2 < -pi)handcoor = 1;
elsehandcoor = 0;
end
end%{
Function: find_flagJ1
Description: 计算scara机器人当前J1关节标志位
Input: 关节1角度theta1(rad)
Output: 机器人J1关节标志位flagJ1
Author: Marc Pony(marc_pony@163.com)
%}
function flagJ1 = find_flagJ1( theta1 )
if (theta1 >= -pi && theta1 <= pi)flagJ1 = 0;
elseflagJ1 = 1;
end
end%{
Function: find_flagJ2
Description: 计算scara机器人当前J2关节标志位
Input: 关节1角度theta2(rad)
Output: 机器人J2关节标志位flagJ2
Author: Marc Pony(marc_pony@163.com)
%}
function flagJ2 = find_flagJ2( theta2 )
if (theta2 >= -pi && theta2 <= pi)flagJ2 = 0;
elseflagJ2 = 1;
end
end%{
Function: scara_forward_kinematics
Description: scara机器人运动学正解
Input: 大臂长L1(mm)，小臂长L2(mm)，丝杆螺距screw(mm)，机器人关节位置jointPos(rad)
Output: 机器人末端位置cartesianPos(mm或rad)
Author: Marc Pony(marc_pony@163.com)
%}
function cartesianPos = scara_forward_kinematics(L1, L2, screw, jointPos)
theta1 = jointPos(1);
theta2 = jointPos(2);
theta3 = jointPos(3);
theta4 = jointPos(4);
x = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
y = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
z = theta3 * screw / (2 * pi);
c = theta1 + theta2 + theta4;
cartesianPos = [x; y; z; c];
end%{
Function: scara_inverse_kinematics
Description: scara机器人运动学逆解
Input: 大臂长L1(mm)，小臂长L2(mm)，丝杆螺距screw(mm)，机器人末端坐标cartesianPos(mm或rad)手系handcoor，标志位flagJ1与flagJ2
Output: 机器人关节位置jointPos(rad)
Author: Marc Pony(marc_pony@163.com)
%}
function jointPos = scara_inverse_kinematics(L1, L2, screw, cartesianPos, handcoor, flagJ1, flagJ2)
x = cartesianPos(1);
y = cartesianPos(2);
z = cartesianPos(3);
c = cartesianPos(4);
jointPos = zeros(4, 1);calculateError = 1.0e-8;
c2 = (x^2 + y^2 - L1^2 -L2^2) / (2.0 * L1 * L2);%若(x,y)在工作空间里，则c2必在[-1,1]里，但由于计算精度，c2的绝对值可能稍微大于1
temp = 1.0 - c2^2;if temp < 0.0if temp > -calculateErrortemp = 0.0;elseerror('区域不可到达');end
end
if handcoor == 0    %left handcoorjointPos(2) = atan2(-sqrt(temp), c2);
else                %right handcoorjointPos(2) = atan2(sqrt(temp), c2);
end
s2 = sin(jointPos(2));
jointPos(1) = atan2(y, x) - atan2(L2 * s2, L1 + L2 * c2);
jointPos(3) = 2.0 * pi * z / screw;if jointPos(1) <= -pijointPos(1) = jointPos(1) + 2.0*pi;
end
if jointPos(1) >= pijointPos(1) = jointPos(1) - 2.0*pi;
endif flagJ1 == 1if jointPos(1) >= 0.0jointPos(1) = jointPos(1) - 2.0*pi;elsejointPos(1) = jointPos(1) + 2.0*pi;end
endif flagJ2 == 1if jointPos(2) >= 0.0jointPos(2) = jointPos(2) - 2.0*pi;elsejointPos(2) = jointPos(2) + 2.0*pi;end
end
jointPos(4) = c - jointPos(1) - jointPos(2);
end%{
Function: scara_shortest_path_inverse_kinematics
Description: scara机器人运动学逆解
Input: 大臂长L1(mm)，小臂长L2(mm)，丝杆螺距screw(mm)，机器人末端坐标cartesianPos(mm或rad)手系handcoor，上一次的关节位置lastJointPos(rad)
Output: 机器人关节位置jointPos(rad)
Author: Marc Pony(marc_pony@163.com)
%}
function jointPos = scara_shortest_path_inverse_kinematics(len1, len2, screw, cartesianPos, handcoor, lastJointPos)
x = cartesianPos(1);
y = cartesianPos(2);
z = cartesianPos(3);
c = cartesianPos(4);
jointPos = zeros(4, 1);calculateError = 1.0e-8;
c2 = (x^2 + y^2 - len1^2 -len2^2) / (2.0 * len1 * len2);%若(x,y)在工作空间里，则c2必在[-1,1]里，但由于计算精度，c2的绝对值可能稍微大于1
temp = 1.0 - c2^2;if temp < 0.0if temp > -calculateErrortemp = 0.0;elseerror('区域不可到达');end
end
if handcoor == 0    %left handcoorjointPos(2) = atan2(-sqrt(temp), c2);
else                %right handcoorjointPos(2) = atan2(sqrt(temp), c2);
end
s2 = sin(jointPos(2));
jointPos(1) = atan2(y, x) - atan2(len2 * s2, len1 + len2 * c2);
jointPos(3) = 2.0 * pi * z / screw;temp = zeros(3, 1);
for i = 1 : 2temp(1) = abs((jointPos(i) + 2.0*pi) - lastJointPos(i));temp(2) = abs((jointPos(i) - 2.0*pi) - lastJointPos(i));temp(3) = abs(jointPos(i) - lastJointPos(i));if temp(1) < temp(2) && temp(1) < temp(3)jointPos(i) = jointPos(i) + 2.0*pi;elseif temp(2) < temp(1) && temp(2) < temp(3)jointPos(i) = jointPos(i) - 2.0*pi;else%do nothingend
end
jointPos(4) = c - jointPos(1) - jointPos(2);
end

clc;
clear;
close all;%% 输入参数
len1 = 200.0; %mm
len2 = 200.0; %mm
screw = 20.0; %mm
linearSpeed = 100; %mm/s
linearAcc = 800;   %mm/s^2
orientationSpeed = 200; %°/s
orientationAcc = 600;   %°/s^2
dt = 0.05; %s%% 单点验证：
%(1)在[-2*pi, 2*pi]中随机生成4个关节角度（模拟示教过程）
%(2)计算标志位flagJ1和flagJ2，手系handcoor
%(3)计算正运动学
%(4)计算逆运动学
%(5)逆运动学结果与随机生成的关节角度作差，验证运动学正逆解的正确性
testCount = 1000000;
res = zeros(testCount, 4);
k = 1;
while k < testCounttheta1 = -2.0*pi + 4.0*pi*rand;theta2 = -2.0*pi + 4.0*pi*rand;theta3 = -2.0*pi + 4.0*pi*rand;theta4 = -2.0*pi + 4.0*pi*rand;flagJ1 = find_flagJ1( theta1 );flagJ2 = find_flagJ2( theta2 );handcoor = find_handcoor( theta2 );jointPos = [theta1, theta2, theta3, theta4];cartesianPos = scara_forward_kinematics(len1, len2, screw, jointPos);jointPos2 = scara_inverse_kinematics(len1, len2, screw, cartesianPos, handcoor, flagJ1, flagJ2);res(k, :) = [theta1, theta2, theta3, theta4] - jointPos2';k = k + 1;
end
flag = all(abs(res - 0.0) < 1.0e-8) % flag = [1 1]则表示正逆解正确while 1%% 直线验证：%(1)在[-2*pi, 2*pi]中随机生成4个关节角度，计算手系handcoor（模拟示教过程）%(2)运动学正解计算直线起点坐标(x,y,z,c)%(3)在工作空间里随机获取直线的终点x，y坐标，z在[-50mm,50mm]里随机生成,%   c在[-30°, 30°]里随机生成，手系与起点的相同（模拟示教过程）%(4)速度规划%(5)直线插补%(6)计算每个插补点的逆运动学%(7)计算每个插补点的正运动学%(8)画出直线插补结果，合成位置、速度、加速度，关节位置曲线，验证正逆解的正确性theta1 = -2.0*pi + 4.0*pi*rand;theta2 = -2.0*pi + 4.0*pi*rand;theta3 = -2.0*pi + 4.0*pi*rand;theta4 = -2.0*pi + 4.0*pi*rand;handcoor = find_handcoor( theta2 );jointPos = [theta1, theta2, theta3, theta4];cartesianPos = scara_forward_kinematics(len1, len2, screw, jointPos);lastJointPos = jointPos;x1 = cartesianPos(1);y1 = cartesianPos(2);z1 = cartesianPos(3);c1 = cartesianPos(4)*180.0/pi;figure(1)set(gcf, 'color','w');a = 0 : 0.01 : 2*pi;plot((len1 + len2)*cos(a), (len1 + len2)*sin(a), 'r--')hold onplot(x1, y1, 'ko')xlabel('x/mm');ylabel('y/mm');axis equal tight[x2, y2] = ginput(1);z2 = -50 + 50*rand;c2 = -30 + 60*rand;L = sqrt((x2 - x1)^2 + (y2 - y1)^2 + (z2 - z1)^2);C = abs(c2 - c1);T = max([1.875 * L / linearSpeed, 1.875 * C / orientationSpeed, ...sqrt(5.7735 * L / linearAcc), sqrt(5.7735 * C / orientationAcc)]);t = (0 : dt : T)';u = t / T;if abs(t(end) - T) > 1.0e-8t = [t; T];u = [u; 1];endu = t / T;s = 10*u.^3 - 15*u.^4 + 6*u.^5;ds = 30*u.^2 -60*u.^3 + 30*u.^4;dds = 60*u - 180*u.^2 + 120*u.^3;len = L * s;vel = (L / T) * ds;acc = (L / T^2) * dds;pos = zeros(length(t), 4);jointPos = zeros(length(t), 4);cartesianPos = zeros(length(t), 4);rate = [x2 - x1, y2 - y1, z2 - z1, c2 - c1] / L;for i = 1 : length(t)pos(i, :) = [x1, y1, z1, c1] + len(i) * rate;pos(i, 4) = pos(i, 4)*pi/180.0;jointPos(i, :) = scara_shortest_path_inverse_kinematics(len1, len2, screw, pos(i, :), handcoor, lastJointPos);cartesianPos(i, :) = scara_forward_kinematics(len1, len2, screw, jointPos(i, :));lastJointPos = jointPos(i, :);endplot3(cartesianPos(:, 1), cartesianPos(:, 2), cartesianPos(:, 3), 'ro');plot3([x1 x2], [y1 y2], [z1 z2], '+','markerfacecolor','k','markersize', 14)figure(2)set(gcf, 'color','w');subplot(3,1,1)plot(t, len)hold onplot([0 t(end)], [L L], 'r--')xlabel('t/s');ylabel('len/mm');subplot(3,1,2)plot(t, vel)hold onplot([0 t(end)], [linearSpeed linearSpeed], 'r--')xlabel('t/s');ylabel('vel/ mm/s');subplot(3,1,3)plot(t, acc)hold onplot([0 t(end)], [linearAcc linearAcc], 'r--')plot([0 t(end)], [-linearAcc -linearAcc], 'r--')xlabel('t/s');ylabel('acc/ mm/s^2');figure(3)set(gcf, 'color','w');subplot(3,1,1)plot(t, len*rate(4))hold onplot([0 t(end)], [c2-c1 c2-c1], 'r--')xlabel('t/s');ylabel('c/°');subplot(3,1,2)plot(t, vel*rate(4))hold onplot([0 t(end)], [orientationSpeed orientationSpeed], 'r--')plot([0 t(end)], [-orientationSpeed -orientationSpeed], 'r--')xlabel('t/s');ylabel('dc/ °/s');subplot(3,1,3)plot(t, acc*rate(4))hold onplot([0 t(end)], [orientationAcc orientationAcc], 'r--')plot([0 t(end)], [-orientationAcc -orientationAcc], 'r--')xlabel('t/s');ylabel('ddc/ °/s^2');figure(4)set(gcf, 'color','w');subplot(2,2,1)plot(t, jointPos(:,1)*180.0/pi)xlabel('t/s');ylabel('J1/°');subplot(2,2,2)plot(t, jointPos(:,2)*180.0/pi)xlabel('t/s');ylabel('J2/°');subplot(2,2,3)plot(t, jointPos(:,3)*180.0/pi)xlabel('t/s');ylabel('J3/°');subplot(2,2,4)plot(t, jointPos(:,4)*180.0/pi)xlabel('t/s');ylabel('J4/°');pause()close all
end

scara机器人运动学正逆解相关推荐

Puma560机器人运动学正逆解
puma560机器人D-H参数 puma560采用的是改进D-H参数,其DH参数表如下: i αi ai di θi 1 0 0 0 t1 2 -90 0 0 t2 3 0 r2 d3 t3 4 -9 ...
MATLAB机器人机械臂运动学正逆解、动力学建模仿真与轨迹规划
MATLAB机器人机械臂运动学正逆解.动力学建模仿真与轨迹规划,雅克比矩阵求解.蒙特卡洛采样画出末端执行器工作空间基于时间最优的改进粒子群优化算法机械臂轨迹规划设计 ID:4610679190520 ...
多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试
多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试 ) DH建模法可以参考这个博客: 还有<机器人>这本书,一定要理论实践相结合,理解后可以用几何法建模也可以用 ...
基于Robotics toolbox的定制/非标机构的运动学正逆解
建立坐标系这一步很重要,如果发现DH参数无法确定,可能是坐标系的建立有问题,返回来重新建立. 我发现网上基本都是六自由度全转动机器人,很少有定制机构的机器人建模,特别是移动+转动的,这也给我DH参数 ...
工业机械人运动学正逆解，简单粗暴！！！！！！
ur机械臂是六自由度机械臂,由D-H参数法确定它的运动学模型,连杆坐标系的建立如上图所示. 转动关节θi是关节变量,连杆偏移di是常数. 关节编号 α(绕x轴) a(沿x轴) θ(绕z轴) d(沿z轴 ...
6轴机器人运动学逆解matlab,六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现...
摘要本文主要是给大家一个系统的概念,如何用Matlab实现六轴机器人的建模和实现轨迹规划.以后将会给大家讲解如何手写正逆解以及轨迹插补的程序.程序是基于Matlab2016a,工具箱版本为Robot ...
【机器人原理与实践（三）】六轴机械臂正逆解控制
文章目录 3.1 空间转换矩阵的理解 3.1.1平移变换 3.1.2旋转变换 3.2 D-H参数法 3.3 建立机械臂模型 3.3.1 机械臂模型介绍 3.3.2 使用Matlab进行示教仿真 3.4 ...
MATLAB机器人正逆解
MATLAB机器人求正逆解手把手教你MATLAB Robotics Toolbox工具箱③ Matlab RoboticToolBox(一)Link参数.三自由度/四自由度逆运动学 https:// ...
SCARA机器人运动学模型建立
1 DH模型 DH模型是目前机器人建模过程中使用最多的方法.此方法不仅简单好用,且适用范围广.如图表达了通用关节-连杆之间相对位置关系. 关节-连杆组合之间位置关系 D-H 建模第一步便是为关节定义坐 ...
有关并联绳驱机器人运动学正解反解的学习（新手）
有关并联绳驱机器人运动学正解反解的学习 Preface(complain) Perface(start) Advantages of parallel robot Disdvantages of pa ...

scara机器人运动学正逆解

文章目录