Note: Descriptions are shown in the official language in which they were submitted.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
TITLE OF THE INVENTION
METHOD FOR ESTABLISHING A COORDINATE SYSTEM
Cross-Reference to Related Applications
This application claims the priority of the previously filed U.S. provisional
application serial number 60/125,545 assigned to the assignee of this
application and
filed on March 22, 1999 and U.S. application serial number 99/23615 entitled
Rotating
Head Optical Transmitter for Position Measurement System filed on October 13,
1999
both of which applications are incorporated herein by this reference in their
entireties.
BACKGROUND OF THE INVENTION
1. Field of the Invention
This invention relates in general to the field of precise position measurement
in
a three-dimensional workspace and more particularly to an improved apparatus
and
method of providing position-related information.
2. Description of Related Art
A variety of endeavors require or are greatly aided by the ability to make a
precise determination of position within a three-dimensional workspace. For
example,
laying out a construction site according to a blueprint requires the
identification at the
actual construction site of a number of actual positions that correspond to
features of
the building on the blueprint.
Despite the many applications which require or are advanced by the ability to
make precise determinations of position, it has historically been relatively
difficult or
expensive to precisely fix the position of any given point relative to an
origin in an
actual three-dimensional workspace.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
2
A variety of techniques are known in the art to measure position, including
land
surveying techniques and global positioning satellite ("GPS") system
techniques.
However, these techniques generally are not precise or require expensive
devices which
are complex and difficult to manufacture with high precision and accuracy.
Additionally, many of these techniques required extensive training, and
therefore
cannot be practiced by those not trained in the art. Land surveying
techniques, for
example, fix position using a precision instrument known as a theodolite. The
theodolite is both an expensive piece of equipment and requires substantial
training to
use. GPS equipment is relatively easy to use, but can be expensive and has
limited
accuracy on a small scale due to a certain amount of intentional error that is
introduced
by the military operators of GPS satellites.
Consequently, there has long been a need in the art for a method and device
that
can quickly and accurately fix positions in a three-dimensional workspace.
There is a
further need in the art for such a method and device which is easy to use and
does not
require extensive training.
Summary of the Invention
It is an object of the present invention to provide a method and device that
can
quickly and accurately fix positions in a three-dimensional workspace. It is a
further
object of the present invention to provide such a method and device that is
easy to
operate and does not require extensive training.
Additional objects, advantages and novel features of the invention will be set
forth in the description which follows or may be learned by those skilled in
the art
through reading these materials or practicing the invention. The objects and
advantages
of the invention may be achieved through the means recited in the attached
claims.
To achieve these stated and other objects, the present invention may be
embodied and described as a position fixing system that includes, at a high
level,
several transmitters and a receiving instrument. The transmitters a preferably
optical
transmitters that transmit laser beams that have been fanned into a plane. The
transmitters transmit signals from stationary locations and the receivers
receive these
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
3
signals. Consequently, the receiving instrument incorporates sensors, e.g.,
light
detectors, that detect the signals from the transmitters. The receiving
instrument then
determines a coordinate system and calculates its position and assorted other
information of interest from these received signals. The receiving instrument
then
displays this information through a user interface. The information may be,
for
example, the location of the receiving instrument or its distance relative to
another
location.
As is clear from the present disclosure, the present invention can be applied
to a
variety of different fields, applications, industries, and technologies. The
present
invention can be used, without limitation, with any system in which
information related
to position must be determined, including without limitation movement,
dimensional
measurement, and position and orientation tracking. This includes without
limitation
many different processes and applications involved in myriad industries. Some
of these
industries and some of their associated processes or applications are: film
making
(digitizing models, virtual sets, camera tracking, automatic focusing),
construction
(trades, power tools, surveying, CAD, equipment control, construction
measurement
and layout), robotics (robot calibration, work cell configuration, mobile
robot
navigation, hazardous waste excavation), law enforcement (accident scene
mapping,
crime scene mapping, incident reconstruction), computers (3D input devices,
video
games), virtual reality (augmented reality, virtual arcades, 3D Internet
experiences),
manufacturing (factory automation, facility decommissioning, parts inspection,
facility
maintenance, manufacturing tooling and inspection, manufacturing measurement),
medical (surgical navigation, smart operating rooms, medical instrumentation),
and
defense (ordnance modeling, simulation training, aircraft fit-checks, ship
retrofit and
repair, site remediation).
Various Figs. are included throughout this disclosure to illustrate a variety
of
concepts, components of several subsystems, manufacturing processes, and
assembly of
several subsystems.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
4
1. Transmitter
The transmitter of the present invention includes a rotating head which sweeps
one ore more, preferably two, fanned laser beams continually through the three-
dimensional workspace in which the receiver will be used to make position
determinations based on the optical signals received from the transmitter. In
this way,
the signals from the transmitter cover the entire three-dimensional workspace.
The
present invention can be used in conjunction with the techniques and apparatus
described in previous provisional patent application U.S.S.N. 60/104,115 to
Pratt, also
assigned to the present assignee, filed on Oct. 13, 1998, and incorporated
herein by
reference.
A. Simplified Optical Path
The receiver preferably has a clear optical path to each transmitter in the
system
during position fixing operation. One of the key advantages of the
transmitters
according to the present invention is the simplification of the optical paths
as
exemplified by the lasers rotating with the head. Additionally, there is no
window in
the preferred transmitter. Therefore, there is no distortion introduced by the
movement
of the laser across a window. As described in detail below, the preferred
embodiment
utilizes a lens or other device which rotates with the laser. Thus, there is
no distortion
caused, for example, by variable window characteristics or angles of incidence
or
between a rotating lens and a fixed laser. The lack of a fixed window also
simplifies
manufacture, maintenance, and operation. The lack of a fixed window does make
it
preferable that a rotating seal be added to the transmitter.
B. Speed of Rotation and Storage of Parameters
The rotating head of the transmitter of the present invention, and the lasers
within it, rotate through a full 360 degrees at a constant, although
configurable,
velocity. As will be explained below, each transmitter in the system needs to
rotate at a
different velocity. Therefore, each transmitter has a velocity that can be
controlled by
the user. Additionally, each transmitter has an easily quantifiable center of
rotation
which simplifies the algorithms for determining position and can simplify the
set-up of
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
the system. A separate synchronization signal, also preferably an optical
signal, fires in
the preferred embodiment once per revolution of the rotating head to assist
the receiver
in using the information received from the transmitter.
The velocity of the rotating head is configurable through the use of, in the
5 preferred embodiment, a field programmable gate array ("FPGA"). Such
configurable
speed control allows the lasers, or other transmitters, to be differentiated
by a receiver
based on their differing speeds of rotation. The use of multiple transmitters,
as is
appreciated by those of ordinary skill in the art, enhances position
detection. Other
advantages are obtained through the use of programmable electronics (FPGAs,
flash
memory, etc). Not only can the desired speed be set by changing the clock to
the phase
locked loop, but the overall gain of the control loop can be programmed to
maximize
performance at the velocity of interest.
C. Beam Type and Number
As described in the incorporated provisional and known in the art, position
detection is also enhanced by using multiple beams and controlling the shape
of those
beams. These beams may be in the same rotating head assembly or in separate
rotating
head assemblies.
Two beams is the preferred number per rotating head assembly, however, more
beams can be used. In particular, another embodiment uses four beams, two for
short
range and two for long range. The two short-range beams would have fan angles
as
large as possible. This will allow the user to operate near the transmitters,
such as in a
room. For long-range, the user would normally be operating away from the
transmitters. Therefore, in that circumstance the vertical extent of the beams
would be
reduced to maximize the range of the system. The beams are, preferably, of
type III
laser. However, the rotation of the beams reduces their intensity to the fixed
observer
such that they can be classified as type I lasers. Safety features are
integrated into the
device to prevent the powering of the lasers when the rotating head is not in
motion. In
the preferred embodiment at least two interlocks are utilized. The first
depends on the
phase lock loop. The lasers are turned off until the system is rotating in
phase-lock for
at least 1024 phase-clock-cycles (approximately 32 revolutions). The second is
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
6
monitoring the absolute speed using the once-per-rev index on the encoder. A
tolerance
is programmed into the system, currently 1-part-in-1000. When the velocity is
outside
that window the laser is disabled and not allowed to operate.
D. Beam Shape
The Transmitter allows flexibility in setting beam characteristics as needed
for
the specific application of the invention. One advantage is that the beam
shape can be
modified. The key is that the beam shape should correspond with correctly
filling the
desired three-dimensional workspace. For construction trades this might be a
room
20m x 20m x Sm in size. For construction machine control this might be a space
1 OOm
x 100m x 1 Om in size. By modifying the beam shape, the energy can be properly
directed.
The beam shape can also be controlled to differentiate beams. This can be done
for multiple beams on a given transmitter or on different transmitters. For a
given
transmitter, the beam of the first and second beams must be differentiated.
One
technique uses their relative position with respect to the strobe in time.
Another
technique is to assure that the beams have different widths ("beam width" or
"divergence angle"). Then, for instance, the first beam could be the "larger"
of the two
beams.
Fanning the beam can be done using a variety of methods known in the art,
including without limitation, rod lenses, pal lenses, and cylindrical lenses.
The use of
rod lenses offers a relatively simple approach, whereas the use of pal lenses
offers
greater control over the energy distribution. The beam typically is emitted
from the
source as a conical beam, then a collimating lens shapes the beam into a
column, then
the fanning lens fans the column.
Rod lenses can be used to increase control on divergence. One of the major
advantages of rod lenses for line generation is that they do not directly
affect the quality
of the beam in the measurement direction (beam direction). Therefore, they
should not
affect the divergence of the laser beam as set by the collimating optics.
Pal lenses can be used to increase control of the energy distribution in the
fan
direction. PAL type lenses can even create "uniform" distributions, where the
energy is
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
7
uniform in the direction of the fan plane. A uniform distribution is often
inefficient,
however, if potential receivers are not uniformly distributed along the entire
fan plane.
In some implementations a "focus" must be created before the lens. In that
implementation, the use of the PAL technique could affect the beam in the
measurement direction.
Gaussian beams can also be used to maximize the performance of the receiver.
Gaussian beams are symmetric beams in that the energy distribution across the
divergence angle or beam width is symmetric. When a simple threshold technique
is
used in the receiver, it important that the pulses be symmetric and be without
shoulders
or sidelobes. It is also helpful if the distribution's shape does not change
with range.
There are several pulse shapes that meet many of these criteria. However, the
Gaussian
distribution meets all of these criteria. With symmetric pulses that do not
have
shoulders or sidelobes, the receiver will be able to detect the center of the
beam. Non-
symmetric pulses, conversely, can cause the receiver to falsely identify the
exact time
when the beam center intersects the receiver.
E. Strobe
Additionally, the synchronization signal, mentioned above, is strobed and must
be symmetric. Therefore, pulse shaping in the flash/strobe pulse generator for
the
synchronization signal is required. A square pulse with equal rise and fall
times is the
desired pulse shape. This pulse is preferably provided to a plurality of LEDs
on the
transmitter that are arranged to send out the synchronization pulse signal in
a multitude
of directions throughout the three-dimensional workspace. The light output of
the
LEDs is directly proportional to the current flowing through the LEDs. Because
of the
high currents involved in creating the strobe, a pulse-forming network must be
used to
assure that the current is a square wave as it passes through the diodes.
F. Communications and Control
A transmitter according to the present invention uses a serial port for
communication and control. This allows calibration data and control parameters
to
easily be transferred. Recall that the transmitters are differentiated by
their speeds.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
8
Therefore a technique must be put in place to simplify the speed changes.
Additionally.
a particular set of transmitter parameters must be made available to the
receiver so that
the receiver can calculate position based on the signals received from the
transmitter.
To create a simple, reliable, and unified technique the preferred embodiment
uses serial
communication between the transmitter and the receiver or test equipment. For
test
purposes, the serial connection is a well-known RS-232 connection. For use in
the
field, the connection is preferably through an infrared serial port. This
allows the
transmitter to be sealed and yet communicate with the outside world. To avoid
interference with the measurement technique, this port is only active when the
lasers are
off.
G. VHDL
Many of the digital designs of a disclosed embodiment are implemented in field
programmable gate arrays (FPGAs). These devices allow complex designs to be
programmed into general-purpose hardware available from multiple vendors. The
programs for these devices are written in a very special computer
understandable
language VHDL (VHSIC [very high-speed integrated circuit] Hardware Description
Language). This is the same language that is used to design microprocessors
and other
semiconductor devices and is now standardized as IEEE 1076.
H. Providing Power to the Laser Head
As explained in the incorporated provisional application, the motor and the
provision of power to the rotating head assembly are key components of a
transmitter
according to the preferred embodiment.
A rotary transformer is used. Several techniques are available for powering
devices in a rotating head. The most common is the use of slip rings.
Unfortunately,
slip rings require physical contact between the ''brushes" and the "slip-
ring". This
creates dust in the system and can cause friction in the motor. The preferred
technique
is to use a rotating transformer. The transformer technique will provide
minimal drag
on the motor. Additionally, through the use of flat signal transformers as
power
transformers, the technique is very compact.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
9
Fly-back control is used on the stator side of the transformer. To minimize
the
number of components in the rotating head, the voltage control is performed on
the
stator side of the transform. To optimize efficiency, a fly-back driving
technique is
utilized.
I. Stability and Precision of Rotation
The stability of the power transfer is also discussed in the incorporated
provisional application. As those of ordinary skill in the art will recognize,
a sine wave
drive motor is a low-cost motor with good inherent stability intra-revolution
and, as
such, is useful in ensuring constant velocity rotation.
The bearing separation should be maximized to achieve optimal results. Any
precession and wobble (wow and flutter in a turntable) will be a source of
error in the
system. It will lead directly to an error in the "z" direction. Using two
precision
bearings and maximizing the distance between the bearings can minimize these
errors.
The strobe flashes of the synchronization signal are based on a once-per-
revolution indicator tied to the motor shaft. There are many ways to create
this shaft
position index. The simplest and preferred technique is to use the index
normally
supplied with an optical encoder. This separate output of the encoder is
directly
equivalent to a shaft position index. An optical encoder disk is used to give
rotation
information. Other devices, including without limitation, tachometers and
synchros
could be used.
The optical encoder disk is typically made of glass and has a series of radial
marks on it which are detected as the disk rotates. Additionally, the disk
typically has a
single "index" mark of a different radius which is used to detect complete
rotations.
The disk system produces a square wave with a frequency dictated by the speed
with
which the radial marks are passing. For example, if the disk is rotating at 1
revolution/second, a 1000 mark disk system would produce a 1000 Hz square wave
( 1000 radial marks/revolution * 1 revolution/second = 1000 Hz).
The speed of the motor is controlled through a feedback phase-locked loop
("PLL") system. The disk system square wave is one input and a clock from the
transmitter system is the other input. The transmitter clock has a selectable
frequency.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
The output of the PLL is used to control the speed of the motor rotation such
that the
PLL remains locked at the selected frequency.
The index mark of the disk can also be used to initiate the strobe pulse
once/revolution.
5 J. Low Manufacture Cost
As noted above, for the receiver to use the signals from the transmitter to
accurately and precisely fix a position in the three-dimensional workspace,
the receiver
must have available a certain set of parameter characteristic of the
transmitter. For
example, as will be explained in detail below, the receiver must know the
angles at
10 which the laser beams are emitted from the transmitter head.
If these angles are pre-defined and the transmitter must then be manufactured
with precision to match the specified parameters, it becomes extremely
expensive to
manufacture the transmitter. Under the principles of the present invention,
the
transmitter can be manufactured without such high precision and without
requiring that
the resulting transmitter conform to pre-specified parameters. Rather, the
operating
parameters of the transmitter required by the receiver are carefully measured
after the
transmitter is manufactured. This process, which will be referred to as
transmitter
calibration herein and described in more detail below, removes the expensive
requirement of a precisely constructed transmitter. Consequently, the system
of the
present invention becomes much less expensive.
The operating parameters of the transmitter are preferably stored
electronically
in the each transmitter after that transmitter is calibrated. Consequently,
each
transmitter preferably incorporates a memory device in which the calibration
parameters can be stored. These parameters can then be communicated to the
receiver
electronically through the serial port, optical or wired, of the transmitter
described
above. The receiver will have a corresponding serial port, optical or wired,
for
receiving data from the transmitter.
Preferably, these parameters are then stored in memory in a Position
Calculation
Engine (PCE) of the receiver and can be updated as required. For example, if a
new
transmitter is added to the system, then a new set of parameters needs to be
loaded into
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
11
the PCE from or for that transmitter. As an additional example, if the
rotation speed of
a transmitter is changed, then this information needs to be updated in the
PCE.
2. Receiving Instrument
In the present system, the receiver or receiving instrument is a wand, an
example of which is illustrated in Fig. 17. The wand provides a light-weight.
mobile
receiving instrument that can be carried anywhere within the three-dimensional
workspace. A tip of the wand is used as the point for which position within
the
workspace is determined based on the signals received from the system
transmitter.
The position of the tip can be continuously calculated by the receiver and
displayed for
the user on a display device provided on the receiver. Consequently, no
extensive
training is required to operate the position fixing system of the present
invention once
the system is set up and functioning.
The wand preferably contains two receivers, which are light detectors if the
transmitter is emitting optical signals as in the preferred embodiment. The
Position
Calculation Engine ("PCE") of the receiving instrument is a processor that
performs
most of the computations of the receiving instrument. The PCE supports any
required
set-up procedure as well as the subsequent tracking, position calculation, and
information display functions. The receiving instrument and PCE will be
described in
detail below.
The Smart Tip, shown in Fig. 17, can also perform computations, as indicated
by the FPGA (field-programmable gate array) and the "i Button" in each Smart
Tip.
The Smart Tip can be present at either end of the wand in the present system
and the
signal "Tip Present" indicates whether there is a Smart Tip on each of the
receiving
instrument ends.
3. Setup
The setup procedure is described in detail below. The setup procedure places
the transmitters in position and commences their operation. The setup
procedure also
allows the system to, among other things, define a useful coordinate system
relative to
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
12
the three-dimensional workspace and begin tracking the wand's location in that
coordinate system.
Brief Description of the Drawings
The above described novel aspects of applicants' improved apparatus and
methods for position measurements can be better understood with reference to
the
attached drawings, in which:
FIG. 1 is an illustration of an improved optical transmitter according to the
present invention, contrasted with a conventional rotating laser;
FIG. 2 is a schematic plan and sectional views illustrating the preferred
embodiments of the optical transmitter apparatus of Fig. 1 according to the
present
invention;
FIG. 3 is a block diagram of the improved optical transmitter for a position
location system and method according to the present invention;
FIG. 4 is an illustration of the rotating optical head and corresponding frame
of
reference according to the transmitter of the present invention;
FIG. 5 is a graphic representation of a fan beam according to the present
invention;
FIG. 6 is a graphic representation of the fan beam of Fig. 5 rotated about the
x
axis;
FIG. 7 is a graphic representation of the fan beam of Figs. 5 & 6 further
rotated
about the z axis;
FIG. 8 is a graphic representation of the plane of the fan beam intersecting a
detector according to the present invention;
FIG. 9 is a graphic representation of the planes of two fan beams intersecting
a
detector according to the present invention;
FIG. 10 is a Cartesian plot of vectors representing intersecting fan beam
planes
according to the present invention;
FIG. 11 is a graphic representation of a single fan beam plane illuminating a
detector according to the present invention;
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
13
FIG. 12 is an illustration of a three-transmitter position measurement system
according to the present invention;
FIG. 13 is a linear time plot of a typical pulse sequence for pulses from an
optical
transmitter according to the present invention;
FIG. 14 is a time plot of the pulse sequence during a single rotation of an
optical
transmitter according to the present invention;
FIG. 15 is a graphical depiction of the pulses emitted during a single
rotation of
an optical transmitter according to the present invention;
FIG. 16 is a plan view of the improved transmitter according to the present
invention illustrating the preferred positioning of the transmitter front and
the zero-
azimuth plane of the improved transmitter;
FIG. 17A is an illustration of a receiving instrument for a position measuring
system according to the present invention;
FIG. 17B is an detailed illustration of the detector and tip assembly of the
receiving instrument of Fig. 17A according to the present invention;
FIG. 17C is a block diagram of some of the important elements of the receiving
instrument according to the present invention;
FIG. 18 shows the data flow of the position measurement receiver according to
the present invention;
FIG. 19 shows the data flow of pulse detection and tracking according to the
present invention;
FIG. 20 is a block diagram of the Position Calculation Engine (PCE) of the
receiving instrument according to the present invention;
FIG. 21 is a block diagram of the field programmable gate array (FPGA) on the
PCE of Fig. 20;
FIG. 22 is a flowchart for Software Object Detector::Entry which is executed
by
the receiving system according to the present invention;
FIG. 23A is a flowchart for Software Object PulseTrackManager::Update which
is executed by the receiving system according to the present invention;
FIG. 23B is a continuation of the flowchart for Software Object
PulseTrackManager::Update of Fig. 23 A;
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
14
FIG. 24 is a flowchart for Software Object PulseTrack::Synchronize which is
executed by the receiving system according to the present invention;
FIG. 25 is a flowchart for Software Object PulseTrack::Track which is executed
by the receiving system according to the present invention;
FIG. 26 is a flowchart for Software Object PulseTrack::Predict which is
executed
by the receiving system according to the present invention;
FIG. 27 is a flowchart for Software Object PulseTrack::Reconcile which is
executed by the receiving system according to the present invention;
FIG. 28 is a flowchart for Software Object Transmitter::Reconcile which is
executed by the receiving system according to the present invention;
FIG. 29 is a flowchart for Software Object
FlyingHeadTransmiterORPGWithDifferentCycle::postSynchronize which is executed
by the receiving system according to the present invention;
FIG. 30 is a flowchart for Software Object PulseTrack::isMultipath which is
executed by the receiving system according to the present invention;
FIG. 31-1 is an illustration of a system according to the present invention
which
is being initialized using a Least Squares Resection method;
FIG. 31-2 is a mathematical illustration of a step in the Least Squares
Resection
method of initializing a position measuring system according to the present
invention;
FIGS. 32-1 and 32-2 are illustrations of a system according to the present
invention which is being initialized using a Quick Calc Method which is part
of the
present invention;
FIG. 32-3 is a mathematical illustration of a step in the Quick Calc Method of
initializing a position measuring system according to the present invention;
FIG 32-4 is an illustration of a first transmitter identified in the Quick
Calc
Method.
FIGS. 32-5, 32-6, 32-7, 32-8, 32-9, 32-10, 32-I I and 32-12 are a mathematical
illustrations steps in the Quick Calc Method of initializing a position
measuring system
according to the present invention;
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
Detailed Description of the Preferred Embodiments
The preferred embodiments of the present invention will now be described in
detail with reference to the drawings.
5 1. The Transmitter
A. Physical Description
With reference to Fig. 1, a transmitter (10) according to the present
invention is
a device physically similar to a rotating laser (11), which is also
illustrated in Fig. 1 for
comparison. However, a conventional rotating laser ( 11 ) has a single
rotating spot
10 beam ( 12) that is swept through a plane as the head of the laser ( 11 )
rotates. In contrast,
the transmitter ( 10) of the present invention emits two rotating fan beams (
14 & 16).
These fan beams ( 14 & 16) sweep through the three-dimensional workspace
within
which the system of the present invention can fix the position of the
receiving
instrument.
15 Fig. 2 shows the preferred assembly of the transmitter (10), particularly
the
rotating head (7) of the transmitter ( 10), according to the present
invention. As shown
in Fig. 2, the rotating head (7) of the transmitter (10) includes two fan
lasers (201 &
202). There are three important angles in the Fig.: B~,l~ , ~ , and ~2 . B~,~f
describes the
angular separation between the two laser modules (201 & 202) in the rotating
head (7)
as viewed from above the transmitter (10). The lasers (201 & 202) are
preferably
disposed with optical axes at approximately 90° to each other. ø , and
~Z describe the
tilt of the fan plane of the lasers (201 & 202), respectively, with respect to
a vertical
plane. As shown in the lower portion of Fig. 2, these two angles are measured
from
vertical, and are nominally set to -30° for beam 1 and +30° for
beam 2. We explain the
sign convention for these angles below. As described above, the actual values
for 9~,~f ,
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
16
~ , and ~2 are determined through a factory calibration process and need not
conform
exactly to the preferred values described herein.
As the transmitter head ( 10) rotates, it scans the measurement field more
fully
described hereafter with the two planes of light ( 14 & 16) emitted by the fan
lasers (201
& 202). The receiver or receiving instrument (24, Fig. 12) is illuminated by
each
laser's fan plane (14 & 16) exactly once during a rotation of the head (7).
In addition to this scanning operation, the transmitter (10) also fires an
optical
strobe as a synchronization signal to the receiver (24). The strobe is fired
from a
plurality of LED's (6, Fig. 3) disposed to emit light at a variety of angles
from the
transmitter (10) so as to cover the three-dimensional workspace. The
synchronization
signal is emitted at a fixed point in the head's revolution so as to be
emitted once per
revolution of the transmitter head (7). The strobe illuminates the receiver
(24) and is
used to provide a zero reference for the rotation of the head (7). This
scanning of the
fan beams ( I 4 & 16) and the strobed synchronization signal process provides
the basis
for the position measurements made by the receiver which will be described in
more
detail below.
Each transmitter (10) in the system rotates at a known and unique rate. This
unique rotational rate allows the software in the receiver to differentiate
between the
transmitters surrounding the three-dimensional workspace. Knowing the speed of
each
transmitter and a zero point for the rotation of that transmitter given by the
synchronization signal, the receiver knows when at what interval and time to
expect to
detect the first and second fan beams ( 14 & I 6) emitted by any particular
transmitter
( 10). Thus, beams detected at the expected interval and timing can be
assigned as
having come from a particular transmitter in the system based on the operating
parameters of that transmitter known by the receiver, i.e., communicated to
the receiver
in the calibration processes mentioned above.
B. Low Cost Transmitter Detailed Description
An improved, low cost optical transmitter useful in a three dimensional
measurement system in accordance with several novel aspects of applicants'
invention
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
17
is illustrated in the logic block diagram of Fig. 3. Throughout the
specification and
drawings, like numerals are used to designate like elements.
As shown in Fig. 3, a transmitter ( 10) of the present invention preferably
includes the rotating head (7) and synchronization or reference signal
emission
assembly (6) as described above. A motor drive assembly (5) drives the
rotating head
(7).
As noted above, the speed of the rotating head is used by the receiver (24) to
recognize the transmitter emitting a particular pair of fan beams (14 & 16).
Therefore,
a motor velocity control circuit (4) is provided to provide power to and
regulate the
velocity of the motor drive assembly (5).
As described above, in order to achieve a low cost optical transmitter ( 10)
and
method, the present invention preferably relies on a calibration procedure
performed
during the manufacture/assembly process to generate unique data for
characterizing
each optical transmitter (10) rather than employing a much higher cost
precision
assembly process. Specifically, angular calibration data is generated during
the
manufacture/assembly process that includes 8~,~~ , ~ , and ~z (as defined
above) for each
transmitter (10). 9«ff describes the angular separation between the two laser
modules
(201 & 202) in the rotating head (7) of each transmitter, and ~ , and ~2
describe the tilt
of the fan plane of the lasers (201 & 202) in each transmitter ( 10),
respectively, with
respect to a vertical plane.
This angular calibration data (9) is preferably stored in calibration data
memory
(2). In addition, data defining the setting for the rotational velocity can be
preloaded
during the manufacturing process and loaded into calibration data memory (2)
or
variable motor control memory (4). If the rotational velocity is adjusted by
the user, the
new rotational velocity value will be recorded in the data memory (2) or the
motor
control memory (4).
A data processor (3) is also provided to control the motor velocity control
unit
(4) and the data calibration memory (2). The data processor (3) is preferably
connected
to a user interface (303), such as a keyboard, so that the calibration data
can be entered
for storage in the calibration memory (2) or so that the angular velocity of
the motor
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
18
drive assembly (5) can be changed and the new value stored in the appropriate
memory
(4 and/or 2).
Alternatively, during field setup, data (8) that sets the velocity of the
motor
drive assembly (5) may be input directly to variable motor control unit (4)
using a cable
port (302) or optical port (301) of the transmitter. That velocity calibration
data must
likewise be stored in memory (4 and/or 2). Likewise, calibration data (9) can
be
entered directly to the calibration memon- (2) through a cable port (302) or
optical port
(301) of the transmitter.
As shown in Fig. 3, the processor (3), calibration memory (2) and velocity
control logic/memory unit (4) each may be connected to the cable port (302) or
optical
port (301 ) of the transmitter so that data can be input or retrieved
therefrom.
Calibration data from the memories (2 & 4) can be output to the optical
receiver (24;
Fig. 12) in a measurement system of the present invention from the memory
units (2 &
4) via the cable or optical output ports (302 & 301). As mentioned above, when
a
transmitter (10) is introduced into a measurement system according to the
present
invention, the calibration data for that transmitter ( 10) must be transferred
to or loaded
into the receiver (24; Fig. 12).
C. Mathematical Description
A mathematical model is now set forth that describes the physical scanning
operation of the transmitter. We use this model in the next section to derive
the
position calculation algorithm.
Physically, the scanning operation of the transmitter (10) is accomplished
with
the two laser fan beams (14 & 16) described above with reference to Figs. 1
and 2.
Each of the fan beams (14 & 16) will be considered individually in this
mathematical
model. To build this model, we first define the transmitter 's reference frame
as shown
in Fig. 4. Each transmitter ( 10) has its own local reference frame, and these
reference
frames are different from the user's reference frame as will be explained
hereinafter.
The reference frames of the various transmitters (10) in the system will be
related to the
user's reference frame as described below. As can be seen in Fig. 4, the head
(7)
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
19
preferably rotates in the positive direction about the z-axis according to the
right hand
rule.
To describe the scan of a fan beam, we start with a vertical plane at y = 0,
i. e. a
plane in the x-z axis as shown in Fig. 5. Mathematically, the plane can be
uniquely
represented by a vector normal to its surface. This plane corresponds to the
plane in
which light would be emitted by a fan laser that is oriented verticall~~. In
Fig. 5, the
plane of the fan beam is drawn as square, but in actuality the plane has a
finite angular
extent as shown by the dotted lines. This angular extent does not affect the
mathematical model, but it does impact the angular field of view of the
transmitter ( 10).
The vector defining this plane is given below.
0
Plane defined by 1
0
As shown in Fig. 6, we next rotate this vertical plane about the x-axis by an
angle ~. This new plane represents a fan laser as inserted into the head of
the
transmitter (10). ø is the physical slant angle described in the previous
section. Each
fan beam emitted from the transmitter ( 1 ) will have a different ~, which is
one of the
calibration parameters determined during manufacture and communicated to the
receiver (24) for use in calculating position. A positive ~ is a right-handed
rotation
about the x-axis, as shown in Fig. 6.
1 0 0 0 0
0 cos ~ -sin ø 1 = cos ~
0 sin ~ cos ~ 0 sin ~
As shown in Fig. 7, we rotate this new plane about the z-axis by an angle 0.
This angle is actually a function of time because it represents the location
of the fan
beam as the transmitter head ( 10) rotates about the z-axis, i. e. 0(t) is the
scan angle at
time t as shown in Figs. 7 and 13.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
cos B(t) -sin B(t) 0 0 -cos sin B(t)
sin ~(t) cos B(t) 0 cos ~ = cos ~cos 0(t)
0 0 1 sin ø sin ~
In the position calculation algorithm as hereinafter explained, this vector
expression represents the laser fan plane at the point in time when it
intersects the
5 detector (24) as shown in Fig. 8. We call this vector expression
-cos sin 0(t)
cos øcos B(t)
sin ~
For each rotation of the transmitter head, the receiver system (24), as
described
10 below, calculates two ° vectors, ' ~ and ~ 2, that describe the
location of the two fan
beams from each transmitter (10) at their intersection point with the detector
(24) (See
Fig. 12). Since ~ is a constant determined through factory calibration, each ~
vector
depends solely on its corresponding scan angle 0, which in turn depends on
timing
measurements made by the receiver system (24) using the synchronization signal
15 concurrently emitted by the transmitter ( 10).
2. Position Calculation
There are two possible methods we can use to calculate the position of a
flight
detector on the receiver (24) or receiving instrument of the present
invention: the
theodolite network method and the preferred non-theodolite transmitter method.
In the
20 receiving instrument of the present invention, the non-theodolite method is
used
because it is faster and more suited to the transmitter's unique design. For
purposes of
comparison, we will briefly discuss the theodolite network method first before
presenting the preferred non-theodolite method.
To use the theodolite network method, the receiver system would calculate the
intersection between the measured azimuth-elevation vectors from each
transmitter ( 10)
to a signal detector as illustrated in Fig. 10. Fig. 9 shows both fan planes
(26 & 28) of
the respective fan beams ( 14 & 16) at their point of intersection with the
light detector
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
21
on the receiver (24). The fan planes (26 & 28) intersect one another in a
line. and this
line is a vector i- that passes through the light detector:
r=viX"v2
Again, while r passes through the light detector, there is no information
about
the length of ~ from the vectors "~ and ~z. Therefore. we can only calculate
the
azimuth and elevation of this vector relative to the transmitter's reference
frame as
shown in Fig. 10 and expressed in the following equations:
az =tan-~~yr
Xr
el =tan-' zr
xr2 + yr2
Figs. 9 and 10 illustrate the limitation of the theodolite method, i.e. it is
only
possible to determine two dimensions of the three dimensional distance between
a
single transmitter and the receiver system (24). We can determine the two
angles to the
receiver (24), i.e. azimuth and elevation, but not the distance.
The next step in the theodolite network method is to calculate ~ vectors for
all
transmitters in the workspace and then calculate the intersection of these
vectors. If the
baseline between the two transmitters and the angles to a receiver from each
transmitter
are known, the position of the receiver can be calculated.
In the preferred non-theodolite method, we take direct advantage of the
transmitter's scanning operation, rather than treating the transmitters as
theodolites. In
Fig. 1 l, we once again illustrate a single fan plane intersecting with the
detector on a
receiver (24). Vector a is the detector's position relative to the
transmitter's origin.
From Fig. 1 l, we may make the important mathematical observation:
~~a=0
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
22
The dot product is zero because these vectors are orthogonal to one another at
the point when the beam plane intersects with the detector. ° is
perpendicular to the
plane by definition, and s is contained within the plane when the plane
intersects the
detector. As we illustrated in Fig. 9, we have two fan planes represented by
~, and ~ 2.
so we actually have two equations per transmitter (10):
~~~a=0
~2.a=0
a contains three unknowns, (x, y, z), so once again we do not yet have enough
information to calculate the third dimension. Adding a third fan beam to the
transmitter
(10) would add a third row to the equation, but this equation would not be
linearly
independent from the first two. Hence, we must add at least one additional
transmitter
to the system which is at a separate location from the first transmitter.
In Fig. 12, we have placed one transmitter ( 10-1 ) at the origin, a second
transmitter (10-2) along the x axis, and a third transmitter (10-3) along the
y axis. Only
two transmitters are strictly required for operation of the system according
to the
present invention. However, three are illustrated here to indicate that
additional
transmitters can be used to improve the accuracy of the position
determinations made
by the system. The axis setup illustrated is arbitrary but is used to show
that the
transmitters are tied together in a common reference frame. As previously
discussed,
we call this common frame the user's reference,frame to differentiate it from
the
transmitters' reference frames described previously.
Since we wish to calculate the location of a point in the user's reference
frame,
we need to specify each transmitter's reference frame in terms of the user's
reference
frame. This is accomplished with a location vector prx and a rotation matrix
Rrx for
each transmitter (10). We can then re-write the equation as:
RrX~'(P-PtX)=0
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
23
In this new equation. p is the location of a detector on the receiving
instrument
(24) in the user's reference frame and is the value we wish to calculate. RrXv
is the
vector describing the laser fan plane in the user's reference frame, whereas ~
itself
describes the laser fan plane in the transmitter's reference frame . p - prX
is a vector
from the transmitter's origin to the detector location in user's reference
frame. For
n >_ 2 transmitters, we have the following set of equations.
Roi,~ ~(p-pi~=0
Ro~,z ~ (p -py = 0
Rz~z,~ ~ ~p - pz ~ = 0
Rz~z,z ~(p-pz)=0
Rn~n,1 ~ (p - pn ~ _ ~
Rnun,2 ~ (p - pn ~ = 0
The first subscript is the transmitter number and the second subscript on ~ is
the
laser beam number. In order to write this series of equations in matrix form
so that we
can solve for p, we rearrange the equations as follows.
Rrx~~(P-pr~~=0
Rrx~~P-Rrx~~pa =0
R~G~p=R~v~prX
T -
(RAG) p = RAG ~ prX
x
We note that (R~"v~T is a 1 x 3 vector, p = y , and Rev ~ p~ is a constant. We
can then
z
put the equations of these equations into the matrix form:
CA 02366711 2001-09-21
WO 00/57131 PCTNS00/07445
24
r
(Rw~
~)
, Roi.~'
r P~
~R~V ~
~ R
z~
~ i
r x ~,z '
(RzGz P~
~) Rz~z
~'Pz
, y .
(Rz~zlz)r = Rzvz,z
pz
z
r
(Rn~n,~~ Rn~n,1
l ' Pn
r Rn~n.2
\RnV ' Pn
n,
2
The shorthand matrix notation is:
A2nx3 P3x1 -bznX~
where the subscripts indicate the dimensions of the matrices. In order to
calculate the
detector's position in the user's reference frame, we simply solve this
equation for p .
To do this, we can apply least-squares reduction to the matrix by multiplying
both sides
by Ar. We would then use a standard square-matrix solve such as LU
decomposition to
find p . Alternatively, we could also use Singular Value Decomposition to
solve for p
directly. SVD is the preferred method for finding a least-squares solution
when the
matrix is ill-conditioned, which is a greater possibility when more
transmitters are
added.
3. Calculating the Scan Angles Using Timint Measurements
As previously discussed, the two ~ vectors from each transmitter ( 10) are
based
on the corresponding scan angles, B,(t) and Bz(t), for the two laser fan beams
(14 & 16)
of the transmitter (10). We now discuss how the receiver system (24)
calculates these
two scan angles. Specifically, to calculate position for a single light
detector on the
receiver system (24), we need B,(t) and Oz(t) for each transmitter (10) in the
three-
dimensional workspace (30).
A typical receiver system (24), to be described hereinafter with reference to
Fig.
21, includes a physical tool or wand on which are a measurement tip and
photodiode
detector circuitry, a Position Calculation Engine (PCE), and a user interface.
As the
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
user moves the tool around in the workspace (30), the photodiode detector
circuitry
receives electrical pulses or strikes every time one of the planes of light or
one of the
optical synchronization strobes illuminates a light detector on the receiver
(24). Using
a high-speed timer, which preferably is built into the PCE, the system makes
5 differential timing measurements between pulses. These timing measurements
are then
used to calculate the scan angles.
Fig. 13 illustrates a typical pulse sequence for a single rotation of the
transmitter
head (7). The time between reference pulses, as indicated by T, is the period
of one
transmitter head revolution. In Fig. 13, the reference pulse is preferably
created by the
10 optical strobe assembly (6, Fig. 3). Between each pair of reference pulses,
the receiver
system (24) makes two differential timing measurements, ~t and ot2, for each
rotation
of the transmitter head (7). These timing measurements correspond to the times
at
which the light detector of the receiver system (24) detects each of the two
fan beams
from a transmitter (10).
15 Fig. 14 relates these time differences to angular differences. We take the
above
plot in time and view it as a circle of 2~ radians representing one revolution
of the
transmitter head (7). We define two angular measurements, a and a2, as the
angle
between the optical reference signal detection pulse and the fan beam 1 and
fan beam 2
detection pulses, respectively. The reader should not confuse this circle with
the actual
20 transmitter head. This circle shows a plot in time and angle as viewed by
the flight
detector on the receiver (24). Using the measured time intervals of and ot2
and the
fact that the transmitter completes one revolution in seconds, we can
calculate a and
a2 by splitting the circle into percentages, as shown in Fig. 15.
Using these percentages and the fact that there are 2n radians in a single
head
25 revolution, we get the following equations for a and a2:
a' = 2~~1- Ot~ + ~t2
T
a = 2~r~1 _ 4t~ + ~t2 + Ot~ ~ = 2~r~1 _ 4tz
T T -T
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
26
Note that the reason the time intervals are measured from beam 1 rather than
the
reference pulse is to provide backward compatibility in the receiver system
software for
other versions of a transmitter, however this could be changed as desired.
For two important reasons, a and a2 are not exactly equivalent to the 8~,~~
angles described in the transmitter model. First, in the model the two beams
are not
separated in azimuth. Rather, they scan together while overlapped as
illustrated in Fig.
9. In the actual transmitter, we separate the two beams azimuthally on the
head to so
that the receiver system can differentiate between them. We define this
angular
separation B~,Jf as described herein above. Therefore, we must subtract Oo
from a2 to
line a2 up with a . Do is determined through factory calibration and is part
of the
calibration data stored in memory (2: Fig. 3).
Second, the a and a2 angles are measured relative to the reference pulse as
shown in Fig. 14. If we relate this measurement to the transmitter model, then
the,front
of the transmitter-its local x-axis-is the point in the head's rotation when
the
reference pulse fires. Therefore, the reference pulse also defines the zero-
azimuth
plane, since azimuth is measured from the transmitter's x-axis. If a single
transmitter is
to be used for azimuth and elevation calculations, it is sometimes desirable
to set the
point on the transmitter (10) where the detector's azimuth will be zero. We
establish
this set point with a factory-calibrated constant called ORP . As shown in
Fig. 16, ORP is
the angular separation between the desired front of the transmitter and the
occurrence of
the reference pulse. The sign of ORP is determined as illustrated. For most
transmitters.
oRP is set to zero because azimuth-elevation measurements relative to a single
transmitter are not required. Therefore, we convert a and a2 to the desired
the scan
angles, 0, and 02, by using the following equations:
D~ = a ~ + DRP
D1 _2~~~-4t~T4t2~+DRP
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
27
B,
B' - 2~ ~ 1 _ Ot, ~ + err - Bolt
T
To summarize, these equations are used to calculate o, and BZ values for each
transmitter that illuminates a light detector on the receiving instrument
(24). Therefore,
if there are two transmitters (10) set up in the workspace (30), four 0 angles
will be
calculated for each detector, and hence four ° vectors will be
calculated. Three
transmitters would result in six ~ vectors, and so on. Using all of the
calculated
vectors, the receiver system (24) then performs the matrix solve described
above for
each light detector on the receiving instrument (24). If more than one light
detector is
present on the receiving instrument (24) and spaced from the other light
detectors on
the instrument (24), more accurate position information can be obtained by
individually
and separately calculating the position of each light detector on the
instrument (24) and
using those results to define the position of a particular point, e.g., the
wand tip, of the
receiving instrument.
4. I2eceivin~ Instrument
In the spatial position measurement system shown in Fig. 12, the receiver
system (24), which includes the data gathering apparatus, may comprise the
portable
wand-shaped receiving instrument (70) shown in Fig. 17A. Receiver (70)
includes a
rod or wand-shaped section (72) and a handle section (74). Rod section (72)
terminates
in a sensor point (76) which is utilized to touch or contact a position within
the
measurement field for which x-y-z data is to be generated.
Referring now to Fig. 17B as well, the rod section (72) includes two
spherically
shaped, spaced apart optical or light detectors (78 & 80) and an electronics
section (82).
The proper alignment and positioning of the sensor point (76) with the
detectors (78 &
80) can be mathematically expressed as:
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
28
p. = d. , Porrv - Pnsrn + P
rro m ner-2
IPnr_rz-Posr~I~
where:
P",, = is the position of the tip 76
P",,,., = is the position of the detector 80 closest to the wand
handle 82
P",;,.2 = is the position of the detector 78 closest to the tip 76
d",, = is the distance 72 from detector-2 78 to the tip 76.
Proper alignment and spacing of the detectors (78 & 80) relative to the sensor
tip (76) along projection line (84) as shown in Fig. 17B is an important
aspect of the
present invention as it permits a user to take accurate measurements within a
workspace
or measurement field without having the receiving instrument (24), i.e., wand
(70),
positioned exactly perpendicular to a transmitter reference plane (See Fig.
12) or any
particular user reference plane. In fact, utilizing a wand tip (76) designed
as described,
permits a user to position the receiver/detector tip (76) and
receiver/detector rod section
(72) without concern for any particular alignment.
Referring now to Fig. 17C, the hand grip section (74) of portable receiver
(70)
of Fig. 17A will now be described. Receiver handle section (74) includes a
trigger
switch (88) to activate the receiver (70) to initiate x-y-z data generation in
response to
illumination of detectors (78 & 80) by two or more transmitters ( 10). This x-
y-z
position data may be generated when electric signals emanating from detectors
(78 &
80) activate or are inputted to an internal programmed computer (90), i.e. the
Position
Calculation Engine (PCE). The PCE (90) is schematically diagrammed for
reference in
Figs. 20 and 21.
The x-y-z position data corresponding to the position of the sensor point tip
(76)
is calculated by the PCE (90) when the trigger (88) is activated. This
position data may
be displayed in a display panel (92) and/or transferred to another data
processor, not
shown, via output data port (94), as will be understood by those skilled in
the data
processing arts.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
29
The output data port (94) is a serial port and may be an optical or wired
port.
Preferably, the port (94) is an optical port and can be used to receive the
calibration data
stored in each transmitter (10) via the optical serial port (301; Fig. 3) or
wired serial
port (302; Fig. 3) of each transmitter (10).
The hand grip portion (74) of the receiving instrument (70) also preferably
includes the power supply (96) for the instrument (70). The power supply (96)
preferably includes a rechargeable battery pack (96A) that feeds power to the
instrument (70), including the PCE (90), communication port (94) and display
device
(92).
5. Pulse Tracking
This section describes how the receiving instrument (24, 70) tracks its
location
as it is moved through the workspace (30). What is described explains, amongst
other
things, how the wand (24) uses the received laser beam strikes and strobe
pulses from
each transmitter ( 10) to determine its location.
A. Summary
The PCE (position calculation engine) (90), as shown schematically in Figs. 20
and 21, contains firmware which translates incoming pulse times into physical
position
in space. This code is partitioned into several major blocks, or tasks. One of
the tasks
performs the job of categorizing and associating these pulse times upon their
arrival.
This process is known as tracking. The following flowcharts and text describe
the
operation of the tracker. There is a separate tracker or tracking function for
each
physical light detector on the receiving instrument (70).
B. Background
As described above, from the perspective of any one fixed location in the
workspace, each transmitter in the system emits a periodic stream of light
pulses.
Within one period, three pulses are emitted from each transmitter. Each
transmitter
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
maintains a unique and stable period, or rate of emission. This allows the
firmware in
the PCE to discriminate between transmitters.
C. Tracking
5 The tracking algorithm maintains an individual pulse tracker for each
emitter,
e.g., laser light source, on each transmitter. When a pulse is received by the
firmware,
its timing characteristic is compared with each of the pulse trackers in the
system in an
attempt to associate it with a known pulse train. There are two distinct
phases to the
tracking process; synchronization and tracking. During the sync phase, an
attempt is
10 made at pairing unknown pulses, within the preset parameters of the
existing pulse
trackers. Once this is accomplished, the tracking mode is entered. whereby
successive
pulses are associated with their assigned pulse trackers. If a sequence of
assignment
failures occurs, tracking mode is exited, and re-synchronization is attempted.
Once synchronization is achieved, a post-sync check is performed to ensure
that
15 the pulses have been identified in the correct order. If an inconsistency
is found, the
pulse trackers are swapped. This helps to avoid unnecessary re-synchronization
cycles.
Under some conditions, environmental interference such as noise and multipath
reflections are experienced. A noise pulse is defined as a pulse which cannot
be
correlated with the period of an existing pulse tracker. Multipath occurs when
20 unexpected pulses are detected at the period of a transmitter.
Fig. 18 provides a diagram of the data flow accomplished in the tracking
function. The flow represented in Fig. 18 from the detector (78) to the PCE
(90) will be
duplicated for each light detector on the instrument (70). As shown in Fig.
18, the
detector (e.g., 78) outputs signals indicative of detection of an optical
pulse to a
25 tracking system (100), which will be described in more detail below. The
tracking
system (100) outputs data to the PCE (90). The PCE (90) also has received the
calibration data (9) from all the transmitters in the system.
Using the output of the tracking system (100) and the calibration data (9),
the
PCE (90) generates the x,y,z position ( 1 O l ) of the detector (78) in the
transmitter's
30 reference frame using the mathematical model described in detail above.
This
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
31
information is then transmitted using a data transfer protocol ( 102) over a
communications link (103) to a user interface system (104).
The user interface system ( 104) may either be mounted on the receiving
instrument (70) or may be a separate system in communication with the
instrument (70)
via communications link ( 103). Within the user interface system ( 104), the
data ( 1 O l )
is received ( 105). The data ( 101 ) is then mapped from the transmitter
reference frame
in which it was created into the user's reference frame ( 106). A user
interface ( 107) is
also part of the system (104) and allows the user to define the user's
reference frame
and the consequent mapping function performed in block ( 106). The data is
then
mapped for display ( 107) on a display device ( 108). The display device (
108) and
display mapping block ( 107) are also under control of the user ( 109) through
the user
interface ( 107). Consequently, the user can have the mapping block ( 107) map
the data
into the most useful format. For example, the data may be mapped by block (
107) into
a numeric display of x, y and z values in the user reference frame for display
on the
display device (108). Alternatively, the data may be mapped by block (107)
into a dot
on a map displayed on the display (108) that indicates the current position of
the
detector (78). Any mapping of the data that is most useful to the user (109)
can be
effected by block (107) prior to display (108).
Fig. 19 illustrates in greater detail the tracking block ( 100) of Fig. 18. As
shown in Fig. 19, each light detector in the system will output a signal (
110) including
square pulses indicative of the detecting of a light pulse from the
transmitter having
been received and detected by the detector. For each pulse, a Field
Programmable Gate
Array (FPGA) (111) will output a signal specifying a time (T,) corresponding
to a
rising edge of the detected light pulse and a time (TZ) corresponding to a
falling edge of
the detected light pulse. These times (T, and TZ) are transmitted to the
tracking
software ( 112), typically executed by the PCE (90).
The tracking software, as will be described in more detail below, associates
each
pulse, defined by times (T, and TZ), with a particular transmitter (10). This
is done as
described above based on the time at which the pulse is received as matched
against a
time a which a pulse from a given transmitter is expected, knowing that
transmitter's
rotational velocity and consequent synchronization pulse duty cycle.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
32
Once each detected pulse is associated with a transmitter, that pulse is also
then
associated with the calibration data (9) for that transmitter. Given the
calibration data
(9) for the transmitter (as described above) and the times (T, and T,) for the
two fanned
light beams and the synchronization pulse for that transmitter, the PCE (90)
can
calculate the 8 value, as shown in Fig. 7, for each the plane of light which
swept the
detector (e.g, 78). Consequently, the position of the detector (78) can be
calculated
using the mathematical model described above.
D. Flowchart Narrative
(I ) Detector: : Entry
Fig. 22 illustrates the main background loop for the system. It is an RTOS
(Real Time Operating System) task, and once launched, continues to execute in
an
infinite loop. Referring to the flowchart (Fig. 22), the following sequence of
events
takes place:
1 ) Check for pulse time events in the queue. These are passed in from the
hardware
FIFO in the FPGA, via an interrupt service routine. If there is another event
in the
queue, add it to the accumulated total time, and pass it through to the update
routine. The update routine generates pulse intervals from the incoming pulse
times.
It also associates the pulses with a particular transmitter.
2) Once the queue has been emptied, run through the list of transmitters to
make sure
that new intervals have been calculated for every transmitter. If they have
not, note
a timeout, and mark the associated transmitter as invisible. This will be
displayed
on the user interface. Otherwise, store the interval data for use in
subsequent
position calculations.
3) Once all transmitters have been updated with interval information, check to
see if
theta calculations have been enabled. Thetas are the equivalent angles between
the
lasers and the reference pulse based upon pulse times and transmitter
rotational rate.
This is a basic calculation required for any subsequent calculations, and must
be
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
33
enabled for any output to be generated. If they are disabled, go back to
looking for
more pulse times.
4) Check that all interval data is valid. If not, clear the theta and
azimuth/elevation
data and check for new pulse times.
5) Perform the calculation to convert angles (thetas) into intervals.
6) If azimuth and elevation calculations are enabled, generate these numbers.
7) If position calculation is enabled, calculate the position (x,y,z) of this
detector.
Send a message to the measurement tool indicating that a position has been
generated for this detector.
j2) PulseTrackMana er~.~.~ Update
The update function of the present invention is shown in flowchart form in
Figs.
23A and 23B. This routine receives pulse times from the Detector "Entry"
routine of
Fig. 22, and associates them with pulse trackers. An individual pulse tracker
exists for
each emitter (i.e., fanned laser light source) in the system, preferably two
per
transmitter and each reference pulse generator (one per transmitter). This
accomplished, an attempt is made to reconcile a set of three pulses and
associate them
with a single transmitter.
The sequence of events is as follows, until a pulse is identified:
8) If currently in synchronization mode, call the synchronization function.
Otherwise,
call the track function.
9) If noise was detected, set the noise flag.
10) If the sync or track function was successful, check to see if a previous
pulse is to be
reconciled. If so, call the reconcile routine. This will perform pulse pair
housekeeping.
11 ) If this pulse has already been processed by another pulse tracker, a
collision has
occurred. Set the "collision" flag.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
34
12) Otherwise, save the pulse for later. It will be used to reconcile the next
pulse from
this emitter.
13) If the sync or track function was not successful, a timeout may have
occurred.
Reconcile the pulse anyway so that the interval from the previous pulse gets
recorded. Continue with the next tracker.
14) Once all pulse trackers have been attempted, if the pulse has been
identified. exit.
15) Otherwise, go back to the top of the list of pulse trackers.
16) For each pulse tracker, check to see if we have exceeded the maximum
number of
sync attempts. If so, reset this tracker, and go to the next one. If it has
recently
been sync'd go on to the next one.
17) If multipath interference has not been detected, go on to the next pulse
tracker.
Otherwise, clear out the unknown pulse list, and insert this one into it.
13) PulseTrack: : Synchronize
The synchronizing function of the present invention is shown in flowchart form
in Fig. 24. The synchronize routine attempts to associate an incoming pulse
with an
existing pulse tracker. Once a regular pattern of pulses has been established,
the system
leaves sync mode and enters track mode, whereby the "track" routine is invoked
instead
of "synchronize". As long as "track" continues to successfully classify
incoming
pulses, the sync function is no longer called. Should the tracker encounter
difficulty in
recognizing pulses as coming from a particular transmitter within the system,
sync
mode is re-invoked, and the system attempts to re-synchronize the pulse
stream.
Loss of synchronization may occur for number of reasons, including noise or
reflections in the environment, or rapid movement of the receiving instrument
(24,70).
As shown in Fig. 24:
1 ) New pulse time of arrival is registered. If the unidentified pulse list is
empty, there
is nothing with which to pair it. Return as such.
2) Has this pulse been paired yet (with another pulse tracker)? If so, is the
interval
from previous pulse within the sync window? If so, the previous pulse may have
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
been noise. Resync. A sync window is defined as the time it takes for the
transmitter head to complete approximately 0.2% of a rotation.
3) If last pulse was NOT noise, is this one within the cycle time of this
tracker? If not,
return "already paired" If so, try to find a match in the unknown pulse list.
Note
5 that if it has not been paired yet, we also end up looking in the unknown
pulse list.
4) If a match is found, see if the match has been already paired. If so,
resynchronize.
Otherwise, save as a match, and place this one in the unknown pulse list, so
subsequent pulses can try to match this one
10 ~4) PulseTrack.~: Track
Once we are in tracking mode, each pulse gets passed to the pulse trackers.
The
tracking function is shown in flowchart form in Fig. 25. If any pulse is
received within
an expected range of when the next pulse should occur based on the known
parameters
of the transmitter, that pulse is accepted as the next pulse for that
transmitter and is
15 accepted by the associated tracker. Each fan beam and the synchronization
pulse from
each transmitter has its own tracker in the receiving system.
As shown in Fig. 25, tracking is performed as follows:
1 ) Can we predict a pulse within a reasonable tracking window? If not, has it
been too
long since the last pulse was registered? If so, resynchronize and return
"timeout".
20 If no timeout yet, just return "not identified". A track window is defined
as the time
it takes for the transmitter head to complete approximately 0.2% of a
rotation.
2) If we can predict the pulse, check to see if noise is present (In this
case, noise is
defined as multiple pulses within the "track window"). If so, reset pointer to
previous pulse, and return "noise".
25 3) If no noise is detected, store current pulse information, and return
success.
f5) PulseTrack.~: Predict
The predicting routine of the present invention is shown in flowchart form in
Fig. 26. This routine interacts with the "Track" routine described above by
attempting
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
36
to predict when the next pulse from any given pulse source should occur. It
then tries
to correlate this expected time with the next actual pulse time. within a pre-
determined
tracking window.
As shown in Fig. 26, the Predicting routine is performed as follows:
1 ) Are there two pulses within one tracking window? If so, noise exists;
return.
2) Based on previous pulses, is this pulse within a reasonable tracking window
of one
or more most recent pulses (Several pulses may be missed before tracking
fails; i.e.:
as may be experienced during a temporary blockage)?
3) If it is, return "success". Otherwise, return "outside window".
(6) Pulse Track: : Reconcile
The Reconciliation function of the present invention occurs in two phases. The
first part is pulse tracker reconciliation (shown in flowchart form in Fig.
27), which
performs pulse pair housekeeping within a particular pulse tracker. The other
is
transmitter level reconciliation (shown in flowchart form in Fig. 28), which
associates a
set of three pulses from a single transmit period with a transmitter.
As shown in Fig. 27, pulse tracker reconciliation proceeds as follows:
1 ) Calculate interval from previous pulse. Calculate the tracking cycle.
2) Store in the saved pulse list.
3) If in sync mode, and not paired yet, return.
4) Otherwise, call "post-sync" to do cleanup. If it is unsuccessful, return as
such.
5) If it was successful, switch to "track" mode.
6) If we were in "track" mode already, check to see if it was recently
achieved. If so,
increment the counter. This affects the way the "update" routine handles the
data.
7) Reconcile at the transmitter level.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
37
~7) Transmitter: : Reconcile
As shown in Fig. 28, transmitter reconciliation is performed as follows:
I ) If this is the reference pulse, check that there are two laser pulses in
the list. If so.
calc a new cycle time and return. Otherwise, just return.
2) If it is a laser pulse, make sure there are good pulses from all 3 pulse
trackers for
this transmitter.
3) Using the pulse time data, calculate the intervals. Save the interval times
for use by
position calculation routines.
j8) Transmitter: : Po stSynchronize
The Post Synchronization routine of the present invention is used as a final
check to make certain that three trackers have synchronized to a single
transmitter in a
consistent manner and is shown in flowchart form in Fig. 29.
1 ) First, check that all three trackers have completed synchronization.
2) Using the reference pulse period, check that the cycle time is reasonable.
If not,
then re-enter the synchronization mode and return an error.
3) Ensure that the pulses have been identified in the correct order. If not,
then swap
them based upon time of arrival.
4) Check that we're locked onto the correct set of pulses. If not, then re-
enter the
synchronization mode and return an error.
5) Otherwise, return "success".
(9) PulseTrack.~: isMultipath
The multipath function of the present invention checks to see if multipath
interference exists; that is, whether some pulses reaching the detectors are
reflections of
the emitters rather than "line of sight" pulse strikes. This is accomplished
by
comparing the pulse currently being classified with previous pulse times and
is shown
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
38
in flowchart form in Fig. 30. As shown in Fig. 30, the multipath routine is
performed
as follows:
1 ) Clear any previous multipath indications in the status word.
2) If the unknown pulse list is empty, multipath is not detectable.
3) Compare the pulse time of this pulse with the pulse times of prior entries
in the
unknown pulse list. If this pulse is a multiple of cycle times away from a
previously received (but unindentified) pulse (within a sync window), then
multipath interference may be present. Increment a counter.
4) If the counter exceeds a preset limit, multipath exists. Set the multipath
status bit,
and re-enter synchronization mode.
6. Least Sguares Resection
The preceding material describes the operation of the position determining
system
of the present invention. However, before the system is fully operational. It
is
necessary to establish and define the user's frame of reference so that the
position
information generated by the system is actually useful to the user. There are
at least
two different means of setting up the system and defining an initial user's
frame of
reference: (1) Least Squares Resection and the (2) Quick Calc Method. The
Quick Calc
method is easier to perform and requires less time and effort than Least
Squares
Resection. Consequently, the Quick Calc method is preferred for purposes of
the
present invention and will be described in detail below. However, for
completeness,
the Least Squares Resection method of setting up the system of the present
invention
will also now be explained.
The Least Squares Resection (LSR) algorithm used with the present invention
ties
the transmitters in the workspace together into a common mathematical
reference
frame, from which position calculations are made. Whereas the Quick Calc setup
algorithm creates a new reference frame for the transmitters, LSR ties the
transmitters
into a previously established reference frame defined by coordinates already
in the
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
39
workspace. Figs. 12 and 31-1 show an example of a user reference frame in
which the
transmitters are arbitrarily placed.
Fig. 31-1 shows the same three transmitters (10-l, 10-2 & 10-3) shown in Fig.
12, which are arranged in an arbitrary coordinate system relative to each
other as was
described with regard to Fig. 12. Each transmitter in the workspace has an
associated
position vector p~ and rotation matrix R~ that relate the transmitter's local
reference
frame to the user's reference frame. p~ and R~ are used in the position
calculation
algorithm.
In order to perform an LSR with the position measurement system of the present
invention, the user must level a receiving instrument (24) with a single light
(or other
signal) detector over at a minimum of three known coordinates in the workspace
as
shown in Fig. 31-1. The receiver system (24) makes scan angle measurements
(measurements of B, and 02 ) for the detector at each location.
After all of the measurements are taken, the algorithm calculates an
approximate
guess location and orientation for each transmitter (10) relative to the user
reference
frame. The algorithm then individually calculates the location and orientation
of each
transmitter (10) using Newton-Raphson iteration. The reader should note that
this
approach is different from the preferred Quick Calc Setup algorithm to be
described
below. In the Quick Calc Setup method, all transmitter locations and
orientations are
solved for simultaneously.
A. Least Sguares Resection Al orb
We will now describe the mathematical algorithm used in the LSR method of
the present invention.
~(1 ) LSR Observations
As stated above, the first step in LSR is for the receiver system to take scan
angle measurements with a leveled one-detector wand (24) at a minimum of three
locations in the workspace for which the position is already known in the
user's
reference frame. For each receiver (24) location, the receiver system records
the scan
angles D, and ~2 from each transmitter (10) to the detector on the receiver
(24). These
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
two scan angles are then converted to their corresponding plane vectors v1 and
v2 . We
call the measurement at each wand (24) location an LSR observation.
For each observation, the user enters the known coordinate ~xr;P, yr;P,zt;P)
and
which the wand's tip (24) has been placed as shown in Fig. 31-1. Since the
wand (24)
5 is leveled over this point, the receiver system calculates the coordinate of
the tool's
detector by making the assumption that the z-axis in the user reference frame
is parallel
to gravity. Since, the detector (10) is mechanically located at a distance
toolLength
from the wand tip, we can write the detector location c,o~ as:
X tiP
10 cn~ = yt;P (3.1 )
zt;P + toolLength
For m different wand (24) locations and n different transmitters (10), we have
the
following data set.
C1 V1,1,1 V1,1,2 ... Vl,n.1 Vl,n,2
15 C2 V2,1,1 V2,1,2 ... V2.n.1 V2,n,2
Cm Vm,l,t Vm,l,2 ... 1%m,n,1 Vm,n,2
The subscript convention is -,o~, rx, beam ~ The first subscript indicates the
wand location,
i. e. this subscript counts observations. The second indicates the
transmitter, and the
third indicates beam 1 or beam 2 on the transmitter ( 10).
Since LSR handles each transmitter (10) separately, the data set for a single
transmitter
(10) is:
C1 Vl,tx,1 Vl,bc,2
e2 ~2,rx.1 . ~2,a,z (3.2)
Cm Vm,tx.1 Vm,bc,2
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
41
Furthermore, it is not required that each transmitter (10) use the same four
observations in the LSR algorithm; we simply do this for convenience in the
operation
of the system. Herein lies another difference between LSR and the preferred
Quick
Calc Setup method. LSR is a resection algorithm because is locates and orients
a single
transmitter (10) in a preexisting reference frame, just like a total station
is aligned to a
set of control points. Quick Calc Setup, by contrast, locates and orients all
transmitters
relative to one another in an arbitrarily defined reference frame.
Since LSR deals with each transmitter individually, we will describe the LSR
setup
process for a single transmitter. Those of skill in the art will understand
that this
process is simply repeated for each transmitter (10) in the system.
~2) Gztess Generation
The LSR algorithm must solve for the locations and orientations of the other
transmitters in the workspace. Therefore, we are solving for the following
quantities:
x~
Prx = Yrx
zrX
RrX = Rx(nc~ ) RY(rYrX ) Rz(~rx )
1 0 0 cos(ryrX ) 0 sin(ryrX ) cos(rzrX ) -sin(rzrX ) 0
0 cos(rxrX ) -sin(rxrX ) 0 1 0 sin(rzrX ) cos(rz~ ) 0
0 sin(rxrX ) cos(rxrx ) -sin(ry~ ) 0 cos(ryrX ) 0 0 1
The order of rotation used is called z y-x,fixed angle. This rotation order is
chosen because the z rotation component is typically the largest rotation for
a
transmitter; i. e. the transmitters are usually close to level. Any rotation
order can be
used, though, as long as RrX is consistent between the LSR algorithm and the
position
calculation algorithm. We define a solution vector x to include the 6 unknowns
for the
transmitter:
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
42
r
X = ~X rx Yrx zr~ ~ r~ rYtx
Before performing a Newton-Raphson iteration, the LSR algorithm generates a
guess for this x vector based on the assumption that the transmitter ( 10) is
leveled in
the x-y plane. In other words, the x vector generated by the guess routine is:
X = ~X rx Ytx O O O lzrx ~r
Using this simplification, a diagram for two LSR observations (Ptl & Pt2)
relative to the transmitter (Tx) can be drawn as shown in Fig. 31-2. This Fig.
shows
two triangles whose common base is parallel to the x-axis. Since the 0 angles
(scan
angles) are measured relative to the front of the transmitter and rzrx is the
angle between
the x-axis and the front of the transmitter when the transmitter is in the xy-
plane, we
have the following formulas for the two ~3 angles:
lj, _ ~,.tx,, ~ ~rx
~z = ~z.rx., + ~rx
The subscript notation on o is the same as was used on the - vector,
specifically
a n~, ~, beam ~ When the transmitter is in the x-y plane, 0 roe, ~, ~ =
B,o~,rx,z . Since this is our
assumption, we simply pick the first 0 angle to calculate /~. An estimate for
the rzrx
angle is determined iteratively, which we will describe later.
Using the law of sines, we make the following observation about the two
triangles.
a~ b~
sin,~~ sin(90-~3~)
az bz
sin ~3z sin(90 - /3z
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
43
Since sin(90 - ~3) = cos(~3) , aro~ = Yn~ -Y~ , and b,o~ = xro~ - x~ and we
can rewrite
the equations as follows.
Y~ -YrX _ x~ -x~
sin R~ cos /~,
Yz -Yr~ _ xz -xr~
sin ~3z cos ~z
Rearranging we get:
x rx sin ~, - yrX cos ~3, = x, sin /3, - y~ cos /~~
x~ sin~3z - y~ cos,Qz = xz sin,l3z - yz cos/3z
Or in matrix form:
sine -cos~3, x~ x~sin~3~-y~cos/~,
[sin /~z -cos ~3z, [yrX ) [x z sin ~3z - Yz cos ~3z
This matrix can then be solved for the location estimate of the transmitter
(xrx, y~ ). To
determine if the solution is valid, the following two inequalities are
checked.
x' x rx > 0
cos ~,
xz xrX > 0
cos /~z
These inequalities must be true in order for the direction of /~~ and ~3z to
match
the direction shown in Fig. 31-2. We do not need to check the y values since
they do
not provide any additional information.
In actuality, we have three or more LSR observations. To accommodate this
extra data, the guess routine takes groups of three adjacent observations and
calculates a
transmitter location estimate for each of the three observation combinations.
The sum
of the distances between these three transmitter location estimates is defined
as the
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
44
residual error, and the guess routine picks the set of observations with the
lowest
residual error. The final transmitter location (xr~, yrx ) is then the average
of the three
transmitter locations from this chosen observation set.
Finally, we describe how the rzrx angle is calculated, since /~, and /~2 are
based on
rzrX . Unfortunately, we do not have enough information to determine rzrX
directly.
Therefore, for each set of observation combinations, the guess algorithm tries
different
rzrX values in increments of 10° starting with 0° and ending
with 350°. Using the
residual check at each step, the best rzrx is chosen. Those rzrx values that
are
mathematically impossible with fail the inequality checks given above.
~3) Solution Calculation
As previously stated, the Least Squares algorithm uses Newton-Raphson
iteration to find the best solution for the given setup. Newton-Raphson uses a
fill
function that evaluates the function to be solved for a given solution vector
x
f(X)=o
where x is defined above. LSR is essentially just a reverse position
calculation, so the
fill function is simply the plane equation described in the position
calculation herein
above described:
Rrxv~(p-prx)=0
When used for position calculation, the quantity for which a solution is found
is
the location of the detector p . All other quantities are known. In LSR, the
quantities
for which a solution is required are the location of the transmitter prx and
the orientation
of the transmitter RrX . The vector p is simply the location of the LSR
observation cm,
and the vector - is the vector of the measured beam plane. Therefore, for m
LSR
observations, we can write the set of equations to be solved:
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
Rtxp,~.~'(C~-l7rx) 0
Rrxv~,t~,2 ' (C~ - Ptx ) 0
R~v2.rx,~'(~2 -Prx) 0
- Rtxv2,tx,2 ' (C2 - Prx ~ 0
Rtxvm,tx.1 ' Cm - htx 0
Rtxum,tx,2 ' (Cm - l~tx )
The Newton-Raphson iteration adjusts R~ and p~ until F(X) is approximately
zero.
The guess algorithm and subsequent Newton-Raphson iteration are performed
5 for all transmitters in the workspace.
7. Quick Calc Setup Method
Having fully described the LSR method of setting up the system of the present
invention , we will now describe the preferred Quick Calc Setup method. This
10 procedure determines, amongst other things, the coordinate system desired
by the user
and the wand's location in that coordinate system.
A. Introduction to the Quick Set-Up Procedure
This procedure ties the transmitters in the workspace together into a common
mathematical reference frame that is or can be readily mapped to the user's
desired
15 reference frame and from which position calculations are made. Fig. 32-1
below shows
an example of a user reference frame.
Each transmitter in the workspace has an associated position vector r p,Y and
rotation matrix R,r that relate the transmitter's local reference frame to the
user
reference frame. p,Y and R,x are used in the position calculation algorithm.
20 In order to perform a Quick Setup with the position measurement system of
the
present invention, the user simply places the two-detector (78 & 80) receiving
instrument (24) in a minimum of three different locations in the workspace as
shown in
Fig. 32-2. The receiver system makes scan angle measurements ( B measurements)
for
both detectors (78 & 80) on the tool at each location.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
46
After all of the measurements are taken, the algorithm calculates vectors from
transmitter (10) to detector (78 or 80) for each of the detector measurements.
Then the
algorithm calculates two rough guesses for the locations and orientations of
the
transmitters ( 10) relative to one another. Using these guesses as a starting
point, the
algorithm performs a Newton-Raphson iteration to precisely locate and orient
the
transmitters ( 10) in a common user reference frame.
B. Quick Setup Algorithm
(I ) Observation Collection
As stated above, the first step in the Quick Setup algorithm is for the
receiver
system to take scan angle measurements of the two-detector receiving
instrument (24)
at a minimum of three locations in the workspace. For each receiving
instrument
location, the receiver system records the scan angles, B, and B2 , from each
transmitter
( 10) to each detector (78 & 80) on the wand (24). These scan angles are then
converted
to their corresponding plane vectors v1 and v2 . The plane vectors are then
converted to
a vector from transmitter ( 10) to detector (78 or 80) as shown in Fig. 32-3
yielding:
r=vlxv2
The r vector is relative to the transmitter's local reference frame as shown
in
the Figures. This vector passes through the detector (78 or 80) but contains
no distance
information. Since there are two detectors (78 & 80) on the receiving
instrument (24),
there are two vectors calculated for each wand location. Therefore, for m
different wand
locations and n different transmitters (10), we have the following data set.
rl,l.l ~~1.1,2 ... rl,ir,l rl,n,2
X2.1.1 r2.1,2 ... r2,a.l ~2.n,2
rm,l.l rnr,l'2 ... ~m.n.l ~m,n,?
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
47
The subscript convention is i;«.~_~ . The first subscript indicates the wand
location. The
second indicates the transmitter, and the third indicates the detector on the
wand (24)
(Detector 1 (78) on the wand (24) is the detector closest to the handle.) We
call each i-
vector a target observation.
In addition to target observations, the receiver system also records the
distance
between the detectors (78 & 80) on the receiving instrument (24). This
distance is used
to provide scale to the iterative solution. The guess routines and Newton-
Raphson
iteration will use this distance for each location of the tool. We call each
instance of this
distance a scale bar observation. Therefore, for n different tool locations,
we have n
different scale bar observations. We may refer to the receiving instrument
(24) as a
scale bar in this document.
12) Guess Generation
In the Quick Setup algorithm, the user reference frame is a somewhat arbitrary
frame that simply ties all transmitters in the workspace together. Once this
frame is
established, it is possible to make position calculation measurements relative
to the
frame's coordinate system. It is also possible to transform the coordinate
system into
another coordinate system, possibly a surveyed reference frame. The user
reference
frame is arbitrary because we can choose any origin and any orientation for
the
2Q principal axes. The only requirement is that the various transmitters in
the workspace
are properly tied together.
As shown in Fig. 32-4, the user reference frame chosen by Quick Setup puts the
first transmitter (10-1) at the origin with no rotation; i.e. the first
transmitter's local
reference frame is used as the user reference frame.
Since we have fixed the location and orientation of the first transmitter (10-
1),
the algorithm must solve for the locations and orientations of the other
transmitters
(e.g., 10-2 & I O-3) in the workspace. Therefore, we are solving for the
following
quantities:
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
48
x,.r
P,.r = Y,.r
Z,.Y
R,., = R.x ~rx,_r ~ Rr ~rY,.r ~ R_ ~rz,.r
1 0 0 cos~ry,Y ~ 0 sin~ry,r ~ cos~rz,Y ~ - sin~rz,l, ~ 0
R,,. = 0 cos~rx,,Y ~ - sin~rx,r ~ 0 1 0 sin~rz,.x ~ cos~rz". ~ 0
0 sin~rx,x ~ cos~rx,Y ~ - sin~ry,Y ~ 0 cos~ry,T ~ 0 0 1
where the first transmitter is given by:
0
p, = 0
0
1 0
0
R,=0 1
0
0 0
1
The order of rotation used in the rotation matrix above is z y-x fixed angle.
This
rotation order is chosen because the z rotation component is typically the
largest
rotation for a transmitter; i.e. the transmitters are usually close to level.
Any rotation
order can be used, though, as long as R,X is consistent between the Quick
Setup
algorithm and the position calculation algorithm. We define a solution vector
x to
include the unknowns for all n transmitters:
1T
I S x - [x2 Y2 Z2 ra 2 ry2 rZ2 . . . xn Yn Zn rxn ~.l'n rZn J
The Quick Setup algorithm generates two different guesses for the x vector
above. Both guess routines are based on the following assumptions:
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
49
1. The transmitters are leveled in the x-y plane and are at the same z-axis
location.
2. The scale bars are placed vertically (perpendicular to the x-y plane) at
each
location.
3. The middle of each scale bar is located in the x-y plane.
These assumptions allow the setup problem to be simplified so that a solution
for the position and orientation of the unknown transmitters can be calculated
directly
rather than through iteration. These guesses are then used as a starting point
for the
Newton-Raphson iteration. Using the assumptions above, the following
information is
calculated for each scale bar and each transmitter:
tn~,,a. A vector in the direction of the center of the scale bar center. The
vector's
length is approximately the distance to the scale bar center (exactly if the
assumptions above are met).
The included angle between the two Y vectors from the given transmitter to
the top and bottom detectors on the tool at the given tool location.
The first index on these quantities is the receiving instrument location, and
the second
is the transmitter number.
The first guess routine uses sets of three scale bar observations to guess the
positions and orientations of the unknown transmitters. In this routine, only
the
t vectors calculated above are required. This guess routine is based on the
intersection
of three spheres in space. We show these spheres from in a plan view in Fig.
32-5. We
define a sphere for each scale bar observation. The center of each sphere is
located
using the corresponding t vector from the first transmitter ( 10-1 ). Remember
that the
first transmitter (10-1) is our reference transmitter, and the final user
reference frame is
going to be based on this transmitter (10-1). The radius of each sphere is the
distance
from the unknown transmitter (10-?) to the scale bar observation, as given by
the
following:
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
~Inc.cr tloc,rx
When the three assumptions above are met, the intersection of the three
spheres
5 will exactly locate the unknown transmitter (10-?) relative to the first
transmitter (10-1),
thereby giving us (x,.r, y,r, z,r ) . Since these assumptions are never
exactly met, we
actually get an approximate location for the unknown transmitter (10-?) (See
Fig. 32-5).
The next step in this first guess routine is to calculate the orientation of
the
unknown transmitter ( 10-?) relative to the first transmitter ( 10-1 ). Since
we have
10 calculated a guess location for the unknown transmitter (x,r,y,r,z,r) , we
can also
calculate vectors that locate the scale bars from the unknown transmitter as
shown in
Fig. 32. In this Fig., we define the s vectors as:
'Sloc,rx - l~oc,~ -prx
These vectors are similar to the t,"~.,,.r . vectors from the unknown
transmitter ( 10-?),
except that these vectors are relative to the first transmitter's reference
frame. The t,~,~._,a.
vectors are relative to the unknown transmitter's reference frame. Therefore,
we have
three vectors from each reference frame that describe the same three points in
space.
We can then calculate the z-y-x fixed angle rotations, rx,r,ry,r,rz,x .
This three-sphere guess routine calculates guesses for adjacent sets of three
scale bars and then averages the guess values for each unknown transmitter (10-
?). In
other words, the routine uses the scale bars at locations l, 2, and 3 for the
first guess,
the scale bars at locations 2, 3, and 4 for the second guess, and so on. This
averaging
helps reduce the effects of errors in the calculations when the three
assumptions above
are not met.
The second guess routine is similar to the first, except that the second
routine
finds a two-dimensional guess using two intersecting circles rather than a
three-
dimensional guess using three intersecting spheres. The second routine
calculates
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
51
guesses for x,Y , y,L. , and rz,Y thereby only affecting position and
orientation only in the
x-y plane only. The z". , rx". , and ry,r are set to zero for this guess.
The first step in this two-dimensional guess is to pick the two scale bar
observations that have the largest included angle ~3,"~.,r,. , between the two
detectors on
the scale bar. The two scale bars are chosen such that /3,"~.,,r . is large
for both the first
transmitter (10-1) and the unknown transmitter (10-?). The purpose of this
criterion is
to select scale bar observations that represent large angular extent between
the top and
bottom scale bar detectors and thus better resolution on the scale bar
measurement.
Because /3,"~.,,x , must be large for both transmitters (10-1 & 10-?), this
selection criterion
will tend to pick the scale bar locations that are equally spaced between the
two
transmitters ( 10-1 & 10-?).
Once the two scale bars are selected, we can create the construct shown in
Fig.
32-7 below. In Fig. 32-7, we put primes on the t vectors and the d distances
to
indicate that we have set the z component to zero, since the guess is
calculated in the x-
y plane only. Zeroing the z component helps reduce errors in both of these
quantities
when the scale bars are tilted. We calculate the intersection of the two
circles to find the
guess location for the unknown transmitter (10-?), (x,Y,y,.T,0.0) .
As with the three-sphere guess routine, we can specify a vector in the first
transmitter's reference frame that locates one of the selected scale bars
relative to the
second transmitter. This is illustrated in Fig. 32-8. Since we also have the t
~ vector
that describes the scale bar relative to the unknown transmitter, we can
determine an
rz,,T angle using the following formula.
S~oc.r.r = t~oc.~ -t'rx
y.
rz,.r = tan-~ Y., _ tan-~
x . xr,"~,~
Using this formula, we calculate the rz,Y angle for both selected scale bars
and
average the two angles to calculate the guess value for the rotation about z.
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
52
The first guess routine is sufficient for most user setups. However, when the
setup deviates significantly from the three assumptions previously listed, the
second
guess routine provides a good backup.
j3) Solution Calculation
As previously stated, the Quick Setup algorithm uses Newton-Raphson iteration
to find the best solution for the given setup. The Newton-Raphson iteration
method
will be understood by those skilled in the art. Since we have two separate
initial guess
vectors, Quick Setup runs two separate iterations and then compares the
solutions for
both using the residual error from the fill functions described below. The
solution with
the lowest residual error is chosen.
Since we have two types of data collected, target observations and scale bar
observations, we have two different functions to be solved. In each case, the
solution
vector for which we are trying to solve is given by x . From this solution
vector, we
construct a position vector p,.r and a rotation matrix R,a. for each unknown
transmitter.
In other words, for a three-transmitter setup, the solution vector is:
x = ~xZ yz z, rx, ryz rz, x~ y~ z~ rx~ ry; rz;~
with position vectors for the three transmitters given by:
0 xz x~
Pi0 Pz Yz Ps Y
= = =
0 ZZ Z3
and rotation matrices for the three transmitters given by:
R, = Rx(O~Ry(O~Rz(0~
R, = Rx(rx, ~Ry(ry, ~Rz(rz,
R~ = Rx(rx~ ~Ry(ry~ ~Rz(rz~
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
53
In order to describe the two types of solve functions, we now introduce a
shorthand
notation for two i- vectors that go through the same detector det at the same
tool
location loc from two different transmitters a and b:
Y~ - rloc,n.det
1 h - rloc.h,det
For target observations, we want to solve for the locations and orientations
of
the unknown transmitters by making the distance between each set of target
rays zero.
Therefore, we use the following formula that describes the distance between
two target
rays.
Rr xRi
disc = ~, a h i~ , ~PU - hh
~~R~ja x Rhrhll
As can be seen from the Fig. 32-9, the target rays are multiplied by the
corresponding transmitter rotation matrix so that the rays will be relative to
the user
reference frame. We use the distance equation for all target observations and
all
combinations of two transmitters. When the correct positions and orientations
of the
unknown transmitters are found, the calculated distance between rays will be
approximately zero.
For scale bar observations, we want to ensure that each recorded scale bar
length is equal to the calculated scale bar length for the given locations and
orientations
of the transmitters in the workspace. Since the solution process is iterative,
the rays to
the detector targets on each scale bar are not going to intersect until the
correct solution
vector is found. Therefore, to check the scale, we must first calculate the
closest point
of intersection for two detector target rays.
In the Fig. 32-10, we again illustrate vectors Rur~, and Rhih , which do not
intersect. The line segment that connects these two vectors represents the
closest point
between the vectors. This line segment is perpendicular to both vectors.
Rather than
calculate the length of this line segment as we did for the target observation
function,
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
54
we instead want to calculate the midpoint on this segment, which represents
the closest
point of intersection. We can calculate this midpoint by constructing three
orthogonal
basis vectors.
n=R~,r,xRnrh
f = R~r~, x n
g = R,,i',, x yt
We then define a reference frame B in which the midpoint lies using the three
normalized basis vectors as rows of B:
B = f'.
We now determine the component of the midpoint along each of the basis
vectors:
n' P~, + ~' hn
n=
2
.f = .f ' P~,
g=g'P,,
Finally, we can calculate the coordinate of the midpoint in the user reference
frame:
n
=B' .f
g
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
This midpoint exactly locates the detector when the correct positions and
orientations of the unknown transmitters are found. The midpoint is calculated
for both
detectors on the scale bar, and then the scale bar length is calculated:
len~.rrn II pnrirll purrcl2 ~)
Finally, the difference between the calculated scale bar length and the actual
length is determined. This difference will be approximately zero when the
correct
transmitter positions and orientations are found.
l2Ylcrrar - leYl~.rr/c leYlU~.mnl
These two types of solution equations make up the fill functions for the
Newton-Raphson iteration.
(4) Solution Check
The final step in the Quick Setup algorithm is a cleanup step. By definition,
the
r vectors start at the transmitters ( 10) and go to the detector targets (78 &
80). This
direction is important because it establishes the locations of the
transmitters (10)
relative to the observations. However, the two fill functions used in the
Newton-
Raphson iteration do not take into account the direction of the rays, only
their distance
apart. Therefore, there are two global minima in the solution space, one in
which the i~
vectors go from transmitter ( 10) to detector (78 & 80), and one in which they
go the
other direction. In some cases, especially when the three assumptions
previously
described are not met, the iteration can go to the wrong minimum.
A simple example of such a solution is given in Fig. 32-11. Note that the
unknown transmitter's orientation is correct. Only the transmitter's location
gets
mirrored. We can see from Fig. 32-11 that the mirrored solution uses the
correct rays.
The rays just go in the wrong direction. We check for a mirrored solution by
calculating
actual detector location given the transmitter solution and then comparing
that solution
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
56
with the actual ray as shown in Fig. 32-12. As evident from this Fig.. we can
simply
check the sign on the dot product between these two vectors. If i- ~ (pde, -
p~.r ~ is
negative, then the solution is mirrored, and the coordinates of the unknown
transmitter
are negated. In this case, we simply flip the signs on the x". , y,,r , and
z,Y coordinates
for the mirrored transmitter. This check is performed on all unknown
transmitters ( 10).
The preceding description has been presented only to illustrate and describe
the
invention. It is not intended to be exhaustive or to limit the invention to
any precise
form disclosed. Many modifications and variations are possible in light of the
above
teaching.
In accordance with the principles of the present invention, the functionality
disclosed herein can be implemented by hardware, software, and/or a
combination of
both. Software implementations can be written in any suitable language,
including
without limitation high-level programming languages such as C++, mid-level and
low-
level languages, assembly languages, and application-specific or device-
specific
languages. Such software can run on a general purpose computer such as a 486
or a
Pentium, an application specific piece of hardware, or other suitable device.
In addition
to using discrete hardware components in a logic circuit, the required logic
may also be
performed by an application specific integrated circuit ("ASIC"), a programmed
programmable logic device ("PLD"), or other device. The system will also
include
various hardware components which are well known in the art, such as
connectors,
cables, and the like. Moreover, at least part of this functionality may be
embodied in
computer readable media (also referred to as computer program products), such
as
magnetic, magnetic-optical, and optical media, used in programming an
information-
processing apparatus to perform in accordance with the invention. This
functionality
also may be embodied in computer readable media, or computer program products,
such as a transmitted waveform to be used in transmitting the information or
functionality.
The preferred embodiment was chosen and described in order to best explain the
principles of the invention and its practical application. The preceding
description is
intended to enable others skilled in the art to best utilize the invention in
various
embodiments and with various modifications as are suited to the particular use
CA 02366711 2001-09-21
WO 00/57131 PCT/US00/07445
57
contemplated. It is intended that the scope of the invention be defined by the
following
claims..
