autonomous navigation of ugv based on ahrs, gps and lidar

Autonomous Navigation of UGV Based on AHRS,GPS and LiDAR

John Liu

Advisor:

Professor Ying-Jeng Wu

Measurement Laboratory, National Yunlin University of Science & Technology

March 12, 2014

John Liu Auto. Navigation of UGV Based on AHRS, GPS and LiDAR

Outline

1 Introduction

2 Hardware Structure

3 Path Planning and Control Rules

4 Experiments

5 Conclusion and Suggestions


Motivates Purpose

Part I

Introduction


Motivates Purpose

Table of Contents

1 Motivates

2 Purpose


Motivates Purpose

2005 DARPA Grand Challenge


Motivates Purpose

Winner: Stanley - Stanford Racing Team


Motivates Purpose

2007 DARPA Urban Challenge


Motivates Purpose

Winner: Boss - Tartan Racing


Motivates Purpose

Google Self-Driving Car - 2009


Motivates Purpose

Google Self-Driving Car - 2014


Motivates Purpose

Yun-Trooper


Motivates Purpose

Yun-Trooper II


Motivates Purpose

Improvements

The Yun-Trooper II offers some improvements over Yun-Trooper:

Smaller, Lighter, Faster

Obstacle Avoidance with LiDAR

Remote Monitoring


Motivates Purpose

Table of Contents

1 Motivates

2 Purpose


Motivates Purpose

Purpose


Introduction Hardware

Part II

Hardware Structure



Table of Contents

1 Introduction

2 Hardware



Hardware Structure



Table of Contents

1 Introduction

2 Hardware



Table of Contents

1 Introduction

2 HardwareComputingSensorsDrive SystemPower SupplyCommunication



BeagleBone Black with Linux



Table of Contents

1 Introduction




Xsens MTi-G

The MTi-G is an integrated GPS and IMU Attitude and HeadingReference System sensor. The internal low-power signal processorruns a real-time Xsens Kalman Filter providing inertial enhanced3D position and attitude estimates.



HOKUYO URG-04LX-UG01

URG-04LX-UG01 is a low-cost laser sensor for area scanning.

Source semiconductor(λ = 785nm)

Input Vol. 5V DC ±5%(USB Power)

Input Cur. 500mA(800mA max)

Distance 20mm∼4000mm

Distance Res. 1mm

Scanning Range ±120 ◦

Angular Res. 0.36 ◦

Sampling Rate 10Hz



Table of Contents

1 Introduction




Driving Motor

Steering Servo

Driving DC Motor

DC Motor Driver



Table of Contents

1 Introduction




LM2596 DC-DC Power Converter

Yun-Trooper II requires 3 different power voltages. The LM2596switch power converter provides adjustable output voltage andhigh output current (3A), which is suitable for Yun-Trooper II.



Table of Contents

1 Introduction




XBee PRO

XBee PRO provides Long communication range (Up to 90m inurban area), compare to the Bluetooth on cellphone.


Introduction Target Angle Calculation Algorithm of Obstacle Avoidance Algorithm of Speed Control Rule

Part III

Path Planning and Control Rules



Table of Contents

1 Introduction

2 Target Angle Calculation

3 Algorithm of Obstacle Avoidance

4 Algorithm of Speed

5 Control Rule



Path Planning



Flow Chart of Navigation Algorithm



Table of Contents

1 Introduction




5 Control Rule



Table of Contents

1 Introduction

2 Target Angle CalculationGeographic Coordinate SystemGeodesicLocal Coordinate System



5 Control Rule



Geographic Coordinate System

Geographic coordinate system is a reference system used todescribe a position on earth. There are two kinds of such system:

ECI

ECEF



ECEF Ellipsoidal Coordinates

ECEF ellipsoidal coordinates are the most common coordinatesystem in describing a position on earth, which defined by Latitudeφ, Longitude λ and Altitude h.



Datum

Different definition of ellipsoid also changes the coordinate system.The ellipsoid used to define the earth is called a datum.

NAD27

NAD83

WGS84

. . .



WGS84 Datum

WGS84 (World Geodetic System 1984) is the standard ellipsoidalcoordinate system used by MTi-G position sensor and most of theGPS.

a 6378137m

b 6356752.3142m

f = (a− b)/a = 1/298.257223563



Table of Contents

1 Introduction




5 Control Rule



Geodesic

The shortest path between two points on the earth, customarilytreated as an ellipsoid of revolution, is called a geodesic. Twogeodesic problems are usually considered:

1 Direct

2 Inverse



Inverse Problem

The relative distance and direction between two location isrequired for autonomous navigation, therefore inverse problem isconsidered in the algorithm.

GeographiLib Library



Table of Contents

1 Introduction




5 Control Rule



Local Tangent Plane

Local tangent plane is the reference system of AHRS.



Target Angle

By the definition of azimuth α1 and yaw ψ, the target angle Θt

relative to robot could be determined:



Table of Contents

1 Introduction




5 Control Rule



Table of Contents

1 Introduction


3 Algorithm of Obstacle AvoidanceVector Field HistogramVector Field Histogram PlusDiscussion and Improvement of VFH+


5 Control Rule



Vector Field Histogram (VFH)

VFH generates a polar histogram of the environment around therobot, identifies wide-enough spaces and calculates correspondingsteering direction.




A cost function G is then applied to every candidate directions, andthe direction which generates the smallest value is then selected:

G = u1 · α+ u2 · β + u3 · γ

where

α = difference between target and candidate direction

β = difference between current direction and candidate direction

γ = difference between previously direction and candidate direction

u1, u2 and u3 are weighting constants




Advantages:

Easily adapt to the data acquired by LiDAREfficient CalculationAdjustable characteristic

Disadvantages:

Ignore the kinematic and dynamic constraintsIgnore robot’s geometryDirection depends on free-spaces



Table of Contents

1 Introduction




5 Control Rule



Vector Field Histogram Plus (VFH+) - Introduction

VFH+ algorithm is an enhanced version of original VFH whichoffers several improvements:

1 Kinematic constraints

2 Robot’s geometry constraints

3 Direction no longer depends on spaces



VFH+ - Four-Stage Process

The VFH+ employs a four-stage data reduction process in order tocompute the new direction of motion:

1 Primary Polar Histogram

2 Binary Polar Histogram

3 Masked Polar Histogram

4 Selection of Steering Direcion



VFH+ - with LiDAR

However, some modification is required in order to implementVFH+ with laser range finder, therefore the process become:

1 Primary Polar Histogram

2 Identifying Free Spaces

3 Blocked Directions

4 Selection of Steering Direction



1: Primary Polar Histogram

A polar histogram Pi of corresponding measured distance andangle di can be generated with following formula:

Pi = a− b · di



2: Identifying Free Spaces - Boundary Vector

Both VFH and VFH+ try to identify free spaces V - spacescapable for the robot to pass through, by different method. Eachfree space Vj is defined by two boundary vectors (BL,BR)j :

BL =[θl dl

]BR =

[θr dr

]



2: Identifying Free Spaces - Hysteresis Filter

VFH+ uses two thresholds τmax and τmin instead of singlethreshold τ in VFH to generate a Binary Histogram Hi, identifyingall the free spaces.

Hi =

1 if Pi ≥ τmax0 if Pi ≤ τminHi−1 otherwise



2: Identifying Free Spaces - Hysteresis Filter

By hysteresis filter, VFH+ has reduced the number of free spaces,which overcomes the frequent oscillations of VFH in narrow indoorenvironment.



2: Free Spaces - Robot’s Geometry

With geometry constraints, free spaces with shrinked boundariesV̂j = (B̂L, B̂R)j of each Vj is calculated:

B̂L =[θl − δl dl cos δl

]B̂R =

[θr + δr dr cos δr

]where

δl = arcsin(wsdl

)

δr = arcsin(wsdr

)



3: Blocked Directions

VFH+ takes the minimum radius of rotation of robot into account,determines the limitation of steering angles φr and φl.



3: Blocked Directions - Detection Histogram

In order to calculate φr and φl, the detection histogram Di isgenerated first:

Di = |Rs sin θi|+√R2s sin2 θi + w2

s + 2Rsws



3. Blocked Directions - Masked Histogram

The masked histogram Mi = di −Di shows whethter the steeringangle is blocked by obstacles.



3. Blocked Directions - Determine φr and φl

φr and φl can be efficiently found by following method:

1) Initially set φr = −π and φl = π

2) For every Mi < 0:

a) If θi < 0 and θi > φr , set φr to θi

b) If θi > 0 and θi < φl , set φl to θi



4. Selection of Steering Direction - Width of Free Spaces

According to the width of each free space V̂j , single or multiplecandidate directions β could be found. The width of a free space isdetermined by its spanning angle ε = θl − θr and a threshold τa,which has 3 kinds of situation:



4. Selection of Steering Direction - Candidate Directions

ε < 0 represents a free space with overlapped boundaries, which isabandoned.

No candidatedirection!




For a free space with 0 ≤ ε ≤ τa, the centered direction is the onlycandidate direction.

βn = θl+θr2




For a free space with 0 < τa < ε, there are 2 or 3 candidatedirections.

βr = θr

βl = θl

If θl < Θt < θr,βT = Θt



4. Selection of Steering Direction - Cost Function

Like VFH, VFH+ also uses a cost function to select the preferreddirection βt:

G(β) = µ1 · (|β −Θt|) + µ2 · |β|+ µ3 · (|β − βt−1|)

and

βt = min {G(c)}

where

Θt = Target direction

β = Candidate directions

βt−1 = Previously selected direction



Table of Contents

1 Introduction




5 Control Rule



Hysteresis Filter

Wrong βt!



Hysteresis Filter - Missing Boundaries

Hi =

1 if Pi ≥ τmax0 if Pi ≤ τminHi−1 otherwise



Hysteresis Filter - One Direction



Hysteresis Filter - Another Direction

H ′i =

1 if Pi ≥ τmax0 if Pi ≤ τminH ′i+1 otherwise



Hysteresis Filter - Combining

H ′′i = Hi OR H ′i



No Candidate Directions

Collision prediction is the indicator of navigation, not candidatedirections!



Boundary Miscalculation of Free Spaces



Histograms of the Environment



Measured Boundary



Actual Boundary



Compensation

In order not to affect the efficiency, only the closest measureddistance is considered for the compensation.



Table of Contents

1 Introduction




5 Control Rule



Table of Contents

1 Introduction



4 Algorithm of SpeedObstacle DensityObstacle Approaching RateCollision Prediction

5 Control Rule



Obstacle Density Function

VFH uses Obstacle density function D to calculate the speed ofrobot in the environment:

D(di) = 1− 1

N

N∑i=1

didmax

The value of D lies between 0 and 1. Therefore, defined amaximum speed vmax and minimum speed vmin, the speed ofrobot in the environment v could be determined:

v = vmin + (1−D(di)) · (vmax − vmin)



Obstacle Density Function

D = 0.01 D = 0.49



Table of Contents

1 Introduction




5 Control Rule



Obstacle Approaching Rate

Speed controlled by D considered only current environment, whichis insufficient for high speed robot. Therefore, Obstacleapproaching rate δ is introduced:

δ = − 1

M

∑j

(dj)t − (dj)t−1T




For high speed robot, the ability to decelerate while approachingan obstacle with high speed is critical. Therefore only rate ofapproaching is considered:

δa = − 1

M

∑j

∆((dj)t − (dj)t−1)

T

where

∆(d) =

{d if d < 0

0 otherwise




In order to integrate obstacle approaching rate with obstacledensity, normalized by maximum speed vmax is required:

δn = − 1

M · vmax

∑j

P ((dj)t − (dj)t−1)

T

And the speed v becomes:

v = vmin + (1− (D(di) + δn)) · (vmax − vmin)

To accompolish smooth travelling speed, value of D(di) + δn islimited under 1.



Table of Contents

1 Introduction




5 Control Rule



Collision Prediction - First Stage

The first stage predicts collision with geometry of robot.



Collision Prediction - Second Stage

The second stage predicts collision on the steering direction withdistance dc.



Table of Contents

1 Introduction




5 Control Rule



Control Rule

if βt is available thensteer ← K · βtspeed ← v

elsesteer ← K · βt−1speed ← vmin

end ifif collision predicted then

speed ← 0end ifsetCommand Steer(steer)setCommand Speed(speed)


Path Planning Experiments Navigation Experiments

Part IV

Experiments



Table of Contents

1 Path Planning Experiments

2 Navigation Experiments



Path Planning Experiments - Env.

Recording Period: 0.5s



Candidate Angle Compensation

Time: 9s Time: N/A



Obstacle Approaching Rate Compensation

Time: 9s Time: 8s



Boundary Miscalculation Compensation - Env.



Boundary Miscalculation Compensation



Table of Contents

1 Path Planning Experiments

2 Navigation Experiments



Navigation Experiments



Navigation Experiment 1



Navigation Experiment 2


Conclusion Suggestions

Part V

Conclusion and Suggestions



Table of Contents

1 Conclusion

2 Suggestions



Conclusion

1 Yun-Trooper → Yun-Trooper II

2 Path planning - GPS and obstacle avoidance

3 Improvement of VFH+ algorithm

4 Obstacle approaching rate



Yun-Trooper → Yun-Trooper II

GPS and LiDAR

BeagleBone Black

GNU/Linux



Conclusion







Improvement of VFH+ algorithm

Hysteresis Filter

No Candidate Direction

Boundary Miscalculation



Conclusion







Table of Contents

1 Conclusion

2 Suggestions



Suggestions

1 Global path planning

2 History of planned path

3 Probabilistic Robotics - SLAM algorithm


autonomous navigation of ugv based on ahrs, gps and lidar

Engineering