- Email: [email protected]
Mathematical Model of a SKID-Steered Mobile Robot for Control and Self-Localisation
Copyright (' IFAC Intelligent Autonomous Vehicles. Sapporo. Japan. 2001
c:
0
[>
Publications www .e lseyier.com locate ifac
MATHEMATICAL MODEL OF A SKID-STEERED MOBILE ROBOT FOR CONTROL AND SELF-LOCALISATION
Frantisek Sole, Ludek Zalud, Bohumil Honzik
Department of Control, Measurement and Instrumentation Brno University of Technology Bozetechova 2, 612 66 Brno, Czech Republic E-Mail : {sole. zalud.honzikb} @dame.fee.vutbr.cz
Abstract: Department of Control, Measurement and Instrumentation has developed a new Skid-steered Mobile Robot (SSMR) recently. The SSMR is an indoor/outdoor remotely/autonomous operable vehicle designed for research in combination of autonomous and man-machine control of mobile robots with application of telepresence principles. The article describes mathematical model of the vehicle dynamics and data-fusion self-localisation system. Copyright © 2001lFAC Keywords : robot, teleoperation, autonomous mobile robots, navigation, model based control. data fusion
I.
INTRODUCTION
Since September 1999 we have been developing a new Universal Telepresence and Autonomous Robot (U.T.A.R.) at our department (see Fig. \.). The main requirements for the design of the robot were: -robot should work in rather hard terrain, not only in "laboratory conditions" -system should work in both autonomous and telepresence (or operator-controlled) modes -system should be reliable. redundant and noise robust to ensure its functionality in heavy duty missions. At present. the hardware part of the robot is finished. The software for the telepresence mode is also complete and is being tested . The main task now is to develop appropriate sophisticated algorithms for the autonomous mode for the U.T.A.R. system .
Fig. I. U.T.A. R.
2.
U.T.A.R. DESCRIPTION
The U.T.A.R. system is intended as a universal fully device with combined capabilities: autonomous mode and telepresence/remote
273
controlled mode. The primary mode is telepresence operated, but the system should be able to switch to the autonomous mode in emergency (lose of the signal. etc.) or as a response to the operator's command. The U.T.A.R. system consists of two units - the robot itself. with on-board computer, sensors, etc. and operator' s computer. 2.1.
inclinomcnlC'f'j ~-DO I :
lKcd\.'T\,",ctc~
la.o;n pro ll.im il ~
] -001:
)QJ1rcr
Iawr IQI.Hr j sdI-Mal...... '
........
Locomotion Subsystem
__ -
The locomotor is basic part of SSMR. Scheme of SSMR locomotor is on Fig. 2. The base frame of SSMR locomotor is a rectangular steel construction. Two banks of two drive wheels are each linked to two DC electrical motors via chain on sprocket drive and gearing. The two drive assemblies for the left and right banks are identical but they operate independently to steer the vehicle. The motors can be driven in either directions, thus causing the vehicle to move forward, backward, right or left. The motors are equipped with incremental encoders and can be controlled either in velocity or current-torque loop. Wheels of the vehicle consist of a rims and shallow tread pneumatic tyres with outer diameter 260mm. Wheel base is 400mm, wheel track is 500mm.
li ll ,
JJ-.- dau
...,---,
do.1I.~1i(lfl I - - - - - -. _--,r~ u~
• i
1
,
mo(h rlCd mboI : evw,lcnce grids
Fig. 3. Simplified scheme of the self-localisation module Since the SSMP turns by skidding, the selflocalisation is rather complicated. We cannot use classical differential odometry ordinarily used in machines with differential steering topology. Additional problem is that the robot should work in rather hard terrain, so the data from the laser proximity scanner cannot be used if the platform is inclined. From the mentioned facts it is noticeable that the only possibility for the satisfactory self-localisation system with reasonable budget is data-fusion from more sensors. All the measured data are firstly mathematically adjusted (filtration, integration, etc.) and then processed in the main self-localisation module. Since the relations among the sensory data reliability are quite complicated and in some cases not well specified, they are partly processed mathematically and partly heuristically. The self-localisation module consists of three relatively independent sub modules: gyroodometry, laser-scanner self-localisation and inertial navigation module. The data from these modules are combined in the data-fusion module. We can say that the laser-scanner sub module works if the robot operates in well structured environment with plain floor. If the robot operates in terrain, the other two modules are used. Since the sensors used in the inertial navigation subsystem are low-cost and the signals from accelerometers must be double-integrated to achieve position, the inertial navigation subsystem output is reliable only for quite short period. It means that the gyroodometry system, is very important. An important part of this sub module is precise and reliable mathematical model of the locomotion subsystem ofU.T.A.R.
Fig. 2. Basic scheme of the locomotor The locomotor transports all onboard robot components, as well as another experimental devices and pay loads. The overall maximum dimensions of the U.T.A.R. are 580x780x750mm, the weight of it is 71kg and it can carry up to 25kg of payload. The maximum speed of the U.T.A.R. is 2km1h. The ground clearance of the robot is 75mm (but may be increased up to 130mm with current wheels). The maximum rise is 30°. 3. U.T.A.R. NAVIGATION The overall simplified scheme of the data-fusion self-localisation subsystem is shown in Fig. 3.
274
4.
MA THEMA TICAL MODEL OF THE SSMR LOCOMOTOR
motion of system of bodies under influence of external forces (Stadler,1995). The only external forces acting on the system are slip and skid forces. According to Fig. 4 we can write for the complete vehicle i=4 i=4 MX= cos
To find a reasonable model of the locomotor one must accept many simplifying assumptions. We assume that the vehicle moves in flat horizontal plane only and that the wheels are in permanent contact with the plane. We also assume that slip and skid forces between the tyres and flat surface can be represented by Coulomb model of friction (Armstrong-Helouvry, 1994) and they are the only significant external forces acting on the locomotor. Thus we neglect forces due to rolling of tyres, drag forces etc. The locomotor is considered as system of four wheels and a rigid platform see Fig. 2. Motion of the locomotor is calculated with respect to inertial frame XY. For modelling purpose we suppose that the locomotor is geometrically symmetric but centre of gravity CG is shifted forward with respect to geometric centre GC of the locomotor. Position of the vehicle with respect to the inertial frame is given by x y co-ordinates of CG an by angle of course
IF)
M:v= IF)
J~ = ( - FI
+ F2 + F3 - F) ) ~ + 2 +(SI + S2 )pa -(S3 + S4 )(l- p)a Similarly we can develop equations of rotary motion for each bank of wheels. From Fig. 5 one can see that the only torques acting on the bank are torques developed by drives an torques developed by longitudinal slip forces. T
~
y+ I
y \
~,()
VS
\
I
!
Fig. 5. Torques acting on the wheels of the SSMR.
I I
Equations describing according to Fig.5
I
2J" 00 1
I
=TI
rotation
of wheels
- r( F2 + F3 )
are
(2)
2J".oo 2 =T2 -r(FI +F4) I
x
Should we know slip and skid forces F, Sand torques T we would be able to calculate accelerations from equations (I) and (2), then after integration we would be able to calculate velocity and finally position of the platform. Thus we calculate slip and skid forces according to Coulomb model.
Fig. 4. Forces acting on the SSMR Individual symbols in figures and subsequent equations represent:
F; = F;maxsign(rOJ i
F longitudinal force between the tyre and surface due to slip S lateral force between the tyre and surface due to skid 00\.2 angular velocity of right or left wheels respectively
specifies position of CG with respect to GC
-
v{)
for i = 1,4 and} = 2 for i =2.3 and} = 1 Si =
Sima,sign(-v;)
for i =1.4 and} = 2 for i =2.3 and} = 1 (3) F max and Srnax are maximum slip and skid forces. The forces are roughly calculated according to following equations
for i =
F;max =Simax L2
F imax
Equations of motion of the locomotor can be developed according to Euler's theorem about
for i = 3,4
275
= Simax =
=J1i Mg(1-p)a/2 J1i Mgpa / 2 (4)
Where J1 is coefficient of friction which depends on many factors as inflation pressure, tire design, roughness of the surface etc. (Wong,1978). Experiments showed that the coefficients of friction are approximately the same for longitudinal and lateral motion for our vehicle. Longitudinal and lateral velocities v P and vS respectively, are calculated according to eq. (5) which follows from FigA F
VI
Equations describing dynamics of the unicycle as a whole are ; =2
Mi =
My=
; =2
IF)
cos
;=)
; =2
;= 2
IF)
sin
J~ = (F)
Is; cos
(6)
;=)
;=1
F. • . b . =v4 =x·coslp+ y.smlp-"2lp
Is; sin
;=)
- F2 )
~
Equations describing dynamics of rotation of wheels of the unicycle are
F F . . . b . v2 =V3 =x , coslp+ y·smlp+"2lp
(5)
JwW) =
v; =v~ =-x·sinlp+ y · coslp+ parp v; = v~ = -x· sin lp + y. cos lp - (1- p)rp
J \\.W2
~ -
= T2
rE;
(7)
- rF2
Slip and skid forces can be expressed with help of Coulomb model as
The complete model of the locomotor for MA TLAB-Simulink is built according to principle shown on Fig.6. Numbers in blocks indicate equations which are used for calculation input output relation of the respective block.
F;
= 0maxsign(rm; -
v;)
for i =1.2
= S;maxsign(-v;)
S;
(8)
for i =1.2 Where maximum values of forces are
F;max = S;max = J1.;Mg / 2 (9)
for i =1.2
Longitudinal and lateral velocities vP and vS of the wheels are calculated according to following equations Fig. 6. Principial scheme of the model for MA TLAB-Simulink
F.
VI
The model is rather complicated and not very transparent. Similar model and correspondingly complicated control law was described in (Caracciolo et al.,1999). We tried to compare our model with model of unicycle (soccer robot) from Fig.7 (Yang, 1999).
b .
F . ' . b . v 2 =x·cos
v;
=v~
(10)
=-x·sinlp+ y·coslp
One can see that the models are very similar. But for the unicycle we usually use the following nonholomic constraints. We suppose that the robot is not skidding thus the lateral velocities of the wheels and the centre of gravity (which we consider to be identical with the geometric centre of the robot) are zero
y ~
, v'
v;
=v~
=-x·sinlp+ y·coslp=O
(11)
We also suppose that wheels are not slipping thus
~,
b . 2 F. . . b . v,- =x·coslp+ y·smm--m=rm, .,.. 2"" -
v,
b\
..
= X· cos lp + Y . sm lp + "2lp
F.
VI s
v,
x
=
X·
.•
COSlp+ y. smlp+-lp = rm)
(12)
Equations (11 ),( 12) can be expressed in matrix form
Fig. 7. Unicycle model ofrobot
276
l
COS( qJ)
0.8..--...--.,..--......---.---.--........--.----,
sin(qJ)
m
0.7·
COS(qJ)
sin(qJ)
-sin(qJ)
cos(qJ) (\ 3)
The coefficient matrix in (13) is non-singular, thus the system has unique solution
X] lo.5COS(q»
0. 5 cos(q»
0.5sin(q»
0. 5 sin(q»
lib
-lib
y=
l
1
~~~::I~
(\4)
Fig.S . Motion of the robot during simulation. CGposition of the center of gravity, GC- position of the geometric center, unicycle - motion of an equivalent unicycle.
Model of motion given by (14) is so called kinematical model and can be used for control of the vehicle provided that constraints (\ I ),(12) are fulfilled . Physically it means that acceleration of motion is small enough so that it may be developed by adhesive forces . When we try to apply the same procedure for the SSMR system we can introduce similar constraints only for longitudinal motion of the vehicle because the wheels must skid laterally during any turn. Introduction no skid condition would limit the movement of the SSMR to straight line motion only. Thus no slip condition for SSMR is
N
........--...,..._--...--~
1~r---,---.---
100 ..... ..... ... ........ , ........... ;........... ; .......... . ;.. ....... ..
F. 4
.
....... .... ;... ·.... ··'· ··.. ·.. ···' .... ·.. ····'·..F3· ·.. ·' .. ·.. ···.. ·
~
o ........... ;........... ;........... ;.... .... .. .;... ..... .. . ;.. ...... .. . :
F4
..... ..... : .......... : ........... : ..... .. .. .. : .. .. ........ ..... ... .. .
-~
b . VI = v = x, COSqJ+ y . SIll qJ-2qJ = r(J)2 F
.0.1 '--_'--_'--_-'--_-'-_-'-_--'-_--'-_-.J .03 ·0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5 m
.
.
'
-100 .... , ...... j • ••
··· · ···~···· ·· ·· ··· ~ ······ · ····~ ··F·i · · ·
··1·· ·········
_1~L-----'----'-----'---..J....--.L-----l
F F. ., b . v2 =V3 =X'COSqJ+Y'SIllqJ+2qJ=r(J)1
o
5
10
15
20
25
30 5
Fig.9. Longitudinal adhesive forces acting on the robot during simulation.
(15)
and in matrix form
N 100F===~====~====~==~====~==~
S 4.3.
COS(qJ) [ COS(qJ)
sin(qJ)
2]l:1
- b/ sin(qJ) b/2
~
= [r(J)l] . r(J), qJ - (16)
,
.......... .. . . .. . ... ; .... ... .. ..
:.......... .:...... .. ... :...... . ..
..
0 .. ······ .. ·: ·· · .. · .. · .. ·· .. ·· · .. · .... ·· ·· .. · .. ·: · .... ·· · ···· .. .. · ··
The system of equations (16) has solution but the solution is not unique, thus the system cannot be used as a model for the SSMR. Nevertheless some ideas of control of the unicycle can be used for control of SSMR. The following figures show some simulation results. Simulation experiment was performed with proportional controllers of velocities of individual banks of wheels
.... .. .... ,.. .. .... ... ,..... .. .. . ,..... .. .. .. ,........... , .. .... .
-~
S 1.2 :
....... ; ........... ;., .... ..... ;............ :. ......... .
-100 ...
-1 ~'------'-----'-----'----'----i...-----J
o
5
10
15
20
25
30
Fig.IO. Lateral adhesive forces acting on the robot during simulation.
(17) The desired values of velocities were set ffi2d=O .IS mls and ffi I d=O mls.
277
5
the i999 IEEE international Conference on Robotics & Automation, Detroit. Michigan. May, 1999 Wong. J.Y. (1978). Theory of Ground Vehicles. lWilley.1978 Yang J. ,M., Kim, l,H . Sliding Mode Motion Control of Nonholonomic Mobile Robots . IEEE Control Systems. April 1999.
-0.2
" ' , ... :, ...... . ; . . .... . : .. .. . . . . ; , . ..... ! ... , .. .. :, ... ... . : .. , .... .
3
:2
:
-O.4'--"---~--'----'---'---'--~---'
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0 .8 m
Fig.ll. Stroboscopic view of robot motion during simulation. Sampling period is 5 s. With desired values set as stated the robot should move along a circle. Initial position of the robot CG was set at the origin of the inertial reference frame with robot's longitudinal axis coinciding with x axis of the frame and with zero velocity. Thus robot should move counter- clockwise as shown in Figs. 8 and 1I (robot's initial orientation is given by positions of wheels 2 and 3). Fig. 8 and Fig. 11 clearly show the difference between the skid steered robot and the unicycle. Results of this simulation experiment were confirmed by experiments with the real system.
5.
CONCLUSION
Mathematical model of the SSMR locomotor was implemented with help of MA TLAB-Simulink. The model was verified by comparison with real experiments. Tests proved qualitatively similar behaviour of the model and real vehicle. The model is intended for improvement of selflocalisation system and for development of AI based control strategies. ACKNOWLEDGEMENT This work was supported by the Ministry of Education of the Czech Republic under Project LNOOB096 REFERENCES Armstrong-Helouvry, B. (1994). Control of machines with friction. Automatica. Champeny-Bares. L.. S. Coppersmith and K. Dowling (1991). The terregator mobile robot. Technical report. Camegie Mellon University. Stadler. W. (1995). Ana(vtical robotics and mechatronic. McGraw Hill. New York. Carraciolo, L..De Luca. A., Iannitti S. (1999). Trajectory Tracking Control of a Four-Wheel Differentially Driven Mobile Robot. Proceedings of
278