Ch10

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

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?”


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.

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




Shneydor, N. A. (1998). Missile Guidance and Pursuit: Kinematics, Dynamics and Control, Horwood Publishing Ltd.

Shneydor’s 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”


Chapter 10 - Guidance Systems 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.

TrajectoryGenerator Autopilot

Control Allocation

Ship

Waves, currentswind

Estimated positions and velocities

Way-Points

Guidance System Control System Navigation System

Weather routingprogram

Weather data


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.




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.

Locke’s 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.


10.1 Target Tracking 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


10.1 Target Tracking

The control objective of a target-tracking scenario can be formulated as:

where is 2-D position of the target (North-East coordinates)

represents either a stationary point or a craft moving by a nonzero and bounded NED velocity:

ptn Nt,Et R2

ptnt

v tnt p t

nt R2

limt

pnt ptnt 0 #


10.1 Target Tracking The total picture:

To be explained on the next pages….


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.1.2 Pure Pursuit Guidance 2-point guidance scheme

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:

vdn p n

p n #

p n : pn ptn #


10.1.3 Constant Bearing Guidance

2-point guidance scheme

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.

vdn v t

n van #

van p n

p n #

p n : pn ptn #


10.1.3 Constant Bearing Guidance

vdnt v t

nt vant #

vant t p nt

|p nt| #

Ua ua2 va

2

p nt : pdnt pt

nt #

van ua,va

t Ua,max|p nt|

p ntp nt p2

#

Approach velocity vector:

Desired approach speed:

V 12

p ntp nt 0, p nt 0 #

V p ntv nt

t p ntp nt|p nt|

Ua,maxp ntp nt

p ntp nt p2

0, p nt 0 #

Lyapunov function candidate (LFC):

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

Convergence and Stability Analyses


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:

et :

Nt Ndt

Et Edt

t dt

#

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.

Trajectory-tracking control laws are classified according to the number of available actuators.


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.

10.2 Trajectory Tracking


10.2.1 Reference Models

Simplest form is to use a low-pass (LP) filter:

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

xd

r(s) hlp (s)

r xd reference model

Bandwidth of the reference model < bandwidth of the vessel motion control system



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:

relative damping ratio

natural frequency

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

where Md, Dd, and Gd are positive design matrices.

The corresponding state-space model is:

hlps ni

2

s22inisni

2

i (i 1, , n

ni (i 1, , n

Md d Dd d

Gdd Gdr

x d Adxd Bdr

Ad 0 I

Md1Gd Md

1Dd

, Bd 0

Md1Gd

, Cd I, 0



The velocity reference model should at least be of order two to obtain smooth reference signals for velocity and acceleration

where

Premultiplying with results in the alternative representation:

dd

Md d Dd d

Gdd Gdr

Dd Md 2, Gd Md 2

diag1,2, ,n

diagn1,n2, ,nn

d 2 d 2d 2rb

Md1

reference model rb d

d



The position and attitude reference model 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:

where

This can also be written:

d

di

rin s

ni2

1T iss22 inisni

2 , i 1, , n

Ti 1/n i 0

di

rin s

ni3

s32i1nis22i1ni

2 sni3

, i 1, , n

d3 2 I

d 2 I2

d 3

d 3rn

Ad

0 I 0

0 0 I

3 2 I2 2 I

, Bd

0

0

3


10.2.1 Reference Models 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 sgnxxmax if |x | xmax

x elsei i

max, i imax



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:

where the nonlinear function could be chosen as:

where are design parameters

and pj > 0 are some integers.

d3 2 I

d 2 I2

d d

d 3

d 3rn

di d i jij| di |

pj di , i 1, , n

ij 0

-1 -0.5 0 0.5 1-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

d(x) = |x|x



Example 10.2 (Reference Model)

Consider the mass-damper-spring reference model: In the figure, we compare responses using: and a saturating element: for an operator step input r = 1. MSS Toolbox: ExRefMod.m

x d vd

v d 2nvd |vd |vd n2xd n

2r

#

#

0, 1

n 1

vmax 1

0 2 4 6 8 10 12 14 16 18 200

2

4

6

8

102nd-order mass-damper-spring reference model

linear dampingnonlinear dampingvelocity saturation

0 2 4 6 8 10 12 14 16 18 200

1

2

3

4

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


Waypoints / constant setpoints Nonlinear

PID controller

Dynamic model of craft including physical limitations

Desired states / time-varying trajectories

feedback

smooth feasible reference signals (to control system)

(from waypoint database)

dt, dt

Guidance System: dynamic model + guidance controller

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.

10.2.2 Trajectory Generation using a Marine Craft Simulator


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

dd

M d Nd gd

#

#

gd J

d Kpd

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.

N diagn1, ,n6 0 #



Example 10.3: The desired reference trajectories (kinematics) of a ship can be modeled as:

with forward speed dynamics:

where K and T are design parameters.

The guidance system has two inputs, thrust and rudder angle:

x d Ud cosd, y d Ud sind

d rd

Tr d rd K

#

#

Nomoto model

mass + quadratic drag formula

LOS angle

Numerical integration of the ODEs with feedback yields a smooth reference trajectory

The yaw dynamics is chosen as:

m Xu u d 12CdA|ud|ud #

ref atan2ylos ydt,xlos xdt. #

Kpd ref Ki 0

t

d refd Kdrd #

Kpud u ref Ki 0

t

ud u refd #



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.

J mind ,d

power,time #

|U| Umax (max speed)

|r| rmax (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.


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.


10.3.1 Path Generation based on Waypoints

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.

Human interface

Waypoint generator

waypoint database

trajectory / path



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.


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:

and other properties like:

wpt. pos x0, y0, z0, x1, y1, z1, , xn, yn, zn

wpt. speed U0, U1, , Un

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


10.3.1 Path Generation based on Waypoints 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”

( , ) 0 0

( , ) 1 1

( , ) 2 2

( , ) 3 3

1

2

East

North

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. The human operator usually specifies a circle with radius Ri around each waypoint (white circles)

wpt. radius R0, R1, , Rn



The radius of the inscribed circle can be computed from Ri as: where is defined in

the figure. 1 1

( , ) 1 1

( , ) 0 0

( , ) 2 2

90- 1 90- 1

1 1

1 1= /

2 2 2= + -2 21

1

R i Ri tan i

The point where the circle arc intersects the straight line represent the turning point of the ship.

i


10.3.2 Line-of-Sight Steering Laws

t st,et R2

Ut : |v | xt2 yt2 0 #

t : atan2yt,xt S : , #

k : atan2yk1 yk,xk1 xk S #

t Rpkpnt pkn #

Rpk :cosk sink

sink cosk SO2 #

Consider a straight-line path implicitly defined by two waypoints pkn xk,yk

R2

pk1n xk1,yk1

R2

Angle of straight line w.r.t. NED

Course angle

Speed

Tracking errors

st along-track distance (tangential to path)

et cross-track error (normal to path)



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.



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 pⁿlos that corresponds to the desired direction of travel.

Enclosure-based Steering (Direct Assignment of the Course Angle χd )

tandt ytxt

ylos ytxlos xt

#

In order to calculate the two unknowns in pⁿlos = [xlos,ylos], the following two equations must be solved (explicit solution in the book):

xlos xt2 ylos yt2 R2 #

tank yk1 yk

xk1 xk

ylos yk

xlos xk constant #

dt atan2ylos yt,xlos xt #


10.3.2 Line-of-Sight Steering Laws For lookahead-based steering, the course angle assignment is separated into two parts:

de p re #

p k #

re : arctanet #

Velocity-path relative angle:

Path-tangential angle w.r.t. NED:

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

where △>0 is the lookahead (carrot) distance.

et2 t2 R2 #

t R2 et2 # Time-varying lookahead distance:


10.3.2 Line-of-Sight Steering Laws An frequently used method for path-following control is line-of-sight (LOS) guidance.

A LOS vector from the craft to a point pⁿlos on the path between two waypoints can be computed. This gives the desired heading yd for the autopilot.

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: Method B (velocity and LOS vectors aligned): Compute the desired course angle using the lookahead-based steering law:

d d #

B A dt atan2ylos yt,xlos xt #

de p re

k arctanKpe #

needs velocity measurements



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:

dt atan2ylos yt,xlos xt #

LOS guidance principle where the sideslip angle β is chosen as zero and compensated for by using integral action.

Kp Kd Ki 0

t

d #

d d # X

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.

X


10.3.2 Line-of-Sight Steering Laws Method B (velocity and LOS vectors aligned): Compute the desired course angle using the lookahead-based steering law:

LOS guidance principle where the sideslip angle β is computed from velocity measurements:

d d #

Avoids deviations to the path.

arcsin vU

#

de p re

k arctanKpe #



The lookahead-based steering law is implemented as a saturating control law:

re arctanKpet # Kp 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.

re arctan Kpet Ki 0

t

ed #



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:

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.

(xk+1,yk+1) (xk,yk)

(xk-1,yk-1)

Rk x

x x

Test: if vehicle positions (x(t),y(t)) is inside circle, choose next waypoint.

Rk+1

xk1 xt2 yk1 yt2 Rk12 #

Date post:	13-Dec-2015
Category:	Documents
Upload:	zaroxx
View:	215 times
Download:	1 times

Ch10

Documents