System for compensated motion of coupled robot axes

Claim 2

Apparatus for controlling a manipulator to effect motion of a tool centerpoint, the manipulator including a first rotating member, first drive means for effecting rotation of the first member, a second rotating member and a second drive means for effecting rotation of the second member, the first and second members being mechanically coupled such that absent counter action by the second drive means, rotation of the first member by the first drive means effects rotation of the second member relative to the first member, the apparatus comprising: (a) control means commanding a location of the tool centerpoint; (b) first measuring means responsive to actual rotation of the first member relative to a reference for measuring rotation of the first member relative to the reference; (c) second measuring means responsive to actual rotation of the second member relative to the first member for measuring rotation of the second member relative to the first member, the second measuring means measuring the rotation of the second member caused by the mechanical coupling of rotation of the first member; (d) first servomechanism means responsive to the control means and the first measuring means for producing a first error signal for controlling the first drive means; and (e) second servomechanism means responsive to the control means, the second measuring means, and the first error signal for producing a second error signal for controlling the second drive means to counteract the rotation of the second member caused by the coupled rotation of the first member.

Claim 3

The apparatus of claim 1 further comprising a transfer function means supplying the first error signal to the second servomechanism means, the transfer function means compensating the first error signal in accordance with the mechanical coupling of the first member to the second member.

Claim 4

The apparatus of claim 2 wherein the control means further comprises means for producing first control signals representing a desired rotation of the first member and second control signals representing a desired rotation of the second member, the first and second control signals being defined by a desired position of the tool centerpoint.

Claim 5

The apparatus of claim 3 wherein the first measuring means further comprises means for producing a first position signal representing the actual rotation of the first member relative to the reference.

Claim 6

The apparatus of claim 4 wherein the first servomechanism means further comprises a first comparator responsive to the first control signal and the first position signal for producing the first error signal.

Claim 7

The apparatus of claim 3 wherein the second measuring means further comprises means for producing a second position signal representing an actual rotation of the second member relative to the first member.

Claim 8

The apparatus of claim 6 wherein the second servomechanism means further comprises a second comparator responsive to the first error signal as compensated by the transfer function means, the second control signal, and the second position signal for producing the second error signal.

Claim 9

Apparatus for controlling a manipulator to effect motion of a tool centerpoint, the manipulator including an upper arm pivotally mounted on a base at a first pivot, a forearm pivotally mounted on the upper arm, a first link pivotally connected to the base at the first pivot, a second link pivotally connected to the first link and pivotally connected to the forearm, the upper arm, forearm, first link, and second link defining a four bar link mechanism, a first drive means for effecting rotation of the upper arm, and second drive means for effecting rotation of the first link, the apparatus comprising: (a) control means for commanding a position of the tool centerpoint; (b) first measuring means responsive to actual rotation of the upper arm relative to the base for measuring rotation of the upper arm relative to the base; (c) second measuring means responsive to actual rotation of the forearm relative to the upper arm for measuring rotation of the forearm relative to the upper arm, the second measuring means measuring rotation of the forearm caused by the mechanically coupled rotation of the upper arm; (d) first servomechanism means responsive to the control means and the first measuring means for producing a first error signal for controlling the first drive means; and (e) second servomechanism means responsive to the control means, the first error signal and the second measuring means for producing a second error signal for controlling the second drive means to counteract the rotation of the forearm caused by coupled rotation of the upper arm.

Claim 10

Apparatus for controlling a manipulator to effect motion of a tool centerpoint, the manipulator including an upper arm pivotally mounted on a base at a first pivot, a forearm pivotally mounted on the upper arm, a first link pivotally connected to the base at the first pivot, a second link pivotally connected to the first link and pivotally connected to the forearm, the upper arm, forearm, first link, and second link defining a four bar link mechanism, a first drive means for effecting rotation of the upper arm, and second drive means for effecting rotation of the first link, the apparatus comprising: (a) control means for commanding a position of the tool centerpoint; (b) first measuring means responsive to actual rotation of the upper arm relative to the base for measuring rotation of the upper arm relative to the base; (c) second measuring means responsive to actual rotation of the forearm relative to the upper arm for measuring rotation of the forearm relative to the upper arm, the second measuring means measuring rotation of the forearm caused by the mechanically coupled rotation of the upper arm; (d) first servomechanism means responsive to the control means and the first measuring means for producing a first error signal for controlling the first drive means; and (e) second servomechanism means responsive to the control means, the first error signal and the second measuring means for producing a second error signal for controlling the second drive means to counteract the rotation of the forearm caused by coupled rotation of the upper arm.

Claim 11

The apparatus of claim 8 further comprising a transfer function means supplying the first error signal to the second servomechanism means, the transfer function means compensating the first error signal in accordance with mechanism coupling between the upper arm and forearm of the four-bar linked mechanism.

Claim 12

The apparatus of claim 9 wherein the control means further comprises means for producing first control signals representing a desired rotation of the upper arm and second control signals representing a desired rotation of the forearm, the first and second control signals being defined by a desired position of the tool centerpoint.

Claim 13

The apparatus of claim 10 wherein the first measuring means further comprises means for producing a first position signal representing an actual rotation of the upper arm relative to the reference.

Claim 14

The apparatus of claim 11 wherein the first servomechanism means further comprises a first comparator responsive to the first control signals and the first position signal for producing the first error signal.

Claim 15

The apparatus of claim 10 wherein the second measuring means further comprises means for producing a second position signal representing an actual rotation of the forearm relative to the upper arm.

Claim 16

The apparatus of claim 13 wherein the second servomechanism means further comprises a second comparator responsive to the first error signal as compensated by the transfer function means, the second control signals, and the second position signal for producing the second error signal.

Claim 17

Method for controlling a manipulator to effect motion of a tool centerpoint, the manipulator including two rotating members which are mechanically coupled such that rotation of a first member effects rotation of a second member relative to the first member, first drive means for effecting rotation of the first member, and second drive means for effecting rotation of the second member, the first drive means being responsive to a first servomechanism means and the second drive means being responsive to a second servomechanism means, the method comprising the step of: (a) producing first control signals representing a desired rotation of the first member defined by a desired position of the tool centerpoint; (b) producing second control signals representing a desired rotation of the second member defined by a desired position of the tool centerpoint; (c) producing in response to actual rotation of the first member a first position signal representing the actual rotation of the first member relative to a reference; (d) producing in response to actual rotation of the second member relative to the first member a second position signal representing the actual rotation of the second member relative to the first member, the second position signal representing rotation of the second member caused by the coupled rotation of the first member; (e) producing a first error signal in response to the first control signal and the first position signal, the first error signal controlling the first drive means; and (f) producing a second error signal in response to the first error signal, the second control signal, and the second position signal, the second error signal controlling the second drive means to counteract the rotation of the second member relative to the first member resulting from the coupled rotation of the first member.

Claim 18

Method for controlling a manipulator to effect motion of a tool centerpoint, the manipulator including two rotating members which are mechanically coupled such that rotation of a first member effects rotation of a second member relative to the first member, first drive means for effecting rotation of the first member, and second drive means for effecting rotation of the second member, the first drive means being responsive to a first servomechanism means and the second drive means being responsive to a second servomechanism means, the method comprising the step of: (a) producing first control signals representing a desired rotation of the first member defined by a desired position of the tool centerpoint; (b) producing second control signals representing a desired rotation of the second member defined by a desired position of the tool centerpoint; (c) producing in response to actual rotation of the first member a first position signal representing the actual rotation of the first member relative to a reference; (d) producing in response to actual rotation of the second member relative to the first member a second position signal representing the actual rotation of the second member relative to the first member, the second position signal representing rotation of the second member caused by the coupled rotation of the first member; (e) producing a first error signal in response to the first control signal and the first position signal, the first error signal controlling the first drive means; and (f) producing a second error signal in response to the first error signal, the second control signal, and the second position signal, the second error signal controlling the second drive means to counteract the rotation of the second member relative to the first member resulting from the coupled rotation of the first member.

Claim 19

The method according to claim 15 further comprising a step of compensating the first error signal as used to produce the second error signal, the compensation accounting for the mechanism coupling of the first member to the second member.