Note: Descriptions are shown in the official language in which they were submitted.
~,%~39~
Description
Target and Control System for Po5_ tioning an
Automatically ~uided Ve icle
Background of the Invention
This invention relates to method and apparatus
for determining the position and orientation of a
taryet with respect to a sensor. Both the target and
the sensing unit may be affixed to movable objects,
although generally the target will be skationary, and
the sensing unit will be attached to a self-propelled
vehicle.
There are many prior art devices which attempt
to determine the location of a vehicle with respect to
a target. In these devices, the vehicle carries a
sensing unit, the output of which controls some
function of the vehicle, such as its forward motion,
~ steering control or the vertical location of the
;~ forks. The target might be the pallet itself or the
- frame or rack on which ~he pallet is supported. Some
of the prior art devices employ specialized marks whose
dimensions are known; others utilize special light
projectors and sense the light reflected from the
target for positioning a vehicle, or a component of the
vehicle, such as the Eorks of a lift truck.
When using a plurality of marks located on the
target, it is difficult, and becomes close to
impossible, to determine accurately all six degrees of
freedom, that is, horizontal position right and left,
horizontal position forward and back, vertical
position, roll, pitch and yaw. These prior ar-t systems
do not give adequate positional information, especially
when the vehicle is positioned directly in front of the
target, to guide the vehicle accurately.
;
~ ~7'739%
68~97-88
Summary of the Inventi~on
This invention is directed to a target member for use in
a positioning system for providing at least three identiflable
images positioned with respect to a sensor carried by a self-
propelled vehicle. These three images are located so as to
provide an unambiguous frame of reference thereby to allow the
determination of all six positional and orientatlonal degrees of
freedom from a single observation of the target by the sensor.
By translating or rotating the plane of the images from
what has traditionally been normal viewing by the sensor to a
plane which is essentially parallel to a line from the sensor to
the target, positlonal ambiguities are resolved and accurate
information regarding the relative location of the vehicle to the
target is obtainable, even when the vehicle is located directly in
front of the $arget.
In one aspect of the present invention, a target member
for use in a positionlng system is provided. The target member
has a support member. At least three reflector elements are
mounted on the support member, each of the reflector elements
being so configured as to form an image of a light source located
~7ithin a predetermined field of view with respect to the support
member. The images define a plane oriented other than normal to a
line from the light source to the plane. The images also define a
circle that does not include the light source.
'7'73.~
68297-88
In another aspect of khe present inventlon, an apparatus
for usa in a docking system in which a movin~ vehicle is ~o be
aligned with and closed to a dock position is provided. A target
member is mounted at the dock position. The target member
includes a generally ~lat support member and at least three non-
planar refle~ting surfaces supported on the member in
predetermined alignment. At least two of the rePlectlng surfaces
both are either concave or convex and the thlrd surface is
opposite to the two. The reflections from a common radiatlon
source are perceived at known spacing and same sense in the two
surfaces and at known spacing and inverted sense wlth respect to
the others in the third surface.
In yet another aspect of the present invention, an
apparatus for use in determining the location of a vehicle within
a predetermined field of view with respect to an object is
provided. A light source is carried by a vehicle. A target
member is mounted on the object. The target member includes a
support member and at least three reflector elements mounted on
the support member ln a predetermined alignment. The reflector
elements together form images o~ the identifying device in an
image plane oriented other than normal to a line from the
identifying device ~o the plane when the identifying device is
located within a predeterminad ~ield of view wlth respect to the
support member. A camera is carried by the vehicle for sensing
~ ;~7739~
68297-888
the directlons of the identifylng device from each of the
refleeting elements.
In another aspect of the present invention, an apparatus
for use in determining the relative location of a vehicle with
respect to a target member is provided. A light source and a
camera is moun~ed on the vehicle. The target memher includes at
least three reflector elements, each selected to form an image o~
the light source, and all the images together forms an image plane
oriented other than normal to a line from the light source to the
target member. A video RAM device records those images on the
image plane of the camera due to ambient light. Thereafterr a
timing logic circuit flashes the light source. A selection
circuit compares the images due to ambient light ~tored in the
recordlng means and the images resulting both from ambient light
and from the light source. A device records those images on the
; image plane of the camera due only to reflections of the light
source.
In yet another aspect of the present invention, a method
for gathering information for determining the relative location of
a target member with respect to a vehicle wherein the vehicle is
provided with a light source and a camera, and wherein the target
member is provided with at least three reflector elements, each
selected to form an image of the light source, and all the imagas
together forming an image plane oriented other than normal to a
. ~
~.~77392
68297-8~3
line from the light source to the target member is provlded. The
method includes the steps of: a) recording the images formed on
the image plane of the camera due to ambient light; b) flashing
the light source; c) comparing the images recorded ln step a) with
the images resulting both from ambient light and from the light
source thereby to isolate those reflections of the light source in
the reflector elements; d) recording those imageY of the
reflections due to the light source; and e) identifyiny the
position of each such reflection of the light source from the
reflector elements on the image plane of the camera and with
respect to each other.
Advantages of the invention will be apparent from the
following description, the accompanying drawingæ and the appended
claims.
Brief Description of the Drawinas
Fig. 1 is a perspective view showing a rack capable of
~ supporting a plurality of pallets, and an automatically gulded
-~ vehicle carrying a light source and a camera for sensing the
reflections of the light source in a re~lector member attached to
a selected pallet;
~ ~7739,~
Fig. 2 iS a perspective view showing the
camera and light sollrce located below and to the right
of a line passing perpendicular to and through the
center of the target or reflector member;
Fig. 3A is a view showing the reflec-tions of
the light source in the reElector elements. Fig. 3~ is
a view showing the reflections from the mirrors on the
target as they appear on the image plane oE the camera;
Fig. 4 is a perspecti.ve view showing the
camera and light source mounted on the forklift vehicle;
Fig. 5 illustrates a target or reflector
member with attached retroreE:Lector members, spherical
reflector elements, and a bar code;
~ ig. 6 is a plan view showing various
locations of an automatically guided vehicle, such as a
fork lift, with respect to a pallet;
Figs. 7A-7D represent the reflections of the
light source in the re~lector elements at the various
locations of the vehicle with respect to the pallet, as
shown in Fiy. 6;
Figs. 8A-8D represents the images of the
reflections sho~n in Figs. 7A-7D as they appear on the
image plane of the vehicle carried camera;
Figs. 9A-9D represent the electrical signals
on the image plane of the sensor at a single location
of the vehicle. Fig. 9A shows the signals due to the
images of the retroreflectors as a result of the ~irst
flash of the light source. Fig. 9B shows the signals
resulting from ambient light. Fig. 9C shows the
signals when the light source is flashed a second
time. Fig. 9D represents the electrical signals that
remain after processing;
Fig. 10 is a vector diagram illustrating the
directional relationship between a first image point P
and the final image point P'. The illustrated vector
is located at the nodal point of the sensor lens;
~7
--7--
Fig. 11 is a block diagram of the video
processing circuit used to identify and locate images
of the retroreflectors and other reflector elements; and
Figsu 12A-12C are timing diagrams showing the
various signals that occur during various times during
the operation of this invention.
Description of the Preferred Embodiment
Referring now to the drawings now which
illustated pre~erred embodiment of this invention, and
particularly to Fig. 1, a storage rack 10 is shown
supporting an object such as a pallet 15. The pallet
15 is provided with a target member 20.
A wooden pallet 15 is shown, and the target
member 20 is illustrated as being a separate component
attached to the palle-t. It should be understood,
however, that any type of pallet structure may be used,
and the target member 20 may either be a separa~e unit
or it may be formed integral with the pallet itself~
A vehicle 30, such as a forklift truck,
carries the identification means 35 (Figs. 2 and 4),
such as a high intensity light source, and an imaging
sensing means 40, which is preferably a miniature TV
camera, suGh as a Sony~XC-37 CCD camera. The light
source and camera are preferably mounted together as a
unit 45, with the light source 35 immediately adjacent
and above the camera lens ~Fig. 4). In the preferred
embodiment, the vertical distance separating the light
source and camera lens is approximately one inch~
The target member 20 is shown in more detail
in Fig. 5, and it includes a generally flat support
member 50, three reflector elements 52l 53, and 54, and
three retroreflector members 62, 63, and 64. A bar
code 71 for uniquely identifying the pallet may also be
printed on or attached to the support member.
~T~ m~ ~
~8--
The light source and camera unit 45 is
preferably aligned with the direction of travel of the
vehicle. It is possible, however, rotatably to mount
the camera on the vehicle so that it ma~ scan through a
large field of view, both horizontally and vertically.
If this were done, however, the camera would be
associated with a drive uni-t and position indicating
device so that the proper corrections would be
considered when calculating the relative location oE
the target.
The pre~erred embodiment of the invention, as
illustrated, employs two convex reflector elements 52
and 54, and one concave reflector element 53. Each o~
the reflector elements is spherical in shape, and all
are horizontally arranged on ~he support member 50.
Both reflectors elements 52 and 54 have the same radius
of curvature, and the radii of curvatures of all of -the
elements and their diameters are selected to provide a
reasonable field of view A (Fig. 6) such that a
reflection of the identifying means 3S will be viewable
by the camera as long as the vehicle is within the
field of view. In the embodiment described, it is
preferred to have a minimum field of view of + 10 from
~he mirror platen normal. Typical mirrors may be
; 25 approximately 1.5 inches in diameter and have a radius
of curvature of 3 inches or greater. Mirrors 52 and 54
may be type 380 convex mirrors, and mirror 53 may be
type 100 concave mirror, both manufac-tured by ~OLYN.
It should be emphasized that the identifying
means 35, while preferably a brilliant light source,
could also be any means that could be detected by
~ensing means 40. A brilliant xenon flash lamp has
been found effective for this purpose.
Referring to Fig. 2, the light/camera unit 45
is shown positioned below and to the right of the
center line 60 passing through the target 20, but
~27~g~
- 9 -
within the field oE view ~. Under these conditions,
the reflectlon of the identifying means or light source
35 appear as images PA, P0, PB in mirrors 52, 53
and 54, respectively, as shown in Fig. 3. Since the
mirrors are curved surfaces in the embodiment shown,
the images PA and PB will appear toward the lower
right portion of mirrors 52 and 54, and the image P0
will be toward the upper left in mirror 53.
Since the unit 45 is also facing essentially
parallel to the center line 60, the images PAI,
P0', PB' of the identifyin~ means would be grouped
on the image plane 70 of the camera towards its upper
left hand corner, as shown in Fig. 3B. (It will be
assumed for the following illustration that the images
formed on the image plane are not inverted or reversed.)
The absolute location of the reflections, the
spacing between the reflections, and the relative
position of all the reflections will provide
information su~ficient to determine from a single
observation the location of the vehicle with respect to
the pallet and the orientation or rotation of the
; pallet. ~s the location o~ the vehicle changes, the
observed position o;E the identifying means or
reflections on the image plane of the camera will also
change, as will be explained.
It will be noted in Fig. 2 that the images
PA, P0 and PB o~ the identifying means 35 in the
reflec-tor elements 52-54 will define a plane 80, and
these images will also define a circle 82. In this
invention, the plane 80 is not normal ~r perpendicular
to the center line 60 of the target 20; it is in fact
essentially parallel to the upper surface of the
pallet. Further, the circle 82 will not include the
; lens of the camera 40. These conditions are necessary
~7739~
-10-
if the reElections are to provide an unambiguous result
when they are analyzed to determine the locatio~ of the
vehicle relative to the target.
Referring now to Figs. 9A-9D, these figures
represent the images appearing on the image plane of
the camera 40 durincJ one sequence of operations
necessary to gather positional in~ormation.
The preferred method of this invention
provides for flashing the light source 35 and recording
the positions of the reElections ~rom the
retroreflector members 62, 63r and 64. The circuit for
accomplishing this is shown in Fig. 11. These
reflections are identified in Fig. 9A as reElections
162, 163, and 164, respectively. These reflections are
easily identified because they each occupy a plurality
of pixels on the image plane 70 of the camera since
they are physically large components of the target 20
and since the retroreflectors return a large percentage
of the light emitted ky the identifying means 35 back
toward the source. For this reason, the effective
sensitivity of the camera is reduced at this stage oE
the operation so that only the reflec~ions of the
~; retroreflectors are likely to be found at the image
plane. Also, because of the high intensity of the
reflected lightt there may be some blooming of the
image. The positions of each oE the retroreflector
images is recorded in memory means.
Microprocessor means 310 performs a
calculation by reference to the positions of the
30 retroreflector images 162-164, and an area 200 is
defined in which the reflections from the reflector
elements 52-54 are likely to be ~oundO This defined
area 200 may be located anywhere on the image plane of
the camera and will vary in area in proportion to the
separation of the vehicle from the target. In other
'
. ,
73~;~
words, the closer the vehicle is to the target, the
more widely separated will be the images, and the
defined area will consequently be larger.
Once the area 200 has been defined on the
image plane, the effective sensitivity of the camera is
increased, and all of the images within the defined
area are recorded in a recording means (Fig. 9B).
These images are those resulting from ambient light and
may include such re1ections 165 as overhead lights,
specular reflections from l~etal objects within the
field of view, and other light: sources.
The nex-t step is to red~ce the effective
sensitivity of the camera and again flash the light
source 35. As shown in Fig. 9C, this time the image
plane will contain the image of the retroreflectors
162-164, the ambient reflections 165, and also
reflections PA, PO and P~ of the light source in
each of the reflector elements 52-54. All of the
images within the defined area 200 are recorded.
Any images from the retroreflectors that bloom
into the defined area are removed from memory, and the
images recorded in Fig~ 9B are effectively subtracted
from those in Fig. 9C, and what remains are images
PA', PO' and PB', reflections of the light source
25 ln the reflector elements 52,54, as shown in Fig. 9D.
The center of each of these images PA',
PO' and PB' are calculated and the video signals
~ from the camera image plane are evaluated in accordance
; with the procedure later defined.
As shown in ~ig. 5, the retroreflector members
62,64 surround the reflector elements 52-54. It should
be apparent, however, that this physical arrangement is
not absoluteLy necessary. The position of each
retroreflector should be known so that an area in which
the reflector elements are positioned can be deined.
` `~
,
~ ,
' ' '
: ' , '
~ z773~æ
~12-
Also, the position of the retroreflector
elements further define a second area 210 in which the
image 170 of the bar code 71 may be found, and at some
appropriate time during the analysis of the image, the
bar code may be read to conEirm that the proper target
is being approached.
It should also be apparent to those skilled in
the art that the reflector elements 52-54 do not all
have to be spherical in shape. A11 that is necessary
is that the images of the identifying means carried by
the vehicle be viewable by the camera. This means that
one or more of the reflector elements could be a
retroreflector. Using spherical mirrors, however,
reduces the cost of the target and also provides
relatively smaller images, images whose position can
therefore be determined with a high degree of accuracy.
~ t is assumed in this description that the
mirrors are evenly spaced and are horizontally aligned,
and that the center line of the target, that is, the
~O center mirror, is the desired final position of the
light/camera unit 45. It should be recognized,
however, that any orientation of the mirrors and any
position of the camera unit with respect to the target
would be acceptable and would not depart from this
invention. A11 that the control circuit would need is
information regarding the final desired position of
each of the reflections on the image plane. Because of
convention, and for purposes of explanation, it will be
assumed that the desired final position will be on the
center line with the images equally spaced on the image
plane and that the images are in a horizontal line.
The area between the retroreflectors is provided with a
dark background in order to minimize random noise and
false data.
,
'~:
~Z773~;~
-13-
Referring now to Figs. 6, 7A-7D and 8A-8D, it
is assumed that the light/camera uni-t 45 is in the same
horizontal plane as the target, and the vehicle 30 is
positioned to the right oE the center line 60 in
location 1. The reflections of the light source are
shown in Fig. 7A, and the images of the reflections on
the image plane 70 of the camera 40 are shown in Fig.
8A and would be located in the left center of the image
plane~ The images are close together and unequally
spaced. The location of the images on the image plane
and t~eir relative spacing are all important to the
calculations for determining the vehicle's ~elative
location with respect to the target.
Assuming the vehicle moves to location 2 in
Fig. 6, the images of the reflections on the image
plane will move apart (due to the closer proximity to
the target) and they will also move toward the right
side of the image plane 70, as shown in Fig. 8B (due to
the change in the direction of the vehicle).
When the vehicle arrives at location 3, it
will be seen in Fig. 8C that the image of the
reflections on the image plane move toward the center
and each reflection moves away from each other
reflection. In this case, the cen-ter reflection is
~- 25 still closer to the left hand reflection because the
vehicle is still not located on the center line of the
target.
Finally, when the vehicle reaches location 4
in Fig. 6, the reflections of the light source will be
centered in the mirrors, as shown in Fig. 7D, and the
images on the image plane will be in the position shown
in Fig. 8D.
Referring now to the block diagram of Fig. 11,
both the light source 35 and the camera ~0 are
connected to electronic control and processing circuits
:
,
~Z~'7392
-14-
300. At the heart of this circuit is a microprocessor
system 310 which provides control signals to control
the flow of data to the remainder of the circuit. A
timing logic circuitr shown generally at 320, responds
-to instructions from the microprocessor system 310 to
control the light source or flash 35, the video
information from the camera 40, and the way that
information is processed and stored in the remainder of
the circuit.
A random access memory or video RAM circuit
330 provides the means for recording the images on the
image plane o~ the video camera 40.
A multiplexer 340, operating under control of
the microprocessor 310 and timing logic circuit 320,
transfers video information into the video RAM 330
through a serial-to-parallel converter 350, and out of
the video RAM 330 through a parallel-to-serial
converter 360~
A digital-to-analog (D/A) converter 370
responds to a digital signal from the microprocessor to
establish a threshold level for the output of the video
- camera unit, and that threshold level determines what
~ video information passes ~rom the camera 40 through a
;~ comparator circuit 380 into a selection circuit 390,
which circuit includes a first AND gate 392, a second
AND gate 394, and an exclusive OR gate 396~
Referring now to the timing diagram of Fig.
12A, a typical output of the camera unit 40 on the
RS170A video line 301 is illustrated. In this diayram,
each in~erval between vertical blanking pulses 302
represents one-half frame. For example, the interval
designated 401 includes all oE the odd numbered lines
of one frame or screen, while the interval 402
represents all of the even numbered lines. The
intervals 401 and 402 together comprise one complete
frame.
, ~ ' . ' ' ~ .~ ' .
~7739~
-15-
A power up signal is provided by the
microprocessor system on line 316 to the timing logic
320 when the system is Eirst turned on to synchronize
the microprocessor system with the timing logic
S circuit. In turn, the timing logic circuit sends a
reset pulse on line 321 to the video camera unit to
initialize this device.
'rhe camera 40 provides a video output signal
to the comparator circuit 380, and part of this output
is a pulse 302 representing the vertical blanking
interval. During each vertical blanking interval, the
timing logic circuit provides a vertical sync pulse
back to the microprocessor on line 324.
The microprocessor system 310, operating under
a software program, controls the sequencing of
operation o~ the entire system. In normal operation,
the microprocessor establishes an initial threshold
level for the camera by sending a digital value on the
microprocessor bus 315 to the D/A converter 370. This
threshold level, shown in Fig. 12A, as the dashed line
372, limits those signals that may pass through the
comparator circuit 3~0. This initial level is set high
enough that all reflections, except from the
retroreflectors within the field o~ view oE the camera,
will be ignored and will not be passed on to the
selection circuit 390.
Ne~t, the microprocessor 310 sends a Elash
enable signal 311 on line 312 to timing logic circuit
320. This signal e~tends through the vertical blanking
interval 302. At the beginning of the vertical
blanking interval, the microprocessor generates a
strobe signal on line 313, and the timing log:ic circuit
320 in response thereto sends a flash trigger pulse 322
on line 323 at the beginning of interval 401 to the
light source or strobe 35.
~Z'~73~;2
--16--
AS previously explained, the light source 35
is a high intensity xenon strobe which floods the area
- in Eront of the camera with a short duration pulse of
high intensity light. The threshold level 372 during
intervals 401 and 402 is set high enough that only the
video signals exceediny the predetermined threshold
value, such as those representing the re~lections
162-164 returned by the retroreflectors 62-64, will be
allowed to pass through the comparat.or 380. Although
the duration of the flash may be measured in
microseconds, the light energy of the re~lections
therefrom will be retained on the camera image plane
for one complete frame.
The video camera unit 40 provides an output on
line 326 from the lnternal camera clock, basically a
3.58 MHz series of pulses, to the timing logic circuit
320. These clock pulses are converted by the timing
logic circuit into pixel clock pulses on line 327, with
each pixel clock pulse representing a single pixel as
it appears on the camera image plane. As shown in
FigsO 12A-12C, pixel clock pulses are provided to the
serial-to-parallel converter 350 and the parallel-to-
serial converter 360 for two intervals after a start
pulse.
The camera, a Sony XC-37 CC~ Camera, has an
image plane providing an array of 384 x 491 pixels.
Half of those pixels will be interrogated during the
first interval and the other half during the second
interval. Thus, each pixel on the camera image plane
is separately and uniquely identified and it can be
determined whether or not the output from each pixel
exceeds the predetermined threshold level.
The output of the comparator 380 is applied on
line 381 to selection circuit 39~, and to both ~he AN~
gate 392 and exclusive OR gate 396. During intervals
~J2773~2
-17-
401 and 402, the exclusive OR enable line 318 is low
and therefore -the video information from the comparator
380 will be passed directly on line 391 into the
serial-to-parallel converter 350.
Since the target 20 typically includes three
retroreflectors in the preferred embodiment, it is
expected that only three intense returns, or images
162, 163 and 164, will exceed the threshold 372, and
therefore these images will be processed through the
serial-to-parallel converter 350 and sent on the video
R~ data bus 355, through the multiplexer 340 under
control of signals provided by timing circuit 320 on
bus 328, and into the video RAM 330 via bus 335 where
they will be s~ored or recorded in electronic Eorm.
In a preferred embodiment of this invention,
because the images of the retroreflectors are so large,
it is possible to speed up the process by scaling the
pixel clock and limiting the video data stored in the
RAM 330 by storing every fourth pixel of every fourth
line of each frame and still detect the presence of the
retroreflectors.
These stored or recorded images of the
retroreflectors are scanned by the microprocessor under
software control so that their locations may be
determined, and subsequent scanning or interrogation of
the video R~ may then be limited to ~he area bounded
by the retroreflectors thereby to speed up the
identification and location of the images of the light
source in the reflector elements~
Referring now to Fig. 12B, the threshold level
is set by the microprocessor to that shown at 373 so
that low level images 165 and 166, which exceed the
threshold level during intervals 403 and 40~, ~ay pass
through the comparator 380. The selection circuit 3g0
remains inactive at khis time, so those images will be
~Z'7'739;~
-18-
recorded in the video RAM 330 in accordance with the
process described above. Thusl the images on the image
plane of the camera due to ambient light are
temporarily recorded for later reference.
Fig. 12C illustrates the next step in the
process. The microprocessor 310 increases the
threshold level slightly to that shown at 374 rrhis
will permit most of the images due to ambient light to
pass through the comparator 3~0, but will eliminate
some that might have been marginal, such as image 166.
This will also tend to eliminate noise from the camera
itself and slight changes in size of the image due to
camera movement.
The flash is once again enabled and it is
triggered a second time at the beginning of interval
405. This time, those signals representing images from
ambient light 165, the images of the retroreflectors
162-164, and also the images PO, PA and PB of the
light source in the reflector elements will be passed
20 by the comparator 380 into the selection circuit 390.
An exclusive 01~ enable signal on line 318 from the
microprocessor 310 will now be present, however~ and
the selection circuit 390 wiLl now be active and will
prevent those signals representing ambient light images
~rom being passed through to the video RAM 330~
The selection circuit 390 provides the means
for comparing the images due to ambient light
temporarily stored in the recording means or memory 330
and the images resulting from both ambient light and
from the light source, and for thereafter recording
only those imayes due to reflections of the light
source in the video RAM 330.
When the signal level on the exclusive OR
enable line 318 is raised (Fig. 12C at the beginning of
35 frame 405), AND gate 394 is enabled, the data
35~
-19-
previously recorded during .intervals 403 and 404 ~Fig.
lZB) will be taken from the video RAM 330 through the
multiplexer 340 into the parallel-to-serial converter
360 via bus 345, the output of which on line 361 is
applied a pixel at a time as the other input to the AND
gate 394. The parallel-to-serial converter 360
receives a latch signal on line 329 ~rom the timing
circuit to enable the converter 360 to accept a block
oE data from the video RAM ancl then to read that data
out to the selection circuit 90 under direction of the
pixel clock signal on line 327. Thus, for each pixel
that contains information relating to ambient light,
there will be an output from AND gate 394 to the
exclusive OR gate 396.
All video signals on line 381 are applied both
to AND gate 392 and to exclusive OR gate 396. Any such
signal that does not have a coun-terpart in the video
RAM, and thus an output on line 361, will be allowed to
pass through AND gate 392 and thereafter be stored in
the video RAM 330, replacing any information previously
stored therein. On the other hand, any signal (image)
~; which previously appeared in the video RAM that does
not have a counterpart on line 381 will be ignored.
: Thus, an image, such as represented by 166, which
appeared and was allowed to pass into the video RAM
during frames 403 and 404, but not during rames 405
and 406 because the threshold level had been increased
slightly, will be ignored.
At the end of frame 404 therefore, only those
~ 30 images appearing on the image plane of the camera which
: are due to reflections of the light source in both the
retroreflectors and the reflector elements will be
recorded in video RAM 330.
Since the location of the retroreflector
images is known and was determined during intervals 401
and 402, the location of the reflec~ions of the light
12~739X:
-20-
source in the reflector elements with respect to each
other and with respect to the image p:Lane of the camera
can now be accurately determined by analyzing the data
stored in RAM 330 and the location of the vehicle in
S relation to the target calculated.
The technique for calculating the relative
positions of the vehicle and l:arget will now be dis-
cussed.
One of the target points, the center one,
PO, is chosen as a target reference point or origin.
The other two pointsl, PA and PB, have
3-dimensional vector offsets from PO. These oEfset
vectors are identified as a and b in the target coordi-
nate system.
TARGET POINTS AS VIEWED BY SENSOR. Consider
an axis system fixed with respect to the camera. The
specific axis system chosen is the right-handed system
shown in Fig. 2, with x representing horizontal,
positive to the right, y representing horizontal,
positive along ~orward lens axis; and z representing
vertical, positive up. The first nodal point of lens,
i.e., the center of the lens of camera 40 for id~alized
"thin" lens is considered the origin.
In this system, the target reference point
PO will be at some vector location, R.
If the pallet is not rotated, then points P~
and PB would be at vector locations R + a and R ~ b,
respectively. In general, however, there will be some
rotation, i.e., a and b will be rotated. There are,
therefore, these forms for vectors in the two axis
systems:
~2773~Z
-2L-
Vector Vector
Target Locations Locations
Point in in
Label_ Pallet Axes Sensor Axes
PO (origin) R
PA a R
PB b R -~
where and are related to the original vectors by a
rotation matrix M:
a = Ma
B = Mb
In short, the image poin-ts will correspond to
three points which, from the viewpoint of the sensor,
are at locations R, R -~, R ~. The vectors R,~ and
~, and the rotation matrix M, are initially unknown.
DIRECTION VECTORS, u, v, w. In general, the
direction of any single source or target point can be
established with a camera system, but not distance.
The direction can be defined by a vector Erom image
point to lens center (thin lens~ or second nodal point
(thick lens)~ See Fig. 9.
For the target points, let the direction
vectors corresponding to PO, PA~ and PB be
called u, v, w, respectively. These vectors could be
chosen as unit vectors (generally convenient for
analysis~. A more convenient and natural normalization
is to scale these vectors so that the lens-axis or
y-component equals focal length. The x and z compo-
nents are then simply the horizontal (~) and vertical
(n) components of location in the focal plane. (With
due regard for signs and "lmage reversal.")
'`
~77;~9Z
-22~
B~SELINE VECTOR EQUATIO~S. In terms of known
direction vectors u, v, w, the basic vector equations
- become
R = ~Ou (1)
R ~ a = ~Av (2)
R + ~ ~ ~BW (3)
where ~0~ ~A~ ~B are (unknown) scalars propor-
tional to distance.
Equations (1) through (3) are underdetermined.
There are 12 unknowns (three components each for R,
a, ~ vectors, plus the three scalar ~'s), and nine
scalar equations. The l'missing" three equations come
from scalar invariance relationships.
SCALAR INVARIANCE RE~ATIONSHIPS: THREE SCALAR
EQUATIONS. Although a and ~ are unknown as vectors,
partial information comes from scalar invariants of
(rigid-body) rotation Specifically, if
a= Ma; ~= Mb, M = rotation matrix,
then
: a ~ a = a 2 = a2 (known) (4)
2 = b2 (known) (5)
aD~ = a-b (known) (6)
These equations correspond to the physical
properties that rotation does not change vector
lengths, and for rigid-body rotation the angle sub-
tended by any two vectors remains unchanged.
.' ' ; .
~;Z7739Z
-23-
ALGORIT~M OVE~VIEW: REQUIREMENrrS, PHILOSOPH~,
PROBLEMS. Equations (1) through (6) collectively form
a fully determined baseline e~uation set. They give
12 nonlinear (scalar) equations in 12 (scalar) un-
knowns, if evaluation of unknown rotation matrix M istemporarily regarded as a "later step."
The algorithmic steps are based upon mathe-
matical analysis that successively reduces the dimen-
sionality of the problem. The ultimate step becomes
that o~ solving a single nonlinear equa-tion in one
unlcnown, after which the reduction steps are re-traced
to give explicit values of the other variables. At
the end oE the retrace, R, ~, and 3 are known.
A separate procedure is then developed to
establish, from computed ~ and ~ and preset a and b
vectors, the rota~ion matrix M. From M, the evaluation
of "standard" rotation angles -- pitch, roll, and yaw
-- is then straightEorward. Evaluation of explicit
pitch, roll, and yaw is convenient for sensitivity
studies and design analysis. For the operational
system, these angles may not be explicitly required.
Although the obvious goal of algorithmic
development is to achieve some method of solving the
large set of simultaneous nonlinear equations, there
is not necessarily uniqueness in the approach. To the
extent possible, the exact approach chosen and docu-
mented here was aimed at (a) minimizing sensitivity to
effects of computer roundoff errors and (b~ providing
For efficient, that is fast, computation.
There are two distinct solutions to a given
problem. An algorithm requirement, therefore, is the
capability to evaluate both solutions and to select
the one that is "correct. ~! Problems of "correctness"
are addressed later.
~2'77392
-24-
The sequence of steps to reduce the baseline
equations, leading to evaluation of the primary vectors
R,~, and ~will now be described.
EQUA'rIOi~S (l) THROUGH (3~: CHANGE IW ~ PARA-
MEirERSo In Equations (1), (2), and (3)~ this change
of variables for the ~'s:
~0: (no change)
~A = ~o (1 +~)
~B = ~o (1 + ~)
gives revised forms
15 R = ~ ou (1')
R + ~ = ~o (1 +~) v (2')
~ + B = ~o (1 +~) w- (3')
This form allows separation of variables as discussed
below, and also eventually allows isolation o "small"
rom "large" quantities.
ELIMINATION OF R. Equation (1') can be
substituted into Equations (2') and (3') to give two
equations for ~ and ~ with ~0 as a parameter:
~/~0 = (1 + ~) v - u = (v - u) + ~ v (7)
~/~0 = (1 + ~) w - u = (w - u) + ~ w ~8)
,
~27739~2
-~5-
These equations are characterized by two
forms of isola~ion of "large" from "small" variables.
First, the R vector, which is generally large relative
to the ~ and 3 vectors, has been eliminated. Second,
the parameter ~0, which is generally large relative
to the , ~ parameters, has been partially isolated.
This isolation will become complete in the next two
steps.
RE~UCTION TO THREE SCALAR EQUATIONS. The
vector Equations (7) and (8) can be converted to three
scalar equations with vector dot-product forms propor-
tional to N2, ~2,~
0 = (V -- U) ~ 2V- (V -- U) + ' V (9)
2 2 2 2 2
0 = (W - U) + 2~iW~ (W - U) + ~i ~ W (10)
= ( V -- U)' (W -- U) ~ V'(W -- U) + ~iW' (V -- U) +~i V'W (ll)
Note that ~2, ~2, and ~-~ are known (Equa-
tions 4, 5 and 6)~ Also, all of the vector dot prod-
ucts on the right-hand sides o these equations are
constants that depend only upon known u, v, w vectors.
These equations, therefore, have the form
2 2 2
~ /~o Al + A2 ~ A3 (9')
2 2 2
~ /~0 = sl + B2~ ~ B3 (10~)
2 2
1 2 3 4 (11 )
with ~2, ~2,~.~, Al, ..~ C4 as known constants,
and with three scalar unknowns ~0, and ~.
ELIMINATION OF~o : REDUCTION TO TWO SCALAR
EQUATIONS. A consequence of the change from (~0,
~A~B) to (~0, ~ ~ iS the way in which ~0 is
isolated in Equations (9) through (11). Elimination
~'27739;2
-26-
of ~0 is now a tri.vial algebraic step, giving two
equations in scalars and ~ only. The obvious forms
are
~A1 + A2E + A3E ~ ~ ~B1 2 3
(~'~)'~A1 + A2 E + A3E ] = ~ CC1 ~ C2 + C3~ + C4~'
These are simply two 2-dimensional polynomials in E
and ~, expressible in form
2 2
D2 + D~E + D4~ + D5~ = (12)
E2 + E3E + E4 ~-~ E5~ = (13)
where D and E coefficients are combinations of (pre-
vious) known constanks.
REDUCTIOW TO EQUATION IN ONE V~RIABLE/E.
Equations (12) and (13) are near the end oE the reduc-
tion process. They could be solved simultaneously,
using (for example) a 2-dimensional version of Newton
successive approximation. That approach is not pre-
ferred, largely because of practical problems in
determining two distinct solut.ions (a requirement
previously reEerred toj, and because convergence would
probably be slower than for alternatives discussed
below.
Equa-tions (12) and (13) could also be combined
to give a purely l-dimensional equation in E--a
fourth-order polynomial equation of form
:E'1 + P2 E + P E + P4 + P5E = (14)
This approach is analytically attractive. In its
:~ final form, the polynomial computations would be
~2773~2
-27-
simple and easily adapted to finding multiple
solutions. Convergence would generally be fast
relative to a 2-variable approach.
The prac-tical disadvantage of the Equation
(14) approach is the complexity, lengthy software code
and computation time involved in evaluating the P
coefficients prior to solving for roots.
The preEerred approach is a l-dimensional
approach of a somewhat different form.
First, note that Equa-tion (13) can be solved
for ~ as an expLicit function of ~:
~g (~ (El + E2 E + E3 ~ ) (15)
E4 + E
Equatlon (12) can then be written as if a function
only of F o
f () = Dl + D2 + D3 + D4~ + D5~ = O (16)
with the understanding that ~ is always evaluated
(from Equation (15)) as a function of ~
An ~-solution, therefore, is a value of ~ for
which Equation ~16) holds. The corresponding iS
~5 then given by Equation (15), once an ~ solution is
identified.
This approach, discussed below, has the
convergence rate of the "pure" l-dimensional approach
(4th-order polynomial), but with a lesser amount of
subsidiary computation.
FINAL EQUATIONS FOR ~ AND~ : GENERAL PROPER-
TIES AND PROBLEMS. The final approach involves finding
the root, or roots, for
) = Dl ~ D2 ~ + D3 + D~ ~ + D5 (S = (16~ repeated)
" ~,7739~
-28-
with ~ as an explicit function of ~:
= -(EI + E2 + E3 ~ ) (15, repeated)
E4 -~ E5 E
For the class of problems and target-point
geometries that occur in applications of the type
described here, there are normally two distinct real
roots to Equation (16), and two complex roots~ Physi-
cal solutions correspond to the two real roots.
OE the two physical solutions, one is"usually" identifiable as not valid. For some of the
retroreflector target-point geometries (vs. mirror
configurations), and in presence oE mosaic quantization
and/or other sources of errors, resolution of correct
versus incorrec-t solution is not necessarily reliable.
This problem is strictly a data error problem, not an
algorithmic problem. Empirical studies indicate that
this type of problem does not occur for the mirror
configuration and realistic distance/angle combina-
tions.
A potential problem exists wit~ Equation
~15), in that for some value of E (say, ~O) a zero
; denominator occurs. Theoretically (iOe., in absence
of roundoff errors)~ this case must imply that the
numerator is also zero and that a definite (finite)
limiting value exists.
In software implementation, this zero/zero
problem is explicity treated only for the following
special case analytically established as physically
possible:
.
= O
El = E~ = 0
~Z 7739Z
-29--
For this case, Equation (15) is replaced with
2 + E3 ~ (15, special form)
E5
No ins-tances of zero~denominator problems
have occurred in several thousands of software execu-
tion of the algorithms. That fact does not, of course,
guarantee that such a problern will never occur. Of
course, additional protective code could be added.
Without such code, however: (a) the likelihood of a
problem is small~ and (b) at worst, an abort of a
pallet load or unload operation would occur.
THE "SECOND ROOT" PRO~LEM. Consider a situa
tion in which (a) a real root, ~1~ has been evaluated
for Equation (16) t and (b~ this root is judged to be
the wrong root. For this case, the second root is
established as the root of:
g() - o (17)
g(~ = f () i:E ~ 1 (17a)
-- 1
= f (~l) if 1 (17b)
Equation (17b) is the limiting form of (17a) as
approaches 1-
This formulation is equivalent to dividing
out an (-1) factor from a pure polynomial form.
The pair of equations (15 and 16) yield
double roots for and ~. Procedures for establishing
both ~ and ~ pairs and selecting the physically valid
values are required.
THE RETRACE STEP. A series o-f steps of
variable elimination, leading eventually to the evalua
tion of the and ~ parameters, has been descrlbed.
~;27~73~2
-30-
Retracing is the process of then working backwards,
with known ~ and ~, to get the three required baseline
vectors R, ~ and ~.
RETRACE To ~0O The ~0 parameter is a
required intermediate variable. It is obtained from
Equation (9'), rewritten in the explicit form
~0 = ~ Al -2 - (18)
It could also be evaluated from Equation (10'), or
(provided ~ O ~ from Equation (11'), since ~ and ~
have, in p~inciple, been evaluated to make these three
equations compatible.
RETRACE FOR R, ~, AND~. With~ , ~, and ~0
established, then:
R = ~Ou (19)
~ = ~ Cv-u + ~ v ~ (20)
= ~0 [w-u + ~ w ] (21)
The steps for determining rotation matrix and
the pitch, roll, yaw angles will now be described4
THE ROTATION PROBLEM: THE M l~ATRIX. Once ~
and ~ are evaluated, a new algorithmic problem arises
in solving for the unknown rotation matrix M that
satisfies
a Ma (22)
~ = Mb. (23)
~27739~
-31-
M is a 3 x 3 matrix, hence with nine elements. These
nine elements are not independent, however. The
constraint that M be a rotation matrix implies that
only three degrees of freedom exist. These three
degrees of freedom can be identified with pitch, roll,
and yaw angles, but that identification is neither
required nor useful in the s-teps to solve for M.
MATRIX FORMULATI0~ N0. 1 T0 GIVE M. The two
Equations (22) and (23) can be supplemented with a
third linearly independent equation, that must be true
for a rigid body rotation:
= M (a ~1 b) (24)
where ~ means vector cross product.
Now write the separate Equations (22) through
(24) as a single matrix equation
~ (25)
( ~ M~a b a ~ b )
(25')
Q = M P
where, for example, P has vector a as its first column,
vector b as second column, etcO
The matrix P is nonsingular (provided only
that the pallet vectors a and b are not collinear), so
an immedia~e formal solution results:
M = Qp 1
In this forml solution would require computa-
tion of the inverse of 3 x 3 P matrix, Eollowed by a
matrix~times-matrix product.
:,
77392
-32-
MATRIX FOl~MUI.ArION NO. 2 ~ro GIVE ~. The
actually implemented algorithm for calculation of the
M matrix avoids the computational effort of matrix
inversion. The concept is to use not the ~ and 3
S vectors (and ~ ~ 3 ) directly, but to use equally
valid combinations that are orthonormal~ For a matrix
with orthonormal columns, inversion is not explicitly
required - the inverse is simply the transpose.
,~ ~ ~
Three base vectors ~, ~, and r will be chosen
by combining and norlnalizing the ~ and B vectors.
Exactly the same (hence compatible) combinations and
normalization will be applied for the a, b vectors,
giving â, b, c. Vectors ~, ~, and y will form an
orthonormal set (orthogonal unit vectors); â, b, c
will also form an orthonormal set; and the same
M-matrix rotation property will hold. A form analogous
to Equation (25) is then
~ ) t27)
( ~ ~ y ) = M ( a b c (27')
Q = MP
but with the distinction that the inverse of P is
simply the transpose of P(_ P'). Therefore
.~ ~
M = QPI (28)
Algorithmic steps to get the orthonormal
vectors are discussed below.
ESTABLISHING â,âO The first vectors, ~ and a,
are trivial:
= unit (~)
a = unit (a) (29)
~Z773~Z
-33~
where un.it "( )" means normalized to unit length.
ESTABLISHING ~b- For ~, we want a vector
orthogonal to ~ (hence, orthogonal to ~), that retains
the information in ~. lrhe simplest construct, and the
one implemented, is to first form the linear combination
~ c ~ (30)
with the constant c chosen as
C = ~J ~ ~ (31)
u
Then ~' will be orthogonal to ~ and ~ The second step
is then simply to nor~alize ~' to a unit vector:
~ = unit (~') (32)
For b, the same steps are followed using a and
: 20 b:
b' = b - ca ~33)
b = unit (b') (34)
Note that (a~b)/a2 is the same as (~ 2 from
scalar invariance properties, so the "c' in Equation
(30) is the same as in Equation (33).
ESTABLISHING y,c~ For ~r a unit vector
orthogonal to ~ and ~ is given by the vector cross
30 product
y ~ (35)
Similarly,
c = a ~ b (36)
~2'7739Z
-34-
Since these are (au-tomatically) unit vectors, explicit
numerical normalization steps are not required.
Equation (28) is the form used for software
evaluation of M. It requires a matrix-times-matrix
; 5 multiplication, no explicit matrix inversion.
PITCH~ ROLL, AND YA~ ANGLES FROM M. The
matrix M can be considered to be the product of three
canonic matrices associated with pure pitch, roll, and
yaw. The convention chosen for the order of multipli-
cation is
M ~ (vector) = P R Y ~ (vector)l
i.e., the yaw matrix Y is first applied -to the vector,
then the roll matrix R, then the pitch matrix P.
The canonic matrices are
/cy -sy o \
20 Y =~sy cy O ~ (38)
O 0
/Cr -St
R =¦0 1 0 (39
:~ St c~
/1 0 0 ~
P =1 cp -sp (40)
~O sp cp
where cy = cos ~yaw anglé), sy a sin (yaw angle),
Cr = cos (roll angle), etc., are used as abbrevia-
tions Eor trigonometric functions of angles. The
product is then
~ r Y crsy ~sr \
M = P R Y = ¦ cps -spsrc cpcy+s s s -s cr ~ (41)
~ p y cp r y p y p r y Cpcr J
.
~' '
-35-
Given the values of the elements (Mij) of the
M matrix, pitch, roll, and yaw angles are then evalu-
atable as inverse trigonometric functions:
pitch angle 2 tan~l . ~-M23~ (42)
M33/
roll angle = slnl (-M13) (43)
yaw angle = tan~l (-M12 ) (44)
Mll
While the method herein described and the form
of apparatus for carrying this method into effect
constitute preferred embodiments of this invention, ik
is to be understood that the invention is not limited
to this precise method and form of apparatus, and that
changes may be made in either without departing from
the scope of the invention, which is defined in the
appended claims.
:` 25
:
;~
~ 35
,.,
