A Control Method of Space Manipulator for Peg-in-Hole Assembly Task Considering Equivalent Stiffness Optimization

Abstract

To meet the control requirements of high precision and high robustness for peg-in-hole assembly tasks, an optimized control method for a peg-in-hole assembly task of a space manipulator is proposed to reduce the system disturbance caused by the change contact status during the assembly process. The first step is to build an equivalent stiffness model, which considers the structure and control characteristics of the space manipulator. Flexibility indices along the assembly direction are then created. On completion of the flexibility indices, the assembly configuration of the manipulator is optimized with the gain of the joint controller. After that, based on the sliding mode impedance control law, the disturbance of contact force is compensated using a zero-sum optimal control compensation strategy. Finally, the correctness and effectiveness of the control method are verified through simulation experiments. The results of the simulation experiments show that the contact force of the space manipulator can be precisely controlled by the method proposed in this paper. Compared with existing methods, the sudden change of contact force and the disturbing force of the base are reduced by 90% and 54%, respectively. A control method of the space manipulator for a peg-in-hole assembly task considering the equivalent stiffness optimization is proposed, which effectively reduces the influence of disturbance caused by contact collision and improves the control robustness of peg-in-hole assembly tasks.

1. Introduction

Deep space exploration and on-orbit service are major research projects in China’s space exploration. The development of related technologies will directly affect the implementation of China’s future space exploration plans and the deployment of related space strategies. During deep space exploration and on-orbit services, there are a lot of heavy and complex space assembly tasks to be completed, such as the dismantling and reorganization of abandoned satellites on-orbit and the construction of space solar power plants [1], the construction of the survey telescope [2], the construction of the Ultra-Large Aperture On-orbit Assembly Space Telescope [3,4], and the automated construction of the remote exploration base [5,6]. These tasks inevitably require space manipulators to complete inserting and screwing actions, which all belong to the peg-in-hole assembly task. As a typical inserting task, the peg-in-hole assembly task is representative of on-orbit operation tasks such as space unit replacement and equipment installation. Therefore, it is of great significance to study the peg-in-hole assembly technology of space manipulators.

When a space manipulator interacts with the environment, a small planning error may cause the object gripped by the manipulator to break away from the contact surface or produce excessive contact force. Unfortunately, there are usually errors in the position data during the assembly process. For example, the pose obtained by computer vision may have an error of ±6 mm [7,8]. The author once studied the pose estimation based on 3D point cloud in an area of 1 m², and the maximum error was about 8 mm. To solve this problem, on the one hand, a hole search operation is required to find the specific position of the hole before the plug-in process (Figure 1). On the other hand, a compliance control method is needed to adjust the relationship between the position of the end of the manipulator and the contact force.

The compliance control method mainly includes hybrid position-force control and the impedance control method [9]. Chen [10] proposed a peg-in-hole assembly algorithm for robotic astronauts based on hybrid position-force control. Wu [11] studied the precision peg-in-hole assembly method of industrial manipulators based on hybrid position-force control. These algorithms are not of interest in this study because they switch between force and position control and need to identify the environmental parameters in real time to adjust the direction of the force. As a result, it is difficult to actualize fully compliant behavior. On the other hand, the impedance control [12] is successful when the manipulator is in free space and also when it is in contact with the environment. Thus, it can also be used in cases of unknown environments. Although the sensorless force controller has various advantages, its accuracy has difficulty meeting the requirements of assembly tasks [13,14], so this paper does not pay attention to these methods.

At the end of the hole search operation and the beginning of the plug-in process, the contact state of the assembly changes. It results in a sudden change in the contact force at the manipulator’s end, further causing disturbance of the base. It is likely to cause the vibration of the spacecraft structure and the instability of the attitude and orbit control system. More seriously, it may even damage the force sensor and other precision instruments due to the contact force amplitude exceeding the range and further damaging the space manipulator. Therefore, how to reduce the sudden change of contact force and suppress the disturbance of the base has become the key to the success of the peg-in-hole assembly task.

Restricted by launch conditions, the space manipulator exhibits significant flexibility due to its lightweight features. The flexibility of its joints and links is mapped to the operating space through the kinematics model of the manipulator, resulting in weak equivalent stiffness at the end of the space manipulator, which changes continuously with the change of configuration. This provides ideas for suppressing the sudden change of contact force and the disturbance of the base caused by it. That is, the manipulator can obtain better flexibility by optimizing the equivalent stiffness of the end so as to suppress the disturbance at the moment of contact collision and enhance the system’s robustness. Qu et al. integrated the stiffness model of each influencing factor into the overall stiffness model of the manipulator through the principle of superposition [15]. Hui et al. used the equivalent stiffness model to compensate for the deformation of the manipulator in the process of controlling the cutting force to improve the trajectory accuracy [16]. Jiao et al. proposed an optimization method based on the optimal equivalent stiffness, which can avoid the singularity and the limit of joint angles while obtaining an operating posture with better equivalent stiffness [17]. Lin et al. established the equivalent stiffness map of the end of the manipulator in the workspace to select the optimal position and determine the posture of the end through the deformation estimation index [18]. Qu et al. designed the fitness function based on the semi-axial length of the equivalent stiffness ellipsoid and used the Genetic Algorithm to optimize the pose of the 7-DOF redundant manipulator [19]. Tian et al. used the Rayleigh quotient to evaluate the stiffness performance of the manipulator and obtained the optimal configuration of the entire workspace with the Genetic Algorithm method [20]. However, there are specific problems with the build of equivalent stiffness model in the above studies. One of these is that they only considered the joint flexibility of the manipulator from the perspective of the structure’s inherent stiffness. Another is that they used the configuration as a decision variable for relevant analysis and optimization. For space manipulators performing assembly tasks, a compliance controller is required to obtain appropriate contact force. Control gain is an essential factor that causes the weak equivalent stiffness and the changing characteristics with the configuration of the end of the manipulator. Therefore, the effect of the control gain on the equivalent stiffness cannot be ignored.

During the process of peg-in-hole tasks, the sudden change in contact force can be regarded as the disturbance of the environment to the space manipulator system. By optimizing the equivalent stiffness of the space manipulator, the disturbance can be reduced to control the sudden contact force within the sensor range. However, the actual value of the contact force still has a significant sudden change compared with the expected value, which does not meet the accuracy requirements of the contact force control during peg-in-hole assembly. In the previous research on compliance control, Christian [21,22] proposed a control strategy that unified impedance control and admittance control based on a hybrid systems framework to improve the stability and performance characteristics of interaction controllers. CAESAR [23] was LWR3′s consistent continuation in the development of a force/torque-controlled robot system, which allowed the required stiffness setting range to be reduced to zero. Li et al. proposed a fuzzy sliding mode impedance controller to solve the problem of low robustness due to the uncertainty and disturbance of the manipulator or the environment [24]; Xiao realized autonomous peg-in-hole assembly by designing fuzzy rules to adjust impedance control parameters, but it requires professionals to refine and summarize the experience of adjusting parameters based on a large number of experiments [25]. Duan et al. designed an adaptive variable impedance model to achieve the trajectory tracking of the dual manipulator subject to external disturbance [26]; based on the collision model and the extended Kalman filter to estimate the environmental stiffness online, Roveda [27] proposed a discrete controller to realize the gain of the adaptive controller. Sadeghian et al. proposed a disturbance observer based on task error detection and conducted a disturbance force detection experiment on the KUKA LWR4 lightweight manipulator [28]; Jia et al. realized the impedance control of unknown disturbance in the nullspace of robotic astronauts by designing a disturbance observer [29]. Dong et al. designed a decentralized robust zero-sum optimal controller for a double-joint reconfigurable robot to realize disturbance compensation in an uncertain environment [30], inspiring this paper’s research. However, the decentralized control system compensates for disturbance in the joint space of the double-joint reconfigurable robot. So, its results cannot be directly applied to the peg-in-hole assembly in on-orbit operation tasks.

To sum up, the above research mainly has the following shortcomings: (1) Some researchers only considered the structural flexibility in the modeling and analysis of the equivalent stiffness of the manipulators but did not consider the influencing factors of the control method, which leads to the insufficient model of the equivalent stiffness. (2) Other researchers tuned the control parameters by estimating the environment stiffness in their work but did not consider the structural stiffness of the manipulator. However, the structural stiffness of the space manipulator may be much smaller than the environmental stiffness. (3) The existing research on impedance control have limitations when applied to space assembly tasks: The controller based on the hybrid systems framework needs to switch the control law frequently; designing fuzzy rules requires researchers to generalize on the basis of a large number of experiments; the stiffness estimation based on the extended Kalman filter leads to insufficient control accuracy; and controllers based on disturbance observers require accurate dynamics models.

Aiming at the above problems, in order to ensure the accuracy and robustness of the peg-in-hole assembly control, a control method of a space manipulator for peg-in-hole assembly tasks considering equivalent stiffness optimization is proposed in this paper. This method can reduce the sudden change in contact force and suppress the disturbing force of the base by determining the appropriate manipulator’s configuration and control gain. It improves the accuracy and robustness of peg-in-hole assembly control.

The research in this paper possesses the following traits:

• By combining the structure and control characteristics of the space manipulator, the controller parameters and configuration are introduced into the equivalent stiffness optimization process of the manipulator.
• By combining the advantages of the sliding mode control method, an impedance control law based on the result of equivalent stiffness optimization is deigned, and the zero-sum game is introduced to compensate for the interference caused by contact collision.

The rest of the paper is composed of five chapters. In the second chapter, the stiffness model of space manipulator is built. In the third chapter, the stiffness optimization method for peg-in-hole assembly is presented. In the fourth chapter, the sliding mode impedance control based on disturbance compensation is designed. In the fifth chapter, the simulation experiment and analysis are carried out. Finally, the conclusions are drawn.

2. Stiffness Modeling of Space Manipulator

2.1. Kinematics and Dynamics Model

In this section, a brief review of the kinematics and dynamics model of space manipulators is presented. The kinematics equations and dynamics equations of the space manipulator studied in this paper have general forms. Equations (1)–(3) are the position-level kinematics equation, the velocity-level kinematics equation, and the Cartesian space dynamics equation of the space manipulator, respectively:

$${}_n{}^{i}T={}_{i+1}{}^{i}T\cdots {}_n{}^{n-1}T=\left[\begin{array}{c}\begin{array}{cccc}{n}_{{}_i}& o_i& a_i& p_i\end{array}\\ \begin{array}{cccc}0& 0& 0& 1\end{array}\end{array}\right],$$

(1)

$$\dot{X}=J_{\theta }(q)\dot{q},$$

(2)

$$F_j=M_x\left(q\right)\ddot{X}+C_x(q,\dot{q})\dot{X}+F_e-d_e,$$

(3)

where ${}_{i+1}{}^{i}T\in {\mathrm{R}}^{4\times 4}$ is the transformation matrix of two adjacent coordinate systems; $n_i$, $o_i$, $a_i$, $p_i\in {\mathrm{R}}^{3\times 1}$ are functions of joint angle $q\in {\mathrm{R}}^{n\times 1}$; $n$ is the number of joints; $J_{\theta }(q)\in {\mathrm{R}}^{6\times n}$ (abbreviated as $J_{\theta }$) is the Jacobian matrix for the space manipulator; $F_j$ is the generalized operating force vector; $M_x(q),C_x(q,\dot{q})$ are the Cartesian space inertia term and the Coriolis force term; $F_e$ is the end contact force in the Cartesian space of the space manipulator; and $d_e$ is the contact force disturbance term.

2.2. Equivalent Stiffness Model

Once the kinematics and dynamics models are obtained, the equivalent stiffness model can be created. The equivalent stiffness of the space manipulator is affected by the structural stiffness (joint stiffness and link stiffness) and control stiffness. The stiffness can be expressed as the inverse matrix of flexibility. For the joint flexibility, the differential motion vector of the link coordinate system can be obtained by calculating the bending and torsion deformation caused by the external load of the joint. For the flexibility of the link, the differential motion vector of the link coordinate system can be obtained by calculating the stretching, bending, and torsion deformation of the link. By analyzing the differential motion of the joint and link subjected to external forces, the flexible deformation of the end of the manipulator can be obtained. Then, the flexibility matrix can be obtained. For the control flexibility, it can be obtained by the joint controller gain and the Jacobian matrix.

2.2.1. Joint Flexibility

When the joint $i$ is subjected to an external load, it will produce bending deformation and torsion deformation. The differential motion of the link coordinate system caused by deformation is shown in Figure 2. The amount of deformation can be calculated by Equation (4) as follows:

$$\{\begin{array}{c}{\tau }_{\theta i}=K_{\mathrm{T}i}\Delta {\theta }_{\mathrm{J}i}\\ \begin{array}{l}{\tau }_{\alpha i}=K_{\mathrm{B}i}\Delta {\alpha }_{\mathrm{J}i}\\ {\tau }_{\beta i}=K_{\mathrm{B}i}\Delta {\beta }_{\mathrm{J}i}\end{array}\end{array}$$

(4)

where $\Delta {\theta }_{\mathrm{J}i}$ is the joint angle error caused by the torsional deformation of joint $i$, $K_{\mathrm{T}i}$ is the torsional stiffness coefficient of joint $i$, ${\tau }_{\theta i}$ is the torque received by the joint $i$, $\Delta {\alpha }_{\mathrm{J}i}$ and $\Delta {\beta }_{\mathrm{J}i}$ are the differential rotation caused by the bending deformation of joint $i$, $K_{\mathrm{B}i}$ is the bending stiffness coefficient of the joint $i$, and ${\tau }_{\alpha i}$ and ${\tau }_{\beta i}$ are the bending moment of the joint $i$.

It can be seen from the differential motion that the end deformation $D_{\mathrm{J}\theta }$ caused by the torsion of the space manipulator joint and the end deformation $D_{\mathrm{J}\alpha }$ and $D_{\mathrm{J}\beta }$ caused by the bending can be expressed as:

$$\{\begin{array}{l}{D}_{\mathrm{J}\theta }=J_{\theta }(q)\Delta {\theta }_{\mathrm{J}}\\ D_{\mathrm{J}\alpha }=J_{\alpha }(q)\Delta {\alpha }_{\mathrm{J}}\\ D_{\mathrm{J}\beta }=J_{\beta }(q)\Delta {\beta }_{\mathrm{J}}\end{array}$$

(5)

where $J_{\alpha }(q),J_{\beta }(q)\in {\mathrm{R}}^{6\times n}$ represents the Jacobian matrix that maps the bending deformation of the joint space to the operation space, which can be abbreviated as $J_{\alpha }$ and $J_{\beta }$, respectively. The joins of the manipulator in this paper are all rotating joints, hence, the i-th column of $J_{\alpha }$ and $J_{\beta }$ can be expressed as:

$$J_{\alpha i}=\left[\begin{array}{c}{\left(p_i\times n_i\right)}_x\\ {\left(p_i\times o_i\right)}_x\\ {\left(p_i\times a_i\right)}_x\\ n_{ix}\\ o_{ix}\\ a_{ix}\end{array}\right], J_{\beta i}=\left[\begin{array}{c}{\left(p_i\times n_i\right)}_y\\ {\left(p_i\times o_i\right)}_y\\ {\left(p_i\times a_i\right)}_y\\ n_{iy}\\ o_{iy}\\ a_{iy}\end{array}\right].$$

According to the duality principle, the static equivalent joint torque can be obtained from the external force $F_e$ at the manipulator’s end:

$$\{\begin{array}{l}{\tau }_{\theta }=J_{\theta }{}^T{F}_e\\ {\tau }_{\alpha }=J_{\alpha }{}^T{F}_e\\ {\tau }_{\beta }=J_{\beta }{}^T{F}_e\end{array}.$$

(6)

From Equations (4)–(6), the deformation of the end of the space manipulator caused by joint deformation can be obtained as:

$$\{\begin{array}{l}{D}_{\mathrm{J}\theta }=J_{\theta }{K}_{\mathrm{T}}{}^{-1}{J}_{\theta }{}^{\mathrm{T}}{F}_e\\ D_{\mathrm{J}\alpha }=J_{\alpha }{K}_{\mathrm{B}}{}^{-1}{J}_{\alpha }{}^{\mathrm{T}}{F}_e\\ D_{\mathrm{J}\beta }=J_{\beta }{K}_{\mathrm{B}}{}^{-1}{J}_{\beta }{}^{\mathrm{T}}{F}_e\end{array},$$

(7)

where $K_{\mathrm{T}}$, $K_{\mathrm{B}}\in {\mathrm{R}}^{n\times n}$ are the diagonal matrices formed by $K_{\mathrm{T}i}$ and $K_{\mathrm{B}i}$, respectively. Since the stiffness of each joint is not zero, these two matrices have inverse matrices.

When the joint deformation is minimal, the total deformation of the end can be expressed by Equation:

$$D_{\mathrm{J}}=D_{\mathrm{J}\theta }+D_{\mathrm{J}\alpha }+D_{\mathrm{J}\beta }=C_{\mathrm{J}}{F}_e,$$

(8)

where $C_{\mathrm{J}}=J_{\theta }{K}_{\mathrm{T}}{}^{-1}{J}_{\theta }{}^{\mathrm{T}}+J_{\alpha }{K}_{\mathrm{B}}{}^{-1}{J}_{\alpha }{}^{\mathrm{T}}+J_{\beta }{K}_{\mathrm{B}}{}^{-1}{J}_{\beta }{}^{\mathrm{T}}$, where $C_{\mathrm{J}}\in {\mathrm{R}}^{6\times 6}$ represents the flexibility of the joint at the end of the space manipulator.

2.2.2. Link Flexibility

When force analysis is carried out on a link $i$, it is regarded as a cantilever beam with a fixed head and a free end. A coordinate system $O_{ei}{X}_{ei}{Y}_{ei}{Z}_{ei}$ is established at the end of the link. The origin of the coordinate system is at the intersection of the end section and the axis. Before the deformation of the link, each coordinate axis of $O_{ei}{X}_{ei}{Y}_{ei}{Z}_{ei}$ is in the same direction with $O_i{X}_i{Y}_i{Z}_i$. As shown in Figure 3, the components of the force on the end of the link along the X, Y, and Z directions of $O_{ei}{X}_{ei}{Y}_{ei}{Z}_{ei}$ are $f_{xi}$, $f_{yi}$, and $f_{zi}$, respectively, and the components of the torque on the end of the link along three axes are ${\tau }_{xi}$, ${\tau }_{yi}$, and ${\tau }_{zi}$, respectively.

The differential motion vector $d_{Li}$ of the coordinate system $O_{ei}{}^{\prime }{X}_{ei}{}^{\prime }{Y}_{ei}{}^{\prime }{Z}_{ei}{}^{\prime }$ after deformation can be derived based on the analysis of the extension, bending, and torsion deformation of the link caused by the external force and torque as follows:

$$d_{Li}=\left[\begin{array}{c}\Delta x_{\mathrm{L}i}\\ \Delta y_{\mathrm{L}i}\\ \Delta z_{\mathrm{L}i}\\ \Delta {\alpha }_{\mathrm{L}i}\\ \Delta {\beta }_{\mathrm{L}i}\\ \Delta {\theta }_{\mathrm{L}i}\end{array}\right]=\left[\begin{array}{cccccc}\frac{l_i}{E{A}_i}& 0& 0& 0& 0& 0\\ 0& \frac{l_i{}^3}{3E{I}_i}& 0& 0& 0& \frac{l_i{}^2}{2E{I}_i}\\ 0& 0& \frac{l_i{}^3}{3E{I}_i}& 0& \frac{-l_i{}^2}{2E{I}_i}& 0\\ 0& 0& 0& \frac{l_i}{G{I}_i{}_p}& 0& 0\\ 0& 0& \frac{-l_i{}^2}{2E{I}_i}& 0& \frac{l_i}{E{I}_i}& 0\\ 0& \frac{l_i{}^2}{2E{I}_i}& 0& 0& 0& \frac{l_i}{E{I}_i}\end{array}\right]\left[\begin{array}{c}{f}_{xi}\\ f_{yi}\\ f_{zi}\\ {\tau }_{xi}\\ {\tau }_{yi}\\ {\tau }_{zi}\end{array}\right],$$

(9)

where $l_i$ is the length of the link $i$ and $E{A}_i$, $E{I}_i$, and $G{I}_i{}_p$ are the tensile stiffness, bending stiffness, and torsional stiffness of the link, respectively.

It can be seen from the differential motion that the six-dimensional deformation caused by the stretching, bending, and torsion of the link can be expressed as:

$$\{\begin{array}{l}{D}_{\mathrm{L}x}=J_x(q)\Delta x_{\mathrm{L}}\\ D_{\mathrm{L}y}=J_y(q)\Delta y_{\mathrm{L}}\\ D_{\mathrm{L}z}=J_z(q)\Delta z_{\mathrm{L}}\\ D_{\mathrm{L}\theta }=J_{\theta }(q)\Delta {\theta }_{\mathrm{L}}\\ D_{\mathrm{L}\alpha }=J_{\alpha }(q)\Delta {\alpha }_{\mathrm{L}}\\ D_{\mathrm{L}\beta }=J_{\beta }(q)\Delta {\beta }_{\mathrm{L}}\end{array},$$

(10)

where $J_x(q),J_y(q),J_z(q)\in {\mathrm{R}}^{6\times n}$ represent the Jacobian matrix where the deformation of the link is mapped from the joint space to the Cartesian space, abbreviated as $J_x$, $J_y$, and $J_z$, and the $i$-th column can be expressed as:

$$J_{xi}=\left[\begin{array}{c}{n}_{ix}\\ o_{ix}\\ a_{ix}\\ 0\\ 0\\ 0\end{array}\right], J_{yi}=\left[\begin{array}{c}{n}_{iy}\\ o_{iy}\\ a_{iy}\\ 0\\ 0\\ 0\end{array}\right], J_{zi}=\left[\begin{array}{c}{n}_{iz}\\ o_{iz}\\ a_{iz}\\ 0\\ 0\\ 0\end{array}\right].$$

Under static conditions, according to the duality principle, the equivalent force of the link can be obtained from the external force $F_e$ at the end of the space manipulator as follows:

$$\{\begin{array}{c}\begin{array}{l}{f}_x=J_x{}^{\mathrm{T}}{F}_e\\ f_y=J_y{}^{\mathrm{T}}{F}_e\\ f_z=J_z{}^{\mathrm{T}}{F}_e\end{array}\\ \begin{array}{l}{\tau }_x=J_{\theta }{}^{\mathrm{T}}{F}_e\\ {\tau }_y=J_{\alpha }{}^{\mathrm{T}}{F}_e\\ {\tau }_z=J_{\beta }{}^{\mathrm{T}}{F}_e\end{array}\end{array}.$$

(11)

According to Equations (9)–(11), the deformation of the terminal of the space manipulator caused by the deformation of links can be obtained as:

$$\{\begin{array}{l}{D}_{\mathrm{L}x}=J_x{K}_{xfx}{J}_x{}^{\mathrm{T}}{F}_e\\ D_{\mathrm{L}y}=J_y\left(K_{yfy}{J}_y{}^{\mathrm{T}}+K_{y\tau \theta }{J}_{\theta }{}^{\mathrm{T}}\right)F_e\\ D_{\mathrm{L}z}=J_z\left(K_{zfz}{J}_z{}^{\mathrm{T}}+K_{z\tau \beta }{J}_{\beta }{}^{\mathrm{T}}\right)F_e\\ D_{\mathrm{L}\alpha }=J_{\alpha }{K}_{\alpha \tau \alpha }{J}_{\alpha }{}^{\mathrm{T}}{F}_e\\ D_{\mathrm{L}\beta }=J_{\beta }\left(K_{\beta fz}{J}_z{}^{\mathrm{T}}+K_{\beta \tau \beta }{J}_{\beta }{}^{\mathrm{T}}\right)F_e\\ D_{\mathrm{L}\theta }=J_{\theta }\left(K_{\theta fy}{J}_y{}^{\mathrm{T}}+K_{\theta \tau \theta }{J}_{\theta }{}^{\mathrm{T}}\right)F_e\end{array},$$

(12)

where $K_{xfx}=\mathrm{diag}{(\frac{l_i}{E{A}_i})}_{n\times n}$, $K_{yfy}=K_{zfz}=\mathrm{diag}{(\frac{l_i{}^3}{3E{I}_i})}_{n\times n}$, $K_{y\tau \theta }=-K_{z\tau \beta }=K_{\beta fz}=K_{\theta fy}=\mathrm{diag}{(\frac{l_i{}^2}{2E{I}_i})}_{n\times n}$, $K_{\alpha \tau \alpha }=\mathrm{diag}{(\frac{l_i}{G{I}_i{}_p})}_{n\times n}$, and $K_{\beta \tau \beta }=K_{\theta \tau \theta }=\mathrm{diag}{(\frac{l_i}{E{I}_i})}_{n\times n}$.

When the link deformation is minimal, the total deformation at the end can be expressed as:

$$D_{\mathrm{L}}=D_{\mathrm{L}x}+D_{\mathrm{L}y}+D_{\mathrm{L}z}+D_{\mathrm{L}\theta }+D_{\mathrm{L}\alpha }+D_{\mathrm{L}\beta }=C_{\mathrm{L}}{F}_e,$$

(13)

where $C_{\mathrm{L}}\in {\mathrm{R}}^{6\times 6}$ indicates the flexibility of the links at the end of the space manipulator, and its expression is shown as in Equation (14):

$$\begin{array}{ll}{C}_{\mathrm{L}}=& J_x{K}_{xfx}{J}_x{}^{\mathrm{T}}+J_y\left(K_{yfy}{J}_y{}^{\mathrm{T}}+K_{y\tau \theta }{J}_{\theta }{}^{\mathrm{T}}\right)+J_z\left(K_{zfz}{J}_z{}^{\mathrm{T}}+K_{z\tau \beta }{J}_{\beta }{}^{\mathrm{T}}\right)+J_{\alpha }{K}_{\alpha \tau \alpha }{J}_{\alpha }{}^{\mathrm{T}}+\\ & J_{\beta }\left(K_{\beta fz}{J}_z{}^{\mathrm{T}}+K_{\beta \tau \beta }{J}_{\beta }{}^{\mathrm{T}}\right)+J_{\theta }\left(K_{\theta fy}{J}_y{}^{\mathrm{T}}+K_{\theta \tau \theta }{J}_{\theta }{}^{\mathrm{T}}\right)\end{array}$$

(14)

2.2.3. Control Flexibility

It is defined that the control gain of the $i$-th joint is $k_i$ and $K_{\mathrm{C}}=\mathrm{diag}(k_1,k_2,\cdots ,k_n)$. The error of each joint caused by the joint torque ${\tau }_j$ can be obtained by Equation (15):

$${\tau }_j=K_{\mathrm{C}}\Delta q.$$

(15)

Thus, the pose error of the end of the manipulator generated by external influence $F_e$ is:

$$D_{\mathrm{C}}=J_{\theta }{K}_{\mathrm{C}}{}^{-1}{J}_{\theta }{}^{\mathrm{T}}{F}_e=C_{\mathrm{C}}{F}_e,$$

(16)

where $C_{\mathrm{C}}\in {\mathrm{R}}^{6\times 6}$ represents the control flexibility at the end of the space manipulator.

In summary, the total deformation $D_{\mathrm{A}}$ at the end of the space manipulator is:

$$D_{\mathrm{A}}=D_{\mathrm{J}}+D_{\mathrm{L}}+D_{\mathrm{C}}=C_{\mathrm{A}}{F}_e,$$

(17)

where $C_{\mathrm{A}}=C_{\mathrm{J}}+C_{\mathrm{L}}+C_{\mathrm{C}}$ denotes the flexibility matrix of the end of the space manipulator, revealing the linear relationship between the external force $F_e$ and the endpoint deformation $D_{\mathrm{A}}$. The inverse matrix $C_{\mathrm{A}}{}^{-1}$ of $C_{\mathrm{A}}$ represents the equivalent stiffness matrix at the end of the space manipulator.

The above is the equivalent stiffness model with more comprehensive factors. The analysis and optimization will be carried out in the next section.

3. Stiffness Optimization Method for Peg-in-Hole Assembly

Before optimizing the stiffness of the space manipulator, the corresponding evaluation index must be established.

3.1. Task Direction Flexibility

The aforementioned space manipulator end flexibility matrix $C_{\mathrm{A}}$ is divided into four $3\times 3$ sub-matrices as follows:

$$C_{\mathrm{A}}=\left[\begin{array}{cc}{C}_{ff}& C_{\tau f}\\ C_{f\tau }& C_{\tau \tau }\end{array}\right],$$

(18)

where $C_{ff}$ denotes the position error under the action of unit force, that is, the position flexibility; $C_{\tau \tau }$ represents the attitude error under the unit torque, that is, the attitude flexibility; $C_{\tau f}$ and $C_{f\tau }$ denote the attitude coupling error under the action of a unit force.

The positional flexibility ellipsoid is designed as:

$$\frac{m^2}{{\lambda }_1{}^2}+\frac{n^2}{{\lambda }_2{}^2}+\frac{k^2}{{\lambda }_3{}^2}=1,$$

(19)

where ${\lambda }_1$, ${\lambda }_2$, and ${\lambda }_3$ are the singular values of the matrix $C_{ff}$. $R_{m,n,k}\in {\mathrm{R}}^{3\times 1}$ is defined along the Z-axis of the space manipulator tool coordinate system to satisfy Equation (19) as the task direction flexibility of the peg-in-hole assembly. ${\Vert R_{m,n,k}\Vert }_2$ denotes the end position error under unit force in the task direction. It can be found that the larger the ${\Vert R_{m,n,k}\Vert }_2$, the smaller the contact force generated by the unit error of the end of the space manipulator. Thus, ${\Vert R_{m,n,k}\Vert }_2$ can be optimized to suppress the sudden change in the contact force during the assembly process.

3.2. Optimization

For the peg-in-hole assembly task with determining the target pose, it is necessary to select the appropriate controller gain and configuration in the joint space to optimize the equivalent stiffness of the space manipulator. The optimization problem can be described as follows:

$$f\left(q,K_C\right)={\Vert R_{m,n,k}\Vert }_2\to \underset{}{\max }.$$

(20)

The Particle Swarm Optimization algorithm is used to solve the optimization problem described by Equation (20). For a manipulator with $n$ degrees of freedom, the dimension of the search space is $Dim=2n$. The decision variables of the algorithm can be designed as:

$$\Theta ={\left[q^{\mathrm{T}}\hspace{0.17em}\hspace{0.17em}{k}^{\mathrm{T}}\right]}^{\mathrm{T}}.$$

(21)

where $k={\left[k_1\hspace{0.17em}{k}_2\hspace{0.17em}\cdots k_i\hspace{0.17em}\cdots k_n\right]}^{\mathrm{T}}$.

If $x_i^j$ and $v_i^j$ are used to denote the position and velocity of the $i$-th particle at step $j$, $p_i^j$ and $p_s^j$ are used to denote the individual historical optimal solution and the group historical optimal solution of the $i$-th particle at step $j$, respectively. Then, the velocity and position update formula can be obtained by the following equation:

$$v_i^{\mathrm{temp}}=\omega v_i^j+c_1\lambda \left(p_i^j-x_i^j\right)+c_2\eta \left(p_s^j-x_i^j\right),$$

(22)

where $\omega$ is called the inertia factor, which denotes the degree of dependence of the particle on the initial value, that is, the ability to explore the global solution space; $c_1$ denotes the cognitive factor; $\lambda$ is randomly distributed on the interval $\left[0,1\right]$; $c_2$ denotes the social factor; and $\eta$ is randomly distributed on the interval $\left[0,1\right]$. $c_1$ and $\lambda$ jointly determine the degree of dependence of the particle on the individual historical optimal solution. $c_2$ and $\eta$ jointly determine the degree of dependence of the particles on the optimal solution of the group.

This paper optimizes the configuration in the null space ($J_{\theta }$ has a null space mapping matrix $N\left(J_{\theta }\right)=\mathrm{E}-J_{\theta }{}^{+}{J}_{\theta }$ at $n\ge 7$). It is assumed that the block matrix $v_i^{\mathrm{temp}}={\left[{\dot{q}}_{ite}^{\mathrm{T}}\hspace{0.17em}\hspace{0.17em}{\dot{k}}_{ite}^{\mathrm{T}}\right]}^{\mathrm{T}}$. Equation (23) is used to map ${\dot{q}}_{ite}^{}\hspace{0.17em}$ to the null space of the space manipulator as follows:

$${\dot{q}}_{in}^{}\hspace{0.17em}=N\left(J_{\theta }\right){\dot{q}}_{ite}^{}\hspace{0.17em},$$

(23)

where $\mathrm{E}$ denotes identity matrix of order $n$. Thus, the particle swarm velocity and position update formula is:

$$\{\begin{array}{c}{v}_i^{j+1}={\left[{\dot{q}}_{in}^{\mathrm{T}}\hspace{0.17em}\hspace{0.17em}{\dot{k}}_{ite}^{\mathrm{T}}\right]}^{\mathrm{T}}\\ x_i^{j+1}=x_i^j+\xi v_i^{j+1}\end{array},$$

(24)

where $\xi$ is the constraint factor, which denotes the degree of inheritance of the speed of the current step update by the particles. The above parameters can be adjusted according to the different needs of the specific optimization problem.

During the optimization process, the velocity and position of the particles are continuously updated according to Equation (24). When the speed of each particle is zero and the position does not change or the algorithm reaches the maximum number of iterations, the calculation is terminated. The historical optimal solution of the group at this time is the final value of the decision vector. The decision vector is equally divided into two groups to obtain the optimized configuration and controller gain.

4. Sliding Mode Impedance Control Based on Disturbance Compensation

4.1. Sliding Mode Impedance Control

The impedance model is designed as follows:

$$M_m{\ddot{X}}_d+B_m{\dot{X}}_d+K_m{X}_d+F_d=M_m{\ddot{X}}_e+B_m{\dot{X}}_e+K_m{X}_e+F_e,$$

(25)

where $X_e$, ${\dot{X}}_e$, and ${\ddot{X}}_e$ denote the actual pose, velocity, and acceleration of the space manipulator in the working space, respectively; $X_d$, ${\dot{X}}_d$, and ${\ddot{X}}_d$ denote the desired trajectory, desired speed, and acceleration of the space manipulator in the workspace, respectively; $M_m$, $B_m$, and $K_m$ are the mass, damping, and stiffness coefficients, respectively; and $K_m=C_{\mathrm{A}}{}^{-1}$. The impedance control law can be designed according to Equation (25):

$$F_f=M_m{\ddot{e}}_x+B_m{\dot{e}}_x\hspace{0.17em}+K_m{e}_x+k_{pf}(F_d-F_e),$$

(26)

where $e_x=X_e-X_d$.

The sliding mode function is designed as follows:

$$\{\begin{array}{l}{\dot{X}}_{\mathrm{R}}={\dot{X}}_d+\Lambda e_x\\ s={\dot{X}}_r-{\dot{X}}_e={\dot{e}}_x+\Lambda e_x\end{array},$$

(27)

where $\Lambda$ is a positive definite diagonal matrix. Equation (26) is introduced into the sliding mode control mode, and the sliding mode control mode is designed considering Equation (3) as:

$$F_r=M_x{\ddot{X}}_r+C_x{\dot{X}}_r+Ks+\eta \tanh \frac{s}{\epsilon }+F_f,$$

(28)

where $K>0$ and $\epsilon >0$.

4.2. Disturbance Compensation

If the disturbance $d_e$ caused by the sudden change of contact force is regarded as an input of a control system, then the robust control problem can be transformed into a two-person zero-sum optimal control problem. $\Phi$ is assumed as:

$$\Phi \left(s,u,d_e\right)=s^T\mathrm{A}s+u^T\mathrm{B}u-{\gamma }^2{d}_e^T{d}_e.$$

(29)

where $\mathrm{A}$ and $\mathrm{B}$ are symmetric positive definite constant matrices, and $\gamma$ is a known normal number. The continuously differentiable performance index is defined as:

$$L\left(s,u,d_e\right)={\displaystyle {\int }_0^{\infty }\Phi \left(s,u,d_e\right)d\tau }.$$

(30)

Based on performance indicators, the local Hamiltonian equation is defined as:

$$H\left(s,u,d_e,\nabla L\right)=\Phi \left(s,u,d_e\right)+\nabla L^{\mathrm{T}}\dot{s},$$

(31)

where $\nabla L=\left(\partial L\left(s,u,d_e\right)/\partial s\right)$.

The goal of the zero-sum game is to find a set of optimal control sequences, $u^{*}$ and $d_e{}^{*}$, that satisfy the relationship of Equation (32):

$$L\left(s,u^{*},d_e\right)\le L^{*}\left(s,u^{*},d_e{}^{*}\right)\le L\left(s,u,d_e{}^{*}\right).$$

(32)

When $u$ and $d_e$ execute their respective optimal control sequences, it can be considered that the saddle point of the two-person zero-sum game problem is obtained. At this time, the local optimal performance index is:

$$L^{*}\left(s,u^{*},d_e{}^{*}\right)=\underset{u}{\min }\underset{d_e}{\max }{\displaystyle {\int }_0^{\infty }\Phi \left(s,u,d_e\right)d\tau }=\underset{d_e}{\max }\underset{u}{\min }{\displaystyle {\int }_0^{\infty }\Phi \left(s,u,d_e\right)d\tau }.$$

(33)

According to Equations (31) and (33), the HJI equation can be obtained as:

$$\Phi \left(s,u^{*},d_e{}^{*}\right)+\nabla L^{*}{}^{\mathrm{T}}\dot{s}=0.$$

(34)

The solution $u^{*}$ and $d_e{}^{*}$, satisfying Equation (34), should have the following form [31]:

$$u^{*}=-\frac{1}{2}{\mathrm{B}}^{-1}\nabla L^{*},\hspace{0.17em}\hspace{0.17em}{d}_e{}^{*}=\frac{1}{2{\gamma }^2}\nabla L^{*}.$$

(35)

The BP neural network is used to approximate the performance index, and the hyperbolic tangent function is selected as the activation function. The ideal neural network can be expressed as:

$$L\left(s,u,d_e\right)=w_c{}^{\mathrm{T}}\tanh \left(s,u,d_e\right)+{\epsilon }_c,$$

(36)

where $w_c$ denotes the unknown neural network weight, and ${\epsilon }_c$ denotes the neural network approximation error. Thus, the gradient of the performance index can be expressed as:

$$\nabla L\left(s,u,d_e\right)=w_c{}^{\mathrm{T}}\nabla \tanh \left(s,u,d_e\right)+\nabla {\epsilon }_c.$$

(37)

Equation (37) is substituted into Equation (31), which is then:

$$H\left(s,u,d_e,\nabla L\right)=\Phi +{\dot{s}}^{\mathrm{T}}{w}_c{}^{\mathrm{T}}\nabla \tanh \left(s,u,d_e\right)-e_H,$$

(38)

where $e_H$ is the approximation error. Thus, the output of the actual neural network is:

$$\widehat{L}\left(s,u,d_e\right)={\widehat{w}}_c^{\mathrm{T}}\tanh (s,u,d_e),$$

(39)

where ${\widehat{w}}_c^{}$ is the estimated value of the weight $w_c$. Thus, the approximation to the Hamiltonian equation is:

$$\widehat{H}\left(s,u,d_e,\nabla \widehat{L}\right)=\Phi +{\dot{s}}^{\mathrm{T}}{\widehat{w}}_c{}^{\mathrm{T}}\nabla \tanh \left(s,u,d_e\right).$$

(40)

The residual function is created according to the steepest gradient descent method:

$$E_c=\frac{1}{2}{\left(\widehat{H}-H\right)}^2.$$

(41)

Thus, the renewal law of ${\widehat{w}}_c$ can be expressed as:

$${\widehat{w}}_c={\widehat{w}}_c-{\alpha }_c\frac{\partial E_c}{\partial {\widehat{w}}_c},$$

(42)

where ${\alpha }_c$ denotes the learning rate.

Thus, $u^{*}$ can be obtained by combining Equations (35) and (39):

$$u^{*}=-\frac{1}{2}{\mathrm{B}}^{-1}{\widehat{w}}_c^{\mathrm{T}}\nabla \tanh \left(s,u,d_e\right).$$

(43)

The sliding mode control law, by substituting Equation (43) into Equation (28), can be rewritten as:

$$F_r=M_x{\ddot{X}}_r+C_x{\dot{X}}_r+Ks+\eta \tanh \frac{s}{\epsilon }\hspace{0.17em}+F_f-\frac{1}{2}{\mathrm{B}}^{-1}{\widehat{w}}_c^{\mathrm{T}}\nabla \tanh \left(s,u,d_e\right).$$

(44)

5. Test and Analysis

To validate the control method proposed in this paper, three comparative stimulations on the space manipulator are designed. The first experiment used the existing control method [29] with disturbance observers but no optimized configuration and control parameters. The second experiment used the existing impedance control method with optimization of configuration and control parameters. The third experiment carries out the control algorithm proposed in this paper based on stiffness optimization and disturbance compensation.

5.1. Subject

The subject of the three experiments is a 7-DOF space manipulator. Figure 4 shows the space manipulator and its Denavit–Hartenberg (DH) model with a coordinate system.

Table 1. Kinematic parameters of the space manipulator.

i	${\mathit{\alpha }}_{\mathit{i}}$ (°)	${\mathit{\alpha }}_{\mathit{i}}$ (mm)	${\mathit{\theta }}_{\mathit{i}}$ (°)	${\mathit{d}}_{\mathit{i}}$ (mm)
1	−90	0	0	166.8
2	90	0	−90	183.1
3	0	544.9	0	−52
4	0	500.9	90	0
5	90	0	−90	0
6	−90	0	−90	−121.1
7	0	0	0	0

shows the kinematic parameters of the space manipulator. The mass parameters are shown in

Table 2. Inertial parameters of the space manipulator.

Link	Center of Mass X (m)	Center of Mass Y (m)	Center of Mass Z (m)	Mass (kg)
Base	0	0	0.032161	1.82
Link1	0	−0.00754	0.00864	8.5
Link2	0	0.00754	0.00864	8.5
Link3	−0.45495	0	0.24168	10.8
Link4	−0.5013	0.00475	0.32914	6.3
Link5	0	0.11454	0.00521	2.13
Link6	−0.00451	−0.12413	0	2.3
Link7	0	−0.00354	−0.00771	1.9

, and the inertia tensor can be obtained from Table 2. The connecting rod is made of aluminum. The diameters of link 3 and link 4 are 96 mm and 70 mm, respectively, and the wall thickness is 5 mm.

The base coordinate system of the space manipulator coincides with the world coordinate system, and its initial configuration is $q_{ini1}={\left[0^{°}\hspace{0.17em}{-90}^{°}\hspace{0.17em}{0}^{°}\hspace{0.17em}{90}^{°}\hspace{0.17em}{-90}^{°}\hspace{0.17em}{-90}^{°}\hspace{0.17em}{0}^{°}\right]}^{\mathrm{T}}$. The end tool clamps a peg with a mass of 5 kg to move from $x_{\mathrm{ini}}=\left[0.0520 \mathrm{m}\hspace{0.17em}0.8051 \mathrm{m}\hspace{0.17em}0.6197 \mathrm{m}\hspace{0.17em}{180}^{\circ }\hspace{0.17em}{0}^{\circ }\hspace{0.17em}{0}^{\circ }\right]$ to $x_{\mathrm{dst}}=\left[0.0520 \mathrm{m}\hspace{0.17em}0.8051 \mathrm{m}\hspace{0.17em}0.4697 \mathrm{m}\hspace{0.17em}{180}^{\circ }\hspace{0.17em}{0}^{\circ }\hspace{0.17em}{0}^{\circ }\right]$. Restricted by the accuracy of the calibration of the vision sensor and the algorithm, there is an error of $\pm 5 \mathrm{mm}$ in $x_{\mathrm{dst}}$. The motion trajectory of the space manipulator adopts the trapezoidal speed planning method with arc transition, the total planning time is $T=10 \mathrm{s}$, and the planning step length is $\Delta T=0.01 \mathrm{s}$. The control frequency is 100 Hz, and the measurement frequency is 1000 Hz.

5.2. Experiments

At the beginning of the experiments, the space manipulator clamped the peg to move linearly along the Z direction of the coordinate system. After the shaft is in contact with the end face of the hole, it enters the hole searching stage and moves along the vortex line, as shown in Figure 5. The contact force between the shaft and the end face of the hole is maintained at $F_d=\left[\begin{array}{cccccc}0& 0& 30 \mathrm{N}& 0& 0& 0\end{array}\right]$. After finding the hole, the space manipulator moves in a straight line through the hole. The contact forces mode is obtained according to the Hertz theory, and the simulation details of the environment are shown in the literature [32]. An error of 5% is added to the dynamics parameters to simulate its uncertainties. Matlab and its Robotics ToolboxCorke (Release 10) are used to simulate and verify the proposed method.

In experiment 1, the existing control method [29] was used, and the configuration and control parameters were not optimized. The simulation results are shown in Figure 6 and Figure 7. It can be seen that the hole searching phase ends at about the 4th second, and the peg loses contact with the hole and then collides. At this time, the contact force becomes 0 N and then abruptly reaches the peak value of 114 N. At the moment of collision, the peak disturbance force of the base comes 1086 N. In a practice project, such a large contact force can easily exceed the 100 N range of the sensor, which will affect the accuracy of the data and even damage the sensor. The tremendous disturbing force of the base will also cause massive interference to the attitude and orbit control of the base spacecraft.

In experiment 2, the existing impedance control method was used, and the configuration and control parameters were optimized. The number of the particle population is 50, the inertia factor is $\omega =0.8$, $c_1=c_2=0.5$, and the maximum iteration is 100 times. The convergence process and optimization results are shown in Figure 8. Before optimization, the joint controller gain is $K_C=5000{\mathrm{E}}_7$ (${\mathrm{E}}_7$ is 7 identity matrix) and the controller gain after optimization is $K_C=\mathrm{diag}(3080,4253,1000,1000,1000,1000,4696)$. The optimized configuration is $q_{ini2}={\left[-{4.5}^{°}\hspace{0.17em}-{96.8}^{°}\hspace{0.17em}{0.4}^{°}\hspace{0.17em}{90.3}^{°}\hspace{0.17em}-{90.7}^{°}\hspace{0.17em}-{96.8}^{°}\hspace{0.17em}{4.5}^{°}\right]}^{\mathrm{T}}$. The simulation results are shown in Figure 9 and Figure 10. It can be seen that due to the optimization of the manipulator’s configuration and controller gain, the peak contact force at the moment of contact collision drops to 77 N, which is 47% lower than that in Experiment 1. The peak disturbance force of the base is 534 N, and the decrease is 51%.

In experiment 3, the control algorithm designed in this paper is used based on stiffness optimization and disturbance compensation. The BP neural network structure is set to 3-6-1, and the initial weight is randomly assigned within the range of $\left[-10,10\right]$. $M_m$, $B_m$, $\mathrm{A}$, and $\mathrm{B}$ are all unit arrays, and $\gamma =1.1$. The neural network is trained up to 1000 times. The initial learning rate is ${\alpha }_c=0.3$, and each training decrements by 0.001, and ${\alpha }_c\ge 0.005$, $k_{pf}=0.35$, $K=2$, $\eta =1.2$, $\Lambda =0.1$, $\epsilon =0.5$. The simulation results are shown in Figure 11 and Figure 12. It can be seen that due to the compensation based on the zero-sum optimal control algorithm, the peak value of the contact force is 38N, which is a 90% drop compared to experiment 1.

A comparison between the experiments without and with the control algorithm proposed in this paper is shown in

Table 3. The comparison between the experiments.

	Experiment 1	Experiment 2	Experiment 3
Sudden change in contact force	114 N	77 N	38 N
lower than experiment 1	/	47%	90%
Peak disturbance force of the base	1086 N	534 N	500 N
lower than experiment 1	/	51%	54%

. It can be seen that the peg-in-hole assembly control method of the space manipulator proposed in this paper can effectively reduce the sudden change in contact force caused by collision and can suppress the disturbing force of the base by comparing the results of three experiments. The disturbance caused by the change of contact status during the assembly process is reduced to meet the control requirements of high precision and high robustness for peg-in-hole assembly tasks performed by a space manipulator.

6. Conclusions

Based on the structure and control characteristics of the space manipulator, a space manipulator peg-in-hole assembly control method considering the optimization of equivalent stiffness is proposed in this paper. Compared with the existing compliance control method, the contributions of this paper mainly include the following points: (1) the method proposed in this paper designs the sliding mode impedance control law based on establishing and optimizing the equivalent stiffness; (2) this paper designs the optimal compensation term for the contact disturbance; (3) the methods proposed in this paper effectively reduce the sudden change in contact force caused by collision and can suppress the disturbing force of the base while controlling the contact force at the end of the space manipulator, thereby improving the accuracy and robustness of the peg-in-hole assembly control. In the later period, the dynamic coupling characteristics of flexible base and manipulator will be further taken into consideration for the multi-arm space robot performing the on-orbit assembly task of large trusses, and the coordinated control method of peg-in-hole assembly under the coupling vibration of multi-arm space robot will be studied.