R&D Review of Toyota CRDL Vol. 40 No. 4

26

Optimum Vehicle Trajectory Control for Obstacle AvoidanceProblem

Yoshikazu Hattori, Eiichi Ono, Shigeyuki Hosoe

ResearchReport

Abstract

In this paper, a new vehicle control algorithmfor avoiding an obstacle within the shortestpossible travel distance is proposed. Thealgorithm consists of two steps. In the first step,the optimal vehicle trajectory and thecorresponding force and moment of the vehicle

are determined using second-order coneprogramming. In the second step, the computedforce and moment are distributed into each tireforce, while using sequential quadraticprogramming with a pseudo-inverse matrix forthe derivation.

Keywords Vehicle dynamics, Predictive control, Optimum control, Collision avoidance,Preview information, Trajectory control

Special Issue Estimation and Control of Vehicle Dynamics for Active Safety

R&D Review of Toyota CRDL Vol. 40 No. 4

1. Introduction

Over the last several decades, there has been anongoing and rapid development in the field ofvehicle dynamics controls. This began with anti-lock brake systems (ABS), traction control systems(TCS), and then continued with the development ofelectronic stability control (ESC) that helps preventside slip of a vehicle. These systems havedramatically enhanced the stability of vehicles.2)

Recently, attention has focused on vehicle dynamicsintegrated management (VDIM). VDIMincorporates a range of functions for vehicledynamics control and actually performs them byusing actuators. The authors have proposed ahierarchical control algorithm (H-VDIM) that iseffective for realizing VDIM by using the steeringand braking systems3) to control each tire force.VDIM enables excellent controllability and stabilitybased on seamless control in a range of situationsfrom ordinary to critical.

In a previously developed version of VDIM, thetarget vehicle dynamics presented as the motion thatthe driver desired were calculated from the driver'soperation of the steering, and brake/acceleratorpedals. With the evolution of technology, however,it has become possible to obtain preview informationsuch as the driving environment by using a camera,radar and/or other sensors, or by using aninformation service provided by the infrastructure.As a result of these changes, the next generation ofVDIM is expected to bring enhancements to safety,ride comfort and energy efficiency, through apredictive control that utilizes this previewinformation.

This report discusses an obstacle avoidanceproblem as a first step toward realizing the next-generation VDIM. A new control algorithm foravoiding an obstacle within the shortest possibledistance is proposed. In this approach, the controlalgorithm is constructed within the framework of H-VDIM, in which the problem is classified into thefollowing two steps. In the first step, the optimaltrajectory and corresponding force and moment ofthe vehicle are determined based on second-ordercone programming. The details of this method aredescribed in Sec. 4. In the second step, the

computed force and moment are optimallydistributed to each tire. An effective algorithm wasdescribed in our previous papers.4, 5) The proposedmethod is confirmed by simulation.

2. Collision avoidance problem

When an active safety system finds an obstacle inthe path of the vehicle, the system either alerts thedriver or takes action to avoid the obstacle. In thesesystems, one of the most important issues involvesdetermining the timing at which the system shouldbecome active. Of course, there are other factorsthat affect the timing, such as the driver'ssensibilities, safety margins, and so on. Although,we can apply an absolute index to indicate whether itis physically possible to determine whether it ispossible to avoid the obstacle. In this paper,therefore, we consider an avoidance problem thatrequires us to determine the distance to an obstaclesuch that, if that distance were any shorter, thevehicle would not be able to avoid the collision.This distance is called the minimum avoidabledistance.

The obstacle avoidance problem is depicted in Fig. 1.The vehicle has an initial speed ν0 , while Ye denotesthe lateral distance that the vehicle has to move toavoid the detected obstacle. This lateral distance is,of course, equal to the lateral size of the obstacle.Note that there are two types of maneuver foravoiding a collision. One is a stopping maneuver,

27

Xes

v0

Fig. 1 Obstacle avoidance problem.

X

Y

ep

e

v0

(a) Stopping maneuver

(b) Passing maneuver

where the brakes are applied to make the vehicle juststop before it reaches the obstacle (Fig. 1(a)). Thesecond is a passing maneuver, where the driversteers the vehicle so that it passes by the obstaclesuch that the lateral speed νye , yaw angle θe , andyaw angle velocity θe are all zero, as shown in Fig. 1(b).Also, Xes and Xep indicate the distances covered bythe vehicle during the stopping and passingmaneuvers. Then, the minimal avoidable distanceXe equals min{Xes , Xep}.

3. Hierarchical vehicle dynamics integratedmanagement algorithm: H-VDIM

The functions that people demand from a VDIMare becoming more and more. As the result, theactuators built into the vehicle are also increasing.Accordingly, the issue of compatibility betweenalgorithms and system configurations is becomingsubstantial.

A hierarchical vehicle dynamics integratedmanagement (H-VDIM) algorithm has beenproposed to satisfy the above requirements (Fig. 2).The H-VDIM algorithm consists of the followinglayers, each connected hierarchically.

[Vehicle Dynamics Control]This layer calculates the desired longitudinal and

lateral forces and yaw moment of the vehicle. Theforces and moment are determined so as to achievethe desired vehicle motion while maintainingstability. The desired motion is estimated by thedriver's pedal inputs and the steering wheel angle.

[Force & Moment Distribution]This layer determines the distribution of each tire

force, so that the total of the tire forces produces the

desired force and moment for the vehicle.[Wheel Control]This layer calculates the target values for each

actuator, such as those for the engine, braking,steering and others. The target values aredetermined so as to generate the desired tire forces.

[Actuator Control]Each actuator system has a corresponding control

unit. The braking system, for example, has actuatedpressure valves to control the braking torque.

The upper layer outputs the target values to thelower layer, while the lower layer feeds back theresults of applying the calculated values. The upperlayer then recalculates the target values dependingon the feedback. This two-way communicationenables each layer to cooperate with the other andmaintains higher robustness against thecharacteristic change of the controlled system andthe variable environment. Adding previewinformation, H-VDIM is enhanced as in Fig. 3. Wecan extend the role of the vehicle dynamics controllayer so that it includes trajectory control based onpredictive control. The actuators used for vehiclecontrol are strongly coupled through body dynamicsthat include nonlinear tire characteristics. Thus, thepredictive control of the trajectories is generally verycomplex and difficult. The proposed H-VDIMapproach helps to separate the control into simplertasks and makes on-line control easier.

4. Trajectory control

4. 1 Formulation of problemThe collision avoidance problem described in

28

R&D Review of Toyota CRDL Vol. 40 No. 4

Driver inputs(Steering, Throttle, Braking)Information

Force and moment of vehicle

Tire forces (Slip ratios)

Driving torque Brake pressure

(Target)

(Result)

(Target)

(Result)

Vehicle dynamics control

Force and moment distribution

Wheel control

Drive train control Brake control Steering control

Steering angle

Fig. 2 Hierarchical vehicle dynamics integratedmanagement algorithm: H-VDIM.

Steering,Pedals

Information

Force and moment of vehicle

Trajectory control

Force and moment distribution

Driver PreviewCamera,Radar

Tire forces (Slip ratios)

Driving Brake pressure

Wheel control

Driver train control Brake control Steering control

Steerin

Tire (

torque a

e

Steering angle

Fig. 3 H-VDIM with preview information.

.

Chapter 2 is first solved as the trajectory control fora rigid body within the framework of H-VDIM (Fig. 4).Where, Fx(t), Fy(t), F(t) are the longitudinal andlateral forces and the resultant force of the rigidbody, and Mz(t) is the moment around the Z-axis.These are constrained by a relationship that consistsof the friction circles of each tire. Further, m and Iz

are the mass and inertia around the Z-axis of therigid body.

In the stopping maneuver, the minimum avoidabledistance is achieved when Fx is minimized, wherethat minimized Fx produces the maximumdeceleration without lateral and rotational motion.In such a situation, the distance Xes that the vehiclemust travel until it stops can be solved relativelyeasily. The calculation is described later. For thepassing maneuver, however, the optimum controland the avoidable distance Xep are described as theproblem for minimizing the performance function Junder the following dynamic equations. Further, theproblem incorporates the equality constraintsexpressed by Eqs. (5)-(7), as well as the inequalityconstraints for the control inputs (Eq. (8)). Notethat, the terminal time of the avoidance is unknown.

[Dynamics equations]

• • • • • • • • • • • • • • • • • • • • • • (1)

x(t) = [νx(t), νy(t), r(t)]T• • • • • • • • • • • • • • • • • • • • • • • • (2)

u(t) = [Fx(t), Fy(t), Mz(t)]T

• • • • • • • • • • • • • • • • • • • • • (3)

[Performance function]

• • • • • • • • • • • • • • • • • • • • • • • (4)

[Constraints] x(0) = [ν0, 0, 0]T

• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (5)

J x t dt v t dtT T

xe e= =∫ ∫0 1 0

( ) ( )

x( ) ( )tm m I

tz

= , ,⎛

⎝⎜

⎞

⎠⎟diag 1 1 1 u

[x2(Τe), x3(Τe)] = [0, 0]• • • • • • • • • • • • • • • • • • • • • • • • • (6)

• • • • • • • • • • • • • • • • • • • (7)

C(u(t))< 0• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (8)Several sequential approximate solutions have

been proposed to solve the above problem as a two-point boundary value problem, but these involve alarge amount of calculation. Accordingly, we solvethis problem by using a two-step optimizationapproach. First, the terminal time Τe is assumed toalready be known as Τ 'e, such that the problem canbe converted to a discrete time, and that theoptimum solutions X'e, u'opt can be solved. Thesecond step is to find the terminal time Τe

minimizing X'e . The former problem can beformulated as a multi-dimensional optimizationproblem using convex programming and the latterbecomes a simple nonlinear optimization with onlyone unknown variable Τe . With this separation ofsolving process, we can obtain the solution withcomparative ease.

4. 2 Constraint of force and momentIt is well-known that the longitudinal and lateral

forces generated between the tires and the roadsurface are constrained within a circle called the"friction circle." Here, we consider the range ofFx(t), Fy(t), Mz(t) as the resultant force of each tire.Using the force and moment distribution algorithmshown in Chapter 5, the range of Fx(t), Fy(t), Mz(t)can be determined. The calculated result is shown inFig. 5.

From the figure, we can see that the range of force

0 2 0 3 0T T

ee ex t dt x t dt Y∫ ∫,⎡

⎣⎢⎤⎦⎥= ,[ ]( ) ( )

29

R&D Review of Toyota CRDL Vol. 40 No. 4

Trajectory

Obstacle

Ye

F(t) F (t)y

F (t)x

I zm

Xe

M (t)z

v0

Fig. 4 Collision avoidance problem for a rigid body.Fig. 5 Constraint on resultant force and moment of the

vehicle.

and moment is a convex set and it is approximatelyrepresented as

C(u(t)) = Fy2(t) + Fy

2(t) + Wr2(t)Mz

2(t) - F 2(t)<0• • • • • • • • • • • • • • • • • • • • • • • • (9)

where Wr is a constant, and F(t) the maximum forceof the vehicle at time t. In this paper, we assumethat F(t) is a constant during the control interval, i.e.

F(t) = F for all t • • • • • • • • • • • • • • • • • • • • • • • • (10)4. 3 Second-order cone programming problemIf a set satisfies for and

, then it is called a cone. A second-ordercone in the N-dimensional space RN is defined asfollows.

• • • • • • • • • • • • • • • • • • (11)

For a tuple of p vectors , the set defined by

• • • • • • • • • • • • • • • • • • (12)

is also called a second-order cone.A second-order cone program is an optimizing

problem involving the minimizing of the linearperformance function under the affine and second-order cone constraints over optimizing variables.The second-order cone programming problem is aspecial case of a semi-definite programmingproblem. Consequently, a primal-dual interior-pointmethod, which is a powerful numerical algorithm fora semi-definite programming problem, can beapplied.6, 7)

Now, we will reformulate the problem given byEqs. (1)-(8) as a second-order cone programmingproblem. For this, let us assume that terminal timeΤ 'e is already known and let us discretize theproblem by sampling period ∆t. The samplingperiod should be small enough so that we canassume that the control inputs u(t) are constantduring each sampling period. By taking anappropriate integer L, the control interval Τ 'e can beexpressed as Τ 'e = L∆t. • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (13)

Next, let us define discrete values {Fx(k), Fy(k),Mz(k), F(k), vx (k), vy (k), x (k), y (k), r (k), θ (k),(k = 0, • • • , L)}, where Fx(k), Fy(k) are the longitudinaland lateral forces, Mz(k) the rotational moment, F(k)

B Bdef

= ∈ = , ,( ){ }i iN i px ( ) 1

B

x x x x= , , ,( ),1 2 p

B ( )N x xN

j

N

j

def

= ∑∈ ≥=

⎧

⎨⎪⎪

⎩⎪⎪

⎫

⎬⎪⎪

⎭⎪⎪

x R 12

2

∀ >λ 0

∀ ∈x Kλx∈KK ⊆ Rn

the maximum force, vx(k), vy(k), the X and Ydirectional speeds, x(k), y(k) the positions in the Xand Y directions, and r(k), θ(k) the yaw anglevelocity and the yaw angle, each at the k-th samplinginterval.

The optimizing variables X are defined by thefollowing equations.

X(k) = [F(k), Fx(k), Fy(k), Wr Mz(k)] • • • • • • • • • • (14)X = [X(0), • • • , X(k), • • • , X(L - 1)]T

• • • • • • • • • • • (15)As shown in Eq. (9), X is an element of the

second-order cone ( ). Further, theperformance function J' is described by equation

J' = x(L). • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (16)The relationships between x(k), vx(k) and Fx(k) areexpressed by the equations

x(k) = x(k - 1) + ∆t vx(k - 1)( k = 1, 2, • • • , L) • • • • • • • • • • • • • (17)

• • • • • • • (18)

Similarly, vy, y, r, θ satisfy some linear relations ofX. In addition, at the terminal sampling time L, Xshould fulfill the following affine constraints.

vx(0) = v0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (19)vy(0) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (20)x(0) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (21)y(0) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (22)r(0) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (23)θ (0) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (24)vy(L) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (25)y(L) = Ye • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (26)r(L) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (27)θ (L) = 0 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (28)uF (k) = F (k = 0, 1, • • • , L - 1)• • • • • • • • • • • • • • • (29)Hence, the collision avoidance problem has been

formulated as an optimizing problem that has thelinear performance function and constraints forvariables in the second-order cone space. Itcan be solved efficiently by applying the second-order cone programming method.

4. 4 Search for terminal timeFirst, we consider the stopping maneuver. If the

maximum decelerating force for ideal straightbraking is F, the distance Xes needed for the vehicleto stop assuming an initial speed v0 and thecorresponding maneuvering time Tes are describedby the equations.

X ∈B

v k vm

F i k Lx xi

kt

x( ) ( ) ( ) ( )= + ∆ = , , , .=

−

∑0 1 20

1

+ ∆ −12

12

mF kt x ( )

X ∈B

30

R&D Review of Toyota CRDL Vol. 40 No. 4

−

−

iNR i p

i

x ∈ = , ,( )1

• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (30)

• • • • • • • • • • • • • • • • • • • (31)

Next, we consider the passing maneuver. Tep is thetime needed for the passing maneuver to avoid theobstacle. In searching for Tep that gives theminimum avoidable distance, we should note thatwe can restrict the range of the search to

Tep < Tes. • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (32)This is because, in the stopping maneuver, fullbraking in the longitudinal direction always givesthe slowest velocity in the X-direction and therefore,if Tep > Tes , then the stopping maneuver obviouslygives a shorter avoidable distance.

On the other hand, Tep should be greater than orequal to the minimum time Tmin needed for thevehicle to move only in the lateral direction to theposition y = Ye and stop there. Tmin is attained if thevehicle is maximally accelerated up to y = Ye /2 andmaximally decelerated until y = Ye . With theelementary calculation, Tmin is given by

. • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • (33)

Thus, if the shortest avoidance is achieved by thepassing maneuver, Tep should lie within the range

. • • • • • • • • • • • • • • • • • • • • • • • • • • • (34)

Accordingly, in the search for the terminal time, theexisting area of the solution is limited, and theoptimal solution can be found with comparative easeby using a general line-search method.

5. Simulation

The results of collision avoidance using the abovealgorithm were confirmed bysimulation. For the simulation, thevehicle was assumed to be a rigidbody. In this problem, however, theinitial and terminal conditions of theyaw angle and yaw angle velocity areall 0. Therefore, the shortestavoidance is achieved, obviously,when all of the tire forces are used fortranslational motion without yawmotion as . Consequently, it issufficient to assume the vehicle to be

Mz≡ 0

2 0mF

Y T mvFe ep≤ ≤

T mF

Ymin e= 2

X v T Fm

T mF

ves es es= − =02

021

212

T v mvFes F

m

= ( ) =0 0 a mass point. Then, the following results of the

trajectory control are solved for the mass point. Theminimum avoidable distance for the initial speed isshown in Fig. 6, where Ye = 3 [m]. At an initialspeed in excess of 18.6 [m/s], the passing maneuverproduces the minimum avoidable distance. Then,the minimum avoidable distance approaches theresult for pure side movement asymptotically.

The target trajectory calculated by the trajectorycontrol and the result achieved by the force andmoment distribution are shown in Fig. 7, for aninitial speed of 20 [m/s]. The arrows indicate theresultant force for each tire force produced by thedistribution control. The distribution controlconsiders the movement of the weight and thenonlinear characteristics of each tire for the weight.Therefore, the target force and moment are notachieved completely. Accordingly, there is sometracking error between the target and the resulttrajectory. When the data for a general passengercar is used, the standard deviation of the force erroris 4.3 [%], the results of the terminal conditions are

31

R&D Review of Toyota CRDL Vol. 40 No. 4

10 20 30 400

10

20

30

40

50

60

Initial speed [m/s]

Min

imum

avo

idab

le d

ista

nce

[m]

Pure sidemovement

Stopping

PassingGood

Fig. 6 Minimum avoidable distance.

Proceeding distance [m]

0 161412108642

1

18

3

2

0

Target trajectoryResult trajectoryResult resultant force

Lat

eral

dis

tanc

e [m

]

Fig. 7 Control result.

Ye = 2.88 [m] and vy(L) = 0.049 [m/s]. All are lessthan 5 [%]. The difference in the avoidable distanceis 0.2 [m] which is almost 1 [%].

6. Conclusion

As a first step toward developing a next-generationVDIM with preview information, a new controlalgorithm for an obstacle avoidance problem wasproposed. Based on the framework of H-VDIM, theproblem is separated into two subsystems that can besolved comparatively easily. The vehicle trajectoryerrors between theoretical optimum solutions andcontrol results by using the proposed algorithm areacceptably small.

References

1) Inagaki, S., et al. : "Analysis on Vehicle Stability inCritical Cornering Using Phase-Plane Method,"AVEC'94, No. 50, (1994), 287-292

2) van Zanten, A. T. : "Control Aspects of the BOSCH-VDC," AVEC'96, (1996), 573-608

3) Hattori, Y., Koibuchi, K. and Yokoyama, T. : "Forceand Moment Control with Nonlinear OptimumDistribution for Vehicle Dynamics," AVEC2002,No.20024577, (2002), 595-600

4) Ono, E., Hattori, Y. and Muragishi, Y. : "Estimationof Tire Friction Circle and Vehicle DynamicsIntegrated Control for Four-wheel DistributedSteering and Four-wheel DistributedTraction/Braking Systems", R&D Rev. of ToyotaCRDL, 40-4(2005), 7

5) Ono, E., Hattori, Y., Muragishi, Y. and Koibuchi, K. :"Vehicle Dynamics Control Based on Tire GripMargin,"AVEC 2004, (2004), 531-536

6) Wolkoxicz, H., Saigal, R. and Vandenberghe, L. :Handbook of Semidefinite Programming Theory,Algorithms, and Applications, (2000), 654, Kluwer'sAcademic Publishers

7) Tamura, A. and Muramatsu, M. : OptimizationMethods (in Japanese), (2002), 233, KyoritsuShuppan

(Report received on Sep. 30, 2005)

32

R&D Review of Toyota CRDL Vol. 40 No. 4

Yoshikazu HattoriResearch fields : Vehicle Dynamics

Control, Vehicle DynamicsAnalysis & Modeling

Academic society : Soc. Instrum. ControlEng., Inst. Syst., Control Inform.Eng., Soc. Automot. Eng. Jpn.

Eiichi OnoResearch fields : Vehicle Dynamics

ControlAcademic degree : Dr. Eng.Academic society : Soc. Instrum. Control

Eng., Soc. Automot. Eng. Jpn.Awards : SICE Award for Outstanding

Paper, 1995SICE Chubu Chapter Award forOutstanding Research, 1999SICE Chubu Chapter Award forOutstanding Technology, 2002IFAC Congress Applications PaperPrize, 2002Paper Award of AVEC, 2002Paper Award of AVEC, 2004

Shigeyuki Hosoe*Research fields : Control Theory, Bio-

Mimetic ControlAcademic degree : Dr. Eng.Academic society : IEEE, Soc. Instrum.

Control Eng., Inst. Syst., ControlInform. Eng., IEE of Jpn., Soc.Automot. Eng. Jpn.

Awards : Annual Paper Award fromSICE, 1984, 1990

* RIKEN, Bio-Mimetic Control Research Center