Linear-Quadratic Optimal Control in Maximal Coordinates Jan Brudigam¨ 1 and Zachary Manchester2 (a) Acrobot basins of attraction (b) Acrobot Abstract— The linear-quadratic regulator (LQR) is an effi- 3.14 cient control method for linear and linearized systems. Typi- cally, LQR is implemented in minimal coordinates (also called generalized or “joint” coordinates). However, other coordinates θ1 are possible and recent research suggests that there may be 1.57 numerical and control-theoretic advantages when using higher- u dimensional non-minimal state parameterizations for dynamical 0 systems. One such parameterization is maximal coordinates, in θ which each link in a multi-body system is parameterized by 2 its full six degrees of freedom and joints between links are -1.57 modeled with algebraic constraints. Such constraints can also max. represent closed kinematic loops or contact with the environ- (min.) ment. This paper investigates the difference between minimal- both and maximal-coordinate LQR control laws. A case study of -3.14 applying LQR to a simple pendulum and simulations comparing 0 1.57 3.14 4.71 6.28 the basins of attraction and tracking performance of minimal- and maximal-coordinate LQR controllers suggest that maximal- Fig. 1: Basins of attraction for maximal- and minimal-coordinate coordinate LQR achieves greater robustness and improved LQR when stabilizing the upright equilibrium (black cross) of tracking performance compared to minimal-coordinate LQR an acrobot from different initial configurations. The maximal- when applied to nonlinear systems. coordinate basin is shown in blue, the minimal-coordinate basin in red, and the basin for both in green. For this example, the maximal- I. INTRODUCTION coordinate basin includes the entire minimal-coordinate basin and Minimal coordinates (also called generalized or “joint” is significantly larger. coordinates) have historically dominated robotic simulation and control, possibly due to the perception that they lead to greater computational efficiency. However, rigid body constraints always arise when using maximal coordinates, dynamics in maximal coordinates with Lagrange multipliers but they can also arise in minimal-coordinate settings, for can be computed with similar efficiency as unconstrained example, for structures with closed-kinematic loops, during dynamics in minimal coordinates [1], [2], and a substantial contact with the environment, or in nonholonomic settings. body of recent work, for example [3]–[5], has suggested that In general, there are several approaches to control such higher-dimensional linear models may have more descriptive mechanically constrained systems. Manipulator contact has power than minimal models when approximating nonlinear been treated by constraining the end effector to always be systems. in contact with the environment and then decomposing the Maximal-coordinate models of robotic systems are math- system into subsystems relating to motion on and off the ematically expressed as differential-algebraic equations constraint manifold [6], [7]. Contact arising with walking (DAEs) in continuous time or algebraic difference equa- robots has been handled by deploying specialized control tions (ADEs) in discrete time. Unfortunately, the classical algorithms on the constraint manifold [8], [9]. Systems with derivation of the linear-quadratic regulator (LQR) is not more general holonomic and nonholonomic constraints have well suited to such systems: If applied naively without been treated in a similar manner by either splitting the system explicitly accounting for “joint” or “manifold” constraints into subsystems [10] or by analyzing the behavior on the in the dynamics, the maximal-coordinate system will typi- constraint manifold [11]. Generally, the described methods cally be mathematically uncontrollable, even if an equivalent are based on minimal (or reduced) coordinate representations minimal-coordinate realization is controllable. It is important and aim at reducing, splitting up, or eliminating the effect to emphasize that the constraints in maximal-coordinate of constraints. A more mathematical perspective on mechani- systems are not artificial state or input constraints that can be cally constrained systems can be obtained by treating them as controlled by an actuator. Rather, they are hard mechanical differential-algebraic equations (DAEs). General control of constraints that are enforced by the physical structure. Such DAEs can be achieved with a variety of nonlinear feedback laws [12], [13]. Optimal control of nonlinear DAEs has 1Jan Brudigam¨ is with the Department of Electrical and Com- also been proposed [14], [15], and methods for systems puter Engineering, Technical University of Munich, Munich, Germany described by linear DAEs have been developed as well [16], [email protected] 2Zachary Manchester is with The Robotics Institute, Carnegie Mellon [17]. For the purpose of deploying linear control methods, University, Pittsburgh, USA [email protected] mechanically constrained systems and DAEs can also be linearized [18]. One common method for the linearization Link a Link b of mechanical systems described in minimal coordinates is to split the coordinates into independent and dependent xb, q variables [19], [20]. For an overview of more general lin- xa, qa b earization schemes for arbitrary coordinates in continuous time see [21]. Global frame The linear-quadratic regulator is a well-established control Fig. 2: Two links connected by a joint. Adopted from [2]. scheme for (locally) linear systems. However, LQR variants are typically used with minimal-coordinate representations and mechanical constraints are eliminated before deriving a A. Maximal Coordinates controller [22]–[24]. There is also a wide variety of state- A single rigid body in space can be described by a position and input-constrained LQR control laws, for example [25]– 3 3 x 2 R , a velocity v 2 R , an orientation (unit quaternion) [29], but these constraints are “virtual.” As such, they can 4 3 q 2 R , and an angular velocity ! 2 R . We can group be physically violated, and part of the control task is to these quantities into a vector ensure constraint satisfaction by an appropriate choice of T inputs. As stated above, the nature of mechanical constraints z = xT vT qT !T : (1) and maximal-coordinate systems treated in this paper is fundamentally different. One or more bodies can be subject to constraints g which Maximal coordinates represent the six degrees of freedom can, for example, represent joints. One example of such a of each body in an articulated structure and the connections physical constraint is the revolute joint connecting links a between bodies are expressed by explicit joint constraints. and b in Fig.2. Such constraints on two bodies can generally Several methods have been developed to efficiently compute be written as implicit equations dynamics of systems described in maximal coordinates, for g(z ; z ) = 0: (2) example [1], [2]. A related approach for trajectory opti- a b mization can be found in [30]. Control of systems de- For each body, we can generally write the unconstrained scribed in maximal coordinates has been mainly developed dynamics as implicit equations in discrete time: for individual vehicles, such as underwater, land, or aerial vehicles [31]–[33]. In contrast, control of articulated systems d0(zk; zk+1; uk) = 0; (3) parameterized in maximal coordinates has found very little where k is the current time step and u are control inputs attention. Some ideas related to this concept can, however, k to the system, assuming a zero-order hold on the controls. be found in cooperative robotics, where several individual Constraints can be treated by introducing Lagrange mul- vehicles form a virtual articulated structure when performing tipliers (constraint forces) λ into the dynamics equations: a common task [34], [35]. T Currently, there exist some derivations of continuous-time d(zk; zk+1; uk; λk) = d0 − G λk = 0; (4) LQR for DAEs [16], [17], and we will add to these concepts by providing a discrete-time LQR derivation for ADEs in where G is the Jacobian of constraints g with respect to z. Sec. II-C. The resulting control law will subsequently be The dynamics equations d and the constraints g of mul- used in Sec. III for the case study of a simple pendulum tiple bodies form a system of algebraic difference equations to demonstrate that a linear maximal-coordinate control law (ADEs). can be interpreted as a nonlinear control law in minimal B. Classical Riccati Recursion coordinates leading to potentially improved performance. This insight is further supported in Sec.IV by comparing the The linear-quadratic regulator (LQR) can be derived for basins of attraction of time-invariant LQR feedback laws in four different scenarios: finite or infinite horizon, and discrete minimal and maximal coordinates, see Fig.1 for an example, or continuous time. The Riccati recursion is one method to and by a comparison of trajectory tracking performance derive LQR for all four scenarios, and, while not always the with time-varying LQR in minimal and maximal coordinates. most efficient method for each scenario, we will build on it Lastly, Sec.V provides a discussion and Sec.VI summarizes for the remainder of the paper for clarity. our conclusions. For discrete-time systems with a finite horizon, the Ric- cati recursion produces feedback gains at each time step. II. BACKGROUND Continuing the recursion further leads to convergence of the We will briefly depict the treatment of rigid body dynamics feedback gains, which is simply the solution to the infinite- in maximal coordinates (see [1] or [2] for details). Subse- horizon problem. Furthermore, by taking the limit of the quently, we review the derivation of the classical uncon- discrete time step as it goes to zero, we can recover the strained linear-quadratic regulator (LQR) from the dynamic continuous time feedback gains for both finite-horizon and programming principle with Riccati recursion (see [36] for infinite-horizon settings. details), and then state the Riccati recursion for LQR in We will provide the basic derivation of the classical maximal coordinates. recursion here to illustrate the general procedure.