Coordinated Collision-Free Trajectory Planning for a Discrete-Serpentine Heterogeneous Dual-Arm Space Robot Based on Equivalent Kinematics

Abstract

Compared with a single discrete or serpentine arm, the discrete-serpentine heterogeneous dual-arm space robot (DSHDASR) combines the advantages of both kinds of arms, enabling it to perform complex on-orbit missions. The structural complexity of DSHDASR and cluttered environments pose a significant challenge in modeling and collision-free motion planning. To tackle the issue, this paper proposes a coordinated collision-free trajectory planning method for DSHDASR based on equivalent kinematics. Firstly, the mechanism and universal kinematics of DSHDASR are analyzed. Then, the equivalent kinematics model is established based on the spatial arc method. The whole kinematics chain of DSHDASR is described by the parameters of equivalent curves composed of space arcs. Furthermore, taking the target satellite transposition as an example, a coordinated collision-free trajectory planning is presented for DSHDASR based on the equivalent kinematics. The trajectory planning problem is formulated as the minimization of the objective function, which consists of kinematics constraint equations and obstacle avoidance constraint equations. The parameters of equivalent curves are obtained by optimizing the objective function, and the joint angles of DSHDASR can be determined using the above parameters. Finally, the mission of the target satellite transposition is simulated, and the results demonstrate the proposed method.

1. Introduction

Space robotic manipulators play a critical role in the on-orbit service (OOS) [1,2], especially in the removal of space debris [3] and the maintenance of malfunctioned satellites [4,5]. Robinson and Davies [6] have categorized the existing robots into three types: discrete robots, serpentine robots, and continuum robots. Continuum robots that are typically driven by air or fluid rarely serve as space manipulators due to the challenges in accurate modeling and precision control [7]. Consequently, discrete robots and serpentine robots are the types most widely adopted in the OOS. Here, the discrete robot refers to a manipulator that is composed of a series of rigid links, each of which is connected by the joint with a single degree of freedom (DOF). For a serpentine robot, it usually consists of multiple, much shorter, rigid links that are connected by numerous joints. Moreover, a single joint has more than one rotational axis. When discrete robots or serpentine robots implement on-orbit operation missions, they face a common challenge, namely the trajectory planning problem.

To address this challenge, scholars have conducted a great amount of research. Early in the research, the trajectory planning of discrete robots attracted significant attention. This is because a discrete robot has no more than seven DOFs, and in most cases, analytical inverse kinematics solutions are available [8,9,10]. Different from discrete robots, serpentine manipulators possess a large number of DOFs, usually more than ten, resulting in the absence of the closed-form analytical solution [11]. Therefore, the approaches utilizing the Jacobian pseudo-inverse as the useful numerical solution are widely employed to deal with the trajectory planning problem of serpentine manipulators [12,13]. Furthermore, another effective approach is based on the configuration analysis of serpentine manipulators. Dione and Hasegawa [14] proposed a novel kinematic resolution method by controlling the strategic points along the serpentine arm. On the basis of the standard forward and backward reaching inverse kinematics (FABRIK) strategy, Niu et al. [15] developed a shape-controllable FABRIK method for tracking the desired trajectory. Discrete manipulators have a high payload capacity, and serpentine arms possess the excellent characteristics of high dexterity and small size. Therefore, a space robotic system that consists of the discrete arm and serpentine arm combines the advantages of both arms and has thus become a significant and active area of research. Ouyang et al. [16] designed a novel hybrid rigid-continuum dual-arm space robot. Meanwhile, a coordinated trajectory planning method based on the dynamic coupling feature was proposed to minimize the interaction influences between the space robot and the target satellite. While the aforementioned studies have tackled the trajectory planning problem for discrete arms, serpentine arms, or heterogeneous arms, the obstacle avoidance was not considered in these methods.

In order to achieve obstacle avoidance in complex environments, a large number of algorithms have been proposed to solve the problem for discrete or serpentine robots. For discrete manipulators, pseudo-inverse type methods are also employed for collision-free trajectory planning [17,18]. Moreover, Papadopoulos et al. [19] employed smooth and continuous polynomials to plan a collision-free path for nonholonomic platforms with discrete arms. Rybus et al. [20] presented a path planning method for a discrete arm installed on free-floating spacecraft. In the algorithm, the path of the discrete manipulator was determined by the coordinates of spline knots. Based on the genetic algorithm, Seddaoui and Saaj [21] developed an optimal trajectory generator that could enable the discrete arm to avoid obstacles. Two kinds of modes were set in the path generator. The former was the direct tracking mode that was used to actively compensate for undesirable dynamic coupling. The latter was the dynamic coupling tracking mode, which was adopted to decrease energy consumption for a discrete space manipulator reaching the target. Furthermore, reference [22] presented a comprehensive review of serpentine arms, summarizing the trajectory planning methods considering obstacle avoidance. The above reference stated that trajectory planning with obstacle avoidance could be classified into two main categories: workspace trajectory planning and joint space trajectory planning. In terms of workspace trajectory planning, the head-tracking method is usually used to address the issue [23]. In practice, compared with workspace trajectory planning, joint space trajectory planning has broader applicability because it is unnecessary to compute inverse kinematics. Regarding joint space trajectory planning, Tian et al. [24] developed an overall arm-shape planning method of serpentine manipulators based on the improved artificial potential field method. Zhang et al. [25] proposed a hybrid algorithm integrating the Rapidly exploring Random Tree (RRT) with shape control to make serpentine arms implement operation tasks in an unstructured environment. Subsequently, an improved RRT algorithm was proposed based on the ancestor node backtracking strategy [26]. Additionally, a range of intelligent optimization approaches have also been developed for the obstacle avoidance of serpentine manipulators, such as the genetic algorithm [27], particle swarm optimization [28], and natural cyclic coordinate descent [29]. Despite recent advances, the above studies have only focused on the obstacle avoidance of either discrete or serpentine manipulators.

As discussed in reference [16], the discrete-serpentine heterogeneous dual-arm space robot was proposed. The key reasons why dual arms are needed and why they have different settings are as follows. When dealing with on-orbit services in complex environments, the discrete-serpentine heterogeneous dual-arm space robot combines the advantages of discrete arms and serpentine arms and can complete tasks better than other single- or dual-arm robots. It takes advantage of the high rigidity and high operability of the discrete arm to capture and move target satellites, and it utilizes the high flexibility and good compliance of the serpentine arm to detect the surrounding environment of the end-effector of the discrete arm to prevent possible dangers. However, little research has focused on the coordinated collision-free trajectory planning of discrete-serpentine heterogeneous dual-arm space robots.

In this paper, a discrete-serpentine heterogeneous dual-arm space robot (DSHDASR), which consists of a non-offset discrete manipulator and a segmented cable-driven hyper-redundant serpentine manipulator, is introduced for the transposition of a target satellite. During mission execution, the discrete arm is the mission manipulator to move the target satellite to the desired pose. Meanwhile, the serpentine manipulator serves as an auxiliary arm, providing real-time monitoring for the discrete arm. However, the numerous payloads on the base of the DSHDASR increase the risk of collision with both arms. To address the issue, a coordinated collision-free trajectory planning method is proposed for the DSHDASR by using the equivalent kinematics. Based on the space arc method, the equivalent kinematics model of the DSHDASR is established. In other words, the kinematics of the DSHDASR can be described by the parameters of equivalent kinematics curves, which are composed of spatial arcs. The collision avoidance is achieved by controlling the distance between the equivalent kinematics curves and collisions.

The novelty and contributions of this paper are as follows. Firstly, a new type of heterogeneous dual-arm space robot is proposed, which is composed of a non-offset discrete manipulator and a segmented cable-driven hyper-redundant serpentine manipulator. The heterogeneous dual-arm space robot has the advantages of both kinds of arms. Secondly, the equivalent kinematics model of the heterogeneous space robot is established through the space arc method, which can reduce the computational load of modeling. Finally, the coordinated obstacle avoidance trajectory planning method is presented based on the equivalent kinematics model. Using the target satellite transposition as an application scenario of the heterogeneous space robot, a co-simulation system is built to verify the proposed method.

The remainder of this paper is organized as follows: Section 2 introduces the structure analysis of the DSHDASR and establishes its kinematics model by traditional method. In Section 3, the equivalent kinematics equations of DSHDASR are derived by the space arc method. Based on the equivalent kinematics model, Section 4 proposes the coordinated collision-free trajectory planning algorithm for the DSHDASR. In Section 5, the coordinated motion planning simulation is carried out to verify the proposed method. Section 6 summarizes the study and presents the conclusions.

2. Mechanism and Kinematics of the Heterogeneous Space Robot

2.1. Structure Analysis of Discrete-Serpentine Heterogeneous Space Robotic System

As shown in Figure 1, the discrete-serpentine heterogeneous dual-arm space robot (DSHDASR) mainly consists of a base, a non-offset discrete manipulator (denoted by Arm-a), and a segmented cable-driven hyper-redundant serpentine manipulator (named Arm-b) with a control box.

The non-offset discrete arm has n_a joints, each of which only possesses a rotation axis driven by a motor. Unlike the discrete arm, the segmented serpentine manipulator is composed of n_b segments, each of which possesses m associated universal joints. Therefore, the number of joints is m × n_b for the serpentine arm. A universal joint contains two rotation axes, i.e., the pitch axis and the yaw axis. The associated universal joints within the same segment are hybrid active and passive joints. In other words, the first universal joint is an active joint that can be moved according to the requirement, and the others are passive joints that synchronously move with the first universal joint in the same direction and angles. In a segment, any two adjacent universal joints are connected by a rigid link. Because the length of each link is equal, all segments have the identical mechanical structure.

Due to their same structure, each segment has the identical joint layout. Here, the number of universal joints in each segment is m for Arm-b and m is an even number (i.e., m = 2, 4, 6, …). If m is equal to 2, the joint layout of each segment is pitch-yaw-yaw-pitch and the degrees of freedom (DoF) is 4. Following the same principle, considering a segment composed of m universal joints, its joint layout consists of (m/2) pitch-yaw-yaw-pitch units. Therefore, the degrees of freedom are 2m. Because the universal joints in a segment are hybrid active and passive, the movement law of universal joints in ith segment can be expressed as follows.

$$\left\{\begin{array}{l}{\theta }_{i,j,1}^{\mathrm{b}}={\theta }_{i,j+1,2}^{\mathrm{b}}\\ {\theta }_{i,j,2}^{\mathrm{b}}={\theta }_{i,j+1,1}^{\mathrm{b}}\end{array}\right. i=1,2,\dots ,n_{\mathrm{b}}; j=1,2,\dots ,m-1$$

(1)

where ${\theta }_{i,j,k}^{\mathrm{b}}$ (k = 1, 2) represents the rotation angle around the kth rotation axis of the jth universal joint in the ith segment. For ease of calculation, we define ${\theta }_{i,j,1}^{\mathrm{b}}$ and ${\theta }_{i,j,2}^{\mathrm{b}}$ as ${\theta }_{i,1}^{\mathrm{b}}$ and ${\theta }_{i,2}^{\mathrm{b}}$. Therefore, all rotation angles in the ith segment can be described by ${\theta }_{i,1}^{\mathrm{b}}$ and ${\theta }_{i,2}^{\mathrm{b}}$ based on Equation (1).

2.2. Kinematics of the Discrete-Serpentine Heterogeneous Dual-Arm Space Robot

As shown in Figure 1, {x_I, y_I, z_I} represents the inertial frame, and the base’s frame is {x₀, y₀, z₀}. Taking the inertial frame as the reference frame, the position vector of the Arm-k (k = a, b) tip can be obtained as follows.

$$\left\{\begin{array}{l}{p}_{\mathrm{e}}^{\mathrm{a}}=r_0+b_0^{\mathrm{a}}+{\displaystyle \sum _{i=1}^{n_{\mathrm{a}}}\left(a_i^{\mathrm{a}}+b_i^{\mathrm{a}}\right)\in {\mathbf{R}}^{3\times 1}}\\ p_{\mathrm{e}}^{\mathrm{b}}=r_0+b_0^{\mathrm{b}}+{\displaystyle \sum _{i=1}^{n_{\mathrm{b}}}{\displaystyle \sum _{j=1}^m\left(a_{i,j}^{\mathrm{b}}+b_{i,j}^{\mathrm{b}}\right)\in {\mathbf{R}}^{3\times 1}}}\end{array}\right.$$

(2)

where $r_0$ is the position vector of the base, and $b_0^k$ (k = a, b) is the vector from base’s centroid to the first joint of Arm-k. $a_i^{\mathrm{a}}$ is the position vector from the ith joint $J_i^{\mathrm{a}}$ to the centroid of the ith link $B_i^{\mathrm{a}}$. $b_i^{\mathrm{a}}$ represents the position vector from the centroid of $B_i^{\mathrm{a}}$ to the $J_{i+1}^{\mathrm{a}}$. $a_{i,j}^{\mathrm{b}}$ is the position vector from the jth joint $J_{i,j}^{\mathrm{b}}$ to the centroid of the jth link $B_{i,j}^{\mathrm{b}}$ in the ith segment. $b_{i,j}^{\mathrm{b}}$ is the position vector from the centroid of $B_{i,j}^{\mathrm{b}}$ to the $J_{i,j+1}^{\mathrm{b}}$.

Differentiating Equation (2), the end-effector velocities of Arm-a and Arm-b are as follows.

$$\left\{\begin{array}{l}{v}_{\mathrm{e}}^{\mathrm{a}}=v_0+{\omega }_0\times b_0^{\mathrm{a}}+{\displaystyle \sum _{i=1}^{n_{\mathrm{a}}}\left[{\omega }_i^{\mathrm{a}}\times \left(a_i^{\mathrm{a}}+b_i^{\mathrm{a}}\right)\right]}\in {\mathbf{R}}^{3\times 1}\\ v_{\mathrm{e}}^{\mathrm{b}}=v_0+{\omega }_0\times b_0^{\mathrm{b}}+{\displaystyle \sum _{i=1}^{n_{\mathrm{b}}}{\displaystyle \sum _{j=1}^m\left[{\omega }_{i,j}^{\mathrm{b}}\times \left(a_{i,j}^{\mathrm{b}}+b_{i,j}^{\mathrm{b}}\right)\right]}}\in {\mathbf{R}}^{3\times 1}\end{array}\right.$$

(3)

where $v_0$ and ${\omega }_0$ are the linear velocity and angular velocity of base, respectively. ${\omega }_i$ is the angular velocity of $J_i^{\mathrm{a}}$, and ${\omega }_{i,j}$ is the angular velocity of $J_{i,j}^{\mathrm{b}}$.

Based on the relative motion, the angular velocities ${\omega }_i$ and ${\omega }_{i,j}$ are obtained as follows.

$$\left\{\begin{array}{l}{\omega }_i^{\mathrm{a}}={\omega }_0+{\displaystyle \sum _{j=1}^i{k}_j^{\mathrm{a}}}{\dot{\theta }}_j^{\mathrm{a}}\in {\mathbf{R}}^{3\times 1}\\ {\omega }_{i,j}^{\mathrm{b}}={\omega }_0+\left[{\displaystyle \sum _{h=1}^{i-1}{\displaystyle \sum _{l=1}^m\left(k_{h,l,1}^{\mathrm{b}}\cdot {\dot{\theta }}_{h,l,1}^{\mathrm{b}}+k_{h,l,2}^{\mathrm{b}}\cdot {\dot{\theta }}_{h,l,2}^{\mathrm{b}}\right)}}\right]+{\displaystyle \sum _{l=1}^j\left(k_{i,l,1}^{\mathrm{b}}\cdot {\dot{\theta }}_{i,l,1}^{\mathrm{b}}+k_{i,l,2}^{\mathrm{b}}\cdot {\dot{\theta }}_{i,l,2}^{\mathrm{b}}\right)}\in {\mathbf{R}}^{3\times 1}\end{array}\right.$$

(4)

where $k_i^{\mathrm{a}}$, ${\dot{\theta }}_i^{\mathrm{a}}$, and $p_i^{\mathrm{a}}$ are the rotation axis vector, angular velocity, and position vector of $J_i^{\mathrm{a}}$, respectively. $k_{i,j,k}^{\mathrm{b}}$ and ${\dot{\theta }}_{i,j,k}^{\mathrm{b}}$ (k = 1, 2) represent the kth rotation axis vector and its corresponding angular velocity of $J_{i,j}^{\mathrm{b}}$. The $p_{i,j}^{\mathrm{b}}$ is the position vector of $J_{i,j}^{\mathrm{b}}$.

Substituting Equation (4) into Equation (3), the end-effector velocities of Arm-a and Arm-b can be rewritten as follows.

$$\left\{\begin{array}{l}{v}_{\mathrm{e}}^{\mathrm{a}}=v_0+{\omega }_0\times \left(p_{\mathrm{e}}^{\mathrm{a}}-r_0\right)+{\displaystyle \sum _{i=1}^{n_{\mathrm{a}}}\left[k_i^{\mathrm{a}}\times \left(p_{\mathrm{e}}^{\mathrm{a}}-p_i^{\mathrm{a}}\right)\right]{\dot{\theta }}_i^{\mathrm{a}}}\in {\mathbf{R}}^{3\times 1}\\ v_{\mathrm{e}}^{\mathrm{b}}=v_0+{\omega }_0\times \left(p_{\mathrm{e}}^{\mathrm{b}}-r_0\right)+{\displaystyle \sum _{i=1}^{n_{\mathrm{b}}}{\displaystyle \sum _{j=1}^m\left[\left(k_{i,j,1}^{\mathrm{b}}{\dot{\theta }}_{i,j,1}^{\mathrm{b}}+k_{i,j,2}^{\mathrm{b}}{\dot{\theta }}_{i,j,2}^{\mathrm{b}}\right)\times \left(p_{\mathrm{e}}^{\mathrm{b}}-p_{i,j}^{\mathrm{b}}\right)\right]}}\in {\mathbf{R}}^{3\times 1}\end{array}\right.$$

(5)

On the basis of the Equation (4), the angular velocities of the Arm-a tip and Arm-b tip can be derived as follows.

$$\left\{\begin{array}{l}{\omega }_{\mathrm{e}}^{\mathrm{a}}={\omega }_0+{\displaystyle \sum _{i=1}^{n_{\mathrm{a}}}{k}_i^{\mathrm{a}}}{\dot{\theta }}_i^{\mathrm{a}}\in {\mathbf{R}}^{3\times 1}\\ {\omega }_{\mathrm{e}}^{\mathrm{b}}={\omega }_0+{\displaystyle \sum _{i=1}^{n_b}{\displaystyle \sum _{j=1}^m\left(k_{i,j,1}^{\mathrm{b}}{\dot{\theta }}_{i,j,1}^{\mathrm{b}}+k_{i,j,2}^{\mathrm{b}}{\dot{\theta }}_{i,j,2}^{\mathrm{b}}\right)}}\in {\mathbf{R}}^{3\times 1}\end{array}\right.$$

(6)

Setting $x_{\mathrm{e}}^k={[{(v_{\mathrm{e}}^k)}^{{\rm T}},{({\omega }_{\mathrm{e}}^k)}^{{\rm T}}]}^{{\rm T}}$ (k = a, b) and combining Equations (5) and (6), the general kinematics of Arm-k under the free-floating condition can be expressed as follows.

$${\dot{x}}_{\mathrm{e}}^k=\left[\begin{array}{l}{v}_{\mathrm{e}}^k\\ {\omega }_{\mathrm{e}}^k\end{array}\right]=J_{\mathrm{b}}^k\left[\begin{array}{l}{v}_0\\ {\omega }_0\end{array}\right]+J_{\mathrm{m}}^k{\dot{\Theta }}^k, k=\mathrm{a},\mathrm{b}$$

(7)

where ${\dot{\Theta }}^k$ is the joint angular velocity of Arm-k. And ${\dot{\Theta }}^{\mathrm{a}}$ and ${\dot{\Theta }}^{\mathrm{b}}$ are set as follows.

$$\left\{\begin{array}{l}{\Theta }^{\mathrm{a}}={\left[\begin{array}{cccc}{\theta }_1^{\mathrm{a}}& {\theta }_2^{\mathrm{a}}& \dots & {\theta }_{n_{\mathrm{a}}}^{\mathrm{a}}\end{array}\right]}^{{\rm T}}\in {\mathbf{R}}^{n_{\mathrm{a}}\times 1}\\ {\Theta }^{\mathrm{b}}={\left[\begin{array}{ccccccc}{\theta }_{1,1}^{\mathrm{b}}& {\theta }_{1,2}^{\mathrm{b}}& \dots & {\theta }_{2n_{\mathrm{b}}-1,1}^{\mathrm{b}}& {\theta }_{2n_{\mathrm{b}}-1,2}^{\mathrm{b}}& {\theta }_{2n_{\mathrm{b}},1}^{\mathrm{b}}& {\theta }_{2n_{\mathrm{b}},2}^{\mathrm{b}}\end{array}\right]}^{{\rm T}}\in {\mathbf{R}}^{2n_{\mathrm{b}}\times 1}\end{array}\right.$$

(8)

$J_{\mathrm{b}}^k$ and $J_{\mathrm{m}}^k$ are Jacobian matrices correlated with the base motion and the Arm-k motion respectively, which can be calculated as follows.

$$J_{\mathrm{b}}^k=\left[\begin{array}{cc}{E}_3& -{\tilde{p}}_{0\mathrm{e}}^k\\ O_3& E_3\end{array}\right]\in {\mathbf{R}}^{6\times 6}, k=\mathrm{a},\mathrm{b}$$

(9)

$$J_{\mathrm{m}}^{\mathrm{a}}=\left[\begin{array}{ccc}{k}_1^{\mathrm{a}}\times \left(p_{\mathrm{e}}^{\mathrm{a}}-p_1^{\mathrm{a}}\right)& \dots & k_{n_{\mathrm{a}}}^{\mathrm{a}}\times \left(p_{\mathrm{e}}^{\mathrm{a}}-p_{n_{\mathrm{a}}}^{\mathrm{a}}\right)\\ k_1^{\mathrm{a}}& \dots & k_{n_{\mathrm{a}}}^{\mathrm{a}}\end{array}\right]\in {\mathbf{R}}^{6\times n_{\mathrm{a}}}$$

(10)

$$J_{\mathrm{m}}^{\mathrm{b}}=\left[\begin{array}{l}{k}_{1,1,1}^{\mathrm{b}}\times \mathsf{\delta }{p}_{\mathrm{e},\left(1,1\right)}^{\mathrm{b}} k_{1,1,2}^{\mathrm{b}}\times \mathsf{\delta }{p}_{\mathrm{e},\left(1,1\right)}^{\mathrm{b}} \dots k_{n_{\mathrm{b}},m,1}^{\mathrm{b}}\times \mathsf{\delta }{p}_{\mathrm{e},\left(n_{\mathrm{b}},m\right)}^{\mathrm{b}} k_{n_{\mathrm{b}},m,2}^{\mathrm{b}}\times \mathsf{\delta }{p}_{\mathrm{e},\left(n_{\mathrm{b}},m\right)}^{\mathrm{b}}\\ k_{1,1,1}^{\mathrm{b}}, k_{1,1,2}^{\mathrm{b}}, \dots k_{n_{\mathrm{b}},m,1}^{\mathrm{b}} k_{n_{\mathrm{b}},m,2}^{\mathrm{b}} \end{array}\right]\cdot H\in {\mathbf{R}}^{6\times \left(2n_{\mathrm{a}}\right)}$$

(11)

$$\mathsf{\delta }{p}_{\mathrm{e},\left(i,j\right)}^{\mathrm{b}}=p_{\mathrm{e}}^{\mathrm{b}}-p_{i,j}^{\mathrm{b}}, i=1,2,\dots ,n_{\mathrm{b}}; j=1,2,\dots ,m$$

(12)

where $E_3$ and $O_3$ are the unit matrix and the null matrix, respectively. $\mathsf{\delta }{p}_{\mathrm{e},\left(i,j\right)}^{\mathrm{b}}$ is the vector from the jth joint of the ith segment to the Arm-b tip. $p_{0\mathrm{e}}^k$ is the vector from the centroid of base to the Arm-k tip, i.e., $p_{0\mathrm{e}}^k=p_{\mathrm{e}}^k-p_0^k$. ${\tilde{p}}_{0\mathrm{e}}^k$ is the skew-symmetric matrix obtained by $p_{0\mathrm{e}}^k$ and it can be computed by the following.

$${\tilde{p}}_{0\mathrm{e}}^k=\left[\begin{array}{ccc}0& -z& y\\ z& 0& x\\ -y& x& 0\end{array}\right], \mathrm{if} p_{0\mathrm{e}}^k={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^{{\rm T}}, k=\mathrm{a},\mathrm{b}$$

(13)

Based on Equation (1), the joint angular velocity and joint angular acceleration of the segmented hyper-redundant serpentine manipulator (Arm-b) can be obtained as follows.

$$\left\{\begin{array}{l}{\dot{\theta }}_{i,j,1}^{\mathrm{b}}={\dot{\theta }}_{i,j+1,2}^{\mathrm{b}}; {\ddot{\theta }}_{i,j,1}^{\mathrm{b}}={\ddot{\theta }}_{i,j+1,2}^{\mathrm{b}}\\ {\dot{\theta }}_{i,j,2}^{\mathrm{b}}={\dot{\theta }}_{i,j+1,1}^{\mathrm{b}}; {\ddot{\theta }}_{i,j,2}^{\mathrm{b}}={\ddot{\theta }}_{i,j+1,1}^{\mathrm{b}}\end{array}\right. i=1,2,\dots ,n_{\mathrm{b}}; j=1,2,\dots ,m-1$$

(14)

Therefore, the coefficient matrix H in Equation (11) is given as follows.

$$H=\left[\begin{array}{c}{H}_1\\ H_2\\ ⋮\\ H_{n_{\mathrm{b}}-1}\\ H_{n_{\mathrm{b}}}\end{array}\right]\in {\mathbf{R}}^{\left(n_{\mathrm{b}}\times 2m\right)\times \left(2n_{\mathrm{b}}\right)}$$

(15)

where

$$H_1=\left[\begin{array}{cccccc}1& 0& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ 1& 0& 0& 0& \dots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 1& 0& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ 0& 1& 0& 0& \dots & 0\\ 1& 0& 0& 0& \dots & 0\end{array}\right]\in {\mathbf{R}}^{2m\times 2n_{\mathrm{b}}}, H_2=\left[\begin{array}{cccccc}0& 0& 1& 0& \dots & 0\\ 0& 0& 0& 1& \dots & 0\\ 0& 0& 0& 1& \dots & 0\\ 0& 0& 1& 0& \dots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& 0& 1& 0& \dots & 0\\ 0& 0& 0& 1& \dots & 0\\ 0& 0& 0& 1& \dots & 0\\ 0& 0& 1& 0& \dots & 0\end{array}\right]\in {\mathbf{R}}^{2m\times 2n_{\mathrm{b}}}$$

(16)

$$H_{n_{\mathrm{b}}-1}=\left[\begin{array}{cccccc}0& \dots & 1& 0& 0& 0\\ 0& \dots & 0& 1& 0& 0\\ 0& \dots & 0& 1& 0& 0\\ 0& \dots & 1& 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& \dots & 1& 0& 0& 0\\ 0& \dots & 0& 1& 0& 0\\ 0& \dots & 0& 1& 0& 0\\ 0& \dots & 1& 0& 0& 0\end{array}\right]\in {\mathbf{R}}^{2m\times 2n_{\mathrm{b}}}, H_{n_{\mathrm{b}}}=\left[\begin{array}{cccccc}0& \dots & 0& 0& 1& 0\\ 0& \dots & 0& 0& 0& 1\\ 0& \dots & 0& 0& 0& 1\\ 0& \dots & 0& 0& 1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& \dots & 0& 0& 1& 0\\ 0& \dots & 0& 0& 0& 1\\ 0& \dots & 0& 0& 0& 1\\ 0& \dots & 0& 0& 1& 0\end{array}\right]\in {\mathbf{R}}^{2m\times 2n_{\mathrm{b}}}$$

(17)

Assuming that the base of DSHDASR is controlled, the base will be motionless while the DSHDASR is implementing on-orbit operations. As a result, the general kinematics of Arm-k can be rewritten as follows.

$${\dot{x}}_{\mathrm{e}}^k=\left[\begin{array}{l}{v}_{\mathrm{e}}^k\\ {\omega }_{\mathrm{e}}^k\end{array}\right]=J_{\mathrm{m}}^k{\dot{\Theta }}^k, k=\mathrm{a},\mathrm{b}$$

(18)

3. Equivalent Kinematics Model Based on Space Arc Method

The discrete-serpentine heterogeneous dual-arm space robotic system combines the advantages of both discrete arms and serpentine arms when conducting the on-orbit operations, such as the target autonomous capture and maintenance. The trajectory planning plays a crucial role in these missions. The traditional approaches always utilize Jacobian-based kinematics to establish the relationship between joint trajectory and end-effector trajectory, thereby obtaining the desired trajectories through kinematic resolution. Although these methods are universal, their computational efficiency is lower. In order to deal with the problem, the equivalent kinematics model of the heterogeneous robotic system is established based on space arc method. Different from traditional methods, the equivalent kinematics model is able to achieve the mapping of end-effector trajectories to parameters of the curves made up by multiple space arcs. By this method, the efficiency of trajectory planning is enhanced greatly. It is important to note that the base of DSHDASR is controlled in the subsequent discussion.

3.1. Equivalent Kinematics Analysis

In the previous section, we established the kinematics of the DSHDASR with any arbitrary degree of freedom (DoF) by the traditional method. To clarify the concept of equivalent kinematics, the number of DoFs for the non-offset discrete manipulator is set as seven. The number of the segments for serpentine arm is set as five, and each segment has six associated universal joints. Therefore, the joint layout and Denavit–Hartenberg (D-H) coordinate frames of Arm-a and Arm-b are shown in Figure 2 and Figure 3, respectively.

As shown in Figure 2, $L_i^{\mathrm{a}}$ is the length of the ith link for Arm-a. $L_1^{\mathrm{a}}$ is equal to $L_7^{\mathrm{a}}$, and $L_3^{\mathrm{a}}$ is equal to $L_5^{\mathrm{a}}$, i.e., $L_1^{\mathrm{a}}=L_7^{\mathrm{a}}$, and $L_3^{\mathrm{a}}=L_5^{\mathrm{a}}$, respectively. As shown in Figure 3, $L_{i,j}^{\mathrm{b}}$ is the length of the ith link in the jth segment for Arm-b. Because all links of Arm-b have the same length, the equation $L_{i,1}^{\mathrm{b}}=L_{i,2}^{\mathrm{b}}=\dots =L_{i,5}^{\mathrm{b}}=L_{i,6}^{\mathrm{b}}$ is satisfied. For simplicity, the length of each link is set as $L^{\mathrm{b}}$.

According to the joint layout and joint movement characteristics of Arm-a and Arm-b, the schematic diagram of the equivalent kinematics for DSHDASR is illustrated in Figure 4. $k_{i,\mathrm{e}}^{\mathrm{a}}$ represents the direction of the ith link end for Arm-a. $p_{\mathrm{e}}^{\mathrm{a}}$ is the position vector of the Arm-a tip, whose direction is $k_{7,\mathrm{e}}^{\mathrm{a}}$, i.e., $k_{\mathrm{e}}^{\mathrm{a}}$. $p_{i,\mathrm{e}}^{\mathrm{b}}$ and $k_{i,\mathrm{e}}^{\mathrm{b}}$ are the position and direction of the ith segment’s endpoint for Arm-b. Therefore, the position vector and direction of the Arm-b tip are $p_{5,\mathrm{e}}^{\mathrm{b}}$ and $k_{5,\mathrm{e}}^{\mathrm{b}}$, respectively.

As shown in Figure 4, each link of the non-offset discrete arm is simplified into a straight line. The first straight line $\overrightarrow{p_1^{\mathrm{a}}{p}_2^{\mathrm{a}}}$ of Arm-a is extended in the opposite direction to obtain the virtual link ${\tilde{p}}_1^{\mathrm{a}}{p}_1^{\mathrm{a}}$ such that the length of ${\tilde{p}}_1^{\mathrm{a}}{p}_1^{\mathrm{a}}$ is equal to that of $p_2^{\mathrm{a}}{p}_4^{\mathrm{a}}$ or $p_4^{\mathrm{a}}{p}_6^{\mathrm{a}}$. Similarly, we extend the line $\overrightarrow{p_6^{\mathrm{a}}{p}_{\mathrm{e}}^{\mathrm{a}}}$ so as to determine the point ${\tilde{p}}_{\mathrm{e}}^{\mathrm{a}}$ and make the length of virtual link $p_6^{\mathrm{a}}{\tilde{p}}_{\mathrm{e}}^{\mathrm{a}}$ equal to that of $p_2^{\mathrm{a}}{p}_4^{\mathrm{a}}$ or $p_4^{\mathrm{a}}{p}_6^{\mathrm{a}}$. As a result, the lengths of ${\tilde{p}}_1^{\mathrm{a}}{p}_1^{\mathrm{a}}$, $p_2^{\mathrm{a}}{p}_4^{\mathrm{a}}$, $p_4^{\mathrm{a}}{p}_6^{\mathrm{a}}$, and $p_6^{\mathrm{a}}{\tilde{p}}_{\mathrm{e}}^{\mathrm{a}}$ are equal. Taking them as tangent lines of spatial arcs, the three inscribed arcs $C_1^{\mathrm{a}}$, $C_2^{\mathrm{a}}$, and $C_3^{\mathrm{a}}$ are created. The equivalent kinematics of Arm-a can be described by the spatial curve that is composed of $C_1^{\mathrm{a}}$, $C_2^{\mathrm{a}}$, and $C_3^{\mathrm{a}}$, which are tangent to the links of Arm-a.

Similarly, each link of the segmented serpentine manipulator is also considered as a straight line. Setting the point $p_{1,1}^{\mathrm{b}}$ as the initial point, we draw a straight line in the opposite direction of $k_{0,\mathrm{e}}^{\mathrm{b}}$ that is parallel to the central axis of the control box. Then, point ${\tilde{p}}_{1,1}^{\mathrm{b}}$ is selected to make the length of ${\tilde{p}}_{1,1}^{\mathrm{b}}{p}_{1,1}^{\mathrm{b}}$ equal to that of $p_{1,1}^{\mathrm{b}}{p}_{1,2}^{\mathrm{b}}$. As discussed in Section 2.1, the length of each link in a segment is equal. Therefore, taking the first segment for an example, if straight lines ${\tilde{p}}_{1,1}^{\mathrm{b}}{p}_{1,1}^{\mathrm{b}}$, $p_{1,1}^{\mathrm{b}}{p}_{1,2}^{\mathrm{b}}$, $p_{1,2}^{\mathrm{b}}{p}_{1,3}^{\mathrm{b}}$, $p_{1,3}^{\mathrm{b}}{p}_{1,4}^{\mathrm{b}}$, $p_{1,4}^{\mathrm{b}}{p}_{1,5}^{\mathrm{b}}$, and $p_{1,5}^{\mathrm{b}}{p}_{1,\mathrm{e}}^{\mathrm{b}}$ are the tangent lines of a space arc, the inscribed arc $C_1^{\mathrm{b}}$ is determined. For the rest of the segments, the inscribed arcs $C_2^{\mathrm{b}}$, $C_3^{\mathrm{b}}$, $C_4^{\mathrm{b}}$, and $C_5^{\mathrm{b}}$ are obtained in the same way. Therefore, the equivalent kinematics of Arm-b can also be expressed by a spatial curve that includes $C_1^{\mathrm{b}}$, $C_2^{\mathrm{b}}$, $C_3^{\mathrm{b}}$, $C_4^{\mathrm{b}}$, and $C_5^{\mathrm{b}}$ that are tangent to the links of Arm-b.

Through the above analysis, the equivalent kinematics of Arm-a or Arm-b are represented by a space curve comprised by multiple arcs. The configurations, end-effector positions, and end-effector directions of Arm-a or Arm-b can be described by the spatial curves. In other words, the kinematic solution problem of Arm-a and Arm-b is transformed into the determination of spatial arc parameters.

3.2. Kinematics Equivalence Model

As shown in Figure 5, the equivalent curves of Arm-a and Arm-b are composed of multiple spatial arcs. Therefore, each equivalent curve can be divided into multiple single-arc sets and double-arc sets. The constraint equations of equivalent kinematics will be derived through the modeling of the single-arc set and the double-arc set. Furthermore, the kinematics solution for Arm-a and Arm-b is achieved by solving the constraint equations. The specific process is given as follows.

3.2.1. Equivalent Kinematics Modeling for Arm-a

The kinematics equivalence curve of Arm-a is composed of the three arcs $C_1^{\mathrm{a}}$, $C_2^{\mathrm{a}}$, and $C_3^{\mathrm{a}}$, as shown in Figure 5. For $C_i^{\mathrm{a}}$ (i = 1, 2, 3), its initial position and tangent vector are $P_i^{\mathrm{a}}$ and $t_i^{\mathrm{a}}$, respectively. The endpoint position and tangent vector of $C_i^{\mathrm{a}}$ are $P_{i+1}^{\mathrm{a}}$ and $t_{i+1}^{\mathrm{a}}$, respectively. $R_i^{\mathrm{a}}$ and ${\phi }_i^{\mathrm{a}}$ are the radius and arc angle of $C_i^{\mathrm{a}}$. Here, the kinematics equivalence curve of Arm-a can be divided into a single-arc set ($C_1^{\mathrm{a}}$) and a double-arc set, which includes the spatial arcs $C_2^{\mathrm{a}}$ and $C_3^{\mathrm{a}}$.

For the single-arc set, ${\tilde{P}}_1^{\mathrm{a}}$ is the intersection point of the line along the direction of $t_1^{\mathrm{a}}$ and the line along the direction of $-t_2^{\mathrm{a}}$. The constraint equations of the single-arc set can be obtained as follows.

$$\left\{\begin{array}{l}{F}_1^{\mathrm{a}}={\phi }_1^{\mathrm{a}}-\arccos \left[{\left(t_1^{\mathrm{a}}\right)}^{{\rm T}}\cdot t_2^{\mathrm{a}}/\left(‖t_1^{\mathrm{a}}‖\cdot ‖t_2^{\mathrm{a}}‖\right)\right]\\ F_2^{\mathrm{a}}=‖P_2^{\mathrm{a}}-P_1^{\mathrm{a}}‖-2R_1^{\mathrm{a}}\cdot \sin \left({\phi }_1^{\mathrm{a}}/2\right)\end{array}\right.$$

(19)

On the basis of the equivalent kinematics analysis, the following equations hold true.

$$\left\{\begin{array}{l}{t}_1^{\mathrm{a}}=k_{1,\mathrm{e}}^{\mathrm{a}}, t_2^{\mathrm{a}}=k_{3,\mathrm{e}}^{\mathrm{a}}\\ P_1^{\mathrm{a}}=p_3^{\mathrm{a}}-\left(L_3^{\mathrm{a}}/2\right)k_{1,\mathrm{e}}^{\mathrm{a}}, P_2^{\mathrm{a}}=p_3^{\mathrm{a}}+\left(L_3^{\mathrm{a}}/2\right)k_{3,\mathrm{e}}^{\mathrm{a}}\\ R_1^{\mathrm{a}}=L_3^{\mathrm{a}}/\left[2\tan \left({\phi }_1^{\mathrm{a}}/2\right)\right]\end{array}\right.$$

(20)

Substituting Equation (20) into Equation (19), the following equations are obtained.

$$\left\{\begin{array}{l}{F}_1^{\mathrm{a}}={\phi }_1^{\mathrm{a}}-\arccos \left[{\left(k_{1,\mathrm{e}}^{\mathrm{a}}\right)}^{{\rm T}}\cdot k_{3,\mathrm{e}}^{\mathrm{a}}/\left(‖k_{1,\mathrm{e}}^{\mathrm{a}}‖\cdot ‖k_{3,\mathrm{e}}^{\mathrm{a}}‖\right)\right]\\ F_2^{\mathrm{a}}=‖\left(L_3^{\mathrm{a}}/2\right)\left(k_{1,\mathrm{e}}^{\mathrm{a}}+k_{3,\mathrm{e}}^{\mathrm{a}}\right)‖-L_3^{\mathrm{a}}\cos \left({\phi }_1^{\mathrm{a}}/2\right)\end{array}\right.$$

(21)

For the double-arc set including the spatial arcs $C_2^{\mathrm{a}}$ and $C_3^{\mathrm{a}}$, the point ${\tilde{P}}_2^{\mathrm{a}}$ is on the line along the direction of $t_2^{\mathrm{a}}$, and the point ${\tilde{P}}_3^{\mathrm{a}}$ is on the line along the direction of $-t_4^{\mathrm{a}}$, respectively. The straight line $\overrightarrow{{\tilde{P}}_2^{\mathrm{a}}{\tilde{P}}_3^{\mathrm{a}}}$ is the common tangent of the two arcs $C_2^{\mathrm{a}}$ and $C_3^{\mathrm{a}}$. Therefore, the constraint equations of the double-arc set are obtained as follows.

$$\left\{\begin{array}{l}{F}_3^{\mathrm{a}}={\phi }_2^{\mathrm{a}}-\arccos \left[{\left(t_2^{\mathrm{a}}\right)}^{{\rm T}}\cdot \left({\tilde{P}}_3^{\mathrm{a}}-{\tilde{P}}_2^{\mathrm{a}}\right)/\left(‖t_2^{\mathrm{a}}‖\cdot ‖{\tilde{P}}_3^{\mathrm{a}}-{\tilde{P}}_2^{\mathrm{a}}‖\right)\right]\\ F_4^{\mathrm{a}}=‖\left[{\tilde{P}}_2^{\mathrm{a}}+\left({\tilde{P}}_3^{\mathrm{a}}-{\tilde{P}}_2^{\mathrm{a}}\right)/2\right]-P_2^{\mathrm{a}}‖-2R_2^{\mathrm{a}}\cdot \sin \left({\phi }_2^{\mathrm{a}}/2\right)\\ F_5^{\mathrm{a}}={\phi }_3^{\mathrm{a}}-\arccos \left[{\left({\tilde{P}}_3^{\mathrm{a}}-{\tilde{P}}_2^{\mathrm{a}}\right)}^{{\rm T}}\cdot t_4^{\mathrm{a}}/\left(‖{\tilde{P}}_3^{\mathrm{a}}-{\tilde{P}}_2^{\mathrm{a}}‖\cdot ‖t_4^{\mathrm{a}}‖\right)\right]\\ F_6^{\mathrm{a}}=‖P_4^{\mathrm{a}}-\left[{\tilde{P}}_2^{\mathrm{a}}+\left({\tilde{P}}_3^{\mathrm{a}}-{\tilde{P}}_2^{\mathrm{a}}\right)/2\right]‖-2R_3^{\mathrm{a}}\cdot \sin \left({\phi }_3^{\mathrm{a}}/2\right)\end{array}\right.$$

(22)

Similarly, through the equivalent kinematics analysis, the following equations can be obtained.

$$\left\{\begin{array}{l}{t}_2^{\mathrm{a}}=k_{3,\mathrm{e}}^{\mathrm{a}}, t_4^{\mathrm{a}}=k_{\mathrm{e}}^{\mathrm{a}}\\ {\tilde{P}}_2^{\mathrm{a}}=p_3^{\mathrm{a}}+L_3^{\mathrm{a}}{k}_{3,\mathrm{e}}^{\mathrm{a}}, {\tilde{P}}_3^{\mathrm{a}}=p_{\mathrm{e}}^{\mathrm{a}}-L_7^{\mathrm{a}}{k}_{\mathrm{e}}^{\mathrm{a}}, P_4^{\mathrm{a}}=p_{\mathrm{e}}^{\mathrm{a}}+\left(L_5^{\mathrm{a}}/2-L_7^{\mathrm{a}}\right)k_{\mathrm{e}}^{\mathrm{a}}\\ R_2^{\mathrm{a}}=L_3^{\mathrm{a}}/\left[2\tan \left({\phi }_2^{\mathrm{a}}/2\right)\right], R_3^{\mathrm{a}}=L_5^{\mathrm{a}}/\left[2\tan \left({\phi }_3^{\mathrm{a}}/2\right)\right]\end{array}\right.$$

(23)

Substituting Equation (23) into Equation (22), the following equations are obtained.

$$\left\{\begin{array}{l}{F}_3^{\mathrm{a}}={\phi }_2^{\mathrm{a}}-\arccos \left[{\left(k_{3,\mathrm{e}}^{\mathrm{a}}\right)}^{{\rm T}}\cdot \left(p_{\mathrm{e}}^{\mathrm{a}}-p_3^{\mathrm{a}}-L_3^{\mathrm{a}}{k}_{3,\mathrm{e}}^{\mathrm{a}}-L_7^{\mathrm{a}}{k}_{\mathrm{e}}^{\mathrm{a}}\right)/\left(‖k_{3,\mathrm{e}}^{\mathrm{a}}‖\cdot ‖p_{\mathrm{e}}^{\mathrm{a}}-p_3^{\mathrm{a}}-L_3^{\mathrm{a}}{k}_{3,\mathrm{e}}^{\mathrm{a}}-L_7^{\mathrm{a}}{k}_{\mathrm{e}}^{\mathrm{a}}‖\right)\right]\\ F_4^{\mathrm{a}}=‖\left(p_{\mathrm{e}}^{\mathrm{a}}-L_7^{\mathrm{a}}{k}_{\mathrm{e}}^{\mathrm{a}}-p_3^{\mathrm{a}}\right)/2‖-L_3^{\mathrm{a}}\cdot \cos \left({\phi }_2^{\mathrm{a}}/2\right)\\ F_5^{\mathrm{a}}={\phi }_3^{\mathrm{a}}-\arccos \left[{\left(p_{\mathrm{e}}^{\mathrm{a}}-p_3^{\mathrm{a}}-L_3^{\mathrm{a}}{k}_{3,\mathrm{e}}^{\mathrm{a}}-L_7^{\mathrm{a}}{k}_{\mathrm{e}}^{\mathrm{a}}\right)}^{{\rm T}}\cdot k_{\mathrm{e}}^{\mathrm{a}}/\left(‖p_{\mathrm{e}}^{\mathrm{a}}-p_3^{\mathrm{a}}-L_3^{\mathrm{a}}{k}_{3,\mathrm{e}}^{\mathrm{a}}-L_7^{\mathrm{a}}{k}_{\mathrm{e}}^{\mathrm{a}}‖\cdot ‖k_{\mathrm{e}}^{\mathrm{a}}‖\right)\right]\\ F_6^{\mathrm{a}}=‖\left(p_{\mathrm{e}}^{\mathrm{a}}+\left(L_5^{\mathrm{a}}-L_7^{\mathrm{a}}\right)k_{\mathrm{e}}^{\mathrm{a}}-p_3^{\mathrm{a}}-L_3^{\mathrm{a}}{k}_{3,\mathrm{e}}^{\mathrm{a}}\right)/2‖-L_5^{\mathrm{a}}\cdot \cos \left({\phi }_3^{\mathrm{a}}/2\right)\end{array}\right.$$

(24)

Combining Equations (21) and (24), the equivalent kinematics model of the non-offset discrete arm is established based on space arc method. According to the joint layout of Arm-a, the variables $k_{1,\mathrm{e}}^{\mathrm{a}}$, $L_3^{\mathrm{a}}$, $L_5^{\mathrm{a}}$, and $L_7^{\mathrm{a}}$ are known. The variables $p_{\mathrm{e}}^{\mathrm{a}}$ and $k_{\mathrm{e}}^{\mathrm{a}}$ are also known while solving the inverse kinematics. Therefore, Equations (21) and (24) are associated with the variables ${\phi }_1^{\mathrm{a}}$, ${\phi }_2^{\mathrm{a}}$, ${\phi }_3^{\mathrm{a}}$, $p_3^{\mathrm{a}}$, and $k_{3,\mathrm{e}}^{\mathrm{a}}$. $p_3^{\mathrm{a}}$ and $k_{3,\mathrm{e}}^{\mathrm{a}}$ can be regarded as a spatial vector. The position coordinates of $p_3^{\mathrm{a}}$ are set as $p_3^{\mathrm{a}}={[\begin{array}{ccc}{x}_3^{\mathrm{a}}& y_3^{\mathrm{a}}& z_3^{\mathrm{a}}\end{array}]}^{{\rm T}}$. Based on the spherical coordinate frame, $k_{3,\mathrm{e}}^{\mathrm{a}}$ can be described as follows.

$$k_{3,\mathrm{e}}^{\mathrm{a}}={\left[\begin{array}{ccc}\sin ({\alpha }_{3,1}^{\mathrm{a}})\cos ({\alpha }_{3,2}^{\mathrm{a}})& \sin ({\alpha }_{3,1}^{\mathrm{a}})\sin ({\alpha }_{3,2}^{\mathrm{a}})& \cos ({\alpha }_{3,1}^{\mathrm{a}})\end{array}\right]}^{{\rm T}}, {\alpha }_{3,1}^{\mathrm{a}}\in \left[0,\pi \right], {\alpha }_{3,2}^{\mathrm{a}}\in \left[0,2\pi \right]$$

(25)

As a result, $F_i^{\mathrm{a}}$ (i = 1, 2,…, 6) is the function related to variables ${\phi }_1^{\mathrm{a}}$, ${\phi }_2^{\mathrm{a}}$, ${\phi }_3^{\mathrm{a}}$, $x_3^{\mathrm{a}}$, $y_3^{\mathrm{a}}$, $z_3^{\mathrm{a}}$ ${\alpha }_{3,1}^{\mathrm{a}}$, and ${\alpha }_{3,2}^{\mathrm{a}}$, i.e.,

$$F_i^{\mathrm{a}}=f_i^{\mathrm{a}}\left({\phi }_1^{\mathrm{a}}, {\phi }_2^{\mathrm{a}}, {\phi }_3^{\mathrm{a}}, x_3^{\mathrm{a}}, y_3^{\mathrm{a}}, z_3^{\mathrm{a}}, {\alpha }_{3,1}^{\mathrm{a}}, {\alpha }_{3,2}^{\mathrm{a}}\right), i=1, 2,\dots , 6$$

(26)

where $f_i^{\mathrm{a}}\left(•\right)$ is the ith constraint equation of the equivalent kinematics for Arm-a. The explicit form of $f_i^{\mathrm{a}}\left(•\right)$ can be obtained according to Equations (21) and (24).

3.2.2. Equivalent Kinematics Modeling for Arm-b

As shown in Figure 5, the equivalent kinematics curve of Arm-b consists of the five arcs $C_1^{\mathrm{b}}$, $C_2^{\mathrm{b}}$, $C_3^{\mathrm{b}}$, $C_4^{\mathrm{b}}$, and $C_5^{\mathrm{b}}$. $P_i^{\mathrm{b}}$ and $t_i^{\mathrm{b}}$ are initial position and tangent vector of $C_i^{\mathrm{b}}$ (i = 1, 2, 3, 4, 5). Its endpoint position and tangent vector are $P_{i+1}^{\mathrm{b}}$ and $t_{i+1}^{\mathrm{b}}$, respectively. $R_i^{\mathrm{b}}$ and ${\phi }_i^{\mathrm{b}}$ are the radius and arc angle of $C_i^{\mathrm{b}}$. The curve of equivalent kinematics is divided into a single-arc set ($C_1^{\mathrm{b}}$) and two double-arc sets. The former double-arc set is composed of the spatial arcs $C_2^{\mathrm{b}}$ and $C_3^{\mathrm{b}}$, and the latter includes the arcs $C_4^{\mathrm{b}}$ and $C_5^{\mathrm{b}}$.

With respect to the single-arc set, ${\tilde{P}}_1^{\mathrm{b}}$ is the intersection point of the line along the direction of $t_1^{\mathrm{b}}$ and the line along the direction of $-t_2^{\mathrm{b}}$. The constraint equations of the single-arc are firstly established as follows.

$$\left\{\begin{array}{l}{F}_1^{\mathrm{b}}={\phi }_1^{\mathrm{b}}-\arccos \left[{\left(t_1^{\mathrm{b}}\right)}^{{\rm T}}\cdot t_2^{\mathrm{b}}/\left(‖t_1^{\mathrm{b}}‖\cdot ‖t_2^{\mathrm{b}}‖\right)\right]\\ F_2^{\mathrm{b}}=‖P_2^{\mathrm{b}}-P_1^{\mathrm{b}}‖-2R_1^{\mathrm{b}}\cdot \sin \left({\phi }_1^{\mathrm{b}}/2\right)\end{array}\right.$$

(27)

Based on the equivalent kinematics analysis, the following equations are obtained.

$$\left\{\begin{array}{l}{t}_1^{\mathrm{b} }=k_{0,\mathrm{e}}^{\mathrm{b}}, t_2^{\mathrm{b}}=k_{1,\mathrm{e}}^{\mathrm{b}}\\ P_1^{\mathrm{b}}=p_{0,\mathrm{e}}^{\mathrm{b}}-\left(L^{\mathrm{b}}/2\right)k_{0,\mathrm{e}}^{\mathrm{b}}, P_2^{\mathrm{b}}=p_{1,\mathrm{e}}^{\mathrm{b}}-\left(L^{\mathrm{b}}/2\right)k_{1,\mathrm{e}}^{\mathrm{b}}\\ R_1^{\mathrm{b}}=L^{\mathrm{b}}/\left[2\tan \left({\phi }_1^{\mathrm{b}}/12\right)\right]\end{array}\right.$$

(28)

where $L^{\mathrm{b}}$ is the length of each link for Arm-b. Substituting Equation (28) into Equation (27), it is rewritten as follows.

$$\left\{\begin{array}{l}{F}_1^{\mathrm{b}}={\phi }_1^{\mathrm{b}}-\arccos \left[{\left(k_{0,\mathrm{e}}^{\mathrm{b}}\right)}^{{\rm T}}\cdot k_{1,\mathrm{e}}^{\mathrm{b}}/\left(‖k_{0,\mathrm{e}}^{\mathrm{b}}‖\cdot ‖k_{1,\mathrm{e}}^{\mathrm{b}}‖\right)\right]\\ F_2^{\mathrm{b}}=‖p_{1,\mathrm{e}}^{\mathrm{b}}-p_{0,\mathrm{e}}^{\mathrm{b}}+\left(L^{\mathrm{b}}/2\right)\left(k_{0,\mathrm{e}}^{\mathrm{b}}-k_{1,\mathrm{e}}^{\mathrm{b}}\right)‖-L^{\mathrm{b}}\sin \left({\phi }_1^{\mathrm{b}}/2\right)/tan\left({\phi }_{12}^{\mathrm{b}}/2\right)\end{array}\right.$$

(29)

The first double-arc set of Arm-b consists of arcs $C_2^{\mathrm{b}}$ and $C_3^{\mathrm{b}}$. ${\tilde{P}}_2^{\mathrm{b}}$ is a point that is on the line along the direction of $t_2^{\mathrm{b}}$. And point ${\tilde{P}}_3^{\mathrm{b}}$ is on the line along the negative direction of $t_4^{\mathrm{b}}$. The exact positions of ${\tilde{P}}_2^{\mathrm{b}}$ and ${\tilde{P}}_3^{\mathrm{b}}$ are determined by making the straight line $\overrightarrow{{\tilde{P}}_2^{\mathrm{b}}{\tilde{P}}_3^{\mathrm{b}}}$ the common tangent of $C_2^{\mathrm{b}}$ and $C_3^{\mathrm{b}}$. Therefore, the constraint equations of the first double-arc set are established as follows.

$$\left\{\begin{array}{l}{F}_3^{\mathrm{b}}={\phi }_2^{\mathrm{b}}-\arccos \left[{\left(t_2^{\mathrm{b}}\right)}^{{\rm T}}\cdot \left({\tilde{P}}_3^{\mathrm{b}}-{\tilde{P}}_2^{\mathrm{b}}\right)/\left(‖t_2^{\mathrm{b}}‖\cdot ‖{\tilde{P}}_3^{\mathrm{b}}-{\tilde{P}}_2^{\mathrm{b}}‖\right)\right]\\ F_4^{\mathrm{b}}=‖\left[{\tilde{P}}_2^{\mathrm{b}}+c_1\left({\tilde{P}}_3^{\mathrm{b}}-{\tilde{P}}_2^{\mathrm{b}}\right)\right]-P_2^{\mathrm{b}}‖-2R_2^{\mathrm{b}}\cdot \sin \left({\phi }_2^{\mathrm{b}}/2\right)\\ F_5^{\mathrm{b}}={\phi }_3^{\mathrm{b}}-\arccos \left[{\left({\tilde{P}}_3^{\mathrm{b}}-{\tilde{P}}_2^{\mathrm{b}}\right)}^{{\rm T}}\cdot t_4^{\mathrm{b}}/\left(‖{\tilde{P}}_3^{\mathrm{b}}-{\tilde{P}}_2^{\mathrm{b}}‖\cdot ‖t_4^{\mathrm{b}}‖\right)\right]\\ F_6^{\mathrm{b}}=‖P_4^{\mathrm{b}}-\left[{\tilde{P}}_2^{\mathrm{b}}+c_1\left({\tilde{P}}_3^{\mathrm{b}}-{\tilde{P}}_2^{\mathrm{b}}\right)\right]‖-2R_3^{\mathrm{b}}\cdot \sin \left({\phi }_3^{\mathrm{b}}/2\right)\end{array}\right.$$

(30)

where $c_1$ is a coefficient, and it is given as follows.

$$c_1=\left[L^{\mathrm{b}}\tan \left({\phi }_2^{\mathrm{b}}/2\right)\right]/\left[2\tan \left({\phi }_2^{\mathrm{b}}/12\right)\right]$$

(31)

According to the equivalent kinematics analysis, the following equations are obtained.

$$\left\{\begin{array}{l}{t}_2^{\mathrm{b} }=k_{1,\mathrm{e}}^{\mathrm{b} }, t_4^{\mathrm{b} }=k_{3,\mathrm{e}}^{\mathrm{b} }\\ P_2^{\mathrm{b}}=p_{1,\mathrm{e}}^{\mathrm{b} }-\left(L^{\mathrm{b}}/2\right)k_{1,\mathrm{e}}^{\mathrm{b}}, P_4^{\mathrm{b}}=p_{3,\mathrm{e}}^{\mathrm{b} }-\left(L^{\mathrm{b}}/2\right)k_{3,\mathrm{e}}^{\mathrm{b}}, {\tilde{P}}_2^{\mathrm{b}}=p_{1,\mathrm{e}}^{\mathrm{b} }+c_1{k}_{1,\mathrm{e}}^{\mathrm{b}}, {\tilde{P}}_3^{\mathrm{b}}=p_{3,\mathrm{e}}^{\mathrm{b} }-c_2{k}_{3,\mathrm{e}}^{\mathrm{b}}\\ R_2^{\mathrm{b}}=L^{\mathrm{b}}/\left[2\tan \left({\phi }_2^{\mathrm{a}}/12\right)\right], R_3^{\mathrm{b}}=L^{\mathrm{b}}/\left[2\tan \left({\phi }_3^{\mathrm{a}}/12\right)\right]\end{array}\right.$$

(32)

where

$$c_2=\left[L^{\mathrm{b}}\tan \left({\phi }_3^{\mathrm{b}}/2\right)\right]/\left[2\tan \left({\phi }_3^{\mathrm{b}}/12\right)\right]$$

(33)

Substituting Equation (32) into Equation (30), the following equations are obtained.

$$\left\{\begin{array}{l}{F}_3^{\mathrm{b}}={\phi }_2^{\mathrm{b}}-\arccos \left[{\left(k_{1,\mathrm{e}}^{\mathrm{b} }\right)}^{{\rm T}}\cdot \left(p_{3,\mathrm{e}}^{\mathrm{b} }-c_2{k}_{3,\mathrm{e}}^{\mathrm{b}}-p_{1,\mathrm{e}}^{\mathrm{b} }-c_1{k}_{1,\mathrm{e}}^{\mathrm{b}}\right)/\left(‖k_{1,\mathrm{e}}^{\mathrm{b} }‖\cdot ‖p_{3,\mathrm{e}}^{\mathrm{b} }-c_2{k}_{3,\mathrm{e}}^{\mathrm{b}}-p_{1,\mathrm{e}}^{\mathrm{b} }-c_1{k}_{1,\mathrm{e}}^{\mathrm{b}}‖\right)\right]\\ F_4^{\mathrm{b}}=‖\left[\left(c_1+L^{\mathrm{b}}/2\right)k_{1,\mathrm{e}}^{\mathrm{b}}+c_1\left(p_{3,\mathrm{e}}^{\mathrm{b} }-c_2{k}_{3,\mathrm{e}}^{\mathrm{b}}-p_{1,\mathrm{e}}^{\mathrm{b} }-c_1{k}_{1,\mathrm{e}}^{\mathrm{b}}\right)\right]‖-L^{\mathrm{b}}\sin \left({\phi }_2^{\mathrm{b}}/2\right)/\tan \left({\phi }_2^{\mathrm{b}}/12\right)\\ F_5^{\mathrm{b}}={\phi }_3^{\mathrm{b}}-\arccos \left[{\left(p_{3,\mathrm{e}}^{\mathrm{b} }-c_2{k}_{3,\mathrm{e}}^{\mathrm{b}}-p_{1,\mathrm{e}}^{\mathrm{b} }-c_1{k}_{1,\mathrm{e}}^{\mathrm{b}}\right)}^{{\rm T}}\cdot k_{3,\mathrm{e}}^{\mathrm{b}}/\left(‖p_{3,\mathrm{e}}^{\mathrm{b} }-c_2{k}_{3,\mathrm{e}}^{\mathrm{b}}-p_{1,\mathrm{e}}^{\mathrm{b} }-c_1{k}_{1,\mathrm{e}}^{\mathrm{b}}‖\cdot ‖k_{3,\mathrm{e}}^{\mathrm{b}}‖\right)\right]\\ F_6^{\mathrm{b}}=‖\left(1+c_1\right)p_{3,\mathrm{e}}^{\mathrm{b} }-\left(c_1{c}_2-L^{\mathrm{b}}/2\right)k_{3,\mathrm{e}}^{\mathrm{b}}+\left(1+c_1\right)\left(p_{1,\mathrm{e}}^{\mathrm{b} }+c_1{k}_{1,\mathrm{e}}^{\mathrm{b}}\right)‖-L^{\mathrm{b}}\sin \left({\phi }_3^{\mathrm{b}}/2\right)/\tan \left({\phi }_3^{\mathrm{b}}/12\right)\end{array}\right.$$

(34)

The constraint equations of the second double-arc set including $C_4^{\mathrm{b}}$ and $C_5^{\mathrm{b}}$ are established as follows.

$$\left\{\begin{array}{l}{F}_7^{\mathrm{b}}={\phi }_4^{\mathrm{b}}-\arccos \left[{\left(t_4^{\mathrm{b}}\right)}^{{\rm T}}\cdot \left({\tilde{P}}_5^{\mathrm{b}}-{\tilde{P}}_4^{\mathrm{b}}\right)/\left(‖t_4^{\mathrm{b}}‖\cdot ‖{\tilde{P}}_5^{\mathrm{b}}-{\tilde{P}}_4^{\mathrm{b}}‖\right)\right]\\ F_8^{\mathrm{b}}=‖\left[{\tilde{P}}_4^{\mathrm{b}}+c_3\left({\tilde{P}}_5^{\mathrm{b}}-{\tilde{P}}_4^{\mathrm{b}}\right)\right]-P_4^{\mathrm{b}}‖-2R_4^{\mathrm{b}}\cdot \sin \left({\phi }_4^{\mathrm{b}}/2\right)\\ F_9^{\mathrm{b}}={\phi }_5^{\mathrm{b}}-\arccos \left[{\left({\tilde{P}}_5^{\mathrm{b}}-{\tilde{P}}_4^{\mathrm{b}}\right)}^{{\rm T}}\cdot t_6^{\mathrm{b}}/\left(‖{\tilde{P}}_5^{\mathrm{b}}-{\tilde{P}}_4^{\mathrm{b}}‖\cdot ‖t_6^{\mathrm{b}}‖\right)\right]\\ F_{10}^{\mathrm{b}}=‖P_6^{\mathrm{b}}-\left[{\tilde{P}}_4^{\mathrm{b}}+c_3\left({\tilde{P}}_5^{\mathrm{b}}-{\tilde{P}}_4^{\mathrm{b}}\right)\right]‖-2R_5^{\mathrm{b}}\cdot \sin \left({\phi }_5^{\mathrm{b}}/2\right)\end{array}\right.$$

(35)

where $c_3$ is a coefficient, and it can be obtained as follows.

$$c_3=\left[L^{\mathrm{b}}\tan \left({\phi }_4^{\mathrm{b}}/2\right)\right]/\left[2\tan \left({\phi }_4^{\mathrm{b}}/12\right)\right]$$

(36)

Based on the equivalent kinematics analysis, the following equations hold.

$$\left\{\begin{array}{l}{t}_4^{\mathrm{b} }=k_{3,\mathrm{e}}^{\mathrm{b} }, t_6^{\mathrm{b} }=k_{5,\mathrm{e}}^{\mathrm{b} }\\ P_4^{\mathrm{b}}=p_{3,\mathrm{e}}^{\mathrm{b} }-\left(L^{\mathrm{b}}/2\right)k_{3,\mathrm{e}}^{\mathrm{b}}, P_6^{\mathrm{b}}=p_{5,\mathrm{e}}^{\mathrm{b} }-\left(L^{\mathrm{b}}/2\right)k_{5,\mathrm{e}}^{\mathrm{b}}, {\tilde{P}}_4^{\mathrm{b}}=p_{3,\mathrm{e}}^{\mathrm{b} }+c_3{k}_{3,\mathrm{e}}^{\mathrm{b}}, {\tilde{P}}_5^{\mathrm{b}}=p_{5,\mathrm{e}}^{\mathrm{b} }-c_4{k}_{3,\mathrm{e}}^{\mathrm{b}}\\ R_4^{\mathrm{b}}=L^{\mathrm{b}}/\left[2\tan \left({\phi }_4^{\mathrm{a}}/12\right)\right], R_5^{\mathrm{b}}=L^{\mathrm{b}}/\left[2\tan \left({\phi }_5^{\mathrm{a}}/12\right)\right]\end{array}\right.$$

(37)

where $c_4=\left[L^{\mathrm{b}}\tan \left({\phi }_5^{\mathrm{b}}/2\right)\right]/\left[2\tan \left({\phi }_5^{\mathrm{b}}/12\right)\right]$ is a coefficient, and it can be obtained as follows. Substituting Equation (37) into Equation (35), it can be obtained as follows.

$$\left\{\begin{array}{l}{F}_7^{\mathrm{b}}={\phi }_4^{\mathrm{b}}-\arccos \left[{\left(k_{3,\mathrm{e}}^{\mathrm{b} }\right)}^{{\rm T}}\cdot \left(p_{5,\mathrm{e}}^{\mathrm{b} }-c_4{k}_{5,\mathrm{e}}^{\mathrm{b}}-p_{3,\mathrm{e}}^{\mathrm{b} }-c_3{k}_{3,\mathrm{e}}^{\mathrm{b}}\right)/\left(‖k_{3,\mathrm{e}}^{\mathrm{b} }‖\cdot ‖p_{5,\mathrm{e}}^{\mathrm{b} }-c_4{k}_{5,\mathrm{e}}^{\mathrm{b}}-p_{3,\mathrm{e}}^{\mathrm{b} }-c_3{k}_{3,\mathrm{e}}^{\mathrm{b}}‖\right)\right]\\ F_8^{\mathrm{b}}=‖\left[\left(c_3+L^{\mathrm{b}}/2\right)k_{3,\mathrm{e}}^{\mathrm{b}}+c_3\left(p_{5,\mathrm{e}}^{\mathrm{b} }-c_4{k}_{5,\mathrm{e}}^{\mathrm{b}}-p_{3,\mathrm{e}}^{\mathrm{b} }-c_3{k}_{3,\mathrm{e}}^{\mathrm{b}}\right)\right]‖-L^{\mathrm{b}}\sin \left({\phi }_4^{\mathrm{b}}/2\right)/\tan \left({\phi }_4^{\mathrm{b}}/12\right)\\ F_9^{\mathrm{b}}={\phi }_5^{\mathrm{b}}-\arccos \left[{\left(p_{5,\mathrm{e}}^{\mathrm{b} }-c_4{k}_{5,\mathrm{e}}^{\mathrm{b}}-p_{3,\mathrm{e}}^{\mathrm{b} }-c_3{k}_{3,\mathrm{e}}^{\mathrm{b}}\right)}^{{\rm T}}\cdot k_{5,\mathrm{e}}^{\mathrm{b}}/\left(‖p_{5,\mathrm{e}}^{\mathrm{b} }-c_4{k}_{5,\mathrm{e}}^{\mathrm{b}}-p_{3,\mathrm{e}}^{\mathrm{b} }-c_3{k}_{3,\mathrm{e}}^{\mathrm{b}}‖\cdot ‖k_{5,\mathrm{e}}^{\mathrm{b}}‖\right)\right]\\ F_{10}^{\mathrm{b}}=‖\left(1+c_3\right)p_{5,\mathrm{e}}^{\mathrm{b} }-\left(c_3{c}_4-L^{\mathrm{b}}/2\right)k_{5,\mathrm{e}}^{\mathrm{b}}+\left(1+c_3\right)\left(p_{3,\mathrm{e}}^{\mathrm{b} }+c_3{k}_{3,\mathrm{e}}^{\mathrm{b}}\right)‖-L^{\mathrm{b}}\sin \left({\phi }_5^{\mathrm{b}}/2\right)/\tan \left({\phi }_5^{\mathrm{b}}/12\right)\end{array}\right.$$

(38)

Combining Equations (29), (34) and (38), the equivalent kinematics model of the segmented serpentine arm is obtained by the space arc method. According to the joint layout of Arm-b, the variables $k_{0,\mathrm{e}}^{\mathrm{a}}$, $p_{0,\mathrm{e}}^{\mathrm{b} }$, and $L^{\mathrm{b}}$ are known. When we deal with the problem of the inverse kinematics solution, $k_{5,\mathrm{e}}^{\mathrm{b}}$ and $p_{5,\mathrm{e}}^{\mathrm{b} }$ are also known. As a result, the equivalent kinematics equations of Arm-b are related to the variables ${\phi }_1^{\mathrm{b}}$, ${\phi }_2^{\mathrm{b}}$, ${\phi }_3^{\mathrm{b}}$, ${\phi }_4^{\mathrm{b}}$, ${\phi }_5^{\mathrm{b}}$, $p_{1,\mathrm{e}}^{\mathrm{b} }$, $k_{1,\mathrm{e}}^{\mathrm{b}}$, $p_{3,\mathrm{e}}^{\mathrm{b} }$, and $k_{3,\mathrm{e}}^{\mathrm{b}}$. Some assumptions are given as follows.

$$\left\{\begin{array}{l}{p}_{1,\mathrm{e}}^{\mathrm{b}}={[x_1^{\mathrm{b}}\hspace{1em}{y}_1^{\mathrm{b}}\hspace{1em}{z}_1^{\mathrm{b}}]}^{{\rm T}}, p_{3,\mathrm{e}}^{\mathrm{b}}={[x_3^{\mathrm{b}}\hspace{1em}{y}_3^{\mathrm{b}}\hspace{1em}{z}_3^{\mathrm{b}}]}^{{\rm T}}\\ k_{1,\mathrm{e}}^{\mathrm{b}}={\left[\sin ({\alpha }_{1,1}^{\mathrm{b}})\cos ({\alpha }_{1,2}^{\mathrm{b}})\hspace{1em}\sin ({\alpha }_{1,1}^{\mathrm{b}})\sin ({\alpha }_{1,2}^{\mathrm{b}})\hspace{1em}\cos ({\alpha }_{1,1}^{\mathrm{b}})\right]}^{{\rm T}}, {\alpha }_{1,1}^{\mathrm{b}}\in \left[0,\pi \right], {\alpha }_{1,2}^{\mathrm{b}}\in \left[0,2\pi \right]\\ k_{3,\mathrm{e}}^{\mathrm{b}}={\left[\sin ({\alpha }_{3,1}^{\mathrm{b}})\cos ({\alpha }_{3,2}^{\mathrm{b}})\hspace{1em}\sin ({\alpha }_{3,1}^{\mathrm{b}})\sin ({\alpha }_{3,2}^{\mathrm{b}})\hspace{1em}\cos ({\alpha }_{3,1}^{\mathrm{b}})\right]}^{{\rm T}}, {\alpha }_{3,1}^{\mathrm{b}}\in \left[0,\pi \right], {\alpha }_{3,2}^{\mathrm{b}}\in \left[0,2\pi \right]\end{array}\right.$$

(39)

The $F_i^{\mathrm{b}}$ (i = 1, 2, …, 10) is related to the variables ${\phi }_1^{\mathrm{b}}$, ${\phi }_2^{\mathrm{b}}$, ${\phi }_3^{\mathrm{b}}$, ${\phi }_4^{\mathrm{b}}$, ${\phi }_5^{\mathrm{b}}$, $x_1^{\mathrm{b}}$, $y_1^{\mathrm{b}}$, $z_1^{\mathrm{b}}$, ${\alpha }_{1,1}^{\mathrm{b}}$, ${\alpha }_{1,2}^{\mathrm{b}}$, $x_3^{\mathrm{b}}$, $y_3^{\mathrm{b}}$, $z_3^{\mathrm{b}}$, ${\alpha }_{3,1}^{\mathrm{b}}$, and ${\alpha }_{3,2}^{\mathrm{b}}$. It can be expressed as follows.

$$F_i^{\mathrm{b}}=f_i^{\mathrm{b}}\left({\phi }_1^{\mathrm{b}}, {\phi }_2^{\mathrm{b}}, {\phi }_3^{\mathrm{b}}, {\phi }_4^{\mathrm{b}}, {\phi }_5^{\mathrm{b}}, x_1^{\mathrm{b}}, y_1^{\mathrm{b}}, z_1^{\mathrm{b}}, {\alpha }_{1,1}^{\mathrm{b}}, {\alpha }_{1,2}^{\mathrm{b}} x_3^{\mathrm{b}}, y_3^{\mathrm{b}}, z_3^{\mathrm{b}}, {\alpha }_{3,1}^{\mathrm{b}}, {\alpha }_{3,2}^{\mathrm{b}}\right), i=1, 2,\dots , 10$$

(40)

where $f_i^{\mathrm{b}}\left(•\right)$ is the ith constraint equation of the equivalent kinematics for Arm-b. Its explicit form can be derived by Equations (29), (34) and (38).

4. Coordinated Collision-Free Trajectory Planning Based on Equivalent Kinematics

4.1. Main Steps for Generating Collision-Free Trajectory

To illustrate the proposed method, the target satellite transposition is taken as an example. Prior to transposition, the target satellite is securely captured by the end-effector of the non-offset discrete manipulator (Arm-a), as shown in Figure 6.

The implementation process of coordinated motion for the dual arm is as follows. As shown in Figure 6, the end points of the discrete arm and serpentine arm are $p_{\mathrm{e}}^{\mathrm{a}}$ and $p_{\mathrm{e}}^{\mathrm{b}}$, respectively. The direction of serpentine arm’s tip is $k_{\mathrm{e}}^{\mathrm{b}}$. During target satellite transposition, the desired trajectory of the target satellite is known; hence, the movement trajectory of the locked point (i.e., $p_{\mathrm{e}}^{\mathrm{a}}$) that is on the satellite is obtained. To ensure effective monitoring, the direction of serpentine arm’s tip must remain coincident with the line that is from point $p_{\mathrm{e}}^{\mathrm{b}}$ to point $p_{\mathrm{e}}^{\mathrm{a}}$. In addition, a certain distance must also be maintained between points $p_{\mathrm{e}}^{\mathrm{b}}$ and $p_{\mathrm{e}}^{\mathrm{a}}$. The movement trajectory of point $p_{\mathrm{e}}^{\mathrm{b}}$ is also obtained according to the trajectory of point $p_{\mathrm{e}}^{\mathrm{a}}$. Therefore, as the discrete arm moves along its trajectory to transpose the target satellite, the serpentine arm will follow in synchronized motion.

The locked point on the docking ring is the capture point. That is to say, the Arm-a tip and the target satellite are connected at the locked point. In the course of the transposition process, Arm-a is used as the mission manipulator to transpose the target satellite from the initial pose (position and attitude) to the desired pose. Meanwhile, the segmented hyper-redundant serpentine manipulator (Arm-b) is employed as the auxiliary arm for offering the monitoring function to ensure the operational safety of Arm-a. Here, the monitoring position of Arm-b is also the locked point. However, in practice, numerous payloads distributed across the base increase the risk of collision between the payloads and two arms during the transposition process. To address the issue, the envelope models of the obstacles and both arms are established, and the coordinated trajectory planning method with obstacle avoidance is proposed based on equivalent kinematics modeling. The flowchart of the algorithm is depicted in Figure 7.

Firstly, the expected movement trajectory of the target satellite’s center of mass is planned based on its initial pose and desired pose. The desired motion trajectories of Arm-a tip and Arm-b tip are also planned by utilizing the pose relationship between each arm and the target. Then, the end-effector pose of both arms and the distance between each arm and obstacles are parameterized by the equivalent kinematics model that is established by the space arc method. Furthermore, the objective function is obtained by the constraint equations of equivalent kinematics and obstacle avoidance. The parameters of all spatial arcs are calculated by solving the objective function. The joint angles of Arm-a and Arm-b are determined based on the relationship between arc parameters and joint angles. Finally, the computed joint angles serve as the command inputs for the servo controllers, and the non-offset discrete arm and segmented serpentine manipulator are actuated in a coordinated manner to achieve target satellite transposition.

4.2. Motion Planning of Target Satellite

As shown in Figure 8, it illustrates the target satellite transposition from the initial pose to the desired pose. This transposition can be considered as the point-to-point motion planning. To ensure the smoothness of the start–stop, the point-to-point motion planning is achieved by the quintic polynomial interpolation method. A 5th-order polynomial is chosen because it represents the minimal-order analytic solution that fulfills all boundary constraints on position, velocity, and acceleration and guarantees acceleration continuity. Therefore, it strikes an optimal balance among motion smoothness, computational load, and practical implementation.

The initial pose and the desired pose of the target are set as follows:

$$\left\{\begin{array}{l}{X}_0={\left[\begin{array}{cccccc}{p}_{0x},& p_{0y},& p_{0z},& {\alpha }_0,& {\beta }_0,& {\gamma }_0\end{array}\right]}^{\mathsf{{\rm T}}}\\ X_{\mathrm{f}}={\left[\begin{array}{cccccc}{p}_{\mathrm{f}x},& p_{\mathrm{f}y},& p_{\mathrm{f}z},& {\alpha }_{\mathrm{f}},& {\beta }_{\mathrm{f}},& {\gamma }_{\mathrm{f}}\end{array}\right]}^{\mathsf{{\rm T}}}\end{array}\right.$$

(41)

where $p_{0x}$, $p_{0y}$, and $p_{0z}$ represent initial positions of the target satellite along x-axis, y-axis, and z-axis, respectively. The desired positions are $p_{\mathrm{f}x}$, $p_{\mathrm{f}y}$, and $p_{\mathrm{f}z}$. And ${\alpha }_0$, ${\beta }_0$, and ${\gamma }_0$ are attitude angles (Z-Y-X Euler angles). Its desired attitude angles are ${\alpha }_{\mathrm{f}}$, ${\beta }_{\mathrm{f}}$, and ${\gamma }_{\mathrm{f}}$, respectively. Assuming the velocity and acceleration of the target satellite are zeros at the initial and terminal states, the motion trajectory of the target satellite is obtained as follows.

$$X\left(t\right)=X_0+{\displaystyle \frac{10\left(X_{\mathrm{f}}-X_0\right)}{t_{\mathrm{f}}^3}}{t}^3-{\displaystyle \frac{15\left(X_{\mathrm{f}}-X_0\right)}{t_{\mathrm{f}}^4}}{t}^4+{\displaystyle \frac{6\left(X_{\mathrm{f}}-X_0\right)}{t_{\mathrm{f}}^5}}{t}^5, 0\le t\le t_{\mathrm{f}}$$

(42)

Therefore, the locked point motion trajectory can also be calculated, and it is the desired trajectory of the Arm-a tip. Because the locked point is the monitoring position of Arm-b, the desired trajectory of the Arm-b tip is obtained according to the relationship between the locked point and the tip. In other words, the desired poses of the Arm-a tip and Arm-b tip are obtained, i.e., $P_{\mathrm{ed}}^{\mathrm{a}}$, $R_{\mathrm{ed}}^{\mathrm{a}}$, $P_{\mathrm{ed}}^{\mathrm{b}}$, and $R_{\mathrm{ed}}^{\mathrm{b}}$.

4.3. Objective Function and Solution Method

According to the pose relationship between each arm and the target satellite, the end-effector motion trajectories of Arm-a and Arm-b are obtained by the motion trajectory of the target satellite that is given in Section 4.2. As a result, the desired poses of the Arm-a tip and Arm-b tip are known. Substituting these data into Equations (26) and (40), the equivalent kinematics constraint equations are obtained as follows.

$$G_1\left({\phi }_1^{\mathrm{a}}, {\phi }_2^{\mathrm{a}}, {\phi }_3^{\mathrm{a}}, x_3^{\mathrm{a}}, y_3^{\mathrm{a}}, z_3^{\mathrm{a}}, {\alpha }_{3,1}^{\mathrm{a}}, {\alpha }_{3,2}^{\mathrm{a}}\right)={\left[F_1^{\mathrm{a}}, F_2^{\mathrm{a}}, F_3^{\mathrm{a}}, F_4^{\mathrm{a}}, F_5^{\mathrm{a}} \right]}^{{\rm T}}=\mathbf{0}$$

(43)

$$G_2\left({\phi }_1^{\mathrm{b}}, {\phi }_2^{\mathrm{b}}, {\phi }_3^{\mathrm{b}}, {\phi }_4^{\mathrm{b}}, {\phi }_5^{\mathrm{b}}, x_1^{\mathrm{b}}, y_1^{\mathrm{b}}, z_1^{\mathrm{b}}, {\alpha }_{1,1}^{\mathrm{b}}, {\alpha }_{1,2}^{\mathrm{b}} x_3^{\mathrm{b}}, y_3^{\mathrm{b}}, z_3^{\mathrm{b}}, {\alpha }_{3,1}^{\mathrm{b}}, {\alpha }_{3,2}^{\mathrm{b}}\right)={\left[F_1^{\mathrm{b}}, F_2^{\mathrm{b}},\dots ,F_{10}^{\mathrm{b}}\right]}^{{\rm T}}=\mathbf{0}$$

(44)

Once the kinematics constraint equations are established, the next step is to formulate the obstacle avoidance constraint equations. As shown in Figure 9, taking the jth inscribed arc of Arm-k as an example, the envelope of Arm-k is a generalized cylinder, which is created by sweeping a circular cross-section with a radius of $R^k$ along the kinematics equivalence curve of Arm-k. It is important to point out that the positions of all obstacles are known. Moreover, since each obstacle can be enclosed by a minimum sphere, a spherical model with a radius of $R_i$ is employed to represent the envelope of an obstacle. Therefore, the key to achieving collision avoidance lies in controlling the distance between both arms and obstacles. Firstly, the envelope of each arm is modeled by spatial arc method, and the envelope of each obstacle is considered as a sphere. Then, the minimum safe distance between the envelope of each arm and the envelope of each obstacle is set. Finally, the obstacle avoidance of both arms is achieved by controlling their minimum Euclidean distance between arms and obstacles.

As shown in Figure 9, $P_{j,n}^k$ is any point on the jth inscribed arc of Arm-k, and the distance between the ith obstacle and the jth inscribed arc of Arm-k is calculated as follows.

$$‖\overrightarrow{o_i{P}_{j,n}^k}‖=\sqrt{{‖\overrightarrow{o_i{\tilde{o}}_i}‖}^2+{‖\overrightarrow{{\tilde{o}}_i{O}_j^k}‖}^2+{‖\overrightarrow{O_j^k{P}_{j,n}^k}‖}^2-2‖\overrightarrow{{\tilde{o}}_i{O}_j^k}‖\cdot ‖\overrightarrow{O_j^k{P}_{j,n}^k}‖\cdot \cos \left({\mathsf{\gamma }}_n^{j,k}\right)}$$

(45)

where $o_i$ is the centroid of the ith obstacle, and ${\tilde{o}}_i$ is the projection of $o_i$ on the plane where the jth inscribed arc of Arm-k is located. The start point and end point of the jth arc are $P_j^k$ and $P_{j+1}^k$. ${\mathsf{\gamma }}_{\mathrm{s}}^{j,k}$ is the angle between $\overrightarrow{{\tilde{o}}_i{O}_j^k}$ and $\overrightarrow{O_j^k{P}_j^k}$. ${\mathsf{\gamma }}_{\mathrm{f}}^{j,k}$ is the angle between $\overrightarrow{{\tilde{o}}_i{O}_j^k}$ and $\overrightarrow{O_j^k{P}_{j+1}^k}$. The angle between $\overrightarrow{{\tilde{o}}_i{O}_j^k}$ and $\overrightarrow{O_j^k{P}_{j,n}^k}$ is ${\mathsf{\gamma }}_n^{j,k}$. If the ith obstacle position and the above plane are known, $‖\overrightarrow{o_i{\tilde{o}}_i}‖$, $‖\overrightarrow{{\tilde{o}}_i{O}_j^k}‖$, and $‖\overrightarrow{O_j^k{P}_{j,n}^k}‖$ are constants. Therefore, the magnitude of $‖\overrightarrow{o_i{P}_{j,n}^k}‖$ only depends on angle ${\mathsf{\gamma }}_n^{j,k}$. The minimum distance between the ith obstacle and the jth arc of Arm-k can be obtained as follows.

$$\left\{\begin{array}{l}{d}_{\min }^{i,j,k}=‖\overrightarrow{o_i{P}_{j,n}^k}‖=\sqrt{{‖\overrightarrow{o_i{\tilde{o}}_i}‖}^2+{‖\overrightarrow{{\tilde{o}}_i{O}_j^k}‖}^2+{‖\overrightarrow{O_j^k{P}_{j,n}^k}‖}^2-2‖\overrightarrow{{\tilde{o}}_i{O}_j^k}‖\cdot ‖\overrightarrow{O_j^k{P}_{j,n}^k}‖\cdot \cos \left({\mathsf{\gamma }}_{\mathrm{s}}^{i,j}\right)}, \mathrm{if} {\mathsf{\gamma }}_{\mathrm{s}}^{i,j}\ge {\mathsf{\gamma }}_{\mathrm{f}}^{i,j}\\ d_{\min }^{i,j,k}=‖\overrightarrow{o_i{P}_{j,n}^k}‖=\sqrt{{‖\overrightarrow{o_i{\tilde{o}}_i}‖}^2+{‖\overrightarrow{{\tilde{o}}_i{O}_j^k}‖}^2+{‖\overrightarrow{O_j^k{P}_{j,n}^k}‖}^2-2‖\overrightarrow{{\tilde{o}}_i{O}_j^k}‖\cdot ‖\overrightarrow{O_j^k{P}_{j,n}^k}‖\cdot \cos \left({\mathsf{\gamma }}_{\mathrm{f}}^{i,j}\right)}, \mathrm{if} {\mathsf{\gamma }}_{\mathrm{s}}^{i,j}<{\mathsf{\gamma }}_{\mathrm{f}}^{i,j} \end{array}\right.$$

(46)

As a result, the minimum Euclidean distance between the ith obstacle and Arm-a and Arm-b can be determined as follows.

$$\left\{\begin{array}{l}{d}_{\min }^{i,\mathrm{a}}=\min \left\{d_{\min }^{i,1,a}, d_{\min }^{i,2,a}, d_{\min }^{i,3,a}\right\}\\ d_{\min }^{i,\mathrm{b}}=\min \left\{d_{\min }^{i,1,\mathrm{b}}, d_{\min }^{i,2,\mathrm{b}}, d_{\min }^{i,3,\mathrm{b}}, d_{\min }^{i,4,\mathrm{b}}, d_{\min }^{i,5,\mathrm{b}}\right\}\end{array}\right.$$

(47)

As discussed above, the envelope radii of the obstacle and both arms are $R_i$ and $R^k$, respectively. Therefore, the obstacle avoidance constraint equations of Arm-a and Arm-b are established as follows.

$$G_3\left({\phi }_1^{\mathrm{a}}, {\phi }_2^{\mathrm{a}}, {\phi }_3^{\mathrm{a}}, x_3^{\mathrm{a}}, y_3^{\mathrm{a}}, z_3^{\mathrm{a}}, {\alpha }_{3,1}^{\mathrm{a}}, {\alpha }_{3,2}^{\mathrm{a}}\right)=\min \left\{d_{\min }^{i,1,a}, d_{\min }^{i,2,a}, d_{\min }^{i,3,a}\right\}\ge d_{\mathrm{sd}}^{\mathrm{a}}$$

(48)

$$\begin{array}{l}{G}_4\left({\phi }_1^{\mathrm{b}}, {\phi }_2^{\mathrm{b}}, {\phi }_3^{\mathrm{b}}, {\phi }_4^{\mathrm{b}}, {\phi }_5^{\mathrm{b}}, x_1^{\mathrm{b}}, y_1^{\mathrm{b}}, z_1^{\mathrm{b}}, {\alpha }_{1,1}^{\mathrm{b}}, {\alpha }_{1,2}^{\mathrm{b}} x_3^{\mathrm{b}}, y_3^{\mathrm{b}}, z_3^{\mathrm{b}}, {\alpha }_{3,1}^{\mathrm{b}}, {\alpha }_{3,2}^{\mathrm{b}}\right)\\ =\min \left\{d_{\min }^{i,1,\mathrm{b}}, d_{\min }^{i,2,\mathrm{b}}, d_{\min }^{i,3,\mathrm{b}}, d_{\min }^{i,4,\mathrm{b}}, d_{\min }^{i,5,\mathrm{b}}\right\}\ge d_{\mathrm{sd}}^{\mathrm{b}}\end{array}$$

(49)

where $d_{\mathrm{sd}}^k$ (k = a, b) is the safety distance threshold between Arm-k and obstacles. It is $d_{\mathrm{sd}}^k=R^k+R_i+{\epsilon }_{\mathrm{osb}}$. ${\epsilon }_{\mathrm{osb}}$ is the safety margin.

Based on Equations (43), (44), (48) and (49), the objective function for generating the coordinated collision-free trajectory of Arm-k is obtained as follows.

$$\left\{\begin{array}{l}\min \left[\begin{array}{l}‖G_1\left({\phi }_1^{\mathrm{a}}, {\phi }_2^{\mathrm{a}}, {\phi }_3^{\mathrm{a}}, x_3^{\mathrm{a}}, y_3^{\mathrm{a}}, z_3^{\mathrm{a}}, {\alpha }_{3,1}^{\mathrm{a}}, {\alpha }_{3,2}^{\mathrm{a}}\right)‖\\ +‖G_2\left({\phi }_1^{\mathrm{b}}, {\phi }_2^{\mathrm{b}}, {\phi }_3^{\mathrm{b}}, {\phi }_4^{\mathrm{b}}, {\phi }_5^{\mathrm{b}}, x_1^{\mathrm{b}}, y_1^{\mathrm{b}}, z_1^{\mathrm{b}}, {\alpha }_{1,1}^{\mathrm{b}}, {\alpha }_{1,2}^{\mathrm{b}} x_3^{\mathrm{b}}, y_3^{\mathrm{b}}, z_3^{\mathrm{b}}, {\alpha }_{3,1}^{\mathrm{b}}, {\alpha }_{3,2}^{\mathrm{b}}\right)‖\end{array}\right]\\ s.t.\left\{\begin{array}{l}‖G_3\left({\phi }_1^{\mathrm{a}}, {\phi }_2^{\mathrm{a}}, {\phi }_3^{\mathrm{a}}, x_3^{\mathrm{a}}, y_3^{\mathrm{a}}, z_3^{\mathrm{a}}, {\alpha }_{3,1}^{\mathrm{a}}, {\alpha }_{3,2}^{\mathrm{a}}\right)‖\ge d_{\mathrm{sd}}^{\mathrm{a}}\\ ‖G_4\left({\phi }_1^{\mathrm{b}}, {\phi }_2^{\mathrm{b}}, {\phi }_3^{\mathrm{b}}, {\phi }_4^{\mathrm{b}}, {\phi }_5^{\mathrm{b}}, x_1^{\mathrm{b}}, y_1^{\mathrm{b}}, z_1^{\mathrm{b}}, {\alpha }_{1,1}^{\mathrm{b}}, {\alpha }_{1,2}^{\mathrm{b}} x_3^{\mathrm{b}}, y_3^{\mathrm{b}}, z_3^{\mathrm{b}}, {\alpha }_{3,1}^{\mathrm{b}}, {\alpha }_{3,2}^{\mathrm{b}}\right)‖\ge d_{\mathrm{sd}}^{\mathrm{b}}\end{array}\right.\end{array}\right.$$

(50)

The objective function mainly comprises two components. The first is determined by the inverse kinematics constraint equations for Arm-a (i.e., function $G_1$) and Arm-b (i.e., function $G_2$), aimed at calculating joint angles for a given end-effector pose. The second is a set of inequality constraint functions (i.e., functions $G_3$ and $G_4$) designed to enforce obstacle avoidance constraints for each robotic arm. The solution process of the objective function is listed in the Algorithm 1. Based on this, the parameters of spatial arcs that form the equivalent kinematics curves can be calculated when the desired poses of Arm-a and Arm-b are given.

The solution process of the objective function is given in the Algorithm 1. A gradient descent-based numerical iterative method is introduced to solve the objective function. During the solution process, the convergence criterion is set to 1.0 × 10⁻⁵. Assuming that the objective function is non-convex, a valid solution can be obtained as long as the convergence precision meets this criterion. In other words, if the local optimum is less than 1.0 × 10⁻⁵, the numerical iterative is then completed, and the valid solution is obtained. This result will serve as the initial value for the next iteration.

For the optimization problem, there are two termination conditions. One is the maximum number of iterations (N_max) that is used to prevent the solver from falling into an infinite loop. The other one is the convergence criterion (1.0 × 10⁻⁵) that is adopted to determine whether the solution meets the requirements for obstacle-avoidance trajectory planning.

Algorithm 1 The solution method for the coordinated collision-free trajectory
1:	Set $X^{\mathrm{a}}=({\phi }_1^{\mathrm{a}}, {\phi }_2^{\mathrm{a}}, {\phi }_3^{\mathrm{a}}, x_3^{\mathrm{a}}, y_3^{\mathrm{a}}, z_3^{\mathrm{a}}, {\alpha }_{3,1}^{\mathrm{a}}, {\alpha }_{3,2}^{\mathrm{a}})$
2:	Set $X^{\mathrm{b}}=\left({\phi }_1^{\mathrm{b}}, {\phi }_2^{\mathrm{b}}, {\phi }_3^{\mathrm{b}}, {\phi }_4^{\mathrm{b}}, {\phi }_5^{\mathrm{b}}, x_1^{\mathrm{b}}, y_1^{\mathrm{b}}, z_1^{\mathrm{b}}, {\alpha }_{1,1}^{\mathrm{b}}, {\alpha }_{1,2}^{\mathrm{b}} x_3^{\mathrm{b}}, y_3^{\mathrm{b}}, z_3^{\mathrm{b}}, {\alpha }_{3,1}^{\mathrm{b}}, {\alpha }_{3,2}^{\mathrm{b}}\right)$
3:	Required: Initialize $X^{\mathrm{a}}$ and $X^{\mathrm{b}}$
4:	Required: $P_{\mathrm{ed}}^{\mathrm{a}}$, $R_{\mathrm{ed}}^{\mathrm{a}}$, $P_{\mathrm{ed}}^{\mathrm{b}}$ and $R_{\mathrm{ed}}^{\mathrm{b}}$
5:	for i = 1 to Nmax do
6:	Calculate the values of $‖G_1(X^{\mathrm{a}})‖$, $‖G_2(X^{\mathrm{b}})‖$, $‖G_3(X^{\mathrm{a}})‖$ and $‖G_4(X^{\mathrm{b}})‖$
7:	if $‖G_1(X^{\mathrm{a}})‖+‖G_2(X^{\mathrm{b}})‖>1.0\times {10}^{-5}$
8:	Calculate $\partial \left(‖G_1(X^{\mathrm{a}})‖\right)/\partial X^{\mathrm{a}}$ to obtain $d{X}^{\mathrm{a}}$
9:	Calculate $\partial \left(‖G_2(X^{\mathrm{b}})‖\right)/\partial X^{\mathrm{b}}$ to obtain $d{X}^{\mathrm{b}}$
10:	Update $X_i^{\mathrm{a}}=X^{\mathrm{a}}+d{X}^{\mathrm{a}}$, $X_i^{\mathrm{b}}=X^{\mathrm{b}}+d{X}^{\mathrm{b}}$ and set $X^{\mathrm{a}}=X_i^{\mathrm{a}}$, $X^{\mathrm{b}}=X_i^{\mathrm{b}}$
11:	Go to step 6
12:	else
13:	if $‖G_3(X^{\mathrm{a}})‖\ge d_{\mathrm{sd}}^{\mathrm{a}}\&\&‖G_4(X^{\mathrm{b}})‖\ge d_{\mathrm{sd}}^{\mathrm{b}}$, then, break
14:	else
15:	Let $‖G_3(X^{\mathrm{a}})‖=d_{\mathrm{sd}}^{\mathrm{a}}$ and $‖G_4(X^{\mathrm{b}})‖=d_{\mathrm{sd}}^{\mathrm{b}}$
16:	end if
17:	end if
18:	end for
19:	Output the solution result $X^{\mathrm{a}}$ and $X^{\mathrm{b}}$

4.4. Calculation of Joint Angles for Arm-a and Arm-b

As discussed above, the parameters of spatial arcs can be obtained by solving Equation (50). For Arm-a, the central angles ${\phi }_1^{\mathrm{a}}$, ${\phi }_2^{\mathrm{a}}$, and ${\phi }_3^{\mathrm{a}}$ of the arcs $C_1^{\mathrm{a}}$, $C_2^{\mathrm{a}}$, and $C_3^{\mathrm{a}}$ are obtained directly. Hence, the joint angles ${\theta }_{2i}^{\mathrm{a}}$ of Arm-a are calculated as follows.

$${\theta }_2^{\mathrm{a}}={\phi }_1^{\mathrm{a}}, {\theta }_4^{\mathrm{a}}={\phi }_2^{\mathrm{a}}, {\theta }_6^{\mathrm{a}}={\phi }_3^{\mathrm{a}}$$

(51)

The position ($p_3^{\mathrm{a}}$) and direction ($k_{3,e}^a$) of the third joint are obtained. Based on the joint layout of Arm-a, the position ($p_{1,\mathrm{e}}^{\mathrm{a}}$) and direction ($k_{1,\mathrm{e}}^{\mathrm{a}}$) of the first link’s endpoint are also known. In addition, the position ($p_{5,\mathrm{e}}^{\mathrm{a}}$) and direction ($k_{5,\mathrm{e}}^{\mathrm{a}}$) of the fifth link’s endpoint are calculated as follows.

$$\left\{\begin{array}{l}{p}_{5,\mathrm{e}}^{\mathrm{a}}=p_{\mathrm{ed}}^{\mathrm{a}}-L_7^{\mathrm{a}}{k}_{\mathrm{ed}}^{\mathrm{a}}\\ k_{5,\mathrm{e}}^{\mathrm{a}}=\left(p_{5,\mathrm{e}}^{\mathrm{a}}-p_{3,\mathrm{e}}^{\mathrm{a}}\right)/‖p_{5,\mathrm{e}}^{\mathrm{a}}-p_{3,\mathrm{e}}^{\mathrm{a}}‖\end{array}\right.$$

(52)

where $p_{3,\mathrm{e}}^{\mathrm{a}}=p_3^{\mathrm{a}}+k_{3,\mathrm{e}}^{\mathrm{a}}$, and $k_{\mathrm{ed}}^{\mathrm{a}}=R_{\mathrm{ed}}^{\mathrm{a}}(1:3,3)$.

When establishing the kinematics model by the D-H method, the normal vector of the ith plane where the ith spatial arc $C_i^{\mathrm{a}}$ is located can be obtained as follows.

$$n_i^{\mathrm{a}}=R_{2i-2}^{\mathrm{a}}\cdot {\left[\begin{array}{ccc}-{\mathrm{s}}_{{\theta }_{2i-1}^{\mathrm{a}}}& {\mathrm{c}}_{{\theta }_{2i-1}^{\mathrm{a}}}& 0\end{array}\right]}^{{\rm T}}, i=1,2,3$$

(53)

where ${\mathrm{s}}_{{\theta }_{2i-1}^{\mathrm{a}}}=\sin ({\theta }_{2i-1}^{\mathrm{a}})$, and ${\mathrm{c}}_{{\theta }_{2i-1}^{\mathrm{a}}}=\cos ({\theta }_{2i-1}^{\mathrm{a}})$. $R_{2i-2}^{\mathrm{a}}$ is the rotation matrix from the (2i − 1)th joint frame to the inertial frame.

On the other hand, the normal vector of the ith plane is also calculated by the geometry method, i.e.,

$$n_i^{\mathrm{a}}=\left(k_{2i-1,\mathrm{e}}^{\mathrm{a}}\times k_{2i+1,\mathrm{e}}^{\mathrm{a}}\right)/‖k_{2i-1,\mathrm{e}}^{\mathrm{a}}\times k_{2i+1,\mathrm{e}}^{\mathrm{a}}‖, i=1,2,3$$

(54)

According to Equations (53) and (54), it can be obtained as follows.

$${\left[\begin{array}{ccc}-{\mathrm{s}}_{{\theta }_{2i-1}^{\mathrm{a}}}& {\mathrm{c}}_{{\theta }_{2i-1}^{\mathrm{a}}}& 0\end{array}\right]}^{{\rm T}}={\left(R_{2i-2}^{\mathrm{a}}\right)}^{-1}\left(k_{2i-1,\mathrm{e}}^{\mathrm{a}}\times k_{2i+1,\mathrm{e}}^{\mathrm{a}}\right)/‖k_{2i-1,\mathrm{e}}^{\mathrm{a}}\times k_{2i+1,\mathrm{e}}^{\mathrm{a}}‖\triangleq \left[\begin{array}{ccc}{n}_{ix}^{\mathrm{a}}& n_{iy}^{\mathrm{a}}& 0\end{array}\right], i=1,2,3$$

(55)

Therefore, the joint angle ${\theta }_{2i-1}^{\mathrm{a}}$ is calculated as follows.

$${\theta }_{2i-1}^{\mathrm{a}}=\arctan 2\left(-n_{ix}^{\mathrm{a}}, n_{iy}^{\mathrm{a}}\right), i=1,2,3$$

(56)

When the angles ${\theta }_1^{\mathrm{a}}$~${\theta }_6^{\mathrm{a}}$ are obtained, the angle ${\theta }_7^{\mathrm{a}}$ is computed by

$${\theta }_7^{\mathrm{a}}=\arctan 2\left(a_{21}, a_{11}\right)$$

(57)

where

$$\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]\triangleq \left[\begin{array}{ccc}{\mathrm{c}}_{{\theta }_7^{\mathrm{a}}}& -{\mathrm{s}}_{{\theta }_7^{\mathrm{a}}}& 0\\ {\mathrm{s}}_{{\theta }_7^{\mathrm{a}}}& {\mathrm{c}}_{{\theta }_7^{\mathrm{a}}}& 0\\ 0& 0& 1\end{array}\right]={\left(R_6^{\mathrm{a}}\right)}^{-1}{R}_{\mathrm{ed}}^{\mathrm{a}}$$

(58)

where $R_6^{\mathrm{a}}$ is the rotation matrix between the frame of the sixth joint to the inertial frame.

For Arm-b, the variables ${\phi }_1^{\mathrm{b}}$, ${\phi }_2^{\mathrm{b}}$, ${\phi }_3^{\mathrm{b}}$, ${\phi }_4^{\mathrm{b}}$, ${\phi }_5^{\mathrm{b}}$, $p_{1,\mathrm{e}}^{\mathrm{b}}$, $k_{1,\mathrm{e}}^{\mathrm{b}}$, $p_{3,\mathrm{e}}^{\mathrm{b}}$, and $k_{3,\mathrm{e}}^{\mathrm{b}}$ are also known by solving Equation (50) According to the joint layout of Arm-b, $p_{0,\mathrm{e}}^{\mathrm{b}}$ and $k_{0,\mathrm{e}}^{\mathrm{b}}$ are determined as follows.

$$\left\{\begin{array}{l}{k}_{0,\mathrm{e}}^{\mathrm{b}}={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^{{\rm T}}, p_{0,\mathrm{e}}^{\mathrm{b}}=p_{1,1}^{\mathrm{b}}-\left(L_1^{\mathrm{b}}/2\right)k_{0,\mathrm{e}}^{\mathrm{b}}\\ k_{5,\mathrm{e}}^{\mathrm{b}}=k_{\mathrm{ed}}^{\mathrm{b}}, p_{5,\mathrm{e}}^{\mathrm{b}}=p_{\mathrm{ed}}^{\mathrm{b}}-\left(L^{\mathrm{b}}/2\right)k_{\mathrm{ed}}^{\mathrm{b}}\end{array}\right.$$

(59)

Based on the equivalent kinematics of Arm-b, the ith segment is modeled as an arc with ${\phi }_i^{\mathrm{b}}$. Since each segment is composed of six links with same mechanical structure, the central angle of the arc corresponding to each link is ${\phi }_i^{\mathrm{b}}/6$. In addition, by the D-H method, the normal vector of the plane where the ith equivalent arc is located can be calculated as follows.

$$n_i^{\mathrm{b}}=R_{i-1}^{\mathrm{b}}\left({\left[\begin{array}{ccc}0& {\mathrm{s}}_{{\theta }_{i,2}^{\mathrm{b}}}& {\mathrm{s}}_{{\theta }_{i,1}^{\mathrm{b}}}{\mathrm{c}}_{{\theta }_{i,2}^{\mathrm{b}}}\end{array}\right]}^{{\rm T}}/\sqrt{1-{\left({\mathrm{c}}_{{\theta }_{i,1}^{\mathrm{b}}}{\mathrm{c}}_{{\theta }_{i,2}^{\mathrm{b}}}\right)}^2}\right), i=1,2,3,4,5$$

(60)

where $R_{i-1}^{\mathrm{b}}$ is the rotation matrix between the frame of the ($i-1$) joint to the inertial frame.

By the geometric approach, the normal vector of the plane and the cosine of the arc angle (${\phi }_i^{\mathrm{b}}/6$) corresponding to each link are, respectively, obtained as follows.

$$\left\{\begin{array}{l}{n}_i^{\mathrm{b}}=\left(k_{i-1,\mathrm{e}}^{\mathrm{b}}\times k_{i,\mathrm{e}}^{\mathrm{b}}\right)/‖k_{i-1,\mathrm{e}}^{\mathrm{b}}\times k_{i,\mathrm{e}}^{\mathrm{b}}‖\\ \cos \left({\phi }_i^{\mathrm{b}}/6\right)={\mathrm{c}}_{{\theta }_{i,1}^{\mathrm{b}}}{\mathrm{c}}_{{\theta }_{i,2}^{\mathrm{b}}}\end{array}\right.$$

(61)

Therefore, the joint angles of Arm-b are determined as follows.

$$\left\{\begin{array}{l}{\theta }_{i,1}^{\mathrm{b}}=\arctan 2\left(n_{iz}^{\mathrm{b}}, {\mathrm{c}}_{{\mathsf{\phi }}_i^{\mathrm{b}}/6}\right)\\ {\theta }_{i,2}^{\mathrm{b}}=\arcsin \left(n_{iy}^{\mathrm{b}}\right)\end{array}\right. i=1,2,3,4,5$$

(62)

where ${\mathrm{c}}_{{\mathsf{\phi }}_i^{\mathrm{b}}/6}=\cos ({\phi }_i^{\mathrm{b}}/6)$. $n_{iz}^{\mathrm{b}}$ and $n_{iy}^{\mathrm{b}}$ are calculated as follows.

$$\left[\begin{array}{c}0\\ n_{iy}^{\mathrm{b}}\\ n_{iz}^{\mathrm{b}}\end{array}\right]\triangleq \left[\begin{array}{c}0\\ {\mathrm{s}}_{{\theta }_{i,2}^{\mathrm{b}}}\\ {\mathrm{s}}_{{\theta }_{i,1}^{\mathrm{b}}}{\mathrm{c}}_{{\theta }_{i,2}^{\mathrm{b}}}\end{array}\right]={\left(R_{i-1}^{\mathrm{b}}\right)}^{-1}\left(\left(k_{i-1,\mathrm{e}}^{\mathrm{b}}\times k_{i,\mathrm{e}}^{\mathrm{b}}\right)/‖k_{i-1,\mathrm{e}}^{\mathrm{b}}\times k_{i,\mathrm{e}}^{\mathrm{b}}‖\right)\sqrt{1-{\mathrm{c}}_{{\phi }_i^{\mathrm{b}}/6}^2} i=1,2,3,4,5$$

(63)

5. Simulation Study

5.1. The Parameters of Simulation Model

A discrete-serpentine heterogeneous dual-arm space robot (DSHDASR) for cooperatively transposing the target satellite is taken as an example for the simulation study. The heterogeneous space robotic system comprises a base, a non-offset discrete manipulator (named by Arm-a), and a segmented cable-driven serpentine manipulator (denoted by Arm-b), as shown in Figure 10. The D-H frames and the D-H parameters are the same as those in our previous research [30,31]. During the transposition of target satellite, the end effector of Arm-a is fixed to the target satellite at the locked point. And Arm-a serves as mission manipulator for moving the target satellite from its initial pose to the desired pose, while the serpentine arm is used as the auxiliary arm to offer monitoring for Arm-a. Here, the monitoring position of Arm-b is also the locked point. To avoid loss of visual tracking for this point, a certain distance between it and the end-effector of Arm-c must be maintained. The satellite-borne radars installed on the base are considered as the potential obstacles. As previously mentioned, there exists a minimal-radius sphere that can fully enclose an obstacle with an arbitrary shape. Therefore, the satellite-borne radars are regarded as spherical obstacles, as shown in Figure 10.

Assuming that the base of the DSHDASR is controlled, any point on the base could be taken as the origin of the inertial frame. For computational convenience, the inertial frame is established at the installation position of Arm-a, as shown in Figure 10. To validate the proposed method, we established a co-simulation model by use of Adams 2024 and MATLAB/Simulink 2020 software. The dynamics models of the DSHDASR and target satellite were built in Adams 2024, and the corresponding planner and controller were realized in MATLAB/Simulink 2020. At the initial moment, the joint angles of Arm-a and Arm-b are given as follows.

$$\left\{\begin{array}{l}{\Theta }^{\mathrm{a}}={\left[\begin{array}{ccccccc}100.21°& 73.88°& -7.98°& 65.73°& -17.31°& -51.88°& 9.67°\end{array}\right]}^{{\rm T}}\\ {\Theta }^{\mathrm{b}}={\left[-0.63° 0.35° -0.26° 2.88° -0.41° -0.34° -1.30° 1.51° 1.24° 8.03°\right]}^{{\rm T}}\end{array}\right.$$

(64)

The center positions of obstacle 1 and obstacle 2 are as follows.

$$\left\{\begin{array}{l}{r}_{\mathrm{obs}1}={\left[\begin{array}{ccc}-1.8078 \mathrm{m}& 1.0169 \mathrm{m}& -0.6020 \mathrm{m}\end{array}\right]}^{{\rm T}}\\ r_{\mathrm{obs}2}={\left[\begin{array}{ccc}-1.7244 \mathrm{m}& 0.7186 \mathrm{m}& -3.0561 \mathrm{m}\end{array}\right]}^{{\rm T}}\end{array}\right.$$

(65)

Moreover, the envelope radii for Arm-a and Arm-b, the radii of the enclosing spheres for both obstacles, and the safety margin are set as follows.

$$\left\{\begin{array}{l}{R}^{\mathrm{a}}=90\mathrm{mm}, R^{\mathrm{b}}=35\mathrm{mm}\\ R_1=R_2=260\mathrm{mm}\\ {\epsilon }_{\mathrm{osb}}=100\mathrm{mm}\end{array}\right.$$

(66)

5.2. The Planned Trajectory of the Target Satellite

To achieve the transposition of target satellite, the desired movement trajectory of the target satellite’s center of mass should be firstly planned. According to Section 4.2, the quintic polynomial interpolation method is introduced to generate the trajectory. In the simulation, the initial and final poses of center of mass are as follows.

$$\left\{\begin{array}{l}{X}_0=\left[\begin{array}{cccccc}-5.3135& -0.9097& -1.2121& -68.70& -20.31& 90.76\end{array}\right]\\ X_{\mathrm{f}}=\left[\begin{array}{cccccc}-3.8552& 2.8612& -2.0035& -122.59& -2.96& 100.89\end{array}\right]\end{array}\right.$$

(67)

Substituting Equation (67) into Equation (42), the variations of position and attitude for the target satellite’s center of mass are calculated. And the variation curves are as shown in Figure 11.

5.3. Target Satellite Transposition Simulation

At the initial simulation time, the homogeneous transformation matrix from the end-effectors of Arm-a or Arm-b to the center of mass of the target satellite are, respectively, given as follows.

$$T_{\mathrm{et}}^{\mathrm{a}}=\left[\begin{array}{cccc}-0.75019& 0.66121& -0.00056& 0.43833\\ -0.66122& -0.75017& 0.00018& 0.40736\\ -0.00030& 0.00050& 0.99999& -2.80831\\ 0& 0& 0& 1\end{array}\right]$$

(68)

$$T_{\mathrm{et}}^{\mathrm{b}}=\left[\begin{array}{cccc}0.26258& -0.92949& -0.25903& 0.22797\\ 0.93281& 0.31319& -0.17826& -0.33989\\ 0.24682& -0.19482& 0.94928& -2.28089\\ 0& 0& 0& 1\end{array}\right]$$

(69)

The total simulation time is 60 s. Based on equivalent kinematics model, the simulation analysis of the coordinated collision-free trajectory planning method was implemented by the co-simulation system. On basis of the simulation results, the curves of pose tracking error for Arm-a are shown in Figure 12. It can be seen that the maximum position tracking errors of the Arm-a tip along the x_I, y_I, and z_I axes are −8.991 × 10⁻⁶ m, −6.207 ×10⁻⁶ m, and −5.889 × 10⁻⁶ m, respectively. The maximum attitude tracking errors of the Arm-a tip, expressed in Z-Y-X Euler angles, are 3.436 × 10⁻⁵ rad, 1.591 × 10⁻⁵ rad, and 1.704 × 10⁻⁵ rad, respectively.

Meanwhile, Arm-b is used to provide auxiliary monitoring for Arm-a by maintaining a fixed distance and specific direction between its tip and the locked point. Figure 13 shows the position and direction tracking errors of the Arm-b tip. In this paper, the optimal monitoring attitude is achieved simply by ensuring that the direction of the Arm-b tip remains parallel to the line that is from the Arm-b tip to the locked point. As a result, the attitude error of Arm-b is only the deviation in the direction of the Arm-b tip. The position tracking errors of the Arm-b tip are 4.803 × 10⁻⁵ m, −5.703 × 10⁻⁵ m, and −8.015 × 10⁻⁵ m with respect to x_I, y_I, and z_I axes, respectively, while the attitude tracking error in the monitoring direction is 4.846 × 10⁻⁵ rad.

Similarly, as shown in Figure 14, the maximum position tracking errors of the target satellite are −1.378 × 10⁻⁵ m, −2.567 × 10⁻⁵ m, and −3.560 × 10⁻⁵ m, respectively. And its maximum attitude tracking errors are 7.404 × 10⁻⁵ rad, 2.214 × 10⁻⁵ rad, and −7.971 × 10⁻⁵ rad, respectively. This indicates that the target satellite center of mass can track the planned trajectory with extremely high precision.

The minimum distances between the two obstacles (obstacle 1 and obstacle 2) and Arm-a or Arm-b are shown in Figure 15. The results show that the minimum distance between obstacle 1 and Arm-a approaches the critical value (0.45 m) when the time is 41.57 s. To ensure a safe distance between Arm-a and obstacle 1, the proposed trajectory planning method utilizing equivalent kinematics is used to maintain the minimum separation of 0.45 m after 41.57 s. Moreover, the minimum distance between obstacle 1 and Arm-b is always greater than 2.063 m, which is significantly greater than the collision threshold. As a result, this demonstrates that Arm-b also maintains a safe distance from obstacle 1 during the simulation.

For obstacle 2, the closest distance between it and Arm-b reaches the critical threshold (0.373 m) when the time is 47.08 s. Thereafter, this distance remains at the critical threshold by using the proposed method, thereby preventing potential collisions. Furthermore, the minimum distance between obstacle 2 and Arm-a exceeds 2.156 m consistently, which means no collision risk exists between Arm-a and obstacle 2. During the simulation, the 3D states of the DSHDASR and the target satellite are illustrated in Figure 16, corresponding to simulation milestones when the simulation time is 0.00 s, 15.00 s, 30.00 s, 41.57 s, 47.08 s, and 60.00 s, respectively.

To further validate the correctness of the proposed method, additional simulation cases were conducted. The desired pose of the target satellite is different in all these cases. The simulation results are listed in

Table 1. The simulation results for additional simulation cases.

Case	Xf	S or F
1	[−3.8117, 2.8220, −2.0436, −126.94, −3.66, −98.99]	S
2	[−3.8356, 2.8412, −2.0232, −124.64, −1.61, −101.65]	S
3	[−3.8757, 2.8816, −1.9837, −126.95, −3.64, −100.62]	S
4	[−3.8957, 2.9012, −1.9640, −123.16, −6.66, −95.95]	S
5	[−3.9160, 2.9212, −1.9437, −127.81, −7.90, −95.65]	S

. Here, the symbol S means success, and F is failure. It can be seen that the proposed method is capable of successfully producing an obstacle-avoidance trajectory for each simulation case.

As we know, gimbal lock occurs when the pitch angle reaches ±90° for the Z-Y-X Euler angles. At this singularity, the roll and yaw axes are aligned, causing the loss of one rotational degree of freedom. Therefore, the pitch angle is constrained within a safe range of −90° to 90° in the simulation. In fact, the variation of the pitch angle is confined within [−21°, −1°] in the simulation; in this case, gimbal lock does not occur. To completely eliminate the issue of gimbal lock, the description of attitude is transitioned from Euler angles to quaternions in the simulation. The specific calculation process is as follows.

Let (α, β, γ) denote the Z-Y-X Euler angles, representing the rotations about the Z, Y, and X axes, respectively. The overall rotation matrix is obtained by the sequential multiplication of elementary rotation matrices.

$$R=R_z(\alpha )R_y(\beta )R_x(\gamma )=\left[\begin{array}{ccc}{\mathrm{c}}_{\alpha }{\mathrm{c}}_{\beta }& {\mathrm{c}}_{\alpha }{\mathrm{s}}_{\beta }{\mathrm{s}}_{\gamma }-{\mathrm{s}}_{\alpha }{\mathrm{c}}_{\gamma }& {\mathrm{s}}_{\alpha }{\mathrm{s}}_{\gamma }+{\mathrm{c}}_{\alpha }{\mathrm{s}}_{\beta }{\mathrm{c}}_{\gamma }\\ {\mathrm{s}}_{\alpha }{\mathrm{c}}_{\beta }& {\mathrm{c}}_{\alpha }{\mathrm{c}}_{\gamma }+{\mathrm{s}}_{\alpha }{\mathrm{s}}_{\beta }{\mathrm{s}}_{\gamma }& {\mathrm{s}}_{\alpha }{\mathrm{s}}_{\beta }{\mathrm{c}}_{\gamma }-{\mathrm{c}}_{\alpha }{\mathrm{s}}_{\gamma }\\ -{\mathrm{s}}_{\beta }& {\mathrm{c}}_{\beta }{\mathrm{s}}_{\gamma }& {\mathrm{c}}_{\beta }{\mathrm{c}}_{\gamma }\end{array}\right]\triangleq \left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

(70)

where ${\mathrm{c}}_{\alpha }=\cos \alpha$, ${\mathrm{c}}_{\beta }=\cos \beta$, ${\mathrm{c}}_{\gamma }=\cos \gamma$, ${\mathrm{s}}_{\alpha }=\sin \alpha$, ${\mathrm{s}}_{\beta }=\sin \beta$, and ${\mathrm{s}}_{\gamma }=\sin \gamma$. Because α, β, and γ are all known, the elements of the matrix $R$ are obtained. Setting the unit quaternion as $q=[q_0, q_1, q_2, q_3]$, we can obtain the following.

$$\begin{array}{cccc}\begin{array}{l}if\left(1+r_{11}+r_{22}+r_{33}\right)\ne 0\\ \left\{\begin{array}{l}{q}_0=\pm {\displaystyle \frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}}\\ q_1={\displaystyle \frac{1}{4q_0}}\left(r_{32}-r_{23}\right)\\ q_2={\displaystyle \frac{1}{4q_0}}\left(r_{13}-r_{31}\right)\\ q_3={\displaystyle \frac{1}{4q_0}}\left(r_{21}-r_{12}\right)\end{array}\right.\end{array}& \begin{array}{l}if \left(1+r_{11}-r_{22}-r_{33}\right)\ne 0\\ \left\{\begin{array}{l}{q}_1=\pm {\displaystyle \frac{\sqrt{1+r_{11}-r_{22}-r_{33}}}{2}}\\ q_2={\displaystyle \frac{1}{4q_1}}\left(r_{12}+r_{21}\right)\\ q_3={\displaystyle \frac{1}{4q_1}}\left(r_{13}+r_{31}\right)\\ q_0={\displaystyle \frac{1}{4q_1}}\left(r_{23}+r_{32}\right)\end{array}\right.\end{array}& \begin{array}{l}if \left(1-r_{11}+r_{22}-r_{33}\right)\ne 0\\ \left\{\begin{array}{l}{q}_2=\pm {\displaystyle \frac{\sqrt{1-r_{11}+r_{22}-r_{33}}}{2}}\\ q_1={\displaystyle \frac{1}{4q_2}}\left(r_{12}+r_{21}\right)\\ q_3={\displaystyle \frac{1}{4q_2}}\left(r_{23}+r_{32}\right)\\ q_0={\displaystyle \frac{1}{4q_2}}\left(r_{13}-r_{31}\right)\end{array}\right.\end{array}& \begin{array}{l}if \left(1-r_{11}-r_{22}+r_{33}\right)\ne 0\\ \left\{\begin{array}{l}{q}_3=\pm {\displaystyle \frac{\sqrt{1-r_{11}-r_{22}+r_{33}}}{2}}\\ q_1={\displaystyle \frac{1}{4q_3}}\left(r_{13}+r_{31}\right)\\ q_2={\displaystyle \frac{1}{4q_3}}\left(r_{23}+r_{32}\right)\\ q_0={\displaystyle \frac{1}{4q_3}}\left(r_{21}-r_{12}\right)\end{array}\right.\end{array}\end{array}$$

(71)

Consequently, gimbal lock is avoided through the parameterization of attitude using quaternions.

6. Conclusions

In this paper, an equivalent kinematics modeling method and a coordinated collision-free trajectory planning method are proposed for a discrete-serpentine heterogeneous dual-arm space robot (DSHDASR) with a non-offset discrete arm (denoted by Arm-a) and a segmented cable-driven serpentine manipulator (named Arm-b). This methodology provides an innovative strategy for the kinematic resolution and motion planning of DSHDASR, directly addressing the challenge of obstacle avoidance in cluttered environments. Based on equivalent kinematics, the overall kinematics model of DSHDASR is represented by spatial curves that are formed by multiple space arcs. Afterwards, the problem of coordinated motion planning with obstacle avoidance for the DSHDASR is transformed as the minimization of the objective function that is established by the equivalent kinematics method. Using the target satellite transposition as an application scenario of the DSHDASR, a co-simulation system is built to verify the proposed method. The simulation results illustrate that the position and attitude tracking errors of the Arm-a tip, the Arm-b tip, and the target’s center of mass are all less than 8.5 × 10⁻⁵ m and 8.0 × 10⁻⁵ rad, respectively. Meanwhile, the distance between obstacles and Arm-a or Arm-b is also greater than the safety distance threshold. This implies that Arm-a can move the target satellite from the initial pose to the desired pose along the planned trajectory, while Arm-b can offer precise auxiliary monitoring, based on the proposed method. Moreover, obstacle avoidance for Arm-a and Arm-b is also achieved through this method.

The current research only focuses on the kinematics and trajectory planning of the heterogeneous dual-arm robot. The application of these methods for heterogeneous multi-arm systems has not been explored. In addition, dynamics modeling of heterogeneous space robots and pose stabilization control for the base of post-capture combined system are not considered. In the future, we will strive to do research on them.