A Terminal Residual Vibration Suppression Method of A Robot Based on Joint Trajectory Optimization

1. Introduction

With the development of science and technology and the rise of industrial automation, industrial robots perform tasks through efficient and precise movements, improving productivity, reducing costs, and creating a safer working environment for humans [1,2,3]. The flexible characteristics of industrial robot joint transmission parts like the RV reducers and gears reduce the overall stiffness of the robot, making it have a more complex vibration mode and easier to vibrate during and after the movement [4,5]. Such vibration problems have become one of the most important factors affecting the performance and stability of robots. Robot vibration suppression technology reduces or eliminates robot vibration through advanced control strategies and design methods, thereby improving motion accuracy and stability.

With an accurate system model, motion profiles or feedforward control can be generated based on optimal trajectory planning to minimize overshoot and residual vibration. Based on the chaotic particle swarm optimization algorithm, the trajectory interpolation parameters can be optimized with the minimum vibrational deformation of the flexible end as the objective function [6]. In literature [7], the dynamic equation of the flexible curve connecting rod was derived by using the absolute nodal coordinate formula, and the optimal trajectory method was used to generate the trajectory with the least vibration of the flexible curve connecting rod. Based on the optimal control theory, the trajectory planning problem of suppressing the residual vibration of the robot was transformed into the optimal control performance problem, and the Fourier series was used to fit the discrete optimal trajectory [8]. Literature [9] proposed a torque feedforward damping strategy based on the chaotic regression tree dynamics model to improve the motion accuracy of the robot in the process of low-speed motion and suppress the vibration of the robot. In order to achieve fast position control, dynamic internal torque compensation based on dynamic model and robust continuous path following control with vibration suppression control were proposed in literature [10]. Literature [11] adopted a fully compensated feedforward torque control method based on the dynamic model, and compared with other feedforward methods, the robot has the fastest response speed, the lowest joint trajectory tracking error and jitter. However, the above methods have high requirements for the model and parameter accuracy of the system, otherwise the interference caused by the uncertain disturbance inside the robot will affect the algorithm control effect.

If an accurate model is not available but the states associated with elastic vibrations can be measured or observed, singular perturbations [12,13] and nonlinear feedback control [14,15] can be implemented to accommodate unmodeled dynamics and perturbations. Iterative learning control is another option to solve these problems, and optimal feedforward control can be implemented through learning when robots perform the same tasks repeatedly [16,17]. Compared with the above methods, the input shaping method can effectively minimize overshoot and residual vibration, and does not require complex dynamic models, no direct measurement of elastic vibrations for online feedback, and no need for repetitive tasks. However, one disadvantage of the use of input shaping in industrial robots is the time delay, which can slow down the overall task for applications with fast stationary to stationary motion such as spot welding, handling, etc. Literature [18,19] proposed an improved zero-delay input shaping so that no delay was introduced. However, this kind of shaping algorithm depends on the vibration frequency parameter of the robot body, and the frequency change in different postures of the robot will affect the shaping effect. In addition, input shaping may change the original motion trajectory, and for applications with high trajectory accuracy requirements such as arc welding and gluing, changing the motion trajectory will reduce the quality of the work.

In order to solve the above problems, this paper proposes a terminal residual vibration suppression method based on joint trajectory optimization. Firstly, the physical feasibility dynamic parameters and nonlinear joint friction parameters are identified, then the optimal trajectory is obtained with the minimum joint tracking error as the optimization goal, and finally the effectiveness of the proposed algorithm is proved by experiments.

2. Identification of Dynamic Parameters Based on Physical Feasibility Constraints

Due to the complexity of the robot components and assembly errors, it is difficult to know the robot dynamics accurately. However, it was found that the dynamic characteristics of the robot are a linear function of its inertial parameters [20], so the problem of obtaining the inertial parameters of the robot can be transformed into a system of linear equations. First, the robot dynamics equation is rewritten into a linear parametric form, as shown in (1)

$$\tau =Y\left(q,\dot{q},\ddot{q}\right)\beta$$

(1)

where $\beta \in {ℝ}^{N_b}$ is the reorganization parameter vector which is called minimum inertia parameter, $Y\left(q,\dot{q},\ddot{q}\right)\in {ℝ}^{n\times N_b}$ is the full-rank observation matrix obtained after reorganization. Afterwards, controlling robot by joint excitation trajectory and collecting the torque value of each joint in the process to obtain a group of test data, formula (2) is obtained according to (1)

$$\mathsf{\Gamma }=W\beta +\rho$$

(2)

where $W={\left[Y{(q(t_1),\dot{q}(t_1),\ddot{q}(t_1))}_{n\times N_b},\dots ,Y{(q(t_M),\dot{q}(t_M),\ddot{q}(t_M))}_{n\times N_b}\right]}^T$ is the observation matrix, $\rho$ is the observation error due to various reasons, $\mathsf{\Gamma }={[\begin{array}{cccc}\tau {(t_1)}^T& \tau {(t_2)}^T& \dots & \tau {(t_M)}^T\end{array}]}^T$ is the joint torque value corresponding to the test data. Finally, according to the least squares method, the minimum inertia parameter estimation vector $\widehat{\beta }$ can be obtained.

Constructing an accurate friction model is an important part of improving the accuracy of the dynamic model, and it is difficult for a single friction model to fully fit the diverse joint features. For coupling joints of an industrial robot, the friction between two joints interacting with each other produce characteristics that differ from the standard friction model. Therefore, the friction model $f_j$ of the coupling joint j is designed as equation (3)

$$f_j=\left\{\begin{array}{l}F{c}_{j-1}\cdot \mathrm{sign}({\dot{q}}_j)+F{v}_{j-1}\cdot {\dot{q}}_j,{\dot{q}}_s\le \left|{\dot{q}}_j\right|<{\dot{q}}_l\\ F{c}_{j-2}\cdot \mathrm{sign}({\dot{q}}_j)+F{v}_{j-2}\cdot {\dot{q}}_j,\left|{\dot{q}}_j\right|\ge {\dot{q}}_l\end{array}\right.$$

(3)

where ${\dot{q}}_s$ and ${\dot{q}}_l$ is pre-identify parameters for the friction model. $F{c}_{j-m}$ and $F{v}_{j-n}$ are the parameters to be identified with minimum inertial parameters synchronously.

The minimum inertial parameters calculated by the least squares method can only identify linear combinations of parameters, and cannot assert the physical feasibility directly. Therefore, it is necessary to add feasibility constraints to the calculation of dynamic parameter identification to obtain the standard parameter form that satisfies the definition of physics. For the connecting rod k, the parameter ${\delta }_k$ to be identified is defined as

$${\delta }_k=\left[L_{k,xx},L_{k,xy},L_{k,xz},L_{k,yy},L_{k,yz},L_{k,zz},l_{k,x},l_{k,y},l_{k,z},m_k\right]$$

(4)

For a 6-joint industrial robot, the mass of connecting rod k must be positive, and the inertial tensor is symmetrical and positive definite. The set of available physical feasibility constraints for the robot is expressed as

$$\mathsf{\Omega }=\left\{\delta \in {ℝ}^n:m_k>0,L_k-\frac{1}{m_k}S{\left(l_k\right)}^{\mathrm{T}}S\left(l_k\right)\succ 0 | k=1,\dots ,6\right\}$$

(5)

where $L_k$ is the inertial tensor of the connecting rod k with respect to the coordinate system of the connecting rod, $l_k$ is the first moment of the vector of inertia, $S\left(\cdot \right)$ is a partial symmetric matrix operator. Combined with the parameters of the friction model $f_{p,i}$ of each joint, the linear matrix inequality of the physical feasibility dynamic parameters is obtained as

$$D\left(\delta \right)=diag\left(D\left({\delta }_1\right),\dots ,D\left({\delta }_6\right),f_{p,1},\dots ,f_{p,6}\right)\succ 0$$

(6)

Since there is a one-to-one mapping relationship $G$ between the minimum inertia parameters $\beta$, the independent terms ${\delta }_d\in \delta$ in the parameters to be identified, and the linkage dynamics parameters [21], the physical feasibility constraints can be transformed into the constraints of the minimum inertia parameters and independent parameters, as shown in (7)

$$D_{\beta }\left(\beta ,{\delta }_d\right)=D\left(\delta \right)=D\left(G^{-1}\left(\beta ,{\delta }_d\right)\right)$$

(7)

$u$ is defined as the distance of the actual and identified values of the minimum inertial parameters as shown in (8)

$$u={‖\mathsf{\Gamma }-W\beta ‖}^2={\left(\mathsf{\Gamma }-W\beta \right)}^T\left(\mathsf{\Gamma }-W\beta \right)$$

(8)

Convert (8) into a linear matrix inequality form as (9)

$${\mathrm{U}}_{\omega }\left(u,\beta \right)=\left[\begin{array}{cc}u& {\left(\mathsf{\Gamma }-W\beta \right)}^T\\ \mathsf{\Gamma }-W\beta & 1\end{array}\right]\ge 0$$

(9)

In summary, the problem of solving the dynamic parameters based on the constraints of physical feasibility can be expressed as the following form of SDP:

$$\underset{(u,\beta ,{\delta }_d)}{\min }u$$

(10)

$$\begin{array}{cc}s.t.& F\left(u,\beta ,{\delta }_d\right)\ge 0\end{array}$$

(11)

where, the linear matrix inequality constraints are defined as (12)

$$F\left(u,\beta ,{\delta }_d\right)=\left[\begin{array}{cc}{U}_{\omega }\left(u,\beta \right)& 0\\ 0& D_{\beta }\left(u,\beta \right)\end{array}\right]$$

(12)

For the optimization problem described in (10), (11) and (12), the optimal solution $u^{*}$ is the sum of the identification errors under constraints, and the standard parameters ${\delta }^{*}$ of robot dynamics satisfying the physical feasibility constraints can be calculated from the mapping of the ${\delta }_d{}^{*}$ and ${\beta }^{*}$.

3. Residual Vibration Suppression Based on the Optimal Trajectory Principle

Unlike the conventional position motion of the robot, the residual vibration is characterized by a small oscillation with the position at the end of the trajectory as the equilibrium point. Bringing the joint vibration response into the forward kinematics model of the robot, it can be seen that the wrist position of the robot during residual vibration is

$$\left\{\begin{array}{l}{p}_{xe}={\displaystyle \sum _{p=1}^N{E}_p\cos \left({\omega }_{p}t-{\varphi }_p\right)}\\ p_{ye}={\displaystyle \sum _{p=1}^N{F}_p\cos \left({\omega }_{p}t-{\varphi }_p\right)}\\ p_{ze}={\displaystyle \sum _{p=1}^N{G}_p\cos \left({\omega }_{p}t-{\varphi }_p\right)}\end{array}\right.$$

(13)

where $E_p$, $F_p$ and $G_p$ is the amplitude of item p。The position error of the robot $P_e(t)$ in cartesian space is a function of the joint position error and velocity error at the end time. Therefore, the problem of reducing residual vibration can be transformed into a problem of reducing the position and velocity error at the end time.

On the basis of the initial value of the trajectory, the optimal trajectory problem can be obtained by setting up objective functions and constraints to meet the requirements [22]. The optimal trajectory algorithm designed in this paper includes three steps: initial trajectory discretization, establishment and solution of objective function and constraints, and barycentric interpolation.

3.1. Initial Trajectory Discretization

According to the robot kinematic model, due to the nonlinear mapping of the terminal pose and the joint angle, the acceleration discontinuity or abrupt change will still occur in the joint space, so the optimization calculation will be carried out in the joint space. For cartesian space trajectory $P(t)$, $0\le t\le t_f$, the angle of each joint in each control cycle is $q_{p,i}\left(t\right)$, on which Chebyshev collocation is performed as shown in (14)

$$q_{p,i}\left(T_k\right)=q_{p,i}\left(t_f\left[\cos \left(k\pi /N\right)+1\right]/2\right),k=0,\dots ,N$$

(14)

where $q_{p,i}\left(T_0\right)$ and $q_{p,i}\left(T_N\right)$ are the joint values corresponding to the initial and ending positions of the trajectory respectively.

One of the advantages of using the polynomial coordination method is that the differential matrix D can be used to derive the variables on the nodes quickly and accurately, so the differential of the position and torque of each joint are shown in (15)

$$\left\{\begin{array}{l}\mathrm{d}{q}_{p,i}\left(T_k\right)/\mathrm{d}T=2\mathrm{D}\cdot q_{p,i}\left(T_k\right)/t_f\\ \mathrm{d}{\tau }_{p,i}\left(T_k\right)/\mathrm{d}T=2\mathrm{D}\cdot {\tau }_{p,i}\left(T_k\right)/t_f\end{array}\right.$$

(15)

3.2. Objective Functions and Constraints

Define the position and velocity of each joint as the optimal problem state variables, and the joint torque as the control variable, as shown in (16)

$$\left\{\begin{array}{l}x\left(T_k\right)={\left[q\left(T_k\right),\dot{q}\left(T_k\right)\right]}^{\mathrm{T}}\\ u\left(T_k\right)=\tau \left(T_k\right)\end{array}\right.$$

(16)

Under the premise that the tracking performance of the control system is guaranteed, the problem of minimizing the position and speed error of the whole process and the start and stop points can be further simplified to the minimum acceleration change rate during motion, so the discretized optimization objective function is obtained as follows

$$J={\displaystyle \sum _{k=1}^N\dot{u}{({\mathrm{T}}_k)}^T{W}_R\dot{u}(T_k)}$$

(17)

where $W_R$ is the weighted diagonal matrix. Theoretically, the greater the inertia, the greater the joint weight is.

The dynamic parameters identified by (10)-(12) can establish more accurate dynamic constraints for each joint, as shown in (18)

$$u\left(T_k\right)=\mathrm{DYN}\left(x\left(T_k\right),{\delta }^{*}\right)$$

(18)

In addition, the position and velocity of the start and end moments of each joint are used as equation constraints, and the position limit, velocity limit, and acceleration limit of each joint are used as inequality constraints at nodes to ensure that the robot starts and stops running. After the above discretization and design of the optimization objective function, the problem of the minimum error of position and velocity at the end point has been transformed into a typical nonlinear programming (NLP) problem with constraints. The algorithm based on the gradient descent principle can be used to solve the optimal control quantity $u^{*}\left(T_k\right)$ and state quantity $x^{*}\left(T_k\right)$ at the node.

3.3. Barycentric Interpolation

Since the calculated optimal state quantity is the value at each node after discretization, it is necessary to select an interpolation algorithm to convert it into the joint position value of each control cycle. The node-based polynomial fitting is achieved by barycentric interpolation [23], which can be expressed as a linear combination of Lagrange polynomials, and the optimized angles of each joint are obtained as shown in (19)

$$\widehat{q}\left(t\right)={\displaystyle \sum _{j=0}^N{q}^{*}\left(T_j\right)}{l}_j\left(t\right)$$

(19)

where $l_j\left(t\right)$ is the j-th Lagrange polynomial, defined as (20)

$$l_j\left(t\right)=\prod _{k=0}^N\left(t-T_k\right)\frac{v_j}{t-T_j}$$

(20)

where $v_j$ is the imputation weight corresponding to node j. For node-based polynomial interpolation, the error of the optimized trajectory and the original trajectory at the node is 0, and the error between the nodes can be obtained by (21)

$$\epsilon \left(t\right)=\mathrm{P}(t)-\mathrm{KINE}\left(\widehat{q}(t)\right)$$

(21)

Based on the above calculations, the motion trajectory with the same number of interpolation points as the pre-optimization trajectory is obtained again, which not only ensures that the optimized start-stop position is the same as the original trajectory, but also ensures the consistency between the optimized running trajectory and the original trajectory. Control the joints of the robot to complete the movement according to the optimized trajectory, so as to reduce the amplitude and duration of residual vibration.

4. Experimental Verification and Analysis of Results

The industrial robot T12B is selected for experimental analysis, and the inertial sensor is selected for vibration testing. The parameters are shown in Table 1. The experiment is divided into two steps, the dynamic parameters are identified firstly, and then the residual vibration suppression experiment is carried out based on the identification results.

4.1. Identification of Dynamic Parameters

The excitation trajectory based on the Fourier series function [24] is selected to control the robot motion, the joint position and torque information are collected, and the dynamic parameters are calculated according to the identification algorithm. Then, the verification trajectory different from the excitation trajectory is selected, the actual torque of each joint during the movement is recorded, and the theoretical torque value is calculated according to the identification parameters and the difference between the two torque is compared to verify the correctness and accuracy of the identification results.

The minimum inertial parameters without considering the constraints of physical feasibility are used, and the dynamic parameter identification results are calculated by the least squares method. The actual torque of each joint and the theoretical torque based on the identified parameters are plotted as shown in Figure 1. It can be seen from the figure that the theoretical torque and the actual torque have a large spike protrusion when the velocity direction changes, and there is always a certain range of error in the constant velocity section.

According to the coupling characteristics of the T12B robot and the unified identification method of joint friction, the friction model such as equation (3) is selected, and the friction model is included in the identification parameter set. The same excitation trajectory is selected, and the test data is substituted into equations (10)-(12) to calculate the kinetic parameters that meet the constraints of physical feasibility. The recalculated kinetic parameters are shown in Table 2, and the identified friction parameters are shown in Table 3.

The parameters of Table 2 are brought into the dynamic formula, and the friction of the joint is calculated according to the friction parameters in Table 3. The actual output torque of each joint and the theoretical torque based on the identified parameters are plotted as shown in Figure 2. Compared with Figure 1, it can be seen that the peak protrusion of the theoretical and actual torque is significantly reduced when the velocity direction changes, and the accuracy in the uniform velocity section is significantly improved, especially the torque errors of joints 3, 4 and 6 are improved significantly.

The root mean square error (RMS) of the theoretical torque and the actual torque of the identification model are shown in Table 4. According to the RMS results, the theoretical value of the torque in the whole process is closer to the actual torque value identified by the constraint-based inertia parameters. Compared with the identification results of the unconstrained and frictionless model, the results of the 4, 5 and 6 joints proposed in this paper are improved by about 80% compared with the original algorithm, which shows that the algorithm has a significant effect on coupling joints and friction-sensitive joints.

4.2. Verification of the Performance of Vibration Suppression

As shown in Figure 3, the robot moves in a straight line from position P1 to position P2 with a linear speed of 200mm/s. P1 and P2 are (590,0,294) and (1317,0,294) respectively under the base coordinate, and the unit is mm. Inertial sensor is installed at the flange to measure vibration during motion.

The experimental trajectory is the linear motion in cartesian space with no displacement in the Y-axis and Z-axis directions. The velocity in the X-axis direction is an S-shaped curve, which ensures a smooth velocity in cartesian space. From the kinematic model, it can be seen that the X-axis direction movement is mainly generated by the linkage of joints 2, 3 and 5, as shown in Figure 4.

However, due to the nonlinearity of the kinematic inverse solution, the above-mentioned joints produce a large velocity step at the start-stop point. In particular, in the deceleration stage of joint 3 at the stop point, the joint acceleration produces a peak value of 0.012, which is about 4 times of the peak acceleration value of the initial stage. The sudden change in acceleration of the deceleration section leads to a large tracking error, which causes residual vibration at the end of the robot.

The acceleration curve in the Z direction of the terminal is obtained by the inertial sensor, as shown in Figure 5. As can be seen from the figure, the vibration with an amplitude of less than 1 in the Z direction at the beginning of the motion and quickly converges to less than 0.5 with the motion. In the stop deceleration phase, the end amplitude is about 3 times that of the starting segment due to the abrupt change in acceleration, and a significant damping oscillation is generated.

According to the optimization model established in this paper, the kinetic parameters identified in Table 1 are brought into the dynamic constraints, the number of collocation nodes is selected as N = 12, the threshold of the number of iterations is 10, and the solution time is 0.2 seconds, and the motion curve of the optimized joint is obtained as shown in Figure 6.

After optimization, the start and stop points of the position curve of each joint are the same as those of the original curve, and the running time is the same. From equation (21), it can be seen that there is a maximum error of 0.5mm between the trajectory and the original trajectory during the process, which ensures the consistency of the running trajectory. The acceleration amplitude of joint 2 and joint 3 in the stop phase is about 0.5 times that of the original trajectory, and the jerk amplitude is basically the same as that of the original trajectory.

After optimization, the vibration at the end of the robot during operation is re-measured, and there is no visible vibration at the end of the entire process. The inertial sensor data in the stop stage of the robot is obtained and another curve is drawn by using the joint input shaping method as shown in Figure 7.

As can be seen from the figure, the maximum amplitude of the original vibration acceleration curve is 2.71, and the amplitude converges to less than 0.3 after 1.58s. After optimization, the maximum amplitude of the curve is 1.04, the amplitude converges to less than 0.3 after 0.46 seconds, the vibration stops completely at 0.7 seconds, the maximum amplitude decreases by 61.62%, and the vibration duration decreases by 70.89%. The input shaping vibration curve has a significant delay compared with the original curve with the maximum amplitude of 1.15 at 0.51 s. The maximum amplitude converges to within 0.3 after 0.69s, but there is still a small vibration after that. The amplitude converges to within 0.1 at 1.64 seconds. From the above analysis, it can be seen that compared with the input shaping method, the proposed method can reduce the residual vibration more effectively.

5. Conclusions

In this paper, a terminal vibration suppression method based on joint trajectory optimization is proposed. This method is based on the identification results of dynamic parameters with physical feasibility constraints, and takes the joint position and velocity tracking difference as the optimization goals. The trajectory obtained by optimization reduces the terminal vibration acceleration amplitude and convergence time, and ensures the consistency between the running trajectory and the original trajectory. The experimental results show that compared with the unconstrained least squares method, the torque error is significantly reduced at the joint velocity reversal, and the RMS of the coupling joint torque is reduced by 82.67% at most. Based on the identification results as dynamic constraint parameters, the maximum acceleration amplitude can be reduced by 61.62% and the vibration duration by 70.89%.