Lines and arcs are a fundamental class of continuous motion trajectory and path specifications and cannot be defined using the move and turn primitives. Under certain circumstances, the turn and drive primitives may be used to simulate an arc command. The LLAMA "DSV" function is a motion expert module primitive that is used in the uncorrected "STUA" function. For greater control, "STUAE" and "STUAC" functions are made available for trajectory generation. The latter modify the motion expert module position estimator to simplify the "algorithm for arcs," which will be presented in a following section. The former does not alter the position estimators, hence it is useful when the robotís estimated position is needed throughout a trajectory. The line command, "drud," which is not part of the motion expert module, does not alter the position estimators.
Since the desired line-arc functions are not implemented by Denning, they can be implemented by using existing expert module primitives and programming capabilities of the LLAMA system to achieve their desired behavior. The primitives which will be used to define these functions are "drive," "steer," "poll drive motor," "poll steer motor," "stop drive motor," "stop steer motor," "set encoder position," and "get encoder position." The "drive" and "steer" primitives engage the motors until told to stop. The "poll" commands can be used to determine if a motor is on or off. The "position" commands can be used to control the distances moved and turned for each line-arc function specification.
An interpreter will send line-arc commands through the command queue. These commands will be trapped by a line-arc module that will obtain speeds, accelerations, distances, etc., from the command specifiers. These specifications will be stored in variables for reference during the execution of the line-arc command. If a line-arc command is executing while a new line-arc command is received, the old command will stop executing and be superseded by the new command. This will occur only if more than one interpreter is sending line-arc commands to the expert computer. Next, depending on the line-arc command type, a "set position" command will be sent to the motion expert module to initialize the robotís position and orientation to all zeros. This will serve as a reference for completing the line-arc command. Finally, the appropriate motor is engaged, an "until flag" is set to true, and control is returned to the expert loop. These steps should require very little processor time, somewhere on the order of 0.05 seconds or less.
While the "until flag" is true, another module will be entered once per expert loop. This module, the "test for completion" module, will read the position estimator from the motion expert module and notify the calling interpreter when motion is complete. Once motion is complete, the "until flag" is set to false to ensure that this module is no longer used during execution of the expert loop. Additionally, occasional checks to see if the motors are turning or in emergency stop mode are performed to prevent lock-up.
To verify that the line-arc functions behave properly, a consistency argument is needed. The consistency argument for the virtual line-arc functions is composed of three parts: (1) Establish the validity of the STUA, steer until angle using an integer variable approach; (2) Ensure that desired motion equals actual motion, to some small error percentage; (3) Ensure that control cannot be usurped by any line-arc module, meaning, a module must not enter an infinite loop. Item (1) can be validated if a special line-arc function, called "STUAE" is introduced. Item (2) is dependent on converting specifications of relative motion to localized absolute measurements and can be proven mathematically given certain constraints. Item (3) can be informally proven by explaining the possible states that the expert loop and line-arc modules can take. The ultimate aim is to show that in a well-behaved system, the algorithms supporting the expert loop and line-arc functions are robust.
(1) STUA - "Steer until angle, no correct and stop, and drive at speed" (2) STUAE - "Steer until angle, accumulate error, and drive at speed" (3) STUAC - "Steer until angle, correct and stop, and drive at speed" (4) drud - "drive until distance" (5) runtil - "reset until command system"Although not part of line-arc scheme the "stop drive motor" command is implicitly needed to terminate (1)-(4) at the interpreter level. In an idealized system, only STUA and drud would be needed to implement the full range of line-arc commands. Since processing line-arc commands in an expert loop introduces slight delays between operations, an STUA command will tend to overshoot the desired turn angle. The STUAC command remedies this situation in that it stops the steer motor, observes the difference between actual and desired turn angle and performs a quick correction of the difference. The STUAE command, which does not stop the steer motor after completion, serves a different purpose: it is generally used in the three line-arc command sequence, STUAE, STUAE, STUAC. The motivation for such a structure will be covered shortly. Usually only one drud command is placed between arcs to execute linear motion.
The STUAE function was developed to allow more accurate specifications of arcs using any arbitrary specification scheme. The caveat is that the specifications must be partitioned among three (or more) STUAE/STUAC commands to achieve higher resolution. Three of these commands were chosen as an adequate number to represent this improved motion specification given integer arguments. To further improve the accuracy of such a sequence of commands, a "running error" is maintained that automatically corrects desired turn angles based on past system performance. The STUAC function, additionally, uses an estimator approach to perform a discrete quasi-closed loop feedback control of motion. Finally, any remaining error is removed by checking the final achieved motion against the desired motion. This last control is assumed to take place during the start of a line maneuver. The STUAC function will be covered shortly.
Arc time and radius are not specifications of the arc commands and must be "forced" into these commands at the expense of quantization. The following equations only apply to mobile robots that have orthogonal drive and steer axes such as the Denning Mobile Robot:
steer speed = turn angle / arc time (8.1) drive speed = 2 ◊ p ◊ arc radius ◊ turn angle / (arc time ◊ 360) (8.2) distance travelled = drive speed ◊ arc time (8.3)Note that the three equations are not independent. Suppose the following specification for desired motion is given:
turn angle = 270 degrees arc time = 20 seconds arc radius = 5 feetThis results in:
drive speed = 1.178 feet per second distance travelled = 23.562 feet steer speed = 13.5 degrees per secondWe see immediately that a drive speed of 1.178 feet per second cannot be achieved, although 1.1 and 1.2 is permissible. However, the steer speed of 13.5 degrees per second is achievable. We can calculate the positional error if we quantize the drive speed:
drive speed = 1.1 feet per second steer speed = 13.5 degrees per second turn angle = 270 degreesWe then recalculate the distance travelled:
distance travelled = 22 feet distance error = 1.562 feetThis error is significant. Before such an error can be reduced a ratio, called the "granularity quotient," or GQ, must be introduced.
Define GQ = resolution / range
GQ(steer) = 0.1 / 800 = 0.000125 GQ(drive) = 0.1 / 20 = 0.005 (actually the denominator is 40, but safety requires a range of about 20)The steer speed quotient is 40 times smaller than the drive speed quotient, a significant difference. If it were possible to restrict the range of the drive speed to near its desired range and adjust the steer speed to meet the specifications, less error would be introduced. By forming an arc from three (or more) arcs with different parameters, it would be possible to introduce less error. The specified speeds, which would differ, would average out within the newly constructed arc. The STUAE, STUAE, STUAC combination does just that.
A heuristic can be developed to aid in this specification:
1. Divide the arc into three equal lengths to form three arcs.
2. Quantize the three arc lengths such that the inner arc is one unit smaller than the outer arcs.
3. Based on the non-quantized arc length, calculate the drive speed for each arc. Quantize the drive speed such that the inner arc has a speed associated with it that is one unit larger than the outer arcs.
4. Calculate and quantize, where appropriate, all remaining parameters.
5. Replace one STUA by a sequence of STUAE, STUAE, STUAC, with parameters just calculated.
This algorithm has been tested through a simulator of the robotís motion. The positional error is in most cases no more than 0.1 feet, but typically near 0.02 feet. Using the STUA approach, errors of 0.5 feet can develop after only a small number of STUAís and drudís. Figure 8.1 gives the graphical output from the simulator. The error for the optimized solution is difficult to discern from the overall path, however, the magnification of the path shows that the optimized error is indeed very small. The thicker curve represents the desired path while the very close curve in the upper right of the magnification represents the optimized path. Notice the large error of the non-optimized solution when compared to the optimized solution.
Fig. 8.1. Simulated Arc and Line Motions, Two Views. A complex trajectory produced by a line-arc simulator. Notice the reduced error of the optimized path solution - it is the arc very close to the thicker arc in the expanded view. Each graduation represents one foot.
The code for this algorithm and specific values for the various parameters generated is given in appendix A. The algorithm assumes instantaneous accelerations.
A part of the proposed strategy will allow arc commands to be concatenated to achieve greater turn angles. Figures 8.2 through 8.4 show various paths through a cluttered environment. These paths are composed of arcs with given radii. A line-arc command sequence would need to use equations 8.1-3 to specify their parameters if a radii specification approach was used.
Fig. 8.2. Large Hall. Two possible trajectories around three objects.
Fig. 8.3. Small Hall. Only one possible continuous-motion trajectory exists in this configuration.
Fig. 8.4. Hall Variation. Two possible trajectories with differing radii. Alternate trajectories exist, but these provide the most basic traversal through these objects.
Restricting arc angles to Ī270 degrees is highly desirable since the potential for angular ambiguity is reduced; however, as will be shown shortly, this arc angle may be any specified amount provided that the absolute angle for any "STUAE" or "STUAC" is less than 340 degrees. The head angle position data obtained from the expert module is given in tenths of degrees in the range [-179.9, 180.0]. Specifying a desired angle in the Ī270 degree range requires a suitable transformation from the head angle reading range of Ī180 degrees.
Due to the finite delay introduced by computing new angles and by obtaining data, overshoot of the desired angle is to be expected. In an ideal system, a search for only the desired angle is needed; in a real system, exceeding the angle must also be detected (overshoot). This additional criteria adds an extra burden on the algorithm. It is claimed that given the above restrictions, if the expected maximum overshoot of the system is known then the maximum angular displacement range can be determined and computed using an algorithmic approach that is satisfied under all input conditions. The allowable desired angular displacement range is the difference between 360 degrees and the maximum overshoot.
Experiments within the expert loop of a very basic controller typically have shown a ten degree overshoot. For safety, we will specify an overshoot of 20 degrees, which gives a turn range of (+340, -340). To establish a consistent initial condition, we will assume that for every sequence of STUA-type, or arc, commands, the initial heading of the robot will be set to zero degrees. Additionally, the head position must be monitored with great frequency to prevent, or at least minimize, overshoot. Overshoot correction takes place in an algorithm, rather than dynamically (such as in a continuous or discrete closed loop system), by altering the desired turn angles of succeeding commands. If this alteration never exceeds the overshoot range then the algorithm will be well behaved.
The following example illustrates a typical difficulty encountered when an out-of-range turn angle is specified. Assume that a naÔve algorithm that compares the current angle to the desired angle is available for execution of arcs. Now suppose a user has told LLAMA to perform a STUA with an angle of +270 degrees. The head will turn forever since the only available head angles are in the range [180, -180] degrees! To avoid this problem, the following algorithm, called the algorithm for arcs, was devised:
1. Initially, set the desired turn angle tangdes to zero and current position of head to zero, assume head is not turning.
2. Establish amount of overshoot and call that amount the screen and ensure screen is always positive.
3. Choose the turn angle such that | turn angle | < | 360-screen | and add it to tangdes. Set head in motion in proper direction.
4. If turning in a negative direction, subtract 360 degrees from tangdes until it is in the range of (-360, 0) degrees, otherwise add 360 degrees until in range (0, 360).
5. If tangdes was originally negative (turning with a negative velocity), subtract screen from the current tangdes, otherwise add it, call this amount end (donít alter tangdes). This provides the overshoot buffer.
6. Get the current angle, if turning negative (if turning positive, go to line 7) then if the current angle is less than end, add 360 degrees to the current angle until it becomes greater; if it is greater than end+360 degrees, subtract until it becomes less than end+360 degrees. If this new "current angle" is less than tangdes, then done, get next command, go to line 3; otherwise repeat this line.
7. If the current angle is greater than end, then subtract 360 degrees from the current angle until it becomes less than end; if it is less than end-360 degrees, add 360 degrees until current angle becomes greater than end-360 degrees. If this new "current angle" is greater than tangdes, then done, get next command, go to line 3; otherwise repeat this line.
This algorithm performs a host of angular manipulations; how can we show that it is indeed well behaved? We can use a graphical proof to show that, indeed, the algorithm is satisfied for all cases. The desired (tangdes) and overshoot angle (end) are either negative or positive, formed thus by suitable transformation in steps 4 and 5. This provides a continuous range of values that do not cross the zero boundary and so circular ambiguity is removed. The following discussion applies to negative turn motions, but applies equally to positive motions provided the angles and sense are reversed.
Given the specifications for motion, we claim that the robot, while turning in a negative direction (similarly for positive direction), can be in only four angular states. These four states are illustrated in figure 8.5. The four states are described by items 2, 4, 5, and 6; items 1 and 3 are for reference purposes only. There are six regions in these four states, given by the Greek letters a, b, f, g, j, q. The robotís current position is said to occupy one of these regions. A small sector, that is represented by either or both g, and j is called the overshoot region. For negative directions, the angle on the counter-clockwise side of this sector is the desired turn angle, while the angle on the clockwise side is the end turn angle (or overshoot angle). Negative motion implies a clockwise direction around the circle, thus the current angle, represented by a ray in figure 8.6, "moves" clockwise until it crosses the desired angle and is hopefully "stopped" before it reaches the end angle. If it goes past the end angle, overshoot has occurred and the system has failed; undesirable behavior will very likely occur.
Fig. 8.5. Four State Position Specifications. The five definied zones for determining angular position from ambiguous angular position data. With the exception of items 1 and 3, all items represent unique zones. Arbitrary angles are given for items 2-6.
Fig. 8.6. Angle Conventions. Positive angles go counter-clockwise and negative clockwise. Zero angle is defined to be at the top of the circle. This figure also shows negative motion alternate angles where appropriate.
Only the current angle will change over time and proceed through one or two regions before it enters the overshoot region. In two of the states (2 and 4), the current angle may pass through both of g and j regions in the overshoot region. If each of the states are examined under different current angles (of which there are a finite number of combinations) and passed through the given algorithm and if it is determined that the motion continues outside of the overshoot region and stops within, then the algorithm is said to be well behaved. Table 8.1 gives some angles that can be used to verify the algorithm. Table 8.2 gives the status of each region upon examination in each state. The results show that the algorithm is well behaved.
Table 8.1. Typical Desired and End Ranges for Negative Motion (in degrees, from fig. 8.5)
Table 8.2. Zone Element Specifications for Negative Motion
An algorithm for lines uses a variation of the algorithm for arcs to control motion. In the interest of not altering the motion expert moduleís position estimator, the distance travelled is determined by calculating the hypotenuse of the triangle that defines the start and current Cartesian coordinates. While the robot is translating, the hypotenuse distance is calculated continuously in the expert loop until the desired distance has been travelled.
This same estimation algorithm is used to control the drud line function. In this case, the empirical scaling value is 3, reflecting the different time, motion and value relationships of this command to the arc commands.
This module requests head or body position from the expert module, performs any change estimator calculations, and then checks to see if motion is complete. It uses steps 6 and 7 in the algorithm for arcs to determine whether motion is complete or not. If motion is complete, the calling interpreter is awakened and informed of the motion status of the line-arc command, the "until flag" is set to false and the module returns control to the expert loop. If motion is not complete, control returns to the expert loop directly.
Additionally, this virtual function group also monitors robot motion. A request is made once every 256 times through the loop to the expert module to determine motion status (about once every second). The robot could be moving, in which case the function completes, or stops. If the robot is stopped, the calling interpreter is awakened and informed that the requested line-arc command was not completed due to one of the three following conditions: (1) An emergency stop occurred; (2) An "s," or stop, command was issued by another interpreter, causing the robot to stop and terminating the line-arc command in progress; (3) An line-arc command was received from another interpreter, whose parameters are used to execute the new motion.
If an emergency stop occurred, the expert module controlling the motors will not allow motion to occur until a "cles," or clear emergency stop is performed. If the system is in emergency stop mode, any new line-arc commands will be refused until a "cles" is sent. A special class of arc commands is provided called the STUAO-type commands. These commands place a value on the stack that represents the exit condition of the corresponding line-arc command. This value can be used to dynamically (from within a macro) recover from an emergency stop condition, rather than expect service from G2. This also applies to a regular stop condition, which can be recovered from by giving an "runtil" command.
The "runtil" command performs step one of the algorithm for arcs. When LLAMA is first started, an effective runtil is executed. G2 need only send a sequence of arc commands to the robot to start the algorithm for arcs. However, if a sequence of STUAE commands is completed without a STUA or STUAC, then the line-arc command state will be compromised. STUA and STUAC reset the line-arc motion variables, much like runtil. Thus STUA and STUAC can be used alone, but STUAE must always be followed by a STUA or STUAC.
Fig 8.7. Results of Line-arc Testing. The scale is in feet. Each position represents the final position of each simulated or real test. The dotted lines show relationships between related trajectories. The Actual Non-Optimized position was recorded using the position encoders-the robot-calculated position was x = 5.1, y = 3.0.
Two performance orders were created that would move the robot according to the trajectory specified in appendix A, illustrated in figure 8.1, and expressed in table 8.3. The actual performance orders are given in table 8.4. The robot was aligned to a one foot grid and its initial position marked on the floor. The "STUA," or non-optimized, performance order was executed first. The final position of the robot was about two feet from the desired position. The three-arc, or optimized, performance order caused the robot to finish farther than the one-arc solution from the desired end position. The final direction of the head in both cases was not too far from the desired direction. Although an overshoot of ten degrees was typical for each arc command, the net errors cancelled out since the robot changed arc directions during the course of the trajectory. If arcs are executed in only one direction, say counter-clockwise, the actual error would be significantly greater.
Table 8.3. Example Line-Arc Trajectory Specifications
Table 8.4. Actual Performance Orders for Line-Arc Examples
Attempts were made to reduce the "STUAC" correction error by obtaining the turn error before the steer motor has come to a complete stop (currently, head angle error calculation and correction occurs after the steer motor stops), but the turn angle correction became worse due to delays in reading the position encoders while the head was in motion. One possible solution is to increase the steer acceleration limits to cause the steer motor to come to complete stop faster. Another solution is to precompensate the "drud" commands to include the distance that is travelled during the "STUAC" correction.
Originally, the "STUA" function was similar to the "STUAC" function-it did not include correction but was otherwise the same. It became apparent that the "STUA" and "STUAC" functionsí modification of the position encoders was unacceptable during trajectories that relied on the position encoders for a running position estimation. If the position of the robot could be obtained with accuracy (as is the case with the position encoders) while the robot was executing a trajectory, the accumulated error of the trajectory could be observed and the robotís trajectory adjusted if the error became too great. This information could be used by the planner to create a new trajectory for each leg in a journey. The "STUA" function was modified to use Denningís "DSV" function, which does not alter the position encoders. This function tends to overshoot the desired angle due to calculation inaccuracies in the motion expert module. The overshoot angle is variable and roughly proportional to the speed of the steer motor. As future work, the planner might include a table that provides a mapping from desired turn angle, etc., to actual turn angle to reduce trajectory errors. This table would be created by testing the robot with various line-arc parameter values and measuring the actual robot performance. The planner should also be designed to work within the limitations of quantized line-arc parameters. A simulator could be incorporated into G2 to aid the planner.
A monitor was designed that resided on the PC and interfaced to the SDLC bus. The robot was controlled from a terminal and commands were given to cause activity on the bus. Command packet data was captured from the bus, which allowed specifying the bus protocol to an exact degree. This technique was used to find an undocumented SDLC event: whenever an expert module is reset, it sends an unsolicited reply status packet to the OS-9 system. If this were not discovered, the potential for erratic system behavior was nearly assured.
SDLC command packets contain a header which specifies which module is to respond to the message. Denning allocated all available module specifiers, or addresses, to various optional hardware. This posed a problem in adding new hardware, such as the speech synthesizer. The address of a module that was not implemented on the University of Washingtonís robots was chosen for the speech module. An additional header field is the function type. To allow many new expert modules to be added, the function type field was separated into two smaller fields. This allowed for sixteen modules of sixteen functions. This enhanced specification is explained in the LLAMA Userís Manual appendix.
Initially, an 8250 serial UART device was used to transmit and receive data on the SDLC bus. The final version of the speech synthesizer module used a variant of the 68705, the 68HC05C8, which had a built-in serial interface. Thus, the 8250 was not needed and was subsequently eliminated from the design.
A standard LM386 one watt power amplifier IC was used to generate listenable speech through a small speaker. The speaker was placed on the robotís head and produced reasonable audio quality. The anti- aliasing filter on the output of the speech synthesizer was altered from the original design to improve understandability.
Once the functionality was verified, the speech synthesizer was tested with LLAMA. In the course of development, a problem appeared: it was not possible to send allophones from more than one interpreter at a time to the speech module without speech becoming garbled. The problem was that monitoring the size of the speech buffer was not indicative of the size of the buffer once transmission began, since another interpreter could have started transmission in the intervening instant. To remedy this situation LLAMA was designed to process all allophone data through the speech computer, regardless of the source. Using this technique, the garbled speech problem was eliminated. There is one potentially undesirable side effect to this approach: Since interpreters wait for their command packets to complete (to ensure that they get executed), multiple speech requests will pause interleaved requests until the speech buffer empties. A proposed remedy is to build a queue, or buffer, that accepts allophones, and sends them to the speech computer. This will free the requesting process and place the processing burden on the speech computer.
Return to Thesis Index