Motion controller for redundant or nonredundant linkages

Claim 3

The apparatus according to claim 1 wherein said joint angle computing means is based on the constrained minimization of a positive semi-definite quadratic form as the objective function referenced to a fixed origin.

Claim 4

The apparatus according to claim 1 wherein said joint angle computing means is based on the constrained minimization of a positive semi-definite quadratic form with a diagonal coefficient matrix as the objective function referenced to a fixed origin.

Claim 5

An apparatus according to claim 1 wherein said joint angle computation means comprises: means for formulating a Jacobian operator of manipulator coordinate transformation equations; and means for performing said joint angle computation without explicity solving for the null space of the Jacobian.

Claim 6

An apparatus according to claim 1 wherein said joint angle computation means includes means for performing an affine transformation whereby the problem is made directly solvable by the Moore-Penrose inverse definition.

Claim 7

An apparatus according to claim 1 wherein said joint angle computation means includes means for performing an affine transformation whereby a problem is produced which, for a given size Jacobian matrix, is solvable by a constant number of algebraic operations, independent of the elements within the matrix.

Claim 8

An apparatus according to claim 1 wherein said joint angle computation means comprises: means for computing a unique set of joint angle values which minimize: ##EQU14## where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the unknown change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression; ##EQU15## where f is the vector of functions mapping joint variables into cartsian coordinates.

Claim 9

An apparatus according to claim 1 wherein said joint angle computation means comprises: a. means for formulating a Jacobian operator of manipulator coordinate transformation equations; b. means for transforming the problem statement of: ##EQU16## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K], {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU17## which is a diagonal matrix; c. means for applying a generalized inverse technique which satisfies the transformed problem statement; and d. means for computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[K]{D}-{θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the unknown change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {θ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression: ##EQU18## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 10

An apparatus according to claim 1 wherein said joint angle computation means comprises: a. means for formulating a Jacobian operator of manipulator coordinate transformation equations; b. means for transforming the problem statement of: ##EQU19## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K], {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU20## which is a diagonal matrix; c. means for transforming the matrix equation [A]{D}={B} into the matrix equation [F]{D}={G} where {G}=[E]{B} and [F]=[E][A] such that [E] produces the following relationship: [I]=[F][F] t ; d. means for calculating the solution of the minimization function as: {D}=[F] t {G}; and e. means for computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[K]{D}-{θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the unknown change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {θ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression: ##EQU21## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 11

An apparatus according to claim 1 wherein said joint angle computation means comprises: a. means for formulating a Jacobian operator of manipulator coordinate transformation equations; b. means for transforming the problem statement of: ##EQU22## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K] {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU23## which is a diagonal matrix; c. means for transforming the matrix equation [A]{D}={B} into the matrix equation [F]{D}={G} where {G}=[E]{B} and [F]=[E][A] such that [E] produces the following relationship: [I]=[F][F] t by the steps of: (1) subtracting from each of the rows of [A] and {B} multiples of their respective first row such that the first row of [A] becomes orthogonal to the remaining rows of [A] and (2) repeating step (1) for each successive row of [A]; d. means for calculating the solution of the minimization function as: {D}=[F] t {G}; and e. means for computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[K]{D}-{θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the unknown change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {Δθ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of activated joints, defined by the following expression: ##EQU24## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 12

The apparatus according to claim 10 further comprising: a. means for determining if the vector magnitude of one of the rows of [A] falls below a predetermined threshold during the execution of said steps c.(1) and c.(2); and b. means responsive to such determination for eliminating the below-threshold matrix row from use in said joint angle computation.

Claim 13

The apparatus according to claim 10 further comprising: a. means for determining if one of the rows of [A] is a linear combination of the previous rows of [A]; and b. means responsive to such determination for eliminating said linearly dependent matrix row from use in said joint angle computation.

Claim 16

An apparatus according to claim 13 wherein said joint angle computation means comprises: means for formulating a Jacobian operator of manipulator coordinate transformation equations; and means for performing said joint angle computation without explicitly solving for the null space of the Jacobian.

Claim 17

An apparatus according to claim 13 wherein said joint angle computation means includes means for performing an affine transformation whereby the problem is made directly solvable by the Moore-Penrose inverse definition.

Claim 18

An apparatus according to claim 13 wherein said joint angle computation means includes means for performing an affine transformation whereby a problem is produced which, for a given size Jacobian matrix, is solvable by a constant number of algebraic operations independent of the elements within the matrix.

Claim 19

An apparatus according to claim 13 wherein said joint angle computation means comprises: means for formulating a Jacobian operator of manipulator coordinate transformation equations; and means for computing a unique set of joint angle values which minimize: ##EQU25## where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the unknown change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression: ##EQU26## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 20

An apparatus according to claim 13 wherein said joint angle computation means comprises: a. means for formulating a Jacobian operator of manipulator coordinate transformation equations; b. means for transforming the problem statement of: ##EQU27## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K], {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU28## which is a diagonal matrix; c. means for applying a generalized inverse technique which satisfies the transformed problem statement; and d. means for computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[K]{D}-{θ.sub.o }) where: n is the number of joints; k i is the predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the unknown change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {θ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression: ##EQU29## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 21

An apparatus according to claim 13 wherein said joint angle computation means comprises: a. means for formulating a Jacobian operator of manipulator coordinate transformation equations; b. means for transforming the problem statement of: ##EQU30## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K], {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU31## which is a diagonal matrix; c. means for transforming the matrix equation [A]{D}={B} into the matrix equation [F]{D}={G} where {G}=[E]{B} and [F]=[E][A] such that [E] produces the following relationship: [I]=[F][F] t ; d. means for calculating the solution of the minimization function as: {D}=[F] t {G}; and e. means for computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[K]{D}-{θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the ith joint; Δθ i is the unknown change in angle for the ith joint from its current position; θ io is a predetermined origin for the i th joint; {θ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression: ##EQU32## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 22

An apparatus according to claim 13 wherein said joint angle computation means comprises: a. means for formulating a Jacobian operator of manipulator coordinate transformation equations; b. means for transforming the problem statement of: ##EQU33## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K], {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU34## which is a diagonal matrix; c. means for transforming the matrix equation [A]{D}={B} into the matrix equation [F]{D}={G} where {G}=[E]{B} and [F]=[E][A] such that [E] produces the following relationship: [I]=[F][F] t by the steps of: (1) subtracting from each of the rows of [A] and {B} multiples of their respective first row such that the first row of [A] becomes orthogonal to the remaining rows of [A] and (2) repeating step (1) for each successive row of [A]; d. means for calculating the solution of the minimization function as: {D}=[F] t {G}; and e. means for computing the change in joint angles from the previously known position such that: {Δθθ=-({θ}-[K]{D}-{θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the ith joint; Δθ i is the unknown change in angle for the ith joint from its current position; θ io is a predetermined origin for the i th joint; {θ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression: ##EQU35## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 23

The apparatus according to claim 20 further comprising: a. means for determining if one of the rows of [A] is a linear combination of the previous rows of [A]; and b. means responsive to such determination for eliminating said linearly dependent matrix now from use in said joint angle computation.

Claim 24

The apparatus according to claim 20 further comprising: a. means for determining if the vector magnitude of one of the rows of [A] falls below a predetermined threshold during the execution of said steps c.(1) and c.(2); and b. means responsive to such determination for eliminating the below-threshold matrix row from use in said joint angle computation.

Claim 27

The method according to claim 23 wherein said joint angle computation step is based on the constrained minimization of a positive semi-definite quadratic form as the objective function referenced to a fixed origin.

Claim 28

The method according to claim 23 wherein said joint angle computation step is based on the constrained minimization of a positive semi-definite quadratic form with a diagonal coefficient matrix as the objective function referenced to a fixed origin.

Claim 29

The method according to claim 23 wherein said joint angle computation step comprises the steps of: formulating a Jacobian operator of manipulator coordinate transformation equations; and computing said joint angles without explicitly solving for the null space of the Jacobian.

Claim 30

The method according to claim 23 wherein said joint angle computation step includes the step of performing an affine transformation whereby the problem is made directly solvable by the Moore-Penrose inverse definition.

Claim 31

The method according to claim 23 wherein said joint angle computation step includes the step of performing an affine transformation whereby a problem is produced which, for a given size Jacobian matrix, is solvable by a constant number of algebraic operations independent of the elements within the matrix.

Claim 32

The method according to claim 23 wherein said joint angle computation step further comprises the steps of: a. formulating a Jacobian operator of manipulator coordinate transformation equations; and b. computing a unique set of joint angle values which minimize: ##EQU36## where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the unknown change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression: ##EQU37## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 33

The method according to claim 23 wherein said joint angle computation step further comprises the steps of; a. formulating a Jacobian operator of manipulator coordinate transformation equations; b. transforming the problem statement of: ##EQU38## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.0 }), [A]=[J°][K] and {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU39## which is a diagonal matrix; c. applying a generalized inverse technique which satisfies the transformed problem statement, d. means for computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[k]{D}-{θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the i th joint; Δθ i is the known change in angle for the i th joint from its current position; θ io is a predetermined origin for the i th joint; {θ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ56 is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of actuated joints, defined by the following expression; ##EQU40## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 34

The method according to claim 23 wherein said joint angle computation step further comprises the steps of: a. formulating a Jacobian operator of manipulator coordinate transformation equations; b. transforming the problem statement of: ##EQU41## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}={K}.sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K] and {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU42## which is a diagonal matrix; c. transforming the matrix equation [A]{D}={B} into the matrix equation [F]{D}={G} where {G}=[E]{B} and [F]=[E][A] such that [E] produces the following relationship: [I]=[F][F] t ; d. calculating the solution of the minimization function as: {D}=[F] t {G}; and e. computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[K]{D}-}θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the ith joint; Δθ i is the unknown change in angle for the ith joint from its current position; θ io is a predetermined origin for the i th joint; {θ} is the vector of the known current position of the joints; {θ o } is the vector of predetermined origins for each joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by n matrix where M is the number of system constraints and n is the number of joints, defined by the following expression: ##EQU43## where f is the vector of functions mapping joint variables into cartesian coordinates.

Claim 35

The method according to claim 23 wherein said joint angle computation step further comprises the steps of: a. formulating a Jacobian operator of manipulator coordinate transformation equations; b. transforming the problem statement of: ##EQU44## into the statement of: minimize 1/2 D.sup.t D subject to: [A]{D}={B} where {D}=[K].sup.-1 ({Δθ}+{θ}-{θ.sub.o }), [A]=[J°][K] and {B}={ΔX}-[J°]{θ.sub.o }+[J°]{θ} and ##EQU45## which is a diagonal matrix; c. transforming the matrix equation [A]≡D}={B} into the matrix equation [F]{D}={G} where {G}=[E]{B} and [F]=[E][A] such that [E] produces the following relationship: [I]=[F][F] t ; by the steps of: (1) subtracting from each of the rows of [A] and {B} multiples of their respective first row such that the first row of [A] becomes orthogonal to the remaining rows of [A] and (2) repeating step (1) for each successive row of [A]; d. calculating the solution of the minimization function as: {D}=[F] t {G}; and e. computing the change in joint angles from the previously known position such that: {Δθ}=-({θ}-[K]{D}-{θ.sub.o }) where: n is the number of joints; k i is a predetermined control value for the i th joint; θ i is the known current position of the ith joint; Δθ i is the unknown change in angle for the ith joint from its current position; θ io is a predetermined origin for the i th joint; {Δθ} is the vector of the unknown change in angle for each joint from its current position; {ΔX} is the vector of the change in cartesian coordinates of the tool position between the current position and the next position; [J°] is the Jacobian matrix evaluated at the current position where the Jacobian is an M by an n matrix where M is the number of system constraints and n is the number of joints, defined by the following expression: ##EQU46## where f is the vector of functions mapping variables into cartesian coordinates.

Claim 36

The method according to claim 32 further comprising the steps of: a. determining if one of the rows of [A] is a linear combination of the previous rows of [A]; and b. eliminating the linearly dependent matrix now from use in said angle computation.

Claim 37

The method according to claim 32 further comprising the steps of: a. determining if the vector magnitude of one of the rows of [A] falls below a predetermined threshold during the execution of said steps c.(1) and c.(2); and b. eliminating the below-threshold matrix row from use in said joint angle computation.