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 [Reference Tarasov36] 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. [Reference Cheah, Kawamura and Arimoto37], 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.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 [Reference Kang, Komoriya, Yokoi, Koutoku and Tanie38]. When the joint jerk is high, the motion of the joint would change abruptly, making the manipulator vibrate or even run out of control [Reference Lee, Park, Lee, Park, Jeong and Kim39, Reference Bearee and Olabi40]. 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.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.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.