Auto Vehicle Guidance PDF

Autonomous Vehicle Guidance Systems
Class Slides Jongrae Kim
Department of Aerospace Engineering University of Glasgow
Course Outline
Class website: moodle website Assessment Term project (20%) Final exam (80%) Text books
[1] Bryson. A. E. Jr. and Ho, Y.-C., Applied optimal control: Optimization, Estimation, and Control, John Wiley & Sons (Chapter 1 & 2) [2] Bertsekas, D., Dynamic programming and optimal control, Vol. 1, Athena Scientific, (Chapter 1 & 2) [3] Several journal papers (available in the moodle)
Course Outline
Introduction Autonomous vehicle path planning Potential function for obstacle avoidance (Ref. 3) Continuous path planning Parameter optimisation (Ref.1, Chapter 1) Optimisation for dynamic systems (Ref.1, Chapter 2) Discrete path planning Discretise the problem space Dynamic programming algorithm (Ref.2, Chapter 1) Deterministic shortest path problem (Ref.2, Chapter 2) Optimal search: Particle swarm optimisation (Ref.3)
Introduction
Autonomous Vehicle Path Planning
Starting Location
Final Location
Introduction
Generate a path for a vehicle to follow Guidance Law
The control law for the vehicle to follow the path: Low level controller (LQR/LQG, H, etc)
This is the topic for Aerospace Control I/II
Introduction
Potential function method for obstacle avoidance
Classical approach Very simple Hard to consider some tight constraints
No sensor noise No threat Perfect information about the environment
Introduction
Clear some ambiguities in the optimisation problem
Are the initial and the final location given? Is the length of the time fixed? Are there any constraints on the velocity and the path?
Is the velocity varying or a constant? Can be the path having sharp corners? etc.
Introduction
Path planning for a mobile robot:
Introduction
Potential Function
Concept from Fluid Mechanics Flow around the obstacles or falling object by gravitational force
[Ref] Oussama Kathib, Real-Time Obstacle Avoidance for Manipulators and Mobile Robots, The International Journal of Robotics Research, Vol. 5, No. 1, Spring 1986, pp. 90-98 Similarity!
Introduction
Potential Function
Consider a case for one obstacle
Attractive potential Repulsive potential
where
Introduction
Newton's 2nd Law of Motion with Conservative Force
Conservative force Newton's 2nd Law of Motion
Attractive force
Repulsive force
Q. Derive it!
Introduction
Ordinary differential equations (ODE)
Assume
Introduction
Solving ODE using MATLAB function ode45
msd_ode.m
function dxdt = msd_ode(time,state, k, c)
msd_main.m
% main m-script file x0 = [1 0]; % initial condition k = 0.5; c = 0.1; ode_option = odeset('RelTol',1e-6,'AbsTol', 1e-9); [time,xout]=ode45(@msd_ode,... [0 10],x0,ode_option,k,c);
x = state(1); vx = state(2); dxdt(1) = vx; dxdt(2) = -k*x-c*vx; dxdt = dxdt(:);
>> msd_main >> figure; plot(time,xout(:,1)); >> figure; plot(time,xout(:,2));
Introduction
Simulation results for two different parameter sets:
Course Outline
Introduction Autonomous vehicle path planning Potential function for obstacle avoidance (Ref. 3) Continuous path planning Parameter Optimisation (Ref.1, Chapter 1) Optimisation for dynamic systems (Ref.1, Chapter 2) Discrete path planning Discretise the problem space Dynamic programming algorithm (Ref.2, Chapter 1) Deterministic shortest path problem (Ref.2, Chapter 2) Optimal search: Particle swarm optimisation (Ref.3)
Continuous Path Planning

Autonomous Vehicle Path Planning
Starting Location
Final Location
Continuous Path Planning: Parameter Optimisation

Problem without constraints
All points that satisfy 1) are called stationary points. In order to satisfy this, all eigenvalues at the point must be positive real. 1) alone is the necessary condition for u* to minimise the cost function. 1) & 2) are the sufficient condition for u* to minimise the cost function.

A point satisfies 1) but point. We call this a singular
We need more information to decide whether the cost at the singular point is minimum or not. e.g.

where
Q. Derive

The second condition for minimality:
Q. Check whether the following matrices are positive definite or not:

Problem with equality constraints
subject to
where

Problem with equality constraints
Stationary Point: Without any constraint,
With the equality constraint,
where

Gradient Direction
Lagrange Multiplier
where

Gradient Direction: 3-dimensional case

Gradient Direction: 3-dimensional case

Necessary Condition

Necessary Condition

Problem with equality constraints: Necessary Condition
(1)
(2)
Define Hamiltonian as follows:
Q. Derive the same necessary condition for the optimality by taking derivative of the Hamiltonian as follows:

Example
Calculate the minimum
Also, show that
i.e, Lagrange multiplier points the opposite direction of the cost change with respect to the constraint while keeps .

Example: Maximum steady rate of climb for aircraft
Derive the necessary condition to maximise the following, i.e. the steady rate of climb:
subject to the following force balance equations:
Horizontal line
where the thrust is constant,
and k1 and k2 are constant.

Problem with Inequality constraints


Necessary Condition for the Optimality

Linear Programming Problems
then, the minimum/maximum occurs at a point where n number of equations of the following is satisfied:
Q. Graphically, explain the reason of the above condition for 2dimensional decision variable y.
Course Outline
Introduction Autonomous vehicle path planning Potential function for obstacle avoidance (Ref. 3) Continuous path planning Parameter Optimisation (ITE, Becerra VM, 2004) Optimisation for dynamic systems (Ref.1, Chapter 2) Discrete path planning Discretise the problem space Dynamic programming algorithm (Ref.2, Chapter 1) Deterministic shortest path problem (Ref.2, Chapter 2) Optimal search: Particle swarm optimisation (Ref.3)

Optimisation using nonlinear programming
This is a discrete path planning algorithm but almost continuous. Reference: Becerra, V.M., Solving Optimal Control Problems with State Constraints Using Nonlinear Programming, IEEE Transactions on Education, Vol. 47, No. 3, pp. 377 384 Nonlinear Programming Problem:
subject to

Pendulum Example
Equation of motion
State-Space Form
where
Initial Condition

Pendulum Example Objective:
where subject to

Pendulum Example: transform the original problem into a parametric optimisation problem
linearise

Pendulum Example: transform the original problem into a parametric optimisation problem
discretise
where
is a discretisation time interval length.

Pendulum Example: compact form where

Pendulum Example: Discretise the cost function

(1)



Pendulum Example: Control input constraint
where is a row vector whose all elements are zero except the kth element, which is 1.

Pendulum Example: Final state constraint
Course Outline
Continuous Path Planning: Optimisation for Dynamic Systems
Optimisation for dynamic systems

Starting Location
Final Location
Optimisation for static systems

Necessary Condition

Necessary Condition
1) Initial condition is free 2) The cost function is given by
3) There could be some constraints on the control and the state, e.g. as follows:
Augmenting the dynamic equation to the cost
where
1st-order Perturbation: the time interval is fixed
Integration by parts the last term in the integral
In order to make the 1st-order perturbation equal to zero: Q. Consider initial and final conditions for various cases: e.g. -fixed/free initial state -fixed/free final state Necessary Conditions for Optimality
Two Point Boundary Value Problem (TPBVP)

Time interval is fixed: Initial condition for the state is given: Final condition for the state is free
2n number of ODE:
2n number of Initial & Final Conditions:

Time interval is fixed: Initial condition for the state is given: Final condition for the state is free 2n number of ODE: 2n number of Initial & Final Conditions:
The control input is given by
The solution satisfies the initial condition for the state and the final condition for the Lagrange multiplier.

2n number of ODE: 2n number of Initial & Final Conditions:
The control input is given by
Solve the above differential equations with the estimate initial Lagrange multiplier Check whether the final value of Lagrange multiplier is equal to the final condition Adjust the estimate and repeat the procedure until it is matched.
Q1. Set up the Two-Point Boundary Value Problem (TPBVP) for the following:
where the initial and the final conditions are given by
Q2. Obtain the optimal control input,
, for the following:
subject to where
Q3. Set up the Two-Point Boundary Value Problem (TPBVP) for the following:
where the initial and the final conditions are given by
Q4. Show that the optimal control input for the following:
where the initial and the final times are fixed, and all initial conditions are equal to zero, is equal to
Course Outline
Discrete Path Planning: Dynamic Programming Algorithm
Optimisation on the discretised space

Starting Location
Final Location
Discretising the space

Constructing a graph: nodes & edges
edges
nodes

Voronoi diagram: 1) Sample n number of points over the space as follows:
2) Construct edges such that: For example, for any point inside this cell, the closest point among the n sample points (ri for all i = 1, 2, ..., n) is r3

Voronoi diagram
MATLAB Command: >>voronoi(x,y);
>> [vx, vy] = voronoi(x,y);

Voronoi diagram
MATLAB Command:
>> [vx, vy] = voronoi(x,y);
>> plot(vx(:,1),vy(:,1));

6 nodes and 5 edges more nodes and more edges

Constructing a cost matrix from the graph
Numbering each node (1) (2) Assume (1) is the starting node and (6) is the end node.
(3) (5)
(4)
(6)

Numbering each node (1) (2) Assume (1) is the starting node and (6) is the end node. For purely a demonstrate purpose, we add two more edges in the original graph.
(3) (5)
(4)
(6)

Numbering each node (1) 10 (2) 10 2 (4) 8 9 6 (3) (6) Assume (1) is the starting node and (6) is the end node. For purely a demonstrate purpose, we add two more edges in the original graph. Assign a cost value for each edges, e.g., distance between two nodes, danger between two nodes, etc.
(5) 3

Numbering each node (1) 10 (2) 10 2 (4) 8 9 6 (3) (6) Assume (1) is the starting node and (6) is the end node. For purely a demonstrate purpose, we add two more edges in the original graph. Assign a cost value for each edges, e.g., distance between two nodes, danger between two nodes, etc. Assign a possible travel direction for each edge (no arrow means the edge can be travelled in both direction)
(5) 3

(1) 10 (2)
Q. Fill the empty terms in the following matrix:
10 2 (4)
6 (6)
(3)
(5) 3
Course Outline
Introduction Autonomous vehicle path planning Potential function for obstacle avoidance (Ref. 3) Continuous path planning Parameter Optimisation (Ref.1, Chapter 1) Optimisation for dynamic systems (Ref.1, Chapter 2) Discrete path planning Discretise the problem space Dynamic programming algorithm (Ref.2, Chapter 1) Optimal search: Particle swarm optimisation (Ref.3)
Dynamic Programming Algorithm

Dynamic Programming Problem
Two principal features
Discrete time dynamic system
Control input
Uncertainty/ Random disturbance
Cost function that is additive over time
as there are random disturbances

Standard Problem
subject to
where
Open Loop Control: decide all control input at the initial state Closed Loop Control: decide each k-th control when the current time is k

Example: (Inventory Control)
40 30 20 10 0 1 2 3 4 5
...... ......

Probability 0.4 0.3 0.2 0.1 0 10 20 30 40

Purchase cost
Shortage cost
Excessive storage cost

subject to
where

Probability 0.4 0.3 0.2 0.1 0 10 20 30 40

1) Case I:
Probability 0.4 0.3 0.2 0.1 0 10 20 30 40
Q. Calculate the costs for the other purchase amount cases and decide the optimal amount of purchase.

Example: (Deterministic Task Scheduling Problem)
Schedule Tasks A, B, C and D B must perform only after A performed D must perform only after C performed 3 5 Initial State 3 3 A 4 2 B 3 D 0 Final State 4
3 0

C B A Initial State C D A C A B D B D D D B D B B

Assign a cost for each edge 3 2 5 Initial State 3 4 C 6 D 3 A A A 3 C B 4 6 2 4 D B D 3 1 3 2 B D B B C B 6 1 D D

Nave way of obtaining the minimum cost path 3 2 5 Initial State 3 4 C 6 D 3 A A A 3 C B 4 6 2 4 D B D 3 1 3 2 C B 6 1 D 16 D 13 B 17 D 10 B 14 B 14

Solve one state problem forward (forward DP) 3 2 5 Initial State 10 C 3 3 4 6 A 5 3 B 7 C 8 A 7 D 9 3 4 6 2 4 C 10 B 12 D 14 B 9 D 11 A 12 6 1 3 1 3 2 D 16 D 13 B 17 D 10 B 14 B 14

Solve one state problem backward (backward DP) 3 2 5 Initial State 10 C 7 3 4 6 A 8 3 B 9 C 5 A 3 D 5 3 4 6 2 4 C 6 B 1 D 3 B 1 D 3 A 2 3 1 3 2 B D B B 6 1 D D

Assign a cost for each edge 3 2 5 Initial State 3 4 C1 6 D2 3 A3 A2 A1 3 C2 B2 4 6 2 4 D31 B32 D32 3 1 3 2 B41 0 D43 B42 B43 0 0 0 C3 B31 6 1 D41 0 D42 0
Final State

DP Algorithm
subject to
Additional Topics
Dijkstras Algorithm
http://www.youtube.com/watch?v=8Ls1RqHCOPw
Q-Learning
http://people.revoledu.com/kardi/tutorial/ReinforcementLearning/index.html http://webdocs.cs.ualberta.ca/~sutton/book/ebook/the-book.html
Course Outline
Introduction Autonomous vehicle path planning Potential function for obstacle avoidance (Ref. 3) Continuous path planning Parameter Optimisation (Ref.1, Chapter 1) Optimisation for dynamic systems (Ref.1, Chapter 2) Discrete path planning Discretise the problem space Dynamic programming algorithm (Ref.2, Chapter 1) Optimal search: Particle swarm optimisation (Refs. 4 and 5)
Particle Swarm Optimisation

Local-Optimisation
The final solution depends on the initial guess
Good initial guess may give the global minimum. But, we don't know how to pick a good initial guess.
may converge to the local minimum instead of the global.
Local minimum
Global minimum

Global Optimisation
Try to find the global minimum with various methods Most of them are heuristic approach
Genetic Algorithm Randomisation Algorithm Particle Swarm Optimisation
However, Be careful!
An algorithm that is the best for any problem does not exist.
No Free-lunch Theorem: Always, there are set of problems that the performance of the algorithm is worse than the others.
No algorithm can be better for all problems than the uniform random sampling algorithm.
Therefore,
Understanding the properties of the problem to be solved is the most important. Choosing an appropriate algorithm is the next important factor.

Particle-Swarm Optimisation (PSO)
1) Spread a number of random points uniformly in the search space and evaluate the function to be minimised at each points
These are the swarm of particles.

2) Update the velocity of each particle using the following equation:
This is the communication range for the 1st particle.

2) Update the velocity of each particle using the following equation:
where and
are the random number between 0 and 1 are the weighting factors.
3) Update the location of each particle

UAV Search Mission Application
Interpret each particle as a UAV Location of particle is the location of UAV The cost function represents the probability of a target that we are searching over the search region. Neighbourhood of each particle is the communication range of each UAV.
How to construct the cost function corresponding to the probability of a target existence at a given location? Formulate the cost function for search mission is beyond the scope of this lecture. If you're interested in, read the following:
[Ref] J. Hespanha and H. Kizilocak., Efficient Computation of Dynamic Probabilistic Maps, In Proc. of the 10th Mediterranean Conference on Control and Automation, July 2002

Example: Rastrigrin function
Global minimum at (0,0) and its cost value is 0.

Example: Rastrigrin function
Global minimum at (0,0) and its cost value is 0.
Using the PSO-toolbox (freely downloadable from internet)

>> pso_options = get_psoOptions; >> pso_options.Obj.f2eval='Rastrigrin'; >> pso_options.Vars.Dim=2; >> [fxmin, xmin, Swarm, history]=pso(pso_options);
Term Project
Term Project #1 (5%)
Implement the path planning algorithm using the potential function method for multiple obstacles (at least 5)
Add the following additional force for each direction
Show the simulation results for various parameter sets Discuss about the simulation results in terms of convergence Discuss about the effect of the additional force for the path planning (Hint. It gives a damping effect of the mass-spring-damper system)
Term Project
Term Project #2 (5%) - Solve the following optimisation problem for the pendulum problem using fmincon in MATLAB
Subject to
Term Project
Term Project #3 (5%)
Construct the cost matrix (A) using voronoi diagram for the path planning problem in the Term Project #1 with the number of obstacles equal to five. Obtain the shortest path using graphshortestpath function in MATLAB Bioinformatics toolbox. Draw the optimal path, obstacles, edges and nodes in 2dimensional space. Design Q-learning algorithm and run as many scenarios as possible until the cost table is completed. Solve the shortest path problem based on the obtained cost table.
Term Project
Term Project #4 (5%) Solve the optimisation problem of Term Project #2 using Particle Swarm Optimisation and compare the results with the ones of Term Project #2

Auto Vehicle Guidance PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Auto Vehicle Guidance PDF

Uploaded by

Copyright:

Available Formats

Autonomous Vehicle Guidance Systems

Class Slides Jongrae Kim

Department of Aerospace Engineering University of Glasgow

This is the topic for Aerospace Control I/II

No sensor noise No threat Perfect information about the environment

Attractive potential Repulsive potential

x = state(1); vx = state(2); dxdt(1) = vx; dxdt(2) = -k*x-c*vx; dxdt = dxdt(:);

>> msd_main >> figure; plot(time,xout(:,1)); >> figure; plot(time,xout(:,2));

Continuous Path Planning

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Q. Check whether the following matrices are positive definite or not:

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

With the equality constraint,

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Define Hamiltonian as follows:

Continuous Path Planning: Parameter Optimisation

Also, show that

Continuous Path Planning: Parameter Optimisation

subject to the following force balance equations:

and k1 and k2 are constant.

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Necessary Condition for the Optimality

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

is a discretisation time interval length.

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Parameter Optimisation

Continuous Path Planning: Optimisation for Dynamic Systems

Optimisation for dynamic systems

Continuous Path Planning: Optimisation for Dynamic Systems

Optimisation for static systems

Optimisation for dynamic systems

Continuous Path Planning: Optimisation for Dynamic Systems

Optimisation for dynamic systems

1) Initial condition is free 2) The cost function is given by

Continuous Path Planning: Optimisation for Dynamic Systems

Augmenting the dynamic equation to the cost

1st-order Perturbation: the time interval is fixed

Continuous Path Planning: Optimisation for Dynamic Systems

1st-order Perturbation: the time interval is fixed

Integration by parts the last term in the integral

Continuous Path Planning: Optimisation for Dynamic Systems

1st-order Perturbation: the time interval is fixed

x = state(1); vx = state(2); dxdt(1) = vx; dxdt(2) = -kx-cvx; dxdt = dxdt(:);