精华内容
下载资源
问答
  • 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
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    展开全文
  • 六轴机器人matlab运动学逆解函数(改进DH模型)

    万次阅读 多人点赞 2018-06-23 17:04:53
    本文采用的模型为之前博客“matlab机器人工具箱一般六轴的DH模型和改进DH模型建立与区别”里面的改进DH模型,参数不再重复给出。 基系与工具坐标系关系为: bT0⋅(0T1⋅1T2⋅2T3⋅3T4⋅4T5⋅5T6)⋅6Te=bTebT0⋅...

    1.理论
    本文采用的模型为之前博客“matlab机器人工具箱一般六轴的DH模型和改进DH模型建立与区别”里面的改进DH模型,参数不再重复给出。
    基系与工具坐标系关系为:
    bT00T11T22T33T44T55T66Te=bTe
    将逆运动学问题简化为:
    0T11T22T33T44T55T6=bT01bTe6Te1
    2.转换为下式求解
    2T33T44T5=1T210T11bT01bTe6Te15T61
    左边:

    left =
    [ cos(theta3)*cos(theta4)*cos(theta5) - sin(theta3)*sin(theta5), - cos(theta5)*sin(theta3) - cos(theta3)*cos(theta4)*sin(theta5), cos(theta3)*sin(theta4), a2 + a3*cos(theta3) - d4*sin(theta3)]
    [ cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3),   cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5), sin(theta3)*sin(theta4),      d4*cos(theta3) + a3*sin(theta3)]
    [                                      -cos(theta5)*sin(theta4),                                         sin(theta4)*sin(theta5),             cos(theta4),                                    0]
    [                                                             0,                                                               0,                       0,                                    1]

    右边:

    right = 
    [ oz*sin(theta2)*sin(theta6) - nz*cos(theta6)*sin(theta2) + nx*cos(theta1)*cos(theta2)*cos(theta6) + ny*cos(theta2)*cos(theta6)*sin(theta1) - ox*cos(theta1)*cos(theta2)*sin(theta6) - oy*cos(theta2)*sin(theta1)*sin(theta6),   ax*cos(theta1)*cos(theta2) - az*sin(theta2) + ay*cos(theta2)*sin(theta1), oz*cos(theta6)*sin(theta2) + nz*sin(theta2)*sin(theta6) - ox*cos(theta1)*cos(theta2)*cos(theta6) - nx*cos(theta1)*cos(theta2)*sin(theta6) - oy*cos(theta2)*cos(theta6)*sin(theta1) - ny*cos(theta2)*sin(theta1)*sin(theta6), px*cos(theta1)*cos(theta2) - pz*sin(theta2) - a1*cos(theta2) + py*cos(theta2)*sin(theta1)]
    [ oz*cos(theta2)*sin(theta6) - nz*cos(theta2)*cos(theta6) - nx*cos(theta1)*cos(theta6)*sin(theta2) - ny*cos(theta6)*sin(theta1)*sin(theta2) + ox*cos(theta1)*sin(theta2)*sin(theta6) + oy*sin(theta1)*sin(theta2)*sin(theta6), - az*cos(theta2) - ax*cos(theta1)*sin(theta2) - ay*sin(theta1)*sin(theta2), oz*cos(theta2)*cos(theta6) + nz*cos(theta2)*sin(theta6) + ox*cos(theta1)*cos(theta6)*sin(theta2) + nx*cos(theta1)*sin(theta2)*sin(theta6) + oy*cos(theta6)*sin(theta1)*sin(theta2) + ny*sin(theta1)*sin(theta2)*sin(theta6), a1*sin(theta2) - pz*cos(theta2) - px*cos(theta1)*sin(theta2) - py*sin(theta1)*sin(theta2)]
    [                                                                                                           ny*cos(theta1)*cos(theta6) - nx*cos(theta6)*sin(theta1) - oy*cos(theta1)*sin(theta6) + ox*sin(theta1)*sin(theta6),                                            ay*cos(theta1) - ax*sin(theta1),                                                                                                           ox*cos(theta6)*sin(theta1) - ny*cos(theta1)*sin(theta6) - oy*cos(theta1)*cos(theta6) + nx*sin(theta1)*sin(theta6),                                                           py*cos(theta1) - px*sin(theta1)]
    [                                                                                                                                                                                                                           0,                                                                          0,                                                                                                                                                                                                                           0,                                                                                         1]
    

    令左右两边相等,求出六个角。、
    逆解顺序如下:
    这里写图片描述
    由于公式比较复杂我仅在此给出想法,各位可自行计算。
    θ1:两式第3行第4列。
    θ3:两式第1行第4列和第2行第4列。
    θ2:两式第1行第4列和第2行第4列。
    θ5:两式第1行第2列和第2行第2列。
    θ4:两式第1行第2列和第2行第2列。
    θ6:两式第3行第1列和第3行第3列。
    3.MATLAB程序
    分两个程序①主函数②function函数
    ①主函数

    clear;
    clc;
    %建立机器人模型
    %       theta    d           a        alpha     offset
    ML1=Link([0       0           0           0          0     ],'modified'); 
    ML2=Link([0       0           0.180      -pi/2       0     ],'modified');
    ML3=Link([0       0           0.600       0          0     ],'modified');
    ML4=Link([0       0.630       0.130      -pi/2       0     ],'modified');
    ML5=Link([0       0           0           pi/2       0     ],'modified');
    ML6=Link([0       0           0          -pi/2       0     ],'modified');
    modrobot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','modified');
    modmyt06=mymodfkine(-pi/3,-pi/3,pi/3,-pi/4,pi/4,pi/6);
    modmyikine=mymodikine(modmyt06)

    ②function函数

    function [ikine_t]=mymodikine(Tbe)
    %    ti   di     ai-1     alphai-1 
    MDH=[0   0       0         0;
         0   0       0.180    -pi/2;
         0   0       0.600     0;
         0   0.630   0.130    -pi/2;
         0   0       0         pi/2;
         0   0       0        -pi/2];
     nx=Tbe(1,1); ny=Tbe(2,1); nz=Tbe(3,1);  ox=Tbe(1,2); oy=Tbe(2,2); oz=Tbe(3,2); ax=Tbe(1,3); ay=Tbe(2,3); az=Tbe(3,3); px=Tbe(1,4); py=Tbe(2,4); pz=Tbe(3,4);
     d4=MDH(4,2);a1=MDH(2,3);a2=MDH(3,3);a3=MDH(4,3);d2=0;d3=0;f1=-pi/2;f3=-pi/2;f4=pi/2;f5=-pi/2;
     %t1(3,4)
     t11=-atan2(-py,px)+atan2((d2-d3)/sin(f1),((px*sin(f1))^2+(py*sin(f1))^2-(d2-d3)^2)^0.5);
     t12=-atan2(-py,px)+atan2((d2-d3)/sin(f1),-((px*sin(f1))^2+(py*sin(f1))^2-(d2-d3)^2)^0.5);
     %t3
     m3_1=pz*sin(f1);
     n3_1=a1-px*cos(t11)-py*sin(t11);
     m3_2=pz*sin(f1);
     n3_2=a1-px*cos(t12)-py*sin(t12);
     t31=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_1^2+n3_1^2-a2^2-a3^2-d4^2)/sin(f3),((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_1^2+n3_1^2-a2^2-a3^2-d4^2)^2)^0.5);
     t32=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_1^2+n3_1^2-a2^2-a3^2-d4^2)/sin(f3),-((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_1^2+n3_1^2-a2^2-a3^2-d4^2)^2)^0.5);
     t33=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_2^2+n3_2^2-a2^2-a3^2-d4^2)/sin(f3),((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_2^2+n3_2^2-a2^2-a3^2-d4^2)^2)^0.5);
     t34=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_2^2+n3_2^2-a2^2-a3^2-d4^2)/sin(f3),-((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_2^2+n3_2^2-a2^2-a3^2-d4^2)^2)^0.5);
     %t2
     m2_1=a2+a3*cos(t31)+d4*sin(f3)*sin(t31);
     n2_1=a3*sin(t31)-d4*sin(f3)*cos(t31);
     m2_2=a2+a3*cos(t32)+d4*sin(f3)*sin(t32);
     n2_2=a3*sin(t32)-d4*sin(f3)*cos(t32);
     m2_3=a2+a3*cos(t33)+d4*sin(f3)*sin(t33);
     n2_3=a3*sin(t33)-d4*sin(f3)*cos(t33);
     m2_4=a2+a3*cos(t34)+d4*sin(f3)*sin(t34);
     n2_4=a3*sin(t34)-d4*sin(f3)*cos(t34);
     t21=atan2(m3_1*m2_1+n2_1*n3_1,m3_1*n2_1-m2_1*n3_1);
     t22=atan2(m3_1*m2_2+n2_2*n3_1,m3_1*n2_2-m2_2*n3_1);
     t23=atan2(m3_2*m2_3+n2_3*n3_2,m3_2*n2_3-m2_3*n3_2);
     t24=atan2(m3_2*m2_4+n2_4*n3_2,m3_2*n2_4-m2_4*n3_2);
     %t5
     m5_1=-sin(f5)*(ax*cos(t11)*cos(t21)+ay*sin(t11)*cos(t21)+az*sin(f1)*sin(t21));
     n5_1=sin(f5)*(ax*cos(t11)*sin(t21)+ay*sin(t11)*sin(t21)-az*sin(f1)*cos(t21));
     m5_2=-sin(f5)*(ax*cos(t11)*cos(t22)+ay*sin(t11)*cos(t22)+az*sin(f1)*sin(t22));
     n5_2=sin(f5)*(ax*cos(t11)*sin(t22)+ay*sin(t11)*sin(t22)-az*sin(f1)*cos(t22));
     m5_3=-sin(f5)*(ax*cos(t12)*cos(t23)+ay*sin(t12)*cos(t23)+az*sin(f1)*sin(t23));
     n5_3=sin(f5)*(ax*cos(t12)*sin(t23)+ay*sin(t12)*sin(t23)-az*sin(f1)*cos(t23));
     m5_4=-sin(f5)*(ax*cos(t12)*cos(t24)+ay*sin(t12)*cos(t24)+az*sin(f1)*sin(t24));
     n5_4=sin(f5)*(ax*cos(t12)*sin(t24)+ay*sin(t12)*sin(t24)-az*sin(f1)*cos(t24));
    
     t51=atan2(((ay*cos(t11)-ax*sin(t11))^2+(m5_1*cos(t31)+n5_1*sin(t31))^2)^0.5,(m5_1*sin(t31)-n5_1*cos(t31))/(sin(f3)*sin(f4)));
     t52=atan2(-((ay*cos(t11)-ax*sin(t11))^2+(m5_1*cos(t31)+n5_1*sin(t31))^2)^0.5,(m5_1*sin(t31)-n5_1*cos(t31))/(sin(f3)*sin(f4)));
     t53=atan2(((ay*cos(t11)-ax*sin(t11))^2+(m5_2*cos(t32)+n5_2*sin(t32))^2)^0.5,(m5_2*sin(t32)-n5_2*cos(t32))/(sin(f3)*sin(f4)));
     t54=atan2(-((ay*cos(t11)-ax*sin(t11))^2+(m5_2*cos(t32)+n5_2*sin(t32))^2)^0.5,(m5_2*sin(t32)-n5_2*cos(t32))/(sin(f3)*sin(f4)));
    
     t55=atan2(((ay*cos(t12)-ax*sin(t12))^2+(m5_3*cos(t33)+n5_3*sin(t33))^2)^0.5,(m5_3*sin(t33)-n5_3*cos(t33))/(sin(f3)*sin(f4)));
     t56=atan2(-((ay*cos(t12)-ax*sin(t12))^2+(m5_3*cos(t33)+n5_3*sin(t33))^2)^0.5,(m5_3*sin(t33)-n5_3*cos(t33))/(sin(f3)*sin(f4)));
     t57=atan2(((ay*cos(t12)-ax*sin(t12))^2+(m5_4*cos(t34)+n5_4*sin(t34))^2)^0.5,(m5_4*sin(t34)-n5_4*cos(t34))/(sin(f3)*sin(f4)));
     t58=atan2(-((ay*cos(t12)-ax*sin(t12))^2+(m5_4*cos(t34)+n5_4*sin(t34))^2)^0.5,(m5_4*sin(t34)-n5_4*cos(t34))/(sin(f3)*sin(f4)));
    
     %t4
     if sin(t51)==0
         t41=0;
     else 
         t41=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t51)*sin(f3)),(-m5_1*cos(t31)-n5_1*sin(t31))/(sin(t51)));
     end
     if sin(t52)==0 
         t42=0;
     else 
         t42=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t52)*sin(f3)),(-m5_1*cos(t31)-n5_1*sin(t31))/(sin(t52)));
     end
     if sin(t53)==0 
         t43=0;
     else 
         t43=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t53)*sin(f3)),(-m5_2*cos(t32)-n5_2*sin(t32))/(sin(t53)));
     end
     if sin(t54)==0 
         t44=0;
     else 
         t44=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t54)*sin(f3)),(-m5_2*cos(t32)-n5_2*sin(t32))/(sin(t54)));
     end
    
     if sin(t55)==0
         t45=0;
     else 
         t45=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t55)*sin(f3)),(-m5_3*cos(t33)-n5_3*sin(t33))/(sin(t55)));
     end
     if sin(t56)==0 
         t46=0;
     else 
         t46=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t56)*sin(f3)),(-m5_3*cos(t33)-n5_3*sin(t33))/(sin(t56)));
     end
     if sin(t57)==0 
         t47=0;
     else 
         t47=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t57)*sin(f3)),(-m5_4*cos(t34)-n5_4*sin(t34))/(sin(t57)));
     end
     if sin(t58)==0 
         t48=0;
     else 
         t48=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t58)*sin(f3)),(-m5_4*cos(t34)-n5_4*sin(t34))/(sin(t58)));
     end
     %t6
     e1=nx*sin(t11)-ny*cos(t11);
     f1=ox*sin(t11)-oy*cos(t11);
     t61=atan2((cos(t41)*e1-cos(t51)*sin(t41)*f1),(cos(t41)*f1+cos(t51)*sin(t41)*e1));
     t62=atan2((cos(t42)*e1-cos(t52)*sin(t42)*f1),(cos(t42)*f1+cos(t52)*sin(t42)*e1));
     t63=atan2((cos(t43)*e1-cos(t53)*sin(t43)*f1),(cos(t43)*f1+cos(t53)*sin(t43)*e1));
     t64=atan2((cos(t44)*e1-cos(t54)*sin(t44)*f1),(cos(t44)*f1+cos(t54)*sin(t44)*e1));
    
     e2=nx*sin(t12)-ny*cos(t12);
     f2=ox*sin(t12)-oy*cos(t12);
     t65=atan2((cos(t45)*e2-cos(t55)*sin(t45)*f2),(cos(t45)*f2+cos(t55)*sin(t45)*e2));
     t66=atan2((cos(t46)*e2-cos(t56)*sin(t46)*f2),(cos(t46)*f2+cos(t56)*sin(t46)*e2));
     t67=atan2((cos(t47)*e2-cos(t57)*sin(t47)*f2),(cos(t47)*f2+cos(t57)*sin(t47)*e2));
     t68=atan2((cos(t48)*e2-cos(t58)*sin(t48)*f2),(cos(t48)*f2+cos(t58)*sin(t48)*e2));
    
     ikine_t=[t11 t21 t31 t41 t51 t61;
              t11 t21 t31 t42 t52 t62;
              t11 t22 t32 t43 t53 t63;
              t11 t22 t32 t44 t54 t64
              t12 t23 t33 t45 t55 t65;
              t12 t23 t33 t46 t56 t66;
              t12 t24 t34 t47 t57 t67;
              t12 t24 t34 t48 t58 t68];

    4.运行结果
    给定关节角为:

    (-pi/3,-pi/3,pi/3,-pi/4,pi/4,pi/6)

    正解得到末端位姿:

    modmyt06

    这里写图片描述
    再用modmyt06去进行逆解,结果如下:
    这里写图片描述
    共有8组解,正解回去位姿符合。
    这里写图片描述
    这里写图片描述
    PS:逆解推荐用arctan反解角度,本程序适用于改进DH模型,仅用于学习,不适用于工业应用。

    展开全文
  • Delta Robot Kinematics 并联机器人 运动学 正解 逆解 本人自己总结 matlab亲测 正反解正确
  • MATLAB机器人逆解

    2020-12-13 23:14:33
    MATLAB机器人求正逆解 手把手教你MATLAB Robotics Toolbox工具箱③ Matlab RoboticToolBox(一)Link参数、三自由度/四自由度逆运动学 https://blog.csdn.net/qq_34917736/article/details/89048930

    MATLAB机器人求正逆解

    手把手教你MATLAB Robotics Toolbox工具箱③

    Matlab RoboticToolBox(一)Link参数、三自由度/四自由度逆运动学
    https://blog.csdn.net/qq_34917736/article/details/89048930

    展开全文
  • 包含了UR机器人运动学建模与运动学逆解的求解过程(解析法),通过实际的机器人参数验证该求解方法的正确性,分析了机器人的奇异位置,并编制好matlab程序便与仿真。
  • 基于Matlab三维三轴机器人求逆解(实现简单的定点测试) (机器人建模与仿真中的机器人逆运动和正运动) 其它关于基于Matlab机器人建模与仿真资料合集请往CSDN博客 “基于Matlab的机器人建模学习资料大整理”...
  • 机器人基础之运动学逆解

    千次阅读 2020-08-06 23:11:09
    机器人基础之运动学逆解概述求解腕点位置求解腕部方位*z-y-z*欧拉角具体求解算例MATLAB实现 概述 运动学逆解是指已知机器人末端位姿,求解各运动关节的位置,它是机器人运动规划和轨迹控制的基础。 以机械臂为例,其...

    概述

    运动学逆解是指已知机器人末端位姿,求解各运动关节的位置,它是机器人运动规划和轨迹控制的基础。
    以机械臂为例,其运动学逆解的求法主要有两类:数值解和解析解。
    数值解法只能求出方程的特解,不能求出所有的解。
    它的优点是计算简单,不需要进行矩阵操作;缺点是由于使用了迭代法(如牛顿迭代),实时性较差。
    这里主要介绍解析解法。
    研究发现,如果串联机械臂在结构上满足下面两个充分条件中的一个,就会有解析解。这两个充分条件也被称为Pieper准则,即:

    1. 三个相邻关节轴线交于一点
    2. 三个相邻关节轴线相互平行

    现代绝大多数工业机械臂都满足Pieper准则的第一个充分条件,即机械臂末端的三个关节的轴线相交于一点,该点通常称为腕点。
    满足第二个充分条件的机器人较少,最典型的例子是SCARA机器人。之前我所在的公司应用SCARA构型的机器人进行工件的上下料。
    假设机械臂满足Pieper准则的第一个充分条件,则机械臂的运动学方程可以写为60T=30T63T {^0_6}T={^0_3}T{^3_6}T其中, 30T\ {^0_3}T规定了腕点的位置, 63T\ {^3_6}T规定了腕部的方位。因此,运动学逆解也分为两步,先求解θ1\theta_1θ2\theta_2θ3\theta_3(腕点位置),再求解θ4\theta_4θ5\theta_5θ6\theta_6(腕部方位)。

    求解腕点位置

    将固连在机械臂腕部的三个坐标系{4}、{5}、{6}的原点设在腕点,则腕点相对于基坐标系{0}的位置为:0P6o=0P4o=10T21T32T3P4o {^0}P_6o= {^0}P_4o={^0_1}T{^1_2}T{^2_3}T {^3}P_4o其中, 3P4o\ {^3}P_4o即为变换矩阵 43T\ {^3_4}T的第4列,即0P4o=10T21T32T[a3d4sinθ3d4cosθ31](n) {^0}P_4o={^0_1}T{^1_2}T{^2_3}T \left[ \begin{matrix} a_3\\ -d_4 sin\theta_3\\ d_4 cos\theta_3\\ 1 \end{matrix} \right] \tag{n}[f1(θ3)f2(θ3)f3(θ3)1]=32T[a3d4sinθ3d4cosθ31](n) \left[ \begin{matrix} f_1(\theta_3) \\ f_2(\theta_3) \\ f_3(\theta_3) \\ 1 \end{matrix} \right] \tag{n}={^2_3}T \left[ \begin{matrix} a_3\\ -d_4 sin\theta_3\\ d_4 cos\theta_3\\ 1 \end{matrix} \right] 则有,f1(θ3)=a3cosθ3+d4sinθ3sinα3+a2f_1(\theta_3)=a_3 cos\theta_3 +d_4 sin\theta_3sin\alpha_3 +a_2 f2(θ3)=a3sinθ3cosα2d4cosθ3cosα2sinα3d4sinα2cosα3d3sinα2f_2(\theta_3)=a_3 sin\theta_3 cos\alpha_2-d_4 cos\theta_3 cos\alpha_2 sin\alpha_3 -d_4 sin\alpha_2 cos\alpha_3 -d_3 sin\alpha_2 f3(θ3)=a3sinθ3sinα2d4cosθ3sinα2sinα3+d4cosα2cosα3+d3cosα2f_3(\theta_3)=a_3 sin\theta_3 sin\alpha_2-d_4 cos\theta_3 sin\alpha_2 sin\alpha_3 +d_4 cos\alpha_2 cos\alpha_3 +d_3 cos\alpha_2 再令[g1(θ2,θ3)g2(θ2,θ3)g3(θ2,θ3)1]=21T[f1(θ3)f2(θ3)f3(θ3)1](n) \left[ \begin{matrix} g_1(\theta_2,\theta_3) \\ g_2(\theta_2,\theta_3) \\ g_3(\theta_2,\theta_3) \\ 1 \end{matrix} \right] \tag{n}={^1_2}T \left[ \begin{matrix} f_1(\theta_3) \\ f_2(\theta_3) \\ f_3(\theta_3) \\ 1 \end{matrix} \right] 则有,g1(θ2,θ3)=cosθ2f1sinθ2f2+a1g_1(\theta_2,\theta_3)=cos\theta_2 f_1-sin\theta_2 f_2 +a_1 g2(θ2,θ3)=sinθ2cosα1f1+cosθ2cosα1f2sinα1f3d2sinα1g_2(\theta_2,\theta_3)=sin\theta_2 cos\alpha_1f_1 + cos\theta_2 cos\alpha_1f_2 -sin\alpha_1 f_3 - d_2sin\alpha_1 g3(θ2,θ3)=sinθ2sinα1f1+cosθ2sinα1f2+cosα1f3+d2cosα1g_3(\theta_2,\theta_3)=sin\theta_2 sin\alpha_1f_1 + cos\theta_2 sin\alpha_1f_2 +cos\alpha_1 f_3 + d_2cos\alpha_1 因此 0P4o=10T[g1g2g31](n) {^0}P_4o={^0_1}T\left[ \begin{matrix} g_1\\ g_2\\ g_3\\ 1 \end{matrix} \right] \tag{n}由于坐标系{1}与基坐标系{0}原点重合、Z轴重合,仅存在绕Z轴的转动,因此,10T=[cosθ1sinθ100sinθ1cosθ10000100001] {^0_1}T= \left[ \begin{matrix} \cos\theta_1 & -\sin\theta_1 & 0& 0 \\ \sin\theta_1 & \cos\theta_1 & 0& 0 \\ 0 & 0 & 1 & 0\\ 0& 0& 0& 1 \end{matrix} \right]进一步可得,0P4o=[xyz1]=[cosθ1sinθ100sinθ1cosθ10000100001][g1g2g31]=[g1cosθ1g1sinθ1g1sinθ1+g1cosθ1g31] {^0}P_4o=\left[ \begin{matrix} x\\ y\\ z\\ 1 \end{matrix} \right]=\left[ \begin{matrix} \cos\theta_1 & -\sin\theta_1 & 0& 0 \\ \sin\theta_1 & \cos\theta_1 & 0& 0 \\ 0 & 0 & 1 & 0\\ 0& 0& 0& 1 \end{matrix} \right]\left[ \begin{matrix} g_1\\ g_2\\ g_3\\ 1 \end{matrix} \right] =\left[ \begin{matrix} g_1cos\theta_1- g_1sin\theta_1\\ g_1sin\theta_1+g_1cos\theta_1\\ g_3\\ 1 \end{matrix} \right]r=x2+y2+z2=g12+g22+g32r=x^2+y^2+z^2=g{^2_1}+g{^2_2}+g{^2_3} 并且z=g3z = g_3代入g1g_1g2g_2g3g_3的表达式,可得:r=(k1cosθ2+k2sinθ2)2a1+k3r=(k_1 cos\theta_2 +k_2 sin\theta_2)2a_1 +k_3 z=(k1sinθ2k2cosθ2)sinα1+k4z =(k_1 sin\theta_2 - k_2 cos\theta_2)sin\alpha_1 +k_4 其中,
    k1(θ3)=f1k_1(\theta_3)=f_1 k2(θ3)=f2k_2(\theta_3)=-f_2 k3(θ3)=f12+f22+f32+a12+d22+2d2f3k_3(\theta_3)=f{^2_1}+f{^2_2}+f{^2_3} + a{^2_1} + d{^2_2} +2d_2 f_3 k4(θ3)=f3cosα1+d2cosα1k_4(\theta_3)=f_3 cos\alpha_1 +d_2 cos\alpha_1 这里分三种情况进行讨论:

    1. a1=0a_1 =0时。r=k3(θ3)r=k_3 (\theta_3),因为rr已知,所以可以求出θ3\theta_3
    2. sinα1=0sin\alpha_1 =0时。z=k4(θ3)z=k_4 (\theta_3),因为zz已知,所以可以求出θ3\theta_3
    3. a1a_1sinα1sin\alpha_1均不为0时,有(rk3)2(2a1)2+(zk4)2sin2α1=k12+k22\frac{(r-k_3)^2}{(2a_1)^2} + \frac{(z-k_4)^2}{sin^2\alpha_1} = k{^2_1}+k{^2_2}这里所有变量均为θ3\theta_3的函数,求解关于θ3\theta_3的非线性方程即可得到θ3\theta_3。需要注意的是,更为准确地说,上述公式中所有变量均为sinθ3sin\theta_3cosθ3cos\theta_3的函数,可令u=tanθ32u=tan\frac{\theta_3}{2} cosθ=1u21+u2cos\theta=\frac{1-u^2}{1+u^2} sinθ=2u1+u2sin\theta=\frac{2u}{1+u^2} 由此,可将原非线性方程转化为关于变量uu的方程。解得uu之后再利用θ3=2arctanu\theta_3=2arctanu得到θ3\theta_3
      利用前面的公式r=(k1cosθ2+k2sinθ2)2a1+k3r=(k_1 cos\theta_2 +k_2 sin\theta_2)2a_1 +k_3 这里代入θ3\theta_3后,所有变量均为θ2\theta_2的函数,类似的,求解该非线性方程即可得到θ2\theta_2
      利用公式x=g1cosθ1g2sinθ1x=g_1 cos\theta_1 - g_2 sin\theta_1 代入θ2\theta_2θ3\theta_3,即可求得θ1\theta_1
      到此,求解得到了θ1\theta_1θ2\theta_2θ3\theta_3

    求解腕部方位

    z-y-z欧拉角

    假设某坐标系分别依次绕动坐标系的Z轴、Y轴、Z轴转动α\alphaβ\betaγ\gamma角度,则旋转矩阵为 R=[cosαsinα0sinαcosα0001][cosβ0sinβ010sinβ0cosβ][cosγsinγ0sinγcosγ0001]R=\left[ \begin{matrix} \cos\alpha& -\sin\alpha& 0 \\ \sin\alpha& \cos\alpha& 0 \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} \cos\beta& 0 & \sin\beta\\ 0 & 1 & 0 \\ -\sin\beta& 0 & \cos\beta \end{matrix} \right]\left[ \begin{matrix} \cos\gamma& -\sin\gamma& 0 \\ \sin\gamma& \cos\gamma& 0 \\ 0 & 0 & 1 \end{matrix} \right] 具体计算得:R=[cosαcosβcosγsinαsinγcosαcosβsinγsinαcosγcosαsinβsinαcosβcosγ+cosαsinγsinαcosβsinγ+cosαcosγsinαsinβsinβcosγsinβsinγcosβ] R=\left[ \begin{matrix} \cos\alpha cos\beta cos\gamma - sin\alpha sin\gamma & -cos\alpha cos\beta sin\gamma-sin\alpha cos\gamma & cos\alpha sin\beta \\ sin\alpha cos\beta cos\gamma + cos\alpha sin\gamma & -sin\alpha cos\beta sin\gamma + cos\alpha cos\gamma & sin\alpha sin\beta \\ -sin\beta cos\gamma & sin\beta sin\gamma & cos\beta \end{matrix} \right] R=[r11r12r13r21r22r23r31r32r33] R=\left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix} \right] α\alphaβ\betaγ\gamma可由以下公式计算得到。β=atan2(r312+r322,r33)\beta=atan2(\sqrt{ r{^2_{31}} + r{^2_{32}}},r_{33}) α=atan2(r23,r13)\alpha=atan2(r_{23},r_{13}) γ=atan2(r32,r31)\gamma =atan2(r_{32},-r_{31})

    具体求解

    60T=30T63T {^0_6}T={^0_3}T{^3_6}T得到63T=30T160T {^3_6}T={^0_3}T^{-1}{^0_6}T 再由齐次变换矩阵和旋转矩阵的关系得到 63R{^3_6}R。.
    许多工业机器人腕部三关节配置成一个球面副,这种球面副可用z-y-z欧拉角的解出这三个关节角。因此利用上面介绍的方法可以求解出θ4\theta_4α\alpha)、θ5\theta_5θ\theta)、θ6\theta_6γ\gamma)。

    算例

    设六自由度机械臂的DH参数表如下。

    i a α\alpha d θ\theta
    1 0 0 0 θ1\theta_1
    2 -30 -90 0 θ2\theta_2
    3 340 0 0 θ3\theta_3
    4 -40 -90 388 θ4\theta_4
    5 0 90 0 θ5\theta_5
    6 0 -90 0 θ4\theta_4

    腕点在基坐标系{0}中的位置向量为 0P4o=[381.3151.819.5](n) {^0}P_4o= \left[ \begin{matrix} 381.3 \\ 151.8\\ 19.5 \end{matrix} \right] \tag{n} 在坐标系{3}的位置为3P4o=[403380](n) {^3}P_4o= \left[ \begin{matrix} -40\\ 338\\ 0 \end{matrix} \right] \tag{n}坐标系{6}相对于基坐标系{0}的齐次变换矩阵为:[00.57360.8192381.300.81920.5736151.810019.50001](n) \left[ \begin{matrix} 0& 0.5736& 0.8192& 381.3\\ 0& -0.8192& 0.5736& 151.8\\ 1& 0& 0& 19.5\\ 0& 0&0&1\\ \end{matrix} \right] \tag{n}

    MATLAB实现

    先封装两个函数calT和getKG。

    function Matrix_T = calT(DH_parameter, start_row, end_row)
    % 输入参数为DH参数表,参数表有四列,参数表的行数是运动轴的个数。比如六自由度机械臂的参数表为6行4列
    % 第一列是Z轴偏置;
    % 第二列是Z轴偏转角;
    % 第三列是X轴偏置;
    % 第四列是X轴偏转角;
    Matrix_T = eye(4);
    for row_index = start_row + 1 : end_row
        % 先绕X轴转theta_X, 再沿X轴平移trans_X,将两个坐标系的Z轴重合
        theta_X = DH_parameter(row_index, 2);
        trans_X = DH_parameter(row_index, 1);
        T_X = rotX(theta_X) * transX(trans_X);
        
        % 先绕Z轴转theta_Z, 再沿Z轴平移trans_Z,将两个坐标系的X轴重合
        theta_Z = DH_parameter(row_index, 4);
        trans_Z = DH_parameter(row_index, 3);
        T_Z = rotZ(theta_Z) * transZ(trans_Z);
       
        Matrix_T = Matrix_T * T_X * T_Z;
    end
    
    function [k1, k2, k3, k4, g1, g2, g3] = getKG(DH_parameter, P_3)
    % 计算k和g
    % DH_parameter: DH参数表
    % P_3:腕点坐标系3中的位置
    
    T_12 = calT(DH_parameter, 1, 2);
    T_23 = calT(DH_parameter, 2, 3);
    
    F_Matrix = T_23 * P_3;
    G_Matrix = T_12 * F_Matrix;
    
    f1 = F_Matrix(1, 1);
    f2 = F_Matrix(2, 1);
    f3 = F_Matrix(3, 1);
    
    g1 = G_Matrix(1, 1);
    g2 = G_Matrix(2, 1);
    g3 = G_Matrix(3, 1);
    
    a1 = DH_parameter(2, 1);
    alpha1 = DH_parameter(2, 2);
    d2 = DH_parameter(2, 3);
    
    k1 = f1;
    k2 = -f2;
    k3 = f1^2 + f2^2 + f3^2 + a1^2 + d2^2 + 2*d2*f3;
    k4 = f3*cos(alpha1) + d2*cos(alpha1);
    

    下面是主程序。

    syms theta1 theta2 theta3 theta4 theta5 theta6 
    
    DH_parameter = [
        0, 0, 0, theta1;
        -30, -90/180*pi, 0, theta2;
        340, 0, 0, theta3;
        -40, -90/180*pi, 338, theta4;
        0, 90/180*pi,0, theta5;
        0, -90/180*pi, 0, theta6;
        ];
        
    P_3 = [-40; 338; 0; 1];
    P_O = [381.3; 151.8; 19.5];
    x = P_O(1, 1);
    z = P_O(3, 1);
    r = P_O'* P_O;
    a1 = DH_parameter(2, 1);
    alpha1 = DH_parameter(2, 2);
    d2 = DH_parameter(2, 3);
    
    [k1, k2, k3, k4, g1, g2, g3] = getKG(DH_parameter, P_3);
    
    % 计算theta3
    s = '(r-k3)^2/4/a1^2+(z-k4)^2/sin(alpha1)^2=k1^2+k2^2'
    s = subs(s, 'r', r);
    s = subs(s, 'z', z);
    s = subs(s, 'a1', a1);
    s = subs(s, 'alpha1', alpha1);
    s = subs(s, 'k1', k1);
    s = subs(s, 'k2', k2);
    s = subs(s, 'k3', k3);
    s = subs(s, 'k4', k4);
    % 变量替换
    s = subs(s, 'cos(theta3)', '(1-u^2)/(1+u^2)');
    s = subs(s, 'sin(theta3)', '2*u/(1+u^2)');
    % 求解非线性方程
    u = solve(s, 'u');
    theta3 = double(2 * atan(u));
    

    计算得:

    theta3 =
    
        2.8628
        2.6414
        0.2646
        0.0432
    

    θ3=0.0432\theta_3=0.0432(弧度)。
    再计算θ2\theta_2

    theta3 = 0.0432;
    % 计算theta2
    s = 'r = (k1 * cos(theta2) + k2 * sin(theta2)) * 2 * a1 + k3';
    s = subs(s, 'a1', a1);
    s = subs(s, 'r', r);
    s = subs(s, 'k1', k1);
    s = subs(s, 'k2', k2);
    s = subs(s, 'k3', k3);
    s = subs(s, 'theta3', theta3);
    % 变量替换
    s = subs(s, 'cos(theta2)', '(1-u^2)/(1+u^2)');
    s = subs(s, 'sin(theta2)', '2*u/(1+u^2)');
    % 求解非线性方程
    u = solve(s, 'u');
    theta2 = double(2 * atan(u));
    

    计算得:

    theta2 =
    
       -0.9058
       -0.8272
    

    θ2=0.9058\theta_2=-0.9058(弧度)。
    再计算θ1\theta_1

    theta3 = 0.0432;
    theta2 = -0.9058;
    s = 'x = cos(theta1) * g1 - sin(theta1) * g2';
    s = subs(s, 'x', x);
    s = subs(s, 'g1', g1);
    s = subs(s, 'g2', g2);
    s = subs(s, 'theta3', theta3);
    s = subs(s, 'theta2', theta2);
    % 变量替换
    s = subs(s, 'cos(theta1)', '(1-u^2)/(1+u^2)');
    s = subs(s, 'sin(theta1)', '2*u/(1+u^2)');
    % 求解非线性方程
    u = solve(s, 'u');
    theta1 = double(2 * atan(u));
    

    计算得:

       theta1 =
    
        0.3795
       -0.3795
    

    θ1=0.3795\theta_1=0.3795(弧度)。
    下面计算θ4\theta_4θ5\theta_5θ6\theta_6
    先封装计算ZYZ欧拉角的函数

    function [alpha, beta, gamma] = calZYZEulerInv(T_Matrix)
    % 输入参数为DH参数表,参数表有四列,参数表的行数是运动轴的个数。比如六自由度机械臂的参数表为6行4列
    % T_Matrix:齐次变换矩阵;
    % alpha:第一次Z轴转角;
    % beta:Y轴转角;
    % gamma:第二次Z轴转角;
    
    R = T_Matrix(1:3, 1:3);
    
    r31 = R(3, 1);
    r32 = R(3, 2);
    r33 = R(3, 3);
    r13 = R(1, 3);
    r23 = R(2, 3);
    
    beta = double(atan2(sqrt(r31^2 + r32^2), r33));
    alpha = double(atan2(r23, r13));
    gamma = double(atan2(r32, -r31));
    

    下面是主程序。

    % 坐标系{6}相对于坐标系{0}
    T_06 = [
     0, 0.5736, 0.8192, 381.3;
     0, -0.8192, 0.5736, 151.8;
     1, 0, 0, 19.5;
     0, 0, 0,1]; 
     
    DH_parameter(1, 4) = 0.3795; % theta1
    DH_parameter(2, 4) = -0.9058; % theta2
    DH_parameter(3, 4) = 0.0432; % theta3
    
    T_03 = calT(DH_parameter, 0, 3); % 坐标系{3}相对于坐标系{0}
    T_36 = T_03 \ T_06; % 坐标系{6}相对于坐标系{3}
    [alpha, beta, gamma] = calZYZEulerInv(T_36);
    

    计算得到结果。

    alpha =
    
       0.8626
    
    
    beta =
    
       1.3394
    
    
    gamma =
    
      -1.5708
    

    至此,运动学逆解计算完成。
    六个关节角度分别为:θ1=21.7\theta_1=21.7^\circθ2=51.9\theta_2=-51.9^\circθ3=2.5\theta_3=2.5^\circθ4=49.4\theta_4=49.4^\circθ5=76.7\theta_5=76.7^\circθ6=90\theta_6=-90^\circ

    展开全文
  • 以后将会给大家讲解如何手写正逆解以及轨迹插补的程序。程序是基于Matlab2016a,工具箱版本为Robotic Toolbox 9.10。1.D-H建模三个两两相互垂直的XYZ轴构成欧几里得空间,存在六个自由度:沿XYZ平移的三个自由度,绕...
  • 用于matlab中的神经网络开发 rbf神经网络求解机器人运动学逆解源码
  • robotics toolbox中的ikine只能得到一个逆解,而6自由度机器人一般会存在10几个逆解,我需要做能耗最优轨迹规划,故需要得到所有逆解来选取其中能耗最低得一个。
  • 四足机器人(二)---运动学逆解和步态规划

    千次阅读 多人点赞 2020-08-20 18:48:19
    四足机器人(二)---运动学逆解和步态规划运动学逆解步态规划MATLAB仿真 运动学逆解 其实运动学分为运动学正解和运动学逆解,二者有什么区别呢?因为在四足机器人中用的是12个舵机,所以运动学正解是已经知道运动...
  • 运动学逆解优化

    2014-04-03 10:14:42
    MATLAB环境下六自由度焊接机器人运动学逆解及优化
  • Matlab Robotics Toolbox】robotics toolbox学习及使用记录,方便自己后面复习、改进。 基于Matlab R2019b 9.5; Peter Corke的Robotics ...运动学逆解 微分运动学(求雅克比矩阵) 0. 前言 在初学机器人学的时候
  • Delta并联机器人逆解程序,自己写的,正逆解完全可以对照上。
  • 该文件以6自由度puma560为例说明机器人的一种正逆运动学的编程方法(正运动学为DH法,逆运动学为解析),以及机器人工具箱(matlab robotic toolbox)初步的运动学使用方法。
  • 机器人逆运动学就是即在已知末端的工具坐标系相对于基坐标系的位姿。计算所有能够到达指定位姿的关节角。求解可能出现: 不存在相应 存在唯一 存在多 我们把机械臂的全部求解方法分为两大类:...
  • 基于Matlab的UR5六轴机器人数值解法求逆解逆解定点及画圆)源码 (机器人建模与仿真中的机器人逆运动) 其它关于基于Matlab机器人建模与仿真资料合集请往CSDN博客 “基于Matlab的机器人建模学习资料大整理”...
  • 基于Matlab的UR5六轴机器人解析解法求逆解逆解定点及画圆)源码 (机器人建模与仿真中的机器人逆运动) 其它关于基于Matlab机器人建模与仿真资料合集请往CSDN博客 “基于Matlab的机器人建模学习资料大整理”...
  • 为了方便验证一般在机器人导论上面会推荐两种方式,一是公式法 二是几何法,推荐使用公式法,使用起来比较方便,但是要注意表格数据的准确性,坐标系建立的准确性以及计算的准确性,不然一个出现问题就会导致计算...
  • %% Compiled by Ezekiel according to robot modeling and control %% 2020/9/25 clear;...%% DH矩阵,DH建模法建立六轴机器人模型 %% th d a alpha L(1) = Link([ 0, 1, 0, pi/2 ], 'qlim','[-pi/4 p
  • 该代码可以实现UR5正逆运动学的求解,其中逆运动学为8组,正运动学输入六个关节角的角度值,UR5的DH坐标即可
  • 基于Matlab的UR5六轴机器人解析解法求逆解逆解定点及画圆)源码 (机器人建模与仿真中的机器人逆运动) 其它关于基于Matlab机器人建模与仿真资料合集请往CSDN博客 “基于Matlab的机器人建模学习资料大整理”...
  • 逆运动学求解 (1)不存在相应的,包括:期望位姿离基坐标系太远,机械臂不够长,末端执行器无法达到该位姿;当机械臂的自由度少于6个自由度时,它将不能达到三维空间的所有位姿(使用ikine()函数会出现错误) ...
  • 以及用机器人工具箱建立的机械臂的DH矩阵的显示如下: 这个机械臂即V-rep中的example的第一个,注意使用时要先将IK group删除,删掉dummy之间的link. Matlab中的代码: 建立DH矩阵: clear deg = pi/180; L(1) ...

空空如也

空空如也

1 2 3 4
收藏数 61
精华内容 24
关键字:

matlab机器人运动学逆解

matlab 订阅