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 oﬀers some improvements over Yun-Trooper:
Smaller, Lighter, Faster
Obstacle Avoidance with LiDAR
Remote Monitoring

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
Computing
Sensors
Drive System
Power Supply
Communication

BeagleBone Black with Linux

Xsens MTi-G
The MTi-G is an integrated GPS and IMU Attitude and Heading
Reference System sensor. The internal low-power signal processor
runs a real-time Xsens Kalman Filter providing inertial enhanced
3D 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

Driving Motor
Steering Servo
Driving DC Motor
DC Motor Driver

LM2596 DC-DC Power Converter
Yun-Trooper II requires 3 diﬀerent power voltages. The LM2596
switch power converter provides adjustable output voltage and
high output current (3A), which is suitable for Yun-Trooper II.

XBee PRO
XBee PRO provides Long communication range (Up to 90m in
urban 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
Geographic Coordinate System
Geodesic
Local Coordinate System
5 Control Rule

Geographic Coordinate System
Geographic coordinate system is a reference system used to
describe a position on earth. There are two kinds of such system:
ECI
ECEF

ECEF Ellipsoidal Coordinates
ECEF ellipsoidal coordinates are the most common coordinate
system in describing a position on earth, which deﬁned 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 ellipsoidal
coordinate system used by MTi-G position sensor and most of the
GPS.
a 6378137m
b 6356752.3142m
f = (a − b)/a = 1/298.257223563

Geodesic
The shortest path between two points on the earth, customarily
treated as an ellipsoid of revolution, is called a geodesic. Two
geodesic problems are usually considered:
1 Direct
2 Inverse

Inverse Problem
The relative distance and direction between two location is
required for autonomous navigation, therefore inverse problem is
considered in the algorithm.
GeographiLib Library

Local Tangent Plane
Local tangent plane is the reference system of AHRS.

Target Angle
By the deﬁnition of azimuth α1 and yaw ψ, the target angle Θt
relative to robot could be determined:

Table of Contents
1 Introduction
Vector Field Histogram
Vector Field Histogram Plus
Discussion and Improvement of VFH+
5 Control Rule

Vector Field Histogram (VFH)
VFH generates a polar histogram of the environment around the
robot, identiﬁes wide-enough spaces and calculates corresponding
steering direction.

A cost function G is then applied to every candidate directions, and
the 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 LiDAR
Eﬃcient Calculation
Adjustable characteristic
Disadvantages:
Ignore the kinematic and dynamic constraints
Ignore robot’s geometry
Direction depends on free-spaces

Vector Field Histogram Plus (VFH+
) - Introduction
VFH+ algorithm is an enhanced version of original VFH which
oﬀers 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 to
compute 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 modiﬁcation is required in order to implement
VFH+ with laser range ﬁnder, 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 and
angle 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 - spaces
capable for the robot to pass through, by diﬀerent method. Each
free space Vj is deﬁned 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 single
threshold τ in VFH to generate a Binary Histogram Hi, identifying
all the free spaces.
Hi =



1 if Pi ≥ τmax
0 if Pi ≤ τmin
Hi−1 otherwise

2: Identifying Free Spaces - Hysteresis Filter
By hysteresis ﬁlter, VFH+ has reduced the number of free spaces,
which overcomes the frequent oscillations of VFH in narrow indoor
environment.

2: Free Spaces - Robot’s Geometry
With geometry constraints, free spaces with shrinked boundaries
ˆVj = ( ˆBL, ˆBR)j of each Vj is calculated:
ˆBL = θl − δl dl cos δl
ˆBR = θr + δr dr cos δr
where
δl = arcsin(
ws
dl
)
δr = arcsin(
ws
dr
)

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 is
generated ﬁrst:
Di = |Rs sin θi| + R2
s sin2
θi + w2
s + 2Rsws

3. Blocked Directions - Masked Histogram
The masked histogram Mi = di − Di shows whethter the steering
angle is blocked by obstacles.

3. Blocked Directions - Determine φr and φl
φr and φl can be eﬃciently 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 ˆVj, single or multiple
candidate directions β could be found. The width of a free space is
determined 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 is
abandoned.
No candidate
direction!

For a free space with 0 ≤ ≤ τa, the centered direction is the only
candidate direction.
βn = θl+θr
2

For a free space with 0 < τa < , there are 2 or 3 candidate
directions.
β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 preferred
direction βt:
G(β) = µ1 · (|β − Θt|) + µ2 · |β| + µ3 · (|β − βt−1|)
and
βt = min {G(c)}
where
Θt = Target direction
β = Candidate directions
βt−1 = Previously selected direction

Hysteresis Filter
Wrong βt!

Hysteresis Filter - Missing Boundaries
Hi =



1 if Pi ≥ τmax
0 if Pi ≤ τmin
Hi−1 otherwise

Hysteresis Filter - One Direction

Hysteresis Filter - Another Direction
Hi =



1 if Pi ≥ τmax
0 if Pi ≤ τmin
Hi+1 otherwise

Hysteresis Filter - Combining
Hi = Hi OR Hi

No Candidate Directions
Collision prediction is the indicator of navigation, not candidate
directions!

Boundary Miscalculation of Free Spaces

Histograms of the Environment

Measured Boundary

Actual Boundary

Compensation
In order not to aﬀect the eﬃciency, only the closest measured
distance is considered for the compensation.

Table of Contents
1 Introduction
Obstacle Density
Obstacle Approaching Rate
Collision Prediction
5 Control Rule

Obstacle Density Function
VFH uses Obstacle density function D to calculate the speed of
robot in the environment:
D(di) = 1 −
1
N
N
i=1
di
dmax
The value of D lies between 0 and 1. Therefore, deﬁned a
maximum speed vmax and minimum speed vmin, the speed of
robot in the environment v could be determined:
v = vmin + (1 − D(di)) · (vmax − vmin)

Obstacle Density Function
D = 0.01 D = 0.49

Speed controlled by D considered only current environment, which
is insuﬃcient for high speed robot. Therefore, Obstacle
approaching rate δ is introduced:
δ = −
1
M
j
(dj)t − (dj)t−1
T

For high speed robot, the ability to decelerate while approaching
an obstacle with high speed is critical. Therefore only rate of
approaching 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 obstacle
density, 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 is
limited under 1.

Collision Prediction - First Stage
The ﬁrst stage predicts collision with geometry of robot.

Collision Prediction - Second Stage
The second stage predicts collision on the steering direction with
distance dc.

Control Rule
if βt is available then
steer ← K · βt
speed ← v
else
steer ← K · βt−1
speed ← vmin
end if
if collision predicted then
speed ← 0
end if
setCommand 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

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

Yun-Trooper → Yun-Trooper II
GPS and LiDAR
BeagleBone Black
GNU/Linux

Conclusion
2 Path planning - GPS and obstacle avoidance

Conclusion
3 Improvement of VFH+ algorithm

Improvement of VFH+
algorithm
Hysteresis Filter
No Candidate Direction
Boundary Miscalculation

Conclusion
3 Improvement of VFH+ algorithm
4 Obstacle approaching rate

Suggestions
1 Global path planning

Suggestions
2 History of planned path

Suggestions
2 History of planned path
3 Probabilistic Robotics - SLAM algorithm

Autonomous Navigation of UGV Based on AHRS, GPS and LiDAR

Recommended

Recommended

More Related Content

Viewers also liked

Viewers also liked (14)

Similar to Autonomous Navigation of UGV Based on AHRS, GPS and LiDAR

Similar to Autonomous Navigation of UGV Based on AHRS, GPS and LiDAR (20)

Recently uploaded

Recently uploaded (20)

Autonomous Navigation of UGV Based on AHRS, GPS and LiDAR