steering threeinput nonholonomic systemscats-fs.rpi.edu/~wenj/ecse641s07/berkeley_firetruck.pdf ·...
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 ��
�