Page 179 - Sensing, Intelligence, Motion : How Robots and Humans Move in an Unstructured World

154 ACCOUNTING FOR BODY DYNAMICS: THE JOGGER’S PROBLEM

motion with the direction toward T i . First, find a solution among the controls
(p, q) such that

(p, q) ∈{(p, q) : p ∈ [−p max , +p max ],q =±q max } (4.8)

where q =+q max if the intermediate target T i lies in the left semiplane, and
q =−q max if T i lies in the right semiplane with respect to the vector of velocity.
That is, force p is chosen so as to keep the maximum velocity allowed by the
surrounding obstacles. To this end, a discrete set of values p is tried until a step
that guarantees a collision-free stopping path is found. At a minimum, the set
should include values −p max ,0,and +p max . The greater the number of values
that are tried, the closer the resulting velocity is to the maximum sought. Force
q is chosen on the boundary, to produce a maximum turn in the appropriate
direction. On the other hand, if because of obstacles no adequate controls in the
range (4.8) can be chosen, this means that maximum braking should be applied.
Then the controls are chosen from the set

(p, q) ∈{(p, q) : p =−p max ,q ∈ (±q max , 0]} (4.9)

where q is found from a discrete set similar to p in (4.8). Note that sets (4.8)
and (4.9) always include at least one safe solution: By the algorithm design,
the straight-line motion with maximum braking, (p, q) = (−p max , 0),is always
collision-free (for more detail, see Ref. 96).

4.2.6 The Algorithm

The resulting algorithm consists of three procedures:
• Main Body. This defines the motion within the time interval [t i ,t i+1 ) toward
the intermediate target T i .
• Define Next Step. This chooses the forces p and q.
• Find Lost Target. This handles the case when the intermediate target goes
out of the robot’s sight.

Also used in the algorithm is a procedure called Compute T i , from the VisBug
algorithms (Section 3.6), which computes the next intermediate target T i+1 and
includes a test for target reachability. Vector V i is the current vector of velocity,
and T is the robot’s target location. The term “safe motion” below refers to the
mechanism for determining the next robot’s position C i+1 such as to guarantee
a stopping path (Section 4.2.2).

Main Body. The procedure is executed at each step’s time interval [t i ,t i+1 ) and
makes use of two procedures, Define Next Step and Find Lost Target (see further
below):