Improved Uncalibrated Visual Servo Strategy for Hyper-Redundant Manipulators in On-Orbit Automatic Assembly

Featured Application

An uncalibrated visual servo strategy for the on-orbit automatic assembly of an ultra-high-caliber optical system. It is mainly employed to realize the static positioning and dynamic tracking of the desired image features in a harsh space environment when the desired image features required for assembly are known.

Abstract

Aiming at on-orbit automatic assembly, an improved uncalibrated visual servo strategy for hyper-redundant manipulators based on projective homography is proposed. This strategy uses an improved homography-based task function with lower dimensions while maintaining its robustness to image defects and noise. This not only improves the real-time performance but also makes the joint space of hyper-redundant manipulator redundant to the homography-based task function. Considering that the kinematic parameters of the manipulator are easily changed in a space environment, the total Jacobian between the task function and manipulator joints is estimated online and used to construct a controller to directly control the manipulator joints without a kinematics model. When designing the controller, the above-mentioned redundancy is exploited to solve the problem of the over-limiting of the joint angles of the manipulator. The KF-SVSF method, which combines the optimality of the Kalman filter (KF) and the robustness of the smooth variable structure filter (SVSF), is introduced to the field of uncalibrated visual servos for the first time to perform the online estimation of the total Jacobian. In addition, the singular value filtering (SVF) method is adopted for the first time to avoid the interference caused by the unstable condition number of the estimated total Jacobian. Finally, simulations and experiments verify the superiority of this strategy in terms of its real-time performance and precision.

1. Introduction

Operating an ultra-high-caliber optical system in high space orbit is one of the key goals of the Chinese space industry and will help scientists to make more detailed observations of the universe [1]. However, it is impossible to transport it to space as a whole or install such a large space system manually in space. Large space structure construction through on-orbit services (OOS) may be the best solution [2,3,4,5,6,7,8,9,10,11,12,13]. OOS missions also include visual inspection, fuel refueling and satellite repair and are of increasing interest to the space industry because of their high economical potential and strategic benefits. There are many interesting challenges in the development and operation of space robots for OOS missions, such as motion estimation and the prediction of target objects, multi-physics modeling and the simulation of space manipulators, practical dynamic parameter identification, dexterous and efficient manipulation, time delay and sensor errors in feedback controls, compliance and intelligent controls, multi-arm coordinated controls and operations, among others [5,6,7]. A visual servo is also an important technology for manipulators for OOS missions [8,9,10,11,12,13]. It uses the image information captured by a camera to position or track space manipulators. This paper mainly considers the uncalibrated visual servo strategy for a hyper-redundant manipulator with a hand–eye system to realize the alignment of both static and dynamic assembly objects when the desired image features are known. Joint flexibility is a problem in space manipulators that may cause vibration. Joint flexibility is ignored in the research presented in this paper; the details of the joint flexibility problem can be found in [14].

The traditional visual servo relies on the accurate calibration of the internal camera parameters and the hand–eye relationship and determines the pose of the manipulator based on the current image information and calibrated system parameters [13,15,16,17,18]. Various parameters of a robotic vision system are likely to change in the complex and harsh space environment, which will have a serious impact on the accuracy of the visual servo. Moreover, frequent system recalibration will greatly increase the cost and workload, and its feasibility is quite low [19,20,21,22]. For these reasons, research into uncalibrated visual servos is very significant.

The term “uncalibrated” mainly means that the internal and external parameters of a camera are unknown. Compared with the position-based visual servo and hybrid visual servo, which both require 3-D reconstruction and are highly sensitive to calibration error, an image-based visual servo is more applicable to this uncalibrated situation [21,22]. An image-based uncalibrated visual servo (IBUVS) uses image information such as the pixel coordinates of target feature points to construct task functions directly and design a controller based on the image Jacobian, which is estimated online in each control period [22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38].

In actual application, some image features may be missing owing to motion blur, inconspicuous background contrast and the occlusion of the visual field, causing the visual servo task to fail. In addition, image transmission noise and the resulting image processing errors also impact the accuracy of visual servos. To solve these problems, researchers have proposed an uncalibrated visual servo based on projective homography (PHUVS) [23]. This method solves the homography matrix by matching as many feature points as possible and uses the elements of the homography matrix to construct the task function. The homography matrix can be solved as long as there are no less than four pairs of matched feature points. Therefore, the visual servo will not be interrupted even if some feature points are lost. In addition, the PHUVS has a certain compensatory effect on image noise. As the number of matched feature points increases, this compensation becomes more obvious and the system shows better robustness to image noise [39]. It is worth mentioning that the dimensions of the homography matrix and task function remain constant regardless of the number of matching feature points, so the real-time performance of system is not affected. However, the original homography-based task function fails to take full advantage of the eight degrees of freedom of the homography matrix. In addition to further improving the real-time performance, this property is of great significance to the application of PHUVS in hyper-redundant manipulators.

The online estimation of the image Jacobian is another pivotal point of an uncalibrated visual servo. Classic image Jacobian estimation methods include the dynamic Broyden–Gauss–Newton method [37], Broyden–RLS method [29], group-based Broyden method [38], Kalman filter (KF) method [23,26,27,36], and particle filter [33,38]. Hao [22] showed that the KF method performs better than other methods when tracking targets with changing directions of motion. Music [29] also compared these methods and showed that the particle filter is slightly more accurate when the noise level is high, but the Kalman filter has better real-time and dynamic performance for mechanical systems. A limitation of Kalman filtering is that its accuracy may decrease or even diverge when there is modeling uncertainty and the statistical characteristics of noise are unknown [31,32]. To solve this problem, researchers have proposed several methods such as the fuzzy adaptive Kalman filter [31,32], Sage–Husa adaptive Kalman filter and Kalman neural network [34], but the performances of these methods are not always stable. A recently proposed variational Bayesian-based adaptive Kalman filter can improve accuracy to a certain extent, but its process is so cumbersome that it takes more than 20 times as much calculation time as the Kalman filter [40].

Considering the complexity of on-orbit maintenance tasks and the need for flexibility, a redundant manipulator with more degrees of freedom is a reasonable choice [41,42]. Redundancy is often used in space manipulators; for example, to minimize the reactions transferred from the manipulator to the spacecraft [43] or to minimize the kinetic energy and joint torques [44]. Therefore, a hyper-redundant manipulator with nine degrees of freedom has been designed to implement on-orbit maintenance tasks for large space facilities. However, its kinematic parameters are affected by factors such as thermal expansion and contraction, corrosion, etc., and often change. It is almost impossible to recalibrate a hyper-redundant manipulator with a complex structure when its parameters change due to working in the harsh space environment for a long time. Meanwhile, a controller that is constructed with the estimated total Jacobian and directly controlled manipulator joints is more accurate and robust to the changes of manipulator parameters. In addition, the estimated total Jacobian of the homography-based task function tends to present an unstable conditional number. This results in a sudden increase of the manipulator velocity in some directions, which causes the manipulator to shake and results in serious errors [45].

On the basis of the above description, an improved uncalibrated visual servo strategy for hyper-redundant manipulators in on-orbit automatic assembly is proposed. In this strategy, an improved task function with robustness to image noise and better real-time performance is constructed; another advantage of this is that the joint space of hyper-redundant manipulators has redundancy. To improve the system accuracy and make the system robust to the variable parameters of manipulator in space, the estimated total Jacobian, which maps the task function and manipulator joints, is used to construct a controller to control the manipulator joints directly without a kinematics model. When designing the controller, the above-mentioned redundancy is exploited to prevent damage to the mechanical structure caused by the over-limiting of the joint angles of the manipulator. Meanwhile, considering that the visual servo system uses an approximate linear model and the measurement noise of the task function is difficult to determine, the KF-smooth variable structure filter (SVSF) method for state space estimation is introduced into the visual servo to perform the online estimation of the total Jacobian. This method combines the KF method and SVSF method and shows better robustness to noise and modeling uncertainty [46,47,48]. In addition, the singular value filtering (SVF) method [45] is adopted to avoid the disturbance caused by the unstable conditional number of the estimated total Jacobian. Finally, this strategy is verified by several simulations and experiments.

2. Projective Homography

The current frame $F$ and the desired frame $F^{\ast }$ of an eye-in-hand camera are defined for problems with an eye-in-hand system in which the camera moves with the manipulator. Figure 1 shows the geometric relationship between these two camera frames. Suppose $M_i={\left[X_i,Y_i,Z_i\right]}^{\prime }$ is the coordinate of the feature point $P_i$ in $F$ and $M_i^{\ast }={\left[X_i^{\ast },Y_i^{\ast },Z_i^{\ast }\right]}^{\prime }$ is the coordinate described by the desired camera, while $p_i={\left[\begin{array}{ccc}{u}_i& v_i& 1\end{array}\right]}^{\prime }$ and $p_i^{\ast }={\left[\begin{array}{ccc}{u}_i^{\ast }& v_i^{\ast }& 1\end{array}\right]}^{\prime }$ are the homogeneous pixel coordinates of feature point $P_i$ captured by the current and desired cameras, respectively. The geometric relationship between $M_i$ and $M_i^{\ast }$ can be expressed as

$$M_i=R{M}_i^{\ast }+t$$

(1)

where $R\in {\mathbb{R}}^{3\times 3}$ and $t\in {\mathbb{R}}^{3\times 1}$ are the orientation matrix and displacement vector between $F$ and $F^{\ast }$ expressed in $F^{\ast }$, respectively.

Suppose each target point $P_i$ is on the plane $\pi$, $d^{\ast }$ is the distance between the desired projection center and plane $\pi$ and $n^{\ast }$ is the normal vector of $\pi$ expressed in the desired camera frame $F^{\ast }$ with length $\Vert n^{\ast }\Vert =1/d^{\ast }$. Based on Equation (1), we can obtain

$$M_i=\left(R+t{n}^{\ast T}\right)M_i^{\ast }$$

(2)

$$G=R+t{n}^{\ast T}$$

(3)

where $G$ is the Euclidean homography matrix. In normalized coordinates, (2) can be expressed as

$$m_i=\frac{Z_i^{\ast }}{Z_i}\left(R+t{n}^{\ast T}\right)m_i^{\ast }$$

(4)

where $m_i={\left[X_i/Z_i,{Y_i/Z}_i,1\right]}^{\prime }$ and $m_i^{\ast }={[X_i^{\ast }/Z_i^{\ast },Y_i^{\ast }/Z_i^{\ast },1]}^{\prime }$. The depth scale is denoted as

$${\alpha }_i=\frac{Z_i^{\ast }}{Z_i}.$$

(5)

The following relationship can be obtained from Equations (2), (3) and (5):

$$p_i={\alpha }_i\left(KG{K}^{-1}\right)p_i^{\ast }$$

(6)

where $K$ is the internal parameter matrix of the eye-in-hand camera [36]. The homography matrix can be obtained as

$$H=KG{K}^{-1}$$

(7)

which refers to the mapping between different images projected from the same object to two different image planes. It has been applied in many domains such as motion estimation, 3-D reconstruction [49] and of course the visual servo. Even in an uncalibrated situation, this matrix can be estimated up to a scale $\beta$ with more than four corresponding image features in different images via

$$\hat{H}=\beta H$$

(8)

where $\beta$ is independent of the depth ratio ${\alpha }_i$ in (5).

Because the images are always disturbed by noise during transmission and processing, the exact solution of $H$ cannot be obtained via the noisy measurement image point $p_i$. However, it is possible to obtain the optimal estimated homography matrix $\hat{H}$ in the sense of least squares. The estimated image point of $P_i$ in the current image can be obtained via ${\hat{p}}_i=\hat{H}{p}_i^{\ast }$. The root-mean-squared (RMS) error between ${\hat{p}}_i$ and the true points can be obtained via $\hat{\epsilon }\approx \epsilon {\left(4/n\right)}^{1/2}$ [38], where $n$ is the number of image features and $\epsilon$ is the RMS error between the measurement point $p_i$ and the true point. It is worth noting that the estimated homography matrix $\hat{H}$ can be equivalently considered as being estimated by the matching points ${\hat{p}}_i\leftrightarrow p_i^{\ast }$. Obviously, if $n>4$, building the task function with the elements of the homography matrix will have a certain compensation effect for image noise.

3. Improved Strategy for the Uncalibrated Visual Servo of a Hyper-Redundant Space Manipulator

In this section, our improved strategy as shown in Figure 2c will be described in detail. The first part details the new task function and provides related proofs. Then, the more robust Kalman filter–smooth variable structure filter (KF-SVSF) method will be introduced to estimate the total Jacobian. Finally, the estimated total Jacobian, processed via SVF to reduce the disturbance caused by an unstable conditional number, is used to build a controller to control the manipulator joints directly for both static positioning and dynamic tracking while limiting the joint angle.

3.1. Improved Homography-Based Task Function

In the original PHUVS strategy proposed by Zeyu Gong [23], the homography-based task function is defined as

$$\begin{array}{l}\hat{h}={\left[\begin{array}{ccc}{\hat{H}}_1& {\hat{H}}_2& {\hat{H}}_3\end{array}\right]}^T\\ ={\left[\begin{array}{cccc}{\hat{h}}_1& {\hat{h}}_2& \cdots & {\hat{h}}_9\end{array}\right]}_{9\times 1}^T\end{array}$$

(9)

where ${\hat{H}}_i$ is the $i\mathrm{th}$ row of the estimated homography matrix. The error vector of the system is denoted by

$$e=h_0-\hat{h}$$

(10)

where $h_0$ is constructed by stacking the rows of the identity matrix $I_{3\times 3}$. The purpose of our strategy can be considered to be making the current camera frame $F$ coincide with $F^{\ast }$. To guarantee this, the constraint $\det (\hat{H})=1$ is defined to obtain the unique projective homography matrix $\hat{H}$ between the current and desired feature points in each iteration. It can be proved that $e=0$ if and only if rotation matrix $R=I_{3\times 3}$ and position vector $t=0$ [23].

In our strategy, the constraint ${\hat{h}}_9=1$ is defined, where $h_9$ is the last element of the homography matrix and a direct linear transformation (DLT) is introduced to estimate the homography matrix. First, it is necessary to normalize the two images, respectively, as follows:

• Translate the feature points as ${\tilde{p}}_i=T_1{p}_i$ and ${\tilde{p}}_i^{\ast }=T_1^{\ast }{p}_i^{\ast }$ to ensure that the centroid of these feature points is at the origin;
• Scale feature points as ${\overline{p}}_i=T_2{\tilde{p}}_i$ and ${\overline{p}}_i^{\ast }=T_2^{\ast }{\tilde{p}}_i^{\ast }$ to make their average distance from the origin equal to $\sqrt{2}$.

On the basis of the properties of homogeneous coordinates, homogeneous simultaneous equations can be constructed with transformed pixel coordinates as

$$\underset{A_{2n\times 9}X=0}{\underbrace{\left(\begin{array}{ccc}⋮& ⋮& ⋮\\ {\overline{p}}_i^{\ast T}& 0^T& {\overline{u}}_i{\overline{p}}_i^{\ast }\\ 0^T& {\overline{p}}_i^{\ast T}& {\overline{v}}_i{\overline{p}}_i^{\ast }\\ ⋮& ⋮& ⋮\end{array}\right)\left(\begin{array}{c}{\overline{H}}_1^T\\ {\overline{H}}_2^T\\ {\overline{H}}_3^T\end{array}\right)}}=0$$

(11)

where $A$ is the coefficient matrix and ${\overline{H}}_i$ is the ith row of $\overline{H}$. The purpose of the above normalization operation is to prevent the coefficient matrix $A$ from becoming ill-conditioned owing to image noise. This solves the problem of numerical instability.

Then, the least squares solution of equation (11) can be calculated via singular value decomposition. Then, we conduct an inverse transform on $\overline{H}$ as

$$\tilde{H}={\left(T_2{T}_1\right)}^{-1}\overline{h}\left(T_2^{\ast }{T}_1^{\ast }\right)$$

(12)

where $\tilde{H}$ is the estimated homography between the current and desired feature points. The homography $\tilde{H}$ can be scaled at any scale.

Thus, $\tilde{H}$ is scaled as $\hat{H}=\tilde{H}/{\tilde{h}}_9$ and the new task function is constructed as

$$\hat{h}={\left[\begin{array}{cccc}{\hat{h}}_1& {\hat{h}}_2& \cdots & {\hat{h}}_8\end{array}\right]}^T{}_{8\times 1}$$

(13)

where $\hat{h}$ reflects the eight degrees of freedom of the homography matrix and further reduces the dimensions of the system. The error vector of system is defined as

$$\begin{array}{c}e={\left[\begin{array}{cccccccc}1& 0& 0& 0& 1& 0& 0& 0\end{array}\right]}^T\\ -{\left[\begin{array}{cccc}{\hat{h}}_1& {\hat{h}}_2& \cdots & {\hat{h}}_8\end{array}\right]}^T\end{array}$$

(14)

where $e=0$ if and only if the rotation matrix $R=0$ and the position vector $t=0$. Its sufficiency and necessity can be proved as follows.

• Necessity proof: If $R=I$ and $t=0$, then $G=I$. On the basis of the similarity of $H$ and $G$, we obtain $H=I$. From Equation (8), we obtain $\hat{H}=H/\beta =I/\beta$. The constraint $h_9=1$ gives $1/\beta =h_9=1$; i.e., $\beta =1$, $\hat{H}=I$, and $e=0$.
• Sufficiency: It is obvious that $\hat{H}=I$ and $H=\beta I$ if $e=0$. From Equation (7), we have

  $$G=K^{-1}HK=\beta I.$$

  (15)

Taking two feature points, the coordinates of these points in $F$ and $F^{\ast }$ can be described as

$$M_i=\beta M_i^{\ast },M_j=\beta M_j^{\ast }.$$

(16)

Obviously, regardless of in which frame this is expressed, the distance between $P_i$ and $P_j$ is constant. That is,

$$\Vert M_i-M_j\Vert =\Vert M_i^{\ast }-M_j^{\ast }\Vert =\beta \Vert M_i^{\ast }-M_j^{\ast }\Vert .$$

(17)

This means that $\beta =1$, which implies $H=I$ and $G=I$. Thus, $R=I$ and $t=0$.

This new task function takes full advantage of the property that the homography matrix has eight degrees of freedom. This not only reduces the dimensions of the task function but also simplifies the state space of the visual servo system that needs to be estimated online, so the real-time performance of system is further improved. Meanwhile, this causes the total Jacobian $J_t\in {\mathbb{R}}^{8\times 9}$, which maps the task function and joint space of the hyper-redundant manipulator, to become redundant. Therefore, this redundancy can be used to finish some secondary tasks such as limiting the joint angle without a manipulator kinematic model during the servo process. Obviously, this improves the robustness of system to changes in the kinematic parameters of manipulators.

Remark 1.

A flaw in the inhomogeneous solution of the homography matrix is that if the true value of$h_9$is 0 or very close to 0, the estimated homography matrix becomes very unstable. In fact, this situation rarely occurs in the visual servo. In static positioning tasks, the task function generally changes near to the desired gradient direction. As long as${\hat{h}}_9$is not equal to 0 at the initial position, its value will never become 0 during the servo process. In tracking tasks, the task function is always close to the desired value.

3.2. Online Estimation of Total Jacobian

The kinematic parameters of the space manipulator change easily in the harsh space environment. To avoid the repeated calibration of the complex hyper-redundant manipulator, the total Jacobian between the task function and the joints of the manipulator is used to construct the controller. The expression of the total Jacobian is

$$J_t={\left[\begin{array}{ccc}\mathsf{\partial }{\hat{h}}_1/\mathsf{\partial }{q}_1& \cdots & \mathsf{\partial }{\hat{h}}_1/\mathsf{\partial }{q}_9\\ ⋮& \ddots & ⋮\\ \mathsf{\partial }{\hat{h}}_8/\mathsf{\partial }{q}_1& \cdots & \mathsf{\partial }{\hat{h}}_8/\mathsf{\partial }{q}_9\end{array}\right]}_{8\times 9}$$

(18)

Therefore, the accuracy of the estimation of the total Jacobian is directly related to the system performance.

The uncalibrated visual servo system of the nine degree-of-freedom (9 DOF) manipulator is modeled as

$$X_{K+1}=X_K$$

(19)

$$Z_{K+1}=C_{K+1}{X}_{K+1}$$

(20)

where $X_K={\left[\begin{array}{cccc}{\left(J_{t1}\right)}_K& {\left(J_{t2}\right)}_K& \cdots & {\left(J_{t8}\right)}_K\end{array}\right]}^T$ and $J_{ti}$ is the $i\mathrm{th}$ row of the estimated total Jacobian $J_t$. $C_K=diag{\left(\begin{array}{ccc}{\dot{q}}_K& \cdots & {\dot{q}}_K\end{array}\right)}_{8\times 72}$ is constructed by the joint velocity of the manipulator, and $Z_K$ can be considered as the change rate of task function $\hat{h}$.

It is worth noting that this model is a linear approximation of the visual servo system, and it causes modeling uncertainty. Meanwhile, because the homography-based task function is used to compensate for the image noise, the statistical characteristics of the measurement noise of the task function become difficult to obtain as well. Therefore, the more robust KF-SVSF method [46,47,48] is introduced into the visual servo to improve the system accuracy.

This method combines the optimality of the KF method with the robustness of the SVSF method. As shown in Figure 3, it introduces the concept of an optimal smoothing variable boundary layer, which is an estimation of the impact of modeling uncertainty and noise. The KF-SVSF method seeks a balance between accuracy and robustness by introducing a saturation limit into the calculated optimal boundary layer. By switching between KF gain and SVSF gain, the optimality of KF is maintained within the limit, while the robustness and stability of SVSF remain outside the limited range.

First, the smooth variable boundary layer can be obtained as follows:

• Calculate the a priori state space:

  $${\hat{X}}_{K+1|K}={\hat{X}}_{K|K}$$

  (21)
• Calculate the a priori state error covariance matrix:

  $$P_{K+1|K}=P_{K|K}+Q_K$$

  (22)
• Calculate the residuals vector:

  $${\hat{Z}}_{K|K}=C{\hat{X}}_{K|K}$$

  (23)

  $${\hat{Z}}_{K+1|K}=C{\hat{X}}_{K+1|K}$$

  (24)

  $$e_{Z,K+1|K}=Z_{K+1}-{\hat{Z}}_{K+1|K}$$

  (25)

  $$e_{Z,K|K}=Z_K-{\hat{Z}}_{K|K}$$

  (26)
• Calculate the smoothing boundary layer ${\psi }_{K+1}$:

  $$S_{K+1|K}=C{P}_{K+1|K}{C}^T+R_{K+1}$$

  (27)

  $$E_{K+1}={\left|e_{Z,K+1|K}\right|}_{abs}+\gamma {\left|e_{Z,K|K}\right|}_{abs}$$

  (28)

  $${\psi }_{K+1}={\left({\overline{E}}_{K+1}^{-1}C{P}_{K+1|K}{C}^T{S}_{K+1}^{-1}\right)}^{-1}$$

  (29)

  where $Q_K$ and $R_{K+1}$ are the process noise covariance matrix and measurement noise covariance matrix, respectively; ${\overline{E}}_{K+1}$ is the diagonal matrix constructed with $E_{K+1}$; and the calculated optimal boundary layer ${\psi }_{K+1}$ of the uncalibrated visual servo system is the diagonal matrix ${\psi }_{K+1}=diag\left(\begin{array}{cccc}{\left({\psi }_1\right)}_{K+1}& {\left({\psi }_2\right)}_{K+1}& \cdots & {\left({\psi }_8\right)}_{K+1}\end{array}\right)$.
• Compare the calculated boundary layer ${\psi }_{K+1}$ with the threshold ${\psi }_S$, which is based on the specific conditions of the system, and switch the appropriate filter gain:
  If ${\psi }_{K+1}\le {\psi }_S$, the KF gain is switched to provide an optimal estimate:

  $$K_{K+1}=C^{+}{\overline{E}}_{K+1}{\psi }_{K+1}^{-1}$$

  (30)
  If ${\psi }_{K+1}>{\psi }_S$, the SVSF gain is switched to provide a robust estimate:

  $$K_{K+1}=C^{+}diag\left[E_{K+1}\circ Sat\left(\frac{e_{Z,K+1|K}}{\psi }\right)\right]\times {\left[diag\left(e_{Z,K+1|K}\right)\right]}^{-1}$$

  (31)

  where $\psi \in {\mathbb{R}}^{8\times 1}$ is the standard boundary layer in SVSF, and this can be adjusted according to the diagonal elements of the calculated smoothing boundary layer matrix ${\psi }_{K+1}$.

Then, we complete the subsequent total Jacobian estimation process:

6. Calculate the a posteriori state error covariance matrix:

$$P_{K+1|K+1}=\left(I-K_{K+1}C\right)P_{K+1|K}{\left(I-K_{K+1}C\right)}^T+K_{K+1}{R}_{K+1}{K}_{K+1}^T$$

(32)

7. Obtain the estimated state vector ${\hat{X}}_{K+1|K+1}$:

$${\hat{X}}_{K+1|K+1}={\hat{X}}_{K+1|K}+K_{K+1}{e}_{Z,K+1|K}$$

(33)

8. Split the state vector ${\hat{X}}_{K+1|K+1}$ into the estimated Jacobian $J_t$ as:

$${\left(J_t\right)}_K={\left[\begin{array}{ccc}{\left({\hat{X}}_{K+1|K+1}\right)}_1& \cdots & {\left({\hat{X}}_{K+1|K+1}\right)}_9\\ ⋮& \ddots & ⋮\\ {\left({\hat{X}}_{K+1|K+1}\right)}_{64}& \cdots & {\left({\hat{X}}_{K+1|K+1}\right)}_{72}\end{array}\right]}_{8\times 9}.$$

(34)

Remark 2.

When carrying out dynamic tracking tasks, the expanded total Jacobian matrix$J_e={\left[J_t,\mathsf{\partial }h/\mathsf{\partial }t\right]}_{8\times 10}$is estimated online, where$\mathsf{\partial }h/\mathsf{\partial }t$represents the Jacobian between the gradient of the task function and the velocity of the target and is important for the construction of a dynamic tracking controller.

3.3. Controller Design

Several advanced controllers based on the estimated Jacobian can be used to realize a well-behaved uncalibrated visual servo; for example, a nonlinear controller with a disturbance observer and a disturbance-rejection controller with an extended-state observer [25]. However, the proportional controller is still the most widely used and developed method in visual servos [22,23,26,29,31,32,33,34,35,36,37,38]. In addition, its simplicity is beneficial for us to verify this robust strategy.

Before constructing the proportional controller, two problems need to be solved. The first is the disturbance caused by the unstable conditional number of the estimated total Jacobian. Through a large number of simulations, it is found that the estimated Jacobian of the homography-based task function tends to present an unstable conditional number and causes drastic changes of the manipulator velocity in certain directions, resulting in severe vibrations and serious errors. Therefore, the SVF method is introduced to deal with this problem [45]. Singular value decomposition is performed on the estimated total Jacobian $J_t$ of the hyper-redundant manipulator according to

$${\left(J_t\right)}_{m\times n}=U_{m\times m}{\sum }_{m\times n}{V}_{n\times n}{}^T={\displaystyle \sum _{i=1}^m{\sigma }_i{u}_i}{v}_i^T$$

(35)

where ${\sigma }_i$ is the singular value of the total Jacobian $J_t$ and $u_i$ and $v_i$ are the $i\mathrm{th}$ columns of $U$ and $V$, respectively. The filtering function $h_{{\sigma }_0,\upsilon }\left(\sigma \right)$ of the singular value ${\sigma }_i$ proposed in the SVF method can maintain the stability of the smallest singular value of the estimated total Jacobian. It is defined as

$$h_{{\sigma }_0,\upsilon }\left({\sigma }_i\right)=\frac{{\sigma }_i^3+\upsilon {\sigma }_i^2+2{\sigma }_i+2{\sigma }_0}{{\sigma }_i^2+\upsilon {\sigma }_i+2}$$

(36)

where ${\sigma }_0$ is the minimum acceptable singular value and $\upsilon$ is a scaling factor that regulates the filtering shape of function $h_{{\sigma }_0,\upsilon }\left(\sigma \right)$. If ${\sigma }_0<\upsilon$ and $\upsilon {\sigma }_0<2$, $h_{{\sigma }_0,\upsilon }\left(\sigma \right)$ can stay monotonic. The pseudo-inverse of the total Jacobian $J_t$ can be developed via the SVF method as

$$J_S{}^{+}=\{\begin{array}{l}{J}_t^T{\left(J_t^T{J}_t\right)}^{-1},\min \left|{\sigma }_i\right|>{\sigma }_0\\ {\displaystyle \sum _{i=1}^m\frac{1}{h_{{\sigma }_0,\upsilon }\left({\sigma }_i\right)}}{v}_i{u}_i^T,else.\end{array}$$

(37)

The restriction on the minimum singular value guarantees the stability of the conditional number of the estimated total Jacobian. Thus, the disturbance caused by the unstable conditional number can be reduced without generating much additional error in the algorithms.

Another problem is that the manipulator joint angles may overrun. If the joint angle is not limited, it may cause a failure of the visual servo or even cause damage to the space manipulator. At this time, the redundancy of the joint space of the hyper-redundant manipulator resulting from the improved homography-based task function can be used to prevent this from happening. The gradient projection based on the redundant Jacobian can be adopted to avoid the joint limit [41,42]. When the joints move away from the limit of joint, allowing the joints to move freely can reduce unnecessary energy consumption. The gradient function is defined as

$$\nabla H\left(q\right)={\left[\begin{array}{ccc}\mathsf{\partial }H\left(q\right)/\mathsf{\partial }{q}_1& \cdots & \mathsf{\partial }H\left(q\right)/\mathsf{\partial }{q}_n\end{array}\right]}^T$$

(38)

$$\{\begin{array}{l}\frac{\mathsf{\partial }H(q)}{\mathsf{\partial }{q}_i}=0,if\Delta \left|\frac{\mathsf{\partial }H(q)}{\mathsf{\partial }{q}_i}\right|\le 0\\ \frac{\mathsf{\partial }H(q)}{\mathsf{\partial }{q}_i}=\frac{{(q_{i,\max }-q_{i,\min })}^2(2q_i-q_{i,\max }-q_{i,\min })}{4{(q_{i,\max }-q_i)}^2{(q_i-q_{i,\min })}^2},else\end{array}$$

(39)

On the basis of these optimizations, the controllers for static positioning and dynamic tracking are designed, respectively, as follows:

• Static positioning controller:

  $$\dot{q}=-\lambda {\left(J_S\right)}_K^{+}{e}_K+\eta (I-{\left(J_S\right)}_K^{+}{\left(J_S\right)}_K)\nabla H\left(q\right)$$

  (40)

  where ${\left(J_S\right)}_K$ is the processed total Jacobian in the $k\mathrm{th}$ period and $\lambda$ is the proportional gain.
• Dynamic tracking controller:

  $$\dot{q}=-\lambda {\left(J_S\right)}_K^{+}\left(e_K-{\left(e_t\right)}_K\right)+\eta (I-{\left(J_S\right)}_K^{+}{\left(J_S\right)}_K)\nabla H\left(q\right)$$

  (41)

  where ${\left(e_t\right)}_K=\left(\mathsf{\partial }h/\mathsf{\partial }t\right)\Delta t$ is the estimation of the task function variation in the $k\mathrm{th}$ period caused by target movement.

4. Simulations

This section describes several simulations of our improved strategy. The performances of this strategy in aligning with both static and dynamic assembly objects are compared with several original strategies. Their abilities of avoiding the joint limit and handling an unstable conditional number of estimated total Jacobian are verified as well.

4.1. System Description

The following simulation adopted the model of a 9 DOF hyper-redundant manipulator with an eye-in-hand camera. This 9 DOF manipulator was independently designed and manufactured by our laboratory. Figure 4 shows the mechanical structure and schematic of this manipulator, and

Table 1. Denavit–Hartenberg parameters of the nine degree-of-freedom (9 DOF) manipulator.

Joint	${\mathit{q}}_{\mathit{n}}$	${\mathit{\alpha }}_{\mathit{n}-1}$	${\mathit{a}}_{\mathit{n}-1}$	${\mathit{d}}_{\mathit{n}}$	Joint Limit
1	q1	0	0	0	±90°
2	q2	90°	165.5	0	±90°
3	q3	−90°	165.5	0	±90°
4	q4	90°	165.5	0	±90°
5	q5	−90°	165.5	0	±90°
6	q6	90°	165.5	0	±90°
7	q7	−90°	165.5	0	±90°
8	q8	90°	165.5	0	±90°
9	q9	−90°	165.5	0	±90°

gives the Denavit–Hartenberg parameters of the manipulator.

A five-megapixel industrial camera was mounted at the end-effector, and its intrinsic parameter was

$$K=\left(\begin{array}{ccc}1132.1& 0& 1280\\ 0& 1132.1& 1024\\ 0& 0& 1\end{array}\right).$$

To simplify the system model, the camera frame $F$ was considered to coincide with the end-effector frame $F_e$.

4.2. Simulations and Discussions of Aligning with Static Assembly Objects

To align with static objects, the various errors after the system reaches the steady state are the most reasonable index for comparing the performances of different uncalibrated visual servo strategies. Several tests were performed with different noise levels to check the robustness of the IBUVS, original PHUVS, improved PHUVS with KF method and improved PHUVS with KF-SVSF method. A square target was chosen with a side length of 200 mm and 49 feature points. Different types of initial pose errors, including a pure translation error $\left[\Delta \chi ,\Delta \theta \right]=[350\mathrm{mm},350\mathrm{mm},100\mathrm{mm},0^{\circ },0^{\circ },0^{\circ }]$, a pure rotation error $\left[\Delta \chi ,\Delta \theta \right]=[0\mathrm{mm},0\mathrm{mm},0\mathrm{mm},-{10}^{\circ },-{10}^{\circ },{45}^{\circ }]$ and a general error $\left[\Delta \chi ,\Delta \theta \right]=[350\mathrm{mm},350\mathrm{mm},100\mathrm{mm},-{10}^{\circ },-{10}^{\circ },{45}^{\circ }]$, were set according to the expected camera motion. The threshold of the boundary layer in the KF-SVSF model is defined as

$${\psi }_S=diag\left[\begin{array}{cccccccc}64& 36& 420& 36& 64& 420& 10& 10\end{array}\right]$$

Table 2. Steady-state error of different strategies in the static positioning test. $\Delta \chi$ is in millimeters, $\Delta \theta$ is in degrees and $\Delta I$ is in pixels. IBUVS: image-based uncalibrated visual servo. OPHUVS: original PHUVS.

Task	Method	Noise Mean
		$\left|\mathit{V}\right|=0.2$			$\left|\mathit{V}\right|=0.5$			$\left|\mathit{V}\right|=1$
		$\mathbf{\Delta }\mathit{\chi }$	$\mathbf{\Delta }\mathit{\theta }$	$\mathbf{\Delta }\mathit{I}$	$\mathbf{\Delta }\mathit{\chi }$	$\mathbf{\Delta }\mathit{\theta }$	$\mathbf{\Delta }\mathit{I}$	$\mathbf{\Delta }\mathit{\chi }$	$\mathbf{\Delta }\mathit{\theta }$	$\mathbf{\Delta }\mathit{I}$
Pure Translation	IBUVS	0.425	0.014	1.121	0.823	0.032	4.541	1.225	0.067	6.346
	OPHUVS	0.183	0.004	0.991	0.302	0.017	2.453	0.395	0.031	3.637
	KF	0.181	0.004	0.987	0.289	0.015	2.278	0.379	0.029	3.308
	KF-SVSF	0.174	0.004	0.886	0.273	0.015	2.052	0.364	0.029	3.081
Pure Rotation	IBUVS	0.341	0.023	1.427	0.537	0.045	5.253	0.873	0.069	7.641
	OPHUVS	0.126	0.004	0.932	0.268	0.016	2.466	0.385	0.034	3.826
	KF	0.124	0.004	0.929	0.251	0.015	2.349	0.374	0.031	3.527
	KF-SVSF	0.117	0.003	0.906	0.242	0.014	2.235	0.304	0.029	3.174
General Motion	IBUVS	0.374	0.029	1.435	0.911	0.049	6.213	1.352	0.072	8.214
	OPHUVS	0.187	0.006	0.813	0.261	0.018	2.716	0.406	0.033	3.826
	KF	0.185	0.006	0.811	0.243	0.017	2.529	0.394	0.032	3.672
	KF-SVSF	0.169	0.006	0.798	0.228	0.015	2.482	0.363	0.031	3.396

shows the errors of all the above tests. The improved PHUVS with the KF-SVSF method performs better than the IBUVS, original PHUVS and improved PHUVS with the KF. Sometimes, the IBUVS does not converge. Figure 5 shows the detailed test results of the improved PHUVS with the KF-SVSF method for general motion with a noise mean of $\left|V\right|=0.5$.

4.3. Simulations and Discussions of Aligning with Dynamic Assembly Objects

The mean square error (MSE) is chosen as an index to compare the performances of different strategies for aligning with dynamic assembly objects. It represents the average image error between the current and desired image features in the servo process, and is defined as

$$MSE=\frac{1}{N}{\displaystyle \sum _{i=1}^N\Vert \Delta I_i\Vert }$$

where $\Delta I_i$ is the image error at the $i\mathrm{th}$ iteration and $N$ is the number of iterations.

The same target is adopted to move in planar and 3D trajectories. Both 2D motion in a double leaf rose path and 3D motion in a triple leaf rose path with different noise levels are simulated. These different paths of the target center are denoted as follows:

$$P_{rose2}=\left[\begin{array}{l}450+300\cos \left(3\omega N\Delta T\right)\sin \left(\omega N\Delta T\right)\\ 800+300\cos \left(\omega N\Delta T\right)\sin \left(3\omega N\Delta T\right)\\ 820\end{array}\right]$$

$$P_{rose3}=\left[\begin{array}{l}450+250\sin \left(3\omega N\Delta T\right)\sin \left(\omega N\Delta T\right)\\ 800+300\cos \left(\omega N\Delta T\right)\sin \left(3\omega N\Delta T\right)\\ 820+vN\Delta T\end{array}\right]$$

where $\omega =\pi /30\mathrm{rad}·{\mathrm{s}}^{-1}$ is the angular velocity in the $X-Y$ plane, $v=2.5\mathrm{mm}·{\mathrm{s}}^{-1}$ is the translation velocity along the $Z$ axis, $\Delta T=0.02s$ is the sampling period and $N$ is the number of iterations. The threshold of the KF-SVSF boundary layer was defined as

$${\psi }_S=diag\left[\begin{array}{cccccccc}128& 42& 1360& 42& 128& 1360& 16& 16\end{array}\right]$$

Table 3. Mean square error (MSE) of different strategies in the dynamic tracking test.

Path	Method	Noise Mean
		$\left|\mathit{V}\right|=0.2$	$\left|\mathit{V}\right|=0.5$	$\left|\mathit{V}\right|=1$
		MSE	MSE	MSE
Double leaf rose	IBUVS	4.97	8.54	11.47
	OPHUVS	3.94	5.21	7.56
	IPHUVS with KF	2.59	3.84	5.54
	IPHUVS with KF-SVSF	2.37	3.56	5.31
Triple leaf rose	IBUVS	5.12	8.96	11.67
	OPHUVS	3.91	5.13	7.48
	IPHUVS with KF	2.66	3.75	5.62
	IPHUVS with KF-SVSF	2.43	3.47	5.39

MSE is in pixels.

gives the MSEs of all the above tests. All of these MSEs are the average of 100 experiments. Figure 6 and Figure 7 show the image errors of the different strategies for the double leaf rose and triple leaf rose paths with a noise mean of $\left|V\right|=0.5$, and Figure 8 and Figure 9 show the camera trajectories of the different strategies. The IBUVS is the least robust method to image noise, and its tracking precision is unacceptable. The improved PHUVS, which directly controls manipulator joints, has better performance than the original PHUVS, as shown in Figure 2a. Meanwhile, the improved PHUVS with the KF-SVSF method shows better tracking precision than the improved PHUVS with the KF.

The abilities of the strategies to avoid the joint limit and handle an unstable conditional number are examined as well. The joint limit was set as $\pm {90}^{\circ }$, and the minimum acceptable singular value of the total Jacobian was set as ${\sigma }_0={10}^{-4}$. Figure 10 shows the unlimited and limited joint angles for these two trajectories. Figure 11 shows the logarithm of the minimum singular value ${\sigma }_{\min }$ of the estimated total Jacobian ${\left(J_t\right)}_K$ and processed Jacobian ${\left(J_S\right)}_K$ in each period. In each tracking trajectory, all joint angles of the super-redundant manipulator are limited to the acceptable range. When unstable condition numbers are caused by the appearance of excessively small minimum singular values, the minimum singular value ${\sigma }_{\min }$ of the processed Jacobian remains above the threshold ${\sigma }_0={10}^{-4}$.

5. Experiments

5.1. Evaluation of Real-Time Performance

Several experiments of systems with different homography-based task functions are carried out. The time of consumed (TOC) is the most appropriate indicator to measure real-time performance. The TOC-H which is the time consumed on the homography matrix estimation, and the TOC-C, which includes the time consumed for Jacobian estimation and control output calculation, are selected as indicators to compare the real-time performance of the system. A PC with an Intel(R) Xeon(R) E5-1620 CPU at 3.5 GHz, 64-b operating system was chosen to run all experiments. The average values of TOC-H and TOC-C in each experiment with more than 10,000 control periods are shown in Table 2. The experiment in the article is only presented to compare the real-time performance of the two strategies. In practical space applications, this part of the function may be implemented using FPGA. Corresponding designs and improvements will be made to meet computational efficiency requirements.

As shown in

Table 4. Detailed value of the TOC of the original PHUVS and improved PHUVS.

Number of Feature Points	IPHUVS			OPHUVS
	TOC-H	TOC-C	Total	TOC-H	TOC-C	Total
4	0.5486	0.8109	1.3595	0.5624	1.0891	1.6515
9	0.7504	0.8109	1.5613	0.7608	1.0892	1.85
16	0.9712	0.8109	1.7821	0.9804	1.0892	2.0696
25	1.2942	0.8108	2.105	1.3022	1.0891	2.3913
36	1.6472	0.8107	2.4579	1.6591	1.0892	2.7483
49	2.2026	0.8108	3.0134	2.2176	1.0893	3.3069

and Figure 12, the TOC-H and TOC-C of the improved homography-based task function are always lower than the original homography-based task function with different numbers of feature points. According to the comparison of the results, the improved homography-based task functions with smaller dimensions show more convincing real-time performance, as expected.

5.2. Evaluation of System Performance

As shown in Figure 13, a 9 DOF hyper-redundant manipulator, 6 DOF universal robot, smart camera and NI real-time system were used to build the experimental platforms. The parameters of the manipulator and the camera are consistent with the parameters set in the simulations. Experiments for aligning with both static and dynamic assembly objects were conducted to verify the improvement of our strategy. A square target was chosen with a side length of 200 mm and 49 feature points. All experiments were performed under an illumination similar to that of actual working conditions to examine the reliability of our strategy.

5.2.1. Static Positioning Experiment

Considering that, in the actual work scene, manipulators often perform general movements to align with assembly targets, the initial pose error of the positioning experiment was set as

$$\left[\Delta \chi ,\Delta \theta \right]=[-226\mathrm{mm},224\mathrm{mm},386\mathrm{mm},{30}^{\circ },{10}^{\circ },{10}^{\circ }]$$

The threshold of the boundary layer in the KF-SVSF model was defined as

$${\psi }_S=diag\left[\begin{array}{cccccccc}64& 36& 420& 36& 64& 420& 10& 10\end{array}\right]$$

Table 5. Steady-state error of different strategies in the static positioning experiment. $\Delta \chi$ is in millimeters, $\Delta \theta$ is in degrees and $\Delta I$ is in pixels.

Method	$\mathbf{\Delta }\mathit{\chi }$	$\mathbf{\Delta }\mathit{\theta }$	$\mathbf{\Delta }\mathit{I}$
IBUVS	1.08	0.023	4.14
OPHUVS	0.96	0.012	2.81
IPHUVS with KF	0.91	0.012	2.76
IPHUVS with KF-SVSF	0.72	0.011	2.59

gives the various steady-state errors of different strategies in positioning experiments, and these results are the average of five repeated experiments. Although the noise of image transmission and processing has a certain impact on the accuracy of target recognition, the improved PHUVS with the KF-SVSF method clearly performs better than other strategies in each comparison. This verifies the superiority of our improved strategy. Figure 14 is the change curve of the proposed improved PHUVS in terms of the image error and task function error. Figure 15 shows the angular velocity of each joint of the manipulator.

5.2.2. Dynamic Tracking Experiment

In the dynamic tracking experiment, the target was fixed to the end-effector of a universal robot and taken to move along different paths. To ensure reliability, both complex planar and 3-D paths were tested. The planar and 3D paths of the target were set as

$$P_{rose2}=\left[\begin{array}{l}840+200\cos \left(3\omega N\Delta T\right)\sin \left(\omega N\Delta T\right)\\ 1690\\ 200+250\cos \left(\omega N\Delta T\right)\sin \left(3\omega N\Delta T\right)\end{array}\right]$$

$$P_{rose3}=\left[\begin{array}{l}840+250\sin \left(3\omega K\Delta T\right)\sin \left(\omega K\Delta T\right)\\ 1690+vK\Delta T\\ 200+250\cos \left(\omega K\Delta T\right)\sin \left(3\omega K\Delta T\right)\end{array}\right]$$

where $\omega =\pi /6\mathrm{rad}·{\mathrm{s}}^{-1}$ is the angular velocity in the $X-Y$ plane, $v=1.25\mathrm{mm}·{\mathrm{s}}^{-1}$ is the translation velocity in the $Z$ direction, $\Delta T=0.04s$ is the sampling period and $K$ is the number of iterations. The threshold of the KF-SVSF boundary layer was defined as

$${\psi }_S=diag\left[\begin{array}{cccccccc}128& 42& 1360& 42& 128& 1360& 16& 16\end{array}\right]$$

Table 6. MSE of different strategies in the dynamic tracking experiment.

	Path	Double Leaf Rose	Triple Leaf Rose
Method
IBUVS		9.43	10.19
OPHUVS		7.32	7.74
IPHUVS with KF		6.28	6.61
IPHUVS with KF-SVSF		5.61	5.89

MSE is in pixels.

gives the mean square error (MSE) of different strategies tracking two different paths, and these results are the average of five repeated experiments. Figure 16, Figure 17, Figure 18 and Figure 19 show the image errors of the different strategies, and the joint angle and joint angular velocity of the proposed improved PUHVS with the KF-SVSF method, in the double leaf rose path and the triple leaf rose paths respectively. Obviously, the improved PHUVS with the KF-SVSF method shows better accuracy and stability than other strategies. During the tracking process, the tracking error is always kept in an acceptable range. It is worth mentioning that, due to the adoption of the SVF method, the system smoothly completes the servo process without severe disturbance. It can also be seen that, because the redundancy resulting from the improved homography-based task function is used for the design of the controller to limit the joint angle, there was no case in which the joint angle exceeded the limit, and the entire experiment was successfully completed.

It is worth noting that, in our experiment, we calculated the corresponding joint increment command $\Delta q=\dot{q}\cdot \Delta T$ through the sampling period $\Delta T$ instead of continuous joint velocities commands $\dot{q}$ to send to the robot. During the execution of each sampling period, joint velocities were automatically planned when moving the increment $\Delta q$. As shown in Figure 17a and Figure 19a, the joint angle was still relatively smooth. However, in Figure 5f, Figure 15, Figure 17b and Figure 19b, high-frequency oscillations were present in joint velocities caused by various noises. If the manipulator were controlled in a speed mode in actual tasks, this might cause vibrations and it might be necessary to add a filter to cut high frequencies. This would add a delay and possibly make this method unsuitable for real-time operations in space. Researchers have noticed this problem and found that PHUVS experiences a much smaller variation amplitude in joint velocities than IBUVS [23]. However, this level of improvement is not sufficient to completely solve the problem, and this is still a shortcoming that needs to be overcome in future applications.

6. Conclusions

This paper proposed an improved uncalibrated visual servo strategy for hyper-redundant manipulators in an on-orbit automatic assembly. An improved homography-based task function with robustness to image noise and better real-time performance was constructed, which made the joint space of the hyper-redundant manipulator redundant to the task function. To improve the system accuracy and avoid the interference caused by the change of manipulator parameters in space, a proportional controller based on the estimated total Jacobian was designed to control the manipulator joints directly without a manipulator Jacobian. The KF-SVSF method, which combines the optimality of the KF method with the robustness of the SVSF method, was introduced to perform the online estimation of the total Jacobian and further reduced the impact of modeling uncertainty and image noise. The SVF method was adopted to reduce the disturbance caused by an unstable conditional number of the estimated total Jacobian without generating much additional error in the algorithms. In addition, the redundancy of the joint space relative to the new task function was exploited to limit the joint angle. Several tasks for aligning with static or dynamic objects were simulated to test the performance of this strategy. The simulation results demonstrated the improved accuracy and robustness of this strategy. The abilities of the system to limit the joint angle and deal with an unstable conditional number were verified as well. The reliability of this strategy was further validated by experiments based on actual working conditions.