Path Planning Using Surface Shape and Ground Properties · Many approaches to robot path planning...

Path Planning Using Surface Shape and Ground Properties

Steven MartinQueensland University of Technology

and CSIRO ICT CentreBrisbane, Australia

[email protected]

Peter CorkeQueensland University of Technology

Brisbane, [email protected]

AbstractThis paper presents a path planning technique forground vehicles that accounts for the dynamics ofthe vehicle, the topography of the terrain and thewheel/ground interaction properties such as fric-tion. The first two properties can be estimated usingwell known sensors and techniques, but the third isnot often estimated even though it has a significanteffect on the motion of a high-speed vehicle. Weintroduce a technique which allows the estimationof wheel slip from which frictional parameters canbe inferred. We present simulation results whichshow the importance of modelling topography andground properties and experimental results whichshow how ground properties can be estimated alonga 350m outdoor traverse.

1 IntroductionMany approaches to robot path planning have been devel-oped and have been used in real-world robot systems suchas the DARPA Urban Challenge [13, 18]. However thesewell known approaches only solve the problem of generat-ing a path that avoids obstacles or minimizes some objectivefunction related to the cost of traversing particular regionsof terrain. Planning dynamically feasible paths increases thedimensionality of the search space, requires modelling thevehicle dynamics rather than just the kinematics, and alsopresupposes knowledge of the vehicle’s inertial and mass pa-rameters, the suspension model, motor speed-torque curve,ground topography, and the frictional and dissipative phe-nomena where the tyre meets the ground (ground properties).This complexity means that the system dynamics is typicallyignored, or grossly simplified, in the planning phase and ac-counted for by an online path-following controller at run time.However to achieve maximum performance it is important tomodel these dynamics, identify the parameters and accountfor them at planning time.

The contributions of this paper are computing dynami-cally feasible paths which the robot has the capability to fol-low. Firstly we investigate the significance of topography and

Figure 1: QUT rOscar: the high-speed ground robot platformused in this work (length ≈ 50cm).

ground properties for a simple simulation example where weuse a Pseudospectral Optimal Control Solver for planning.Secondly we demonstrate the feasibility of online estimationof ground properties using the small-scale robot, shown infigure 1, for a 350m traverse we estimated the traction avail-able.

1.1 Prior WorkA simple approach is to avoid regions which are difficult totraverse and a traversability metric can be defined to describehow difficult it is to traverse a region, and can be computedfrom properties such as slope, surface roughness, surface de-formation, vehicle vibration, vehicle stability, friction, en-ergy, completion time, colour or texture [2,8,9,16,17]. Someof these properties can be estimated in advance of arrival us-ing, for example, laser scanner point clouds or camera im-agery. Traversability metrics are useful as they provide a sim-ple measure which can be used to optimise a global or localplan. Their disadvantage is that the properties are inferredrather than experienced.

Rather than avoid potentially difficult terrain the vehiclecan make the best effort at run time to meet the plan. Thesimplest and most common approach to handling ground and

Proceedings of Australasian Conference on Robotics and Automation, 7-9 Dec 2011, Monash University, Melbourne Australia.

of 7

vehicle properties is to replan often. This is usually achievedusing a two tier planner: global and local planning. A globalplanner searches the space to find a kinematically feasiblepath to the goal, perhaps taking some traversability data intoaccount. Then a local planner accounts for vehicle dynam-ics, unknown obstacles and unmodelled properties and gen-erates vehicle control inputs over a short look-ahead window.The local planner is run at a high rate to correct for discrep-ancies between the vehicle/terrain model and the real world.Such a structure is employed on the Willow Garage PR2 usingthe navigation software included in ROS middleware environ-ment [12], and for outdoor robots such as [6], [7] and [14].

Rapid update of the local plan is essential to limit the di-vergence from the planned path, and this may limit the lookahead window of the planner and the complexity of the mod-elled vehicle dynamics. A consequence of limited look aheadis reduced ability to plan around large obstacles, while a sim-plistic vehicle model can produce infeasible or poor localpaths.

The task of following a global or local path is a controlproblem known as path following which seeks to minimisethe deviation of the vehicle from a given path. Typical state-of-the-art path following controllers use a dynamic vehiclemodel to predict the vehicles response and compute controlinputs to minimise path deviation. This model is iterativelyrefined using the error in predicted trajectory, typically calcu-lated from RTK GPS to compute unknown parameters, usu-ally slip rates or cornering stiffness. Recent path followershave been shown to improve the vehicle’s ability to accuratelyfollow paths at high speeds of up to 8m/s. [10, 11]

The key assumption made by such approaches is that theterrain will be consistent, and recent developments have fo-cussed on improving the rapid convergence at terrain tran-sitions. [10] Another disadvantage of these algorithms is thatparameters are not generalised for variations in vehicle speed,path radius of curvature or vehicle. This makes the use ofsuch parameters in path planning difficult. Some initial inves-tigations into generalisation of slip rates has been conductedby Marcovitz [15] however this has only been demonstratedon a limited data set and is unused in path planning.

2 Terrain ModellingThe effect of terrain is often ignored in path planning butaccounting for surface topography and ground properties isimportant to optimise performance. Ground properties in-clude characteristics such as friction, deformation and surfaceshear strength. We assume that properties are consistent for aknown terrain type.

To investigate the advantages of accounting for terrain weconstructed a simple optimisation-based path planner. Wemodelled the vehicle as a unicycle since it simplified the mod-elling of friction and is a commonly used approximation to afour wheel vehicle. We also assume that the ground proper-

ties are dominated by friction allowing dissipative phenom-ena, such as ground deformation, to be ignored.

A five-dimensional state space model is used to describethe unicycle.

X =

xyvxvyθ

, X =

vxvyaxayθ

(1)

U =

[ζ

θ

](2)

The inputs are the fraction of motor torque ζ and the turnrate θ with limits

0 < ζ < 1 (3)

−π

4< θ <

π

4rad/s (4)

Figure 2 shows the forces on the unicycle. FW is the forcedue to torque on the wheel, FT is the force due to lateral fric-tion and S is the force due to slope.

The desired torque is,

T = TMax

(1−

VVX

VMax

)ζ (5)

where torque decreases linearly up to maximum velocity,VMax. The longitudinal force on the wheel

FW = H(−(|VVX |−VMax)

)(H(

Fr−Twr

)·Fr +H

(Twr−Fr

)· T

wr

)(6)

is zero if the vehicle velocity exceeds VMax and is limited byeither motor torque or available friction, where friction is

Fr = µN (7)

and N is the normal force.The lateral force

FT =−sgn(VVY

)H(∣∣VVY

∣∣)Fr (8)

always opposes the direction of motion and applies a maxi-mum retarding force.

The acceleration of the vehicle in the world frame is

ax =FW cosθ +FT sinθ +SX

m(9)

ay =FW sinθ +FT cosθ +SY

m(10)


of 7

Figure 2: Forces on unicylce model. S is force in X−Y planedue to slope and Fw & Ft are longitudinal and lateral wheelforces.

and the Heaviside, sign and absolute functions are imple-mented as smooth functions where b = 0.01.

H(x) =1+ tanh

( xb

)2

(11)

sgn(x) = tanh( x

b

)(12)

|x|=√

x2 +b2 (13)

2.1 Path PlanningFinding a path through terrain was posed as an optimisationproblem. We solved for minimum traversal time over a to-pography modelled by a mixture of Gaussians

Z =n

∑i=1

hie− (X−xi)

2+(Y−yi)2

2σ2 (14)

where n = 4 and the height of the peaks is given by h =[2,1,1,4] centered at [(−5,5),(−5,−5),(5,5),(1,−3)] andσ = 2.

For varying types of driving surface friction, µ , can rangefrom µ ≈ 0.10 on ice up to µ ≈ 0.85 for dry asphalt [5]. Toinvestigate the effect of wheel friction on a robot’s ability totraverse the environment µ was modelled by

µ = 0.8 · |Y |+0.2 (15)

The maximum velocity of the vehicle was VMax = 5m/swith a maximum applied torque TMax = 3N.m. The mass ofthe vehicle m = 3kg and the wheel radius wr = 0.1m. Thevehicle was commanded to drive from a position of (-10,0)to (10,0) and with varying levels of model fidelity. If no to-pography or friction was given, the optimisation assumed aconstant height of Z = 0m and a constant friction of µ = 0.4.

The Pseudospectral Optimal Control Solver, PSOPT, [1]was used and was able to compute a single path taking≈ 5 seconds, in 15-30 seconds on a HP Compaq 8000 Elitewith Core2Duo E8600 processor and 4Gb of RAM. Figure 3

Figure 3: Paths A (red), B (blue), C (green) and D (light blue)are shown as as defined in table 1. The contours representheight of the terrain and surface shaded by friction.

shows the optimised paths and table 1 shows the model fi-delity and the planned time for execution, Tp.

Path D is a straight line from starting point to the goal, orshortest distance path, as there is no additional criteria to beoptimised. Path C is biased in the Y-direction towards higherfriction but as the optimisation has no knowledge of the to-pography the path fails to avoid the large peak centered at(1,-3).

Path B is optimised based on topography and the optimisedtrajectory for this path appears counter intuitive. The solutionis optimal as the torque which can be applied by the motor islimited by vehicle velocity and there is no loss of energy overtime. The vehicle accelerates up the edge of the peak thenuses the gained potential energy to accelerate to a velocitygreater than VMax. Resulting in a trade off between motortorque, friction, time taken to climb the hill and the increasedspeed after the descent.

Path A accounts for both friction and topography and plansa route which avoids peaks and is biased towards higherwheel friction. The planned execution time is higher thanpaths B and D but the actual time to follow path A is expectedto be lower due to the over estimation of friction in B and D.Path A also utilises the same stored energy effect resulting ina lower Tp than path C.

2.2 SimulationA vehicle using a pure pursuit controller [3] was simulatedto investigate the performance of a vehicle following thesepaths in environment with full fidelity. The controller outputsθ ∗ based on the robot’s pose and a look ahead distance of1.0m. To perform the traversal in minimum time the simu-lated vehicle always accelerates at a maximum rate or ζ ∗=1.


of 7

Figure 4: Paths A (red), B (blue), C (green) and D (lightblue) show the planned (solid) and followed (dashed) pathfrom simulation.

Terrain Model Tp Tf Avg ∆ Max ∆

Friction Topog. (s) (s) (m) (m)A X X 4.58 4.20 0.22 0.73B x X 4.31 4.46 0.34 1.18C X x 5.14 4.49 0.71 2.45D x x 4.35 4.39 0.52 1.91

Table 1: Comparison of planned, Tp, simulated, Tf , executiontime and path deviation for planning with varying fidelity.

The look ahead distance was manually tuned and the perfor-mance of the simulated vehicle following each path is shownin figure 4 and table 1.

The simulation following path A reached the goal in thefastest time, Tf and also had the smallest average and max-imum deviation from the path. As expected all other pathswith lower model fidelity had longer completion times andlarger deviations. Path C which accounted only for frictionled to a worse result than path D which had no knowledgeof the terrain. This is a special case but supports the needfor accurate terrain information in order to optimise vehicleperformance.

As expected Path A with the most complete model fidelityhad the best performance indicating that friction and slopehave a significant impact on vehicle dynamics. Arguably theperformance of the simulated vehicle may be improved bya more sophisticated path following controller but improve-ments are expected for any controller in challenging terrain.

3 Mapping Ground PropertiesBased on these results it is important to be able to deter-mine the characteristics of the ground within an environmenthowever, measuring these properties directly is difficult [4].

Traversability metrics have been used to infer these values us-ing other measurable properties but these may not reflect thetrue ground properties. We have used an alternative approachof characterising terrain based on a vehicles response. Thissection details the development of a metric describing trac-tion from sensor data using a small-scale mobile platform.

3.1 VehicleThe vehicle used for the mapping is the QUT rOscar, shownin figure 1. It is based on a Traxxas Slash 1/8th scale RC carmodel, it is rear wheel drive and has an estimated top speedof 10m/s (36km/hr).

The car is equipped with a UTM-30LX Hokoyu laser scan-ner with a range of 30m at 40Hz, Xsens MTI-g AHRS system,Microstrain 3DM-GX2 and rear-wheel motor encoder. Thesesensors are connected via USB interface to a Gumstix Fireembedded linux system. The Gumstix is running Ubuntu 9.10with ROS C Turtle middleware. All sensors are interfaced toROS and the distributed functionality used to remotely logdata from the vehicle.

3.2 ExperimentsThe vehicle’s response to a step input was examined. It wasaccelerated from rest by a constant motor controller com-mand. The vehicle then decelerated to a stop and the trialwas repeated. The response to a step input was used as it wasexpected the excitation would produce a more deterministicsignal than steady state operation. This was also influencedby the absence of accurate position information, such as RTKGPS.

The vehicles response to a step input was examined onthree known ground types. The response of the vehicle on dif-ferent ground types is clearly distinguishable for grass, graveland tiled surfaces as shown by typical response curves in fig-ure 5.

The acceleration is measured by the Microstrain 3DM-GX2, for each trial the gravity vector is calculated while atrest and the vehicles orientation is assumed to be constant forthe trial duration. The net acceleration of the vehicle frameis calculated for each trial. The Microstrain 3DM-GX2 wasused over the Xsens MTI-g as it was located at the vehiclecentre of gravity. The MTI-g AHRS was primarily used to re-solve measurement location from using GPS. This data, andall the data used for all further calculations has been filteredusing a zero-order phase delay filter based on a third orderButterworth filter with a cut off frequency of 2.5Hz.

The calculation of ground properties, such as friction andshear strength, from acceleration is difficult due to the com-plexity of the tyre/ground interaction. Instead the term trac-tion is used to refer to the net effect of these properties onvehicle motion.

The tiled surface is a high traction area indicated by a largeacceleration peak and a slow rise in wheel velocity. Thegravel is a low traction area and this can be seen in the low ac-celeration with a sharp initial rise in wheel velocity. The grass


of 7

Figure 5: Typical wheel velocity and longitudinal accelera-tion responses of vehicle on grass, gravel and tiled surface.

is an intermediate surface showing characteristics of both sur-faces. Figure 6, shows the acceleration of the vehicle for re-peated trials on each surface. There is significant variationwhich is expected as no attempt was made to ensure identicalpaths were followed for repeated trials.

Figure 6: Filtered acceleration response for repeated trials oneach surface.

Figure 7: Filtered slip ratio for repeated trials on each surface.

3.3 MetricA typical measure used to characterise the ground propertiesin path following algorithms is the longitudinal slip ratio. Theslip ratio is calculated as shown,

slip ratio =ωr−VVX

VVX(16)

where ω is measured by the motor encoder and the longitu-dinal vehicle velocity, VVX is obtained from integrated IMUacceleration. The definition of the slip ratio results in infiniteslip when VVX =0.

As the vehicle is accelerating from rest this is problematicand makes the result highly susceptible to noise and drift fromintegrated acceleration, as can be seen in figure 7. Instead analternate traction metric is proposed

TR =

∫ t0

VVX dt∫ t0 ωr dt

(17)


of 7

Figure 8: Traction metric as measured for multiple trials onknown surfaces.

based on the ratio of longitudinal velocity to wheel velocityeach integrated over window of t=0.6s for the rOscar vehicle.An index of 1 indicates high traction and an index of 0, lowtraction.

4 ResultsThe traversability metric was applied to the previous data andfigure 8 shows the result for each trial classified by groundtype. The metric shows a clear difference in traction.

Using the defined metric the rOscar platform was used tomap the traction properties over an outdoor route. The vehi-cle was run in a 350m loop containing, concrete, grass, graveland dirt surfaces. The vehicle used the same setup as the pre-vious experiments with GPS used to localise each measure-ment.

The concrete area shown in figure 9 is clearly identified asthe region with most available traction but no clear distinctioncan be made between the gravel/dirt and grass regions. Thiscould be attributed to the slope of the terrain which is notaccounted for in the metric, terrain variability or the vehiclereaching its limit of performance.

The off-road terrain which was traversed could be classi-fied as very rough for a vehicle of this scale. This is shown bythe blue samples in the gravel and dirt region, where TR ≈ 0and the vehicle had to be assisted after becoming beached.We expect the difficulty of all off-road regions contributed tothe variation of traction on the grass and gravel/dirt surfacesmasking any distinction between the surfaces.

5 ConclusionsWe have shown that surface shape and ground properties areimportant when planning high-speed paths and that ignoringthese properties leads to sub-optimal plans which are diffi-cult to follow at run time. We used a Pseudospectral Opti-mal Control Solver for planning based on a simplified, butdynamic, vehicle model, and the topography of the terrainand the wheel/ground interaction properties such as friction.

Figure 9: Traction plotted over test area, red indicates hightraction and blue low traction regions. Samples take in gravel& dirt and concrete regions is shown, all other samples areassumed to be gathered on grass

Well known techniques can be applied to estimating the vehi-cle model parameters and well known sensors and techniquescan be applied to topography estimation. We introduce a tech-nique which allows the estimation of wheel slip from wheelspeed and inertial sensing, and from which traction can be in-ferred. We presented experimental results for calculating thetraction metric from a 350m outdoor traverse and were ableto identify high traction areas.

We have a large program of ongoing work. The optimisa-tion and simulation will be extended to include a tyre frictionmodel to improve accuracy and the results of simulation andplanning will be compared to real world experiments. Our ex-perimental traverse included a significant slope which we didnot explicitly account for in the ground property estimation.We will conduct a more comprehensive mapping of the oper-ational area to provide the topography data required for op-timised paths planned through the environment, and the im-provement will be compared to other naive planning methods.

The metric we introduced captures only a very generalmeasure of the ground properties. The interaction of the vehi-cle with the terrain is very complex and we will characterisea more complete description of the properties. The currentmetric focuses primarily on traction however other proper-ties such as ground deformation and roughness will also havea significant impact on vehicle performance. We also needto explicitly account for uncertainty in the topography andground property estimates and how this affects planning andrun-time control, and how to infer ground properties from


of 7

laser or camera imagery.We also plan to use visual odometry to determine the actual

motion of the vehicle rather than rely on inertial/GPS estima-tion.

AcknowledgementsThe authors are grateful for the technical and scholarship sup-port from the CSIRO Autonomous Systems Lab.

References[1] V.M. Becerra. Solving complex optimal control prob-

lems at no cost with PSOPT. In Computer-Aided Con-trol System Design (CACSD), 2010 IEEE InternationalSymposium on, pages 1391–1396. IEEE.

[2] C. Brooks, K. Iagnemma, and S. Dubowsky. Vibration-based terrain analysis for mobile robots. In Robotics andAutomation, 2005. ICRA 2005. Proceedings of the 2005IEEE International Conference on, pages 3415–3420.IEEE, 2005.

[3] R.C. Coulter. Implementation of the pure pursuit pathtracking algorithm. 1992.

[4] P. Giguere and G. Dudek. Clustering sensor data for au-tonomous terrain identification using time-dependency.Autonomous Robots, 26(2):171–186, 2009.

[5] F. Gustafsson. Slip-based tire-road friction estimation*1. Automatica, 33(6):1087–1099, 1997.

[6] Thomas M. Howard and Alonzo Kelly. Optimal roughterrain trajectory generation for wheeled mobile robots.International Journal of Robotics Research, 26(2):141– 166, 2007.

[7] T.M. Howard, C.J. Green, and A. Kelly. State spacesampling of feasible motions for high performance mo-bile robot navigation in highly constrained environ-ments. In Field and Service Robotics: Results of the 6 thInternational Conference(STAR: Springer Tracts in Ad-vanced Robotics Series Volume 42), volume 42, pages585–593. Springer, 2008.

[8] K. Iagnemma and S. Dubowsky. Mobile Robots inRough Terrain. Springer, 2004.

[9] D. Kim, J. Sun, S.M. Oh, J.M. Rehg, and A. Bobick.Traversability classification using unsupervised on-linevisual learning for outdoor robot navigation. In Proc. ofIntl Conf. on Robotics and Automation (ICRA). IEEE.Citeseer, 2006.

[10] Roland Lenain, Benoit Thuilot, Christophe Cariou,and Philippe Martinet. Mixed kinematic and dynamicsideslip angle observer for acurate control of fast off-road mobile robots. Journal of Field Robotics, 27:181–196, 2009.

[11] Chang Boon Low and Danwei Wang. Maneuverabilityand path following control of wheeled mobile robot inthe presence of wheel skidding and slipping. Journal ofField Robotics, 27:127–144, 2009.

[12] E. Marder-Eppstein, E. Berger, T. Foote, B. Gerkey, andK. Konolige. The office marathon: Robust navigationin an indoor office environment. In Robotics and Au-tomation (ICRA), 2010 IEEE International Conferenceon, pages 300–307. IEEE, 2010.

[13] M. Montemerlo, J. Becker, S. Bhat, H. Dahlkamp,D. Dolgov, S. Ettinger, D. Haehnel, T. Hilden, G. Hoff-mann, B. Huhnke, et al. Junior: The stanford entry in theurban challenge. Journal of field Robotics, 25(9):569–597, 2008.

[14] M. Pivtoraiko, I.A.D. Nesnas, and A. Kelly. Au-tonomous robot navigation using advanced motionprimitives. In Aerospace conference, 2009 IEEE, pages1–7. IEEE.

[15] Forrest Rogers-Marcovitz. On-line mobile robotic dy-namic modeling using integrated perturbative dynam-ics. Master’s thesis, Robotics Institute, Carnegie MellonUniversity, Pittsburgh, PA, May 2010.

[16] Zvi Shiller and Yu-Rwei Gwo. Dynamic motion plan-ning of autonomous vehicles. IEEE Transactions onRobotics and Automation, 7:241–249, 1991.

[17] B. Sofman, J. Bagnell, A. Stentz, and N. Vandapel. Ter-rain classification from aerial data to support ground ve-hicle navigation. Technical report, Robotics Institute,Carnegie Mellon University, 2005.

[18] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner,MN Clark, J. Dolan, D. Duggins, T. Galatali, C. Geyer,et al. Autonomous driving in urban environments: Bossand the urban challenge. Journal of Field Robotics,25(8):425–466, 2008.


of 7

Path Planning Using Surface Shape and Ground Properties · Many approaches to robot path planning...

Documents

Transcript of Path Planning Using Surface Shape and Ground Properties · Many approaches to robot path planning...