- Email: [email protected]
Reduced equations of motion for a wheeled inverted pendulum
8th Vienna International Conference on Mathematical Modelling 8th Conference on Modelling 8th Vienna Vienna18International International Conference on Mathematical Mathematical Modelling February - 20, 2015. Vienna University of Technology, Vienna, 8th Vienna International Conference on Mathematical Modelling February 18 -- 20, 2015. Vienna University of Technology, Available online at Vienna, www.sciencedirect.com February 18 20, 2015. Vienna University of Technology, Vienna, Austria February 18 20, 2015. Vienna University of Technology, Vienna, Austria Austria Austria
ScienceDirect
IFAC-PapersOnLine 48-1 (2015) 328–333
Reduced Reduced Reduced Reduced
equations of motion for equations of motion for equations of motion for equations of motion for inverted pendulum inverted pendulum inverted pendulum inverted pendulum
a a a a
wheeled wheeled wheeled wheeled
∗ ∗∗ ∗∗ Sergio Delgado , Ravi N. Banavar ∗∗ ∗∗ Sergio Delgado ∗∗∗∗ ,,, Sneha Sneha Gajbhiye Gajbhiye ∗∗ Banavar ∗∗ ∗∗ , Ravi N. ∗∗ Sergio Sergio Delgado Delgado , Sneha Sneha Gajbhiye Gajbhiye ∗∗ ,, Ravi Ravi N. N. Banavar Banavar ∗∗ ∗ Technische Universit¨ a tt M¨ u nchen, Boltzmannstr. 15, D-85748, ∗ ∗ Universit¨ a u nchen, Boltzmannstr. 15, D-85748, ∗ Technische Technische Universit¨ a tt M¨ M¨ u nchen, Boltzmannstr. 15, D-85748, ∗ Garching (Tel: +49-89-289 15679; e-mail: [email protected]). Technische Universit¨ a M¨ u nchen, Boltzmannstr. 15, D-85748, Garching (Tel: +49-89-289 15679; e-mail: [email protected]). ∗∗ Garching (Tel: +49-89-289 15679; e-mail: [email protected]). Garching (Tel: +49-89-289 15679; e-mail: [email protected]). Indian Institute of Technology Bombay, India (e-mail: ∗∗ ∗∗ Indian Institute of Technology Bombay, India (e-mail: ∗∗ of ∗∗ Indian Indian Institute Institute of Technology Technology Bombay, Bombay, India India (e-mail: (e-mail: {banavar,sneha}@sc.iitb.ac.in) {banavar,sneha}@sc.iitb.ac.in) {banavar,sneha}@sc.iitb.ac.in) {banavar,sneha}@sc.iitb.ac.in)
Abstract: This paper develops the equations of motion in the reduced space for the wheeled Abstract: This paper develops the equations of motion in the reduced space for the wheeled Abstract: This paper develops the equations of motion in the reduced space for the wheeled inverted pendulum, which is an underactuated mechanical system subject to nonholonomic Abstract: This paper develops the equations of motion in the reduced space for the wheeled inverted pendulum, which is an underactuated mechanical system subject to nonholonomic inverted pendulum, which is an underactuated mechanical system subject to nonholonomic constraints. The equations are derived from the Lagrange-d’Alembert principle using variations inverted pendulum, which is an underactuated mechanical system subject to nonholonomic constraints. The equations are derived from the Lagrange-d’Alembert principle using variations constraints. The from principle using constraints.with The equations equations are are derived derivedequations from the the Lagrange-d’Alembert Lagrange-d’Alembert principlespace, using variations variations consistent consistent with with the the constraints. constraints. The The equations equations are are first first derived derived in in the the shape shape space, space, and and then, then, consistent the constraints. The are first derived in the shape and then, aconsistent coordinate transformation is performed to get the equations of motion in more suitable with the constraints. The equations are first derived in the shape space, and then, a coordinate transformation is performed to get the equations of motion in more suitable a coordinate transformation is performed to get the equations of motion in more suitable coordinates for the purpose of control. a coordinate transformation is performed to get the equations of motion in more suitable coordinates for the purpose of control. coordinates coordinates for for the the purpose purpose of of control. control. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Underactuated mechanical systems, nonholonomic constraints. Keywords: Underactuated mechanical systems, nonholonomic constraints. Keywords: Underactuated Underactuated mechanical mechanical systems, systems, nonholonomic nonholonomic constraints. constraints. Keywords: 1 1. straints . While traditional approaches like the 1 1. INTRODUCTION INTRODUCTION 1 straints traditional approaches like the LagrangeLagrange1. 1 straints While traditional approaches like 1 ... While 1. INTRODUCTION INTRODUCTION straints While traditional approaches like the the LagrangeLagranged’Alembert equations lead to the equations d’Alembert equations equations lead lead to to the the equations equations of of motion motion d’Alembert of motion of nonholonomic mechanical systems (see, e. g., Pathak d’Alembert equations lead to the equations of motion of nonholonomic mechanical systems (see, e. g., Pathak of nonholonomic mechanical systems (see, e. g., Pathak The Wheeled Inverted Pendulum (WIP) and its comet al. [2005]), geometric approaches help to understand of nonholonomic mechanical systems (see, e. g., Pathak The Wheeled Inverted Inverted Pendulum Pendulum (WIP) (WIP) -- and and its comcom- et al. [2005]), geometric approaches help to understand the the The et al. geometric approaches help to the The Wheeled Wheeled Inverted Pendulum (WIP) -interest and its itsincommercial version, the Segway -- has gained the structure and the intrinsic properties of the system. There al. [2005]), [2005]), geometric approaches help to understand understand the mercial version, the Segway has gained interest in the et structure and the intrinsic properties of the system. There mercial version, the Segway has gained interest in the structure and the intrinsic properties of the system. There mercial version, the Segway -maneuverability has gained interest in the structure past several years due to its and simple and the intrinsic properties of the system. There is a lot of work regarding the modeling of nonholonomic past several years due to its maneuverability and simple aa lot of work regarding the modeling of nonholonomic past due and is work regarding the of past several several years yearse.g. due to to its its maneuverability maneuverability and simple simple is construction systems, for example Bloch [2003], Ostrowski [1999], a lot lot of ofsee work regarding the modeling modeling of nonholonomic nonholonomic construction (see (see e.g. Grasser Grasser et et al. al. [2002], [2002], Segway Segway [2015, [2015, is see for example Bloch [2003], Ostrowski [1999], construction (see Grasser et Segway [2015, systems, see for example Bloch [2003], Ostrowski [1999], construction (see e.g. e.g.systems Grasserbased et al. al.on[2002], [2002], Segway [2015, systems, Jan]). Other robotic the WIP are becomBloch et al. [1996], Bloch et al. [2009] and the references systems, see for example Bloch [2003], Ostrowski [1999], Jan]). Other Other robotic robotic systems systems based based on on the the WIP WIP are are becombecom- Bloch et al. [1996], Bloch et al. [2009] and the references Jan]). Bloch et al. [1996], Bloch et al. [2009] and the references Jan]). Other as robotic systems based on the WIP are becoming popular well in the robotic community for human Bloch et al. [1996], Bloch et al. [2009] and the references therein. These geometric tools help understand the meching popular as well in the robotic community for human These geometric tools help understand the meching as well community for human therein. geometric tools help understand the ing popular popular as well in in the the robotic robotic community forworks human assistance or transportation as can can be be seen in in the the of therein. therein.ofThese These geometric tools helpmotion understand the mechmechanism locomotion, i. e., the way is generated by assistance or transportation as seen works of anism of locomotion, i. e., the way motion is generated by assistance or transportation as can be seen in the works of anism of locomotion, i. e., the way motion is generated by assistance or transportation as can be seen in the works of Li et al. [2012], Nasrallah et al. [2007], Baloh and Parent changing the shape of the mechanical system. anism of locomotion, i. e., the way motion is generated by Li et al. al. [2012], Nasrallah Nasrallah et et al. al. [2007], Baloh Baloh and and Parent Parent the shape of the mechanical system. Li changing the shape of the mechanical system. Li et et al.A[2012], [2012], Nasrallah etvertical al. [2007], [2007], Baloh and coaxial Parent changing [2003]. WIP consists of a body with two changing the shape of the mechanical system. [2003]. A WIP consists of a vertical body with two coaxial [2003]. A [2003]. wheels. A WIP WIP consists consists of of a a vertical vertical body body with with two two coaxial coaxial Symmetries driven Symmetries can can be be exploited exploited to to develop develop dynamical dynamical models models driven wheels. wheels. Symmetries can be exploited to develop dynamical models driven in a reduced space. Roughly speaking, the Lagrangian L Symmetries can be exploited to develop dynamical models driven wheels. in aa reduced space. Roughly speaking, the Lagrangian L in reduced space. Roughly speaking, the Lagrangian L The stabilization and tracking control for the WIP is chalexhibits a symmetry if it does not depend on one configuin a reduced space. Roughly speaking, the Lagrangian L The stabilization and tracking control for the WIP is chalexhibits a symmetry if it does not depend on one configuThe stabilization and tracking control for the WIP is chalexhibits a symmetry if it does not depend on one configuThe stabilization andbelongs tracking control forofthe WIP is chal- exhibits lenging: the system to the class underactuated a symmetry if it does not depend on one configuration variable, lets say, q . The variable q is called cyclic. j j lenging: the system belongs to the class of underactuated ration variable, lets say, qqjj .. The variable qqjj is called cyclic. lenging: to ration variable, say, lenging: the the system system belongs belongs to the the class class of of underactuated underactuated mechanical variable, lets lets say, invariant qjj . The The variable variable qjj is is called called cyclic. cyclic. The Lagrangian is thus under transformations in mechanical systems, systems, since since the the control control inputs inputs are are less less than than ration The Lagrangian is thus invariant under transformations in mechanical systems, since the control inputs are less than The Lagrangian is thus invariant under transformations in mechanical systems, since the control inputs are less than the number of configuration variables: There a total of cyclic coordinates. Lie group action and symmetry reducThe Lagrangian is thus invariant under transformations in the number of of configuration configuration variables: variables: There There are are aa total total of of cyclic coordinates. Lie group action and symmetry reducthe cyclic coordinates. Lie group action and symmetry reducthe number number of configuration variables: There are a total of two control variables τ and τ which are the torques ap1 and τ2 which are the torques aption has been successfully applied to model other types cyclic coordinates. Lie group action and symmetry reductwo control variables τ has applied to other types two variables ττ111 and ττ222 six which are aption has been been successfully successfully applied to model model other types two control control variables plied to rotate the wheels, and configuration variables, which are the the torques torques ap- tion tion has been successfully applied to model other types 1 and 2 six of nonholonomic mechanical systems in the differential plied to rotate the wheels, and configuration variables, of nonholonomic mechanical systems in the differential plied to rotate the wheels, and six configuration variables, of nonholonomic nonholonomic mechanical systems the in works the differential differential namely, the xand yposition of the WIP on the horizontal plied to rotate the wheels, and six configuration variables, geometric framework. See for example by Bloch of mechanical systems in the namely, the xand yposition of the WIP on the horizontal geometric framework. See for example the works by Bloch namely, WIP framework. See for the by Bloch namely, the the xx- and and yy- position position of of the theeach WIP on on the the horizontal horizontal geometric plane, et al. [1996], Ostrowski Gajbhiye and Banavar framework. See [1999], for example example the works works by Bloch plane, the the relative relative rotation rotation angle angle of of each each of of the the wheels wheels with with geometric et al. [1996], Ostrowski [1999], Gajbhiye and Banavar plane, the relative rotation angle of of the wheels with et al. [1996], Ostrowski [1999], Gajbhiye and Banavar respect to the body φ and φ , the orientation angle θ, and plane, the relative rotation angle of each of the wheels with 1 and φ2 , the orientation angle θ, and [2012]. As shown, e. g., by Ostrowski [1999], the resulting et al. [1996], Ostrowski [1999], Gajbhiye and Banavar respect to the body φ 1 and φ2 [2012]. As shown, e. by Ostrowski [1999], the respect to body φ orientation angle [2012]. As can shown, e. g., g., by Ostrowskiform [1999], the resulting resulting the tilting angle α. In the system is restricted by respect to the the body φ111 addition, and φ222 ,, the the orientation angle θ, θ, and and [2012]. As shown, e. g., by Ostrowski [1999], the resulting equations be put in aa simplified containing apart the tilting angle α. In addition, the system is restricted by equations can be put in simplified form containing apart the tilting angle α. In addition, the system is restricted by equations can be be put put in aa simplified simplified form containing apart nonholonomic (nonintegrable) constraints and is thus not the tilting angle α. In addition, the system is restricted by from the reduced equations of motion, also the momentum equations can in form containing apart nonholonomic (nonintegrable) constraints and is thus not from the reduced equations of motion, also the momentum nonholonomic (nonintegrable) constraints and is thus not from the reduced equations of motion, also the momentum nonholonomic (nonintegrable) constraints and is thus not smoothly stabilizable at a point as proven by Brockett and reconstruction equation, which describe the dynamics from the reduced equations of motion, also the momentum smoothly stabilizable stabilizable at at a a point point as as proven proven by by Brockett Brockett and reconstruction equation, which describe the dynamics smoothly and reconstruction equation, which describe the dynamics [1983]. These constraints not restrict the state space on smoothly stabilizable at do a point as proven by Brockett of the system along the group directions. That how the and reconstruction equation, which describe theis, dynamics [1983]. These constraints do not restrict the state space on of the system along the group directions. That is, how the [1983]. These constraints do not restrict the state space on of the system along the group directions. That is, how the which the dynamics evolve, but the motion direction at a [1983]. These constraints do not restrict the state space on of the system along the group directions. That is, how the system translates and rotates in space due to the change which the dynamics evolve, but the motion direction at a system translates and rotates in space due to the change which the dynamics evolve, but the motion direction at a system translates and rotates in space due to the change given point: The rolling constraint impedes a sideways mowhich the dynamics evolve, but the motion direction at a in the shape variables. Bloch et al. [2009] further show system translates and rotates in space due to the change given point: The rolling constraint impedes a sideways mothe shape variables. Bloch et al. [2009] further show given point: The rolling constraint impedes a moin the shape Bloch et [2009] further show givenand point: The rolling constraint impedes a sideways sideways mo- in tion, the forward velocity of the WIP and its yaw rate the advantage of using the Hamel equations obtain the in the shape variables. variables. Bloch et al. al. [2009] to further show tion, and the forward velocity of the WIP and its yaw rate the advantage of using the Hamel equations to obtain the tion, and the forward velocity of the WIP and its yaw rate the advantage of using the Hamel equations to obtain the are directly given by the angular velocity of the wheels. tion, and the forward velocity of the WIP and its yaw rate reduced nonholonomic equations of motion: The momenthe advantage of using the Hamel equations to obtain the are directly given by the angular velocity of the wheels. reduced nonholonomic equations of motion: The momenare directly given by the angular velocity of the wheels. reduced nonholonomic equations of motion: The momenWheeled robots have largely been considered as purely are directly given by the angular velocity of the wheels. reduced nonholonomic equations of motion: The momentum equation is in this case given in a body frame which Wheeled robots have largely been considered as purely tum equation equation is is in in this case case given given in in aa body body frame frame which which Wheeled robots have largely been as purely Wheeled robots have largely been considered considered as motion purely tum kinematic systems, due to the simplification in the appears to be more natural in aa spatial the tum equation is in this this casethan given in a bodyframe, framefor which kinematic systems, due simplification in motion appears to be more natural than in spatial frame, for the kinematic systems, analysis. due to to the theThe simplification in the theneeds motion appears to be more natural than in a spatial frame, for the kinematic systems, due to the simplification in the motion and controllability WIP, however, to latter is rarely conserved for systems with nonholonomic appears to be more natural than in a spatial frame, for the and controllability analysis. The WIP, however, needs to latter is rarely conserved for systems with nonholonomic and controllability analysis. The WIP, however, needs to latter is rarely conserved for systems with nonholonomic be stabilized by dynamic effects, such that the complete and controllability analysis. The WIP, however, needs to latter is rarely conserved for systems with nonholonomic constraints. The derivation of the reduced nonholomonic be stabilized by dynamic effects, such that the complete The derivation of reduced nonholomonic be stabilized by dynamic effects, such the complete constraints. The derivation of the the reduced nonholomonic dynamics need be taken into account. mechanical be stabilized byto dynamic effects, such that thatIn the complete constraints. constraints. The derivation of the reduced nonholomonic equations can be done as well using the constrained Ladynamics need to be taken into account. In mechanical equations can be done as well using the constrained Ladynamics need to be taken into account. In mechanical equations can be done as well using the constrained Ladynamics need to be taken into account. In mechanical systems with nonholonomic constraints the configuration grangian and a so-called Ehresmann connection which equations can be done as well using the constrained Lasystems with nonholonomic constraints the grangian and aa so-called Ehresmann connection which systems with nonholonomic constraints the configuration configuration grangian and so-called Ehresmann connection which space Q is a finite dimensional smooth manifold, T Q is the systems with nonholonomic constraints the configuration relates motion along the shape directions with the motion grangian and a so-called Ehresmann connection which space Q is a finite dimensional smooth manifold, T Q is the motion along the shape directions with the motion space Q is dimensional smooth manifold, T is relates tangent -- the velocity phase space -- and aa Q smooth space Q bundle is a a finite finite dimensional smooth manifold, T Q is the the relates relates motion motion along along the the shape shape directions directions with with the the motion motion tangent bundle the velocity phase space and smooth tangent bundle the velocity phase space and a smooth 1 (non-integrable) distribution D ⊂ T Q defines the conThe distribution D defines the admissible velocities tangent bundle the velocity phase space and a smooth 1 (non-integrable) (non-integrable) distribution distribution D D ⊂ ⊂ T TQ Q defines defines the the concon- 111 The The distribution distribution D D defines defines the the admissible admissible velocities velocities (non-integrable) distribution D ⊂ T Q defines the conThe distribution D defines the admissible velocities 2405-8963 © Copyright © 2015, 2015,IFAC IFAC (International Federation of Automatic Control) 328Hosting by Elsevier Ltd. All rights reserved. Copyright 2015, IFAC 328 Copyright © 2015, IFAC 328 Peer review© of International Federation of Automatic Copyright ©under 2015,responsibility IFAC 328Control. 10.1016/j.ifacol.2015.05.011
MATHMOD 2015 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
along the group directions. The approach is based on taking admissible virtual displacements from the Lagrange d’Alembert principle. Admissible means, that the variations satisfy the constraints (given by the connection). This paper follows this modeling tool. Note that we are not imposing the constraints before taking variations, we are taking variations according to the constraints. Several control laws have been applied to the WIP, mostly using linearized models as can be seen in Li et al. [2012]. There is still the need to exploit the nonlinear geometric structure of the WIP to stabilize and control the system using coordinate-free control laws. Nasrallah et al. [2007] develop a model based on the Euler-Rodrigues parameters and analyze the controllability of the WIP moving on an inclined plane. Pathak et al. [2005] develop a model using the Lagrange-d’Alembert equations and check the strong accessibility condition. The aim of this note is to explore the motion of the system in the reduced (shape) space which lead to some net displacement of the mobile robot (motion in the group space) independently from the starting point. Additionally, we present the equations of motion in more suitable coordinates 2 for control or trajectory planning purposes: Since the shape space of the WIP is not fully actuated, the control task becomes difficult in these coordinates. The choice of the model can be done depending on which better suits the task. Notation: Contrary to most of the literature, we use here the matrix/vector representation instead of the index convention. Readers are encouraged to read the referred literature for the wide-used index convention. Further, we use the following simplified notation for the transposed ∂ T Jacobian: ∂xT = ∂x . 2. EQUATIONS OF MOTION IN SHAPE SPACE
Consider the configuration space Q = G × S, where S denotes the shape space and G denotes the group space: Q is a trivial principal bundle with fibers G over a base manifold S. The shape space, as the name suggests, denotes the space of the possible shapes of the system. As stated by Ostrowski [1999], this division is natural in mechanisms that locomote, like mobile robots, where position changes are generated by (mostly cyclic) changes in the shape. See for example the oscillations of a snake-board which create the forward motion, or the rotation of the wheels of a mobile platform resulting in a platforms displacement due to the rolling-without-slipping interaction with the environment. The internal shapes of the WIP are solely defined by the relative angles of the wheels with respect to the body. And since the gravity acts on the WIP depending on the tilting angle (the gravity breaks the symmetry), and it is crucial for the stability of the system, the tilting angle is also considered as a shape variable (more on that later). Note that the net motion resulting from a change in shape is independent from the initial position (we assume, that the WIP is moving on a horizontal plane). Mathematically speaking is this nothing but an invariance (symmetry) of the Lagrangian under a change in position (group) coordinates. We are therefore interested in the reduced equations of motion in shape space variables.
On the configuration space Q = G × S, the Lagrangian is a function L : T G × T S → R and the distribution characterizing the nonholonomic constraints is given by D ⊂ T Q. A curve q(t) on Q is said to satisfy the constraints if q(t) ˙ ∈ Dq , ∀t. This nonholonomic restriction can also be given in local coordinates as g˙ + AT s˙ = 0, (1) where g ∈ G and s ∈ S, and the matrix A describes how g˙ and s˙ are related to each other due to the constraints. Recall that the equations governing the dynamics of the system satisfy the Lagrange-d’Alembert Principle (Fext denote the external forces) T δ L(q, q)dt ˙ + Fext δq dt = 0, (2)
which is equivalent to ∂L d ∂L T − − Fext δq dt = 0. (3) dt ∂ q˙ ∂q Independent from the Lie-group structure, we can intrinsically eliminate the Lagrange-multipliers which arise from the constraint forces and write the reduced equations of motion using the Ehresmann connection (1), which is nothing but a way to split the tangent space into a horizontal (tangent to the shape space) and a vertical (tangent to the group space) part 3 . The curves q(t) solving the equations of motion need to satisfy the constraints. Thus, the variations δq = (δs, δg) are of the form δg + AT δs = 0 (see Bloch et al. [1996]). We assume, that the external forces are input torques τ and only act on the shape T variables, i. e., Fext δq = τ T δs. This assumption is valid, since we will consider group space motion only as a result of a change in the shape variables, and we neglect friction forces. Equation (3) takes the following form d ∂L ∂L T − − τ δs dt dt ∂ s˙ ∂s ∂L d ∂L − AT δs dt = 0. − (4) dt ∂ g˙ ∂g To eliminate the group velocities g, ˙ define the constrained Lagrangian Lc (s, g, s) ˙ = L(s, g, s, ˙ −AT s). ˙ (5) The following relationships hold ∂L ∂L ∂ g˙ ∂L ∂L T ∂Lc = + = − A (6) ∂ s˙ ∂ s˙ ∂ g˙ ∂ s˙ ∂ s˙ ∂ g˙ ∂L ∂L ∂ g˙ ∂L ∂L ∂(AT s) ˙ ∂Lc = + = − (7) ∂s ∂s ∂ g˙ ∂s ∂s ∂ g˙ ∂s ∂L ∂L ∂ g˙ ∂L ∂L ∂(AT s) ˙ ∂Lc = + = − . (8) ∂g ∂g ∂ g˙ ∂g ∂g ∂ g˙ ∂g According to (4), and using the mentioned relationships (6) - (8), the equations of motion in terms of the constrained Lagrangian Lc are given by d T ∂ Lc − ∂sT Lc + A ∂gT Lc = τ − B ∂gT˙ L, (9) dt s˙ where d T d T B ∂gT˙ L = A ∂g˙ L − A ∂ L dt g˙ dt T T + A ∂g (A s) ˙ − ∂sT (AT s) ˙ ∂gT˙ L ⇒ B = A˙ − ∂ T (AT s) ˙ + A ∂ T (AT s). ˙ (10) s
3
2
The same equations of motion can be found in Pathak et al. [2005]
329
329
g
The reader is referred to the references for detailed information regarding Ehresmann connections
MATHMOD 2015 330 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
The equations of motion can be further simplified if there additionally exists a symmetry with respect to the group variables, meaning that the constrained Lagrangian is independent from g and thus ∂gT Lc = 0. In that case the equations of motion are d T ∂ Lc − ∂sT Lc = τ − B ∂gT˙ L. (11) dt s˙ Note that these are not the Euler-Lagrange equations for the constrained Lagrangian Lc (s, s) ˙ - that would have been the case for holonomic (integrable) constraints. The term B ∂gT˙ L on the right hand side of (11) would be missing if we had imposed the constraints before taking the variations as shown in Bloch [2003]. This ”forcing” term is written in terms of the curvature of the Ehresmann connection (1) as stated in Bloch et al. [1996], and can be interpreted as additional gyroscopic forces. 3. THE WHEELED INVERTED PENDULUM Figure 1 shows a simple scheme of the WIP. It is basically a body of mass mB (center of mass at a distance b from the wheels rotation axes) mounted on two wheels of radius r. The distance between the wheels is 2d and their mass is denoted by mW . The wheels are directly attached to the body and can rotate independently. Since they are actuated by motors sitting on the body itself, a tilting motion will automatically rotate the wheels by the tilting angle if the wheels are blocked. The body needs to be stabilized in the upper position through a back and forth motion of the system similar to the inverted pendulum on a cart. In this section we introduce the configuration variables and move on to defining the velocities, the Lagrangian and the constraints. α
α
Sz r, mW , JW
Ix
τ2 , φ2
τ1 , φ1 By
Iy
Sx
2d
θ
θ
α
φ1/2
z-axis
y-axis
y-axis
Note that φ1 and φ2 are the relative angles of the left and right wheel with respect to the body. This definition seems natural: Since the wheels sit on the body, we can measure the relative angle of rotation with respect to the body. The absolute wheel rotation angles are given by ϕ1 = α + φ1 and ϕ2 = α + φ2 . Now, let rS be the origin of the Bcoordinate frame 4 (position of the shaft) given as rS = x I eˆ1 + y I eˆ2 + r I eˆ3 , (15) where r is the wheel radius. A point X on the pendulum (body/bar) will hence be given in inertial coordinates as
Bz Sy
The configuration space Q = G × S of the system is thus (R2 × S1 ) × (S1 × S1 × S1 ). Let X be a point on the pendulum, then the relationship between the coordinates of X in different frames is given by I X = RIS (θ)RSB (α) B X; S X = RSB (α) B X, where RIS (θ) is the orientation of the shaft with respect to the inertial frame, and RSB (α) is the orientation of the pendulum with respect to the shaft given by cos θ − sin θ 0 (12) RIS (θ) = Rθ = sin θ cos θ 0 , 0 0 1 cos α 0 sin α 0 1 0 RSB (α) = Rα = . (13) − sin α 0 cos α Analogously, let X be a point on the left wheel - wheel 1 (the right wheel case works the same way). The orientation of the wheel with respect to the pendulum is given by cos φ1 0 sin φ1 0 1 0 , (14) RW1 (φ1 ) = Rφ1 = − sin φ1 0 cos φ1 such that the coordinate representation of X in the inertial and the shaft frame reads I X = Rθ Rα Rφ1 1 X , S X = Rα Rφ1 1 X. Vectors represented in one of the systems (sys) can be transformed into a different coordinate system by the following rule: I-sys −−−− → S-sys −−−−→ B-sys −−−−→ W1/2 -sys.
mB , JB , b Iz
(2) Heading angle around the I z-axis (θ ∈ S1 ) (3) Tilting angle around the S y-axis (α ∈ S1 ) (4) Relative rotation angle of each of the wheels with respect to the body around the Wj y-axis, which coincides with the B y-axis (φ1 ∈ S1 and φ2 ∈ S1 )
Bx
Fig. 1. The Wheeled Inverted Pendulum We will mainly make use of three different coordinate systems: The inertial I-System, the shaft S-System, which has been rotated around the I z-axis by the yaw angle θ and will be used only for visualizing purposes, and the body fixed B-System, which is attached to the pendulum’s body. For completeness, we also introduce the Wj - frames (for j = 1, 2), which are fixed to the wheels. The following notation has been adopted. I (∗) - inertial frame, S (∗) shaft frame, B (∗) - body (pendulum) frame, Wj (∗) - wheel j frame. The triples (i eˆ1 , i eˆ2 , i eˆ3 ) for i = {I, S, B, Wj } denote the unit vectors of the respective coordinate system. The set of generalized coordinates describing the WIP consists of
(x)b = Rθ Rα X + rS . (16) For X being a point on the left (right) wheel expressed in body fixed coordinates, its inertial position is given as (x)w1 = Rθ Rα (Rφ1 X + d B eˆ2 ) + rS , (17) w2 (18) (x) = Rθ Rα (Rφ2 X − d B eˆ2 ) + rS .
(1) Coordinates of the origin of the body-fixed coordinate system in the horizontal plane (x, y ∈ R2 )
4 Note that the origin of both, the S- and the B-coordinate frame coincide
330
3.1 Velocities Differentiating (15) we get the translational velocity of the origin of the B-frame ˙ S = x˙ I eˆ1 + y˙ I eˆ2 . (19) Ir
MATHMOD 2015 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
Now, we differentiate (16), (17), and (18) to calculate the inertial velocity of a point on the pendulum, and on the wheels, which is given as (x) ˙ b = (R˙ θ Rα + Rθ R˙ α ) X + r˙ S (20) (x) ˙ w1 = (R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) w2
(x) ˙
+ Rθ Rα R˙ φ1 X + r˙ S , = (R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) + Rθ Rα R˙ φ2 X + r˙ S .
(21) (22)
T
Since all rotation matrices satisfy R(t) R(t) = I, ∀t, by differentiating with respect to time we get the relation RT R˙ + R˙ T R = 0, meaning that RT R˙ is skew symmetric. ˙ defined as The matrix ω ˆ = RT R, 0 −ω3 ω2 (23) ω = ω3 0 −ω1 −ω2 ω1 0
denotes the relative angular velocity of the body with respect to its body fixed coordinate frame and expressed in the body frame. The body velocities can therefore be given as T ( ωθ )Rα + ω α ) X + r˙ S , (x) ˙ b = Rθ Rα (Rα w1
(x) ˙
(24)
T Rθ Rα (Rα ( ωθ )Rα
= +ω α )(Rφ1 X + d B eˆ2 ) ωφ1 ) X + r˙ S , (25) + Rθ Rα Rφ1 (
T ( ωθ )Rα + ω α )(Rφ2 X − d B eˆ2 ) (x) ˙ w2 = Rθ Rα (Rα ωφ2 ) X + r˙ S . (26) + Rθ Rα Rφ2 (
3.2 Lagrangian
0 JWxx 0 0 JWyy 0 JW = . 0 0 JWxx (30) The kinetic energy of the body is given as 1 1 TB = mB �r˙ S +b(R˙ θ Rα +Rθ R˙ α )B eˆ3 �2 + ωbT JB ωb , (31) 2 2 where ωb = B ωb is the absolute rotation of the body in body coordinates T ω b = Rα ( ωθ )Rα + ω α . Let ωwj = ωb + ωφj be the absolute rotation of the wheel j in its wheel fixed frame Wj . From (21) and (22), and with the fact, that RαB eˆ2 = S eˆ2 , and R˙ αB eˆ2 = 0, the kinetic energy of the wheels takes the form 1 T 1 TW1 = mW �r˙ S + dR˙ θ S eˆ2 �2 + ωw (32) JW ωw1 2 2 1 1 1 T JW ωw2 . (33) TW2 = mW �r˙ S − dR˙ θ S eˆ2 �2 + ωw 2 2 2 The potential energy is given by V = mB gb cos α. The Lagrangian is simply L = TB + TW1 + TW2 − V .
3.3 Constraints Since the wheels roll without slipping on the plane, the velocity of the center points of the wheels (rc )1 = rS + dRθ Rα B eˆ2 (34) (35) (rc )2 = rS − dRθ Rα B eˆ2 is solely given by their (absolute) rotation T T (˙rc )1 = rRθ Rα ( ωw1 )Rα Rθ I eˆ3
The kinetic energy T is the sum of the kinetic energy terms of each of the bodies (pendulum and wheels) 1 1 T = �(x) ˙ b �2 ρ(X)dV + �(x) ˙ w1 �2 ρ(X)dV 2 B 2 W1 1 �(x) ˙ w2 �2 ρ(X)dV. (27) + 2 W2
The potential energy due to the gravity is given by
V = mb gb (I eˆ3 T Rθ Rα B eˆ3 ) = mb gb �I eˆ3 , Rθ Rα B eˆ3 �, (28) where b B eˆ3 denotes the vector from the origin of the body frame to the body’s center of mass, expressed in body fixed coordinates, and g the gravity constant. The Lagrangian is defined as L = T − V . Using (20), (21), (22), and (28), L can be written as 1 L= �(R˙ θ Rα + Rθ R˙ α ) X + r˙ S �2 ρ(X)dV 2 B 1 �(R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) + 2 W1 + Rθ Rα R˙ φ1 X + r˙ S �2 ρ(X)dV 1 + �(R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) 2 W2 + Rθ Rα R˙ φ2 X + r˙ S �2 ρ(X)dV − mb gb �I eˆ3 , Rθ Rα B eˆ3 �.
0 JBxx 0 0 JByy 0 , JB = 0 0 JBzz
331
(29)
Assume that the moment of inertia of body and the wheels have the following diagonal form (in body-fixed coordinates) 331
(36)
T T ωw2 )Rα Rθ I eˆ3 . (37) (˙rc )2 = rRθ Rα ( Differentiating (34) and (35), and comparing them to the equations above yields the rolling constraints as a subset D of T Q T T r˙ S + d(R˙ θ Rα + Rθ R˙ α ) B eˆ2 = rRθ Rα ( ωw1 )Rα Rθ I eˆ3 (38) T T ˙ ˙ ωw2 )Rα Rθ I eˆ3 . r˙ S − d(Rθ Rα + Rθ Rα ) B eˆ2 = rRθ Rα ( (39)
4. GROUP ACTION, INVARIANCE OF THE LAGRANGIAN AND DISTRIBUTION, AND EQUATIONS OF MOTION The left (or right) action of a Lie group G on a smooth manifold M is a mapping Φ : G × M → M . Assuming the action of G is free and proper (Φ is simple, and therefore M/G is a smooth manifold and the mapping π : M → M/G is a submersion), the Lagrangian L is said to be invariant under the group action if L remains invariant under the induced action of G on T M . For more details on this topic see, e.,g., Marsden and Ratiu [1994], or Holm et al. [2009]. Consider the configuration space ˜ = (R3 × S1 ) × (S1 × Q as a submanifold of the space Q 1 1 ˜ S × S ) = G × S, where S denotes the shape space and consists of the tilt angle α, and the relative wheel angles φ1 and φ2 . On this extended space, the Lagrangian is a ˜ : TQ ˜ → R and the distribution characterizing function L ˜ ⊂ T Q. ˜ Note that Q ˜ is a trivial the constraints is given by D ˜ principal bundle with fibers G over a base manifold S. If
MATHMOD 2015 332 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
˜ and the Distribution D ˜ are invariant the Lagrangian L ˜ the dynamics can be reduced under the action of G, ˜ G ˜ (the set of orbits), which is to the quotient space Q/ diffeomorphic to ˜ g × S. The left action of the Lie group 5 ¯ e3 = eˆ3 }. ˜ = {(s, R) ¯ ∈ R3 × S1 |Rˆ (40) G ˜ is given by on the manifold Q Φ(R,s) :((rS , Rθ ), (Rα , Rφ1 , Rφ2 )) ¯ ¯ θ ), (Rα , Rφ1 , Rφ2 )). ¯ S + s, RR → ((Rr
(41) The left action on the tangent-lifted coordinates of the ˜ is manifold Q T Φ(R,s) :((˙rS , R˙ θ ), (R˙ α , R˙ φ1 , R˙ φ2 )) ¯ ¯ r˙ S , R ¯ R˙ θ ), (R˙ α , R˙ φ1 , R˙ φ2 )). → ((R (42) ˜ Claim: The Lagrangian L = L|Q and the distribution D are invariant under the action of the group ¯ e3 = eˆ3 }. ˜ = {(s, R) ¯ ∈ R3 × S1 |Rˆ (43) G ˜ the Lagrangian (29) is Proof. Under the left action of G, given by � 1 ¯ R˙ θ Rα + Rθ R˙ α ) X + R ¯ r˙ S �2 ρ(X)dV L= �R( 2 B � 1 ¯ R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) �R( + 2 W1 ¯ θ Rα R˙ φ1 X + R ¯ r˙ S �2 ρ(X)dV + RR � 1 ¯ R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) + �R( 2 W2 ¯ θ Rα R˙ φ2 X + R ¯ r˙ S �2 ρ(X)dV + RR ¯ θ Rα B eˆ3 � − mb gb �I eˆ3 , RR (44) � 1 2 = �(R˙ θ Rα + Rθ R˙ α ) X + r˙ S � ρ(X)dV 2 B � 1 �(R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) + 2 W1 + Rθ Rα R˙ φ1 X + r˙ S �2 ρ(X)dV � 1 + �(R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) 2 W2 + Rθ Rα R˙ φ2 X + r˙ S �2 ρ(X)dV ¯ θ Rα B eˆ3 �, − mb gb �I eˆ3 , RR (45) ¯T R ¯ = I. The where it has been used the fact that R ¯ T I eˆ3 = I eˆ3 . Lagrangian coincides with (29), since R ˜ Q is described by the equations The distribution D = D| (38) and (39). Since Dq ⊂ Tq Q, under left group action of ˜ the distribution D becomes G ¯ rS = ∓ dR( ¯ R˙ θ Rα + Rθ R˙ α ) B eˆ2 R˙ (46) T T T T T T ¯ R ¯ R˙ α + Rφ R˙ φ )Rα Rθ R ¯ I eˆ3 ¯ θ Rα (Rα R + rRR 1/2
⇒ r˙ S = ∓ d(R˙ θ Rα + Rθ R˙ α ) B eˆ2 T ˙ + rRθ Rα (Rα Rα + RφT R˙ φ 1/2
1/2
1/2
T T )Rα Rθ I eˆ3 .
4.1 Constrained equations of motion of the WIP Since a curve q(t) satisfies the constraints if q(t) ˙ ∈ Dq , ∀t, we can express the distribution given by (38) and (39) 5
Translation, and rotation about the z-axis
332
as D = { (s, g, s, ˙ g) ˙ ∈ T Q | g˙ + AT s˙ = 0 }, for the group variables g = (x, y, θ)T , and shape variables s = (α, φ1 , φ2 )T . The constraints are satisfied in that specific set of local coordinates for � � 1 −2 r cos θ −2 r sin θ r0 −r cos θ −r sin θ /d . A= (47) 2 −r cos θ −r sin θ −r/d We can get the constrained Lagrangian Lc by replacing g˙ = −AT s˙ in (29) according to (47). Due to the symmetry of the Lagrangian and the distribution, Lc = Lc (s, s) ˙ is additionally independent from g. The governing equations describing the dynamics of the system are thus given by (11). The input is simply the torque applied to the wheels T
τ = (0 τ1 τ2 ) .
(48) ∂ 2 Lc ∂ s∂ ˙ s˙ ,
The mass matrix for this system is defined as Mc = which explicitly results in � � Mc11 Mc12 Mc13 (49) Mc = Mc12 Mc22 Mc23 , Mc13 Mc23 Mc33 where Mc11 = c3 + r2 c1 + 2rc2 cos α r r2 Mc12 = Mc13 = c2 cos α + c1 2 2 r2 r2 Mc23 = c1 − 2 (c4 sin2 α + c5 ) 4 4d r2 r2 Mc22 = Mc33 = c1 + 2 (c4 sin2 α + c5 ), 4 4d and with 1 c1 = mB + 2mW + 2 2 JWyy r c2 = mB b, c3 = mB b2 + JByy c4 = mB b2 + JBxx − JBzz d2 JW . r2 yy The matrix of the Coriolis and centrifugal forces Cc can be derived from the Christoffel symbols (Bloch [2003]) or by using the following relations: 1 Cc s˙ = M˙ c s˙ − ∂sT (s˙ T Mc s) ˙ (50) 2 T M˙ c = Cc + Cc . (51) It can be explicitly written as −r c2 α˙ sin α −δ φ˙ 1−2 δ φ˙ 1−2 Cc = −r/2 c2 α˙ sin α + δ φ˙ 1−2 δ α˙ −δ α˙ , ˙ −r/2 c2 α δ α˙ ˙ sin α − δ φ1−2 −δ α˙ r2 ˙ ˙ ˙ where δ = 4d2 c4 sin α cos α, φ1−2 = φ1 − φ2 . The forcing term on the right hand side is −B ∂gT˙ L| = Jc s, ˙ g=−A ˙ s˙ � � 0 −β β r3 where Jc = β 0 β , β = 2 c2 (φ˙ 2 − φ˙ 1 ) sin α. 4d −β −β 0 The term corresponding to the gravitational forces is simply the gradient of the potential � � −c2 g sin α 0 . (52) ∇s V = 0 c5 = 2JWxx + JBzz + 2mW d2 + 2
MATHMOD 2015 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
The equations of motion can be now written as the equations of motion in the reduced (shape) space and the reconstruction equation 6 Mc s¨ + Cc s˙ + ∇s V = τ + Jc s˙ (53) ˙ g˙ = −AT s.
(54)
4.2 Change of coordinates Since the shape variables are not fully actuated, separating the equations of motion into dynamics of the shape variables, and reconstruction equations does not simplify the control problem, nor any controllability analysis or trajectory planing. Thus, we want to get a functional relationship between the inputs and the group variables instead. We also need to consider the tilting angle α, for this variable is critical for the stability of the WIP. Let us consider the �relation � � r ˙ r �˙ v= φ1 + φ˙ 2 + 2α˙ , φ2 − φ˙ 1 θ˙ = 2 2d to get the equations of motion in terms of the forward ˙ Let us acceleration v˙ of the WIP and the yawing rotation θ. ˙ T , such that ξ˙ = T −1 s, ˙ introduce the velocities ξ˙ = (v, α, ˙ θ) for the constant matrix � � 0 1 0 1 −d T = /r −1 /r . (55) 1/r −1 d/r In the new coordinates ξ, the equations of motion are M ξ¨ + C ξ˙ + ∇ξ V = τ˜ + J ξ˙ (56) T ˙ (57) g˙ = −A T ξ. The Mass matrix considerably simplifies to c1 c2 cos α 0 , (58) c3 0 M = T T Mc T = c2 cos α 2 0 0 c4 sin α + c5 such that the matrix of the Coriolis and centrifugal forces becomes 0 0 −c2 α˙ sin α −c4 θ˙ sin α cos α , 0 C = 0 (59) 0 c4 θ˙ sin α cos α c4 α˙ sin α cos α the term corresponding to the gravitational forces is � � 0 (60) ∇ξ V = −c2 g sin α , 0 and the forcing term on the right hand side is 0 0 c2 θ˙ sin α ˙ ξ. 0 0 0 J ξ˙ = (61) −c2 θ˙ sin α 0 0 In the new coordinates ξ, the systems input is given by �1 � /r (τ1 + τ2 ) T (62) τ˜ = T τ = −(τ1 + τ2 ) , d/r (τ2 − τ1 ) and the reconstruction equation g˙ = −AT T ξ˙ for the position is simply x˙ = v cos θ y˙ = v sin θ. Note that the new coordinates ξ consist of the length of the path, and the tilting and yaw angles, α and θ, respectively. The equations of motion (56) are the same derived 6
There is no momentum equation for this system.
333
333
by Pathak et al. [2005] using the Lagrange-d’Alembert equations, since the assumptions made for developing the model are identical. 5. CONCLUSION In this paper we have derived the reduced equations of motion for the WIP from the Lagrange-d’Alembert Principle and identifying inherent symmetries in the system. Our immediate goal is to identify the Lie-Poisson structure for this nonholonomic system and employ this feature in arriving at a suitable control law for the objective of stabilization and, later on, trajectory tracking. ACKNOWLEDGEMENTS A part of this work was performed during the first author’s visit to the Systems and Control group, IIT Bombay. The first author thanks the TUM Graduate School for supporting this work. REFERENCES Baloh, M. and Parent, M. (2003). Modeling and model verification of an intelligent self-balancing two-wheeled vehicle for an autonomous urban transportation system. In Proc. Conf. Comp. Intellig., Robot. Autonom. Syst. Bloch, A.M. (2003). Nonholonomic mechanics and control. Interdisciplinary applied mathematics: Systems and control. Springer. Bloch, A.M., Krishnaprasad, P.S., Marsden, J.E., and Murray, R.M. (1996). Nonholonomic mechanical systems with symmetry. Archive for Rational Mechanics and Analysis, 136(1), 21–99. Bloch, A.M., Marsden, J.E., and Zenkov, D.V. (2009). Quasivelocities and symmetries in non-holonomic systems. Dynamical Systems, 24(2), 187–222. Brockett, R.W. (1983). Asymptotic Stability and Feedback Stabilization. Defense Technical Information Center. Gajbhiye, S. and Banavar, R.N. (2012). The EulerPoincar´e equations for a spherical robot actuated by a pendulum. In Proc. 4th IFAC Workshop on Lagrangian and Hamiltonian Methods for Non Linear Control. Grasser, F., D’Arrigo, A., Colombi, S., and Rufer, A.C. (2002). Joe: a mobile, inverted pendulum. IEEE Transactions on Industrial Electronics, 49(1), 107–114. Holm, D.D., Schmah, T., and Stoica, C. (2009). Geometric Mechanics and Symmetry. Oxford University Press Inc., New York. Li, Z., Yang, C., and Fan, L. (2012). Advanced Control of Wheeled Inverted Pendulum Systems. Springer. Marsden, J.E. and Ratiu, T.S. (1994). Introduction to Mechanics and Symmetry. Springer-Verlag, New York. Nasrallah, D., Michalska, H., and Angeles, J. (2007). Controllability and posture control of a wheeled pendulum moving on an inclined plane. Robotics, IEEE Transactions on, 23(3), 564–577. Ostrowski, J.P. (1999). Computing reduced equations for robotic systems with constraints and symmetries. IEEE T. Robotics and Automation, 15(1), 111–123. Pathak, K., Franch, J., and Agrawal, S. (2005). Velocity and position control of a wheeled inverted pendulum by partial feedback linearization. Robotics, IEEE Transactions on, 21(3), 505–513. Segway (2015, Jan). [online]. http://www.segway.com.
ScienceDirect
IFAC-PapersOnLine 48-1 (2015) 328–333
Reduced Reduced Reduced Reduced
equations of motion for equations of motion for equations of motion for equations of motion for inverted pendulum inverted pendulum inverted pendulum inverted pendulum
a a a a
wheeled wheeled wheeled wheeled
∗ ∗∗ ∗∗ Sergio Delgado , Ravi N. Banavar ∗∗ ∗∗ Sergio Delgado ∗∗∗∗ ,,, Sneha Sneha Gajbhiye Gajbhiye ∗∗ Banavar ∗∗ ∗∗ , Ravi N. ∗∗ Sergio Sergio Delgado Delgado , Sneha Sneha Gajbhiye Gajbhiye ∗∗ ,, Ravi Ravi N. N. Banavar Banavar ∗∗ ∗ Technische Universit¨ a tt M¨ u nchen, Boltzmannstr. 15, D-85748, ∗ ∗ Universit¨ a u nchen, Boltzmannstr. 15, D-85748, ∗ Technische Technische Universit¨ a tt M¨ M¨ u nchen, Boltzmannstr. 15, D-85748, ∗ Garching (Tel: +49-89-289 15679; e-mail: [email protected]). Technische Universit¨ a M¨ u nchen, Boltzmannstr. 15, D-85748, Garching (Tel: +49-89-289 15679; e-mail: [email protected]). ∗∗ Garching (Tel: +49-89-289 15679; e-mail: [email protected]). Garching (Tel: +49-89-289 15679; e-mail: [email protected]). Indian Institute of Technology Bombay, India (e-mail: ∗∗ ∗∗ Indian Institute of Technology Bombay, India (e-mail: ∗∗ of ∗∗ Indian Indian Institute Institute of Technology Technology Bombay, Bombay, India India (e-mail: (e-mail: {banavar,sneha}@sc.iitb.ac.in) {banavar,sneha}@sc.iitb.ac.in) {banavar,sneha}@sc.iitb.ac.in) {banavar,sneha}@sc.iitb.ac.in)
Abstract: This paper develops the equations of motion in the reduced space for the wheeled Abstract: This paper develops the equations of motion in the reduced space for the wheeled Abstract: This paper develops the equations of motion in the reduced space for the wheeled inverted pendulum, which is an underactuated mechanical system subject to nonholonomic Abstract: This paper develops the equations of motion in the reduced space for the wheeled inverted pendulum, which is an underactuated mechanical system subject to nonholonomic inverted pendulum, which is an underactuated mechanical system subject to nonholonomic constraints. The equations are derived from the Lagrange-d’Alembert principle using variations inverted pendulum, which is an underactuated mechanical system subject to nonholonomic constraints. The equations are derived from the Lagrange-d’Alembert principle using variations constraints. The from principle using constraints.with The equations equations are are derived derivedequations from the the Lagrange-d’Alembert Lagrange-d’Alembert principlespace, using variations variations consistent consistent with with the the constraints. constraints. The The equations equations are are first first derived derived in in the the shape shape space, space, and and then, then, consistent the constraints. The are first derived in the shape and then, aconsistent coordinate transformation is performed to get the equations of motion in more suitable with the constraints. The equations are first derived in the shape space, and then, a coordinate transformation is performed to get the equations of motion in more suitable a coordinate transformation is performed to get the equations of motion in more suitable coordinates for the purpose of control. a coordinate transformation is performed to get the equations of motion in more suitable coordinates for the purpose of control. coordinates coordinates for for the the purpose purpose of of control. control. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Underactuated mechanical systems, nonholonomic constraints. Keywords: Underactuated mechanical systems, nonholonomic constraints. Keywords: Underactuated Underactuated mechanical mechanical systems, systems, nonholonomic nonholonomic constraints. constraints. Keywords: 1 1. straints . While traditional approaches like the 1 1. INTRODUCTION INTRODUCTION 1 straints traditional approaches like the LagrangeLagrange1. 1 straints While traditional approaches like 1 ... While 1. INTRODUCTION INTRODUCTION straints While traditional approaches like the the LagrangeLagranged’Alembert equations lead to the equations d’Alembert equations equations lead lead to to the the equations equations of of motion motion d’Alembert of motion of nonholonomic mechanical systems (see, e. g., Pathak d’Alembert equations lead to the equations of motion of nonholonomic mechanical systems (see, e. g., Pathak of nonholonomic mechanical systems (see, e. g., Pathak The Wheeled Inverted Pendulum (WIP) and its comet al. [2005]), geometric approaches help to understand of nonholonomic mechanical systems (see, e. g., Pathak The Wheeled Inverted Inverted Pendulum Pendulum (WIP) (WIP) -- and and its comcom- et al. [2005]), geometric approaches help to understand the the The et al. geometric approaches help to the The Wheeled Wheeled Inverted Pendulum (WIP) -interest and its itsincommercial version, the Segway -- has gained the structure and the intrinsic properties of the system. There al. [2005]), [2005]), geometric approaches help to understand understand the mercial version, the Segway has gained interest in the et structure and the intrinsic properties of the system. There mercial version, the Segway has gained interest in the structure and the intrinsic properties of the system. There mercial version, the Segway -maneuverability has gained interest in the structure past several years due to its and simple and the intrinsic properties of the system. There is a lot of work regarding the modeling of nonholonomic past several years due to its maneuverability and simple aa lot of work regarding the modeling of nonholonomic past due and is work regarding the of past several several years yearse.g. due to to its its maneuverability maneuverability and simple simple is construction systems, for example Bloch [2003], Ostrowski [1999], a lot lot of ofsee work regarding the modeling modeling of nonholonomic nonholonomic construction (see (see e.g. Grasser Grasser et et al. al. [2002], [2002], Segway Segway [2015, [2015, is see for example Bloch [2003], Ostrowski [1999], construction (see Grasser et Segway [2015, systems, see for example Bloch [2003], Ostrowski [1999], construction (see e.g. e.g.systems Grasserbased et al. al.on[2002], [2002], Segway [2015, systems, Jan]). Other robotic the WIP are becomBloch et al. [1996], Bloch et al. [2009] and the references systems, see for example Bloch [2003], Ostrowski [1999], Jan]). Other Other robotic robotic systems systems based based on on the the WIP WIP are are becombecom- Bloch et al. [1996], Bloch et al. [2009] and the references Jan]). Bloch et al. [1996], Bloch et al. [2009] and the references Jan]). Other as robotic systems based on the WIP are becoming popular well in the robotic community for human Bloch et al. [1996], Bloch et al. [2009] and the references therein. These geometric tools help understand the meching popular as well in the robotic community for human These geometric tools help understand the meching as well community for human therein. geometric tools help understand the ing popular popular as well in in the the robotic robotic community forworks human assistance or transportation as can can be be seen in in the the of therein. therein.ofThese These geometric tools helpmotion understand the mechmechanism locomotion, i. e., the way is generated by assistance or transportation as seen works of anism of locomotion, i. e., the way motion is generated by assistance or transportation as can be seen in the works of anism of locomotion, i. e., the way motion is generated by assistance or transportation as can be seen in the works of Li et al. [2012], Nasrallah et al. [2007], Baloh and Parent changing the shape of the mechanical system. anism of locomotion, i. e., the way motion is generated by Li et al. al. [2012], Nasrallah Nasrallah et et al. al. [2007], Baloh Baloh and and Parent Parent the shape of the mechanical system. Li changing the shape of the mechanical system. Li et et al.A[2012], [2012], Nasrallah etvertical al. [2007], [2007], Baloh and coaxial Parent changing [2003]. WIP consists of a body with two changing the shape of the mechanical system. [2003]. A WIP consists of a vertical body with two coaxial [2003]. A [2003]. wheels. A WIP WIP consists consists of of a a vertical vertical body body with with two two coaxial coaxial Symmetries driven Symmetries can can be be exploited exploited to to develop develop dynamical dynamical models models driven wheels. wheels. Symmetries can be exploited to develop dynamical models driven in a reduced space. Roughly speaking, the Lagrangian L Symmetries can be exploited to develop dynamical models driven wheels. in aa reduced space. Roughly speaking, the Lagrangian L in reduced space. Roughly speaking, the Lagrangian L The stabilization and tracking control for the WIP is chalexhibits a symmetry if it does not depend on one configuin a reduced space. Roughly speaking, the Lagrangian L The stabilization and tracking control for the WIP is chalexhibits a symmetry if it does not depend on one configuThe stabilization and tracking control for the WIP is chalexhibits a symmetry if it does not depend on one configuThe stabilization andbelongs tracking control forofthe WIP is chal- exhibits lenging: the system to the class underactuated a symmetry if it does not depend on one configuration variable, lets say, q . The variable q is called cyclic. j j lenging: the system belongs to the class of underactuated ration variable, lets say, qqjj .. The variable qqjj is called cyclic. lenging: to ration variable, say, lenging: the the system system belongs belongs to the the class class of of underactuated underactuated mechanical variable, lets lets say, invariant qjj . The The variable variable qjj is is called called cyclic. cyclic. The Lagrangian is thus under transformations in mechanical systems, systems, since since the the control control inputs inputs are are less less than than ration The Lagrangian is thus invariant under transformations in mechanical systems, since the control inputs are less than The Lagrangian is thus invariant under transformations in mechanical systems, since the control inputs are less than the number of configuration variables: There a total of cyclic coordinates. Lie group action and symmetry reducThe Lagrangian is thus invariant under transformations in the number of of configuration configuration variables: variables: There There are are aa total total of of cyclic coordinates. Lie group action and symmetry reducthe cyclic coordinates. Lie group action and symmetry reducthe number number of configuration variables: There are a total of two control variables τ and τ which are the torques ap1 and τ2 which are the torques aption has been successfully applied to model other types cyclic coordinates. Lie group action and symmetry reductwo control variables τ has applied to other types two variables ττ111 and ττ222 six which are aption has been been successfully successfully applied to model model other types two control control variables plied to rotate the wheels, and configuration variables, which are the the torques torques ap- tion tion has been successfully applied to model other types 1 and 2 six of nonholonomic mechanical systems in the differential plied to rotate the wheels, and configuration variables, of nonholonomic mechanical systems in the differential plied to rotate the wheels, and six configuration variables, of nonholonomic nonholonomic mechanical systems the in works the differential differential namely, the xand yposition of the WIP on the horizontal plied to rotate the wheels, and six configuration variables, geometric framework. See for example by Bloch of mechanical systems in the namely, the xand yposition of the WIP on the horizontal geometric framework. See for example the works by Bloch namely, WIP framework. See for the by Bloch namely, the the xx- and and yy- position position of of the theeach WIP on on the the horizontal horizontal geometric plane, et al. [1996], Ostrowski Gajbhiye and Banavar framework. See [1999], for example example the works works by Bloch plane, the the relative relative rotation rotation angle angle of of each each of of the the wheels wheels with with geometric et al. [1996], Ostrowski [1999], Gajbhiye and Banavar plane, the relative rotation angle of of the wheels with et al. [1996], Ostrowski [1999], Gajbhiye and Banavar respect to the body φ and φ , the orientation angle θ, and plane, the relative rotation angle of each of the wheels with 1 and φ2 , the orientation angle θ, and [2012]. As shown, e. g., by Ostrowski [1999], the resulting et al. [1996], Ostrowski [1999], Gajbhiye and Banavar respect to the body φ 1 and φ2 [2012]. As shown, e. by Ostrowski [1999], the respect to body φ orientation angle [2012]. As can shown, e. g., g., by Ostrowskiform [1999], the resulting resulting the tilting angle α. In the system is restricted by respect to the the body φ111 addition, and φ222 ,, the the orientation angle θ, θ, and and [2012]. As shown, e. g., by Ostrowski [1999], the resulting equations be put in aa simplified containing apart the tilting angle α. In addition, the system is restricted by equations can be put in simplified form containing apart the tilting angle α. In addition, the system is restricted by equations can be be put put in aa simplified simplified form containing apart nonholonomic (nonintegrable) constraints and is thus not the tilting angle α. In addition, the system is restricted by from the reduced equations of motion, also the momentum equations can in form containing apart nonholonomic (nonintegrable) constraints and is thus not from the reduced equations of motion, also the momentum nonholonomic (nonintegrable) constraints and is thus not from the reduced equations of motion, also the momentum nonholonomic (nonintegrable) constraints and is thus not smoothly stabilizable at a point as proven by Brockett and reconstruction equation, which describe the dynamics from the reduced equations of motion, also the momentum smoothly stabilizable stabilizable at at a a point point as as proven proven by by Brockett Brockett and reconstruction equation, which describe the dynamics smoothly and reconstruction equation, which describe the dynamics [1983]. These constraints not restrict the state space on smoothly stabilizable at do a point as proven by Brockett of the system along the group directions. That how the and reconstruction equation, which describe theis, dynamics [1983]. These constraints do not restrict the state space on of the system along the group directions. That is, how the [1983]. These constraints do not restrict the state space on of the system along the group directions. That is, how the which the dynamics evolve, but the motion direction at a [1983]. These constraints do not restrict the state space on of the system along the group directions. That is, how the system translates and rotates in space due to the change which the dynamics evolve, but the motion direction at a system translates and rotates in space due to the change which the dynamics evolve, but the motion direction at a system translates and rotates in space due to the change given point: The rolling constraint impedes a sideways mowhich the dynamics evolve, but the motion direction at a in the shape variables. Bloch et al. [2009] further show system translates and rotates in space due to the change given point: The rolling constraint impedes a sideways mothe shape variables. Bloch et al. [2009] further show given point: The rolling constraint impedes a moin the shape Bloch et [2009] further show givenand point: The rolling constraint impedes a sideways sideways mo- in tion, the forward velocity of the WIP and its yaw rate the advantage of using the Hamel equations obtain the in the shape variables. variables. Bloch et al. al. [2009] to further show tion, and the forward velocity of the WIP and its yaw rate the advantage of using the Hamel equations to obtain the tion, and the forward velocity of the WIP and its yaw rate the advantage of using the Hamel equations to obtain the are directly given by the angular velocity of the wheels. tion, and the forward velocity of the WIP and its yaw rate reduced nonholonomic equations of motion: The momenthe advantage of using the Hamel equations to obtain the are directly given by the angular velocity of the wheels. reduced nonholonomic equations of motion: The momenare directly given by the angular velocity of the wheels. reduced nonholonomic equations of motion: The momenWheeled robots have largely been considered as purely are directly given by the angular velocity of the wheels. reduced nonholonomic equations of motion: The momentum equation is in this case given in a body frame which Wheeled robots have largely been considered as purely tum equation equation is is in in this case case given given in in aa body body frame frame which which Wheeled robots have largely been as purely Wheeled robots have largely been considered considered as motion purely tum kinematic systems, due to the simplification in the appears to be more natural in aa spatial the tum equation is in this this casethan given in a bodyframe, framefor which kinematic systems, due simplification in motion appears to be more natural than in spatial frame, for the kinematic systems, analysis. due to to the theThe simplification in the theneeds motion appears to be more natural than in a spatial frame, for the kinematic systems, due to the simplification in the motion and controllability WIP, however, to latter is rarely conserved for systems with nonholonomic appears to be more natural than in a spatial frame, for the and controllability analysis. The WIP, however, needs to latter is rarely conserved for systems with nonholonomic and controllability analysis. The WIP, however, needs to latter is rarely conserved for systems with nonholonomic be stabilized by dynamic effects, such that the complete and controllability analysis. The WIP, however, needs to latter is rarely conserved for systems with nonholonomic constraints. The derivation of the reduced nonholomonic be stabilized by dynamic effects, such that the complete The derivation of reduced nonholomonic be stabilized by dynamic effects, such the complete constraints. The derivation of the the reduced nonholomonic dynamics need be taken into account. mechanical be stabilized byto dynamic effects, such that thatIn the complete constraints. constraints. The derivation of the reduced nonholomonic equations can be done as well using the constrained Ladynamics need to be taken into account. In mechanical equations can be done as well using the constrained Ladynamics need to be taken into account. In mechanical equations can be done as well using the constrained Ladynamics need to be taken into account. In mechanical systems with nonholonomic constraints the configuration grangian and a so-called Ehresmann connection which equations can be done as well using the constrained Lasystems with nonholonomic constraints the grangian and aa so-called Ehresmann connection which systems with nonholonomic constraints the configuration configuration grangian and so-called Ehresmann connection which space Q is a finite dimensional smooth manifold, T Q is the systems with nonholonomic constraints the configuration relates motion along the shape directions with the motion grangian and a so-called Ehresmann connection which space Q is a finite dimensional smooth manifold, T Q is the motion along the shape directions with the motion space Q is dimensional smooth manifold, T is relates tangent -- the velocity phase space -- and aa Q smooth space Q bundle is a a finite finite dimensional smooth manifold, T Q is the the relates relates motion motion along along the the shape shape directions directions with with the the motion motion tangent bundle the velocity phase space and smooth tangent bundle the velocity phase space and a smooth 1 (non-integrable) distribution D ⊂ T Q defines the conThe distribution D defines the admissible velocities tangent bundle the velocity phase space and a smooth 1 (non-integrable) (non-integrable) distribution distribution D D ⊂ ⊂ T TQ Q defines defines the the concon- 111 The The distribution distribution D D defines defines the the admissible admissible velocities velocities (non-integrable) distribution D ⊂ T Q defines the conThe distribution D defines the admissible velocities 2405-8963 © Copyright © 2015, 2015,IFAC IFAC (International Federation of Automatic Control) 328Hosting by Elsevier Ltd. All rights reserved. Copyright 2015, IFAC 328 Copyright © 2015, IFAC 328 Peer review© of International Federation of Automatic Copyright ©under 2015,responsibility IFAC 328Control. 10.1016/j.ifacol.2015.05.011
MATHMOD 2015 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
along the group directions. The approach is based on taking admissible virtual displacements from the Lagrange d’Alembert principle. Admissible means, that the variations satisfy the constraints (given by the connection). This paper follows this modeling tool. Note that we are not imposing the constraints before taking variations, we are taking variations according to the constraints. Several control laws have been applied to the WIP, mostly using linearized models as can be seen in Li et al. [2012]. There is still the need to exploit the nonlinear geometric structure of the WIP to stabilize and control the system using coordinate-free control laws. Nasrallah et al. [2007] develop a model based on the Euler-Rodrigues parameters and analyze the controllability of the WIP moving on an inclined plane. Pathak et al. [2005] develop a model using the Lagrange-d’Alembert equations and check the strong accessibility condition. The aim of this note is to explore the motion of the system in the reduced (shape) space which lead to some net displacement of the mobile robot (motion in the group space) independently from the starting point. Additionally, we present the equations of motion in more suitable coordinates 2 for control or trajectory planning purposes: Since the shape space of the WIP is not fully actuated, the control task becomes difficult in these coordinates. The choice of the model can be done depending on which better suits the task. Notation: Contrary to most of the literature, we use here the matrix/vector representation instead of the index convention. Readers are encouraged to read the referred literature for the wide-used index convention. Further, we use the following simplified notation for the transposed ∂ T Jacobian: ∂xT = ∂x . 2. EQUATIONS OF MOTION IN SHAPE SPACE
Consider the configuration space Q = G × S, where S denotes the shape space and G denotes the group space: Q is a trivial principal bundle with fibers G over a base manifold S. The shape space, as the name suggests, denotes the space of the possible shapes of the system. As stated by Ostrowski [1999], this division is natural in mechanisms that locomote, like mobile robots, where position changes are generated by (mostly cyclic) changes in the shape. See for example the oscillations of a snake-board which create the forward motion, or the rotation of the wheels of a mobile platform resulting in a platforms displacement due to the rolling-without-slipping interaction with the environment. The internal shapes of the WIP are solely defined by the relative angles of the wheels with respect to the body. And since the gravity acts on the WIP depending on the tilting angle (the gravity breaks the symmetry), and it is crucial for the stability of the system, the tilting angle is also considered as a shape variable (more on that later). Note that the net motion resulting from a change in shape is independent from the initial position (we assume, that the WIP is moving on a horizontal plane). Mathematically speaking is this nothing but an invariance (symmetry) of the Lagrangian under a change in position (group) coordinates. We are therefore interested in the reduced equations of motion in shape space variables.
On the configuration space Q = G × S, the Lagrangian is a function L : T G × T S → R and the distribution characterizing the nonholonomic constraints is given by D ⊂ T Q. A curve q(t) on Q is said to satisfy the constraints if q(t) ˙ ∈ Dq , ∀t. This nonholonomic restriction can also be given in local coordinates as g˙ + AT s˙ = 0, (1) where g ∈ G and s ∈ S, and the matrix A describes how g˙ and s˙ are related to each other due to the constraints. Recall that the equations governing the dynamics of the system satisfy the Lagrange-d’Alembert Principle (Fext denote the external forces) T δ L(q, q)dt ˙ + Fext δq dt = 0, (2)
which is equivalent to ∂L d ∂L T − − Fext δq dt = 0. (3) dt ∂ q˙ ∂q Independent from the Lie-group structure, we can intrinsically eliminate the Lagrange-multipliers which arise from the constraint forces and write the reduced equations of motion using the Ehresmann connection (1), which is nothing but a way to split the tangent space into a horizontal (tangent to the shape space) and a vertical (tangent to the group space) part 3 . The curves q(t) solving the equations of motion need to satisfy the constraints. Thus, the variations δq = (δs, δg) are of the form δg + AT δs = 0 (see Bloch et al. [1996]). We assume, that the external forces are input torques τ and only act on the shape T variables, i. e., Fext δq = τ T δs. This assumption is valid, since we will consider group space motion only as a result of a change in the shape variables, and we neglect friction forces. Equation (3) takes the following form d ∂L ∂L T − − τ δs dt dt ∂ s˙ ∂s ∂L d ∂L − AT δs dt = 0. − (4) dt ∂ g˙ ∂g To eliminate the group velocities g, ˙ define the constrained Lagrangian Lc (s, g, s) ˙ = L(s, g, s, ˙ −AT s). ˙ (5) The following relationships hold ∂L ∂L ∂ g˙ ∂L ∂L T ∂Lc = + = − A (6) ∂ s˙ ∂ s˙ ∂ g˙ ∂ s˙ ∂ s˙ ∂ g˙ ∂L ∂L ∂ g˙ ∂L ∂L ∂(AT s) ˙ ∂Lc = + = − (7) ∂s ∂s ∂ g˙ ∂s ∂s ∂ g˙ ∂s ∂L ∂L ∂ g˙ ∂L ∂L ∂(AT s) ˙ ∂Lc = + = − . (8) ∂g ∂g ∂ g˙ ∂g ∂g ∂ g˙ ∂g According to (4), and using the mentioned relationships (6) - (8), the equations of motion in terms of the constrained Lagrangian Lc are given by d T ∂ Lc − ∂sT Lc + A ∂gT Lc = τ − B ∂gT˙ L, (9) dt s˙ where d T d T B ∂gT˙ L = A ∂g˙ L − A ∂ L dt g˙ dt T T + A ∂g (A s) ˙ − ∂sT (AT s) ˙ ∂gT˙ L ⇒ B = A˙ − ∂ T (AT s) ˙ + A ∂ T (AT s). ˙ (10) s
3
2
The same equations of motion can be found in Pathak et al. [2005]
329
329
g
The reader is referred to the references for detailed information regarding Ehresmann connections
MATHMOD 2015 330 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
The equations of motion can be further simplified if there additionally exists a symmetry with respect to the group variables, meaning that the constrained Lagrangian is independent from g and thus ∂gT Lc = 0. In that case the equations of motion are d T ∂ Lc − ∂sT Lc = τ − B ∂gT˙ L. (11) dt s˙ Note that these are not the Euler-Lagrange equations for the constrained Lagrangian Lc (s, s) ˙ - that would have been the case for holonomic (integrable) constraints. The term B ∂gT˙ L on the right hand side of (11) would be missing if we had imposed the constraints before taking the variations as shown in Bloch [2003]. This ”forcing” term is written in terms of the curvature of the Ehresmann connection (1) as stated in Bloch et al. [1996], and can be interpreted as additional gyroscopic forces. 3. THE WHEELED INVERTED PENDULUM Figure 1 shows a simple scheme of the WIP. It is basically a body of mass mB (center of mass at a distance b from the wheels rotation axes) mounted on two wheels of radius r. The distance between the wheels is 2d and their mass is denoted by mW . The wheels are directly attached to the body and can rotate independently. Since they are actuated by motors sitting on the body itself, a tilting motion will automatically rotate the wheels by the tilting angle if the wheels are blocked. The body needs to be stabilized in the upper position through a back and forth motion of the system similar to the inverted pendulum on a cart. In this section we introduce the configuration variables and move on to defining the velocities, the Lagrangian and the constraints. α
α
Sz r, mW , JW
Ix
τ2 , φ2
τ1 , φ1 By
Iy
Sx
2d
θ
θ
α
φ1/2
z-axis
y-axis
y-axis
Note that φ1 and φ2 are the relative angles of the left and right wheel with respect to the body. This definition seems natural: Since the wheels sit on the body, we can measure the relative angle of rotation with respect to the body. The absolute wheel rotation angles are given by ϕ1 = α + φ1 and ϕ2 = α + φ2 . Now, let rS be the origin of the Bcoordinate frame 4 (position of the shaft) given as rS = x I eˆ1 + y I eˆ2 + r I eˆ3 , (15) where r is the wheel radius. A point X on the pendulum (body/bar) will hence be given in inertial coordinates as
Bz Sy
The configuration space Q = G × S of the system is thus (R2 × S1 ) × (S1 × S1 × S1 ). Let X be a point on the pendulum, then the relationship between the coordinates of X in different frames is given by I X = RIS (θ)RSB (α) B X; S X = RSB (α) B X, where RIS (θ) is the orientation of the shaft with respect to the inertial frame, and RSB (α) is the orientation of the pendulum with respect to the shaft given by cos θ − sin θ 0 (12) RIS (θ) = Rθ = sin θ cos θ 0 , 0 0 1 cos α 0 sin α 0 1 0 RSB (α) = Rα = . (13) − sin α 0 cos α Analogously, let X be a point on the left wheel - wheel 1 (the right wheel case works the same way). The orientation of the wheel with respect to the pendulum is given by cos φ1 0 sin φ1 0 1 0 , (14) RW1 (φ1 ) = Rφ1 = − sin φ1 0 cos φ1 such that the coordinate representation of X in the inertial and the shaft frame reads I X = Rθ Rα Rφ1 1 X , S X = Rα Rφ1 1 X. Vectors represented in one of the systems (sys) can be transformed into a different coordinate system by the following rule: I-sys −−−− → S-sys −−−−→ B-sys −−−−→ W1/2 -sys.
mB , JB , b Iz
(2) Heading angle around the I z-axis (θ ∈ S1 ) (3) Tilting angle around the S y-axis (α ∈ S1 ) (4) Relative rotation angle of each of the wheels with respect to the body around the Wj y-axis, which coincides with the B y-axis (φ1 ∈ S1 and φ2 ∈ S1 )
Bx
Fig. 1. The Wheeled Inverted Pendulum We will mainly make use of three different coordinate systems: The inertial I-System, the shaft S-System, which has been rotated around the I z-axis by the yaw angle θ and will be used only for visualizing purposes, and the body fixed B-System, which is attached to the pendulum’s body. For completeness, we also introduce the Wj - frames (for j = 1, 2), which are fixed to the wheels. The following notation has been adopted. I (∗) - inertial frame, S (∗) shaft frame, B (∗) - body (pendulum) frame, Wj (∗) - wheel j frame. The triples (i eˆ1 , i eˆ2 , i eˆ3 ) for i = {I, S, B, Wj } denote the unit vectors of the respective coordinate system. The set of generalized coordinates describing the WIP consists of
(x)b = Rθ Rα X + rS . (16) For X being a point on the left (right) wheel expressed in body fixed coordinates, its inertial position is given as (x)w1 = Rθ Rα (Rφ1 X + d B eˆ2 ) + rS , (17) w2 (18) (x) = Rθ Rα (Rφ2 X − d B eˆ2 ) + rS .
(1) Coordinates of the origin of the body-fixed coordinate system in the horizontal plane (x, y ∈ R2 )
4 Note that the origin of both, the S- and the B-coordinate frame coincide
330
3.1 Velocities Differentiating (15) we get the translational velocity of the origin of the B-frame ˙ S = x˙ I eˆ1 + y˙ I eˆ2 . (19) Ir
MATHMOD 2015 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
Now, we differentiate (16), (17), and (18) to calculate the inertial velocity of a point on the pendulum, and on the wheels, which is given as (x) ˙ b = (R˙ θ Rα + Rθ R˙ α ) X + r˙ S (20) (x) ˙ w1 = (R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) w2
(x) ˙
+ Rθ Rα R˙ φ1 X + r˙ S , = (R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) + Rθ Rα R˙ φ2 X + r˙ S .
(21) (22)
T
Since all rotation matrices satisfy R(t) R(t) = I, ∀t, by differentiating with respect to time we get the relation RT R˙ + R˙ T R = 0, meaning that RT R˙ is skew symmetric. ˙ defined as The matrix ω ˆ = RT R, 0 −ω3 ω2 (23) ω = ω3 0 −ω1 −ω2 ω1 0
denotes the relative angular velocity of the body with respect to its body fixed coordinate frame and expressed in the body frame. The body velocities can therefore be given as T ( ωθ )Rα + ω α ) X + r˙ S , (x) ˙ b = Rθ Rα (Rα w1
(x) ˙
(24)
T Rθ Rα (Rα ( ωθ )Rα
= +ω α )(Rφ1 X + d B eˆ2 ) ωφ1 ) X + r˙ S , (25) + Rθ Rα Rφ1 (
T ( ωθ )Rα + ω α )(Rφ2 X − d B eˆ2 ) (x) ˙ w2 = Rθ Rα (Rα ωφ2 ) X + r˙ S . (26) + Rθ Rα Rφ2 (
3.2 Lagrangian
0 JWxx 0 0 JWyy 0 JW = . 0 0 JWxx (30) The kinetic energy of the body is given as 1 1 TB = mB �r˙ S +b(R˙ θ Rα +Rθ R˙ α )B eˆ3 �2 + ωbT JB ωb , (31) 2 2 where ωb = B ωb is the absolute rotation of the body in body coordinates T ω b = Rα ( ωθ )Rα + ω α . Let ωwj = ωb + ωφj be the absolute rotation of the wheel j in its wheel fixed frame Wj . From (21) and (22), and with the fact, that RαB eˆ2 = S eˆ2 , and R˙ αB eˆ2 = 0, the kinetic energy of the wheels takes the form 1 T 1 TW1 = mW �r˙ S + dR˙ θ S eˆ2 �2 + ωw (32) JW ωw1 2 2 1 1 1 T JW ωw2 . (33) TW2 = mW �r˙ S − dR˙ θ S eˆ2 �2 + ωw 2 2 2 The potential energy is given by V = mB gb cos α. The Lagrangian is simply L = TB + TW1 + TW2 − V .
3.3 Constraints Since the wheels roll without slipping on the plane, the velocity of the center points of the wheels (rc )1 = rS + dRθ Rα B eˆ2 (34) (35) (rc )2 = rS − dRθ Rα B eˆ2 is solely given by their (absolute) rotation T T (˙rc )1 = rRθ Rα ( ωw1 )Rα Rθ I eˆ3
The kinetic energy T is the sum of the kinetic energy terms of each of the bodies (pendulum and wheels) 1 1 T = �(x) ˙ b �2 ρ(X)dV + �(x) ˙ w1 �2 ρ(X)dV 2 B 2 W1 1 �(x) ˙ w2 �2 ρ(X)dV. (27) + 2 W2
The potential energy due to the gravity is given by
V = mb gb (I eˆ3 T Rθ Rα B eˆ3 ) = mb gb �I eˆ3 , Rθ Rα B eˆ3 �, (28) where b B eˆ3 denotes the vector from the origin of the body frame to the body’s center of mass, expressed in body fixed coordinates, and g the gravity constant. The Lagrangian is defined as L = T − V . Using (20), (21), (22), and (28), L can be written as 1 L= �(R˙ θ Rα + Rθ R˙ α ) X + r˙ S �2 ρ(X)dV 2 B 1 �(R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) + 2 W1 + Rθ Rα R˙ φ1 X + r˙ S �2 ρ(X)dV 1 + �(R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) 2 W2 + Rθ Rα R˙ φ2 X + r˙ S �2 ρ(X)dV − mb gb �I eˆ3 , Rθ Rα B eˆ3 �.
0 JBxx 0 0 JByy 0 , JB = 0 0 JBzz
331
(29)
Assume that the moment of inertia of body and the wheels have the following diagonal form (in body-fixed coordinates) 331
(36)
T T ωw2 )Rα Rθ I eˆ3 . (37) (˙rc )2 = rRθ Rα ( Differentiating (34) and (35), and comparing them to the equations above yields the rolling constraints as a subset D of T Q T T r˙ S + d(R˙ θ Rα + Rθ R˙ α ) B eˆ2 = rRθ Rα ( ωw1 )Rα Rθ I eˆ3 (38) T T ˙ ˙ ωw2 )Rα Rθ I eˆ3 . r˙ S − d(Rθ Rα + Rθ Rα ) B eˆ2 = rRθ Rα ( (39)
4. GROUP ACTION, INVARIANCE OF THE LAGRANGIAN AND DISTRIBUTION, AND EQUATIONS OF MOTION The left (or right) action of a Lie group G on a smooth manifold M is a mapping Φ : G × M → M . Assuming the action of G is free and proper (Φ is simple, and therefore M/G is a smooth manifold and the mapping π : M → M/G is a submersion), the Lagrangian L is said to be invariant under the group action if L remains invariant under the induced action of G on T M . For more details on this topic see, e.,g., Marsden and Ratiu [1994], or Holm et al. [2009]. Consider the configuration space ˜ = (R3 × S1 ) × (S1 × Q as a submanifold of the space Q 1 1 ˜ S × S ) = G × S, where S denotes the shape space and consists of the tilt angle α, and the relative wheel angles φ1 and φ2 . On this extended space, the Lagrangian is a ˜ : TQ ˜ → R and the distribution characterizing function L ˜ ⊂ T Q. ˜ Note that Q ˜ is a trivial the constraints is given by D ˜ principal bundle with fibers G over a base manifold S. If
MATHMOD 2015 332 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
˜ and the Distribution D ˜ are invariant the Lagrangian L ˜ the dynamics can be reduced under the action of G, ˜ G ˜ (the set of orbits), which is to the quotient space Q/ diffeomorphic to ˜ g × S. The left action of the Lie group 5 ¯ e3 = eˆ3 }. ˜ = {(s, R) ¯ ∈ R3 × S1 |Rˆ (40) G ˜ is given by on the manifold Q Φ(R,s) :((rS , Rθ ), (Rα , Rφ1 , Rφ2 )) ¯ ¯ θ ), (Rα , Rφ1 , Rφ2 )). ¯ S + s, RR → ((Rr
(41) The left action on the tangent-lifted coordinates of the ˜ is manifold Q T Φ(R,s) :((˙rS , R˙ θ ), (R˙ α , R˙ φ1 , R˙ φ2 )) ¯ ¯ r˙ S , R ¯ R˙ θ ), (R˙ α , R˙ φ1 , R˙ φ2 )). → ((R (42) ˜ Claim: The Lagrangian L = L|Q and the distribution D are invariant under the action of the group ¯ e3 = eˆ3 }. ˜ = {(s, R) ¯ ∈ R3 × S1 |Rˆ (43) G ˜ the Lagrangian (29) is Proof. Under the left action of G, given by � 1 ¯ R˙ θ Rα + Rθ R˙ α ) X + R ¯ r˙ S �2 ρ(X)dV L= �R( 2 B � 1 ¯ R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) �R( + 2 W1 ¯ θ Rα R˙ φ1 X + R ¯ r˙ S �2 ρ(X)dV + RR � 1 ¯ R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) + �R( 2 W2 ¯ θ Rα R˙ φ2 X + R ¯ r˙ S �2 ρ(X)dV + RR ¯ θ Rα B eˆ3 � − mb gb �I eˆ3 , RR (44) � 1 2 = �(R˙ θ Rα + Rθ R˙ α ) X + r˙ S � ρ(X)dV 2 B � 1 �(R˙ θ Rα + Rθ R˙ α )(Rφ1 X + d B eˆ2 ) + 2 W1 + Rθ Rα R˙ φ1 X + r˙ S �2 ρ(X)dV � 1 + �(R˙ θ Rα + Rθ R˙ α )(Rφ2 X − d B eˆ2 ) 2 W2 + Rθ Rα R˙ φ2 X + r˙ S �2 ρ(X)dV ¯ θ Rα B eˆ3 �, − mb gb �I eˆ3 , RR (45) ¯T R ¯ = I. The where it has been used the fact that R ¯ T I eˆ3 = I eˆ3 . Lagrangian coincides with (29), since R ˜ Q is described by the equations The distribution D = D| (38) and (39). Since Dq ⊂ Tq Q, under left group action of ˜ the distribution D becomes G ¯ rS = ∓ dR( ¯ R˙ θ Rα + Rθ R˙ α ) B eˆ2 R˙ (46) T T T T T T ¯ R ¯ R˙ α + Rφ R˙ φ )Rα Rθ R ¯ I eˆ3 ¯ θ Rα (Rα R + rRR 1/2
⇒ r˙ S = ∓ d(R˙ θ Rα + Rθ R˙ α ) B eˆ2 T ˙ + rRθ Rα (Rα Rα + RφT R˙ φ 1/2
1/2
1/2
T T )Rα Rθ I eˆ3 .
4.1 Constrained equations of motion of the WIP Since a curve q(t) satisfies the constraints if q(t) ˙ ∈ Dq , ∀t, we can express the distribution given by (38) and (39) 5
Translation, and rotation about the z-axis
332
as D = { (s, g, s, ˙ g) ˙ ∈ T Q | g˙ + AT s˙ = 0 }, for the group variables g = (x, y, θ)T , and shape variables s = (α, φ1 , φ2 )T . The constraints are satisfied in that specific set of local coordinates for � � 1 −2 r cos θ −2 r sin θ r0 −r cos θ −r sin θ /d . A= (47) 2 −r cos θ −r sin θ −r/d We can get the constrained Lagrangian Lc by replacing g˙ = −AT s˙ in (29) according to (47). Due to the symmetry of the Lagrangian and the distribution, Lc = Lc (s, s) ˙ is additionally independent from g. The governing equations describing the dynamics of the system are thus given by (11). The input is simply the torque applied to the wheels T
τ = (0 τ1 τ2 ) .
(48) ∂ 2 Lc ∂ s∂ ˙ s˙ ,
The mass matrix for this system is defined as Mc = which explicitly results in � � Mc11 Mc12 Mc13 (49) Mc = Mc12 Mc22 Mc23 , Mc13 Mc23 Mc33 where Mc11 = c3 + r2 c1 + 2rc2 cos α r r2 Mc12 = Mc13 = c2 cos α + c1 2 2 r2 r2 Mc23 = c1 − 2 (c4 sin2 α + c5 ) 4 4d r2 r2 Mc22 = Mc33 = c1 + 2 (c4 sin2 α + c5 ), 4 4d and with 1 c1 = mB + 2mW + 2 2 JWyy r c2 = mB b, c3 = mB b2 + JByy c4 = mB b2 + JBxx − JBzz d2 JW . r2 yy The matrix of the Coriolis and centrifugal forces Cc can be derived from the Christoffel symbols (Bloch [2003]) or by using the following relations: 1 Cc s˙ = M˙ c s˙ − ∂sT (s˙ T Mc s) ˙ (50) 2 T M˙ c = Cc + Cc . (51) It can be explicitly written as −r c2 α˙ sin α −δ φ˙ 1−2 δ φ˙ 1−2 Cc = −r/2 c2 α˙ sin α + δ φ˙ 1−2 δ α˙ −δ α˙ , ˙ −r/2 c2 α δ α˙ ˙ sin α − δ φ1−2 −δ α˙ r2 ˙ ˙ ˙ where δ = 4d2 c4 sin α cos α, φ1−2 = φ1 − φ2 . The forcing term on the right hand side is −B ∂gT˙ L| = Jc s, ˙ g=−A ˙ s˙ � � 0 −β β r3 where Jc = β 0 β , β = 2 c2 (φ˙ 2 − φ˙ 1 ) sin α. 4d −β −β 0 The term corresponding to the gravitational forces is simply the gradient of the potential � � −c2 g sin α 0 . (52) ∇s V = 0 c5 = 2JWxx + JBzz + 2mW d2 + 2
MATHMOD 2015 February 18 - 20, 2015. Vienna, Austria Sergio Delgado et al. / IFAC-PapersOnLine 48-1 (2015) 328–333
The equations of motion can be now written as the equations of motion in the reduced (shape) space and the reconstruction equation 6 Mc s¨ + Cc s˙ + ∇s V = τ + Jc s˙ (53) ˙ g˙ = −AT s.
(54)
4.2 Change of coordinates Since the shape variables are not fully actuated, separating the equations of motion into dynamics of the shape variables, and reconstruction equations does not simplify the control problem, nor any controllability analysis or trajectory planing. Thus, we want to get a functional relationship between the inputs and the group variables instead. We also need to consider the tilting angle α, for this variable is critical for the stability of the WIP. Let us consider the �relation � � r ˙ r �˙ v= φ1 + φ˙ 2 + 2α˙ , φ2 − φ˙ 1 θ˙ = 2 2d to get the equations of motion in terms of the forward ˙ Let us acceleration v˙ of the WIP and the yawing rotation θ. ˙ T , such that ξ˙ = T −1 s, ˙ introduce the velocities ξ˙ = (v, α, ˙ θ) for the constant matrix � � 0 1 0 1 −d T = /r −1 /r . (55) 1/r −1 d/r In the new coordinates ξ, the equations of motion are M ξ¨ + C ξ˙ + ∇ξ V = τ˜ + J ξ˙ (56) T ˙ (57) g˙ = −A T ξ. The Mass matrix considerably simplifies to c1 c2 cos α 0 , (58) c3 0 M = T T Mc T = c2 cos α 2 0 0 c4 sin α + c5 such that the matrix of the Coriolis and centrifugal forces becomes 0 0 −c2 α˙ sin α −c4 θ˙ sin α cos α , 0 C = 0 (59) 0 c4 θ˙ sin α cos α c4 α˙ sin α cos α the term corresponding to the gravitational forces is � � 0 (60) ∇ξ V = −c2 g sin α , 0 and the forcing term on the right hand side is 0 0 c2 θ˙ sin α ˙ ξ. 0 0 0 J ξ˙ = (61) −c2 θ˙ sin α 0 0 In the new coordinates ξ, the systems input is given by �1 � /r (τ1 + τ2 ) T (62) τ˜ = T τ = −(τ1 + τ2 ) , d/r (τ2 − τ1 ) and the reconstruction equation g˙ = −AT T ξ˙ for the position is simply x˙ = v cos θ y˙ = v sin θ. Note that the new coordinates ξ consist of the length of the path, and the tilting and yaw angles, α and θ, respectively. The equations of motion (56) are the same derived 6
There is no momentum equation for this system.
333
333
by Pathak et al. [2005] using the Lagrange-d’Alembert equations, since the assumptions made for developing the model are identical. 5. CONCLUSION In this paper we have derived the reduced equations of motion for the WIP from the Lagrange-d’Alembert Principle and identifying inherent symmetries in the system. Our immediate goal is to identify the Lie-Poisson structure for this nonholonomic system and employ this feature in arriving at a suitable control law for the objective of stabilization and, later on, trajectory tracking. ACKNOWLEDGEMENTS A part of this work was performed during the first author’s visit to the Systems and Control group, IIT Bombay. The first author thanks the TUM Graduate School for supporting this work. REFERENCES Baloh, M. and Parent, M. (2003). Modeling and model verification of an intelligent self-balancing two-wheeled vehicle for an autonomous urban transportation system. In Proc. Conf. Comp. Intellig., Robot. Autonom. Syst. Bloch, A.M. (2003). Nonholonomic mechanics and control. Interdisciplinary applied mathematics: Systems and control. Springer. Bloch, A.M., Krishnaprasad, P.S., Marsden, J.E., and Murray, R.M. (1996). Nonholonomic mechanical systems with symmetry. Archive for Rational Mechanics and Analysis, 136(1), 21–99. Bloch, A.M., Marsden, J.E., and Zenkov, D.V. (2009). Quasivelocities and symmetries in non-holonomic systems. Dynamical Systems, 24(2), 187–222. Brockett, R.W. (1983). Asymptotic Stability and Feedback Stabilization. Defense Technical Information Center. Gajbhiye, S. and Banavar, R.N. (2012). The EulerPoincar´e equations for a spherical robot actuated by a pendulum. In Proc. 4th IFAC Workshop on Lagrangian and Hamiltonian Methods for Non Linear Control. Grasser, F., D’Arrigo, A., Colombi, S., and Rufer, A.C. (2002). Joe: a mobile, inverted pendulum. IEEE Transactions on Industrial Electronics, 49(1), 107–114. Holm, D.D., Schmah, T., and Stoica, C. (2009). Geometric Mechanics and Symmetry. Oxford University Press Inc., New York. Li, Z., Yang, C., and Fan, L. (2012). Advanced Control of Wheeled Inverted Pendulum Systems. Springer. Marsden, J.E. and Ratiu, T.S. (1994). Introduction to Mechanics and Symmetry. Springer-Verlag, New York. Nasrallah, D., Michalska, H., and Angeles, J. (2007). Controllability and posture control of a wheeled pendulum moving on an inclined plane. Robotics, IEEE Transactions on, 23(3), 564–577. Ostrowski, J.P. (1999). Computing reduced equations for robotic systems with constraints and symmetries. IEEE T. Robotics and Automation, 15(1), 111–123. Pathak, K., Franch, J., and Agrawal, S. (2005). Velocity and position control of a wheeled inverted pendulum by partial feedback linearization. Robotics, IEEE Transactions on, 21(3), 505–513. Segway (2015, Jan). [online]. http://www.segway.com.