P-satI-D Shape Regulation of Soft Robots

Soft robots are intrinsically underactuated mechanical systems that operate under uncertainties and disturbances. In these conditions, this letter proposes two versions of PID-like control laws with a saturated integral action for the particularly challenging shape regulation task. The closed-loop system is asymptotically stabilized and matched constant disturbances are rejected using a very reduced amount of system information for control implementation. Stability is assessed on the underactuated dynamic model through the Invariant Set Theorem for two relevant classes of soft robots, i.e., elastically decoupled and elastically dominated soft robots. Extensive simulation results validate the proposed controllers.


I. INTRODUCTION
C ONTINUUM soft robots are robotic systems made of continuously deformable materials [1]. Because of their compliant nature, soft robots have applications ranging from maintenance [2] to human rehabilitation [3]. In soft robotics, several open challenges need yet to be addressed, spanning from perception to control. These mainly stem from the infinitedimensional nature of soft robots, whose dynamics is governed by nonlinear partial differential equations. To overcome this challenge, researchers have proposed approaches to obtain finite-dimensional but accurate descriptions of soft robots [4], [5], [6]. Several works, such as [7], [8], [9], have used finitedimensional models for control purposes both in configuration and task space. Most of these new approaches build upon fully actuated approximations of the dynamics, which allow reconducting the control problem to standard rigid robot control theory. However, obtaining these simplified models implies relying on very coarse approximations. Developing model-based controllers this way has been a valuable heuristic to showcase the power of model-based controllers. Unfortunately, at the same time, using such rough approximations can lead to an erroneous assessment of stability and sub-optimal performance, which are essential features for systems that are supposed to interact with humans.
Therefore, a growing attention has been building around the topic of developing controllers for soft robots that operate outside the restrictive fully actuated framework. An early work dealing with this challenge is [10], which approximates the dynamics of soft robots as large-scale linear systems. This allows assessing that a control loop consisting of an observer and a controller derived from a reduced-order model can successfully stabilize the original dynamics. The architecture is validated experimentally. In [11], the authors propose an underactuated template model for the control design of soft robots. After studying the main system properties, they show how collocated and non-collocated feedback linearization asymptotically stabilize the arm in its unstable straight configuration. Also, [12] presents a controller for shape regulation of planar soft robots by matching the robot with an underactuated rigid-link model. Even if theoretically sound, these controllers have limited practical applicability as they have complex forms and are potentially unrobust to external disturbances and model uncertainty.
A first work taking this direction is [13], which proposes a variation of the integral IDA-PBC technique for soft manipulators. This strategy still relies on model-based cancellations and, as such, it is potentially unrobust to model uncertainties. Remarkably, [14], [15], [16] investigated experimentally the use of PID regulators for controlling the shape of soft robots. However, these references do not provide stability proof and assume full actuation. We follow an alternative path with our recent works [17], [18], [19]. There, we proposed PD+ control laws for two classes of soft robots: elastically dominated and elastically decoupled. The goal is to have simple -and thus robust -controllers that are provably stable in the general setting and parsimoniously use the information on the robot dynamics. In this letter, we move a further step toward developing robust and simple shape regulators for soft robots under non-rough discretizations by adding a saturated integral (satI) action to the regulators of [17] and [18]. Fig. 1 shows a block scheme of the proposed control architecture. Including an integral term requires extreme care because the dynamic couplings with the soft robot dynamics may lead to instability. In addition to stability, we prove that (i) the steady state elastic force is learned through the integral term. As a result, the stiffness completely disappears from the control laws, avoiding the need to estimate its value accurately. Furthermore, in analogy with the internal model principle, (ii) the closed-loop system rejects matched constant disturbances.

A. Notation
Vectors and matrices are denoted with bold lower-and uppercase letters, respectively, whereas scalars with lowercase normal ones. The symbol I n represents the n × n identity matrix. Zero vectors and matrices of proper dimension are denoted as 0 and O, respectively. For a vector x ∈ R h , its p-norm is denoted as x p , p ≥ 2 and for p = 2 simply x . Similarly, if A ∈ R h×l , A is the matrix norm induced by the Euclidean norm of vectors. Given a symmetric matrix A, i.e., A = A T , its smallest and largest eigenvalues are λ min (A) and λ max (A). If λ min (A) > 0 (or ≥ 0), then A is said to be positive definite (or semidefinite), and it is denoted as A > 0 (or A ≥ 0). To simplify the notation, arguments of the functions are omitted when clear from the context. Furthermore, given a continu- x (f ) ∈ R h×h denote, respectively, the gradient and Hessian of f . For g(x) : R h → R l , its Jacobian is J x (g) ∈ R l×h .

B. Dynamic Model
Consider a continuum soft robot described by where θ ∈ R n denotes the configuration vector, M θ (θ) > 0 is the mass matrix, C θ (θ,θ)θ collects Coriolis and centrifugal effects, and G θ (θ) = ∇ θ (U g (θ)) contains the forces due to gravity, being U g (θ) the gravitational potential energy. Elastic and dissipation forces are modeled, respectively, through K θ θ and D θ (θ)θ, with K θ > 0 and D θ (θ) > 0. Finally, τ ∈ R m groups the generalized input forces, projected into the configuration space through the constant actuation matrix A ∈ R n×m , and d ∈ R m is a constant disturbance, i.e.,ḋ = 0. In [18] it is shown that (1) includes a reasonably large class of soft robots. Indeed, if the columns of A are linearly independent, it is always possible to define a linear change of coordinates q = T θ such that the dynamics becomes where the configuration vector q has been divided into actuated and unactuated variables, denoted from now on q a ∈ R m and q u ∈ R n−m , respectively, and the dynamic terms have been partitioned accordingly.

C. Properties
Without loss of generality, the dynamic model (2) satisfies a set of properties previously reported for robots with revolute joints [20], which we list below for the sake of readability.
Property 1: There exist constants k M , k m > 0 such that, for all q, x ∈ R n , Property 2: There exists a constant k C > 0 such that, for all q,q ∈ R n ,

D. A Class of Saturated Functions
In Sections III and IV, control laws for shape regulation of two classes of underactuated soft robots are presented. These extend the controllers of [17], [18] by adding an integral action driven by the following class of saturated functions.

Property 4: ([21]) Given
hβ, the following inequalities hold Property 5: Proof: We prove only the right-hand side of the above inequality because similar arguments can also be used for the left-hand side.
Furthermore, the monotonicity of s(y) and (i) imply that σ(y) > 0 and, consequently, Property 7: Given k 4 = α 2 and k 5 = α 3 , the following inequalities hold Proof: The first inequality follows observing that J x (s(x)) is a diagonal matrix. The second is an immediate consequence of |s( The set S takes inspiration from previous works on PID control with saturated integral action of rigid robots [21], [23]. However, the actual context does not allow using the class of saturation functions proposed in those references because Property 6, which enables exploiting the elasticity in the control laws and stability proofs, is not generally valid.

III. P-SATI-D+ CONTROL FOR ELASTICALLY DECOUPLED SOFT ROBOTS
This section presents a PID controller with saturated integral action and gravity cancellation for shape regulation of elastically decoupled soft robots. To this end, we first introduce the following definition.
Definition 2: A soft robot is said to be elastically decoupled if there is no elastic coupling between the actuated and unactuated variables, i.e., Note that no special condition on K uu is needed, except that K uu > 0. To extend the PD+ regulator presented in [17], we add a saturated integral action driven by the tracking error of the actuated variables. In particular, we consider the collocated control law whereq a := q a,d − q a is the tracking error with q a,d the desired set point, The following theorem shows that the control law (5), which we call P-satI-D+, guarantees bounded closed-loop trajectories and asymptotic convergence ofq a to 0, provided that K P and γ are appropriately tuned.
Theorem 1: For an elastically decoupled soft robot there exist finite constants α P > 0 andγ > 0 such that, for all K P > α P I m and γ >γ, the trajectories of the closed-loop system (2)-(5) are bounded and converge asymptotically to the equilibrium (q a q uqaqu ) = (q a,d q u,d 0 0), where q u,d is a solution to Proof: Defining the additional state variables the P-satI-D+ can be rewritten as which leads to the closed-loop (3), shown at the bottom of this page. Note also that z has the same dynamics of the integral. Consider the Lyapunov-like function which is radially unbounded since and the right-hand-side of the above inequality diverges to +∞ as (q a q uq z) tends to ∞.
Taking the time derivative of V along the trajectories of the closed-loop system yieldṡ

Replacing (3) in the above equation one obtainṡ
In view of Properties 4-6, for q a ≤ β,V is bounded bẏ According to Sylvester's criterion, Q is positive definite if and only if Q 11 > 0, det Q > 0.
Both conditions can be fulfilled by taking .
On the other hand, for q a > β, it is possible to writė V < −Q 11 q 2 + Q 12 k 3 q − k 2 Q 22 β, which is a quadratic function in q pointing downward. Consequently,V < 0 if and only if its minimum value is negative, i.e., which is satisfied for .
The thesis follows applying the Global Invariant Set theorem [24], and noting thatV = 0 if and only ifq a = 0 anḋ q = 0. Remark 1: Without further assumptions other than (4), there might exists more than one equilibrium for the unactuated variables compatible with that reached by the actuated ones. Thus, even if q a converges asymptotically to q a,d , the system does not have a globally asymptotically stable equilibrium in general.

IV. P-SATI-D CONTROL FOR ELASTICALLY DOMINATED SOFT ROBOTS
In this section, we present our main result for elastically dominated soft robots. First, the following statement defines what an elastically dominated soft robot is.
Definition 3: A soft robot with stiffness matrix is said to be elastically dominated if it satisfies λ min (K uu ) > k ∂G . (9) The inequality (9) guarantees that the potential field is convex with respect to the unactuated variables, implying that (6) admits a unique solution q u,d for all q a,d . As a consequence, q d = (q T a,d q T u,d ) T can be rendered globally asymptotically stable (GAS) by using the P-satI-D control law whereq = q d − q is the tracking error, K P > 0, K D ≥ 0, K I > 0, γ > 0 are control gains, and s a (q) = s(q a ) denotes the first m components of s(q) ∈ S(β, α 1 , α 2 , α 3 ). In analogy with Theorem 1, the following result formalizes that, if K P and γ are large enough, then (10) ensures that q d is a GAS equilibrium for the closed-loop system. Theorem 2: For an elastically dominated soft robot there exist finite constants α P > 0 andγ > 0 such that, for all K P > α P I m and γ >γ, the trajectories of the closed-loop system (2)-(10) are bounded and converge asymptotically to the equilibrium (q a q uqaqu ) = (q a,d q u,d 0 0) where q u,d is the unique solution to G u (q a,d , q u ) + K ua q a,d + K uu q u = 0. (11) Proof: To prove that (11) admits a unique solution, consider the function P u (q u ) : R n−m → R defined as where U g is bounded and K > 0. Thus, P u has (at least) a local minimum. Noting that ∇ q u (P u ) = G u (q a,d , q u ) + K ua q a,d + K uu q u , it follows that q u,d is an extremum of P u . Furthermore, (9) implies that the Hessian of P u is positive definite. Thus, P u is strictly convex with unique global minimum given by the unique solution of (11). Defining the additional state variables and exploiting the identity G u (q d ) + K ua q a,d + K uu q u,d = 0, the closed-loop dynamics takes the form (12), shown at the bottom of this page, where − k 3 k M q , and the right-hand-side of the above inequality grows unbounded as (qq z) tends to ∞.
Taking the time derivative of V yieldṡ After some computations, (12) leads tȯ Moreover, the following inequalities holḋ Similarly, by the Mean Value theorem for vector-valued functions [25], it is possible to write Recalling that J x (G(x)) is symmetric, and invoking Properties 3 and 6, it follows Proceeding as in the proof of Theorem 1, we obtain, for q ≤ β, According to Sylvester's criterion, Q > 0 if and only if By Schur's criterion, (13) reduces to and The first inequality holds from (9), while the latter can be enforced by choosing K P large enough. Note that a sufficient + C(q,q) condition to satisfy (15) is λ min (K P ) > α P := − λ min (K aa ) + λ max (K I ) + k ∂G Finally, (14) is guaranteed if Conversely, when q > β,V is bounded bẏ or, equivalently, The thesis follows by definingγ := max{γ 1 ;γ 2 } and applying again the Global Invariant Set theorem. Remark 2: In contrast to elastically decoupled soft robots, elastically dominated ones are stabilizable via simpler regulators since almost no knowledge of the dynamic parameters is needed, and only the actuated variables must be measured. However, both controllers do not require the robot stiffness because they learn the static elastic forces during the task execution. As a result, these control laws are more robust with respect to those of [17], [18].
Remark 3: Thanks to the natural damping in the dynamics, the controllers (5) and (10) do not need the derivative action, i.e., K D can be set to zero. However, even if it does not affect stability, the damping injection may play a fundamental role in the closed-loop performance.
Remark 4: The proposed control laws stabilize the dynamics also under fully actuated approximations, i.e., when m = n.

V. SIMULATION RESULTS
Here, we validate the results of Sections III and IV through extensive simulations on different planar soft robots moving in the vertical plane rotated so that the gravitational force yields a destabilizing effect on the dynamics. We model the robots under the piecewise constant curvature (PCC) assumption. A couple applied at its distal ends bends each actuated segment.
In all the simulations, the function s(x) = s p (x); p = 2, is used for the integral action, which is initialized to zero, and the robot starts at rest in the straight configuration, i.e., q(0) = 0 [rad] andq(0) = 0 [rad/s].
See the multimedia material for a video of the simulations.

A. Simulation 1 (Elastically Decoupled and Dominated)
Consider a planar soft robot with two actuated segments, each discretized into four CC segments. Consequently, the actuation matrix takes the form [7]  Being K θ diagonal and large enough, the robot is elastically decoupled and dominated. As a consequence, both the P-satI-D+ (5) and P-satI-D (10) control laws can be used. The reference for the actuated variables is (in [rad]) which results in the overall desired configuration Figs. 2(a) and (d) report the time evolution of the configuration variables under the P-satI-D+ and the P-satI-D, respectively. Both controllers impose, as expected, the desired configuration; however, the P-satI-D+ performs better than the P-satI-D. Indeed, the former yields slightly smaller overshoots compared to the latter. The P-satI-D+ learns only the static elastic forces at q d , whereas the P-satI-D also the gravitational torques. Consequently, the integral of the P-satI-D accumulates a larger error, resulting in higher peaks at the beginning of each motion. Note that the performance of the P-satI-D could be improved by providing an estimate of G a (q d ), if available. Figs. 2 Fig. 3(a) shows the time evolution of the configuration variables. The actuated variable quickly converges to the desired value, while the unactuated ones remain bounded and reach an equilibrium point. As expected, the q u variables oscillate significantly because we do not have direct control authority over them. Furthermore, the Coriolis and centrifugal forces couple the evolution of q u with q a . Since the regulator does not perform any unnecessary cancellation of model terms for stability, the evolution of q u , which mainly depends on the  system damping, may also affect q a . The control torque, shown in Fig. 3(b), increases in magnitude as soon as d appears, and the integral action quickly compensates for the external disturbance. Because of the excessive weight, the actuator collapses, and the task is accomplished by moving mainly the tip, as it appears from the stroboscopic plot of the robot in Fig. 3(c).

C. Simulation 3 (Elastically Dominated)
Finally, consider a conical-shaped tentacle of isotropic material. The robot is modeled through four CC segments with length Differently from the previous simulation, we choose K P = 0.3 [Nm/rad] and K I = 0.6 [Nm/rads]. Being the stiffness large enough, the potential field is convex and the robot is elastically dominated with again q a = θ 1 + θ 2 + θ 3 + θ 4 . Thus, the P-satI-D law (10)  . Figs. 4(a) and (b) illustrate the closed-loop evolution of the configuration variables and of the control input, respectively. As expected, q converges to the desired set point. Since the elastic forces support the arm against gravity, the curvatures attain similar values, and the robot no longer collapses, in contrast to Simulation 2. This fact can also be seen in Fig. 4(c), which illustrates the robot motion.

VI. CONCLUSIONS AND FUTURE WORK
In this letter, we proposed new classes of controllers for elastically decoupled and elastically dominated soft robots. For the former, the actuated variables can be regulated to any desired target configuration through a PID with saturated integral action and with gravity cancellation. Along the same line of thought, elastically dominated soft robots can be globally asymptotically stabilized using a simple PID controller with saturated integral action. The integral yields two benefits: (i) it does remove from the control law a term that accounts for the elastic forces, and (ii) it makes the closed-loop robust with respect to matched constant disturbances. Finally, the theoretical results have been verified through extensive simulations. Future work will be devoted to the experimental validation of these controllers, and to the study of tuning techniques for the proposed regulators.