Abstract
The six-degree-of-freedom flexible joint manipulator is a complex system that suffers from the problem that the trajectory planning results are inconsistent with the control results. To keep the planned trajectory within the control range of the manipulator, a hierarchical structure control strategy is designed, which consists of a trajectory planning layer, a model predictive control layer, and a bottom control layer. Specifically, first, the target joint angles are obtained by a time-optimal trajectory planning algorithm based on a genetic algorithm in the trajectory planning layer. Second, in the model predictive control layer, considering the system physical constraints, the model predictive controller is adopted to provide the set points for the Proportion-Differentiation (PD) controllers. Finally, in the bottom control layer, the manipulator moves along the target trajectory under the PD controllers with the feedback control law. The simulation results show that, compared with the PD control strategy, the hierarchical structure control strategy can achieve better control performance and reduce the tracking error of the terminal trajectory by 33.70%.
1. Introduction
A flexible joint manipulator is light and small and consumes little energy, making this technology widely used in aerospace, medical, industrial assembly, and other fields [1, 2]. In practical applications, to meet different mission requirements, it is necessary to optimize the trajectory of the manipulator and implement precise control. Many achievements have been made in the trajectory planning and control implementation of manipulator systems [3, 4]. However, the existing results of manipulator control do not consider the consistency between the trajectory optimization results and the control results; that is, the trajectory optimization results may not be feasible in the control implementation.
The “hierarchical structure” approach is mainly used to deal with “multiperiod, multiobjective, multilevel model and optimization” complex systems in industrial production [5, 6]. In the hierarchical structure, each level has its own division of labour, which makes complex problems easy to deal with [7]. The 6-DOF flexible joint manipulator control strategy is designed for industrial hierarchical structure systems [8]. Figure 1 compares the traditional control structure in the process industry and the hierarchical control structure designed in this paper (where FC, PC, TC, and LC represent flow, pressure, temperature, and liquid level control, resp.). The right portion indicates the basic components of the hierarchical structure, which consist of the trajectory planning layer, the MPC layer, and the bottom control layer. Compared with the existing hierarchical structure in the industry, the main function of the trajectory planning layer is to obtain the target trajectory, the MPC layer makes the terminal trajectory of the manipulator approximate the target trajectory, and the bottom control layer implements the final control.
Trajectory planning is the basis of manipulator motion control. When the trajectory planning of a manipulator is carried out, it is usually possible to maximally optimize some of the indicators while satisfying the given constraints. There are many research results in trajectory planning, among which the time-optimal trajectory planning algorithm of manipulators is studied more [9, 10]. The position, velocity, acceleration, second-order acceleration, and other motion constraints of the manipulator are taken as the premise, and a higher-order polynomial is taken as the curve path between the path points [11]. However, the calculation of higher degree polynomials is complex. Bazaz et al. use the time-optimal trajectory planning method based on cubic spline interpolation, which is widely used and simple to calculate [12]. Because of the low order of the cubic spline interpolation function, the acceleration of the manipulator is discontinuous. The 3-5-3 polynomial interpolation is used to plan the trajectory of a manipulator [13]. Time-optimal trajectory planning is a nonlinear dynamic optimization problem. The constraints are generally nonlinear, the calculation process is complex, and the calculation amount is large. For the optimization of such problems, 3-5-3 polynomial interpolation has no optimization attribute. It can be solved by an intelligent optimization algorithm, such as particle swarm optimization algorithm, artificial neural network algorithm, ant colony algorithm, and simulated annealing algorithm. These algorithms have advantages and disadvantages in the application of the robot trajectory optimization process [14–16]. At present, the genetic algorithm is the most possible general algorithm for trajectory planning and optimization. For example, L. Tian et al. propose the GA method, which is an effective optimization method for the trajectory planning of robot manipulators. The GA method can find an optimal trajectory for the robot even in a complicated environment [16]. Therefore, in this chapter, the optimal time in the process of manipulator motion is taken as the objective function, the path and the maximum angular velocity are taken as the constraints, and the genetic algorithm is used to optimize the interpolation time and time. The optimal trajectory planning method is designed.
The flexible joint manipulator is a multivariable, highly nonlinear complex system with strong coupling, which has uncertainties, including parameter perturbation and external interference. The complexity of the system makes designing a control strategy difficult. Therefore, the problem of manipulator motion control has become an area of active research in recent years. PID control has been widely used in the control of manipulators because the approach is simple, flexible, and independent of the system model and shows other advantages [17, 18]. However, with the increasingly complex structure of manipulators, the PID controller method has been unable to meet the ever-increasing demand for control performance [19–21]. Therefore, a more efficient control strategy needs to be designed. Considering that the multidegree-of-freedom (DOF) manipulator is composed of several joints, model predictive control (MPC) can be applied to manipulator control [22]. For example, a real-time motion control strategy for the manipulator based on the constraint MPC is proposed in [23], which can control the achieved target position under the condition of physical constraints. A robust hierarchical multiloop method is proposed in [24]. The sliding mode control and MPC are used to design the internal and external loops, respectively. The optimal evolution of the manipulator control trajectory is guaranteed. The effectiveness of three control strategies based on sliding mode control, torque control, and MPC is compared, and MPC has the best control effect [25]. In general, considering the advantages of MPC, such as explicit constraints and flexible design of objective functions, it is obvious that its application in manipulator control is expanding.
However, the MPC approach requires much computation and yields a slow response speed, and it is not easy to control the multi-DOF manipulator. A PD controller has good dynamic performance and a faster response speed than the model predictive controller. Therefore, in the hierarchical structure control strategy designed in this paper, the set points obtained by the model predictive controller are sent to the PD controller.
To summarize, the hierarchical structure control strategy, that is, a control strategy of “trajectory planning layer + MPC layer + bottom control layer” for the 6-DOF flexible joint manipulator, is designed. Among the layers, the trajectory planning layer is designed to obtain the target trajectory based on a GA, the MPC layer makes the terminal trajectory of the manipulator approach the target trajectory, and the bottom control layer realizes the final control.
The main contributions of this paper are as follows: (1) based on the appropriate assumptions in [26], a 6-DOF flexible joint model is established, and a dynamics model of a flexible joint manipulator is built based on a simplification of the Lagrange equation; (2) referring to the industrial hierarchical structure system, a hierarchical structure control strategy of a 6-DOF flexible joint manipulator is designed; and (3) the control strategy, which includes the trajectory planning layer, the MPC layer, and the bottom control layer, realizes stable operation of the manipulator under the optimal trajectory.
The organization of this paper is as follows. In Section 2, the dynamics model and state space model of the flexible joint manipulator are established. In Section 3, a time-optimal 3-5-3 polynomial interpolation trajectory planning method based on a GA is designed in the trajectory planning layer. Two controllers, the model predictive controller in the MPC layer and the PD controller in the bottom control layer, are designed in Section 4. In Section 5, the hierarchical structure control strategy is tested and analyzed, and Section 6 gives the conclusions of this paper.
2. The 6-DOF Flexible Joint Manipulator Model
The dynamics of the manipulator are very important in the control of manipulator. The dynamic model of the manipulator is the premise of the high-performance control strategy design. In this chapter, the Lagrange method is used to establish the dynamic equation of the manipulator.
2.1. Dynamics Modelling
The manipulator joint is composed of a power unit (motor), a transmission (reducer), a shaft, and a sensor. The model of the flexible link manipulator is complex, and the influence of flexibility on the motion performance is small. Therefore, the link flexibility is ignored, and the subject is simplified as a flexible joint manipulator. Spong suggested that the spring stiffness coefficient be used to describe the characteristics of flexible joints [26]. For the flexible joint in Figure 2, the following two assumptions are made during the modelling process.
Assumption 1. The link flexibility is ignored, and only the joint flexibility is considered.
Assumption 2. A flexible joint is a linear spring between the motor rotor and a link.
The relationship (Figure 2) between joint torque and stiffness coefficient can be stated as follows:where and the definitions of , and are shown in Table 1. According to Assumptions 1 and 2, the dynamics model of a flexible joint manipulator is based on a simplification of the Lagrange equationwhere (2a) is the dynamic equation for a rigid joint and (2b) is the mathematical model for a flexible joint. The meanings of the other notations are also listed in Table 1.
2.2. State Space Model
The dynamic equation (2) is a nonlinear strongly coupled multivariable equation, for which it is difficult to establish a state space equation. Therefore, it is necessary to linearize and discretize the equation to obtain the discrete state space equation. A Taylor expansion is used to linearize the input and output.
The state variable of the manipulator is defined as , which can be obtained by a Taylor expansion:
According to (2), the relationship between the acceleration of the joint and the state variables of the manipulator can be obtained:
The state variable of the manipulator at the current moment is , and the sampling period is . Each component of the state variable is discretized by a Taylor expansion, and the discrete state space equation can be obtained:where , , , and are matrices related to current state variables. , , , and . Equation (5) is the discrete state space model of the manipulator.
3. Trajectory Planning
The control strategy of the 6-DOF manipulator designed in this paper adopts a hierarchical structure, that is, “trajectory planning layer + MPC layer + bottom control layer.” In this structure, the role of the trajectory planning layer is to obtain the target trajectory through an intelligent algorithm. According to the requirements of the task, the trajectory planning of the manipulator can calculate the target trajectory and convert it into the parameters of the manipulator joint space, which lays the foundation for the accurate and stable execution of the task. In this section, a time-optimal trajectory planning method based on 3-5-3 polynomial interpolation is designed, which uses a GA to optimize each interpolation time.
3.1. Construction of the 3-5-3 Polynomial Function
Given the spatial coordinates of the starting point, two trajectory points, and the terminal point of the manipulator in the rectangular coordinate system, the joint angles of each joint at the four interpolation points can be obtained by solving the inverse kinematics. is the angle of -th joint interpolation, where represents the joint number and represents the ordinal number of the four interpolation points. The constraints for trajectory planning are as follows: the given initial points , , and are path points, the terminal point is , the position between path points is given, the continuity of the velocity and acceleration is given, and the velocity and acceleration at the starting and ending points are 0. A 3-5-3 polynomial interpolation curve trajectory is adopted between points, and the general formula of the 3-5-3 polynomial of the segment -th joint iswhere , and are the first cubic polynomial trajectories, the second polynomial trajectories, and the third joint cubic polynomial trajectories. The unknown coefficients , and are the -th coefficients of the interpolation function of the -th joint trajectory.
According to the above conditions, the unknown coefficients can be deduced and solved. in (7) represents the interpolation time of the polynomial of segment 3 of the -th joint, is only related to , and
Equation (8) is the position matrix of the -th joint angle,
Equation (9) is the solution of unknown coefficients ,
3.2. Trajectory Planning Based on the GA
A method of time-optimal trajectory planning is designed that takes the optimal time in the working process of the manipulator as the objective function and the path and maximum angular velocity as the constraints. The time-optimal trajectory planning problem is a nonlinear dynamic optimization problem whose constraints are generally nonlinear, the calculation process is complex, and the calculation amount is large. For optimization of these kinds of problems, the 3-5-3 polynomial interpolation has no optimization property and can be solved by an intelligent optimization algorithm. In this paper, a time-optimal trajectory planning method that uses a GA to optimize each interpolation time is designed.
The optimization objective is that each joint runs in the shortest time within the scope of the kinematic constraints, and its fitness function iswhere and , , and are the velocities of each polynomial of the -th joint changing with time. If each joint is optimized separately, then the optimization objective function of the -th joint iswhere and are the velocity and maximum limit velocity of the -th joint, respectively.
3.3. Trajectory Planning Algorithm
How to select independent variables to obtain the optimal time is the key problem to solve for the 3-5-3 polynomial interpolation function using a GA. In (7) to (9), times , , and are unknown variables to be optimized, and the coefficients are also unknowns to be solved. The unknown coefficients are taken as independent variables, and , , and are taken as dependent variables. According to (7)–(9), the dimension of the variables to be optimized is 14. If the search space of time variables , , and is optimized directly, then the dimension of variables to be optimized is reduced to 3, decreasing the complexity and difficulty of population optimization.
To make the joint speed converge to the constraint as quickly as possible, two fitness function switching control is adopted. If any velocity does not conform to (12), then the fitness function of the individual is , where is the -th group of the group population and is the number of segments of polynomial interpolation. While finding the optimal individual, the genetic algorithm reduces until the constraint body condition is satisfied. If the maximum speed of the three segments is consistent with (12), then (11) is used as the fitness function, and the genetic algorithm iteratively reduces the running time of each segment of the joint towards the optimization goal.
Specifically, the steps of the GA for time-optimal trajectory planning of the -th joint of the manipulator are as follows:Step 1:(initialize the population). populations are randomly generated in the time search space of three interpolation functions of the -th joint: populations , , and .Step 2:By substituting m groups of time variables , , and into (7)–(9), the unknown coefficients of the 3-5-3 polynomial are obtained.Step 3:The coefficients of the 3-5-3 polynomial are substituted into (6), and the velocity function of the joint angle is obtained by using the time derivative to judge whether the real-time velocity satisfies (11).Step 4:According to the constraints, the fitness function is selected to calculate the fitness value of each individual.Step 5:The GA operation is as follows:Step 6:Constitute new populations , , and .Step 7:If the termination condition (usually a good fitness value or a preset maximum number of iterations ) is met, stop; else, go to Step 2.Step 8:Complete the time optimization of each segment. The maximum value of each joint in each period of time is taken to ensure that kinematic constraints are met; that is, .
The GA requires cyclic iteration. The GA studied in this paper searches in the search space of the optimization target. When (7) is calculated, the number of iteration steps is large, which may lead to nonfull rank matrices. Therefore, during each iteration, it is necessary to test whether there are any individuals with an actual value of zero in the newly generated population. If so, individual values are randomly regenerated.
4. Trajectory Tracking
The hierarchical structure control strategy of the six-degree-of-freedom manipulator is divided into three parts. First, the target joint angles of the six flexible joints are obtained in the trajectory planning layer. Then, in the MPC layer, the target joint angles are passed to the model predictive controller, and the optimized joint angles are obtained by solving (13). Further, in the bottom control layer, the optimized joint angles are transferred to the PD controller, the joint motion is driven by the torques obtained through (14), and the joint angles are measured by sensors. Finally, the joint angles are fed back to the model predictive controller for correction and input into forward kinematics to obtain the terminal trajectory.
4.1. Model Predictive Controller in the MPC Layer
Considering that MPC is a multivariable control strategy and the constraints can be explicitly processed in the optimization problem, this approach is widely used for manipulator control [27]. In this paper, the six-input-six-output MPC controller is selected for the 6-DOF manipulator.
The MPC strategy explicitly incorporates the model and constraints of the system for determining control inputs. At each step , the state space model is used to predict future outputs. At step , an optimization problem involving future horizon prediction information (predictive horizon ) is solved online, and the control input of the future horizon (control horizon ) is calculated. At step , the optimization problem is solved again with new measurements. Thus, the control and prediction horizon recede by one step as time moves ahead by one step.
The MPC strategy is adopted to design the model predictive controller and optimize the joint angle as follows:where is the control input; is the cost function; and represents the target angle of the -th joint, which is solved using inverse kinematics. Only the first obtained control input is applied to the system as the given value of the PD controller.
4.2. PD Controller in the Bottom Control Layer
However, the MPC is computationally expensive and slow in response, and it is not easy to control the multidegree-of-freedom manipulator. Compared with model predictive controller, the PD controller has better dynamic performance and faster response speed. Therefore, in the hierarchical control strategy designed in this paper, the model predictive controller with six inputs and six outputs is used to optimize the joint angle, and then the set values are transmitted to the bottom PD controllers to achieve.
PD feedback control is simple and easily implemented in engineering practice. Here, the PD controller is designed as follows:where is the actual -th joint angular velocity; is the optimized -th joint angle of the upper model predictive controller; is the optimized -th joint driving torque of the PD controller; and and are the proportional and differential terms, respectively, both of which are diagonal positive definite matrices.
4.3. The Overall Algorithm
The overall algorithm is as follows. (1)If (off-line stage)(2){(3) Set the D-H parameters of the 6-DOF flexible joint manipulator.(4) Set the maximum simulation step .(5) Set predictive horizon and control horizon in the model predictive controller.(6)}(7)else if (online stage)(8){(1) Obtain the target joint angle in the trajectory planning layer.(2) Input the target joint angle into the model predictive controller, and obtain the optimized joint angle by solving (13).(3) Pass the optimized joint angle into the PD controller; the torque is obtained by (14) to drive the joint action.(4) Feed back the joint angle to the model predictive controller for correction.(5) Inject into the forward kinematics, and obtain the terminal trajectory .(9)}
5. Simulation and Experiment
To verify the effectiveness of the hierarchical structure control strategy, the designed strategy is simulated using the Robotics Toolbox of MATLAB 2015a.
5.1. Simulation of Trajectory Planning
The simulation object in this section is a 6-DOF flexible joint manipulator. The kinematics model of the manipulator is constructed by the D-H method. The D-H parameters of the joints are shown in Table 2, where is the -th link twist angle, is the -th link length, and is the offset value of the -th joint.
In the Cartesian coordinate system, the interpolation points of the manipulator are given as shown in Table 3.
Through the inverse kinematics solution, the joint angles corresponding to the initial point, path points, and terminal point of the joints are obtained, as shown in Table 4.
The 3-5-3° polynomial interpolation method based on PSO is used to optimize the interpolation time. The parameters are set as follows: set the particle swarm to 20, the number of iteration steps to 50, the inertia weight to and , and the weight factors to and . The maximum allowable velocity of the joint is /s, and the velocity and acceleration at the initial point and the terminal point are 0. After 50 iterations, the joint interpolation times are s, s, and s. The joint position curve, velocity curve, and acceleration curve are shown in Figures 3–5.
The steps in Section 3.3 are followed to determine the best time for each interpolation segment. Among the parameters, the population size is 100, the number of evolution generations is 50, the hybridization rate is , and the variation rate is . The maximum allowable velocity of the joint is /s, and the velocity and acceleration at the initial point and the terminal point are 0. According to Step 8, the joint interpolation times are s, s, and s.
After time optimization, the curves of the joint position, velocity, and acceleration are as shown in Figures 6–8. The joint velocity of the manipulator is close to , and the acceleration is relatively stable.
Compared with the PSO results in the previous section, the GA is 15.905 7 s shorter than PSO in time. Although the two algorithms can reach the optimal time under the kinematic constraints, after the optimization of GA, the joint speed is closer to the maximum speed, and the convergence speed is faster.
Although the PSO has the characteristics of simple structure and easy parameter adjustment, it still needs to use a random number to generate particle speed when generating a new particle swarm, thus reducing the convergence speed of particles. Although the GA is also based on the mask crossover of random generation, this paper can quickly eliminate vulnerable individuals by setting the crossover rate so that the individuals in the population can maintain high adaptability.
The PSO has a fast convergence speed. However, when the particles are too concentrated, they are likely to fall into local minima. Therefore, as shown in Table 5, in 10 simulation tests, the optimization effect of most particle swarm optimization algorithms is basically consistent with that of the genetic algorithm and sometimes even better than the genetic algorithm. However, when the particle swarm optimization algorithm falls into the local minimum, its optimization effect is far less than that of the genetic algorithm, such as the fifth simulation result. Because the mutation operator of the genetic algorithm can ensure that the genetic algorithm jumps out of the local minimum, the stability of the genetic algorithm is higher than that of the particle swarm optimization algorithm, so the stability of the genetic algorithm is higher.
5.2. Simulation of the Hierarchical Structure Control Strategy
The rigidity coefficient is , while the predictive horizon and the control horizon are and , respectively. The maximum simulation step is . The sampling period is chosen as 0.02 s. Considering the complexity of MPC, in each sampling period , MPC calculates a group of set points only once, while the PD controller calculates 30 times to track the set points. The simulation results are shown in Figures 9–16.
Compared with the PD control strategy, the terminal trajectory of the hierarchical structure control strategy is more stable, and the tracking performance is much better, which shows that the designed strategy can restrain the jitter of flexible joints more effectively. For convenience of analysis, the projection of the trajectories in Figure 9 on the plane is shown in Figure 10, and further, the angle trajectories of each joint are shown in Figures 11–16. From Figures 1– 16, we obtain the same conclusions.
The sum of squared error (SSE) is adopted to evaluate the effectiveness of the two control strategy:where is the actual value and is the target value. The SSE comparison is shown in Table 6. For the joint trajectories, the SSE of the hierarchical structure control strategy is always smaller than that of the PD control strategy, and for the terminal trajectories, the SSE of the hierarchical structure control strategy is reduced by compared with the PD control strategy. In general, the designed hierarchical structure control strategy can coordinate and control the 6-DOF flexible joint manipulator more effectively than the PD control strategy.
To further illustrate the anti-interference and antidisturbance performance of the proposed method, here, we design two cases. In Case 1, we add the white noise ([−0.001, 0.001]) in the feedback path. In Case 2, we add the disturbance (the amplitude is 0.005, and the time of duration is 1s) in the feedback path. The simulation results are shown in Figures 17 and 18, respectively. From Figures 17 and 18, we can clearly find that the tracking performance of the proposed strategy is much better than that of the PD strategy in both of the above two cases.
5.3. Experiment of the Hierarchical Structure Control Strategy
In this paper, the dynamics and control theory of 6-DOF manipulator are studied, and the hierarchical structure control strategy is verified by simulation, but there are differences between theory and practice. Therefore, this section that has carried on the experimental research to the manipulator, through the experiment, has verified the validity and the feasibility of the control strategy.
In this paper, we use the desktop manipulator, LeArm, as shown in Figure 19. The LeArm manipulator has six movable joints, with joint 1 moving left and right to realize base rotation; joint 2 and joint 3 moving back and forth; joint 4 moving up and down; joint 5 moving left and right to realize mechanical claw rotation; joint 6 closing the mechanical claw.
The D-H parameters of the arm are shown in Table 7. The joints of the LeArm manipulator are driven by actuators and cannot directly feed back the actual joint angle. Therefore, in this experiment, the BWT901CL angle sensor is used to measure the joint angle, and the measured joint angle is fed back to the model predictive controller through a serial port for feedback correction, thus avoiding the error caused by the model mismatch.
In order to verify the effectiveness of the hierarchical structure control strategy of the 6-DOF manipulator, based on MPC, an experimental study was carried out on the LeArm manipulator. Firstly, the target position of each joint is given; then, the hierarchical structure control strategy designed in this paper and the traditional PD control strategy are, respectively, used to make the manipulator reach the target position, as shown in Figure 19, which are three states in the process of robot arm motion; finally, taking joint 2 as an example and taking joint 3 and joint 4 as research objects, the joint angle curves under the two control strategies are compared in Figures 18–22.
As shown in Figures 20–22, by comparing and analyzing the curves under the two control strategies, the effect of the hierarchical control strategy is obviously better than that of the PD control strategy. Although these two control strategies can drive the manipulator to the target position in a limited time, the difference is that the hierarchical structure control strategy can get the predicted output of the actuator in the time domain in advance. When the manipulator is close to the target position, the speed of the actuator is limited, which makes the manipulator fast and stable and reduces the overshoot.
Generally speaking, the hierarchical structure control strategy is outstanding in terms of rapidity and overshoot suppression, which verifies the effectiveness of the hierarchical structure control strategy designed in this paper for the manipulator control process. However, the hierarchical structure is more complex than that of the PD strategy, which brings in more challenges during the implementation in the actual application; at the same time, the computation burden is much heavier, and the computing devices with higher performance are needed. In other words, the performance improvements of the proposed strategy come at the expense of the control system complexity and the computation increase.
6. Conclusions
In this paper, based on the dynamics analysis of a 6-DOF flexible joint manipulator, the trajectory planning and control implementation of the manipulator are thoroughly studied. By designing a hierarchical structure control strategy, the manipulator can achieve the target trajectory quickly and stably. Simulation results show that the control performance of the designed strategy is better than that of the PD control strategy. In the future, the following two points should be further paid more attention: (1) the research results should consider the actual condition, such as the system uncertainty and external disturbance; (2) the proposed strategy should be implemented online to the actual application scenarios through the embedded devices.
Data Availability
The data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.