Trajectory planning for multi-input-multi-output linear systems subject to nonlinear inequality constraints on the output

Trajectory planning is an imperative aspect in aviation, robotic manipulation, navigation of mobile robots, and unmanned arial and underwater vehicles. A popular approach to trajectory planning is to formulate it in the setting of a constrained optimization problem. In this approach the cost of control is minimized subject to path constraints specified by nonlinear inequality constraints on the output trajectory at predefined time instances. This problem was first solved for the case of single-input-multi-output linear systems. In the present study the results have been extended to the more general multiinput-multi-output linear systems. The convex nature of the resulting optimization problem ensures a unique solution. A methodology based on the Lagrange multiplier technique is used for the computation of the unique solution. An explicit solution for the optimal output trajectory as well as the controller that will ensure the real time generation of the solution are derived in terms of the solution to the nonlinear equations. An example ubiquitous in the field of nonholonomic mobile robots is used to illustrate the results derived.


INTRODUCTION
Planning is an important part of all day-to-day activities.For instance when one needs to travel from one place to another, it may not be possible to reach the destination by travelling in a straight line.This is influenced by various factors such as obstacles, cost, safety, time, etc. Therefore, it is necessary to plan a proper course or path based on these factors.This process is usually called route planning (Fujimura, 1991) and is an imperative task in many areas such as robotics, aeronautics, space travelling, navigation and biomechanics.For example consider an intelligent mobile robot working in a factory and suppose it is required that it carries an object from a point A to another point B. Several factors need to be considered prior to actually deciding the course and motion of the robot.There may be other objects between A and B that prevent the robot from moving in a straight line.There may be certain areas where the robot may not function properly and hence need to be avoided.The robot may have certain movement and speed limitations or there may be some intermediate points the robot has to stop and do something else.Thus the solution is to decide on a suitable path and a course of action the robot needs to follow.This is usually not an easy task since there are limitations on the available resources that need to be considered as well.Further it might also be needed for the resources to be used in an optimal way so as to reduce the cost or time.The terms path planning or motion planning refer to this type of computational process of moving an object from one place to another, subject to the constraint of the optimal use of resources.Therefore the problem is naturally formulated as a constrained optimization problem (Fujimura, 1991;Somlo et al., 1997;Sciavicco & Siciliano, 2000;Vossen & Maurer, 2006).
In order to avoid confusion between the terms often used as synonyms, the difference between a path and a trajectory needs to be explained.A path denotes the locus of points in space that the object has to follow in the execution of the assigned motion.Thus a path is a purely geometric description of motion.On the other hand, a trajectory is a path on which a time law is specified.For example, the velocity and the acceleration information along a path may be specified.Thus path planning is the determination of the geometry of motion and trajectory planning is the determination of the time history of motion.This paper focused on trajectory planning problem, which is a fundamental problem in control theory, robotic manipulation and mobile robot navigation.Usually the problem appears in two different versions.The most general version is that the complete route is not known in advance and must be planned online.The other, and the simpler version is that we are given a sequence of target points and target times and are required to reach the target points at the target times in an approximate sense.This is typical in problems such as path planning in air traffic control, problems in industrial robotics and in switched control systems.For instance in switched control systems, it is required to generate curves that pass through pre-defined states at pre-defined time instances, where switching needs to take place.In principle, it can be conceived that the inputs to a trajectory planning algorithm are the path description, path constraints and the constraints imposed by the dynamics of the object, whereas the outputs are the trajectories usually specified in terms of a sequence of points and time instances at which the object should be at the given points.In order to realize the motion prescribed by path and trajectory planning, the necessary control action needs to be computed.Thus, the ultimate goal of the trajectory planning problem is to generate the reference inputs or the control action to the motion control system, which ensures that the object follows the planned trajectory.
A review of the recent state of the art trajectory planning for robotic manipulators can be found in Gregory et al. (2012).Much of the hitherto existing literature is concerned with time-optimality criteria.The consideration of minimum energy criteria is less common with Lampariello et al. (2003), Vossen and Maurer (2006) and Gregory et al. (2012) providing an overview of the existing methods.All these methods only consider equality type constraints on the final state.
To the best of our knowledge only Egerstedt andMartin (1998, 2001) considers inequality constraints on the final state.For linear single input systems they propose the use of standard Lagrange multiplier methods to solve an energy optimal problem subject to the constraints of linear dynamics and nonlinear inequality constraints on the output at pre-defined time instances.The major contribution of the present work is to generalize this result to the multi-input-multi-output (MIMO) case.Necessary and sufficient conditions are proved for the existence of a unique solution.Explicit solutions as well as the controller that will ensure that the system output will exactly follow the optimal output trajectory in real time are constructed.
The trajectory planning problem is formulated with introduction to relevant notations, the space we seek the solution to be in and the assumptions.The solution to the trajectory planning problem is obtained via the Lagrange multiplier method.This method was chosen since the trajectory planning problem can be formulated in the form of a standard constrained optimization problem.Typically the constraints on the end state are handled as equality constraints.We relax this and allow nonlinear inequality constraints on the end state as well as the more general problem of nonlinear inequality constraints on the output at pre-defined time instances.This controller is applied to the widely used example of the dynamically feedback linearizable kinematic model of the nonholonomic wheeled mobile robot (unicycle model).

PROBLEM FORMULATION AND NOTATIONS
The trajectory planning problem is mathematically formulated in this section.It is treated in the natural framework of a constrained optimization problem.The trajectory planning problem consists of two main aspects.One of which is to require that a given object follow a desired path while the other is to ensure that certain optimality criteria also be met.The optimality criteria usually arises out of the additional desire to achieve the desired tracking with minimum energy expenditure.Thus the trajectory planning problem can be formulated as an optimization problem of minimizing a given cost functional subject to the constraint that the object follow a given desired path exactly or as close to it as possible.
The cost functional is usually a measure of the control energy and can be expressed in the form of where u(t) is the control action on the object at time t, R is a given positive definite matrix representative of the respective weighting of each of the control components on the cost functional and [0,T] is the given time interval of interest.In this work the object is assumed to have controllable and observable linear dynamics given by, where x£ n ; u,y£ m and A, B, C are matrices of compatible dimensions.The output y(t) given by equation ( 3) describes the path of the object, u(t) is the control action and equation ( 2) describes the dynamics of the object.To make the case simple, in this paper it is assumed that initial data x 0 = 0. However the reader may refer to Palamakumbura et al. (2013) for the details of the more computationally involved case of x 0 ≠ 0.
The linearity assumption turns out to be a reasonable assumption especially in trajectory planning of commercial aircrafts since they have dynamics that can be linearized by feedback transformations.In reality, following a desired path exactly is not an easy task and sometimes may even be impossible.Hence, it is customary to reduce the path following requirement to an interpolation requirement where for a given sequence of ordered pairs is required that the object be at α i at time t i £.With respect to the linear system described by equation ( 2) -( 3) this amounts to the requirement that y(t i ) = α i .However this may also be too restrictive a problem to solve.In Egerstedt andMartin (1998, 2001) interval constraints are used to further relax the interpolation requirement.In here we use spherical constraints.That is, it is required that the object be within a ball B i , of suitable radius α i , centered at α i , at time t i , rather than it interpolates the given points, hence resulting in a quadratic inequality constraint.Therefore the problem in general can be formally defined as the problem of finding a control u(.) that belongs to a certain class of functions such that it minimizes (1) subject to the constraint that the object be within the ball B i at time t i .The class of admissible controls belongs to the Hilbert space H of measurable functions u[ 0,T ] such that < u, u > is finite where < , > is the inner product on H defined as Note that R is the positive definite matrix appearing in the cost function (1).This general problem will be formally stated below.
Let S = {(t i , α i );i =1,2,...N} be the given sequence of time instances and points and let B i = {z: z -Į i a i } be a ball around α i with radius a i .The radii a i s control the scale of relaxation in space.The relaxed requirement stated earlier can be thus stated as a quadratic constraint of the form In order to simplify the notations we define Thus the output y(t i ) of the system can be expressed as Note that the constraint ( 4) is also a function of the control action u(.).Thus the quadratic constraint trajectory planning problem can be formally stated as follows.

SOLUTIONS TO THE OPTIMIZATION PROBLEM
The above formulation of the trajectory planning problem is in the form of a constrained optimization problem.It can be shown that the cost functional J(u), given by equation ( 1), is a convex functional on H and the constraint ( 4) is a convex functional from H to x£ n .
The Lagrange multiplier method is the most suitable to solve such problems (Luenberger, 1969).Following the Lagrange multiplier theory (Luenberger 1969;Bertsekas, 1982Bertsekas, , 1995;;Nash & Ariela, 1996) it is seen that the Lagrange multiplier vector can be identified as a vector in the dual of the range of the constraint function and appears as a hyperplane in the range, thus we can equivalently reformulate the problem by constructing the Lagrangian, ... (7) and solving the optimal control problem λ ≥ 0 u ... (8) max min H (u, λ) Here λ ≥ 0 means that for each i, λ i ≥ 0. Now our goal is to minimize the functional H (u, λ) over the Hilbert space H.To find the necessary conditions for a minimum u, we calculate the Gateaux derivative in the direction of a control w (Luenberger, 1969) and obtain the following.
where Λ is the block diagonal matrix given by

%
and I m is the m × m identity matrix.It is clear that the optimal cost in the quadratic constraint problem is cubic in λ and quadratic in τ.Now in order to find the optimal u(.), we have to find the optimal λ and the optimal τ.
For that we calculate the Gateaux derivative of equation ( 12) with respect to τ and λ (Luenberger, 1969).To find optimal τ; We want D r H(τ,λ)γ ≡ 0 for all γ.Therefore we have From equation ( 13), we see that the only unknown appearing in the expression for optimal τ is λ.Now to find the optimal λ we compute, where W is the Nm × Nm block diagonal matrix From convexity it follows that a unique solution to the optimal control problem, min u H (u, λ) exists and therefore that this optimal u * is in fact given by the above expression.
From the above expression for optimal u we see that it is convenient to introduce the parameter τ i given by . Here the vectors n i and i .With this the optimal u must have the form Now by substituting equation ( 9) in equation ( 7) we form a new H(τ, λ) from the H(u, λ).Prior to that we define the matrix G to be the Grammian matrix (10) and define the matrix Now by substituting equation (9) in equation (7) we get Now u * is a function of τ and λ and hence the Lagrangian is a function of τ and λ.With the above expressions for the matrices G and G i , the Lagrangian can be written as a function of λ and τ in a simplified form, for all W.But we see from equation ( 14) that it involves both W and W i and as in the form in (14), it is not possible to derive an expression without W and W i .Hence we introduce another Nm×Nm block diagonal matrix I i as the matrix whose i th m × m diagonal block is the m × m identity matrix I m for i = 1,2,...,N, and the other entries are 0. With this we can re-write the equation (14) as Hence we have a system with N equations for λ = ( λ 1 , λ 2 ,..., λ N ) and by solving the system (16) with the expression for optimal τ we can find the optimal λ and τ.Now once we substitute equation (13) in equation ( 16), we get a system of nonlinear equations for λ.This is a system of N fourth order polynomial equations in λ i .Since the problem was formulated as a convex programming problem, the existence of the solution is guaranteed.However there can be more than one acceptable solution for the Lagrange multipliers, λ.If this is the case, we compute the Lagrangian equation ( 7) for all possible values and pick the value which solves equation (8).This gives the optimal control, u * which minimizes the desired cost function.We state this result formally as a lemma below: Lemma 3.1.Let ( τ * , λ * ) be the solution of the coupled set of equations ( 13) and ( 16) such that H( τ, λ) given by equation ( 12) is maximized.Then the optimal control equation ( 9) corresponding to ( τ * , λ * ) solves the optimal trajectory planning problem.

An example of optimal path planning for mobile robots
This section contains an example illustrating the results obtained in the previous section.Given a controllable and observable system our algorithm for solving the quadratic constraint problem is: • Construct the Lagrangian (7).
• If there are more than one real set of solutions pick the one that maximizes (12).
To simultaneously solve the coupled quartic polynomial system of equations ( 13) and ( 16) for optimal ( τ * , λ * ), any standard numerical nonlinear equation solving procedure may be used.
The example considered in this paper is the nonlinear kinematic unicycle model of a mobile robot.where (x,y,θ)  SE(3) (v, u w )  are the kinematic controls and z = (x,y)  2 , the position of the center of the reference frame fixed on the mobile robot, is the output of interest.The objective is to plan the trajectory for this system such that the mobile robot moves through the neighbourhood of the sequence of points The mobile robot is required to pass through a circle of radius r i centered at α i = (x i , y i ) at the time instant t i .
The 2-input 2-output mobile robot system is not state feedback linearizable with respect to the input u = (v, u w ) and output z = (x,y).Nevertheless it can be readily verified that it is dynamic output feedback linearizable with respect to the input u = (u v , u w ) and output z = (x, y) where now the dynamically extended system is given by: ))}, a unique optimal value of λ given by λ * (187.8,119.5, 35.9) is obtained.The trajectory of the output y(t) = (x (t), y(t)), y(t) vs t, the corresponding optimal control u * , and the heading angle θ are plotted in Figures 1 -4, respectively when the initial conditions of the robot is chosen to be (x(0), y (0), θ(0), v (0)) = (0,0,0,0).From equation (9) we see that the optimal control is a linear combination of the functions L i (t) given by equation (5).Since in this case A is a nilpotent matrix of order two we have that the L i (t) are linear functions of the time t.