steering threeinput nonholonomic systemscats-fs.rpi.edu/~wenj/ecse641s07/berkeley_firetruck.pdf ·...

23

Upload: others

Post on 11-Mar-2020

2 views

Category:

Documents


0 download

TRANSCRIPT

Steering Three�Input Nonholonomic Systems�

The Fire Truck Example �

L� G� Bushnell� D� M� Tilbury and S� S� Sastry

Electronics Research Laboratory

Department of Electrical Engineering and Computer Sciences

University of California at Berkeley

Berkeley� CA �����

August �� ����

Abstract

In this paper� we steer wheeled nonholonomic systems that can be representedin a so�called chained form� Su�cient conditions for converting a multiple�inputsystem with nonholonomic velocity constraints into a multiple�chain� single�generator chained form via state feedback and a coordinate transformation arepresented along with sinusoidal and polynomial control algorithms to steer suchsystems� Our example is the three�input nonholonomic system of a �re truck�or tiller truck� In this three�axle system� the control inputs are the steeringvelocities of both the �rst and third �or tiller� axles and the forward drivingvelocity of the truck� Simulation results are given for parallel parking� left handturning� right hand turning� and changing lanes� Comparison is made to thesame vehicle without tiller steering�

Keywords� chained form� nonholonomic motion planning� controllability�

� Introduction

Chained form systems were �rst introduced in �Murray and Sastry ���� as a class of systemsinspired from �Brockett ���� to which one could convert a number of interesting examples�including a car and a car with one trailer� and for which it was easy to derive steeringcontrol laws� These examples had two inputs and their chained forms had one chain� Inthis paper� we convert systems with more than two inputs into a chained form with morethan one chain� The method we present for transforming a drift�free� nonholonomic systeminto chained form is analogous to the method for exact linearization of a nonlinear systempresented in �Isidori �����

A method for steering chained form systems with sinusoids was �rst stated in �Murrayand Sastry ������ Other practical and e�cient methods for steering include polynomialcontrols �Tilbury� Murray� and Sastry ���� and piecewise constant controls �Monaco and

�Supported in part by NSF�IRI �������� DMT would like to acknowledge an AT�T Ph�D� Fellowshipfor partial support� Please send all email correspondences to bushnell�eecs�berkeley�edu�

Normand�Cyrot ������ In this paper� we will look at using sinusoidal and polynomial controlinputs for steering the �re truck system� a nonholonomic system with three inputs�

Our investigation of the �re truck system is fundamental to understanding multiple�steering� multiple�trailer nonholonomic systems� which may be used in practice in man�ufacturing plants� nuclear power plants� or any area unsafe for human operators� Thecontributions of this paper are the introduction of a three�input system as an example of anonholonomic system that can be steered in chained form� steering algorithms for multiple�input chained form systems� su�cient conditions for converting a nonholonomic systemwith three or more inputs into a chained form system� and simulations that show how extrasteering wheels can result in greater maneuverability�

We are interested in steering mechanical systems with nonholonomic� or non�integrable�constraints on their velocities� These constraints can be written as

�i�x� x � � i � �� �� � � � � k �

where x � Rn is the state of the system and �i�x� � Rn� i � �� � � � � k are row vectors� Suchconstraints arise� for example� when a wheel rolls without slipping� If these constraints areintegrable� they are called holonomic constraints and yield level surfaces hi�x� � ci for someconstant ci to which the trajectory of the system is restricted� If these constraints are notintegrable� they are called nonholonomic constraints�

We assume that the �i� i � �� � � � � k are linearly independent and smooth� The cor�responding codistribution ��x� � spanf���x�� � � � � �k�x�g has dimension k� Therefore�we can �nd an �n � k��dimensional distribution ��x� � spanfg��x�� � � � � gn�k�x�g� withgi�x� � Rn� i � �� � � � � n�k such that � � ��� i�e�� �i�x� � gj�x� � �� for all �i � �� gj � ��Then a mechanical system with the above nonholonomic linear velocity constraints can berepresented as a control system with inputs ui as follows�

x � g��x�u��t� � � � �� gn�k�x�un�k�t� �

Our motion planning problem therefore consists of controlling the drift�free system

x�t� � g��x�u��t� � � � �� gm�x�um�t�

where x � open set U � Rn� ui�t� � R for i � �� � � � � m� m � n� and the gi are smooth

linearly independent vector �elds� All subsequent conditions are assumed to hold on theopen set U � Given x� and xf � we wish to �nd a control law u � �u��t�� � � � � um�t�� to steerx��� � x� to x�T � � xf on the time interval ��� T ��

Recent work �Murray and Sastry ���� in the area of controlling nonholonomic systemsby using sinusoidal inputs has concentrated on systems with two inputs� If the system meetscertain su�cient conditions allowing it to be transformed into what we call a single�chain�single�generator chained form

z� � v� z� � v�

z� � z�v����

zn � zn��v�

by a change of coordinates and state feedback� then the system may be steered by settingthe inputs v� and v� to be sinusoids at integrally related frequencies� We call this a chained

form system because the derivative of each state depends on the state directly above it ina chained fashion� This particular chained form is reminiscent of Brunovsky normal form�Indeed� with the input v� set to �� the coordinates z�� � � � � zn are in Brunovsky canonicalform� We note� however� that chained form systems are nonlinear� drift�free� and bilinear inthe input and state variables� The input v� that appears in the chain is called the generatinginput�

In this paper� we extend the above idea to systems with three or more inputs� The planis to transform a three�input system with nonholonomic velocity constraints into a two�chain� single�generator chained form by state feedback and a coordinate transformation� Inthis special form� it can be controlled using sinusoidal or polynomial input functions� Thisidea is also extended to the general case of m � � inputs in the appendix�

The outline of this paper is as follows� In Section �� our three�input example of anonholonomic system is introduced� The kinematic equations are derived and representedas a control system� This example� the �re truck� will be used throughout the paper toillustrate the analytical results� In Section � we give su�cient conditions for transforminga three�input nonholonomic system into a two�chain� single�generator chained form andshow how the example can be transformed into this special form� In Section �� we presenta step�by�step sinusoidal algorithm� an all�at�once sinusoidal algorithm� and a polynomialalgorithm to control these systems� In Section �� simulation results are given for the �retruck example� In Section �� conclusions are drawn�

� The Fire Truck

Fire trucks are used to carry aerial ladders� tools and equipment and have the main purposeof rescue and ventilation� They are mainly used by �re departments in large cities in theUnited States and have great maneuverability through the narrow city streets due to theextra steering on the third axle� or tiller�

The �re truck is an example of a three�input nonholonomic system� It is mathematicallymodeled as two planar rigid bodies supported by three axles� The support of the rear body�or trailer� is over the center of the rear axle of the front body� or cab �axle�to�axle hitching��The �rst and third axles are allowed to pivot� while the middle axle is rigidly �xed to the cabbody� The wheels are assumed to roll but not slip� thus giving linear velocity constraints�

The derivation of the kinematic equations for the �re truck refers to Figure �� where weemphasize the truck�s two rigid bodies� The states of the mathematical model� all functionsof time� are chosen as follows� �x�� y�� is the Cartesian location of the center of the rear axleof the cab� �� is the steering angle of the front wheels with respect to the cab body� and ��is the orientation of the cab body with respect to the horizontal axis of the inertial frame�The states �x�� y�� ��� ��� are described similarly for the trailer� except that �� is the angleof the rear wheels with respect to the trailer body�

Let the distance between the front and rear axles of the cab be L�� and the distancebetween the centers of the rear axles of the cab and trailer be L�� This gives the holonomicconstraints

x� � x� � L� cos �� y� � y� � L� sin ��

x� � x� � L� cos �� y� � y� � L� sin �� �

The six coordinates x �� �x�� y�� ��� ��� ��� ��� are su�cient to represent the positions of thecab� trailer and wheels�

θ1

φ1

φ2 θ

2

L1

L0

22(x , y )

0 0(x , y )

1 1(x , y )

Figure �� The con�guration of the �re truck�

For mechanical systems with wheels rolling and turning on a surface� the non�slippingconstraint states that the velocity of a body in the direction perpendicular to each wheelmust be zero� In terms of coordinates� for a wheel centered at location �x� y� and at anangle � with respect to the horizontal axis of the �xed frame� the constraint is

� � vx sin�� vy cos� �

In order to simplify our kinematic model of the �re truck� each pair of wheels is modeledas a single wheel centered at the midpoint of the axle�� Requiring that the wheels do notslip gives the three linear velocity constraints

� � x� sin��� � ���� y� cos��� � ���� ��L� cos��

� � x� sin �� � y� cos ��

� � x� sin��� � ���� y� cos��� � ��� � ��L� cos�� �

These constraints can be expressed more compactly as �i�x� � x � �� where the �i�x� arerow vector �elds in R

��

���x� � � sin��� � ��� � cos��� � ��� � �L� cos�� � � ����x� � � sin �� � cos �� � � � � ����x� � � sin��� � ��� � cos��� � ��� � � � L� cos�� � �

The corresponding codistribution is ��x� � spanf���x�� ���x�� ���x�g� Since � hasdimension three and the state space is of dimension six� we can �nd a three�dimensionaldistribution ��x� �� spanfg��x�� g��x�� g��x�g such that �i�x� � gj�x� � �� for all �i �

�It may be shown �Alexander and Maddocks �� that in fact the two wheels have di�erent angles andtheir normals all intersect at a single point� If� for example� we let ��� and ��� be the angles of the front wheelsof the cab� we can derive a holonomic constraint that can be used to eliminate one of these two variables�Thus� we only need keep track of one of the wheel angles�

�� gj � �� A simple calculation shows that the following vector �elds form a basis for ��

g� �

�BBBBBBBB�

cos ��sin ��

��L�

tan���

� �L�

sec�� sin��� � �� � ���

�CCCCCCCCA

g� �

�BBBBBBB�

������

�CCCCCCCA

g� �

�BBBBBBB�

������

�CCCCCCCA

� ���

The nonholonomic constraints �i�x� � x � �� for all �i � � are equivalent to x � �� i�e�� x is a linear combination of vector �elds in �� Therefore the kinematic model of the �retruck as a control system with three inputs is written as

x � g��x�u� � g��x�u� � g��x�u� � ���

We have chosen the basis fg�� g�� g�g for � so that the input u� corresponds to the forwarddriving velocity of the truck� u� corresponds to the steering velocity of the front wheels ofthe cab and u� corresponds to the steering velocity of the rear wheels of the trailer�

� Converting to Two�Chain� Single�Generator Chained Form

The foregoing kinematic equation for the �re truck becomes complicated when we investigatehow to �nd control inputs fu�� u�� u�g that will steer the state x � R� from an initial pointto a desired point� Chained form systems are constructed in such a way that they are easilysteered with sinusoidal� polynomial� or piecewise constant inputs�

Recall that the Lie bracket of vector �elds f and g is de�ned as

�f� g��x� ���g

�xf�x��

�f

�xg�x�

adkfg�x� �� �f�x�� adk��f g�x��

where ad�fg�x� �� g�x�� The Lie bracket is invariant under coordinate transformations� Adistribution � is said to be involutive if f� g � � implies �f� g� � ��

Deriving conditions to transform a nonholonomic system with two or more inputs intochained form is not di�cult if we consider the method for exact linearization of a nonlinearsystem with drift via state feedback and a coordinate transformation as presented in Chapter� of �Isidori ����� In analogy to this method� we state and prove the following su�cientconditions for transforming a three�input� drift�free� nonholonomic system to chained form�The appendix extends this to an �m � ���input system�

Proposition � �Converting to Two�Chain� Single�Generator Chained Form�Consider a three�input� drift�free� nonholonomic system

x � g��x�u� � g��x�u� � g��x�u� ��

with smooth� linearly independent input vector �elds g�� g�� g�� There exists a feedback trans�formation on some open set U � R

n

��� �� � � ��x�

u � �x�v

to transform the system ��� into two�chain� single�generator chained form

�� � v� �� � v� � � v� �� � ��v� � � �v�

������

��� n� � n���v� �n� � �n���v�

���

if there exists a basis f�� f�� f� for �� �� spanfg�� g�� g�g which has the form

f� ��

�x��

nXi��

f i��x��

�xi

f� �nXi��

f i��x��

�xi

f� �nXi��

f i��x��

�xi���

such that the distributions

G� �� spanff�� f�g

G� �� spanff�� f�� adf�f�� adf�f�g

���

Gn�� �� spanfad if�f�� ad i

f�f� � � � i � n � �g ���

have constant dimension on U � are all involutive� and Gn�� has dimension n� � on U �

The proof is in the appendix�

Remark �� The condition that G�� � � � � Gn�� all be involutive is somewhat redundant� asin the exact linearization conditions in �Isidori ���� Section ������ since the involutivity ofsome distributions in the sequence may imply the involutivity of others�

Remark �� Recall that a system of the form x �Pm

i�� fi�x�ui is completely controllableif the involutive closure of the distribution �� � spanff�� � � � � fmg at each con�gurationis equal to the entire state space Rn �see Chow�s Theorem �Hermann and Krener �������For the case of three inputs� the system is completely controllable since the involutiveclosure of �� � spanff�� f�� f�g is spanned by the vector �elds f�� f�� adf�f�� � � � � adn�f� f��f�� adf�f�� � � � � adn�f� f�� which span R

n� Thus� the system in two�chain� single�generatorchained form ��� is completely controllable� since controllability is una�ected by state feed�back and coordinate transformation� This argument easily extends to the case of m � �inputs�

Example� Proposition � is now used to �nd the chained form equations for the �re trucksystem� The vector �elds f�� f�� f� which satisfy the conditions of the proposition are

f� �

�BBBBBBBB�

�tan ��

��L�

sec �� tan���

� �L�

sec �� sec�� sin��� � �� � ���

�CCCCCCCCA� f� �

�BBBBBBB�

������

�CCCCCCCA� f� �

�BBBBBBB�

������

�CCCCCCCA

where f� � sec ��g� with g� as in equation ���� These can be considered as the original inputvector �elds after the input transformation �u� � u� cos ��� In this representation three ofthe states are controlled directly� x�� ��� ��� so their velocities are the inputs�

The distributions Gi in equation ��� are now constructed� and their involutivity ischecked� By way of notation� de�ne f� �� adf�f�� f� �� adf�f� and f� �� ad�f�f�� Re�call that x � �x�� y�� ��� ��� ��� ����

G� � spanff�� f�g

G� � spanff�� adf�f�� f�� adf�f�g

G� � spanff�� adf�f�� ad�f�f�� f�� adf�f�� ad�f�f�g

� spanff�� adf�f�� ad�f�f�� f�� adf�f�g � spanff�� f�� f�� f�� f�g

� span

�������������������

�BBBBBBB�

������

�CCCCCCCA�

�BBBBBBB�

���

��L�

sec� �� sec ����

�CCCCCCCA�

�BBBBBBBB�

��L�

sec� �� sec� �����

�L�L�

cos��� � ��� sec� �� sec�� sec� ��

�CCCCCCCCA�

�BBBBBBB�

������

�CCCCCCCA�

�BBBBBBB�

�����

�L�

cos��� � ��� sec� �� sec ��

�CCCCCCCA

����������������

The distribution G� has dimension� n�� � � on U � fx � ������ ��� ��� �� �� ��� g � R

��It may also be veri�ed that G�� G� and G� are involutive on U and the functions h� � x��h� � y� and h� � �� satisfy equation ����� Note that there is a lack of uniqueness in the h

functions�

Remark �� In the open set U � R�� the �re truck is not in a jack�knife con�guration� that

is� the wheels are less than ��� relative to their respective bodies� and the cab�s body angleis not �����

The coordinate transformation ��� �� � � ��x� from equation ���� is computed as fol�lows�

�� � h� � x�

�� � L�f�h� �

L�tan�� sec� ��

�� � Lf�h� � tan ��

�� � h� � y�

�This can be easily checked by showing that the ve vector elds that de ne G� along with f� are linearlyindependent� i�e��

det�f� f� f� f� f� f�� ���

L�

�L�

cos��� � �� sec��� sec

��� sec

��� �

� � Lf�h� � ��

L�sin��� � �� � ��� sec�� sec ��

� � h� � �� �

This is a valid coordinate transformation since the matrix ���x

is nonsingular�The derivatives with respect to time give the following equations� showing the re�

quired state feedback to transform the system equations into the two�chain� single�generatorchained form

�� � �u� � v�

�� �

L��

tan� �� tan �� sec� �� �u� ��

L�sec� �� sec� �� u� � v�

�� ��

L�tan�� sec� �� �u� � ��v�

�� � tan �� �u� � ��v�

� ��

L�L�cos��� � ��� tan�� sec�� sec� �� �u�

��

L��

cos��� � �� � ��� sin��� � �� � ��� sec� �� sec� �� �u�

��

L�cos��� � ��� sec� �� sec �� u� � v�

� � ��

L�sin��� � �� � ��� sec�� sec �� �u� � �v� �

Remark � Using the �re truck system as a reference� we can infer characteristics ofchained form systems� The generating input� v� is related to the driving velocity of thesystem by u� � sec ��v�� For the �re truck� the �rst chain corresponds to the �rst twoaxles �one steerable� one passive� and the second chain corresponds to the third steerableaxle� In general multiple�steering� multiple�trailer systems� the single�generator chain formcan be applied with the number of chains corresponding to the number of steerable axlesin the system� The paper �Tilbury et al� ����� shows that such a composite system can beconverted into multiple�input chained form� but that in general� dynamic state feedback isneeded to achieve the transformation�

When the attachment point between the cab and the trailer is not located at the center ofthe rear axle of the cab� but at some distance o� of the axle �kingpin hitch�� as is commonlythe case for cars pulling trailers� the above procedure applies with no modi�cation� Thekinematic equations and the h functions needed for transforming this system to chainedform are derived as follows�

Refer to Figure � for the system states and parameters and let x �� �x�� y�� ��� ��� ��� ����The three row vectors representing the non�slipping constraints have the form�

���x� � � sin��� � ��� � cos��� � ��� � �L� cos�� � � ����x� � � sin �� � cos �� � � � � ����x� � � sin��� � ��� � cos��� � ��� � L� cos��� � �� � ��� � L� cos�� � �

In the same way as before� we can construct a control system with vector �elds g�� g�� g�which are orthogonal to ��x� � spanf���x�� ���x�� ���x�g� We choose these vector �eldssuch that the inputs have the interpretation that u� is the driving velocity of the rear wheels

φ2 θ

2

L122(x , y )

θ1

φ1

L0

0 0(x , y )

1 1(x , y )

L2

Figure �� The �re truck with o��axle hitching�

of the cab� u� is the steering velocity of the front axle� and u� is the steering velocity of thetiller� The control system is then written as

x � g��x�u� � g��x�u� � g��x�u� � ���

with g� � ��� �� �� �� �� ���� g� � ��� �� �� �� �� ��� and

g� �

�BBBBBBBB�

cos ��sin ��

��L�

tan���

�L�L�

sec����L� sin��� � �� � ���� L� cos��� � �� � ��� tan���

�CCCCCCCCA

It is straightforward to check that equation ��� reduces to the original system in equation ���when L� � ��

The chained form coordinates for this system are found in an analogous manner to thosefor the original �re truck system using

f� � sec ��g� f� � g� f� � g�h� � x� h� � y� h� � �� �

Note that these are the same h functions we used for the �re truck system with axle�to�axlehitching�

� Steering Two�Chain� Single�Generator Chained Form Sys�

tems

In this section� we present three algorithms for steering a system in the two�chain� single�generator chained form of equation ���� The �rst two algorithms use sinusoidal controlinputs� one steers one level in the chains at a time in a step�by�step fashion� and theother steers all levels in the chains at once� The third algorithm uses polynomial inputs�

Other methods for steering two�chain� single�generator chained form systems exist� suchas the method using piecewise constant inputs described in �Tilbury and Chelouah ����for the �re truck example� The method of steering nonholonomic systems using piecewiseconstant inputs was �rst introduced in �Monaco and Normand�Cyrot ����� as multiratedigital control�

We have found that although all of these steering algorithms will �nd a path betweenany two con�gurations� the resulting trajectories look �nicer� for some methods than forothers� Therefore choosing a method will depend on the type of trajectory desired� Thestep�by�step sinusoidal method is not recommended in practice� but only included here toemphasize the chained form structure� The all�at�once sinusoidal method works well fortrajectories that have a reversal� such as parallel parking� The polynomial and piecewiseconstant methods work well for trajectories without back�ups�

Recall that we want to solve the following control problem� given ���� ��� �� and��f � �f � f� �nd a control law v � �v��t�� v��t�� v��t�� to steer ������ ����� ���� � ���� ��� ��to ���T �� ��T �� �T �� � ��f � �f � f� in the given time interval ��� T ��

The step�by�step sinusoidal algorithm is an extension of the algorithm for two�inputsystems in �Murray and Sastry ����� The algorithm exploits the decoupling of the twochains� allowing for simultaneous steering� The main idea� considering for a moment only the� chain� is that if v� � sin �t and v� � cos ��t� then �� will have a frequency componentat ��� �� will have a frequency component at �� � ���� � � �� and �� will have a frequencycomponent at zero� By simple integration over one period� this yields net movement in ��while ��� � � � � ���� return to their previous values� Thus� at the �th step in the algorithm�the states at the �th level in the chain are driven to their �nal positions�

For the time interval ��� T �� the tops of the chains are steered with constant inputs

v� ��

T��f� � ���� v� �

T��f� � ���� v� �

T�f� � ��� �

driving ��� �� and � to their �nal positions� For level � � �� � � � � n�� n� � �� � � � � n� in thechains� the inputs are set as

v� � sin �t v� � cos ��t v� � � cos ��t

over the time interval ��T� ��� ��T �� where � � ��T

and � and � are chosen such that

�f� � ����T � � �

������ T f� � ���T � �

��

������ T �

This causes the states �� and � to reach their �nal values and the �rst � coordinates ineach chain� namely ��� ��� � � � � ����� �� � � � � ���� to return to their �nal values� When one ofthe chains is shorter than the other� that chain�s input can be set to zero when there areno more states to steer�

Although this method works well� it can be tedious in practice because of the manysteps that are needed� The trajectories that are generated consist of many segments anddo not always follow a direct path between the start and goal con�gurations�

In �Tilbury� Murray� and Sastry ���� an �all�at�once� sinusoidal method was proposedfor the two�input case� This method was extended in �Tilbury et al� ����� for multiple�input systems and will be summarized here for the three�input case� In this method� onlyone step is used with all of the necessary frequencies set in the inputs as follows�

v� � a� � a� sin �t

��

v� � b� � b� cos�t � � � �� bn� cosn��t

v� � c� � c� cos�t � � � �� cn� cosn��t � ��

Given initial and �nal states such that j���� ��� ��� ��f � �f � f�j � � for a small �� the localexistence of these parameters was proven in �Tilbury� Murray� and Sastry ���� for single�chain systems and is easily extendible to multiple�chain systems� The basic idea of theproof is to de�ne a local di�eomorphism� � � Rn �� R

n� which maps the input parameters�a�� b�� � � � � bn�� c�� � � � � cn�� except a� to ��f � �f � f�� where ��a�� b�� � � � � bn� � c�� � � � � cn�� isthe value of ���T �� ��T �� �T�� when the chained form system ��� is integrated over ��� T �starting at the given initial state ���� ��� �� with the inputs ��� The input parameters arethen found by selecting a value for a� and taking the inverse transformation �����f � �f � f��In practice� the �nal state is not within an � ball around the initial state� In this case� weconnect the two given states by a �nite number of � balls� and apply the above methodwithin each ball� It is not clear how to apply this method when the transformation tochained form has singularities� since in the original coordinates the parameter � may be afunction of x�

The third steering method uses polynomial inputs and was �rst presented in �Tilbury�Murray� and Sastry ���� for two�input system and in �Tilbury et al� ����� for the multiple�input case� For three�input systems� we set the �rst input� v�� to be constant over the entiretrajectory and the other inputs to be Taylor polynomials

v� � �v� � b� � b�t � � � �� bn� t

n�

v� � c� � c�t � � � �� cn�tn� �

���

with the number of parameters on each input chosen to be equal to the number of statesin its chain� The time needed to steer the system is determined from the change in the ��coordinate� T � �f� � ��� � Integrating the chained form equations with inputs as in ��� andinitial condition ������ ����� ���� � ���� ��� ��� and evaluating at time T � the parametersb�� � � � � bn� � c�� � � � � cn� can be found in terms of the initial and �nal states from setting���T �� ��T �� �T�� � ��f � �f � f�� This equation is easily solved for the parameters bi� ci byinverting a matrix which is linear in the parameters�

For trajectories that require �f� � ��� � as in the parallel parking maneuver� the polynomialmethod will not work� An easy way to remedy this situation is to pick an intermediate point�with �� not equal to the given ���� �f� � and then plan the path in two pieces�

� Simulation Results

The simulation of the �re truck system is performed on the transformed system in thechained form

�� � v� �� � v� � � v� �� � ��v� � � �v� �� � ��v� �

����

The transformed states are steered from an initial point to a �nal point by using thetransformed inputs as constructed in the step�by�step sinusoidal steering algorithm� theall�at�once sinusoidal steering algorithm� or the polynomial steering algorithm� Then theinverse coordinate transformation

x� � ��

��

Figure � The parallel parking trajectory using sinusoidal inputs� The �re truck starts aty� � � and ends at zero� with all other coordinates starting and ending at zero�

y� � ��

�� � tan���L� �� cos��tan�� ����

�� � tan�� ��

�� � � � tan�� �� � tan���L� � cos�tan�� ��� sec�� � tan�� ����

�� � � ����

with L� � � and L� � � is calculated to extract the trajectory of the �re truck in the originalcoordinates� The results are presented for the parallel�parking maneuver using sinusoidalinputs� To see the advantage of having the extra steering wheel� we show both left andright corner trajectories and a change�lane trajectory using polynomial inputs for the �retruck and the same system without tiller steering�

Since the coordinate transformation to chained form is a di�eomorphism on the openset U � fx � �� � ��� ��� ��� �� �� ��

� g � Rn� we need to avoid these singular points in the

con�guration space when picking the initial and �nal con�gurations for the simulation� Onepractical solution is to plan a path that does not start or end the �re truck in a singularcon�guration� then rotate the resulting trajectory about the origin to yield the desiredtrajectory� This will be explained later�

Figure shows the parallel parking maneuver which results from using the step�by�stepsinusoidal steering method or the all�at�once sinusoidal steering method� In the originalcoordinates� this corresponds to steering the �re truck from y� � � to zero with all othercoordinates starting and ending at zero� The inputs for this trajectory are found from thelast step of the step�by�step sinusoidal algorithm� where the only state that must be changedis y� � ��� the last coordinate of the � chain� or from the all�at�once sinusoidal method

v� � a� � a� sin�t

v� � b� � b� cos�t � b� cos���t�

v� � c� � c� cos�t ����

with a� � b� � b� � c� � c� � �� a� � �� b� � �������� and � � � over the interval ��� ����We note that in order to limit the size of the con�guration states or the inputs� one has

to take into account the coordinate transformation to chained form since the simulation isperformed on the system in chained form� From the equations of the �re truck system� the

��

0 2 4 6 8 10 12

-2

0

2

4

6

8

x

y

0 1 2 3 4 5 6 7-4

-2

0

2

4

6

8

t

u0

, u

1,

u2

0 2 4 6 8 10 12 14 16 18 20-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

t

u0

, u

1,

u2

x� y phase plot� Regular inputs� Inputs with limit�

Figure �� The parallel parking trajectory with and without control limitations using thesame initial and �nal positions as in Figure � The x� y phase plot shows the trajectoriescorresponding to the regular inputs �solid line� and limited inputs �dashed line�� For theinput plots� the driving velocity u� is the solid line� the steering velocity of the front wheelsu� is the dotted line� and the steering velocity of the tiller u� is the dashed line�

steering wheel angles can take values between ���� and ���� In reality�� the front steeringwheel angle is limited to ���� � �� � ��� and the tiller steering wheel angle is limitedto ���� � �� � ���� Limiting these angles can be approximated in the simulation bylimiting the two chained form inputs v� and v�� For the limited inputs� we used the sameinput parameters as for the regular inputs in equation ����� with � � �� over the interval��� ���� Figure � shows the x � y phase plot with and without the control limitation andthe corresponding inputs� The e�ect of limiting the control inputs is to stretch out thetrajectory four times as long in the x direction�

In Figure �� the chained form and original input functions needed to parallel park the�re truck as in Figure are shown� The chained form inputs are the open�loop control lawsfor the system in two�chain� single�generator chained form� The physical inputs� however�depend on the states of the system� as can be seen in the following equations�

u� � sec ��v�

u� � L� cos� �� cos� ��

v� � v�

L��

tan� �� tan �� sec� ��

u� ��L� cos� �� cos ��

cos��� � ���

v� � v�

cos��� � ��� tan��L�L� cos�� cos� ��

�cos��� � �� � ��� sin��� � �� � ���

L�� cos� �� cos� ��

���

Polynomial control inputs are now used to show the control design and performancedi�erences when the �re truck does not have tiller steering� For this system� we used thecoordinate transformation to chained form �from using h� � x� and h� � y� in equation ����without the chain and following the procedure as for the �re truck� or following theprocedure in �Tilbury� Murray� and Sastry �����

�� � x�

�Data from Berkeley Fire Department Station No� ��

0 1 2 3 4 5 6 7-4

-2

0

2

4

6

8

t

u0

, u

1,

u2

0 1 2 3 4 5 6 7-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

t

v0,

v1,

v2

Physical inputs� Chained form inputs�

Figure �� Physical and chained form inputs for the parallel parking trajectory shown inFigure � For the inputs� u�� v� are the solid lines� u�� v� are the dotted lines� and u�� v� arethe dashed lines�

�� �sec� �� sec���� � ���

L�L��

L� sin���� � ��� tan�� � L� tan��� � ��� �

L� tan��cos��� � ���

�� ��

L�sec� �� tan��� � ���

�� � tan ��

�� � y� �

which kinematically is a two�axle car pulling one trailer�For the �re truck in the chained form of equation ����� there are two chains� one of

length three and one of length two� so we use the polynomial control inputs

v� � �

v� � b� � b�t � b�t�

v� � c� � c�t

to steer the system� For the �re truck without tiller steering �a �ve�state system�� there isonly one chain of length four with the two inputs being

v� �� �� � �

v� �� �� � b� � b�t � b�t� � b�t

� �

In all of the following simulations� the initial and �nal states of the two systems are thesame�

Figures � and show the advantage of having tiller steering when making a ��� turn�For both of these trajectories� the �re truck goes through the singular point �� � ���� Toavoid this singularity for the right hand turn� we simulated the trajectory with the initialand �nal con�gurations at ��� and ����� respectively� then rotated the entire resultingpath by ���� The left hand turn was simulated similarly� noting that the path is wider thanthe right hand turn in the same intersection due to the convention of driving on the righthand side of the road�

The inputs with and without tiller steering for the right hand turn trajectory are shownin Figure �� We see that the steering velocity for the system without tiller steering� which

��

With tiller steering� Without tiller steering�

Figure �� With and without tiller steering for a ��� right hand turn in an intersection� Westeered the �re truck from an initial state �x�� y�� � ��� �� with the body angles �� � �� ���� and steering angles �� � �� � � to a �nal state �x�� y�� � ����� ����� with body andsteering angles all zero�

0 2 4 6 8 10 12-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

t

u0

, u

1,

u2

0 2 4 6 8 10 12-3.5

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

t

u0

, u

1

With tiller steering� Without tiller steering�

Figure �� The inputs for the �re truck with and without tiller steering for the ��� right handturn trajectory shown in Figure �� The driving velocity u� is the solid line� the steeringvelocity of the front wheels u� is the dotted line� and the steering velocity of the tiller u� isthe dashed line�

��

With tiller steering� Without tiller steering�

Figure � With and without tiller steering for a ��� left hand turn in the same intersection�We steered the �re truck from an initial state �x�� y�� � ��� �� with the body angles �� ��� � ��� and steering angles �� � �� � � to a �nal state �x�� y�� � ����� ��� with bodyangles � and steering angles at zero�

is just the steering velocity of the front wheels� switches back and forth more� Of greaterinterest� however� is that the magnitude of the input u� without tiller steering is larger thanthe inputs of the �re truck with tiller steering� In some sense� the control for the systemwithout tiller steering has to work harder�

Figure � shows how the tiller steering assists the vehicle when it changes lanes on afreeway� Figures �� and �� con�rm our �ndings that the system without tiller steeringworks harder than with tiller steering� In addition� we notice that in the right hand cornertrajectories� the system without a tiller crosses over into the lane of on�coming tra�c� Allof the above maneuvers are smoother for the �re truck� which justi�es our initial hypothesisthat the tiller adds maneuverability�

� Conclusions

Su�cient conditions have been given for transforming a three�input� drift�free nonholonomicsystem into a two�chain� single�generator chained form via a coordinate transformation andstate feedback� In this special form� the system was shown to be completely controllableand easy to steer using sinusoidal input functions with integrally related frequencies orpolynomial inputs� The steering algorithms presented provide methods for open�loop� point�to�point control without obstacles�

The example used to illustrate the ideas of this paper was a �re truck� The kinematicequations were derived using nonholonomic linear velocity constraints for the non�slippingconditions of the wheels rolling on the road� The system was transformed into a two�chain�single�generator chained form and steered using sinusoids and polynomial control inputs�Simulation results compared the �re truck to the same system without tiller steering� the�re truck has greater maneuverability and can execute the same maneuvers in a narrower

��

With tiller steering� Without tiller steering�

Figure �� With and without tiller steering for changing lanes on a freeway� We steered the�re truck from an initial position �x�� y�� � ��� �� with the body and steering angles all zeroto a �nal position �x�� y�� � ���� �� with body and steering angles all zero� The vehiclemoves a total of � units in the x direction and � units in the y direction�

With tiller steering� Without tiller steering�

Figure ��� With and without tiller steering for a ��� right hand turn in an intersection�We steered the �re truck from an initial position �x�� y�� � ��� �� with the body angles�� � �� � ��� and steering angles �� � �� � � to a �nal state �x�� y�� � ����� ����� withbody angles �� � �� � ����� and steering angles at zero�

��

0 2 4 6 8 10 12-0.5

0

0.5

1

1.5

2

t

u0

, u

1,

u2

0 2 4 6 8 10 12

1

1.2

1.4

1.6

1.8

2

t

u0

0 2 4 6 8 10 12

-120

-100

-80

-60

-40

-20

0

t

u1With tiller steering� Without tiller steering�

Figure ��� The inputs for the �re truck with and without tiller steering for the ��� righthand turn trajectory shown in Figure ��� The driving velocity u� is the solid line� thesteering velocity of the front wheels u� is the dotted line� and the steering velocity of thetiller u� is the dashed line�

space and with less steering e�ort�Future work includes controlling wheeled nonholonomic vehicles when there are obstacles

present� Preliminary work in this area is presented in �Sahai� Secor� and Bushnell ������

Acknowledgements� The authors would like to thank Captain David Orth� tillermanGeorge Fisher and driver Bob Humphrey at Berkeley Fire Department Station No� � fordemonstrating their tiller truck� We also thank Richard Murray and Greg Walsh for manyinspiring discussions� In addition� we would like to thank the reviewers for their suggestionsand comments�

References

Alexander� J� C� and Maddocks� J� H� ��� On the maneuvering of vehicles� SIAM Journalof Applied Mathematics� �����!���

Brockett� R� W� ���� Control theory and singular Riemannian geometry� New Directionsin Applied Mathematics� eds� P� J� Hilton and G� S� Young� New York� Springer�Verlag�pp� ��!���

Hermann� R� and Krener� A� J� ����� Nonlinear controllability and observability� IEEETransactions on Automatic Control� �����!����

Isidori� A� ���� Nonlinear Control Systems� New York� Springer�Verlag� Second edition�

Monaco� S� and Normand�Cyrot� D� ����� An introduction to motion planning undermultirate digital control� Proceedings of the IEEE Control and Decision Conference�Tucson� Arizona� pp� ���!����

Murray� R� M� and Sastry� S� S� ����� Grasping and manipulation using multi�ngered robothands� Robotics� Proceedings of Symposia in Applied Mathematics� ed� R� W� Brockett�American Mathematical Society� �����!���

Murray� R� M� and Sastry� S� S� ���� Nonholonomic motion planning� Steering usingsinusoids� IEEE Transactions on Automatic Control� �������!����

Sahai� A�� Secor� M�� and Bushnell� L� ����� An obstacle avoidance algorithm for a carpulling trailers with kingpin hitching� Proceedings of the IEEE Control and DecisionConference� Lake Buena Vista� Florida�

Tilbury� D� and Chelouah� A� ���� Steering a three�input nonholonomic system usingmulti�rate controls� Proceedings of the European Control Conference� Groningen� TheNetherlands� pp� ���!����

Tilbury� D�� Murray� R�� and Sastry� S� ���� Trajectory generation for the N�trailer problemusing Goursat normal form� Proceedings of the IEEE Control and Decision Conference�San Antonio� Texas� pp� ���!���� To appear in IEEE Transactions on AutomaticControl�

Tilbury� D�� S"rdalen� O�� Bushnell� L�� and Sastry� S� ����� A multi�steering trailer system�Conversion into chained form using dynamic feedback� Proceedings of the IFAC Sym�posium on Robot Control� Capri� Italy� To appear in IEEE Transactions on Roboticsand Automation�

Appendix

Proof of Proposition ��We �rst remark on the dimension of each distribution� di �� dimGi� By construction�

d� � � and by assumption� dn�� � n � �� Since G� G� � � � Gn���

� � d� � d� � � � � � dn�� � n � � �

Let n� be the smallest integer less than n such that

dimGn� � n� � � dimGn��� � n� �

and let n� be the integer when dimGn��� �rst drops by two� Thus� adif�f� and adif�f� eachgive new directions up to some level� n�� when one chain saturates and the other chaincontinues to give new directions until the state space is spanned�

di �

�����

��i� �� i � �� � � � � n��i� �� � �n� � �� i � n�� � � � � n�n� � i � n�� � � � � n� �

with n� � n� � � � n� �� Without loss of generality� this proof will use the case n� n��A basis for Gn�� can then be chosen which is given by the �rst n� brackets of f� with f�and the �rst n� brackets of f� with f�

Gn�� � � � � � Gn� � spanff�� adf�f�� � � � � adn�f� f�� f�� adf�f�� � � � � adn�f� f�g

��

�where f� and f� have been renumbered if necessary��We note that because of the special form ��� of the vector �elds� none of the vector �elds

in Gn�� has an entry ��x�

� thus

spanff�� f�� adf�f�� � � � � adn�f� f�� f�� adf�f�� � � � � adn�f� f�g

has dimension n on U � Then since the distribution Gn�� is involutive and of dimensiondn�� � n�� on U � the Frobenius Theorem �Isidori ���� gives that there exists n�dn�� � �smooth function h� � U � R such that dh� � X � � for all X � Gn�� and furthermore�dh� � f��x� �� a��x� �� �� With f� in the special form of equation ���� we can choose h� tobe x� which gives dh� � f��x� � �� It can also be veri�ed that none of the vector �elds inGn�� has an entry in the �rst coordinate� giving dh� � X � � for all X � Gn��� By thedimension argument� Gi � Gn�� for i � n�� � � � � n� �� thus dh� � Gi for i � n�� � � � � n� ��

The distribution Gn��� drops dimension by one by removing the vector �eld adn�f� f�from Gn�� � Gn� � Since this distribution is involutive and dn��� � n � �� there existtwo smooth functions whose derivatives span G�

n���� One of these functions is h� since it

annihilates Gn� � Gn���� Let h� be the second function independent of h� and note thatdh� � adn�f� f��x� �� a��x� �� ��

At the next step� the vector �eld adn���f�

f� is removed from Gn��� to get the involutivedistribution Gn��� of dimension dn��� � n�� The functions dh� and dh� annihilate Gn����We will need the following property of Lie derivatives to �nd the third smooth function thatannihilates Gn����

Lf�g��x� � LfLg��x�� LgLf��x� ���

for a smooth function �� Using this property and the fact that dh� annihilates Gn��� we�nd that dLf�h� annihilates Gn��� since

� � dh� � adjf�fk � Ladjf�fkh� � L

f��adj��

f�fkh� � Lf�Ladj��

f�fkh� � Ladj��

f�fkLf�h�

� �dLf�h� � adj��f�fk � � j � n� � � k � �� �

Furthermore�

a��x� � dh� � adn�f� f� � �dLf�h� � adn���f�f� �

This procedure continues to the distribution Gn� which is annihilated by dh�� dh�� dLf�h��� � �� dLn��n���

f�h��

Starting with the involutive distribution Gn���� the distributions drop dimension bytwo� For Gn���� the vector �elds adn�f� f� and adn�f� f� are removed from Gn� � The one�forms

dh�� dh�� dLf�h�� � � � � dLn��n���f�

h� annihilate Gn��� Gn� � In addition� equation ��� and

the fact that dLn��n���f�

h� annihilates Gn� gives

dLn��n�f�

h� � adj�n��n�f�fk � � � � j � n� � � k � �� �

which shows that dLn��n�f�

h� annihilates Gn���� There is one more function whose di�eren�tial also annihilates Gn���� we call it h� and note that dh� � adn�f� f��x� �� a��x� �� ��

At the next step� the vector �elds adn���f�f� and adn���f�

f� are removed from Gn���

to get the involutive distribution Gn��� of dimension dn��� � ��n� � ��� The one�forms

��

dh�� dh�� dLf�h�� � � � � dLn��n�f�

h�� dh� annihilate Gn��� Gn���� To see that the one�forms

dLn��n���f�

h� and dLf�h� also annihilate Gn���� we use equation ��� and the fact that

dLn��n�f�

h� and dh� annihilate Gn��� to get

dLn��n���f�

h� � adj�n��n���f�fk � � � � j � n� � � k � ��

and

� � dh� � adjf�fk � �dLf�h� � adj��f�fk � � j � n� � � k � �� �

Furthermore�

a��x� � dh� � adn�f� f� � �dLf�h� � adn���f�f� �

This procedure continues to the distribution G� which is annihilated by dh�� dh�� � � ��dLn���

f�h�� dh�� � � �� dL

n���f�

h��In summary� we have found three functions� h�� h�� h� such that

dh� � Gj � � j � n � �dLk

f�h� � Gj � � j � n� � �� � � k � n� � �� j

dLkf�h� � Gj � � j � n� � �� � � k � n� � �� j �

����

These three functions are used to de�ne the chained form coordinates as follows

�� � h� �� � Ln�f�h� � � Ln�

f�h�

�� � Ln���f�

h� � � Ln���f�

h����

������ n��� � Lf�h�

�n��� � Lf�h� n� � h��n� � h� �

����

To verify that the above coordinate transformation is valid� we show that it is a localdi�eomorphism� First the derivative of the coordinate transformation is calculated withrespect to x� ��

�x� This is then multiplied on the right by the nonsingular matrix M whose

columns are the n independent vector �elds f�� f�� adf�f�� � � � � adn�f�f�� f�� adf�f�� � � � � adn�

f�f��

��

�x�M �

� �

dh�dLn�

f�h�

���dLf�h�dh�

dLn�f�h�

���dLf�h�dh�

������������������

hf� f� adf�f� � � � adn�f� f� f� adf�f� � � � adn�f� f�

i

��

� �

� � � � � � � � � � � � � � � � �a��x� � � � � � � � � � � � �

� �a��x��� �

��� �� ��

������

��� ��� �

� � ����

���� � �

� � ����

������

���� � �

� � � ���

� � � � � � � � � a��x� � � � � � � � � � � � � � � � �a��x� � � � ��� � �

�� ���� � �a��x�

� � ����

������

���� � �

� � ����

���� � �

� � �

� � � � � � � � � � � a��x�

�����������������������

The functions a��x� and a��x� are nonzero by de�nition� Equation ���� is used to get thezeros in the matrix� Using row operations� it can be shown that the above matrix� ��

�x�M �

is equivalent to a nonsingular diagonal matrix with

���a��x�� � � � � a��x���a��x�� � � � � a��x�

on the diagonal� and thus has full rank� Therefore the Jacobian matrix ���x

must also benonsingular locally� which implies that ��� �� � � ��x� is a local di�eomorphism and a validcoordinate transformation on the open set U �see �Isidori ���� Proposition ������

To compute the input transformation� we take derivatives of the transformed coordi�nates� cancelling terms by using the zero entries of the above matrix ��

�x�M �

�� � u� �� � Ln���

f�h�u� � Lf�L

n�f�h�u� � Lf�L

n�f�h�u�

�� � Ln�f�h�u� � ��u�

��� �n� � Lf�h�u� � �n���u�

� � Ln���f�

h�u� � Lf�Ln�f�h�u�

� � Ln�f�h�u� � �u�

���

n� � Lf�h�u� � n���u� �

Therefore the input transformation

v� � u�

v� � Ln���f�

h�u� � Lf�Ln�f�h�u� � Lf�L

n�f�h�u�

v� � Ln���f�

h�u� � Lf�Ln�f�h�u�

will result in the two�chain� single�generator chained form ���� �

Proposition � �Converting to m�Chain� Single�Generator Chained Form�Consider the drift�free nonholonomic system

x � g��x�u� � � � �� gm���x�um�� ����

��

with smooth� linearly independent input vector �elds gi� There exists a feedback transfor�mation on some open set U � R

n

z � ��x�

u � �x�v

to transform the system ��� into m�chain� single�generator chained form

z�� � v� z�� � v� z�� � v� � � � zm� � vm z�� � z��v� z�� � z��v� zm� � zm� v�

������

���

z�n� � z�n���v����

���

z�n� � z�n���v�� � �

��� zmnm � zmnm��v�

if there exists a basis f�� � � � � fm�� for �� �� spanfg�� � � � � gm��g which has the form

f� ��

�x��

nXi��

f i��x��

�xi

fj �nXi��

f ij �x��

�xi� � j � m � �

such that the distributions

Gj � spanfad if�f�� � � � � ad i

f�fm�� � � � i � jg � � j � n� �

have constant dimension on U � are all involutive� and Gn�� has dimension n� � on U �

The proof follows the same method as in the proof of Proposition ��