CH 10

Chapter 10 - Guidance Systems
10.1 Target Tracking

10.2 Trajectory-Tracking
10.3 Path Following for Straight-Line Paths
10.4 Path Following for Curved Path
Answers the question Where do you want to go?

1
Lecture Notes TTK 4190 Guidance and Control of Vehicles (T. I. Fossen)

From Section 9.2:
Guidance is the action or the
system that continuously
computes the reference (desired)
position, velocity and attitude
(PVA) of a marine craft to be used
by the control system
The PVA data are usually provided to the human operator and the navigation system. The
basic components of a guidance system are motion sensors, external data like weather data
(wind speed and direction, wave height and slope, current speed and direction) and a
computer. The computer collects and processes the information, and then feeds the results
to the motion control system. In many cases, advanced optimization techniques are used to
compute the optimal trajectory or path for the marine craft to follow. This might include
sophisticated features like fuel optimization, minimum time navigation, weather routing,
collision avoidance, formation control and schedule meetings.

Shneydors guidance definition:
The process for guiding the path of an object towards a given point, which in general
may be moving
Charles Stark Draper, the father of inertial navigation, stated:
Guidance depends upon fundamental principles and involves devices that are similar
for vehicles moving on land, on water, under water, in air, beyond the atmosphere
within the gravitational field of earth and in space outside this field
Shneydor, N. A. (1998). Missile Guidance and Pursuit: Kinematics, Dynamics and Control, Horwood Publishing Ltd.
3

Guidance and control systems usually consists of:
An attitude control system
A path-following control system
Basic attitude control system: heading autopilot, while roll and pitch are regulated to zero or left
uncontrolled.
The task of the path-following controller is to keep the craft on the prescribed path with some
predefined dynamics, for instance given by a speed control system.
Weather routing
program
Weather data
Waves, currents
wind
Way-Points
Trajectory
Generator
Guidance System
Autopilot
Control
Allocation
Control System
4
Ship
Estimated
positions and
velocities
Navigation System

Motion Control Scenarios:
Setpoint Regulation (point stabilization) is a special case where the desired position and
attitude are chosen to be constant.
Trajectory Tracking, where the objective is to force the system output y(t) m to track a
desired output yd(t) m. The desired trajectory can be computed using reference models
generated by low-pass filters, optimization methods or by simulating the marine craft motion
using an adequate model of the craft. Feasible trajectories can be generated in the presence of
both spatial and temporal constraints.
Path Following is to follow a predefined path independent of time. There are no temporal
constraints. Spatial constraints can, however, be added to represent obstacles and other
positional constraints if they are known in advance.
5

Guided missiles have been operational since World War II, so organized research on guidance
theory has been conducted almost as long as organized research on control theory.
Consequently, the most rich and mature literature on guidance is probably found within the
guided missile community.
Lockes definition of a guided missile:
A space-traversing unmanned vehicle which carries within itself the means for controlling
its flight path
Locke, A. S. (1955). Guidance, D. Van Nostrand Company Inc.
6

It is convenient to distinguish between a maneuvering (accelerating) and a nonmaneuvering
(nonaccelerating) object
An interceptor typically undergoes three operational phases: launch, midcourse and terminal
We will consider three terminal guidance strategies (see figure below):
Line-of-Sight (LOS) Guidance
Pure Pursuit (PP) Guidance
Constant Bearing (CB) Guidance
7

The control objective of a target-tracking scenario can be formulated as:
limpn t pnt t 0 #
t
where pnt N t ,E t R2 is 2-D position of the target (North-East coordinates)

pnt t represents either a stationary point or a craft moving by a nonzero and
bounded NED velocity:
v nt t p nt t R2
8

The total picture:
To be explained on the next pages.

9
10.1.1 Line-of-Sight Guidance
3-point guidance scheme

The interceptor must constrain its motion along the reference-target line of sight
Typically employed for surface-to-air missiles (beam-rider guidance)
The interceptor (approach) velocity van is pointed to the LOS vector to obtain the
desired velocity vd
10
10.1.2 Pure Pursuit Guidance

The interceptor must align its linear velocity along the interceptor-target line of sight
Equivalent to a predator chasing a prey in the animal world
Typically employed for air-to-surface missiles
Deviated pursuit guidance is a variant of PP guidance (also called fixed-lead guidance)
The interceptor aligns its velocity va along the LOS vector between
the interceptor and the target by choosing the desired velocity as:
11
v nd
p n
n
p
p n : pn pnt #
10.1.3 Constant Bearing Guidance

Often referred to as parallel navigation
Typically employed for air-to-air missiles
Used for centuries by mariners to avoid collisions at sea
Proportional navigation is the most common implementation method
(optimal for scenarios involving nonmaneuvering targets)
The interceptor aligns the interceptor-target velocity van along the LOS vector between
the interceptor and the target.
v nd v nt v na #
v na
p n
n
p
p n : pn pnt #
12
10.1.3 Constant Bearing Guidance

Convergence and Stability Analyses
v nd t v nt t v na t #
v na u a , va
V 1 p n t p n t 0, p n t 0
2
Approach velocity vector:

v na t t
t U a,max
p n t
|p n t|
p n t : pnd t pnt t #
|p n t|
p
t p n t
Lyapunov function candidate (LFC):
2p
V p n t v n t
p n t p n t
t
|p n t|
p n t p n t
U a,max
p n t p n t 2p
0, p n t 0
Desired approach speed:

Ua
u 2a v 2a
13
The origin p(t) = 0 is UGAS and ULES
10.2 Trajectory Tracking

Tracking of a time-varying reference trajectory in 3 DOF (surge, sway and yaw) is achieved by
minimizing the tracking error:
Nt N d t
et :
Et E d t
t d t
Trajectory-tracking control laws are classified according to the number of available actuators.
Classification of Trajectory-Tracking Controllers:

Three or more controls: This is referred to as a fully actuated dynamic positioning (DP)
system and typical applications are crab-wise motions (low-speed maneuvering) and
stationkeeping where the goal is to drive e(t) 0.
This is the standard configuration for offshore DP vessels.
14
10.2 Trajectory Tracking

Two controls and trajectory tracking: Trajectory-tracking in 3 DOF, e(t), with only two
controls, u(t), is an underactuated control problem which cannot be solved using linear
theory. This problem has limited practical use.
Two controls and weather-optimal heading:
If the ship is aligned up against the mean resulting
force due to wind, waves and ocean currents a
weathervaning controller can be designed such that
only two controls, u(t), are needed to stabilize the
ship in 3 DOF. In this approach, the heading angle is
left uncontrolled and allowed to vary automatically
with the mean environmental forces.
Two controls and path following: It is standard procedure to define a 2-D workspace
(along-track and cross-track errors) and minimize the cross-track error by means of an LOS
path-following controller. Hence, it is possible to follow a path by using only two controls
(surge speed and yaw moment). For a conventional ship this is achieved by using a rudder
and a propeller only. Since the input and output vectors are of dimension two, the 6-DOF
system model must be internally stable.
One control: It is impossible to design stationkeeping systems and trajectory-tracking
control systems in 3 DOF for a marine craft using only one control.
15
10.2.1 Reference Models

Simplest form is to use a low-pass (LP) filter:
xd
(s) hlp (s)
r
where r is the command and xd is the desired
state
Choice of filter should reflect the dynamics of the
craft feasible trajectory:
Speed limitations
Acceleration limitations
reference model
xd
Bandwidth of the reference model < bandwidth of the vessel motion control system
16

For marine craft it is convenient to use reference models motivated by the
dynamics of mass-damper-spring systems to generate the desired state trajectories:
h lp s
2n i
s 22 i n i s 2n i
relative damping ratio

n i (i 1, , n natural frequency
i (i 1, , n
This model can be written as a MIMO mass-damper-spring system:
M d d D d d Gd d Gd r
where Md, Dd, and Gd are positive design matrices.
The corresponding state-space model is:
x d Ad x d B d r
Ad
1
M 1
d G d M d D d
Bd
17
0
M 1
d Gd
C d I, 0

The velocity reference model should at least be of order two to obtain smooth reference
signals for velocity d and acceleration d
where
M d d D d d Gd d Gd r
D d M d 2,
Gd M d 2
diag 1 , 2 , , n
diag n 1 , n 2 , , n n
Premultiplying with M 1
results in the alternative representation:
d
d 2 d 2 d 2 rb
rb
reference model
18
d
d

The position and attitude reference model d is typically chosen of third order for filtering of
steps in the n-frame input r.
This suggests a first-order LP-filter in cascade with a mass-damper-spring system:
d i
where
r ni
2n i
1T i ss 22 i n i s 2n i
i 1, , n
Ti 1/ n i 0
This can also be written:
d i
r ni
3n i
s 32 i 1 n i s 22 i 1 2n i s 3n i
i 1, , n
d 2 I d 2 I 2 d 3 d 3 rn
Ad
3 2 I 2 2 I
19
0
,
Bd
0
3

One drawback with a linear reference model is that the time constants in the model often
yields a satisfactory response for one operating point of the system while the response for
other amplitudes of the operator input ri results in a completely different behavior. This is
due to the exponential convergence of the signals in a linear system.
The performance of the linear reference model can also be improved by including
saturation elements for velocity and acceleration:
satx
sgnxx max if |x| x max

x
else
20
i max
,
i
i max
i

Nonlinear damping can also be included in the reference model to reduce the velocity for
large amplitudes or step inputs ri. This suggests the modification:
3
d 2 I d 2 I 2 d d d 3 d 3 rn
where the nonlinear function could be chosen as:
d i d i j ij | d i | p j d i ,
i 1, , n
d(x) = |x|x
1
0.8
where ij 0 are design parameters

and pj > 0 are some integers.
0.6
0.4
0.2
0
-0.2
-0.4
-0.6
-0.8
-1
-1
21
-0.5
0.5

Example 10.2 (Reference Model)
Consider the mass-damper-spring

reference model:
x d v d
2nd-order mass-damper-spring reference model

10
linear damping
nonlinear damping
velocity saturation
#6
v d 2 n v d |v d |v d 2n x d 2n r #4
n 1
2
0
In the figure, we compare responses

using:
10
12
14
16
18
20
10
12
14
16
18
20
0, 1
3
and a saturating element:
v max 1
for an operator step input r = 1.
2
1
0
MSS Toolbox: ExRefMod.m
Desired position and velocity for a step input r = 10.
22
10.2.2 Trajectory Generation using a

Marine Craft Simulator
Guidance System: dynamic model + guidance controller
Waypoints /
constant setpoints
(from waypoint
database)
Nonlinear
PID controller
Dynamic model of
craft including physical
limitations
feedback
Desired states /
time-varying trajectories
d t, d t
smooth feasible
reference signals
(to control system)
Saturation elements for velocity and acceleration should be included to keep these
quantities within their physical limits.
A switching strategy between the set-points (waypoints) must also be adopted.
23

Generate a time-varying reference trajectory using a closed-loop model of the craft where the
time constants, relative damping ratios and natural frequencies are chosen to reflect physical
limitations of the craft.
d J d d #
Md Nd g d
N diagn 1 , , n 6 0 #
g d J d K p d ref K d d
where ref is the desired reference and (d,d) represents the desired
states. The PD control law is a guidance controller since it is applied to
the reference model.
In addition to this, it is smart to include saturation elements for velocity
and acceleration to keep these quantities within their physical limits.
24

Example 10.3: The desired reference trajectories (kinematics) of a ship can be modeled as:
x d U d cos d ,
y d U d sin d
with forward speed dynamics:

m X u u d 1 C d A|u d |u d #
2
mass + quadratic drag formula
The yaw dynamics is chosen as:
d r d
# Nomoto
Tr d r d K #
model
where K and T are design parameters.

The guidance system has two inputs, thrust and rudder angle:
t
K p u d u ref K i u d u ref d #
0
K p d ref K i d ref d K d r d #
ref atan2y los y d t, x los x d t. #
LOS angle
Numerical integration of the ODEs with feedback yields a smooth reference trajectory
25
10.2.3 Optimal Trajectory Generation

Optimization methods can be used for trajectory and path generation. This gives a
systematic method for inclusion of static and dynamic constraints. However, the price to be
paid is that an optimization problem must be solved on-line in order to generate a feasible
time-varying trajectory.
Implementation and solution of optimization problems can be done using linear
programming (LP), quadratic programming (QP) and nonlinear methods. All these methods
require that you have a solver that can be implemented in your program.
power,time #
J min
,
d
|U| U max (max speed)

|r| r max (max turning rate)
|u i | u i,max (saturating limit of control u i )
|u i | u i,max (saturating limit of rate u i )
etc.
26
10.3 Path Following for Straight-Line Paths

A trajectory describes the motion of a moving object through space as a function of time.
The object might be a craft, projectile or a satellite, for example.
A trajectory can be described mathematically either by the geometry of the path, or as the
position of the object over time.
Path following is the task of following a predefined path independent of time, that is there
are no temporal constraints. Spatial constraints, however, can be added to represent
obstacles and other positional constraints.
A frequently used method for path control is line-of-sight (LOS) guidance. A LOS vector from
the craft to the next waypoint or a point on the path (straight line) between two waypoints
can be used for both course and heading control.
If the craft is equipped with a heading autopilot the angle between the LOS vector and the
prescribed path can be used as set-point for the heading autopilot. This will force the craft
to track the path.
27
10.3.1 Path Generation based on Waypoints

Human
interface
Waypoint
generator
waypoint
database
trajectory / path
The waypoints are stored in a waypoint database and used

for generation of a trajectory or a path for the moving vessel
to follow. Both trajectory-tracking and path-following control
systems can be designed for this purpose.
Sophisticated features like weather routing,
obstacle avoidance and mission planning can
be incorporated in the design of waypoint
guidance systems.
28

The waypoint database can be generated using the following criteria:
Mission: the vessel should move from some starting point (xo,yo,zo)
to the terminal point (xn,yn,zn) via the waypoints (xi,yi,zi).
Environmental data: information about wind, waves, and currents
can be used for energy optimal routing (or avoidance of bad
weather for safety reasons)
Geographical data: information about shallow waters, islands etc.
should be included
Obstacles: floating constructions and other obstacles must be
avoided.
Collision avoidance: avoiding moving vessels close to your own
route by introducing safety margins.
Feasibility: each waypoint must be feasible, in that it must be
possible to maneuver to the next waypoint without exceeding
maximum speed and turning rate.
29

The route of a ship or an underwater vehicle is usually specified in terms of waypoints. Each
waypoint is defined using Cartesian coordinates (xk,yk,zk) for i=1,,n. The waypoint
database therefore consists of:
wpt. pos x 0 , y 0 , z 0 , x 1 , y 1 , z 1 , , x n , y n , z n
and other properties like:
wpt. speed U 0 , U 1 , , U n
wpt. heading 0 , 1 , , n
The three states (xi,yi,i) are also
called the pose and they describe
the start and end configurations of
the craft given by two waypoints
30

In practice it is common to represent the desired path using straight lines and circle arcs to
connect the waypoints (Dubins path). The famous result of Dubins (1957) states that:
The shortest path (minimum time) between two configurations (x,y,) of a craft moving at
constant speed U is a path formed by straight lines and circular arc segments
The drawback, in comparison to a cubic interpolation strategy, is that a jump in the
desired yaw rate rd is experienced.
The desired yaw rate along the
straight line is rd = 0 while it is
rd = constant on the circle arc during
steady turning. Hence, there will be a
jump in the desired yaw rate during
transition from the straight line to the
circle arc.
North
(3,3)
East
(2,2)
2
The human operator usually specifies

a circle with radius Ri around each
waypoint (white circles)
1
wpt. radius R 0 , R 1 , , R n
(0,0)
(1,1)
31

The point where the circle arc intersects the straight line represent the
turning point of the ship.
The radius of the
inscribed circle can be
computed from Ri as:
R i R i tan i
(0,0)
(2,2)
where i is defined in
the figure.
90-1
90-1
(1,1)
1=1/
32
= + -221
10.3.2 Line-of-Sight Steering Laws

Consider a straight-line path implicitly defined by two waypoints
Ut : |v|
xt 2
yt 2
0 #
Speed
pnk xk , yk R2
pnk1 xk1 ,yk1 R2
t : atan2y t, x t S : , # Course angle

k : atan2y k1 y k , x k1 x k S #
Angle of straight line w.r.t. NED
t R p k pn t pnk #Tracking errors
R p k :
cos k sin k
sin k
cos k
SO2 #
t st, et R2
st along-track distance (tangential to path)

et cross-track error (normal to path)
33

Two different guidance principles can be used to steer along the LOS vector:
Enclosure-based steering
Lookahead-based steering
and at the same time regulate the cross-track error e(t) to zero.
The two steering methods essentially operate by the same principle, but as will be made
clear, the lookahead-based steering motivated by missile guidance has several advantages
over the enclosure-based approach.
34

Consider a circle with radius R > 0 enclosing p = [x,y]. If the circle radius is chosen sufficiently
large, the circle will intersect the straight line at two points. The enclosure-based strategy for
driving e(t) to zero is then to direct the velocity toward the intersection point plos that
corresponds to the desired direction of travel.
Enclosure-based Steering (Direct Assignment of the Course Angle d )
tan d t
yt
y yt
los
xt
x los xt
d t atan2y los yt, x los xt #
In order to calculate the two

unknowns in plos = [xlos,ylos], the
following two equations must be
solved (explicit solution in the book):
x los xt 2 y los yt 2 R 2 #
y yk
tan k x k1
k1 x k
y yk
x los
constant #
los x k
35

For lookahead-based steering, the course angle assignment is separated into two parts:
et 2 t 2 R 2 #
d e p r e #
Path-tangential angle w.r.t. NED:
Time-varying lookahead distance:
p k #
Velocity-path relative angle:

r e : arctan
et
where >0 is the lookahead

(carrot) distance.
- Easier to compute than the
enclosure-based approach.
- Also valid for all cross-track errors,
whereas the enclosure-based
strategy requires R |e(t)|
36
R 2 et 2

An frequently used method for path-following control is line-of-sight (LOS) guidance.
A LOS vector from the craft to a point plos on the path between two waypoints can be
computed. This gives the desired heading yd for the autopilot.
Method A (body x-axis and LOS
vector aligned): Assume that the
velocity is unknown and compute
the desired heading angle
according to the enclosure-based
steering law:
Method B (velocity and LOS vectors

aligned): Compute the desired
course angle using the lookaheadbased steering law:
d e p r e
k arctanK p e #
37
d d # needs velocity measurements

2-D line-of-sight principle

Method A (body x-axis and LOS vector aligned): Assume that the velocity is unknown
and compute the desired heading angle according to the enclosure-based steering law:
Kd
K i
d #
K p
0
d d #
LOS guidance principle where the sideslip angle is chosen as

zero and compensated for by using integral action.
The price to be paid is that the
craft will behave like an object
hanging in a rope and the craft's
lateral distance to the path will
depend on the magnitude of the
environmental forces and thus the
sideslip angle . This is due to the
fact that = only if =0.
38

Method B (velocity and LOS vectors aligned): Compute the desired course angle using
the lookahead-based steering law:
d e p r e
k arctanK p e #
d d #
LOS guidance principle where the sideslip angle is computed from velocity measurements:
arcsin
v
U
Avoids deviations to the path.
39

The lookahead-based steering law is implemented as a saturating control law:
r e arctanK p et #
K p 1/ 0
A small lookahead (carrot) distance D implies aggressive steering,

which intuitively is confirmed by a correspondingly large proportional gain Kp
in the saturated control interpretation
Integral action is not necessary in a purely kinematic setting, but can be used to follow
straight-line paths while under the influence of constant ocean currents even without
having access to velocity information.
t
r e arctan K p et K i ed
40

Circle of Acceptance for Surface Vessels
When moving along the path a switching mechanism for selecting the next waypoint is
needed. Waypoint (xk+1,yk+1) can be selected on a basis of whether the vehicle lies within a
circle of acceptance with radius Rk around waypoint (xk,yk). Moreover if the vehicle positions
(x(t),y(t)) at time t satisfy:
x k1 xt 2 y k1 yt 2 R 2k1 #
the next waypoint (xk+1,yk+1) should be selected (k should be incremented to k=k+1)

For ships, a guideline could be to choose Rk+1 equal to two ship lengths, that is Rk+1 =2Lpp.
Test: if vehicle positions (x(t),y(t)) is inside circle, choose next waypoint.
(xk-1,yk-1)
x
(xk,yk)
x
Rk
41
(xk+1,yk+1)
x
Rk+1

CH 10

Загружено:

Сведения о документе

Оригинальное название

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

CH 10

Загружено:

Авторское право:

Доступные форматы

Chapter 10 - Guidance Systems

10.1 Target Tracking

Answers the question Where do you want to go?

Chapter 10 - Guidance Systems

Chapter 10 - Guidance Systems

Chapter 10 - Guidance Systems

Chapter 10 - Guidance Systems

Chapter 10 - Guidance Systems

Locke, A. S. (1955). Guidance, D. Van Nostrand Company Inc.

10.1 Target Tracking

10.1 Target Tracking

where pnt N t ,E t R2 is 2-D position of the target (North-East coordinates)

10.1 Target Tracking

To be explained on the next pages.

10.1.1 Line-of-Sight Guidance

3-point guidance scheme

10.1.2 Pure Pursuit Guidance

2-point guidance scheme

10.1.3 Constant Bearing Guidance

2-point guidance scheme

10.1.3 Constant Bearing Guidance

Approach velocity vector:

Lyapunov function candidate (LFC):

Desired approach speed:

The origin p(t) = 0 is UGAS and ULES

10.2 Trajectory Tracking

Classification of Trajectory-Tracking Controllers:

10.2 Trajectory Tracking

10.2.1 Reference Models

10.2.1 Reference Models

relative damping ratio

This model can be written as a MIMO mass-damper-spring system:

10.2.1 Reference Models

10.2.1 Reference Models

10.2.1 Reference Models

sgnxx max if |x| x max

10.2.1 Reference Models

where ij 0 are design parameters

10.2.1 Reference Models

Consider the mass-damper-spring

2nd-order mass-damper-spring reference model

In the figure, we compare responses

and a saturating element:

MSS Toolbox: ExRefMod.m

Desired position and velocity for a step input r = 10.

10.2.2 Trajectory Generation using a

A switching strategy between the set-points (waypoints) must also be adopted.

10.2.2 Trajectory Generation using a

10.2.2 Trajectory Generation using a

with forward speed dynamics:

mass + quadratic drag formula

The yaw dynamics is chosen as:

where K and T are design parameters.

ref atan2y los y d t, x los x d t. #

10.2.3 Optimal Trajectory Generation

|U| U max (max speed)

10.3 Path Following for Straight-Line Paths

10.3.1 Path Generation based on Waypoints

The waypoints are stored in a waypoint database and used

10.3.1 Path Generation based on Waypoints

10.3.1 Path Generation based on Waypoints

10.3.1 Path Generation based on Waypoints

The human operator usually specifies