Path Selection Based on Local Terrain Feature forUnmanned Ground Vehicle in Unknown Rough Terrain Environment

Masato Kondo, Kenji Sunaga, Yuichi Kobayashi, Toru Kaneko, Yuji Hiramatsu, Hokuto Fujii and Tsuyoshi Kamiya

Abstract— In this paper, we propose an autonomous nav-igation for Unmanned Ground Vehicles (UGVs) by a pathselection method based on local features of terrain in unknownoutdoor rough terrain environment. The correlation between alocal terrain feature obtained from a path and a value of thepath obtained from the global path planning is extracted inadvance. When UGV comes to a branch while it is running,the value of path is estimated using the correlation with localterrain feature. Thereby, UGV navigation is performed bypath selection under the criterion of shortest time in unknownoutdoor environment. We experimented on a simulator andconfirmed that the proposed method can select more effectivepaths in comparison with a simple path selection method.

I. INTRODUCTION

Unmanned ground vehicles (UGVs) have been developedfor various applications such as exploration and investigationof volcanos [1], planets [2], agriculture [3], forestry [4]and battlefields [5]. Some of these UGVs are manuallycontrolled [6] since it is more reliable. Autonomous con-trol, however, also has its merit to reduce load of humanmonitor. Therefore, autonomous control of UGVs have beenalso extensively investigated. Some of the issues related toautonomous control of UGVs are navigation, path planning[7], and control of vehicle.

Autonomous UGVs are recently applied to navigation inthe urban environments [7,8]. Roads and paths are relativelyeasy to find because they are on flat surfaces in such environ-ments, and thus navigation in urban environment is gatheringmore attention. But navigation of UGVs in uneven and roughterrains is also important and useful since it is related tothe above mentioned applications [1,2,3,4,5]. In contrast toUGVs navigation in urban environment, UGVs have to dealwith a rough and unstructured environment, and thereforesliding and unevenness affect the navigation. Therefore, thenavigation to a goal considering the environment is required.

As an example of an autonomous navigation in outdooruneven terrain environment, there is a research projectcalled Grand Challenge which is held by DARPA (DefenseAdvanced Research Projects Agency) of United States ofAmerica [10]. In Grand Challenge, a map information isprepared in advance. The car verifies the map informationwith information obtained by LRF (Laser Range Finder),

M. Kondo, K. Sunaga, Y. Kobayashi and T. Kaneko arewith the Department of Mechanical Engineering, ShizuokaUniversity, 3-5-1 Johoku, Naka-ku, Hamamatsu-shi, Shizuoka, Japanf0230031@ipc.shizuoka.ac.jp

Y. Hiramatsu, H. Fujii and T. Kamiya are with the Yamaha Motor Co.,Ltd., 2500 Shingai, Iwata-shi, Shizuoka, Japan

GPS (Global Positioning System), and IMU (Inertial Mea-surement Unit). As an example of navigation method, there isa navigation using potential field-based algorithm [11]. Thepotential field [12] is formed based on rollover, skidding,and dangerous object on the map prepared in advance. Thetrajectory is generated based on the potential field, andthe UGV performs high speed navigation by following thetrajectory. However, in case of an operating UGV in an actualenvironment, an accurate map information cannot be alwaysobtained. For this reason, a navigation to a goal without amap information in unknown environment is required.

There are autonomous navigation methods based on S-LAM (Simultaneously Localization and Mapping) in un-known outdoor environment. SLAM is a method for localiza-tion of the robot and generating a map simultaneously, andvarious methods have been developed [13]. In most cases,however, a navigation to a goal is simply determined from theshortest path on the maps created with information obtainedby local observation.

In this study, we aim to realize an autonomous navigationof a UGV in unknown outdoor rough terrain environment.In this paper, we propose a path selection method based onlocal terrain feature for performing the UGV autonomousnavigation in unknown rough terrain environment. The maincontribution of the paper is to present a method that enablesa UGV to select more effective path compared with asimple strategy of choosing ’closer’ path even in unknownenvironment. The selection is based on three-dimensionalterrain information. Thus, the correlation between the localterrain feature amount obtained in the path and the value ofpath obtained in the global path planning. Thereby, the UGVnavigation is performed by path selection under the criterionof shortest time in unknown outdoor environment. We willverify the proposed method on a simulator while comparingwith a simple path selection method.

II. PROBLEMS

In this chapter, the problem setting is described. In addi-tion, an outline of the simulator used in the experiment isdescribed.

A. Problem setting

In this study, we adopt the outdoor environment which hasa branch and up-down terrain. The appearance of the off-roadcourse is shown in Fig. 1. The objective of the problem isto navigate the vehicle to a given goal in the shortest time.

More specifically, a UGV is required to decide which pathto proceed at a junction (see for example Fig. 2) so that thetotal duration to reach the goal is minimum. The conditionsand hypotheses are listed below:

• The map information is not prepared.• The UGV is equipped with LRF, GPS and IMU.• Observation of environment (which contains ground,

slope, tree, stone, etc) is obtained by LRF as three-dimensional points in the vehicle coordinate system.

• The current three-dimensional position of the UGV inthe world coordinate is obtained by GPS.

• The three-dimensional posture of the UGV in the worldcoordinate is obtained by IMU.

• The position of the goal in the world coordinate isknown.

B. Utilization of local terrain information

Suppose UGV is located on a junction without globalmap information (see Fig. 3). A simple way for navigationis to select the path closer to the direction of the goal.However, the determination of direction in the simple methodis based only on two-dimensional plane information. Thatis, selection of more efficient path is possible by consideringthree-dimensional configuration, including the information ofheight, of the paths.

An exemplar situation is shown in Fig. 3. UGV selectspath direction at a branch. An observation range of LRFis inside the circle. Dotted lines represent an environmentnot observed by the UGV. The goal point is higher than thepoint of the branch. In this case, direction of the left pathis closer to the goal direction compared with the right path.Thus the simple method will select the left path. However,this selection is not effective in this case.

In our method, path selection is based on a considerationof the three-dimensional configuration of the environment.It can be seen in the figure that left path is flat and rightpath is going up. Knowing that the goal point is higherthan the current point and considering that the UGV hasto climb up a hill sooner or later, it can select the right path,which does not seem to be closer to the goal direction whenonly two-dimensional information is considered. This way ofconsideration was valid specifically in this example, but weexpect similar tendency exists throughout various branchesand junctions.

Fig. 1. Off-road course. Fig. 2. Example of branch.

C. Simulator configuration

In this study, targeted actual rough terrain has followingfeatures: height difference, rough terrain and changes infriction. We collect data efficiently by using the simulatorwhich simulates the actual environment. An appearance ofsimulator is shown in Fig. 4. The simulator has “environmentmodel”, “vehicle dynamic model” and “vehicle control sys-tem”. The configuration of the simulator is shown in Fig. 5.The simulator is created using Unity [14], which is a game-development system that includes physics engine.

Environment model consists of three-dimensional poly-gons that represent the shape of the terrain. The shape of theterrain is constructed based on the real environment depictedin Fig. 1, where the shape of the real terrain is measured byLRF equipped with a human-operated vehicle. Based on thepolygon surface of the terrain, LRF sensing is emulated bythe environment model. Terrain is also attributed friction forthe vehicle locomotion.

Vehicle dynamic model represents dynamic characteristicsof the vehicle. When an accelerator and a steering areinput, a position, posture, and speed at next time are outputcorresponding to the terrain information. In order to increasethe accuracy of the simulation, the test results of the actualvehicle is used and equivalent two-wheel model is adoptedfor dynamic properties of vehicle. Measurements of IMUand GPS sensors are simulated using current position andattitude without noise.

Vehicle control system performs a steering and a accel-eration. LOS (Line of Sight) algorithm [15] is used for thesteering. It determines the yaw rate toward the intersectionof a linear path and a circle around the vehicle.

III. PATH SELECTION BASED ON THE LOCAL TERRAINFEATURE

In this chapter, path selection based on the feature of localterrain is described. The method for extracting correlationbetween the local terrain feature and the value of the pathfrom simulator is described.

A. Outline of the proposed method

Outline of the proposed method is shown in Fig. 6. Themethod consists of online and offline processes. In Fig. 6,the left part and the right part represent the online and offlineprocesses, respectively.

Goal

Fig. 3. Exemplar situation. Fig. 4. Appearance of simulator.

SimulatorPC

Route information

Environmentaland Geographical information

Control command

Velocity and acceleration informationLocation and attitude information

Geographical information Environment model

Vehicle control system

Vehicle dynamic model

Fig. 5. Simulator configuration.

In the offline process, the path on simulator is representedby a node-link structure. The node-link structure set onsimulator is shown in Fig. 7. As shown in Fig. 6, currentposition of UGV is obtained by GPS. Runway informationaround the UGV is obtained and a path is extracted fromthe runway information. A local terrain feature is obtainedby these information. The value of path is calculated byDP (Dynamic Programming) [16] using the running timebetween nodes. The correlation between the value of path andlocal terrain feature obtained from each node is extracted.

In the online process, the local terrain feature is obtainedas with the offline process. Then, the value of path isestimated using the correlation and a local terrain feature.At a branch, the path is selected based on estimated values.

B. Path extraction

The flow of path extraction is shown in Fig. 8. Three-dimentional points obtained by the LRF are divided into agrids (Fig. 8(a)). Local shape of environment correspondingto each grid is approximated by a plane using LRF points

Offline Online

V*(s)

CCACorrelation

CorrelationValue estimationPath selection

DPg

Runway informationPath extraction

Current Position

f

GPS LRF

g

Runway informationPath extraction

Current Position

f

GPS LRF

Fig. 6. Outline of the proposed method.

Fig. 7. Node-link structure on simulator.

included in the region of the grid. In this study, variancein height and inclination of each plane is used for runwayfeature. Whether each region is travelable or not is deter-mined by linear SVM (Support Vector Machine) using thesefeature (Fig. 8(b)). SVM is constructed based on travelableinformation given manually in advance. In Fig. 8(b), whiteregions and red regions represent travelable regions anduntravelable regions, respectively. Black regions are gridswhich have few or no points for the plane approximation.The potential of the untravelable region on the horizontalangle P is calculated by following.

P =∑i,j

1√2πσ2

exp

{−(θ − θi,j)

2

2σ2

}(1)

Here, θi,j denotes an angle to the untravelable grid (i,j).The path directions where potential is locally minimal areextracted (Fig. 8(c)).

C. Node-link structure and Collection of the value of thestate

The travelable path on targeted outdoor uneven terrainenvironment is represented by node-link structure. The node-link structure is set manually. A node is placed on a midpointof the place before path branches as shown in Fig. 9.

The set of nodes is denoted by E and the total numberof nodes is denoted by N . There are two possible directionsof vehicle at each node. Let q1n and q2n denote directionsof at node n in the world coordinate, where q1n and q2nare set so that −π ≤ q1n < 0 and 0 ≤ q2n < π hold.The state of the vehicle is defined corresponding to thedirection of the vehicle at a node. That is, state s1n and s2ncorrespond to directions q1n and q2n at node n, respectively.Using this notation, the set of state is defined as S ={s11, s21, . . . , s1N , s2N}.

When node n is set to the goal, the values of all ofthe states are calculated by DP. Let V(n)(s) denote valueof state s ∈ S under the condition that the goal is set at

(a) Grids.@(b) Travelable regions. @(c) Path directions.

Fig. 8. Flow of path extraction.

Node Link

Fig. 9. Example of node-link structure.

node n. V(n)(s) represents accumulated expected reward asfollowing;

V(n)(s) =

T∑k=0

γkRk(s, s′), (2)

where γ denotes discount rate (0 < γ ≤ 1) and Rk(s, s′)

denotes reward of actions on transition from s to s′ at timestep k. In our problem, reward is defined by the time costrequired for transition from a node to an adjacent node,which is multiplied by −1. The durations for transitionsamong nodes are obtained by running the vehicle in thesimulation environment in advance, where LOS strategy withconstant velocity [15] is used for navigation between nodes.Note that direct transition from a node to an adjacent nodeis possible only with the same direction. In other words,transition between states whose directions are opposite is notpossible (at least, directly), even when the states are adjacentin the node-link structure. To take account of the constraint oftransition, R(s, s′) is set as R(s, s′) = −Rmax if directionsof the vehicle are opposite in state s and s′. The value ofthe states corresponding to node n are fixed as zero. Thatis, Vn(s2n) = Vn(s

2n) = 0. Vn(s) ∀s ∈ S are calculated for

n = 1, . . . , N .

D. Feature extraction from LRF observation

For representing the shape of the terrain of path whetherit is up or down, we designed a lower-dimensional featurecompared with LRF information. The flow of converting LRFinformation into the low-dimensional feature is shown in Fig.10. From LRF information that is observed at a node, featuresrelated to a terrain of path in forward direction of the vehicleis calculated. LRF information obtained in front range of180 degree (-90 to 90 degree) of vehicle is used. First, LRFinformation within the travelable area is extracted. Next, theregion of interest is determined by maximum and minimumvalue of vertical rotational angle ψ. Let fi denote averagedistance of LRF points from the sensor at i-θ divided space.Let division number of ψ space be M and the LRF featureis defined as follows.

f = [f1 . . . fM ]⊤ ∈ RM (3)

Path 2Path 1

Indistinguishable

Untraversable

(a) LRF observation.

Path 1

Untraversable

Indistinguishable

θ

ψRegion of interest

@(b) ψ-θ plane.

Fig. 10. Local terrain feature.

E. Correlation extraction between the value of path andterrain feature

The method for extracting the correlation between thevalue of path (described in section III-C), terrain featureamount (described in section III-D) and goal position isdescribed. Let rmn ∈ R and hmn ∈ R denote distance(in two-dimensional space) between and height differencebetween node m and n, respectively (see Fig. 11). The setof adjacent nodes which are connected to node m by a linkis defined as N (m). Now, let n denote a goal node. Foradjacent node l ∈ N (m) of node m, the direction of pathfrom node m toward node l in the reference frame of goalnode n is denoted by θlmn.

θlmn = |θmn − θml| (4)

Here, θmn represents horizontal angle of the vector fromnode m to node n in the world coordinate (see Fig. 12).Under the condition that node n is the goal, value of choosingpath l at node m can be represented by V(n)(s

i∗(l,n)l ), where

i∗(m,n) = arg mini=1,2

(|θmn − qim|). (5)

Note that i∗(m,n) ∈ {1, 2} represents direction of thevehicle on node m that is closer to vector from node mto node n.

Using the notations mentioned above, a vector represent-ing geometrical information of path l at node m in thereference frame of goal node n is defined by

glmn = [rmn hmn θ

lmn]

⊤ ∈ R3. (6)

Correspondingly, terrain feature vector for path l at node min the reference frame of goal node n is defined by f lmn ∈RM . By organizing these vectors, integrated feature vectoris defined by

xi = [gl ⊤mn f l ⊤

mn ]⊤ ∈ R3+M , (7)

where i denotes index of integrated feature vector. Theintegrated feature vectors are collected for all paths at allnodes and under reference frames of all goal nodes. Thus,the total number of the integrated feature vectors is denotedby Npath and given by follows.

Npath =N∑

m=1

|N (m)| ·N (8)

The value of path corresponding to xi is defined as follows.

yi = V(n)(si∗(l,n)l ) (9)

m

n

rmnhmn

Fig. 11. Position of goal.

m

n

lθmn θml

θ lmn

Fig. 12. Direction of path.

By organizing above, the data matrix is defined as follows.

X = [x1 . . .xNpath] ∈ R3+M×Npath (10)

Y = [y1 . . . yNpath] ∈ R1×Npath (11)

Canonical correlation analysis (CCA) [17] is performed bythese data matrices, and coefficients a ∈ R3+M and b ∈ Rare determined by maximizing the correlation ρ(a, b), whichρ is defined by

ρ(a, b) =a⊤SXYb√

a⊤SXXa√bSYYb

. (12)

SXX = 1Npath

X⊤X, SYY = 1Npath

Y⊤Y, and SXY =1

NpathX⊤Y are covariance matrices, where X and Y are

given by

X = X − 1

N

Npath∑i=1

xi[1 . . . 1], (13)

Y = Y − 1

N

Npath∑i=1

yi[1 . . . 1]. (14)

F. Path selection

In the online process, path selection is performed usingestimated value of path. The directions of path are extractedas with offline process. Let total number of directions atbranch be L and index of path be denoted by ipath. Integratedfeature xipath is obtained from travelable region closer tothe directions of path. By using determined correlation aand Integrated feature xipath , estimated value of path wipath

is given by wipath = a⊤xipath . The path is selected byfollowing.

ipath = arg minipath=1,...,L

wipath (15)

IV. EXPERIMENT

The proposed method was evaluated by comparing witha simple path selection method, which was implemented bychanging evaluation of path. Instead of the definition of xi

using (7), xi is defined without LRF feature as xi = glmn ∈

R3. This method is denoted by A: conventional method.The proposed method is denoted by method B. Then, weconfirmed whether there is a difference between A and B.

A. Experiment Conditions

The total number of nodes is set as N = 83. The totalnumber of path is set as Npath = 9378, and the number ofdimensions of LRF feature is set as M = 16. When eachnode is set as start and goal, the total number of pathway is83× 82 = 6806.

B. Experiment Results

When each node is set to be start and goal, the resultof pathways determined by methods A and B is shown inTable I. Success means that it was possible to reach the goalby selecting paths by the method. From Table I, it can beseen that success rate was improved by the proposed method.Causes of failures are 1) falling into dead-ends and 2) getting

stuck in the same loop. Contents of the failures are shownin Table II. From Table II, it can be seen that failure causedby dead-ends is not so much different from the conventionalmethod. There are dead-ends that are inherently difficult toavoid without map information, in general. Hence decreasingthis type of failure is not in the scope of our study.

An example of looping pathway is shown in Fig. 13. InFig. 13, path is branched to left and right from the start.In this example, right path is closer to the goal. Therefore,when the value is estimated using goal position only, rightpath was selected. Here, the goal is higher than the branchand the local terrain feature suggests that the right path isgoing down and the left path is going up. (see Fig. 14). Bytaking this information into account, left path, leading to thegoal, was selected.

TABLE IRESULT OF PATHWAY DETERMINED BY EACH METHOD

Results A BSuccess 5074 / 6806 5249 / 6806

Failure 1732 / 6806 1557 / 6806

TABLE IICOMPARISON OF THE TYPE OF FAILURE

Types A BDead-end 1543 / 1732 1529 / 1557

Loop 189 / 1732 28 / 1557

Start

Goal

Fig. 13. Example of a looping path.

Fig. 14. Terrain on simulator.

From same start and goal nodes on Fig. 13, the pathwaydetermined by proposed method is shown in Fig. 15. Thelooping pathway is determined using the value of pathestimated by the conventional method. In contrast, succeededpathway is determined by adding LRF feature as shown inFig. 15. From this fact, we confirmed that LRF feature isrelated to the value of path at shortest time pathway to thegoal.

V. NAVIGATION ON THE SIMULATOR

We verified the autonomous navigation using the pathselection based on the local feature amount on simulator.The directions of path are extracted and the autonomousnavigation was performed by the proposed path selection.The input speed was 0.5 m/s, radius of LRF measurementwas 25 m, size of field was 50×50 m, and size of grid was1×1 m.

The trajectory of autonomous navigation is shown in Fig.16. It shows that UGV succeeded to navigate autonomouslytoward the goal while avoiding untravelable region by theproposed method. In this result, UGV performed the pathselection at four junctions.

VI. CONCLUSIONS AND FUTURE WORKS

A. Conclusions

In this paper, we proposed a path selection method basedon local terrain features for autonomous UGV navigationin unknown outdoor rough terrain environment. A simulatorwas constructed based on the actual outdoor rough terrain en-vironment and the vehicle. By using the simulator, the valueof the path, goal position, and LRF feature were collected atthe path of shortest time. Then, the correlation between themwas calculated with CCA. The proposed method realizedmore effective path selection by considering the local terrainfeatures. In addition, it was confirmed that UGV could reachthe goal autonomously based on the proposed path selection.

B. Future Works

In the future, we will conduct experiment using theUGV in actual outdoor rough terrain environment. Further,duration was used for definition of the reward of DP. Thepath selection based on the cost considering the energy isthought to be a future work.

Start

Goal

Fig. 15. Example of succeeded pathway by the proposed method.

Start

Goal

Fig. 16. Trajectory of Traveling.

REFERENCES

[1] D. Caltabiano, G. Muscato, F. Russo: “Localization and self-calibrationof a robot for volcano exploration”, Proc. of IEEE International Con-ference on Robotics and Automation, Vol. 1, pp. 586-591, 2004.

[2] J. Carsten, A. Rankin, D. Ferguson, and A. Stentz: “Global Planningon the Mars Exploration Rovers: Software Integration and SurfaceTesting,” Journal of Field Robotics, Vol. 26, No. 4, pp. 337-357, 2009.

[3] K. Kurashiki, T. Fukao, et al.: “Orchard traveling UGV using particle fil-ter based localization and inverse optimal control”, Proc. of IEEE/SICEInternational Symposium on System Integration, pp. 31-36, 2010.

[4] S. Zhou, J. Xi, et al.: “Self-supervised learning to visually detect terrainsurfaces for autonomous robots operating in forested terrain”, Journalof Field Robotics, Vol. 29, pp. 277-297, 2012.

[5] S. Fish: “UGVs in Future Combat Systems”, Proc. of SPIE, UnmannedGround Vehicle Technology VI, pp. 288-291, 2004.

[6] Y. Fukuoka, K. Hoshi, R. Kurosawa: “Negotiating Uneven Terrainwith a Compliant Designed Unmanned Ground Vehicle Equipped withLocomotive Master-Slave Operation”, Journal of Field Robotics, Vol.30, pp. 349-370, 2013.

[7] K. Zhao, Y. Li: “Path Planning in Large-Scale Indoor EnvironmentUsing RRT”, Proc. of 32nd Chinese Control Conference, pp. 5993-5998, 2013.

[8] B. Yamauchi: “Autonomous Urban Reconnaissance Using Man-portableUGVs”, Proc. of SPIE Conference on Unmanned Systems TechnologyVIII, 2006.

[9] T. Braun, H. Schaefer, K. Berns: “Topological Large-scale Off-roadNavigation and Exploration RAVON at the European Land RobotTrial 2008”, Proc. of IEEE/RSJ International Conference on IntelligentRobots and Systems, pp. 4378-4392, 2009.

[10] S. Thrun, M. Montemerlo, et al.: “Stanley: The Robot that Won theDARPA Grand Challenge”, Journal of Field Robotics, Vol. 23, pp. 661-692, 2006.

[11] K. Iagnemma, S. Shimoda, Z. Shiller: “Near-Optimal Navigation ofHigh Speed Mobile Robots on Uneven Terrain”, Proc. of IEEE/RSJInternational Conference on Intelligent Robots and Systems, pp. 4098-4103, 2008.

[12] J. Latombe: “Robot Motion Planning”, Springer, 1991.[13] T. Saitoh, M. Sanpei, Y. Kuroda: “Effective Strategy for Autonomous

Navigation without Prior Knowledge in FastSLAM”, Proc. of 2009IEEE Workshop on Robotic Intelligence in Informationally StructuredSpace, 2009.

[14] http://unity3d.com/[15] T. I. Fossen: “Marine Control Systems: Guidance, Navigation and

Control of Ships, Rigs and Underwater Vehicles”, Marine Cybernetics,2002.

[16] S. Thrun, W. Burgard, D. Fox: “Probabilistic Robotics”, MIT Press,2005.

[17] R. S. Sutton, A. G. Barto: “Reinforcement Learning: An Introduction”,MIT Press, 1998.