real-time optimal control of moving vehicles in changing environments

IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 2, NO. 4, DECEMBER 1994 447

Real-Time Optimal Control of Moving Vehicles in Changing Environments Markos Papageorgiou and Alexander Steinkogler

Abstruct- An optimal control approach to vehicle motion control in dynamic environments, including moving obstacles, is presented along with a numerical optimization algorithm for calculation of the optimal control trajectories. Low computation times allow for the solution of the optimal motion control problem to be effectuated on-line, via the rolling horizon concept, based on updated real-time measurements of the number, the positions, the speeds, and the dimensions of the obstacles. The presented concept provides considerable flexibility with respect to the vehicle model, the vehicle mission, and the motion constraints to be considered. Simulation examples demonstrate the efficiency of the approach for some selected vehicle missions and conflict situations.

I. INTRODUCTION N the last decade much research has been devoted to the I navigation and path control of moving vehicles such as

mobile robots or robot arms. A particularly interesting problem in this context is path planning in changing environments which include moving obstacles or other moving vehicles. Proposed solutions and algorithms can be classified in several groups.

In a first class of methods, the path planning problem is resolved by accounting for geometric constraints only, without explicit consideration of the vehicle dynamics. These methods derive an optimal path for a complicated workspace cluttered with immobile obstacles (e.g., [l], [5], [6]) or with mobile obstacles [2], [4]. In the case of changing environments, the number, the dimensions, and the future trajectories of the moving obstacles are assumed to be known before plan generation. Due to the high computational effort, application of these methods is only possible to off-line path generation.

A second class of methods attempts to express the moving plan task in the format of an optimal control problem where vehicle dynamics are explicitly taken into account while (immobile or moving) obstacles are incorporated as inequality constraints [3], [SI, [17], [ 181. Again, number, dimensions, and future trajectories of moving obstacles are assumed to be known before plan generation. This approach offers a remarkable flexibility with respect to the vehicle model, the vehicle constraints, and the optimality criterion to be considered for optimal plan generation. Solution of the optimal control problem is achieved off-line by adequate numerical algorithms.

Manuscript received October 22, 1992; revised March 25, 1994. Recom-

M. Papageorgiou is with the Lehrstuhl fur Steuerungs- und Regelungstech-

A. Steinkogler is with the Lehrstuhl fur Systemtheorie der Elektrotechnik,

IEEE Log Number 9406372.

mended by Associate Editor, M. Steinbuch.

nik, Technische Universitat Munchen, D-80290 Munchen, Germany.

Universitat des Saarlandes, D-6604 1 Saarbriicken, Germany.

To avoid local minima and obtain globally optimal moving plans, Tominaga and Bavarian [ 171 suggested a suitable modification of the basic optimal control approach. Nevertheless, optimal control approaches are probably less suitable for global path finding in complicated static environments with general nonconvex constraints.

To enable real-time control of moving vehicles, using measurements from a changing environment, a fairly general framework based on stochastic dynamic programming was suggested in [ 1 11. This approach is attractive first because of its weak assumptions and second because the main computational effort is required off-line, namely for the derivation of a control law which is then applied in real time with moderate calculation requirements. Nevertheless, the method can hardly be directly applied to highly complex conflict situations due to the exponentially increasing computation effort required for off-line calculation of the control law.

Finally, perhaps the most popular approach for real-time control strategy design is the artificial potential approach [7], [16]. This remarkably simple and elegant approach is mainly suitable for local obstacle avoidance with immobile or low speed obstacles. Moreover, the flexibility with respect to the kind of possible vehicle missions appears rather limited, and the artificial potential approach may lead to a standstill of the vehicle due to local minima. Alternative approaches within this family include diffusion equation strategies [ 141.

In summary, development of generally applicable strategies for real-time control of moving vehicles in dynamic environments still face a number of open questions. This paper suggests a fairly general real-time control concept with the following main characteristics:

The basic approach follows the lines of optimal control methodology and is very similar to the one in [17] although it has been independently developed [ 151. The formulated optimal control problem is resolved by application of a powerful numerical algorithm allowing computation times of less than one second for some investigated conflict situations. In view of the low computation times, solution of the optimal control problem is effectuated in real-time, applying the rolling horizon method. More precisely, optimal moving plans are calculated each T seconds (e.g., T = Is) for a much longer horizon T >> T based on:

a) Updated measurements of the number, the dimensions, the current positions, and the speeds of obstacles.

1063-6536/94$04.00 0 1994 IEEE

448 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 2, NO. 4. DECEMBER 1994

b) Extrapolation of the obstacles’ movements to cover the time horizon 7.

This approach offers, first, the flexibility and general ap- plicability of optimal control methodology (although only a limited number of control scenarios are actually investigated in this paper). Second, due to frequent recalculation of the optimal moving plan based on updated real-time measurements, sensitivity of control results with respect to a priori unknown obstacle movements, vehicle model inaccuracies, and further unpredictable disturbances remains low. Third, it is not necessary to assume a priori knowledge of the number, the dimensions, and the future trajectories of moving obstacles. On the other hand, the suggested approach has not been tested and is probably less suitable for global path finding in complicated static environments, e.g., whole floors including several rooms with multiple possible paths.

The structure of the paper is as follows. Section I1 presents the basic optimal control problem formulation for vehicle plan generation. Sections I11 and IV describe the numerical solution algorithm and the rolling horizon concept respectively. Finally Sections V and V I are devoted to demonstration examples and summarizing remarks respectively.

11. OPTIMAL CONTROL OF MOVING VEHICLES

A. A General Optimal Control Problem

A general optimal control problem (OCP) which can be resolved by application of the algorithm to be presented in Section 111, is as follows:

Given an initial condition x ( 0 ) = XO, find the optimal control and state trajectories u(k) E R“, and x ( k + 1) E R”, k = 0, . . . , K - 1, respectively, so as to minimize the scalar cost criterion

K--l

J = .19[x(K)1 + cp[x(k), u(k), kl (1) k=O

subject to the dynamic equality constraints

x ( k + 1) = f [x (k ) , u(k), IC]

u m i n ( k ) 5 ~ ( k ) 5 u m a x ( k )

(2 )

and the inequality constraints

(3)

taken for k = O , . . . , K - 1.

In this discrete-time formulation, we denote the variables’ values at discrete times k . T by . ( k ) = . ( k T ) where T is the sample time interval, e.g., T = Is, and k = 0, 1, 2, . . . is the discrete-time index. The optimization horizon K may be fixed or free (to be provided by the optimization routine). The cost criterion (1) essentially expresses the control goal while the dynamic equality constraints (2 ) and the inequality constraints (3) represent the dynamic mathematical model of the process under control and possible control restrictions respectively.

In the remainder of this section, we will provide an optimal control formulation for moving vehicles which fits the format of (1)-(3).

B. Representation of Obstacles

For the sake of simplicity we will assume that the vehicle and the obstacles move without changing their respective orientations or that they have a circular (in three dimensions: spherical) form or that they can be represented by a circular/spherical hull. Each of these assumptions implies that we may, without loss of generality, consider the vehicle to be a moving point, its dimensions being transferred to the obstacles. These assumptions are not binding for the present concept and, in fact, they can be removed to consider more general situations as explained in [ 111.

We denote by x ( J ) ( k ) the distance of a reference point of a (generally moving) object no. j from the vehicle at time k . T . Then, the p-dimensional piece of space (p being equal to two or three) in vehicle’s coordinates which is occupied by the obstacle (augmented by the vehicle’s dimensions according to our assumption) may be expressed by the set

E(3)(IC) = (6 E RP I g ( ’ ) [ ~ ( ~ ) ( k ) , 61 5 0 } (4)

where g ( J ) is a vector of functions describing the shape and dimensions of the obstacle (typically, the individual functions 9:) represent lines, cycles, ellipses, etc.). For example, in two dimensions (p = 2), if the obstacle j is a disc with radius R and if its reference point is the disc center, we have

2 ( J ) ( k ) = (6 E R2([<1 - z y ( k ) ] 2

+ [& - 2$)(k)I2 - R2 5 0 ) . ( 5 )

According to this representation, a collision of the vehicle with the obstacle j takes place iff

g ( J ) [ x ( J ) ( k ) , 01 5 0. (6)

Equation (4) provides a fairly general obstacle representation which allows consideration of the whole immobile vehicle’s environment as a single obstacle; see [ l l ] for details. Nev- ertheless, the scope of the present paper is limited to cases involving circular or elliptic vehicle and obstacles, straight corridor walls, and other simple shapes.

C. The Vehicle Model

The optimal control approach allows for a broad spectrum of possible vehicle models to be considered according to the particular application requirements. The utilized vehicle model must merely meet the form of model equations ( 2 ) and of constraints (3). The most simple model for vehicle movement in a p-dimensional Cartesian space reads

~ ( k + 1) ~ ( k ) + ~ ( k ) . T (7)

where x E R P is the vehicle position and v E RP is the vehicle speed which is assumed to be completely (all p components) or partially controllable. Possible control constraints for the consideration of speed limitations are

(8) vmin 5 ~ ( k ) 5 vmax .

Tominaga and Bavarian [ 171 assume the vehicle acceleration a E R P instead of the vehicle speed to be controllable in

IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 2, NO. 4, DECEMBER 1994 449

which case (7) is augmented by

v(k + 1) = v(k) + a(k). T (9)

where v(k) now becomes a further state variable and a(k) is the new (possibly constrained) control input. In [3], [8] more sophisticated dynamic vehicle models are considered which include vehicle orientations and angular speeds, thus allowing for motion control of robot arm movements. All these models can be expressed in the format of (2) and (3). Possible state variable constraints which do not fit the structure of (2) and (3) may be included as additional penalty terms in the cost criterion to be minimized.

D. The Vehicle Mission

The vehicle mission is essentially reflected in the choice of the cost criterion to be minimized. The optimal control approach allows for a whole spectrum of possible vehicle missions to be considered. For example, the vehicle may have to move in a given space direction (e.g., move along (or cross) a corridor or a factory hall) of a p-dimensional space with a speed component w1 which is as close as possible to a given desired speed V I , d , avoiding collisions with obstacles and avoiding unnecessary deviations. We will call this Mission 1. Mission 1 may be expressed by the following cost criterion to be minimized

K-1 I P

N

+ Z4[g(j)(x(jW7 0)I j (10) j=1

where N is the number of obstacles. The following clarifica- tions are required with regard to this cost criterion choice:

The first term in the sum penalizes any retarding or acceleration of the vehicle along its moving direction w1.

The second term penalizes any deviations along the other directions 212, . . . , wp. For the third term let us first define the function as follows

4 = e- (1 1)

where &) = m;dx{g!j)[x(j)(k), o]}.

With this definition, the third term penalizes collision with an obstacle. To see this, we note that according to (6) a collision occurs iff g ( j ) [ x ( j ) ( I C ) , 01 5 0, i.e., if the origin falls into the space occupied by an object no. j. In this case, we have &) 5 0, hence 4 becomes greater one and is penalized in (10). Else, for increasing positive values of &), 4 obtains gradually decreasing values in the range [0, 11 which corresponds to a smooth penalization of approach to obstacle no. j. Taking squares in the first two terms leads to stronger penalization of larger deviations. Absolute values or any other positive definite penalizing function may be used instead.

The choice of the weighting parameters a1, . . . , aP deter- mines the relative importance of delays and of deviations as compared to the collision risk. Too high values of these parameters may lead to a risky policy whereas too low values may lead to a hesitating moving. The weighting parameters must be specified off-line via a trial-and-error procedure.

Another vehicle mission (Mission 2) may be to follow a given trajectory avoiding collisions with obstacles and avoiding unnecessary deviations. Assume a sequence of given desired positions x d ( k ) , k = 0,. . . , K , on this trajectory. We also define d[x(k)] to be the geometrically minimum distance of the vehicle from the prescribed trajectory at time k. Obviously, for a given desired trajectory, this distance only depends on the current vehicle position x( k ) . With these definitions, Mission 2 may be tackled by minimizing the following cost criterion

K-1 I

k=O N

j = 1

where 1 1 . [ I 2 = -T. is the quadratic norm and a 1 , a2

are weighting parameters. The first term in (12) penalizes spacehime deviations of the vehicle from the prescribed trajectory while the second term penalizes purely space deviations from the prescribed trajectory. As in (lo), the third term in (12) penalizes collisions with obstacles.

Yet another vehicle mission (Mission 3) may be to move towards (and reach) a given final position xd with a speed that is as close as possible to a given desired (scalar) speed Vd,

avoiding collisions with obstacles and avoiding unnecessary deviations. In this case, which is a generalization of Mission 1, we obtain at each time instant a desired speed vector vd(k) in dependence from the current vehicle speed x(k) from

vd(k) = wd ’ [x(k) - x d ] / l l x ( k ) - x d l l - (13)

Thus Vd(k) has the direction x(k) - x d (from the current position to the desired position) and a length Wd that equals the desired scalar speed. With these definitions, Mission 3 may be achieved by minimization of the following cost criterion

allv(k) - vd(k)1I2 + E 4 [ g ( ’ ) ( X ( ’ ) ( k ) , O)]} N

J = k=O j = 1 “’( (14)

where a is again a weighting parameter. In a similar way, a number of alternative or complementary

missions may be considered in the cost criterion by adding or exchanging penalty terms with their corresponding weighting factors. One may, for example, provide suitable terms for hitting (instead of avoiding collision with) obstacles [ 1 11. In [17] a final desired state F ( K ) = 0 is considered via a quadratic penalty end-time term X ( K > ~ G X ( K ) where G is a weighting matrix. In [3], [8] the cost criterion minimizes power consumption when moving from a given initial position to a final position.

450 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 2, NO. 4, DECEMBER 1994

E. An Optimal Motion Control Problem

We conclude that there is a great variety of vehicle models, motion constraints, and vehicle missions which can be incorporated in the optimal control methodology frame where each combination leads to a corresponding optimal control problem formulation. For all of these problems we will, for the time being, make the following assumptions with respect to the obstacles:

1) At time k = 0 the number N , the initial positions x(j)(O), and the initial speeds v(j)(O), j = 1, . . . , N of the obstacles are known.

2) The obstacles move with constant speeds v(j)(O) during the optimization horizon.

3) The dimensions of the obstacles, expressed by g ( j ) , j = I , . . . , N , are known.

Clearly, these assumptions will generally prove wrong, e.g., because moving obstacles may change speeds or because new obstacles may show up, etc. The rolling horizon concept to be presented in Section IV will take care of these environment changes.

Under the above assumptions, the distance of an obstacle from the vehicle at future times k = 1, .. . , K may be expressed in terms of the vehicle’s position x(k)

x(j)(k) = ~ ( k ) - v(’)(O). k .T - ~ ( 0 ) + x(’)(O) - ~ ( 0 ) . (15)

Consequently we may replace g ( j ) [x(j) ( k ) , 01 = G(j) [x( k ) ] to be included in (lo), (12), or (14). In other words, under the above assumptions, the last term of the cost criterion depends upon the state variables x(k) only.

We may now, as an example, state the following optimal motion control problem (OMCP):

1) Given N , x(j)(O), v(j)(O), x(0); 2) Find the optimal control and state trajectories u(k),

3) So as to minimize the cost criterion (lo), (12), or (14) subject to model (7) and constraints (8).

x ( k + l ) , k = O , . . . , K - l ;

Clearly, different vehicle models will give rise to different optimal motion control problems, but the overall problem structure (and hence the solution algorithm) remains the same.

The utilization of penalty terms (1 1) for collision avoidance may resemble the artificial potential function approach [71. It is important, however, to emphasize that both approaches are fundamentally different from each other. The optimal control approach utilizes information on the vehicle motion and the obstacles’ speeds to provide automatically a veritable motion planning. This may, for example, include vehicle acceleration to quickly pass a risky region, early deviation to avoid later complications, or vehicle deceleration to let a moving obstacle pass, etc. On the contrary, artificial potential strategies simply react fairly myopically in the very vicinity of an obstacle without taking into account its speed and without considering optimal deviation manoeuvres in case of multiple moving obstacles; see [ 151 for a more detailed comparison and discussion of the two methods.

111. A NUMERICAL SOLUTION ALGORITHM

A. Preliminaries

This section describes a numerical solution algorithm for the OCP presented in the last section. The algorithm is based on the necessary optimality conditions for OCPS’s [13] and may be found in more detail in [9], [lo].

For a given admissible trajectory u(k), k = 0, . . . , K - 1, the according state trajectory x(k) can be found by solving (2) starting with the known initial condition, and hence the cost criterion can be regarded depending on the control variables only, i.e., J = J ( u ) . The gradient of J with respect to u(k) on the equality constraints surface is given by

g ( k ) = a J / a u ( k ) + [afT/au(k)]X(k + 1) (16)

where the costate variables X E R” satisfy

X ( k ) = aJ /ax(k) + [afT/ax(k)]X(k + l),

k = O , . . . , K - l (17) X(K) = ae/ax(K). (18)

The gradient g ( k ) given in (16) corresponds to dH/du(k) where H is the usual Hamiltonian of the optimal control problem.

The component no. i of the projected gradient with respect to the admissible control region (3) is given by

In analogy to the L2-space of continuous-time systems, we define the scalar product of two vector trajectories ~ ( k ) , [ ( k ) , k = 0 , ‘ . . , K - 1, as follows

K-1

(7, C) = C7(k)TC(k). (20) k=O

Furthermore, we define a saturation vector function sat (7) with components

{vi else.

With above definitions, the necessary conditions of optimality [13] for OCP are given by (2), (17), (18), and

vi,max if vi > vi,max sati (7) = vi,min if vi < ~ i , m i n

Ti(k) = 0 vz Vk. (21)

Hence if these equations are satisfied simultaneously by some trajectories x(k), u(k), and X(k), a stationary point of the optimal control problem has been found.

B. Algorithm Structure

The necessary conditions for optimality constitute a two- point-boundary-value-problem (TPBVP). A well-known algo- rithmical structure for direct solution of the TPBVP can be described as follows:

Step I ) Select an admissible initial control trajectory u(O)(k), k = 0 , . . . , K - 1; set iteration index 1 = 0.

IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 2, NO. 4, DECEMBER 1994 45 1

Step 2 ) Using u(')(k), k = 0, . . . , K - 1, solve (2) from known initial condition to obtain x(')(k + 1); using u(')(IC), x(')(k + l), IC = O,.. . ,K - 1, solve (17) from terminal condition (18) to obtain A( ' ) (k + 1).

~tep3)~s ingu( ' ) (k) ,x( ' ) (k) , A ( ' ) ( ~ + I ) , IC = o , . . - ,K- I , calculate the gradients g ( ' ) ( k ) and # ) ( k ) from (16) and (19), respectively.

Step 4) Specify a search direction p(')(k), k = 0, . . . , K - 1 (see below).

Step 5 ) Apply a one-dimensional search routine along the p(')-direction to obtain a new, improved control trajectory u('+l)(k), i.e.,

~(u('++l)) = min J[sat (U(') + a(')p('))] d')

where a(') is the scalar step length (see below). Step 6) If, for a given scalar E > 0, the condition

(-y('), -y('))) < E holds, stop; otherwise, set 1: = 1 + 1 and go back to Step 2).

The described algorithm is known to be insensitive with respect to poor initial guess trajectories U(') ( k ) . For the optimal motion control problem considered here, the initial control trajectory u(')(lc) may be selected to correspond exactly to the desired vehicle movement without considering possible obstacle collisions [15].

No matrix operations are required for algorithm execution. Computer storage and computer time requirements usually increase polynomially with the problem dimension. Unfor- tunately, a precise (a priori) theoretical evaluation of the required computation time is not possible for this kind of algorithms.

In the case of a convex cost criterion (l), linear model equations (2), and linear constraints (3), a global minimum will be reached at convergence. In the general nonlinear case, the algorithm will reach at least a local minimum at convergence. For the specific application of optimal vehicle motion planning, however, a suitable modification of the problem formulation was suggested in [17] so as to avoid locally optimal paths.

C. SpeciJication of a Search Direction p(')

Several techniques for specifying a search direction p(') in Step 4) of the algorithm can be applied including steepest descent, conjugate gradients, and variable metric (see [12] for a comparative study). For example, search direction using a conjugate gradient rule reads

and for 1 2 1

where

Restarting with (22) should be ordered any time a sufficient negativity condition

does not hold where the scalar parameter B may be chosen in the order of 0.1. Moreover, periodical restarting, e.g., every m . K iterations, may be ordered to improve the convergence rate of the algorithm. Finally, a scaled version of (22)-(24) may also speed up convergence under certain conditions [lo].

D. One-Dimensional Search Routine

The one-dimensional search routine required in Step 5) of the algorithm proceeds by first producing an estimate for the line step using quadratic interpolation. So long as the location of the line-minimum on the search direction is not exceeded, further steps are taken, each with a double length than the previous step. Then, when the line minimum has finally been exceeded, the routine continues the search by use of quadratic interpolation until a suitable stop criterion is fulfilled; see [ 101 for details.

E. Remarks

The described optimal control algorithm is similar to the one utilized in [17]. There are, however, a number of important differences. First, the use of conjugate gradients instead of steepest descent as a search direction [Step 4)] leads to a dramatic speed up of convergence. Second, due to the one-dimensional routine [Step 5)] , improvement of the control variables at each iteration is optimized which leads to guaranteed convergence and lower computation times. Third, the algorithm utilized here is capable of handling control constraints (3). Finally, the optimal control problem considered here is a discrete-time one.

As it stands, the described numerical algorithm is only applicable to optimal control problems with fixed final times K . Consideration of problems with free final time (including minimum time problems) requires suitable modifications of the algorithm [ 101. These modifications typically involve multiple problem solutions with different final times K using the algorithm of Section I11 -B and may therefore increase the required computation time considerably.

The presented optimal control algorithm is known to be robust (regarding convergence) and very efficient (regarding computation time). Computation times for the examples of vehicle motion control to be presented in Section V, do not exceed 1 s on an IBM-compatible 80386 (20 MHz) with coprocessor 80387.

Due to the low computation time, the presented algorithm may be embedded in a closed-loop structure to be carried out in real time which will be described in the next section. In this context, it is important to underline that the proposed algorithm is a feasible one, i.e., all model equations and constraints are satisfied at each iteration. Hence, in a real-time environment, the algorithm may be stopped at any time, if some prescribed time limits are exceeded.

IV. ROLLING HORIZON CONCEPT

The rolling horizon concept provides the way of imbedding an initially open-loop optimal control approach in a closed- loop structure utilizing real-time measurements. The rolling


horizon approach in the context of optimal vehicle motion control is as follows:

At time IC = 0 collect measurements of the number N , the positions x(j)(O), the speeds v(j)(O), and the dimensions g ( j ) of the obstacles included in the vehicle’s environment; based on these measurements and under the assumption that the obstacles will move with the same speed in the future, solve the optimal motion control problem using the numerical algorithm with a sufficiently long optimization horizon K to provide

Apply u(0), . . . , U(K - 1) to the vehicle where n, 1 5 n << K is the chosen repetition period. At time k = K collect new measurements as above including updated information on the obstacle dimensions; solve the optimal motion control problem to provide

Apply U ( & ) , . . . , u(2n - 1) to the vehicle. At time IC = 26 collect new measurements and solve the optimal motion control problem to provide

u(0), . . . ,u(K - 1).

u ( K ) ; . . , u ( ~ + K - 1).

u ( ~ I c . ) , . . . , u ( ~ I c . + K - 1).

and so forth.. . . The advantage of this approach is due to the combination of

the efficient optimal control procedure with the utilization of real-time measurements. The frequency of recalculation of the optimal control trajectories is determined by the chosen value of n. In fact, every K time steps, an optimal dynamic motion plan over a period K >> K is calculated taking into account all knowledge made available by the real-time measurements and making a reasonable extrapolation of obstacle movements into the future. Clearly the rolling horizon concept is only applicable if the execution time of the numerical algorithm remains well below n . T . Usually, computation times in the order of the sampling time interval T are sufficient.

V. DEMONSTRATION EXAMPLES

A. General Assumptions

We will now present some simulation examples demonstrat- ing the behavior of the suggested optimal control procedure combined with the rolling horizon concept with sample time interval T = 1, repetition period IC. = 1, and optimization horizon K = 25. For all examples, the vehicle model (7) with constraints (8) have been considered in a two-dimensional space ( p = 2). All mobile obstacles of these particular examples are assumed to have an elliptic (or circular) shape with radii T-?), r;). The penalty function 1c, included in the last term of cost criteria (IO), (12), or (14) was defined as

+[x(j)(l~>l = e-‘

with

which is slightly different than (11). For each example, to facilitate visibility of the corresponding figures, the physical dimensions of the vehicle and of the involved obstacles are

t

vchi cl e

obstacle t Fig. 1. Collision avoidance in a comdor: Mission 1 .

only depicted at their corresponding initial positions. For all examples reported here, computation times required for (re-) calculation of the optimal vehicle trajectories at each time step did not exceed 1 s.

B. Mission 1

In the first example, we will consider Mission 1 for the vehicle. More precisely, the vehicle’s mission is to move with a desired speed Vd, 1 in the direction of the horizontal axis 1 of the two-dimensional space avoiding collisions with moving obstacles. For the example considering Mission 1, the utilized weighting parameters in (IO) have been chosen a1 = a2 = 0.02. The reported results, however, do not change significantly for a range 0.2 5 ai 5 0.01, i = 1, 2, of the weighting parameters [ 151.

In the first example, depicted on Fig. 1, we have a desired speed along the axis 1 of vd, 1 = 2 and a maximum admissible speed of umax, i = 2.1 for both coordinates i = 1 and 2. The example involves two obstacles moving in a nonpredictable way in a corridor with a given width. The task of the vehicle is to move horizontally in the opposite direction avoiding collision with the obstacles and the corridor walls and without having any a priori information about the future movement of the obstacles. The corridor walls are considered here as immobile obstacles that are included in the cost criterion (10) according to the penalty term (11). Based on real-time measurements of the current obstacle movement and on the extrapolation and assumptions mentioned in Section 11-E, the vehicle succeeds, using the rolling horizon concept, to manage the fairly difficult conflict situation in an effective way.

C. Mission 2 Figs. 2(a) and 2(b) depict two scenarios based on Mission

2, where the vehicle attempts to move on the axis I of the two-dimensional space with desired speed vd, 1 = 2 and with umax, i = 2.1, i = 1, 2, avoiding collisons with moving obstacles. From this particular mission, we obtain for (12) the two-dimensional vector Xd(k) = [x1(0) + Vd, 1 . k . T OIT and the shortest distance d [ x ( k ) ] = 1 q ( I C ) I . The results with Mission 2 reported here have been obtained with the weighting parameters a1 = a 2 = 0.02 in cost criterion (12). In each of the examples depicted in Figs. 2(a), 2(b), three obstacles

IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 2, NO. 4, DECEMBER 1994

~

453

a x i s 2

h r l

a x i s 2

a x i s 1

t o b s t a c l e

(b)

Fig. 2. Following a trajectory: The axis 1 (Mission 2).

are again moving along nonpredictable trajectories in the two- dimensional space and the vehicle has to find its collision-free path based on current measurements only, without knowledge of the future obstacle movements, by applying the rolling horizon concept. Note that the vehicle actually revises its decision on the direction and the extent of the necessary deviations whenever a significant change of obstacle motion takes place. Note also, that the vehicle automatically tums back to the axis 1, the prescribed trajectory, after having crossed the risky area.

D. Mission 3 Fig. 3 depicts a final scenario based on Mission 3, where

the vehicle attempts to reach the final point (16, 0) in the two-dimensional space with desired scalar speed ud = 2 and with V,,~,; = 2.1, i = 1, 2, avoiding collisions with moving obstacles. For this particular mission, we obtain from (13) the desired vector speed

vd(k) = 2 { x ( k ) - [16 0 ] ’ } / J [ ~ i ( k ) - 161’ + ~‘(k)’ to be considered in the cost criterion (14). The chosen weighting parameter in (14) has been again a = 0.02. As seen in Fig. 3, two mobile obstacles moving along nonpredictable trajectories, are included in the vehicle’s environment. The vehicle, starting at (- 10, 0), again finds a dynamic, collision- free path to reach the destination point (16,O). Note that at time instants k = 7, 8, 9 the vehicle chooses to practically stand still and let the two obstacles pass left and right of it, before continuing its way towards the final point.

The described optimal motion control approach has lead to encouraging results in a high number of further scenarios including complex conflict situations with multiple moving obstacles. Some of these further examples are reported in [15].

a i V

\ des ti n a t i o n p o i n t

Fig. 3. Reaching a destination point: (16,O) (Mission 3).

TABLE I COMPUTATION TIMES IN s FOR DIFFERENT SCENARIOS

No. of Obstacles

1 2 3 4 5 Scenario Type

Series 1 (easy) 0.04 0.05 0.05 0.06 0.07 Series 2 (complex) 1.1 1.22 1.97 2.71 3.26

E. Computation Time

It is hardly possible to make general statements regarding the required computation time related to the suggested approach because computation time depends not only on the number of involved obstacles and on the mission type but, mainly, on the degree of complexity of the conflict situation that can be hardly parameterized. As an example, Table I provides computation times required for calculation of optimal motion control (Mission 1) in a two-dimensional space with K = 20 using an IBM-compatible 80386 (20 MHz) with coprocessor 80387 for two series of scenarios each involving one to five obstacles. The first series involves “easy” situations with very few necessary vehicle deviations. The second series involves very complex conflict situations, each requiring a real slalom of the vehicle between moving obstacles. It may be seen from Table I that consideration of many obstacles leads to a roughly proportional increase of computation time, the value of which, on the other hand, is mainly dependent on the degree of conflict complexity. We recall, however, that, due to the feasibility (see Section 111-E) of the suggested numerical algorithm, calculations may be stopped at any time, even if the stopping criterion in Step 6) (see Section 111-B) is not fulfilled. In fact, the basic shape of the optimal motion trajectory is formed in the first few algorithm’s iterations, and only an increasingly fine tuning is effectuated in later iterations. For example, all successful motion trajectory calculations reported in this brief paper were effectuated with an upper limit of one s for the computation time.

VI. CONCLUSION

An optimal control approach for real-time vehicle motion control based on the rolling horizon concept has been presented. The approach relies on an efficient numerical optimization algorithm which enables the solution of the optimal control problem to be effectuated in real time. The


optimal control approach allows for a variety of possible vehicle models, vehicle missions, and motion constraints to be directly considered. This optimal control approach results automatically in intelligent vehicle movement during complex conflict situations without involving complicated rules and other heuristics.

REFERENCES

[ l ] R. A. Brooks, “Solving the find-path problem by good representation of free space,” IEEE Trans. Syst., Man, Cybem., vol. SMC-13, pp. 190-197, 1983.

[2] M. Erdmann and T. Losano-Ptrez, “On multiple moving objects,” in Proc. IEEE Con$ Robotics and Automation, San Francisco, CA, 1986,

[3] C. Femandes, L. Gurvits, and Z. X. Li, “A variational approach to optimal nonholonomic motion planning,” in Proc. 1991 IEEE Int. Con$ Robotics and Automation, Sacramento, CA, 1991, pp. 6 8 M 8 5 .

[4] K. Fujimura and H. Samet, “A hierarchical strategy for path planning among moving obstacles,” IEEE Trans. Robotics and Automation. vol. 5, pp. 6 1 4 9 , 1989.

[5] P. Graglia and A. Meystel, “Planning minimum time trajectory in the transversality space of a robot,” in Proc. IEEE Int. Symp. Intel. Contr., Philadelphia, PA, 1987, pp. 82-87.

[6] S. S. Iyengar, C. C. Jorgensen, S. V. N. Rao, and C. N. Weisbin, “Robot navigation algorithms using learned spatial graphs,” Robotica, vol. 4,

[7] 0. Khatib, “Real-time obstacle avoidance for manipulators and mobile

pp. 1419-1424.

pp. 93-100, 1986.

robots,” Int. J . Robotics Res., vol. 5, pp. 90-98, 1986.

[8] S. Lewald, G. Abba, T. Rutschke, and E. Ostertag, “Minimal power consumption for an industrial robot with six axes,’’ in Proc. IFAC Symp. on Robot Contr., Vienna, Austria, 1991, pp. 605409.

[9] M. Papageorgiou, “Optimal multireservoir network control by the discrete maximum principle,” Water Resources Res., vol. 21, pp. 1824-1830, 1985.

[ 101 __ , “Optimierung-statische, dynamische, stochastische Vetfahrenfur die Anwendung. ”

[I I] M. Papageorgiou and T. Bauschert, “A stochastic control framework for moving vehicles in changing environments,” in Proc. IFAC Symp. Robot Contr., Vienna, Austria, 1991, pp. 765-770.

[12] M. Papageorgiou and R. Mayr, “Comparison of direct optimization algorithms for dynamic network flow control,” Optimal Contr. Appl. Meth., vol. 9, pp. 175-185, 1988.

[13] J. B. Pearson and R. Sridhar, “A discrete optimal control problem,” IEEE Trans. Automat. Contr., vol. 11, pp. 171-174, 1966.

[14] G. Schmidt and W. Neubauer, “High-speed robot path planning in time- varying envronment employing a diffusion equation strategy,” in Proc. Europe Robotics Intel. Syst. Con$ EURISCON ’91, Corfu, Greece, 19 91.

[ 151 A. Steinkogler, “Regelungsstrategien fur mobile Objekte in dynamischer Umgebung,” Diploma Thesis No. 886, Lehrstuhl f i r Steuerungs- und Regelungstechnik, Technische Universitat Munchen, Munich, Germany, 1991.

[16] R. B. Tilove, “Local obstacle avoidance for mobile robots based on the method of artificial potentials,” in Proc. I990 IEEE Int. Con$ Robotics and Automation, Los Alamitos, CA, 1990, pp. 566-571.

[I71 H. Tominaga and B. Bavarian, “Solving the moving obstacle path planning problem using embedded variational methods,” in Proc. 1991 IEEE Int. Con$ on Robotics and Automation, Sacramento, CA, 1991,

[18] C. H. Wu, D. T. Lee, K. Freter, and K. S. Hwang, “An automated collision-avoidance motion planner among moving objects or ma- chines,” in Proc. 30th IEEE Con$ Decis. Contr., Brighton, England, 1991, pp. 2422-2427.

Munich, Germany: Oldenburg-Verlag, 1991.

pp. 450-455.

real-time optimal control of moving vehicles in changing environments

Documents