System and Method of Positional Control with Backlash Compensation

A device for preventing backlash in a gear train used for positioning an object, comprising:

• A. a first drive gear having a plurality of gear teeth;
• B. a first motor having a drive shaft coupled to said first drive gear and said first motor shaft;
• C. a second drive gear having a plurality of gear teeth;
• D. a second motor having a drive shaft coupled to said second drive gear;
• E. a driven gear having a plurality of gear teeth, the gear teeth of said first and second drive gears engaging the gear teeth of said driven gear;
• G. said first motor transmitting a first motor torque to said driven gear through said first drive gear, said second motor transmitting a second motor torque to said driven gear through said second drive gear, said first motor torque and said second motor torque causing a rotational movement of said driven gear;
• F. a plurality of sensors for measuring said first and second motor torque, said first and second motor positions, and a position of said driven gear, where low-pass filters may be applied while measuring said motor torque;
• G. a system controller defining a nominal trajectory of said driven gear;
• H. a position feedback controller for said driven gear for calculating a speed set point based at least upon said position of said driven gear and said nominal trajectory;
• I. a motor position and phase controller with a computing means for:
• calculating a difference between said first motor torque and said second motor torque;
• providing upper and lower torque threshold levels;
• providing a step size for a phase set point of said first and second motors;
• comparing said torque difference to said torque upper threshold level and said torque lower threshold level;
• adjusting said phase set point for said first and second motors based at least upon the comparing, where said phase set point is reduced by one said step size if said difference is greater than or equal to said torque upper threshold level, said phase set point is increased by one said step size if said difference is less than or equal to said torque lower threshold level, and said phase set point is kept constant if said difference is greater than said torque lower threshold level and less than said torque upper threshold level;
• calculating the position of a virtual axis by integration over time of said speed set point;
• calculating a position set point of said first motor by adding half of said phase set point to said virtual axis position;
• calculating a position set point of said second motor by subtracting half of said phase set point from said virtual axis position;
• and
• J. a controlling means for controlling the position of said first motor axis and said second motor axis, said controlling means comprising a phase adaption algorithm, said algorithm providing a motor torque set point for said first motor and second motor based on said position set point and said measured motor position, said controlling means positioning said object by transmitting the motor torque set point to said first motor and second motor.