Trajectory Planning for Closed Kinematic Chains Applied to Cooperative Motions in Health Care

Physical interaction of humans can be a challenging. For some practically relevant situations a reasonable model is given by a closed kinematic chain of a planar rigid body mechanism. From a control perspective a cooperative motion, e. g. assisted standing‐up, can be seen as an optimal control problem (OCP) with input and state restrictions. Due to the algebraic constraints and the comparably high number of joints involved, proper formulation of such a problem is not trivial. In this contribution we compare two different modeling approaches: discretization of the full dynamical problem and iterative solution of consecutive stationary problems. The latter turns out to be significantly faster while still providing – in some sense – optimal solutions.


Introduction: Modelling of Cooperative Standing-Up Motion
Motivated e. g. by [1] and references therein we consider the basic mechanical aspects of cooperative standing-up motion, which takes place e. g. in geriatric care. The goal of this contribution is not to provide realistic numbers but rather a methodology to simplify further research. Following [1] we model each of the two persons (care giver and receiver) as chains ofn L = 3 links (rigid bodies) connected byn J actuated revolute joints and assume that both chains are fixed to the same basis (i. e. the ground) and the motion is entirely planar. We further assume that the torque which can be applied to each joint is bounded. The connection of the two kinematic chains (i. e. persons) is also modeled as an (unactuated) revolute joint. The resulting mechanism thus has n L = 2n L = 6 links and n J = 2n J + 1 = 7 joints. We denote the configuration coordinates (actuated joint angles) with (θ 1 , ..., θ 2nJ ) T =: θ and the respective torques with (τ 1 , ..., τ 2nJ ) T := τ . The degree of freedom of this single closed loop is F = n J − 3 = 4, cf. [2, Section 1.2.1]. The closing condition of the loop -enforcing that both independent link sequences have the connection joint as common point -introduces a constraint expressable as two scalar algebraic equations.
To derive a dynamical model for such closed kinematic loops, literature offers various approaches, where the three main lines can be summarized as follows: a) directly using the differential algebraic equation (DAE) system for further steps such as motion planning and simulation, b) replacing the rigid connection by a virtual spring [3] and c) symbolic resolution of the algebraic constraints and deriving an ODE system in minimal coordinates. However, for approach c) the symbolic resolution is only feasible for "small" systems such as four-bar linkages [4], whereas in b) the virtual spring might provoke a stiff ODE and introduce artificial oscillations.
Hence, we concentrate on approach a), which can be based on the Lagrangian equations of the first kind. For the above two chains we denote the Cartesian coordinates of their endpoints by E 1 (θ) and E 2 (θ), respectively, and formulate the closing condition term to be a(θ) Thereby, A(θ) = ∂a ∂θ is the Jacobian of the constraint vector, (λ 1 , λ 2 ) T =: λ is the vector of the Lagrangian multipliers [5, Sec. VI.5] and H(θ) := I 6 | − A(θ) assembles the mapping from µ to the scalar ODEs, cf. [6]. Note that, the two-personsystem is overactuated as we have 2n J = 6 input torques but only F = 4.

Motion Planning via Optimization
Mechanical systems like (1) in general have differential index three which requires either index reduction or special algorithms or both for simulation. However, for motion planning via optimization the algebraic constraint is not a special feature, as the (discretized) system dynamics is regarded as a set of equality constraints anyway. In particular (1a) can be transformed to statespace formẋ = f (x, τ , λ) with dimension n s = 2n L = 12 which allows e. g. Euler-discretization c 1,k := ∆t·f (x k , τ k , λ k )− This construction has a simple structure, yet for a reasonable number of discretization steps the resulting nonlinear program becomes quite big. E. g. for N = 100 we have N con = N · (n s + 2) = 1400 and dim(Z) = (N + 1) · n s + N · (2n L + 2) = 2012 for the number of constraints and variables, respectively. By usage of standard PC hardware (3.2 GHz per core, 16 GB RAM) and the software packages CasADi with underlying IPOPT this results in calculation times of 10 2 s up to 5 · 10 3 s, depending on initial guess, initial state and entries in R, Q. While this might be acceptable for a few runs, it is doubtlessly restraining for parameter studies.

Simplification: Quasi-Stationary Trajectory Planning
From an intuitive mechanical perspective on equation (1a), the hypothesis arises that during a carefully assisted stand-up motion spanning several seconds the main load, which must be compensated by the input torques τ , comes from the gravity terms in K(θ) rather than from M(θ)θ + C(θ,θ). This assumption allows to subdivide the one big coupled problem into N smaller problems which are only coupled in forward time and thus can be solved iteratively. In particular, we consider J k (Z k ,P k ) = µ T k Rµ k +σ k → min subject to constraints (c 1,k ,c 2,k ,c 3,k ) withZ k ,c 1,k andc 2,k having meanings analogous to Sec. 2. By P k we denote auxiliary parameters regarded in the additional set of constraintsc 3,k (Z k , P k ) which consist of equalities and inequalities. The most important of these parameters is y E,k determining the desired y-coordinate of the connection point E 1 (θ) = E 2 (θ) via one scalar equation-entry inc 3,k . Because the desired motion is standing-up, this parameter can be increased monotonically in each iteration step k e. g. following a sigmoid function. Further entries of P k includeZ k−1 andZ k−2 which allow a) to constrain the (pseudo-) velocity and acceleration via inequality conditions inc 3,k and b) to also penalize these terms in the objective function via σ k .
The result of this iteration over k is a sequence of "optimal equilibria", i. e. a path, given by (θ k , µ k ). Because the system under consideration features full actuation, any path can be realized by a dynamical trajectory. Hence, with suitable interpolation it is possible to construct a mapping t → θ(k(t)) which, together with its derivatives, can be used to evaluate the left hand side of (1a). The resulting underdetermined system of linear equations L(θ,θ,θ) = H(θ)µ can be used as constraints for µ T Rµ → min to obtain the true applicable inputs for each desired time instant.

Results and Discussion
The approach proposed in Sec. 3 yields N nonlinear programs, each with dimZ = 14 and dim(c 1,k ,c 2,k ,c 3,k ) = 22. With this approach the cumulated computation time for all N = 100 problems is less than 1 s. This speedup of ≈ 10 3 is a beneficial consequence of the fact that in nonlinear optimization computation time and resource demand depend disproportionately on the problem size. Fig. 1 shows that the motivating assumption for Sec. 3 is "moderately violated", especially during the initial phase. Nevertheless the proposed method yields valid trajectories which a) follow a path of optimal equilibria and b) in every time instant distribute the load optimally between the entries of µ according to | · | R . The result in Fig. 1 uses R = diag(1, 1, 1, 30, 30, 30, 0.1, 0.1) to reduce the load for the care receiver (orange lines). For system parameters, implementation details and animated visualization we refer to [7].