Collision-Free Trajectory Planning Optimization Algorithms for Two-Arm Cascade Combination System

Abstract

As a kind of space robot, the two-arm cascade combination system (TACCS) has been applied to perform auxiliary operations at different locations outside space cabins. The motion coupling relation of two arms and complex surrounding obstacles make the collision-free trajectory planning optimization of TACCS more difficult, which has become an urgent problem to be solved. For the above problem, this paper proposed collision-free and time–energy–minimum trajectory planning optimization algorithms, considering the motion coupling of two arms. In this method, the screw-based inverse kinematics (IK) model of TACCS is established to provide the basis for the motion planning in joint space by decoupling the whole IK problem into two IK sub-problems of two arms; the minimum distance calculation model is established based on the hybrid geometric enveloping way and basic distance functions, which can provide the efficient and accurate data basis for the obstacle-avoidance constraint condition of the trajectory optimization. Moreover, the single and bi-layer optimization algorithms are presented by taking motion time and energy consumption as objectives and considering obstacle-avoidance and kinematics constraints. Finally, through example cases, the results indicate that the bi-layer optimization has higher convergence efficiency under the premise of ensuring the optimization effect by separating variables and constraint terms. This work can provide theoretical and methodological support for the efficient and intelligent applications of TACCS in the space arena.

1. Introduction

1.1. Background

In the field of modern aerospace, space robots have been used to help people complete certain tasks inside and outside space capsules. With the development of aerospace technology, different kinds of space robots have been studied and applied for on-orbit servicing tasks, such as a 6-DOFs manipulator [1], 7-DOFs manipulator [2], 14-DOFs super redundant manipulator [3], and a dual-arm humanoid robot [4]. The safe and efficient operation of space robots depends on the motion planning, which has always been the focus of researchers in recent studies [5,6]. For example, Shrivastava et al. proposed a jerk-optimized motion planning method for the redundant space robot which used the gray wolf optimization algorithm to search for the optimal result [7]; Xie et al. presented the trajectory planning method for dual-arm free-floating space robot to minimize the final base attitude change and ensure the trajectory smoothness by using an enhanced bidirectional approach [8]. In recent years, the two-arm cascade combination system (TACCS) shown in Figure 1 has been developed and applied to handling and maintenance outside capsules, and it is obtained through the docking combination of two manipulators (which can largely increase the workable area and the robotic dexterity) and better adapted to the operational tasks on the outer surface of the cylindrical cabin. Two manipulators in TACCS can be regarded as the fixed manipulator (first-level arm) and the mobile manipulator (second-level arm), respectively, and the base coordinate frame (BCF) of the latter can be changed by the joint movement of the former.

1.2. Literature Review of Motion Planning

In existing studies, the motion planning of manipulators has been the focus of research aimed at finding efficient, accurate and safe modes of application. Actually, the motion planning of TACCS has the same problem with that of the mobile manipulator (composed of a mobile car and a manipulator), which is how to consider the influence of the motion of the first-level arm or the mobile car to the motion performance of the second-level arm or the manipulator. There exist two solving methods for the motion planning of the mobile manipulator: one of them is that the motion planning algorithms of two subsystems are carried separately (called SMP in the following), the other is that the motion of the high-DOFs system is planned as a whole (called WMP in the following).

For SMP, researchers mainly focused on the improvement of the whole planning efficiency of the robotic system in previous studies. For example, Li et al. presented a hierarchical motion planning method for the mobile manipulator, which includes two planning stages: firstly the path of the two-dimensional mobile base was optimized using a hybrid sampling strategy that combined a bridge test and uniform sampling, aiming to improve the planning efficiency; secondly the secure configuration and trajectory of the manipulator was searched [9]. Chen et al. also proposed a hierarchical motion planning method which used the optimization-based A* algorithm and the sampling-based heuristic algorithm for the collision-free motion plannings of the mobile base and the manipulator, respectively, which can effectively improve the planning efficiency and success rate [10]. Rastegarpanah et al. used an A* algorithm and rapidly exploring random trees (RRTs) to separately plan the paths for the mobile platform and manipulator and completed the task of picking up an object from one table to one box [11]. In order to consider the coupling relation of configurations of two subsystems, criteria like reachability or manipulability of the manipulator’s end effector has been considered to decouple the planning of the two subsystems. For example, Colucci et al. developed a motion planning algorithm based on a manipulability index which considers the mobile base and manipulator separately and provides the closed inverse kinematics algorithm for the redundant manipulator [12,13]. Actually, the motion planning for only the mobile car or manipulator has been popular for quite a long time. Some mature technologies have been proposed. For the motion safety, some distance calculation or collision detection algorithms between the robot and surrounding obstacles based on different envelope ways were developed by scholars. For example, basic geometries like spheres and cylinders [14,15], and hierarchical bounding boxes [16] were used to envelope the robotic arms or obstacles and then applied to solve the collision-free motion planning problem of the manipulator. For the motion stability or trajectory-tracking accuracy, a cubic/quintic polynomial curve [17,18], B-spline curve [19,20] or segmented function curve [21,22] were utilized to interpolate the motion trajectory, which can eliminate vibrations caused by speed or acceleration mutations when the robot starts and stops. Moreover, the motion time [23,24], energy consumption [25], angular jerk [26,27] or their combination [28] were always taken as the objectives of the motion planning optimization of the manipulator.

For WMP, the grid-based [29], sampling-based [30] and optimization-based [31] methods were proposed in previous studies. The grid-based method are generally proposed based on the free space represented by a swept volume or a graph-creating algorithm, and since the dimension of space related to the number of robotic joints has a major impact on the planning efficiency, it is more applicable for the path planning of the mobile car or the low-DOFs robot. For example, Liu et al. considered the mobile manipulator (composed of a 2-DOFs manipulator and 3-DOFs mobile device) as a whole system and presented a path planning method based on representation space [29]. The sampling-based methods have been developed well to solve the path planning of mobile manipulators with high DOFs, including RRTs, probabilistic roadmap (PRM) and their modifications. For example, Dai et al. presented a path planning method based on a novel potential bidirectional RRT* for redundant manipulators in joint space [32] which can effectively improve the obstacle-avoidance ability of the robot; Chen et al. proposed an improved PRM based on a new sampling strategy using the virtual force field and the design of a new connection strategy to improve the planning efficiency [33]. However, the above methods are mostly used for the path planning of the robot, which refers to determining a pure geometric description of a series of positions of the end effector or mobile base; additionally, it does not consider the system kinematics, including the change in velocities or accelerations over time, which means the motion stability or motion time cannot be planned or optimized. The optimization-based methods are mostly presented by the modeling and solving of the motion planning optimization problem, wherein constraints for the obstacle avoidance, accuracy, kinematics and dynamics can be considered, and multiple optimization objectives, including the motion time, energy consumption and joint jerks, can be taken to ensure the working efficiency, energy conservation and motion stability [34,35,36]. In existing works, many intelligent optimization algorithms (like the particle swarm optimization (PSO) algorithm [35], genetic algorithm [36], etc.) have been introduced to solve the above optimization problem. But for the mobile manipulators with high DOFs, the high optimization performance (including the ability of searching for the global optimal result, the convergence efficiency) is difficult to obtain. Therefore, many scholars focus on the modification of the current optimization algorithms or the proposal of the novel optimization algorithm. For example, Jin et al. used the chaotic PSO to solve the motion trajectory planning optimization problem, which improved the premature phenomenon of the traditional PSO algorithm [37]; Li et al. proposed a novel hybrid heuristic algorithm, which combined PSO and a whale optimization algorithm (PSO-WOA) to solve the multi-objective optimization problem of the trajectory planning of the space robot, considering the end-effector pose, base disturbance, motion time and manipulability as objectives [38]; Cao et al. proposed a modified multi-objective PSO algorithm (GMOPSO) by combining a mutation operator, annealing factor and feedback mechanism for the trajectory planning of fruit picking manipulator [39].

The comparison of the existing representative works is shown in

Table 1. Comparison of some representative works wherein the superscript ^#F means the robot type is free-floating space robot.

Works	Robot Type	DOFs	Planning Methods	Safety	Stability	Efficiency	Energy Consumption	Consideration of Motion Coupling
Ref. [9]	Mobile manipulator	3 + 6	A* algorithm; Uniform sampling-based algorithm	Yes	No	No	No	No
Ref. [10]		3 + 6	A* algorithm; Sampling-based heuristic algorithm	Yes	No	No	No	No
Ref. [11]		3 + 6	A* algorithm; RRT	Yes	No	No	No	No
Refs. [12,13]		5 + 6	A* algorithm; RRT	Yes	No	No	No	Yes
Ref. [29]		3 + 2	A* algorithm	Yes	No	No	No	Yes
Ref. [39]		3 + 6	PSO-based optimization	Yes	Yes	Yes	Yes	No
Ref. [30]	High-DOFs robot	14 or 21	Sampling-based algorithm	Yes	No	No	No	/
Ref. [31]		5	PSO-based optimization	No	Yes	Yes	No	/
Ref. [32]		Any	Bidirectional RRT*	Yes	No	No	No	/
Ref. [33]		Any	PRM	Yes	No	No	No	/
Ref. [37]		6 #F	CPSO-based optimization	No	Yes	Yes	No	/
Ref. [38]		7 #F	PSO-WOA-based optimization	Yes	Yes	Yes	No	/
The proposed method	TACCS	6 + 6	PSO-based optimization	Yes	Yes	Yes	Yes	Yes

. Since only the path planning was implemented without optimization in many related works, the robot type, DOFs, planning methods, operational performance (like safety, manipulability, stability, efficiency, energy consumption) and the consideration of motion coupling of all works are listed in the comparison table in order to compare them more comprehensively. From the comparison, we can observe that the optimization-based planning method can directly obtain an optimal motion trajectory with the minimum of efficiency or energy consumption as the objective for the robot, not only a collision-free operational path, and it can better control the motion performance of the robot in terms of obstacle avoidance, motion stability, tracking accuracy, efficiency, or energy consumption, etc. However, the low planning optimization efficiency has always been a problem and the focus of the current research.

1.3. Problem Formulation

As shown in Figure 1, when one manipulator (DOFs ≥ 6) is mounted as the real end effector of the other manipulator (DOFs ≥ 6), the whole robot will be a super redundant system with DOFs of more than 12 which has been applied to perform on-orbit servicing tasks in the aerospace field, such as assembly, maintenance, the capture of cooperative and non-cooperative objects, deployment and debris removal [40]. Moreover, there exist many protruding structures outside the cylindrical cabin surface used for measurement, external docking, entrance, etc., which means the robotic motion planning needs to consider the obstacle avoidance. Therefore, the high redundancy, the large cylindrical working surface and the complex surrounding obstacles make the collision-free motion planning of TACCS quite difficult. In addition, from the summary of the current motion planning methods, the optimization-based planning method can better control the motion performance of the robot. However, since TACCS has the higher DOFs, as well as faces the special and complex working environment, the motion planning optimization becomes more difficult. The difficulties are mainly reflected in several aspects: (1) the accurate analytical solution of the inverse kinematics of the TACCS is the main basis for its motion planning in joint space, which can better ensure the motion stability of robotic joints, but it has always been a problem due to the high redundancy; (2) the accurate minimum distance between manipulator and its workspace is difficult to calculate quickly, which is very important in obtaining an efficient and reliable motion planning algorithm; (3) the number of variables and constraints increases over the robotic DOFs, which makes the trajectory optimization less efficient and more easily trapped into the local optimum.

In this work, a collision-free and time–energy–minimum (CF-TEM) trajectory planning optimization method is proposed for TACCS considering the dexterity of the second-level arm under the complex and specific working environment outside the capsule. Three problems are solved based on the following contributions: (1) the IK problem of TACCS is decoupled into two IK sub-problems of two arms and then the screw-based IK model is established, wherein the IK solution of second-level arm is related to the end-effector pose of the first-level arm; (2) the minimum distance calculation model is established based on the hybrid geometric enveloping way and basic distance functions, wherein TACCS is enveloped by a convex polyhedrons, slices-based circular/polygonal edges and planes, and the the surrounding obstacles are enveloped by convex polygonal planes; (3) the single and bi-layer optimization algorithms are both proposed by taking motion time and energy consumption as objectives and considering constraints of obstacle avoidance and robotic kinematics, and they are finally compared in terms of efficiency and effect of optimization, aiming to find an efficient and reliable optimization algorithm for the motion planning problem of the system.

The remainder of this work is organized as the following three sections: Section 2 presents the screw-based inverse kinematics of TACCS, which gives the explicit mathematical mapping from the target pose of the end effector in a base coordinate frame (BCF) of the system to joint angles of two arms. Section 3 gives the minimum distance calculation model based on the hybrid geometric envelope way, considering the characteristics of operation task and obstacles outside the capsule; Section 4 gives the single and bi-layer optimization models and algorithms of the system; Section 5 discusses the results obtained by two optimization algorithms from the efficiency and effect of optimization through a simulation case; Section 6 gives the main conclusions and future works.

2. Screw-Based Inverse Kinematics Model of TACCS

For the inverse kinematics (IK) analysis, the kinematics parameters and three coordinate frames of TACCS are shown in Figure 2, wherein the frame 1 ($o-xyz$) refers to the BCF of the whole system and also the BCF of the first-level arm; the frame 2 ($o_{E1}-x_{E1}{y}_{E1}{z}_{E1}$) refers to the end-effector coordinate frame (EECF) of the first-level arm and also the BCF of the second-level arm; the frame 3 ($o_{E2}-x_{E2}{y}_{E2}{z}_{E2}$) refers to the EECF of the second-level arm, which is directly related to the operational task. Then, the IK solution problem can be described as the solving of joint angles of two arms by taking the end-effector target pose of the second-level arm in frame 1 as a known condition. Moreover, in Figure 2, $q_i$ represents one point taken from the rotational axis of the $ith$ joint of one arm of TACCS, while $q_e$ is the real end effector of each arm and $l_i$ means the kinematics parameters that will be used in the following IK model. Since the pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}$ of the end effector of the first-level arm has a major impact on the operation effect of the second-level arm, it will be optimized in the motion planning optimization of the system, which means the IK problem of TACCS can be decoupling into the IK sub-problems of two arms. Through the above decoupling processing, the IK sub-problem 1 aims to solve the joint angles of the first-level arm with the given pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}$ in frame 1; IK sub-problem 2 aims to solve the joint angles of the second-level arm with the given pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2}}$ in frame 1. In the traditional IK problem, the end-effector pose of manipulator is given in its own base coordinate frame, that is, for IK sub-problem 1, the inverse solution can be modeled directly using the screw-based method, but for IK sub-problem 2, the pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2}}$ in frame 1 must be transferred in frame 2, as then the solution can be modeled based on the new end-effector pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2},\mathit{N}}$.

It is noted that, for the whole IK problem of TACCS, the operation task determines the pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2}}\in {\mathbf{ℛ}}^{\mathbf{4}\times \mathbf{4}}$, which can be expressed by a homogeneous coordinate matrix as follows:

$$\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2}}=\left[\begin{array}{cc}{\mathit{R}}_{\mathit{e}}& {\mathit{q}}_{\mathit{e}}\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& x\\ n_y& o_y& a_y& y\\ n_z& o_z& a_z& z\\ 0& 0& 0& 1\end{array}\right]$$

(1)

wherein ${\mathit{R}}_{\mathit{e}}\in {\mathbf{ℛ}}^{\mathbf{3}\times \mathbf{3}}$ and ${\mathit{q}}_{\mathit{e}}\in {\mathbf{ℛ}}^{\mathbf{3}}$ denote the orientation and the 3D coordinates of the end effector, respectively.

Moreover, in the following optimization of the pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}$, three linear displacements $\mathsf{\Delta }x$, $\Delta y$, $\Delta z$ along three axes and three rotational angles $\alpha$, $\beta$, $\gamma$ around three axes of frame 1 are taken as optimization variables. Then, the optimal pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}\in {\mathbf{ℛ}}^{\mathbf{4}\times \mathbf{4}}$ can be expressed by the following matrix:

$$\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}=\left[\begin{array}{cccc}{c}_{\alpha }{c}_{\beta }& c_{\alpha }{s}_{\beta }{s}_{\gamma }-s_{\alpha }{c}_{\gamma }& c_{\alpha }{s}_{\beta }{c}_{\gamma }+s_{\alpha }{s}_{\gamma }& \Delta x\\ s_{\alpha }{c}_{\beta }& s_{\alpha }{s}_{\beta }{s}_{\gamma }+c_{\alpha }{c}_{\gamma }& s_{\alpha }{s}_{\beta }{c}_{\gamma }-c_{\alpha }{s}_{\gamma }& \Delta y\\ -s_{\beta }& c_{\beta }{s}_{\gamma }& c_{\beta }{c}_{\gamma }& \Delta z\\ 0& 0& 0& 1\end{array}\right]$$

(2)

wherein $c_{\alpha }=cos\alpha , s_{\alpha }=sin\alpha$; $c_{\beta }=cos\beta , s_{\beta }=sin\beta$; $c_{\gamma }=cos\gamma , s_{\gamma }=sin\gamma$.

Since the poses $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2}}$ and $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2},\mathit{N}}$ have the transfer relation $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2}}=\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}·\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2},\mathit{N}}$, the pose $\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2},\mathit{N}}\in {ℛ}^{\mathbf{4}\times \mathbf{4}}$ can be expressed by

$$\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2},\mathit{N}}={\left[\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}\right]}^{-1}·\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2}}$$

(3)

Based on the Chasles principle, the motion screw of the rotational joint is defined by $\mathit{\xi }={\left[\begin{array}{cc}\mathit{\omega }& \mathit{\theta }\end{array}\right]}^T$, and the exponential form of the screw coordinate can be written as follows,

$$exp\left(\widehat{\mathit{\xi }}\theta \right)=\left[\begin{array}{cc}exp\left(\widehat{\mathit{\omega }}\theta \right)& \left[I-exp\left(\widehat{\mathit{\omega }}\theta \right)\right]\left(\mathit{\omega }\times \mathit{v}\right)+\theta \mathit{\omega }{\mathit{\omega }}^{\mathit{T}}\mathit{v}\\ 0& 1\end{array}\right]$$

(4)

wherein $\mathit{\omega }$ is the unit vector of the rotational axis; $\widehat{\mathit{\omega }}$ is the anti-symmetric matrix of $\mathit{\omega }$, while setting $\mathit{\omega }=\left({\omega }_1,{\omega }_2,{\omega }_3\right)$, $\widehat{\mathit{\omega }}=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$; $\theta$ represents the rotational angle; $\mathit{v}$ means the linear velocity of the rotational motion, $\mathit{v}=\mathit{q}\times \mathit{\omega }$, where $\mathit{q}$ denotes the 3D coordinate of one point located on the rotational axis; $I$ is a $3\times 3$ identity matrix; $exp\left(\widehat{\mathit{\omega }}\theta \right)$ can be calculated by $exp\left(\widehat{\mathit{\omega }}\theta \right)=I+\widehat{\mathit{\omega }}sin\theta +{\widehat{\mathit{\omega }}}^2\left(1-cos\theta \right)$.

Based on the above analysis, two IK sub-problems can be transferred into the general IK problems. Taking the robotic model shown in Figure 2 as the example case, two arms are manipulators with the same configuration but different values to the kinematics parameters listed in

Table 2. Kinematics parameters of two arms.

Parameters	${\mathit{l}}_1$(mm)	${\mathit{l}}_2$(mm)	${\mathit{l}}_3$(mm)	${\mathit{l}}_4$(mm)	${\mathit{l}}_5$(mm)	${\mathit{l}}_6$(mm)
First-level arm	540	540	1494	1260	270	332
Second-level arm	300	300	830	700	150	184.5

. According to our previous work [41], the IK problem of each arm can be firstly decomposed into three sub-problems: one is for the rotation about three non-intersecting axes; the second is for the rotation about two intersecting axes; and the third is for the rotation about one single axis. For the first two IK sub-problems, the rotational motions are shown as Figure 3, respectively, wherein $c_1,c_2,c_3$ are the three transition points of two motions, and their 3D coordinates ${\mathit{c}}_{\mathbf{1}}=\left[x_1,y_1,z_1\right],{\mathit{c}}_{\mathbf{2}}=\left[x_2,y_2,z_2\right],{\mathit{c}}_{\mathbf{3}}=\left[x_3,y_3,z_3\right]$ can be obtained by solving a hexagonal quadratic equation system and a ternary quadratic equation system. Based on the above calculations, the IK solution of the arm (the angular displacements of all joints of the arm) can be explicitly modeled and can be expressed as Equation (5). The detailed solving process can be referred to through reference [41].

$$\{\begin{array}{l}{\theta }_1=\mathrm{atan}2(x_{2}y-y_{2}x,x_{2}x-y_{2}y)\\ {\theta }_2=\mathrm{atan}2[x_2(z_1-l_1)-x_1(z_2-l_1),x_1{x}_2-(z_1-l_1)(z_2-l_1)]\\ {\theta }_3=\mathrm{atan}2[x_1{l}_4,l_4(z_1-l_1-l_3)]\\ {\theta }_4=\mathrm{atan2}(x_3{y}_{e1}-y_3{x}_{e1},x_3{x}_{e1}-y_3{y}_{e1})\\ {\theta }_5=\mathrm{atan2}[x_3{l}_2,l_2(z_3-l_1-l_3-l_4)]\\ {\theta }_6=\mathrm{atan}2(y_{e2},x_{e2})\end{array}$$

(5)

where, ${\left[x_{e1},y_{e1},z_{e1},1\right]}^T={\left[exp\left({\widehat{\mathit{\xi }}}_1{\theta }_1\right)exp\left({\widehat{\mathit{\xi }}}_2{\theta }_2\right)exp\left({\widehat{\mathit{\xi }}}_3{\theta }_3\right)\right]}^{-1}{\left[x_{s1},y_{s1},z_{s1},1\right]}^T$. In this equation, $\left[x_{s1},y_{s1},z_{s1}\right]$ is the initial 3D coordinates of one point located on the sixth joint axis but not on the fourth and fifth axes; ${\left[x_{e2},y_{e2},z_{e2},1\right]}^T={\left[exp\left({\widehat{\mathit{\xi }}}_1{\theta }_1\right)\cdots exp\left({\widehat{\mathit{\xi }}}_5{\theta }_5\right)\right]}^{-1}{\left[x_{s2},y_{s2},z_{s2},1\right]}^T$, in this equation, $\left[x_{s2},y_{s2},z_{s2}\right]$ is the initial 3D coordinates of one point located out of the sixth joint axis.

Then, IK models can be written as two functions, ${\mathit{\theta }}_{\mathit{A}\mathbf{1}}=fun\_IK\_A1\left(\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{1}}\right)$ and ${\mathit{\theta }}_{\mathit{A}\mathbf{2}}=fun\_IK\_A2\left(\mathit{g}\mathit{s}{\mathit{t}}_{\mathit{E}\mathbf{2},\mathit{N}}\right)$, which together form an important basis for the motion planning optimization of the TACCS.

3. Minimum Distance Calculation Based on Hybrid Geometric Envelope

The minimum distance calculation is an important basis for the spatial constraint determination of the motion planning optimization. In real applications, the TACCS will be applied to perform works with targets at different places on the capsule body. Moreover, the outer surface of the capsule may have some raised structures for docking or measuring, which makes the accurate distance calculation more difficult. In order to solve the above problem, this work uses the circular slices and convex polygonal planes to envelop the robotic links and connection structure between two arms, respectively, also using the convex polygonal planes to envelop the capsule and other obstacle structures located at the surface of the capsule, respectively. The envelope structures are shown in Figure 4. From Figure 4, the environmental obstacles include convex obstacles and cylindrical obstacles. Since the distance calculation between the circular slice and the cylinder is difficult, the cylinder is also enveloped by convex polygonal planes, which needs to firstly envelope the end circle using its outer joined regular polygon. For the envelope of a circle, the largest error between the original and new geometry can be calculated by $ϵ=R\left[1/cos\left(\pi /N_e\right)-1\right]$ (as shown in Figure 5); therefore; if we hope $ϵ\le 5 \mathrm{mm}$, the number of edges must meet the condition as $N_e\ge \pi /acos\left[R/\left(5+R\right)\right]$ (based on this condition, when $R=50 \mathrm{mm}$, $N_e\ge 7.3$).

In our previous study [42], the basic functions of the minimum distance between the circular edge and the convex polygonal plane and that between two convex polygonal planes have been established, which can be represented by $fun\_CP\left(\rho ,\varsigma \right)$ and $fun\_PP\left({\varsigma }_1,{\varsigma }_2\right)$, wherein $\rho$ represents one circular edge and is determined by the coordinate of the circular center and $\varsigma$ represents one convex polygonal plane and is determined by the coordinates of polygonal vertices. Based on the envelope ways shown in Figure 3, the structure of the TACCS is finally expressed as a set of geometries, including circular edges and convex polygonal planes in mathematics, and the structure of its workspace obstacles are expressed as a set of convex polygonal planes. Therefore, the minimum distance between TACCS and its workspace obstacles can be written as follows:

$$DI=\min \left\{fun\_CP\left({\rho }_{R,i},{\tau }_{O,k}\right), fun\_PP\left({\rho }_{R,j},{\tau }_{O,k}\right) ∕1\le i\le I, 1\le j\le J, 1\le k\le K\right\}$$

(6)

wherein ${\rho }_{R,i}$ and ${\rho }_{R,j}$ denote the $ith$ circular edge and the $jth$ polygonal plane; ${\tau }_{O,k}$ means the $kth$ polygonal plane of obstacles; $I$, $J$ are the numbers of circular edges and polygonal planes of the TACCS, and $K$ is the number of polygonal planes of obstacles.

As shown in Figure 5, the envelope of circle will lead to the largest error, $ϵ$. In order to further analyze the influence of $ϵ$ on the error of the shortest distance calculation, the distance between the circular plane with different radii ($R=50, 100, 150, 200 \mathrm{mm}$) and an circular edge ($R_e=150 \mathrm{mm}$) are taken as objectives. For the analysis, the center locations of two circles are set by $\mathit{C}=\left(0,0,0\right)$, ${\mathit{C}}_{\mathit{e}}=\left(200,300,400\right)$, and their normal vectors are set by $\mathit{n}=\left(0,0,1\right)$, ${\mathit{n}}_{\mathit{e}}={\displaystyle \frac{\sqrt{3}}{3}}·\left(1,1,1\right), \mathrm{respectively}$. During the calculation process, taking different values for $ϵ$, and determining the minimum number of edges by $N_e=\mathrm{ceil}\left\{\pi /acos\left[R/\left(ϵ+R\right)\right]\right\}$ ($\mathrm{ceil}\left\{\right\}$ represents the function that returns the minimum integer larger than the given real number), the shortest distance between the circular plane and the circular edge can be predicted by the distance between the convex polygonal plane $\varsigma$ and the circular edge $\rho$ can be calculated by the function $d_p=fun\_CP\left(\rho ,\varsigma \right)$; the real shortest distance $d_r$ can be calculated by taking $N_e=1000$, which can make the convex polygonal plane close enough to the circular outline; then the error of the shortest distance calculation is determined by $E=d_r-d_p$.

Based on the above strategy, the errors of the shortest distance under different values of $ϵ$ are shown in Figure 6, from which the following conclusions can be obtained: (1) The error basically increases with the increase of the value of $ϵ$; this is because the increase of $ϵ$ leads to the lower number of edges of the enveloped polygon (for example, when $ϵ\le 5$, the errors for four circular planes are all less than 5 mm, but when $R=15$ mm, $ϵ=17$, the error reaches 11.51 mm). (2) In some cases, even where $ϵ$ takes the large value, the error may be very small. The reasons for that are as follows: $ϵ$ represents the largest error between the original circle and its enveloped polygon, which means some points on their edges coincide or are close together; moreover, the shortest distance between the enveloped polygonal plane and the edge must be the distance between two point elements located on two geometries, and so when the point element at the polygonal plane is located just at the position where the envelope error is small, the predicted shortest distance will be very close to its real value. (3) In order to both ensure the accuracy and efficiency, the reasonable value for $ϵ$ should be taken, since, the smaller the value is, the lower the calculation efficiency and the higher the calculation accuracy. In real applications, the value can be taken according to the safety threshold value, which is set for the absolute safety of the robotic motion.

Moreover, the pose of the TACCS determines the locations of feature points of the envelope geometries, which can be calculated based on screw-based conversion matrices as follows:

$${\mathit{q}}_{\mathit{e}}\prime =\left[{{\displaystyle \prod }}_{i=1}^{N}exp\left({\widehat{\mathit{\xi }}}_i{\theta }_i\right)\right]{\mathit{q}}_{\mathit{s}}\prime$$

(7)

wherein ${\mathit{q}}_{\mathit{s}}\prime$ and ${\mathit{q}}_{\mathit{e}}\prime$ represents the start and final homogeneous coordinates of one point in frame 1, and they are 4D vectors extended from the corresponding 3D coordinate ${\mathit{q}}_{\mathit{s}}$ by adding a unit column, i.e., ${\mathit{q}}_{\mathit{s}}\prime ={\left({\mathit{q}}_{\mathit{s}},1\right)}^T\in {\mathbf{ℛ}}^{\mathbf{4}}$; $N$ means the number of joints that affects the pose of this point.

Therefore, since the angular displacement ${\theta }_i$ of the $ith$ joint changes over time during the operational motion, locations of feature points are related to time, which means the minimum distance during motion can be described as a function over time and expressed by $DI\left(t\right)$. The above function is a major basis for the following motion planning optimization.

4. Single and Bi-Layer Optimization Models and Algorithms

In the motion planning problem of TACCS, the variables of optimization include the following six motion variables: $\Delta x$, $\Delta y$, $\Delta z$, $\alpha$, $\beta$, $\gamma$, $T_{A1}$ and $T_{A2}$. $T_{A1}$ and $T_{A2}$ denote the motion times of the first-level arm and the second-level arm, respectively. In order to ensure the operational stability, it is supposed that two arms do not move simultaneously in real applications, which means although the motion dexterity of the second-level arm will be influenced by the pose of the first-level arm, the coupling influence is not dynamic. For this problem, considering both the total motion time and the motion energy consumption as the optimization objective, the objective of the single optimization can be modeled as follows:

$$f_S=w_1\left(T_{A1}+T_{A2}\right)+w_{2}w\prime \left\{{{\displaystyle \sum }}_{i=1}^{D_{A1}}{{\displaystyle \int }}_{t=0}^{T_{A1}}{S}_{A1,i}\left(t\right)+{{\displaystyle \sum }}_{i=1}^{D_{A2}}{{\displaystyle \int }}_{t=0}^{T_{A2}}{S}_{A2,i}\left(t\right)\right\}$$

(8)

wherein $w_1$ and $w_2$ are weighting factors for items of the motion and energy consumption, $w_1+w_2=1$; $w\prime$ denotes the coefficient used to eliminate the influence of the magnitude difference of items; $D_{A1}$ and $D_{A2}$ mean the numbers of DOFs of two arms; $S_{A1,i}\left(t\right)$ and $S_{A2,i}\left(t\right)$ are time functions of the angular displacement of the $ith$ joint of two arms, respectively, which can be obtained through motion trajectory interpolation. In this work, the quintic polynomial interpolation method is used, and the function of one joint angle can be expressed by

$$S\left(t\right)=a_1{t}^5+a_2{t}^4+a_3{t}^3+a_4{t}^2+a_{5}t+a_6$$

(9)

wherein the coefficients of the function can be calculated by $a_1=6\left({\theta }_E-{\theta }_S\right)/T^5$, $a_2=-15\left({\theta }_E-{\theta }_S\right)/T^4$, $a_3=10\left({\theta }_E-{\theta }_S\right)/T^3$, $a_4=a_5=0$ and $a_6={\theta }_S$, respectively, which are obtained by the boundary conditions according to requirements of movement stability as $S\left(0\right)={\theta }_S$, $S\left(T\right)={\theta }_E$ and $\dot{S}\left(0\right)=\dot{S}\left(T\right)=\ddot{S}\left(0\right)=\ddot{S}\left(T\right)=0$; $\dot{S}\left(t\right)$ and $\ddot{S}\left(t\right)$ are the functions of angular velocity and acceleration of the joint and can be obtained through one derivative and two derivatives of $S\left(t\right), \mathrm{respectively}$.

Then, considering kinematics and obstacle-avoidance constraints, the single optimization problem can be modeled as follows:

$$\{\begin{array}{l}\mathbf{minimize}:\hspace{1em}f=f_S(\mathrm{\Delta }x,\mathrm{\Delta }y,\mathrm{\Delta }z,\alpha ,\beta ,\gamma ,T_{A1},T_{A2})\\ \mathbf{subject\; to}:\hspace{1em}\mathrm{\Delta }x\in [\mathrm{\Delta }{x}_{min},\mathrm{\Delta }{x}_{max}]\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{\Delta }y\in [\mathrm{\Delta }{y}_{min},\mathrm{\Delta }{y}_{max}]\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathrm{\Delta }z\in [\mathrm{\Delta }{z}_{min},\mathrm{\Delta }{z}_{max}]\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\alpha \in [{\alpha }_{min},{\alpha }_{max}]\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\beta \in [{\beta }_{min},{\beta }_{max}]\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\gamma \in [{\gamma }_{min},{\gamma }_{max}]\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{S}_{A1,i}(t)\in D_{A1,i};S_{A2,i}(t)\in D_{A2,i}\\ \max \left[|{\dot{S}}_{A1,i}(t)|\right]\le V_{A1,i,max};\max \left[|{\ddot{S}}_{A1,i}(t)|\right]\le A_{A1,i,max}\\ \max \left[|{\dot{S}}_{A2,i}(t)|\right]\le V_{A2,i,max};\max \left[|{\ddot{S}}_{A2,i}(t)|\right]\le A_{A2,i,max}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\min [DI(t)]\ge D{I}_{\mathit{thred}}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{T}_{A1}+T_{A2}\le T_{max}\end{array}$$

(10)

where in the subscripts $min$ and $max$ mean the upper and lower bounds of the parameter, for example $\Delta x_{min}$ and $\Delta x_{max}$, are the upper and lower bounds of the linear displacement of the end effector of the first-level arm, respectively; $D_{A1,i}$ and $D_{A2,i}$ are the allowable range of the angular displacement of the $ith$ joints of two arms, respectively; the functions $\max \left[g\right]$ and $\min \left[g\right]$ refer to taking the maximum value and the minimum value of the function $g$ respectively; $V_{A1,i,max}$ and $A_{A1,i,max}$ are the maximum values of the angular velocity and acceleration allowed by the $ith$ joints of the first-level arm, respectively; $DI\left(t\right)$ is the minimum distance function of TACCS; and $D{I}_{thred}$ denotes the threshold value taken for the reliable obstacle avoidance.

Therefore, even if the allowable ranges or maximum values of the angular displacements, velocity and acceleration of all joints of each arm are the same, there are 14 constraint conditions in the above model. In order to increase the optimization efficiency, the upper and lower values of variables $\Delta x$, $\Delta y$, $\Delta z$, $\alpha$, $\beta$ and $\gamma$ need to be limited as much as possible. For this purpose, this work proposed a feasible space analysis method based on the IK functions of two arms. In this method, the Monte Carlo method is firstly used to randomly take values for variables $\Delta x$, $\Delta y$, $\Delta z$, $\alpha$, $\beta$, $\gamma$; then the target pose of the end effector of the first-level arm and the base frame of the second-level arm can be determined, based on which the IK functions of two arms can be solved; finally, values of variables with valid solutions in two IK problems can be recorded, based on which the feasible space of frame 2 (as Figure 2) is obtained, and can help to determine the upper and lower bounds of the corresponding variable. The framework of this method is shown in Figure 7.

Since the number of variables and constraint conditions have a major impact on the convergence time and effect of the optimization, the bi-layer optimization strategy is also proposed in this work. In this strategy, the optimization problem is decoupled into two layers of sub-optimizations: the first layer of sub-optimization is the end-effector pose optimization of the first-level arm considering the kinematics and obstacle-avoidance constraints, as well as its influence on the operational dexterity of the second-level arm; the second layer includes two sub-optimizations, which are the motion trajectory optimizations of two arms, respectively, based on the optimal result of the first layer. Moreover, the bi-layer optimization problem also considers the motion time and energy consumption of the arms. Therefore, the first sub-optimization takes the sum of Euclidean distances of all joints of two arms as the object, which refers to the equivalent index of the energy consumption; the other sub-optimizations take the motion time and the length of motion path of the arm as the object. Then, the objectives of three sub-optimizations can be expressed by

$$f_{B-1}=\sqrt{{{\displaystyle \sum }}_{i=1}^{D_{A1}}{{\theta }_{A1,i}}^2+{{\displaystyle \sum }}_{i=1}^{D_{A2}}{{\theta }_{A2,i}}^2}$$

(11)

$$f_{B-2}=w_1{T}_{A1}+w_{2}w\prime {{\displaystyle \sum }}_{i=1}^{D_{A1}}{{\displaystyle \int }}_{t=0}^{T_{A1}}{S}_{A1,i}\left(t\right)$$

(12)

$$f_{B-3}=w_1{T}_{A2}+w_{2}w\prime {{\displaystyle \sum }}_{i=1}^{D_{A2}}{{\displaystyle \int }}_{t=0}^{T_{A2}}{S}_{A2,i}\left(t\right)$$

(13)

wherein ${\theta }_{A1,i}$ and ${\theta }_{A2,i}$ are the inverse solutions of the $ith$ joint at the target pose of the end effector for two arms, respectively.

The bi-layer optimization framework is shown in Figure 8, wherein there exists a global judgement for obstacle avoidance of the whole robotic motion before the second-layer sub-optimizations. From this framework, the optimal values of the first-layer optimization is the input for sub-optimization of the second-layer optimization, which means two sub-optimizations can perform parallel computing and output optimal trajectories. In actual applications, the parallel computing can largely improve the execution efficiency of the bi-layer optimization algorithm.

Moreover, in the global judgement, the motion times of two arms are firstly taken reasonably to determine the overall robotic poses at different motion times, since their values have a minimal influence on the robotic pose at a certain point of the trajectory but largely affect the motion speed and acceleration of each joint; then, the smallest distance between the TACCS and its surrounding obstacles during motion can be calculated based on the model of the minimum distance of Section 3; finally, the smallest distance is compared with its threshold value (taken to ensure the absolutely safety of the system) to complete the judgement, which can detect the possible collisions that may happen during motion accurately and reliably. In the bi-layer optimization framework, if the global judgement condition is not satisfied, it then returns to the first-layer optimization, or else it continues the second-layer optimizations. The above strategy can further help improve the efficiency of the whole optimization, which can be modeled as follows:

$$\{\begin{array}{ll}\mathbf{minimize}:& f=f_{B-1}(\mathrm{\Delta }x,\mathrm{\Delta }y,\mathrm{\Delta }z,\alpha ,\beta ,\gamma )\\ \mathbf{subject\; to:}:& \mathrm{\Delta }x\in [\mathrm{\Delta }{x}_{\mathit{min}},\mathrm{\Delta }{x}_{\mathit{max}}]\\ & \mathrm{\Delta }y\in [\mathrm{\Delta }{y}_{\mathit{min}},\mathrm{\Delta }{y}_{\mathit{max}}]\\ & \mathrm{\Delta }z\in [\mathrm{\Delta }{z}_{\mathit{min}},\mathrm{\Delta }{z}_{\mathit{max}}]\\ & \alpha \in [{\alpha }_{\mathit{min}},{\alpha }_{\mathit{max}}]\\ & \beta \in [{\beta }_{\mathit{min}},{\beta }_{\mathit{max}}]\\ & \gamma \in [{\gamma }_{\mathit{min}},{\gamma }_{\mathit{max}}]\\ & {\theta }_{A1,i}\in D_{A1,i};{\theta }_{A2,i}\in D_{A2,i}\\ & D{I}_E\ge D{I}_{\mathit{thred}}\\ & \hspace{1em}\hspace{1em}\Downarrow \\ \mathbf{Judgement}:& min[DI(t)]\ge D{I}_{\mathit{thred}}\\ & \hspace{1em}\hspace{1em}\Downarrow \\ \mathbf{minimize}:& f=f_{B-2}\left(T_{A1}\right)\\ \mathbf{subject\; to}:& S_{A1,i}(t)\in D_{A1,i}\\ & \max \left[|{\dot{S}}_{A1,i}(t)|\right]\le V_{A1,i,\mathit{max}}\\ & \max \left[|{\ddot{S}}_{A1,i}(t)|\right]\le A_{A1,i,\mathit{max}}\\ & T_{A1}\le T_{A1,\mathit{max}}\\ & \hspace{1em}\hspace{1em}\Downarrow \\ \mathbf{minimize}:& f=f_{B-3}\left(T_{A2}\right)\\ \mathbf{subject\; to}:& S_{A2,i}(t)\in D_{A2,i}\\ & \max \left[|{\dot{S}}_{A2,i}(t)|\right]\le V_{A1,i,\mathit{max}}\\ & \max \left[|{\ddot{S}}_{A2,i}(t)|\right]\le A_{A2,i,\mathit{max}}\\ & T_{A2}\le T_{A2,\mathit{max}}\end{array}$$

(14)

wherein $D{I}_E$ means the minimum distance when the TACCS is at its target pose.

In this work, the particle swarm optimization algorithm (PSO) is adopted to solve the optimization models proposed above; the PSO is an intelligent algorithm that searches for the optimal solution in the solution space by a certain number of particles and has advantages of easy implementation and fewer algorithm parameters. Based on the calculation functions of IK and the minimum distance of TACCS, pseudo codes of algorithms based on the single and bi-layer optimizations can be written as Algorithm A1 and Algorithm A2 in Appendix A. For the PSO, parameters include inertia weight $\omega$, maximum flying velocity $v_{max}$ of particles and the number of particles $N_p$, and they have major impacts on the convergence speed and optimization effect. Among them, the value of $v_{max}$ for each variable can be taken by 10% to 20% of its searching range, and if the ranges for the variables are different, the ratio $\phi$ of the value to its corresponding range can be taken as a uniform parameter for values of the maximum velocities of all variables, $0.1\le \phi \le 0.2$. Therefore, for the motion planning optimization, reasonable values of PSO parameters ($\omega$, $\phi$ and $N_p$) should be taken to obtain the optimal results according to several trails of each optimization. Moreover, in order to ensure obtain the global optimal result, the stopping criterion for iteration is defined by $\max \left[\left|f_{k-\tau -1}-f_{k-\tau }\right|, 0\le \tau \le 14\right]<\epsilon ,$ which means that the change in the objective value during fourteen successive iterations is less than a threshold value $\epsilon =0.001$.

5. Verification Based on Simulation Cases

In order to show the performance of two proposed optimization algorithms, this section designed two example cases. The principle of determining initial conditions (including the target pose and locations of obstacles) for the simulations involves ensuring the solvability of the optimization problem, i.e., there is at least one collision-free path during the conversion from the initial pose to the target pose. The operational environment of case 2 is shown in Figure 4 (the target structure is located on one side of capsule), and for that of case 1, the target structure and its surrounding obstacles are located on the top of the capsule. The target pose and the initial 3D locations of the feature points of the TACCS and obstacles in frame 1 (

Table A1. Target pose and the initial 3D locations of feature points of TACCS and obstacles in frame 1.

Structures	Characteristic Points		Coordinate			Radius/mm (Number of Edges)
	Structure Type	Number	X/mm	Y/mm	Z/mm
Robot	Polygon 1	1	−200	−200	400	-
		2	200	−200	400	-
		3	200	200	400	-
		4	−200	200	400	-
		5	−200	−200	470	-
		6	200	−200	470	/
		7	200	200	470	/
		8	−200	200	470	/
	Cylinder 2	1	0	0	470	180
		2	0	0	1010	180
	Cylinder 3	1	0	0	1010	180
		2	0	405	1010	180
	Cylinder 4	1	0	405	1010	198
		2	0	740	1010	198
	Cylinder 5	1	0	540	1010	198
		2	0	540	2504	198
	Cylinder 6	1	0	405	2504	198
		2	0	740	2504	198
	Cylinder 7	1	0	405	2504	180
		2	0	0	2504	180
	Cylinder 8	1	0	0	2504	180
		2	0	0	3764	180
	Cylinder 9	1	0	−270	3764	180
		2	0	270	3764	180
	Cylinder 10	1	0	0	3764	153
		2	0	0	4034	153
	Cylinder 11	1	0	0	4034	153
		2	0	0	4366	153
	Polygon 12	1	−200	−200	4366	/
		2	200	−200	4366	/
		3	200	200	4366	/
		4	−200	200	4366	/
		5	−200	−200	4516	/
		6	200	−200	4516	/
		7	200	200	4516	/
		8	−200	200	4516	/
	Cylinder 13	1	0	0	4516	100
		2	0	0	4816	100
	Cylinder 14	1	0	0	4816	100
		2	0	225	4816	100
	Cylinder 15	1	0	415	4816	110
		2	0	225	4816	110
	Cylinder 16	1	0	300	4816	110
		2	0	300	5646	110
	Cylinder 17	1	0	415	5646	110
		2	0	225	5646	110
	Cylinder 18	1	0	0	5646	100
		2	0	225	5646	100
	Cylinder 19	1	0	0	5646	100
		2	0	0	6346	100
	Cylinder 20	1	0	−150	6346	100
		2	0	150	6346	100
	Cylinder 21	1	0	0	6346	85
		2	0	0	6496	85
	Cylinder 22	1	0	0	6496	85
		2	0	0	6680	85
Obstacles	Polygonal obstacles 1 (Cases 1,2)	1	850	450	500	/
		2	1150	450	500	/
		3	1150	750	500	/
		4	850	750	500	/
		5	850	450	1300	/
		6	1150	450	1300	/
		7	1150	750	1300	/
		8	850	750	1300	/
	Polygonal obstacles 2 (Cases 1,2)	1	1840	−660	500	/
		2	2160	−660	500	/
		3	2160	−340	500	/
		4	1840	−340	500	/
		5	1840	−660	1500	/
		6	2160	−660	1500	/
		7	2160	−340	1500	/
		8	1840	−340	1500	/
	Cylindrical obstacles 3 (Cases 1,2)	1	800	−500	550	200 ($N_e$= 12)
		2	800	−500	1300	200 ($N_e$= 12)
	Cylindrical obstacles 4 (Cases 1,2)	1	1800	500	550	200 ($N_{e }$= 12)
		2	1800	500	1500	200 ($N_e$= 12)
	Polygonal obstacles 5 (Case 1)	1	2850	−150	700	/
		2	3150	−150	700	/
		3	3150	150	700	/
		4	2850	150	700	/
		5	2850	−150	850	/
		6	3150	−150	850	/
		7	3150	150	850	/
		8	2850	150	850	/
	Polygonal obstacles 6 (Case 1)	1	3580	−120	700	/
		2	3820	−120	700	/
		3	3820	120	700	/
		4	3580	120	700	/
		5	3580	−120	1100	/
		6	3820	−120	1100	/
		7	3820	120	1100	/
		8	3580	120	1100	/
	Cylindrical obstacles 7 (Case 1)	1	3000	−550	550	150 ($N_e$= 12)
		2	3000	−550	1000	150 ($N_e$= 12)
	Cylindrical obstacles 8 (Case 1)	1	3000	550	550	150 ($N_e$= 12)
		2	3000	550	1000	150 ($N_e$= 12)
	Polygonal obstacles 9 (Case 2)	1	2850	388.91	601.04	/
		2	3150	388.91	601.04	/
		3	3150	601.04	388.91	/
		4	2850	601.04	388.91	/
		5	2850	494.97	707.11	/
		6	3150	494.97	707.11	/
		7	3150	707.11	494.97	/
		8	2850	707.11	494.97	/
	Polygonal obstacles 10 (Case 2)	1	3580	410.12	579.83	/
		2	3820	410.12	579.83	/
		3	3820	579.83	410.12	/
		4	3580	579.83	410.12	/
		5	3580	692.97	862.67	/
		6	3820	692.97	862.67	/
		7	3580	862.67	692.97	/
		8	3580	862.67	692.97	/
	Cylindrical obstacles 11 (Case 2)	1	3000	0	777.82	150 ($N_e$= 12)
		2	3000	318.2	1096.02	150 ($N_e$= 12)
	Cylindrical obstacles 12 (Case 2)	1	3000	777.82	0	150 ($N_e$= 12)
		2	3000	1096.02	318.2	150 ($N_e$= 12)
Case	Target point	1	3000	0	950	/
		2	3000	636.4	676.4	/

) are given for the following simulation analysis based on the above basic principle.

From the proposed method, there also exists one important step before the optimization, which is to determine the upper and lower values of variables $\Delta x$, $\Delta y$, $\Delta z$, $\alpha$, $\beta$ and $\gamma$ according to the framework shown in Figure 8. Figure 9 and Figure 10 show the feasible spaces of variables in two example cases, wherein valid solutions of variables $\Delta x$, $\Delta y$ and $\Delta z$ and that of variables $\alpha$, $\beta$ and $\gamma$ are plotted as points in a 3D coordinate system, respectively, which finally form point clouds to show the feasible spaces.

In each sub-figure of Figure 9 and Figure 10, the red box is used to show the final space size chosen to further decrease the allowable ranges of variables that can help improve the optimization efficiency. The box selection principles include (1) trying to select regions with dense point clouds, which refers to the point clouds with higher density; (2) ensuring that the proportion of selected points to all points is greater than a reasonable value; and (3) taking the limits of the red box based on the above proportion requirement. From Figure 9 and Figure 10, since the density of the points is uniform, the regions for variables $\alpha$, $\beta$ and $\gamma$ are taken as the whole; the distribution of point clouds for variables $\Delta x$, $\Delta y$ and $\Delta z$ features sparse edges and a dense middle, and so a reasonable value of the proportion can largely improve the optimization efficiency under the premise of ensuring the optimization effect. In order to show the influence of the proportion of variables $\Delta x$, $\Delta y$ and $\Delta z$ on the optimization effect, the iterative curves of the sub-optimization 1 (since the size of the allowable range of variables $\Delta x$, $\Delta y$ and $\Delta z$ has a major impact on sub-optimization 1 of the bi-layer optimization) and the single optimization under different values of the proportion (50%, 70%, 90% and 100%) are shown in Figure 11, respectively, from which, when the proportion is taken by 90%, the optimization efficiency can be largely improved with only a small loss of the objective value. From two figures, the allowable ranges of six variables in two example cases can be finally obtained.

Moreover, as presented in Section 4, PSO parameters (including the inertia weight $\omega$, the velocity ratio $\phi$ and the particle number $N_p$) have a major influence on the optimization effect, and they are always related to ranges of variables and the complexity of the optimization problem. Therefore, different values for these parameters should be taken to perform the optimizations before cases analysis, aiming to determine the reasonable values for PSO parameters and ensure the optimization effect. Iterative curves for the single optimization and three sub-optimizations of the bi-layer optimization with different values of PSO parameters are shown in Figure 12. From this figure, the reasonable values for the PSO parameters can be determined as $\left[\omega =0.8, \phi =0.15, N_p=15\right]$, $\left[\omega =1.2, \phi =0.15, N_p=10\right]$ and $\left[\omega =0.8, \phi =0.10, N_p=10\right]$ for the first-layer optimization (sub-optimization 1), the second-layer sub-optimizations of the bi-layer optimization (sub-optimizations 2 and 3) and the single optimization, respectively, which can ensure the fast convergence speed and global optimization capability.

Since the randomness of PSO iterations has a major impact on the convergence time of optimization, in order to analysis the influence degrees of the randomness for two proposed algorithms, the single and bi-layer optimizations for two example cases are performed 30 times. The comparison of convergence times of two optimization algorithms is shown in

Table 3. Comparison of convergence times of two optimization algorithms.

Example Case	Optimization	$\mathit{\mu }/\mathit{s}$	$\mathit{\sigma }/\mathit{s}$	${\mathit{c}}_{\mathit{v}}/\mathit{\%}$
Case 1	Single	145.54	51.67	35.51
	Bi-layer	40.12	10.58	26.39
	Difference	72.43%	79.52%	25.68%
Case 2	Single	52.48	18.66	35.55
	Bi-layer	19.08	3.26	17.10
	Difference	63.64%	82.53%	51.90%

, wherein $\mu$, $\sigma$ and $c_v$ mean the average convergence time, the standard deviation and the coefficient of variation of 30 optimizations, respectively, while $\mu$ is used to evaluate the average optimization efficiency, $\sigma$ and $c_v$ are used to evaluate the robustness of the algorithms, $\sigma$ can reflect the dispersion of convergence time, $\sigma =\sqrt{{\displaystyle \frac{1}{30}}{\displaystyle \sum }_{i=1}^{30}{\left(t_i-\mu \right)}^2}$ and $c_v$ is the dimensionless representation of $\sigma$, $c_v=\sigma /\mu \times 100\%$, (and it can be used to compare the convergence performances of different optimizations). From the result, we can obtain the following conclusions: (1) The average convergence times of the bi-layer optimization algorithm in two cases are reduced by 72.43% and 63.64%, respectively, compared with that of the single optimization algorithm, which are calculated by $\left({\mu }_s-{\mu }_b\right)/{\mu }_s\times 1$00%, wherein ${\mu }_s$ and ${\mu }_b$ are the average convergence time of the single and bi-layer algorithms. The reason is that reducing the number of variables can largely increase the optimization speed; additionally, the collision judgement during the whole motion is placed outside the optimization iteration process. (2) Values of the dispersion of the bi-layer optimization algorithm in two cases are reduced by 79.52% and 82.53%, respectively, and values of the coefficient of variation in two cases are reduced by 25.68% and 51.90%, respectively, compared with that of the single optimization algorithm. That is, the randomness of PSO iterations has a greater impact on the convergence time of the single optimization algorithm. (3) Although the initial conditions have a major impact on the convergence time of optimizations, the average convergence times of the single optimization in two cases are obviously larger than that of the bi-layer optimization, which indicates that the advantage of bi-layer optimization is confidently certain under different initial conditions.

The constraints and the optimal results of the main parameters based on three algorithms, including the bi-layer optimization, single optimization and traditional optimization, are listed in

Table 4. Constraints, optimal results of main parameters based on three algorithms, including the bi-layer opt. (bi-layer optimization), single opt. (single optimization) and trad. opt. (traditional optimization).

Example Case	Parameter	Constraints	Bi-Layer Opt.	Single Opt.	Difference 1	Trad. Opt.	Difference 2
Case 1	$\mathsf{\Delta }x/\left(\mathrm{mm}\right)$	[1500, 3000]	2016.08	1628.35	-	2500.00	-
	$\mathsf{\Delta }y/\left(\mathrm{mm}\right)$	[−1000, 1000]	604.94	675.01	-	815.81	-
	$\mathsf{\Delta }z/\left(\mathrm{mm}\right)$	[820, 2200]	2148.84	1797.81	-	2097.21	-
	$\alpha /\left(\mathrm{deg}\right)$	[−180, 180]	92.66	−24.45	-	104.74	-
	$\beta /\left(\mathrm{deg}\right)$	[−90, 90]	76.65	64.71	-	5.24	-
	$\gamma /\left(\mathrm{deg}\right)$	[−180, 180]	86.73	2.64	-	90.00	-
	$V_{max}/\left(\mathrm{deg}/\mathrm{s}\right)$	≤120	77.71	64.51	-	55.59	-
	$A_{max}/\left(\mathrm{deg}/{\mathrm{s}}^2\right)$	≤200	103.80	95.67	-	54.13	-
	$T \left(\mathrm{s}\right)$	≤20	4.67	4.98	6.22%	4.51	3.55%
	$Path$ $\left(\mathrm{deg}\right)$	-	9.82	9.06	8.39%	11.06	11.21%
	F	-	14.49	14.04	3.21%	15.57	6.94%
Case 2	$\Delta x/\left(\mathrm{mm}\right)$	[1500, 2800]	1871.38	1652.65	-	1653.14	-
	$\Delta y/\left(\mathrm{mm}\right)$	[−200, 1800]	1102.91	1246.74	-	1214.89	-
	$\Delta z/\left(\mathrm{mm}\right)$	[800, 1880]	1394.95	1521.54	-	1619.18	-
	$\alpha /\left(\mathrm{deg}\right)$	[−180, 180]	22.37	−77.82	-	67.21	-
	$\beta /\left(\mathrm{deg}\right)$	[−90, 90]	72.61	25.74	-	45.72	-
	$\gamma /\left(\mathrm{deg}\right)$	[−180, 180]	17.02	−72.95	-	59.02	-
	$V_{max}/\left(\mathrm{deg}/\mathrm{s}\right)$	≤120	95.97	96.94	-	81.29	-
	$A_{max}/\left(\mathrm{deg}/{\mathrm{s}}^2\right)$	≤200	136.82	138.06	-	115.61	-
	$T \left(\mathrm{s}\right)$	≤20	4.21	4.37	3.66%	5.45	22.75%
	$Path \left(\mathrm{deg}\right)$	-	11.37	11.42	0.44%	10.84	4.89%
	F	-	15.58	15.79	1.33%	16.29	4.36%

, wherein $V_{max}$ and $A_{max}$ mean the maximum values of angular velocity and acceleration of all robotic joints and the values of difference 1 and difference 2 are calculated by $\left|p_s-p_b\right|/p_s\times 100\%$ and $\left|p_t-p_b\right|/p_t\times 100\%$, while $p_s$, $p_b$ and $p_t$ represent the parameter values based on single optimization, bi-layer optimization and traditional optimization, respectively. The results show that values for parameters $\mathsf{\Delta }x$, $\mathsf{\Delta }y$, $\mathsf{\Delta }z$, $\alpha$, $\beta$, $\gamma$, $V_{max}$ and $A_{max}$, based on three algorithms, are all less than their corresponding limits given in constraints; compared with the bi-layer optimization, the overall optimization objectives in two cases obtained based on the traditional optimization increase 6.94% and 4.36%, respectively. In particular, the energy consumption increases 11.21% in case 1, and the motion time increases 22.75% in case 2; the differences of the overall optimization objectives based on the bi-layer optimization and the single optimization in two cases are only 3.21% and 1.33%, respectively. The main reasons for the differences include (1) the hierarchical processing of the optimization problem taking into account the end-effector pose of the first-level arm on the motion of the second-level arm, which can rapidly find the optimal result and finally improve the optimization efficiency, but it also sacrifices a small amount of the valid solution space, which leads to some objectives based on single optimization being potentially less than that based on bi-layer optimization (for example, the overall objective based on single optimization is 3.21% smaller than that based on bi-layer optimization in case 1); (2) in spite of the above reasons, it is very difficult for single optimization to find the real global optimal solution, so its objectives may be higher than that of the bi-layer optimization (for example, the overall objective based on single optimization is 1.33% larger than that based on bi-layer optimization). For the constraints based on bi-layer optimization, the maximum velocity and acceleration of robotic joints are 77.71 $\mathsf{d}\mathsf{e}\mathsf{g}/\mathsf{s}$ and 103.80 $\mathsf{d}\mathsf{e}\mathsf{g}/{\mathsf{s}}^2$, and they are obviously less than their allowable values 120 $\mathsf{d}\mathsf{e}\mathsf{g}/\mathsf{s}$ and 200 $\mathsf{d}\mathsf{e}\mathsf{g}/{\mathsf{s}}^2$, respectively. Also, Figure 13 gives the variation in the shortest distance between the TACCS and its obstacles over motion time based on two algorithms, which indicates that the optimal trajectories of the TACCS based on two algorithms can ensure the motion safety of the system; additionally, in order to clearly show the details of the shortest distance between the TACCS and obstacles, the corresponding structure no. and obstacle no. are also shown in Figure 10 (for example, R2-O1 means the distance between the second robotic structure and the first obstacle is the shortest distance, wherein the no. information can be found in Table A1).

Moreover, taking example case 1 as the objective, curves of angular parameters of each joint of TACCS and a series of motion poses relative to capsule based on the bi-layer optimization algorithm are shown in Figure 14 and Figure 15, and those based on the single optimization algorithm are shown in Figure A1 and Figure A2, which can further intuitively demonstrate the optimization effect of two algorithms, including the guarantee of the motion stability and safety. Moreover, taking example case 2 as the objective, a series of motion poses relative to the capsule based on the bi-layer and single optimization algorithms are shown in Figure A3 and Figure A4.

From the comprehensive analysis results of convergence speed and optimization effect, it can be concluded that the bi-layer optimization has a higher execution efficiency under the premise of ensuring the optimization effect.

6. Conclusions and Future Works

6.1. Conclusions

This work proposed the single and bi-layer optimization algorithms for the collision-free motion planning problem of the TACCS, aiming to optimize the operating time and energy consumption and considering both the kinematics and obstacle-avoidance constraints under the complex operational environment. For the implementation of optimizations, the functions for the inverse kinematics of TACCS and the shortest distance between the TACCS and its obstacles are established based on screw theory and a hybrid geometric envelope, respectively. During the optimization problem modeling, a single optimization problem with more than eight variables is converted into three optimization sub-problems with fewer variables of bi-layer optimization, which aims to improve the convergence efficiency of optimizations. Finally, two algorithms are applied into two example cases. Through experimental simulations, three main conclusions are obtained as follows:

(1)

The reasonable PSO parameters are selected to perform optimizations of two algorithms, both considering their influences on convergence time and optimization effect. Moreover, a box selection principle is designed to determine the allowable ranges of motion variables of the first level arm, which can further improve the execution speed of algorithms by sacrificing very little workspace. The above strategies can help ensure and improve the optimization performances of two algorithms.

(2)

The performance indexes are taken as the average value and dispersion of convergence times of 30 executions, which are obtained in two example cases and based on two algorithms. The comparison of performance indexes indicates that the bi-layer optimization has a higher convergence speed than the single optimization, which can be increased by more than 60%, and the randomness of the PSO iterations has less of an impact on the convergence speed of the bi-layer optimization.

(3)

The optimal results based on two algorithms are compared to show the significance of this work. The results indicate that values of motion parameters and shortest distances during the whole motion are less than their corresponding limits given in constraints; additionally, the overall optimization objective based on the bi-layer optimization is smaller than that based on the single optimization. Combining with the comparison result of convergence speed, it can be concluded that the bi-layer optimization has a higher execution efficiency under the premise of ensuring the optimization effect.

6.2. Future Works

In this work, the proposed algorithms can be used for the collision-free motion planning of the TACCS under the complex structured environment, for which the locations of obstacles and targets are given information, i.e., they are fixed or regularly changed. Therefore, this work cannot solve the real-time collision-free motion planning of the TACCS in randomly dynamic scenarios, which will be one focus of future works. Moreover, the real application of the proposed algorithms will be the other focus of future works, for which we will actively seek cooperation opportunities with aerospace enterprises and apply our result in actual scenarios.