Nonlinear Modeling and Control of a Unicycle

Transcription

1 Dynamics and Control, 9, (1999) c 1999 Kluwer Academic Publishers, Boston. Manufactured in The Netherlands. Nonlinear Modeling and Control of a Unicycle YOAV NAVEH Faculty of Mechanical Engineering, Technion Israel Institute of Technology, Haifa 32000, Israel PINHAS Z. BAR-YOSEPH merbygr@emlp.technion.ac.il Faculty of Mechanical Engineering, Technion Israel Institute of Technology, Haifa 32000, Israel YORAM HALEVI merhy01@tx.technion.ac.il Faculty of Mechanical Engineering, Technion Israel Institute of Technology, Haifa 32000, Israel Editor: M. J. Corless Received August 11, 1998; Accepted May 27, 1999 Abstract. A unicycle system is composed of a unicycle and a rider. This system is inherently unstable, but together with a skilled rider can be autonomously controlled and stabilized. A dynamical investigation, a control design and a numerical solution of a nonlinear unicycle autonomous model are presented. The use of a nonlinear model for the control design is shown in this paper to be of great importance. A three-rigid-body physical model was selected for the dynamical study of the system. In a linearized model important physical characteristics of the unicycle system disappear (e.g. interactions between the longitudinal and lateral systems are being neglected), and therefore it is not recommended to be used for the control design. A nonlinear control law, which replaces the rider in stabilizing the model, was derived in the present work, using a nonlinear unicycle model. A simulation study shows good performance of this controller. Time spectral element methods are developed and used for integrating the nonlinear equations of motion. The approach employs the time discontinuous Galerkin method which leads to A-stable high order accurate time integration schemes. Keywords: nonlinear dynamics, unicycle, time finite element 1. Introduction The system, including both a unicycle and a rider, presents an example of an inherently unstable system, which can be self controlled. This characteristic is obviously illustrated by watching an experienced rider. It also enables us to build an autonomous robot, which offers the opportunity for a study in the field of multibody dynamics and control, with a comparison to human ability. The unicycle rider balances himself mainly by applying two torques. The first torque is applied on the wheel by the rider s legs (using the pedals). It produces the longitudinal accelerations, which are required to maintain the unicycle s upright position. Lateral stability is maintained mainly by steering the wheel towards the direction in which the unicycle is falling. This action is achieved by means of a twisting motion of the rider s torso relative to his lower body. Changing the wheel direction causes a centrifugal force, (which depends on the forward speed), that counterbalances and corrects the fall. The rider has to return his upper body to the original position, and does so in a more moderate way which has an opposite, but much lesser effect on the wheel direction.

2 280 NAVEH, BAR-YOSEPH AND HALEVI Figure 1. Unicycle and rider mechanical model. An autonomous unicycle model has already been constructed and studied in previous works. Schoonwinkel [1] constructed a model to represent the unicycle system. His model is comprised of three rigid bodies, a wheel, a frame and a turntable. The frame represents both the unicycle s frame and the rider s lower body and the turntable represents the rider s upper body, as illustrated in Fig. 1. This model is simple enough to be easily constructed, and yet, it maintains the main physical characteristics of the unicycle system. Schoonwinkel s research concentrated mainly on the robot construction and on the evaluation of balance sensors. He achieved a partial stability in the longitudinal direction only, and had to provide his model with extra lateral support. Vos [2,3] used a similar model and focused on the unicycle control problem. The control design was based on the linear model, which is achieved by a linearization of the nonlinear equations of motion. The obtained linear model artificially separates between the longitudinal and lateral dynamics. This enables treating the system as if it consists of two separated uncoupled systems. The first system is the unicycle longitudinal dynamics and the second one is the lateral dynamics. Two, completely separated, controllers were used in order to facilitate the control design procedure. Unfortunately this separation prevents, totally,

3 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 281 the physical interaction (Coriolis and gyroscopic effects) between the two systems, from affecting the resulting controller. Consequently, the obtained controller, though optimal for the nominal system, will have difficulties in handling those interactions, while they occur in a maneuvering of the system. A partial solution of this problem is by considering the strong effect of the wheel speed (which belongs to the longitudinal system) on the lateral dynamics, using a continuous gain scheduling adaptive controller (see Vos [3]). The disadvantage of the linear model proved in Vos s final attempts to stabilize his model, where only some of them were successful. The unsuccessful ones, in which the unicycle fell, were due mainly to the above mentioned interactions that were not taken into account while designing the controller [3]. These interactions appear in the equations of motion as nonlinear expressions, and therefore, were totally disregarded by the linear model. Multibody dynamics analysis usually involves numerical integration of the nonlinear ODE s describing the system, (the system s equations of motion). Integration of the nonlinear ODE s is frequently executed using one of the classical time integration schemes: Runge Kutta, Euler, Newmark, etc. Using the time finite element method [4], for this purpose, offers a new approach for the integration of the governing nonlinear ODE s. The time spectral element method, which is a high order method, developed by Zrahia and Bar-Yoseph [5], has since been successfully used [6 9]. Use of a high order method for the integration of ODE s yields an exponential convergence rate ( p-version where p is the polynomial degree of approximation). This high convergence rate of the time spectral element method enabled the high numerical efficiency and the high order of accuracy in the unicycle problem solution. The objectives of our study are: i) to continue the study of the unicycle control, as an example of an unstable vehicle, by using a new stabilizing nonlinear control law, and ii) to present a unique implementation of the time spectral element method to integrate the nonlinear equations of motion. The present paper focuses on (i) while (ii) is dealt with in Ref. [10]. Emphasis has been put on designing the control law on the basis of the nonlinear model that includes the physical interactions between the longitudinal and lateral systems, as has been concluded to be of high importance. Actually, in the present work there is no separation to longitudinal and lateral systems and a full two input state feedback controller has been designed. Because separation is inevitable while using a linear model, the terms longitudinal and lateral can still be useful in the context of this paper. Due to the physical limitations of a human rider, the system s range of operation is restricted. This restriction limits the variation of the generalized coordinates with a maximal value, and allows us to simplify the long and complicated nonlinear equations of motion. A simplified nonlinear mathematical model is obtained, with a small loss of accuracy. The numerical integration of the governing equations of motion was performed by the adaptive time spectral element method based upon the discontinuous Galerkin approach [10 11]. The outline of this paper is as follows: in Section 2, the model selecting process, deriving the equations of motion and simplifying them, is elaborated. In Section 3, the control problem is presented and the control design, based on the simplified but yet nonlinear model, is detailed. Finally in Section 4 conclusions are drawn.

4 282 NAVEH, BAR-YOSEPH AND HALEVI Figure 2. Wheel-surface friction model. 2. Problem Statement 2.1. Unicycle Model A model, similar to the one used by Schoonwinkel [1], i.e. three rigid bodies with two control torques (Fig. 1), had been selected to represent the unicycle and rider system. The complete nonlinear equations of motion of this model were derived using Kane s formalism [12]. Physical parameters, describing a young rider (which are also suitable to the physical parameters of the mechanical model), were chosen. The equations of motion, after substitution of the physical parameters (for simplicity of presentation), are presented in Appendix B. As can be noticed they are highly nonlinear, contain many terms and are very complex. These equations were simplified, as will be described in the following subsection, to obtain a simpler model that maintains the main physical characteristics of the system. The only friction effects considered are those related to the wheel-surface interaction. In the longitudinal direction the friction is considered by applying a non-slip condition between the wheel and the surface at the contact point. This constraint is non holonomic and implies the non holonomic characteristics of the system. In the wheel yawing direction a combination of Coulomb and viscous friction torques M f = f ψ ψ F ψ sign( ψ) (1) is assumed to act on the wheel by the surface (Fig. 2). Because the lateral control is based on the friction reaction, it is important to develop a realistic friction model. This can be achieved by calculation and adjustment of the two constants f ψ and F ψ in Eq. (1) using

5 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 283 on-line measurements (see Vos [2]). In the present work only a simulation study is invoked and it is assumed that this friction model is accurate Simplification of the Equations of Motion The motivation to simplify the equations stems from their high complexity and is supported by the limited range of operation which is of interest. Out of this range the unicycle cannot be stabilized at all because of the rider s physiology that limits the maximum available control torques. The operation range of forward and sideward tilt angles are determined by the maximal angles which can still be recovered by applying the maximal torques. The wheel torque u w is bounded by u w < 15Nm which limits the forward tilt angle θ to the range θ < 10. The lateral torque applied by the torso u T is bounded by u T < 50Nm and this limits the side tilt angle ϕ, to about ϕ < 9 (where o = 2 rad/sec). Simplification of the equations was performed inside the operation range, i.e. below the maximal values that the tilt angles θ and ϕ can maintain, and was carried out in two stages. In the first stage nonlinear terms, including trigonometric expressions of θ and ϕ were expanded to a power series. An order of magnitude analysis was later performed to distinguish negligible nonlinear terms resulted by this series expansion. The question raised while expanding to power series is, what is the minimal required order of approximation? The answer can be found by examining the equations of motion M(q) q + f (q, q) + B(q)u = 0. (2) The numerical integration scheme (and implicitly the true solution) requires the solution of Eq. (2), to obtain the generalized acceleration vector q. The real objective of the approximation is to keep M 1, rather than M, close to its true value. The measure on M, which best reveals the behavior of M 1 is the condition number. Figure 3 depicts the condition number of M as a function of θ and ϕ as it deviates from the equilibrium state θ = ϕ = 0 assuming, for example, θ = ϕ (i.e. moving along a straight line in the θ ϕ plane). The cases shown in Fig. 3 are: the exact M matrix and the first and second order approximations. The condition number corresponding to the first order approximation of M increases significantly in size inside the operating range with no relation to the accurate one. The large value of the condition number indicates that the solution of Eq. (2), for the generalized accelerations q, cannot be carried out accurately if this first order approximation is to be considered. The minimum approximation order is thus two. Remark. A method frequently used to obtain a simplified form of the equations of motion is by linearizing the generalized velocities, preceding to the remaining calculation of the equations of motion (i.e. linearizing the expressions for the generalized velocities before deriving the accelerations etc.). The obtained model is similar to the above mentioned first order approximation model. The equations that were supposed to describe the unicycle behavior in simulation [1,2], were derived in this approach. As a result, some of the simulations there indicated instability which was a consequence of inadequate modeling and did not represent a real physical phenomenon.

6 284 NAVEH, BAR-YOSEPH AND HALEVI Figure 3. Condition number of mass matrix M (θ, φ) vs. tilt angle while θ = φ. The second stage of simplifying the equations was to eliminate the nonlinear terms with a small contribution to the generalized accelerations. The equations of motion (2) were transformed to the form of q = L(q, q) + N 1 (q, q) + N j (q, q) + N N (q, q) (3) where the generalized acceleration in Eq. (3) is equal to a sum of: linear terms L, and nonlinear terms N j ( j = 1..N). An upper bound to the generalized acceleration values were calculating using typical maximum values of the generalized coordinates and generalized velocities, marked as ( q, q), that were previously obtained in simulation (performed inside the operating range). q = L i (q, q) + N il ( q, q) + + N ij ( q, q) + N in ( q, q) (4) The maximal contribution of each nonlinear term along the simulation was compared with the bound of the appropriate acceleration, and expressions with a relatively small contribution to q were omitted, as shown in Eq. (5). N ij ( q, q) < q i (5) C The linear expression L(q, q), which is dominant for small deviations from equilibrium, was kept in the equation regardless of its magnitude, to maintain the model completeness.

7 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 285 The simplification procedure depends on determining the value of the factor C. A small value removes too many expressions and affects the accuracy of the obtained equations, and a large one does not simplify the equations sufficiently. By trial and error the value C = 30 was found to be a good trade-off between complexity on one hand, and adequate presentation of the original equations on the other. The simplified equations obtained by this procedure are presented in Appendix C (when comparing it to Appendix B one should remember that in Appendix C the equations are already in an explicit form). The simplified equations were examined in simulations in open and closed loop. An open loop simulation comparing the behavior of the nonlinear, simplified nonlinear and linearized models is shown in Fig. 4. The simplified model follows the full nonlinear model closely (Figs. 4a b) in the whole range of interest. This is also demonstrated in Fig. 4c which shows that the simplified model is also consistent from an energy point of view. The numerical simulations were carried out using the time spectral element method. The method and its application for integrating the nonlinear governing equations of motion is described in Appendix D. 3. Controller Design 3.1. Open Loop Dynamics The model has five degrees of freedom, which generally corresponds to ten state variables. However only seven of them are required for a full description of its dynamics. The state vector is comprised of the generalized coordinates and the generalized velocities, x = ( α θ θ φ φ ψ η ) T (6) The control variables of the model are the wheel and the turntable torques. u = (u w u T ) T (7) With these definitions a state space realization of the simplified set of equations, from Appendix C, was carried out in a pseudo linear form: ẋ = A(x)x + B(x)u (8) where A(x) and B(x) are given by: 3.54x x 2 x x 5 x 1 2.7x x 3 x 2 x x x x x 3 x 1 x A(x) = g 1.53x (x 1 + α 0 ) x 3 (x 1 + α 0 ) 2.6F ψ 0.1x 4 1.4x 4 1.7gx α x 2 x x 5 x x 2 x x 3 x 5 x x 0.1x x 4 2.9gx α (x 1 + α 0 ) + 2.6F ψ 0.35x 0.3x 3 x 5 x x 3 x 6 x 2 x 3 5

8 286 NAVEH, BAR-YOSEPH AND HALEVI Figure 4. Simulation of the simplified nonlinear equations in comparison with the full nonlinear model and the linearized model. Open loop with θ 0 = φ 0 = 0.6. (a) forward tilt, (b) lateral tilt, (c) energy conservation. B(x) = x x x x x x3 2 (9)

9 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 287 The state vector is partitioned into longitudinal ( α, θ and θ) and lateral ( φ,φ, ψ and η) parts, and the control variables are partitioned accordingly. The wheel torque is attributed to the longitudinal dynamics and the turntable torque belongs to the lateral dynamics. The realization (9) identifies the longitudinal (the first three rows) and the lateral (the last four rows) systems by the corresponding parts of the state vector. The interaction between the longitudinal and the lateral systems can be observed by the non zero offdiagonal submatrices of A(x) in Eq. (9). A linearization of the model about the equilibrium point, which is an upstanding unicycle in a steady motion, eliminates those interactions by making A block diagonal (all the elements of x become zero except for x 1 = o, which is the nominal wheel speed). The linearization reduces the system to an inverted spherical pendulum with a base that is free to move in the surface plane. We shall refer to that system, from now on, as the nominal linear system or simply the linear system. Because of the non interacting nature of this model we can discuss the longitudinal and the lateral systems separately. Inspecting the properties of each of the linear systems can help in understanding the behavior of the more complicated nonlinear model. The longitudinal system poles are p long = 0, 8.1, 8.1 rad/s which shows a symmetrical pair of the pendulum s stable and unstable poles and an integrator for the wheel s free roll. The longitudinal zeros (the zeros of the transfer function from u w to θ) z long = 4, 3.6 rad/s show the non-minimum phase characteristic of the system. The poles of the lateral system p lateral = 3.3, 3.3, 0, rad/s also show a symmetrical pair of stable and unstable poles with a larger time constant than the longitudinal system, due to the longer related pendulum. The other poles are: an integrator of the free turntable turning motion and a slow pole due to the friction opposing the unicycle s precession motion. The zeros of the lateral system (those of the transfer function from u T to φ) are z lateral = 3.7, 3, 0.06 rad/s (9) which also indicates on a non-minimum phase system Control Design The analysis in the previous subsection indicates that the controller has to stabilize two coupled, unstable, non-minimum phase systems, which is not an easy task. The control design is based on the LQ method which minimizes the cost criterion ( J = x T Qx + u T Ru ) dt (10) 0

10 288 NAVEH, BAR-YOSEPH AND HALEVI where Q 0 and R > 0 are weighting matrices. For a linear system the solution of the optimization problem (minimum of (10) subjected to the constraint ẋ = Ax + Bu), is a state feedback of the form u = Fx (11) The feedback gain matrix F, which contains constant elements, is calculated as follows [13] F = R 1 B T P (12) where P is the solution of the associate algebraic Riccati equation PA + A T P PBR 1 B T P + Q = 0 (13) The structure of the coefficients matrix F depends on the selection of the cost matrices Q and R. It can be shown that by applying the LQ method to the linear system and selecting Q in a similar block diagonal form as A and a diagonal R, will result in a coefficient matrix F having the following form: [ ] f11 f F = 12 f (14) f 24 f 25 f 26 f 27 As can be observed this structure of F maintains the separation between the longitudinal and lateral systems. Actually, in that case the controller is also divided into longitudinal and lateral controllers. In this work we selected the following weighting matrices. Q is diagonal with a value of 10 for θ and φ and unit for all other state variables. R = diag{0.7, 0.08} where these values were chosen to keep the resulting torques below the maximal allowable maxima. The operation of such LQ controller, that was designed for the linear system, is demonstrated by the system response to the initial condition in lateral tilt φ. As shown in Fig. 5 (the linear controller is in dashed lines) the lateral system directly affects the longitudinal system. While initial conditions were applied only in the lateral direction, the longitudinal tilt reacts to the effects caused by the lateral system. The controller action, as shown in Fig. 6, could not anticipate this effect of the lateral system on the longitudinal system, and did not prepare for it (u w (0) = 0). The longitudinal tilt motion is regulated by the longitudinal control system, but there is an obvious delay in its operation. An improved response is obtained if the controller design is based on the nonlinear model, which contains all the interactions. This is explained in the sequel. Considering the interactions between the systems, and still using the linear LQ method, can be done in two ways. Either by choosing a full, i.e. not block diagonal, matrix Q, a procedure that should include a priori knowledge about the importance of the interactions, or by performing a linearization around a point other than the equilibrium point. The second option yields an optimal controller for small perturbations around the linearization point. This controller is not adequate for large deviations from the linearization point and should be continuously adjusted. We consider a control law that is obtained by linearization around various points in the operation range. Each element of the state feedback F of Eq. (11), varies continuously between its values obtained by the LQ method applied in those points.

11 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 289 Figure 5. Response of the linear and the nonlinear controllers, φ 0 = 5.6. (a) wheel speed, (b) yaw rate, (c) forward tilt, (d) lateral tilt. Vos [3] suggested the feedback F = F( α), which depends on the wheel speed only. In this work this approach is extended to obtain F = F( q) as a function of several variables of the state vector. In the most general form one can solve continuously (or practically at each sampling instant) the algebraic Riccati equation P(t )A(x(t )) + A T (x(t ))P(t ) P(t )B(x(t ))R 1 B T (x(t ))P(t ) + Q = 0 (15)

12 290 NAVEH, BAR-YOSEPH AND HALEVI Figure 6. Control action of the linear and nonlinear controllers. φ 0 = 5.6. (a) wheel torque, (b) turntable torque. calculate F(t ) = R 1 B T (x(t ))P(t ) (16) and use the input u(t ) = F(t )x(t ) (17) This method is known as a state dependent Riccati equation (SDRE), [14]. In order to reduce the amount of calculations required in the evaluation of the controller, (for each time step), it was restricted to depend only on the most effective variables. The vector q was selected to be as follows q = [ α, θ, φ] T (18) which means that F depends on θ, φ and α only. Linear controllers were evaluated at the required points, to allow a curve fitting of a second order polynomial in the three dimensional space, for each element of F, as follows f ij ( q) = f ij (0) + D ij q q T E ij q (19)

13 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 291 where D ij R 3 and E ij R 3 3 are approximations of the first and second order derivatives of f ij with respect to q. We define q = [ α, θ, φ] T as maximum deviations in the rate of operation and then, dropping the indices i, j of f for convenience, D ij = f ( f ( α, 0, 0) f ( α, 0, 0) q = q=0 2 α ) f (0, 0, φ) f (0, 0, φ) 2 φ and for k, l = 1, 2, 3 ( f ( q+k ) f ( q k ) 2 f (0) 4 q k 2 (E ij ) kl = ( f ( q+k+l ) f ( q k l ) f ( q k+l ) f ( q +k l ) 4 q k q l where, for example f ( q 3.2 ) = f (0, + θ, φ) f (0, θ,0) f (0, θ, 0) 2 θ ), k = l ), k l, Introducing the dependence of F on q into the state feedback (11) we get the nonlinear control law u = F(x) x (22) with third order terms in x. Performance of the nonlinear controller, (varying F), in comparison to LQ controller designed for the nominal linear system (constant F), are shown in Fig. 6. The nonlinear controller anticipates the interaction (u w (0) 0), and the system s response to initial condition in φ, is improved. The controller stabilized the unicycle also in more complicated maneuvers, in spite of no formal proof for the stability of the closed loop system. The controller design totally excluded the nonlinear effects of the surface friction. Those nonlinear effects are assumed to be canceled by the use of a nonlinear bang-bang control. This nonlinear addition to the controller provides the minimum torque that is required to overcome the static friction. (20) (21) 4. Conclusions In this paper the importance of the physical understanding of the system s behavior to controller design, is demonstrated. Since the linear model of the system hides important physical characteristics, the use of a more complicated (yet much simpler than the full nonlinear) nonlinear model is required. A simple nonlinear control law was derived and was shown by means of simulations to stabilize the system. It is based on LQ designs at various operating points and continuous gain scheduling of the state feedback matrix. The

14 292 NAVEH, BAR-YOSEPH AND HALEVI scheduling is based on second order approximation in several state variables, hence the overall control law is a third order polynomial in the state variables. This controller shows improved results in simulation as demonstrated. The proposed numerical method in the implicit scheme shows an error super convergence rate of 2p + 1 at the top-end nodal points and of p + 2 at inner Gauss Radau points [4,10,11]. This is in comparison to the convergence rate of p + 1 obtained at the internal nodal points. An error estimator was added to the implicit scheme, in order to allow adaptive refinement of the obtained solution [10]. The obtained algorithm, which has a robust stability property, was successfully applied to integrate the equations of motion and is the subject of the companion paper [10]. Appendix A: Nomenclature: A(x) B(x) C f ψ F F ψ M N p q M f u w u T u T x - State dependent pseudo linear system matrix - State dependent pseudo linear input matrix - Magnitude factor for the nonlinear equations - Viscous friction parameter - State feedback matrix - Static friction parameter - Mass matrix - Base function vector - Order of approximation - Generalized coordinates vector - Yaw friction torque - Wheel torque - Wheel torque - Turntable torque - State vector (comprised of q and q) Greek symbols α - Wheel rotation angle θ - Forward frame s tilt angle about the wheel axis τ - Local coordinate of the element ϕ - Sideward tilt angle about an axis, parallel to heading direction o - Nominal wheel speed (required for lateral control) η - Turntable angular position ψ - Side tilt angle

15 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 293 Appendix B: Equations of Motion ψ-direction: {21 cos(ϕ) 2 cos(θ) cos(ϕ) cos(θ)(cos(ϕ) 2 1)} ψ 0.6 cos(ϕ) cos(θ) η +{5.9 cos(ϕ) sin(θ) + 21 cos(ϕ) cos(θ) sin(θ)} ϕ {2sin(ϕ) cos(θ) sin(θ)} α {22 sin(ϕ) + cos(θ) sin(ϕ)} θ {21 sin(ϕ) sin(θ) cos(θ) 5.9 sin(θ) α ψ 5.9 sin(θ) sin(ϕ)} ϕ 2 ϕ-direction: sin(θ) sin(ϕ) θ 2 +{41 cos(ϕ) cos(θ) 2 θ ϕ 0.04 cos(ϕ)} ϕ α +{ 12 sin(θ) cos(ϕ) sin(θ) 41 sin(θ) cos(θ) cos(ϕ) 2 } θ ψ sin(θ) cos(ϕ) θ η {41 cos(θ) 2 sin(ϕ) cos(ϕ) sin(ϕ) cos(ϕ) + 24 cos(θ) sin(ϕ) cos(ϕ)} ϕ ψ sin(ϕ) cos(θ) ϕ η f ψ ψ = 0 (B1) {5.9 sin(θ) cos(ϕ) + 21 sin(θ) cos(θ) cos(ϕ)} ψ 0.6 sin(θ) η {21 cos(θ) cos(θ)} ϕ +{12 cos(θ) cos(ϕ)1.1 cos(ϕ) + 41 cos(θ) 2 cos(ϕ)} θ ψ + 2 cos(ϕ) α ψ + 9.9g sin(ϕ) +{(21 cos(θ) ) sin(ϕ) cos(ϕ) + 12 cos(θ) sin(ϕ) cos(ϕ)} ψ sin(θ) cos(θ) θ ϕ 0.6 cos(θ) θ η + 30g sin(ϕ) cos(θ) 0.6 cos(θ) sin(ϕ) ψ η + 12 sin(θ) θ ϕ cos(θ) cos(ϕ) α ψ = 0 (B2) α-direction: {2 sin(ϕ) cos(θ) sin(ϕ)} ψ + 2 α cos(θ) θ + 12 cos(θ) cos(ϕ) ϕ ψ 12 sin(θ) sin(ϕ) θ ψ 5.9 sin(θ) ψ sin(θ) θ 2 + f ψ sin(ϕ) ψ = u w (B3) θ-direction: {22 sin(ϕ) cos(θ) sin(ϕ)} ψ α + 22 θ +{41 cos(θ) 2 cos(ϕ) cos(ϕ) + 12 cos(θ) cos(ϕ)} ϕ ψ {5.9 sin(θ) cos(ϕ) 2 21 sin(θ) cos(θ) cos(ϕ) sin(θ)} ψ sin(θ) sin(ϕ) ψ α +{21 sin(θ) cos(θ) sin(θ)} ϕ sin(θ) cos(ϕ) ψ η 0.6 cos(θ) ϕ α 30 sin(θ) cos(ϕ)g = u w (B4) η-direction: 0.6 cos(θ) cos(ϕ) ψ 0.6 sin(θ) ϕ 0.6 η 0.6 cos(θ) θ ϕ sin(θ) cos(ϕ) θ ψ cos(θ) sin(ϕ) ϕ ψ = u T (B5)

16 294 NAVEH, BAR-YOSEPH AND HALEVI Appendix C: Simplified Equations of Motion ψ = 0.1 φ α 1.4 φ θ + 1.7θφg φ θ ψ( 0 + α) 2.6 f ψ ψ 0.8θ θ ψ θφ ψ η θ θ η + 2.6u T (C1) φ = 1.53 θ ψ ψ( 0 + α) φ ψ φg + 2.3u T (C2) α = 14.6θ θ 2 20gθ 2.65 φ ψ θφ ψ α θφ ψ φ ( θ 2 )u w 2.6φu T (C3) θ = 6.7gθ 1.77 ψ φ 1.78θφ ψ α θ ψ 2 + ( θ 2 )u w 2.6φu T (C4) η = φ( 0 + α) 2.9θφg 0.65θ ψ( 0 + α) f ψ ψ 0.9θφ ψ θ θ η ( θ 2 )u T (C5) Appendix D: Time Spectral Elements The numerical integration of the closed loop nonlinear ODE s, starting at the initial time t 0 and ending at the final simulation time t f, is performed in a step by step form. The initial conditions required for each step are provided by the solution of the previous step. The use of the simplified equations creates difficulty in finding the appropriate variational principle, for the new problem (if there is a variational principle to that problem at all). Using a weighted residual method on the ODE s at hand, guarantees the outcome of a variational formulation for the problem. The time integration scheme can be constructed through time discontinuous weighted residual methods. These methods, in conjunction with the spectral element approach, lead to A-stable, high order accurate time finite element methods [11]. In the present study, the time discontinuous Galerkin method has been employed as this scheme, as compared to other weighted residual methods, has the highest error convergence rate. The variational formulation obtained by weighting the equations of motion (8) over the n th element domain, in accordance with the time discontinuous Galerkin method, is as follows: t n+1 t n w(ẋ A(x)x B(x)u)dt = w(0)[x(t 0 ) x 0 ] (D1) The continuity of the approximated variables is weakly enforced by a jump operator, which can be observed on the right hand side of Eq. (D1), thus satisfying the initial condition in

17 NONLINEAR MODELING AND CONTROL OF A UNICYCLE 295 an average sense. By considering the control law (22), Eq. (D1) becomes as follows: t n+1 w(ẋ (A(x) B(x)F(x))x)dt = w(0)[x(t 0 ) x 0 ] (D2) t n which is the weak formulation of the closed loop (over the n th element). Formulation (D2) is considered a mixed formulation due to the set of first order equations (8), that have been weighted. A distinct approximation of the generalized coordinates q and velocities q is obtained. Use of the grouping approximation [15] yielded a special structure of the nonlinear coefficient matrix, observed in the obtained nonlinear set of algebraic equations: K M 11 (x) M 12 (x) M 1n (x) x 1 f 1 M 21 (x) K M 22 (x) M 2n (x) x = f 2. (D3). M n1 (x) M n2 (x) K M nn (x) x n f n where: f = ( x 1 (t 0 ) x x 2 (t 0 ) x x n (t 0 ) x 1 n 0 0 ) T The special structure of the coefficient matrix defined by the matrices K and M. K = 1 1 N Ṅ T dτ, M = t NN T dτ that need to be calculated only once (for a constant step size t). The nonlinearity is considered by multiplication of the columns of the constant matrix M, with the values of the components of the nonlinear realization matrix A(x), taken from Eq. (5), evaluated at the nodal points a ij (t k ), as follows: M ij (x) = [ a ij (t 1 )M 1,...,a ij (t k )M k,...,a ij (t np )M np ] As a result of that structure the amount of calculations to obtain the coefficient matrix, is reduced. Another result of that structure is that a diagonal matrix M yields a diagonal M ij (x), which is used for increasing the computation efficiency. Solution of the nonlinear algebraic set of equations (D3) was accomplished by the Newton Raphson method. This implicit solution, for all the time levels within the element, makes the solution robust and unconditionally stable. The numerical method yields accurate results and super convergence is obtained at the Gauss-Radau integration points of the element, as shown in Refs. [10,11]. Acknowledgment Y. N. is grateful for the generous help of the Gutwirth Fellow Fund.

18 296 NAVEH, BAR-YOSEPH AND HALEVI References 1. Schoonwinkel, A., Design and test of a computer stabilized unicycle, Ph.D. Thesis, Stanford University, Vos, D. W., Nonlinear control of an autonomous unicycle robot: practical issues, Ph.D. Thesis, MIT, Department of Aeronautics and Astronautics, Vos, D. W. and Von Flotow, A. H., Dynamic and nonlinear adaptive control of an autonomous unicycle: theory and experiment, in Proc. 29th CDC, IEEE, Honolulu, Hawaii, pp , Aharoni, D. and Bar-Yoseph, P., Mixed finite element formulation in the time domain for solution of dynamic problems, Computational Mechanics, vol. 9, pp , Zrahia, U. and Bar-Yoseph, P., Space time spectral element method for solution of second-order hyperbolic equations, Computer Methods in Applied Mechanics and Engineering, vol. 116, pp , Ben-Tal, A., Bar-Yoseph, P. Z., and Flashner, H., Space-time spectral element method for optimal slewing of a flexible beam, Int. J. Numer. Methods Eng., vol. 39, pp , Ben-Tal, A., Bar-Yoseph, P., and Flashner, H., Optimal maneuver of a flexible arm by space-time finite element method, AIAA, J. Guidance, Control and Dynamics, vol. 18, pp , Bar-Yoseph, P. Z., Fisher, D., and Gottlieb, O., Spectral element method for nonlinear temporal dynamical systems, Computational Mechanics, vol. 18, pp , Bar-Yoseph, P. Z., Fisher, D., and Gottlieb, O., Spectral element method for nonlinear spatio-temporal dynamics of an Euler Bernoulli beam, Computational Mechanics, vol. 19, pp , Naveh, Y., Bar-Yoseph, P. Z., and Halevi, Y., Time spectral element modeling and control of a unicycle, (submitted for publication). 11. Bar-Yoseph, P. Z., Novel spectral and finite element method for unsteady heat transfer problems, chapter in de Vahl Davis, G., and Arinc, F. (eds.), Advances in Computational Heat Transfer, Begell House: New York, Kane, T. R. and Levinston, D. A., Dynamics: Theory and Applications. McGraw-Hill: New York, Kwakernaak, H. and Sivan, R., Linear Optimal Control Systems, Wiley: New York, Friedland, B., Advanced Control Systems Design, Prentice-Hall, Fletcher, C. A. J., Computation Techniques for Fluid Dynamics, Springer-Verlag, 2nd ed., vol. 1, pp , 1991.