Trajectory Planning of Five DOF Manipulator: Dynamic Feed Forward Controller over Computed Torque Controller

Trajectory Planning of Five DOF Manipulator: Dynamic Feed Forward Controller over Computed Torque Controller

Maged M. Abou Elyazed,

Mohamed H. Mabrouk, Mootaz E. Abo Elnor, Hussein M. Mahgoub

Mechanical Equipment Department M.T.C.

Cairo, Egypt

Abstract Industrial robot control has been a challenge for decades especially for those manipulators which are assigned to work in hazardous environment. The DFF (Dynamic Feed- Forward) controller and the CTM (Computed-Torque Method) based controller, are generally used for dynamic control of industrial robots. This paper presents a comparison of the two controllers applied on a five DOF manipulator case study. To verify the design of the controllers, the dynamic models of the manipulator used in this work are developed in two different ways. Firstly, the model is designed through Newton-Euler method for real time computation. Secondly the model is designed through simulations using Matlab Sim-mechanics for evaluating the developed controller. The experimental setup depends on using both controllers with a five DOF educational manipulator. For data analysis, a comparison between the computed torque controller and the dynamic feed forward controller is held on the same system under same conditions. The results show the efficiency of using the dynamic feed forward controller over the computed torque controller as a basis for manipulators' trajectory planning.

Keywords Graphical User Interface, Industrial Robots, Robot Modeling And Control.

1. INTRODUCTION

   It is well established that robot manipulators are dynamically coupled, time-varying, and highly nonlinear systems that are extensively used in industrial applications. The traditional manipulator control schemes are based on independent joint control or classical linear control as a Single-Input/Single Output (SISO) system and any coupling effects due to the motion of the other links can be treated as a disturbance.

   Practically, Euler-Lagrange dynamic equations of robot form a nonlinear and Multi-Input/Multi Output (MIMO) multivariable system [1]. While the end effectors of the robot manipulators are to follow some desired trajectories as accurate as possible, the trajectory tracking problem becomes more complicated.

   A conceptually simple nonlinear controller, called computed-torque controller, can fully compensate the nonlinear Coriolis and centripetal forces (continuous nonlinearities) in the robot motion and lead to high accuracy control for a variety of robot speeds and different shape

   like dealing with hazardous materials and other sensitive applications, the precise control of the robot arms has become an essential requirement. Motivated by such control requirements, for practical and complex control problem of robot manipulators, many research contributions have been reported in the field of robotic control schemes such as proportional-integration-derivative (PID) control, PD control, PI control [2], feed-forward compensation control (DFF) [3], adaptive control [4,5], variable structure control [6], computed torque control (CTC) [1,7,8].

   This paper presents the separate use of CTC controller and DFF controller with a 5-DOF robotic manipulator case study for trajectory tracking problem. The performance of CTC controller is then compared to that of DFF controller through calculating of the error in robot end-effector position with respect to the desired position for each controller. The dynamics of the robot manipulator is modeled using Matlab® Sim-mechanics toolbox. We modify the toolbox to include online evaluation of a nonlinear gravity compensation term to instantaneously compute current robot configuration. The use of such toolbox allows the possibility to verify model-based control algorithm [9, 10].

2. MATHEMATICAL MODEL

   1. Newton-Euler Model

      Now the problem of computing the joints' torques corresponding to a given trajectory of a manipulator is considered. Assuming that the position, velocity, and acceleration of the joints are determined, the joint torques corresponding to the required trajectory can be calculated using the kinematics and the mass distribution information of the robot. The algorithm for computing joint torques includes two main processes. First, link velocities and accelerations are iteratively computed from link 1 to link n applying Newton-Euler equations for each link. Second, forces and torques of interaction and joint actuator torques are computed recursively from link n back to link 1. For the outward iterations from link 1 to link n (i from 0 to n-1 ), the algorithm equations for the case of all rotational joints manipulator of n joints are as follows [11],

      i1 Ri1i Zi1

      workspace. With the use of the robots in critical applications

      i1 i i i1 i1

      (1)

      R R x Z Zi1

      ¨

      i1 i1 i i1 i i1 i1

      i1 i i i i i1 i1 i1

      (2)

      Vi1 Ri1 (i xPi

      • i x i xPi

        Vi )

        i1 i i i1 i i i1 i

        Vi1 i1xPi1 i1 x i1 x Pi1 Vi1 )

        (3)

        ci1

        i1 ci1 i1 i1 ci1 i1

        (4)

        F m V

        i1 i1

        i1 i1 ci1

        N I xI

        i1 i1 i1 i1 i1 i1 i1 ci1 i1 i1 ci1 i1

        Inward iterations from link n to link 1 (i from n to 1 )

        (5)

        (6)

        Fig. 1. RV-2AJ robot arm CAD model

        f i Ri

        f i1 Fi

        i i1 i1 i

        ni Ni1 Ri

        ni1 Pi xFi Pi

        xRi

        f i1

        (7)

        Fig. 2. RV-2AJ robot manipulator model

        i i1 i1 i1 ci i i1 i1 i1

        n Z

        Ti i

        i i i

        (8)

        (9)

3. EXPERIMENTAL SETUP

   In this paper, we use a five DOF Mitsubishi® RV-2AJ

   Where is the rotational velocity, is the angular acceleration, V is the linear acceleration from link to link, Vc is linear acceleration of the mass center, F is the inertial force; and N is inertial Torque at mass center of each link. f and n are force and torque so that they appear as iterative relationships from higher-numbered neighbor to lower- numbered neighbor. Finally, R is rotation matrix, P is

   educational manipulator, as shown in Fig. 3. To feed the robot with the updated target point position calculated by the developed target positioning system, serial communication between the computer and robot controller need to be installed. MELFA basic language is used through MATLAB® environment to control the manipulator via the computer serial port [12]. Both CTC based controller and DFF controller are used one by one for controlling the robot manipulator for trajectory tracking problem. The design of both controllers is

   position vector from joint to joint, and

   from joint to center of mass.

   1. Matlab® Sim-mechanics Model

   Pc is position vector

   explained in the next section.

   A graphical user interface (GUI) environment, shown in Fig. 4, is developed to facilitate the communication between the computer and the robot. Path defining which is simply

   Sim-mechanics is a Matlab® toolbox that enable importing CAD model from a CAD environment to the Simulink environment as blocks of links connected by joints. Moreover, Simulink provides users with a tool to specify bodies and their mass properties, their possible motions, kinematic constraints, coordinate systems and the means of initiating and measuring motions. This makes it easier to do the analytical modeling without going through a complex modeling process. Firstly, a CAD model for our case study, the fiveDOF (RV-2AJ) robot manipulator, needs to be built. Autodesk Inventor® is used as a suitable platform for building such a model, as shown in Fig. 1. Then the manipulator CAD model is imported to Matlab® Simulink as an XML file by the aid of Sim-mechanics toolbox. The RV-2AJ robot manipulator model is shown in Fig. 2.

   determining the dominant points that define a path is performed easily through the interface. Also the interface gives charts which represent on-line evolution of robot end effector current position related to the defined path and robot joints operating parameters with time.

   Fig. 3. Experimental setup (Mitsubishi® RV-2AJ) educational manipulator

   Fig. 4. Graphical user interface (GUI)

4. DECOUPLING CONTROL

   Decoupling control can be accomplished by using the feedback linearization of nonlinear systems, Lyapunove design, or inverse dynamics control. The work presented in this paper is influenced by feedback linearization control method to solve the problem of manipulator control. In some sense, the linearizing control law implements an inverse model of the system being controlled. The nonlinearities in

   Fig. 5. A model-based manipulator-control system

   The model-based manipulator-control system is shown in Fig. 5. Using equations from Eq. (10) to Eq. (15); it is easy to show that the closed-loop system is characterized by the error equation as follows,

   E K E K E 0

   the system cancel those in the inverse model; these, together

   d v p

   (16)

   with a servo law, results in a linear closed-loop system. There are a many control algorithms for robot manipulators that depend on decoupling control. This paper used two of the

   Equation (16) can be written on a joint-by-joint basis to be

   most commonly used dynamic controllers for the industrial robot manipulators; the CTC (Computed Torque Controller)

   ed Kve K pe 0

   (17)

   and DFF (Dynamic Feed-Forward) controller.

   1. Computed Torque Controller (CTC)

      In the previous section a model and the corresponding equations of motion of the robot is developed, and the dynamics are simply described by the following equation

      The computed torque controller computes the on-line

      dynamics of the manipulator, using the sampled joint position and velocity data. where; V is centrifugal and coriolis terms, G () is gravity terms, is the joint positions, torque needed to achieve the given trajectory, d joint target positions, E is the error between target and actual position. Kp controller proportional constant of the PD controller, Kv controller

      derivative constant of the PD controller.

      M V , G()

      (10)

      It can be seen from Fig. 6 that the computed torque is

      The problem of controlling a complicated nonlinear robot manipulator system can be handled by the partitioned controller scheme presented in [5, 11, 13]. This control scheme has two parts, the linearizing model-based portion expressed by equations from Eq. (11) to Eq. (13) and the servo-law portion expressed by equations from Eq. (14) to Eq. (15) as follows,

      calculated on-line by two function blocks ( M , and V, G() ). Both of these blocks consist of Matlab® m-file based on Newton Euler method to calculate required torque. The second block contains coriolis, centripetal, and gravity forces (continuous nonlinearities in the robot motion). These nonlinearities are calculated online using the measured values for positions (tp, tp, tp, th4, and tp) and the

      M

      V , G()

      d Kv E K p E

      (11)

      (12)

      (13)

      (14)

      measured velocities (thd1, thd2, thd3, thd4, and thd5) for the five actuators respectively. These nonlinearities are delivered to the robot model as (C.T1, C.T2, C.T3, C.T4, and C.T5) to compensate its nonlinearities, therefore the robot system is linearized. On the other hand the error between the desired and the measured positions and velocities are calculated and then delivered to PD controller that work as a servo loop outside the linearized inner loop (linearized robot system) to

      get (T1,T2,T3,T4,T5), where

      • K E K E . The PD

      E d

      (15)

      d v p

      controller function block delivers the robot model with

      as (B.T1, B.T2, B.T3, B.T4, and B.T5)

5. RESULTS AND DISCUSSION

The results are discussed here in two main perspectives; the controllers performance during the path tracking process and the robustness of the controllers in spite of parameters uncertainties.

1. Controllers Performance during Path Tracking

   For evaluating the performance of each controller, we use the CTC controller and the DFF controller to control the motion of the 5-DOF RV-2AJ robot manipulator to perform a linear quantic trajectory in three dimensional spaces forcing the path to have specific position, velocity, and acceleration at boundaries, so a fifth order polynomial is needed to express

   0 1 2 3 4 5

   Fig. 6. Matlab® function block diagram for the CTC controller

   Xt a a t a t2 a t3 a t4 a t5

   (21)

2. Dynamic Feed Forward (DFF) Controller

DFF controller is usually used as an alternative to the CTC controller for the on-line computation requirements. The dynamic model is computed as a function of the desired path only, and so when the desired path is known in advance, values could be computed off-line before motion begins. At run time, the pre-computed torque histories would then be read out of memory. The control scheme for this kind of

The other coordinates kinematic behavior is determined as functions of (X) by a geometric space curve. In this case all velocities and accelerations at the start and the end are assigned to be zero. Fig. 8 and Fig. 9 respectively show the time evolution of position, velocity, and acceleration along the presented path for the robot end-effector and the robot joints.

end-effector position

300

250

X-axis

200 Y-axis

Z-axis

controllers is shown in Fig. 7.

It is clearly seen in Fig. 7 that the model based portion is outside the servo-law portion. It is worth mentioning that servo portion is the portion that work to eliminate the error using the feedback measured positions and velocities. It can be described in equation form from Eq. (18) to Eq. (20) as follows

150

100

end-effector velocity end-effector acceleration

0.05

0

-0.05

0.02

10 20 30 40 50 60 70 80 90 100

time

10 20 30 40 50 60 70 80 90 100

time

T1 T 2

T1 M V , G()

(18)

(19)

0.01

0

-0.01

T 2 EK

p EKv

(20)

-0.02

10 20 30 40 50 60 70 80 90 100

time

Fig. 8. Robot end-effector position, velocity, and acceleration along the along the quantic path versus time.

Fig. 7. Control scheme with the model-based portion "outside" the servo

100

joint angle

50

0

-50

-100

joint velocity

10

0

-10

joint acceleration

5

0

-5

Joint1

Joint2

Joint3

Joint4

10 20 30 4050 60 70 80 90 100

time

60

60

Fig. 9. Robot joints position, velocity, and acceleration along the quantic path versus time

end-effector position(x-axis)

160

150

position(mm)

140

130

120

110

100

267.005

0 2 4 6 8 10 12

Time

(a)

Refrence

CTC

DFF

end-effector position(y-axis)

-3

x 10

14

12

(a)

Path tracking error(y-axis)

10

267

position(mm)

8

Error

6

266.995 4

2

266.99

130.01

0 2 4 6 8 10 12

Time

(b)

end-effector position(z-axis)

0

-2

0 2 4 6 8 10 12

Time

(b)

130

129.99

position(mm)

129.98

129.97

Refrence CTC

DFF

0.09

0.08

0.07

0.06

Path tracking error(z-axis)

129.96

129.95

129.94

129.93

0.05

Error

0.04

0.03

0.02

129.92

0 2 4 6 8 10 12

Time

(c)

0.01

0

0 2 4 6 8 10 12

Time

Fig. 10. Path tracking performance for end-effector (a) x-axis, (b) y-axis, and

(c) z-axis

Fig. 10 (a, b, c) shows the end-effector position in x, y, and z coordinates respectively versus time using the CTC controller presented in solid line and the DFF controller presented in dashed line. The figure shows that both controller matching the desired Cartesian path, however results show that path tracking performance for DFF controller is closer to the reference path than that of CTC controller especially for y and z axes.

(c)

Fig. 11. End-effector position error in (a) x-axis, (b) y-axis, and (c) z-axis

Fig. 11 (a, b, c) respectively show the end-effector position error along x, y, and z axes versus time along the path for both CTC and DFF control methods with and without load. It is obviously seen that DFF have a better accuracy than the CTC. In the other hand for both controllers, accuracy has a noticeable change with load adding, but DFF still more accurate than CTC. These results verify the superiority of DFF controller over the CTC controller. The maximum error along the path with CTC method is about 78*10-6 m, in the other hand, the maximum error along the path with DFF method is about 45*10-6 m.

B. Robustness of the Controllers to Uncertainities

In robot control algorithm applications the problem of handling the uncertain parameter is very important. In this work, the controller robustness is evaluated by using uncertain payload mass where the same controllers are used to control the same robot in two cases. The first case is when the robot grips the maximum gripping load and the other case when it has an empty gripper.

End-effector position(x-axis)

Fig. 12 (a, b, c) shows the robot end-effector position in x, y and z coordinates respectively versus time using the CTC controller and the DFF controller both with and without load. The figure shows that the path error becomes larger when load is added, especially in case of CTC. Error increases because of the sensitivity of these methods to the model parameters. So the accurate parameters are necessary for dynamic control. It can be noticed that path error become

larger when uncertainties are added, especially in case of CTC. The error increases because of the sensitivity of these

160

150

position(mm)

140

130

120

110

100

0 2 4 6 8 10 12

Time

(a)

End-effector position(y-axis)

methods to the model parameters. So the accurate parameters are necessary for dynamic control.

CONCLUSIONS

Robot path planning is one of the most important tasks especially for manipulators that need to follow a special path in hazard environments. In this paper, the performance of two controllers, dynamic feed forward (DFF) controller and the computed torque method (CTM) based controller, have been experimentally investigated using both controllers with a 5 DOF educational robot manipulator (Mitsubishi® RV-2AJ). The results show that the designed controllers are effective and useful. Also the results prove that DFF controller is better than CTC based controller, and more Robust. For some extent of system parameter uncertainties, the effectiveness of dynamics based model is still superior to the conventional

267

position(mm)

266.995

one. But for the best performance, exact information of system parameters is required.

REFERENCES

266.99

266.985

130.01

130

129.99

129.98

position(mm)

129.97

129.96

129.95

129.94

129.93

129.92

129.91

0 2 4 6 8 10 12

Time

(b)

End-effector position(z-axis)

1. F.L. Lewis, C.T. Abdallah, and D.M. Dawson, Control of Robot Manipulators, New York: Macmillan, 1993.

2. J.Y.S. Luh, "Conventional controller design for industrial robots-a tutorial", IEEE Trans. Systems Man Cybernet. 13 (1983) 298316.

3. P.K. Khosla, T. Kanade, "Experimental evaluation of nonlinear feedback and feedforward control schemes for manipulator", Int. J. Robot. Res. 7 (1988) 1828.

4. J.J.E. Slotine, W. Li, "Adaptive manipulator control: a case study", IEEE Trans. Automat. Control 33(1988) 9951003.

5. FarzinPiltan, N. Sulaiman, A. Jalali& F. Danesh Narouei, Design of model free adaptive fuzzy computed torque controller: applied to nonlinear second order system, International Journal of Robotics and Automation, 2 (4):232-244,2011.

6. Mark W.Spong and M.Vidyasagar, Robot Dynamics and Control, John Wiley & Sons,2004.

7. Zuoshi Song, Jianqiang Yi, Dongbin Zhao, Xinchun Li, A computed torque controller for uncertain robotic manipulator systems: fuzzy approach Fuzzy Sets and Systems 154 (2005) 208226, China.

8. C. S. Kim, K. W. Lee, "Robust control of robot manipulators using dynamic compensator", Control and Automation, 2009.ICCA 2009.

   IEEE International Conference, 9-11 Dec., pp 10071012,2009

9. FarzinPiltan, Mohammad Hossein Yarmahmoudi, PUMA-560 robot manipulator position computed torque control methods using matlab/simulink and their integration into graduate nonlinear control and matlab courses IJRA, Volume (3): Issue (3): 2012

10. DanutReceanu Modeling and simulation of the nonlinear computed torque control in simulink/matlab for an industrial Tech Science Press SL, vol.10, no.2, pp.95-106, 2013

    0 2 4 6 8 10 12

    Time

    (c)

    Fig. 12. Path tracking performance for the end-effector with & without load for (a) x-axis, (b) y-axis, (c) z-axis

11. John J. Craig, Introduction to Robotics Mechanics and Control, third edition, Pearson Prentice Hall, 2005.

12. Mitsubishi Melfa RV-2AJ manipulator user manual, 2010.

13. Reza N. Jazar, Theory of Applied Robotics, second edition, Springer Science+Business Media, LLC 2010