Motion Planning of Ground Simulator for Space Instable Target Based on Energy Saving

Abstract

In order to achieve the high-precision motion trajectory in ground experiment of space instable target (SIT) while reducing the energy consumption of the motion simulator, a robot motion planning method based on energy saving is proposed. Observable-based ground robot motion experiment system for SIT is designed and motion planning process is illustrated. Discrete optimization mathematical model of energy consumption of motion simulator is established. The general motion form of the robot joints in ground test is given. The optimal joint path of motion simulator based on energy consumption under discontinuous singularity configuration is solved by constructing the complete energy consumption directed path and Dijkstra algorithm. An improved method by adding the global optimization algorithm is used to decouple the coupled robot joints to obtain the minimum energy consumption path under the continuous singularity configuration of the motion simulator. Simulations are carried out to verify the proposed solution. The simulation data show that total energy saving of motion simulator joints adopting the proposed method under the condition of non-singularity configuration, joints coupled motion with continuous singularity configuration, and coexistence of non-singularity path and continuous singularity path are, respectively, 72.67%, 28.24%, and 62.23%, which proves that the proposed method can meet the requirements of ground motion simulation for SIT and effectively save energy.

1. Introduction

As unstable targets in space continue to increase and become more threatening, maneuvering tasks, such as on-orbit removal, have become more and more urgent [1,2,3,4]. Space instable targets are objects to be manipulated in orbit, such as satellites and space debris, in space operation missions. The movement of SIT is complicated, and their manipulation task is also difficult [5]. In order to verify the feasibility of the space control method and optimize the control algorithm, it is necessary to conduct ground manipulation simulation experiments of SIT [6,7]. The primary task of the ground simulation test is to accurately simulate the relative spatial movement and relative attitude motion between SIT and space operation aircraft.

A serial robot with multi-DOF, widely used in areas such as grinding, polishing, and parts processing, has the advantages of a large range of pose motion and high repeat positioning accuracy. It is an effective method to achieve the given motion of SIT in ground test platform through using six-DOF industrial robot [8,9]. During simulating given space movement in the ground test, it is necessary to optimize the robot path on the premise that the simulation accuracy meets the requirements, and at the same time ensure the dynamic characteristics of the simulator. With the increasingly tense global energy situation, the optimization and control of robot energy consumption has gradually become an important goal in industrial systems [10].

Many motion planning methods and trajectory optimization algorithms have been proposed and studied to achieve smooth movement, obstacle avoidance, and flexible contact collisions in different application scenarios [11,12,13,14,15]. Interpolation, curve fitting the trajectory of the robot joint angular displacement or the cartesian pose of the robot end target position, generally including polynomial curve, Bézier spline, and NURBS curve, is adopted to smooth the path in robot motion planning to reduce acceleration shock and movement jitter [16,17]. In high-precision machining of parts, Pavel [18] adopts the analytic geometry method on the basis of the symmetries in the Euclidean plane and achieves a trajectory error of less than 5% in experiments, which proves the feasibility of generating complex processing motion for robot. Modelled predictive control approach, based on vision system and linear interpolation, is applied for the task of ball-catching to generate robot real-time trajectory [19]. Aiming at the multi-path optimization issue, R.A. [20] uses genetic algorithms to search for the shortest path without collision, which connects preset points in the robot’s task space. Chaos enhanced accelerated particle swarm optimization (CAPSO) is developed by Taghavifar [21] to plan the optimal path of a wheeled robot, and ensures the shortest path from the starting point to the target position by avoiding collision with static and dynamic obstacles. The improved algorithm combining Dijkstra algorithm or A* algorithm with other algorithms can effectively perform the shortest path search, which has the advantages of a small calculation and fast convergence speed, and is used in varieties of scenarios of robot operations [22,23,24,25,26].

In terms of optimal motion planning combined with minimal energy consumption and robot operation tasks, many implementation methods have emerged. One DoF mechatronic system is studied by Giovanni to reduce the energy consumption and the minimum energy consumption conditions have been found in a closed form taking into account the chance of recovering the braking energy [27]. A robot configuration selection method based on the lowest power consumption is studied by Abdullah and verified through a straight-line scenario and a square-path scenario, which achieve the goal of optimizing the calculated joint configuration [28]. Luo uses Chebyshev interpolation points and direct iteration method to plan the trajectory of industry robotic manipulators for energy minimization, and the method is applied to the three-link plane mechanism, which obtained optimal joint angles under the premise that the start and end points of the movement are known [29]. A task-related analysis is used by Scalera to enhance the energy efficiency of a 4-DOF parallel robot [14]. Carabin adopts the trajectory planning mean, which is on the basis of electromagnetic field model and the derivation of the energy formulation, to realize the minimum-energy consumption of the Cartesian robot [30]. Bitar proposed a method of obtaining energy-optimized trajectory planning while managing obstacle constraints for ASVs [31]. The enhanced bacterial foraging optimization algorithm is researched by Abbas and applied to path planning in two-dimensional motion space [32]. An artificial bee colony algorithm and a genetic algorithm is studied to optimize the working path length and welding time of the welding robot, which minimizes the total amount of robot joint movement [33,34]. Sathiya [35] takes the execution time and execution tasks of mobile robots as the optimization goals, and proposes the application of evolutionary algorithms to achieve multi-objective optimization in trajectory planning. Under the conditions of kinematics and dynamics constraints, Amruta [36] presents the EMOTLBO approach to get the best joint motion that defines the weld path, which effectively enhances the efficiency of the robot in operation tasks. Wang [37] applies the improved whale optimization algorithm to obtain the constant velocity motion of the robot end effector, which improves the processing efficiency and quality of the grinding task. Under the working conditions of the power generation mode and the energy exchange, Christian [38] proposes an energy-based robot model based on B-spline functions and realizes the optimization of energy between different joints. Aiming at the robot PTP motion planning without collision, Francisco [39] uses a third-order polynomial method to fit the joint space motion, which obtains an approximate optimal time trajectory and considers the constraint of energy consumed.

Inspired by the above-mentioned research works and considering the particularity of ground simulation of space instability targets, there are still several issues that need to be studied and solved. Firstly, it is hard to completely avoid robot singularity configuration due to the large coverage of the motion range in ground simulation experiment of SIT [40,41], which makes the motion optimization with singular position particularly important. Secondly, although the intelligent optimization algorithm based on the global search is sufficiently robust to acquire the optimized path, the optimization result is more likely to converge prematurely in the process of processing multi-objective optimization [42], which will cause the problem of difficulty in motion reproduction and simulation validity when the robot exists with different error sources. Furthermore, there may be multiple optimal motion paths or cases where the sub-optimal path is better than the optimal path on certain evaluation functions and the path search algorithm needs to be improved accordingly. Finally, the spin motion of SIT on orbit is not restricted, that is, it can be rotated infinitely, in contrast to the limited simulation of rotational motion subjected to robot structure. How to achieve the simulation of spin motion as much as possible with minimal energy consumption through the combined motion and motion optimization of the robot joints is also an issue of motion planning of the SIT movement simulation robot, especially when singularity causes multi-axis coupling.

Focusing on the characteristics of ground motion simulation of SIT and the mentioned challenges above, the study proposes an optimization method of simulator motion path including a singularity configuration based on energy saving. Ground motion planning of SIT is performed through motion mapping and path optimization of motion simulation robots. The local optimal solution is avoided through path search algorithm on the basis of constructing a complete directional motion path of energy consumption. The versatility of ground motion simulation of SIT is achieved using the presented approach and it enhances the authenticity of the ground experiment and reduces the energy consumption of the ground system.

The rest of the paper is organized as follows: ground motion planning method of SIT and motion optimization model of SITMSR are described in Section 2 and Section 3, respectively; solution to the optimal joint trajectory of SITMSR and results obtained by a series of simulations are presented in Section 4 and Section 5, respectively; and conclusions are drawn in Section 6.

2. Ground Motion Planning Method of SIT

2.1. Ground Motion Simulation State Description of SIT

The motion of SIT in space is in the free-floating state, which can be decomposed into attitude movement and centroid translation movement. The attitude general form of SIT is spin movement around the axis of maximum inertia and the nutation angle will change with the pose controllability in the close-range orbit and attitude tracking stage and the final approach stage according to space dynamics [4]. Position movement is the three-dimensional space motion obtained after task-level trajectory planning. Therefore, the movement of the SIT is a motion of 6-DOF in space. The motion simulation state of SIT in the ground experiment is described in Figure 1.

Where ${\mathrm{O}}_{\mathrm{GS}}{\mathrm{X}}_{\mathrm{GS}}{\mathrm{Y}}_{\mathrm{GS}}{\mathrm{Z}}_{\mathrm{GS}}$ is the inertial coordinate system of the ground experiment system, ${\mathrm{O}}_{\mathrm{IT}}{\mathrm{X}}_{\mathrm{IT}}{\mathrm{Y}}_{\mathrm{IT}}{\mathrm{Z}}_{\mathrm{IT}}$ is the framework fixed in SIT, ${\overrightarrow{r}}_{\mathrm{GS}}$ denotes centroid vector of SIT in ${\mathrm{O}}_{\mathrm{GS}}{\mathrm{X}}_{\mathrm{GS}}{\mathrm{Y}}_{\mathrm{GS}}{\mathrm{Z}}_{\mathrm{GS}}$.${\phi }_{\mathrm{IT}}$, ${\theta }_{\mathrm{IT}}$ and ${\psi }_{\mathrm{IT}}$ are precession angle, nutation angle, and spin angle defined in ${\mathrm{O}}_{\mathrm{IT}}{\mathrm{X}}_{\mathrm{IT}}{\mathrm{Y}}_{\mathrm{IT}}{\mathrm{Z}}_{\mathrm{IT}}$.

To ensure the effectiveness of the ground test, the angular velocity and translation velocity are required to meet the given precision. The expressions of ground simulation motion accuracy of SIT are as follows:

$$\left|{}^{\mathrm{GS}}V_{\mathrm{ITa}}-{}^{\mathrm{GS}}\dot{r}_{\mathrm{IT}}\right|\le {\mathbf{\gamma }}_{\mathrm{v}}$$

(1)

$$\left|{}^{\mathrm{IT}}\omega _{\mathrm{ITa}}-{}^{\mathrm{IT}}\omega _{\mathrm{IT}}\right|\le {\mathbf{\gamma }}_{\mathsf{\omega }}$$

(2)

where ${}^{\mathrm{GS}}V_{\mathrm{ITa}}$ and ${}^{\mathrm{GS}}\dot{r}_{\mathrm{IT}}$ are the actual velocity and the given velocity of the mass center of SIT defined in ${\mathrm{O}}_{\mathrm{GS}}{\mathrm{X}}_{\mathrm{GS}}{\mathrm{Y}}_{\mathrm{GS}}{\mathrm{Z}}_{\mathrm{GS}}$ during the ground simulation experiment, respectively. ${}^{\mathrm{IT}}\omega _{\mathrm{ITa}}$ and ${}^{\mathrm{IT}}\omega _{\mathrm{IT}}$ are the actual angular velocity and the given angular velocity. ${\mathbf{\gamma }}_{\mathrm{v}}$ and ${\mathbf{\gamma }}_{\mathsf{\omega }}$ are the given velocity error and angular velocity error.

The observable angular velocity in the ground test can be obtained by projecting Equation (2) in ${\mathrm{O}}_{\mathrm{GS}}{\mathrm{X}}_{\mathrm{GS}}{\mathrm{Y}}_{\mathrm{GS}}{\mathrm{Z}}_{\mathrm{GS}}$. Taking ${}^{\mathrm{IT}}\mathbf{\omega }_{\mathrm{ITa}}$ as an example, we get:

$${}_{\mathrm{GS}}{}^{\mathrm{IT}}\mathbf{\omega }_{\mathrm{ITa}}={}_{\mathrm{GS}}{}^{\mathrm{IT}}T{T}_{\mathsf{\phi }\mathsf{\theta }}{\dot{R}}_{\mathrm{Eu}}$$

(3)

where ${}_{\mathrm{GS}}{}^{\mathrm{IT}}\mathbf{\omega }_{\mathrm{ITa}}$ is the projection of ${}^{\mathrm{IT}}\mathbf{\omega }_{\mathrm{ITa}}$ in ${\mathrm{O}}_{\mathrm{GS}}{\mathrm{X}}_{\mathrm{GS}}{\mathrm{Y}}_{\mathrm{GS}}{\mathrm{Z}}_{\mathrm{GS}}$, ${}_{\mathrm{GS}}{}^{\mathrm{IT}}T$ is the homogeneous transformation matrix from ${\mathrm{O}}_{\mathrm{IT}}{\mathrm{X}}_{\mathrm{IT}}{\mathrm{Y}}_{\mathrm{IT}}{\mathrm{Z}}_{\mathrm{IT}}$ to ${\mathrm{O}}_{\mathrm{GS}}{\mathrm{X}}_{\mathrm{GS}}{\mathrm{Y}}_{\mathrm{GS}}{\mathrm{Z}}_{\mathrm{GS}}$, $T_{\mathsf{\phi }\mathsf{\theta }}$ is the angular velocity projection matrix, and ${\dot{R}}_{\mathrm{Eu}}={\left[\begin{array}{ccc}{\dot{\phi }}_{\mathrm{IT}}& {\dot{\theta }}_{\mathrm{IT}}& {\dot{\psi }}_{\mathrm{IT}}\end{array}\right]}^T$.

Through the analysis of the simulation state of the ground motion of SIT, it can be seen that the state exists where projection of the centroid velocity and angular velocity of the ground motion in ${\mathrm{O}}_{\mathrm{GS}}{\mathrm{X}}_{\mathrm{GS}}{\mathrm{Y}}_{\mathrm{GS}}{\mathrm{Z}}_{\mathrm{GS}}$ is not zero, which means 6-DOF motion of SIT should be simulated in the ground test. In other words, velocity motion and angular velocity motion of SIT in the ground test possess the generality of space motion.

2.2. Composition and Motion Planning Process of SITMSRS

Compared with parallel robot and cooperative manipulator, 6-DOF industrial robots have the characteristics of large motion range, high load capacity, and high motion accuracy. At the same time, they have the advantages of small envelope space, high technology maturity, and low cost over a truss robot. Therefore, the 6-DOF industrial robot system is chosen to simulate the motion of SIT in the ground experiment. The space instable target motion simulation robot system (SITMSRS), mainly composed of a 6-DOF industrial robot (SITMSR), the space instable target simulator (SITS), a motion control computer, and electrical system, is designed to achieve the planned positions and attitudes given above. SITS is installed on the end flange of SITMSR, which equipped with capture ring, on-board analogue elements, and six-axis force/torque sensor. The capture ring is the capture object for the space manipulator to carry out takeover tasks during the ground test. The capture ring and on-board analogue elements are used as recognition features for motion parameter estimation and 3D reconstruction. The six-axis force/torque sensor can measure the forces and moments applied on SITS. SITS need to have the same properties as the real SIT, including the width and height of the SIT, the geometric size and material of the capture ring, etc. The motion control computer is used to realize the planning and optimization of the motion trajectory and interact with the robot system. The composition of designed SITMSRS is shown in Figure 2.

The spatial movement of the centroid of the SIT and attitude movement, including precession, nutation, and spin planned by the motion control computer, can be achieved by joint compound motion of SITMSR to form the general motion form of SIT. It is hard to control angular velocity of SITMSR joints directly. Nevertheless, the spatial position ${}^{\mathrm{GS}}r_{\mathrm{IT}}$ and attitude ${}^{\mathrm{GS}}P_{\mathrm{IT}}$ can be obtained by integrating the velocity and angular velocity of the SIT in the ground simulation experiment. Thus, the ground motion of the SIT is simulated by controlling the angular displacement motion of the SITMSR joints.

Since there are multiple solutions for the angular displacement of robot joints under the same position and pose of the SIT, it is necessary to optimize the joint movement of SITMSR to save energy consumption. The input to robot motion control is based on discrete point planning, and when the number of discrete points N_dis is constant, the motion planning of the SIT is transformed into the path planning and optimization problem of the SITMSR joint motions. The motion planning flowchart of SITMSRS is indicated in Figure 3.

From Figure 3, the joint path optimization of the SITMSR based on energy saving is the crucial part of the above motion planning process and needs to be further studied in detail.

3. Motion Optimization Model of the SITMSR Based on Energy Saving

3.1. Mathematical Model of Joint Trajectory of the SITMSR

Define q_i (i = 1, 2, ⋯, 6 as generalized coordinate of SITMSR and SITMSR kinematics is modeled by the D-H method. Suppose ${\mathrm{OX}}_0{\mathrm{Y}}_0{\mathrm{Z}}_0$ is the base coordinate system and ${\mathrm{OX}}_i{\mathrm{Y}}_i{\mathrm{Z}}_i$ is the joint coordinate system. The definition of DH parameters and joint coordinate systems of SITMSR are shown in Figure 4.

Suppose the centroid coordinates of SIT is $r_c$ and it can be calculated as follows:

$${}_6{}^{0}T=T_{\mathrm{s}}{}_{\mathrm{s}}{}^{6}T{}^{-1}$$

(4)

$${}_6{}^{0}T=f\left({\mathrm{q}}_1,{\mathrm{q}}_2,{\mathrm{q}}_3,{\mathrm{q}}_4,{\mathrm{q}}_5,{\mathrm{q}}_6,{\mathrm{a}}_1,{\mathrm{a}}_2,{\mathrm{a}}_3,{\mathrm{d}}_1,{\mathrm{d}}_4,{\alpha }_1,{\alpha }_3,{\alpha }_4,{\alpha }_5\right)$$

(5)

$${\mathbf{T}}_{\mathrm{s}}=\left[\begin{array}{cc}{}^{\mathrm{GS}}\mathbf{P}_{\mathrm{IT}}& {}^{\mathrm{GS}}\mathbf{r}_{\mathrm{IT}}\\ 0& 1\end{array}\right]$$

(6)

$${}_{\mathrm{s}}{}^6\mathbf{T}=\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{r}}_{\mathrm{c}}\\ 0& 1\end{array}\right]$$

(7)

where ${}_6{}^{0}T$ is the homogeneous transformation matrix of SITMSR and ${\mathbf{T}}_{\mathrm{s}}$ is the position and attitude matrix planned of SIT in the ground experiment. a₁, a₂, a₃, d₁, and d₄ are the geometric parameters of the coordinate transformation and α₁, α₃, α₄, and α₅ are the rotation angles of the joint coordinate system using D-H method as shown in Figure 4.

The value of q_i, which is also the joint angular displacement of each axis of the SITMSR, can be obtained by solving Equations (4)–(7).

The motion control of the SITMSR is based on the displacement control of the discrete points in the ground experiment. The angular displacement is discretized based on the dispersing of the given trajectory. The approach of polynomial fitting is used to carry out the continuity of the discrete path of the SITMSRS.

3.2. Discrete Optimization Mathematical Model of Energy Consumption

SITMSR need to accurately simulate the given motion of SIT firstly, which meet the given motion simulation error. Secondly, it is necessary to ensure the total energy consumption of the SITMSR axes is low during the simulating trajectory. Additionally, in order to avoid violent measurement data jitter of the six-axis force/torque sensor measurement values, the smoothness of the movement of the SITS is also required. Based on the above considerations, the objective function in the mathematical model of motion optimization of SITMSR can be described as follows:

$$\min \mathbf{F}=\left[\begin{array}{c}{f}_1\\ f_2\end{array}\right]=\left[\begin{array}{c}{N}_{dis}\\ \frac{t_p}{N_{dis}-1}{\displaystyle \sum _{i=1}^{N_q}{\displaystyle \sum _{j=1}^{N_{dis}}\left|T_{qi,j}{\dot{q}}_{i,j}\right|}}\end{array}\right]$$

(8)

where $t_p$ is the given motion simulation time, ${\dot{q}}_{i,j}$ is the joint angular velocity, N_q is the number of SITMSR joint axes, N_dis is the number of discrete points of a given trajectory of SITS,${\dot{q}}_{i,j}$ is the angular velocity of q_j at the discrete point j, and $T_{qi,j}$ is the joint drive torque when the angular displacement is q_i,j.

The constraint conditions corresponding to above objective function are:

$$\mathrm{s}.\mathrm{t}.\{\begin{array}{c}\left|{}^{\mathrm{GS}}V_{\mathrm{ITa}}-{}^{\mathrm{GS}}\dot{r}_{\mathrm{IT}}\right|\le {\mathbf{\gamma }}_{\mathrm{v}}\\ \left|{}^{\mathrm{IT}}\omega _{\mathrm{ITa}}-{}^{\mathrm{IT}}\omega _{\mathrm{IT}}\right|\le {\mathbf{\gamma }}_{\mathsf{\omega }}\\ \left|\frac{{}^{\mathrm{IT}}\mathbf{M}_{\mathrm{ITa}}-{}^{\mathrm{IT}}\mathbf{M}_{\mathrm{IT}}}{{}^{\mathrm{IT}}\mathbf{M}_{\mathrm{IT}}}\right|\le {\mathbf{\gamma }}_{\mathrm{M}}\\ \begin{array}{c}{q}_{i,j}\in \left[q_{imin},q_{imax}\right]\\ \frac{t_p\left|q_{i,j}-q_{i,j-1}\right|}{N_{traj}-1}\le {\dot{q}}_{imax}\\ \frac{t_p{\dot{q}}_{i,j}}{N_{traj}-1}\le {\ddot{q}}_{imax}\end{array}\end{array}$$

(9)

where ${}^{\mathrm{IT}}\mathbf{M}_{\mathrm{ITa}}$ and ${}^{\mathrm{IT}}\mathbf{M}_{\mathrm{IT}}$ are the values of the six-axis force/torque sensor in SITS when the trajectory is given and when the trajectory is planned, respectively. ${\mathbf{\gamma }}_{\mathrm{M}}$ is the allowable value of relative error between ${}^{\mathrm{IT}}\mathbf{M}_{\mathrm{ITa}}$ and ${}^{\mathrm{IT}}\mathbf{M}_{\mathrm{IT}}$, $N_{traj}$ is the number of discrete points from the joint path to the joint trajectory. ${\dot{q}}_{imax}$ and ${\ddot{q}}_{imax}$ are the maximum allowable values of the joint angular velocity and the angular acceleration, respectively.

SITMSR dynamics equations in discrete form using Newton–Euler algorithm is presented in Equation (10).

$${\mathbf{T}}_{\mathrm{q}\mathrm{i},\mathrm{j}}={}_{\mathrm{i}+1}{}^{\mathrm{i}}\mathbf{T}\times {\mathbf{T}}_{\mathrm{q}\mathrm{i}+1,\mathrm{j}}-{\mathbf{f}}_{\mathrm{i},\mathrm{j}}\times {\mathbf{r}}_{\mathrm{i}-1,\mathrm{c}\mathrm{i}}+{}_{\mathrm{i}+1}{}^{\mathrm{i}}\mathbf{T}\times {\mathbf{f}}_{\mathrm{i}+1,\mathrm{j}}\times {\mathbf{r}}_{\mathrm{i},\mathrm{c}\mathrm{i}}+{\mathbf{\omega }}_{\mathrm{i},\mathrm{j}}\times \left({\mathbf{I}}_{\mathrm{i}}\times {\mathbf{\omega }}_{\mathrm{i},\mathrm{j}}\right)+{\mathbf{I}}_{\mathrm{i}}\times {\mathbf{a}}_{\mathrm{i},\mathrm{j}}$$

(10)

where ${\mathbf{f}}_{\mathrm{i},\mathrm{j}}$ is the interaction force between link i and link i + 1 at the discrete point j of the trajectory, ${\mathbf{r}}_{\mathrm{i},\mathrm{c}\mathrm{i}}$ is the position vector of the gravity center of link i in the framework of OX_iY_iZ_i, ${\mathbf{\omega }}_{\mathrm{i},\mathrm{j}}$ is the angular velocity of the link i, ${\mathbf{I}}_{\mathrm{i}}$ is the inertial tensor matrix and ${\mathbf{a}}_{\mathrm{i},\mathrm{j}}$ is the linear acceleration at the end point of the link i.

SITMSR is considered as rigid bodies in the process of dynamic modeling, and the effects of viscous force are ignored. The measurements for six-axis force/torque can be obtained by solving the dynamics model of SITS based on Newton–Euler algorithm. The expressions of ${\mathbf{f}}_{\mathrm{i},\mathrm{j}}$, ${\mathbf{r}}_{\mathrm{i},\mathrm{c}\mathrm{i}}$, ${\mathbf{\omega }}_{\mathrm{i},\mathrm{j}}$, and ${\mathbf{a}}_{\mathrm{i},\mathrm{j}}$ are obtained in [28]. ${\mathbf{f}}_{7,\mathrm{j}}$ and ${\mathbf{\tau }}_{7,\mathrm{j}}$ in the framework of OX₆Y₆Z₆ can be written as:

$$\left({\mathbf{f}}_{7,\mathrm{j}},{\mathbf{\tau }}_{7,\mathrm{j}}\right)=\left[\left(-G_{\mathrm{SITS}},0,0\right),-{\mathbf{r}}_{\mathrm{c}}\times {\mathbf{f}}_{7,\mathrm{j}}\right]$$

(11)

where G_SITS is the gravity of SITS.

4. Solution to the Optimal Joint Trajectory of SITMSR

4.1. The General Form of Joint Motion Trajectories

For the planned motion of SITS is complex and diverse, it leads to the generality of the motion of SITMSR. As a result, the compound situations of singularity configuration and non-singularity configuration of SITMSR should be considered. The singularity can be judged by the full rank of Jacobian matrix, which can also be quickly determined by using three singular factors extracted from the joint expressions of the Jacobian matrix. Another method is to judge whether there is a problem of DOF degradation. The general forms of the SITMSR joint motion trajectories are seen in Figure 5.

To facilitate the solution of the optimal trajectory, the joint movement above can be segmented into paths with non-singularity configuration (Stages 2 and 4 in Figure 5) and paths with singularity configuration. Paths with singularity configuration are divided into trajectory with multipoint singular position (Stage 1 in Figure 5) and trajectory with continuous singular position (Stage 3 in Figure 5). Although joint q_i and q_k have constraints, respectively, even under the premise of coupled angular displacement constraints, there are infinite sets of solutions for their joint angular displacements. If the robot continues to run in this condition, then the robot is in the continuous singularity configuration as shown in Stage 3. We solved each segment of the optimal path separately, and, finally, merged the optimal solutions of the multiple segments to obtain the optimal trajectory of SITMSR under the general path form of SIT in the ground experiment.

4.2. Optimal Selection of N_dis

From the motion planning of SITMSRS and motion optimization model based on energy saving, it can be seen that N_dis is closely related to the simulation accuracy of a given motion trajectory. The ground motion simulation error of SIT is large when N_dis is small, which, obviously, does not meet the simulation requirements. This is because when the motion of SITS is determined, the fewer the number of discrete points, the greater the deviation of the motion trajectory between two adjacent discrete points, and the greater the error between the corresponding velocity and angular velocity and the ideal state. The trajectory simulation precision will continue to improve as N_dis continues to increase and it is paradoxical that the total number of paths will also increase exponentially leading to lower efficiency in solving the optimal trajectory.

Motion planning of the given trajectory of SITS can be adopted to select the optimized N_dis, the approach of polynomial fitting is used to carry out the continuity of the discrete path of SITMSRS. Optimal selection process of N_dis is depicted in Figure 6. N_d0 represents iteration initial value of N_dis.

The optimization flow above can be applied to the case where there are multiple discontinuous singular positions of the path to be optimized of SITMSRS. Figure 7 elaborates the motion optimization approach in the case of non-continuous singular path points. Here, $q_{{}_{Di,1}}^{*}$, $q_{{}_{Di,N}}^{*}$, and $q_{{}_{Di,N_{dis}}}^{*}$ are true values of path initial singularity, singularity at discrete point N, and path terminal singularity, respectively.

Approximate solutions of joint angular displacements at finite intermittent singular points can be achieved by choosing the appropriate N_dis through the above approach, which reduces the complexity of path optimization at a certain level.

4.3. Solution to the Optimal Path with Discontinuous Singularity Configuration

4.3.1. Complete Path Solution Space Generation of Joints Power

In the compound movement of each joint of SITMSR to simulate the planned motion of SIT, there are several sets of solutions at each discrete point to satisfy a given position and attitude. Complete energy consumption path solution space is needed to generate by expanding the solution of each joint at each discrete path point into a full solution matrix. Figure 8 illustrates the generation process of complete path solution space of joint power.

$q_{i,N_{dis},mi,ni}$ and $q_{N_{dis},m,n}$ in Figure 8 represent the (10mi+ni)-th angular displacement solution and solution vector when q_i at path discrete point N_dis, respectively. ${\mathbf{T}}_{N_{dis},m,n}$ and ${\mathbf{P}}_{N_{dis},m,n}$ are the corresponding joint driving torque and joint energy consumption solution vectors, respectively. mi, ni, and ${\mathbf{P}}_{N_{dis},m,n}$ are calculated as follows:

$$mi=\left(N_{isol}-ni\right)/10$$

(12)

$$ni=\mathrm{mod}\left(N_{isol},10\right)$$

(13)

$$m=\max \left(mi\right)$$

(14)

$$n=\mathrm{mod}\left(N_{sol},10\right)$$

(15)

$$N_{sol}=\max \left(N_{isol}\right)$$

(16)

$${\mathbf{P}}_{N_{dis},m,n}={\left(\begin{array}{cccccc}{\mathbf{P}}_{1,N_{dis},m,n}& {\mathbf{P}}_{2,N_{dis},m,n}& {\mathbf{P}}_{3,N_{dis},m,n}& {\mathbf{P}}_{4,N_{dis},m,n}& {\mathbf{P}}_{5,N_{dis},m,n}& {\mathbf{P}}_{6,N_{dis},m,n}\end{array}\right)}^{\mathrm{T}}$$

(17)

where N_isol is the number of solutions of $q_{i,j}$ and N_sol is the maximum value of N_isol at all path discrete points of SITMSR.

4.3.2. Directed Motion Paths Construction of Energy Consumption

Constructing a networked directed non-circular motion paths of joints energy consumption of SITMSR, consisting of path nodes, directed paths, and path weights, is carried out to implement the path optimization of SITMSR on the basis of generating complete path solution space of joints power. The path node of energy consumption takes each vector of the path solution space as a node, and all nodes in the path are numbered in order. In addition, the start node and the end node are added to form a complete path to be optimized. The average of the product of the path discrete time and the sum of the energy consumption is used as the weight of the path between the two nodes except the start node and the end node. The nodes in the first row have one path weight to the initial node and N_sol path weights to other nodes. The node in row N_dis has one path weight to the destination node and N_sol path weights to other nodes. Aside from the first and last two rows, the number of input path weights and output path weights of other nodes are both N_sol. Figure 9 constructs the directed energy consumption path without continuous singularity of SITMSR.

4.3.3. Solution to Optimal Energy Consumption Path with Discontinuous Singularity

From the building of directed energy consumption path, solving the optimal path of SITMSR is searching the shortest path starting from the initial node to the end node, which meet the requirements of Equations (8) and (9). The node vectors and the corresponding weight vectors need to be established first, and then get the shortest path from the created directed motion paths through the Dijkstra algorithm. Other shortest path or second-shortest path can be achieved by repeatedly calling the Dijkstra algorithm on the basis of modifying the weight vector. The solution to the optimal path based on energy saving without continuous singularity is described as follows:

Step 1: Calculate the starting node vector ${\mathbf{S}}_{\mathrm{I}\mathrm{n}\mathrm{i}}$ from node (N_disN_sol+1) to the N_sol nodes in the first row, end node vector ${\mathbf{t}}_{\mathrm{I}\mathrm{n}\mathrm{i}}$, and corresponding path weight vector ${\mathbf{L}}_{\mathrm{I}\mathrm{n}\mathrm{i}}$.

Step 2: Compute the starting node vector ${\mathbf{S}}_{\mathrm{I}\mathrm{n}\mathrm{i}\mathrm{k}}$ between node k located from row one to row (N_dis − 1) and the next row of nodes, end node vector ${\mathbf{t}}_{\mathrm{I}\mathrm{n}\mathrm{i}\mathrm{k}}$, and weight vector ${\mathbf{L}}_k$. Here, $k=1,2,\cdots ,\left(N_{dis}-1\right)N_{sol}$.

The expression of ${\mathbf{S}}_{\mathrm{I}\mathrm{n}\mathrm{i}\mathrm{k}}$, ${\mathbf{t}}_{\mathrm{I}\mathrm{n}\mathrm{i}\mathrm{k}}$, and ${\mathbf{L}}_k$ are given as follows:

$${\mathbf{S}}_{\mathrm{I}\mathrm{n}\mathrm{i}\mathrm{k}}=k{\mathbf{I}}_{1\times N_{sol}}$$

(18)

$$m_{1k}=\left(k-1-n_k\right)/N_{sol}+1$$

(19)

$$n_{1k}=\mathrm{mod}\left(k-1,N_{sol}\right)$$

(20)

$$m_{2k}=\left[k-\left(m_{1k}-1\right)N_{sol}-n_{2k}\right]/10$$

(21)

$$n_{2k}=\mathrm{mod}\left[k-\left(m_{1k}-1\right)N_{sol},10\right]$$

(22)

$$n_{k1}=\mathrm{mod}\left(k,N_{sol}\right)$$

(23)

$$t_{Inik}=\left(m_{1k}{N}_{sol}+1,m_{1k}{N}_{sol}+2,\cdots ,\left(m_{1k}+1\right)N_{sol}\right)$$

(24)

$${\mathbf{L}}_k=\frac{t_p}{2\left(N_{dis}-1\right)}\left(\left|{\mathbf{P}}_{m_{1k},m_{2k},n_{2k}}\right|{\mathbf{I}}_{1\times n}+\left|\left({\mathbf{P}}_{m_{1k}+1,0,1},{\mathbf{P}}_{m_{1k}+1,0,2},\cdots ,{\mathbf{P}}_{m_{1k}+1,m,n}\right)\right|\right)$$

(25)

In Equations (18)–(25), the definition of $n_{1k}$ is to obtain path discrete point sequence number $m_{1k}$ where the node k is located. $n_{k1}$ is introduced to get the row number and the column number of the solution vector where node k is located.

Step 3: Obtain the starting node vector ${\mathbf{S}}_{\mathrm{E}\mathrm{n}\mathrm{d}}$ from the N_sol nodes in row N_dis to node $\left(N_{dis}{N}_{sol}+2\right)$, end node vector ${\mathbf{t}}_{\mathrm{E}\mathrm{n}\mathrm{d}}$, and corresponding path weight vector ${\mathbf{L}}_{\mathrm{E}\mathrm{n}\mathrm{d}}$.

In Steps 1 and 3, node $\left(N_{dis}{N}_{sol}+1\right)$ and node $\left(N_{dis}{N}_{sol}+2\right)$ are artificially added nodes to construct the solution path to minimum energy consumption, which do not affect the solution of the optimal path. Consequently, the path weight between the initial node and the end node, and other directly connected nodes, can be set as a constant; here we take its value as 1.${\mathbf{S}}_{\mathrm{I}\mathrm{n}\mathrm{i}}$, ${\mathbf{t}}_{\mathrm{I}\mathrm{n}\mathrm{i}}$, ${\mathbf{L}}_{\mathrm{I}\mathrm{n}\mathrm{i}}$, ${\mathbf{S}}_{\mathrm{E}\mathrm{n}\mathrm{d}}$, ${\mathbf{t}}_{\mathrm{E}\mathrm{n}\mathrm{d}}$, and ${\mathbf{L}}_{\mathrm{E}\mathrm{n}\mathrm{d}}$ can be presented as follows:

$${\mathbf{S}}_{\mathrm{I}\mathrm{n}\mathrm{i}}=\left(N_{dis}{N}_{sol}+1\right){\mathbf{I}}_{1\times N_{sol}}$$

(26)

$${\mathbf{t}}_{\mathrm{I}\mathrm{n}\mathrm{i}}=\left(1,2,\cdots ,N_{sol}\right)$$

(27)

$${\mathbf{L}}_{\mathrm{I}\mathrm{n}\mathrm{i}}={\mathbf{L}}_{\mathrm{E}\mathrm{n}\mathrm{d}}={\mathbf{I}}_{1\times N_{sol}}$$

(28)

$${\mathbf{S}}_{\mathrm{E}\mathrm{n}\mathrm{d}}=\left[\left(N_{dis}-1\right)N_{sol}+1,\left(N_{dis}-1\right)N_{sol}+2,\cdots ,N_{dis}{N}_{sol}\right]$$

(29)

$${\mathbf{t}}_{\mathrm{E}\mathrm{n}\mathrm{d}}=\left(N_{dis}{N}_{sol}+2\right)$$

(30)

Step 4: Form the complete start node vector ${\mathbf{S}}_{\mathrm{I}\mathrm{n}\mathrm{t}}$, end node vector ${\mathbf{t}}_{\mathrm{I}\mathrm{n}\mathrm{t}}$, and weight vector ${\mathbf{L}}_{\mathrm{I}\mathrm{n}\mathrm{t}}$ of SITMSR motion path to be optimized.

Step 5: Use the Dijkstra algorithm to solve one shortest path ${\pi }_1$ from node $\left(N_{dis}{N}_{sol}+1\right)$ to node $\left(N_{dis}{N}_{sol}+2\right)$, and acquire the node number vector $\mathbf{O}\mathbf{p}\mathbf{t}\mathbf{N}{\mathbf{o}}_{\mathrm{o}\mathrm{p}1}$, path weight vector ${\mathbf{L}}_{\mathrm{o}\mathrm{p}1}$, and path value $f_{2\mathrm{o}\mathrm{p}1}$ of ${\pi }_1$.

Step 6: Traverse the path weights of other nodes except the node paths related to the initial node and the end node of ${\pi }_1$ in Step 5 one by one, and individually assign a new weight of $+\infty$. Update the path weight vector ${\mathbf{L}}_{\mathrm{I}\mathrm{n}\mathrm{t}}$ before solving the new directional motion path and remove duplicate path solutions to achieve the vector of the node solution number, the path weight vector, and the path value of the other shortest paths.

Step 7: Reverse coding to acquire SITMSR joint angle matrix ${\mathbf{q}}_{N,m_{2k},n_{2k}}$ which corresponds to the node number vector $\mathbf{O}\mathbf{p}\mathbf{t}\mathbf{N}{\mathbf{o}}_{\mathrm{o}\mathrm{p}M}$ in the shortest motion path ${\pi }_M\left(M=1,2,\cdots \right)$. Determine the optimal energy consumption path by comparing the maximum joint jitter. Here it is assumed that the optimal path is ${\pi }_{\mathrm{O}\mathrm{p}\mathrm{t}}$, and the node number vector, the path weight vector, and the path value are $\mathbf{O}\mathbf{p}\mathbf{t}\mathbf{N}{\mathbf{o}}_{\mathrm{o}\mathrm{p}\mathrm{t}}$, $L_{\mathrm{o}\mathrm{p}\mathrm{t}}$, and $f_{2\mathrm{o}\mathrm{p}\mathrm{t}}$, respectively.

The values of the objective functions can be determined through Step 7 by:

$$\min f_2=f_{2\mathrm{o}\mathrm{p}\mathrm{t}}-2$$

(31)

Step 8: Extract each path node except node $\left(N_{dis}{N}_{sol}+1\right)$ and node $\left(N_{dis}{N}_{sol}+2\right)$ of $\mathbf{O}\mathbf{p}\mathbf{t}\mathbf{N}{\mathbf{o}}_{\mathrm{o}\mathrm{p}\mathrm{t}}$, and get the $q_{i,j}$ of SITMSR according to the directional motion path and the coding rules in the complete solution space and motion simulation errors by the first three constraints in Equation (9).

4.4. Solution to the Optimal Singular Path

Since the finite number of discontinuous singular points can be solved by trajectory planning, the main focus here is on the condition of the continuous singularity configuration. When SITMSR is in the singular motion path, theoretically, there are countless solutions because of the coupled motion of multi-axis motion. Singularity configuration makes it difficult to directly solve the optimal motion path based on energy saving by way of the directed node path. The reason is that when there is no singular position, the path weight between the nodes of the SITMSR path network is unique. However, there are countless path weights between the nodes at each discrete location in the singular path.

When solving the optimal energy consumption path of SITMSR with multipoint singular position and continuous singular position in given trajectory of SITS, the joint angular displacements of the singular position at the discrete point N can be set as unknown variables to be optimized. Assume that the joint angle of the coupled motion axis caused by the singularity configuration is $q_{Di,N}\left(2\le Di\le 6\right)$, and, thus, the total number of additional parameters to be optimized in the complete path solution space is $N_{dis}Di$. The constraints of $q_{Di,N}$ are:

$${\displaystyle \sum q_{Di,N}}=Q_{1,N,mi,ni},\prime Q_{1,N,mi,ni}\in \left[{\displaystyle \sum q_{Dimin}},{\displaystyle \sum q_{Dimax}}\right]$$

(32)

where $Q_{1,N,mi,ni}$ is the (10mi+ni)-th solution of $q_{Di,N}$ at path discrete point N.

It is hard to solve the above situation by directly using the path optimization method under the condition of non-continuous singularity position. An approximate method is to assume that the angular displacements of the coupled joints have motion constraints, and then decouple the joint motions at singular positions. The minimum energy consumption path is obtained by adjusting the coupling coefficient and using the solution algorithm for non-continuous singular configuration. The linear relationship of the coupled joints can be expressed as:

$$q_{Di,N}={\lambda }_i{q}_{D1,N}$$

(33)

where ${\lambda }_i$ is the coupling factors of $q_{Di,N}$.

Another feasible method is to take the objective function, constraint function, and constraint function of the coupling joint as the fitness function of the intelligent optimization algorithm to solve the above problems. The problem with this method is that when Di and N_dis are small, the optimal motion path of SITMSR can be achieved. Nevertheless, the convergence process of the above optimization method is longer and the optimization fails in the case where N_dis takes a larger value. Therefore, it is necessary to improve the path optimization algorithm for the non-singular condition, so that it can handle path optimization at the SITMSR singularity configuration.

When generating the complete path solution space of energy consumption with singular positions, take the joint angle constraint relationship in Equation (32) of the coupled joint as a whole, and combine with the other uncoupled joint angle displacements to form the whole path solution space. When SITMSR is in a path with singularity, ${\mathbf{q}}_{N_{dis},m,n}$ is defined as:

$${\mathbf{q}}_{N_{dis},m,n}={\left[\underset{7-Di}{\underbrace{\begin{array}{cc}{q}_{1,N_{dis},m,n}& \begin{array}{cc}{q}_{2,N_{dis},m,n}& \begin{array}{cc}\cdots & Q_{1,N_{dis},m,n}\end{array}\end{array}\end{array}}}\right]}^T$$

(34)

The optimal path of the uncoupled joints and the optimal path of the constraint relationship of the coupled joints ${\pi }_{\mathrm{Q}\mathrm{O}\mathrm{p}\mathrm{t}}$ can be acquired according to the first seven steps when optimizing with non-singular paths on the basis of generating the complete solution space of the singular path and the corresponding directional motion path on the basis of solving the optimal path of the coupling joints, which form ${\pi }_{\mathrm{O}\mathrm{p}\mathrm{t}}$ together.

To solve the optimal motion path of each coupled joint angle, the decoupling mathematical optimization model needs to be additionally introduced for secondary optimization to gain the coupled joint angle displacements for any value of $q_{Di,N}$ to get the minimum value of objective function f₂ under path constraints ${\pi }_{\mathrm{Q}\mathrm{O}\mathrm{p}\mathrm{t}}$. It is an optimization problem with linear constraints, which can be expressed as:

$$\min F_{\mathrm{D}}=\frac{t_p}{N_{dis}-1}{\displaystyle \sum _{i=1}^{N_q}{\displaystyle \sum _{N=1}^{N_{dis}}\left|P_{Di,N}\right|}}, \mathrm{s}.\mathrm{t}.\{\begin{array}{c}{q}_{Di,N}\in \left[q_{Dimin},q_{Dimax}\right]\\ {\displaystyle \sum q_{Di,N}}=Q_{1,N}\end{array}$$

(35)

where $Q_{1,N}$ is the value at the discrete point N of ${\pi }_{\mathrm{Q}\mathrm{O}\mathrm{p}\mathrm{t}}$ and $P_{Di,N}$ is the power of $q_{Di,N}$.

The optimal path of each coupled joint under the singular path can be sought through applying the global optimization algorithm to decoupling the mathematical optimization model above. The optimal energy consumption trajectory of the uncoupled joint can be obtained according to Step 8 in the solution to the optimal path without singularity and the path fitting based on the polynomial difference.

5. Results

To verify the effectiveness of the proposed method, optimization solutions based on energy saving of SITMSR using KuKa KR60 programmed by Matlab introduced in previous sections are tested under different movements given in the SIT ground motion experiment. Three examples are shown in this section. The path in the first one is under the condition of discontinuous singularity configuration. Example 2 reveals the energy optimization results of coupled joints using three different solution methods under continuous singularity configuration of SITMSR. Example 3 shows the energy consumption of continuous singularity configuration and non-singularity configuration under the premise of given trajectory. The parameters and corresponding values required for examination are summarized in

Table 1. The parameters and corresponding values required for examination.

${\mathbf{a}}_1\left[\mathbf{m}\mathbf{m}\right]$	${\mathbf{a}}_2\left[\mathbf{m}\mathbf{m}\right]$	${\mathbf{a}}_3\left[\mathbf{m}\mathbf{m}\right]$	${\mathbf{d}}_1\left[\mathbf{m}\mathbf{m}\right]$	${\mathbf{d}}_4\left[\mathbf{m}\mathbf{m}\right]$	${\mathit{\alpha }}_1[°]$	${\mathit{\alpha }}_3[°]$	${\mathit{\alpha }}_4[°]$	${\mathit{\alpha }}_5[°]$	${\mathbf{\gamma }}_{\mathit{\omega }}\left[°/\mathbf{s}\right]$
350	850	145	815	820	−90	−90	90	−90	0.1
${\mathsf{\gamma }}_v\left(\left[\mathrm{m}\mathrm{m}/\mathrm{s}\right],\left[°/\mathrm{s}\right]\right)$			$G_{SITS}\left[\mathrm{N}\right]$	${\mathbf{I}}_7\left[\mathrm{k}\mathrm{g}\cdot {\mathrm{m}}^2\right]$		$N_{traj}$	${\dot{\phi }}_{IT1}[°/\mathrm{s}]$	${\dot{\theta }}_{IT1}[°/\mathrm{s}]$
(0.3, 0.1)			271.26	(1.17, 0.91, 1.39)		1000	2	$\sin \left(1.05t_p\right)$
${\dot{\psi }}_{IT1}[°/\mathrm{s}]$	${\dot{\psi }}_{IT2}[°/\mathrm{s}]$		${\mathsf{\gamma }}_M\left(\left[\mathrm{N}\right],\left[\mathrm{N}/\mathrm{m}\right]\right)$		$C_4[\left(\mathrm{N}\cdot \mathrm{m}\cdot \mathrm{s}\right)/\mathrm{r}\mathrm{a}\mathrm{d}]$		$C_6[\left(\mathrm{N}\cdot \mathrm{m}\cdot \mathrm{s}\right)/\mathrm{r}\mathrm{a}\mathrm{d}]$
2	(2 × 10−4)${\mathrm{t}}_{\mathrm{P}}^3$		(0.05, 0.015)		0.4		0.2

.

5.1. Example 1

The angular velocity of precession, nutation, and spin of SITS are ${\dot{\phi }}_{IT1}$, ${\dot{\theta }}_{IT1}$, and ${\dot{\psi }}_{IT1}$ in this example, respectively. The initial value of nutation angle is −3°. In order to make full use of the robot’s reachable range of motion and to simulate the relative motion between the SIT and the aircraft operation in space as realistically as possible, the motion of O_IT in SITS is set as spiral movement. Other forms of positional movement can also be set, which do not affect the verification of the proposed method. On average, the total time taken to complete the motion planning is 3.95 s in Example 1. Figure 10 shows the trajectory of SIT in the ground experiment in Example 1.

The above given trajectory is discretized by setting different values of N_dis, and the cubic polynomial is used to fit and discretize the joint paths of SITMSR. Figure 11a shows the relationship between velocity and angular velocity errors and N_dis of SITS in motion planning. Force and torque error measured by six-axis force/torque sensor between the given motion and the planned motion are given in Figure 11b.

It can be seen from the Figure 11 that when N_dis = 100, the velocity, angular velocity, force, and torque errors are 0.032 mm/s, 0.160 mm/s, 0.193 mm/s, 0.003°/s, 0.024°/s, 0.064°/s, 0.005 N, 0.026 N, 0.028 N, 4.755 × 10⁻⁴ N·m, 0.006 N·m, and 0.011 N·m, respectively, all of which meet the required motion planning accuracy.

The search algorithm based on the solution to the optimal path with discontinuous singularity configuration is used when N_dis = 100 to get $\mathbf{O}\mathbf{p}\mathbf{t}\mathbf{N}{\mathbf{o}}_{\mathrm{o}\mathrm{p}\mathrm{t}}$ and $f_{2\mathrm{o}\mathrm{p}\mathrm{t}}$ of π_Opt with minimum energy consumption. Here, the shortest path π_SL is selected for comparison with π_Opt. The path node number of π_Opt and π_SL and the comparison of path length with N_dis are shown in Figure 12a. The comparison of energy consumption with time are shown in Figure 12b.

As can be seen from the above curves, compared with path π_SL, although the optimal path π_Opt rotates 23.43% more in the joint angular displacement, its energy consumption is saved by 72.67%.

There are also differences in joints driving the torque of SITMSR on the above paths. Figure 13a,b, respectively, present the performance of joints driving the torque in the planned motion path of π_Opt and π_SL. The maximum joint driving the torque when the SITMSR moves along the planning path π_Opt is only 63.84% of that along path π_SL.

In terms of smoothness of motion, the jitter of the robot joints under the path π_Opt and path π_SL are shown in Figure 14a,b. The maximum jitter value of SITMSR when moving along path π_SL is 0.034 rad/s³, which occurs at joint 4, while the maximum jitter value of path π_Opt is 0.162 rad/s³, which also happens at joint 4. The joint jitter along the optimal path of energy consumption is, obviously, larger than that along the shortest path, which means that the joints are also subject to greater impact, but its value is within the range of the SITMSR.

The comparison results of path π_Opt and path π_SL in Example 1 are summarized in

Table 2. The comparison results of path π_Opt and path π_SL.

Path	Path Length [°]	Energy Consumption [J]	${\left|{\mathbf{T}}_{\mathbf{q}\mathbf{i},\mathbf{j}}\right|}_{\mathbf{m}\mathbf{a}\mathbf{x}}[\mathbf{N}\cdot \mathbf{m}]$	${\left|{\stackrel{⃛}{\mathbf{q}}}_{\mathbf{i},\mathbf{j}}\right|}_{\mathbf{m}\mathbf{a}\mathbf{x}}[\mathbf{r}\mathbf{a}\mathbf{d}/{\mathbf{s}}^3]$
πOpt	813.65 (+23.43%)	1179.24 (−72.67%)	1456.40 (+36.1%)	0.162 (+0.128)
πSL	623.03	4315.39	2281.50	0.034

.

5.2. Example 2

In this example, the condition where the wrist joint of SITMSR is in continuous singularity configuration, that is, the rotation axis of robot joint four and joint six is collinear. Under the precondition that the end trajectory of SITS is given, the trajectory of other joints is determined, except for q₄ and q₆. Therefore, optimizing the joint space path of the coupling joint is focused to ensure optimal energy consumption. The precession angular velocity and nutation angular velocity of SITS are set as zeros and the angular velocity of spin of SITS is ${\dot{\psi }}_{IT2}$. The constraints of the angular displacement and the angular velocity of the coupled joints are shown in Figure 15a. The trajectory of the centroid of SITS is shown in Figure 15b.

The proposed algorithm (method 1) in the previous section is applied to optimize the energy consumption of the coupling joints in the case of continuous singularity of SITMSR. The friction torque during the motion of the coupled joint is considered, and it is assumed that it satisfies a linear relationship with the angular velocity, and the friction coefficients of q₄ and q₆ are C₄ and C₆, respectively. The convergence curve of F_D with iteration is shown in Figure 16a. The average running time of motion planning based on energy saving using method 1 is 36.67 s. After the solution is completed, the trajectory of each coupled joint can be obtained, as shown in Figure 16b. It can be seen from the optimization results that when the iteration runs to Step 68, the energy consumption converges to 2.49 J, and the maximum residual error is 1.02 × 10⁻¹²°. The rotation angle of q₄ is 76.89° more than that of q₆.

In order to verify the effectiveness of the above solving of the algorithm, a directional path search algorithm based on the linear decoupling method is used to solve the above issue. The relationship between energy consumption and its ratio to method 1 and λ₁ is shown in Figure 17a. The angular displacements of the coupled joints and energy consumption when λ₁ = 1.17 are shown in Figure 17b.

In Figure 17, the energy consumption of the coupled joints decreases with the increase of λ₁ until it reaches the minimum value of 3.47 J. Compared with method 1, the minimum relative increase energy consumption is 28.24%, and the value of λ₁ reaches the maximum at this time. If λ₁ continues to increase, it will cause the joint angular displacement to exceed the range of motion.

The genetic algorithm is adopted to solve the minimum energy consumption under the condition in Example 2. Set the initial population number of the genetic algorithm to 300, and the maximum generation to 800. Since the results of the genetic algorithm are different each time, the joint angular displacements take a set of solutions corresponding to the minimum energy consumption value. The convergence curve of energy consumption of SITMSR in the case of continuous singularity using genetic algorithm is shown in Figure 18a. The angular displacements of the coupled joint corresponding to the minimum energy consumption obtained by the genetic algorithm is shown in Figure 18b.

It can be seen from the Figure 18 that the minimum energy consumption of the genetic algorithm solution is 2.78 J, which is similar to method 1, which also verifies the effectiveness of the proposed solution method. The comparison of the results of the above three methods under the condition of continuous singularity configuration of SITMSR are summarized in

Table 3. The comparison of the results of the above three methods under the condition of continuous singularity configuration of SITMSR.

Method	Energy Consumption [J]	Relative Energy Consumption [%]
Global optimization algorithm	2.49	−28.24
Linear decoupling method	3.47	—
Genetic algorithm	2.78	−19.88

.

5.3. Example 3

In this example, the given centroid trajectory of SITS is shown in Figure 18b, and its precession and nutation motion settings are the same as in Example 2, but the spin angular velocity is 0.5 times that in Example 2. The optimization method of continuous singularity of SITMSR is used to optimize the coupled joint motion, and then the directional path map of energy consumption is constructed for path optimization. Here, the non-singularity path π_Ns is compared with the path π_Opt, which is also the energy-efficient path. The relationship between path length ratio and energy ratio of π_Ns and π_Opt with time are shown in Figure 19.

Although the lengths of the two paths are approximately the same, the optimal path π_Opt saves 6982.36 J compared to the path π_Ns. The comparison results of path π_Ns and path π_Opt in Example 3 are shown in

Table 4. The comparison results of path π_Ns and path π_Opt in Example 3.

Path	Path Length [°]	Energy Consumption [J]	Relative Energy Consumption [%]
πOpt	354.00 (−0.45)	2740.72	−62.23%
πNs	354.45	7256.43	—

.

6. Conclusions

In this paper, a motion planning method of SITMSR based on energy saving and its corresponding joint trajectory solving algorithm are proposed. Simulations based on different given motions of SIT in the ground experiment are conducted to verify the effectiveness of presented approach. The above research mainly consists of three parts: first, the motion trajectory and accuracy requirements of SIT are mapped to the ground test system (SITMSRS); second, discrete optimization mathematical model and constraints of energy consumption of SITMSR are established; third, the Dijkstra algorithm and adding the global optimization algorithm are used to get the solutions of the SITMSR path with minimal energy consumption under the condition of discontinuous singularity configuration and continuous singularity configuration, and the trajectory of each joint is obtained through polynomial fitting and discretization of solved path.

The general form of SIT makes it difficult for ground simulation robots to avoid singular positions completely, which brings new challenges to motion planning based on energy optimization. The proposed method is based on the kinematics and dynamics of the robot, which helps to solve the problem of optimal energy consumption when manipulator exists singularity configuration under a given end trajectory. From the results in this paper, the robot consumes less energy in singularity configuration under the premise that the trajectory of the motion is satisfied. In addition, the motion planning method based on energy saving is also applicable to 7-DOF manipulators and robot systems with more DOF. The algorithm presented can obtain the global optimal solution. However, the dynamics of the robot becomes complicated with the increase of DOF, which will increase the computation amount of the proposed method and the cost of hardware implementation.

Future work will further improve the efficiency of the algorithm operation. The main work is speeding up the generation process of complete path solution space of joints power based on robot dynamics. The interface of cyber–human interaction will need to be designed and the motion planning method will need to be embedded in the real-time software of the ground semi-physical test system. High-performance computer hardware will be used to support the operation of the algorithm. In addition, joint stiffness should be considered when the proposed method is applied to a cooperative manipulator with 7-DOF or robots with more DOF. The evaluation will be carried out to quantify its impact on energy consumption in the process of robot motion planning.