Position Control of a 3-CPU Spherical Parallel Manipulator

The paper presents the first experimental results on the control of a prototypal robot designed for the orientation of parts or tools. The innovative machine is a spherical parallel manipulator actuated by 3 linear motors; several position control schemes have been tested and compared with the final aim of designing an interaction controller.The relative simplicity of machine kinematics allowed to test algorithms requiring the closed-loop evaluation of both inverse and direct kinematics; the compensation of gravitational terms has been experimented as well.


Introduction
Parallel kinematics machines, PKMs, are known to be characterized by many advantages like a lightweight construction and a high stiffness but also present some drawbacks, like the limited workspace, the great number of joints of the mechanical structure, and the complex kinematics, especially for 6-dof machines [1].Therefore the A. 's proposed to decompose fullmobility operations into elemental subtasks, to be performed by separate reduced mobility machines, similarly to what is already done in conventional machining operations.They envisaged the architecture of a mechatronic system where two parallel robots cooperate in order to perform complex assembly tasks.The kinematics of both machines is based upon the same 3-CPU topology but the joints are differently assembled so as to obtain a translating parallel machine (TPM) with one mechanism and a spherical parallel machine (SPM) with the other.
This solution, at the cost of a more sophisticated controller, would lead to the design of simpler machines that could be used also stand-alone for 3-dof tasks and would increase the modularity and reconfigurability of the robotized industrial process.The two robots are now available at the prototypal stage, and the present paper reports the first experiments on the motion control of the orienting device (SPM).

Robot's Architecture and Kinematics
2.1.Mechanical Architecture.Since the detailed description of machine's kinematics and prototype design has been provided already in Callegari et al. [2], hereby only the most relevant aspects are recalled.
The spherical parallel machine under study is made of three identical serial chains connecting the moving platform to the fixed base, as shown in Figure 1; each leg is composed by two links: the first one is connected to the frame by a cylindrical joint (C), while the second link is connected to the first one by a prismatic joint (P) and to the end-effector by a universal joint (U); for this reason its mechanical architecture is commonly called 3-CPU.A few manufacturing conditions, already investigated for a general pure rotational tripod by Karouia and Hervè [3], must be fulfilled in order to constraint the end-effector to a spherical motion: (i) the axes of the cylindrical joints (a  ,  = 1, 2, 3) are aligned along the x, y, z axes of the base frame and intersect at the center O of the spherical motion; (ii) the axis b  of each prismatic pair is perpendicular to the axis of the respective cylindrical joint a  ; (iii) the first axis of each universal joint is perpendicular to the plane of the corresponding leg (plane identified by the axes a  and b  ); (iv) the second axis of the 3 universal joints (resp., for the leg 1, 2, and 3) are aligned along the  1 ,  1 ,  1 axes of a local frame centered in  (coincident with O) and attached to the mobile platform.
For a successful operation of the mechanism, one mounting condition must be satisfied too; assembly should be operated in such a way that the two frames (x 0 , y 0 , z 0 ) and (x 1 , y 1 , z 1 ) come to coincide when the robot is in its homing position.Such configuration is obtained when the three displacements   are equal to the length of the second link  and the displacements of the prismatic joints   are equal to the constant distance .If the mounting conditions are verified, the points  and  remain fixed and coincident while the moving platform performs a spherical motion around them.

Kinematic Relations.
The platform is actuated by driving the strokes of the 3 cylindrical joints; therefore joint space displacements are gathered into the following vector q: The position kinematics of the robot expresses the relation between the orientation of the mobile platform and the displacements of the actuators; the attitude of the machine in space is fully provided by the rotation matrix   R, that can also be conveniently expressed as a composition of elemental rotations.In the development of robot's kinematics, the following Cardan angles set is used: The position kinematics of the robot is simply expressed by where   is the element at the th row and th column of rotation matrix   R. The solution of the direct position kinematics (DPK) problem requires the computation of the rotation matrix   R as a function of internal coordinates q, which has been solved already by Carbonari et al. [4].According to Innocenti and Parenti-Castelli [5], a maximum number of 8 different configurations can be worked out; however, a single feasible solution is found when the real workspace of the robot is considered; that is, the actual mobility of the joints is taken into consideration.Inverse position kinematic (IPK) problem admits just one solution and it is trivially solved by working out joint displacements q in (3).
Turning to differential kinematics, the expression of the analytic Jacobian J  is immediately obtained as a function of the Cardan angles and their rates: By taking into account the relation between the derivatives of the Cardan angles and the angular velocity : the geometric Jacobian J  is easily obtained too: with 2.3.User Frames.In order to better define the tasks to be commanded and visualize the obtained results, it is useful to choose a different set of reference frames, as shown in Figure 2. The fixed frame  * (x * 0 , y * 0 , z * 0 ) is defined as follows: (i) the origin is located at the center of the moving platform when it assumes its initial configuration; (ii) the z * 0 axis is aligned to the vector g of gravity acceleration; (iii) the x * 0 axis lies on the upper plane of the platform and points toward the axis a 1 of the cylindrical joint of the first leg; (iv) the y * 0 axis is placed according to the right-hand rule.The mobile frame  * (x * 1 , y * 1 , z * 1 ) is coincident with the fixed frame  * (x * 0 , y * 0 , z * 0 ) when the platform is in its initial configuration.Of course, since the frames are not placed at the center of the spherical motion, the two origins  * and  * will be coincident only in the home position.
Once the location of the new frame  * has been defined by means of the   * R rotation matrix, the orientation of the mobile platform can be described in the new frames by where it has been used the identity   * R =   * R. Of course, being the mobile and fixed frames modified, also the Cardan angles   ,   ,   that yield the rotation matrix  *  * R are different from the previously described set (, , ): Henceforth these angles are used to describe the orientation of the manipulator and to assign the tasks of the mobile platform; since they are assumed as external coordinates for the computation of the differential kinematics, the analytic and the geometric Jacobians are worked out again as previously described, providing similar but more complex relations.

Control Algorithms
3.1.Overview.Several kinds of control schemes have been tried on the 3-CPU SPM, with the immediate goal of testing the prototypal robot but aiming at the final design of an efficient co-operative environment for mechanical assembly.In the end, 3 different algorithms have been studied in simulation and then experimentally tested: (i) a conventional joint resolved PID [6]; (ii) a joint resolved PID with the compensation of gravity forces [7]; (iii) a task-space PID with gravity compensation [8].
In all control schemes, the PID loop has been computed as usual in the following way: with u() control action and e() input position error; K  , T  , and T  are, respectively, the proportional gain, integral time, and derivative time matrices of the PID regulator.

Joint Resolved PID.
First, a conventional joint resolved PID has been considered; see Figure 3.The error signal ã is computed in the joint space as a difference between the desired position of the sliders a  and their actual values a:   Since planning is programmed in the orientation space by assigning the desired configuration of the robot   , the corresponding position of the actuated joints is computed by means of inverse kinematics relations.The actuation effort of the motors is computed as where the diagonal matrices K  , T  , and T  have been introduced already in the previous section.

Joint Resolved PID with Gravity Compensation.
In robotics the effects of gravitational field are often much more relevant than the other dynamics terms, at least for the small/moderate velocities attained during assembly tasks; such terms can be easily evaluated by means of the virtual work principle, as worked out in Callegari et al. [2].Thus, a compensation term can be introduced by adding the force vector: where J  is the analytic Jacobian matrix,   is the mass of the ith member, J  is the Jacobian that links the velocity of the centre of gravity of the ith member to the vector ȧ , and g is the gravity acceleration.The resulting control scheme is shown in Figure 4.

Task Space PID with Gravity
Compensation.The third control scheme that has been taken into consideration is a task space PID, with the compensation of the gravitational terms; see Figure 5: where φ is the error signal in the task space and the PID gains K   , T   , and T   are diagonal matrices once again.This algorithm is computationally more expensive than the previous one, since it requires the evaluation of direct kinematics that for PKMs is more complex than inverse kinematics; on the other hand, it could prove useful, for example, in vision assisted assembly tasks with position-based controls, as already experimented on the 3-CPU translating parallel machine by Palmieri et al. [9].12)-( 14) on the real-time controller, it was took into consideration the sensitiveness to noise of differentiation.Considering the Laplace transform of (10), the mentioned problem has been numerically mitigated by substituting the classic derivative term      with the following derivative operator:

Implementation in Real-Time Controller. During the implementation of algorithms (
where  has been chosen equal to 10. Another problem in the implementation of PID controllers over a real-time system is the windup effect.This phenomenon is due to the integral action which saturates the actuators output.Figure 6 shows a modified scheme of the PID control which implements a typical anti-windup strategy; the model of the actuator, mentioned in the scheme, was easily obtained after identification of the motor mechanical and electrical parameters summarized in Table 2.

Simulation Results
4.1.Simulation Environment.Figure 7 shows the virtual prototyping environment used at the Robotics Laboratory of the Polytechnic University of Marche for the design of automated and robotized systems, in particular for the design and virtual testing of parallel kinematic manipulators.The mechanical design is developed through conventional CAD tools, which allow to easily define even the most complex geometries and also to perform, for example, by means of FEM modules, the needed structural analyses; the interface with a multibody code allows to perform closed-loop dynamic analyses, with different levels of difficulty according to the associativity of the used programs.In this case, the LMS Virtual.Lab Motion package has been used, which is able to handle conveniently also complex situations like, for instance, the occurrence of an impact.The multibody package receives in input from the controller the actuation torques and integrates the equation of direct dynamics, providing in output the state variables assumed to be measured.The control system, which is implemented in the Matlab/Simulink environment, computes the control actions by taking into account the commanded task and sometimes, just like the present case, by also exploiting the complete or partial knowledge of robot's dynamics (inverse dynamics model).If the task is constrained by the contact with the environment, like is usually the case for assembly, the contact forces can be evaluated too, to set up more efficient force control schemes.It is noted that, by  In this way, by means of the mentioned prototyping software, a model of the spherical robot has been made available for the design of the control system and for the

Simulation Analysis.
A few test cases have been set up in simulation to evaluate the performances of the 3 PID controllers described in Section 3. The figures show the response of the system when the robot started at rest from the home configuration (  =   =   = 0) and was required to attain the set point: Such task is very challenging for machine's controller because the set point lies close to a singular configuration of the robot and algorithms (13) and ( 14) require the inversion of the Jacobian matrix.Figure 8 shows the different performances, in simulation, between the three different controllers: C1, C2, C3 are, respectively, joint resolved PID, joint resolved PID with gravity compensation, and task space PID with gravity compensation.It is noted that the robot is not kept at its home position by means of the brakes but the motors are used to this aim instead, then the set point has been applied in all trials at the time instant  = 0.5 s.The orientation trajectories in the task space show the better behaviour of the closedloop system when it is equipped with the conventional PID algorithm, due to the mentioned presence of singularities.
The simulations also return useful information for what regards the control effort forces, which are plotted in Figure 9: in all cases the application of the set point causes a peak in the required forces, which saturates the actuators.
In the end, it is noted that the task space PID with gravity compensation is more sensitive to parameter variations.This is due to the intrinsic characteristics of robot prototype, which has no external sensor and many singular configurations.In this way, all the information about the task space is obtained through the direct kinematics and the robot Jacobian.Small errors in the computation may affect heavily control system's performance.on the PXI/FlexMotion hardware.The force developed by the sliders is obtained by directly setting the current loop of the drivers, according to the usual relation between the current  and the thrust :

Experimental Setup. The prototype robot is shown in
where the torque constant   characterizes the performances of the motor; see Table 2.
With reference to the symbols introduced in Figure 1, the main design data of the prototype are collected in Table 3.
A series of experimental tests have been carried out in order to validate the numerical model described in the previous sections and to experimentally assess the performances of control laws (12)-( 14).Results are here reported.

Case Study A.
The first case study was already investigated in simulation, so that numerical and experimental results can now be compared.The platform at the home position has been requested to attain once again the task space set point (16), which corresponds to the following motor strokes: Many tests have been performed for each one of the three control laws (12)-( 14) and in the figures some experimental results are presented; values of one of the experimental trials are represented by circle markers while the averaged quantities are represented by solid lines.
Figure 11 presents some results obtained with the conventional PID loop.It is seen that steady state is achieved in less than one second without significant oscillations, due to the pretty high mechanical damping of the system.The corresponding actuation forces   are rather large in the first instants, approaching motors' saturation thrusts, then they settle along the static value required for gravity compensation.
By observing Figures 11,12,13, and 14, it results that the introduction of a gravity compensation term into the joint loop brings in system's dynamics overshoots that increase Figure 16 shows the trend of the workspace variable vector  = [      ]  in the three different cases, while Figure 17 plots the actuation forces developed by the motors.

Conclusions
The paper presented the first experiments in driving a prototypal SPM developed at the Polytechnic University of Marche by means of linear controllers.The use of a virtual simulation environment can be very profitable in the design of robots' controllers and even in the draft tuning of their parameters.In the present case, three controllers have been first designed in simulation and then implemented on an embedded system for real-time application by means of rapid prototyping software.The task-space controller with the compensation of the gravity terms provided poorer performances than conventional joint-space loops, but in A. 's opinion it could be due to calibration errors, that would heavily affect the computation of direct kinematics, which is required with the present sensing equipment; the use of ANN controllers could be profitable to overcome unmodeled disturbances due to static friction or calibration errors [10].
As a matter of fact, task-space control schemes would be very useful for the realization of interaction controllers, see Siciliano and Villani [11], which is the objective of A. 's coming researches, since they aim at performing mechanical assembly by means of cooperating PKMs [12].
The simplicity of direct kinematics of this machine (in comparison with the usual complexity of PKMs) allows an efficient implementation of algorithms with loop closures in the task space; the same can be said for the easy compensation of the static unbalance of robot's links.
These features and the possible use of visual servoing suggest a possible implementation of control schemes based on force control, where machine's dynamics has to be computed in task-space coordinates, which is rather "natural" for parallel kinematics machines.

Figure 1 :
Figure 1: Kinematic schemes of the 3-CPU robot (a) and geometry of the legs (b).

Figure 9 :
Figure 9: Response to step input: control efforts.

Figure 10 :
Figure 10: The prototype of the spherical parallel machine.

Figure 12 :
Figure 12: Joint-space PID with gravity compensation: platform's trajectory in task-space (a) and joint space errors (b).

Figure 13 :
Figure13: Joint-space PID with gravity compensation:   total force provided by the motors,   gravity component.

Figure 14 :
Figure 14: Task-space PID with gravity compensation: platform's trajectory in workspace (a) and joint space errors (b).

Figure 15 :
Figure 15: Task-space PID with gravity compensation:   total force provided by the motors,   gravity component.

Figure 17 :
Figure 17: Comparative behavior of the three controllers: actuation forces.

5. 3 .
Case Study B. The second case study was aimed at comparing the performances of the 3 controllers and therefore an elemental task was chosen; the wrist started in quiet at the home pose and was requested to reach the configuration:
the same code used during the simulations in the virtual prototyping environment has been directly ported to the realtime control hardware afterwards.

Table 2 :
Parameters of the linear drives.

Table 3 :
Mechanical data of the robot prototype.