Trajectory planning of large redundant manipulator considering kinematic constraints and energy efficiency

Abstract

For the large redundant manipulator, due to its long working distance and large mass, the number of links (i.e., manipulator’s arms) that can be driven to move simultaneously is limited. Otherwise, the control accuracy and motion stability of the manipulator will deteriorate. Focusing on that, a weighted Newton iteration (WNI) algorithm for trajectory planning of the manipulator is firstly proposed, where the motion of the manipulator joints is controlled by a weight matrix, which is constant and related to each link’s energy consumption. To dynamically adjust the weight matrix according to kinematic constraints and acquire better energy efficiency, an adaptive WNI (AWNI) algorithm is further proposed. In AWNI, the weight matrix is adjusted in real-time during the planning process, with considerations of the kinematic constraints and the energy consumption of the manipulator. The switch of the links between the working state and the non-working state is made through the weight matrix to achieve flexible control of the manipulator motion. Two evaluation functions are established to validate the effectiveness of AWNI in energy saving and motion stability control. Taking a 6 degrees of freedom (DOF) manipulator as an example, simulation experiments on trajectory planning are carried out and the results show the effectiveness of the proposed AWNI algorithm.

1. Introduction

With the emergency requirement in modern industrial and construction fields, the redundant manipulator has become indispensable engineering equipment in these fields. The redundant manipulator is a kind of manipulator whose degree of freedom is greater than that of the task space. The kinematically redundant manipulator presents higher mobility than the non-redundant for the desired task. In addition, there are infinite joint angle configurations to complete the trajectory planning task with a given end effector pose. The movement attitude of the large redundant manipulator is usually controlled by the operator through a joystick, which requires high experience of the operator. Under this situation, the motion trajectory and positioning accuracy of the manipulator are often poor. In recent years, with the increasing demand for long-distance operation, large redundant manipulators with 5–7 links and a total length of more than 50 m have gradually emerged. However, due to the large structure and heavy load, the driving power is not sufficient to support all links to move at the same time. In addition, motion stability needs to be maintained since the large multi-DOF manipulator is sensitive to vibration. Given the above problems, it is urgent to propose a trajectory planning method that can maintain motion stability as well as consider energy efficiency for the large redundant manipulator.

During the modeling of the redundant manipulator, it is regarded as a kinematic chain system and each link is expressed with respect to its adjacent link [1]. As more links are used in modeling a system, the procedures required to derive the relevant motion equations become more laborious and complicated [2, 3]. To overcome this problem, it is essential to apply a recursive formulation to automatically obtain the governing equations. The first formulation developed is Lagrange-Euler equations with computational complexity O(n). However, it is found to be inefficient due to the high number of algebraic operations involved [4]. Another mainstream formulation is the Gibbs-Appell equation. However, its computational complexity is O(n²) and therefore does not meet the computational efficiency requirement. Compared with the above equations, the Newton-Euler equations, with the computational complexity O(n) and rapid convergence speed, are found to be the most appropriate formulation for the trajectory planning of redundant manipulator [5].

Tremendous work has been carried out on the trajectory planning of the manipulators. Aiming at various joint ranges and working scenarios, specific trajectory planning algorithms with considerations of energy saving [6], obstacle avoidance [7], and trajectory smoothing [8] are proposed for the redundant manipulators. The trajectory planning algorithms set the corresponding constraints and optimization objectives according to the application scenarios of the manipulator to solve the optimal trajectory [9].

The manipulator trajectory planning algorithms can be classified into two categories: algorithms based on motion relationship [10, 11] and algorithms based on heuristic search [12, 13]. The trajectory planning algorithms based on motion relationship use known information, such as the initial position or historical posture, to summarize a trajectory planning pattern for the manipulator to follow. The motion of the manipulator is expressed as a nonlinear equation and the limits of joint angles, velocities or accelerations are transferred into constraints, which turns a trajectory planning problem into a nonlinear optimization problem. There are plenty of motion relationship-based trajectory planning researches aiming at energy saving. Ayten et al. [14] transferred kinematic and dynamic constraints into a cost function to reduce computational complexity. Also, they took the redundant links as a single link to make the method easier to compute and compatible with the addition of more links. However, this method neglects the energy-consuming difference between links and might not converge. Potter et al. [15] proposed an optimal trajectory planning algorithm of redundant manipulator with energy saving based on fourth-degree polynomials. They presented an optimal weighting vector that establishes the influence of each joint on the total energy consumption. However, the singularity problem will raise with the lack of consideration of joint limits. In refs. [16, 17], trajectory planning with minimum energy consumption was transferred into a nonlinear optimization problem and the multi-objective optimization method was proposed to achieve the optimal solution. However, the multi-objective function lacks consideration of jerks and constraining conditions in actual situations. Also, there are motion relationship-based trajectory planning researches aiming at motion stability. Pham et al. [18] extended the classical algorithm of time-optimal path parameterization to the case of redundantly actuated robot systems. The authors designed a numerical search for an approximate optimal acceleration to ensure the smoothness of the obtained profile and converted the polygon constraints back into the half-plane inequalities to speed up the computation. However, this method transforms the contact between human and the manipulator into torque constraints to plan the trajectory and thus is not suitable for large manipulators. Su et al. [19] proposed a Pythagorean-Hodograph curves-based trajectory planning algorithm. They classified the different operations according to the geometric relationship of the path. The trajectory planning was carried out for each situation and the curved motion segments were solved by adopting the corresponding polynomial motion law. However, this method allows joint reciprocating motion during the working process and it could cause serious consequences in the large manipulators. Shen et al. [20] investigated a new discrete-time repetitive path planning (RPP) scheme depicted in the pseudoinverse-type formulation for redundant robot manipulators. The scheme was derived from the discretization of the continuous-time RPP scheme using a special numerical difference formula, which had been previously used to discretize a minimum norm velocity scheme in other studies. It was proved that the proposed RPP scheme has the property of a cube pattern in the end effector planning accuracy. However, this method ignores the consideration of joint limits. Pei et al. [21] split the trajectory into several sub-trajectories and used an inverse kinematic-based model to obtain the angular displacements of joints corresponding to the final positions of the end effector in each sub-trajectory. To guarantee motion stability, the quintic polynomial curve was utilized to interpolate in joint space to obtain the continuous angular jerk. However, the interpolation will reduce the end effector’s position precision. In general, the trajectory planning algorithms based on motion relationship can find the solutions satisfying the constraints quickly, but they have many disadvantages, such as only finding the local optimal solutions, requiring convergence gradients, and failing for discontinuous functions.

The trajectory planning algorithms based on heuristic search usually propose criteria to evaluate the optimization objective and then apply the heuristic search method to find the global optimal solutions. Heuristic search performs an ordered search on the planning domain and makes the generated trajectory approach the optimal trajectory. Various constraints can be adopted with strong compatibility. Many heuristic search-based trajectory planning researches aiming at energy saving have been proposed. Elshabasy et al. [22] developed a redundant manipulator trajectory planning method based on the combination of genetic algorithm and constrained function with the consideration of minimum energy consumption. However, with the motion stability not taken into consideration, the jerks of the joints might increase dramatically so it is not suitable for large manipulators. Agarwal [23] proposed a trajectory planning method based on a new fuzzy clustering model, which obtains links’ posture data to establish an expert system. The optimal solution with various objectives can be achieved through the expert system. However, this method is too dependent on the richness of data. In refs. [24, 25], an improved PSO algorithm was applied to perform the trajectory planning task. The optimal solution with energy saving was achieved by stochastically generating locations in the workspace and selecting the best candidate. However, this method requires predefined hyperparameters, and it is difficult to balance computation complexity and convergence speed. Also, there are plenty of heuristic search-based trajectory planning researches aiming at motion stability. Kang et al. [26] combined intelligent hill climbing and a genetic algorithm to plan the trajectory under certain constraints, while Marcos et al. [27] proposed a new method that combines the closed-loop pseudoinverse method with the genetic algorithm to do the trajectory planning of redundant robots. In both cases, with the consideration of motion stability, good results can be achieved with various joint constraints and limits. However, neither method can distinguish distal and proximal joints and satisfy the energy saving demand. Lin [28] proposed a hierarchical genetic algorithm for manipulator path planning, which consists of a global path planner and a local motion planner. The global path planner plans a path for the manipulator end effector, and the local motion planner plans the manipulator’s configurations along the path by a genetic algorithm with a non-random initial population. The planned trajectory is smoother, but the convergence speed is slower. Shrivastava et al. [29] calculated the jerk-optimized trajectory of a redundant manipulator with the gray wolf optimizer. The approach can generate a minimal difference in the joint trajectory to guarantee joint stability. However, the precision of the end effector will inevitably reduce. In brief, the trajectory planning algorithms based on heuristic search can find the optimal solutions under certain evaluation criteria, but they require long computational time, therefore not suitable for real-time trajectory planning.

Due to redundant degrees of freedom, the trajectory planning problem of the large redundant manipulator can be transformed into a multibody nonlinear problem. For the multibody nonlinear problem where analysis and computation are expensive, a robust optimization strategy with rapid solution convergence characteristics is highly desirable. To perform trajectory planning under multiple constraints, the Newton iteration(NI) method is applied in recent researches [30–32]. However, the traditional NI-based trajectory planning method cannot solve the following problems.

The motion range of the joints is constrained during the working process; therefore, the solutions solved by the trajectory planning algorithms may not be valid [33]. And if some joints reach the limits, the trajectory planning algorithms will fail to find solutions because of the singularity problem [34, 35]. Furthermore, due to the limitations of the driving power, all the joints of the manipulator cannott be activated simultaneously. Link motion switching should be considered when planning the trajectory of the manipulator.

In this paper, a trajectory planning method of redundant manipulators based on both motion relationship and heuristic search is proposed, which takes the switch of link motion and kinematic constraints into consideration, and aims at improving energy efficiency and motion stability. Firstly, a weighted Newton iteration (WNI) algorithm is proposed by combining NI, a motion relationship-based method, with heuristic search. In WNI, the NI method is firstly used to find the solution set of trajectory planning for the link system, then the weighted method is applied to provide a weight matrix for the motion of the joints in the system to select the optimal solution. Based on that, to achieve better energy efficiency and motion stability, an adaptive weighted Newton iteration (AWNI) algorithm is further proposed. In AWNI, the weight matrix is adjusted adaptively during the planning process according to the kinematic constraints and energy consumption of the manipulator. The weight matrix takes the joint limits into consideration, which guarantees that the joints approaching the limits contribute less on the trajectory planning and gradually stop working. Also, the AWNI algorithm selects another joint to continue the trajectory planning and switches the working joint. In conclusion, the AWNI algorithm can acquire the optimal solution under the constraints above for low energy consumption and better motion stability. Correspondingly, two evaluation functions that can represent the aforementioned targets are defined to evaluate the effectiveness of the trajectory planning of the large redundant manipulator.

This paper has the following innovations:

• A trajectory planning algorithm for large redundant manipulators, AWNI, is proposed, which exploits both motion relationship and heuristic search. First, Newton iteration, which is a motion relationship-based method, is applied to solve the solution set. Then, a heuristic search-based method is applied to find the optimal solution of a loss function with the adaptively weighted joints. Link motion switching and the limits of the joints are respectively considered to achieve better energy efficiency and motion stability.

• A weight matrix is proposed to find the optimal solution for energy saving and motion stability. In terms of the energy consumption of proximal and distal joints, the proposed AWNI algorithm selects a group of working joints and adjusts the weight matrix adaptively according to the working state of each joint to achieve better energy saving. The weight value of the joint approaching the limit adaptively increases to make sure the joint contributes less on the trajectory planning and gradually stops so that motion stability can be guaranteed.

• Two evaluation functions are established to intuitively compare the differences between various algorithms in terms of energy saving and motion stability control. Simulation verifies that the AWNI algorithm outperforms NI and WNI algorithms in trajectory planning of large redundant manipulators.

The rest of this paper is organized as follows. Section 2 analyzes the kinematic model of the redundant manipulator and elaborates on the application of the Newton iteration algorithm in redundant manipulator trajectory planning. Section 3 proposes the WNI trajectory planning algorithm, aiming at improving energy efficiency and motion stability, and establishes two evaluation functions to evaluate the algorithm’s performance with the aforementioned targets. Section 4 further proposes the AWNI algorithm to adjust the weight matrix adaptively during the planning process, according to the kinematic constraints and link motion switching. Section 5 carries out simulation experiments and makes a discussion about the simulation results. Section 6 summarizes the study.

2. Kinematic description and posture solving of the redundant manipulator

2.1. Kinematic description of the redundant manipulator

In most cases, a large redundant manipulator consists of several links and a rotating base. Figure 1 shows the schematic diagram of a six-link redundant manipulator. The rotating base enables the link system to rotate around the y axis, and each link makes itself move in the XOY coordinate plane by the corresponding joint.

Figure 1. Schematic diagram of the manipulator.

Let the angle of the rotating base relative to the positive direction of the x axis be $\theta _{0}$ . The actuated links are numbered sequentially as the link $i(i=1\ldots 6)$ . For each link, $l_{i}$ denote its length, and $\theta _{i}(i=1\ldots 6)$ denotes the angle between link i and link i−1. In terms of the structural characteristic of the links, the manipulator’s kinematic model can be established with the D-H [36] method.

Assuming the center point of joint #1 is the initial point and its coordinates are $[\begin{array}{l@{\quad}l@{\quad}l} 0 & 0 & 0 \end{array}]^{T}$ , the relationship between the initial point and the manipulator’s terminal center point can be described as follows

(1) \begin{equation} \boldsymbol{P}=\left[\begin{array}{l} p_{x}\\ p_{y}\\ p_{z} \end{array}\right]=\left[\begin{array}{c} c0\left(l_{1}c1-l_{2}c12+l_{3}c123-l_{4}c1234+l_{5}c12345-l_{6}c123456\right)\\ l_{1}s1-l_{2}s12+l_{3}s123-l_{4}s1234+l_{5}s12345-l_{6}s123456\\ s0\left(l_{1}c1-l_{2}c12+l_{3}c123-l_{4}c1234+l_{5}c12345-l_{6}c123456\right) \end{array}\right]\! .\end{equation}

where $c0=\cos (\theta _{0}),\, s0=\sin (\theta _{0}),\, c1=\cos (\theta _{1}),\, s1=\sin (\theta _{1}),\, c12=\cos (\theta _{1}+\theta _{2}),\, s12=\sin (\theta _{1}+\theta _{2})$ and so on, $\boldsymbol{P}$ is the 3 × 1 coordinate vector of the manipulator’s terminal center point.

Therefore, when $\theta _{0}$ is determined, Eq. (1) can be rewritten as

(2) \begin{equation} \boldsymbol{P}=F(\boldsymbol{\Theta }) .\end{equation}

where $\boldsymbol{\Theta }$ is the joint angle vector composed of $\theta _{i}(i=1\ldots 6)$ , which directly determines the posture of the manipulator’s links.

2.2. Posture solving based on Newton iteration

Toward the trajectory planning of the redundant manipulator under the given motion path, the key task is to divide the whole path into small steps and solve the optimal posture of manipulator links (i.e., the joint angle vector $\boldsymbol{\Theta }$ ) in each step.

Obviously, from Eq. (2) we can find that the partial derivative can be expressed as

(3) \begin{equation} \dot{\boldsymbol{P}}=J(\boldsymbol{\Theta })\dot{\boldsymbol{\Theta }} .\end{equation}

where $\dot{\boldsymbol{P}}$ is the velocity matrix of the manipulator’s terminal center point, $\dot{\boldsymbol{\Theta }}$ is the angular velocity matrix of the manipulator’s joints, $J(\boldsymbol{\Theta })$ is the 3 × 6 Jacobian matrix which describes the transformation relationship from $\dot{\boldsymbol{\Theta }}$ to $\dot{\boldsymbol{P}}$ under current joint angles $\boldsymbol{\Theta }$ .

Since $J(\boldsymbol{\Theta })$ is not a positive definite matrix, the inverse matrix of $J(\boldsymbol{\Theta })$ cannot be solved directly. Define $J^{+}(\boldsymbol{\Theta })$ as the pseudoinverse matrix of $J(\boldsymbol{\Theta })$ .

(4) \begin{equation} J^{+}(\boldsymbol{\Theta })=J^{T}(\boldsymbol{\Theta })\left(J(\boldsymbol{\Theta })J^{T}(\boldsymbol{\Theta })\right)^{-1} .\end{equation}

From the work of Cheah et al. [37], it can be seen that the progressive stability and boundedness of $J^{+}(\boldsymbol{\Theta })$ can be guaranteed with the continuous recalculation of $J^{+}(\boldsymbol{\Theta })$ in the iteration process.

From Eqs. (3) and (4), the angular velocity matrix of the manipulator’s joints can be given as

(5) \begin{equation} \dot{\boldsymbol{\Theta }}=J^{+}(\boldsymbol{\Theta })\dot{\boldsymbol{P}} .\end{equation}

The Newton iterative equation for solving $\boldsymbol\Theta$ can be expressed as

(6) \begin{equation} \boldsymbol\Theta _{i}=\boldsymbol{\Theta }_{i-1}+J^{+}\left(\boldsymbol{\Theta }_{i-1}\right)\text{error}_{i-1}, \end{equation}

(7) \begin{equation} \text{error}_{i}=\boldsymbol{P}_{\text{goal}}-\boldsymbol{P}_{i} .\end{equation}

where error _i is the error function in the iterative calculation and is used to measure the gap between the target point $\boldsymbol{P}_{\text{goal}}$ and the current terminal center point $\boldsymbol{P}_{\boldsymbol{i}}$ in the ith iteration. Compare $\| \text{error}_{i}\| _{2}$ with the threshold $\xi$ to determine whether the iterative calculation result satisfies the required accuracy.

3. Trajectory planning based on WNI algorithm

3.1. Solution set for trajectory planning

Since the manipulator has redundant degrees of freedom, there are infinite number of posture solutions in its trajectory planning. After obtaining the solution set to the link posture with the given target coordinates, it is necessary to find the optimal solution satisfying the constraints and optimization objectives. The posture solution set can be obtained by

(8) \begin{equation} \Delta \boldsymbol{\Theta }=J^{+}\left(\boldsymbol{\Theta }_{\text{NI}}\right)\Delta \boldsymbol{P}+\left(I-J^{+}\left(\boldsymbol{\Theta }_{\text{NI}}\right)J\left(\boldsymbol{\Theta }_{\text{NI}}\right)\right)\boldsymbol{z}, \end{equation}

(9) \begin{equation} \boldsymbol{\Theta }_{\text{op}}=\boldsymbol{\Theta }_{\text{origin}}+\Delta \boldsymbol{\Theta } .\end{equation}

In Eq. (8), $\Delta \boldsymbol{\Theta }$ is the variation of the manipulator’s joint angles, $\Delta \boldsymbol{P}$ is the variation of the manipulator’s end effector coordinates, $\boldsymbol{\Theta }_{\text{NI}}$ is the posture solution with Newton iteration, which is also the minimum norm solution of Eq. (2). $(I-J^{+}(\boldsymbol{\Theta }_{\text{NI}})J(\boldsymbol{\Theta }_{\text{NI}}))$ is link’s null-space projection matrix under the angles of $\boldsymbol{\Theta }_{\text{NI}}$ . Given the characteristic of $J^{+}, (I-J^{+}(\boldsymbol{\Theta }_{\text{NI}})J(\boldsymbol{\Theta }_{\text{NI}}))J^{+}(\boldsymbol{\Theta }_{\text{NI}})=0$ , which means the two terms multiplied are orthogonal. In Eq. (9), $\boldsymbol{\Theta }_{\text{origin}}$ is the original joint angle vector of the manipulator, $\boldsymbol{\Theta }_{\text{op}}$ is the joint angle vector corresponding to the target coordinates of the end effector. The calculation result of $\boldsymbol{\Theta }_{\text{op}}$ varies with the 6 × 1 arbitrary vector z , and the posture solution set of the target point for trajectory planning can be formed with different $\boldsymbol{\Theta }_{\text{op}}$ .

Although the solution set can be obtained with the NI method, the optimal solution with best energy saving and motion stability control remains unknown. The optimal solution of $\boldsymbol{\Theta }_{\text{op}}$ can be obtained by an optimized z during the whole process of trajectory planning. Therefore, a weighted Newton iteration (WNI) algorithm is proposed in this paper, which applies the NI algorithm to find the solution set and the heuristic method with a weight matrix for the joint motion to find the optimal posture solution.

3.2. Principle of WNI algorithm

In the process of calculating $\Delta \boldsymbol{\Theta }$ by changing z , $\boldsymbol{\Theta }_{\text{NI}}$ cannot remain the same when $\Delta \boldsymbol{\Theta }$ changes, which badly influences the position accuracy of the end effector. It is necessary to propose a trajectory planning strategy, which combines Newton iteration and z -optimization to simultaneously satisfy the constraints and accuracy limits.

The energy consumed by the manipulator in a trajectory is used for driving the angular movements of the joints. Therefore, the energy consumption is closely related to the changes of the joint angles. A loss function is proposed and expressed as

(10) \begin{equation} Q=\sum _{i=1}^{6}W_{i}\Delta \theta _{i}^{2} .\end{equation}

where $W_{i}$ is the weight assigned to the squared angle variation $\Delta \theta _{i}$ of the ith joint to control the link motion, $Q$ is the computed loss. To obtain the optimal energy saving solution, the weight value assigned to each joint should be determined by its energy consumption. Since the proximal joints consume more energy than the distal joints performing the same amount of angular movement, larger weights are assigned to the joints closer to the base, and vice versa. Therefore, the angular changes of the proximal joints will be set smaller during the optimization of the loss function. Furthermore, an adjustment strategy for the weights is needed to guarantee stability when the joints approach the limits. When the working links and the corresponding joint weights are determined, the $\Delta \boldsymbol{\Theta }=[\begin{array}{lll} \Delta \theta _{1} & \cdots & \Delta \theta _{6} \end{array}]^{T}$ that minimizes the loss function can be found, which is the optimal solution for calculating the posture $\boldsymbol{\Theta }_{\text{op}}$ by Eq. (9).

Under the actual operation conditions of the 6-DOF large redundant manipulator, to ensure the operation accuracy and stability, the number of links moving simultaneously must not exceed four. The four distal links are often used as the main working links, due to their lighter weight and better controllability. In addition, the direction of joint motion should remain unchanged to prevent link vibrations caused by reciprocating motion. Under the constraints of the direction of joint motion and the number of working joints/links, there may be no posture that can reach the specific target position. In that case, the working links should be switched so that the trajectory planning can be continued. During the optimization of $\Delta \boldsymbol{\Theta }$ , the switch of the working links should also be performed when a working link reaches the motion boundary, as a result of the corresponding joint reaching its limit.

As mentioned above, it is possible that the state of the links needs to be switched between working and non-working. However, the states of all links in Eq. (5) are set to be the working state by default. Therefore, in the solving process for trajectory planning, $\boldsymbol{\Theta }$ and its corresponding Jacobian matrix $J(\boldsymbol{\Theta })$ need to be constantly modified according to the current link states.

Let the serial numbers of working links be j, k, m and n, where $1\leq j\lt k\lt m\lt n\leq 6$ , then $\boldsymbol{\Theta }$ and $J(\boldsymbol{\Theta })$ can be modified as

(11) \begin{equation} \boldsymbol{\Theta }=\left[\begin{array}{l@{\quad}l@{\quad}l@{\quad}l} \theta _{j} & \theta _{k} & \theta _{m} & \theta _{n} \end{array}\right]^{T}, \end{equation}

(12) \begin{equation} J(\boldsymbol{\Theta })=\left[\begin{array}{llll} \dfrac{\partial p_{x}}{\partial \theta _{j}} & \dfrac{\partial p_{x}}{\partial \theta _{k}} & \dfrac{\partial p_{x}}{\partial \theta _{m}} & \dfrac{\partial p_{x}}{\partial \theta _{n}}\\ \dfrac{\partial p_{y}}{\partial \theta _{j}} & \dfrac{\partial p_{y}}{\partial \theta _{k}} & \dfrac{\partial p_{y}}{\partial \theta _{m}} & \dfrac{\partial p_{y}}{\partial \theta _{n}}\\ \dfrac{\partial p_{z}}{\partial \theta _{j}} & \dfrac{\partial p_{z}}{\partial \theta _{k}} & \dfrac{\partial p_{z}}{\partial \theta _{m}} & \dfrac{\partial p_{z}}{\partial \theta _{n}} \end{array}\right]\! .\end{equation}

In combination with Eqs. (8)–(12), the loss function Q can be described as a function of vector z . So, finding the optimal solution is equivalent to solving the following equation

(13) \begin{equation} \boldsymbol{z}_{\text{op}}=\text{arg min}Q \!.\end{equation}

Equation (13) can be transformed to

(14) \begin{equation} \frac{\partial Q}{\partial \mathbf{z}}=0 .\end{equation}

It can be obtained from Eq. (14) that

(15) \begin{equation} \boldsymbol{z}_{\text{op}}=\boldsymbol{A}^{-1}\boldsymbol{B} .\end{equation}

where $A=(I-J^{+}J)^{T}\boldsymbol{W}(I-J^{+}J), B=-(I-J^{+}J)^{T}\boldsymbol{W}J^{+}\Delta P, \boldsymbol{W}=\left[\begin{smallmatrix} W_{j} & 0 & 0 & 0\\ 0 & W_{k} & 0 & 0\\ 0 & 0 & W_{m} & 0\\ 0 & 0 & 0 & W_{n} \end{smallmatrix}\right]$ is a 4 × 4 weight matrix associated with the current working joints to calculate Q. The optimal solution of $\boldsymbol{\Theta }_{\text{op}}$ can be obtained by substituting the optimized $\boldsymbol{z}_{\text{op}}$ in Eqs. (8) and (9).

The manipulator trajectory planning algorithm based on WNI is shown in Algorithm 1 and Fig. 2.

Algorithm 1: Manipulator trajectory planning algorithm based on WNI.

Figure 2. Flowchart of manipulator trajectory planning based on WNI algorithm.

3.3. Establishment of evaluation functions

Two evaluation functions E ₁ and E ₂ are proposed to intuitively compare the differences between various algorithms in terms of energy saving and motion stability control.

On the one hand, due to the long working distance and large mass of the redundant manipulator, it is desirable that the planning algorithm can complete the task with minimum energy consumption. In the working process, the motion of each link is controlled by the angular movement of a corresponding joint. The energy consumption of the manipulator between two different postures can be evaluated as the summed absolute change of the kinetic energy of all links. Therefore, the evaluation function for energy saving is expressed as

(16) \begin{equation} E_{1}=\frac{1}{Z}\sum _{t=1}^{N}\sum _{i=1}^{6}\left| \frac{1}{2}J_{i}\omega _{i,t}^{2}-\frac{1}{2}J_{i}\omega _{i,t-1}^{2}\right|, \end{equation}

where Z represents the length of the working path, $t(t=1\ldots N)$ represents the tth step of the whole working path, $i(i=1\ldots 6)$ represents the ith joint and link of the manipulator, $J_{i}$ is the moment of inertia of link i, $\omega _{i,t}$ and $\omega _{i,t-1}$ are the angular velocities of joint i in different steps.

On the other hand, the manipulator’s motion stability is closely related to the jerks of the joints [38]. When the joint jerk is high, the motion of the joint would change abruptly, making the manipulator vibrate or even run out of control [39, 40]. The evaluation function for motion stability can be defined by the maximum jerk the joints generate during the working process. Therefore, the evaluation function for motion stability control is expressed as

(17) \begin{equation} E_{2}=\max p_{i} .\end{equation}

where $p_{i}$ is the average angular jerk of joint i.

$E_{1}$ calculates the absolute changes of the manipulator’s kinetic energy in each step and then adds them up to evaluate the energy consumption during the working process, a higher E ₁ indicates higher energy consumption. $E_{2}$ calculates the maximum angular jerk of all joints. In the working process, the most severe vibration occurs when the joint jerk is at its maximum, therefore a smaller $E_{2}$ indicates better motion stability.

When calculating $E_{1}$ and $E_{2}$ , what we know are the time spent on the joint motion in each step and the length of each step. So, the average angular velocity of the joint in each step is used to replace the instantaneous angular velocity at the step dividing point. And then, the joint angular acceleration and jerk can be obtained by the difference method.

4. Adaptive adjustment of the weight matrix

4.1. Configuration of weight matrix

The weight matrix W is determined by each joint’s position, motion state and distance from its limit. A constant weight matrix is difficult to adapt to different working conditions; therefore, it is necessary to recalculate W constantly and adaptively during the trajectory planning. The elements of $\boldsymbol{W}$ can be adjusted based on the current link posture as follows

(18) \begin{equation} W_{i}=\frac{T_{i}+S_{i}}{\frac{\theta _{\text{boundary}\_ i}-\theta _{\text{origin}\_ i}}{\theta _{\text{boundary}\_ i}-\theta _{\text{threshold}\_ i}}}=\frac{\left(T_{i}+S_{i}\right)\left(\theta _{\text{boundary}\_ i}-\theta _{\text{threshold}\_ i}\right)}{\left(\theta _{\text{boundary}\_ i}-\theta _{\text{origin}\_ i}\right)},\quad i=1\ldots 6 .\end{equation}

where $\mathbf{T}=[\begin{array}{lll} T_{1} & \cdots & T_{6} \end{array}]$ is a 1 × 6 constant weight parameter vector, $\mathbf{S}=[\begin{array}{lll} S_{1} & \cdots & S_{6} \end{array}]$ is a 1 × 6 constant state parameter vector, $\theta _{\text{boundary}\_ i}$ is the limit to the angle of joint i, $\theta _{\text{threshold}\_ i}$ is the preset angle threshold of joint i (a certain distance from the angle limit).

As mentioned above, the large redundant manipulator cannot move all its links at the same time, and the four distal links are often used as the main working links. The closer a joint is to the base, the more difficult it is to drive it, and a larger weight should be assigned to it in the loss function. Therefore, in Eq. (18), the weight parameters $T_{i}(i=1\ldots 6)$ are set to constant values according to the driving difficulty of the links. The state parameters $S_{i}(i=1\ldots 6)$ are set according to the motion state of the link. The links should move slower during acceleration and deceleration than in the normal working state. So, $S_{i}$ is set to a predefined positive number during acceleration and deceleration, and zero in the normal working state. According to Eq. (18), during the motion of the manipulator, the variation of W is relatively small when the original joint angles $\boldsymbol{\Theta }_{\text{origin}}$ differ greatly from the preset angle thresholds. After reaching the threshold, due to the property of the inverse proportional function in Eq. (18), the weight will increase rapidly, reducing the angular change of the joint until it stops (in the non-working state). By taking the current link posture into account, we can prevent the solution results from exceeding the joint limits. Meanwhile, the vibration caused by a sudden change in the links’ motion state during acceleration and deceleration can be reduced by adaptively adjusting the weight matrix W under different link postures.

4.2. Adaptive weighted Newton iteration (AWNI) algorithm

In trajectory planning, if the joint reciprocates, the corresponding link will violently vibrate and affect the control accuracy. Therefore, it is necessary to guarantee that the joint will not reciprocate during the entire motion. The direction of the joint motion should be determined as the working path is set. If the result in Eq. (8) is opposite to the default direction, reset the result as 0 and continue the iterative calculation until a valid solution is found.

As described above, set the angle threshold $\boldsymbol{\Theta }_{\text{threshold}}=[\begin{array}{lll} \theta _{\text{threshold}\_ 1} & \cdots & \theta _{\text{threshold}\_ 6} \end{array}]$ for all joints. When the angle of a joint is close to the corresponding angle threshold in $\boldsymbol{\Theta }_{\text{threshold}}$ , the actuated links should be switched, which means that the corresponding link should be set to non-working state and an originally non-working link should be actuated instead. Then W should be reconstructed with the new set of working links. The trajectory planning algorithm continues with the new weight matrix W .

In conclusion, by integrating the WNI algorithm in Section 3 and the weight matrix’s adaptive adjusting method described above, the AWNI algorithm is proposed. The whole trajectory is divided into some sub-trajectories and planned step by step. At a certain sub-trajectory, assuming the original joint angle vector is $\boldsymbol{\Theta }_{\text{origin}}$ , the corresponding weight matrix W is constructed and the joint angle vector $\boldsymbol{\Theta }_{\text{op}}$ corresponding to the target coordinates can be achieved by

(19) \begin{equation} \boldsymbol{\Theta }_{\text{op}}=\boldsymbol{\Theta }_{\text{origin}}+\text{arg\,min}\left(\sum W_{i}{\Delta \theta _{i}^{2}}\right) \end{equation}

Algorithm 2 shows the adaptive adjustment of weight matrix W and the joint/link motion switching in the AWNI algorithm.

Algorithm 2: Adaptive adjustment of weight matrix and joint/link motion switching in AWNI algorithm.

Table 1. Kinematic parameters of each link.

5. Simulation and Validation

5.1. Configuration of parameters

As shown in Fig. 3, take a 6-DOF truck-mounted concrete pump manipulator as an example. The torsion angle $\alpha$ and base tangent length a in the D-H method are all set as 0. Table 1 shows the kinematic parameters of the manipulator.

Figure 3. Diagram of the large redundant manipulator.

5.2. Determining joint threshold

From Eq. (18), it can be seen that the threshold $\boldsymbol{\Theta }_{\text{threshold}}$ must be settled before dynamic weight calculation. $\boldsymbol{\Theta }_{\text{threshold}}$ reflects the working range of the links and has a great influence on W . Empirically, $\boldsymbol{\Theta }_{\text{threshold}}$ should be close to $\boldsymbol{\Theta }_{\text{boundary}}$ to prevent wasting too much workspace. To verify the effectiveness of $\boldsymbol{\Theta }_{\text{threshold}}$ in the AWNI algorithm, define $\boldsymbol{\Theta }_{\text{threshold}}=\boldsymbol{\Theta }_{\text{boundary}}-\Delta \boldsymbol{\Theta }_{\text{threshold}}$ . A threshold descriptor $\Delta \theta _{\text{threshold}}$ is introduced to formulate $\Delta \boldsymbol{\Theta }_{\text{threshold}}$ and nine groups of $\Delta \boldsymbol{\Theta }_{\text{threshold}}$ are set, where $\Delta \boldsymbol{\Theta }_{\text{threshold}}=\boldsymbol{V}(\Delta \theta _{\text{threshold}}), \Delta \theta _{\text{threshold}}=1^{\circ},2^{\circ},\ldots 9^{\circ}$ . $\boldsymbol{V}(x)$ generates a 6 × 1 vector and each of its elements equals x. Generally, the distal links are more likely to reach the threshold, also, the range of the 4th and 5th joints both exceed 180°, therefore the 3rd and 6th joints are the appropriate experimental target joints. Respectively take the 3rd joint and 6th joint as tests, set the links’ initial pose as $\boldsymbol{\Theta }_{\text{initial}}=[\begin{array}{l@{\quad}l@{\quad}l@{\quad}l@{\quad}l@{\quad}l} 70^{\circ} & 168^{\circ} & 168^{\circ} & 208^{\circ} & 178^{\circ} & 98^{\circ} \end{array}]^{T}$ , then plan the trajectory towards positive Y direction. During each test, to verify the effectiveness of $\boldsymbol{\Theta }_{\text{threshold}}$ in trajectory planning and avoid the influence of the other joints, calculate the weights of the target joints with Eq. (18) and the weights of the other joints remain constant. In case 1, the 6th joint is chosen as target joint, while in case 2, the 3rd joint is chosen as target joint. The results of $E_{1}$ and $E_{2}$ are shown in Figs. 4 and 5. It can be seen that the increase of $\Delta \theta _{\text{threshold}}$ causes the increase of $E_{1}$ and the decrease of $E_{2}$ , which means higher energy consumption and better motion stability. In the nine groups, a relatively smaller $\Delta \theta _{\text{threshold}}$ means that the movable range of the joint beyond the angle threshold is smaller, so the proximal joint will start earlier, which is detrimental to the energy saving of the manipulator. Meanwhile, the joints’ deceleration generates in the range of $\Delta \theta _{\text{threshold}}$ ; therefore, a larger $\Delta \theta _{\text{threshold}}$ is more suitable considering the motion smoothness.

Figure 4. Generalized E ₁ in nine groups.

Figure 5. Generalized E ₂ in nine groups.

Consequently, the energy consumption of the manipulator increases when $\Delta \theta _{\text{threshold}}$ becomes larger, which will lead to higher work costs. And the motion stability of the joint gets poor when $\Delta \theta _{\text{threshold}}$ become smaller, which will affect the working accuracy. Under normal conditions, the working process of the manipulator should consider both energy consumption and motion stability. To sum up, the experiment results show that the appropriate value of $\Delta \theta _{\text{threshold}}$ is between $4^{\circ}$ and $6^{\circ}$ .

5.3. Verification of AWNI algorithm

With the joints’ initial angle $\boldsymbol{\Theta }_{\text{initial}}=[\begin{array}{llllll} 75^{\circ} & 140^{\circ} & 150^{\circ} & 150^{\circ} & 130^{\circ} & 90^{\circ} \end{array}]^{T}$ , the initial end point’s coordinate can be calculated to be (28,048, 3685, 0). Assume that the working path is as follows: (1) 10,000 mm along the X axis; (2) −10,000 mm along the X axis; (3) 10,000 mm along the Y axis, and (4) −10,000 mm along the Y axis. Divide the working path into sub-trajectories with a step size of 100 mm, and the starting position of each sub-trajectory is the current pose and the ending point is the desired target point of the end effector. To prove the advantages of dynamically adjusting the weight matrix, we set an experimental group with a constant weight matrix. Separately apply (a) NI, (b) WNI with the weight matrix, with $W_{1}=6, W_{2}=5, W_{3}=4, W_{4}=3, W_{5}=2, W_{6}=1$ , and (c) AWNI to plan the links’ posture on the path. Set Newton iteration threshold $\xi =0.1$ mm. The links’ posture varieties are compared in Fig. 6.

Figure 6. Planned trajectory in case algorithm: (a) NI in case 3, (b) WNI in case 3, (c) AWNI in case 3, (d) NI in case 4, (e) WNI in case 4, (f) AWNI in case 4, (g) NI in case 5, (h) WNI in case 5, (i) AWNI in case 5, (j) NI in case 6, (k) WNI in case 6 and (l) AWNI in case 6.

As shown in Fig. 6 and Table 2, six links/joints work at the same time following the NI algorithm planning result. After adding the weighted optimization of the loss function in the trajectory planning, the WNI algorithm still drives six links/joints to complete the trajectory planning tasks, but it relies more on the distal joints. When applying the AWNI algorithm, distal links/joints #3–#6 work firstly, if a working link approaches its joint angle limit, it can be replaced by another alternative link/joint (in case 3, link/joint #6 is replaced by link/joint #2).

Table 2. Joints’ angle variety results in the simulation experiment.

As shown in Figs. 7, 8 and Table 3, the trajectory planning result of NI algorithm leads to high energy consumption and poor motion stability, so it is not recommended in engineering practice. The WNI algorithm manages to plan the trajectory with a relatively smaller energy consumption. But the constant weight matrix does not consider the joints’ pose, the jerk increases sharply when a joint approaches the limit. Compared to WNI, the weight matrix of AWNI algorithm is adaptively adjusted during the planning process according to the kinematic constraints and energy consumption of the manipulator. Evaluation functions $E_{1}$ and $E_{2}$ validate that the AWNI algorithm has less energy consumption and better motion stability than NI and WNI algorithms. All the results above have demonstrated that the performance of AWNI is superior to that of NI and WNI algorithms.

Table 3. E _1, E ₂ in cases 3, 4, 5 and 6.

Figure 7. Generalized E ₁ trend in cases 3, 4, 5 and 6.

Figure 8. Generalized E ₂ trend in cases 3, 4, 5 and 6.

6. Conclusions

This study combines Newton iteration with an adaptive weighted method and proposes a trajectory planning algorithm AWNI with consideration of energy saving and motion stability control for large redundant manipulators. Such objectives are achieved by optimizing a loss function where different weights are assigned to the working joints according to their energy consumption. The weight matrix of the AWNI algorithm is adjusted adaptively during the trajectory planning process for three benefits: (1) to prevent reciprocating motion; (2) to ensure the motion of the links is within the boundaries, and; (3) to improve performance in terms of energy consumption and motion stability. The working joints are replaced by alternative joints when they reach the limits. Two evaluation functions $E_{1}$ and $E_{2}$ are designed to compare the energy consumption and motion stability of different algorithms. The effect of threshold descriptor $\Delta \theta _{\text{threshold}}$ is studied, and the results show that the appropriate range of $\Delta \theta _{\text{threshold}}$ is between $4^{\circ}$ and $6^{\circ}$ . Trajectory planning in four directions is carried out with NI, WNI and AWNI algorithms, respectively. The results show that the AWNI algorithm outperforms NI and WNI algorithms as far as $E_{1}$ and $E_{2}$ are concerned.

The future works include improving the convergence performance of the AWNI algorithm and proposing a better generating method of the weight matrix W .