Application of the Kuka Kube Test-Bed for the Hardware-in-the-Loop Validation of the Space Manipulator Control System

The dynamic equations of the system are based on the work performed by Seweryn and Banaszkiewicz (2008) and Rybus et al. (2017). The configuration of the system is described by the generalised coordinates vector as follows: (1) qp=[rsT ΘsT θmT]T {{\bf{q}}_p} = {[{\bf{r}}_s^T\;\;\;\;{\boldsymbol {\Theta }}_s^T\;\;\;\;{\boldsymbol {\theta }}_m^T]^T} where r_s ∈ ℝ³ denotes the position vector of the satellite and Θ_s ∈ ℝ³ denotes the attitude of the satellite described by Euler angles in ZYX convention, whereas θ_m = [θ_m1 θ_m2 … θ_mn−1 θ_mn]^T ∈ ℝⁿ is the vector of manipulator joint angular positions.

The vector of generalised velocities of the system is given by (2) qv=[vsT ωsT θ˙mT]T {{\bf{q}}_v} = {[{\bf{v}}_s^T\;\;\;\;{\boldsymbol {\omega }}_s^T\;\;\;\;{\boldsymbol {\dot \theta }}_m^T]^T} where vs=r˙s∈ℝ3 {{\bf{v}}_s} = {{\bf{\dot r}}_s} \in {\mathbb {R}^3} denotes the translational velocity of the satellite centre of mass (CoM), ωs=TΘ−1Θ˙s∈ℝ3 {{\boldsymbol {\omega }}_s} = {\bf{T}}_\Theta ^{ - 1}{{\bf{\dot \Theta }}_s} \in {\mathbb {R}^3} denotes the angular velocity of the satellite (T_Θ ∈ ℝ^3×3 is the transformation matrix that maps Euler angles time derivatives into the angular velocity vector) and θ˙m∈ℝn {{\boldsymbol{\dot \theta }}_m} \in {\mathbb {R}^n} is the vector of manipulator joint angular velocities.

The vector of generalised forces acting on the system is given by (3) Q=FsT τsT τmTT {\bf{Q}} = {\left[ {{\bf{F}}_s^T\;\;\;\;{\boldsymbol {\tau }}_s^T\;\;\;\;{\boldsymbol {\tau }}_m^T} \right]^T} where F_s ∈ ℝ³ denotes the vector of forces acting on the satellite CoM, τ_s ∈ ℝ³ denotes the vector of torques acting on the satellite and τ_m ∈ ℝⁿ denotes the joint driving (control) torques vector.

The general form of dynamic equation of motion for the free-floating space manipulator is given by (4) MsMsmMsmTMmq˙v+CsCmqv+GsGm=Q \left[ {\matrix{ {{{\bf{M}}_s}} & {{{\bf{M}}_{sm}}} \cr {{\bf{M}}_{sm}^T} & {{{\bf{M}}_m}} \cr } } \right]{{\boldsymbol {\dot q}}_v} + \left[ {\matrix{ {{{\bf{C}}_s}} \cr {{{\bf{C}}_m}} \cr } } \right]{{\bf{q}}_v} + \left[ {\matrix{ {{{\bf{G}}_s}} \cr {{{\bf{G}}_m}} \cr } } \right] = {\bf{Q}} where M_s ∈ ℝ^6×6 is the mass matrix of the satellite, M_m ∈ ℝ^n×n is the mass matrix of the manipulator, M_sm ∈ ℝ^6×n is the mass matrix that couples motion of the manipulator with motion of the satellite, C_s ∈ ℝ^6×(6+n) and C_m ∈ ℝ^n×(6+n) are Coriolis matrices of the satellite and the manipulator, respectively, Gs=FGST 01×3T∈ℝ6 {{\bf{G}}_s} = {\left[ {{\bf{F}}_{{{\bf{G}}_S}}^T\;\;\;\;{{\bf{0}}_{1 \times 3}}} \right]^T} \in {\mathbb {R}^6} , where FGST {\bf{F}}_{{{\bf{G}}_S}}^T denotes the vector of gravitational force acting on the satellite CoM and 0_1×3 denotes 1×3 vector filled with zeros, and G_m ∈ ℝⁿ denotes the vector of reaction forces and torques acting on the manipulator joints and induced by the gravitational force. The mass and Coriolis matrices of the system are defined by Rybus et al. (2022).

In case of the free-floating space manipulator, the state vector x=qpT qvTT {\bf{x}} = {\left[ {{\bf{q}}_p^T\;\;\;\;{\bf{q}}_v ^T} \right]^T} has 12 + 2n components. The orbital motion of the system is not considered. The gravitational potential energy of the system is usually assumed to be equal to zero. The gravity gradient is neglected due to the relatively small size of the manipulator (Cavenago et al., 2019). In addition, in close proximity of the potential target satellite, the satellite Attitude and Orbit Control System (AOCS) is assumed to be turned off, hence F_s = 0_3×1 and τ_s = 0_3×1. Such a system has conserved total momentum and angular momentum – the system is nonholonomic (Ratajczak and Tchoń, 2020). In case of the fixed-base manipulator working on Earth, the state vector x=[θmT θ˙mT]T {\bf{x}} = {[{\boldsymbol {\theta }}_m^T\;\;\;\;{\boldsymbol {\dot \theta }}_m^T]^T} has 2n components. The joint reaction torques G_m caused by the gravitational force have significant influence on the system dynamics. The dynamic equation of motion (4) then takes much simpler form (Featherstone and Orin, 2008).

In the following section, the control scheme for the space manipulator is presented. The forward kinematics of the system on the velocity level is given by (5) veeωee=Jsvsωs+Jmθ˙m \left[ {\matrix{ {{{\bf{v}}_{ee}}} \cr {{{\boldsymbol {\omega }}_{ee}}} \cr } } \right] = {{\bf{J}}_s}\left[ {\matrix{ {{{\bf{v}}_s}} \cr {{{\boldsymbol {\omega }}_s}} \cr } } \right] + {{\bf{J}}_m}{{\boldsymbol {\dot \theta }}_m} where J_s ∈ ℝ^6×6 denotes the Jacobian of the satellite and J_m ∈ ℝ^6×n denotes standard Jacobian of the manipulator expressed in Π_ine (Rybus et al., 2017). In case of fixed-base manipulators, the right-hand side of equation (5) consists only of the second component as the velocity of the base (satellite) equals zero.

Let us assume that the initial total momentum and angular momentum of the system are equal to zero. The nonholonomic constraints are then formed by relations expressing the momentum and angular momentum as follows: (6) PL=H2vsωs+H3θ˙m=06×1 \left[ {\matrix{ {\bf{P}} \cr {\bf{L}} \cr } } \right] = {{\bf{H}}_2}\left[ {\matrix{ {{{\bf{v}}_s}} \cr {{{\boldsymbol {\omega }}_s}} \cr } } \right] + {{\bf{H}}_3}{{\boldsymbol {\dot \theta }}_m} = {{\bf{0}}_{6 \times 1}} where matrices H₂ ∈ ℝ^6×6 and H₃ ∈ ℝ^6×n were presented by Rybus et al. (2017).

Equation (6) can be solved for the translational and angular velocity of the satellite as follows: (7) vsωs=−H2−1H3θ˙m \left[ {\matrix{ {{{\bf{v}}_s}} \cr {{{\boldsymbol {\omega }}_s}} \cr } } \right] = - {\bf{H}}_2^{ - 1}{{\bf{H}}_3}{{\boldsymbol {\dot \theta }}_m}

Substituting (7) into (5) yields (8) veeωee=Jm−JsH2−1H3θ˙m=Jdθ˙m \left[ {\matrix{ {{{\bf{v}}_{ee}}} \cr {{{\boldsymbol {\omega }}_{ee}}} \cr } } \right] = \left( {{{\bf{J}}_m} - {{\bf{J}}_s}{\bf{H}}_2^{ - 1}{{\bf{H}}_3}} \right){{\boldsymbol {\dot \theta }}_m} = {{\bf{J}}_d}{{\boldsymbol {\dot \theta }}_m} where the matrix J_d ∈ ℝ^6×n is called the Dynamic Jacobian.

The Dynamic Jacobian is dependent on both configuration of the manipulator and the attitude of the satellite. Equation (8) can be solved for joint angular velocities as follows: (9) θ˙m=Jd#veeωee {{\boldsymbol {\dot \theta }}_m} = {\bf{J}}_d^\# \left[ {\matrix{ {{{\bf{v}}_{ee}}} \cr {{{\boldsymbol {\omega }}_{ee}}} \cr } } \right] where # denotes the inverse of the square matrix or Moore–Penrose pseudoinverse in case of non-square matrix.

We consider controlling the end-effector (tip of the manipulator) to the desired position and orientation expressed in Π_ine. The desired velocity of the end-effector is evaluated by multiplying the control error by constant gain coefficients as follows: (10) veedesωeedes=Kvreedes−reeactKωTΘ−1Θeedes−Θeeact \left[ {\matrix{ {{{\bf{v}}_{e{e_{des}}}}} \cr {{{\boldsymbol {\omega }}_{e{e_{des}}}}} \cr } } \right] = \left[ {\matrix{ {{{\bf{K}}_v }\left( {{{\bf{r}}_{e{e_{des}}}} - {{\bf{r}}_{e{e_{act}}}}} \right)} \cr {{{\bf{K}}_\omega }{\bf{T}}_\Theta ^{ - 1}\left( {{{\boldsymbol {\Theta }}_{e{e_{des}}}} - {{\boldsymbol {\Theta }}_{e{e_{act}}}}} \right)} \cr } } \right] where subscript des denotes the desired values, subscript act denotes the actual, measured values, Θ_ee ∈ ℝ³ is the orientation vector of the end-effector described by Euler angles in ZYX convention and K_υ ∈ ℝ^3×3 and K_ω ∈ ℝ^3×3 denote the diagonal positive-definite gain matrices for translational and angular velocities, respectively.

Substituting the end-effector velocity in (9) with the desired values expressed by the right-hand side of (10) yields the final form of the control law: (11) θ˙mdes=Jd#Kvreedes−reeactKωTΘ−1Θeedes−Θeeact {{\boldsymbol {\dot \theta }}_{{m_{des}}}} = {\bf{J}}_d^\# \left[ {\matrix{ {{{\bf{K}}_v }\left( {{{\bf{r}}_{e{e_{des}}}} - {{\bf{r}}_{e{e_{act}}}}} \right)} \cr {{{\bf{K}}_\omega }{\bf{T}}_\Theta ^{ - 1}\left( {{{\boldsymbol {\Theta }}_{e{e_{des}}}} - {{\boldsymbol {\Theta }}_{e{e_{act}}}}} \right)} \cr } } \right]

The desired manipulator joint angular velocities are then sent to the manipulator Proportional-Integral-Derivative (PID) joint controllers (Seweryn et al., 2017). The presented approach forms a cascade control system, where the outer loop is a P controller of the position and orientation of the end-effector and the inner loop is a PID controller of the manipulator joint angular velocities.

In this paper, we use hardware-in-the-loop experiments to validate only the outer stage of the cascade controller represented by control law (11). Therefore, the controller equations on the joint level are omitted. This is caused by the fact that the joint controllers used in the experiments are directly applied from the KUKA robot software delivered by the manufacturer. In other words, the desired joint velocities obtained with (11) are directly sent to the KUKA industrial controller.

The KUKA KUBE test-bed is presented in Figure 2. The test-bed is functionally divided into two subsystems: 1) executive part mounted inside cubic 2.5 m × 2.5 m × 2.5 m frame and 2) operator stand for ambidextrous control with both vision and force feedback. Test stand complies with International Standards Organisation/Technical Specification (ISO/TS 15066:2016: Robots and robotic devices – Collaborative robots norm). Executive elements consist of two KUKA LBR iiwa 14 R820 robotic arms equipped with universal tool mounts. Robots operate from side-mounted linear guides. KUKA LBR iiwa 14 R820 is a 7 Degrees of Freedom (DoF) articulated cobot capable of measuring joint torques both with and without gravity compensation. It is also capable of estimating forces acting on its end-effector. Denavit–Hartenberg (DH) parameters of the KUKA robot are presented in Table 1. KUBE test stand and its capabilities are well described by Granosik et al. (2016).

The developed software solution consists of two main subsystems: control loop and logging loop. Control loop was implemented in MATLAB using KUKA Sunrise Toolbox (KST) developed by Safeea and Neto (2018). KST consists of two parts: MATLAB client scripts operating on Personal Computer (PC) and MATLAB server java application operating on KUKA controller. Data exchange frequency achieved by Transmission Control Protocol (TCP) connection provided by KST was sufficient to control the robot, but too slow for logging purposes as it reached only ~40 Hz. Logging was performed by using fast research interface (FRI) functionality provided by KUKA. FRI provides User Diagram Protocol (UDP) connection capable of sending full state of the robot with the frequency up to 1 kHz. In addition, another PC with Universal Serial Bus (USB) webcam was used to record the experiments. Both FRI logging and recording were triggered and terminated from the main control loop.

To implement control loop, dynamic parameters of LBR iiwa 14 R820 were needed. Parameters used were taken from the work performed by Stürz et al. (2017). Those parameters were also used by Safeea and Neto (2018) when developing KST.

Due to inability of FRI and KST of simultaneous operation on KUKA controller in as-is state and the low rate of KST connection, it was necessary to modify the controller side of both. In addition, the KST TCP connection speed was increased by concatenating existing KST commands for force, position and torque measurements into one command. Final architecture of connections is presented in Figure 3 (variables presented in the diagram are explained in sections 2 and 4). Logical diagram of the system is presented in Figure 4. The block ‘Perform main control loop iteration’ is discussed in Section 4.1.

Figure 2.

Figure 3.

Figure 4.

The schematic representation of the performed experiment is presented in Figure 5. Subscript sim denotes variables related to the model plant calculated on the MATLAB level parallel to the experiment, while subscript test denotes variables related to the test-bed measurements. No subscript indicates that the variable is related to both model plant and test-bed. The KUKA manipulator is mounted on a fixed base and is operated in gravitational environment. The manipulator joint positions and velocities are measured on the test-bed with encoders. The model plant (marked as black and white system in Figure 5) is represented by the free-floating space manipulator, whose end-effector is controlled by the cascade control system based on the Dynamic Jacobian control law.

The actual current position of the end-effector is equal to the simulated position of the end-effector (r_ee)_sim ∈ ℝ³ and is represented by the sum of the simulated position of the satellite (r_s)sim ∈ ℝ³, the manipulator mounting point vector r_q ∈ ℝ³, which depends on geometrical system parameters, and the measured (on the test-bed) position of the end-effector with respect to the manipulator base (r_{1_ee})_test ∈ ℝ³: (12) reeact=(ree)sim=(rs)sim+rq+(r1−ee)test {{\bf{r}}_{e{e_{act}}}} = {({{\bf{r}}_{ee}})_{sim}} = {({{\bf{r}}_s})_{sim}} + {{\bf{r}}_q} + {({{\bf{r}}_{{1_ - }ee}})_{test}}

The current position of the satellite (r_s)_sim is evaluated by calculating and integrating equation (7) in each step of the control system, while the measured position of the end-effector with respect to its base is calculated with forward kinematics based on test-bed joint position measurements (θ_m)_test.

The input of the control system is given by the constant desired position of the end-effector with respect to the inertial frame r_eedes. It is worth noting that even though these values are constant with respect to the inertial frame, the desired end-effector position with respect to the manipulator base (r_{1_eedes})_test will change with time as a result of the simulated satellite motion (r_s)_sim. Similar relations are obtained on the orientation level, which is omitted here.

Equation (12) shows the main idea of the hardware-in-the-loop experiment. We strive to validate the control system of the free-floating manipulator, whereas the on-ground test-beds do not allow us to introduce the motion of the satellite and the free-floating nature of the satellite-manipulator system. Therefore, we introduce a model which accounts for the free-floating nature of the system and we design the experiment so that the model is calculated in real time parallel to the experiment. As a result, the variables of interest for the control system (position and orientation of the end-effector with respect to the inertial frame) depend on both test-bed measurements (r_{1_ee})_test and results of the simulated model (r_s)_sim. This concept allows us to combine the robot's hardware components with the free-floating nature of the system, which has high impact on the control system but is very difficult to be emulated on Earth.

Figure 5.

The calculation of the position and attitude of the satellite with the use of (7) as well as evaluation of the desired joint angular velocities using control law (11) are performed in the loop programmed with MATLAB KST. The program is running parallel to the experiment and its procedures are called out in each step of the control system. The desired joint velocities are then sent to the KUKA joint controllers that are able to control manipulator joints in the inner loop of the cascade control system.

To summarise, the plant model loop operations are presented as a diagram in Figure 6 and summarised as a list of operations below: 1)

Receiving the information about the current KUKA manipulator joint angular positions (θ_m)_test as well as the current end-effector position (r_{1_ee})_test and orientation (Θ_{1_ee})_test with respect to the manipulator mounting point

Figure 6.

The parameters of the scenario are described herein. The mass of the satellite is set to 100 kg, whereas the moments of inertia around the main axes of the satellite are 2.83 kgm² in X axis, 6.08 kgm² in Y axis and 7.42 kgm² in Z axis. Products of inertia of the satellite are assumed to be zero. The satellite CoM is initially placed at the origin of Π_ine, and the initial attitude of the satellite coincides with the orientation of Π_ine. The initial configuration of the KUKA manipulator is θ_m1 = −26.25 deg, θ_m2 = 65.96 deg, θ_m3 = 0.72 deg, θ_m4 = 67.57 deg, θ_m5 = −63.15 deg, θ_m6 = −2.56 deg and θ_m7 = 140.79 deg. The initial velocities of the system are set to zero. The initial end-effector position with respect to Π_ine is r_ee = [1.057 m 0.174 m 0.328 m]^T, while the initial orientation of the end-effector is Θ_ee = [−92.15 deg −38.26 deg −86.22 deg]^T. The desired end-effector position and orientation with respect to Π_ine are r_eedes = [0.948 m −0.225 m 0.094 m]^T and Θ_eedes = [−88.38 deg −37.27 deg −56.99 deg]^T. The gain matrices K_υ and K_ω are set to be identity matrices (each gain equal to 1). The experiment is set to last 20 s. The frames captured by the camera inside the KUKA KUBE test-bed showing the performed experiment are presented in Figure 7.

Figure 7.

The results of the experiment are compared with the results of the simulation of the free-floating space manipulator performed after the test. The simulations performed after the experiment are not the same as the model plant used during the hardware-in-the-loop test. These simulations use the dynamic model (4) to simulate the free-floating manipulator motion using the experimental results as an input, whereas the MATLAB control loop used nonholonomic constraints (7) to evaluate the current satellite motion induced by the manipulator. It is also worth noting that the simulations performed to validate the behaviour of the system during the experiment were conducted in the open-loop mode – there was no control system implemented in the simulation. The reason is that the purpose of these simulations is to validate if the free-floating dynamics were correctly included in (12) during experiments.

The simulation is designed using the inverse dynamics approach (Basmadji et al., 2020). The signals of the manipulator joint angular positions measured in the experiment are used to calculate the driving torques using equation (4). Then, the evaluated driving torques are applied to solve the Simulink SimMechanics model of the KUKA manipulator mounted on the free-floating satellite. The simulations were performed with fixed time step equal to 0.005 s. The dynamics equations were integrated numerically using third-order Bogacki–Shampine formula. Due to the fact that calculating numerical derivatives of the measured joint positions is sensitive to encoder noise, the measurements are approximated with the fifth-order polynomials. The measured manipulator joint angles and their approximation used for the simulation are presented in Figure 8, while the approximation error is presented in Figure 9. The manipulator end-effector position and orientation with respect to Π_ine evaluated in the test and in the simulation are shown in Figure 10, while the differences between them are shown in Figure 11. Finally, the position and attitude of the satellite integrated in the MATLAB loop are compared with the simulation results in Figure 12, while the differences are presented in Figure 13.

Figure 8.

Figure 9.

Figure 10.

Figure 11.

Figure 12.

Figure 13.

The KUKA manipulator is equipped with torque sensors mounted in each manipulator joint. These measurements show the net torque (τ_mnet)test acting in the axis of rotation for each joint. To validate the correctness of the simulation tool, additional simulation using Simulink SimMechanics model of the fixed-base KUKA manipulator is performed. The time step and the numerical integration method used are the same as in previous simulations. Proper gravitational acceleration vector is set, and joint reaction torques are compared with the test-bed measurements. The time of evaluation for each iteration of the MATLAB loop during the experiment is presented in Figure 14. As an example, we chose to present the torque measured in the second joint compared with simulation results in Figure 15. It is observed that the simulation results are characterised by high torque peaks. The reasoning behind them will be discussed in the next section. To increase the readability of the torque signals, the Y axes limits for joint torque results in Figure 16 are set, so that the simulation torque peaks are not visible. The differences between the net torques measured during the experiments and obtained in the simulations are presented in Figure 17.

Figure 14.

Figure 15.

Figure 16.

Figure 17.