Dynamics and Control of Constrained Multibody Systems modeled with Maggi's equation: Application to Differential Mobile Robots Part I

Quasivelocity techniques such as Maggi's and Boltzmann-Hamel's equations eliminate Lagrange multipliers from the beginning as opposed to the Euler-Lagrange method where one has to solve for the n configuration variables and the multipliers as functions of time when there are m nonholonomic constraints. Maggi's equation produces n second-order differential equations of which (n-m) are derived using (n-m) independent quasivelocities and the time derivative of the m kinematic constraints which add the remaining m second order differential equations. This technique is applied to derive the dynamics of a differential mobile robot and a controller which takes into account these dynamics is developed.


Introduction
Differential Wheeled Mobile Robots (DWMR) are perhaps one of the most widely used systems to demonstrate modeling techniques and control of mechanical systems subject to nonholonomic constraints [1] [2]. Often, kinematic constraints cannot be written as time derivatives of some functions of the generalized coordinates. The control of a mechanical system with nonholonomic constraints has been studied extensively and quite often, kinematic control is designed ignoring the dynamics of the system. It has been demonstrated in [3] that a mechanical system with nonholonomic constraints can be controlled regardless of the structure of the nonholonomic constraints. It was demonstrated also that a nonholonomic system cannot be stabilized to a single equilibrium point by a smooth feedback [4]. One approach to obtaining the differential equations of motion of a mechanical system described by n generalized coordinates and m nonholonomic constraints is the use of Euler-Lagrange equation. In order to enforce the m nonholonimic constraints, m Lagrange multipliers λ s are introduced which are functions of time. Overall one has a total of (n+m) equations to solve. Often, however, one is not interested in the Lagrange functions λ s . Maggi's equation eliminates the λ s from the beginning. In this paper, the dynamics of a DWMR will be modeled using Maggi's equation and a controller will be designed using the Dynamic Extension Algorithm to track an arbitrary path. The control law design is shown in the companion paper [6]. 2. Kinematics of a differential mobile robot Consider the diagram of the differential mobile robot depicted in figure 1 there are three bodies involved; the platform of the robot and the wheels. There are four degrees-of-freedom associated with the DWMR. The coordinates (x p , y p ) of the midpoint of the axle, the heading angle φ, the mobile robot's right wheel's rotation through θ r , and the robot's left wheel's rotation through angle θ l . A vector of generalized coordinates for the mobile robot is thus given by γ = [x p , y p , φ, θ r , θ l ] T , where the subscript T stands for transpose. Using Koenig's theorem, the kinetic energy is written as in (1). Here, it is convenient to select the midpoint P of the axle of the mobile robot as the reference point. Figure 1. Diagram of a differential mobile robot where v i is the velocity of the reference point of the i th body, ρ i is the position of the center of mass of the i th body relative to P expressed in a translating frame placed at P , and ω i is the angular velocity of the body measured in the body's frame. The rotation matrix I T B which transforms body coordinates to inertial coordinates is given by The coordinates of the left wheel W L and right wheel W R expressed in the inertial frame are given respectively by The velocity of the left and right wheels are computed respectively and are given by To find the kinematic constraint equations, the velocities in equations (4) and (5)are transformed back to the body coordinate frame by multiplying equations (4) and (5) by the transpose of I T B . They are given by The constraints of rolling without slipping implies that the velocity of a wheel is equal to the angular velocity of the wheel about its axis multiplied by the radius of the wheel and the side velocities of the wheels must equal zero. In other words the velocity of the wheels expressed in the body coordinates frame must be given bẏ The kinetic energy of the mobile robot using eq.(1) is given by where I = I c + 2I m + 2m w b 2 + m c d 2 , and m c , m w are the masses of the platform and the mass of the wheels respectively.

Dynamics of the differential mobile robot
In general, the dynamical equations of motion of a mobile robot with n generalized coordinates γ = [γ 1 . . . γ n ] T subject to m constraints can be written as shown in (10) by applying Euler-Lagrange's equation Maggi's equation is a quasivelocity method. In general, there is no set procedure on selecting quasivelocities. For a mechanical system of n generalized coordinates with m nonholonomic constraints, n quasivelocities are selected such that m of them span the constraint space, and (n-m) are independent quasivelocities. During the selection, one keeps in mind that the matrix Ψ obtained as in (13) is invertible with an inverse Φ.
where u = [u 1 . . . u n ] T , is a quasivelicity vector. Equation (13) can be rewritten as Note that these equations do not explicitly involve time otherwise there must be additional terms.
Using equation (15), virtual displacements are given as The first (n-m) δ j are selected independently. Using Lagrange's principle of virtual displacement we have with T (γ,γ), the unconstrained kinetic energy defined above. After substituting for the virtual displacement as defined in (17), we have Using the fact that the first (n-m) δ j are independent, the coefficients of (19) must equal zero and we have The resulting equation (20) is Maggi's equation. Let us explicitly define quasivelocities as Applying (20) to the unconstrained kinetic energy and using the first and second columns of Φ, we have The time derivative of the constraint equations together with (23) form the second-order differential equations of motion of the differential mobile robot and is given by where and Let us define the state vector q = [x p , y p , φ, θ r , θ l ,ẋ p ,ẏ p ,φ,θ r ,θ l ] T by choosing the state space variables q i , 1 ≤ i ≤ 10 as q i = γ i for 1 ≤ i ≤ 5 q i =γ i−5 for 6 ≤ i ≤ 10 The system of equations can be expressed in the forṁ More explicitlyq q 10 f 6 (q 3 , q 6 , q 7 , q 8 ) f 7 (q 3 , q 6 , q 7 , q 8 ) f 8 (q 3 , q 6 , q 7 , q 8 ) f 9 (q 3 , q 6 , q 7 , q 8 ) f 10 (q 3 , q 6 , q 7 , q 8 ) where the entries of (29) are explicitly given in the companion paper.

Conclusion
Maggi's equation is a quasivelocity method used to derive the equations of motion of dynamical systems subject to linear nonholonomic constraints. It results from the application of the concept of Lagrange's virtual displacements i.e., the Lagrange's principle. Maggi's method eliminates the Lagrange multipliers which are used to enforce the nonholonomic constraints from the start.