1 Introduction

Multibody system dynamics is an important branch in the field of modern mechanics. It provides a strong tool for estimating dynamic performances and optimizing the design of many mechanical systems in several engineering fields, especially robotics^[1]. A strong need to study the dynamic modeling, design, and control of complex multibodies in modern engineering problems emerges. Robots are of interest in dynamic control applications, given many circumstances wherein a human cannot readily and rapidly respond or has to repeatedly execute the same task^[2]. An industrial robot is a robot system used in a manufacturing sector. Industrial robots are automated, programmable, and capable of movement on two or more axes. Wittenburg^[3] published the first book on multibody systems based on the Lagrange method. Kane et al.^[4] proposed the Kane method including vector mechanics and analytical mechanics. The modeling techniques of multibody systems are mainly based on the Newton-Euler vector mechanics method, d'Alembert analytical mechanics method, Gauss extremum principle, and transfer matrix method^[1].

Differential algebraic equations (DAEs) can describe multisystem robots with the absolute coordinate method^[5]. There are many methods for solving DAEs, e.g., the direct numerical integration method, default direct correction method, Bayo number method, quadrature rectangle (QR) method, and singular value decomposition (SVD) method^[6]. The symplectic discretization form could conserve energy and satisfies the constraint condition at each discrete step^[7]. Peng et al.^[8] introduced the symplectic method for the instantaneous optimal control of multibody system trajectory tracking. However, there are some difficulties in solving DAEs with the existing methods, e.g., the singularity of sparse matrix inversion and the convergence of the algorithm. Therefore, the particle swarm optimization (PSO) is introduced in this paper, where the DAEs are solved by algebraic solution and finding the minimum value of the objective function instead of using matrix inversion and iterations, which essentially avoids the appearance of singularity and accelerates the convergence. PSO is an evolutionary computation technique based on a nature system^[9-14]. It has no evolution operators, and has fewer parameters than other intelligent algorithms. Several examples are carried out, and the results demonstrate that the PSO algorithm performs well in solving symplectic discretization equations. Different control methods have been investigated for mobile robots with various algorithms. An instantaneous optimal control algorithm is developed for the control applications to seismically excited linear, nonlinear, and hysteretic structural systems^[15]. The algorithm constructs its objective function at each discrete step, satisfying the state equation and constraint equation during the process of finding the minimum value of the objective function. Such a model is about the functional extremum problem, and we use PSO to find the next position of the system with external force and the best external force fitting the target function of the control system, which we name as double-PSO-based instantaneous optimal control.

The rest of the paper is arranged as follows. In Section 2, the symplectic method of DAEs and the instantaneous optimal control are introduced. In Section 3, the PSO-based symplectic method for uncontrolled and controlled systems is introduced. Some examples are illustrated to test the effectiveness of the proposed method in Section 4. In Section 5, conclusions are summarized.

2 Symplectic method of DAEs and the instantaneous optimal control

From a dynamics viewpoint, a robot can be considered as a multibody system^[5]. Therefore, the modeling of multibody system dynamics can be used for the modeling of various robots. The symplectic method can conserve the energy of the system and keep the constraint condition constant if no external force emerges^[8]. We focus on finding a method that will make the behaviors of multibody system predictable such that it can move along the ideal path when an external force is applied.

2.1 Symplectic method of DAEs

For a multibody system, we start with absolute coordinates to establish our model. The number of independent quantities that must be specified to uniquely define the position of any system is called the number of degrees of freedom, which we denote as N. These quantities must be in Cartesian coordinates, and the conditions of the problem may render some other choices of coordinates highly convenient. Any s quantities q=(q₁, q₂, ..., q_s), which could completely define the position of a system, are called the generalized coordinates of the system, and the derivatives are called its generalized velocities^[16]. Φ(q, t), M, and λ denote the constraint equations, generalized mass matrix, and Lagrange multiplier, respectively. Then, the DAEs of a multibody system model can be described as follows:

(1)

(2)

Now, we are going to solve the above equations with the symplectic method^[8]. The second-order differential equations are hard to be solved. Therefore, we change them into a first-order ordinary differential equation. Consider

(3)

while

(4)

Then, DAEs can be described as follows:

(5)

We disperse a continuous time step with the step length h. In a discrete time step h, according to the symplectic method, we assume that the Lagrange multiplier λ is constant and is described as λ_k. With the Eulerian difference methods, we obtain

(6)

The symplectic method requires that, at each discrete step, Eq. (2) should be satisfied. Hence, the discrete nonlinear equation is

(7)

where

2.2 Instantaneous optimal control algorithm

The instantaneous optimal control algorithm is developed to control seismically excited linear, nonlinear, and hysteretic structural systems^[15]. The algorithm constructs its objective function J_E at each discrete step, satisfying the state and constraint equations so as to find the minimum value of J_E,

(8)

where u_c^T(t) is the control vector, and D^T(t) describes the difference between the control routine and the real routine at a given instant. The control routine can be described as . Therefore,

S and R are the weight matrices. Then, the DAEs can be transformed into

(9)

where B is the control pose matrix. We use the PSO method to find the fitness u_c(t).

3 PSO-based symplectic method and double-PSO-based instantaneous optimal control 3.1 Introduction of the PSO method

PSO is an evolutionary computation technique, which is a search method based on a nature system^{[7, 9-14]}. PSO has no evolution operators and has fewer parameters than other intelligent algorithms. Each potential solution is called a particle. The number of the particles is denoted by n, and the dimensions of the particle are denoted by d. The solution space of d dimensions initially has a group of n particles with random positions and random velocities. The position and velocity of the particle group are denoted by P(c, n) and V(c, n), respectively, where c represents the step number and n denotes the number of the particle group. p(c, j) and v(c, j) denote the position and velocity of the jth particle in the cth particle group. The particles have memory, and each of them can learn from its previous best position p_best and the best position in the entire swarm, i.e., the global best position g_best. The values of p_best and g_best are determined by the target function. The basic concept of the PSO technique lies on accelerating each particle toward its previous best position and global best position. The velocity of each particle in the next step is determined by the distance between the present position and its previous best position, the distance between the present position and the global best position, and the velocity at present. The three parts of the velocity multiply different weights, i.e., c₁, c₂, and w. w denotes the inertial weight, and controls the exploitation of the search space because it dynamically adjusts the velocity. The constants c₁ and c₂, termed as the cognition component and the social component, respectively, are the acceleration constants changing the particle velocity, making it toward the previous best position and the global best position (see Fig. 1).

The main steps in the PSO process are as follows:

(ⅰ) At the first step, initialize n particles with random positions and random velocities for d dimensions.

(ⅱ) Evaluate the fitness of each particle in the swarm, take the positions of the particles into a target function J, and get the result of the function.

(ⅲ) For each iteration, compare the value of each particle with its p_best. If the current value is better than p_best, set p_best to be equal to the current value and the position of p_best to be the current location in the d-dimensional space. Compare p_best with g_best. If p_best is better than g_best, set the position of g_best to be that of p_best and g_best to be equal to p_best in the d-dimensional space.

(ⅳ) Compare g_best with the target. If the result is satisfactory, the position of g_best is the solution that we desire. If not, let the particles move to new positions to find fine answers.

(ⅴ) The velocity and position of the jth particle in the (c+1)th particle group are changed according to

(10)

(11)

(ⅵ) V_max/V_min is the maximum/minimum allowable velocity for the particles. If the velocity of the particle v(c+1, j) is not in [V_min, V_max], the velocity will be limited to V_min/V_max. The particle position is also limited in a certain range. If p(c+1, j) is out of the range, it will be pulled back to the edge.

(ⅶ) Repeat Steps (ⅱ) to (ⅵ) until the request of Step (ⅳ) fits or the mix step is reached.

3.2 PSO-based symplectic method

PSO can be used in the numerical calculation of the dynamic system equations. Now, we focus on Eq. (7). After we set the initial state of the system, we start with k=1 and end up with k=T. q^k is available, and q^k+1 unknown quantities emerge. The equations are made up of s equations containing s unknown quantities. Solving the equations means that we can obtain q^k of the next step. If we have the initial state and s equations, every step of the system will finally be solved. In the PSO method, the particle provides the initial position of d dimensions, which could make sure that the (s-d) unknown quantities can be derived from (s-d) equations in Eq. (7). Normally, d is near N (the free degree). After that, d equations (f_l1=0, f_l2=0, ..., f_ld=0) are left. Hence, we define the target function as follows:

(12)

The closer the result of J_f is to 0, the more satisfied the target function is. Equation (7) is a nonlinear equation, and the analytical solution cannot be normally found. In this method, the (s-d) unknown quantities are derived from d given quantities. This means that the results of (s-d) relative equations are zero. Make sure that the remaining d equations (f_l1=0, f_l2=0, ..., f_ld=0) approaching to zero can make each component of Eq. (7) approach or equal to zero. If

(13)

the algorithm converges. Moreover, when J_f^c is less than a certain number J_f⁰, we think that the answer jumping out of the circle is accurate enough. We define this method as the PSO-based symplectic method.

3.3 Double-PSO-based instantaneous optimal control

When it comes to a controlled system, Eq. (1) is changed to Eq. (9). The difference between them is the external force, which we are going to solve for. The discrete form is

(14)

With N-degrees-of-freedom, theoretically, applying N external forces can control the system. To find the external forces, we use the PSO method again. The double-PSO-based instantaneous optimal control using two PSO systems is used to solve the problem. We use the PSO to find the external force, which performs at an external loop, as L_PSOE. The subscript E signifies "external". PSO performs at the internal loop, which is written as L_PSOI. In a nutshell, we use L_PSOI to determine the mechanical behavior of the dynamic system with an external force and L_PSOE to determine the best external force, which makes the system move along the setting routine at that moment. First, there are n_E particles of d_E dimensions, where

and each dimension represents an independent external force. Second, F_E can be solved with the PSO-based symplectic method as mentioned before, since the external force is a linear term and is set up in the first step, which will not change the method. Finally, the target function is Eq. (8). If J_E is satisfied, the force at g_best is the external force we are looking for. If not, change the velocity and position of the particle and then cycle. Figure 2 shows the entire procedure.

4 Modeling and control of a three-degree-of-freedom system 4.1 Model description

A robotic arm is a type of a mechanical arm, which is usually programmable, with functions similar to a human arm. The arm may be the sum of the mechanism or may be part of a more complex robot. The links of such a manipulator are connected by hinges^[18]. A robotic arm with the mobile robot base is commonly seen in many areas, e.g., in a transfer robot, rescue robot, and explosive ordinance disposal (EOD) robot. We modify the robot as that in Fig. 3.

The reasons for choosing the model are as follows:

(Ⅰ) The track in two-dimensional space is simple to display and describe. Testing the reliability of the algorithm is easy for us.

(Ⅱ) The model is not simplified "so" simple. A three-degree-of-freedom model has many applications in reality. The model itself is useful in many areas mentioned in the article.

(Ⅲ) The number of the involved dynamic equations is considerable, making it a perfect example for transferring them into more sophisticated questions.

Figure 3 exhibits two homogeneous rods and a moving slider. The rods are hinged together with the moving slider. Evidently, the other end of Rod 2 is free. The frictionless slider can only move along the X-axis. Our purpose is to figure out its movement and its change over time for a specific condition without applying any external force on it. Furthermore, we can maneuver the system toward the specific direction or alongside the fixed routine by applying the external force with instantaneous optimal control.

The slider's movement on the X-axis and the two-rod system hinging together in two-dimensional plain makes the system have three-degree-of-freedom, which leads to four constraint equations. The rods are signed as Rod 1 and Rod 2 separately. L is the length of the rod, and L₁=L₂=0.2 m. The diameter of the rods is 0.02 m, and the mass is

The rods are made up of steel. The mass of the slider is

The red points are in the middle of the rods, and the corresponding coordinates are shown in Fig. 3. Hence, the generalized coordinates are

(15)

The constraint equation is

(16)

In this case, Eq. (7) can be decomposed into the following items:

We define that d=2 and the dimensions of the particles are x₁^k+1 and x₂^k+1, respectively. The 16 unknown quantities can be derived from 16 equations (see Fig. 4).

Item 15 is solved as

Unfortunately, more than one θ₁^k+1 will be obtained since arccos is a multivalued function. Hence, ensuring the same precision, we take the advantage of and to estimate the value of θ₁^k+1.

In Item 2, the following Lagrange method is used:

(17)

We assume that an equation has the same second-order accuracy, i.e.,

(18)

If y^k+Δy'₁>0, θ₁^k+1∈ [0, π); else if y^k+Δy'₁ < 0, θ₁^k+1∈ [π, 2π). With Eq. (12), we can obtain q_k+1 by using the PSO-based symplectic method in Subsection 3.2. It is denoted that J_f is the sum of the square of the left-hand side of Item 12 and the square of the left-hand side of Item 13. Using the PSO-based symplectic method in Subsection 3.2 can help us obtain q_k+1. Therefore, if the step T and the length h are chosen, we can get the method of the system precisely within Th seconds.

4.2 Consequence of an uncontrolled system

The two-rod system shows a totally different routine when the rods start with the mass center up or down the X-axis. We analyze them separately to verify the algorithm. The system starts with , x₃=0 and , x₃=0, respectively, and all the initial velocities are 0 m/s. h=0.002, T=10 000, n=120, J_f⁰=1×10^-26, c₁=c₂=2, and w=0.1^[19]. Figure 5 exhibits the trajectory. Figure 6 shows the energy and relative error of the length of rods changing with the passing time. The units of the X- and Y-axes shown in the figures are meter. The potential energy zero point is y=0.

As shown in Fig. 6(a), the maximum relative error of energy is 1.560 181×10^-6, and the average relative error of energy is 1.665 564×10^-7. The maximum relative error of the length of Rod 1 is 1.023 239×10^-7, and the average relative error of the length of Rod 1 is 1.148 461×10^-8. The maximum relative error of the length of Rod 2 is 2.424 987×10^-7, and the average relative error of the length of Rod 2 is 2.581 005×10^-6.

As shown in Fig. 6(b), the maximum relative error of energy is 2.494 497×10^-4, and the average relative error of energy is 1.410 311×10^-5. The maximum relative error of the length of Rod 1 is 1.023 239×10^-7, and the average relative error of the length of Rod 1 is 6.759 031×10^-9. The maximum relative error of the length of Rod 2 is 3.678 200×10^-7, and the average relative error of the length of Rod 2 is 1.588 214×10^-8.

The system error starting above the horizontal plane is larger than that below the horizontal plane because its routine is more complicated and chaotic. Both examples demonstrate the stability of the algorithm.

From the results, we can conclude that the symplectic method based on PSO has the following advantages:

(ⅰ) The method could predict the chaotic movement of the system in a precise manner, and the maximum and average relative errors of the constraints and energy are extremely small. It conserves the energy, and keeps the constraint condition. The two circumstances are both accurate. The rods starting with lower energy, which is beneath the X-axis, have better results because they have less chances to run into the extreme positions. However, even the relative errors of the extreme positions are small and acceptable, they are few compared with the other positions.

(ⅱ) The PSO method can be applied to parallel computing, i.e., it can perform extremely fast computations. The computation speed is determined by many factors. The number of particles, the accuracy of answers, and the parameters of PSO are all closely connected with the convergence time.

(ⅲ) The PSO parameters are few and convenient to be determined.

All of them prove the reliability and precision of the PSO-based symplectic method.

4.3 Consequence of double-PSO-based instantaneous optimal control

The effects of robotic control are mainly relative to two points. One is the ability to lift an object, and the other is the ability to avoid obstacles. Subsequently, we are going to testify whether the system can perform the two tasks and analyze the controlled behavior, including the change in the external force, the accuracy of the routine, and the time of calculation. When we testify our system's ability to avoid obstacles, we introduce the RRT method to plan the routine. The RRT method can find a not-the-best routine to avoid obstacles. The advantage is that it can plan a path without human participation. The combination of the two methods leads to the scalability of the double-PSO-based instantaneous optimal control and the circumstance adaptability of the robot using this technology.

4.3.1 Control the robot to lift an object

The target routine is a straight line to move the tip of Rod 2 vertically. It has a fundamental application in robots. Therefore, we let the system start with , x₃=0 and all the initial velocity be 0 m/s.

which balances the control accuracy and the magnitude of the external force. M₁, M₂, and F₃ are the external moments and force acting on the hinge. Dx(t) and Dy(t) denote the differences between the planning and the actual routines at the same moment. The total time is 4 s.

The two groups of particles and their parameters are suitably defined.

Figure 7 depicts that the routine of the controlled system strictly follows the given routine. It proves that the algorithm is functional. Meanwhile, Fig. 8 illustrates that the error is relatively small and the external force converges. The system starts with no motion, and we define the system to move with a certain velocity. Therefore, at the beginning, the system must accelerate, which requires a large external force, where a relative large error is present. Evidently, the convergence of the external force and the small number of errors in the routine proves that the algorithm performs well.

4.3.2 Controlling the robot to avoid obstacle

The target routine is designed by the RRT method^[17] to move the system from one place to another so as to avoid obstacles. The obstacle is 0.1 m×0.1 m, and is placed at the horizontal ground of y=-0.4. The mission of the system is to carry something from to x=0.5 (see Fig. 9). We let the system start with , x₃=0 and all initial velocities be 0 m/s.

which balances between the accuracy of control and the magnitude of the external force. M₁ and M₂ denote the external moments acting on the hinge between the slider and Rod 1 and between Rods 1 and 2, respectively, and F₃ denotes the force acting on the slider. Dx(t) and Dy(t) denote the difference between the planning routine and the actual routine at the same moment. In this case, the routine (x(t), y(t)) is calculated by the RRT algorithm, which means that we do not have to find the routine ourselves. The total time is 4 s. h=0.002, T=2 000, v_y=0.05 m·s^-1, and v_x=0 m/s. The two groups of particles and their parameters are suitably defined.

Figure 9 depicts the system routine, where the final position and gesture of the system are shown as two black bars to the right. From the figure, we can see that, for a given accuracy of the control algorithm, we cannot tell the difference between the blue and red lines visually, and only at the start point and the corner can a little difference be spotted (see the inserted figures (a) and (b)). The system moves along the setting routine successfully and precisely. The average error of the routine is 3.549 8×10^-7, and the maximum error is 1.669 7×10^-5 (see Fig. 10). At the beginning of the movement, similarly, a fluctuation of the control external force and error emerges because of the difference between the setting speed and the initial speed. The second small fluctuation is due to the change in the direction of the routine. However, at the steps around 1 300 to 1 400, the external force shakes severely, and the accuracy of the control decreases dramatically. We are curious of what is going on at these points.

The method appears to have the risk of breaking down at any position because neither the turning direction point nor the starting point emerges at approximately T=1 300. However, as is shown in Fig. 11, where the blue dashed line indicates the paths, and the black straight line with a solid circle represents the system, the posture makes the two-bar-system hard to move along the routine. The routine is the reason behind the problem. Furthermore, from the figure, we know that the RRT method is the method used to find a fair routine and not the one to find the best routine. Thus, even in this circumstance, the PSO method can find its way, which proves the reliability of the algorithm.

5 Conclusions

A smart algorithm, the PSO method is introduced in this paper. We combine the PSO method with the symplectic method to solve the DAEs in uncontrolled and controlled robotic systems, and the results show high precision with conserved energy and constant constraint conditions. We also introduce the double-PSO-based instantaneous optimal control. Several examples are performed to verify the applications, and the results show that the method could be used to solve functional extremum problems with high accuracy and quick convergence and has the ability to work with other intelligent algorithms.

The double-PSO-based instantaneous optimal control method is a new method. It can solve robotic control problems, and opens a new dimension of combining dynamic control with intelligent algorithms. It has high reliability and flexibility, and can be used in different circumstances. However, the method has an extraordinary solution with regard to the control mechanics, which consumes the computation load. Reducing the computing time needs parallel computation, which will be one of our future study directions. If parallel computation is successfully compiled, we can take the advantage of the double-PSO-based instantaneous optimal control method in the robotic and real-time control area.