key: cord-0575619-pt6cr5jb
authors: Gehlhar, Rachel; Ames, Aaron D.
title: Model-Dependent Prosthesis Control with Interaction Force Estimation
date: 2020-11-10
journal: nan
DOI: nan
sha: c8644086585bb8df5cf81fc3b20f304a858dc79f
doc_id: 575619
cord_uid: pt6cr5jb

Current prosthesis control methods are primarily model-independent - lacking formal guarantees of stability, relying largely on heuristic tuning parameters for good performance, and neglecting use of the natural dynamics of the system. Model-dependence for prosthesis controllers is difficult to achieve due to the unknown human dynamics. We build upon previous work which synthesized provably stable prosthesis walking through the use of rapidly exponentially stabilizing control Lyapunov functions (RES-CLFs). This paper utilizes RES-CLFs together with force estimation to construct model-based optimization-based controllers for the prosthesis. These are experimentally realized on hardware with onboard sensing and computation. This hardware demonstration has formal guarantees of stability, utilizes the natural dynamics of the system, and achieves superior tracking to other prosthesis trajectory tracking control methods.

Powered prostheses are generally controlled by modelindependent methods such as impedance control [1] , [2] , [3] . These methods rely on heuristic tuning methods to achieve good behavior, lack formal guarantees of stability, and do not utilize the natural dynamics of the system. In [4] , some model-dependence was incorporated into prosthesis control methods to achieve a robust controller. However this method did not account for the interaction force between the human and the prosthesis, which acts as an input to the prosthesis dynamics. The methods in [5] , [6] accounted for the interaction force in constructing a feedback linearizing controller for a prosthesis that was demonstrated in simulation. Generalizing these ideas, in [7] , [8] , the authors introduced the notion of separable systems and defined a class of RES-CLF controllers to yield provably stable hybrid periodic orbits for separable systems with zero dynamics.

CLFs provide formal guarantees of stability and RES-CLFs [9] in particular give strong enough conditions for hybrid systems (systems with impacts) with zero dynamics (uncontrollable dynamics) [10] . Quadratic programs (QPs) provide a means to implement a CLF constraint while optimizing a cost and provide a flexible framework to incorporate feasiblity constraints such as torque bounds. CLFs in QPs have been realized in simulation in various works [11] , [12] , [13] , [8] , but few to date on hardware [14] . One difficulty in implementing these controllers on hardware is the typical required inversion of the inertia matrix, which is computationally expensive and prone to error. An alternative CLF-QP was developed in [15] using an inverse dynamics (ID) approach to overcome this challenge and achieved dynamic crouching behavior in experiment on a 3D underactuated compliant bipedal robot. This ID-CLF-QP is the starting basis for developing an implementable CLF-QP on our robotic prosthesis.

When trying to implement a CLF-QP on a prosthesis, an additional challenge arises since the human dynamics are unknown. While [16] applied a CLF-QP to a prosthesis, this was done in a model-independent fashion and required a feed-forward impedance control input term to overcome the limitations of the model-independent nature. To implement a model-based prosthesis controller, knowledge of the interaction force between the human and prosthesis is required. While force sensors could provide these measurements, they are expensive, noisy, and not robust to the multi-directional forces and torques present in walking. These conditions of force sensors pose implementation issues for using their measurements directly as real-time feedback and restrict prosthesis controllers from being fully model-dependent.

The main result of this paper is the synthesis of modeldependent controllers using force estimation. We leverage RES-CLFs and their formal guarantees in the context of the ID-CLF-QP framework. The unknown dynamics of the human enter the prosthesis dynamics via interaction forces, so we estimate these forces to complete the model-dependent nature. Inspired by the average acceleration discrete algorithm in [17] , we developed a force estimation method with on-board velocity measurements. To demonstrate these results, we realize the controller on-board the AMPRO3 prosthesis [18] , shown in Fig. 1 . In particular we demonstrate that the model-based ID-CLF-QP results in accurate tracking. More generally, we are thus able to transfer the formal guarantees afforded by RES-CLFs to hardware, with the result being stable prosthesis locomotion in practice.

In this work, Section II overviews the separable system framework used to develop a RES-CLF for the prosthesis based solely on local information. Section III lays out our specific controller construction for a robotic subsystem. This section describes the discrete force estimation method and how the controller is respectively formed to be incorporate this estimate. The force estimate completes the prosthesis subsystem dynamics to enable model-dependent prosthesis control which we demonstrate in simulation in IV and experiment in V, yielding provably stable human-prosthesis walking. The main contribution of this paper is the first realization of fully model-dependent prosthesis control, bringing the human into the loop of prosthesis control with strong formal guarantees of stability.

To support the main contribution of this paper, we first review how these formal guarantees are established for a robotic subsystem like the prosthesis. Since the prosthesis is connected to a human, it is not an independent system, but rather a subsystem of a larger system, of which it does not have full information. We show how to develop a class of controllers for a robotic subsystem that use only local information but lead to stability guarantees for the full-order separable robotic system. Robotic Control System. For an η DOF robotic system in 2D space, consider the coordinates q = (q T l , q T f , q T s ) T ∈ R η which define the configuration space Q. To create a separable robotic system, we consider the portion of the system defined with coordinates q s ∈ R ηs to be a robotic subsystem that is rigidly attached with a 3 DOF fixed joint (x, z Cartesian position, and pitch), with coordinates q f , to the rest of the system with coordinates q l ∈ R η l , where η l +η s +3 = η. The subsystem has m s actuators and the rest of the system has m r . This Euler-Lagrange equation with positional constraints gives the dynamics [19] :

Here D(q) is the inertia matrix; H(q,q) the vector sum of Coriolis, centrifugal, and gravity forces; B the actuation matrix, λ h = (F T f , λ T g ) T ∈ R 3+ηg the constraint wrenches to enforce the fixed joint and the η h contact holonomic constraints, respectively; and J h (q) the Jacobian of the holonomic constraints of the fixed joint and contacts. These dynamics and constraints can be used to solve for λ h by,

(3) Robotic Subsystem. By defining floating base coordinates q B ∈ R 3 for the subsystem at the connection point with the rest of the system, we can define the robotic subsystem with its own configuration coordinatesq = (q T B , q T s ) T ∈ Rη, with η = 3 + η s , and write the constrained subsystem dynamics,

HereJ h (q) is the Jacobian of theη h holonomic constraints for the contacts of the subsystem with constraint wrench λ h , and F f is the interaction forces and moment (we call interaction force for simplicity) between the subsystems given as input to these subsystem dynamics, projected to the base coordinates withJ f . Separable Subsystems. We can write the robotic full-order dynamics (1) as an ODE using the states x q = (q T ,q T ) T :

our ODE takes the following form:

The 0 appears in the actuation matrix g(x) because the fixed joint present in this system completely decouples the subsystem dynamics from the actuation u r of the rest of the system since all the interaction goes through the constraint wrench for the fixed joint. See [7] for details. Since q f ∈ x r , the control input u s still affects the dynamics of x r (t). In [7] , this system was defined as a separable control system, which has the unique attribute that the dynamics of x s (t) only depends on u s and not u r . We separate this separable system into a separable subsystem and remaining system [7] , [8] , defined respectively:

Equivalent Subsystem We can write the robotic subsystem dynamics (4) as an ODE following a method similar to that used for the full-order dynamics, but this time only the dynamics of x s (t) = (q T s ,q T s ) T are used such that we obtain an alternative expression for the dynamics of x s (t) [7] :

For this subsystem to equate to (7) , there must exist a transformation T (x) = X that yields the following conditions: f s (x) =f s (X ) and g s (x) =ḡ s (X ) for all x. This transformation exists for this robotic system and is given in [7] . While the separable subsystem (7) still depends on the full-order states x, this equivalent subsystem [7] only depends on local states and measurable states and inputs. In practicex r can be measured with an IMU and ζ with a force sensor.

Separable Subsystem RES-CLF. Now that the subsystem is defined in local coordinates X , a whole class of modeldependent controllers can be constructed for the subsystem in terms of locally available information. In [8] we defined a RES-CLFV s ε (x s ) for an equivalent separable subsystem such that for all 0 < ε < 1 and X ∈ R ns+nr+n f ,

wherec s 1 ,c s 2 , andc s 3 are positive constants. This leads to a class of controllers that satisfyV s

The ε term was included in the formulation of CLFs in [9] to give a faster rate of convergence for hybrid systems such that the system and its zero dynamics would not be destabilized by the impacts present in the hybrid system. Main Theoretic Idea. The work of [8] proved when a RES-CLF stabilizes the remaining system (8), any controller u s in this classK s ε (X ) guarantees stability for the fullorder hybrid system with zero dynamics. Hence, this novel methodology of separating a robotic system and creating an equivalent subsystem enables construction of modeldependent subsystem controllers with only local information while guaranteeing stability of the full-order system and utilizing the natural dynamics. For the human-prosthesis system, we assume the human stabilizes itself since central pattern generator research suggests biological walkers exhibit stable rhythmic behavior [20] , (i.e. have limit cycles), and our class of RES-CLF controllers in [8] for the remaining human system includes all stabilizing controllers for these hybrid limit cycles.

To construct the ID-CLF-QP of [15] we construct a RES-CLF for the robotic subsystem and formulate it in a QP without inverting the inertia matrix. Our ID-CLF-QP has an additionalJ T f (q)F f term in the dynamics as in (4) to account for the interaction force between the subsystems. We finally formulate this controller in a hardware implementable way with force estimation to arrive at the form used to achieve provably stable prosthesis control in experiment.

To construct the ID-CLF-QP of [15] , we form subsystem outputs with which we construct our RES-CLF using the methods of [9] . We show how the ID-CLF-QP incorporates this RES-CLF without inverting the dynamics. CLF Construction. To enforce desired trajectories on our robotic subsystem, we define linearly independent outputs,

where y a s (x s ) are the actual outputs and y d s (τ (x s ), α) are the desired outputs defined by parameters α and modulated by the state-based phase variable τ (x s ) [10] . For our robotic application, we consider position modulating outputs. We take the derivatives alongf s (X ) andḡ s (X ) to relate the outputs to the control input u s :

Here L 2 f s y s (X ) and LḡsLfs y s (X ) are Lie derivatives [21] and LḡsLfsy s (X ) is invertible since the outputs are linearly independent. Hence our system is feedback linearizable [21] and our feedback linearizing controller is,

where ν is our auxiliary control input and by construction ν =ÿ s . By applying this control law, our output dynamics are linearized and can be written as a linear system with coordinates ξ = (y T s ,ẏ T s ) T ,

Using this linear system we construct a CLF by solving the continuous time algebraic Riccati equation (CARE),

for P = P T > 0, with the user selected weighting matrix Q = Q T > 0. From the method of [9] , we construct a RES-CLF for our subsystem by the following:

To obtain our convergence constraint, we take the derivative,

with Lie derivatives along the linearized output dynamics as,

ID-CLF-QP+F f . To formulate the ID-CLF-QP in terms of X , we write this RES-CLF and its derivative in terms of x s and X since ξ depends on x s , through the outputs y s (x s ) andẏ s (x s ), and ν depends on X through the relationship, obtained from (13):

This gives us the subsystem RES-CLF (10) wherec s 1 = λ min (P ).c s 2 = λ max (P ), andc s 3 = γ. The expression (14) requires multiple inversions of the inertia matrixD(q) and holonomic constraint termJ h (q) which is computationally expensive and prone to numerical error. To avoid this, we recall ν =ÿ s and rewrite the outputs y s (x s ) in terms of the robotic subsystem's configuration coordinatesq, since it is a positional constraint: y s (q).

Shown in [15] , for position-modulating outputs y s (q), we can equivalently writeÿ s as,

To find a control input u s close to the feedback linearizing controller (13) with PD gains on our output accelerations, ν = K p y s (x s ) + K dẏ s (x s ) := ν pd , we minimize the difference between (15) and ν pd in our QP cost. We also include the holonomic constraints in the cost as soft constraints since these are difficult to satisfy exactly on hardware. Considering the variables Υ = [q T , u T s ,λ T h , δ] T ∈ R ηv , with η v = η + m s +η h + 1, and using the terms,

we formulate our ID-CLF-QP+F f :

is a regularization term to make the problem well posed, σ and ρ are weighting terms, and δ is a relaxation term such that the torque bounds (−u max , u max ) can always be met. (The arguments on J y ,J y are left out for notational simplicity.) This controller selects the joint accelerationsq, control input u s , and holonomic constraint wrenchλ h to satisfy the robotic subsystem dynamics (4) and the subsystem RES-CLF (10) while optimally aiming to satisfy the holonomic constraints (5) and smoothly track the desired trajectories.

Implementing this controller on hardware requires knowledge of the interaction force F f . Since a force sensor is not available on the prosthesis platform we developed a method to estimate the interaction force using discrete calculations of acceleration. We include this estimated term in the dynamics of our QP and realize the QP at sample time to implement on hardware. Force Estimation. We estimate the joint accelerationq est based on the discrete velocity measurements and time,

where k represents the current time step and k −1 represents the previous time step. Finding the difference between our estimated acceleration and the expected acceleration based on the dynamics from the previous time step,

we multiply this by the inertia matrix of the previous time step to obtain what we consider the residual dynamics F k−1 : We essentially back-calculate the interaction force that caused the acceleration difference. Note (18) cancels D(q k−1 ) in (17), such that inertia matrix inversion is not required. To obtain a smoother signal, we average the residual dynamics measurements for N time steps:

By calculating the force projected into joint space, we are smoothing the exact signal we input to the dynamics and do not need a pseudo-inverse ofJ f . ID-CLF-QP+F est . We replaceJ T f F f of (16) with F avg k−1 and evaluate the QP at sample time:

Although we use the residual dynamics estimate from the previous time step to model the dynamics at the current time step, when run in a controller at a high enough frequency this method should capture the residual dynamics well enough.

To demonstrate this ID-CLF-QP+F est we first apply it to a prosthesis model in simulation while the human portion of the system is controlled by a method unknown to the prosthesis. The accuracy of the force estimation is also tested. Amputee-Prosthesis Model. We construct an amputeeprosthesis model as a planar bipedal robot comprised of 8 links: torso, 2 human thighs, prosthesis partial thigh, a human and prosthesis calf, and a human and prosthesis foot. The interface between the human right thigh and prosthesis partial thigh is modeled as a 3 DOF fixed joint, as described in Section II, giving η = 12. The subsystem coordinates of the prosthesis are knee θ pk and ankle pitch θ pa , q s = (θ pk , θ pa ) T , giving η s = 2. The rest of the system coordinates are the floating base coordinates θ B , and the pitch of the human's left hip θ lh , left knee θ lk , and right hip θ rh : q r = (θ T B , θ lh , θ lk , θ rh ) T . See Fig. 2 . All the pitch joints are actuated, making m r = 4 and m s = 2.

The human parameters are obtained with a subject's height and weight and the parameters in [22] , [23] . The prosthesis parameters are based off of the prosthesis platform AMPRO3 [18] used in this study. We use this prosthesis model to obtain the dynamics of (4) for use in (20) on the prosthesis platform, but omit the ankles in trajectory generation and simulation because it is more comfortable for the human user to have the prosthesis ankle have varying set point PD control instead of following a trajectory.

Hybrid Systems and Human-Like Gait Generation. To account for both the continuous and discrete dynamics in human walking, we model it as a hybrid system [24] . Because the human-prosthesis system is asymmetric, we consider two continuous domains, D ps for prosthesis stance and D pns for prosthesis non-stance. These domain indices are denoted as v ∈ {ps, pns}. Each domain has a holonomic constraint on the respective stance foot. The domains are connected by events in a directed graph, specifically the event when the non-stance foot strikes the ground. The impact dynamics for these transitions are explained in [25] .

To find a human-like walking trajectory for the model, human walking motion capture data is taken and Bézier polynomials are fit to the joint trajectories. A state-based phase variable, going from 0 to 1, modulates the trajectories [10] . We run an optimization to minimize the difference between the outputs (the joints) while satisfying the dynamics (1), feasibility constraints, and a hybrid zero dynamics condition [10] such that the outputs are invariant through impact. The optimization solution gives parameters to define the desired trajectories y d s (τ (x s ), α) and the outputs to simulate the human side. See [26] for details. Fig. 3 shows the resulting trajectories match the human data well. By finding a prosthesis knee trajectory similar to a human's knee trajectory and is provably stable when the rest of the system is following the human-like trajectories, we assume the human can still stabilize itself with the prosthesis. Hence the condition required for our main theoretical idea is satisfied.

Simulation Results. We restrict our attention to implementing the proposed controller in the stance domain D ps where the interaction force is the largest and the prosthesis' stability is critical as it supports the human. In practice we calculate the base coordinatesq B , base velocitiesq B , and phase variable τ (x s ) with inverse kinematics using the knee and ankle data and assuming the foot is flat on the ground. The swing domain D pns requires an IMU to provide information about this domain's main unknown, the base coordinates. This remains for future work. We prescribe a feedback linearizing control law to the human side to closely track the human-like trajectories in simulation. Variations of the ID-CLF-QP controller are implemented on the prosthesis in stance and a feedback linearizing control law in swing to enforce the output (12), where y a s (x s ) = θ pk . The ID-CLF-QP+F f is implemented with the exact interaction force F f calculated with (3), since F f ∈ λ h , based on a feedback linearizing control law u. The ID-CLF-QP+F est used the force estimator (19) with N = 1 since averaging is unnecessary in simulation. Finally the ID-CLF-QP was used without any interaction force information.

The resultant control inputs are shown in Fig. 4a and tracking results in Fig. 5 . The ID-CLF-QP+F f and ID-CLF-QP+F est achieved practically exact tracking results and had very similar control inputs. This suggests the force estimator estimates the force well enough to give similar performance as when using the exact force. The ID-CLF-QP with no consideration of the interaction force outputs a very different control input and had terrible tracking, indicating the significance of accounting for the force. To compare the force estimate with the actual computed force, the summation of the constraint wrenches and interaction force projected into joint space is taken since the constraint wrench calculation for the subsystem controller (20) is coupled with the interaction force estimate and hence they cannot be individually compared with the constraint forces and interaction force calculated with the full-order dynamics (1). Fig. 4b compares the actual force components calculated by (J h (q)J f (q))λ h to the estimated force componentsJ f (q))λ h + F, showing the force estimation works with high accuracy.

The platform used to demonstrate the model-based control method is described in this section followed by experimental results of the proposed controller. The results verify this controller meets our formal condition for exponential stability and it outperforms the less model-dependent controllers. Prosthesis Platform AMPRO3. The custom-built powered prosthesis AMPRO3 used in this work is described in [18] , and briefly described here. The device has an iWalk adapter such that an able-bodied human can test the device. A different adapter can be used to connect this device directly into an amputee's socket. The mechanical design consists of a knee and ankle pitch joint which are both controlled with their own DC brushless motors (MOOG BN23), with about 1 Nm peak torque. Each motor is connected to a harmonic gearbox through a timing belt. Both the timing belt and harmonic gearbox contribute to the mechanical reduction for each joint: 120 for the knee and 175 for the ankle.

Each motor is controlled by an ELMO motion controller (Gold Solo Whistle) which receives position and velocity feedback from incremental encoders and receives input from the microprocessor. A Beaglebone Black (BBB) microprocessor runnning at 200 Hz handles all of the computations on board, taking input from the motion controllers and outputting a commanded torque to the motion controllers. The controller algorithms are coded in C++ packages and run with ROS. The whole prosthesis system is powered by a 9-cell 4400 mAh Li-Po battery (Thunder Power RC). The components described here can be seen in Fig. 2 .

Hardware Results. The ID-CLF-QP+F est was implemented on the prosthesis platform in stance (with N = 10 in (19) ) and superior trajectory convergence and tracking were achieved compared to a model-independent PD controller and the ID-CLF-QP controller without consideration for the force. An able-bodied human tested the device in walking for over 20 consecutive steps with each controller. The ankle had a PD controller with varying set point. A PD controller was applied to the knee in swing, but did not perfectly converge to the trajectory. Hence the output starts off the trajectory in stance, explaining the jump present in the desired trajectory in Fig. 5 . However, the ID-CLF-QP+F est recovers from this disturbance and converges to the trajectory, demonstrating the advantage of the exponential convergence of a modelbased RES-CLF. Fig. 5 also shows the significant tracking improvement exhibited by the ID-CLF-QP+F est in stance compared to the other controllers. The rapid convergence and superior tracking are two important results of this work.

Main Result. The primary result of this work is implementing a model-dependent controller on a prosthesis with formal guarantees of stability. Fig. 6 shows this result where the CLF derivative is plotted with its stability bound, indicating the prosthesis satisfies this formal guarantee of stability. (The slight breaking of the bound is due to the relaxation term in the CLF-QP). When the CLF condition is well below its bound, the control input, shown in the bottom of Fig.  6 , has a small magnitude because the controller is letting the natural dynamics of the system bring it to its desired trajectory. This effect is especially significant considering the prosthesis starts off the trajectory at the beginning of the stance phase and this precisely demonstrates the advantage of model-dependent control over model-independent control. A controller without model information would respond to the large error with a large torque which would require more energy and the sudden movement could cause discomfort to the user. This model-dependent controller, on the other hand, allows the natural dynamics of the system to bring the prosthesis to its desired trajectory without using more energy and yielding a less aggressive movement for the user.

(Note: Due to COVID-19 restrictions, the results of this study are restricted to one subject. Future work will demon- strate the control method on more subjects.)

In this work, the novel methodology of developing RES-CLFs for separable systems [7] , [8] is realized on a prosthesis platform, demonstrating the first experimental realization of a model-dependent prosthesis controller that accounts for interaction forces. As such, this is the first instance of realizing prosthesis control with formal guarantees of stability for the full-order hybrid system with zero dynamics. These guarantees with consideration for the interaction forces ensure safety of the user and a responsiveness to the real-time dynamics are novel relative to existing prosthesis control methods. Being able to implement model-dependent controllers on a prosthesis platform opens the door to applying various nonlinear control techniques to prostheses and other robotic subsystems, thereby improving performance.

Future work will apply this control method in the swing phase by incorporating an IMU into the prosthesis platform. Ways to improve the accuracy of the force estimation method will also be investigated. Episodic learning could adapt our force estimate to systematically reduce uncertainty while maintaining stability, similar to the work of [27] . Including a force sensor at the socket could measure the interaction force in real-time and using the estimation method presented in this paper along with the aforementioned learning method could address the issues of measurement noise and uncertainty.

Design and control of a powered transfemoral prosthesis

Virtual constraint control of a powered prosthetic leg: From simulation to experiments with transfemoral amputees

Configuring a powered knee and ankle prosthesis for transfemoral amputees within five specific ambulation modes

Robust control of a powered transfemoral prosthesis device with experimental verification

Hybrid invariance and stability of a feedback linearizing controller for powered prostheses

American Control Conference (ACC)

Stable, robust hybrid zero dynamics control of powered lower-limb prostheses

Control of separable subsystems with application to prostheses

Separable control lyapunov functions with application to prostheses

Rapidly exponentially stabilizing control Lyapunov functions and hybrid zero dynamics

Feedback control of dynamic bipedal robot locomotion

Embedding of slip dynamics on underactuated bipedal robots through multi-objective quadratic program based control

Optimal robust control for bipedal robots through control lyapunov function based quadratic programs

Time dependent control lyapunov functions and hybrid zero dynamics for stable robotic locomotion

Torque saturation in bipedal robotic walking through control lyapunov functionbased quadratic programs

An inverse dynamics approach to control lyapunov functions

Quadratic program based control of fully-actuated transfemoral prosthesis for flat-ground and up-slope locomotion

Average acceleration discrete algorithm for force identification in state space

Preliminary results on energy efficient 3D prosthetic walking with a powered compliant transfemoral prosthesis

A Mathematical Introduction to Robotic Manipulation

A model of the neuro-musculo-skeletal system for human locomotion

Nonlinear Control Systems

Anatomical data for analyzing human motion

Geometry and inertia of the human body -review of research

Human-inspired control of bipedal walking robots

Models, feedback control, and open problems of 3d bipedal robotic walking

Data-driven characterization of human interaction for model-based control of powered prostheses

Episodic learning with control lyapunov functions for uncertain robotic systems*