APPARATUS. SYSTEM AND COMPUTER PROGRAM FOR CONTROLLING A

TOOL

Priority Claim

This application claims priority of United States Provisional Patent Application No. 60/826,619, filed on September 22, 2006.

Field of the Invention

The present invention generally relates to devices and systems used to reproduce or replicate human handwriting. The present invention also relates generally to devices and systems used to control tools on a remote basis.

Background of the Invention

A variety of pen writing mechanisms have been invented over the years. Mechanically, the designs typically incorporated articulated arms or XY plotter type mechanisms. One of the earlier uses of a pantograph mechanism in a writing reproduction machine is described in U.S. Patent No. 2,332,511 to Glassman et al.

U.S. Patent No. 3,733,612 to Huston et al. teaches a signature reproduction machine that converts the position of a pen with respect to a piece of paper to signals recorded on a magnetic tape. The data stored on the magnetic tape can then be played back to reproduce the writing.

Further, U.S. Patent No. 6,425,185 to Regnault et al. discloses a machine to reproduce writing, the machine including a pantograph. The machine includes a pen lift mechanism.

MITSUBISHI™ markets its MELFA™ RP-I AH/3 AH/5 AH industrial robot having an arm moveable along all points of a surface. The industrial robot includes a five-joint closed link mechanism that includes a series of robotic actuators at each joint.

However, these prior art references, and the prior art generally, do not disclose control of such devices that can accommodate various speeds of movement of a tool along a target area of an object, and pressures exerted by such tool on such target area. Further, these writing devices are generally intended to write on individual sheets of paper and therefore do not accommodate media of varying thicknesses. Moreover, these prior art devices are generally not operable to accept streamed data in real time, and move a tool in real time on the basis of the streamed data. Most particularly, the prior art devices are generally not operable to enable movement of a tool along a predetermined travel path, with the accuracy and speed required for numerous applications, including for example real time reproduction of human writing.

On the basis of the foregoing, what is needed is an apparatus, system and computer program for overcoming the disadvantages and shortcomings of the prior art. There is also a need for such an apparatus and system that is made of relatively inexpensive parts and that is easy to produce and maintain.

Summary of the Invention

The present invention provides an apparatus, system and computer program for controlling movement of a tool along a defined path.

In a first aspect of the present invention, a system for controlling movement of a tool along a defined path is provided, the system including a moving assembly that is operable to hold the tool, and to move the tool within a target area of an object held adjacent to the moving assembly, characterized in that the moving assembly comprises: at least two arm members, each arm member having a first end and a second end opposite from the first end, each arm member being spaced apart from one another and linked to a base by a first pivot, and each arm member including a second pivot disposed between the first end and the second end, wherein the first pivot and the second pivot enable the arm members to move in X and Y directions; at least one first actuator linked to the arm members at the first end, the first actuator being operable to move the arm members to a plurality of locations within an associated range of motion, and a first sensor linked to the first actuator for determining the location of the arm member; a holding assembly linked to the arm members at their second end, the holding assembly being operable to

hold the tool; at least one second actuator linked to the holding assembly and being operable to move the tool to a plurality of locations in a vertical axis between the holding assembly and the target area, and a second sensor linked to the second actuator for determining the location of the tool relative to the target area; and at least one controller, the controller being linked to the first actuator and the second actuator, the controller being operable to: selectively actuate the at least one first actuator to move the arm members so as to move the tool along a selected path in the target area, including at a selected velocity; and selectively actuate the at least one second actuator to lower the tool in the vertical axis to bring the tool in contact with the target area and optionally to exert selected pressure on the target area at selected points along the selected path.

In another aspect of the present invention, the controller is operable to: obtain sensor data from each of the first sensor and the second sensor; extrapolate from such sensor data the location of the tool relative to the target area; and based on travel path data associated with a travel path defined for the tool along the target area, the travel path data including coordinate data for contact between the tool and the target area, velocity of travel of the tool at selected points of the travel path, and pressure exerted on the target area at selected points of the travel path, generate control signals in real time for the moving assembly bringing the tool in contact with the target area in compliance with the travel path data.

In yet another aspect of the present invention, the controller is operable to define the travel path data such that XY positions for the tool, velocity data associated with XY positions, and pressure data for the tool to exert on the target surface are converted into temporally equally spaced real time travel path instructions for the applicable time intervals, and to convert the travel path instructions into actuator space commands.

In a still other aspect of the present invention, a controller for controlling the movement of a moving assembly is provided, in accordance with the present invention. In a still other aspect of the present invention a control computer program in accordance with the present invention is provided, as well as a method for controlling a moving assembly for moving a tool along a defined path.

In one aspect, the present invention is an apparatus having a pantograph mechanism. The pantograph mechanism is operable to control a tool or instrument, e.g., a writing implement, such that the tool is applied to an object, e.g., a piece of paper or a book. The pantograph

mechanism is movable in relation to a ground member to provide a range of motion to accommodate objects of different sizes.

In one particular embodiment of the present invention, the pantograph mechanism, which is articulated and has two degrees of freedom, comprises: a moving frame; a stabilizing frame; a pair of inner links and a pair of outer links; and a tool link. The inner links are driven by two electric actuators whose positions can be measured using sensors. The tool link is equipped with an actuated lift mechanism whose position can be measured using a sensor. The tool link is connected to the tool. The lift mechanism is preferably spring-loaded such that a loss of power to the lift motor results in the tool being removed from the object. Preferably, a spring extending from an anchor point on the moving frame to the tool link is used to pre-load the actuators to reduce any backlash effects. Essentially, one of the aspects of the invention is to pre-load the drive mechanism of the apparatus of the present invention. In a particular example of this preloading, this is accomplished by a spring placed (for example as shown in FIG. 5) so as to preloads the gearboxes on the actuators associated with the arms as well as the joints disposed on such arms. When writing is considered, the tool (in this case a pen) may relatively abrupt velocity changes in reproducing human handwriting, such velocity changes referring to changes in the speed and direction of the movement of the pen. Reproducing these changes may result in "wobbling" of the output script, particularly if the input signals of the human handwriting are obtained in real time. Pre-loading of the drive mechanism creates a "stiffness" in the mechanism overall that reduces the "wobbling" and therefore contributes to an improvement in the accuracy of the movement of the moving assembly.

In addition, a torsional spring can be included on the tool link and can be pre-loaded to account for tool weight. The moving frame is connected to the ground member by a vertical drive mechanism, the vertical drive mechanism having a motor and a sensor. The vertical drive mechanism allows the pantograph mechanism to move vertically with respect to an object. A sensor senses the contact between the stabilizing frame and an object.

In another aspect, the present invention is a system for controlling a tool, wherein the system is operable to receive a data feed and control the tool on the basis of the data feed. The data feed can include live data streamed to the system on a substantially real time basis.

According to a particular embodiment of this aspect of the present invention, the system includes a computer having a feedback control system operable to receive real time data via a network connection and can write the received data in real time. The control system includes a computer program having a trajectory generation utility that ensures that the tool moves at a maximum velocity achievable by the actuators while ensuring that all points commanded to the system are attained.

In yet another aspect, the computer program includes a control loop for maintaining contact between the tool and the object, for example a pen and paper, respectively, with programmable compliance and force, thus ensuring, for example, that the pen follows any uneven surface variations of the paper without damaging it.

Brief Description of the Drawings

A detailed description of the preferred embodiments is provided herein below by way of example only and with reference to the following drawings, in which:

FIG. 1 illustrates an apparatus for controlling movement of a tool in accordance with the present invention;

FIG. 2 is a schematic of a contact and pressure algorithm;

FIG. 3 is a schematic of a trajectory generation algorithm;

FIG. 4 illustrates and alternate embodiment of an apparatus for controlling movement of a tool in accordance with the present invention; and

FIG. 5 illustrates another alternate embodiment of an apparatus for controlling movement of a tool in accordance with the present invention.

In the drawings, one embodiment of the invention is illustrated by way of example. It is to be expressly understood that the description and drawings are only for the purpose of illustration and as an aid to understanding, and are not intended as a definition of the limits of the invention.

Detailed Description of the Invention

With reference to FIG. 1, an embodiment of an apparatus in accordance with the present invention includes a ground member (1) coupled to a moving frame (2) which is driven by an electrical actuator and is equipped with a position sensor. It should be understood that "ground" refers to a part of a mechanical device that remains stationary with respect to moving parts, i.e. the moving parts move relative to the ground.

In a representative embodiment, a moving frame (2) is coupled to an inner link A (3) as well as an inner link B (4). Both inner link A (3) and inner link B (4) are driven by an electric actuator equipped with a position sensor. The inner link A (3) is coupled to an outer link A (5), and the inner link B (4) is coupled to an outer link B (6). The outer link A and the outer link B are coupled together. A tool link (7) is coupled to outer link B and is driven by an electric actuator equipped with a position sensor. The tool link (7) holds a tool or instrument, for example, a pen.

A linear spring (8) is optionally connected from the coupling between outer link A and outer link B to an anchor point on the moving frame, and may be pre-tensioned to reduce any backlash. Also, a torsional spring (9) may be coupled between outer link B and the tool link and pre-loaded to counteract the mass of the tool and keep it away from the object in case of power loss.

A stabilizing frame (10), in accordance with one embodiment, may be loosely coupled to the moving frame and is equipped with sensors to detect contact with an object, for example, a book.

It should be understood that the apparatus is controlled by providing control voltages to the actuators and by measuring the sensor signals in order to apply a tool to an object in a controlled manner.

The apparatus described above is preferably linked to a computer, the computer including control software for controlling the apparatus, i.e. with sufficient data acquisition capabilities to measure the sensor states from the apparatus described above and to apply control signals to the actuators. The computer may include a network connection, with the network connection operable to asynchronously receive tool position and pressure data from a remote connection. The data could be live or recorded, and provided through an input device. For example, a suitable input device could include a standard digitizing tablet having a touch screen. It should be understood

that the functionality of the controller of the present invention can also be implemented to hardware that is either linked to the apparatus or forms part of the apparatus.

In particular, the present invention provides a contact and pressure utility (not shown) (implemented for example by means of a suitable algorithm) that is operable to control the compliance of tool contact with an object, as well as the pressure (force) applied by the tool upon the object. A block diagram of a representative contact and pressure algorithm is shown in FIG. 2, and illustrates the operation of the controller of the present invention.

The tool position is measured (11) and applied to a position control system (12) (in one representative embodiment of the present invention). In a particular implementation of the present invention, a gain KComp (13) controls the mechanical compliance of the contact. The tool lift position control system may be a standard Proportional-Derivative (PD) controller in which the P term is equivalent to the KComp term described above and naturally thereby determines the stiffness of the joint. While the tool is in contact with an object, the error between the command and the actual tool position (14) represents a measure of the current in the actuator and thus the pressure applied by the tool lift mechanism. Thus, tool pressure (in accordance with this particular implementation) is not directly measured but rather inferred from the position error and the implicit stiffness programmed via KComp.

Alternatively (24), if more accuracy is required, the current in the actuator can be directly measured from the amplifier (15) and used to calculate the applied pressure from motor characteristics. The desired pressure may be commanded to another feedback loop consisting of a pure integral control with anti-windup. As shown in the FIG. 2, this loop generates a virtual and mechanically unattainable position command to the pen lift PD controller via a limited integrator (16) with gain (17) such that the applied pressure tracks the desired pressure command in a stable manner while maintaining contact compliance.

In another aspect of the present invention, the controller of the present invention is operable to generate travel path data. In one implementation of the controller, this is achieved using a trajectory generation algorithm that operates as shown in FIG. 3. The trajectory generation algorithm processes incoming data and generates real time rate and acceleration limited commands for the actuators (in the form of control voltages or signals). In particular, the trajectory generation algorithm receives commanded XY positions for the tool as well as

pressure P (18) at an undetermined frequency and converts them to temporally equally spaced real time XY positions with controlled and limited and programmable Cartesian velocity (19). The processed data are output (20) to an inverse kinematics algorithm (21) that converts the Cartesian coordinates to actuator space commands which are in turn are applied to the actuator control algorithm.

Alternatively, the incoming data (18) can be time stamped (T) and the algorithm will command the robot to track the commanded positions and associated pressure with a pre set rate limit.

When asynchronous XY and P data arrives to the algorithm, the distance between the robot's present coordinates and its desired coordinates is computed. The algorithm then determines the subsequent step sizes and duration that must be commanded to the robot to ensure that the maximum accelerations and velocities are not exceeded. These steps are subsequently commanded to the robot at the real time sampling frequency of the controller until the final destination command is attained. Once this condition is achieved, the next point is obtained from the buffered data. If there is no next point, the robot holds its position until another point is reached.

If timestamps T accompany the data, the aforementioned step sizes are computed to either match the computed velocity from the timestamps T or to match the maximum speed attainable by the robot. In this approach, the robot will mimic the original motion more realistically; slowing down when the user slows down and speeding up to maximum speed when the user moves quickly.

Although the embodiment described herein and depicted in the drawings is particularly directed at controlling a writing implement to be applied to a writing surface, it should be expressly understood that the present invention can be readily extended to any application where it is desired to apply a tool to an object in a controlled fashion, especially where the tool is controlled on a remote basis. For example, the present invention can be implemented in applications for soldering using a soldering tool, manipulation of parts and tools for microassembly, surgery, microsurgery, pick-and-place manufacturing, painting, etc. The robot can also be used in cooperative tasks where it forms part of a larger system comprising two or more such (or different) robots cooperating together to complete a task.

The system of the present invention may be provided using numerous parts or sub-assemblies without departing from the present invention. FIG. 1 illustrates one particular structure in accordance with the present invention. For example, this embodiment of the invention is implemented as set out in the paragraph below.

All links are constructed in T6 aluminum. Bearings with quarter inch shafts constitute the rotational joints between the first and second links and between the two second links. MICROMO™ 35mm DC brush motors with 66:1 gearboxes are the actuators for the two first links. The tool lift mechanism is implemented using a 23mm MICROMO™ DC brush motor with a 14:1 gearbox followed by a 90 degree 3: 1 gearbox. Thus the motor frame longitudinal axis is aligned with the longitudinal axis of the second link to which it is mounted. The tool holder is attached to the output of the 90 degree gear reduction. A commercially available pen is coupled to the tool holder. The vertical travel is attained using a 35mm motor with a 5: 1 gear ratio driving a NOOK™ lead screw with .125" pitch into a NOOK™ ball nut. All motorized axes are equipped with US DIGITAL™ 1024 count incremental optical encoders. The stabilizing frame is equipped with spring-loaded microswitches which trigger when the externally exerted force pressure exceeds 0.5 lbs. A spring is extended between the joint at the tip and the frame onto which the two link motors are mounted and eliminates backlash in task space. Another spring is extended between the tool holder and the frame onto which the tool lift motor is mounted ensuring that the tool is always lifted if the motor is inactive. The motors are driven with QUANSER™ linear current amplifiers which along with all sensor signals, are interfaced to a standard desktop PC via a QUANSER™ Q4 hardware in the loop board. Control systems are designed using SIMULINK™ and run in realtime at 2kHz using the WINCON™ realtime package. All control algorithms are implemented via code generation from SIMULINK™ and application specific C based S functions. Communications with a TCP/IP network is attained via the QUANSER™ TCP/IP blocks available from the WINCON™ library.

A COMPAQ™ tablet is used as the digitizing device for handwriting. Natural handwriting is digitized using a custom application running on the tablet and streamed at the fastest possible rate to the realtime control system running the device.

Other embodiments are possible. For example, FIG. 4 illustrates an embodiment of the invention in which the components of the moving assembly of the present invention are "inverted" relative

to the embodiment illustrated in FIG. 1. Also, the parts and/or or sub-assemblies may be simplified, or for certain applications where greater speed or accuracy is desired, more complicated or costly parts, sub-assemblies, or structures may be used. FIG. 4, for example, illustrates a somewhat simpler structure than what is shown in FIG. 1, for example.

In another aspect of the present invention, a method for controlling a moving assembly is provided. In one aspect of this method, an initialization sequence in the real time code directs the vertical drive mechanism to move towards a sheet of paper (or some other target object) placed underneath it and stops moving when the sensors embedded in the stabilizing frame detect its presence. The real time control system subsequently waits for data to arrive from the tablet. When the data starts to arrive, the algorithm as described in FIG. 3 starts generating intermediate command points at the rate of 2kHz and submits them to the joint level controllers. During this time, the tool lift mechanism is also actuated via the algorithm as described in FIG. 2 and applies the appropriate pressure onto the paper. Once all these loops are active, the robot starts reproducing the handwriting onto the sheet of paper as it arrives from the tablet asynchronously.

Although the actuators used here are DC brush gearmotors, these may be replaced by DC brushless, stepper, direct drive, linear or AC motors. One could also use hydraulic or pneumatic actuators. The sensors, apart from the chosen optical incremental encoders, might be potentiometers, resolvers, magnetic encoders, absolute encoders or any other appropriate position or rate transducer. The amplifiers should of course be selected for the motors accordingly. The gearboxes need not be of the planetary type and can readily be replaced by other gearboxes or harmonic drives. The realtime control system should be deterministic but need not run on a PC. It could be embedded into any realtime processor, DSP, microcontroller or FPGA. The network connection is not limited to TCP-IP but could be any synchronous or asynchronous network connection including but not limited to serial, USB and firewire. The input device in this example is a tablet but could be any motion digitizing device such as a microscribe, a mouse or a joystick.

It will be appreciated by those skilled in the art that other variations of the preferred embodiment may also be practised without departing from the scope of the invention.