# A Hierarchical Structure Control Strategy Based on MPC for a Six-DOF Flexible Joint Manipulator

Authors: Ningbo Jing; Ming Bu; Qi Ni; Hongguang Pan; Xuebin Qin; Xianmin Ma
Journal: Mathematical Problems in Engineering
 (2021)

---

## 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%.


---

## Body

## 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.Figure 1
Comparison of hierarchical control structures.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 Section2, 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.Figure 2
The model of thei-th flexible joint in the manipulator.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 (Figure2) between joint torque and stiffness coefficient can be stated as follows:(1)τi=Kiθi−qi,where i=1,2,…,6 and the definitions of τi,Ki,qi, and θi 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 equation(2a)Hqq¨+Fq,q˙q˙+Gq=Kθ−q,(2b)Jθ..+Kθ−q=τm,where (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.Table 1
Notations.
NotationMeaningqLink angle vector,q∈ℝ6×1q˙Link velocity vector,q˙∈ℝ6×1q¨Link acceleration vector,q¨∈ℝ6×1qii-th link angleθJoint angle vector,θ∈ℝ6×1θ..Joint acceleration vector,θ..∈ℝ6×1θii-th joint angleτmJoint driving torque vector,τm∈ℝ6×1τiTorque acting on thei-th linkHqInertia matrix for the rigid links,H=HT,H∈ℝ6×6Fq,q˙q˙Torque vector caused by Coriolis force andcentrifugal forces,F∈ℝ6×6GqTorque vector caused by gravity,G∈ℝ6×1JDiagonal matrix of actuator inertias reflectedto the link side of gears,J∈ℝ6×6KDiagonal matrix containing the joint stiffnesscoefficients,K∈ℝ6×6Kii-th joint stiffness coefficients


### 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 asx=θ,θ˙T, which can be obtained by a Taylor expansion:(3)xt+Δt=θt+Δtθ˙t+Δt22θ¨θ˙t+Δtθ¨.According to (2), the relationship between the acceleration θ¨ of the joint and the state variables of the manipulator can be obtained:(4)θ¨=J−1τm−Hqq¨−Cq,q˙q˙−Gq.The state variable of the manipulator at the current moment isxn, and the sampling period is T. Each component of the state variable is discretized by a Taylor expansion, and the discrete state space equation can be obtained:(5a)xn+1=xt+n+1T=Axn+Bτmn+Gp,(5b)yn=Cxn,where A, B, Gp, and C are matrices related to current state variables. A=I6TI6O6∗6I6, B=T2/2−1TM−1, Gp=−T2/2M−1Hqq¨+Cq,q˙q˙+Gq−TJ−1Hqq¨+Cq,q˙q˙+Gq, and C=I6O6∗6. Equation (5) is the discrete state space model 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.Figure 2
The model of thei-th flexible joint in the manipulator.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 (Figure2) between joint torque and stiffness coefficient can be stated as follows:(1)τi=Kiθi−qi,where i=1,2,…,6 and the definitions of τi,Ki,qi, and θi 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 equation(2a)Hqq¨+Fq,q˙q˙+Gq=Kθ−q,(2b)Jθ..+Kθ−q=τm,where (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.Table 1
Notations.
NotationMeaningqLink angle vector,q∈ℝ6×1q˙Link velocity vector,q˙∈ℝ6×1q¨Link acceleration vector,q¨∈ℝ6×1qii-th link angleθJoint angle vector,θ∈ℝ6×1θ..Joint acceleration vector,θ..∈ℝ6×1θii-th joint angleτmJoint driving torque vector,τm∈ℝ6×1τiTorque acting on thei-th linkHqInertia matrix for the rigid links,H=HT,H∈ℝ6×6Fq,q˙q˙Torque vector caused by Coriolis force andcentrifugal forces,F∈ℝ6×6GqTorque vector caused by gravity,G∈ℝ6×1JDiagonal matrix of actuator inertias reflectedto the link side of gears,J∈ℝ6×6KDiagonal matrix containing the joint stiffnesscoefficients,K∈ℝ6×6Kii-th joint stiffness coefficients


## 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 asx=θ,θ˙T, which can be obtained by a Taylor expansion:(3)xt+Δt=θt+Δtθ˙t+Δt22θ¨θ˙t+Δtθ¨.According to (2), the relationship between the acceleration θ¨ of the joint and the state variables of the manipulator can be obtained:(4)θ¨=J−1τm−Hqq¨−Cq,q˙q˙−Gq.The state variable of the manipulator at the current moment isxn, and the sampling period is T. Each component of the state variable is discretized by a Taylor expansion, and the discrete state space equation can be obtained:(5a)xn+1=xt+n+1T=Axn+Bτmn+Gp,(5b)yn=Cxn,where A, B, Gp, and C are matrices related to current state variables. A=I6TI6O6∗6I6, B=T2/2−1TM−1, Gp=−T2/2M−1Hqq¨+Cq,q˙q˙+Gq−TJ−1Hqq¨+Cq,q˙q˙+Gq, and C=I6O6∗6. 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.ϕij is the angle of i-th joint interpolation, where i=1,2,3,4,5,6 represents the joint number and j=0,1,2,3 represents the ordinal number of the four interpolation points. The constraints for trajectory planning are as follows: the given initial points ϕi0, ϕi1, and ϕi2 are path points, the terminal point is ϕi3, 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 i-th joint is(6a)hi1t=ai13t3+ai12t2+ai11t1+ai10,(6b)hi2t=ai25t5+ai24t4+ai23t3+ai22t2+ai21t1+ai20,(6c)hi3t=ai33t3+ai32t2+ai31t1+ai30,where hi1t,hi2t, and hi3t are the first cubic polynomial trajectories, the second polynomial trajectories, and the third joint cubic polynomial trajectories. The unknown coefficients ai1j,ai2j, and ai3j are the j-th coefficients of the interpolation function of the i-th joint trajectory.According to the above conditions, the unknown coefficientsaij can be deduced and solved. ti in (7) represents the interpolation time of the polynomial of segment 3 of the i-th joint, D is only related to t, and(7)D=t13t12t1100000000003t122t11000000000006t120000000000000000t25t24t23t22t21000−100005t244t233t222t21000−10000020t2312t226t22000−2000000000000t33t32t3100−200000003t322t3100−1000000006t3200−1001000000000000100000000000010000000000000000000000000100000000010000.Equation (8) is the position matrix of the i-th joint angle,(8)a=ai13,ai12,ai11,ai10,ai25,ai24,ai23,ai22,ai21,ai20,ai33,ai32,ai31,ai30T=invD∗b.Equation (9) is the solution of unknown coefficients aij,(9)b=0,0,0,0,0,0,ϕi3,0,0,ϕi0,0,0,ϕi2,ϕi1T.


### 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 is(10a)ft=min∑i=16ti1+ti2+ti3,(10b)s.t.1,2,(10c)maxVi1≤Vmax,(10d)maxVi2≤Vmax,(10e)maxVi3≤Vmax,where i=1,2,…,6 and Vi1, Vi2, and Vi3 are the velocities of each polynomial of the i-th joint changing with time. If each joint is optimized separately, then the optimization objective function of the i-th joint is(11)fit=minti1+ti2+ti3,(12)maxVi≤Vimax,where Vi and Vimax are the velocity and maximum limit velocity of the i-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 t1, t2, and t3 are unknown variables to be optimized, and the coefficients aij are also unknowns to be solved. The unknown coefficients aij are taken as independent variables, and t1, t2, and t3 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 t1, t2, and t3 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 isvi,j, where i is the i-th group of the M group population and j is the number of segments of polynomial interpolation. While finding the optimal individual, the genetic algorithm reduces vi,j 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 thei-th joint of the manipulator are as follows:Step 1:
(initialize the population).M populations are randomly generated in the time search space of three interpolation functions of the i-th joint: populations POPt1, POPt2, and POPt3.Step 2:
By substitutingm groups of time variables ti1, ti2, and ti3 into (7)–(9), the unknown coefficients aij of the 3-5-3 polynomial are obtained.Step 3:
The coefficientsaij 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 newM×3 populations POPt1, POPt2, and POPt3.Step 7:
If the termination condition (usually a good fitness value or a preset maximum number of iterationsNmax) 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,t1=maxti1,t2=maxti2,andt3=maxti3.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.


## 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.ϕij is the angle of i-th joint interpolation, where i=1,2,3,4,5,6 represents the joint number and j=0,1,2,3 represents the ordinal number of the four interpolation points. The constraints for trajectory planning are as follows: the given initial points ϕi0, ϕi1, and ϕi2 are path points, the terminal point is ϕi3, 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 i-th joint is(6a)hi1t=ai13t3+ai12t2+ai11t1+ai10,(6b)hi2t=ai25t5+ai24t4+ai23t3+ai22t2+ai21t1+ai20,(6c)hi3t=ai33t3+ai32t2+ai31t1+ai30,where hi1t,hi2t, and hi3t are the first cubic polynomial trajectories, the second polynomial trajectories, and the third joint cubic polynomial trajectories. The unknown coefficients ai1j,ai2j, and ai3j are the j-th coefficients of the interpolation function of the i-th joint trajectory.According to the above conditions, the unknown coefficientsaij can be deduced and solved. ti in (7) represents the interpolation time of the polynomial of segment 3 of the i-th joint, D is only related to t, and(7)D=t13t12t1100000000003t122t11000000000006t120000000000000000t25t24t23t22t21000−100005t244t233t222t21000−10000020t2312t226t22000−2000000000000t33t32t3100−200000003t322t3100−1000000006t3200−1001000000000000100000000000010000000000000000000000000100000000010000.Equation (8) is the position matrix of the i-th joint angle,(8)a=ai13,ai12,ai11,ai10,ai25,ai24,ai23,ai22,ai21,ai20,ai33,ai32,ai31,ai30T=invD∗b.Equation (9) is the solution of unknown coefficients aij,(9)b=0,0,0,0,0,0,ϕi3,0,0,ϕi0,0,0,ϕi2,ϕi1T.


## 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 is(10a)ft=min∑i=16ti1+ti2+ti3,(10b)s.t.1,2,(10c)maxVi1≤Vmax,(10d)maxVi2≤Vmax,(10e)maxVi3≤Vmax,where i=1,2,…,6 and Vi1, Vi2, and Vi3 are the velocities of each polynomial of the i-th joint changing with time. If each joint is optimized separately, then the optimization objective function of the i-th joint is(11)fit=minti1+ti2+ti3,(12)maxVi≤Vimax,where Vi and Vimax are the velocity and maximum limit velocity of the i-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 t1, t2, and t3 are unknown variables to be optimized, and the coefficients aij are also unknowns to be solved. The unknown coefficients aij are taken as independent variables, and t1, t2, and t3 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 t1, t2, and t3 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 isvi,j, where i is the i-th group of the M group population and j is the number of segments of polynomial interpolation. While finding the optimal individual, the genetic algorithm reduces vi,j 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 thei-th joint of the manipulator are as follows:Step 1:
(initialize the population).M populations are randomly generated in the time search space of three interpolation functions of the i-th joint: populations POPt1, POPt2, and POPt3.Step 2:
By substitutingm groups of time variables ti1, ti2, and ti3 into (7)–(9), the unknown coefficients aij of the 3-5-3 polynomial are obtained.Step 3:
The coefficientsaij 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 newM×3 populations POPt1, POPt2, and POPt3.Step 7:
If the termination condition (usually a good fitness value or a preset maximum number of iterationsNmax) 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,t1=maxti1,t2=maxti2,andt3=maxti3.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 stepk≥1, the state space model is used to predict future outputs. At step k, an optimization problem involving future horizon prediction information (predictive horizon Np) is solved online, and the control input of the future horizon (control horizon Nc) is calculated. At step k+1, 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:(13a)minθk|k,θk+1|k,…,θk+Nc−1|kJpk,(13b)s.t.5,(13c)θk+m|k∈−180°,180°,0≤m≤Nc−1,(13d)θk+m|k=θk+Nc−1|k,Nc≤m≤Np−1,where θk+m|k∈ℝ6×1 is the control input; Tex translation failed is the cost function; and θid· represents the target angle of the i-th joint, which is solved using inverse kinematics. Only the first obtained control input θ∗k|k 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:(14)τim∗=−Kpθi−θi∗−Kdθi.,where θ.i is the actual i-th joint angular velocity; θi∗ is the optimized i-th joint angle of the upper model predictive controller; τim∗ is the optimized i-th joint driving torque of the PD controller; and Kp and Kd 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)
Ifk=0 (off-line stage)(2)
{(3)
Set the D-H parameters of the 6-DOF flexible joint manipulator.(4)
Set the maximum simulation stepkmax.(5)
Set predictive horizonNp and control horizon Nc in the model predictive controller.(6)
}(7)
else if0<k≤kmax (online stage)(8)
{(1)
Obtain the target joint angleθid in the trajectory planning layer.(2)
Input the target joint angleθid into the model predictive controller, and obtain the optimized joint angle θi∗ by solving (13).(3)
Pass the optimized joint angleθi∗ into the PD controller; the torque τim∗ is obtained by (14) to drive the joint action.(4)
Feed back the joint angleθi to the model predictive controller for correction.(5)
Injectθi into the forward kinematics, and obtain the terminal trajectory X.(9)
}


## 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 stepk≥1, the state space model is used to predict future outputs. At step k, an optimization problem involving future horizon prediction information (predictive horizon Np) is solved online, and the control input of the future horizon (control horizon Nc) is calculated. At step k+1, 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:(13a)minθk|k,θk+1|k,…,θk+Nc−1|kJpk,(13b)s.t.5,(13c)θk+m|k∈−180°,180°,0≤m≤Nc−1,(13d)θk+m|k=θk+Nc−1|k,Nc≤m≤Np−1,where θk+m|k∈ℝ6×1 is the control input; Tex translation failed is the cost function; and θid· represents the target angle of the i-th joint, which is solved using inverse kinematics. Only the first obtained control input θ∗k|k 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:(14)τim∗=−Kpθi−θi∗−Kdθi.,where θ.i is the actual i-th joint angular velocity; θi∗ is the optimized i-th joint angle of the upper model predictive controller; τim∗ is the optimized i-th joint driving torque of the PD controller; and Kp and Kd 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)
Ifk=0 (off-line stage)(2)
{(3)
Set the D-H parameters of the 6-DOF flexible joint manipulator.(4)
Set the maximum simulation stepkmax.(5)
Set predictive horizonNp and control horizon Nc in the model predictive controller.(6)
}(7)
else if0<k≤kmax (online stage)(8)
{(1)
Obtain the target joint angleθid in the trajectory planning layer.(2)
Input the target joint angleθid into the model predictive controller, and obtain the optimized joint angle θi∗ by solving (13).(3)
Pass the optimized joint angleθi∗ into the PD controller; the torque τim∗ is obtained by (14) to drive the joint action.(4)
Feed back the joint angleθi to the model predictive controller for correction.(5)
Injectθi into the forward kinematics, and obtain the terminal trajectory X.(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 Table2, where αi is the i-th link twist angle, ai is the i-th link length, and di is the offset value of the i-th joint.Table 2
D-H parameters of the manipulator.
Jointsθαad1θ190°002θ20431 mm03θ3−90°20.3 mm150 mm4θ490°0431 mm5θ5−90°006θ6000In the Cartesian coordinate system, the interpolation points of the manipulator are given as shown in Table3.Table 3
The interpolation points of the manipulator in Cartesian space.
Starting pointFirst interpolation pointSecond interpolation pointTerminal point(−431, −150, 452.1)(−444.1, −128.5, 452.2)(−468.1, −130.5, 452.2)(−478.7, −149.2, 452.1)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 Table4.Table 4
The interpolation points in joint space.
PointsJoints123456ϕ0000000ϕ1−2.78870.845 7−0.862721.566 2−2.082321.567 4ϕ2−2.45654.138 5−4.2978−80.2392−2.353880.237 3ϕ3−0.10056.328 1−6.6750−140.7006−0.3724145.687 4The 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 towmax=0.9 and wmin=0.4, and the weight factors to c1=2 and c2=2. The maximum allowable velocity of the joint is Vmax=1rad/s, and the velocity and acceleration at the initial point and the terminal point are 0. After 50 iterations, the joint interpolation times are t1=11.3831s, t2=4.2968s, and t3=14.2823s. The joint position curve, velocity curve, and acceleration curve are shown in Figures 3–5.Figure 3
Joint position based on PSO.Figure 4
Joint velocity based on PSO.Figure 5
Joint acceleration curve based on PSO.The steps in Section3.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 pc=0.8, and the variation rate is pm=0.5. The maximum allowable velocity of the joint is Vmax=1rad/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 t1=7.3008s, t2=2.5050s, and t3=4.2507s.After time optimization, the curves of the joint position, velocity, and acceleration are as shown in Figures6–8. The joint velocity of the manipulator is close to Vmax, and the acceleration is relatively stable.Figure 6
Joint position based on GA.Figure 7
Joint velocity based on GA.Figure 8
Joint acceleration curve based on GA.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 Table5, 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.Table 5
Trajectory optimization results statistics.
AlgorithmSimulation times12345678910PSO14.02s14.25s13.89s18.23s29.96s15.31s13.68s14.12s17.88s15.55sGA14.98s14.32s15.01s14.24s14.06s16.94s18.55s13.62s17.50s14.22sNote. The PSO and GA are compared in joint angle, angular velocity, and angular acceleration curve.


### 5.2. Simulation of the Hierarchical Structure Control Strategy

The rigidity coefficient isK=104, while the predictive horizon and the control horizon are Np=30 and Nc=5, respectively. The maximum simulation step is kmax=705. The sampling period T is chosen as 0.02 s. Considering the complexity of MPC, in each sampling period T, 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.Figure 9
Terminal trajectories of the manipulator.Figure 10
Terminal trajectories of the manipulator (xy plane).Figure 11
Angle trajectories of joint 1.Figure 12
Angle trajectories of joint 2.Figure 13
Angle trajectories of joint 3.Figure 14
Angle trajectories of joint 4.Figure 15
Angle trajectories of joint 5.Figure 16
Angle trajectories of joint 6.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 Figure9 on the xy 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:(15)SSE=∑k=1kmaxEk−Edk2,where Ek is the actual value and Edk 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 33.70% 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.Table 6
SSE comparisons.
StrategyTerminal trajectoryJoints123456PD strategy1841.900 00.131 40.109 30.122 359.671 40.108 059.584 9Hierarchical structure strategy1221.200 00.078 20.085 70.095 945.087 50.062 245.022 8Improvement33.70%40.49%21.59%21.58%24.44%42.41%24.44%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 Figures17 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.Figure 17
Terminal trajectories (noise).Figure 18
Terminal trajectories (disturbance).


### 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 Figure19. 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.Figure 19
The LeArm manipulator.The D-H parameters of the arm are shown in Table7. 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.Table 7
D-H parameters of LeArm manipulator.
Jointsθαad1θ100149 mm2θ290°003θ30100 mm04θ4090 mm05θ50156 mm06θ6000In 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 Figure19, 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.Figure 20
Joint 2 position curve.Figure 21
Joint 3 position curve.Figure 22
Joint 4 position curve.As shown in Figures20–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.


## 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 Table2, where αi is the i-th link twist angle, ai is the i-th link length, and di is the offset value of the i-th joint.Table 2
D-H parameters of the manipulator.
Jointsθαad1θ190°002θ20431 mm03θ3−90°20.3 mm150 mm4θ490°0431 mm5θ5−90°006θ6000In the Cartesian coordinate system, the interpolation points of the manipulator are given as shown in Table3.Table 3
The interpolation points of the manipulator in Cartesian space.
Starting pointFirst interpolation pointSecond interpolation pointTerminal point(−431, −150, 452.1)(−444.1, −128.5, 452.2)(−468.1, −130.5, 452.2)(−478.7, −149.2, 452.1)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 Table4.Table 4
The interpolation points in joint space.
PointsJoints123456ϕ0000000ϕ1−2.78870.845 7−0.862721.566 2−2.082321.567 4ϕ2−2.45654.138 5−4.2978−80.2392−2.353880.237 3ϕ3−0.10056.328 1−6.6750−140.7006−0.3724145.687 4The 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 towmax=0.9 and wmin=0.4, and the weight factors to c1=2 and c2=2. The maximum allowable velocity of the joint is Vmax=1rad/s, and the velocity and acceleration at the initial point and the terminal point are 0. After 50 iterations, the joint interpolation times are t1=11.3831s, t2=4.2968s, and t3=14.2823s. The joint position curve, velocity curve, and acceleration curve are shown in Figures 3–5.Figure 3
Joint position based on PSO.Figure 4
Joint velocity based on PSO.Figure 5
Joint acceleration curve based on PSO.The steps in Section3.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 pc=0.8, and the variation rate is pm=0.5. The maximum allowable velocity of the joint is Vmax=1rad/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 t1=7.3008s, t2=2.5050s, and t3=4.2507s.After time optimization, the curves of the joint position, velocity, and acceleration are as shown in Figures6–8. The joint velocity of the manipulator is close to Vmax, and the acceleration is relatively stable.Figure 6
Joint position based on GA.Figure 7
Joint velocity based on GA.Figure 8
Joint acceleration curve based on GA.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 Table5, 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.Table 5
Trajectory optimization results statistics.
AlgorithmSimulation times12345678910PSO14.02s14.25s13.89s18.23s29.96s15.31s13.68s14.12s17.88s15.55sGA14.98s14.32s15.01s14.24s14.06s16.94s18.55s13.62s17.50s14.22sNote. The PSO and GA are compared in joint angle, angular velocity, and angular acceleration curve.


## 5.2. Simulation of the Hierarchical Structure Control Strategy

The rigidity coefficient isK=104, while the predictive horizon and the control horizon are Np=30 and Nc=5, respectively. The maximum simulation step is kmax=705. The sampling period T is chosen as 0.02 s. Considering the complexity of MPC, in each sampling period T, 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.Figure 9
Terminal trajectories of the manipulator.Figure 10
Terminal trajectories of the manipulator (xy plane).Figure 11
Angle trajectories of joint 1.Figure 12
Angle trajectories of joint 2.Figure 13
Angle trajectories of joint 3.Figure 14
Angle trajectories of joint 4.Figure 15
Angle trajectories of joint 5.Figure 16
Angle trajectories of joint 6.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 Figure9 on the xy 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:(15)SSE=∑k=1kmaxEk−Edk2,where Ek is the actual value and Edk 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 33.70% 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.Table 6
SSE comparisons.
StrategyTerminal trajectoryJoints123456PD strategy1841.900 00.131 40.109 30.122 359.671 40.108 059.584 9Hierarchical structure strategy1221.200 00.078 20.085 70.095 945.087 50.062 245.022 8Improvement33.70%40.49%21.59%21.58%24.44%42.41%24.44%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 Figures17 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.Figure 17
Terminal trajectories (noise).Figure 18
Terminal trajectories (disturbance).


## 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 Figure19. 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.Figure 19
The LeArm manipulator.The D-H parameters of the arm are shown in Table7. 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.Table 7
D-H parameters of LeArm manipulator.
Jointsθαad1θ100149 mm2θ290°003θ30100 mm04θ4090 mm05θ50156 mm06θ6000In 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 Figure19, 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.Figure 20
Joint 2 position curve.Figure 21
Joint 3 position curve.Figure 22
Joint 4 position curve.As shown in Figures20–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.



---

*Source: 1396458-2021-12-24.xml*