精华内容
下载资源
问答
  • SCARA机器人

    2013-02-25 15:33:59
    关于SCARA机器人的建模及运动仿真的论文
  • Scara机器人matlab代码SCARA_Robot_Modelling_and_Control 该项目由一个 SCARA 机器人模型组成,它在 Matlab 中构建了机器人模型,并在 Simulink 中实现了不同的控制。 在这里我们可以找到项目组成的所有文件。 这是...
  • 库卡SCARA机器人KR 5 scara R550样本pdf,库卡SCARA机器人KR 5 scara R550样本
  • 库卡SCARA机器人KR 10 scara R600样本pdf,库卡SCARA机器人KR 10 scara R600样本
  • SCARA机器人预研报告

    2016-05-17 17:04:05
    SCARA机器人预研报告
  • SCARA机器人的运动轨迹为研究对象,借助Solid Works的草图功能进行凸轮机构的空间布局,同时根据SCARA机器人机械臂的运动规律,运用图解法进行凸轮机构轮廓设计,成功设计出2个具有相互影响关系的凸轮机构。...
  • SCARA机器人的设计毕业设计
  • SCARA机器人使用Robotics Toolbox for MATLAB工具箱,做了一定的动力学仿真
  • SCARA机器人零点标定

    2020-05-06 00:27:55
    SCARA机器人采用在关节处的两臂之间用定位销的方式将相邻两臂固定在理想零点位置,并以此作为该关节零点位置的标定。
  • SCARA机器人obj文件

    2017-03-19 22:28:25
    TRIO-Basic从入门到精通教程六课程配套资源,打包了Scara机器人obj文件。带有3d机器人仿真资料。
  • scara机器人运动学正逆解

    千次阅读 多人点赞 2020-03-14 14:59:02
    文章目录一、scara机器人运动学正解二、scara机器人运动学逆解1、正装scara机器人运动学逆解2、吊装scara机器人运动学逆解三、MATLAB代码 一、scara机器人运动学正解   末端BBB的xxx坐标为向量OA\bf{OA}OA与向量...

    一、scara机器人运动学正解

    scara机器人运动学正解
      末端BBxx坐标为向量OA\bf{OA}与向量AB\bf{AB}xx轴上投影之和,末端BByy坐标亦然:
    {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.
      第三轴为丝杆上下平移运动,设丝杆螺距ss,末端BBzz坐标:
    z=θ3s2π(2) z=\frac{\theta_3s}{2\pi} \tag 2
      末端BB的姿态角cc
    c=θ1+θ2+θ4(3) c=\theta_1+ \theta_2+ \theta_4 \tag 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}

    二、scara机器人运动学逆解

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

    scara机器人运动学逆解
      连接OBOB,过BBBCBC垂直于OAOACC,在ΔOAB\Delta 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
      记c2=cosθ2c_2=cos\theta_2,式(5)可写成:
    c2=x2+y2L12L222L1L2(6)c_2=\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}\tag 6
      记s2=sinθ2s_2=sin\theta_2,则s2s_2有两个解:
    s2=±1c22(7)s_2=\pm\sqrt{1-c_2^2}\tag 7
      取正数解,机器人处于右手系(right handcoor);取负数解,机器人处于左手系(left handcoor);特殊地,若s2=0s_2=0,机器人处于奇异位置(singular position),此时θ2=kπ(kZ)\theta_2=k\pi(k\in{Z}),一般 θ2{2π,π,0,π,2π}\theta_2\in{\{-2\pi,-\pi,0,\pi,2\pi}\}
      至此,可求得θ2\theta_2
    θ2=atan2(s2,c2)(8)\theta_2=atan2(s_2,c_2)\tag 8
      如上图,易得:
    α=atan2(y,x)(9)\alpha=atan2(y,x)\tag 9
      在ΔOBC\Delta OBC中,记r=OBr=\|OB\|
    sinβ=BCOB=L2s2r(10)sin\beta=\frac{\|BC\|}{\|OB\|}=\frac{L_2s_2}{r}\tag {10}
    cosβ=OCOB=OA+ACOB=L1+L2c2r(11)cos\beta=\frac{\|OC\|}{\|OB\|}=\frac{\|OA\|+\|AC\|}{\|OB\|}=\frac{L_1+L_2c_2}{r}\tag {11}
      由于r>0r>0,由式(10)(10)和式(11)(11)得:
    β=atan2(L2s2,L1+L2c2)(12)\beta=atan2(L_2s_2,L_1+L_2c_2)\tag {12}
      至此,可求得θ1\theta_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}
      由式(4)(4)容易求得θ3\theta_3θ4\theta_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}
      在求解θ2\theta_2时,完全可以根据式(6)(6)用反余弦函数acosacos求得,但这里采用了双变量反正切函数atan2atan2,至于原因,可以参见这篇博文:为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数?
      由于atan2atan2的值域为[π,π][-\pi,\pi],可得关节角度θ1\theta_12π-2\pi2π2\pi之间(区间左端点2π-2\pi和区间右端点2π2\pi不一定能达到),θ2[π,π]\theta_2\in[-\pi,\pi]θ3\theta_3范围与丝杆螺距sszz轴的行程有关,θ4\theta_4一般范围在[2π,2π][-2\pi,2\pi]。对于正装scara机器人,为避免机构干涉,一般θ1[3π4,3π4]\theta_1\in[-\frac{3\pi}{4},\frac{3\pi}{4}]θ2[2π3,2π3]\theta_2\in[-\frac{2\pi}{3},\frac{2\pi}{3}]。 式(14)(14)的运动学逆解无法做到使机器人在工作空间360°无死角运动,需要将其推广,使得θ1[2π,2π]\theta_1\in[-2\pi,2\pi]θ2[2π,2π]\theta_2\in[-2\pi,2\pi],即吊装scara机器人关节1和关节2的角度范围。

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

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

      对于θ1\theta_1,当flagJ1=1flagJ1=1时:
      若θ10\theta_1\geq0,则θ1=θ12π\theta_1=\theta_1-2\pi
      若θ1<0\theta_1<0,则θ1=θ1+2π\theta_1=\theta_1+2\pi

      对于θ2\theta_2,当flagJ2=1flagJ2=1时:
      若θ20\theta_2\geq0,则θ2=θ22π\theta_2=\theta_2-2\pi
      若θ2<0\theta_2<0θ2=θ2+2π\theta_2=\theta_2+2\pi

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

    3、几个值得思考的问题

    (1)、手系handcoor的确定

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

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

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

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

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

    三、正逆解正确性验证

    1、单点验证

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

    2、直线验证

      (1)在[2π,2π][-2\pi,2\pi]中随机生成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;
    else
        handcoor = 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;
    else
        flagJ1 = 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;
    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(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.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(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.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
    

    1
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    展开全文
  • 2019年中国SCARA机器人行业概览.pdf
  • matlab机器人工具箱搭建scara机器人模型并进行轨迹规划
  • EPSON-G系列SCARA机器人pdf,提供“EPSON-G系列SCARA机器人”免费资料下载,主要包括产品技术参数、尺寸等内容,可供选型参考。
  • SCARA机器人的设计定稿论文
  • 爱普生RS-SCARA 机器人
  • 爱普生LS-SCARA机器人
  • 【机器人学习】SCARA机器人正逆运动学分析与直线轨迹规划.rar
  • 1. SCARA机器人建模

    千次阅读 多人点赞 2020-03-08 17:04:58
    这篇博客主要介绍利用robotic toolbox建立scara机器人模型并实现机器人的简单运动。

    目录

     

    1. 引言

    2. 建模

    3. 让机器人动起来

    4. 总结


    1. 引言

    SCARA机器人拥有四个自由度,三个旋转自由度,一个平移自由度。在我的机器人学专栏中已经介绍完了机器人正运动学---连杆坐标系与DH参数(后面简称参考博客)。有了这个基本的数学工具我们已经可以对机器人进行建模并完成一些简单的仿真。这篇文章将利用robotic toolbox建模一个四自由度的SCARA机器人。

    在这个专栏中,如无特殊说明我使用的Matlab版本为R2018a,robotic toolbox版本为10.1,操作系统为Ubuntu16.04。关于如何安装robotic toolbox这里就不再赘述了,直接搜索”robotic toolbox安装“会有很多相关教程。不过网络环境有限制的话可能无法登录这个工具箱的官网,因此我把这个工具箱上传到了github,https://github.com/hitgavin/robotic_toolbox/tree/master/robotic_toolbox_10.1

    2. 建模

    关于如何建立机器人连杆的DH坐标系请见我的参考博客。进行机器人的运动学仿真,我们首先要写出这个机器人的DH参数表。在Matlab中也就是先建立一个矩阵:

    l1 = 0.225; % unit m
    l2 = 0.175; 
    d3 = 0.05;
    
    scara_dh = [
    % theta    d    a    alpha    
        0      0    l1     0      
        0      0    l2     pi      
        0      d3   0      0      
        0      0    0      0     
        ];

    在利用robotic toolbox建立机器人的模型时通常是利用DH参数建立所有的连杆,然后再利用这些连杆去创建机器人,robotic toolbox中利用Link函数可以创建平移和旋转两种类型的连杆。SCARA机器人有四个关节,因此虽然表面上它只有三个连杆,我们还是要建立四个,因为在SCARA中的滚珠丝杠花键有两个自由度。SCARA机器人的各个连杆定义如下:

    % joint-type 0 for revolute, 1 for prismatic
    %          theta  d     a   alpha joint-type
    L(1) = Link([0    0     l1    0       0], 'standard');
    L(2) = Link([0    0     l2    pi      0], 'standard');
    L(3) = Link([0    d3    0     0       1], 'standard');
    L(4) = Link([0    0     0     0       0], 'standard');

    Link函数的第一个参数描述连杆属性,这是一个向量,前四个元素是DH参数,最后一个是连杆类型,0代表旋转,1代表平移。Link函数的第二个参数‘standard’代表这是标准DH参数,正如你所想,还有修改DH参数。我们在参考博客中提到标准DH参数是按传动轴建立坐标系的,修改DH参数则是按驱动轴建立坐标系,关于这一点在我的机器人学专栏后续文章中会进行介绍,这里不再展开了。

    连杆建立好之后接下来就是创建一个机器人了,robotic toolbox已经为我们封装了极为简洁的接口,即SerialLink函数。在这里创建SCARA机器人只需要一行代码。

    scara = SerialLink(L, 'name', 'scara400');

    第一个参数就是传入Link的数组L,第二个参数用于设置机器人的名字,我们取名为scara400,还可以有其他一些参数具体可以参考这个库的说明文档。

    以上就是创建机器人模型的基本过程,当然有很多其他参数比如动力学参数,工具坐标系,关节限位等都没有设置,但是我们已经有了一个可以运动的机器人了(或许),其他的参数我们后面会陆陆续续用到。

    3. 让机器人动起来

    以上创建了SCARA机器人模型,接下来我们想办法让机器人动起来,其实很简单,只需要几行代码:

    L(3).qlim = [0.05 0.3];
    q1 = [0 0 0.05 0];
    q2 = [pi/4 pi/6 0.2 pi/8];
    q = jtraj(q1, q2, 100);
    scara.plot(q);

    代码执行后你应该能看到机器人从初始位置运动到终止位置的动画了。下面我们逐行解释一下代码。

    第一行是为了给平移的连杆设置一下关节限位,如果没有这句话后面的代码执行会报错(这就是前面说或许的原因),这也说不好是为什么,更奇葩的是这个限位不能是负值,所以如果你细心观察DH参数表和参考博客中的DH参数表会发现第二个连杆的\alpha角在这里设置成了\pi,就是为了让连杆2坐标系的z轴朝下,这样连杆3的平移方向就是z轴正向了,也就避免了限位参数为负。

    第二行和第三行分别设置了机器人的起始关节位置和终止关节位置。第四行是个插值函数,我们这里暂且不提,这个函数执行结果是一个关节位置矩阵,每一行代表机器人的一个位姿(就是各个关节的关节位置)。

    最后一行是让机器人依次经过q中的各个位置。

    以下是模型显示的一个截图。

    完整的代码已经上传到github,https://github.com/hitgavin/robotic_toolbox/tree/master/scara_model

    4. 总结

    这篇文章主要介绍了如何利用robotic toolbox创建一个简单的机器人并驱动机器人运动。

    展开全文
  • scara 机器人说明书

    2013-08-31 13:25:21
    R350 Z200型SCARA机器人,包含各种参数如果要中文翻译可以联系本人
  • SCARA机器人手眼标定之目标抓取实例及程序
  • SCARA机器人抓取圆柱物体的视觉标定方法,陈甦欣,叶建辉,本文以SCARA机器人依靠视觉引导抓取圆柱物体为例,提出了一种简单有效的视觉标定方法。该方法通过图像坐标系和机器人坐标系之间的�
  • matlab搭建scara机器人的gui界面:正运动学机器人工具箱开发程序
  • 通过建立SCARA机器人的机械结构模型,设计主要的机械部件和控制电路,上位机,完成综合调试,最后经过实验验证系统的稳定性
  • Scara机器人视觉引导测试报告,测试和分析项目需求和最终的解决方案。
  • SCARA机器人手眼标定执行和结果保存实例及源程序
  • 基于解耦控制的SCARA机器人轨迹跟踪控制器设计pdf,基于解耦控制的SCARA机器人轨迹跟踪控制器设计
  • scara机器人雅克比矩阵

    千次阅读 2020-03-15 11:49:59
    文章目录一、scara机器人雅克比矩阵1、雅克比矩阵推导2、机器人末端速度与关节速度、末端加速度与关节加速度之间相互转换(1)、已知关节速度,计算末端速度(2)、已知关节加速度,计算末端加速度(3)、已知末端速度,...

    一、scara机器人雅克比矩阵

    1、雅克比矩阵推导

      scara机器人的运动学正解为:
    {x=L1cosθ1+L2cos(θ1+θ2)y=L1sinθ1+L2sin(θ1+θ2)z=θ3s2πc=θ1+θ2+θ4(1) \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 1 \end{cases}
      参考我另一篇博文:scara机器人运动学正逆解
      上式两边对时间tt求导,得:
    {vx=L1sinθ1θ1˙L2sin(θ1+θ2)(θ1˙+θ2˙)vy=L1cosθ1θ1˙+L2cos(θ1+θ2)(θ1˙+θ2˙)vz=θ3˙s2πvc=θ1˙+θ2˙+θ4˙(2) \begin{cases} v_x=-L_1sin\theta_1\dot{\theta_1}-L_2sin(\theta_1+\theta_2)(\dot{\theta_1} + \dot{\theta_2}) \\ v_y=L_1cos\theta_1\dot{\theta_1}+L_2cos(\theta_1+\theta_2) (\dot{\theta_1} + \dot{\theta_2}) \\ v_z=\frac{\dot{\theta_3}s}{2\pi} \\ v_c=\dot{\theta_1}+ \dot{\theta_2}+ \dot{\theta_4} \tag 2 \end{cases}
      上式写成矩阵形式,得:
    [vxvyvzvc]=J[θ1˙θ2˙θ3˙θ4˙](3) \left[ \begin{matrix} v_x \\ v_y \\ v_z \\ v_c \end{matrix} \right] = J \left[ \begin{matrix} \dot{\theta_1} \\ \dot{\theta_2} \\ \dot{\theta_3} \\ \dot{\theta_4} \end{matrix} \right] \tag{3}
      其中JJ为雅克比矩阵:
    J=[L1sinθ1L2sin(θ1+θ2)L2sin(θ1+θ2)00L1cosθ1+L2cos(θ1+θ2)L2cos(θ1+θ2)0000s/(2π)01101](4) J=\left[ \begin{matrix} -L_1sin\theta_1-L_2sin(\theta_1+\theta_2) & -L_2sin(\theta_1+\theta_2) & 0 & 0\\ L_1cos\theta_1+L_2cos(\theta_1+\theta_2) & L_2cos(\theta_1+\theta_2) & 0 & 0 \\ 0 & 0 & s/(2\pi) & 0 \\ 1 & 1 & 0 & 1 \end{matrix} \right] \tag{4}
      由式(3)可知,雅克比矩阵是机器人末端速度与关节速度的桥梁,是两者间的映射关系。雅克比矩阵的行列式为:
    J=L1sinθ1L2sin(θ1+θ2)L2sin(θ1+θ2)00L1cosθ1+L2cos(θ1+θ2)L2cos(θ1+θ2)0000s/(2π)01101=L1L2s2πsinθ2(5) \begin{aligned} |J|&=\left| \begin{matrix} -L_1sin\theta_1-L_2sin(\theta_1+\theta_2) & -L_2sin(\theta_1+\theta_2) & 0 & 0\\ L_1cos\theta_1+L_2cos(\theta_1+\theta_2) & L_2cos(\theta_1+\theta_2) & 0 & 0 \\ 0 & 0 & s/(2\pi) & 0 \\ 1 & 1 & 0 & 1 \end{matrix} \right| \\ &= \frac{L_1L_2s}{2\pi}sin{\theta_2} \\ \tag{5} \end{aligned}

      当sinθ2=0sin{\theta_2}=0时,J=0|J|=0,雅克比矩阵不可逆,此时机器人处于奇异位置。在奇异位置,无法通过机器人末端速度和雅克比矩阵计算得到机器人关节速度。或者说,在奇异位置,通过机器人末端速度和雅克比矩阵计算得到的机器人关节速度为无穷大,这在实际物理世界是不可能的。因此,机器人笛卡尔空间插补运动(直线、圆弧、样条插补等)一定要考虑并避开奇异位置。

    2、机器人末端速度与关节速度、末端加速度与关节加速度之间相互转换

    (1)、已知关节速度,计算末端速度

      已知机器人在工作空间内任意位置(即使是奇异位置,即sinθ2=0sin{\theta_2}=0)的关节位置和关节速度,通过雅克比矩阵,可以根据式(3)直接求得。

    (2)、已知关节加速度,计算末端加速度

      式(3)两边对时间t求导,得:
    [axayazac]=J˙[θ1˙θ2˙θ3˙θ4˙]+J[θ1¨θ2¨θ3¨θ4¨](6) \left[ \begin{matrix} a_x \\ a_y \\ a_z \\ a_c \end{matrix} \right] = \dot{J} \left[ \begin{matrix} \dot{\theta_1} \\ \dot{\theta_2} \\ \dot{\theta_3} \\ \dot{\theta_4} \end{matrix} \right] +J\left[ \begin{matrix} \ddot{\theta_1} \\ \ddot{\theta_2} \\ \ddot{\theta_3} \\ \ddot{\theta_4} \end{matrix} \right] \tag{6}
      其中:
    J˙=[L1cosθ1θ1˙L2cos(θ1+θ2)(θ1˙+θ2˙)L2cos(θ1+θ2)(θ1˙+θ2˙)00L1sinθ1θ1˙L2sin(θ1+θ2)(θ1˙+θ2˙)L2sin(θ1+θ2)(θ1˙+θ2˙)0000000000](7) \dot{J}=\left[ \begin{matrix} -L_1cos\theta_1\dot{\theta_1}-L_2cos(\theta_1+\theta_2) (\dot{\theta_1}+\dot{\theta_2}) & -L_2cos(\theta_1+\theta_2)(\dot{\theta_1}+\dot{\theta_2}) & 0 & 0\\ -L_1sin\theta_1\dot{\theta_1}-L_2sin(\theta_1+\theta_2)(\dot{\theta_1}+\dot{\theta_2}) & -L_2sin(\theta_1+\theta_2)(\dot{\theta_1}+\dot{\theta_2}) & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{matrix} \right] \tag{7}

    (3)、已知末端速度,计算关节速度

      当机器人不在奇异位置时,根据式(3),关节速度如下计算:
    [θ1˙θ2˙θ3˙θ4˙]=J1[vxvyvzvc](8) \left[ \begin{matrix} \dot{\theta_1} \\ \dot{\theta_2} \\ \dot{\theta_3} \\ \dot{\theta_4} \end{matrix} \right] = J^{-1}\left[ \begin{matrix} v_x \\ v_y \\ v_z \\ v_c \end{matrix} \right] \tag{8}
      由于3,4轴关节速度很容易根据式(2)单独求得:
    {θ3˙=2πvz/sθ4˙=vcθ1˙θ2˙(9) \begin{cases} \dot{\theta_3}=2\pi v_z / s \\ \dot{\theta_4}=v_c-\dot{\theta_1}- \dot{\theta_2} \\ \tag {9} \end{cases}
      只考虑1,2轴关节速度的计算显得更加简单,计算量也更小。令:
    {s1=sinθ1s12=sin(θ1+θ2)c1=cosθ1c12=cos(θ1+θ2)(10) \begin{cases} s_1=sin\theta_1 \\ s_{12}=sin(\theta_1+\theta_2) \\ c_1=cos\theta_1 \\ c_{12}=cos(\theta_1+\theta_2) \tag {10} \end{cases}

    {a12=L2s12a11=L1s1+a12b1=vxa22=L2c12a21=L1c1+a22b2=vy(11) \begin{cases} a_{12}=-L_2s_{12} \\ a_{11}=-L_1s_1+a_{12} \\ b_1=v_x \\ a_{22}=L_2c_{12} \\ a_{21}=L_1c_1+a_{22} \\ b_2=v_y \\ \tag {11} \end{cases}
      式(2)转化为关于θ1˙\dot{\theta_1}θ2˙\dot{\theta_2}的二元一次方程组:
    {a11θ1˙+a12θ2˙=b1a21θ1˙+a22θ2˙=b2(12) \begin{cases} a_{11}\dot{\theta_1}+a_{12}\dot{\theta_2}=b_1 \\ a_{21}\dot{\theta_1}+a_{22}\dot{\theta_2}=b_2 \\ \tag {12} \end{cases}

      上式解得:
    {θ1˙=(a12b2a22b1)/(a11a22a12a21)θ2˙=(a11b2a21b1)/(a11a22a12a21)(13) \begin{cases} \dot{\theta_1}=-(a_{12}b_2 - a_{22}b_1)/(a_{11}a_{22} - a_{12}a_{21}) \\ \dot{\theta_2}=(a_{11}b_2 - a_{21}b_1)/(a_{11}a_{22} - a_{12}a_{21}) \\ \tag {13} \end{cases}

    (4)、已知末端加速度,计算关节加速度

      当机器人不在奇异位置时,根据式(6),关节加速度如下计算:
    [θ1¨θ2¨θ3¨θ4¨]=J1([axayazac]J˙[θ1˙θ2˙θ3˙θ4˙])(14) \left[ \begin{matrix} \ddot{\theta_1} \\ \ddot{\theta_2} \\ \ddot{\theta_3} \\ \ddot{\theta_4} \end{matrix} \right]=J^{-1}\left( \left[ \begin{matrix} a_x \\ a_y \\ a_z \\ a_c \end{matrix} \right] -\dot{J} \left[ \begin{matrix} \dot{\theta_1} \\ \dot{\theta_2} \\ \dot{\theta_3} \\ \dot{\theta_4} \end{matrix} \right] \right) \tag{14}
      由于3,4轴关节加速度很容易根据式(9)单独求得:
    {θ3¨=2πaz/sθ4¨=acθ1¨θ2¨(15) \begin{cases} \ddot{\theta_3}=2\pi a_z / s \\ \ddot{\theta_4}=a_c-\ddot{\theta_1}- \ddot{\theta_2} \\ \tag {15} \end{cases}
      只考虑1,2轴关节加速度的计算显得更加简单,计算量也更小。式(2)两边对时间t求导,得:

    {ax=L1(cosθ1θ1˙2+sinθ1θ1¨)L2[cos(θ1+θ2)(θ1˙+θ2˙)2         +sin(θ1+θ2)(θ1¨+θ2¨)]ay=L1(sinθ1θ1˙2+cosθ1θ1¨)+L2[sin(θ1+θ2)(θ1˙+θ2˙)2         +cos(θ1+θ2)(θ1¨+θ2¨)](16) \begin{cases} a_x=-L_1(cos\theta_1\dot{\theta_1}^2 + sin\theta_1\ddot{\theta_1})-L_2[cos(\theta_1+\theta_2)(\dot{\theta_1} + \dot{\theta_2})^2 \\ \ \ \ \ \ \ \ \ \ + sin(\theta_1+\theta_2)(\ddot{\theta_1} + \ddot{\theta_2})] \\ a_y=L_1(-sin\theta_1\dot{\theta_1}^2+cos\theta_1\ddot{\theta_1})+L_2[-sin(\theta_1+\theta_2) (\dot{\theta_1} + \dot{\theta_2})^2 \\ \ \ \ \ \ \ \ \ \ +cos(\theta_1+\theta_2) (\ddot{\theta_1} + \ddot{\theta_2}) ]\\ \tag {16} \end{cases}
      令:
    {d1=ax+L1c1θ1˙2+L2c12(θ1˙+θ2˙)2d2=ay+L1s1θ1˙2+L2s12(θ1˙+θ2˙)2(17) \begin{cases} d_1=a_x+L_1c_1\dot{\theta_1}^2+L_2c_{12}(\dot{\theta_1} + \dot{\theta_2})^2 \\ d_2=a_y +L_1s_1\dot{\theta_1}^2+L_2s_{12}(\dot{\theta_1} + \dot{\theta_2})^2\\ \tag {17} \end{cases}

      式(16)转化为关于θ1¨\ddot{\theta_1}θ2¨\ddot{\theta_2}的二元一次方程组:
    {a11θ1¨+a12θ2¨=d1a21θ1¨+a22θ2¨=d2(18) \begin{cases} a_{11}\ddot{\theta_1}+a_{12}\ddot{\theta_2}=d_1 \\ a_{21}\ddot{\theta_1}+a_{22}\ddot{\theta_2}=d_2 \\ \tag {18} \end{cases}

      上式解得:
    {θ1¨=(a12d2a22d1)/(a11a22a12a21)θ2¨=(a11d2a21d1)/(a11a22a12a21)(19) \begin{cases} \ddot{\theta_1}=-(a_{12}d_2 - a_{22}d_1)/(a_{11}a_{22} - a_{12}a_{21}) \\ \ddot{\theta_2}=(a_{11}d_2 - a_{21}d_1)/(a_{11}a_{22} - a_{12}a_{21}) \\ \tag {19} \end{cases}

    二、雅克比矩阵验证

    1、点到点运动

      (1)在[π,π][-\pi, \pi]中随机生成4个关节角度,生成点到点运动起点与终点(模拟示教过程)
      (2)速度规划(详见另一篇博文:归一化5次多项式速度规划)
      (3)ptp插补得到关节位置、速度、加速度
      (4)运动学正解得到笛卡尔位置(x,y,z,c),并作差分运算,估计笛卡尔分轴速度、加速度
      (5)利用雅克比矩阵计算笛卡尔分轴速度、加速度
      (6)绘图比较(4)与(5)的差异,验证雅克比矩阵正确性

    2、直线运动

      (1)给定直线运动起点与终点,手系handcoor,标志位flagJ1和flagJ2(模拟示教过程)
      (2)速度规划(详见另一篇博文:归一化5次多项式速度规划)
      (3)直线插补得到笛卡尔位置、速度、加速度
      (4)运动学逆解得到关节位置,并作差分运算,估计关节速度、加速度
      (5)利用雅克比矩阵计算关节速度、加速度
      (6)绘图比较(4)与(5)的差异,验证雅克比矩阵正确性

    三、MATLAB代码

    %{
    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.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
    
    clc;
    clear;
    close all;
    syms L1 L2 theta1 theta2 s real
    syms dtheta1 dtheta2 dtheta3 dtheta4 ddtheta1 ddtheta2 ddtheta3 ddtheta4 real
    syms vx vy vz vc ax ay az ac real;
    
    J = [-L1*sin(theta1) - L2*sin(theta1 + theta2),  -L2*sin(theta1 + theta2), 0, 0
        L1*cos(theta1) + L2*cos(theta1 + theta2),  L2*cos(theta1 + theta2), 0, 0
        0, 0, s/(2*pi), 0
        1, 1, 0, 1];
    detJ = simplify(det(J))
    
    dJ = [-L1*cos(theta1)*dtheta1 - L2*cos(theta1 + theta2)*(dtheta1 + dtheta2), -L2*cos(theta1 + theta2)*(dtheta1 + dtheta2), 0, 0
        -L1*sin(theta1)*dtheta1 - L2*sin(theta1 + theta2)*(dtheta1 + dtheta2), -L2*sin(theta1 + theta2)*(dtheta1 + dtheta2), 0, 0
        0, 0, 0, 0
        0, 0, 0, 0];
    
    cartesianVel = simplify(J * [dtheta1; dtheta2; dtheta3; dtheta4])
    cartesianAcc = simplify(dJ * [dtheta1; dtheta2; dtheta3; dtheta4] + J * [ddtheta1; ddtheta2; ddtheta3; ddtheta4])
    jointVel = simplify(J \ [vx; vy; vz; vc])
    jointAcc = simplify(J \ ([ax; ay; az; ac] - dJ * jointVel))
    
    %% 输入参数
    len1 = 200; %mm
    len2 = 200; %mm
    screw = 20; %mm
    speedJ = 200; %°/s
    accJ = 1200; %°/s^2
    linearSpeed = 100; %mm/s
    linearAcc = 800;   %mm/s^2
    orientationSpeed = 200; %°/s
    orientationAcc = 600;   %°/s^2
    dt = 0.001; %s
    
    %% 点到点运动, 验证雅克比矩阵正确性
    %(1)在[-pi, pi]中随机生成4个关节角度,生成点到点运动起点与终点(模拟示教过程)
    %(2)速度规划
    %(3)ptp插补得到关节位置、速度、加速度
    %(4)运动学正解得到笛卡尔位置(x,y,z,c),并作差分运算,估计笛卡尔分轴速度、加速度
    %(5)利用雅克比矩阵计算笛卡尔分轴速度、加速度
    %(6)绘图比较(4)与(5)的差异,验证雅克比矩阵正确性
    theta = (-pi + 2.0*pi*rand(2, 4))*180.0/pi;
    L = max(abs(diff(theta)));
    T = max([1.875 * L / speedJ, sqrt(5.7735 * L / accJ)]);
    
    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;
    rate = diff(theta) / L;
    jointPos = zeros(length(t), 4);
    jointVel = zeros(length(t), 4);
    jointAcc = zeros(length(t), 4);
    cartesianPos = zeros(length(t), 4);
    cartesianVel = zeros(length(t), 4);
    cartesianAcc = zeros(length(t), 4);
    for i = 1 : length(t)
        jointPos(i, :) = (theta(1, :) + len(i) * rate)*pi/180.0;
        jointVel(i, :) = (vel(i) * rate)*pi/180.0;
        jointAcc(i, :) = (acc(i) * rate)*pi/180.0;
        cartesianPos(i, :) = scara_forward_kinematics(len1, len2, screw, jointPos(i, :));
        cartesianPos(i, 4) = cartesianPos(i, 4)*180.0/pi;
        
        theta1 = jointPos(i, 1);
        theta2 = jointPos(i, 2);
        J = [-len1*sin(theta1) - len2*sin(theta1 + theta2),  -len2*sin(theta1 + theta2), 0, 0
            len1*cos(theta1) + len2*cos(theta1 + theta2),  len2*cos(theta1 + theta2), 0, 0
            0, 0, screw/(2*pi), 0
            1, 1, 0, 1];
        
        dtheta1 = jointVel(i, 1);
        dtheta2 = jointVel(i, 2);
        dtheta3 = jointVel(i, 3);
        dtheta4 = jointVel(i, 4);
        ddtheta1 = jointAcc(i, 1);
        ddtheta2 = jointAcc(i, 2);
        ddtheta3 = jointAcc(i, 3);
        ddtheta4 = jointAcc(i, 4);
        
        dJ = [-len1*cos(theta1)*dtheta1 - len2*cos(theta1 + theta2)*(dtheta1 + dtheta2), -len2*cos(theta1 + theta2)*(dtheta1 + dtheta2), 0, 0
            -len1*sin(theta1)*dtheta1 - len2*sin(theta1 + theta2)*(dtheta1 + dtheta2), -len2*sin(theta1 + theta2)*(dtheta1 + dtheta2), 0, 0
            0, 0, 0, 0
            0, 0, 0, 0];
        
        cartesianVel(i, :) = (J * [dtheta1; dtheta2; dtheta3; dtheta4])';
        cartesianAcc(i, :) = (dJ * [dtheta1; dtheta2; dtheta3; dtheta4] + J * [ddtheta1; ddtheta2; ddtheta3; ddtheta4])';
        cartesianVel(i, 4) = cartesianVel(i, 4)*180.0/pi;
        cartesianAcc(i, 4) = cartesianAcc(i, 4)*180.0/pi;
    end
    cartesianVel2 = diff(cartesianPos) ./ diff(t);
    cartesianAcc2 = diff(cartesianVel2) ./ diff(t(2:end));
    
    figure(1)
    set(gcf, 'color','w');
    subplot(2,2,1)
    plot(t, cartesianVel(:, 1), '--r')
    hold on
    plot(t(2:end), cartesianVel2(:, 1), 'b')
    xlabel('t/s');
    ylabel('v_x/ mm/s');
    subplot(2,2,2)
    plot(t, cartesianVel(:, 2), '--r')
    hold on
    plot(t(2:end), cartesianVel2(:, 2), 'b')
    xlabel('t/s');
    ylabel('v_y/ mm/s');
    legend('通过雅克比矩阵换算','通过差分换算');
    subplot(2,2,3)
    plot(t, cartesianVel(:, 3), '--r')
    hold on
    plot(t(2:end), cartesianVel2(:, 3), 'b')
    xlabel('t/s');
    ylabel('v_z/ mm/s');
    subplot(2,2,4)
    plot(t, cartesianVel(:, 4), '--r')
    hold on
    plot(t(2:end), cartesianVel2(:, 4), 'b')
    xlabel('t/s');
    ylabel('v_c/ °/s');
    
    figure(2)
    set(gcf, 'color','w');
    subplot(2,2,1)
    plot(t, cartesianAcc(:, 1), '--r')
    hold on
    plot(t(3:end), cartesianAcc2(:, 1), 'b')
    xlabel('t/s');
    ylabel('a_x/ mm/s^2');
    subplot(2,2,2)
    plot(t, cartesianAcc(:, 2), '--r')
    hold on
    plot(t(3:end), cartesianAcc2(:, 2), 'b')
    xlabel('t/s');
    ylabel('a_y/ mm/s^2');
    legend('通过雅克比矩阵换算','通过差分换算');
    subplot(2,2,3)
    plot(t, cartesianAcc(:, 3), '--r')
    hold on
    plot(t(3:end), cartesianAcc2(:, 3), 'b')
    xlabel('t/s');
    ylabel('a_z/ mm/s^2');
    subplot(2,2,4)
    plot(t, cartesianAcc(:, 4), '--r')
    hold on
    plot(t(3:end), cartesianAcc2(:, 4), 'b')
    xlabel('t/s');
    ylabel('a_c/ °/s^2');
    
    %% 直线运动, 验证雅克比矩阵正确性
    %(1)给定直线运动起点与终点,手系handcoor,标志位flagJ1和flagJ2(模拟示教过程)
    %(2)速度规划
    %(3)直线插补得到笛卡尔位置、速度、加速度
    %(4)运动学逆解得到关节位置,并作差分运算,估计关节速度、加速度
    %(5)利用雅克比矩阵计算关节速度、加速度
    %(6)绘图比较(4)与(5)的差异,验证雅克比矩阵正确性
    handcoor = 0;
    flagJ1 = 0;
    flagJ2 = 0;
    pos = [200 100 0 60
        250 200 20 80]; %mm °
    x1 = pos(1, 1);
    y1 = pos(1, 2);
    z1 = pos(1, 3);
    cc1 = pos(1, 4);
    x2 = pos(2, 1);
    y2 = pos(2, 2);
    z2 = pos(2, 3);
    cc2 = pos(2, 4);
    
    L = sqrt((x2 - x1)^2 + (y2 - y1)^2 + (z2 - z1)^2);
    C = abs(cc2 - cc1);
    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;
    cartesianPos = zeros(length(t), 4);
    cartesianVel = zeros(length(t), 4);
    cartesianAcc = zeros(length(t), 4);
    jointPos = zeros(length(t), 4);
    jointVel = zeros(length(t), 4);
    jointAcc = zeros(length(t), 4);
    rate = [x2 - x1, y2 - y1, z2 - z1, cc2 - cc1] / L;
    for i = 1 : length(t)
        
        cartesianPos(i, :) = [x1, y1, z1, cc1] + len(i) * rate;
        cartesianVel(i, :) = vel(i) * rate;
        cartesianAcc(i, :) = acc(i) * rate;
        
        cartesianPos(i, 4) = cartesianPos(i, 4)*pi/180.0;
        jointPos(i, :) = scara_inverse_kinematics(len1, len2, screw, cartesianPos(i, :), handcoor, flagJ1, flagJ2);
        
        theta1 = jointPos(i, 1);
        theta2 = jointPos(i, 2);
        s1 = sin(theta1);
        s12 = sin(theta1 + theta2);
        c1 = cos(theta1);
        c12 = cos(theta1 + theta2);
        
        vx = cartesianVel(i, 1);
        vy = cartesianVel(i, 2);
        vz = cartesianVel(i, 3);
        vc = cartesianVel(i, 4)*pi/180.0;
        
        a12 = -len2*s12;
        a11 = -len1*s1 + a12;
        b1 = vx;
        a22 = len2*c12;
        a21 = len1*c1 + a22;
        b2 = vy;
        temp = a11*a22 - a12*a21;
        
        dtheta1 = -(a12*b2 - a22*b1)/temp;
        dtheta2 = (a11*b2 - a21*b1)/temp;
        dtheta3 = 2*pi*vz/screw;
        dtheta4 = vc - dtheta1 - dtheta2;
        jointVel(i, :) = [dtheta1, dtheta2, dtheta3, dtheta4];
        
        ax = cartesianAcc(i, 1);
        ay = cartesianAcc(i, 2);
        az = cartesianAcc(i, 3);
        ac = cartesianAcc(i, 4)*pi/180.0;
        
        d1 = ax + len1*c1*dtheta1^2 + len2*c12*(dtheta1 + dtheta2)^2;
        d2 = ay + len1*s1*dtheta1^2 + len2*s12*(dtheta1 + dtheta2)^2;
        ddtheta1 = -(a12*d2 - a22*d1)/temp;
        ddtheta2 = (a11*d2 - a21*d1)/temp;
        ddtheta3 = 2*pi*az/screw;
        ddtheta4 = ac - ddtheta1 - ddtheta2;
        
        jointAcc(i, :) = [ddtheta1, ddtheta2, ddtheta3, ddtheta4];
    end
    jointVel2 = diff(jointPos) ./ diff(t);
    jointAcc2 = diff(jointVel2) ./ diff(t(2:end));
    
    figure(3)
    set(gcf, 'color','w');
    subplot(2,2,1)
    plot(t, jointVel(:, 1)*180.0/pi, '--r')
    hold on
    plot(t(2:end), jointVel2(:, 1)*180.0/pi, 'b')
    xlabel('t/s');
    ylabel('vj1/ °/s');
    subplot(2,2,2)
    plot(t, jointVel(:, 2)*180.0/pi, '--r')
    hold on
    plot(t(2:end), jointVel2(:, 2)*180.0/pi, 'b')
    xlabel('t/s');
    ylabel('vj2/ °/s');
    legend('通过雅克比矩阵换算','通过差分换算');
    subplot(2,2,3)
    plot(t, jointVel(:, 3)*180.0/pi, '--r')
    hold on
    plot(t(2:end), jointVel2(:, 3)*180.0/pi, 'b')
    xlabel('t/s');
    ylabel('vj3/ °/s');
    subplot(2,2,4)
    plot(t, jointVel(:, 4)*180.0/pi, '--r')
    hold on
    plot(t(2:end), jointVel2(:, 4)*180.0/pi, 'b')
    xlabel('t/s');
    ylabel('vj4/ °/s');
    
    figure(4)
    set(gcf, 'color','w');
    subplot(2,2,1)
    plot(t, jointAcc(:, 1), '--r')
    hold on
    plot(t(3:end), jointAcc2(:, 1), 'b')
    xlabel('t/s');
    ylabel('aj1/ °/s^2');
    subplot(2,2,2)
    plot(t, jointAcc(:, 2), '--r')
    hold on
    plot(t(3:end), jointAcc2(:, 2), 'b')
    xlabel('t/s');
    ylabel('aj2/ °/s^2');
    legend('通过雅克比矩阵换算','通过差分换算');
    subplot(2,2,3)
    plot(t, jointAcc(:, 3), '--r')
    hold on
    plot(t(3:end), jointAcc2(:, 3), 'b')
    xlabel('t/s');
    ylabel('aj3/ °/s^2');
    subplot(2,2,4)
    plot(t, jointAcc(:, 4), '--r')
    hold on
    plot(t(3:end), jointAcc2(:, 4), 'b')
    xlabel('t/s');
    ylabel('aj4/ °/s^2');
    

    1
    2
    3
    4

    展开全文
  • 绘制scara机器人工作空间

    千次阅读 2020-03-17 22:45:52
    文章目录一、绘制scara机器人工作空间二、MATLAB代码 一、绘制scara机器人工作空间   如上图,scara机器人大臂长L1L_1L1​,小臂长L2L_2L2​,θ1\theta_1θ1​为关节1角度,θ2\theta_2θ2​为关节2角度,且θ1...

    一、绘制scara机器人工作空间

    scara机器人运动学正解
      如上图,scara机器人大臂长L1L_1,小臂长L2L_2θ1\theta_1为关节1角度,θ2\theta_2为关节2角度,且θ1[θL1,θU1],θ2[θL2,θU2]\theta_1\in[\theta_{L1},\theta_{U1}],\theta_2\in[\theta_{L2},\theta_{U2}]
    在这里插入图片描述
      scara机器人工作空间由四段圆弧组成(如上图),圆弧方程如下:
    {x2+y2=(L1+L2)2x2+y2=L12+L22+2L1L2cosθ(xL1cosθL1)2+(yL1sinθL1)2=L22(xL1cosθU1)2+(yL1sinθU1)2=L22(1) \begin{cases} x^2+y^2=(L_1+L_2)^2 \\ x^2+y^2=L_1^2+L_2^2+2L_1L_2cos\theta \\ (x-L_1cos\theta_{L1})^2+(y-L_1sin\theta_{L1})^2=L_2^2 \\ (x-L_1cos\theta_{U1})^2+(y-L_1sin\theta_{U1})^2=L_2^2 \tag 1 \end{cases}
      其中,左手系时,θ=θL2\theta=\theta_{L2};右手系时,θ=θU2\theta=\theta_{U2}

    二、MATLAB代码

      绘制2D圆弧:

    %{
    Function: draw_2d_arc
    Description: 绘制平面圆弧
    Input: 圆弧圆心(x0, y0),半径r,起始角度theta1(rad),结束角度theta2(rad), 曲线样式选择options
    Output: 无
    Author: Marc Pony(marc_pony@163.com)
    %}
    function draw_2d_arc(x0, y0, r, theta1, theta2, options)
    deltaTheta = 0.1 * pi / 180;
    theta = theta1 : deltaTheta : theta2;
    x = x0 + r * cos(theta);
    y = y0 + r * sin(theta);
    plot(x, y, 'LineStyle', options.LineStyle, 'Color', options.Color, 'LineWidth', options.LineWidth);
    axis equal;
    end
    

      绘制scara机器人工作空间:

    %{
    Function: draw_scara_workspace
    Description: 绘制scara机器人工作空间
    Input: 大臂L1,小臂L2,关节1限位角度thetaLimit1(rad),关节2限位角度thetaLimit2(rad),手系handcoor
    Output: 无
    Author: Marc Pony(marc_pony@163.com)
    %}
    function draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
    thetaL1 = thetaLimit1(1);
    thetaU1 = thetaLimit1(2);
    thetaL2 = thetaLimit2(1);
    thetaU2 = thetaLimit2(2);
    
    hold on;
    if(handcoor == 1) %right handcoor
        options.LineStyle = '-';
        options.Color='g';
        options.LineWidth = 3;
        
        x0 = 0;
        y0 = 0;
        r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
        alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
        thetaStart = thetaL1 + alpha;
        thetaEnd = thetaU1 + alpha;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = 0;
        y0 = 0;
        r = L1 + L2;
        thetaStart = thetaL1;
        thetaEnd = thetaU1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = L1 * cos(thetaU1);
        y0 = L1 * sin(thetaU1);
        r = L2;
        thetaStart = thetaU1;
        thetaEnd = thetaU1 + thetaU2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        x0 = L1 * cos(thetaL1);
        y0 = L1 * sin(thetaL1);
        r = L2;
        thetaStart = thetaL1;
        thetaEnd = thetaL1 + thetaU2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        title('Workspace in right handcoor', 'fontsize', 16);
    else  %left handcoor
        options.LineStyle = '-';
        options.Color='b';
        options.LineWidth = 3;
        
        x0 = 0;
        y0 = 0;
        r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
        alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
        thetaStart = thetaL1 - alpha;
        thetaEnd = thetaU1 - alpha;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = 0;
        y0 = 0;
        r = L1 + L2;
        thetaStart = thetaL1;
        thetaEnd = thetaU1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = L1 * cos(thetaU1);
        y0 = L1 * sin(thetaU1);
        r = L2;
        thetaStart = thetaU1 + thetaL2;
        thetaEnd = thetaU1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        x0 = L1 * cos(thetaL1);
        y0 = L1 * sin(thetaL1);
        r = L2;
        thetaStart = thetaL1 + thetaL2;
        thetaEnd = thetaL1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        title('Workspace in left handcoor', 'fontsize', 16);
    end
    set(gcf, 'color', 'w');
    axis off;
    end
    

      绘制scara机器人工作空间草图:

    %{
    Function: draw_scara_workspace_sketch
    Description: 绘制scara机器人工作空间草图
    Input: 大臂L1,小臂L2,关节1限位角度thetaLimit1(rad),关节2限位角度thetaLimit2(rad),手系handcoor
    Output: 无
    Author: Marc Pony(marc_pony@163.com)
    %}
    function draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
    
    thetaL1 = thetaLimit1(1);
    thetaU1 = thetaLimit1(2);
    thetaL2 = thetaLimit2(1);
    thetaU2 = thetaLimit2(2);
    
    hold on;
    if(handcoor == 1) %right handcoor
        options.LineStyle = '-';
        options.Color='g';
        options.LineWidth = 3;
        
        x0 = 0;
        y0 = 0;
        r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
        alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
        thetaStart = thetaL1 + alpha;
        thetaEnd = thetaU1 + alpha;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = 0;
        y0 = 0;
        r = L1 + L2;
        thetaStart = thetaL1;
        thetaEnd = thetaU1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = L1 * cos(thetaU1);
        y0 = L1 * sin(thetaU1);
        r = L2;
        thetaStart = thetaU1;
        thetaEnd = thetaU1 + thetaU2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        x0 = L1 * cos(thetaL1);
        y0 = L1 * sin(thetaL1);
        r = L2;
        thetaStart = thetaL1;
        thetaEnd = thetaL1 + thetaU2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        %-------------
        options.LineStyle = '--';
        options.Color='r';
        options.LineWidth = 0.5;
        
        x0 = 0;
        y0 = 0;
        r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
        thetaStart = 0;
        thetaEnd = 2 * pi;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        r = L1 + L2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = L1 * cos(thetaU1);
        y0 = L1 * sin(thetaU1);
        r = L2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        x0 = L1 * cos(thetaL1);
        y0 = L1 * sin(thetaL1);
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        xA1 = L1 * cos(thetaL1);
        yA1 = L1 * sin(thetaL1);
        xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
        yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
        xA2 = L1 * cos(thetaU1);
        yA2 = L1 * sin(thetaU1);
        xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
        yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
        xC1 = (L1 + L2) * cos(thetaL1);
        yC1 = (L1 + L2) * sin(thetaL1);
        xC2 = (L1 + L2) * cos(thetaU1);
        yC2 = (L1 + L2) * sin(thetaU1);
        
        plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
        plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
        
        fontsize = 15;
        delta = 25;
        text(0, 0, 'O', 'Fontsize', fontsize);
        text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
        text(xB1, yB1 - delta, 'B_1', 'fontsize', fontsize);
        text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
        text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
        text(xC1, yC1, 'C_1', 'fontsize', fontsize);
        text(xC2, yC2, 'C_2', 'fontsize', fontsize);
        title('Workspace sketch in right handcoor', 'fontsize', 16);
        
    else  %left handcoor
        options.LineStyle = '-';
        options.Color='b';
        options.LineWidth = 3;
        
        x0 = 0;
        y0 = 0;
        r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
        alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
        thetaStart = thetaL1 - alpha;
        thetaEnd = thetaU1 - alpha;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = 0;
        y0 = 0;
        r = L1 + L2;
        thetaStart = thetaL1;
        thetaEnd = thetaU1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = L1 * cos(thetaU1);
        y0 = L1 * sin(thetaU1);
        r = L2;
        thetaStart = thetaU1 + thetaL2;
        thetaEnd = thetaU1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        x0 = L1 * cos(thetaL1);
        y0 = L1 * sin(thetaL1);
        r = L2;
        thetaStart = thetaL1 + thetaL2;
        thetaEnd = thetaL1;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        %-------------
        options.LineStyle = '--';
        options.Color='r';
        options.LineWidth = 0.5;
        
        x0 = 0;
        y0 = 0;
        r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
        thetaStart = 0;
        thetaEnd = 2 * pi;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        r = L1 + L2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
        
        x0 = L1 * cos(thetaU1);
        y0 = L1 * sin(thetaU1);
        r = L2;
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        x0 = L1 * cos(thetaL1);
        y0 = L1 * sin(thetaL1);
        draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
        
        xA1 = L1 * cos(thetaL1);
        yA1 = L1 * sin(thetaL1);
        xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
        yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
        xA2 = L1 * cos(thetaU1);
        yA2 = L1 * sin(thetaU1);
        xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
        yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
        xC1 = (L1 + L2) * cos(thetaL1);
        yC1 = (L1 + L2) * sin(thetaL1);
        xC2 = (L1 + L2) * cos(thetaU1);
        yC2 = (L1 + L2) * sin(thetaU1);
        
        plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
        plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
        
        fontsize = 15;
        delta = 25;
        text(0, 0, 'O', 'fontsize', fontsize);
        text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
        text(xB1, yB1 + delta, 'B_1', 'fontsize', fontsize);
        text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
        text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
        text(xC1, yC1, 'C_1', 'fontsize', fontsize);
        text(xC2, yC2, 'C_2', 'fontsize', fontsize);
        title('Workspace sketch in left handcoor', 'fontsize', 16);
    end
    set(gcf, 'color', 'w');
    axis off;
    end
    
    clc;
    clear;
    close all;
    L1 = 200;
    L2 = 200;
    thetaLimit1 = [-135, 135] * pi / 180;
    thetaLimit2 = [-145, 145] * pi / 180;
    
    %% 画工作空间
    figure(1);
    handcoor = 0;
    draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
    figure(2);
    handcoor = 1;
    draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
    
    %% 画工作空间草图
    figure(3);
    handcoor = 0;
    draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
    figure(4);
    handcoor = 1;
    draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
    
    展开全文

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 546
精华内容 218
关键字:

scara机器人