Work cycle optimization problem of manipulator with revolute joints

In this work the problem of motion modeling and work cycle optimization of manipulator with revolute joints has been considered. The motion equations of the manipulator elements under any spatial work cycle conditions have been formulated. The formulation has been completed by using the classic vector mechanics and Lagrange equations of second kind. The equations of motion of the system have been obtained using commercial software. The chosen motion model for each considered actuator is point-to-point motion model with quasi-trapezoid velocity profile. Additionally, the problem of optimization of a particular work cycle has been presented. The optimization objective has been chosen as minimization of loads (torques) in actuators. The objective function has been formulated using performance indexes and the design variables are rated velocity value and initial time value of work cycle in each considered actuator. The formulated optimization problem has been solved using constrained Multi-Objective Particle Swarm Optimization algorithm. A numerical computation has been completed using specially performed software and results of the computation have been attached to the paperwork.

problem of modeling and analysis of dynamics of a truck crane and its components. From the viewpoint of this work, it is worth to quote works [4][5][6][7]. In the works, the problems of modeling and optimization of robot manipulators have been presented with different objective functions and constraints applied to the algorithm.
In this work, the problem of modeling of the dynamics of 4R manipulator has been presented. Additionally, the problem of optimization of the point-to-point work cycle has been formulated and solved. An exemplary computation has been performed and results of the computation have been attached to the paperwork.

Kinematics and dynamics of the manipulator
The manipulator with four revolute joints (4R manipulator) allows positioning an end-effector of the manipulator in a three dimensional workspace and, additionally, allows rotating the gripping device attached to the manipulator. The system of this kind is an open kinematic chain and has been presented in simple form on Fig. 1.
The kinematics and dynamics of the considered system has been formulated in Cartesian global coordinate system OXYZ, as shown on Fig. 1. The model of the manipulator consists of four rigid bodies connected by 1-DOF revolute joints P, Q, S and N. All functions of kinematics have been determined using classical mechanics by introducing local coordinate systems permanently attached to bodies of the considered kinematic chain. The problem of kinematics of open kinematic chains is widely described in works [8][9][10][11][12].
The problem of inverse dynamics of robot manipulators with revolute joints involves determining the torque variations in each considered joint, whereas the position, velocity and acceleration functions are known. The best way to solve Fig. 1 The scheme of the 4R manipulator the problem is to formulate proper functions of mechanical (kinetic and potential) energy and to use Lagrange equations of second kind. If L is the Lagrangian, the equations of dynamics of considered manipulator are: The generalized coordinates are: The Lagrangian is total kinetic energy of the system minus its total potential energy. Because each element of the system is considered as a rigid body, a kinetic energy of a particular element is a sum of kinetic energy in translational and rotational motions. A potential energy of a particular element is simply weight of the element multiplied by a distance to the minimum of potential energy (the OXY plane of the global frame).
In this work, the problem of optimization of 4R manipulator is also presented. The optimization objective is to minimize torques in each considered actuator. The objective function can be formulated using a performance index [12]. For a particular actuator, the index has a form:

Motion model
In this work, the point-to-point model of motion has been accepted. In literature, various models of velocity profile can be met. For instance, the profile can be chosen as trapezoid, sinusoidal or parabolic [12]. In this work a quasi-trapezoid  From the standpoint of optimizing, the most important parameters are starting time of the work cycle and its rated velocity. An angular displacement in each considered joint can be simply computed as: A maximum acceleration and duration of rated velocity keeping are equal to: Design variables can be collected to a vector: 4 Particle swarm optimization algorithm The particle swarm optimization algorithm is one of the most modern stochastic optimization technique, it was firstly proposed by Kennedy and Eberhart in 1995 in work [13]. From the beginning, this method is widely developed and many applications and modifications have been formulated so far, for instance [14][15][16]. In robotics, this method is often used to find optimal geometrical and inertia parameters of stationary robots, such as manipulators [4][5][6][7]. It is also used in mobile robotics to find an optimal trajectory for mobile robot in twodimensional workspace.
The PSO algorithm is based on observation of phenomenon occurring in nature, such as foraging of swarm of insects or shoal of fish. Each particle of the swarm is able to remember and use its experience that is taken from the whole iteration process and, also, is able to communicate with other members. The swarm of particles is able to identify "good" areas of the domain and can search deeply in these areas for an optimum.
Initial values of design variables (the position of particular particle) are random. Then, in an iteration step n +1, distance covered by a particle in m-th direction (the velocity of the particle in m-th direction) can be described as following: where χ is a constriction factor, V (n) m is a velocity in previous iteration step, w is a weight coefficient, r 1 and r 2 are random real numbers taken from (0;1), c 1 and c 2 are learning factors, p m is a personal best position of the considered particle from the whole iteration process and g m is a global best position obtained by entire swarm. In the formula, three different influence components can be identified: the first is an inertia influence, the second is a personal influence and the third is a social influence. There is another version of this formula, where the global best position g m is replaced with a local best position l m . In this version, each particle has specified neighborhood and compares its personal best position with members of the neighborhood only.
Additionally, a maximum velocity in each considered direction should be set to protect the swarm from explosion: where V (max) m is the maximum velocity in m-th direction.
A new position for each particle in each considered direction is equal to: During the iteration, values of design variables have to satisfy some constraints. All variables have to be positive. Signs of the velocities are known and depend on signs of the angular displacements in each considered actuator (Chapter 3). Additionally, the velocities are constrained by maximum velocities that are available in each actuator. Moreover, maximum time of work cycle is specified and a maximum torque value is known for each joint. All formulated constraints are listed below: Considering previously introduced model of motion (Chapter 3): In PSO algorithm, constraints are introduced using a penalty function. There is necessary to reformulate constraints into a following form: The penalty function can be assumed as: where r is number of constraints. In Eq. (17), α is a correcting factor for the penalty function and p k are correcting factors for each considered constraint. The max function takes the value of 0 when σ k (x) < 0 and σ k (x) when σ k (x) > 0. The h(n) function depends on the iteration step and has been assumed as: Using the considerations contained in Sections 2 and 3, the objective function for unconstrained optimization problem can be formulate as: where w i are weight coefficients and P i functions are described by Eq. (3). After introducing the penalty function into the objective function, a new objective function is given by a following formula: The PSO algorithm, used in this work, has been presented in a simplified form in Fig. 4.

The exemplary computation
The algorithm, introduced in previous chapters, has been used to perform the exemplary computation. The problem of optimization of the work cycle of 4R manipulator has been investigated. The design variables are starting time and rated velocity in each considered joint, so there are 8 design variables. The work cycle includes a motion from a start point A to a final point B with simultaneous rotation of the gripping device and a load. The gripping device and the load are considered as one rigid body. Cartesian coordinates of chosen points are A {0.5,0.2,0.8} and B {−1.3,0.9,0.5}. The inverse kinematics problem for the 3R manipulator has been solved and specific rotation of gripping device has been set from -π /3 to π /3 [rad]. Complete inverse kinematics results are:    5,7,9,11). Additionally, velocity timedependencies has been added (Figs. 6,8,10,12).

Conclusion
In this work, the problem of modeling and optimization of the dynamics of 4R manipulator has been presented. The pointto-point motion model with a quasi-trapezoid velocity profile has been accepted. Equations of motion have been obtained using classical vector mechanics and Lagrange equations of second kind. The optimization problem has been solved using constrained Multi-objective particle swarm optimization algorithm. The design variables are rated velocity in each actuator and starting time of the work cycle. The objective function is based on the minimization of torques in actuators using performance index.
The algorithm can be used to investigate other optimization problems with different objective function and different design variables. The consideration can be used for solving optimization problem for manipulators with revolute and prismatic joints, the key is to reformulate the objective function and to identify design variables.