Sensing Intelligence Motion - How Robots & Humans Move - Vladimir J. Lumelsky Part 7 docx

Under the Maximum Turn Strategy algorithm, assuming zero velocity, V S = 0, at the start position S, at each step of the path there exists at least one stopping path.. Choosing the next

Figure 4.6 Because of its inertia, immediately after its positionC ithe robot temporarily

“loses” the intermediate targetT i (a) The robot keeps moving around the obstacle until

it spotsT i, and then it continues towardT i (b) When because of an obstacle the whole

segment(C i , T i ) becomes invisible at point C k+1, the robot stops, returns back toC i, and then moves towardT i along the line(C i , T i ).

Convergence. To prove convergence of the described procedure, we need toshow the following:

(i) At every step of the path the algorithm guarantees collision-free motion.(ii) The set of intermediate targetsTi is guaranteed to lie on the convergentpath

(iii) The planning strategy guarantees that the current intermediate target willnot be lost

Together, (ii) and (iii) assure that a path to the target position T will be found

if one exists Condition (i) can be shown by induction; condition (ii) is provided

by the VisBug procedure (see Section 3.6), which also includes the test for target

reachability Condition (iii) is satisfied by the procedure Find Lost Target of the

Maximum Turn Strategy The following two propositions hold:

Proposition 2 Under the Maximum Turn Strategy algorithm, assuming zero velocity, V S = 0, at the start position S, at each step of the path there exists

at least one stopping path.

By design, the stopping path is a straight-line segment Choosing the next step

so as to guarantee existence of a stopping path implies two requirements: Thereshould be at least one safe direction of motion and the value of velocity thatwould allow stopping within the visible area The latter is ensured by the choice

of system parameters [see Eq (4.1) and the safety conditions, Section 4.2.2] As

to the existence of safe directions, proceed by induction: We need to show that

if a safe direction exists at the start point and at an arbitrary stepi, then there is

a safe direction at the step(i + 1).

Since at the start point S the velocity is zero, VS= 0, then any direction

of motion at S will be a safe direction; this gives the basis of induction The

induction proceeds as follows Under the algorithm, a candidate step is acceptedfor execution if only its direction guarantees a safe stop for the robot if needed.Namely, at pointC i, step i is executed only if the resulting vector V i+1 at C i+1

will point in a safe direction Therefore, at step (i + 1), at the least this very

direction presents a safe stopping path

Remark: Proposition 2 will hold for V S = 0 as well if the start point S is known

to possess at least one stopping path originating in it

Proposition 3 The Maximum Turn Strategy is convergent.

To see this, note that by design of the VisBug algorithm (see Section 3.6.3), eachintermediate target Ti lies on a convergent path and is visible at the momentwhen it is generated

That is, the only way the robot can get lost is if at the following step(s)point Ti becomes invisible due to the robot’s inertia or an obstacle occlusion:This would make it impossible to generate the next intermediate target,Ti+1, asrequired by VisBug However, if point Ti does become invisible, the procedure

Find Lost Target is invoked, a set of temporary intermediate targets T i t+1 aredefined, each with a guaranteed stopping path, and more steps are executed untilpointTi becomes visible again (see Figure 4.6) The setT i t+1 is finite because offinite distances between every pair of points in it and because the set must liewithin the sensing range of radiusr v Therefore, the robot always moves toward

a point which lies on a path that is convergent to the targetT

4.2.7 Examples

Examples shown in Figures 4.7a to 4.7d demonstrate performance of the imum Turn Strategy in a computer simulated environment Generated paths areshown by thicker lines For comparison, also shown by thin lines are paths pro-duced under the same conditions by the VisBug algorithm Polygonal shapes arechosen for obstacles in the examples only for the convenience of generating thescene; the algorithms are oblivious to the obstacle shapes

Max-To understand the examples, consider a simplified version of the relationshipthat appears in Section 4.2.3, Vmax=√2rv pmax=√2rv · fmax/m In the simu-

lations, the robot’s massm and control force fmaxare kept constant; for example,

an increase in sensing radius r v would “raise” the velocity Vmax Radius r v isthe same in Figures 4.7a and 4.7b In the more complex scene (b), because ofthree additional obstacles (three small squares) the robot’s path cannot curve asfreely as in scene (a) Consequently, the robot moves more “cautiously,” that is,slower; the path becomes tighter and closer to the obstacles, allowing the robot

T

ruS

T P

Figure 4.7 In each of the four examples shown, one path (indicated by the thin line)

is produced by the VisBug algorithm, and the other path (a thicker line) is produced by the Maximum Turn Strategy, which takes into account the robot dynamics The circle at pointS indicates the radius of vision r v.

to squeeze between obstacles Accordingly, the time to complete the path is 221units (steps) in (a) and 232 units in (b), whereas the path in (a) is longer thanthat in (b)

Figures 4.7c and 4.7d refer to a more complex environment The differencebetween these two situations is that in (d) the radius of vision r v is 1.5 timeslarger than that in (c) Note that in (d) the path produced by the Maximum TurnStrategy is noticeably shorter than the path generated by the VisBug algorithm.This has happened by sheer chance: Unable to make a sharp turn (because of itsinertia) at the last piece of its path, the robot “jumped out” around the cornerand hence happened to be close enough toT to see it, and this eliminated a need

for more obstacle following

Note the stops along the path generated by the Maximum Turn Strategy; theyare indicated by sharp turns These might have been caused by various reasons:For example, in Figure 4.7a the robot stopped because its small sensing radius

rv was insufficient to see the obstacle far enough to initiate a smooth turn InFigure 4.7d, the stop at pointP was probably caused by the robot’s temporarily

losing its current intermediate target

We will now consider the second strategy for solving the Jogger’s Problem The

same model of the robot, its environment, and its control means will be used as

in the Maximum Turn Strategy (see Section 4.2.1)

The general strategy will be as follows: At the given step i, the kinematic

motion planning procedure chosen—we will use again VisBug algorithms—identifies an intermediate target point, T i, which is the farthest visible point on

a convergent path Normally, though not always, Ti is defined at the boundary

of the sensing rangerv Then a single step that lies on a time-optimal trajectory

toTi is calculated and executed; the robot moves from its current positionCi tothe next positionCi+1, and the process repeats

Similar to the Maximum Turn Strategy, the fact that no information is availablebeyond the robot’s sensing range dictates a number of requirements There must

be an emergency stopping path, and it must lie inside the current sensing area.

Since parts of the sensing range may be occupied or occluded by obstacles, thestopping path must lie in its visible part Next, the robot needs a guarantee ofstopping at the intermediate targetTi, even if it does not intend to do so That

is, each step is to be planned as the first step of a trajectory which, given therobot’s current position, velocity, and control constraints, would bring it to a halt

atT i (though, again, this will be happening only rarely)

The step-planning task is formulated as an optimization problem It is theoptimization criterion and procedure that will make this algorithm quite different

from the Maximum Turn Strategy At each step, a canonical solution is found

which, if no obstacles are present, would bring the robot from its current position

C i to its current intermediate targetT i with zero velocity and in minimum time

If the canonical path happens to be infeasible because it crosses an obstacle, a

collision-free near-canonical solution path is found We will show that in this

case only a small number of path options need be considered, at least one ofwhich is guaranteed to be collision-free

By making use of theL∞-norm within the duration of a single step, we ple the two-dimensional problem into two one-dimensional control problems andreduce the task to the bang-bang control strategy This results in an extremelyfast procedure for finding the time-optimal subpath within the sensing range Theprocedure is easily implementable in real time Since only the first step of thissubpath is actually executed—the following step will be calculated when newsensor information appears after this (first) step is executed—this decreases theerror due to the control decoupling Then the process repeats One special casewill have to be analyzed and incorporated into the procedure—the case whenthe intermediate target goes out of the robot’s sight either because of the robotinertia or because of occluding obstacles

decou-4.3.1 The Model

To a large extent the model that we will use in this section is similar to the modelused by the Maximum Turn Strategy above There are also some differences Forconvenience we hence give a complete model description here

As before, the scene is two-dimensional physical space W ≡ (x, y) ⊂ 2; itmay include a finite set of locally finite static obstacles O ∈ W Each obstacle

Ok ∈ O is a simple closed curve of arbitrary shape and of finite length, such that

a straight line will cross it in only a finite number of points Obstacles do nottouch each other; if they do, they are considered one obstacle

The robot is a point mass, of mass m Its vision sensor allows it to detect any

obstacles and the distance to them within its sensing range (radius of vision)—a

disk D(Ci , rv ) of radius rv centered at its current locationCi At momentti, the

robot’s input information includes its current velocity vector Vi and coordinates

ofCi and of target locationT

The robot’s means to control its motion are two components of the

accel-eration vector u= f/m = (p, q), where m is the robot mass and f the force

applied Controls u come from a set u( ·) ∈ U of measurable, piecewise

continu-ous bounded functions in 2, U = {u(·) = (p(·), q(·))/p ∈ [−pmax, pmax], q∈[−qmax, qmax]} By taking mass m = 1, we can refer to components p and q as

control forces, each within a fixed range|p| ≤ pmax,|q| ≤ qmax;pmax, qmax> 0.

Force p controls the forward (or backward when braking) motion; its positive

direction coincides with the robot’s velocity vector V Forceq, the steering

con-trol, is perpendicular top, forming a right pair of vectors (Figure 4.8) There is

no friction: For example, given velocity V, the control values p = q = 0 will

result in a constant-velocity straight-line motion along the vector V.

Without loss of generality, assume that no external forces exceptp and q act

on the system Note that with this assumption our model and approach can stillhandle other external forces and constraints using, for example, the techniquesuggested in Ref 95, whereby various dynamic constraints such as curvature,engine force, sliding, and velocity appear in the inequality describing the limi-tations on the components of acceleration The set of such inequalities defines aconvex region of the ¨x ¨y space In our case the control forces act within the inter-

section of the box [−pmax, pmax]× [−qmax, qmax], with the half-planes defined

by those inequalities

The task is to move in W from point S (start) to point T (target) (see

Figure 4.1) The control of robot motion is done in stepsi, i = 0, 1, 2, Each

step i takes time δt = t i+1− t i = const; the path length within time interval

δt depends on the robot velocity Vi Steps i and i + 1 start at times t i and

ti+1, respectively;C0= S Control forces u(·) = (p, q) ∈ U are constant within

the step

We define three coordinate systems (follow Figure 4.8):

• The world frame, (x, y), is fixed at point S.

• The primary path frame, (t, n), is a moving (inertial) coordinate frame Its

origin is attached to the robot; axis t is aligned with the current velocity

vector V, axis n is normal to t Together with axis b, which is a cross product b= t × n, the triple (t, n, b) forms the Frenet trihedron, with the plane of t and n forming the osculating plane [97].

• The secondary path frame, (ξi, ηi ), is a coordinate frame that is fixed during

the time interval of step i The frame’s origin is at the intermediate target

Ti; axisξi is aligned with the velocity vector Vi at time ti, and axisηi isnormal toξi

For convenience we combine the requirements and constraints that affect thecontrol strategy into a set, called  A solution (a path, a step, or a set of

control values) is said to be -acceptable if, given the current position Ci and

velocity Vi,

(i) it satisfies the constraints |p| ≤ pmax,|q| ≤ qmax on the control forces,(ii) it guarantees a stopping path,

(iii) it results in a collision-free motion

4.3.2 Sketching the Approach

The algorithm that we will now present is executed at each step of the robot path.The procedure combines the convergence mechanism of a kinematic sensor-basedmotion planning algorithm with a control mechanism for handling dynamics,resulting in a single operation As in the previous section, during the step timeintervali the robot will maintain within its sensing range an intermediate target

point Ti, usually on an obstacle boundary or on the desired path At its currentpositionCi the robot will plan and execute its next step towardTi Then atCi+1

it will analyze new sensory data and define a new intermediate targetTi+1, and so

on At times the currentTi may go out of the robot’s sight because of its inertia

or due to occluding obstacles In such cases the robot will rely on temporary

intermediate targets until it can locate point Ti again

The Kinematic Part. In principle, any maze-searching procedure can be lized here, so long as it allows an extension to distant sensing For the sake ofspecificity, we use here a VisBug algorithm (see Section 3.6; either VisBug-21

uti-or VisBug-22 will do) Below, M-line (Main line) is the straight-line

connect-ing points S and T ; it is the robot’s desired path When, while moving along

the M-line, the robot encounters an obstacle, the M-line, the intersection point

between M-line and the obstacle boundary is called a hit point, denoted as H

The corresponding complementary intersection point between the M-line and the

obstacle “on the other side” of the obstacle is a leave point, denoted L Roughly,

the algorithm revolves around two steps (see Figure 4.1):

1 Walk from S toward T along the M-line until detect an obstacle crossing

the M-line, say at pointH Go to Step 2.

2 Define a farthest visible intermediate targetTi on the obstacle boundary inthe direction of motion; make a step toward Ti Iterate Step 2 until detectM-line Go to Step 1.

The actual algorithm will include additional mechanisms, such as a finite-timetarget reachability test and local path optimization In the example shown inFigure 4.1, note that if the robot walked under a kinematic algorithm, at point

P it would make a sharp turn (recall that the algorithm assumes holonomic

motion) In our case, however, such motion is not possible because of the robotinertia, and so the actual motion beyond point P would be something closer tothe dotted path

The Effect of Dynamics. Dynamics affects three algorithmic issues: safetyconsiderations, step planning, and convergence Consider those separately

Safety Considerations. Safety considerations refer to collision-free motion Therobot is not supposed to hit obstacles Safety considerations appear in a number

of ways Since at the robot’s current position no information about the scene

is available beyond the distance rv from it, guaranteeing collision-free motionmeans guaranteeing at any moment at least one “last resort” stopping path Oth-erwise in the following steps new obstacles may appear in the sensing range, andcollision will be imminent no matter what control is used This dictates a certain

relationship between the velocity V, mass m, radius rv, and controls u= (p, q).

Under a straight-line motion, the range of safe velocities must satisfy

where d is the distance from the robot to the stop point That is, if the robot

moves with the maximum velocity, the stop point of the stopping path must be

no further than rv from the current position C In practice, Eq (4.10) can be

interpreted in a number of ways Note that the maximum velocity is proportional

to the acceleration due to control, which is in turn directly proportional to theforce applied and inversely proportional to the robot massm For example, if mass

m is made larger and other parameters stay the same, the maximum velocity will

decrease Conversely, if the limits on(p, q) increase (say, due to more powerful

motors), the maximum velocity will increase as well Or, an increase in the radius

rv (say, due to better sensors) will allow the robot to increase its maximumvelocity, by the virtue of utilizing more information about the environment.Consider the example in Figure 4.1 When approaching pointP along its path,

the robot will see it at distance rv and will designate it as its next intermediatetarget T i Along this path segment, point T i happens to stay at P because no

further point on the obstacle boundary will be visible until the robot arrives at

P Though there may be an obstacle right around the corner P , the robot needs

not to slow down since at any point of this segment there is a possibility of astopping path ending somewhere around pointQ That is, in order to proceed with

maximum velocity, the availability of a stopping path has to be ascertained atevery stepi Our stopping path will be a straight-line path along the corresponding

vector Vi If a candidate step cannot guarantee a stopping path, it is discarded.4

Step Planning. Normally the stopping path is not used; it is only an “insurance”

option The actual step is based on the canonical solution, a path which, if

fully executed, would bring the robot from C i to T i with zero velocity and

in minimum time, assuming no obstacles The optimization problem is set upbased on Pontryagin’s optimality principle We assume that within a step timeinterval [ti , ti+1) the system’s controls (p, q) are bounded in the L∞-norm, andapply it with respect to the secondary coordinate frame (ξi , ηi) The result is

a fast computational scheme easily implementable in real time Of course onlythe very first step of the canonical path is explicitly calculated and used in theactual motion At the next step, a new solution is calculated based on the newsensory information that arrived during the previous step, and so on With such astep-by-step execution of the optimization scheme, a good approximation of theglobally time-optimal path from Ci toTi is achieved On the other hand, littlecomputation is wasted on the part of the path solution that will not be utilized

If the step suggested by the canonical solution is not feasible due to obstacles,

a close approximation, called the near-canonical solution, is found that is both

feasible and-acceptable For this case we show, first, that only a finite number

of path options need be considered and, second, that there exists at least one pathsolution that is-acceptable A special case here is when the intermediate target

goes out of the robot’s sight either because of the robot’s inertia or because ofoccluding obstacles

Convergence. Once a step is physically executed, new sensing informationappears and the process repeats If an obstacle suddenly appears on the robot’sintended path, a detour is arranged, which may or may not require the robot tostop The detour procedure is tied to the issue of convergence, and it is builtsimilar to the case of normal motion Because of the effect of dynamics, the con-vergence mechanism borrowed from a kinematic algorithm—here VisBug—willneed some modification The intermediate target points Ti produced by VisBuglie either on the boundaries of obstacles or on the M-line, and they are visiblefrom the corresponding robot’s positions

However, the robot’s inertia may cause it to move so that Ti will becomeinvisible, either because it goes outside of the sensing rangerv (as after pointP ,

Figure 4.1) or due to occluding obstacles (as in Figure 4.11) This may endangerpath convergence A safe but inefficient solution would be to slow down or tokeep the speed small at all times to avoid such overshoots The solution chosen(Section 4.3.6) is to keep the velocity high and, if the intermediate targetTi goesout of sight, modify the motion locally untilTi is found again

4 A deeper, multistep analysis would be hardly justifiable here because of high computational costs, though occasionally it could produce locally shorter paths.

4.3.3 Dynamics and Collision Avoidance

Consider a time sequenceσ t = {t0, t1, t2, ,} of the starting moments of steps.Stepi takes place within the interval [t i , t i+1), (t i+1− t i ) = δt At moment t i therobot is at the positionC i, with the velocity vector Vi Within this interval, based

on the sensing data, intermediate target T i (supplied by the kinematic planning

algorithm), and vector Vi, the control system will calculate the values of controlforcesp and q The forces are then applied to the robot, and the robot executes

step i, finishing it at point Ci+1 at momentti+1, with the velocity vector Vi+1.Then the process repeats

Analysis that leads to the procedure for handling dynamics consists of threeparts First, in the remainder of this section we incorporate the control constraintsinto the robot’s model and develop transformations between the primary pathframe and world frame and between the secondary path frame and world frame

Then in Section 4.3.4 we develop the canonical solution Finally, in Section 4.3.5

we develop the near-canonical solution, for the case when the canonical solution

would result in a collision The resulting algorithm operates incrementally; forces

p and q are computed at each step The remainder of this section refers to the

time interval [t i , t i+1) and its intermediate target T i, and so index i is often

dropped

Denote (x, y)∈ 2 the robot’s position in the world frame, and denoteθ the

(slope) angle between the velocity vector V= (V x, Vy) = ( ˙x, ˙y) and x axis of

the world frame (Figure 4.8) The planning process involves computation of the

controls u= (p, q), which for every step define the velocity vector and eventually

Obstacle

Ti

t n

p q

Figure 4.8 The coordinate frame(x, y) is the world frame, with its origin at S; (t, n) is

the primary path frame, and (ξ i , η i ) is the secondary path frame for the current robot

positionC.

the path, (x(t), y(t)), as a function of time Taking mass m= 1, the equations

of motion become

¨x = p cos θ − q sin θ

¨y = p sin θ + q cos θ

The angle θ between vector V = (V x, Vy) and x axis of the world frame is

The transformations between the world frame and secondary path frame, from

(x, y) to (ξ, η) and from (ξ, η) to (x, y), are given by



ξ η



= R


ξ η

+



xT yT



(4.12)where

R=

cosθ sinθ

− sin θ cos θ



R
is the transpose matrix of the rotation matrix between the frames(ξ, η) and

(x, y), and (xT , yT ) are the coordinates of the (intermediate) target in the world

frame(x, y).

To define the transformations between the world frame(x, y) and the primary

path frame (t, n), write the velocity in the primary path frame as V = V t To

find the time derivative of vector V with respect to the world frame(x, y), note

that the time derivative of vector t in the primary path frame (see Section 4.3.1)

is not equal to zero It can be defined as the cross product of angular velocity

ω = ˙θb of the primary path frame and vector t itself: ˙t = ω × t, where angle θ

is between the unit vector t and the positive direction of x axis Given that the

control forcesp and q act along the t and n directions, respectively, the equations

of motion with respect to the primary path frame are

˙V = p

˙θ = q/V

Since p and q are constant over the time interval t ∈ [t i , ti+1), the solution for

V (t) and θ (t) within the interval becomes

V (t) = pt + V0

θ (t) = θ0+q log(1 + tp/V i )

where θ0 andV0are constants of integration and are equal to the values of θ (t i )

and V (t i ), respectively By parameterizing the path by the value and direction

of the velocity vector, the path can be mapped into the world frame(x, y) using

the vector integral equation

r(t)=

 t i+1

t i

Here r(t) = (x(t), y(t)), and t is a unit vector of direction V, with the projections

t= (cos(θ), sin(θ)) onto the world frame (x, y) After integrating Eq (4.14),

obtain the set of solutions

x(t)=2p cos θ (t) + q sin θ(t)

4p2+ q2 V2(t) + A

y(t)= −q cos θ (t) − 2p sin θ(t)

4p2+ q2 V2(t) + B (4.15)where terms A and B are

A = x0−V0 (2 p cos(θ0 ) + q sin(θ 0 ))

4 p2+ q2

B = y0+V0 (q cos(θ0 ) − 2 p sin(θ 0 ))

4 p2+ q2and V (t) and θ (t) are given by (4.13).

Equations (4.15) describe a spiral curve Note two special cases: Whenp= 0and q= 0, Eqs (4.15) describe a straight-line motion under the force along thevector of velocity; when p = 0 and q = 0, the force acts perpendicular to the

vector of velocity, and Eqs (4.15) produce a circle of radiusV2

0/ |q| centered at

the point (A, B).

4.3.4 Canonical Solution

Given the current positionCi = (x i , yi ), the intermediate target Ti, and the

veloc-ity vector Vi = ( ˙x i , ˙y i ), the canonical solution presents a path that, assuming no

obstacles, would bring the robot fromC i toT i with zero velocity and in minimumtime The L∞-norm assumption allows us to decouple the bounds on accelera-tions in ξ and η directions, and thus treat the two-dimensional problem as a set

of two one-dimensional problems, one for controlp and the other for control q.

For details on obtaining such a solution and the proof of its sufficiency, refer toRef 99

The optimization problem is formulated based on the Pontryagin’s optimalityprinciple [100], with respect to the secondary frame(ξ, η) We seek to optimize

a criterionF , which signifies time Assume that the trajectory being sought starts

at timet = 0 and ends at time t = t f (for “final”) Then, the problem at hand is

solu-ts,η (“s” stands for “switch”), respectively

The switch curves for control switchings are two connected parabolas in thephase space(ξ, ˙ξ ),

The time-optimal solution is then obtained using the bang-bang strategy for ξ

andη, depending on whether the starting points, (ξ, ˙ξ ) and (η, ˙η), are above or

below their corresponding switch curves, as follows:

dx dt

Point of

switching

Start position

Final

position

Switch curve

h

dt dh

Start position

Point of switching

Final position

Switch curve

Figure 4.9 (a) The start position in the phase space(ξ, ˙ξ ) is above the switch curves.

(b) The start position in the phase space(η, ˙η) is under the switch curves.

switch curves For example, if the initial conditions forξ and η are as shown in

tees time-optimal motion in bothξ and η directions, as long as the time interval

considered is sufficiently small Substituting this control law in the equations ofmotion (4.15) produces the canonical solution

To summarize, the procedure for obtaining the first step of the canonicalsolution is as follows:

1 Substitute the current position/velocity(ξ, η, ˙ξ , ˙η) into the equations (4.16)

and (4.17) and see if the starting point is above or below the switch curves

2 Depending on the above/below information, take one of the four possiblebang-bang control pairsp, q from (4.18).

3 With this pair (p, q), find from (4.15) the position C i+1 and from (4.13)

the velocity Vi+1and angleθ i+1 at the end of the step If this step toC i+1

crosses no obstacles and if there exists a stopping path in the direction

Vi+1, the step is accepted; otherwise, a near-canonical solution is sought(Section 4.3.5)

Note that though the canonical solution defines a fairly complex multistep pathfromC i toT i, only one—the very first—step of that path is calculated explicitly.The switch curves (4.16) and (4.17), as well as the position and velocity equations(4.15) and (4.13), are quite simple The whole computation is therefore very fast

4.3.5 Near-Canonical Solution

As discussed above, unless a step that is being considered for the next momentguarantees a stopping path along its velocity vector, it will be rejected Thisstep will be always the very first step of the canonical solution If the stoppingpath of the candidate step happens to cross an obstacle within the distance found

from (4.10), the controls are modified into a near-canonical solution that is both

-acceptable and reasonably close to the canonical solution The near-canonical

solution is one of the nine possible combinations of the bang-bang control pairs

(k1· pmax, k2· qmax), where k1 and k2 are chosen from the set {−1, 0, 1} (see

Figure 4.10)

Since the canonical solution takes one of those nine control pairs, the canonical solution is to be chosen from the remaining eight pairs This set isguaranteed to contain an -acceptable solution: Since the current position has

near-been chosen so as to guarantee a stopping path, this means that if everything

shown in Figure 4.10 If, for example, the canonical solution is u2i, then the canonical solution will be the first -acceptable control pair u j = (p, q) from

near-the sequence (u3, u1, u4, u0, u8, u5, u7, u6) Note that u5is always-acceptable.

4.3.6 The Algorithm

The complete motion planning algorithm is executed at every step of the path,and it generates motion by computing canonical or near-canonical solutions ateach step It includes four procedures:

(i) The Main Body procedure monitors the general control of motion toward

the intermediate targetTi In turn, Main Body makes use of three

proce-dures:

(ii) Procedure Define Next Step chooses the controls (p, q) for the next step.

(iii) Procedure Find Lost Target deals with the special case when the

inter-mediate targetT i goes out of the robot’s sight

(iv) Main Body also uses the procedure called Compute T i, taken directlyfrom the kinematic algorithm (for example, VisBug-21 or VisBug-22,