scara机器人运动学正逆解

文章目录

  • 一、scara机器人运动学正解
  • 二、scara机器人运动学逆解
    • 1、正装scara机器人运动学逆解
    • 2、吊装scara机器人运动学逆解
    • 3、几个值得思考的问题
      • (1)、手系handcoor的确定
      • (2)、标志位flagJ1与flagJ2的确定
      • (3)、选取最短关节路径逆解
  • 三、正逆解正确性验证
    • 1、单点验证
    • 2、直线验证
  • 四、MATLAB代码

一、scara机器人运动学正解

scara机器人运动学正逆解_第1张图片
  末端 B B B x x x坐标为向量 O A \bf{OA} OA与向量 A B \bf{AB} AB x x x轴上投影之和,末端 B B B y y y坐标亦然:
{ x = L 1 c o s θ 1 + L 2 c o s ( θ 1 + θ 2 ) y = L 1 s i n θ 1 + L 2 s i n ( θ 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)
  第三轴为丝杆上下平移运动,设丝杆螺距 s s s,末端 B B B z z z坐标:
z = θ 3 s 2 π (2) z=\frac{\theta_3s}{2\pi} \tag 2 z=2πθ3s(2)
  末端 B B B的姿态角 c c c
c = θ 1 + θ 2 + θ 4 (3) c=\theta_1+ \theta_2+ \theta_4 \tag 3 c=θ1+θ2+θ4(3)
  综上,scara机器人的运动学正解为:
{ x = L 1 c o s θ 1 + L 2 c o s ( θ 1 + θ 2 ) y = L 1 s i n θ 1 + L 2 s i n ( θ 1 + θ 2 ) z = θ 3 s 2 π 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机器人运动学逆解

scara机器人运动学正逆解_第2张图片
  连接 O B OB OB,过 B B B B C BC BC垂直于 O A OA OA C C C,在 Δ O A B \Delta OAB ΔOAB中,由余弦定理:
c o s ( π − θ 2 ) = L 1 2 + L 2 2 − ( x 2 + y 2 ) 2 L 1 L 2 (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)
  记 c 2 = c o s θ 2 c_2=cos\theta_2 c2=cosθ2,式(5)可写成:
c 2 = x 2 + y 2 − L 1 2 − L 2 2 2 L 1 L 2 (6) c_2=\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}\tag 6 c2=2L1L2x2+y2L12L22(6)
  记 s 2 = s i n θ 2 s_2=sin\theta_2 s2=sinθ2,则 s 2 s_2 s2有两个解:
s 2 = ± 1 − c 2 2 (7) s_2=\pm\sqrt{1-c_2^2}\tag 7 s2=±1c22 (7)
  取正数解,机器人处于右手系(right handcoor);取负数解,机器人处于左手系(left handcoor);特殊地,若 s 2 = 0 s_2=0 s2=0,机器人处于奇异位置(singular position),此时 θ 2 = k π ( k ∈ Z ) \theta_2=k\pi(k\in{Z}) θ2=kπ(kZ),一般 θ 2 ∈ { − 2 π , − π , 0 , π , 2 π } \theta_2\in{\{-2\pi,-\pi,0,\pi,2\pi}\} θ2{2π,π,0,π,2π}
  至此,可求得 θ 2 \theta_2 θ2
θ 2 = a t a n 2 ( s 2 , c 2 ) (8) \theta_2=atan2(s_2,c_2)\tag 8 θ2=atan2(s2,c2)(8)
  如上图,易得:
α = a t a n 2 ( y , x ) (9) \alpha=atan2(y,x)\tag 9 α=atan2(y,x)(9)
  在 Δ O B C \Delta OBC ΔOBC中,记 r = ∥ O B ∥ r=\|OB\| r=OB
s i n β = ∥ B C ∥ ∥ O B ∥ = L 2 s 2 r (10) sin\beta=\frac{\|BC\|}{\|OB\|}=\frac{L_2s_2}{r}\tag {10} sinβ=OBBC=rL2s2(10)
c o s β = ∥ O C ∥ ∥ O B ∥ = ∥ O A ∥ + ∥ A C ∥ ∥ O B ∥ = L 1 + L 2 c 2 r (11) cos\beta=\frac{\|OC\|}{\|OB\|}=\frac{\|OA\|+\|AC\|}{\|OB\|}=\frac{L_1+L_2c_2}{r}\tag {11} cosβ=OBOC=OBOA+AC=rL1+L2c2(11)
  由于 r > 0 r>0 r>0,由式 ( 10 ) (10) (10)和式 ( 11 ) (11) (11)得:
β = a t a n 2 ( L 2 s 2 , L 1 + L 2 c 2 ) (12) \beta=atan2(L_2s_2,L_1+L_2c_2)\tag {12} β=atan2(L2s2,L1+L2c2)(12)
  至此,可求得 θ 1 \theta_1 θ1
θ 1 = α − β = a t a n 2 ( y , x ) − a t a n 2 ( L 2 s 2 , L 1 + L 2 c 2 ) (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 = a t a n 2 ( y , x ) − a t a n 2 ( L 2 s 2 , L 1 + L 2 c 2 ) θ 2 = a t a n 2 ( s 2 , c 2 ) θ 3 = 2 π z s θ 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)用反余弦函数 a c o s acos acos求得,但这里采用了双变量反正切函数 a t a n 2 atan2 atan2,至于原因,可以参见这篇博文:为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数?
  由于 a t a n 2 atan2 atan2的值域为 [ − π , π ] [-\pi,\pi] [π,π],可得关节角度 θ 1 \theta_1 θ1 − 2 π -2\pi 2π 2 π 2\pi 2π之间(区间左端点 − 2 π -2\pi 2π和区间右端点 2 π 2\pi 2π不一定能达到), θ 2 ∈ [ − π , π ] \theta_2\in[-\pi,\pi] θ2[π,π] θ 3 \theta_3 θ3范围与丝杆螺距 s s s z z z轴的行程有关, θ 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=θ12π

  对于 θ 1 \theta_1 θ1,当 f l a g J 1 = 1 flagJ1=1 flagJ1=1时:
  若 θ 1 ≥ 0 \theta_1\geq0 θ10,则 θ 1 = θ 1 − 2 π \theta_1=\theta_1-2\pi θ1=θ12π
  若 θ 1 < 0 \theta_1<0 θ1<0,则 θ 1 = θ 1 + 2 π \theta_1=\theta_1+2\pi θ1=θ1+2π

  对于 θ 2 \theta_2 θ2,当 f l a g 2 = 1 flag2=1 flag2=1时:
  若 θ 2 ≥ 0 \theta_2\geq0 θ20,则 θ 2 = θ 2 − 2 π \theta_2=\theta_2-2\pi θ2=θ22π
  若 θ 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π,π)时,机器人处于右手系,此时 h a n d c o o r = 1 handcoor=1 handcoor=1
  当 θ 2 ∈ ( − π , 0 ) ⋃ ( π , 2 π ) \theta_2\in(-\pi,0) \bigcup(\pi,2\pi) θ2(π,0)(π,2π)时,机器人处于左手系,此时 h a n d c o o r = 0 handcoor=0 handcoor=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[π,π]时, f l a g J 1 = 0 flagJ1=0 flagJ1=0;否则, f l a g J 1 = 1 flagJ1=1 flagJ1=1
  当 θ 2 ∈ [ − π , π ] \theta_2\in[-\pi,\pi] θ2[π,π]时, f l a g J 2 = 0 flagJ2=0 flagJ2=0;否则, f l a g J 2 = 1 flagJ2=1 flagJ2=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([email protected])
%}
function handcoor = find_handcoor( theta2 )
if (theta2 > 0.0 && theta2 < pi) || (theta2 > -2.0 * pi && theta2 < -pi)
    handcoor = 1;
else
    handcoor = 0;
end
end

%{
Function: find_flagJ1
Description: 计算scara机器人当前J1关节标志位
Input: 关节1角度theta1(rad)
Output: 机器人J1关节标志位flagJ1
Author: Marc Pony([email protected])
%}
function flagJ1 = find_flagJ1( theta1 )
if (theta1 >= -pi && theta1 <= pi)
    flagJ1 = 0;
else
    flagJ1 = 1;
end
end

%{
Function: find_flagJ2
Description: 计算scara机器人当前J2关节标志位
Input: 关节1角度theta2(rad)
Output: 机器人J2关节标志位flagJ2
Author: Marc Pony([email protected])
%}
function flagJ2 = find_flagJ2( theta2 )
if (theta2 >= -pi && theta2 <= pi)
    flagJ2 = 0;
else
    flagJ2 = 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([email protected])
%}
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([email protected])
%}
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.0
    if temp > -calculateError
        temp = 0.0;
    else
        error('区域不可到达');
    end
end
if handcoor == 0    %left handcoor
    jointPos(2) = atan2(-sqrt(temp), c2);
else                %right handcoor
    jointPos(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) <= -pi
    jointPos(1) = jointPos(1) + 2.0*pi;
end
if jointPos(1) >= pi
    jointPos(1) = jointPos(1) - 2.0*pi;
end

if flagJ1 == 1
    if jointPos(1) >= 0.0
        jointPos(1) = jointPos(1) - 2.0*pi;
    else
        jointPos(1) = jointPos(1) + 2.0*pi;
    end
end

if flagJ2 == 1
    if jointPos(2) >= 0.0
        jointPos(2) = jointPos(2) - 2.0*pi;
    else
        jointPos(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([email protected])
%}
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.0
    if temp > -calculateError
        temp = 0.0;
    else
        error('区域不可到达');
    end
end
if handcoor == 0    %left handcoor
    jointPos(2) = atan2(-sqrt(temp), c2);
else                %right handcoor
    jointPos(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 : 2
    temp(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 nothing
    end
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 < testCount
    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;
    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 on
    plot(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-8
        t = [t; T];
        u = [u; 1];
    end
    u = 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, :);
    end
    plot3(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 on
    plot([0 t(end)], [L L], 'r--')
    xlabel('t/s');
    ylabel('len/mm');
    subplot(3,1,2)
    plot(t, vel)
    hold on
    plot([0 t(end)], [linearSpeed linearSpeed], 'r--')
    xlabel('t/s');
    ylabel('vel/ mm/s');
    subplot(3,1,3)
    plot(t, acc)
    hold on
    plot([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 on
    plot([0 t(end)], [c2-c1 c2-c1], 'r--')
    xlabel('t/s');
    ylabel('c/°');
    subplot(3,1,2)
    plot(t, vel*rate(4))
    hold on
    plot([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 on
    plot([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机器人运动学正逆解_第3张图片
scara机器人运动学正逆解_第4张图片
scara机器人运动学正逆解_第5张图片
scara机器人运动学正逆解_第6张图片

你可能感兴趣的:(机器人学)