Solving Nonholonomic Systems with the Tau Method

A numerical procedure based on the spectral Tau method to solve nonholonomic systems is provided. Nonholonomic systems are characterized as systems with constraints imposed on the motion. The dynamics is described by a system of differential equations involving control functions and several problems that arise from nonholonomic systems can be formulated as optimal control problems. Applying the Pontryagins maximum principle, the necessary optimality conditions along with the transversality condition, a boundary value problem is obtained. Finally, a numerical approach to tackle the boundary value problem is required. Here we propose the Lanczos spectral Tau method to obtain an approximate solution of these problems exploiting the Tau toolbox software library, which allows for ease of use as well as accurate results.


Introduction
Nonholonomic systems are a class of nonlinear systems that cannot be stabilized by a continuous time-invariant feedback, i.e., at a certain time or state there are constraints imposed on the motion (nonholonomic constraints). These systems are controllable but they cannot move instantaneously in certain directions. They belong to a class of nonlinear differential systems with nonintegrable constraints imposed on the motion [1].
Nonholonomic control systems, which result from formulations of nonholonomic systems that include control inputs, are nonlinear control problems requiring nonlinear treatment. There is ample literature on the formulation of the equations of motion and on the dynamics of nonholonomic systems, being [2] an excellent survey for examples. Nonholonomic control systems have been studied in the context of robot manipulation, mobile robots, wheeled vehicles, and space robotics, just to mention a few. In the case of wheeled vehicles, the kinematics and dynamics can be modeled based on the assumption that the wheels are ideally rolling. Typical constraints of wheeled vehicles are rolling contact, like rolling between the wheels and the ground without slipping, or sliding contact such the sliding of skates.
The solution of nonholonomic optimal control problems can be obtained following a standard procedure, which consists of applying Pontryagin's maximum/minimum principle to obtain set of equations along with initial and terminal conditions resulting into a two-point boundary value problem (BVP) [3].
In this work we propose the use of the spectral Tau method to obtain approximate solutions of nonholonomic optimal control problems through the associated BVP.
The spectral Tau method produces a polynomial approximation of the solution of the differential problem. It is based on solving a system of linear algebraic equations, obtained by imposing that all conditions are verified exactly and the residual is orthogonal to the first elements of an orthogonal polynomial basis [4].
The paper is organized as follows. Section 2 describes the system model of the nonholonomic wheeled vehicle and Section 3 explains the optimal control formulation. A brief description of the Tau method is presented in Section 4. An illustrative example with numerical results is provided in Section 5 and some conclusions are drawn in Section 6.

Nonholonomic Wheeled Vehicle Model
Vehicle models are usually described by a set of ordinary differential equations that define the dynamics of the vehicle and the relationship between the state variables and control input. The kinematic model of a wheeled vehicle can be defined by the following differential equationṡ where (x, y) ∈ R 2 is the robot's position in space, θ is the angle with respect to the x-axis, φ is the steering wheel's angle with respect to the robot's longitudinal axis, and v is the velocity. (see Figure 1). A nonholonomic car-like robot is a car model which rolls without slipping between the wheels and the ground. This constraint is expressed by the equation [5] x sin(θ) −ẏ cos(θ) = 0. (1) The simplest model corresponds to a robot with a single wheel: the unicycle model. In this model the wheel rolls on a plane while keeping its body vertical. It is an unrestricted model since it can rotate freely while standing in its position (x, y). Furthermore, the dynamics are characterized by The kinematic model of a car-like robot has the same state variables as the unicycle model and its dynamic is represented by where u ∈ [− 1 r , 1 r ] stands for the curvature and r for the turning radius of the robot that corresponds the maximum curvature [6].

Optimal Control Problems
An optimal control problem (OCP) can be formulated as where J is the cost function, x is the state vector representing the dynamics, u is the control vector, x 0 is the initial configuration and x f is the final configuration.
The solution of the OCP can be obtained following a standard procedure, which consists in applying Pontryagin's maximum principle and obtaining the necessary optimality conditions along with the transversality condition resulting into a two-point boundary value problem (BVP).

Considering the Hamiltonian function
where F and f are the functions described above and λ = [λ 1 (t), λ 2 (t), . . . , λ n (t)] is a vector of co-state variables, and considering that (x, u * ) is a controlled trajectory defined over the interval [t 0 , t f ] then (x, u * ) is optimal, for all admissible controls u, if the Pontryagin's maximum principle holds, i.e., The Pontryagin maximum principle guarantees that if (x, u * ) is an optimal pair, a solution of the problem (4), then the first order necessary conditions together with the stationary conditions satisfies the Hamiltonian maximization with transversality conditions given by [3] λ This reduces the constrained problem (4) to an unconstrained differential equations system (6)- (8). Usually nonlinear, this system of differential equations can be approximately solved by a numerical method. The next section is devoted to introducing the Tau method, which will be used to numerically tackle the problem.

Tau Method
The spectral Tau method produces a polynomial approximation, y n (x), of the solution, y(x), of a given differential problem Dy(x) = f (x), satisfying a set of conditions defined on an interval ]a, b[. Introduced by Lanczos in 1938 [7] to compute approximate solutions of linear differential problems with polynomial coefficients and right-hand side, the Tau method solves a tuned system of linear algebraic equations obtained by imposing that the conditions are verified exactly and that the residual is minimized in a quadrature sense, i.e, is orthogonal to the first elements of an orthogonal polynomial basis. It can be applied, indifferently, to initial, boundary or mixed value problems and it can be implemented with any orthogonal basis. We begin by introducing the method for the original case and then shed some light on how to extend it for the solution of nonlinear problems with non-polynomial coefficients.

Preliminaries and Notation
Let P be the space of all algebraic polynomials and let D : P → P be a linear differential operator of order ν ≥ 1 with polynomial coefficients represented by and y be the exact solution of the differential problem where f ∈ P is a polynomial or a convenient polynomial approximation and g j are ν linear functionals, acting on C ν [a, b], representing the (supplementary) conditions. The main idea of the Tau method is to approximate y by the polynomial y n , solution of the perturbed problem where τ n is a polynomial perturbation close to zero in ]a, b[. Choosing an orthogonal polynomial basis P = [P 0 , P 1 . . .], then the coefficients of y n are determined imposing that τ n is orthogonal to P i , i = 0, 1, . . . , n − ν.
The original idea of the Tau method [8] is based on the minimax property of Chebyshev polynomials and on the fact that the solution y n of (11) depends continuously on the residual τ n . Later generalized to more general bases [4], the method looks for a residual τ n that minimizes the weighted norm ||.|| w associated to the sequence P. Indeed, P is orthogonal with respect to the weight function w(x) on [a, b] w and δ ij is the Kronecker delta, and (11) is achieved by imposing τ n , P j = 0, j = 0, 1, . . . , n − ν, that is, τ n = O(P n−ν ). Using suitable matrices, see for example [4,9,10], the differential problem (11) is translated into an algebraic problem. Such matrices must be computed with criteria in order to ensure stable computations [10].

Nonlinear Problems
Nonlinear differential problems are solved iteratively by first linearizing the problem and then applying the Tau method to the linear inner problem. Let be a differential equation, where F is a differential operator that could be nonlinear in y and on its derivatives. From y m , an approximation of the exact solution y, we take the first order Taylor polynomial of F centered in y m to approximate F (13) Applying the Tau method to the linear differential Equation (14), an iterative process is implemented to get increasingly better approximations for the differential problem D m y m+1 = 0, m = 0, 1, . . .
For additional details on the use of the Tau method for nonlinear problems the reader is invited to read [11].

Numerical Experiments
The proposed example is based on the work of [12] and it will be tackled by the Tau method, described in Section 4.
The problem at hand is an optimal control problem of the form (4) with F = 1 2 (u 2 1 + u 2 2 ) and G = 1 2 x T (t f )Qx(t f ) where x is the vector of state variables and Q is the weighting matrix.

System Model
Consider an automated vehicle that moves on a horizontal plane, the contact of each wheel with the floor is assumed to satisfy the rolling without slipping condition and the control inputs are the torque generated by two motors mounted on the wheels. For a fixed final time, it is desired to find the control inputs that minimizes the energy of the final state. This system can be modeled by where the position coordinates (x, y), and the heading angle θ, define the system configuration. The mass m, the inertia I, the wheels' radius R, the half-length of the axis L, are parameters of the system and µ 1 and µ 2 are torques generated by the motors. Defining the control inputs u = [u 1 u 2 ] T as and the state variables x 4 =ẋ cos(θ) +ẏ sin(θ) −θ(x sin(θ) − y cos(θ)) x 5 =θ.
Equations (17)-(19) can be reduced to the following system of differential equations: Applying the Hamiltonian H, defined in (5), , and calculating the necessary conditions (6), the stationary conditions (7) and the transversality conditions (8), the following second order system of differential equations is obtained: forẋ 1 = x 4 ,ẋ 2 = x 5 ,λ 4 = −λ 1 andλ 2 = 0, with initial and transversality conditions given by Since this is a nonlinear differential system, in order to implement the Tau method, differential equations need to be linearized. Expressions of the form uv and uv 2 will be replaced, respectively, by Thus, the differential system becomes . .] represents the action of boundary conditions (20) over the polynomial base elements and P'(t i ) its derivatives. Matrix D represents the differential operator ,m (M) + N 2 and M and N are matrices described in [4] representing, respectively, the multiplication and the differentiation operator.

Numerical Results
In this section we report the numerical results for the example described in Section 5.1 with initial positions (x, y) = (10, 3) and heading angle 0 • . The time interval is [t 0 , f f ] = [0, 5] and the system parameters used in the simulation are m = 10 kg, I = 1.2 kg·m 2 , R = 0.05 m and L = 0.1 m. The weighting matrix is Q = 10I, where I stands for the identity matrix.
The simulation results were obtained using the Tau Toolbox with Chebyshev polynomials. The state trajectories x 1 , x 2 and x 3 and the optimal trajectory for the position (x, y) are illustrated in Figures 2 and 3. The trajectories were obtained with 5th order Chebyshev polynomials.    Table 1 presents the values for the functional J defined in (16) using polynomials of degree n = 5, 10 and 15. Since u 1 = −λ 4 and u 2 =ẋ 5 , the integral 5 0 1 2 (u 2 1 + u 2 2 )dt and x(5) T Qx(5) can be calculated using the the approximate solutions for the state variables x i , i = 1, . . . , 5 and the co-state variable λ 4 .
As the polynomial multiplication and differentiation, the integration can be set into algebraic operation as well, using a suitable matrix [4].

Conclusions
The Lanczos spectral Tau method was used to compute approximate polynomial solutions for nonholonomic systems. A detailed illustration on the approximation procedure is offered. The Tau toolbox provides the appropriate environment to solve systems of ordinary differential problems while allowing for accurate solutions, whenever the sought solution is regular. Numerical results for this dynamical optimization problem confirm both aspects: ease of use and accuracy of approximation.