Motion Control of Three Links Robot Manipulator (Open Chain) with Spherical Wrist

Robot manipulator is a multi-input multi-output system with high complex nonlinear dynamics, requiring an advanced controller in order to track a specific trajectory. In this work, forward and inverse kinematics are presented based on Denavit Hartenberg notation to convert the end effector planned path from cartesian space to joint space and vice versa where a cubic spline interpolation is used for trajectory segments to ensure the continuity in velocity and acceleration. Also, the derived mathematical dynamic model is based on Eular Lagrange energy method to contain the effect of friction and disturbance torques beside the inertia and Coriolis effect. Two types of controller are applied ; the nonlinear computed torque control (CTC) and the simpler form of its Proportional Derivative plus Gravity (PD+G) where they are designed to reduce the tracking trajectory errors which tend to zero where the used Kp and Kv gains are 900,60. Also, the RMS errors for tracking a step input of CTC were equal to [2.5E-14, 4.4E-14, 5.0E-14, -4.7E-14, 3.9E-14, -4.6E-14] (deg) and of PD+G were equal to [-1.77E-5, -1.22E-6, -4.28E-6, -8.97E-6, -1.32E-5, 1.05E-5] (deg) for joints one to six, respectively. The results show that CTC is more accurate but requires additional acceleration input and is more computationally extensive and PD+G controller is performed with acceptable tracking errors in manipulator position control applications.


Introduction
The precise dynamic model of a robot is an important step to achieve high performance robot control [1]. The linear classical controllers are commonly used to control industrial robot but they had limitation because the uncertainty in dynamic model and not sufficient response in different wide ranges so there is a need to applied a nonlinear controller to achieve suitable trajectory tracking by linearizing the system and cancelling the nonlinearly in the dynamic model. H. AL-Qahtani et al [2] presented the dynamics and control of a robotic having four links where the manual tuning PID gains obtains a satisfactory manipulator response, while Wathik and Wael [3] presented the modeling and control of the LabVolt 5250 robot arm 5 DOF by assuming each link is a homogenous cylinder, F. Pitan et al [4] presented a CTC method using matlab Simulink for puma 560 robot manipulator position, based on the simplified mathematical dynamic model of Armstrong et al [5], D. Receanu [6] presented the basics of nonlinear CTC application for a two link planner robot where the PD gain are selected for best performance ( critical damping), MATLAB Simulink was used to simulate an arbitrary circular path to examine the analysis, H. AL-Dois et al [7] presented a comprehensive study of dynamic performance of serial robot manipulator where they presented a numerical example for PUMA 560 as an illustrative case, the kinematic manipulability and dynamic manipulability as end effector performance are measured. J. Kim et al [8] present the dynamic modelling and motion control for an arbitrary three links robot manipulator using the PD-CTC as commanded joints torques and extract the input signals by using image processing technique where a pinhole camera is used to capture the motion of a user's arm as reference angle inputs and simulate the tracking motion.
In this work; motion control of a three link robot arm with spherical wrist (six DOF) as shown in Figure (1) is done by two stage the first one is a trajectory planning by specifying the cartesian positon and orientation of the end effector and transform this via points to joint space by application of inverse dynamic also the cubic polynomials are generated for each interval of time by using cubic spline interpolation, the second stage is the trajectory tracking by applying two type of controller CTC and the simpler form of its PD +G using MATLAB Simulink where the resulted joint response was inverted to cartesian space again by the forward kinematic transformation matrix. The paper is organized sequentially as follow: manipulator forward and inverse kinematic, dynamic modelling, trajectory planning, controller design, result and discussion and conclusions.

Manipulator Forward and Invers kinematics
In this paper, Denavit Hartenberg (DH) convention is based to solve the forward kinematics of the manipulator by attaching coordinate frame system at each joint and specifying the four parameter of DH(αi, ai, θi and di )where: [9] ai: (link length) is the distance between zi-1 and zi axes along the xi axis. αi: (link twist) is the required rotation of zi-1 to zi axes about the xi axis. di: (joint offset) is the distance between xi-1 and xi axes along the zi-1 axis. θi: (joint angle) is the required rotation of xi-1 to xi axes about the zi-1axis. The transformation matrix of frame {i} relative to previous frame {i-1} is: In other side the inverse kinematics is subdivided to end effector position problem and orientation problem [10], the position vector of transformation matrix from base to wrist center is: Can be solved numerically by Newton -Raphson method: [11] ) ( ) ( .. (4) Where: i = arbitrary counter.
[ ] , Which can be taken from Pw vector in equation (3). 1  The orientation matrix of end effector may be calculated as: And the solution is computed as ZYZ Euler angles when θ ହ ∈ ሾ0, πሿas follow: [12]

Manipulator Dynamics Modelling
By Eular -Lagrange formulation the equations of motion can be derived as: ...(9) Where: L = K-U The kinetic energy of i th link and of i th motor is: .. (11) Where: v and ω: linear and angular velocities of mass center respectively. I 0 : mass moment in base frame coordinates. Since: (13) And the rotation of principle inertia axes of i th link is: .. (14) As shown in figure (3), the total kinetic energy for the assigned robot manipulator is: ..(15) Substituting equations (12)(13)(14) in (15): ..(18) And the potential energy of i th link and i th actuator is: ..(20) Where: g: gravity acceleration vector in base coordinate frame which is ሾ0 0 −9.81ሿ And the total potential energy of the robot manipulator: By noticing that U doesn't depend on q& then equation (9) can be written as: ..(23) By substituting equations (17) and (22) into equation (23), getting that: ...(28) As a result the equations of motion are: And the final matrix form of equations can be written as: [13] Christoffel symbol first type [14] v F : Viscous friction coefficients, diagonal matrix (6x6).
..(32) In order to solve the arm dynamic, equation (32) may be converted to ordinary differential equation system (ODEs). So the state space representation of the robot manipulator (MIMO system like) can be written as: [15] { }

Trajectory Planning
The time behavior of a pre-planned path called trajectory planning, a cubic polynomial is the lowest order that can be used to satisfy the position and velocity constrains at the initial and final joint points. where each joint starts and ends its motion at the same time, each via points of a divided path segments are converted into a set of desired joints angles by application of the inverse kinematics where cubic spline interpolation schemes is used to compute the functions for each interval as shown in Figure (4).
Since each pair of knots is connected by cubic function so the second derivative within each interval is straight line and equal zero for ends knots (conditions), the function of interior interval can be written as: [12] ( ) Where Π && can be evaluated from equation (16) which is based on the fact that first derivative at knots must be continuous. [12].

Controller Design
The system is termed as an open loop system by applying a feed forward torque and closed loop system by applying feedback control torques as illustrated in figure (5). Since is too difficult to cover the exact dynamic model then the closed loop controller ensure to vanishing the dynamics errors.
To track trajectory, the joint variable error may defined as: ... response. [15] The overall computed torque input become as shown in figure (6)

Results and Discussions
Matlab m-files and Matlab Simulink is used to embrace the theoretical work as shown in figure (7) Figures (8 & 9) show the dynamic response from rest to rest of a step input with zero friction and external forces where the PD gains are subjected on manual tuning with value n ω = 30 rad/s, the settling errors for joints one to six respectively are [2.5E-14, 4.4E-14, 5.0E-14, -4.7E-14, -3.9E-14, -4.6E-14] (deg) which may be considered as zero errors. Also figure (10) shows the computed torques which start with relative high due to high error and settling on [-4.22E-12, -2.74, -4.33, -1.56E-3, -1.58, 4.19E-2] (N.m), that show the maximum torque at joint 2 and 3 because the gravity play roll in increasing the holding torques in vertical plane. Figure (11) shows the dynamic response of step input using the simpler PD +Gravity controller were the RMS of errors were [-1.77E-5, -1.22E-6, -4.28E-6, -8.97E-6, -1.32E-5, 1.05E-5] (deg). The tracking results were satisfactory relative to the proposed model, also PD +Gravity easier in application since there is no need to feed a reference inputs of velocity and acceleration. Figure (12

Conclusions
From the control point of view, the computed torque control technique guaranteed to reduce the tracking error tends to zero through linearizing the system, and specifying the desired trajectory , & des des des q q q & && . The results show that CTC more accurate but required additional acceleration input and computational extensive while PD+G controller is perform with acceptable tracking errors in manipulator position control applications like pick and place. So the PD+G can be used to track a continuous path with via points with acceptable errors and satisfactory robot performance.