A Velocity-Based Impedance Control System for a Low Impact Docking Mechanism (LIDM)

In this paper, an impedance control algorithm based on velocity for capturing two low impact docking mechanisms (LIDMs) is presented. The main idea of this algorithm is to track desired forces when the position errors of two LIDMs are random by designing the relationship between the velocity and contact forces measured by a load sensing ring to achieve low impact docking. In this paper, the governing equation of an impedance controller between the deviation of forces and velocity is derived, and simulations are designed to verify how impedance parameters affect the control characteristics. The performance of the presented control algorithm is validated by using the MATLAB and ADAMS software for capturing simulations. The results of capturing simulations demonstrate that the impedance control algorithm can respond fast and has excellent robustness when the environmental errors are random, and the contact forces and torques satisfy the low impact requirements.


Introduction
In order to dock two vehicles using a conventional mechanical docking assembly, the vehicles must be pressed together with sufficient forces to re-align the misalignment of the soft capture ring [1]. The action of forcing two vehicles together, particularly in space, might result in damages to one or both of the vehicles or sensitive systems [2]. Thus, a type of docking mechanism which provides low impact mating (i.e., a low impact docking mechanism, abbreviated as LIDM) has been developed by NASA [3], ESA [4] and Chinese research institutions [5], respectively, in order to solve the problems noted above. In particularly, the LIDM has a reconfigurable control system [1], which permits a load sensing ring with an electromagnetic capture mechanism to perform a "soft" capture and mate two vehicles together. As a result, a specified desired force, the ideal contact force between two docking mechanisms, could be tracked during the capture process. Force tracking is the key to the success of capturing, which can be solved by compliance control. Impedance force tracking control is very practical in the field of robotic compliance control and the main concept is based on the impedance equation, which is the relationship between force and position/velocity error [6].
The impedance control technique proposed by Hogan [7] is one of the fundamental approaches for force tracking control of robot manipulators with constrained motion. Then, the performance of impedance control was improved and the application was expanded to other fields by many researchers [8][9][10]. Differing from the hybrid position and force control approach [11], impedance control regulates the force between a manipulator and the environment by defining the target impedance between position and contact force. The desired force is indirectly controlled by prespecifying a robot-desired displacement, which is determined by the stiffness and location of the environment [12]. One of the major practical difficulties with impedance control is that the environmental stiffness cannot be known precisely. Therefore accurate desired displacement cannot be designed to achieve accurate force control. In the past, many attempts have been made to solve this problem. Lasky and Hsia [13] employed a separate desired displacement modification control loop by using integral control. Lee [14] formulated the generalized impedance relationship between a motion error and a contact force error, and Seraji [15] generated a reference position using adaptive control. A neural network or fuzzy approach of force control was introduced to solve a number of uncertainty problems [16]. Besides, the accurate environment positions cannot be available in advance in impedance control, another difficulty for its implementation, and poorly estimated environmental information may cause poor force tracking results [17]. Especially for application of force control with random errors by applying a desired force, the exact estimation of environment position is difficult. Therefore, it is difficult to design an expected docking trajectory of two LIDMs, as the relative position errors of two vehicles are random.
In this paper, an impedance control algorithm based on velocity is proposed. The practical difficulties mentioned above of impedance control and the environmental stiffness that cannot be known precisely, are satisfactorily solved in this paper by introducing feedback information of contact forces and torques measured by force sensors. In addition, a desired velocity is introduced instead of the desired displacement since the precise desired displacement cannot be deduced from the position random errors between two LIDMs. A relational function between contact force and velocity of load sensing ring is designed in this algorithm to track desired force as well as the shape of contact surface. Besides, a filter is set in this algorithm as the control law of contact forces and torques of two LIDMs. The filter, which can directly affect each of the contact forces and torques, can affect the trajectory whereby the LIDM responds to the same external forces and torques by changing the filtering functions, and a group of suitable filtering functions may result in a better trajectory for the capturing process. In order to validate the algorithm, a series of simulations with MATLAB and ADAMS are presented. First of all, the governing equation between deviation of forces and velocity of an impedance controller is derived, and simulations are carried out to validate how impedance parameters affect the control characteristics. Then, the model of LIDM is built in ADAMS referring to the Low Impact Docking System of NASA, and a control module of LIDM is generated in ADAMS. Finally, the control system is built in MATLAB via the usage of the control module. The results of capturing simulations demonstrate that impedance control based on velocity is suitable for the LIDM, as well as, that the present algorithm is robust and the filter is necessary for the impedance control system.

Features of the LIDM and Dynamic Model
The LIDM comprises a six-DOF platform, a tunnel and a control subsystem [1,2]. As shown in Figure 1a, the six-DOF platform is composed of a load sensing ring, a base ring, one or more electromagnets, one or more striker plates, a plurality of actuators, and a plurality of alignment guides. The load sensing ring and the base ring are coupled together using several actuators, base connection points and upper connection points. Structurally, the load sensing ring is comprised of an annular outer face, an inner face and a variety of load cells as shown in Figure 1b. In addition, the six-DOF platform incorporates an active load sensing system so as to automatically and dynamically adjust the load ring during capture, instead of requiring significant force to push and realign the load ring. Unlike the mechanical trip latches that require a tripping force for capture, the LIDM uses electromagnets to achieve "soft" capture. Furthermore, the LIDM could also be controlled as a damper in lieu of interconnected linear actuators and separate load attenuation system, to eliminate the residual motion and dissipate the forces resulted from ramming two vehicles together. Therefore, the contact force and torque fluctuations can be maintained within a small range, and according to reference [18], it can be known that the maximum of contact forces and torques are not more than 450 N and 450 N·m, respectively. The dynamic model of the LIDM can be expressed as follows: where M(q), C(q), g(q) represent the inertia matrix, centrifugal term and gravity term respectively; q represents the six-DOF generalized coordinate vector of the load sensing ring; F, Ff and Fe represent the generalized driving force vector, generalized friction force vector and generalized external force vector respectively. In this paper, each generalized force is named "Force" below, which represents a six-dimensional vector and contains three forces and three torques along three axes of the coordinate. Similarly, the generalized displacement and the generalized velocity are named "Displacement" and "Velocity" respectively below. F is given from reference [19]: where J represents the Jacobian Matrix, f = [f1 f2 f3 f4 f5 f6] T represents the driving force matrix of actuators.
In addition, there should be a certain relationship between external force and load cells as described previously, which can be expressed as follows. Firstly, two coordinate frames are defined, namely O-XYZ and O1-X1Y1Z1, which are fixed to the base ring and the load sensing ring respectively as shown in Figure 1. Then, the coordinates of load cell connection points ai and Ai can be described in O1-X1Y1Z1. Through the screw theory [20], the Jacobian Matrix Js T and the external Force in O1-X1Y1Z1 can be obtained, as expressed below: where ai and Ai represent the coordinates of the upper and the base connection points of load cells separately in O1-X1Y1Z1, Fe' represents the external Force vector in O1-X1Y1Z1, fs is a vector composed of the values of six load cells. Thus, the external Force vector in O-XYZ can be described as follows: where R is the rotation matrix transformed from O1-X1Y1Z1 to O-XYZ, and can be expressed as: where α, β, θ represent the Euler angles about axes Z, Y, X respectively. According to Equations (2) and (5), the dynamic model of LIDM can be rewritten as:

The Flexible Model of LIDM
The LIDM is a rigid structure system, but when controlled by a force tracking control system, it can be treated as a flexible system with spring and damper characteristics. Therefore, the LIDM is supposed to be a mass-spring-damper system. The supposed model along one direction is shown in Figure 2, where md, kd, and bd represent the mass, stiffness and damping respectively, x and xd represent the actual displacement and the desired displacement respectively, fr and f represent the desired force and the external force respectively. The flexibility of supposed model is determined by parameters md, kd, and bd, which are selected based on Equation (6). According to the flexible model of LIDM, the governing equation of an impedance controller can be built as descried in next section.

Impedance Controller Based on Velocity
The impedance control is based on the concept that it is neither position nor force, but the dynamic relationship between them that should be controlled [6]. In this section, q is replaced by X for the purpose of mathematical tractability, where X represents the six-DOF generalized coordinate vector of load sensing ring. According to Figure 2, the relation is an impedance equation given by: where E represents the deviation of Force between the external Force and the desired Force, while E = Fe − Fr, in which Fr represents the desired Force; X and Xd represent the actual Displacement and the desired displacement, respectively; Md, Bd and Kd are respectively 6 × 6 constant-positive-diagonal matrices of desired inertial, damping and stiffness. Apparently, a desired Displacement is necessary for an impedance control system based on the position from Equation (7). However, the desired Displacement of load sensing ring cannot be obtained, since the initial docking conditions of LIDM are random. Therefore, an impedance control method based on velocity is introduced to solve this problem. Thus, the relationship between force and velocity can be expressed as follows: where V and Vd represent actual Velocity and desired Velocity, respectively. When the initial docking conditions are zero, Equation (8) can be expressed in the Laplace domain as follows: Let: where Vf (s) represents the Velocity offset, thus, the Vf (s) can be obtained from Equation (9), and expressed as follows: According to Equation (11), the structure diagram of the impedance controller is established as shown in Figure 3. If there is no external Force on the LIDM (e.g., Fe = 0) and the desired Force is assumed to equal to zero (e.g., Fr = 0), the motion of load sensing ring follows the desired Velocity. Conversely, the motion of the load sensing ring is controlled by the correction of Velocity and the desired Velocity simultaneously. According to Equation (11) and Figure 3, the dynamic relationship between deviation of Force and correction of Velocity can be adjusted to adapt to external environment by changing the impedance parameters. Detail information about how impedance parameters affect control characteristics will be shown in the next section.

The Influences of Impedance Parameters on Control Characteristics
The purpose of impedance control based on velocity is to achieve an ideal dynamic relationship between the velocity of the load sensing ring and external forces by choosing a set of suitable impedance parameters. Thus, it is necessary to research how to choose the suitable impedance parameters. In this section, three simulations in one direction are presented to introduce how the impedance parameters affect control characteristics. The input function of simulations is a step function, and the results are shown in Figures 4-6.    Figure 4, the inertial parameter Md of the impedance controller primarily affects the reaction rate of the responses. If a lower value is selected for Md, there will be a rapid response to external forces, but it will result in a larger acceleration on actuators simultaneously. On the contrary, if a larger value is selected for Md, the response rate of the impedance controller would be slow, resulting in a stronger external force.
The second simulation shows how damping parameter Bd affects control characteristics by changing the value of Bd and keeping Kd and Md constant, while Kd = 200 N/mm, Md = 100 Kg. According to Figure 5, the damping parameter of the impedance controller primarily affects the peak and regulation time of responses. With the increase of Bd, the peak of response decreases, and the regulation time reduces first and then increases. When the value of Bd is equal to zero, the response of the impedance controller is undamped oscillation and the regulation time approaches to infinity. The vibration of response is not suitable for the compliance of LIDM, because it could result in an enormous external force.
The third simulation shows how the stiffness parameter Kd affects the control characteristics by changing the value of Kd and keeping Md and Bd constant, while Md = 100 Kg, Bd = 2000 Kg/s. According to Figure 6, it can be concluded that the stiffness parameter Kd of the impedance controller primarily affects the attenuation of response. When Kd is equal to zero, there is no attenuation. Additionally, the decay rate of response would increase, if the value of Kd increases.
According to the results of simulations, if one or more of contact forces or/and torques are more than the maximum of requirements of low impact, a smaller Md should be selected to decrease them by increasing the reaction rate of the responses. The damping parameter Bd must be maintained in a certain range where the oscillation of LIDM can be eliminated. When one or more of contact forces or/and torques are over the maximum of requirements of low impact, a greater Bd should be selected to decrease them by increasing the peak of responses. The stiffness parameter Kd of the impedance controller primarily affects the attenuation of responses. It is positive to maintain two LIDMs constant contact with each other during the process of capturing when the responses can decay (e.g., Kd > 0) in this control system. However, some larger internal forces of LIDM may be caused, if one greater Kd is selected.

The LIDM Model in ADAMS and the Control Module in MATLAB
The model of LIDM built in ADAMS is shown in Figure 7. In order to simplify the simulations, the inoperative parts of LIDM are removed. The ADAMS model comprises two docking assemblies, the active docking assembly (below in Figure 7) and the passive docking assembly (above in Figure 7). The active docking assembly is composed of base ring, actuators and load sensing ring. The passive docking assembly is only comprised of annular outer face and alignment guides. Contact forces and torques of two docking assemblies, delivered to the impedance controller, can be measured by load sensing ring in real-time. The actuators can receive velocity signals from impedance controller to adjust the position and posture of load sensing ring. Besides, the initial docking conditions of two LIDM can be set through adjusting the position and posture of passive docking assembly.
The ADAMS model can be used to generate a control module through "controls" of soft ADAMS. The control module of LIDM in MATLAB is shown in Figure 8. Vj, v1, v2, v3, v4, v5, v6 are input variables of the control module, where Vj represents the relative closing speed of two LIDMs, and v1, v2, v3, v4, v5,  v6 represent the driving velocities of six actuators. α, β, θ, x, y, z, f1, f2, f3, f4, f5, f6 , s are the output  variables, where f1, f2, f3, f4, f5, f6 represent the values of the load cells, and s represents the relative distance single of two LIDMs, through which completion of capture tasks can be detected.

The Components of the Control System
The control system built for LIDM consists of control modules, desired Force, impedance control, forward solution, Velocity transformation J T , desired velocity, filter and other modules as shown in Figure 9. Fr The desired Force provides desired forces and torques along three Cartesian axes, which represents the ideal interaction Force between the two LIDMs. The input of impedance control part is the difference between actual external Force Fe and desired contact Force Fr. The output is Velocity offset Vf. Vr represents the actual Velocity of load sensing ring in Cartesian-space which has been adjusted to adapt to the docking environment, while Vr = Vf + Vd. vr is driving velocity vector of six actuators, converted from velocity Vr by equation vr = J Vr. The length vector of six actuators l can be converted to the position and attitude angles (i. e., x, y, z, α, β, θ) of load sensing ring in Cartesian coordinates by forward solution. In addition, the control module is generated in ADAMS, which is the interface between ADAMS and MATLAB, and contacts the LIDM model in ADAMS with control system in MATLAB. Therefore, the control module can be seen as a MATLAB module that has the same effect on the model in ADAMS. In order to reduce computational complexity, x, y, z, α, β, θ can be directly obtained from control module, thus the forward solution in this simulation is unnecessary. However, this method is not suitable for physical test. Besides, the values of load cells can be converted to the contact Forces Fe in Cartesian-space through Jacobian matrix RJs T .
A filter is set in the feedback loop to control the values of external Forces passing the filter. When any one of the absolute values of input forces and torques of filter (except Fey) is less than or equal to critical value, the output of it is equal to zero, on the contrary, the output value is equal to input. In addition, whatever the input value is, the output of filter about Fey is always equal to zero. The filter can not only remove some interference from environment, but also control the sensitivity about forces and torques along three Cartesian axes, which determines who responses to the same input data first. Without filter, the docking of two LIDMs may not be successful.

Simulation Setups
The control system is built in MATLAB using the control module, as shown in Figure 10. The sample time of simulation is 0.001 s. The simulation is mainly built to validate the performance of impedance control based on velocity in uncertain environment and the reasonableness of LIDM model. The impedance parameters are given by: where Fei and Mei represent the force component and the torque component of Fe along i-axis respectively.
The initial docking conditions of two LIDMs are shown in Table 2, where φ, Φ and ψ represent the yaw angle, pitch angle and roll angle respectively, Xe and Ze represent the transversal offset along X-axis and Z-axis in Cartesian space, while Vj represents the relative speed of two LIDMs.

Simulation of the Capturing Process of Two LIDMs
The purpose of the simulation is to study whether impedance control based on velocity is suitable for low impact docking of LIDMs or not. In this simulation, the impedance controller controls the velocity of actuators, so the load sensing ring can track the desired Force Fr = [0,0,0,0,0,0] T as well as the desired Velocity Vd = [0,0,0,0,0,0] T . The results are shown in Figures 11-19, where alpha, beta, and theta represent α, β, θ respectively. Figures 11 and 12 show the simulation results of Case 1. The initial contact time of two LIDMs is close to 27 s, and the contact forces do not exceed 150 N and torques do not exceed 60 N·m during the capturing process. Figures 13 and 14 show the simulation results of Case 2. The initial contact time of two LIDMs is around 19 s, and the contact forces do not exceed 200 N and torques do not exceed 100 N·m during the process of capturing.        Figure 17 shows the docking process of two LIDMs of Case 4. According to Figures 18 and 19, the initial contact time of two LIDMs is close to 16 s, and the contact forces do not exceed 250 N and torques do not exceed 150 N·m during the capturing process. According to Figures 11-19, it is easy to find out that there exist some larger forces or torques changing suddenly in all the results of simulations. This phenomenon is triggered by changing the type of contact of two LIDMs from one to another.   According to the simulation results of the four cases, it can be concluded that the contact forces and torques between two LIDMs are instantly translated to adapt to the docking environment with impedance control based on velocity, and the contact forces and torques can be controlled in a small range during the process of capturing. Furthermore, the maximum of contact force and torque are less than 250 N and 150 N·m, respectively, in simulations meeting the requirements, the maximum of contact force and torque are not more than 450 N and 450 N·m, respectively, of docking two LIDMs with low impact forces. Besides, the instantaneous stronger forces are caused by changing the type of contact of two LIDMs from one to another suddenly.

The Influences of Filtering Functions on Control Characteristics
The purpose of this section is to study how filter affects the control characteristics of LIDM. In this section, another filtering function is given as shown in Equation (13) As shown in Figures 20-25, the largest contact force is close to 4000 N while the largest contact torque is more than 300 N·m. Comparing the forces and torques of Figures 20 and 21 with Figures 18 and 19, the largest contact force and torque based on Equation (13) are far more than the values based on Equation (12). Besides, the largest value of force changing suddenly is far more than the value of normal forces during the process of capturing. By comparison, the filter based on Equation (12) is more suitable for the process of capturing, because the sensitivity of forces and torques along three axes of filter has more significant influences during capture. The final values of translation displacement and angular displacement are shown in Figures 22-25. Two simulations are consistent with the same initial docking conditions as shown in Table 2, but the process of moving is entirely different. Without prewired movement path in this method, the motion of load sensing ring is not regular. Thus, how to set the sensitivities about forces and torques of filter along three axes in Cartesian can affect the moving path of the load sensing ring in a manner. In summary, it can be concluded that a filter with suitable filtering functions might greatly improve the capturing process.

Conclusions
In the capturing simulation by ADAMS and MATLAB, the results of force tracking based on suitable filtering functions are satisfactory. The contact forces and torques of two LIDMs are controlled in a small rang, that the maximum of contact force and torque are not more than 250 N and 150 N·m, respectively, meeting the requirements of docking two LIDMs with low impact forces. In particular, the control system can maintain the contact forces/torques close to expected values when the initial docking conditions are random. Thus, the impedance control algorithm based on velocity is robust and the control system is suitable for LIDM. Although the results are effective, there is still space or room for improvement to reduce the values of contact forces and torques, possibly by choosing more proper impedance parameters or filtering functions.