1 Introduction

High-voltage power cable is the important channel of power transmission, the regular high-voltage line live maintenance operation is an important guarantee to the normal operation of transmission line, manual operation is not only labor-intensive, low operation efficiency but also a great security risk, and the contradiction between the manual operation mode and modern high-quality power transmission has become increasingly prominent, therefore, the development of advanced and practical automation equipment which can replace manual maintenance has become one of the research hotspots in the implementation of “Mading in China 2025”. As a kind of special operation robot used in high-voltage transmission line, power cable mobile robot (Pouliot et al. 2015, 2012; Ramirez 2014; Buehringer et al. 2010) has a wide range of applications in live operation and maintenance of power line in the state grid sectors. The various operations (Montambault et al. 2010; Wang 2015; Debenest et al. 2010) including the replacement of insulator strings are routine tasks of the power departments. During the robot operation motion, the robot double manipulators move from the initial posture to the operation posture, and then to the ideal posture so as to locating the operation objects through point to point motion planning. However abrupt position, velocity, acceleration state changes when joint motion can cause shocks, oscillations and result in unstable motion of the robot manipulator, which will decrease the motion accuracy of the manipulator. Therefore, the proper trajectory motion planning and its optimization method is significantly important, the motion planning can be achieved by selecting the appropriate method so that the trajectory of the joint can satisfy the state constraints of joint position, velocity and acceleration under the premise of obtaining continuous, smooth and steady trajectory, and it not only prevents the joint state from exceeding its intrinsic range, but also keep the robot mechanical system, hardware system far away from damage and unnecessary operation accidents. Through optimization control of the joint motion state during the robot motion, it can satisfy the requirements of the joint state constraints, which can not only ensure the safety and reliability of the robot during motion, but also improve the robot dynamic performance and extend the system service cycle of robot manipulator. In the aspect of joint trajectory optimization, the main optimization objectives contain the shortest time and shortest path (Aridhi et al. 2003), the minimum energy consumption and minimum joint torque (Mokryani et al. 2013; Sattarpour et al. 2016), the joint state constraints (Kayacan et al. 2015). Such as Bin et al. (2008, 2011) proposed a joint motion optimization method, through the optimization of joint trajectory motion time and it obtains a joint trajectory which can satisfy both the velocity constraint and the joint force constraint, Haili et al. (2010) proposed a optimization method which making the robot joint trajectory meet the given state constraints, SARAVANAN (Saravanan et al. 2008; Haddad et al. 2010) realized the joint trajectory by NURBS curve, and make the joint trajectory satisfy a variety of constraint conditions through the optimization of the trajectory motion time. Liu (2015) aimed at carrying a heavy payload to a desired posture, a multi-objective optimization based method for maximum payload trajectory planning of free floating space manipulator is proposed, Marchese et al. (2016) develop a soft-robotic manipulation system which is capable of autonomous, dynamic, and safe interactions with humans and its environment using multi-target planning methods. Chiddarwar et al. (2012) proposed a triangular spline curve based joint trajectory planning method, and the joint trajectory satisfies multiple constraints by optimizing the trajectory motion time. Therefore, we can get the conclusion from the above analysis that polynomial interpolation algorithm is one of the most commonly used method for joint trajectory planning. However there are two disadvantages with the conventional polynomial interpolation algorithm. First, the joint trajectory is related to the trajectory endpoint, once the trajectory endpoint changes, the trajectory function needs to be recalculated, which reduce the algorithm practicability to a certain extent. Second, the trajectory planning is too idealistic which only take the joint state constraints of the trajectory endpoints into consideration, however, it ignores the joint state constraints in the intermediate process, that is to say, the robot driving mechanism joint motor is constrained at any moment rather than just the trajectory endpoints. Under normal circumstances, when the joint motor rotates at low speed, the joint position should only ensure it not exceed the limit value, at this time joint velocity and acceleration are difficult to exceed the constraint range. However, as the joint motor rotates at high speed, the joint velocity and acceleration can easily exceed its limit value, with the result is that the joint motor can cause excessive motor drive current which will damage the robot hardware system. The more important is that the above results are mainly studied from the standpoint of single or double or multi-objective optimization. However, the kinematic parameters, the kinetic parameters, the trajectory planning algorithms and their multiple mapping mechanisms with different motion performances can hardly to be seem. Therefore, this paper take this perspective as an entry point, the robot joint motion planning optimization can be achieved through the kinematics, kinetics modeling and trajectory planning algorithm so as to improve the robot motion performance.

Based on the above analysis, this paper proposed an improved polynomial interpolation joint trajectory planning algorithm, the normalized joint motion time is taken as interpolation polynomial variable. In this way, the joint motion trajectory is only related to the state of trajectory endpoint and the joint motion time, and it is not directly related to the moment of joint trajectory. When the moment of endpoint change, the new joint trajectory function can be also obtained only by transforming and mapping of joint trajectory normalized time into physical time, in this way, it reduces the computational complexity and improves the practicability of the algorithm. In order to make the joint trajectory planning move more closer to the actual motion and solve the global state constraint problem of joint trajectory, the performance parameters of motion trajectory are proposed, and the optimization joint trajectory which satisfy the global constraints, it can be also obtained by optimizing the joint motion time, this method avoids the occurrence of joint motion over shoot and improves the joint motion efficiency and safety of robot operation, and also the research of this paper is becoming one of the hot spots of “Mading in China 2025” strategy and in accordance with the theme of intelligent manufacturing and smart manufacturing.

2 System architecture of motion optimization

Robot path planning is the basis of joint trajectory planning, and trajectory planning is the premise of trajectory optimization. When the robot moves in accordance with the setting trajectory and corresponding obtains a battery of time series regarding robot joint variables, this is called the robot trajectory planning, and it can be divided into point to point trajectory planning and continuous path trajectory planning (Tao et al. 2010, 2014, 2017a) according to the way of motion, as shown in Fig. 1, the basic hierarchical structure and schematic diagram of manipulator motion trajectory optimization is presented. The goal of robot trajectory optimization is to achieve better performance for robot motion. Robot trajectory planning can be divided into three levels from the bottom to the top. The bottom level is to solve the most basic path planning issues, when considering the problems of robot kinematics and obstacle constraints on the basis of path planning, it can be raised to the level of trajectory planning. At the trajectory planning level, when considering joint space kinetics constraints and Cartesian space posture constraints, it can further rise to the level of joint optimization. After considering the joint variables such as joint position, velocity, acceleration and other energy consumption or time constraints at the level of joint optimization. At this time, the output joint variable can make the robot achieve the state optimal value under the premise of satisfying the multi-objective constraint performance. It is a higher level of trajectory planning and robot system design. The issue to be solved by the trajectory optimization (Tao et al. 2008, 2012, 2017b) is the multi-objective optimization process of the robot motion control, including kinematics, non-collision avoidance, joint space kinetics constraints, Cartesian space posture constraints, time, path, energy consumption and so on. The goal of optimization is to make motion time, motion path, energy consumption as small as possible on the premise of no-collision avoidance and joint state constraints, and the intelligent optimization algorithm such as ACO (Ni et al. 2014), PSO (Linda et al. 2012) are always used to solve this problem. Finally, the system output is the joint space optimal position, velocity, acceleration state variable value and the outputs can be used as robot motion control command. The key problem to be solved by trajectory optimization is the multi-mapping and optimal matching between robot kinematic parameters, kinetics parameters, algorithm parameters and different motion performances. In order to accomplish this goal, in Sect. 3, an improved fifth interpolation algorithm has been proposed and in Sect. 4, its corresponding multi-objective motion optimization model has been established, then in Sect. 5, its mix controller has been designed, and the validity and engineering practicability are verified by simulation experiment and live operation test in Sect. 6.

Fig. 1
figure 1

Architecture of robot motion optimization

Full size image

3 Method and improvement

3.1 Algorithm improvement idea

The method of manipulator joint motion planning based on n-degree polynomial interpolation is to determine the unique polynomial of the n-degree interpolation trajectory through the joint boundary state (value) at the two trajectory endpoints. Supposing the trajectory starting point state is \({x_0},{\dot {x}_0},{\ddot {x}_0} \ldots\) and the trajectory end point state is \({x_f},{\dot {x}_f},{\ddot {x}_f} \ldots\) respectively. The conventional polynomial is a function regarding physical time t, due to the different boundary state correspond to different endpoint moments, polynomial coefficients generally need to be recalculated. In order to reduce the influence of the selection of the endpoint time on the interpolation polynomial and so as the calculation amount of the polynomial coefficient can be reduced as much as possible. The polynomial time variable t can be normalized and the physical time t can be transformed into a dimensionless variable normalization time \({t_\alpha }\), and its corresponding definition is shown in Eq. (1), where k is the time contraction factor. Therefore, the joint trajectory function can be expressed as Eq. (2), the joint polynomial function is \({x_{(n)}}({t_\alpha })\), regarding normalized time \({t_\alpha }\), it can be obtained the polynomial trajectory through the end point joint status values.

$${t_\alpha }=k\frac{{t - {t_0}}}{{{t_f} - {t_0}}}(0 \leq {t_\alpha } \leq 1)$$
(1)
$${x_{(n)}}({t_\alpha })={x_0}+({x_f} - {x_0}) \cdot {\Phi _{(n)}}({t_\alpha }),\quad \quad 0 \leq {\Phi _{(n)}}({t_\alpha }) \leq 1$$
(2)

In Eq. (2), \({\Phi _{(n)}}({t_\alpha })\) represents a random function regarding \({t_\alpha }\), with its range is between 0 and 1, it is used to real-time correction of the joint trajectory function. When \({\Phi _{(n)}}({t_\alpha })=0\), it can represent the trajectory start point, when \({\Phi _{(n)}}({t_\alpha })=1\), it can represent the trajectory end point. When \(0<{\Phi _{(n)}}({t_\alpha })<1\), it can represent an intermediate point of the joint trajectory during the motion.

3.2 Algorithm improvement process

In Eq. (1), taking k = 1, regarding Eq. (2), calculating its first order, second order, third order until \(\tfrac{1}{2}(n - 1)\) order derivative respectively, thereby we can obtain the joint trajectory velocity, acceleration, jerk state function and so on, which is shown in Eq. (3).

$$\left\{ \begin{gathered} {{\dot {x}}_{(n)}}({t_\alpha })=\frac{{{x_f} - {x_0}}}{{{t_f} - {t_0}}}{{\dot {\Phi }}_{(n)}}({t_\alpha }) \\ {{\ddot {x}}_{(n)}}({t_\alpha })=\frac{{{x_f} - {x_0}}}{{{{({t_f} - {t_0})}^2}}}{{\ddot {\Phi }}_{(n)}}({t_\alpha }) \\ {{\dddot x}_{(n)}}({t_\alpha })=\frac{{{x_f} - {x_0}}}{{{{({t_f} - {t_0})}^3}}}{{\dddot \Phi }_{(n)}}({t_\alpha }) \\ \ldots \\ x_{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}({t_\alpha })=\frac{{{x_f} - {x_0}}}{{{{({t_f} - {t_0})}^{(\tfrac{{n - 1}}{2})}}}}\Phi _{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}({t_\alpha }) \\ \end{gathered} \right.$$
(3)

If the joint motion time is defined as \({t_m}={t_f} - {t_0}\), the position deviation of the trajectory endpoint is defined as \(P={x_f} - {x_0}\), substituting boundary conditions of joint variables into the Eqs. (2) and (3), then Eq. (4) can be obtained.

$$\left\{ \begin{gathered} {x_{^{{(n)}}}}({t_0})={x_0}+P{\Phi _{(n)}}({\text{0}})={x_0}~~~~~~~~~~ \hfill \\ {x_{^{{(n)}}}}({t_f})={x_0}+P{\Phi _{(n)}}(1)={x_f} \hfill \\ {{\dot {x}}_{^{{(n)}}}}({t_0})=\frac{P}{{{t_m}}}{\mathop \Phi \limits^{.} _{(n)}}(0)={{\dot {x}}_0} \hfill \\ {{\dot {x}}_{^{{(n)}}}}({t_f})=\frac{P}{{{t_m}}}{\mathop \Phi \limits^{.} _{(n)}}(1)={{\dot {x}}_f} \hfill \\ {{\ddot {x}}_{^{{(n)}}}}({t_0})=\frac{P}{{t_{m}^{2}}}{\mathop \Phi \limits^{{..}} _{(n)}}(0)={{\ddot {x}}_0} \hfill \\ {{\ddot {x}}_{^{{(n)}}}}({t_f})=\frac{P}{{t_{m}^{2}}}{\mathop \Phi \limits^{{..}} _{(n)}}(1)={{\ddot {x}}_f} \hfill \\ \cdots \hfill \\ x_{{_{{^{{(n)}}}}}}^{{^{{\left(\tfrac{{n - 1}}{2}\right)}}}}({t_0})=\frac{P}{{{{({t_m})}^{\left(\tfrac{{n - 1}}{2}\right)}}}}\Phi _{{_{{(n)}}}}^{{^{{\left(\tfrac{{n - 1}}{2}\right)}}}}(0)=x_{0}^{{\left(\tfrac{{n - 1}}{2}\right)}} \hfill \\ x_{{_{{^{{(n)}}}}}}^{{^{{\left(\tfrac{{n - 1}}{2}\right)}}}}({t_f})=\frac{P}{{{{({t_m})}^{\left(\tfrac{{n - 1}}{2}\right)}}}}\Phi _{{_{{(n)}}}}^{{^{{\left(\tfrac{{n - 1}}{2}\right)}}}}(1)=x_{f}^{{\left(\tfrac{{n - 1}}{2}\right)}} \hfill \\ \end{gathered} \right.$$
(4)

Therefore, the boundary condition of function \({\Phi _{(n)}}({t_\alpha })\) can be obtained through the transformation of Eq. (4), and it is given as Eq. (5).

$$\left\{ \begin{gathered} {\Phi _{(n)}}({\text{0}})=0~~~~~~~~~~ \hfill \\ {\Phi _{(n)}}(1)=1 \hfill \\ {{\dot {\Phi }}_{(n)}}(0)=\frac{{{{\dot {x}}_0}{t_m}}}{P} \hfill \\ {{\dot {\Phi }}_{(n)}}(1)=\frac{{{{\dot {x}}_f}{t_m}}}{P} \hfill \\ {{\ddot {\Phi }}_{(n)}}(0)=\frac{{{{\ddot {x}}_0}t_{m}^{2}}}{P} \hfill \\ {{\ddot {\Phi }}_{(n)}}(1)=\frac{{{{\ddot {x}}_f}t_{m}^{2}}}{P} \hfill \\ \cdots \hfill \\ \Phi _{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}(0)=\frac{{x_{0}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}{{({t_m})}^{\left( {\tfrac{{n - 1}}{2}} \right)}}}}{P} \hfill \\ \Phi _{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}(1)=\frac{{x_{f}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}{{({t_m})}^{\left( {\tfrac{{n - 1}}{2}} \right)}}}}{P} \hfill \\ \end{gathered} \right.$$
(5)

From the boundary condition of Eq. (5), an unique n-order polynomial \({\Phi _{(n)}}({t_\alpha })\) can be determined as Eq. (6).

$${\Phi _{(n)}}({t_\alpha })={A_n}{({t_\alpha })^n}+{A_{n - 1}}{({t_\alpha })^{n - 1}}+ \cdot \cdot \cdot +{A_2}{({t_\alpha })^2}+{A_1}{t_\alpha }+{A_0}$$
(6)

Calculating its first order, second order until \(\tfrac{1}{2}(n - 1)\) order derivative regarding Eq. (6) respectively, we can obtain Eq. (7).

$$\left\{ {\begin{array}{*{20}{c}} {{{\dot {\Phi }}_{(n)}}({t_\alpha })=n{A_n}{{({t_\alpha })}^{(n - 1)}}+(n - 1){A_{n - 1}}{{({t_\alpha })}^{(n - 2)}}+ \cdots +2{A_2}{t_\alpha }+{A_1}} \\ {{{\ddot {\Phi }}_{(n)}}({t_\alpha })=n(n - 1){A_n}{{({t_\alpha })}^{(n - 2)}}+(n - 1)(n - 2){A_{n - 1}}{{({t_\alpha })}^{(n - 3)}}+ \cdots +2{A_2}} \\ \cdots \\ {\Phi _{{(n)}}^{{\left( {\frac{{n - 1}}{2}} \right)}}({t_\alpha })=n(n - 1) \ldots \left( {n - \frac{{n - 3}}{2}} \right){A_n}{{({t_\alpha })}^{\left( {\frac{{n+1}}{2}} \right)}}+(n - 1)(n - 2) \ldots } \\ {\left( {n - \frac{{n - 1}}{2}} \right){A_{n - 1}}{{({t_\alpha })}^{\left( {\frac{{n - 1}}{2}} \right)}}+ \cdots +\left( {\frac{{n - 1}}{2}} \right)!{A_{\left( {\frac{{n - 1}}{2}} \right)}}} \end{array}} \right.$$
(7)

Substituting the boundary condition of Eq. (5) into Eqs. (6) and (7), we can obtain Eq. (8).

$$\left\{ \begin{gathered} {A_0}={\Phi _{(n)}}(0) \hfill \\ {A_n}+{A_{n - 1}}+ \cdots +{A_1}+{A_0}={\Phi _{(n)}}(1) \hfill \\ {A_1}={{\dot {\Phi }}_{(n)}}(0) \hfill \\ n{A_n}+(n - 1){A_{n - 1}}+ \cdot \cdot \cdot +2{A_2}+{A_1}={{\dot {\Phi }}_{(n)}}(1) \hfill \\ \cdots \hfill \\ \left( {\frac{{n - 1}}{2}} \right)!{A_{\left( {\tfrac{{n - 1}}{2}} \right)}}=\Phi _{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}(0) \hfill \\ n(n - 1) \cdots \left( {n - \frac{{n - 3}}{2}} \right){A_n}+(n - 1)(n - 2) \ldots \hfill \\ \left( {n - \frac{{n - 1}}{2}} \right){A_{n - 1}}+ \cdots +\left( {\frac{{n - 1}}{2}} \right)!{A_{\left( {\tfrac{{n - 1}}{2}} \right)}}=\Phi _{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}(1) \hfill \\ \end{gathered} \right.$$
(8)

In order to facilitate the calculation, Eq. (8) can be rewritten into the form of matrix and the polynomial coefficients can be calculated through Eq. (9).

$$\left[ \begin{gathered} {A_n} \\ {A_{n - 1}} \\ {A_{n - 2}} \\ \cdots \\ {A_{(\frac{{n - 1}}{2})}} \\ ... \\ {A_2} \\ {A_1} \\ {A_0} \\ \end{gathered} \right]=\left[ {\begin{array}{*{20}{c}} 0&0& \cdots & \cdots & \cdots & \cdots & \cdots &0&1 \\ 1&1& \cdots & \cdots & \cdots & \cdots & \cdots &1&1 \\ 0&0& \cdots & \cdots & \cdots & \cdots & \cdots &1&0 \\ n&{n - 1}& \cdots & \cdots & \cdots & \cdots & \cdots &2&1 \\ \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \\ \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \\ \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \\ \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \\ {n(n - 1) \cdot \cdot \cdot \left( {n - \frac{{n - 3}}{2}} \right)}&{(n - 1)(n - 2) \ldots \left( {n - \frac{{n - 1}}{2}} \right)}& \cdots & \cdots & \cdots &{\left( {\frac{{n - 1}}{2}} \right)!}& \cdots & \cdots & \cdots \end{array}} \right]_{{(n+1) \times (n+1)}}^{{ - 1}}\left[ \begin{gathered} {\Phi _{(n)}}(0) \hfill \\ {\Phi _{(n)}}(1) \hfill \\ {{\dot {\Phi }}_{(n)}}(0) \hfill \\ {{\dot {\Phi }}_{(n)}}(1) \hfill \\ \cdots \hfill \\ \Phi _{{(n)}}^{{\left( {\tfrac{{n - 3}}{2}} \right)}}(0) \hfill \\ \Phi _{{(n)}}^{{\left( {\tfrac{{n - 3}}{2}} \right)}}(1) \hfill \\ \Phi _{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}(0) \hfill \\ \Phi _{{(n)}}^{{\left( {\tfrac{{n - 1}}{2}} \right)}}(1) \hfill \\ \end{gathered} \right]$$
(9)

With the calculated coefficients of Equation \({\Phi _{(n)}}({t_\alpha })\), the joint polynomial trajectory function regarding \({t_\alpha }\) can be also obtained. For general joint motion, as long as the performance requirements of joint position, velocity and acceleration can be satisfied, then the better trajectory planning performance can be also achieved. Therefore, under normal circumstances, fifth polynomial interpolation algorithm is adequate to satisfy the requirement of joint trajectory position, velocity, acceleration constraint performances. Corresponding, the coefficients of the fifth polynomial interpolation can be obtained as shown in Eq. (10).

$$\left\{ \begin{gathered} {A_5}= - 6{\Phi _{(5)}}(0)+6{\Phi _{(5)}}(1) - 3{{\dot {\Phi }}_{(5)}}(0) - 3{{\dot {\Phi }}_{(5)}}(1) - 0.5{{\ddot {\Phi }}_{(5)}}(0)+0.5{{\ddot {\Phi }}_{(5)}}(1) \hfill \\ {A_4}=15{\Phi _{(5)}}(0) - 15{\Phi _{(5)}}(1)+8{{\dot {\Phi }}_{(5)}}(0)+7{{\dot {\Phi }}_{(5)}}(1)+1.5{{\ddot {\Phi }}_{(5)}}(0) - {{\ddot {\Phi }}_{(5)}}(1) \hfill \\ {A_3}= - 10{\Phi _{(5)}}(0)+10{\Phi _{(5)}}(1) - 6{\mathop \Phi \limits^{.} _{(5)}}(0) - 4{{\dot {\Phi }}_{(5)}}(1) - 1.5{{\ddot {\Phi }}_{(5)}}(0)+0.5{{\ddot {\Phi }}_{(5)}}(1) \hfill \\ {A_2}=0.5{{\ddot {\Phi }}_{(5)}}(0) \hfill \\ {A_1}={{\dot {\Phi }}_{(5)}}(0) \hfill \\ {A_0}={\Phi _{(5)}}(0) \hfill \\ \end{gathered} \right.$$
(10)

Due to the position, speed, acceleration performance requirements can be able to meet by the fifth polynomial trajectory, therefore, setting n = 5 in the boundary condition Eqs. (5), then substituting n = 5 into Eq. (10), then Eq. (11) can be obtained.

$$\left\{ \begin{gathered} {A_5}=6+\frac{{ - 3{{\dot {x}}_0}{t_m} - 3{{\dot {x}}_f}{t_m} - 0.5{{\ddot {x}}_0}t_{m}^{2}+0.5{{\ddot {x}}_f}t_{m}^{2}}}{P} \hfill \\ {A_4}= - 15+\frac{{8{{\dot {x}}_0}{t_m}+7{{\dot {x}}_f}{t_m}+1.5{{\ddot {x}}_0}t_{m}^{2} - {{\ddot {x}}_f}t_{m}^{2}}}{P} \hfill \\ {A_3}=10+\frac{{ - 6{{\dot {x}}_0}{t_m} - 4{{\dot {x}}_f}{t_m} - 1.5{{\ddot {x}}_0}t_{m}^{2}+0.5{{\ddot {x}}_f}t_{m}^{2}}}{P} \hfill \\ {A_2}=\frac{{0.5{{\ddot {x}}_0}t_{m}^{2}}}{P} \hfill \\ {A_1}=\frac{{{{\dot {x}}_0}{t_m}}}{P} \hfill \\ {A_0}=0 \hfill \\ \end{gathered} \right.$$
(11)

Therefore, the fifth polynomial \({\Phi _{(n)}}({t_\alpha })\) can be obtained as Eq. (12). By substituting Eq. (12) into Eq. (2), we can get the joint trajectory function \({x_{(5)}}({t_\alpha })\) as shown in Eq. (13) based on standardization time.

$$\begin{aligned} {\Phi _{(5)}}({t_\alpha }) & =\left( {6+\frac{{ - 3{{\dot {x}}_0}{t_m} - 3{{\dot {x}}_f}{t_m} - 0.5{{\ddot {x}}_0}t_{m}^{2}+0.5{{\ddot {x}}_f}t_{m}^{2}}}{P}} \right)t_{\alpha }^{5} \\ & \quad +\left( { - 15+\frac{{8{{\dot {x}}_0}{t_m}+7{{\dot {x}}_f}{t_m}+1.5{{\ddot {x}}_0}t_{m}^{2} - {{\ddot {x}}_f}t_{m}^{2}}}{P}} \right)t_{\alpha }^{4} \\ \quad +\left( {10+\frac{{ - 6{{\dot {x}}_0}{t_m} - 4{{\dot {x}}_f}{t_m} - 1.5{{\ddot {x}}_0}t_{m}^{2}+0.5{{\ddot {x}}_f}t_{m}^{2}}}{P}} \right)t_{\alpha }^{3} \\ \quad +\frac{{0.5{{\ddot {x}}_0}t_{m}^{2}}}{P}t_{\alpha }^{2}+\frac{{{{\dot {x}}_0}{t_m}}}{P}{t_\alpha } \\ \end{aligned}$$
(12)
$$\begin{aligned} {x_{(5)}}({t_\alpha }) & =\left( {6P - 3{{\dot {x}}_0}{t_m} - 3{{\dot {x}}_f}{t_m} - 0.5{{\ddot {x}}_0}t_{m}^{2}+0.5{{\ddot {x}}_f}t_{m}^{2}} \right)t_{\alpha }^{5} \\ & \quad +\left( { - 15P+8{{\dot {x}}_0}{t_m}+7{{\dot {x}}_f}{t_m}+1.5{{\ddot {x}}_0}t_{m}^{2} - {{\ddot {x}}_f}t_{m}^{2}} \right)t_{\alpha }^{4} \\ & \quad +\left( {10P+6{{\dot {x}}_0}{t_m} - 4{{\dot {x}}_f}{t_m} - 1.5{{\ddot {x}}_0}t_{m}^{2}+0.5{{\ddot {x}}_f}t_{m}^{2}} \right)t_{\alpha }^{3} \\ & \quad +0.5{{\ddot {x}}_0}t_{m}^{2}t_{\alpha }^{2}+3{{\dot {x}}_0}{t_m}{t_\alpha }+{x_0} \\ \end{aligned}$$
(13)

From Eq. (13), we can conclude that the obtained joint trajectory equation is only related to the velocity at start point and end point, time deviation (motion time) between start point and end point, there is no direct relationship with the selection of start point position and start point moment, therefore, the method can achieve the purpose of improving the algorithm.

4 Manipulator motion planning

4.1 Model establishment and optimization objectives

The robot double manipulators motion planning can be achieved through the coordination motion of the joints. Therefore, the trajectory of the double manipulators need to be mapped to the joint space so as to realize the motion planning of single manipulator, double manipulators and the whole robot from the joint space. The purpose of joint trajectory planning is to make the dual manipulator accurately capture and locate the operation objects with the relative optimal motion performance from the initial posture to ideal posture in the condition of satisfying the full state constraint of the joint trajectory and non-collision avoidance. Figure 2 is the trajectory planning of double manipulators for robot insulator strings replacement. In the initial state, in order to maintain the balance of the robot gravity center during motion, the double manipulators make an angle of about 45° with the horizontal plane (As shown in Fig. 2a). By rotation, stretch, horizontal, vertical movement of multi-joint motion planning. Double manipulators move from the initial posture to the ideal posture to achieve insulator steel cap and bowl head hanging plate clamp (As shown in Fig. 2b). The corresponding rotation joint motion trajectory is a parabola, while the corresponding moving joint motion trajectory is a straight line. Thus, the entire trajectory is a combination of parabola and straight line. The double manipulators enter into the initial operation position after location the suspension clamp. With the double manipulators coordinated movement so as to reach the operation space, which is an important prerequisite for the robot to complete the whole operation. After completing the operation, double manipulators need to return to the initial posture by the reverse manner.

Fig. 2
figure 2

Trajectory planning of double manipulators for robot insulator strings replacement

Full size image

As shown in Fig. 2a, the centroid of robot double manipulator clamping mechanism can be abstracted as the ideal particle. During the point to point \({\text{A}} \to {\text{B,C}} \to {\text{D}}\) (A and C are the initial centroid positions of double manipulator, B and D are the ideal centroid positions of double manipulators) trajectory motion of robot manipulator centroids, setting the initial time as t0, setting the termination time as tf, the definition of joint motion state is \(\xi (t)=\{ x(t),\dot {x}(t),\ddot {x}(t)\}\) (t0→tf). Motion status collection contain any point on the trajectory from the start point to the end point of the joint movement, the position, velocity and acceleration state information at any moment. It is a state sequence regarding time t. According to the robot inherent mechanical parameters and joint electrical parameters, the maximum and minimum trajectory state can be obtained as \(\xi {(t)_{\hbox{max} }}=\{ x{(t)_{\hbox{max} }},\dot {x}{(t)_{\hbox{max} }},\ddot {x}{(t)_{\hbox{max} }}\}\) and \(\xi {(t)_{\hbox{min} }}=\{ x{(t)_{\hbox{min} }},\dot {x}{(t)_{\hbox{min} }},\ddot {x}{(t)_{\hbox{min} }}\}\). During the robot motion, there should be non-collision between the double operation manipulators or between the manipulators and the operation environment. If \({{\mathbb{C}}_1},{{\mathbb{C}}_2}\) are joint constraint space of manipulator 1 and manipulator 2 respectively, \(({\theta _1},{d_1},{d_2},{\theta _2},{d_3},{d_4},{d_5})\) is the joint variable. Then \({{\mathbb{C}}_1},{{\mathbb{C}}_2}\) can be expressed as Eq. (14). If function f () represents positive kinematics, \({{\mathbb{C}}_1},{{\mathbb{C}}_2}\) indicates the operation space that the double manipulators can be reached during the motion time. Then \({{\mathbb{C}}_1},{{\mathbb{C}}_2}\) can be expressed as Eq. (15). If \({{\mathbb{Q}}_3}\) is the operation environment space, we can get the non-collision avoidance space \({\mathbb{R}}\) for trajectory planning of double manipulators as Eq. (16). Therefore, the aim of trajectory optimization is to make the parameter of the improved polynomial trajectory planning algorithm as an evaluation index of joint trajectory performance, on the premise of ensuring the continuity, smoothness, stability and non-collision avoidance of point to point trajectories of double manipulators. By selecting the motion time tm, the state constraint of the full joint status in non-collision avoidance space should be satisfied with \(\xi {(t)_{\hbox{min} }} \leq \xi (t) \leq \xi {(t)_{\hbox{max} }}\). This is a premise for robot safety operation. At the same time, point to point trajectory planning can be completed with the shortest time. If S is defined as the manipulator operation space, the mathematical description model of the optimization objective is shown in Eq. (17).

$$\left\{ \begin{gathered} {{\mathbb{C}}_1}=\{ ({\theta _1},{d_1},{d_2})\left| {0 \leq {\theta _1} \leq } \right.\pi ,\quad 0 \leq {d_1} \leq 150\,{\text{mm}},\quad 0 \leq {d_2} \leq 91\,{\text{mm}}\} \hfill \\ {{\mathbb{C}}_2}=\{ ({\theta _2},{d_3},{d_4},{d_5})\left| {0 \leq {\theta _2} \leq } \right.\pi ,\quad 0 \leq {d_3} \leq 212\,{\text{mm}},\quad 0 \leq {d_4} \leq 170\,{\text{mm}},\quad 0 \leq {d_5} \leq 150\,{\text{mm}}\} \hfill \\ \end{gathered} \right.$$
(14)
$$\left\{ \begin{gathered} {{\mathbb{Q}}_1}=f(t,{\theta _1},{d_1},{d_2}) \hfill \\ {{\mathbb{Q}}_2}=f(t,{\theta _2},{d_3},{d_4},{d_5}) \hfill \\ \end{gathered} \right.$$
(15)
$$\begin{gathered} {\mathbb{R}}=\{ ({\theta _1},{d_1},{d_2},{\theta _2},{d_3},{d_4},{d_5})\left| {{{\mathbb{Q}}_1} \cap } \right.{{\mathbb{Q}}_2} \cap {{\mathbb{Q}}_3}= \hfill \\ \emptyset ,({\theta _1},{d_1},{d_2}) \in {{\mathbb{C}}_1},({\theta _2},{d_3},{d_4},{d_5}) \in {{\mathbb{C}}_2}\} \hfill \\ \end{gathered}$$
(16)
$$\begin{array}{*{20}{l}} {{x_{(5)}}{{(t)}_{optimal}}=}&{f({x_0},{{\dot {x}}_0},{{\ddot {x}}_0},{x_f},{{\dot {x}}_f},{{\ddot {x}}_f})\left| {{{({t_{motion}})}_{optimal}}} \right.} \\ {s.t.}&{min(t)} \\ {s.t.}&{\xi {{(t)}_{\hbox{min} }} \leq \xi (t) \leq \xi {{(t)}_{\hbox{max} }},t \in \left[ {{t_0},{t_f}} \right]} \\ {s.t.}&{S \in {\mathbb{R}}} \end{array}$$
(17)

4.2 Optimization principle

During joint trajectory planning, if the start joint position and the end joint position of any path are all within the range of the joint motion angle, then it only needs the joint rotation angle from the initial moment to the end moment always keep monotonically increasing or monotonically decreasing state, it can ensure that the joint motion angle is always in the joint motion range within the motion time. Otherwise, if it is not monotonous, then in the process of joint motion, it may occur exceed the maximum motion angle, or less than the minimum motion angle, thus the joint position is beyond the range of joint angle restriction. Therefore, this paper analyzes the effect of joint motion time on joint trajectory from the view of position constraint, when manipulator motion planning using polynomial method, any joint trajectory endpoint needs to meet the joint position constraint requirements, if the joint position from start to end moment monotonically increasing or decreasing throughout, it can ensure that the joint position to meet its motion constraints. According to the improved algorithm of polynomial interpolation trajectory planning, the motion trajectory of the joint is only related to the start or end states of the trajectory and motion time, it has nothing to do with the endpoint moment. Therefore, through optimizing the joint motion time, the trajectory of the joint position can monotonously increase or decrease during the motion time, so as to ensure that the trajectory of the joint position satisfies the constraint condition. That is to say, the joint velocity only needs to ensure that the joint velocity keep constant during the motion time. When the velocity is greater than zero, the position trajectory assumes a monotone increase state, and when the velocity is less than zero, the position trajectory is monotonically decrease state. Based on the above analysis, the condition which can guarantee the joint trajectory satisfy with its constraint can be express as Eq. (18).

$${\dot {x}_{(n)}}({t_\alpha }) \geq 0\quad or\quad {\dot {x}_{(n)}}({t_\alpha }) \leq 0 \Leftrightarrow {\dot {\Phi }_{(n)}}({t_\alpha }) \geq 0\quad or\quad {\dot {\Phi }_{(n)}}({t_\alpha }) \leq 0$$
(18)

Due to the fifth polynomial trajectory can be able to meet the position speed acceleration performance requirements, therefore, taking the fifth polynomial interpolation algorithm as an example, Eq. (18) can be summarized as a unified form as Eq. (19), then Eq. (19) can be turned into Eq. (20), from which we can obtain the joint trajectory optimal time.

$${\dot {\Phi }_{(5)}}(0) \times {\dot {\Phi }_{(5)}}(1) \geq 0$$
(19)
$$5{A_1}{A_5}+4{A_1}{A_4}+3{A_1}{A_3}+2{A_1}{A_2}+{({A_1})^2} \geq 0$$
(20)

Substituting the coefficients Eq. (11) into Eq. (20), through solving the inequality, the optimal joint motion time can be obtained.

5 Mixing constraint control of robot joint state

5.1 Position-velocity mixing control

Theoretically speaking, the position, velocity, acceleration state of robot joint should be limited within a certain range, the joint velocity and acceleration mainly relate to joint motor itself and some inherent electrical parameters, in addition to the effects of these intrinsic parameters, the operation robot can be also affected by multiple tasks such as operation assignments, operation status and operation loads. Therefore, the robot joint also has its own velocity and acceleration constraints. On the basis of solving the problem of robot joint position constraints, we can also solve the problem of velocity constraint using robot kinetics model. Figure 3 shows the principle of robot position-velocity mixing control. Setting \({x_*},{\dot {x}_*},{\ddot {x}_*}\) as ideal value of robot joint position, velocity and acceleration variable, they are the input of the control system, comparing the joint ideal position with the actual position so as to obtain the position error, and it is adjusted by the position gain matrix so as to obtain joint position control input signal. As the same, the joint ideal velocity is compared with the actual velocity so as to obtain the velocity error, and it is adjusted by the velocity gain matrix so as to obtain joint velocity control input signal. The ideal control force can be obtained by ideal position, velocity and acceleration through robot positive kinetics, it combines with the joint position control, velocity control and forward feedback torque control into the robot control torque, after the actual velocity can be obtained through robot inverse kinematics, then the joint actual position variable can be obtained by integral transformation, therefore, it feedback to the actual joint velocity and joint position, and makes the position and velocity two feedbacks form a complete double closed-loop control system respectively, wherein one of the feedback is used to control the position of robot joint and the other is used to control the velocity of robot joint, through continuous dynamics adjustment of positive kinetics, inverse kinetics and forward feedback torque compensation, the actual joint position and velocity are constantly approaching ideal joint position and velocity values until system performance requirements are satisfied.

Fig. 3
figure 3

Position and velocity mixing control

Full size image

From the principle of position-velocity mixing control, we can see that the key points is to determine the appropriate forward feedback control \({\tau _c}\), regarding general robot system, kinetic equation can be expressed as Eq. (21).

$$M(x)\ddot {x}+C(x,\dot {x})\dot {x}+G(x)=\tau$$
(21)

Substituting \(\ddot {x}=\ddot {e}+{\ddot {x}_*}\) into Eq. (21), then we can obtain Eq. (22), and\(e\),\(\dot {e}\),\(\ddot {e}\)are joint position, velocity, acceleration error respectively.

$$M(x)\ddot {e}+M(x){\ddot {x}_*}+C(x,\dot {x})\dot {x}+G(x)=\tau$$
(22)

Taking the appropriate control torque as Eq. (23).

$$\tau =M(x)({\ddot {x}_*} - {{\varvec{K}}_v}\dot {e} - {{\varvec{K}}_p}e)+C(x,\dot {x})\dot {x}+G(x)$$
(23)

Substituting Eq. (23) into Eq. (22), then we can obtain Eq. (24).

$$\ddot {e}+{{\varvec{K}}_v}\dot {e}+{{\varvec{K}}_p}e=0$$
(24)

Due to \({{\varvec{K}}_p},{{\varvec{K}}_v}\) are positive definite position and velocity gain matrix, therefore, error system Eq. (24) is stable, and position-velocity mixing control system can be stabilized by selecting proper forward feedback torque compensation, and the joint variable can converge to the ideal value from the actual value.

5.2 Position-acceleration mixing control

In many occasions, in addition to satisfying the control performance requirements of joint position-velocity for robot joint variables, in some cases, the control performance requirements of position-acceleration is also needed, therefore, based on the design principle in the above section, it is easy to get the basic structure of position-acceleration mixing control which is shown in Fig. 4. The control principle is the same with position-velocity mixing control. Through double feedback control of joint position variables and velocity variables respectively, and by selecting the appropriate forward feedback control torque, all these make the system stable and keep good performance of joint motion.

Fig. 4
figure 4

Position-acceleration mixing control

Full size image

5.3 Position-velocity-acceleration mixing control

In summary, the control method can be generalized to get a mixing control that meet more practical application requirements, that is position-speed-acceleration mixing control, the control structure is shown in Fig. 5, it has the upper, middle and lower three feedback loops which are used to adjust the joint position, velocity, acceleration respectively, and makes the system stable and obtain sound joint motion performance by selecting the appropriate forward feedback control torque.

Fig. 5
figure 5

Position-velocity-acceleration mixing control

Full size image

6 Experiment

6.1 Simulation

Taking the power cable robot rotation joint parabolic trajectory planning as an example, set the joint start status as \({x_0}=1{\text{rad}}\), \({\dot {x}_0}=0.5{\text{rad}} \cdot {{\text{s}}^{ - 1}}\), \({\ddot {x}_0}=0.52{\text{rad}} \cdot {s^{ - 2}}\), the joint end status as \({x_f}=6.4{\text{rad}}\), \({\dot {x}_f}=2.3{\text{rad}} \cdot {\text{s}}\), \({\ddot {x}_f}=1.22{\text{rad}} \cdot {s^{ - 2}}\), taking joint motion time range as \([{t_0}=0s,{t_f}=5s]\), in the MATLAB environment, we can get the simulation results of fifth interpolation algorithm and its improved algorithm for joint position trajectory under the same endpoint time shown in Fig. 6, the conventional fifth polynomial position of the fifth polynomial trajectory planning curve is shown in Fig. 6a. Keeping the original joint start and end status constant under the premise of ensuring that the joint motion time 5 s constant, the endpoint time interval is selected as [t0 = 1.5 s, tf = 6.5s], the joint trajectory at different end point obtained by the improved algorithm is shown in Fig. 7.

Fig. 6
figure 6

The position trajectory curve of the same endpoint using different algorithms. a Trajectory curve of fifth interpolation algorithm joint position (t0 = 0 s, tf =5 s). b Trajectory curve of improved algorithm joint position (t0 = 0 s, tf =5 s)

Full size image
Fig. 7
figure 7

The position trajectory curve of different endpoint using improved algorithm. a Trajectory curve of improved algorithm joint position (t0 = 0 s, tf =5 s). b Trajectory curve of improved algorithm joint position (t0 = 1.5 s, tf =6.5 s)

Full size image

From the first group simulation experiments, it can be concluded that the joint trajectory of the improved algorithm (Fig. 6b) and the fifth interpolation algorithm (Fig. 6a) are completely consistent, the only difference is the range of the horizontal axis, regarding Fig. 6a, the horizontal range is [0 s, 5s], and Fig. 6b horizontal axis range is [0,1], it just happens that the physical joint motion time is standardized, therefore, the improved algorithm can completely realize the trajectory planning function of the fifth interpolation algorithm, and the simulation results keep consistent with the theoretical analysis of the improved algorithm. From the second group of simulation experiments, we can get the conclusion that the improved algorithm trajectory planning curve (Fig. 7b) at the time when selecting the endpoint [t0 = 1.5 s, tf = 6.5s] is also completely consistent with the simulation result of selecting the endpoint time [t0 = 0 s, tf = 5s], therefore, it is verified that the joint trajectory of the improved algorithm is only related to the state of the endpoint moment and the joint motion time, but has nothing to do with the selection of endpoint moment. If the trajectory endpoint changes momentarily, the mapping of trajectory and normalization time needs to be transformed into the mapping of trajectory and physical time so as to obtain the new joint trajectory function after changing of the endpoint moment, in this way, the improvement reduces the computational complexity of the interpolation function to a certain extent and enhances the practicability of the algorithm.

We can know from the principle analysis part of improve algorithm that articulation time can be used as a performance evaluation parameter to improve the polynomial algorithm, in order to study the effect of joint motion time on joint trajectory performance, regarding the above joint status, the trajectory simulation results obtained by randomly setting the joint motion time and the optimal joint motion time in MATLAB environment are shown in Fig. 8, when joint motion time t = 1 s which randomly given, the joint position trajectory shown in Fig. 8a, the joint optimal motion time interval can be calculated as 0 < t < 0.51355 by Eq. (20), and the joint motion time is selected as tm=0.5 s within its scope, the joint trajectory obtained by the improved algorithm is shown in Fig. 8b.

Fig. 8
figure 8

The position trajectory curve of the different trajectory motion time tm. a Position trajectory curve of the joint motion time tm = 1 s. b Position trajectory curve of the joint motion time tm = 0.5 s

Full size image

According to the third group simulation results, it can be seen that the joint trajectory motion appears overshoot when the joint motion time is given as tm= 1 randomly, when the trajectory standard time is about 6.4, the angle position of the joint trajectory is 0.64 rad which the joint trajectory have already reached the endpoint position, however, the joints continued to move, after the standardization moment 0.64, the joint angle position appears 6.4 again when at normalization time 0.96, the trajectory arrival at the end position the second time, that is to say, when the joint motion time tm=1 s is set randomly, the joint angle position reach the end position twice, the reason is the additional joint motion after the first time reaching the end position, this kind extra motion not only reduces the efficiency of joint motion, but also increases the energy consumption of the joint motion, and even the joint position may exceed the range of its joint inherent constraints, more importantly, it may cause joint system damage and unnecessary operation accidents. However, after the algorithm optimization, setting the motion time as tm= 0.5 s, when the joint trajectory is about 0.5 at the normalization time and the joint angle position is 6.4 rad, which exactly reaches the end position (Fig. 8b), and it avoids the overshoot of the joint trajectory caused by artificially setting the joint motion time, which can improve the motion efficiency from the aspect of the joint motion state.

6.2 Field operation experiment

In order to further verify the engineering practicability of the improved polynomial interpolation trajectory planning algorithm and the position-velocity-acceleration mixing constraint control method, the experiments of robot insulators strings assist replacement on the analog transmission line, and the wire type is LGJ-400, insulator type is XP-7, the number of insulators are ten pieces. Robot double manipulators motion trajectory and the main operation process is shown in Fig. 9 from initial posture to the bowl hanging plate and the insulator holding state. The trajectory performance parameters comparison in the field operations of the traditional algorithm and improved algorithm of manipulator 1 and manipulator 2 shown in Table 1, the operation status of manipulator joints shown in Table 2.

Fig. 9
figure 9

Field operation experiment. a Initial posture of double manipulators. b Operation posture of double manipulators. c Bowl head hanging plate clamping of manipulator 1. d Insulator clamping of the manipulator 2

Full size image
Table 1 Performance comparison of manipulator motion
Full size table
Table 2 Joint status of the robot joint motion
Full size table

From the simulation results in Table 1, we can get the conclusion that, the joint motion of double manipulators appear no overshoot when trajectory planning using the improved algorithm, and the overshoot of joint motion were 32 and 35% when using fifth interpolation algorithm for trajectory planning. Therefore, it can be concluded that the improved algorithm can effectively eliminate the overshoot of the joint motion. Under the control of the improved algorithm in the actual operation, motion time reduce 16 s for manipulator 1 from the initial posture to the ideal posture, and motion time reduce 26 s for manipulator 2 from the initial posture to the ideal posture, all these are due to the trajectory planning using improved algorithm, which can effectively eliminate the overshoot of joint motion, so that under the control of the improved algorithm, the motion time is shorter from the initial posture to the ideal posture than that of the fifth interpolation algorithm. Therefore, no matter in what conditions, simulation experiments and field operation experiments are completely consistent, the improved algorithm plays a role in optimizing joint space movement. It can be seen from Table 2 that the joint actual position, velocity and acceleration in the process of robot operation are all within the limits range, and without exceeding the limits range of the joint state, through the coordinated movement of the manipulator, the double manipulators move from the initial state to the operation state, then to the state of the bowl head and insulator clamping, in this process, the robot double manipulators joints move continuously, smoothly and steadily which achieve the non-collision avoidance trajectory planning between the double manipulators and the double manipulators and the operation environment. It can be seen that the improved polynomial interpolation joint trajectory planning and the mixing state constraint control can solve the problem of joint state constraints in the robot operation, this method has strong engineering practicality and improves the efficiency of the joint space motion, and further reflects the intelligence of the robot operation.

7 Conclusion

  1. (1)

    The paper proposed an n-order improved polynomial interpolation trajectory algorithm based on the combination of time standardization and time contraction factor, which not only decrease the impact of end point moment selection on trajectory performance, but also increase the generality and practicability of the algorithm.

  2. (2)

    The paper established a multi-objective motion optimization model of robot manipulator and its corresponding control method which satisfy the requirements of non-collision obstacle avoidance, minimum motion time, and mixing state joint motion constraint based on robot kinetics, and obtain better motion performance.

  3. (3)

    The joint motion time is taken as a performance evaluation parameter of motion trajectory, and the mapping relationship between trajectory performance and motion time is also obtained, which not only avoid excessive joints motion, but also improve efficiency of the space joint motion. At last, simulation experiment and field operation verify the algorithm effectiveness and engineering practicality.