Mechatronic Model Based Computed Torque Control of a Parallel Manipulator

Abstract

With high speed and accuracy the parallel manipulators have wide application in the industry, but there still exist many difficulties in the actual control process because of the time-varying and coupling. Unfortunately, the present-day commercial controlles cannot provide satisfying performance for its single axis linear control only. Therefore, aimed at a novel 2-DOF (Degree of Freedom) parallel manipulator called Diamond 600, a motor-mechanism coupling dynamic model based control scheme employing the computed torque control algorithm are presented in this paper. First, the integrated dynamic coupling model is deduced, according to equivalent torques between the mechanical structure and the PM (Permanent Magnetism) servomotor. Second, computed torque controller is described in detail for the above proposed model. At last, a series of numerical simulations and experiments are carried out to test the effectiveness of the system, and the results verify the favourable tracking ability and robustness.

Keywords

Parallel manipulator Motor-mechanism coupling dynamic model Computed torque control

1. Introduction

The parallel manipulators have potential advantages in terms of stiffness, accuracy, high speed, and payload over their serial counterparts. The parallel manipulators have wide applications, such as Pick and Place operation and so on. The way to fulfill the advantages on excellent high-speed and high-precision control performance of the parallel manipulators has become one of the key research points of the scholars and engineers.

The dynamic model of the parallel manipulators is a kind of non-linearity with characteristics of time-varying and coupling [3]. And most nonlinear mechanical systems comprise driven motors, coupling gears and the nonlinear mechanism [2]. Note that the complex motor-mechanism coupling modeling will cost a lot of time, in addition, control methods also influenced the control performance dramatically, so how to design the model based controller is very important to fulfill the performance of mechanisms. In the history of the control method development, it can be divided into classical control algorithm, modern control algorithm and intelligent control algorithm. The classical control, such as simple PD and PID linear control, is simple but not good performance for nonlinear system. The modern control like motion control is used more and more because it is simple in the structure and better performance in control. The intelligent control can prove the best control performance but it needs a lot of experience and is always hard to design. In the nowdays, many modern and intelligent control techniques have been developed for the nonlinear mechanical system [9], such as adaptive control, fuzzy control, sliding mode control, computed torque control and neural network control. Fuzzy control is a valid method but difficult to design and analyze suitable fuzzy rules. The artificial neural networks control is such a method that has the capability of learning from processes. And it does not need the control model, but is difficult to design the number of middle layer of network. Sliding mode control is a new method but some bounds on system uncertainties must be preestimated. Among these algorithms, the motion control, like computed torque control, is a high-performance rotational or translational control of torque and speed [5]. The computed torque control is utilized to linearize the nonlinear dynamic of manipulator [4]. It can provide excellent tracking performance by considering nonlinear compensations with the precise dynamic model [1]. Consider the sensitivity to the parameters and disturbance, several computed torque controllers have also tried to circumvent the problems of uncertainties using some adaptive techniques [6].

Usually, the non-linear motor-mechanism coupling dynamics of manipulator is not under fully consideration in most dynamic modeling. For example, in the design of a hybrid controller with neural network [8], part of the motor dynamics is proposed for the hybrid controller. In addition, the other way is that the model was often equivalent by scholars to the robot model in [7]. In one word, the above models were about the simple mechanisms only, and among these models, the mechatronic dynamic modeling of parallel system has been discussed but was very scarce.

The structures of paper are organized as following: In part 2, the 2-DOF parallel manipulator called Diamond 600 is introduced in brief. In part 3, the mechanism and motor dynamic models are deduced. With the equivalent torque, the motor-mechanism coupling dynamic model is fomulated. The controller employing computed torque algorithm is also described in detail for coupling model in this part. The simulations and experiments are provided in part 4 and 5 respectively. The last part is the conclusions.

2. System Description

Fig.1 depicts 3D solid modeling of a high speed parallel manipulator known as the diamond 600 for Pick and Place operation. The manipulator consists of a static platform, a moving platform and two kinematic chains. The two parallelogram strut structures consist of the framework, input link, passive input link, bracket, and bracket, inner distal link, moving platform, outer distal link. All the components are connected through the revolute joints.

Fig. 1.

Solid model of the parallel manipulator

3. Motor-Mechanism Dynamic Model and Control

The control block diagram is shown as Fig. 2. The whole system can be divided into two parts. One is the motor-mechanism coupling model and the other is computed torque controller. From the desired values in the workspace and the inverse kinematic model, the ideal angular displacement, velocity and acceleration can be got as a reference values. The errors between the actual condition and the ideal condition can be regarded as the input of the controller. The ideal voltages and currents can be achieved using the state equations in the ideal condition at the same time. The compensations can be implemented in the inner loop, and the accurate outputs can be calculated by the precise coupling model.

Fig. 2.

Control block diagram of the system

A. Mathematical Model of Diamond Manipulator

Because of the identical and parallelogram strut structure of the actuated input links and distal links, the original model can be simplified into a 2-DOF with 5 bar manipulator system shown in Fig. 3 during kinematics analysis.

Fig. 3.

Sketch of the locomotion

The inverse kinematics is to determine the rotation angular angle θ_1i (i = 1,2) with the position vector r = (x y)^T of the moving platform reference point O′.

r - sgn (i) e e_{1} - l_{1} u_{i} = l_{2} w_{i}

(1)

where l₁, l₂, u _i, w _i are the length and the unit vector of the actuated input and distal links associate with the i_th chain, e is the distance between O and A_i (i = 1, 2).

The u _i, w _i can be determined by take norm of both side of (1).

\begin{matrix} u_{i} = {(cos θ_{1 i} sin θ_{1 i})}^{T}, w_{i} = {(cos θ_{2 i} sin θ_{2 i})}^{T}, \\ e_{1} = {(1 0)}^{T}, sgn (i) = {\begin{matrix} 1 & i = 1 \\ - 1 & i = 2 \end{matrix} . \end{matrix}

Take time derivation on each side of (1), leads to

v = l_{1} {\dot{θ}}_{1 i} Q u_{i} + l_{2} {\dot{θ}}_{2 i} Q w_{i}

(2)

where v is the velocity of point O′ and Q is the rotation matrix that enables vector to rotate anticlockwise an angle π/2 about z axis.

Take dot product of w _i^T on both side of (2), and we notice that w _i^T Qw _i = 0, rewrite the equation in matrix form

{\dot{θ}}_{1} = J v

(3)

where $J = \frac{1}{l_{1}} [\begin{array}{l} \frac{cos θ_{21}}{w_{1}^{T} Q u_{1}} & \frac{sin θ_{21}}{w_{1}^{T} Q u_{1}} \\ \frac{cos θ_{22}}{w_{2}^{T} Q u_{2}} & \frac{sin θ_{22}}{w_{2}^{T} Q u_{2}} \end{array}]$ is velocity Jacobin matrix.

Differentiating (2) with respect to time yields the acceleration a . In matrix form, we have the mapping model of the acceleration at the same time

f (v) \begin{matrix} {\overset{..}{θ}}_{1} = J a + f (v) \\ [\begin{matrix} v^{T} (\frac{w_{1}^{T} u_{1}}{l_{1}^{2}} \frac{w_{1} w_{1}^{T}}{{(w_{1}^{T} Q u_{1})}^{3}} + \frac{1}{l_{1} l_{2}} \frac{u_{1} u_{1}^{T}}{{(w_{1}^{T} Q u_{1})}^{3}}) v \\ v^{T} (\frac{w_{2}^{T} u_{2}}{l_{1}^{2}} \frac{w_{2} w_{2}^{T}}{{(w_{2}^{T} Q u_{2})}^{3}} + \frac{1}{l_{1} l_{2}} \frac{u_{2} u_{2}^{T}}{{(w_{2}^{T} Q u_{2})}^{3}}) v \end{matrix}] \end{matrix}

(4)

The inverse dynamic modeling which is the foundation of controller design, is to determine the applied force or power of the servomotor with the inertia and moving parameters of the parallel manipulator.

The following assumptions are made to simplify the model. The movement pairs are ideal ones that there is no energy dissipation caused by the friction; The moments of the inertia of the distal and passive input links are negligible because they are made by slender bars; Their masses are divided evenly and concentrated at two endpoints B_i and O′; As the bracket and the moving platform undergo pure translation, the motions of any points on these components can therefore be represented by those of B_i and O′ respectively.

Thus, considering the joint force, gravity and inertia force gives the following equation using the Virtual Work Principle while I_A is the equivalent rotary inertia of mechanism.

{(- m a)}^{T} δ r + {(τ - I_{A} {\overset{..}{θ}}_{1})}^{T} δ θ_{1} = 0

(5)

Substituting δθ₁ = Jδ_r into (5) leads to

τ = (m J^{- T} + I_{A} J) a + I_{A} f (v)

(6)

Substituting (3) and (4) in to (6) yields the relationship of the torque and the angular displacement as following:

τ = m J^{- T} J^{- 1} ({\overset{..}{θ}}_{1} - f ({\dot{θ}}_{1})) + I_{A} {\overset{..}{θ}}_{1}

(7)

B. Field-Oriented Control PM Synchronous Motor Modeling

The 3-phase alternating current servomotor is widely used in the industry, but the parameters of the servomotors, such as voltage, current and so on, are time-varying in real work situation. They are hard to describe in the coordinate system. Fortunately, the Park Transform can be employed to transform the 3-phase parameters into the equivalent parameters in the arbitrary coordinate system. The coordinate system is established on the rotor as shown in Fig. 4.

Fig. 4.

Coordinate system o – dq of servomotor

Field-orientated control is a technique that provides a decoupling method. It can change the currents into two components of stator current: one producing the air gap flux and the other producing the torque. Therefore, it provides independent control of torque and flux, which is similar to a separately excited DC motor. Therefore, with the implementation of the Field-orientated control method, the PM synchronous motor drive system can be simplified as a DC motor, as is shown in Fig. 4.

According to the equivalent relationship of Park Transform, we can obtain

f_{q d 0} = K_{s} f_{a b c}

(8)

where f is the arbitrary parameter of the motor, qdo and abc depicted the corresponding reference frame, θ and ω_m are the angular displacement and velocity of rotor.

\begin{matrix} f_{q d 0} = {[f_{q} f_{d} f_{0}]}^{T}, f_{a b c} = {[f_{a} f_{b} f_{c}]}^{T} \\ θ = \int_{0}^{t} ω_{m} (ξ) d ξ + θ (0) \\ K_{s} = \frac{2}{3} [\begin{matrix} cos θ & cos (θ - \frac{2 π}{3}) & cos (θ + \frac{2 π}{3}) \\ sin θ & sin (θ - \frac{2 π}{3}) & sin (θ + \frac{2 π}{3}) \\ \frac{1}{2} & \frac{1}{2} & \frac{1}{2} \end{matrix}] \end{matrix}

Because the resistances r _s and inductances L _s of the 3-phase PM motor are symmetric, for the Y-connection mode, the resistance and inductance can be expressed as

r = d i a g [r_{a} r_{b} r_{c}], L = [\begin{matrix} L & M & M \\ M & L & M \\ M & M & L \end{matrix}]

where L_l is the leakage inductance and L_m is the magnetizing inductance of each phase. M = −1/2L_m and L = L_l + L_m.

Applying the KCL (Kirchhoff Current Law) and ignoring the influence of the d axis on q axis, we can achieve the voltage U_q in the q axis.

U_{q} = R_{q} i_{q} + L i_{q}

(9)

where R = K r ( K _s)⁻¹, L = K _s L ( K _s)⁻¹, R_q and L in Eq.(9) are the corresponding component for q axis. Suppose the BEF (Back Electromotive Force) e = K_g ω_m, K_g is the speed coefficient of the BEF, the voltage equation in the q axis can be rewritten as

U_{q} = R_{q} i_{q} + L i_{q} + e

(10)

And the output torque of the motor, which is determined by the control current, can be expressed as

τ_{m o t o r} = K_{m} i_{q} - J_{m} \dot{ω}

(11)

where K_m is coefficient of the relationship between the torque and the current, J_m is the rotary inertia of the rotor.

Considering the torque of the bearing τ_B = Bω_m which B is the damping coefficient, the torque of the reducer τ_R which n is the reduction ratio and the torque of the friction τ_L, ω is the angular velocity of mechanism, the total output torque is

τ_{o u t} = n (K_{m} i_{q} - n J_{m} \dot{ω} - n B ω - τ_{L}) - τ_{R}

(12)

C. Motor-Mechanism Coupling Model

From equations (12) and (7) leads to

n K_{m} i_{q} - n^{2} B {\dot{θ}}_{1} - n^{2} J_{m} {\overset{..}{θ}}_{1} - τ_{R} = m J^{- T} J^{- 1} ({\overset{..}{θ}}_{1} - f ({\dot{θ}}_{1})) + L_{A} {\overset{..}{θ}}_{1}

(13)

Combined with the voltage equation (10), suppose x₁, x₂, x₃ are the angular displacement, velocity and current respectively. V is the corresponding control voltage. We can deduce the state equations as following

{\dot{x}}_{1} = x_{2}

(14)

{\dot{x}}_{2} = G \times [n K_{m} x_{3} - n^{2} B x_{2} + G 2 f (x_{2})]

(15)

x_{3} = 1 ∕ L (V - R x_{3} - n K g x_{2})

(16)

where $G = (m J^{- T} J^{- 1} + n^{2} J + I_{A})^{- 1}, G 2 = m J^{- T} J^{- 1}$ .

D. Design of Computed Torque Controller

The manipulator's dynamics can be expressed as

(m J^{- T} J^{- 1} + n^{2} J_{m} + I_{A}) {\overset{..}{θ}}_{1} + (n^{2} B {\dot{θ}}_{1} - m J^{- T} J^{- 1} f ({\dot{θ}}_{1})) - τ_{R} = n K_{m} i_{q}

Writing in the standard form

M (θ_{1}) {\overset{..}{θ}}_{1} + V (θ_{1}, {\dot{θ}}_{1}) + τ_{d} = τ

(17)

where M (θ) is the inertia matrix, $V (θ_{1}, {\dot{θ}}_{1})$ is the sum of the centrifugal, Coriolis and gravity terms, τ_d is disturbance and friction component. τ is the control torque in this section.

e = θ_{1 d} - θ_{1}

(18)

Suppose the desired trajectory θ_1d has been selected for the motion of active link. To ensure the trajectory tracking accuracy in the joint space, define the tracking errors as To demonstrate the influence of the input τ on the tracking errors, differentiate to obtain

\dot{e} = {\dot{θ}}_{1 d} - {\dot{θ}}_{1}

(19)

Consider the control law corresponding to a computed torque scheme with nonlinear gain matrices as

τ = M ({\overset{..}{θ}}_{1 d} + K_{p} e + K_{i} \int e d t) + V (θ, \dot{θ}) + τ_{d}

(20)

where K _p, K _i, K _d are the 2 × 2 proportional, integral and derivative gain matrices, respectively. The classical computed torque controller usually assumes that matrices are symmetric and positive definite. The stability of the results concerning the computed torque method with nonlinear gains has been proved based on the Lyapunov stability theorem [8]. The trajectory errors can be minimized by the reasonable parameters.

4. Simulation

By use of Runge-Kutta fourth order numerical integration method, state equations are solved for the motor-mechanism coupling system. For numerical simulations, Table 1 shows the geometry and inertia parameters of the parallel manipulator. Table 2 shows the parameters of Fuji AC servomotor of GYG152CC2-T2E and its driving system. There are two reducers between the motors and the mechanism.

Table 1.

Geometry and inertia parameters of the mechanism

Length of the input link l1(m)	0.25
Length of the distal link l2 (m)	0.54
Mass of the movable platform m1(kg)	0.55
Mass of the input link m2 (kg)	0.705
Mass of the distal link m3 (kg)	0.387
Mass of the bracket m4 (kg)	0.172

Table 2.

Parameters of the GYG152CC-T2E servomotor

Resistance of the stator winding Rqs (Ω)	0.18
Inductance of the armature Lms (mH)	1.86
Leakage inductance Lls (mH)	0.66
Torque coefficient Km(kg·m/A)	0.716
Inertia of the motor rotor Jm (10–4kg·m)	22.33
Transmission ratio n	15

A series of simulations are conducted to demonstrate the performance of computed torque control algorithms. In order to enable the end-effector to pick an object from one place to another, the desired trajectory is preset as three straight lines which are vertical to each other in Fig.5. The acceleration mode is in cycloid pattern. Under the control of the computed torque algorithm, the performances of manipulate are shown in the Fig. 6 –10..

Fig. 5.

Trajectory of end effector in the workspace

Fig. 6.

Errors of angular displacement

Fig. 7.

Errors of angular velocity

Fig. 8.

Errors of the trajectory in workspace

Fig. 9.

Errors of driving torque

Fig. 10.

Currents of the 3-phase of the servomotor

Fig.6 shows the manipulator's errors of trajectory in the operation space. There are initial errors, and the errors reduce to very small values in a short time. In the middle of the trajectory, the errors are a little larger because of the highest speed. But the last point in trajectory can be accurately tracked because the errors of angular displacement converge to small value. It is very important to the Pick and Place operation. In the Fig. 7, the initial errors can be cancelled out quickly. With the accurate tracking of the angular displacement, the angular velocity can be tracked in small errors too. At the end of the processes, the errors are almost zero. In Fig. 8, it depicts the errors of the manipulator in the workspace. Based on the errors of the angular displacement and the angular velocity, the larger errors appear in the middle segment of the whole trajectory and the minor errors at the end of the movement. Fig. 9 describes the torque errors. The actual torque follows the desired torque when it is in the low speed, and the errors become large with the increasement of speed. And under the control effect, the errors can be compensated and the end torque can be considered as the ideal one. All of the results can provide reliable tracking performance of the designed control algorithm. The small end errors are suitable to the operation such as Pick and Place which requires high end precision. Fig. 10 depicts the 3-phase currents of the servomotor as the effect of control. There is 120 degree discrepancy in the 3-phase currents. In the middle of three segments, the currents are big because of the high speed and control torque. The highest speed in the segment 2 is higher than the other two, so the currents are larger than the ones of the other two segments. In addition, they are not equal to each other because they have to transform into the current in q-axis.

5. Implement

The proposed controller has been applied to the Diamond 600. The experimental equipments and schematic diagram of the system are shown in Fig. 11. The structures of PC+PCI7344+Servomotor GYG152CC2-T2E are chosen as the control hardware. The UMI (Universal Motion Interface), software of NI Measurement & Automation Explore and LabVIEW are also chosen to build up the control system. The servomotors are set in the torque mode. PC computes sends control signals to the servo drivers through the PCI7344 and UMI. The feedback signals such as angular displacement and velocity are acquired from the encoders through analog to digital conversion. The angular displacement and velocity are shown in the Fig. 12 and 13 respectively.

Fig. 11.

Photograph of experimental equipments

Fig. 12.

Displacement of the servomotors

Fig. 13.

Angular velocity of the servomotors

Fig.12 and 13 show the real angular displacement and velocity generated by the servomotors. We can see that there are no movements at the beginning but there are disturbances as the initial errors from 0.11s, and it will take some time to decrease the disturbance and overcome inertia from the 0.11s to 0.15s. The errors of the angular displacement and velocity are a little big at this phase. We also find that there are still big errors around 0.25s and 0.61s because the directions change at these moments. The errors in the middle are bigger because of the higher angular velocity, but end errors of the angular displacement and velocity convert to zero. The precise end tracking ability is suitable for the Pick and Place operation.

6. Conclusion

The investigation proposes a model based controller for a high speed parallel manipulator. Combined mechanism dynamics with the motor dynamics, the precise and feasible motor-mechanism coupling model can be obtained. Model-based computed torque controller is also proposed for the high performance. It can get rid of the initial disturbance and provide the small steady end errors for the operation. The simulation and experiment also verify the validity of the controller, along with the favourable tracking ability and robustness.

Footnotes

7. Acknowledgements

The research is supported by the NFSC, grant No. 50375106, the State Scholarship Fund, Grant No. 2004812032.

References

Acosta

Marichal

G. N.

& Moreno

(1999), A robotic system based on neural network controllers, Artificial Intelligence in Engineering, Vol. 13, No.4, pp. 393–398, ISSN: 0954-1810

Albagul

& Wahyu

(2004). Dynamic modeling and adaptive traction control for mobile robots, International Journal of Advanced Robotic System, Vol. 1, No. 3, pp. 614–620, ISSN: 1729-8806

Codourey

& Yoshikawa

(1996). Dynamic modeling and mass matrix evaluation of the DELTA parallel robot for axes decoupling control, Proceedings of the 1996 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vol. 3, pp. 1211–1218, ISBN: 078033213X, Osaka, Japan, Nov. 1996, IEEE, Piscataway, NJ, USA

Codourey

(1998). Dynamic modeling of parallel robots for computed torque control implementation, International Journal of Robotics Research, Vol. 17, No. 12, pp. 1325–1336, ISSN: 0278-3649

Huang

C. Q.

(2005). Parallel force/position controllers for robot manipulators with uncertain kinematics, International Journal of Robotics and Automation, Vol. 20, No. 3, pp.158–167,ISSN: 0826-8185

Imura

Sugie

& Yoshikawa

(1994), Adaptive robust control of robot manipulators-theory and experiment, IEEE Transaction Robotic Automation, Vol. 10, No. 5, pp. 705–710, ISSN:1042-296X

Kovecses

Piedboeuf

J. C.

& Lange

(2003). Dynamics modeling and simulation of constrained robotic systems, IEEE/ASME Transactions on Mechatronics, Vol. 8, No. 5, pp. 165–177, ISSN: 1083-4435

Lin

F. J.

& Wai

R. J.

(2001). A Hybrid Computed Torque Controller Using Fuzzy Neural Network for Motor-Quick-Return Servo Mechanism, IEEE/ASME Transactions on Mechatronics, Vol. 6, No. 3, pp. 75–89, ISSN: 1083-4435

Martynyuk

A. A.

(2000). Survey of some classical and modern developments of stability theory, Nonlinear Analysis, Theory, Methods and Applications, Vol. 40, No. 1, pp.483–496, ISSN: 0362-546X