Simulink program for path planning of SCARA robot with forward simula on ...29 Figure 16.. 10 Figure 3 General diagram of control system for SCARA Robot.. Theory of mo ment trajectory: v
OVERALL OF SCARA ROBOT
General of SCARA Robot
1.1.1 De ni on of SCARA Robot:
SCARA Robot is abbrevia on of Selec ve Compliance Assembly Robot Arm or Selec ve Compliance Ar culated Robot Arm It normally con gured as a 4 DOF Robot With the parallel con gure of revolute joints, SCARA Robot is used widely in vertical assembly task or in parallel plane (selec ve direc on) applica on
In this project, the goal is to design an industrial use SCARA Robot with considera on about both economically and e ciency The Robot will be controlled with familiar stepper motor or servo motor and control circuit which should be available on the market Control program for the robot should be available for every personal computer for convenient and versa le use of the robot This program will be using G-code for control the SCARA robot
Robot is designed to become machines with high accuracy and exibility Therefore, Robot in general or SCARA in speci c, are used as replacement for the tradi onal workers in factory nowadays To ful ll this purpose, robot design must follow some technical parameters given These parameters will demonstrate and ensure the ability of robot to handle some certain tasks For example, some technical speci ca ons of SCARA Robot Adept One XL are shown below:
Figure 1 Model of a SCARA Robot
Working area of each joint:
Maximum velocity of each link:
SCARA is one of standard con gura ons for robot arm In primary design procedure, we have some speci ca ons for the robot as follow:
- Maximum velocity of end e ector: 9570 mm/s
- Transmission of nal link: Ball screw
Electronics components for the system must depend on the type of motor being used in the robot To obtain high accuracy, stepper motor or servo motor will be used in the general transmission An important part of electronics system design is design control circuit for servo motor This part and the control program are the two most important factors will a ect the e ciency of robot In this project, we only focus on the control program of servo motor Some primary factor before design must be no ced are :
- Type of motor: Unipolar or Bipolar
- Working mode: Full or half step
- Characteriza on of torque and velocity
The control program for the robot must capable of performing these tasks:
- Con nuous path calcula ng
- Start and stop mo on of all axis at the same me
Requirements for control program of robot: The control system a er design must be checked with some requirements for quality con rma on
- Percent of overshoot: less than 20%
- Se ling me: less than 0.7 s
Working principle and speci c parameters analysis of SCARA Robot
1.2.1 Working principle of control system:
Firstly, all sensors will obtain the informa on, characteris c of the target based on the se ng program For example, informa on about color and weight of the target, distance to the loca on of object will be collected through sensors
From these informa on, control program of the robot will calculate the parameters, create and send the command to the control system to perform the opera on with robot arm in a fast and precise manner
1.2.2 Components in the control system of robot:
- Motors: The choice of motors is made from mechanical system design of SCARA robot
- Sensors: For SCARA robot, sensors must include 2 rota on angle sensor (SR-1, SR-2) for two revolute joints, a linear mo on sensor (SP-3) for the third joint and a force sensor (SF- 4) for the jaw at end e ector Another sensor might be added for be er control of the robot or other applica on may require The model of sensor on the robot can be shown as:
Figure 2 Sensor configuration on SCARA Robot
Figure 3 General diagram of control system for SCARA Robot
- Control program: This can be done with available applica on as LinuxCNC, or program wri en on MatLab/Simulink.
Robot Model design
With the reference of DENSO HM 4070G robot, we had built a 3 DOF SCARA robot model, which suitable with the technical speci ca on given
Transmission system of the robot model include:
- Ball-screw transmission: This mechanical pair convert rotation mo on into transla on mo on Lead screw also able to perform the same conversion with high accuracy and transmission ra o However, lead screw rarely use for transmission of primary mo on due to its low e ciency
- Belt transmission: This is one of the most common type of transmission in Mechanical Belt system transmits torque and velocity between two sha s with larger distance than gear transmission Belt transmission has several advantages as: Low noise, low shock, large distance transmission, no lubrica on needed, and low maintain fee
KINETICS AND KINEMATICS OF SCARA ROBOT
The kinema cs analysis of SCARA Robot
We represent the SCARA robot by the kine cs diagram for analysis as follow:
Figure 5 Kinematics diagram of SCARA robot
From the kinema cs diagram of SCARA robot above (Figure 8), we create the D-H parameters table as follow:
Where: θi: angle of rota on of frame (i-1) around axis z for axis x to match axis x i-1 i-1 i. di: distance of origin of 2 frames (i-1) and (i) in z axis direc on i-1 αi: angle of rota on of frame (i-1) around x axis for z to match z i i-1 i ai: distance of origin of 2 frames (i-1) and (i) in x axis direc on i
By using D-H table and Denavit Hartenberg shortcut, we can create the homogenous – transforma on matrix If we denote i-1 Hi as the homogenous transforma on matrix from frame (i-1) to frame i then: i-1Hi = [ cos(𝜃 𝑖 ) −𝑐𝑜𝑠(𝛼𝑖)𝑠𝑖𝑛 𝜃( 𝑖) 𝑠𝑖𝑛(𝛼 𝑖 )𝑠𝑖𝑛 𝜃( 𝑖) 𝑎𝑖cos(𝜃 𝑖 ) 𝑠𝑖𝑛 𝜃( 𝑖 ) 𝑐𝑜𝑠(𝛼 𝑖 )cos (𝜃𝑖) −𝑠𝑖𝑛(𝛼𝑖)cos (𝜃𝑖) 𝑎 𝑠𝑖𝑛 𝜃𝑖 ( 𝑖 )
Then from D-H Table and (Eq.3.1) we obtain the transforma on matrix of SCARA robot as:
0H1 =[ cos (𝑞 1 ) −sin (𝑞 1 ) 0 𝑎 1 cos (𝑞 1 ) sin (𝑞 1 ) cos (𝑞 1 ) 0 𝑎 1 sin (𝑞 1 )
1H2 = [ cos (𝑞 2 ) −sin (𝑞 2 ) 0 𝑎2cos (𝑞 2 ) sin (𝑞 2 ) cos (𝑞 2 ) 0 𝑎 2 sin (𝑞 2 )
Then the matrix of transforma on from frame 0 of robot to frame 3 is:
0H3=[ cos(𝑞 1 +𝑞 2 ) − sin(𝑞1+𝑞 2 ) 0 𝑎 2 cos(𝑞 1 +𝑞 2 ) +𝑎 1 cos(𝑞 1 ) sin(𝑞 1 +𝑞 2 ) cos(𝑞 1 +𝑞 2 ) 0 𝑎 2 sin(𝑞 1 +𝑞 2 ) +𝑎 1 sin(𝑞 1 )
(𝑬𝒒 𝟐.𝟐) But from the de ni on of homogenous transforma on matrix we have:
R is rota on matrix between two frames d is displacement vector of origin of two frames
From (Eq.3.2) and (Eq.3.3) we have:
Posi on of end e ector in frame 0 (denoted as ) is: 0 rE
Orienta on of end e ector ( 0 RE) is:
0RE = [cos (𝑞 1 +𝑞 2 ) −sin (𝑞 1 +𝑞 2 ) 0 sin (𝑞 1 +𝑞 2 ) cos (𝑞 1 +𝑞 2 ) 0
Accelera on of end e ector:
With the geometric parameters and limita on of joint variable as:
𝑞 𝟑 = (−400 ÷ 0 𝑚𝑚) Replacing into Eq 2.4, we obtain:
For inverse kinema cs, given the posi on of the end e ector, we will nd the values of joint variables to obtain the same pose
Assume the posi on of end e ector is:
Refer to Eq 2.4, we have set of equa ons:
Then we obtain two equa ons with two unknown sin(q ) and cos(q 1 1):
Using Crammer’s rule, the unknowns are found to be:
↔𝑞1= arctan (sin(𝑞 1 ) cos(𝑞 1 )) = tan −1 (𝑎 1 𝑦𝑒+𝑎2(𝑦 𝑒 𝑐𝑜𝑠(𝑞 2 ) −𝑥 𝑒 sin (𝑞 2 ))
𝑎 𝑥 1 𝑒 +𝑎 2 (𝑥 𝑒 𝑐𝑜𝑠(𝑞 2 ) +𝑦 𝑒 sin (𝑞 2 ))) (Eq 2 )21 Subs tute real parameters of robot to set of equa ons:
The kine cs analysis of SCARA Robot
To solve the forward kine cs of SCARA robot, we are going to set up the deriva ve equa on of mo on for the robot The mo on equa on of robot is built from the Lagrange equa on with the general form as:
𝜕𝑞 𝑖) 𝑇 +𝑄 𝑖 ∗ (Eq 2.23) This equa on is equivalent to the general form of dynamical equa on:
By denote as posi on vector for center of mass of link j, represented in frame i Then i p cj assuming that all links are homogeneous with rela vely small cross sec on compare to the length The posi on vectors of the centers of mass are given by:
𝑙 is the third link length
As the assump on about the geometry shape of link and the orienta on of each link in kinema cs diagram in Figure 8, we have the link iner a matrices about their centers of mass and expressed in their respec ve link frames as:
i 𝐼𝑗 is the iner a matrices of link j in frame i
By mul ply the iner a matrices above i 𝐼 𝑖 with the rota on matrices 0 Ri, we have the iner a matrices about their centers of mass and expressed in the base frame as:
The posi on vectors of the centers of mass of links 1, 2, and 3 with respect to the various link frames and expressed in the base frame are:
The link Jacobian submatrices, J and J , are obtained by subs tu ng the Eq 2.27 to the vi wj equa on:
Then the link Jacobian submatrices are:
The manipulator iner a matrix is obtained by subs tu ng Eq.2.26 and Eq.2.28 into the equa on:
Taking the par al deriva ves of the manipulator iner a matrix with respect to 𝑞 𝑖 in accordance with:
We have the velocity coupling factor V is: i
Assuming that the accelera on of gravity points in the nega ve 𝑧 0 - direc on or vector form is 𝒈= [0, 0, −𝑔] 𝑇 Then from the Eq.2 and accordance: 28
We have the gravita onal vector as:
For a SCARA robot, if we let 𝝉= [𝜏 1 ,𝜏 2 ,𝜏 3 ] 𝑇 be the vector of joint torques generated by the motors (actuators), 𝑭 𝑒 = [𝒇 𝑒 𝑇 ,𝒏 𝑒 𝑇 ] 𝑇 be the vector of resultant force and moment exerted at the end e ector, and 𝒇 𝑟 = [𝑏 1 𝑞 1 ,𝑏 2 𝑞 2 ,𝑏 𝑞 3 3 ] be the fric onal torques or forces in the joints, then the virtual work of the robot is:
𝛿𝒙=𝐽 𝛿𝒒 𝑇 with J is the manipulator Jacobian matrix
By subs tu ng these two rela onships into Eq.3.21 and equa ng the result, the generalized force vector has the form as:
Finally, by subs tu ng Eq.2.30, 2.31, 2.32, and 2.34 to Eq.2.24, we have the general form of dynamical equa on for the SCARA robot and nish the forward kine cs analysis
Assuming that in our robot model, there are no external forces exerted at the end e ector and the joint fric on is negligible Thus, the vector of joint torques and the vector of generalized forces are equivalent or:
𝑸=𝝉 𝑇 Then from the general form of dynamical equa on for the SCARA robot with all parameter matrices M, V, G, and above yields: Q
Set of equations in Eq.2.35 are the dynamical equations of mo on for the 3-dof SCARA arm The gravita onal effects do not appear in the rst two equa ons, because the accelera on of gravity is parallel to the rst two joint axes Also, the mo on of the third joint is completely independent of the rst two, and the third link merely acts as a load to the mo on of the rst two joints With this solu on, we completed the inverse dynamics analysis of SCARA arm.
DESIGN OF WORKING SPACE AND ROBOT TRAJECTORY
Working space
Refer to Eq 2.10, forward kinema cs equa ons of robot are:
By using Matlab to simulate the working space of robot, we obtain the result has hollow cylinder-shape as we expected
Figure 6 Working space in XY plane
Figure 7 Working space in 3D coordinate
Robot trajectory design
3.2.1 Theory of mo ment trajectory: ve
The design of movement trajectory is closely related to the problem of control the movement of robot from one to another posi on in working space The path, which is built based on the trajectory, will be used as desired des na on for posi on-control system of the robot Thus, the accuracy of trajectory design will a ect the quality of robot mo on
Common form of path for robot movement is a high order polynomial This type of trajectory will assure the requirement of posi on, velocity, and accelera on along every point on the mo on path For high quality trajectory, the design of path should follow some remarks:
- The end e ector must guarantee to pass each point in the working space or moving in some certain trajectory The path of the robot must be a con nuous curve in some amount of me
- No sudden jump in velocity or accelera on should appear
- The trajectory normally has the form of common curve
Some familiar type of curve for the trajectory are:
There are two main techniques in trajectory design are:
- Trajectory design in joint space: Robot design problem in which the robot is considered to move from A to B in space without the considera on of any other point between A and B
- Trajectory design in task space: Robot design problem in which the robot is considered to move from A to B in space with the considera on of other posi on that the robot has passed during the movement
In this task, I will use the trajectory in the form of third-order polynomial:
3.2.2 Design third-order trajectory in joint space:
When building the path in joint space, we normally consider the posi on of joint at the beginning and ending of each movement, also no ce on the velocity and accelera on of each joint If we keep track of all the data of joints, then the joints can move with di erent me, velocity, and accelera on, which lead to di erent trajectory, but the posi on requirement will be sa s ed
For designing the trajectory, we choose 2 points A (𝑥 𝐴 ,𝑦 𝐴 ,𝑧 𝐴 ), B (𝑥 𝐵 ,𝑦 𝐵 ,𝑧 𝐵 ) arbitrary in the working space From the inverse kinema cs equa on Eq 2.22, the correspond joint variables can be found as A (𝑞 1𝐴 ,𝑞 2𝐴 ,𝑞 3𝐴 ) and B (𝑞 1𝐵 ,𝑞 2𝐵 ,𝑞 3𝐵 )
From the two set of joint variables has been found, we are able to design the path from
𝑞 𝑖𝐴 𝑞to 𝑖𝐵 (𝑖= 1 ÷ 3) follow the third-order trajectory in some amount of me 𝑡 𝑐 (𝑠) The joint variable in the form:
From Eq.3.1 and Eq.3.2, we have:
An example of trajectory design:
Design the movement path for the robot from posi on A(350 , 0, 100) to posi on B √2(350 175+ √2,175√2,300) in the dura on of 2 seconds From the inverse kinema cs equa on, we found the numerical value of joint variables at A and B as:
𝑞 3𝐵 = −100 Replacing into Eq.3.3, we have coe cients of each joint equa on as:
Figure 8 Joint variables diagram of the problem
Third-order trajectory design in pick-and-place applica on of SCARA robot:
In order to inves gate the path-planning guideline we have proposed, we perform the test on a pick-and-place problem with the path de ne with 5 points as follow:
Figure 9 Mark points of pick and place object application
In which detail of the procedure is shown below :
S (0.1, 0.6, 0.2) is the star ng point of robot
Procedure SA’ (1) A’A (2) Halt AA’ (3) A’B’ (4) B’B (5) Halt BB’ (6)
For this problem, we use Simulink and Matlab to solve the problem First, we determine the joint variables combina on required at each mark point by inverse kinema cs
Figure Simulink program for determine set of joint variables at mark point 10
Next, build up the func on for joint variable, velocity, and accelera on for each procedure based on start value of joint variable, end value of joint variable, and me dura on to perform procedure Then by subs tute a time signal to the func on, we obtain the path planning for the procedure The Simulink to perform this task is:
Figure Trajectory planning block 11 The results obtained from the program are shown below:
Figure Joint variables versus time diagram 12
28 Figure Joint velocity versus time diagram 13
Figure Joint acceleration versus time diagram 14
For checking the solu on, we can use forward kinema cs equa ons in order to construct again the path of robot during the pick and place process This whole program can be connected and is done in Simulink as:
Figure Simulink program for path planning of SCARA robot with forward simulation 15
By the procedure above, the nal result of the path for pick and place task of the robot is generated as follow:
Figure Theoretical working trajectory of the robot for the task in 3D coordinate 16
3.2.3 Design of third-order trajectory in task space:
For this part, the requirement is design the robot to follow a straight line across 2 points
A and B The simplest method is to split the path to many small sec on to solve for the necessary angle similar to joint space The quality of the path is higher when each sec on is ge ng smaller
With the problem of pick-and-place similar to the previous part, we can separate the path to 50 consecu ve sec on to perform the designing We show again the problem below:
Figure 17 Mark points of pick and place object application
In which detail of the procedure is shown below:
S (0.1, 0.6, 0.2) is the star ng point of robot
Procedure SA’ (1) A’A (2) Halt AA’ (3) A’B’ (4) B’B (5) Halt BB’ (6)
To design the trajectory for robot, we usually use the n-order polynomial approxima on method The form being used in this project is the third-order polynomial with the characteris cs as: Simple for design, assure the con nuous of the velocity, and reduce the bumpy and glitch when robot is working
Design the trajectory in task space with straight line path:
In general, the func on of a line goes through 2 points 𝑀(𝑥 0 ,𝑦 0 ,𝑧 0 ) and 𝑁(𝑥 𝑒 ,𝑦𝑒,𝑧𝑒) is:
The displacements of end e ector E along x, y, and z axis are following the third-order polynomial respect to me:
With the me for robot to move from M to N follow a straight line is 𝑡 𝑒 (𝑠) Ini al condi on for the problem:
We obtained the set of coe cient as:
From the set of equa ons, we program a Simulink version for calculate the joint variable value for robot path planning in task space as:
Figure Simulink program for path planning in task space 18
Result of the program is shown below:
By using forward kinema cs, we can rebuild the path that the end e ector of robot traveled through while perform the pick and place task is:
Figure End effector trajectory in 3D space 22
ROBOT LINK MODELING
Modeling and nd transfer func on for a robot link
Figure General diagram of a robot link with gear transmission 24
𝐽 𝑎 is the moment of iner a of the motor 𝜃 𝑚 is the rota on angle at motor sha
𝐽𝑚 is the moment of iner a at the connec on
𝜃𝑠 is the rota on angle at the load
𝐽 𝑙 is the moment of iner a of subsequent component on the link
𝐵 𝑚 is the damper coe cient of motor
𝜏 𝑚 is the torque at motor sha 𝐵 𝑙 is the damper coe cient of load
𝜏𝑙 is the torque of iner a at load
Figure General diagram of electromechanical system for a robot link 25
From transmission ra o, we have:
𝜃𝑠 = 𝑑𝑡 𝜕𝜃 = 𝑠 𝜔 𝑠 is the angular velocity of load (Rad/s)
𝑑𝑡 is the angular accelera on of load (Rad/s ) 2
Equa on (4.3) is equivalent with:
𝜏 𝑙 −𝐵 𝜔 𝑙 𝑠 =𝐽 𝑙 𝜔 𝑠 (Eq.4.4) Apply d’ Alembert principle for the motor sha , we have:
We de ne 𝐽 𝑒𝑓𝑓 𝑎𝑛𝑑 𝐵 𝑒𝑓𝑓 such that:
𝐽𝑒𝑓𝑓= (𝐽 𝑎 +𝐽𝑚+𝑛 2 𝐽𝑙) is the total e cient moment of iner a act on the motor sha
𝐵𝑒𝑓𝑓= (𝐵 𝑚 +𝑛 2 𝐵𝑙) is the total e cient damper coe cient act at the motor sha Then (4.6) is equivalent to:
Since all motors used in the robot are DC motor, the electrical connec on of the motor can be represented as in Figure 26 The back electromo ve force 𝑣 𝑏 (𝑡) shown in the Figure can be calculated as:
𝑣 𝑏 (𝑡)=𝐾 𝑏 𝜃 𝑚 (𝑡) =𝐾 𝑏 𝜔 𝑚 (𝑡) (Eq.4.8) With 𝐾 𝑏 is the constant of propor onality called the back EMF constant (V.s / Rad) L and
R are the inductor and resistor of the armature circuit, respec vely Using Kirchho Voltage Law, we obtain the following equa on:
𝑑𝑡+𝑅𝑖=𝑣(𝑡)−𝐾 𝑏 𝜔 𝑚 (𝑡) (Eq.4.9) Taking the Laplace transform, we get:
(𝐿𝑠+𝑅) 𝐼(𝑠) =𝑉(𝑠) −𝐾 𝑏 𝑊(𝑠) (Eq.4.10) Since the DC motor working in the linear region, the moment developed by the motor will be propor onal to the armature current:
With 𝐾 𝑚 is the motor torque constant, which depends on the motor and the magne c eld characteris cs Taking Laplace transform to equa on (4.7), we get:
Subs tute result from (4.14) to (4.10):
With the assump on that the armature inductance, L, is small compared to the armature resistance, R, which is usual for a DC motor, we can approximate (4.15) and (4.16) as:
This is the transfer func on with input signal is the voltage applied to the DC motor and output signal is the angular posi on of the motor sha :
Transfer func on for motor of link 1:
Transfer func on for motor of link 2:
𝑉 2 (𝑠) (Rad/Volt) (Eq.4.20) For link 3, we have the transla on mo on with ball-screw system Thus, we need another component to nd the transfer func on of link 3 Consider the ball-screw system with input signal is the angular velocity of screw 𝜔(𝑡) and the output is the displacement 𝑦(𝑡) of the grasp connect with the ball screw
Figure Ball-screw system 26 With P (m) is the pitch of screw, we have equa on as:
𝑦(𝑡) = 𝑃 2𝜋 ∫ 0 𝑡 𝜔(𝑡)𝑑𝑡 (Eq.4.21) With zero ini al condi on, taking Laplace transform:
Then the transfer func on for ball-screw mechanism is:
Figure Block diagram of ball-screw 27
The transfer func on of link 3 transmits through the ball-screw:
The transfer func on through motor for link 3:
4.1.1 Calculate transfer func on of power transmission for each link:
Figure Model for total transfer function of link 1 28
The input signal is the voltage applied to the motor, output is the rota ng angle of link 1 Now we will nd all parameters to calculate transfer func on 𝐺 𝑚1 (𝑠)
Moment of iner a 𝐽 𝑙1 of link 1 is determined as the iner al moment along calculate along the z-axis and act on joint 1 It includes all iner al moment of subsequent link By using the SolidWorks, we obtained the result (where blue parts are calculated parts):
Figure Model for calculate inertia moment for link 1 29
Figure Mass properties of subsequent links act on link 1 30
From evalua on, maximum value of 𝐽 𝑙1 equal 9.88 (kg.m ) 2
Figure Information of motor for link 1 31
𝑟𝑎𝑑) With all the parameters as:
Then we obtained the transfer func on of motor for link 1 by equa on (4.16): 3
𝑉𝑜𝑙𝑡) The total transfer func on for link 1:
𝑉𝑜𝑙𝑡) (Eq.4.27) Nyquist Diagram for transfer func on 𝐺 1 (𝑠):
Figure Nyquist Diagram of transfer function G (s) 32 1
From the plot, we no ce that the point (-1+j0) with the red mark (+) is not covered by the Nyquist diagram Thus, the closed loop system is stable by Nyquist Criterion
Figure Bode diagram of transfer function G (s) 33 1
With Bode plot, for a closed loop with transfer func on 𝐺 𝑐 ( )𝑠 to be stable then the Bode plot of the open loop 𝐺 𝑜 (𝑠) must have both gain and phase margin (Gm and Pm respec vely) larger than 0 From Figure 34, it is obvious that Bode plot of 𝐺 1 (𝑠) has gain margin Gm and phase margin Pm both larger than 0 Then we can conclude that this close loop is stable
Figure Model for transfer function of link 2 34With the same procedure as calculate for link 1, we nd moment of iner a for 𝐽 𝑙2 :
42 Figure Model for calculate inertia moment for link 2 35
Figure Mass properties of selected part 36
Figure Information of motor for control section 2 37
𝑟𝑎𝑑) With all the parameters as:
The transfer func on through motor of link 2: 3
𝑠 (𝑅𝑎𝑑 𝑉𝑜𝑙𝑡) Total transfer func on of link 2:
𝑉𝑜𝑙𝑡) (Eq.4.28) Nyquist Diagram for transfer func on 𝐺 2 (𝑠):
Figure 38 Nyquist Diagram of transfer function G (s) 2
From the plot, we no ce that the point (-1+j0) with the red mark (+) is not covered by the Nyquist diagram Thus, the closed loop system is stable by Nyquist Criterion
Figure Bode diagram of transfer function G (s) 39 2
With Bode plot, for a closed loop with transfer func on 𝐺 𝑐 ( )𝑠 to be stable then the Bode plot of the open loop 𝐺 𝑜 (𝑠) must have both gain and phase margin (Gm and Pm respec vely) larger than 0 From Figure 34, it is obvious that Bode plot of 𝐺 2 (𝑠) has gain margin Gm and phase margin Pm both larger than 0 Then we can conclude that this close loop is stable
Figure Block diagram for total transfer function of link 3 40
Input signal is the voltage applied to motor, output signal is the transla on displacement of mechanism a ached to ball screw (Link 3)
P is the pitch of ball screw: P = 0.0 m 25
Moment of iner al for link 3:
Figure Selected part to calculate inertial moment 41
Figure Mass properties of selected part 42
Figure Motor information of link 3 43
𝑟𝑎𝑑) With all the parameters as:
The transfer func on through motor of link 3:
𝑠 (𝑅𝑎𝑑 𝑉𝑜𝑙𝑡) Total transfer func on of link 3:
𝑉𝑜𝑙𝑡) (Eq.4.29) Nyquist Diagram for transfer func on 𝐺 3 (𝑠):
Figure 44 Nyquist Diagram of transfer function G (s) 3
From the plot, we no ce that the point (-1+j0) with the red mark (+) is not covered by the Nyquist diagram Thus, the closed loop system is stable by Nyquist Criterion
Figure Bode diagram of transfer function G (s) 45 2
With Bode plot, for a closed loop with transfer func on 𝐺 𝑐 ( )𝑠 to be stable then the Bode plot of the open loop 𝐺 𝑜 (𝑠) must have both gain and phase margin (Gm and Pm respec vely) larger than 0 From Figure 34, it is obvious that Bode plot of 𝐺 2 (𝑠) has gain margin Gm and phase margin Pm both larger than 0 Then we can conclude that this close loop is stable
4.1.2 Construct Simulink diagram to model one link of the robot:
Apply d’ Alembert principle for one link of robot:
𝑑𝑡) From Kirchho ’s rule for motor circuit:
𝑑𝑡) From these results, the Simulink diagram for link 1 of the robot is:
Figure Simulink diagram for transfer function of link 1 servo motor 46
Figure Transfer function of servo motor for link 1 47
Simula on and evaluate stability and other quality speci cs
Figure Simulink model for link 1 with negative feedback 48 From (4.27), the open loop total transfer func on for link 1:
With nega ve feedback, the equivalent closed-loop transfer func on for link 1 is:
By similar procedure, we have:
Figure 49 Simulink model for link 2 with negative feedback From (4.28), the open loop total transfer func on for link 2:
8.2 ×10 −4 𝑠 3 + 0.11 𝑠 2 + 0.89𝑠 With nega ve feedback, the equivalent closed-loop transfer func on for link 2 is:
Figure 50 Simulink model for link 3 with negative feedback From (4.29), the open loop total transfer func on for link 3:
1.56 × 10 −5 𝑠 3 + 10 × 10.2 −3 𝑠 2 + 1.78 𝑠 With nega ve feedback, the equivalent closed-loop transfer func on for link 3 is:
4.2.2 Calculate steady state error and evaluate response quality:
We will evaluate the error and response characteris cs of each link with a step signal input With closed-loop transfer func on for each link calculated for previous part, we obtained the result as:
Figure Step response for link 1 51
- Steady state: Final value equal 1, thus steady state error equal 0 and system is stable 4.2.2.2 Link 2:
Figure Step response for link 2 52
- Steady state: Final value equal 1, thus steady state error equal 0 and system is stable 4.2.2.3 Link 3:
Figure Step response for link 3 53
- Steady state: Final value equal 1, thus steady state error equal 0 and system is stable.
CONTROL SYSTEM DESIGN
PID controller
Figure Block diagram of a PID controller 55
With purpose of control the robot to a desire position 𝑥 𝑑 correspond to some joint coordinate with a required trajectory For this project, we consider a simple and popular 𝑞𝑑 control rule is PID Voltage signal is controlled by adjust propor onal to error of posi on and velocity of present with set value
According to PID rule, the voltage value is calculated as:
𝑒(𝑡): error func on of desire joint variable value with value of joint variable measure at the me t
𝑞 𝑑 (𝑡): value of desired joint variable at time t, which can be obtained from the trajectory design
𝑞(𝑡): actual value of joint variable at me t, which can be obtained from feedback system of the robot (rota ng angle sensors integrated on the robot)
Figure Simulink diagram for complete motion control of a robot link 56
Figure Simulink diagram of controller block 57
Modeling control system for SCARA robot
Figure 3D model of robot imported to Matlab 58
Model for control system of SCARA robot:
Figure Modeling of control system for SCARA robot with Simulink 59
With input signal are desired joint variables, which are calculated from Matlab func on for a speci c trajectory These values then pass through the controller block, transmission system, and 3D model of robot to process physical operation on the robot By measure physical state of robot, we obtained the output signals are real joint variables of robot
For modeling the control system, the subsystem of SCARA model is built by import the 3D model of the robot to Simulink:
Figure Model of SCARA robot in Simulink 60
With input signals are joint value from controller block and transmission system, output signals are value of joint variables measured from encoders and sensors from 3D model.
Determine parameters for PID controller
Using Tune PID tool given by Matlab to choose the set of PID coe cients suitable for each link At Select tuning method sec on, click Tune… bu on to start tuning and choosing PID set of parameters
Figure Model for PID tuning and checking for each link 61
Figure Block PID controller in Simulink 62
At Tuning Tools sec on, changing two sliders to obtain the diagram as desired Press
Update block to save the new set of parameters to PID block
Result obtained a er nish tuning for each PID block is shown in the table below:
Figure Step response for link 1 with the set of chosen PID parameters 64
- Steady state error is zero and system is stable
Figure Step response for link 2 with the set of chosen PID parameters 65
- Steady state error is zero and system is stable
Figure Step response for link 3 with the set of chosen PID parameters 66
- Steady state error is zero and system is stable
Then the system has sa s ed all requirements at every links for percent of overshoot, se ling me, and steady state error With these results, we now obtain a complete control system for the SCARA robot for a pick-and-place process, which is designed in part 3.2.3
58 Figure Diagram of set and real value of joint variable 1 67
Figure Joint variable different diagram over time of link 1 68
Figure Diagram of set and real value of joint variable 2 69
Figure Joint variable different diagram over time of link 2 70
60 Figure Diagram of set and real value of joint variable 3 71
Figure Joint variable different diagram over time of link 3 72