Region of Attraction Propagation Through Bilateral Contact Constraints during Robotic capture for Active Debris Removal

Contact Dynamics of Free-Floating Robotic Systems

In this chapter, the contact dynamics involved during robotic On-Orbit Servicing and Active Debris Removal will be discussed. A general mathematical introduction to impulsive contact dynamics for multibody systems can be found in Glocker, C., & Pfeiffer, F., 1992 and Hurmuzlu, Y., & Marghitu, D. B., 1994 and is recommended before reading this chapter. It should be noted that the equations given in this chapter are only valid for non-sliding contact i.e. the forces are within the friction cone. There are two reasons for this: this is simpler to model and allows the analysis of the system for controller composition, and friction cone constraints can be added to trajectory planning and control to ensure non-sliding contact.

Unilateral Contact Dynamics and Contact Map in Legged Robots

A good place to start the discussion on contact dynamics is to look at the field of Legged Robotics, such as humanoids, quadrupeds, etc. While walking, contact is repeatedly made and broken with the ground and understand the mechanics of contact is critical for the study of legged locomotion. Thus, it is no surprise that the legged robotics community has a rich literature on contact dynamics and state mapping through contact. One of the simplest cases of contact is a impulsive contact which results in an perfectly inelastic collision, here, between the robot feet and ground, such that the feet does not bounce back up. Due to the (relatively) infinite mass of the Earth, these contacts impose unilateral constraints on the dynamics of the robot system. A derivation of the contact mapping adapted from Grizzle, J. W., et. al 2014 is given here.

The equations of motion for a robot system, such as a humanoid or quadruped, can be written as:

\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} = \tau_g(q) + B(q)u + J^T F_c + F_f \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [1]\]

Here the terms given are:

$q \in \mathbb{R}^{7+n}$ - Generalized Coordinates of the System using quaternion for base orientation
$M(q) \in \mathbb{R}^{6+n \times 6+n}$ - Mass-Inertia Matrix of the System
$C(q, \dot{q}) \in \mathbb{R}^{6+n \times 1}$ - Coriolis Force Terms
$\tau_g(q) \in \mathbb{R}^{6+n \times 1}$ - Generalized Forced due to Gravity
$B(q) \in \mathbb{R}^{6+n \times 6+n}$ - Matrix to Map the Control inputs to generalized forces
$u \in \mathbb{R}^{6+n \times 1}$ - Control Inputs
$J \in \mathbb{R}^{6 \times 6+n}$ - Jacobian to Map the Contact Forces to Generalized Forces
$F_c \in \mathbb{R}^{6 \times 1}$ - Contact Forces
$F_f \in \mathbb{R}^{6+n \times 1}$ - Generalized Friction Forces

We assume the friction forces to be zero. Furthermore, we assume an impulsive contact force $F_c$ acting over time from $t$ to $t + \Delta t$, where $\Delta t \rightarrow 0$ signifying impulsive contact. During contact, the generalized positions of there system does not change, while the velocity incurs a discontinuous change. We can integrate equation [1] from $t$ to $t + \Delta t$ and assuming that there are no impulsive joint actuation forces/torques applied, to obtain the following equation representing the velocity map during contact:

\[M(q) \Delta \dot{q} = J^T I_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [2]\] \[M (\dot{q}^+ - \dot{q}^-) = J^T I_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [3]\]

Here, $\dot{q}^+ \in \mathbb{R}^{6+n \times 1}$ and $\dot{q}^- \in \mathbb{R}^{6+n \times 1}$ represent the velocities before and after the contact. Now, $I_c$ is the integrated intensity of the contact wrench over the infinitesimal impact period: $I_c = \int_t^{t+\Delta t} F_c$. Equation [3] also signifies the conservation of generalized momentum (see Hurmuzlu, Y., & Marghitu, D. B., 1994).

A kinematic constraint during contact for legged robots, is that the leg is in contact with the ground through the contact phase i.e flat foot contact instead of heel and then toe strike. Assuming no sliding or rotation at the foot, this implies that the spatial velocity of the leg at the point of contact is zero. This kinematic constraint can be expressed using the following relation:

\[J(q)\dot{q}^+ = V_{foot} = 0 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [4]\]

Combining equations [3] and [4], give us the full contact model for an impulsive contact in matrix form:

\[\begin{bmatrix} M & -J^T \\ J & 0 \end{bmatrix} \begin{bmatrix} \dot{q}^+ \\ I_c \end{bmatrix} = \begin{bmatrix} M\dot{q}^- \\ 0 \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [5]\]

It can be observed that the matrix on the left side of the equation is a square matrix and is full rank if $J$ is full-rank i.e. there are no kinematic singularities or redundant constraints. Another observation that can be made from equation [5] is that the post-impact velocities and the impact wrench are related linearly to the pre-impact velocities. Block matrix inversion using Schur complement can be performed on equation [5] to get an expression for post-impact velocity (equation 49 from Grizzle, J. W., et. al 2014):

\[\dot{q}^+ = (I_{N+6} - M^{-1} J^T (J M^{-1} J^T)^{-1} J) \dot{q}^- \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [6]\]

This equation can be represented in a simpler form to showcase the contact mapping of velocities as:

\[\dot{q}^+ = \Delta (q) \dot{q}^- \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [7]\]

Almost every paper on legged robotic has one of the equations [5], [6], or [7] in one form or another (Grizzle, J. W., et. al 2014).

Bilateral Contact Dynamics and Contact Map for Space Robots

Contact for space robotics occurs in following cases when a chaser satellite with a robot arm captures a target satellite for either Active Debris Removal (ADR), or On-Orbit Servicing (OOS). The difference between the two cases is that the debris satellite is usually tumbling uncontrollably and the OOS target satellite (usually) has a functional attitude control system. In either case, the implicit assumption made in the previous section: Earth/Ground has relatively infinite mass when compared to the robot, is no longer valid. Thus, the derivation cannot be directly applied to space robotics. However, the central idea of finding a mapping relating the pre-capture and post-capture states is valuable and can be used in space-based applications (ADR/OOS). The following derivation of such mapping is adapted from Cyril, X., et. al 1993. The equations of motion for a chaser satellite with a robot arm and a target satellite represented as rigid body can be written as:

\[M_c(q_c)\ddot{q}_c + C_c(q_c, \dot{q}_c)\dot{q}_c = B(q_c)u_c + J_c^T F_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [8]\] \[M_t(q_t)\ddot{q}_t + C_t(q_t, \dot{q}_t)\dot{q}_t = B(q_t)u_t + J_t^T F_t \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [9]\]

Here, the subscripts $c$ and $t$ refer to chaser and target spacecrafts respectively. For target spacecraft, we take $n=0$ to get the matrix dimensions. Furthermore, it can be noted that $F_c = - F_t \in \mathbb{R}^{6 \times 1}$. Pre-multiplying equation [9] with $J_c^T (J_t^T)^{-1} \in \mathbb{R}^{6+n \times 6}$, and substituting $-F_c$ for $F_t$, we get:

\[J_c^T (J_t^T)^{-1} (M_t\ddot{q}_t + C_t\dot{q}_t) = J_c^T (J_t^T)^{-1} B(q_t)u_t -J_c^T F_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [10]\]

Adding equations [8] and [10], we get:

\[J_c^T (J_t^T)^{-1} (M_t\ddot{q}_t + C_t\dot{q}_t) + M_c\ddot{q}_c + C_c\dot{q}_c = B_c u_c + J_c^T (J_t^T)^{-1} B_t u_t \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [11]\]

Integrating the above equation [11] over an impulsive contact with the duration $\Delta t \rightarrow 0$, and assuming, similar to previous section, that there are no impulsive actuation forces/torques applied, we get the following equation:

\[J_c^T (J_t^T)^{-1} M_t \Delta \dot{q}_t + M_c \Delta \dot{q}_c = 0 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [12]\] \[J_c^T (J_t^T)^{-1} M_t (\dot{q}_t^+ - \dot{q}_t^-) + M_c (\dot{q}_c^+ - \dot{q}_c^-) = 0 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [13]\]

For the case of ADR and OOS, the collision is assumed to be a plastic collision i.e. the chaser spacecraft robot arm’s end-effector is rigidly attached to the target post capture, thus signifying a capture. If the collision was elastic, the target spacecraft would bounce away from chaser. Hence, a plastic collision describes the consequent behavior of the system: a multibody system where the chaser and target spacecraft are part of a single multibody system. The target is rigidly attached to the chaser robot arm’s end-effector and adds to the last link’s mass-inertia properties. This provides us with a kino-dynamic velocity constraint in a similar fashion as the one in previous section (with zero velocity of the foot during contact). The constraint is kino-dynamic in nature as free-floating robot kinematics cannot be separated in a meaningful way from their dynamics. The post-capture velocity of the target, after being captured by the chaser, can be given by the following relation:

\[V_t = \dot{q}_t^+ = \hat{J}_{ct} \dot{q}_c^+ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [14]\]

Here, $\hat{J}_{ct} \in \mathbb{R}^{6 \times 6+n}$ is the Generalized Jacobian Matrix (GJM) (from Umetani and Yoshida, 1989) for the combined chaser-target system post-capture, as denoted by the subscript $ct$. The GJM can be expressed as follows:

\[\hat{J}_{ct} = J_{m, ct} - J_{b, ct} M_{b, ct}^{-1}M_{bm, ct} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [15]\]

Here, the subscripts $b,ct$ and $m,ct$ refer to the base and the manipulator robot arm of the combined chaser-target satellite system post-capture. This gives us the following terms:

$J_{m, ct}$ - Manipulator Jacobian of the combined Chaser-Target System Post-Capture

$J_{b, ct}$ - Base Jacobian of the combined Chaser-Target System Post-Capture

$M_{b, ct}$ - Mass-Inertia Matrix of the Chaser’s Base for the combined Chaser-Target System Post-Capture

$M_{bm, ct}$ - Dynamic Coupling Mass-Inertia Matrix for the combined Chaser-Target System Post-Capture

Here in equation [15], all the terms are evaluated for a post-capture system i.e. the multibody system containing both the chaser and target. Equation [14] can be substituted in equation [13] to give the following contact mapping:

\[J_c^T (J_t^T)^{-1} M_t \hat{J}_{ct} \dot{q}_c^+ - J_c^T (J_t^T)^{-1} M_t \dot{q}_t^- + M_c \dot{q}_c^+ - M_c \dot{q}_c^- = 0 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [16]\]

The above equation can be rearranged as:

\[(J_c^T (J_t^T)^{-1} M_t \hat{J}_{ct} + M_c) \dot{q}_c^+ = M_c \dot{q}_c^- + J_c^T (J_t^T)^{-1} M_t \dot{q}_t^- \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [17]\] \[\dot{q}_c^+ = G^{-1} H \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [18]\]

where:

\[G = J_c^T (J_t^T)^{-1} M_t \hat{J}_{ct} + M_c \\ H = M_c \dot{q}_c^- + J_c^T (J_t^T)^{-1} M_t \dot{q}_t^-\]

The mapping of states through contact as given in equation [18] provides a simple relationship to find post-contact state of the system. This equation is similar to the one derived in Cyril, X., et. al 1993 with a key difference: this derivation uses the GJM in equation [14] for the target velocity constraint post-impact.

The map given in equation [18] shows that the post-capture state is only dependent on the initial conditions before contact of the chaser and the target and the contact force impulse is eliminated from the mapping. Such a mapping could be used for post-capture dynamic simulation for ADR and OOS scenarios.

However, it can be argued that eliminating the contact impulse force from the mapping is not always in the best interest, specially for controller synthesis. The force-torque measurements from a Force-Torque sensor attached to the end-effector of the chaser’s robot arm is a valuable source of information during the capture operation. Instead of eliminating this information in our equations, it can instead be used for controller synthesis and robustness analysis. This idea will be shown below with further elaboration of how robustness analysis can be done with the information of wrenches (forces/torques) during contact.

Instead of adding equations [8] and [10] as done above, we can instead subtract them to keep the contact force term in the resulting equation. Subtracting [8] from [10] gives us:

\[J_c^T (J_t^T)^{-1} (M_t\ddot{q}_t + C_t\dot{q}_t) - M_c\ddot{q}_c - C_c\dot{q}_c = J_c^T (J_t^T)^{-1} B(q_t)u_t - B(q_c)u_c - 2J_c^T F_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [19]\]

Integrating the above equation [19] over an impulsive contact with the duration $\Delta t \rightarrow 0$, and assuming, same as before, that there are no impulsive actuation forces/torques applied, we get the following equation:

\[J_c^T (J_t^T)^{-1} M_t \Delta \dot{q}_t - M_c \Delta \dot{q}_c = -2J_c^T I_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [20]\] \[J_c^T (J_t^T)^{-1} M_t (\dot{q}_t^+ - \dot{q}_t^-) - M_c (\dot{q}_c^+ - \dot{q}_c^-) = -2J_c^T I_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [21]\]

Here, $I_c$ is the integrated intensity of the contact wrench over the infinitesimal impact period: $I_c = \int_t^{t+\Delta t} F_c$. Substituting equation [14] in equation [21] gives:

\[J_c^T (J_t^T)^{-1} M_t (\hat{J}_{ct} \dot{q}_c^+ - \dot{q}_t^-) - M_c (\dot{q}_c^+ - \dot{q}_c^-) = -2J_c^T I_c \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [22]\]

The above equation [22] can be rearranged to give the contact mapping as:

\[\dot{q}_c^+ = K^{-1}L \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [23]\]

where

\[K = M_c - J_c^T (J_t^T)^{-1} M_t \hat{J}_{ct} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [24] \\ L = M_c \dot{q}_c^- + 2J_c^T I_c - J_c^T (J_t^T)^{-1} M_t \dot{q}_t^- \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [25]\]

The contact map in equation [23] includes the pre-contact state of chaser and the target, as well as the impulsive contact forces experienced during the contact. This mapping of states before and after bilateral impulsive contact can be used in design and analysis of control systems for capture of a target satellite for ADR/OOS missions.

Propagating Region of Attraction Through the Contact Map

Let us assume that Post-capture detumbling of target space debris during an ADR mission uses a Time-Varying Linear Quadratic Regulator (TVLQR) controller whose Region of Attraction (RoA) is known for post-capture state. This post-capture state used as the initial state for detumble controller synthesis can be written as:

\[X_0 = \begin{bmatrix} q^+ \\ \dot{q}^+ \end{bmatrix} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [26]\]

The region of attraction is then represented using a quadratic time-varying function, which is also the optimal cost-to-go function of the TVLQR (Reist, P., & Tedrake, R., 2010, Tedrake, R., et. al 2010):

\[X(t)^T S(t) X(t) < \rho (t) \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [27]\]

Here, $\rho (t)$ is a time-varying scalar representing the level sets of the cost-to-go/lyapunov function. The inlet of this funnel is the initial RoA condition on the initial post-capture state, which can be written using equation [26] as:

\[X_0^T S_0 X_0 < \rho_0 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [28]\] \[\begin{bmatrix} q^+ \\ \dot{q}^+ \end{bmatrix}^T S_0 \begin{bmatrix} q^+ \\ \dot{q}^+ \end{bmatrix} < \rho_0 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [29]\]

During impulsive contact, the generalized positions of the system are assumed to not change during the short duration, the superscript (${}^+$) can be dropped from the generalized positions in the above equation [29]. We can also use equation [23] in equation [29] to express post capture velocities as a function of pre-capture state and the impulse contact wrench. We can then rewrite equation [29] as:

\[\begin{bmatrix} q \\ K^{-1} L \end{bmatrix}^T S_0 \begin{bmatrix} q \\ K^{-1} L \end{bmatrix} < \rho_0 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [30]\]

Equation [30] is of significant importance as it is expressed in terms of only pre-impact states and the impact wrenches and does not contain any dependency on post-impact states. This is in contrast with the earlier RoA of post-capture detumbling given in equation [27] which is expressed in terms of post-capture states. Thus, equation [30] allows us to check online if the pre-capture state and the measured wrenches (using a force-torque sensor mounted at the chaser robot arm’s end-effector) will result in a successful detumble upon capture.

References

[Glocker, C., & Pfeiffer, F., 1992] Glocker, C., & Pfeiffer, F. (1992). Dynamical systems with unilateral contacts. Nonlinear Dynamics, 3(4), 245–259. https://doi.org/10.1007/BF00045484

[Hurmuzlu, Y., & Marghitu, D. B., 1994] Hurmuzlu, Y., & Marghitu, D. B. (1994). Rigid Body Collisions of Planar Kinematic Chains With Multiple Contact Points. The International Journal of Robotics Research, 13(1), 82–92. https://doi.org/10.1177/027836499401300106

[Grizzle, J. W., et. al 2014] Grizzle, J. W., Chevallereau, C., Sinnet, R. W., & Ames, A. D. (2014). Models, feedback control, and open problems of 3D bipedal robotic walking. Automatica, 50(8), 1955–1988. https://doi.org/10.1016/j.automatica.2014.04.021

[Cyril, X., et. al 1993] Cyril, X., Jaar, G. J., & Misra, A. K. (1993). Effect of payload impact on the dynamics of a space robot. 1993 International Conference on Intelligent Robots and Systems, 00(7), 2070–2075. https://doi.org/10.1109/iros.1993.583916

[Umetani and Yoshida in 1989] Umetani, Yoji, and Kazuya Yoshida. Resolved motion rate control of space manipulators with generalized Jacobian matrix. IEEE Transactions on robotics and automation, 5(3), pp.303-314. 1989.

[Tedrake, R., et. al 2010] Tedrake, R., Manchester, I. R., Tobenkin, M., & Roberts, J. W. (2010). LQR-trees: Feedback motion planning via sums-of-squares verification. International Journal of Robotics Research, 29(8), 1038–1052. https://doi.org/10.1177/0278364910369189

[Reist, P., & Tedrake, R., 2010] Reist, P., & Tedrake, R. (2010). Simulation-based LQR-trees with input and state constraints. Proceedings - IEEE International Conference on Robotics and Automation, 5504–5510. https://doi.org/10.1109/ROBOT.2010.5509893