MATLAB Simscape Project

 

 Simple 3-DOF Planar Parallel Robot in Simulink environment

Kaixian QU

  1. Model display
    This robot is made of 2-DOF Planar Parallel Robot with 1-DOF pencil on the vertical plane. The goal is to control 2 motors to drive the pencil go along a straight line, and another motor to make pencil up and down.  My Simulink model consists of 6 modules(see picture below).
  2. Trajectory display
    The pencil goes along a straight line from (0, 0.3, 0.05) to (0.3, 0.5, -0.05) in 5 seconds


    I also tested its straightness via Curve Fitting Tool. As you can see, the R-square is 1, which means that the trajectory is perfectly straight.fit

  3. Function solving inverse kinematics 
    len1 = .35;
    len2 = .35;
    len3 = .35;
    len4 = .35;
    l = .6;
    syms x y theta1 theta2 theta4 theta5
    eqyl = (len1 * sin(theta1) + len2 * sin(theta1 + theta2) == y);
    eqyr = (len3 * sin(theta4 + theta5) + len4 * sin(theta5) == y);
    eqxl = (len1 * cos(theta1) + len2 * cos(theta1 + theta2) == x);
    eqxr = (len3 * cos(theta4 + theta5) + len4 * cos(theta5) == l - x);
    [theta1, theta2, theta4, theta5] = solve(eqxl, eqyl, eqxr, eqyr, theta1, theta2, theta4, theta5);
    theta1
    theta5

    Given the expected values of X, Y coordinates, this function outputs the required value of 2 motor angles.

    function [theta5,theta1] = fcn(x, y)
    
    theta1 = 2*atan((7*y + (x^2*(-(x^2 + y^2)*(100*x^2 + 100*y^2 - 49))^(1/2)) ...
        /(x^2 + y^2) + (y^2*(-(x^2 + y^2)*(100*x^2 + 100*y^2 - 49))^(1/2))...
        /(x^2 + y^2))/(10*x^2 + 7*x + 10*y^2));
    theta5 = 2*atan((35*y + ((25*x^2 - 30*x + 25*y^2 + 9)*...
        (- 100*x^2 + 120*x - 100*y^2 + 13))^(1/2))/(50*x^2 - 95*x + 50*y^2 + 39));
    end