Computer based control system

Description

BACKGROUND

The present invention relates to motion control, and more particularly to an improved microcontroller based control system for apparatus such as a robotics mechanism.

In the past, non-linear control problems required complicated intermediate steps involving linearization of differential equations. The linearization resulted in controls that were not exact, so that the required trajectory was not precisely followed by the mechanical system. Also, the computation of control force using linearized equations required powerful microprocessors.

Controls for mechanical systems (such as robotic systems, structural systems, etc.), have been developed using linear feedback, non-linear feedback, or non-linear cancellation. However, none of these systems provide a closed form, real-time control force signal for any prescribed trajectory. The required feedback and the computation of linearized equations make the control force signal computationally expensive and imprecise. It is believed that the only prior art disclosing a closed form, real-time solution to the control problem is the doctoral thesis of Hee-Chang Eun, entitled

On the Dynamics and Control of Constrained Mechanical and Structural Systems.

However this work was limited to trajectories that are not dependent on measurement parameters, and had a restrictive limitation on the weighting of control forces. Also, Hee-Chang's work requires {1,2,3,4} inversion of matrices, which is computationally restrictive.

SUMMARY

The present invention meets the needs by a control system wherein closed form control forces are determined in real time and applied to satisfy specific trajectory requirements of a physical plant which can be a nonlinear mechanical system. Generally, the present invention provides a system and method for determining exact control forces to be applied to any nonlinear mechanical system so that certain physical points in the system follow user defined trajectory requirements. As used herein, trajectory requirements are mathematical relationships between measurement quantities. The trajectory requirements can have functional dependence on a measurement vector such as position in space, velocity of the measurement vector and time; and the user also has the ability to apply weighting factors to the control forces. Further, fast computation of control forces that includes matrix manipulation is enabled using any {1,4} inverse of a computed matrix that is derived from dynamic attributes and measurements of the system. Moreover, the weighted control force resources are minimized at each instant of time, not just in an overall sense for the complete trajectory. Thus the invention provides a closed form, real-time solution to the problem of the control of non-linear mechanical and structural systems.

In one aspect of the invention, a control system is provided for operating a physical plant to follow predetermined desired trajectory requirements, wherein forces acting on the elements can include inertial forces, externally applied forces, and control forces, the plant having interrelated mechanical elements and a response characteristic described by M(x,t){umlaut over (x)}=F(x,{dot over (x)},t) in generalized coordinates x(t) wherein F is exclusive of control forces. The control system includes a control computer; means for storing in the computer values corresponding to M and F of the response characteristic; means for measuring a p-vector y(t) related to x(t); means for specifying the trajectory requirements as h

i

(y,{dot over (y)},t)=0, i=1,2, . . . , s and/or g

i

(y,t)=0, i=1,2, . . . , (m−s); means for transforming and storing in the computer the trajectory requirements as {dot over (h)}=−ƒ

(1)

(h,t) and/or {umlaut over (g)}=−ƒ

(2)

(g,{dot over (g)},t), wherein a fixed point h=0=g={dot over (g)}is asymptotically stable within a domain of attraction that includes deviations from the trajectory requirements; means for determining control forces as

F^{c} = G^{*} ([\begin{matrix} D (h) - f^{(1)} (h, t) \\ \tilde{D} (g) - f^{(2)} (g, \dot{g}, t) \end{matrix}] - [\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} 0 \\ \tilde{D} (φ) \end{matrix}] - ([\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} C \\ \frac{\partial φ}{\partial x} \end{matrix}]) M^{- 1} F),

G* being any {1,4} inverse of BM

−1

; and means for driving the plant by the control forces, thereby to generate the desired trajectory.

The response characteristic of the plant can be nonlinear. The control system can further include means for storing a positive definite matrix N for weighting of control forces; and wherein the means for determining control forces is further responsive to the matrix N, whereby

F^{c} = N^{- 1 / 2} G^{*} ([\begin{matrix} D (h) - f^{(1)} (h, t) \\ \tilde{D} (g) - f^{(2)} (g, \dot{g}, t) \end{matrix}] - [\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} 0 \\ \tilde{D} (φ) \end{matrix}] - ([\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} C \\ \frac{\partial φ}{\partial x} \end{matrix}]) M^{- 1} F) .

G* being any {1,4} inverse of B(N

½

M)

−1

.

The trajectory transformation equations {dot over (h)}=−ƒ

(1)

(h,t) and {umlaut over (g)}=−ƒ

(2)

(g,{dot over (g)},t) can be globally asymptotically stable at the point h=0=g={dot over (g)}. The p-vector can be linearly related to the response vector x(t). Alternatively, the means for specifying the p-vector further includes means for specifying a k-subvector being linearly related to the system response vector x(t); means for specifying a (p-k)-subvector having components being nonlinear functions of elements of the vector x(t) and possibly the time t; and means for determining physical values of the elements of the vector x(t) that are in the (p-k)-subvector.

In another aspect of the invention, a method for controlling the plant to follow predetermined desired trajectory requirements includes the steps of:

(a) providing a control computer,

(b) determining and storing in the computer a response characteristic of the plant as a matrix of force elements being related to a matrix of mass elements and acceleration in a vector of generalized coordinates x(t);

(c) specifying a measurement p-vector y(t) related to x(t);

(d) specifying the trajectory requirements as h

i

(y,{dot over (y)},t)=0, i=1,2, . . . , s and/or g

i

(y,t)=0, i=1,2, . . . , (m−s);

(e) transforming and storing in the computer the trajectory requirements as {dot over (h)}=−ƒ

(1)

(h,t) and/or {umlaut over (g)}=−ƒ

(2)

(g,{dot over (g)},t), wherein a fixed point h=0=g={dot over (g)} is asymptotically stable within a domain of attraction that includes deviations from the trajectory requirements;

(f) determining and storing in the computer measurements of the p-vector;

(g) determining control forces as

F^{c} = G^{*} ([\begin{matrix} D (h) - f^{(1)} (h, t) \\ \tilde{D} (g) - f^{(2)} (g, \dot{g}, t) \end{matrix}] - [\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} 0 \\ \tilde{D} (φ) \end{matrix}] - ([\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} C \\ \frac{\partial φ}{\partial x} \end{matrix}]) M^{- 1} F),

G* being any {1,4} inverse of BM

−1

;

(h) applying the control forces; and

(i) repeating steps (f-h) for generating a control trajectory of the plant corresponding to the trajectory requirements.

The method can include the further step of specifying a positive definite matrix N for weighting of control forces, and wherein the step of determining control forces uses

F^{c} = N^{- 1 / 2} G^{*} ([\begin{matrix} D (h) - f^{(1)} (h, t) \\ \tilde{D} (g) - f^{(2)} (g, \dot{g}, t) \end{matrix}] - [\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} 0 \\ \tilde{D} (φ) \end{matrix}] - ([\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\begin{matrix} C \\ \frac{\partial φ}{\partial x} \end{matrix}]) M^{- 1} F)

G* being any {1,4} inverse of B(N

½

M)

−1

.

The step of specifying the p-vector can include the further steps of:

(a) specifying a k-subvector being linearly related to the system response vector x(t);

(b) specifying a (p-k)-subvector having components being nonlinear functions of elements of the vector x(t) and possibly the time t; and

(c) determining physical values of the elements of the vector x(t) that are in the (p-k)-subvector.

DRAWINGS

These and other features, aspects, and advantages of the present invention will become better understood with reference to the following description, appended claims, and accompanying drawings, where:

FIG. 1

is a schematic block diagram illustrating a preferred control system according to the invention.

FIG. 2

is a pictorial diagram of an exemplary 3-link robot arm plant portion of the control system of

FIG. 1

;

FIGS. 3A and 3B

are plotted comparisons of desired and actual trajectories for respective first and second case implementations of the system of

FIG. 1

having the exemplary plant portion of FIG.

2

.

FIGS. 4A and 4B

are plots of a first joint angle θ

1

of the plant portion of

FIG. 2

for the respective first and second case implementations of

FIGS. 3A and 3B

;

FIGS. 5A and 5B

are plots of a second joint angle θ

2

of the plant portion of

FIG. 2

for the respective first and second case implementations of

FIGS. 3A and 3B

;

FIGS. 6A and 6B

are plots of a control torque on a first joint of the plant portion of

FIG. 2

for the respective first and second case implementations of

FIGS. 3A and 3B

;

FIGS. 7A and 7B

are plots of a control torque on a second joint of the plant portion of

FIG. 2

for the respective first and second case implementations of

FIGS. 3A and 3B

;

FIGS. 8A and 8B

are plots of a control torque on a third joint for the respective first and second case implementations of

FIGS. 3A and 3B

;

FIGS. 9A and 9B

, and

FIGS. 9C and 9D

are plots of respective errors E

1

and E

2

in satisfaction of the trajectory requirements for the respective first and second case implementations of

FIGS. 3A and 3B

;

FIGS. 10A and 10B

are plots of errors in satisfaction of the trajectory requirements for the respective first and second case implementations of

FIGS. 3A and 3B

when starting from an incorrectly measured initial state;

FIG. 11

is a plotted comparison of desired and actual trajectories for a third case implementation of the system of

FIG. 1

having the exemplary plant portion of

FIG. 2

when starting from an incorrectly measured initial state; and

FIG. 12

are plotted coordinate errors in satisfaction of the trajectory requirements of the implementation for the third case implementation, showing rapid convergence to the exact trajectory requirements.

DESCRIPTION

The present invention is directed to a control system that is particularly effective in utilizing motive resources of a physical plant for satisfying trajectory requirements that depend on measurements, without imposing excessive computational complexity. Principally, required control forces are determined so that specific trajectory requirements can be exactly satisfied by a given nonlinear mechanical system. The invention uses measurements of state variables, and transforms the measured velocity and displacement trajectory requirements of the plant into required accelerations, and thereby applies “exact” control forces to the mechanical system. Multiple control forces can be advantageously weighted by matrix manipulation for conserving plant resources and improving response dynamics. Further to ensure robustness in the satisfaction of the constraint requirements, these transformed constraint requirements can be further transformed and incorporated into any system of differential equations which ensures that the original constraint requirements are asymptotically always satisfied. The above-identified transformations can be done in a single step. The transformation of original constraint requirements in terms of accelerations allows indusion of trajectory requirements which are (a) not integrable, (b) integrable, or (c) a mix of the two. Further, the specific matrix manipulations permit functionally dependent trajectory requirements to be satisfied without ill-effects; the simple matrix manipulations carried out in accordance with the invention lead to the production in real time of the exact control forces which minimize the weighted control cost at each instant of time. The simplicity and speed of the method makes it amenable to real time digital or analog control.

With reference to

FIG. 1

of the drawings, a control system

10

according to the present invention includes a mechanical or structural system (or plant)

11

that is generally described by the following equation

M

(

x,t

)

{umlaut over (x)}=F

(

x,{dot over (x)},t

),

{dot over (x)}

(0)=

x

0

, {dot over (x)}

(0)=

{dot over (x)}

0

(Eq.1)

where, t is time, x(t) is an n-vector of generalized coordinates, and M represents an n by n mass matrix M

12

that is positive definite. F represents a vector F

14

that contains externally applied generalized forces on the plant

11

along with inertial forces, which may be functions of x, {dot over (x)} and t. The mechanical plant

11

may be linear or nonlinear, conservative or non-conservative. The control system

10

determines a specific generalized control force n-vector F

c

to be applied at each instant of time to the system (plant

11

) described by Equation (1), so that the controlled system which is now described by the equation M(x,t){umlaut over (x)}=F(x,{dot over (x)},t)+F

c

satisfies given user-prescribed trajectory requirements.

Measurements

The mechanical plant

11

has n-degrees of freedom and a p-measurement vector y(t)

16

comprising a k-subvector, the components of which are each linearly related to the response x(t) of the system; and a (p-k)-subvector, the components thereof being nonlinear functions of some r, 1≦r≦n, elements x

a

1

, x

a

2

, . . . , x

a

r

, of the vector x(t). Here, each a

i

belongs to the set of numbers 1 to n. The vector y(t) is given by the following equation:

\begin{matrix} y (t) = [\frac{y^{(l)} (t)}{y^{(nl)} (t)}] = [\frac{Cx (t)}{φ (x^{(r)} (t), t)}] & (Eq .2) \end{matrix}

where C is a constant k by n matrix, and the (p-k)-vector Φ has each of its components as a function of the vector x

(r)

(t)=[x

a

1

, x

a

2

, . . . , x

a

r

]

T

, and possibly the time t. These measurements y(t) and {dot over (y)}(t) taken from the system are utilized in the following way to find the control forces which ensure that the system satisfies a given set of trajectory requirements.

Desired Trajectory

The main idea behind the present invention is to find and apply a control force F

c

(t)

18

using a control computer

20

so that the mechanical plant

11

follows the user-specified trajectory requirements

31

given by the following equations:

h

i

(

y,{dot over (y)},t

)=0

,i

=1,2

, . . . , s;

(Eq.3)

and

g

i

(

y,t

)=0

,i=

1,2, . . . , (

m−s

) (Eq.4)

Trajectory requirements specified by Equations (3) and (4) must be physically meaningful in that the system should be able to satisfy them. Equations (3) and (4) describing the trajectory requirements are specified as implicit functions. Instead of implicitly describing the trajectory requirements by h and/or g, they may also be explicitly described so that {dot over (y)} is an explicit function of y,t and/or y is an explicit function of t. Further, the equation h=0 31 could be integrable, or nonintegrable, and the equations h=0 and g=0 31 could be linear or nonlinear in their arguments. By integrable is meant that Equation (3) could be integrated to yield a requirement of the form of Equation (4); by nonintegrable, is meant that Equation (3) cannot be integrated without knowledge of the state, x, of the system, which in turn cannot be determined a priori without knowledge of the control forces needed to satisfy the trajectory requirements given by Equation (3).

The transformation of the trajectory requirement equations into a form that makes computation of F

c

18

robust and efficient is best achieved by having derivatives of trajectory requirement equations 31 represented by any equations 32 such that h=0=g is a fixed point for the transformed equations 32 and this fixed point is globally asymptotically stable. The equations 32 are the following:

(

i

)

{dot over (h)}=−ƒ

(1)

(

h,t

), (Eq.5a)

h being an s-vector with components h

i

, so that h(t)→0 as t→∞. An example is {dot over (h)}=−∇V(h), where V(h) is a positive definite, radially unbounded function with V(0)=0, then h=0 is globally asymptotically stable. Another example is {dot over (h)}

i

=−α

i

h

i

, 1≦i≦s, with constants α

i

>0 for 1≦i≦s; here, global exponential asymptotic stability of h=0 is obtained. Yet another example is {dot over (h)}

i

=−α

i

th

i

, and α

i

>0 for 1≦i≦s, with asymptotic convergence to h=0.

(

ii

)

{umlaut over (g)}=−ƒ

(2)

(

g,{dot over (g)},t

) (Eq.5b)

g being an (m-s)vector with components g

i

, so that g(t), {dot over (g)}(t)→0 as t→∞. An example is {umlaut over (g)}

i

=−(α

i

{dot over (g)}

i

+β

i

g

i

), with α

i

>0, β

i

>0, for 1≦i≦(m−s). Another example is {umlaut over (g)}

i

=−(ζ

i

{dot over (g)}

i

+ω

i

g

i

+η

i

g

3

), with constants ζ

i

, ω

i

>0 and constants η

i

>0, for 1≦i≦(m−s).

Equations (5a) and (5b) that are not globally asymptotically stable may also be used to represent the derivatives of trajectory equations 32, provided that the fixed point h=0=g={dot over (g)} of equation 32 has a large enough domain of attraction, thereby ensuring that the trajectories starting in that domain asymptotically approach the fixed point h=0=g={dot over (g)}. By domain of attraction is meant the region in phase space (h,g,{dot over (g)}) from which trajectories are attracted to the fixed point h=0=g={dot over (g)}.

Differenfiating Equations (3) and (4) gives

\begin{matrix} \frac{\partial h_{i}}{\partial \dot{y}} \ddot{y} = - [\frac{\partial h_{i}}{\partial t} \dot{y} + \frac{\partial h_{i}}{\partial t}] \equiv D (h_{i}); i = 1, 2, \dots, s; and & (Eq .6) \\ \frac{\partial g_{i}}{\partial y} \ddot{y} = - [\frac{\partial^{2} g_{i}}{\partial t^{2}} + 2 \frac{\partial}{\partial y} (\frac{\partial g_{i}}{\partial t}) \dot{y} + {\dot{y}}^{T} \frac{\partial^{2} g_{i}}{\partial y^{2}} \dot{y}] \equiv \tilde{D} (g_{i} (y, t)); i = 1, 2, \dots, (m - s), & (Eq .7) \end{matrix}

where the operator

\frac{\partial}{\partial y}

is a p-dimensional row vector, and the operators D(h

i

) and {tilde over (D)}(g

i

(y,t)) are defined in equations (6) and (7) respectively. For computational ease, Equations (6) and (7), along with Equations (5a) and (5b) are combined to give the following:

\begin{matrix} \begin{matrix} \hat{A} (y, \dot{y}, t) \ddot{y} \equiv [\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] \ddot{y} = [\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] [\frac{{\ddot{y}}^{(l)}}{{\ddot{y}}^{(nl)}}] \\ = \hat{[\begin{matrix} D (h) - f^{(1)} (h, t) \\ \tilde{D} (g) - f^{(2)} (g, \dot{g}, t) \end{matrix}] \equiv b} (y, \dot{y}, t), \end{matrix} & (Eq .8) \end{matrix}

where the matrix Â

33

is m by p, and the m-vector {circumflex over (b)}

34

comprises the s-vector D(h) and the (m−s)-vector {tilde over (D)}(g), the i-th component thereof being given by Equations 6 and 7 respectively.

Now, differentiating Equation (2) gives the following equation:

\begin{matrix} \ddot{y} (t) = [\frac{{\ddot{y}}^{(1)} (t)}{y^{(nl)} (t)}] = [\frac{C \ddot{x} (t)}{\frac{\partial φ}{\partial x} \ddot{x} + \tilde{D} (φ)}] = [\frac{C}{\frac{\partial φ}{\partial x}}] \ddot{x} + [\frac{0}{\tilde{D} (φ)}], & (Eq .9) \end{matrix}

where the components, {tilde over (D)}(Φ

i

(x,t)), of the (p-k)-vector {tilde over (D)}(Φ)

36

are defined in a manner corresponding to that in Equation (7). Using Equation (9) in Equation (8) gives the following equation:

\begin{matrix} \begin{matrix} B (y, \dot{y}, x^{(r)}, t) \ddot{x} \equiv \hat{A} [\frac{C}{\frac{\partial φ (x^{(r)}, t)}{\partial x}}] \ddot{x} = \hat{b} - \hat{A} [\frac{0}{\tilde{D} (φ (x^{(r)}, t)}] \\ \equiv b (y, \dot{y}, x^{(r)}, {\dot{x}}^{(r)}, t), \end{matrix} & (Eq .10) \end{matrix}

where B

35

is an m by n matrix, and is a product of Â

33

and the p by n matrix Ĉ

26

. The vector b

38

is defined to represent the right hand side of Equation (10) and is calculated as:

b={circumflex over (b)}−Δb,

(Eq.11)

where Δb

37

is given by:

\begin{matrix} Δ b = \hat{A} [\frac{0}{\tilde{D} (φ (x^{(r)}, t)}] . & (Eq .12) \end{matrix}

If all measurements are linearly related to the system response x(t), then the measurement p-vector in Equation (2) consists of only y

(1)

and Equation (10) simplifies to:

B

(

y,{dot over (y)},t

)

{umlaut over (x)}≡ÂC{umlaut over (x)}={circumflex over (b)}=b,

(Eq.13)

where both matrix B and vector b are determined from measurements y, {dot over (y)} and time t.

Control Force Weighting

The present invention allows weighting of the control forces using N as representing any n by n matrix N

21

that is positive definite. The values of elements in the matrix N may be any predetermined amounts depending on the specific control forces desired for the situation. For example, if one joint in a mechanical system is required to handle less torque than others for structural reasons, then the torque on that joint may be given a higher weighting.

The control forces F

c

18

are then computed by minimizing at each instant of time the following cost quantity J given by:

J

(

t

)=(

F

c

)

T

N

(

x

(

t

),

t

)

F

c

(Eq.14)

where N(x(t),t)

21

is the weighting matrix at each instant of time t. An example of N(x(t),t) is N=M

−½

(x(t),t); another is N equal to a suitable diagonal matrix all of whose diagonal elements are positive constants.

The magnitude of the control forces F

c

18

, which will cause the mechanical plant

11

to exactly follow the trajectory requirements

32

, is then computed as:

F

c

=N

−½

G*[b−BM

−½

F]=N

−½

G*[b−d]

(Eq.15)

where G* denotes any {1,4} inverse of the matrix G, and the matrix G denotes the matrix G=B(N

½

M)

−1

. A proof of Equation (15) is given below.

Proof of Eq. 15

Let

z

(

t

)=

N

½

F

c

=N

½

(

M{umlaut over (x)}−F

), (Eq.A1)

then

J

(

t

)=∥

z

(

t

)∥

2

2

, (Eq.A2)

and from Equation (A1) the following equation,

{umlaut over (x)}

=(

N

½

M

)

−1

(

z+N

½

F

), (Eq.A3)

is obtained.

Since the controlled system must satisfy Equation (10),

B{umlaut over (x)}=b (Eq.A4)

Using equation (A3) in equation (A4), the controlled system must satisfy the relation

B

(

N

½

M

)

−1

(

z+N

½

F

)=b (Eq.A5)

which yields, after setting G=B(N

½

M)

−1

,

Gz=b−BM

−1

F.

(Eq.A6)

But the vector z which satisfies (Eq.A6) while minimizing (Eq.A2) is given by

z

(

t

)=

G*

(

b−BM

−1

F

), (Eq.A7)

where G* is any {1,4}-inverse of the matrix G.

Using z from Equation (A7) in Equation (A1), the relation

F

c

=N

−½

G*

(

b−BM

−1

F

) (Eq.A8)

is obtained.

Control Force Output

Now, using the predetermined values of N

21

, the values of N

½

22

and N

−½

23

are computed. Next, using B

35

, N

½

22

, and M

12

, the value of G

24

is computed as

G=B

(

N

½

M

)

−1

(Eq.16)

G*

25

is any {

1

,

4

} inverse of G

24

.

Using the matrix M

12

, the force vector F

14

and the computed value of B

35

, a value of d(t)

39

is calculated as

d

(

t

)=

BM

−1

F.

(Eq.17)

The value of e(t)

27

is then calculated as the difference between b(t)

38

and d(t)

39

so that

e

(

t

)=

b

(

t

)−

d

(

t

). (Eq.18)

Computation of the exact control force F

c

18

is performed in box

28

using the values of N

−½

23

, G*

25

, and e(t)

27

,

F

c

=N

−½

G* e

(

t

). (Eq.19)

Control Computer

As shown in

FIG. 1

, the control computer

20

receives and stores the mass matrix M

12

, the force matrix F

14

, the weighting matrix N

21

, and the h and g trajectory definitions

31

. It will be understood that these definitions can be fixed in firmware or otherwise incorporated in a control program that is loaded into the computer

20

when the control program is dedicated to controlling a single previously defined plant

11

. However, it is contemplated that some or all of these definitions are variable, either in connection with the use of the computer

20

with different embodiments of the plant

11

, with a variably configurable plant

11

, or when the trajectory and/or weighting definitions are not fixed. The control computer

20

also receives and processes the measurements y

16

and outputs the control forces F

c

, in analog and/or digital form. Analog processing is continuous, digital processing being discrete in both value and time. Normally, the processing of digital data is in a rapid repetitive fashion. The computer

20

implements computations as described above for each of the blocks

22

-

28

and

31

-

39

. In digital implementations, the computations can utilize fixed-point or floating-point math, floating-point being suitable in most applications. Preferred digital implementations of the control computer

20

include 32-bit microprocessors such as those of the 68000-series microprocessors that are available from Motorola Corp. of Phoenix, Ariz., and the 486-, 586-, and 686-series microprocessors that are available from Intel Corp. of Sunnyvale, Calif. A suitable program environment that provides floating-point operations for implementing the operations to be performed by the control computer

20

is available as MATLAB® from Mathworks Inc. of Natick, Mass. In a so, MATLAB environment on a personal computer such as Model XPS-90 of Del Corp., Dover Del., several million floating-point operations (FLOPS) are performed per second. The blocks

26

-

28

and

33

-

39

are preferably implemented for rapid processing in that a they generate real-time control loop variables. Thus when high sampling rates and/or rapid transient response are required, some or all of these blocks can be implemented for reduced processing delays using methods that are within the skill of control system programmers. In many applications implementing the present invention, resort to special programming techniques is not required. When the trajectory requirements define a physical trajectory having movements on the order of meters, the level of accuracy of the controlled motion can typically be 10

−6

meter (one micron).

In accordance with the above, the present invention provides a method for controlling a mechanical system or plant which can be nonlinear, conservative or non-conservative as described by

M

(

x,t

)

{umlaut over (x)}=F

(

x,{dot over (x)},t

),

a preferred implementation of the method including the steps of:

(a) Specifying a measurement vector including a k-subvector being linearly related to the system response vector x(t) and a (p-k)subvector having components being nonlinear functions of elements of the vector x(t) and possibly the time t;

(b) Determining physical values of the elements of the vector x(t) that are in the (p-k)-subvector;

(c) Specifying trajectory requirements of the form h

i

(y,{dot over (y)},t)=0,i=1,2, . . . , s and/or g

i

(y,t)=0,i=1,2, . . . , (m−s);

(d) Transforming the trajectory requirements as {dot over (h)}=−ƒ

(1)

(h,t), and/or {umlaut over (g)}=−ƒ

(2)

(g,{dot over (g)},t)

wherein {dot over (h)}=−ƒ

(1)

(h,t), and {umlaut over (g)}=−ƒ

(2)

(g,{dot over (g)}, t) are asymptotically stable within a domain of attraction about a fixed point h=0=g={dot over (g)}, the domain of attraction being sufficiently large to include deviations from the specified trajectory requirements;

(e) Specifying a positive definite matrix N of any magnitude for a desired weighting of control forces;

(f) Measuring the elements of the p-vector;

(g) Determining specific control forces F

c

=N

−½

G* e(t) wherein G* is any {1,4}-inverse of the matrix G, thereby minimizing J(t)=(F

c

)

T

N(x(t), t)F

c

at every instant of time;

(h) Applying the control forces to the plant and

(i) Repeating steps f-h, thereby generating the control trajectory in operation of the plant.

EXAMPLE

With further reference to

FIGS. 2-12

, the performance of the model proposed in

FIG. 1

was examined in computer simulation experiments using a three-link robot arm

30

shown in FIG.

2

. The robot arm

30

has three degrees of freedom in the vertical plane and represents a simple physical embodiment of the model plant

11

of FIG.

1

. The arm

30

operates relative to a rigid horizontal body or base

40

, and includes respective first, second, and third links

41

,

42

, and

43

that are serially connected end-to-end to the base

40

by corresponding first, second, and third pivotable joints

51

,

52

, and

53

, the first joint

51

being located on the base

40

. The third link

43

acts as an end-effector arm, a free end thereof defining an effector tip

54

. The simulation experiments were directed to satisfaction of various trajectory requirements by the system

10

. At each of the joints

51

,

52

and

53

is a torque motor which can apply a torque to control the motion of the robot arm in response to the control computer

20

.

The masses of the three links

41

,

42

, and

43

are m

1

, m

2

, m

3

, respectively; their moments of inertia about their centers of mass are I

1

, I

2

, I

3

, respectively. The properties of the various links, the joint stiffnesses and the joint dampings of the robot arm

30

are shown in Table 1. They are chosen to represent a typical industrial robot. The quantities k

1

, k

2

, and k

3

are the spring stiffnesses (in units of torque/rad) at the three joints

51

,

52

, and

53

, respectively, and the quantities c

1

, c

2

, and c

3

are the corresponding damping values (in units of torque-sectrad) at these three joints. The second joint

52

is subjected to a horizontal force Psin(pt), and the end-effector tip

54

is subjected to a horizontal force Qsin(qt) and a vertical force Rsin(rt).

TABLE 1

Physical Parameters of Robot Arm 30

Link 41 /

Link 42 /

Link 43 /

Parameter

Joint 51 / θ

1

Joint 52 / θ

2

Joint 53 / θ

3

Mass of link

50

50

20

(kg)

Length of link

2

2

0.8

(m)

Joint Stiffness,

300

300

200

k

(N-m/rad)

Joint Damping,

8

8

6

c

N-m-sec/rad

Three cases were considered for demonstrating different features of the invention. In each case, the magnitudes of the external disturbing forces on the robot arm

30

of

FIG. 2

are as follows: P=100N; Q=300N; R=200N; p=q=1 rad/sec; r=−1 rad/sec. It should be noted that these “disturbing forces” are of the same order of magnitude as the forces exerted by gravity on the robot links. The specific task required of the method in the test simulation is the determination of the torques (generalized forces) needed to be applied at the joints

51

,

52

and

53

of the robotic arm

30

of

FIG. 2

to satisfy particular trajectory requirements using a defined set of measurements of the arm

30

.

Plant Dynamics of Example

The equation of motion of the uncontrolled system is described by the equation:

M{umlaut over (x)}=ƒ=−{L{tilde over (x)}+K

s

x+C

a

{dot over (x)}+e}+ƒ

(e)

(Eq.20)

where,

\begin{matrix} x = [\begin{matrix} θ_{1} \\ θ_{2} \\ θ_{3} \end{matrix}]; e = [\begin{matrix} q_{1} Cos (θ_{1}) \\ q_{2} Cos (θ_{2}) \\ q_{3} Cos (θ_{3}) \end{matrix}]; \tilde{x} = [\begin{matrix} {\dot{θ}}_{1}^{2} \\ {\dot{θ}}_{2}^{2} \\ {\dot{θ}}_{3}^{2} \end{matrix}]; f^{(e)} = [\begin{matrix} f_{1}^{(e)} \\ f_{2}^{(e)} \\ f_{3}^{(e)} \end{matrix}]; & (Eq .21) \\ M = [\begin{matrix} a_{1} & b_{1} c_{12} & b_{2} c_{31} \\ b_{1} c_{12} & a_{2} & b_{3} c_{23} \\ b_{2} c_{31} & b_{3} c_{23} & a_{3} \end{matrix}]; L = [\begin{matrix} 0 & b_{1} s_{12} & - b_{2} s_{31} \\ - b_{1} s_{12} & 0 & b_{3} s_{23} \\ b_{2} s_{31} & - b_{3} s_{23} & 0 \end{matrix}]; and, & (Eq .22) \\ K_{s} = [\begin{matrix} k_{1} + k_{2} & - k_{2} & 0 \\ - k_{2} & k_{2} + k_{3} & - k_{3} \\ 0 & - k_{3} & k_{3} \end{matrix}]; C_{d} = [\begin{matrix} c_{1} + c_{2} & - c_{2} & 0 \\ - c_{2} & c_{2} + c_{3} & - c_{3} \\ 0 & - c_{3} & c_{3} \end{matrix}] . & (Eq .23) \end{matrix}

Here,

a

1

=I

1

+(

m

1

/4

+m

2

+m

3

)

l

1

2

; a

2

=I

2

+(

m

2

/4

+m

3

)

l

2

2

; a

3

=I

3

+m

3

/4

l

3

2

; (Eq.24)

b

1

=(

m

2

/2

+m

3

)

l

1

l

2

; b

2

=m

3

/2

I

1

I

3

; b

3

=m

3

/2

l

2

l

3

; (Eq. 25)

q

1

=(

m

1

/2

+m

2

+m

3

)

{overscore (g)}l

1

; q

2

=(

m

2

/2

+m

3

)

{overscore (g)}l

2

; q

3

=m

3

{overscore (g)}l

3

/2, (Eq.26)

where {overscore (g)} is acceleration due to gravity, 9.81 m/sec

2

;

c

ij

=Cos(θ

i

−θ

j

);

s

ij

=Sin(θ

i

−θ

j

); (Eq. 27)

ƒ

1

(e)

=l

1

Cos(θ

1

)

R

Sin(

rt

)−

l

1

Sin(θ

1

){

P

Sin(

pt

)+

Q

Sin(

qt

)}; (Eq.28)

ƒ

2

(e)

=l

2

Cos(θ

2

)

R

Sin(

rt

)−

l

2

Sin(θ

2

)

Q

Sin(

qt

); (Eq.29)

and,

ƒ

3

(e)

=l

3

Cos(θ

3

)

R

Sin(

rt

)−

l

3

Sin(θ

3

)

Q

Sin(

qt

). (Eq.

30)

The initial conditions are set forth below in Tables 2, 3, and 4, respectively, which summarize the details of the simulations for corresponding Cases 1, 2, and 3.

Measurements and Trajectory Requirements

In Case 1 and Case 2, angular measurements u(t), v(t), and w(t) are defined as follows:

u

(

t

)=θ

1

(

t

)−θ

2

(

t

); (Eq.31)

v

(

t

)=θ

1

(

t

); (Eq.32)

and

w

(

t

)=θ

3

(

t

), (Eq.33)

so that the following trajectory requirements are satisfied:

h

1

(

u,v,w,{dot over (u)},{dot over (v)},{dot over (w)},t

)=

{dot over (u)}

(

t

)+

au

(

t

))

{dot over (v)}

2

(

t

)

w

(

t

)=0, (Eq.34)

and

g

1

(

u,v,w,t

)=

w

(

t

)−

{w

(0)+

b

tan

−1

(

t

)}=0, (Eq.35)

which need to be maintained during the motion of the system through the imposition of a control force 3-vector F

c

. Thus the measurement vector y(t)=[u v w]

T

. The measurements are linear functions of the angles. The system

10

is started from initial (equilibrium) conditions at time t=0. Since the measurements are linear functions of the coordinates x(t), equation 13 (B(,y,{dot over (y)},t){umlaut over (x)}≡ÂC{umlaut over (x)}={circumflex over (b)}=b) can be used as described above to obtain the trajectory requirements in the required form.

Thus

\begin{matrix} \hat{A} = [\begin{matrix} \frac{\partial h}{\partial \dot{y}} \\ \frac{\partial g}{\partial y} \end{matrix}] = [\begin{matrix} 1 & 2 a u \dot{v} w & 0 \\ 0 & 0 & 1 \end{matrix}]; C = [\begin{matrix} 1 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{matrix}]; & (Eq .36) \end{matrix}

so that matrix B in equation 13 is given by

\begin{matrix} B = \hat{A} \hat{C} = [\begin{matrix} 1 + 2 a u \dot{v} w & - 1 & 0 \\ 0 & 0 & 1 \end{matrix}] . & (Eq .37) \end{matrix}

Results

Case 1 determines the specific control torques at joints

51

,

52

and

53

of

FIG. 2

so that the above mentioned trajectory requirements are satisfied. It illustrates the use of functions ƒ

(1)

(h,t) and ƒ

(2)

(g,{dot over (g)},t) which are time dependent and nonlinear, respectively, and a weighting matrix N for the control torques at the joints

51

,

52

, and

53

which is dependent on the mass matrix M of the system. The {11,4}-generalized inverse G*

25

was utilized in determining the specific control torques to be applied at the joints

51

,

52

, and

53

as functions of time. Given the data at any time (t), the computations for digitally determining the control forces for each of blocks

22

-

28

and

31

-

39

in the MATLAB environment include approximately 4635 FLOPS, which translates into a processing time of one millisecond or less on most personal computers. It will be understood that in most cases a subset only of the blocks (blocks

26

-

28

and

33

-

39

, for example) is required to be repetitively computed, significantly reducing the processing time in successive cycles. The specific details of Case 1 are summarized in the Table 2 which follows.

TABLE 2

Case 1

u(t) = θ

1

(t) − θ

2

(t);

Measurements

v(t) = θ

1

(t); w(t) = θ

3

(t);

Initial Conditions

θ

t=0

(rad)

θ

1

= 2; θ

2

= 1.384; θ

3

= 1.226

{dot over (θ)}

t=0

(rad/sec)

θ

1

= 0.2; θ

2

= 0.2016; θ

3

= 0.1

Trajectory

h = {dot over (u)}(t) + au(t){dot over (v)}

2

(t)w(t) = 0;

Requirements

a = 0.05

g = w(t) − w(0) − b arctan(t) = 0;

b = 0.10

Function ƒ

(1)

(h,t)

δ

0

th; δ

0

= 10

Function ƒ

(2)

({dot over (g)},g,t)

ζ

0

{dot over (g)}+ ζ

1

sin(g); ζ

0

= 3; ζ

1

= 5

Weighting Matrix N

M

−1/2

G*

{1,4}-generalized inverse

Case 2 (see Table 3) requires that specific control torques be determined to fulfill the same trajectory requirements as for Case 1, but a different weighting matrix N for the control torques at each of the joints

51

,

52

, and

53

of

FIG. 2

is specified. These weightings on the control torques are of practical importance and are usually related to the capabilities of the torque motors located at each of the three joints. The specific weighting matrix shown in Table 3 below illustrates a requirement that more weight be placed on reducing the control torque at the first joint

51

in comparison to that at the second and third joints

52

and

53

. The computations for each of blocks

22

-

28

and

31

-

39

now include only approximately 1310 FLOPS, significantly fewer than in Case 1, above. The simulation also illustrates by example that a different function ƒ

(1)

(h,t) could be substituted for the one used in Case 1 (see Tables 2 and 3).

TABLE 3

Case 2

u(t) = θ

1

(t) − θ

2

(t);

Measurements

v(t) = θ

1

(t); w(t) = θ

3

(t);

Initial Conditions

θ

t=0

(rad)

θ

1

= 2; θ

2

= 1.384; θ

3

= 1.226

{dot over (θ)}

t=0

(rad/sec)

θ

1

= 0.2; θ

2

= 0.2016; θ

3

= 0.1

Trajectory

h = {dot over (u)}(t) + au(t){dot over (v)}

2

(t)w(t) = 0;

Requirements

a = 0.05

g = w(t) − w(0) − b arctan(t) = 0;

b = 0.10

Function ƒ

(1)

(h,t)

δ

0

h + δ

1

h

3

; δ

0

= 10; δ

1

= 5

Function ƒ

(2)

({dot over (g)},g,t)

ζ

0

{dot over (g)} + ζ

1

sin(g); ζ

0

= 3; ζ

1

= 5

Weighting Matrix N

Diag[10, 1, 1]

G*

{1,4}-generalized inverse

A comparison of desired and actual trajectories of w(t)=θ

3

(t) at joint

53

of

FIG. 2

for Cases 1 and 2 is shown in

FIGS. 3A and 3B

respectively. Note in

FIGS. 3A and 3B

that the actual trajectory follows exactly the desired trajectory in each case, thereby giving a single line plot of trajectories with respect to time. The actual motions of the joints

51

and

52

are shown in

FIGS. 4 and 5

respectively.

FIGS. 6

,

7

and

8

show the specific control torque on joints

51

,

52

, and

53

, respectively, required to be employed for the system to satisfy the given trajectory requirements. Note in

FIG. 6

that the control torque on joint

51

is significantly lower for Case 2 than the corresponding torque for Case 1. In Case 2, a diagonal matrix with higher weighting for control torque for joint

51

compared with joints

52

and

53

was specified. This allowed the user to reduce the required control torque on joint

51

. The errors in the satisfaction of the two trajectory requirements for Cases 1 and 2 are given in FIG.

9

. The trajectory requirements demand that h and g be zero for all time. The errors plotted in

FIG. 9

are E

1

≡h and E

2

≡g, see Tables 2 and 3 above. Note the y-scales of the plots; both these errors are of the order of the relative error bound (10

−8

) used in numerically integrating the differential equations in the simulation.

FIGS. 10A and 10B

deal with the same two Cases 1 and 2, respectively, except that the initial condition θ

2

at t=0 of the robot arm

51

is taken to be in error by 0.1 rad. so that θ

2

(t=0)=1.284 rads. for each case. The initial state thus does not satisfy the trajectory requirement h=0. Note how the errors in the satisfaction of the trajectory requirements for both Cases 1 and 2 rapidly go to zero, indicating that these requirements get rapidly satisfied even when the system starts off by initially violating them.

Case 3 illustrates the determination of the specific control torques required at joints

51

,

52

and

23

of

FIG. 2

so that the end-effector tip

54

of the robot executes a “figure-8” trajectory. Measurements are made at the tip of the end effector; these measurements are nonlinear functions of the state. The simulation experiment in Case 3 includes the effect of an error in the measurement of the initial state of the robotic system. It is shown that if there is an initial error at time t=0 between the desired and actual trajectory requirements, the control forces which are determined according to this invention cause the actual trajectory to rapidly converge to satisfy the desired trajectory requirements. Measurements are made at: xtip, x-coordinate of tip of end-effector arm

43

in

FIG. 2

; ytip, y-coordinate of tip of end-effector arm

43

in FIG.

2

. The other particulars for Case 3 are summarized in Table 4.

A comparison of desired and actual trajectory requirements of the end-effector tip

54

of

FIG. 2

for Case 3 is shown in FIG.

11

. Note in

FIG. 11

that the actual trajectory which has an initial error at t=0 converges rapidly to the desired trajectory. After converging, the actual trajectory follows exactly the desired trajectory. The variation of error (with respect to time) in the satisfaction of the trajectory requirements is given in

FIG. 12

which shows that this error rapidly reaches zero. The error is extremely small, being approximately one micron, the range of movement being approximately 0.4 meter. This is the same order of magnitude as the integration errors in the computations done by the control computer

20

.

TABLE 4

CASE 3

xtip: x-coordinate of the tip of end effector 54

Measurements

ytip: y-coordinate of the tip of end effector 54

Initial Conditions

θ

t=0

(rad)

θ

1

= 2; θ

2

= 1.384 − error*; θ

3

= 1.226

{dot over (θ)}

t=0

(rad/sec)

θ

1

= 0.3; θ

2

= −3.357; θ

3

= 7.93

Trajectory

g

1

(θ

1

,θ

2

,θ

3

,t) = xtip(t) -

Requirements

{xtip(0) + b + bCos(π − αt};

g

2

(θ

1

,θ

2

,θ

3

t) = ytip(t) -

{ytip(0) + bSin(π − βt}

α = 1 rad/sec; β = 2 rad/sec; b = 0.2 m

Function

δ

0

{dot over (g)}

1

+ δ

1

sin(g

1

); δ

0

=3; δ

1

=5

ƒ

1

(2)

({dot over (g)}

1

, g

1

,t)

Function

ζ

0

{dot over (g)}

2

+ ζ

1

sin(g

2

); ζ

0

= 3; ζ

1

= 5

ƒ

1

(2)

({dot over (g)}

2

,g

2

,t)

Weighting Matrix

M

−1/2

N

G*

{1,4}-generalized inverse

*error = 0.01 rads.

Conclusion

The marked advantages of the present invention, which are believed to advance the state of the art, include the following:

1. It provides exact control force so that the mechanical system satisfies a set of user defined trajectory requirements. These trajectory requirements may be: integrable or nonintegrable equations; linear or nonlinear equations; explicit or implicit equations in y and {dot over (y)}. If there is an offset between the trajectory requirements and the initial state of the system, the offset is rapidly eliminated.

2. It allows weighting of control forces using any positive definite matrix N, thereby enabling the use of optimal control forces that minimize F

cT

NF

c

.

3. It minimizes F

cT

NF

c

at every instant of time. Prior controllers minimized integral

\int_{0}^{To} F^{cT} {NF}^{c} ⅆ t,

thereby introducing inaccuracies and perturbations in the compliance of actual trajectory requirements with required trajectory requirements.

4. It allows using any {1,4} inverse of the matrix G, which is faster to compute than a {1,2,3,4} inverse.

5. It executes the trajectory requirements as measured. The measurements may be linear or non-linear functions of the state of the system. The trajectory requirements may be integrable or nonintegrable or a combination of both. The only limitation is that the specified trajectory requirements should be physically attainable. An example is that the trajectory requirements cannot specify an end effector to be at two different locations at the same time, or to have two different velocities at the same time.

6. It provides flexibility in trajectory definition by allowing a combination of requirements, some requirements on measurement y and t, and others tS on measurements y, y and t, and still others on just {dot over (y)} and t.

7. It allows using any set of differential equations 5A and 5B as long as the fixed point h=g={dot over (g)}=0 of these equations is globally asymptotically stable, or the fixed point h=g={dot over (g)}=0 is asymptotically stable within a domain of attraction which includes any deviations from the desired trajectory requirements.

8. It eliminates the need to determine whether the trajectory requirements are functionally independent. Optimal control procedures of the prior art are believed to present great difficulties with trajectory requirements that are not independent.

Although the present invention has been described in considerable detail with reference to certain preferred versions thereof, other versions are possible. The invention has application in diverse dynamical systems such as the control of multiple spacecraft required to move in formation. Therefore, the spirit and scope of the appended claims should not necessarily be limited to the description of the preferred versions contained herein.

Claims

1. A control system for operating a physical plant having interrelated mechanical elements and a response characteristic described by M(x,t){umlaut over (x)}=F(x,{dot over (x)},t) in generalized coordinates x(t) to follow predetermined desired trajectory requirements, wherein forces that can act on the elements include inertial forces, externally applied forces and control forces, F being exclusive of control forces, the system comprising:(a) a control computer; (b) means for storing in the computer values corresponding to M and F of the response characteristic; (c) means for measuring a p-vector y(t) related to x(t); (d) means for specifying the trajectory requirements as hi(y,{dot over (y)},t)=0,i=1,2, . . . , s and/or gi(y,t)=0,i=1,2, . . . , (m−s); (e) means for transforming and storing in the computer the trajectory requirements as {dot over (h)}=−ƒ(1)(h,t) and/or {umlaut over (g)}=−ƒ(2)(g,{dot over (g)},t), wherein a fixed point h=0=g={dot over (g)} is asymptotically stable within a domain of attraction that includes deviations from the trajectory requirements; (f) means for determining control forces as Fc=G*⁢([D⁡(h)-f(1)⁡(h,t)D~⁡(g)-f(2)⁡(g,g.,t)]-[∂h∂y.∂g∂y]⁡[0D~⁡(φ)]⁢([∂h∂y.∂g∂y]⁡[C∂φ∂x])⁢M-1⁢F),(g) means for driving the plant by the control forces, thereby to generate the desired trajectory therein.
2. The control system of claim 1, wherein the response characteristic of the plant is nonlinear.
3. The control system of claim 1, further comprising:(a) means for storing a positive definite matrix N for weighting of control forces; and (b) wherein the means for determining control forces is further responsive to the matrix N, whereby Fc=N-1/2⁢G*⁢([D⁡(h)-f(1)⁡(h,t)D~⁡(g)-f(2)⁡(g,g.,t)]-[∂h∂y.∂g∂y]⁡[0D~⁡(φ)]-([∂h∂y.∂g∂y]⁡[C∂φ∂x])⁢M-1⁢F), G* being any {1,4} inverse of BM−1; and any {1,4} inverse of B(N½M)−1.
4. The control system of claim 3, wherein the response characteristic of the plant is nonlinear.
5. The control system of claim 3, wherein the trajectory transformation equations {dot over (h)}=−ƒ(1)(h,t) and {umlaut over (g)}=−ƒ(2)(g,{dot over (g)},t) are globally asymptotically stable at the point h=0=g={dot over (g)}.
6. The control system of claim 5, wherein the response characteristic of the plant is nonlinear.
7. The control system of claim 1, wherein the p-vector is linearly related to the response vector x(t).
8. The control system of claim 7, wherein the response characteristic of the plant is nonlinear.
9. The control system of claim 1, wherein the means for specifying the p-vector further comprises:(a) means for specifying a k-subvector being linearly related to the system response vector x(t); (b) means for specifying a (p-k)-subvector having components being nonlinear functions of elements of the vector x(t) and possibly the time t; and (c) means for determining physical values of the elements of the vector x(t) that are in the (p-k)subvector.
10. The control system of claim 9, wherein the response characteristic of the plant is nonlinear.
11. A method for controlling a plant having interrelated mechanical elements and a response characteristic described by M(x,t){umlaut over (x)}=F(x,{dot over (x)},t) in generalized coordinates x(t) to follow predetermined desired trajectory requirements, wherein forces that can act on the elements include inertial forces, externally applied forces and control forces, wherein F is exclusive of control forces, the method comprising the steps of:(a) providing a control computer; (b) determining and storing in the computer a response characteristic of the plant as a matrix of force elements being related to a matrix of mass elements and acceleration in a vector of generalized coordinates x(t); (c) specifying a measurement p-vector y(t) related to x(t); (d) specifying the trajectory requirements as hi(y,{dot over (y)},t)=0,i=1,2, . . . , s and/or gi(y,t)=0, i=1,2, . . . , (m−s); (e) transforming and storing in the computer the trajectory requirements as {dot over (h)}=−ƒ(1)(h,t) and/or {umlaut over (g)}=−ƒ(2)(g,{dot over (g)},t), wherein a fixed point h=0=g={dot over (g)} is asymptotically stable within a domain of attraction that includes deviations from the trajectory requirements; (f) determining and storing in the computer measurements of the p-vector; (g) determining control forces as Fc=G*⁢([D⁡(h)-f(1)⁡(h,t)D~⁡(g)-f(2)⁡(g,g.,t)]-[∂h∂y.∂g∂y]⁡[0D~⁡(φ)]-([∂h∂y.∂g∂y]⁡[C∂φ∂x])⁢M-1⁢F),G* being any {1,4} inverse of BM−1;(h) applying the control forces; and (i) repeating steps (f-h) for generating a control trajectory of the plant corresponding to the desired trajectory.
12. The method of claim 11, comprising the further step of specifying a positive definite matrix N for weighting of control forces, and wherein the step of determining control forces uses Fc=N-1/2⁢G*⁢([D⁡(h)-f(1)⁡(h,t)D~⁡(g)-f(2)⁡(g,g.,t)]-[∂h∂y.∂g∂y]⁡[0D~⁡(φ)]-([∂h∂y.∂g∂y]⁡[C∂φ∂x])⁢M-1⁢F), G* being any {1,4} inverse of B(N½M)−1.
13. The method of claim 12, wherein the response characteristic of the plant is nonlinear.
14. The method of claim 12, wherein the trajectory transformation equations {dot over (h)}=−ƒ(1)(h,t) and {umlaut over (g)}=−ƒ(2)(g,{dot over (g)},t) are globally asymptotically stable at the point h=0=g={dot over (g)}.
15. The method of claim 14, wherein the response characteristic of the plant is nonlinear.
16. The method of claim 11, wherein the p-vector is linearly related to the response vector x(t).
17. The method of claim 16, wherein the response characteristic of the plant is nonlinear.
18. The method of claim 11, wherein the step of specifying the p-vector comprises the further steps of:(a) specifying a k-subvector being linearly related to the system response vector x(t); (b) specifying a (p-k)-subvector having components being nonlinear functions of elements of the vector x(t) and possibly the time t; and (c) determining physical values of the elements of the vector x(t) that are in the (p-k)-subvector.
19. The method of claim 18, wherein the response characteristic of the plant is nonlinear.

RELATED APPLICATION

This application claims the benefit of U.S. Provisional Application No. 60/085,561, filed May 15, 1998.

US Referenced Citations (16)

Number	Name	Date
4620829	Herve	Nov 1986
4663726	Chand et al.	May 1987
4807153	Onaga et al.	Feb 1989
4826392	Hayati	May 1989
4843566	Gordon et al.	Jun 1989
4925312	Onaga et al.	May 1990
4990838	Kawato et al.	Feb 1991
5023808	Seraji	Jun 1991
5150026	Seraji et al.	Sep 1992
5179514	Rastegar et al.	Jan 1993
5206930	Ishikawa et al.	Apr 1993
5336982	Backes	Aug 1994
5390288	Toda et al.	Feb 1995
5499320	Backes et al.	Mar 1996
5550953	Seraji	Aug 1996
5594644	Hasegawa et al.	Jan 1997

Non-Patent Literature Citations (1)

Entry
“On the Dynamics and Control of Constrained Mechanical and Structural Systems”, Thesis by Hee-Chang Eun; 117 pp.; Apr. 1995.

Provisional Applications (1)

	Number	Date	Country
	60/085561	May 1998	US

Computer based control system

Information

Patent Number

Date Filed

Date Issued

Inventors

Examiners

Agents

CPC

US Classifications

Field of Search

US