Target-tracking and path planning for vehicle following

in jungle environment

Cheng Chen, Han Wang, *Ng Teck Chew, *Javier Ibañez-Guzmán,*Shen Jian,+Chan Chun Wah

School of Electrical and Electronic Engineering, Nanyang Technological University50 Nanyang Drive, Singapore 639798

{pg01768970;wh}@ntu.edu.sg*Singapore Institute of Manufacturing Technology, Singapore 638075

{tcng;javierg;jshen}@SIMTech.a-star.edu.sg+Defence Science and Technology Agency, Singapore

cchunwah@dsta.gov.sg

Abstract

In this paper, we proposed a robot vehicle follow-ing algorithm which can navigate a 10 ton armoredpersonnel carrier to follow a leading vehicle(MPV)in the jungle. This algorithm comprises two compo-nents, the first one is a target tracking module whichcan detect and track the lead vehicle from the mea-surements of a SICK laser scanner; the second one isa obstacle avoidance module which takes the track-ing results and the local environment description asinput, it then generates the set-points for the vehicleto follow. Plenty of trials have been carried out inthe jungle of Singapore, our technique’s validity androbust is demonstrated and tested, the results willbe showed in this paper.

1 Introduction

Autonomous vehicle has received widely attention inthe past several years, a lot of research work has beencarried out to provide the vehicles with capability tonavigate by itself. The potential applications of au-tonomous vehicle range from daily life to military.Kuan et.al.[1] implemented an autonomous road fol-lowing vehicle for military purpose, using stereo vi-sion as the primary sensor. The road boundariesare segmented from the images and then set pointsare generated. Compared with such high-end imple-mentation, Lee[2] used a single SICK laser scanneras exterior sensor to lead its small robot to follow aperson, his work also shows the capability to avoidobstacles. The following technique can also be usedin intelligent transportation system, Piao[3] imple-mented his car-following algorithm on automobiles,and thorough experiments were carried out in severalcities, both in urban and suburb. Generally, vehiclefollowing can be regarded as a special situation ofkeeping vehicle formation. Such formation can be ofgreat value for military purpose, e.g. platoon: vehi-cles can follow each other at very close distance anda single driver can convey several vehicles.

The implemented system is part of a project on

autonomous unmanned ground vehicles (AUGV) fornatural environments for operation in day and nightconditions. The system consists of four subsystemsnamely: vehicle control, piloting, visual guidanceand tele-operation. The vehicle following functionis implemented as part of a module within the pi-loting sub-system. It consists mainly of a 2D timeof flight(TOF) laser sensor; a GPS and an industrialPC computer for processing purposes. There is an-other GPS on board the leader vehicle; it transmitsits location continuously via a RF-modem. Thesedata is used as an initial guess when the leader vehi-cle is no longer within the field of view (FOV) of thelaser sensor on board of the follower vehicle. A targetdetection algorithm is being used based on a geomet-ric model of the leader vehicle. The estimated globalposition of the leader vehicle was determined usingGPS data in the follower vehicle, these are used astarget points that the follower vehicle has to attain.For this purpose the path planner treats each vehicleposition received as a moving target to be attainedand uses its obstacle avoidance capabilities to ensurethat the path is obstacle free.

The paper describes in detail the combined ap-proach taken and presents experimental results of thevehicles evolving in the jungle of Singapore. Section2 gives full details of the system architecture and thevehicle detection model. The combined approach forvehicle following and the tracking algorithm are de-scribed in detail in Section 3. The field results arepresented and discussed in Section 4. Finally, section5 concluding remarks and directions for future workare presented.

2 System architecture

The main challenge addressed in this paper is the safecontrol for the vehicle following function of a largetracked follower vehicle (around 12 tons) to followa leader vehicle in a natural environment. The fol-lower vehicle has been converted into a drive-by-wiresystem. It has a vehicle control unit to ensure the

dynamic response of the vehicle despite its high non-linear characteristics. The working environment forboth vehicles is an equatorial forest(see Fig.7). Theterrain is covered by trees and bushes with open ar-eas either covered by light vegetation or non-tarmacroads. Climatic conditions can change suddenly andthe terrain will become muddy or dusty very easily.

The primary sensor used to track the leader isa COTS 2D laser scanner from SICK GMBH. Thistype of 2D sensor has a major limitation: it can losetrack of the leader vehicle on an uneven terrain. Tocompensate for this a GPS unit is mounted on theleader vehicle. The GPS system complements thelaser scanner if the leader vehicle is out of sight ofthe laser scanner, else , the information is discarded.

The presence of natural obstacles like trees,branches, bushes, rocks, etc are common in thetest site, and the passages are very narrow, whichleaves very little room for tracking errors. So thevisual guidance capabilities of the AUGV are usedtogether with the obstacle avoidance/path-planningmodule to negotiate all these obstacles based on theresulting obstacle map. In order to take into ac-count the vehicle dynamic constrains such as max-imum acceleration and turning rates, the dynamicadaptor described in[4] is used to generate setpoints.Fig.1 shows the setup of the vehicle following system,which comprises several components for the leaderand follower vehicles.

Industrial PC

SICK

MOXA

RS422

GPS satelliteGPS Reveiver

transmiter

Comm. Tower

Radio Modem

Radio Modem

RS232

Ethernet

PositioningModule

NavigationModule

Environment

Vehicle Followingmodule

Leadvehicle

Other Modules

Leadingvehicle

Figure 1: The hardware architecture of vehicle fol-lowing module

2.1 The Leader and following Vehicle

The leader vehicle is a multi-purpose vehicle(MPV)1.8m (W), 1.8m (H), 3m (L). A GPS receivertogether with an RF-modem are mounted onboard.The RF-transmitter transmits the leader position tothe follower vehicle. There is no transmission of ve-locity or heading data. It is assumed that the leadervehicle has a constant velocity with white accelera-tion noise as part of the approach.

The follower vehicle is a 12-tons tracked Logis-tics Armored Ambulance Carrier. It is equippedwith a positioning module, which comprises a looselycoupled GPS-IMU for vehicle localization. Severalmodules are used for autonomous operations, how-ever, for the vehicle following function only the Vi-sual Guidance Sub-System is of concern, togetherwith the path-planning and obstacle avoidance unit.The Visual Guidance System(VGS) includes a pairof trinocular stereo-vision sensors and a pair of 2Dlasers range scanners, their data is fused into an oc-cupational map that represents the scene in frontof the vehicle. A RF-receiver is used to receive theleader’s position. The data-flow within the vehiclefollowing system is showed in Fig.2. The two mainsub-modules will be detailed in following two sec-tions.

VehicleControl Unit

Path Planner &Obstacle

Avoidance

VehicleLocalization

Segmentation

Tracking andRetrieving

Global position

Heading/speed

TargetPosition

Odometry &GPS

Laser scanner

Vehicle Detection& Tracking

Vehicle Following &obstacle Avoidance

GPS on thelead vehicle

Lead position

Figure 2: Data flow in VFM

3 Vehicle Detection and Track-

ing Module

A 2D SICK laser range sensor running at the speedof 0.1 second/frame is used to detect the leader ve-hicle. For every scan, the following algorithms areexecuted to detect the leader vehicle: Fig.3(a) andFig. 3(b) shows a view of typical laser scans: (a)is the raw range measurements, for convenience, weonly display the data within 30 meters; (b) is themeasurements in Cartesian coordinate, several roadsegments are similar to the lead’s shape.

0 50 100 150 200 250 300 3500

5

10

15

20

25

30

measurement ID

rang

e (u

nit:

m)

The learer vehicle

(a) The raw range se-quence

−10 −5 0 5 10 15 200

5

10

15

20

25

30

local x (unit: m)

loca

l y (u

nit:

m) Lead vehicle

(b) In local coordinate

Figure 3: A typical frame of observation.

3.1 Segmentation

A pre-processing process that filters out noise inthe laser scan, a discontinuity check was also im-plemented to segment out the potential objects andnoise. Consecutive laser readings are checked againstits neighbors for discontinuities, where the differencein range reading exceeds a certain threshold value.As the laser scanning resolution is only 0.5 degrees,few laser returns are expected when the leader vehi-cle is at more than 20 meters. It is therefore impor-tant for the tracking distance to fall within 20 meterswith respect to the laser scanner position.

3.2 Vehicle Identification

Since we know the rear part of the MPV beingtracked, and its size is time-invariant, we can searchall the potential segments to find the true one.Hough transform and line fitting can all be used forthis purpose, however, we found that the MPV onlyreflects very limited laser beams, so a total of 3 in-dicators are sufficient to identify the leader vehicle.They are:

1. Length of a segment: The length of the poten-tial lines after segmentation is computed by firstprojecting each segment into a Cartesian coor-dinate using standard Polar-to-Cartesian trans-formation equations. The length of a segment

is then computed as the sum of the distance be-tween the neighbouring points throughout thesegment. The computed length of the targetsegment should equal to the width of the lead-ing vehicle. Due to the laser angle resolution,some tolerance is included.

2. Total range points in a segment: Since the widthof the leading vehicle is time invariant, the to-tal number of laser points reflected from theleader vehicle can be estimated based on theinter-vehicle distance.

In the ideal scenario, these 2 indicators can becomputed by simple geometrical calculation. In thetrial, the difference between the ideal values and theobserved ones are referred as the similarity of a seg-ment. A threshold acquired from experiments areused to filter out those dissimilar segments. The leftwill processed by tracking algorithm, as explainedlater.

3.3 Vehicle Tracking and retrieving

The tracking of the leading vehicle is done in theglobal coordinate. The observation is first projectedto the north-east coordinate according to the vehi-cle’s on-board localization, by a simple coordinatetransformation, as showed in Fig.4. We assume thatthe lead vehicle moves in a constant speed, and usea Kalman Filter to predict the lead’s position. Thevalid searching window for new observation is com-puted from the KF’s prediction. If more than one ob-jects fall into this window, we simply use the Nearest-Neighbor(NN) approach to find one as the real leadvehicle. The techniques used here can be found in [5].As the system is deployed in natural environments

Figure 4: The local coordinate and global coordinate.(XE ,XN , φ) are read from the GPS/IMU; γ and βare computed from the identification routing.

and only a single scan laser is used for tracking pur-poses, a loss of the track of the leader vehicle is com-mon due to the pitching effect of the follower vehicle,that is, when the road is uneven, the sensor mountedon the vehicle may shake up and down, which makesthe scan plane unparallel to the ground. To take

into account this uncontrollable effect, a target re-tracking algorithm has been developed. It consistsof two stages: self-retrieval and remote GPS aidedretrieval, as showed in Fig.5.

Normal

Tracking

Target

lost?

Self-

Retrieving

routing

Y

Temporary target

Confirmed?

Y

N

Time Out?

N

N

Using

Remote GPS

Y

Confirmed?

Temporary

target

Y

N

Figure 5: The retrieval routing used to re-localizethe lead vehicle

Under normal conditions where the leader vehi-cle is within line of sight of the follower vehicle, theself-retrieval method is used. This method runs thevehicle identification algorithm to map out the mostprobable leader vehicle based on the model describedbefore. If the vehicle identification algorithm fails toretrieve the leader vehicle for a few attempts, remoteGPS data from the leader vehicle will be used as theglobal position of the leader vehicle until this is againin the FOV of the laser. The result of the trackingis the global navigation position (North and East)of the leader vehicle, information passed to the pathplanner module for the vehicle following task.

3.4 Compensating the compass error

The vehicle heading is estimated by the on-boardcompass, this estimate is of great importance intracking module above. Unfortunately, the interiorsensors themselves cannot measure the heading ac-curately. Here we use the laser scanner’s data to cor-rect the raw estimate from interior sensor. A match-ing approach is first developed, this method find themost likely correspondence between two successiveframes of range data. These associations can thenbe used to find a translation of the sensor(vehicle)in least square manner, as explained by Lu[6]. Thiscalculated motion also comes with a variance esti-mate, which inspire us to use Maximum A Posteriorto fuse the two set of estimates from compass andlaser scanner.

3.5 Path Planning and Obstacle

Avoidance

Several the path planning’s components are used alsoto provide the autonomous navigation functionality.

1. Positioning Module: It provides position and at-titude information of the vehicle in real-time ata rate of 10Hz. So an INS is developed using amedium-cost Inertial Measurement Unit (IMU)in conjunctions with a GPS device that boundsIMU originated drifts. A Kalman filter is usedfor position estimation and corrections. It pro-vides the ground truth with respect to which allposition estimations are made.

2. Sensor Fusion Module: This module fuses therange data from the laser scanners and trinocu-lar stereovision components. It outputs an oc-cupational map (obstacle map) at a rate of 5Hzto the navigation module for path planning andobstacle avoidance.

3. Navigation Module: This module continuouslyreceives the estimated position of the leading ve-hicle from Vehicle Detection and Tracking Mod-ule at 10 Hz with respect to the global naviga-tion frame. At the same time, it also receivesdata on the vehicle position plus environmen-tal information in the form of a local obstaclemap. The location of the leader vehicle is esti-mated by mapping the information from the ve-hicle detection and tracking module with respectto the global navigation frame and therefore it isconsidered as the global way point for the pathplanner. Based on the above information, thepath-planer generates the shortest obstacle freepath to the leader vehicle, as well as the attain-able heading and speed for the follower vehicleto achieve the vehicle following purposes. Thevehicle’s dynamic constraints are also incorpo-rated . There is also an algorithm to maintain aconstant longitudinal distance between the twovehicles, performing speed control of the fol-lower vehicle. The path planner will stop thefollower vehicle if their distance is less than 8mfor safety purposes.

3.6 Local path-planning for Vehicle

Following

The path planner operates also at 5 Hz. A hy-brid algorithm that combines the Modified DistanceTransform (MDT)[7] with a Vector Field Histogram(VFH)[8][9] is used for path planning and obstacleavoidance to achieve vehicle following. It should benoted that our algorithm is also used to achieve ve-hicle’s autonomous motion when moving from pointto point. This hybrid algorithm estimates the head-ing of the follower vehicle by MDT, and modifies the

heading with the VFH, by doing so, we acquire bothMDT’s efficiency and VFH’s safety, and eliminatethe well-known oscillation effect of VFH.

4 FIELD TEST RESULTS

During implementation, initially the leader vehiclewas followed by a light pickup(driven manually) inorder to assess the robustness of the algorithms.Since it’s time consuming to carry out a trial inreal jungle environment, we set up simulation pro-gram which uses the data collected to test our al-gorithm’s performance, after satisfying performanceis achieved, the entire system was transplanted ontothe Logistics Armoured carrier, and a set of trials areconducted at a testing field that comprises bushes,tropical trees and non-paved terrain. Fig.6 shows thelocal obstacle map used by the path planner. It canbe observed that the planner generates the red linepath rather than the black dotted line path whichcuts across some obstacles as it tries to move the ve-hicle using the shortest distance. This is because theplanner takes into account the presence of obstacles.The planner needs to know only the current locationof the leader vehicle instead of its entire path.

Output path (RED)

Leader vehicle Nearest local target to

Leading vehicle

Obstacles

Undesired path (Black dotted)

Pre-setlocal targets

Output path (RED)

Leader vehicle Nearest local target to

Leading vehicle

Obstacles

Undesired path (Black dotted)

Pre-setlocal targets

Figure 6: A frame of local map.

Fig.7 shows the leader vehicle traveling at the testsite, for confidentiality reasons, it is difficult to shownthe follower vehicle. Both vehicles have been run atthe test site, traveling distances up to 2 km withthe leading vehicle moving up to a maximum speedof 15km/h. Fig.8 shows the path of the leader ve-hicle and the follower vehicle. Images in Fig.7 arecaptured at various time during the traverse showedin Fig.8, The stars indicate the resulting trackingpath by the follower vehicle as recorded by the on-board positioning module. The dots are the calcu-lated global position of the leader vehicle based onthe laser scanning data. The number-labels indicatethe relative time index from starting point.

(a)

(b)

(c)

(d)

Figure 7: The testing field and the lead vehicle, these4 pictures are captured by cameras mounted on thefollower.

The plots show that the follower vehicle is ableto follow the trajectory of the leader vehicle success-fully despite the unevenness of the terrain, presenceof dense foliage across tracks, and typical conditionsfound in jungle-like environments. It should be re-marked that the discontinuous segments during thepath following task, indicate that the follower vehi-cle has lost line of sight of the leader vehicle. This isdue to the strong unevenness of terrain, which causesthe laser to lose track of the leader vehicles. Underthese conditions, the system relies on GPS informa-tion from the leader vehicle to continue with the ve-hicle following.

At points 1 to 4, where the terrain is very irregu-lar, the follower vehicle is totally dependent on thepositions retrieved from the leader vehicle. Fig.9shows the zoom in view of the tracking at time index19. It can be observed from the path that the followervehicle is not actually responding to the estimatedposition of the leader vehicle. This is caused by theslow dynamic response of such skid-steer heavy vehi-cle. Due to the presence of dense foliage and henceblocking of GPS signals plus multi-path effects, GPSdata from the leader vehicle is not very reliable, thereare occasional position jumps. In these scenarios, thetracking will fail if the system depends on GPS alone.Finally, results have shown that by using our obsta-cle avoidance path planning algorithms, the systemis able to generate its own path for following when ithave perceived that the leading vehicle is making agradual turn, this is a result of obstacle negotiation

0 100 200 300 400 500 600 7000

50

100

150

200

250

300

350

400

24 6

8

9

11

13

14

16

18

19

21

23

24

2628

29

31

Easting (m)

No

rth

ing

(m

)

Msg sent outonboard GPSremote GPS

Figure 8: The crosses represent the path leader ve-hicle’s path as recorded by the GPS on the leadervehicle.

(Fig.7(c)).

430 440 450 460 470 480 490 500 510

5

10

15

20

25

30

35

40

45

50

19

Easting (m)

No

rth

ing

(m

)

Msg sent outonboard GPSremote GPS

Figure 9: Zoom in view of the tracking path, some-where around instance 19.

5 Conclusion

A novel approach to solve the vehicle following prob-lem has been developed and tested in a natural en-vironment. By combining the vehicle identificationand tracking together with the path planner algo-rithms, the system is able to follow the trajectoriesof the leading vehicle closely whilst avoiding any po-tential obstacles. It has been found that using asingle line laser alone increases the dependency onthe use of GPS data from the leader vehicle. Bothsensors have their limitations when operating in anuneven terrain and under heavy tree foliage. By de-ploying such a system in natural terrain, several is-sues have been solved and others discovered experi-mentally. The combination of a proven path-plannerand obstacle avoidance algorithm that includes thevehicle dynamics has proved to be very effective to-gether with the vehicle tracking algorithms. Cur-

rently, the system still dependent a lot on the GPSsystem mounted on the leader vehicle when the ter-rain for following is highly uneven. To increase therobustness, research is carried on to incorporate in-formation from the stereo-vision sensors, and a true3D laser is also under development.

Acknowledgment

This project, Ulysses, was sponsored by Defence Sci-ence Technology Agency (DSTA), Singapore.

References

[1] D. Kuan, G. Phipps, and A.-C. Hsueh, “Au-tonomous robotics vehicle road following,” IEEETrans. on Pattern Analysis and Machine Intelli-

gence, vol. 10, no. 5, pp. 648–658, 1988.

[2] C. Y. Lee, H.Gonzalez-Banos, and J.C.Latombe,“Real time tracking of an unpredicatble tar-get amidst unknow obstacles,” in Proc. of In-ternational Conference on Control, Automation,

Robotics and Vision, (Singapore), pp. 596–601,Dec 2002.

[3] J. Piao and M. McDonald, “Low speed car fol-lowing behaviour from floating vehicle data,” inProc. of the IEEE Intelligent Vehicles Sympo-

sium, pp. 462–467, June 2003.

[4] M. Adams, G. J. Ibanez, and H. Wang, “Safepath planning and control constraints for au-tonomous goal,” in Proc. of International Con-ference on Intelligent Robotics and Systems,(Lausanne, Switzerland), Oct 2002.

[5] Yaakov.Bar-shalom and Thomas.E.Fortmann,Tracking and data assciation. London: AcademicPress, 1988.

[6] F. Lu, Shape registration using optimization formobile robot navigation. Ph.d thesis, The Uni-versity of Toronto, 1995.

[7] C. Y. Tuck, H. Wang, T. L. Phuan, H. Wang,and W. Soh, “Vision guided agv using distancetransform,” in Proc. of International Symposiumon Robotics, (Seoul), pp. 1416–1421, April 2001.

[8] J.Borenstein and Y.Koren, “The vector fieldhistogram-fast obstacle avoidance for mobilerobots,” IEEE Trans. on Robotics and Automa-tion, vol. 7, no. 3, pp. 278–288, 1991.

[9] A.Tay, S. Jiang, G. J. Ibanez, and C.W.Chan,“Autonomous vehicle navigation strategies-localized navigation with a global objective,” inProc. of International Conference on Informa-

tion Technology and Application, 2002.