Adaptive Impedance Control of a Novel Automated Umbilical System for Propellant Loading

Abstract

In this paper, an automated umbilical system based on a 6-dof (degree of freedom) hydraulic parallel mechanism is proposed to automate the rocket propellant loading process. The mechanical structure, vision acquisition algorithm, and control algorithm used in the system are described in detail in the paper. To address the fluid nonlinearity problem of the hydraulic drive system, nonlinear compensation and three-state feedback control are used in the paper to enhance the performance of the hydraulic system. For the problem of force tracking during the docking process between the umbilical system and the rocket, an adaptive impedance control algorithm based on the online environmental parameter estimation is proposed in the paper, which effectively reduces the contact force during the docking process. The dynamic tracking and docking experiments indicate that this automated umbilical system features rapid reaction speed, high measurement precision, and good flexibility, which can be used to realize the auto-mating and following task for the propellant loading robot in a hazardous environment.

1. Introduction

The ground umbilical system typically refers to the connecting device between the launch vehicle and the launch tower used to provide propellant refueling, electrical power and control, hazardous gas detection, and system checkout for the rocket [1]. Umbilical systems usually include alignment systems, mating and locking systems, fluid connectors, electrical connectors, and control/inspection systems. Due to the oscillation of the rocket body under wind load, the umbilical system is usually manually aligned with the rocket fuel port by technicians.

One of the challenges of the umbilical system is the mating and dismating function between the connector and the fuel port. Currently, the connector is manually docked to the fuel port by technicians and the umbilical remains attached to the rocket throughout propellant loading and launch preparation until the rocket lifts off [1,2]. A passive dismating device was designed to enable the disengagement function of the rocket and umbilical system, the fuel line can be rapidly removed to make the launch vehicle safe, but explosive bolts and a mechanical tear-off feature are used to disconnect the lines immediately after launch, offering the possibility of damage to the rocket body during launch. Rocket propellant is notable for being explosive, volatile, or corrosive, and inhalation or exposure to small amounts can result in poisoning and injury to technicians [1,3]. The current manual method of reconnecting fuel lines after an aborted launch takes over 15 h and has many safety concerns for both technicians and astronauts.

In addition, with the increase of rocket carrying capacity, the corresponding umbilical connector and pipeline loads are also increasing, and the connector loads of some launch vehicles currently under development have reached several hundred kilograms, which are completely beyond the scope of manual mating [4]. Therefore, an automatic umbilical system for rocket propellant refueling is proposed to reduce the labor intensity of operators and improve operational safety, while still maintaining reliability.

The use of robots for rocket umbilical docking can be broadly divided into three distinct phases [2]. The first stage addresses the rocket’s swaying under the influence of external wind loads and uses machine vision to enable the robot to approach and track the umbilical target. The second stage is the mating process between the umbilical connector and the fuel filler, which will require a combination of mechanical guidance, compliance, and active force feedback control, for which little research has been done. The third stage is after the umbilical system is successfully mated and locked to the rocket. In this case, the automated umbilical system needs to follow the rocket by means of force-controlled oscillations in order to avoid large contact forces. In other words, the robot must then have considerable flexibility so that the rocket can easily guide the umbilical system through its motion to keep the contact force within an acceptable range.

The umbilical mating problem requires an automatic mating robot with a wide range of sensing capabilities of the environment [5,6]. First, the launch vehicle is a dynamic structure, which constantly undergoes random motion under the influence of wind load and propellant weight changes. Secondly, the mating problem requires a sufficiently high tracking accuracy precision of the umbilical connector to the rocket filler port to achieve smooth docking. Finally, the contact force between the umbilical system and the rocket during mating and follow-through must be kept within a low range to avoid damage to the rocket body.

NASA and Roscosmos (Russian Federal Space Agency) have carried out continuous research on launch-vehicle-connector automatic docking technology and docking mechanism [1,2,3]. On the U.S. side, research on automatic umbilical docking mechanisms such as balance arm type and telescopic type was carried out for Saturn V, Atlas V, X-33, and other types of launch vehicles/vehicles in the early stage. In 2004, Kennedy Space Center developed a prototype remote intelligent umbilical docking system, which has a passive follower function to reduce the potential hazards during propellant refueling work by operators, but no application reports and related academic papers have been published; in 2007, NASA proposed an “automatic docking ground umbilical connector system” research program for maintaining reliability and reducing the operation time and invested manpower for ground umbilical connection [1,7]; in 2012, Kennedy Space Center developed a linear umbilical docking locking mechanism consisting of inner and outer plates for Ares I. The outer plate is used to connect with the rocket, and the inner plate is manually pushed to dock the connector with the rocket [3].

In Russia and the former Soviet Union, the Tsyklon and Zenit launch vehicles were the first to use the cone-rod guided automatic docking mechanism to complete the automatic docking and undocking between the connector and the rocket, but because their docking parts were located at the tail of the rocket, the influence of external factors was small, and the docking process was relatively stationary and relatively uncomplicated [1,2].

Most of the information related to the umbilical systems of NASA and Roscosmos is not published, and it is difficult for us to learn the specific implementation principles and performance indicators. Dun et al. [5] and Zhou et al. [8] proposed an automatic umbilical system based on a 3-dof parallel mechanism, but it did not investigate the compliant force control during docking stage. You et al. [9] proposed an automatic umbilical system based on the Scara manipulator, but it was still unable to track the rocket motion with high accuracy due to the structural limitation of the 3-dof structure.

In this paper, a novel automatic umbilical system based on a hydraulic 6-dof parallel mechanism is proposed to achieve automatic mating and dismating work of umbilical connectors. It has a larger load capacity than the conventional 3-dof parallel mechanism and can accurately track the rocket filler port in six degrees of freedom in the working space. The hydraulic system is used to further increase the load-carrying capacity of the automatic umbilical system, and the hydraulic system has a higher power density than the motor system to meet the current demand for large loads in the umbilical system.

In order to integrate the different working phases of the automatic umbilical system into the same control framework, a hybrid control framework of vision and force is proposed in the paper to realize the approach, tracking, and docking process of the umbilical system and to avoid the impact of state switching on the system stability. A vision-positioning system [10] that integrates a monocular camera and laser range finders is used to achieve accurate tracking of the rocket fuel port, which has higher definition accuracy and lower time lag compared with the traditional positioning methods. For the contact force problem generated during the mating and following stages of the umbilical system, this paper adopts an adaptive impedance control algorithm based on online environmental parameter estimation to guide the umbilical connector interacting with the rocket during mating and to allow the robot to track the dynamic motion of the rocket after mating. Finally, the dynamic tracking and docking experiments are conducted for different working stages of the umbilical system. The experimental results show that the visual tracking error and the magnitude of the contact force are within the required range, and the novel automated umbilical connection system proposed in the paper is feasible and has sound performance.

2. System Description

The entire experimental system consists of a hydraulic parallel manipulator and a motion simulator of the target object. In this section, the various parts of the system are described in detail. After that, the working principle of the vision system employed in the article is introduced, and the kinematics and dynamics modeling of the platform are carried out.

2.1. System Components

The schematic diagram of the automated umbilical system is shown in Figure 1. The fixed platform of the parallel mechanism is connected to the launch tower or the service arm, and the connector and hydraulic pipeline are driven by the movable platform to dock and connect with the propellant filling port of the rocket. Due to external influences that can cause the launch vehicle to swing randomly [11], the automated umbilical system employs machine vision to locate the rocket, and employs an adaptive impedance controller to achieve a stable and safe contact with the rocket to avoid damage to the system.

At present, we do not have the conditions for direct field experiments, so we designed a simulation experiment system to verify the feasibility of the scheme. The experiment system was designed to simulate the rocket propellant loading process under the influence of wind, as shown in Figure 2. Located in the middle of Figure 2 is the automated umbilical system, mainly including a hydraulic parallel platform (HPM) and a docking device as the end-effector. The fixed platform of the parallel mechanism is connected to the basement, which should be the rocket launch tower during the actual filling process. The moving platform on the right side is equipped with a docking device to realize the docking and locking function with the rocket refueling port. A 6-dof force sensor is located between the dynamic platform and the docking device to measure the contact force during the docking process. A monocular camera and a laser rangefinder are mounted on top of the moving platform to locate the fuel filler as the target object.

Located on the right side of Figure 2 is a simulation device for rocket body motion, with a target object installed in its middle corresponding to the fuel filling port in the real propellant filling process. The motion simulator device has slideways in x, y, and z directions, and simultaneous motion in all three directions can be achieved by stepper motors to simulate the rocket’s oscillation under the influence of wind.

2.2. Vision Positioning Method

Computer vision technology has been widely used in industry, medicine, aerospace, agriculture, and other fields because of its noncontact, low-cost, and easy-to-implement advantages [12]. According to the number of visual sensors employed, the visual servo system can be divided into monocular vision, binocular vision, and multieye visual positioning [13]. In this article, the novel automated umbilical system uses a fusion of monocular vision and laser rangefinder to locate the rocket fuel port and guide the parallel mechanism to approach the target object.

As shown in Figure 3, the image acquisition module first collects the target video image input by the camera in real time and the target detection module quickly confirms the distance of the target by pattern matching; then the target center detection and orientation modules calculate the position and orientation of the target by image segmentation and target tracking according to the distance of the target; the target center estimation module calculates the target center’s position by bundle adjustment according to the location of the target element; the target orientation estimation module will calculate the main direction of the target according to the position of the direction element and outputs the rotation angle [13,14].

To identify the position and pose of the target, the target must first be accurately detected, the target and direction elements in the target must be accurately extracted, and then the key parameters must be estimated through robust statistics. Figure 4 shows the results of target image analysis.

As shown in Figure 4, (a) is the original target image; (b) is the binary image; (c) is the target detection result; (d) is the target detection result; (e) is the bulls eye estimation result, and the target position is marked by a green cross; and (f) is the estimated result of the main direction of the target.

Through the target image analysis, the position deviation of the camera axis relative to the target center (x_d, y_d), and the roll angle γ of the target in the camera image plane can be accurately identified.

Due to its own structure limitation, the measurement accuracy of monocular vision in the camera’s optical axis direction is relatively low, the manipulator cannot accurately perceive depth information. In this paper, the three laser-ranging sensors measure the distance between the camera and the target separately, estimate the inclination along the line of sight of the camera by analyzing the distance difference, and output the pitch angle β and the azimuth angle α.

As shown in Figure 2, by applying a laser rangefinder, the distance acquisition module measures the depth of the target and inclination estimation module estimates the tilt of the target by analyzing the deviation of the laser-ranging sensor. At last, the pose estimation module integrates the data collected by the camera and the laser rangefinder to estimate the position and orientation of the target.

A complete description of the relative position of the camera and the target requires six parameters: the position of the camera’s optical center on the three axes of the target coordinate system (x, y, z) and the inclination of the camera’s optical axis around the three axes (α, β, γ). Based on the distance Z_d from the camera’s optical center to the intersection of the target surfaces, the coordinates of the camera’s optical center in the target coordinate system (x_c, y_c, z_c) can be obtained by a simple calculation [12]:

$$x_c=-z_d·\sin \alpha ·\cos \gamma +z_d·\sin \beta ·\sin \gamma$$

(1)

$$y_c=-z_d·\sin \alpha ·\sin \gamma +z_d·\sin \beta ·\cos \gamma$$

(2)

$$z_c{=\mathrm{z}}_d·\cos \alpha ·\cos \beta$$

(3)

Using the position deviation identified by the vision system (x_c, y_c) to modify these data, the correct position of the camera within the target coordinate system can be expressed as

$$x=x_d\frac{z_c}{f}-x_c$$

(4)

$$y=y_d\frac{z_c}{f}-y_c$$

(5)

$$z=z_c$$

(6)

The vision system can only measure the relative position between the target object and the camera, the precise target’s location relative to the basement coordinate cannot be obtained directly. Since the camera is located at a fixed position on the platform, we can transform the position information according to the relative relationship between the various coordinate systems [14].

Here are the descriptions of the various coordinate systems involved: the basement coordinate system {B} of the parallel mechanism is $O_B-X_B{Y}_B{Z}_B$, the eye-in-hand camera coordinate system {C} is $O_C$—$X_C{Y}_C{Z}_C$, the moving platform coordinate system {P} is $O_P$—$X_P{Y}_P{Z}_P$, and the vision target coordinate system is $O_t$—$X_t{Y}_t{Z}_t$, the docking target object coordinate system is $O_o$—$X_o{Y}_o{Z}_o$, as shown in Figure 2.

The end-effector and the planar target are both installed at a fixed position of the docking experiment system, and the position relationship between the moving platform and the basement can be calculated by kinematics. Therefore, the position relationship between the target object coordinate and the basement can be derived as

$${}_o{}^B\mathit{T}={}_P{}^B\mathit{T}·{}_C{}^P\mathit{T}·{}_t{}^C\mathit{T}·{}_o{}^t\mathit{T}$$

(7)

where ${}_o{}^B\mathit{T}$ represents the coordinate transformation matrix between the basement and the target object, ${}_P{}^B\mathit{T}$ represents the coordinate transformation matrix between the basement and the moving platform, ${}_c{}^P\mathit{T}$ represents the coordinate transformation matrix between the moving platform and the camera, ${}_t{}^c\mathit{T}$ represents the coordinate transformation matrix between the camera and the planar target, and ${}_o{}^t\mathit{T}$ represents the coordinate transformation matrix between the planar target and the docking device.

$${}^B\mathit{X}_o={}_o{}^B\mathit{T}·{}^o\mathit{X}_o={}_P{}^B\mathit{T}·{}_c{}^p\mathit{T}·{}_t{}^c\mathit{T}·{}_o{}^t\mathit{T}·{}^o\mathit{X}_o$$

(8)

where ${}^B\mathit{X}_o$ represents the target object’s position vector in the basement coordinate system {B} and ${}^o\mathit{X}_o$ represents the docking device’s position in the target object coordinate system.

The end-effector’s position vector ${}^P\mathit{X}_e$ in the moving platform coordinate {P} is known according to the design, so the end-effector’s position vector ${}^B\mathit{X}_e$ in the basement coordinate {B} can be expressed as

$${}^B\mathit{X}_e={}_P{}^B\mathit{T}·{}^P\mathit{X}_e$$

(9)

Thus, the position deviation vector ${}^B\mathsf{\Delta }\mathit{X}$ of the end effector and the target object in the basement coordinate can be derived as

$${}^B\mathsf{\Delta }\mathit{X}={}^B\mathit{X}_o-{}^B\mathit{X}_e={}_P{}^B\mathit{T}·{}_c{}^p\mathit{T}·{}_t{}^c\mathit{T}·{}_o{}^t\mathit{T}·{}^o\mathit{X}_o-{}_t{}^B\mathit{T}·{}^t\mathit{X}_e$$

(10)

2.3. Manipulator Modeling

The main structure of the automated umbilical system adopts a 6-dof parallel mechanism, which has higher precision, faster response, and higher load-bearing capacity than traditional tandem robots with parallel structure. The six degrees of freedom of motion enable it to achieve precise tracking of the rocket in the working space without generating large contact forces on the rocket due to its own structural limitations [15].

Manipulator’s kinematics is a theory that describes the motion of the manipulator without considering the mass or the forces affecting the motion [16]. The kinematics of the parallel manipulator can be classified into inverse kinematics and forward kinematics. The inverse kinematics is a mathematical process of calculating the actuator parameters based on the given manipulator’s position and orientation [17]. Using an analytical method, the actuator length can be derived as

$$\mathit{l}=\Vert {\mathit{R}}_l\left(\mathit{q}\right)·\mathit{A}+\mathit{c}-\mathit{B}\Vert$$

(11)

where $\mathit{l}\in {\mathit{R}}^6$ indicates the actuator length vector from upper joints to lower joints, $\mathit{A}\in {\mathit{R}}^{3\times 6}$ and $\mathit{B}\in {\mathit{R}}^{3\times 6}$ are the moving platform and basement joint points matrix, respectively, ${\mathit{R}}_l\in {\mathit{R}}^{6\times 6}$ is the rotation matrix from moving platform coordinates {P} to basement coordinate {B}, $\mathit{c}\in {\mathit{R}}^3$ is the position vector of the moving platform, and $\mathit{q}\in {\mathit{R}}^6$ is the state vector of the platform generalized coordinate.

The relationship [18] between the generalized velocity vector $\dot{\mathit{q}}$ and the actuator velocity vector $\dot{\mathit{l}}$ can be formulated as

$$\dot{\mathit{l}}={\mathit{J}}_{lq}\left(\mathit{q}\right)\dot{\mathit{q}}$$

(12)

The forward kinematic concerns the end effector’s states with the given actuator’s length in the basement coordinate system {B}. In practical application, the Newton–Raphson method is used to estimate the manipulator’s status, thus the forward kinematics of parallel manipulator can be expressed as

$${\mathit{q}}_{j+1}={\mathit{q}}_j+\mathit{J}{\left({\mathit{q}}_j\right)}^{-1}\left({\mathit{l}}_o-{\mathit{l}}_j\right)$$

(13)

where j is the interaction number, ${\mathit{q}}_j\in {\mathit{R}}^6$ is the state vector of the platform coordinate,${\mathit{l}}_j\in {\mathit{R}}^6$ is the solving actuator vector during the jth iterative calculation, and ${\mathit{l}}_o\in {\mathit{R}}^{6\times 1}$ is the initial measured actuator length vector. $\mathit{J}\left({\mathit{q}}_n\right)\in {\mathit{R}}^{6\times 6}$ is the Jacobian matrix that relates the body coordinate and the actuator velocity.

The dynamic equations of parallel robots can be deduced by various methods. In this paper, the Kane method [19] is used to describe the dynamic model of the parallel mechanism.

$$\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}\left(\mathit{q},\dot{\mathit{q}}\right)\dot{\mathit{q}}+\mathit{G}\left(\mathit{q}\right)={\mathit{J}}_{lq}{\left(\mathit{q}\right)}^{\mathrm{T}}\mathit{f}+{\mathit{F}}_e$$

(14)

where $\mathit{q}\in R^6$ is the manipulator’s position vector, $\mathit{M}\left(\mathit{q}\right)\in R^{6\times 6}$ is the mass matrix, $C\left(\mathit{q},\dot{\mathit{q}}\right)\in R^6$ is a matrix of centrifugal and Coriolis terms, $\mathit{G}\left(\mathit{q}\right)\in R^{6\times 6}$ is a vector of the gravity term, $f\in R^6$ is a 6 × 1 vector of actuator force, ${\mathit{J}}_{lq}\in R^{6\times 6}$ is the parallel manipulator’s Jacobin matrix, and ${\mathit{F}}_{\mathit{e}}\in R^6$ is a vector of generalized contact force.

3. Hydraulic System Control Scheme

3.1. Hydraulic System Modeling

Compared with the usual motor drive system, the hydraulic system has a higher power output ratio [20]. This means that the hydraulic system can output higher power under the same volume and weight. In rocket propellant filling work, we hope that the automated docking system can carry a higher weight in a limited working space, so the automated umbilical system chooses a hydraulic actuator to drive it.

However, the hydraulic actuator also has some shortcomings due to its own structural characteristics. For example, the lower frequency response compared to the motor drive system and the nonlinear flow rate generated by the servo valve will be analyzed later [21].

The electrohydraulic servo system is mainly composed of the electrohydraulic servo valve, hydraulic cylinder, displacement sensor, pressure sensor, and inertia load. The single rod asymmetric hydraulic cylinder can effectively reduce the length of the actuator and make the structure compact, which is conducive to improving the stiffness of the system. The performance parameters of the hydraulic cylinders are listed in

Table 1. Hydraulic cylinder parameters.

Symbol	Name	Value
D1	Piston diameter	40 mm
D2	Piston rod diameter	28 mm
Lw	Hydraulic cylinder stroke	±196 mm
ε	Cylinder buffer length	25 mm
l0	Cylinder initial length	1200 mm
lmax	Cylinder initial length	1617 mm
vma	Piston rod maximum speed	0.25 m/s

.

The control system of the asymmetric cylinder is shown in Figure 5, where x_v is the servo valve spool displacement and y is the hydraulic cylinder piston rod displacement.

According to the throttling flow formula [22], the flow rate into the left chamber of the hydraulic cylinder is

$$q_1=\{\begin{array}{cc}{C}_{d}w{x}_v\sqrt{\frac{2}{\rho }\left(p_s-p_1\right)}& x_v\ge 0\\ C_{d}w{x}_v\sqrt{\frac{2}{\rho }\left(p_1-p_r\right)}& x_v<0\end{array}$$

(15)

The flow rate into the right chamber of the hydraulic cylinder is

$$q_2=\{\begin{array}{cc}{C}_{d}w{x}_v\sqrt{\frac{2}{\rho }\left(p_2-p_r\right)}& x_v\ge 0\\ C_{d}w{x}_v\sqrt{\frac{2}{\rho }\left(p_s-p_2\right)}& x_v<0\end{array}$$

(16)

where q₁ and q₂ are the flow rates of the left and right chambers of the hydraulic cylinder, respectively, p₁ and p₂ are the pressures of the left and right chambers of the hydraulic cylinder, respectively, p_s is the oil source pressure, p_r is the hydraulic line pressure, C_d is the flow rate coefficient, w is the throttle area gradient, x_v is the servo valve spool displacement, and ρ is the hydraulic oil density.

The flow continuity equation for the left chamber of the hydraulic cylinder is

$$q_1=A_1\dot{l}+\frac{v_1}{{\beta }_e}{\dot{p}}_1+c_{ic}\left(p_1-p_2\right)+c_{ec}{p}_1$$

(17)

The flow continuity equation for the right chamber of the hydraulic cylinder is

$$q_2=A_2\dot{l}-\frac{v_2}{{\beta }_e}{\dot{p}}_2+c_{ic}\left(p_1-p_2\right)-c_{ec}{p}_2$$

(18)

where A₁ and A₂ are piston effective area of the left and right chambers; l is piston rod displacement; v₁ and v₂ are the volumes of the left and right chambers, respectively; c_ic and c_re are the internal leakage coefficient and external leakage coefficient of the hydraulic cylinder, respectively.

The hydraulic system force balance equation is

$$f_l=A_1\left(p_1-p_2\right)=m_l\ddot{l}$$

(19)

Define the load flow $q_L=\frac{1}{2}\left(q_1+q_2\right)$, load pressure $p_L=p_1-p_2$, considering $p_s=p_1+p_2$ and $p_r=0$, we can add Equations (15) and (16) to obtain the load flow

$$q_L=C_{d}w{x}_v\sqrt{\frac{p_s-\mathrm{sign}(x_v)p_L}{\rho }}$$

(20)

When the hydraulic cylinder moves near the neutral position, it can be approximated as v₁ = v₂, and we can add Equations (17) and (18) to obtain

$$q_L=A\dot{l}+\frac{V_t}{4{\beta }_e}{\dot{p}}_L+c_{tc}{p}_L$$

(21)

where $c_{tc}=c_{ic}+0.5c_{ec}$ is total hydraulic cylinder leakage coefficient and V_t is the total volume of the two chambers of the hydraulic cylinder.

According to Equations (19)–(21), the nonlinear model of the electrohydraulic servo system is represented as the block diagram shown in Figure 6. The nonlinear part mainly considers the nonlinear influence of the servo valve flow equation.

3.2. Hydraulic System Nonlinear Compensation

There are many nonlinear factors in the electrohydraulic servo system [23], including the dead zone of the servo valve, hysteresis, the Coulomb friction of the hydraulic actuator, etc. The main consideration in this article is the nonlinear effect of the flow equation of the servo valve. When the load changes make the steady-state operating point of the system deviate from zero, the nonlinearity of the flow equation will cause the valve flow coefficient to be different during forward and reverse movement, resulting in the problem of asymmetric movement.

It can be seen from Equation (21) that the load flow rate is not proportional to the spool displacement, and the load flow rate is a nonlinear function of two variables, the spool displacement and the load pressure.

In order to have a linear proportional relationship between the load flow and the spool displacement, a nonlinear compensation function h(p_L) is introduced before the nonlinear model shown in Figure 6. After the hydraulic system undergoes nonlinear flow compensation, it can linearize the flow nonlinearity equation. The nonlinear compensation method is shown in Figure 7.

In this case, the spool displacement of the servo valve is

$$x_v^{*}=h\left(p_L\right)x_v$$

(22)

where $x_v^{*}$ is the spool displacement after nonlinear flow compensation.

We define the flow nonlinear compensation function h(p_L) as

$$h(p_L)=\sqrt{\frac{p_s}{p_s-\mathrm{sign}(x_v)p_L}}$$

(23)

This nonlinear compensation function is related to the load pressure. The load pressure p_L can be measured by a pressure sensor. The servo valve flow rate after nonlinear flow compensation is:

$$q_L=C_{d}w{x}_v^{*}\sqrt{\frac{p_s-\mathrm{sign}(x_v^{*})p_L}{\rho }}=C_{d}w{x}_v\sqrt{\frac{p_s}{\rho }}$$

(24)

Comparing Equation (24) with Equations (17), (20) and (21), it can be seen that there is a linear relationship between the compensated servo valve flow rate and the spool displacement.

When the moving direction of the hydraulic system changes, the flow gain will change abruptly due to the nonlinearity of the servo valve [24], making the acceleration feedback signal distorted, and this phenomenon is more obvious when the frequency is low. When the frequency is higher, the acceleration is larger, the inertial force gradually plays a dominant role, and the acceleration distortion phenomenon will be relatively weakened. After adopting the flow nonlinear compensation strategy, the flow gain is independent of the load pressure and can be regarded as a constant, and there is no obvious distortion in the feedback signal of acceleration for either low- or high-frequency motion. In the actual electrohydraulic servo system, in addition to the flow nonlinearity, other nonlinearities of the hydraulic system, such as magnetic loops, dead zones, and Coulomb friction, also have adverse effects on the system, so the actual system will still have some acceleration distortion after using the flow nonlinearity compensation.

Because of the effect of servo valve nonlinearity, the hydraulic system has a large signal tracking error for acceleration. As shown in Figure 8, the hydraulic system tracks sinusoidal signals with frequencies of 5, 10, 20, and 30 Hz, and the tracking curves of acceleration are compared. From Figure 9, it can be seen that the nonlinear compensation can effectively reduce the tracking distortion of the hydraulic system for the acceleration signal. Furthermore, as the signal frequency increases, the compensation effect is more obvious.

3.3. Three-State Feedback Control

From Equations (19) and (22) to Equation (24), the open-loop transfer function of the single-degree-of-freedom electrohydraulic displacement servo control system can be obtained as

$$\frac{L}{U}=\frac{k_{sh}}{s(\frac{s^2}{{\omega }_h^2}+\frac{2{\zeta }_h}{{\omega }_h}s+1)}$$

(25)

where the system open-loop gain $k_{sh}=k_{sv}{k}_q/A$, ${\omega }_h$ is the hydraulic servo system inherent frequency, and ${\zeta }_h$ is the hydraulic servo system damping ratio.

The inherent frequency ${\omega }_h$ and damping ratio ${\zeta }_h$ of the hydraulic servo system are expressed as

$${\omega }_h=\sqrt{\frac{4{\beta }_e{A}^2}{V_t{m}_l}}$$

(26)

$${\zeta }_h=\frac{k_{ce}}{A}\sqrt{\frac{{\beta }_e{m}_l}{V_t}}$$

(27)

where $k_{ce}=k_c+c_{tc}$ is the total flow pressure coefficient.

Typically, hydraulic damping ratios of 0.15 to 0.3 are difficult for an uncorrected electrohydraulic servo system using only proportional link control to make the system wider than ${\omega }_h$. Common methods for improving the damping ratio in the design of the physical structure of hydraulic systems include the use of positive opening valves, bypass leakage, and increasing the viscous damping of the load [25].

In this paper, a three-state feedback control strategy is used to adjust the system parameters. The three states refer to the displacement, velocity, and acceleration of the hydraulic cylinder piston. There are more design methods for three-state feedback control, such as using the root locus method [26] and the pole-placement method [27] to design the feedback controller. In this section, the three-state control strategy of the electrohydraulic servo system is designed based on the state feedback in the state space.

The state variable g of the electrohydraulic servo system can be expressed as

$$\mathit{g}={\left[\begin{array}{ccc}l& \dot{l}& \ddot{l}\end{array}\right]}^{\mathrm{T}}$$

(28)

According to Equations (25)–(27), the equation of state of the system can be expressed as

$$\{\begin{array}{l}{\dot{g}}_1=g_2\hfill \\ {\dot{g}}_2=g_3\hfill \\ {\dot{g}}_3={\omega }_h^2{k}_{sh}u-{\omega }_h^2{g}_2-2{\zeta }_h{\omega }_h{g}_3\hfill \end{array}$$

(29)

Converting Equation (29) into the form of a matrix differential equation gives

$$\dot{\mathit{g}}=\mathit{A}\mathit{g}+\mathit{B}u$$

(30)

$$\mathit{A}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& -{\omega }_h^2& -2{\zeta }_h{\omega }_h\end{array}\right]$$

(31)

$$\mathit{B}={\left[\begin{array}{ccc}0& 0& {\omega }_h^2{k}_{sh}\end{array}\right]}^{\mathrm{T}}$$

(32)

Obviously, since the coefficient matrices A and B of the state equations belong to the standard controllable matrices, arbitrary configurations of the poles can be achieved through state feedback.

The output equation of the system is

$$l={\mathit{C}}_{\mathit{g}}\mathit{g}$$

(33)

$${\mathit{C}}_{\mathit{g}}=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]$$

(34)

The state feedback matrix of the system can be expressed as

$${\mathit{K}}_{\mathit{f}}=\left[\begin{array}{ccc}{k}_{df}& k_{vf}& k_{af}\end{array}\right]$$

(35)

After three-state feedback adjustment, the system can be represented as a block diagram, as shown in Figure 10.

The state equation of the adjusted system is

$$u=k_{df}r-{\mathit{K}}_{\mathit{f}}\mathit{g}$$

(36)

$$\dot{\mathit{g}}=\left(\mathit{A}-\mathit{B}{\mathit{K}}_{\mathit{f}}\right)\mathit{g}+\mathit{B}{k}_{df}r$$

(37)

After the state feedback modification, the coefficient matrix of the state equation is

$$\mathit{A}-\mathit{B}{\mathit{K}}_{\mathit{f}}=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ -k_{df}{k}_{sh}{\omega }_h^2& -{\omega }_h^2-k_{vf}{k}_{sh}{\omega }_h^2& -2{\zeta }_h{\omega }_h-k_{af}{k}_{sh}{\omega }_h^2\end{array}\right]$$

(38)

The characteristic equation of the coefficient matrix is

$$\left|s\mathit{I}-\left(\mathit{A}-\mathit{B}{\mathit{K}}_{\mathit{f}}\right)\right|=\left|\begin{array}{ccc}s& -1& 0\\ 0& s& -1\\ k_{df}{k}_{sh}{\omega }_h^2& {\omega }_h^2+k_{vf}{k}_{sh}{\omega }_h^2& s+2{\zeta }_h{\omega }_h+k_{af}{k}_{sh}{\omega }_h^2\end{array}\right|=s^3+\left(2{\zeta }_h{\omega }_h+k_{af}{k}_{sh}{\omega }_h^2\right)s^2+\left({\omega }_h^2+k_{vf}{k}_{sh}{\omega }_h^2\right)s+k_{df}{k}_{sh}{\omega }_h^2$$

(39)

According to the characteristic equation, after three-state feedback, the stable condition of the system is:

$$\left(2{\zeta }_h{\omega }_h+k_{af}{k}_{sh}{\omega }_h^2\right)\left({\omega }_h^2+k_{vf}{k}_{sh}{\omega }_h^2\right)>k_{df}{k}_{sh}{\omega }_h^2$$

(40)

In order to make the three-state feedback system have better dynamic characteristics, the closed-loop amplitude–frequency characteristics need to be close to one in the widest possible frequency band. It is easy to prove that for a third-order system, such an ideal closed-loop transfer function should have the following typical form [28]:

$$G_3\left(s\right)=\frac{1}{\frac{s^3}{{\omega }_{nc}^3}+2\frac{s^2}{{\omega }_{nc}^2}+2\frac{s}{{\omega }_{nc}}+1}$$

(41)

where ${\omega }_{nc}$ is the integrated frequency of the ideal third-order transfer function.

The characteristic equation of this equation is

$$s^3+2{\omega }_{nc}{s}^2+2{\omega }_{nc}^{2}s+{\omega }_{nc}^3=0$$

(42)

Comparing with Equation (39), the following relationship is obtained:

$$\{\begin{array}{l}2{\omega }_{nc}=2{\zeta }_h{\omega }_h+k_{af}{k}_{sh}{\omega }_h^2\hfill \\ 2{\omega }_{nc}^2={\omega }_h^2+k_{vf}{k}_{sh}{\omega }_h^2\hfill \\ {\omega }_{nc}^3=k_{df}{k}_{sh}{\omega }_h^2\hfill \end{array}$$

(43)

Although the electrohydraulic servo system with three-state feedback can theoretically achieve any configuration of poles, the actual system’s inherent frequency and damping ratio cannot be increased indefinitely, and are limited by the dynamic characteristics of the servo amplifier, servo valve, and sensors. The difference between the inherent frequency of the servo valve and the inherent frequency of the dynamic mechanism determines the magnitude of the inherent frequency and damping ratio that can be increased by the three-state feedback.

Figure 11 shows the closed-loop frequency characteristics and step response under PID control and three-state feedback control, respectively. It should be noted that after the three-state feedback, there is no obvious resonance peak at the intrinsic frequency of the servo valve, and the bandwidth of the system is further improved. From the step response waveform, it can be seen that the number of oscillations of the system becomes less after using the three-state feedback, which means that the damping ratio of the system becomes larger under the three-state feedback.

4. Force Tracking Control Scheme

According to the working requirements of the rocket umbilical docking system, when docking with the rocket analog interface or following its movement, contact force will be generated at the interface; at this time the simple position control cannot guarantee the requirements of the contact force, and the force controller must be designed to realize the control of the contact force during the movement to make it meet the requirements. Aiming at this problem, an adaptive impedance control algorithm based on an online environment estimation is proposed to achieve compliance control during and following contact [29].

4.1. Analysis and Design of Impedance Controller

When performing a task with an impedance control strategy, the robot adjusts the impedance controller parameters to achieve stable contact and desired force control based on the relationship between the end position and the force [30]. In practical engineering applications, the impedance relationship between the robot end and position deviation is generally established in the form of a second-order differential equation with the mathematical expression of the desired impedance model:

$${\mathit{M}}_d(\stackrel{••}{\mathit{X}}-\stackrel{••}{{\mathit{X}}_d})+{\mathit{B}}_d(\stackrel{•}{\mathit{X}}-\stackrel{•}{{\mathit{X}}_d})+{\mathit{K}}_d(\mathit{X}-{\mathit{X}}_d)=-{\mathit{F}}_e$$

(44)

where ${\mathit{M}}_d$, ${\mathit{B}}_d$, ${\mathit{K}}_d$ are the desired inertia matrix, damping matrix, and stiffness matrix; $\mathit{X}$, $\dot{\mathit{X}}$, $\ddot{\mathit{X}}$ are position, velocity, and acceleration vectors of the robot end-effector; ${\mathit{R}}_d$ is the desired trajectory of robot end-effector; and ${\mathit{F}}_e$ is the robot and environment contact force vector.

As shown in Figure 12, to enable force tracking, the desired contact force and the actual contact force error signal $E=F_d-F_e$ can be used as the driving force of the desired impedance model, so the expression of the new desired impedance model is

$${\mathit{M}}_d(\stackrel{••}{\mathit{X}}-\stackrel{••}{{\mathit{X}}_d})+{\mathit{B}}_d(\stackrel{•}{\mathit{X}}-\stackrel{•}{{\mathit{X}}_d})+{\mathit{K}}_d(\mathit{X}-{\mathit{X}}_d)=\mathit{E}$$

(45)

In Cartesian coordinates, the mass, damping, and stiffness matrices are generally in the form of diagonal matrices for low frequency kinematic systems if modal space coupling is not considered. In order to reduce the difficulty of analysis, we can use one-dimensional variables $f_d$, $f_e$, $m_d$, $b_d$, $k_d$ to replace the multidimensional variables $F_d$, $F_e$, $M_d$, $B_d$, $K_d$ when analyzing a single-degree-of-freedom model.

The impedance model can be represented as a mass–spring–damping system. In free motion, the robot is an independent controlled object; when in contact with the environment, it can be seen as an integrated dynamic system consisting of the robot end and the external environment. At this point, the environmental dynamics model needs to be considered, considering only the environmental stiffness factor, and its dynamics model is

$$f_e=k_e(x-x_e)$$

(46)

When the environmental damping factor is considered, the dynamics is modeled as

$$f_e=b_e\stackrel{.}{x}+k_e(x-x_e)$$

(47)

4.2. Stability Analysis of Impedance Control

A block diagram of a single degree of freedom transfer function based on impedance control is shown in Figure 12. It consists of two loops: the position inner loop servo control and the force outer loop based on impedance control.

This impedance-based control system is not only to design the structural form of the impedance control model, but also to make reasonable adjustments to the parameters of the target impedance model to ensure stable contact with the external environment, and the control performance of the contact force.

The environment is deformed by contact with the robot end effector, and the linearized relationship between force and motion established at the point of contact can be described as

$$f=m_e\ddot{p}+c_e\dot{p}+k_e$$

(48)

where $p=x-x_e$ is the actual deformation of the robot end in contact with the environment, $x_e$ is the position of the contact point, $m_e$, $c_e$, and $k_e$ are the inertia, damping, and stiffness of the environment, respectively.

The key to impedance control is to maintain the relationship between contact force and position error. Taking into account the multiple degrees of freedom of the robot, the impedance controller can be expressed as

$$-\mathit{F}={\mathit{M}}_d\ddot{e}+{\mathit{B}}_d\dot{e}+{\mathit{K}}_d$$

(49)

where $e=x_d-x$ is the position error; and ${\mathit{M}}_d$, ${\mathit{B}}_d$, and ${\mathit{K}}_d$ are the inertia matrix, damping matrix, and stiffness matrix of the impedance model, respectively.

From this equation, the end-effector displacement after contact occurs can be expressed as

$$\mathit{X}={\mathit{G}}_p(s){\mathit{X}}_d+{\mathit{G}}_d{(s)}^{-1}\mathit{F}$$

(50)

Substituting Equation (49) into Equation (50) we obtain

$$\mathit{F}(s)={[{\mathit{G}}_d{(s)}^{-1}+{\mathit{G}}_e{(s)}^{-1}]}^{-1}{\mathit{G}}_p(s){\mathit{p}}_0(s)={\mathit{Z}}_C(s){\mathit{p}}_0(s)$$

(51)

For how to define the necessary and sufficient conditions for impedance control under stable force/position tracking, scholars agree with the following statements [31]. In fact, if the robot does not break away from the environment during the process of contact with the environment, it can be considered that the process of overcontact is stable. A stable contact process should be at the end of the robot so that the deformation of the environment is always positive, and there should be no large jumps. The most critical issue is the rigid collision between the robot end and the environment. The controller should ensure that the end of the robot stays in contact with the environment from the beginning of the collision contact until the collision kinetic energy is completely absorbed.

Assuming that the actual deformation of the environment p is always positive (compression is positive), it can be expressed as

$$|x(t;t_0,x_{t0})|>\left|x_e\right|$$

(52)

where the end effector is always in contact with the environment when $t\ge t_0$. At time $t_0$, the end-effector begins to come into contact with the environment, $x_{t0}=x_e$. It can also be equivalently expressed that there is always a contact force $F(t)\ge 0$ in the direction of contact.

We can use this stability criterion to calculate the conditions that the system parameters should satisfy when the impedance control is stable. The environmental deformation p should always be positive during the contact action, then the corresponding impedance transfer function is a positive real transfer function. The positive real transfer function can describe the behavior of the passive mechanical system, that is, the force that keeps the system in stable contact is always applied by the external environment. In addition to this, to obtain a relatively simple impedance control transfer function, the position servo system is assumed to be ideal, ${\mathit{G}}_P=\mathit{I}$. Since both the target impedance transfer function and the environmental dynamics transfer function are assumed to be Hurwitz polynomials, the necessary conditions for contact stability in the constrained direction can be expressed as

$$R_e\{{\mathit{G}}_d(s)/[{\mathit{G}}_d(s)+{\mathit{G}}_e(s)]\}\ge 0 s=j\omega ,\omega \in [0,\infty ]$$

(53)

From these equations, it can be concluded that the impedance control system should satisfy the following parameters when it is stable:

$$\{\begin{array}{l}{\zeta }_d=\frac{b_d}{2\sqrt{m_d{k}_d}}\\ \kappa =\frac{k_e}{k_d}\\ {\xi }_d\ge 0.5(\sqrt{1+\kappa }-1)\end{array}$$

(54)

4.3. Force Tracking Error Analysis

The collision dynamics model of the robot end in contact with the environment is shown in Figure 13. The variable $x$ and $x_d$ denote the actual trajectory and desired trajectory of the robot end, respectively, $f$ denotes the actual contact force, and the environmental dynamics model is simplified to a linear spring, so the contact force can be simply expressed as

$$f=\{\begin{array}{ll}{k}_e(x-x_e)& (x\ge x_e)\\ 0& (x<x_e)\end{array}$$

(55)

where $k_e$ is equivalent stiffness of environmental and end-effector force sensors, and $x_e$ is the environmental position when no deformation occurs.

Setting the force error $e=f_r-f_e$, it can be derived from the environmental dynamics model that

$$x=\frac{f_e}{k_e}+x_e=\frac{1}{k_e}(f_r-e)+x_e$$

(56)

In order to ensure that the expected force f is constant, the deformation of the environment by the expected trajectory must also be constant, so we have ${\ddot{x}}_d={\dot{x}}_d=0$. Substituting Equation (56) into the target impedance model of Equation (45), the differential equation of the force error is obtained

$$m_{d}e+b\stackrel{.}{e}+(k_d+k_e)=m_d\stackrel{..}{f_r}+b_d\stackrel{.}{f_r}+k_d\stackrel{}{f_r}-k_d{k}_e(\stackrel{}{x_d}-\stackrel{}{x_e})$$

(57)

Assuming that $f_r$ is a constant value, then ${\ddot{f}}_r={\dot{f}}_r=0$, the steady-state force error is

$$e_{ss}=\frac{k_d}{k_d+k_e}[\stackrel{}{f_r}+k_e(\stackrel{}{x_e}-\stackrel{}{x_d})]=k_{e\mathit{q}}(\frac{f_r}{k_e}+x_e-x_d)$$

(58)

The equivalent stiffness of control system and environment is

$$k_{e\mathit{q}}=\frac{k_d{k}_e}{k_d+k_e}$$

(59)

Then the steady-state contact force with the environment is

$$f_{ss}=f_r-e_{ss}=k_{e\mathit{q}}(\frac{f_r}{k_d}+x_d-x_e)$$

(60)

From Equation (60), the steady-state force error $e_{ss}$ is a function of the desired force $f_r$ and the desired position $x_d$. Therefore, the ideal desired trajectory is

$$x_d=\frac{f_r}{k_d}+x_e$$

(61)

At this time, the steady-state force error $e_{ss}$ is zero. It can be seen that if the environment position $x_e$ and stiffness k can be accurately known, then an accurate desired trajectory $x_d$ can be given according to Equation (61), so that the contact force $f$ between the robot end and the environment can accurately track the desired force $f_r$. According to Equation (60), the deformation caused by the interaction between the desired trajectory $x_d$ and the environment is $\frac{f_r}{k_d}$, so as to ensure the desired contact force $f_r$.

In fact, the environmental parameters $x_e$ and $f_e$ are generally unknown. $\mathsf{\Delta }{x}_e$ represents the difference between the actual and estimated environmental position, and $\mathsf{\Delta }{k}_e$ represents the difference between the actual and estimated stiffness, then we can derive that

$$\begin{array}{l}\mathsf{\Delta }{x}_e=x_e-{\tilde{x}}_e\\ \mathsf{\Delta }{k}_e=k_e-{\tilde{k}}_e\end{array}$$

(62)

where $x_e$ and $k_e$ are actual values of environmental position and stiffness, and ${\tilde{x}}_e$ and ${\tilde{k}}_e$ are the estimates of environmental position and stiffness.

The steady-state force error at the desired position is obtained from the equation $x_d={\tilde{x}}_e+\frac{f_r}{{\tilde{k}}_e}$ as

$$e_{ss}=\frac{k_d}{k_d+k_e}(k_e\mathsf{\Delta }{x}_e-\frac{\mathsf{\Delta }{k}_e}{{\tilde{k}}_e}\stackrel{}{f_r})$$

(63)

The force steady-state error Equation (63) shows that it is difficult to accurately estimate the environmental position $x_e$, resulting in the presence of $\mathsf{\Delta }{x}_e$ and because the environmental stiffness value $k_e$ is usually large, resulting in a large value of the steady-state force error $e_{ss}$. This makes it inevitably difficult to ensure that the contact force between the robot and the environment stably tracks the given desired force.

4.4. Impedance Control Based on Online Estimation of Environmental Parameters

From the analysis above, it can be concluded that if the stiffness and deformation of the environment can be obtained, then impedance control can be well implemented to track the specific force; however, the parameters of the environment are often unknown and time-varying under actual working conditions [32]. In this section, we design an online estimation controller for environmental parameters based on the second method of Lyapunov stability theory. The second method is a criterion established from the energy point of view [33]. If the energy inside the system decays with time, it can be assumed that the system will reach the stationary state sooner or later, and the system is stable at that time.

This section focuses on estimating the environmental parameters $k_e$ and $x_e$ online in real time, and then solving the desired trajectory $x_d$ in real time. From the previous section, it can be seen that if $k_e$ and $x_e$ can be known precisely, the desired trajectory $x_d$ can be set as

$$x_d=x_e+\frac{f_r}{k_e}$$

(64)

Since the displacement and deformation of the environment cannot be measured directly, the desired trajectory x_d can be set as

$$x_d={\tilde{x}}_e+\frac{f_r}{{\tilde{k}}_e}$$

(65)

where ${\tilde{x}}_e$ and ${\tilde{k}}_e$ are the estimated values of $x_e$ and $k_e$, respectively.

The following relationship can be derived from Equation (55):

$$f=k_e(x-x_e)=k_{e}x-k_x$$

(66)

$$\tilde{f}={\tilde{k}}_e(x-{\tilde{x}}_e)={\tilde{k}}_{e}x-{\tilde{k}}_x$$

(67)

where ${\tilde{k}}_x={\tilde{k}}_e{\tilde{x}}_e$ is the estimated value of $k_x=k_e{x}_e$, which can also be seen as a predicted displacement value based on the estimated parameters of the current environment.

Set the difference of the contact force as

$$\tilde{f}-f=[x - 1]\varphi$$

(68)

The main goal is to design a control strategy that can make $\tilde{f}$ converge to $f$ when time t tends to infinity by continuously adjusting ${\tilde{k}}_e$ and ${\tilde{k}}_x$ based on the prediction force error $\tilde{f}-f$. The following section derives the conditions to achieve $\tilde{f}$ convergence to $f$.

According to Equations (55) and (65), the following can be derived:

$$f={\tilde{k}}_e(x-{\tilde{x}}_e)={\tilde{k}}_e(x-x_d+\frac{f_r}{{\tilde{k}}_e})=-{\tilde{k}}_e\left(x_d-x\right)+f_r$$

(69)

Knowing from the target impedance, Equation (45), Equation (55) is used to eliminate $(x_d-x)$. In addition, note that if $\tilde{f}=f$, then k is a constant value, and we can get

$$m_d\stackrel{..}{e}+b_{}\stackrel{.}{e}+(k_d+{\tilde{k}}_e)e=0$$

(70)

Thus, if $\tilde{f}\to f$, then $f\to f_r$ or ${\tilde{k}}_e\to -K$. Obviously, the environmental stiffness value should be a positive value; the latter is not true. Therefore, it can be concluded that if $\tilde{f}\to f$, then $f\to f_r$.

Therefore, in this paper, we develop a Lyapunov-based algorithm to update the estimated parameters ${\tilde{k}}_e$ and ${\tilde{k}}_x$, thus the contact force $f$ will eventually converge to $f_r$. Define the Lyapunov function as

$$V={\varphi }^{\mathit{T}}\mathsf{\Gamma }\varphi$$

(71)

where $\mathsf{\Gamma }$ is a second-order positive definite real symmetric matrix. We assume that $\varphi$ can be written as

$$\stackrel{.}{\varphi }=-{\mathsf{\Gamma }}^{-1}\left[\begin{array}{l} x\\ -1\end{array}\right]\left(\tilde{f}-f\right)$$

(72)

Then, from Equations (71) and (72), we obtain

$$\stackrel{.}{V}=2{\varphi }^{\mathit{T}}\mathsf{\Gamma }\stackrel{.}{\varphi }=-2{\varphi }^{\mathit{T}}\left[\begin{array}{l} x\\ -1\end{array}\right]\left(\tilde{f}-f\right)=-2{\left(\tilde{f}-f\right)}^2$$

(73)

It is obvious that $\stackrel{·}{V}(\varphi ,t)$ is negative semidefinite. If $\varphi$ is adjusted according to Equation (72), when $t\to \infty$, then from Equations (67) and (70) we can show that $f\to f_r$. From the definition, it is clear that

$$\varphi =\left[\begin{array}{l}{\varphi }_k\\ {\varphi }_x\end{array}\right]=\left[\begin{array}{l}{\tilde{k}}_e-k_e\\ {\tilde{k}}_x-k_x\end{array}\right]=\left[\begin{array}{l}{\tilde{k}}_e-k_e\\ {\tilde{k}}_e{\tilde{x}}_e-k_e{x}_e\end{array}\right]$$

(74)

$$\stackrel{.}{\varphi }=\left[\begin{array}{c}\stackrel{.}{{\tilde{k}}_e}\\ \stackrel{.}{{\tilde{k}}_e}{\tilde{x}}_e+{\tilde{k}}_e\stackrel{.}{{\tilde{x}}_e}\end{array}\right]=-{\mathsf{\Gamma }}^{-1}\left[\begin{array}{c}x\\ -1\end{array}\right]\left(\tilde{f}-f\right)$$

(75)

$$\left[\begin{array}{cc}1/{\gamma }_1& 0\\ 0& 1/{\gamma }_2\end{array}\right]\left[\begin{array}{c}{\dot{\tilde{k}}}_e\\ {\dot{\tilde{k}}}_e{\tilde{x}}_e+{\tilde{k}}_e{\dot{\tilde{x}}}_e\end{array}\right]=\left[\begin{array}{c}-x\\ 1\end{array}\right]\left(\tilde{f}-f\right)$$

(76)

$$\mathsf{\Gamma }=\left[\begin{array}{cc}1/{\gamma }_1& 0\\ 0& 1/{\gamma }_2\end{array}\right]$$

(77)

The parameters ${\gamma }_1$ and ${\gamma }_2$ are positive real constants. From this analysis, it can be concluded that

$$\stackrel{.}{{\tilde{k}}_e}=-{\gamma }_{1}x\left(\tilde{f}-f\right)$$

(78)

$$\stackrel{.}{{\tilde{x}}_e}=\frac{\left(\tilde{f}-f\right)}{{\tilde{k}}_e}({\gamma }_{1}x{\tilde{x}}_e+{\gamma }_2)$$

(79)

Therefore, the force control algorithm based on online parameter estimation can be written as

$$x_d={\tilde{x}}_e+\frac{f_r}{{\tilde{k}}_e}$$

(80)

$${\tilde{k}}_e(t)={\tilde{k}}_e(0)-{\gamma }_1{\displaystyle {\int }_0^{t}x(\tilde{f}-f)}dt$$

(81)

$${\tilde{x}}_e(t)={\tilde{x}}_e(0)+{\gamma }_1{\displaystyle {\int }_0^t\frac{(\tilde{f}-f)}{{\tilde{k}}_e}(x{\tilde{x}}_e+\frac{{\gamma }_2}{{\gamma }_1})}dt$$

(82)

This is our design of adaptive impedance control based on the estimation of online environmental parameters using Lyapunov’s second method. The method above uses the parameters ${\tilde{x}}_e$ and ${\tilde{k}}_e$ derived from the online estimation to compute the desired trajectory x_d by equivalently replacing the actual environmental parameters $x_e$ and $k_e$. The previous analysis also demonstrates that when $t\to \infty$, then $f\to f_r$, $x_e$ and $k_e$ is independent of whether the parameters x_e and $k_e$ are known.

4.5. Simulation of Force Control Based on Online Environmental Parameter Estimation

The simulation conditions are set to consider only the environmental stiffness in which the actual stiffness of the simulation environment is $k_e=\mathrm{10,000}$ N/m, the actual position is x_e = 0.05 m, and the desired contact force is f_r = 10 N. The environmental parameters are initially estimated to be ${\tilde{x}}_e\left(0\right)=0.048$ m and ${\tilde{k}}_e\left(0\right)=\mathrm{10,000}$ N/m, and the two controller parameters are ${\gamma }_1=400$ and ${\gamma }_2=3.4$; the results are shown in Figure 14.

From Figure 15 and Figure 16, the estimated values of the environmental parameters at steady state of the system are

$${\tilde{x}}_e=0.0501 \mathrm{m},{\tilde{k}}_e=\mathrm{10,891} \mathrm{N}/\mathrm{m}$$

(83)

It can be seen that although the expected contact force is well tracked, the results of online parameter estimation have a certain deviation from the actual parameters, which have a strong relationship with the initial value of the variable. The main reason is that we regard the convergence of the prediction deviation $\tilde{f}\to f$ as the control objective. Therefore, this control strategy can approximate the desired trajectory when combining the environmental position and the stiffness parameter value.

4.6. Overall Control Loop

As shown in Figure 17, the entire control system consists of a position-controlled inner loop and force-controlled outer loop, wherein the inner loop is designed to ensure the system accuracy and rapidity of the system to track a specific movement with a high sample rate (500 Hz) linear variable differential transformer (LVDT); the outer control loop computes an appropriate trajectory to approach the target object based on the fusion of 6-dof force sensor (100 Hz) and vision sensor (25 Hz).

The entire experiment consists of four sections: approach process, docking and inserting process, tracking process and separate process. Throughout the entire experiment, the system controller calculates the reference trajectory of the parallel manipulator based on relative position feedback and interaction force information. As shown in Figure 17, the motion planning module is designed to plan the whole experiment process and calculate the manipulator’s path based on finite state machine theory.

To ensure the safety and reliability of the manipulator’s interacting process, the manipulator is designed to approach the target in a smooth trajectory. The constraint conditions are defined as

$$\mathrm{a}. \mathrm{position} \mathrm{constraint}: z(t_0)=z_0, z(t_0+\mathsf{\Delta }t)=0$$

(84)

$$\mathrm{b}. \mathrm{speed} \mathrm{constraint}: v(t_0)=0, v(t_0+\mathsf{\Delta }t)=0$$

(85)

$$\mathrm{c}. \mathrm{acceleration} \mathrm{constraint}: a(t_0)=0, a(t_0+\mathsf{\Delta }t)=0$$

(86)

where t₀ is system initial time, $\mathsf{\Delta }t$ is system control period, and z₀ is the distance between the manipulator and the tracking target along z direction.

In this paper, the fifth-order polynomial interpolation is used to obtain the moving trajectory that meets the constraint requirement. The trajectory equation can be described as

$$z(t)=z_0+\frac{10}{\mathsf{\Delta }{t}^3}(0-z_0){(t-t_0)}^3-\frac{15}{\mathsf{\Delta }{t}^4}(0-z_0){(t-t_0)}^4+\frac{6}{\mathsf{\Delta }{t}^5}(0-z_0){(t-t_0)}^5$$

(87)

As shown in Equation (87), the fifth-order polynomial interpolation generates a rather smooth trajectory that has a continuous function of speed and acceleration, thus the impact force interacting with the environment can be reduced as much as possible. It is necessary to calculate the minimum range of the variable $\mathsf{\Delta }t$ to determine the algorithm’s running time, which can generally be obtained by the optimization algorithm.

In the process, the hydraulic parallel manipulator approaches the target, when there are obstacles between the mechanism and the target, or the relative position and speed deviation are too large for normal docking, the path planning module will stop approaching the target to prevent damage to the target.

5. Experimental Result

5.1. Experiment System Introduction

The overall experiment system includes two subsystems as shown in Figure 18. One part consists of the hydraulic parallel manipulator and the system controller, which includes a hydraulic power manager and motion controller. The other part of the rocket simulation system includes the motion simulator controller and rocket motion simulator. The rocket motion simulator is used to move the target object along x–y–z, the three orthogonal directions of Cartesian coordinate, in order to simulate the rocket’s realistic motion in the environment, and the simulator manager PC2 is employed to control the docking device to realize tracking, locking, unlocking, and other tasks with the pose information acquired from the vision sensor.

The controllers of the two subsystems are both based on an NI PXI-8196 embedded controller which communicates with the CAN bus. The vision sensor system is composed of a TFN-0130 laser rangefinder with 1 mm measuring accuracy and a SONY Super HAD CCD camera with 658 × 492 image resolution. To measure the interact force during the entire contact process, we employ a 6-dof force plate named BERTEC FP9090, which has a full scale of 2000 N force in the x direction, 10,000 N force in the y and z direction, and 4500 Nm torque around the x direction.

The docking device on the end-effector is equipped with a 97 mm length guide pin while the target object also has two guide holes with special designed chamfers. Each joint of the manipulator is actuated by a hydraulic cylinder with a 450 mm stroke and 66 mm cylinder diameter. The rated pressure of the hydraulic power source is 14 MPa. In consideration of the actuator’s maximum displacement the hydraulic cushioning, the workplace of the parallel manipulator is ±250 mm in the x direction and ±380 mm in the y and z direction. The maximum range of motion and speed of the target movement simulator is ±120 and ±100 mm/s, respectively.

To validate the effectiveness of the proposed algorithm, several dynamic experiments were conducted. First, a dynamic tracking experiment was conducted to evaluate the manipulator’s capability of following a specified trajectory based on hydraulic servo control. Next, an entire dynamic docking experiment was performed to verify the force-tracking control scheme validity.

5.2. Dynamic Tracking Experiment

The main purpose of this experiment was to research the dynamic target tracking performance of the manipulator and validate the inner position control loop (presented in Section 4.1) to prepare for the next state dynamic docking experiment.

The target’s motion simulator is driven by a stepper motor for triangular wave motion, the amplitude of the triangular wave is ±100 mm, and the moving speed is 100 mm/s in y direction and 80 mm/s in z direction. At the beginning of the experiment, the parallel manipulator was located in its initial position and the midpoint of the triangular wave was x = 0, y = 0, z = 120 mm. The experimental data are shown in Figure 19. Figure 19a,b illustrates the dynamic position tracking data, including the desire and measured position in the x and y direction; Figure 19c illustrates the inertial force caused by the manipulator movement measured by a force sensor plate mounted on the end effector.

In the entire experiment, the relative position error in the y and z directions is comparatively small to ensure successful docking, as shown in Figure 19a,b. Due to the influence of inertial force, force information measured by the force sensor plate presents a triangular wave with relatively small noise. The given positions and the feedback position calculated by the forward kinematics solution are illustrated in Figure 19c. From the position and force information during the tracking process, we can confirm the tracking accuracy of the parallel manipulator and verify the effectiveness of the hydraulic servo control scheme proposed in Section 3.

5.3. Dynamic Inserting Experiment

The purpose of this experiment is to verify the effectiveness of the position/force hybrid control algorithm implemented by the parallel manipulator and the vision system. The entire dynamic docking experiment process is divided into five stages: preparation, approach, docking, following, and returning.

In this experiment, the motion simulator carrying the target object is actuated by stepping motors moving in a triangular wave along the y and z directions. The amplitudes of the triangular wave in two directions are both ±100 mm, while the velocity of the target movement is ±50 mm/s along the y direction and ±40 mm/s along the z direction. In the initial stage of the experiment, the manipulator is located in its initial zero position and the target object is located $P_1=\left[133 , -26, -93\right]$ in Cartesian coordinates. Then, the experiment enters the next stage and the manipulator begins to track the moving target object in the y and z directions. When the feedback relative position errors in the y and z directions are both less than 10 mm, moving forward command along the x direction generated by the trajectory planning algorithm is given to the manipulator. If the relative distance of the manipulator and the target object is less than 150 mm, the boundary value in which the manipulator is allowed to keep going rises to 7 mm, which will guarantee docking security.

The relative position and force information obtained in the dynamic docking experiment are demonstrated in Figure 20. In the beginning part of the tracking stage, the relative position errors decrease quickly and get into the specific range at 15 s, as shown in Figure 20b. Then, the manipulator approaches the object target smoothly and continuously until the 50 s, when the end effector starts to contact the docking device. From Figure 20c, we can see that the contact force is well controlled under ±150 N in the position/force hybrid control structure, and we can also see that during the tracking stage from 15 to 65 s, the relative position along the x direction controlled by the trajectory generator is comparatively smooth. Experimental results show the relatively favorable efficiency in the design of proposed method.

6. Conclusions

In this paper, a novel automated umbilical mating system based on a hydraulic 6-dof parallel manipulator is proposed to realize the umbilical docking task during the process of rocket propellant loading. A hybrid control strategy of vision, force, and position is proposed, which integrates the three phases of approach, docking, and following in the entire loading process into the same framework, avoiding the impact of switching different control modes on the system stability. The automated docking device adopts a fusion of laser range finder and monocular camera to locate the moving fuel port and guide the automated docking system approaching the rocket fuel port. A hybrid force/position control is applied in the inner loop of the control system to ensure that the contact forces during docking and follow-through remain within a certain range without damaging the rocket. To address the characteristics of the hydraulic parallel mechanism employed in the automated docking device, the nonlinear compensation and gravity compensation are used in the analysis to enhance the dynamic performance of the robot.

A series of experiments were conducted for different working phases of the automated umbilical system. The results show the effectiveness of the proposed algorithm and that the automated docking system has sufficient accuracy and compliance to achieve the working requirements. In the dynamic tracking experiment, the manipulator’s ability to track a high-speed target object is illustrated. When the target object moves at a high speed, up to 50 mm/s, relative position error in the x direction can remain below 10 mm. A dynamic docking experiment is then presented to demonstrate the algorithm’s effectiveness to interact with a high-speed moving target, in which the manipulator is controlled to track and insert into guide holes while the interacting force is well controlled within 300 N.

Some aspects of the control algorithm presented need to be optimized to enhance the experimental performance. The tracking accuracy and contact force control depend on the vision system’s perception precision and resolution. In the next experiment plan, an advanced vision system with higher performance will be employed to measure the target position and orientation. In addition, a novel Kalman filter will be developed to eliminate the signal noise.