|
Claims  |
|
|
What is claimed is:
1. In a manipulator system of the type having a master control console and
a slave manipulator arm controlled by signals from said master control
console, said master control console including a translation controller
and a rotation controller, and said slave manipulator arm including
pivoted arm segments moved by translational servo means in response to
signals generated by said translation controller and pivoted wrist units
moved by rotational servo means in response to signals generated by said
rotation controller, the improvement wherein the signals generated by each
of said translation controller and said rotation controller are rate
command signals, said translational servo means includes rate sensor means
for generating signals proportional to pivot rates of said pivoted arm
segments and force computer means responsive to the rate command signals
from said translation controller and the signals proportional to pivot
rates of said pivoted arm segments for computing translation forces, said
rotational servo means includes sensor means for generating signals
proportional to pivot rates of said pivoted wrist units and torque
computer means responsive to the rate command signals from said rotation
controller and the signals proportional to pivot rates of said pivoted
wrist units for computing rotation torques, and display means on said
master control console responsive to said force computer means and said
torque computer means for displaying translation forces and rotation
torques.
2. The improvement in a manipulator system as recited in claim 1 wherein
said pivoted arm segments comprise three pivots providing three degrees of
freedom of movement and sid pivoted wrist units comprise three pivots
providing three degrees of freedom of movement.
3. The improvement in a manipulator system as recited in claim 2 wherein
said rate command signals from said translation controller represent
range, azimuth and elevation rate commands and the rate command signals
from said rotation controller represent pitch, yaw and roll rate commands,
said translational servo means further includes position sensor means for
generating signals proportional to pivot positions of said pivoted arm
segments, and said rotational servo means further includes position sensor
means for generating signals proportional to pivot positions of said
pivoted wrist units, further comprising transformation means responsive to
the signals generated by said position sensor means in each of said
translational servo means and said rotational servo means and to the
signals generated by said rate sensor means in said translational servo
means and connected to each of said force computer means and said torque
computer means for providing control of the terminal end of said slave
manipulator arm in cartesian space.
4. The improvement in a manipulator system as recited in claim 1 wherein
said pivoted arm segments comprise two segments and three providing three
degrees of freedom of movement, said three pivots being defined as
shoulder pitch, shoulder yaw and elbow pitch, and said rate command
signals from said translation controller represent range, azimuth and
elevation rate commands, said translational servo means including a
shoulder pitch rate sensor, shoulder yaw rate sensor and an elbow pitch
rate sensor and comprising an elbow pitch servo responsive to said range
rate command and said elbow pitch rate sensor for generating an elbow
pitch torque, a shoulder yaw servo responsive to said azimuth rate command
and said shoulder yaw rate sensor for generating a shoulder yaw torque,
and a shoulder pitch servo responsive to said elevation rate sensor and
said elbow pitch rate sensor for generating a shoulder pitch torque.
5. The improvement in a manipulator system as recited in claim 1 wherein
said pivoted wrist units comprise three pivots providing three degrees of
freedom of movement, said three pivots being defined as wrist pitch, wrist
yaw and wrist roll and said rate command signals from said rotation
controller represent wrist pitch, wrist yaw and wrist roll rate commands,
said rotational servo means including a wrist pitch rate sensor, a wrist
yaw rate sensor and a wrist roll rate sensor and comprising a wrist pitch
servo responsive to said wrist pitch rate command and said wrist pitch
rate sensor for generating a wrist pitch torque, a wrist yaw servo
responsive to said wrist yaw rate command and said wrist yaw rate sensor
for generating a wrist yaw torque, and a wrist roll rate servo responsive
to said wrist roll rate command and said wrist roll rate sensor for
generating a wrist roll torque.
6. The improvement in a manipulator system as recited in claim 1 wherein
said pivoted arm segments comprise two segments and three pivots providing
three degrees of freedom of movement, said three pivots being defined as
shoulder pitch, shoulder yaw and elbow pitch, and said rate command
signals from said translation controller represent range, azimuth and
elevation rate commands, said translational servo means including a
shoulder pitch rate sensor, a shoulder yaw rate sensor and an elbow pitch
rate sensor and comprising an elbow pitch servo responsive to said range
rate command and said elbow pitch rate sensor for generating an elbow
pitch torque, a shoulder yaw servo responsive to said azimuth rate command
and said shoulder yaw rate sensor for generating a shoulder yaw torque,
and a shoulder pitch servo responsive to said elevation rate command, said
shoulder pitch rate sensor and said elbow pitch rate sensor for generating
a shoulder pitch torque, and wherein said pivoted wrist units comprise
three pivots providing three degrees of freedom of movement, said three
pivots being defined as wrist pitch, wrist yaw and wrist roll and said
rate command signals from said rotation controller represent wrist pitch,
wrist yaw and wrist roll rate commands, said rotational servo means
including a wrist pitch rate sensor, a wrist yaw rate sensor and a wrist
roll rate sensor and comprising a wrist pitch servo responsive to said
wrist pitch rate command and said wrist pitch rate sensor for generating a
wrist pitch torque, a wrist yaw servo responsive to said wrist yaw rate
commands and said wrist yaw rate sensor for generating a wrist yaw torque,
and a wrist roll rate servo responsive to said wrist roll rate command and
said wrist roll rate sensor for generating a wrist roll torque.
7. The improvement in a manipulator system as recited in claim 6 wherein
said display means comprises first, second and third force meters
connected to said elbow pitch servo, shoulder yaw servo and shoulder pitch
servo, respectively, to display range, azimuth and elevation computed
forces, respectively, and first, second and third torque meters connected
to said wrist pitch servo, wrist yaw servo and wrist roll servo,
respectively, to display wrist pitch, wrist yaw and wrist roll computed
torques, respectively. |
|
|
|
|
Claims  |
|
|
Description  |
|
|
BACKGROUND OF THE INVENTION
The present invention generally relates to master-slave manipulator arm
systems, and more particularly to a new rate control system for such
master-slave manipulator arms which presents force and moment information
to the operator.
In general, manipulator systems are complex servo systems and incorporate
humans in the feed-back and command segments of the loop. Such systems are
used in environments which are unsafe for the presence of the human
operator. For example, manipulator systems have been used in such diverse
areas as underwater systems, systems exposed to high nuclear radiation and
extra terrestrial systems. In the manipulator system, the human operator
element is by far the least predictable element in the servo loop, and a
design which minimizes the performance sensitivities to the human operator
element variations is the desirable servo configuration.
Most maniptulator arm control systems presently used in industry are the
bilateral position mode type and are used for dexterous manipultive tasks
requiring force reflection. In one type of bilateral position mode control
system, input rate commands are integrated and compared with arm position
values. Integrators are incorporated into the system, and gimbal data is
secured from the arm in the form of joint angular positions. A
characteristic of this system is that forces and torques applied to or by
the arm can be held after the controllers have been returned to null. This
means that a force build-up in a direction not desired must be nulled by
controller commands. If the applied forces and moments are displayed to
the operator, the operator can readily and quite accurately null these
unwanted forces. To accomplish the manipulative task with this type of
system, the operator builds up forces in the direction required while at
the same time nulling forces in the other directions. This continued
nulling procedure consumes a large portion of the total manipulative task
time and therefore greatly contributes to operator fatigue.
Another problem with the bilateral position mode type control system is the
tendency to build up error signals which can be come larger than the level
corresponding to the maximum force capability of the arm. When this
occurs, it is necessary to apply a command to reduce the error signal to
where a force change can be detected. During this time, however, the
operator had no indication of exactly what is happening, and it becomes
relatively easy to loose control of the arm by not knowing immediately
what to do to regain control. This problem can be avoided by introducing
logic into the control equations such that the error signal could not
continue to build up after it reached a maximum level. This solution
increases the complexity of the manipulator control system without
producing any substantial improvement in the tip precision in the
manipulator arm.
SUMMARY OF THE INVENTION
It is therefore an object of this invention to provide a manipulator
control system which is basically simplier and less costly to implement
than bilateral position mode type control systems, but has far more
precision and flexibility than the bilateral position mode type control
system.
It is another object of the invention to provide a manipulator control
system wherein the mental and physical fatigue of the human operator are
greatly reduced thereby increasing the performance of the overall system.
These and other objects are attained by providing a rate control system
which incorporates a technique of visually displaying the forces and
moments applied to or by the arm in the accomplishment of any task. The
implementation of this technique does not require the use of any type of
force sensors to obtain the displayed data. The control system according
to the invention is a true rate system in that for a free moving arm, arm
tip velocities are proportional to the deflection of the rate hand
controllers. The same implementation, however, also allows commanding
forces and moments at the arm tip, proportional to controller deflection,
for the static situation such as working at task panels. Force and moment
values in six degrees of freedom of movement are independently calculated
in the translational and rotational servos from input rate commands and
actual gimbal rates. These force and moment values are displayed to the
operator at the same time that the corresponding forces and moments are
being applied through the arm to a task panel. Among the several
advantages of the rate control system according to the present invention
as compared with the bilateral position mode type control system is that
the motion of the arm is much smoother since motion coupling between
translational and rotational motion is much less with the rate system. Not
only is operator mental and physical fatigue much less with the rate
system, an added advantage is that the operator training times are much
less for the rate system. Furthermore, the rate control system can be used
with any coordinate reference frame and permits easy implementation of
desirable control features which require conversion from one coordinate
reference frame to another.
A characteristic of the rate control system according to the invention is
that for a fixed task panel, forces and moments applied to or by the arm
can be held only by maintaining a controller deflection. In other words,
when translation and rotation controller deflections are nulled in a
"hands-off" operation, all forces and moments on the arm are quickly and
automatically nulled. This characteristic is very desirable since it is
not necessary to actively null forces or moments in unwanted directions.
By controlling the rate loop gains, the time required to automatically
null unwanted forces can be made variable.
BRIEF DESCRIPTION OF THE DRAWINGS
The foregoing and other objects, advantages, aspects and uses of the
invention will be better understood from the following detailed
description with reference to the drawings, in which:
FIG. 1 is a perspective illustration of the slave manipulator arm showing
the six degrees of freedom of movement of the arm;
FIG. 2 is a block diagram of the elbow pitch servo;
FIG. 3 is a block diagram of the shoulder yaw servo;
FIG. 4 is a block diagram of the shoulder pitch servo;
FIG. 5 is a block diagram of the wrist pitch servo;
FIG. 6 is a block diagram of the wrist yaw servo;
FIG. 7 is a block diagram of the wrist roll servo;
FIG. 8 is a block diagram of the complete translation servo system; and
FIG. 9 is a block diagram of the complete rotation servo system.
DESCRIPTION OF THE PREFERRED EMBODIMENT
FIG. 1 illustrates the basic configuration of the manipulator arm which
comprises a base 10 and pivoted arm segments 11 and 12. The arm segment 11
is joined to the base 10 through a shoulder joint comprising a yaw gimbal
13 and a pitch gimbal 14. The arm segments 11and 12 are connected by an
elbow joint comprising an elbow pitch gimbal 15. Translational motion of
the manipulator arm is provided by the combined rotations of the gimbals
13, 14, and 15. The coordinate system used to define the translational
movement of the manipulator arm may be cartesian, cylindrical or
spherical. Because of the nature of the motions involved in the gimbals
13, 14 and 15, the spherical coordinate system comprising range, azimuth
and elevation is conveniently used. As illustrated in FIG. 1, the
corresponding motion vectors are the range vector r, the yaw vector .psi.
and the elevation vector .theta.The position of gimbal 13 is defined as
shoulder yaw .psi..sub.s the position of the gimbal 14 is defined as
shoulder pitch .theta..sub.s, and the position of gimbal 15 is defined as
elbow pitch .theta..sub.e. The spherical vectors range r, azimuth .psi.
and elevation .theta. are related to the gimbal positions by the following
equations:
r = 2Lcos.gamma., where .gamma. = .sup..theta. e/2
.theta. .sup..theta. s + .gamma.
.psi. = .sup..psi. s
where L equals arm seqment lengths.
Rotational motion of the tip end of the manipulator arm is provided by
three pivoted wrist units 16, 17 and 18. The wrist unit 16 is connected to
the end of the arm segment 12 by means of a gimbal 19. The wrist units 16
and 17 are joined by a gimbal 20 the wrist unit 18 is attached to wrist
unit 17 through a gimbal 21. The axes of rotation of the gimbals 19, 20
and 21 are each mutually perpendicular. The position of gimbal 19 is
defined as wrist pitch .theta..sub.w, and position of gimbal 20 is defined
as wrist yaw .psi..sub.w, and the position of gimbal 21 is defined as
wrist roll .phi..sub.w.
Thus, the manipulator arm illustrated in FIG. 1 has six degrees of freedom
of movement, three of which are translational and three of which are
rotational. In order to provide rotation in each of the gimbals, separate
torque motors are provided. Since the control system according to the
invention is a rate control system, each gimbal is also provided with a
rate sensor such as a tachometer. Also as will become clear from the
development of the servo equations, position sensors such as
potentiometers are required at certain of the gimbals. Specifically,
position sensors are required at gimbals 14, 15, 19 and 20.
The six degrees of freedom of motion problem of the manipulator arm are
divided into two three degrees of freedom of motion problems.
Translational control of the wrist point is provided by range, azimuth and
elevation rate commands originating from the translation controller
operated by the human operator. Rotational control of the wrist assembly
is provided by pitch, yaw and roll rate commands originating from the
rotation controller operated by the human operator.
Referring now to FIG. 2, the range servo implements the following equation:
##EQU1##
where K.sub.1 = an operator variable gain which determines the
translational controller sensitivity, K.sub.2 = an operator variable gain
which determines the translational motions servo stiffness, L = the length
of segments 11 or 12, and T.sub..theta.e = the torque applied to the
gimbal 15. It will be recalled that the range vector r is a function of
the cosine of .gamma., where .gamma. = .theta..sub.e /2, and therefore the
range rate R is a function of both sine .gamma. and .theta..sub.e. Thus,
both rate and position sensors are required at the gimbal 15. Because of
the mass and hence the inertia associated with the manipulator arm the
range rate is also affected by rotation about the gimbal 14. Therefore, a
factor proportional to the torque applied to the gimbal 14 T.theta..sub.s
must be added to the servo equation as follows:
##EQU2##
where the constant K is determined by both system and environmental
parameters. For example, if the manipulator arm were used in an underwater
situation, both the mass of the arm and the buoyancy of the arm are
factors to be considered in defining the constant K. This constant, unlike
the variable gains K.sub.1 and K.sub.2 , is not operator variable.
In FIG. 2, the range rate command R is applied to one input of a multiplier
22. The multiplier 22 is conveniently a voltage variable gain amplifier
having a gain K.sub.1 proportional to an input control voltage V.sub.1.
The input control voltage V.sub.1 is operator variable. The output of the
multiplier 22 which is proportional to K.sub.1 R is applied to the input
of a divider 23. The divider 23 divides K.sub.1 R by -Lsin.gamma.. Since
.gamma. is equal to 1/2 .theta..sub.e , the other input to the divider 23
is 1/2 .theta..sub.e derived from the position sensor at gimbal 15. The
output of the divider 23 is a derived gimbal rate command which is applied
through the limiter 24 to the positive input of a differential amplifier
25. The purpose of the limiter 24 is to control the magnitude of the
derived gimbal rate command and thus prevent the rate for, in this case,
the elbow joint from exceeding designed values as the manipulator arm is
extended to the extremes of its operating volume. The differential
amplifier 25 operates as the comparing or summing junction of the servo
and receives the actual gimbal rate .theta..sub.e of the gimbal 15 from
filter 26 at its negative input. The filter 26 is a tachometer ripple
filter which provides a smooth d.c. signal to the negative input of the
differential amplifier 25.
The output of the differential amplifier 25 is the error signal from which
the torquie drive signal to the torque motor at the gimbal 15 is derived.
This error signal is applied to the multiplier 27 which multiplies the
error signal by the variable gain K.sub.2. This variable gain is
controlled by the input control voltage V.sub.2 which is operator
variable. The output of multiplier 27 is a signal proportional to the
range translational force F.sub.r which is used to provide a display to
the operator on the master control console as will be described in more
detail hereinafter. The output of multiplier 27 is also connected to one
input of multiplier 28 which multiplies the computed foresignal by
-Lsin.gamma. to produce an output proportional to the moment. The other
input to the multiplier 28 is equal to .theta. .theta..sub.e and is,
again, derived from the position sensor at the gimbal 15. The moment
output from the multiplier 28 is summed in summing amplifier 29 with a
signal proportional to the factor KT.sub..theta.s which is related to the
torque applied to the gimbal 14. The output signal from the summing
amplifier 29 is applied via the compensation network 30 and the limiter 31
to the torque motor at gimbal 15. The limiter 31 is provided to control
the torque commands derived from large error signals.
Turning now to FIG. 3, the azimuth rate servo implements the following:
##EQU3##
where K.sub.3 and K.sub.4 = operator variable gain which determine
translational controller sensitivity and translational motion servo
stiffness, respectively, and T.sub..psi.s = the torque signal applied to
the torque motor for the gimbal 13. It will be appreciated from the
azimuth rate servo equation that azimuth rate A is a function of both
shoulder yaw rate .psi..sub.s and .gamma.. Therefore, the azimuth rate
servo requires inputs from both the shoulder yaw rate sensor at gimbal 13
and the elbow position sensor at gimbal 15.
With reference to FIG. 3, the azimuth rate command signal A from the master
control console is applied to one input of multiplier 32. Multiplier 32
may again be a variable gain amplifier having a gain K.sub.3 controlled by
a control voltage V.sub.3 which is operator variable. The output of
multiplier 32 K.sub.3 A is applied to one input of divider 33. Divider 33
divides the product K.sub.3 A by 2Lsin.gamma.; therefore, the other input
to the divider 33 is 1/2 .theta..sub.e which is derived from the position
sensor at gimbal 15. The output of divider 33 is applied through limiter
34 to the positive input of the differential amplifier 35. Limiter 34 has
the same function as limiter 24 in FIG. 2. The differential amplifier 35
functions as the summing junction of the servo and receives as its
negative input the shoulder yaw rate .psi..sub.s from the tachometer at
the gimbal 13 through the filter 36. The error signal at the output of the
differential amplifier 35 is multiplied in multiplier 37 by the variable
gain K.sub.4 which is controlled by the operator variable control voltage
V.sub.4. The output of multiplier 37 is the derived yaw force
F.sub..psi.s. This force is also displayed on the master control console.
The output of multiplier 37 is connected to one input of multiplier 38
which multiplies the yaw force F.sub..psi.s by 2Lsin.gamma. to develop a
moment output signal. This moment output signal is applied through
compensation network 39 and limiter 40 to the torque motor at gimbal 13.
The purpose of limiter 40 is the same as limiter 31 in FIG. 2.
Referring now to FIG. 4, the elevation rate servo implements the following
equation:
##EQU4##
where K.sub.5 and K.sub.6 = variable gains which determine translational
controller sensitivity and translational motion servo stiffness,
respectively, and T.sub..theta.s = the signal applied to the torque motor
at gimbal 14. As will be appreciated from the elevation rate servo
equation, the elevation rate E is a function of elbow pitch position,
elbow pitch rate and shoulder pitch rate. Therefore, the elevation rate
servo must receive as inputs the output of the elbow position sensor and
the elbow pitch rate sensor at gimbal 15 and the shoulder pitch rate
sensor at gimbal 14.
In FIG. 4, the elevation rate command E is applied to one input of
multiplier 41 which multiplies the elevation rate command by a variable
gain K.sub.5 which is controlled by the operator variable control voltage
V.sub.5. The output of multiplier 41 K.sub.5 E is applied to one input of
divider 42. Divider 42 divides the product K.sub.5 E by 2Lcos.gamma.. The
other input to the divider 42 is therefore 1/2 .theta..sub.e derived from
the position sensor at gimbal 15. The derived elevation command signal
from divider 42 is applied by way of limiter 43 to the positive input of
differential amplifier 44. The negative input to the differential
amplifier 44 is supplied by the output of summing amplifier 45. The inputs
summing amplifier 45 are derived from the elbow pitch rate sensor at
gimbal 15 and the shoulder pitch rate sensor at gimbal 14. More
specifically, the shoulder pitch rate .theta..sub.s is applied by means of
tachometer ripple filter 46 directly to one input of the summing amplifier
45. The elbow pitch rate .theta..sub.e is applied by means of tachometer
ripple filter 47 and attenuator 48 to the other input of the summing
amplifier 45. The attentuation factor of the attenuator 48 is equal to
1/2.
The differential amplifier 44 has the function of the summing junction of
the elevation rate servo and produces an error signal at its output. This
error signal is applied to one input of multiplier 49 which multiplies the
error signal by the variable gain K.sub.6 which is controlled by the
operator variable control voltage V.sub.6. The output of multiplier 49 is
the derived shoulder pitch force F.sub..theta.s which is displayed on the
master control console. The output of multiplier 49 is also connected to
one input of multiplier 50 which multiplies the should pitch force
F.sub..theta.s by 2Lcos.gamma. to derive a moment signal. This moment
signal is applied by means of compensation network 51 and limiter 52 to
the torque motor at gimbal 14.
Unlike the translational servo equations, the rotational servo equations
have identically the same form. More specifically, referring to FIGS. 5, 6
and 7, the wrist pitch, yaw and roll rate servo equations are as follows:
K.sub.8 (K.sub.7 .theta..sub.wc -.theta..sub.w) = T.sub..theta.w
K.sub.10 (K.sub.9 .psi..sub.wc -.psi..sub.w) = T.sub.-w
K.sub.12 (K.sub.11 .theta..sub.wc -.theta..sub.w) = T.sub..theta.w
where K.sub.7, K.sub.9 and K.sub.11 = variable gains which determine
rotational controller sensitivity, K.sub.8, K.sub.10 and K.sub.12 are
variable gains which determine the rotational motion servo stiffness, and
T.sub..theta.w, T.sub..phi.w and T.sub..phi.w = the torque signals applied
to the torque motors at gimbals 19, 20 and 21, respectively. As will be
described in more detail hereinafter, it is a relatively simple matter to
translate from one coordinate system to another to facilitate task
requirements. For example, it was "natural" to select spherical
coordinates to describe the translational motion of the free flying
manipulator arm. However, once the tip end of the manipulator arm has
reached its desired destination, often the movements of the terminal
device attached to the tip end of the manipulaotr arm are best controlled
in cartesian coordinates. One feature of the invention which is described
in more detail hereinafter is a "hawk" mode wherein all three wrist
gimbbals 19, 20 and 21 are driven to prevent a change in the attitude of
the terminal device as a result of range, azimuth or elevation
translational motion. This is accomplished by deriving "hawk" command
signals for wrist pitch rate .theta..sub.wh, wrist yar rate .theta..sub.wh
and wrist roll rate .theta..sub.wh. These "hawk" command signals are
applied to the rotational servo systems according to the following
equations:
K.sub.8 (K.sub.7 .theta..sub.wc +.theta..sub.wh =.theta..sub.w) =
T.sub..theta.w
K.sub.10 (K.sub.9 .psi..sub.wc+.psi..sub.wh -.psi..sub.w) = T.sub..psi.w
K.sub.12 (K.sub.11 .phi..sub.wc +.phi..sub.wh -.phi..sub.w) = T.sub..phi.w
Referring now to FIG. 5, the wrist pitch rate command signal .theta..sub.wc
from the rotation controller operated by the human operator is applied as
one input to the multiplier 53 which multiplies this command signal by the
variable gain K.sub.t which is controlled by the operator variable control
voltage V.sub.7. The output of multiplier 53 is applied by way of summing
amplifier 54 and limiter 55 to the positive input of differential
amplifier 56. When the system is in the "hawk" mode, the hawk command
signal .theta..sub.wh is applied to the other input of the summing
amplifier 54. The differential amplifier 56 is the summing junction for
the wrist pitch rate servo and receives as its other input the output
.theta..sub.w from the tachometer at gimbal 19 through the tachometer
ripple filter 57. The error signal output from the differential amplifier
56 is multiplied by the variable gain K.sub.8 in multiplier 58 controlled
by the output variable control voltage V.sub.8. The output of multiplifer
58 is proportional to the wrist pitch torque T.sub..theta.w, and this
signal is both displayed on the master control console and applied by
means of the servo compensation network 59 and the limiter 60 to the
torque motor at gimbal 19. The limiters 55 and 60 serve analogous
functions to the limiters 24 and 31, respectively, in FIG. 2.
FIG. 6 shows the wrist yaw rate servo wherein the wrist yaw rate command
.psi..sub.wc is applied as one input to multiplier 61. Multiplier 61
multiplies the wrist yaw rate command by a variable gain K.sub.9 which is
controlled by the operator variable control voltage V.sub.9. The output of
multiplier 61 is applied through summing amplifier 62 and limiter 63 to
differential amplifier 64. Differential amplifier 64 is the summing
junction of the wrist yaw rate servo and receives as its other input the
wrist yaw rate .psi..sub.w from the rate sensor at gimbal 20 through
tachometer ripple filter 65. The error output signal from the differential
amplifier 64 is multiplied in multiplier 66 by a variable gain K.sub.10
controlled by the operator variable control voltage V.sub.10. The output
of multiplier 66 is proportional to the torque T.sub..psi.w applied at the
gimbal 20. This signal is both displayed on the master control console and
applied through the servo compensation network 67 and the limiter 68 to
the torque motor at the gimbal 20.
The wrist roll rate servo shown in FIG. 7 follows the same construction as
the wrist pitch rate and wrist yaw rate servos shown in FIGS. 5 and 6.
More specifically, the wrist roll command .phi..sub.wc is applied as one
input to multiplier 69 which multiplies the command signal by the variable
gain K.sub.11 controlled by the control voltage V.sub.11. The output of
multiplier 69 is applied by way of summing amplifier 70 and limiter 71 to
one input of the differential amplifier 72. The differential amplifier 72
is the summing junction of the wrist rate servo and receives as its other
input the wrist roll rate .phi..sub.w through the tachometer ripple filter
73. The error output signal from the differential amplifier 72 is
multiplied in multiplier 74 by the variable gain K.sub.12 controlled by
the control voltage V.sub.12. The output of the multiplier 74 is the
derived wrist roll torque T.sub..phi.w and this signal is both displayed
on the master control console and applied through the servo compensation
network 75 and the limiter 76 to the torque motor at the gimbal 21.
FIG. 8 shows the complete translation servo system and generally comprises
an operator's console 77 and the three manipulator arm translation servos
78. The operator's console 77 comprises a translational controller 79
which is operated in three degrees of freedom by a "joy" stick 80. The
output of the translational controller 79 are the range, azimuth and
elevation rate command signals R, A and E. These are provided by means of
the selector switch 81 to the inputs of each of the translation rate
servos. The outputs of the translational controller 79 are also provided
to the inputs of a translation T matrix which translates the spherical
coordinate range, azimuth elevation rate command signals into cartesian
coordinate rate signals. The selector switch 81 may be moved to its lower
position by the operator to select cartesian coordinate rate command
signals in place of the spherical coordinate rate command signals. The
transformation T matrix equations are as follows:
##EQU5##
where
D.sub.ij = Euler angle coordinate transformation from i.sup.th to j.sup.th
coordinate system, and
x.sub.3, y.sub.c, z.sub.3 = system 3 body rate Cartesian coordinates.
Substituting the D transformations yields:
##EQU6##
where
##EQU7##
FIG. 1 illustrates the four system body rate cartesian coordinates. In the
first cartesian coordinate system, the vector x.sub.1 is parallel with the
arm segment 11. In the second cartesian coordinate system, the vector
x.sub.2 is parallel with the arm segment 12. In the third cartesian
coordinate system the vector x.sub.3 is parallel with the axis of wrist
segment 18. Finally, in the fourth cartesian coordinate system the x.sub.4
vector is parallel with the range vector r. The output of the selector
switch 81 are applied to the inputs of multipliers 22, 32 and 41,
respectively.
The operator console 77 also includes a variable gain select panel 83,
typically in the form of a panel of six potentiometers for establishing
the control voltages V.sub.1 to V.sub.6. The outputs of this variable gain
select panel 83 are provided by each of the multipliers 22, 27, 32, 37, 41
and 49. The operator's console also includes a force meter display 84
typically constituted by three galvanometer-type meters. These meters are
each responsive to the outputs of multipliers 27, 37 and 49, respectively.
Also as shown in FIG. 8, the input KT.sub..theta.s to amplifier 29 (see
FIG. 2) is provided by attenuator 93 connected to the output of multiplier
50.
FIG. 9 shows the complete rotation servo system and comprises an operator's
console 85 and a rotation servo system 86. The operators console includes
a rotational controller 87 which is operated by means of a three degree of
freedom "joy" stick 88. The outputs of the rotational controller 87 are
the wrist pitch, yaw and roll rate commands .theta..sub.wc .psi..sub.wc
and .phi..sub.wc . These commands are connected to the inputs of
multipliers 53, 61 and 69, respectively. The operator's console 85 also
includes a "hawk" command matrix 89 with an operator selector switch 90.
With selector switch 90 closed, the "hawk" command matrix 89 provides hawk
command signals .theta..sub.wh, .psi..sub.wh and .phi..sub.wh to summing
amplifiers 54, 62 and 70, respectively. The full "hawk" equations are as
follows:
##EQU8##
where D.sub.ij = Euler angle coordinate transformation from i.sup.th to
j.sup.th coordinate system,
D.sub..epsilon.l/.sup.-1 = Euler rate transformation from shoulder pitch
and yaw gimbal rates to axis system 1 body rates, and D.sub..epsilon.3 =
Euler rate transformation from axis system 3 body rates to wrist gimbal
rates. substituting the D transformations yields:
##EQU9##
Like the operator's console 77, the operator's console 85 includes a
variable gain select panel comprising six potentiometers to establish the
control voltages V.sub.7 to V.sub.12 which are supplied to each of the
multipliers 53, 58, 61, 66, 69 and 74. The operator's console 85 also
includes a torque meter display in the form of three galvanometer-type
meters which are responsive to the outputs of multipliers 58, 66 and 74,
respectively.
It will be appreciated that FIGS. 8 and 9 have been separated for
convenience of illustration only since the translation and rotation servo
systems are substantially independent. However, in the actual embodiment
of the invention, the operator's console 77 and 85 are unitary structures
and may provide in addition to the force meter display 84 and torque meter
display 92, a closed circuit television monitor where direct viewing of
the manipulator arm is not possible. With respect to the transformation T
matrix 82 and the "hawk" mode matrix 89, those skilled in the art will
recognize that these matrices are readily implemented from the equations
given hereinabove. A review of these equations will show that not only is
a position sensor required at the gimbal 15 as previously mentioned,
position sensors are also required at gimbals 14, 19 and 20 to implement
the equations. More specifically, the transformation T matrix 82 receives
as inputs, not only the outputs of the translational controller 79, but
also outputs from the position sensors at gimbals 15, 19 and 20. The sine
and cosine functions requires by the transformational T matrix are readily
obtained through the use of resolvers as is well known in the art. On the
other hand, the "hawk" mode control matrix 89 requires as inputs the
outputs of position sensors at gimbals 14, 15, 19 and 20 and also the
outputs of the rate sensors at gimbals 13, 14 and 15. Again, the
trigonometric functions required by the "hawk" mode command matrix are
readily implemented using resolvers and dividers as is well known in the
art.
Having described the invention in terms of a preferred embodiment, it will
be appreciated by those skilled in the art that the invention can be
practiced by other and different embodiments within the scope of the
appended claims.
* * * * *
|
|
|
|
|
Description  |
|