Autonomous correction of alignment error in a master-slave robotic system

1. A method of correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave, the method comprising:

causing a processor associated with the master to receive master actuator orientation signals (R MCURR ) representing the orientation of the master actuator relative to a master reference frame;

causing the processor to generate end effector orientation signals (R EENEW ) representing the end effector orientation relative to a slave reference frame, in response to:

said master actuator orientation signals (R MCURR );

master base orientation signals (R MBASE ) representing previous-saved values of said master actuator orientation signals (R MCURR ); and

slave base orientation signals (R EEBASE ) representing previous-saved values of said end effector orientation signals (R EENEW );

causing the processor to produce control signals based on said end effector orientation signals, for transmission from the master to the slave;

causing the processor to receive an enablement signal for selectively enabling said control signals to be transmitted from the master to the slave whereby the master transmits said control signals to the slave when said enablement signal is active and does not transmit said control signals to the slave when said enablement signal is not active and such that when said enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when said enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector;

when said enablement signal transitions from said not active state to said active state, causing the processor to:

save said values of said master actuator orientation signals (R MCURR ) as said master base orientation signals (R MBASE ) to create said previous-saved values of said master actuator orientation signals (R MCURR ); and

save said values of said end effector orientation signals (R EENEW ) as said slave base orientation signals (R EEBASE ) to create said previous-saved values of said end effector orientation signals (R EENEW );

causing the processor to detect a difference; between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the difference representing a difference in physical alignment between the tool and the master relative to their respective reference frames, the causing comprising computing a difference rotation matrix that carries the end effector orientation signals into the master actuator orientation signals according to the relation:

R EE _ TO _ MASTER =R EENEW −1 R MCURR

Where: R EENEW −1 is an inverse matrix of the end effector orientation signal R EENEW represented by a 3×3 matrix; and

R MCURR is the master actuator orientation signal at a current time step represented by a 3×3 matrix;

in response to detecting said difference, causing the processor to adjust said saved slave base orientation signals (R EEBASE ) to ultimately have the same values as said saved master base orientation (R MBASE ) values so that subsequent generations of said end effector orientation signals (R EENEW ) cause said control signals to cause said tool to satisfy an alignment criterion; and

causing the processor to determine an angle of rotation associated with the difference rotation matrix according to the relation:

φ EE _ TO _ MASTER =a cos(0.5 trace( R EE _ TO _ MASTER )−1).

2. The method of claim 1 further comprising causing said processor to autonomously adjust said saved slave base orientation (R EEBASE ) signals.

3. The method of claim 1 further comprising causing the processor to determine whether the angle of rotation associated with the difference rotation matrix (R EE _ TO _ MASTER ) meets a criterion.

4. The method of claim 3 further comprising causing the processor to determine an angular speed of rotation of the difference rotation matrix (R EE _ TO _ MASTER ) which represents the difference in rotation between the previous-saved master handle orientation signals and the current master handle orientation signals.

5. The method of claim 4 further comprising causing the processor to determine whether the angular speed of rotation of the difference rotation matrix meets a criterion.

6. The method of claim 5 further comprising causing the processor to determine a misalignment axis and an incremental correction angle by which the slave base orientation signals (R EEBASE ) is to be rotated about the misalignment axis.

7. The method of claim 6 further comprising causing the processor to:

generate a correction rotation matrix for adjusting the current slave base orientation (R EEBASE ) signals by the incremental correction angle in a misalignment plane; and

adjust said current slave base orientation (R EEBASE ) signals with the correction matrix.

8. The method of claim 7 further comprising causing the processor to produce new end effector orientation signals (R EENEW ) using said adjusted current slave base orientation (R EEBASE ) signals and causing the processor to generate said control signals using said new end effector signals (R EENEW ).

9. A non-transitory computer readable medium encoded with codes for directing a processor to execute the method of claim 1 .

10. A method of correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave, the method comprising:

causing a processor associated with the master to receive master actuator orientation signals (R MCURR ) representing the orientation of the master actuator relative to a master reference frame;

causing the processor to generate end effector orientation signals (R EENEW ) representing the end effector orientation relative to a slave reference frame, in response to:

said master actuator orientation signals (R MCURR ); and

master-slave misalignment signals (R Δ ), representing a product of previously saved values of said master actuator orientation signals (R MCURR ) and said end effector orientation signals (R EENEW );

causing the processor to produce control signals based on said end effector orientation signals, for transmission from the master to the slave;

causing the processor to receive an enablement signal for selectively enabling said control signals to be transmitted from the master to the slave whereby the master transmits said control signals to the slave when said enablement signal is active and does not transmit said control signals to the slave when said enablement signal is not active and such that when said enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when said enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector;

when said enablement signal transitions from said not active state to said active state, causing the processor to:

compute the master-slave misalignment signals (R Δ ) as a difference between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the misalignment signals representing a difference in physical alignment between the tool and the master relative to their respective reference frames, the difference based on a difference rotation matrix that carries the end effector orientation signals into the master actuator orientation signals according to the relation:

R EE _ TO _ MASTER =R EENEW −1 R MCURR

Where: R EENEW −1 is an inverse matrix of the end effector orientation signal R EENEW represented by a 3×3 matrix; and

R MCURR is the master actuator orientation signal at a current time step represented by a 3×3 matrix;

causing the processor to determine an angle of rotation associated with the difference rotation matrix according to the relation:

φ EE _ TO _ MASTER =a cos(0.5 trace( R EE _ TO _ MASTER )−1);

causing the processor to detect a second difference between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the second difference representing the difference in physical alignment between the tool and the master relative to their respective reference frames; and

in response to detecting said second difference, causing the processor to adjust said master-slave misalignment signals (R Δ ) to reduce the alignment difference to satisfy an alignment criterion so that subsequent generations of said end effector orientation signals (R EENEW ) cause said control signals to cause said tool to be physically aligned with said master within the alignment criterion.

11. The method of claim 10 further comprising causing the processor to:

generate a correction rotation matrix for adjusting the current master-slave misalignment (R Δ ) signals by an incremental correction angle in the misalignment plane; and

adjust said current master-slave misalignment (R Δ ) signals with the correction matrix.

12. The method of claim 11 further comprising causing the processor to produce new end effector orientation signals (R EENEW ) using said adjusted master-slave misalignment (R Δ ) signals and causing the processor to generate said control signals using said new end effector orientation signals (R EENEW ).

13. An apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave, the apparatus comprising:

computing means associated with the master, the computing means for:

receiving master actuator orientation signals (R MCURR ) representing the orientation of the master actuator relative to a master reference frame;

generating end effector orientation signals (R EENEW ) representing the end effector orientation relative to a slave reference frame, in response to:

said master actuator orientation signals (R MCURR );

master base orientation signals (R MBASE ) representing previous-saved values of said master actuator orientation signals (R MCURR ); and

slave base orientation signals (R EEBASE ) representing previous-saved values of said end effector orientation signals (R EENEW );

producing control signals based on said end effector orientation signals, for transmission from the master to the slave;

receiving an enablement signal for selectively enabling said control signals to be transmitted from the master to the slave whereby the master transmits said control signals to the slave when said enablement signal is active and does not transmit said control signals to the slave when said enablement signal is not active and such that when said enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when said enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector;

responsive to a transition of the enablement signal from said not active state to said active state, for:

saving said values of said master actuator orientation signals (R MCURR ) as said master base orientation signals (R MBASE ) to create said previous-saved values of said master actuator orientation signals (R MCURR ); and

saving said values of said end effector orientation signals (R EENEW ) as said slave base orientation signals (R EEBASE ) to create said previous-saved values of said end effector orientation signals (R EENEW );

detecting a difference between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the difference representing a difference in physical alignment between the tool and the master relative to their respective reference frames, wherein detecting the difference comprises computing a difference rotation matrix that carries the end effector orientation signals into the master actuator orientation signals according to the relation:

R EE _ TO _ MASTER =R EENEW −1 R MCURR

Where: R EENEW −1 is an inverse matrix of the end effector orientation signal R EENEW represented by a 3×3 matrix; and

R MCURR is the master actuator orientation signal at a current time step represented by a 3×3 matrix;

adjusting said saved slave base orientation signals (R EEBASE ) to ultimately have the same values as said saved master base orientation (R MBASE ) values so that subsequent generations of said end effector orientation signals (R EENEW ) cause said control signals to cause said tool to satisfy an alignment criterion, in response to detecting said difference; and

determining an angle of rotation associated with the difference rotation matrix according to the relation:

φ EE _ TO _ MASTER =a cos(0.5 trace( R EE _ TO _ MASTER )−1).

14. The apparatus of claim 13 , wherein the computing means is further for autonomously adjusting said saved slave base orientation (R EEBASE ) signals.

15. The apparatus of claim 13 , wherein the computing means is further for determining whether the angle of rotation associated with the difference rotation matrix (R EE _ TO _ MASTER ) meets a criterion.

16. The apparatus of claim 15 , wherein the computing means is further for determining an angular speed of rotation of the difference rotation matrix (R EE _ TO _ MASTER ) which represents the difference in rotation between the previous-saved master handle orientation signals and the current master handle orientation signals.

17. The apparatus of claim 16 , wherein the computing means is further for determining whether the angular speed of rotation of the difference rotation matrix (R EE _ TO _ MASTER ) meets a criterion.

18. The apparatus of claim 17 , wherein the computing means is further for determining a misalignment axis and an incremental correction angle by which the slave base orientation signals (R EEBASE ) is to be rotated about the misalignment axis.

19. The apparatus of claim 18 , wherein the computing means is further for:

generating a correction rotation matrix for adjusting the current slave base orientation (R EEBASE ) signals by the incremental correction angle in a misalignment plane; and

adjusting said current slave base orientation (R EEBASE ) signals with the correction matrix.

20. The apparatus of claim 19 , wherein the computing means is further for producing new end effector orientation signals (R EENEW ) using said adjusted current slave base orientation (R EEBASE ) signals and generating said control signals using said new end effector signals (R EENEW ).

21. An apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave, the apparatus comprising:

computing means associated with the master, the computing means for:

receiving master actuator orientation signals (R MCURR ) representing the orientation of the master actuator relative to a master reference frame;

generating end effector orientation signals (R EENEW ) representing the end effector orientation relative to a slave reference frame, in response to:

said master actuator orientation signals (R MCURR ); and

master-slave misalignment signals (R Δ ), representing a product of previously saved values of said master actuator orientation signals (R MCURR ) and said end effector orientation signals (R EENEW );

producing control signals based on said end effector orientation signals, for transmission from the master to the slave;

receiving an enablement signal for selectively enabling said control signals to be transmitted from the master to the slave whereby the master transmits said control signals to the slave when said enablement signal is active and does not transmit said control signals to the slave when said enablement signal is not active and such that when said enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when said enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector;

responsive to a transition of the enablement signal from said not active state to said active state, for:

computing the master-slave misalignment signals (R Δ ) as a difference between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the misalignment signals representing a difference in physical alignment between the tool and the master relative to their respective reference frames, the difference based on a difference rotation matrix that carries the end effector orientation signals into the master actuator orientation signals according to the relation:

R EE _ TO _ MASTER =R EENEW −1 R MCURR

Where: R EENEW −1 is an inverse matrix of the end effector orientation signal R EENEW represented by a 3×3 matrix; and

R MCURR is the master actuator orientation signal at a current time step represented by a 3×3 matrix;

determining an angle of rotation associated with the difference rotation matrix according to the relation:

φ EE _ TO _ MASTER =a cos(0.5 trace( R EE _ TO _ MASTER )−1);

detecting a second difference between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the second difference representing the difference in physical alignment between the tool and the master relative to their respective reference frames; and

responsive to detecting said second difference, for adjusting said master-slave misalignment signals (R Δ ) to reduce the alignment difference to satisfy an alignment criterion so that subsequent generations of said end effector orientation signals (R EENEW ) cause said control signals to cause said tool to be physically aligned with said master within the alignment criterion.

22. The apparatus of claim 21 , wherein the computing means is further for:

generating a correction rotation matrix for adjusting the current master-slave misalignment (R Δ ) signals by an incremental correction angle in the misalignment plane; and

adjusting said current master-slave misalignment (R Δ ) signals with the correction matrix.

23. The apparatus of claim 22 , wherein the computing means is further for:

producing new end effector orientation signals (R EENEW ) using said adjusted master-slave misalignment (R Δ ) signals; and

generating said control signals using said new end effector orientation signals (R EENEW ).

24. An apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave, the apparatus comprising:

a processor associated with the master operably configured to:

receive master actuator orientation signals (R MCURR ) representing the orientation of the master actuator relative to a master reference frame;

generate end effector orientation signals (R EENEW ) representing the end effector orientation relative to a slave reference frame, in response to:

said master actuator orientation signals (R MCURR );

master base orientation signals (R MBASE ) representing previous-saved values of said master actuator orientation signals (R MCURR ); and

slave base orientation signals (R EEBASE ) representing previous-saved values of said end effector orientation signals (R EENEW );

produce control signals based on said end effector orientation signals, for transmission from the master to the slave;

receive an enablement signal for selectively enabling said control signals to be transmitted from the master to the slave whereby the master transmits said control signals to the slave when said enablement signal is active and does not transmit said control signals to the slave when said enablement signal is not active and such that when said enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when said enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector;

when said enablement signal transitions from said not active state to said active state:

save said values of said master actuator orientation signals (R MCURR ) as said master base orientation signals (R MBASE ) to create said previous-saved values of said master actuator orientation signals (R MCURR ); and

save said values of said end effector orientation signals (R EENEW ) as said slave base orientation signals (R EEBASE ) to create said previous-saved values of said end effector orientation signals (R EENEW );

detect a difference, between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the difference representing a difference in physical alignment between the tool and the master relative to their respective reference frames, the processor configured to detect the difference by computing a difference rotation matrix that carries the end effector orientation signals into the master actuator orientation signals according to the relation:

R EE _ TO _ MASTER =R EENEW −1 R MCURR

Where: R EENEW −1 is an inverse matrix of the end effector orientation signal R EENEW represented by a 3×3 matrix; and

R MCURR is the master actuator orientation signal at a current time step represented by a 3×3 matrix;

determine an angle of rotation associated with the difference rotation matrix according to the relation:

φ EE _ TO _ MASTER =a cos(0.5 trace( R EE _ TO _ MASTER )−1); and

adjust said saved slave base orientation signals (R EEBASE ) to ultimately have the same values as said saved master base orientation (R MBASE ) values so that subsequent generations of said end effector orientation signals (R EENEW ) cause said control signals to cause said tool to satisfy an alignment criterion, in response to detecting said difference.

25. The apparatus of claim 24 wherein said processor is further configured to autonomously adjust said saved slave base orientation (R EEBASE ) signals.

26. The apparatus of claim 24 wherein said processor is further configured to determine whether the angle of rotation associated with the difference rotation matrix (R EE _ TO _ MASTER ) meets a criterion.

27. The apparatus of claim 26 wherein said processor is further configured to determine an angular speed of rotation of the difference rotation matrix (R EE _ TO _ MASTER ) R DIFF which represents the difference in rotation between the previous-saved master handle orientation signals and the current master handle orientation signals.

28. The apparatus of claim 27 wherein said processor is further configured to determine whether the angular speed of rotation of the difference rotation matrix (R EE _ TO _ MASTER ) meets a criterion.

29. The apparatus of claim 28 wherein said processor is further configured to determine a misalignment axis and an incremental correction angle by which the slave base orientation signals (R EEBASE ) is to be rotated about the misalignment axis.

30. The apparatus of claim 29 wherein said processor is further configured to:

generate a correction rotation matrix for adjusting the current slave base orientation (R EEBASE ) signals by the incremental correction angle in a misalignment plane; and

adjust said current slave base orientation (R EEBASE ) signals with the correction matrix.

31. The apparatus of claim 30 wherein said processor is further configured to produce new end effector orientation signals (R EENEW ) using said adjusted current slave base orientation (R EEBASE ) signals and generate said control signals using said new end effector signals (R EENEW ).

32. An apparatus for correcting an alignment error between an end effector of a tool associated with a slave and a master actuator associated with a master in a master-slave robotic system in which an orientation of the end effector is remotely controlled by an orientation of the master actuator by producing and transmitting control signals at the master for controlling the slave, the apparatus comprising:

a processor associated with the master operably configured to:

receive master actuator orientation signals (R MCURR ) representing the orientation of the master actuator relative to a master reference frame;

generate end effector orientation signals (R EENEW ) representing the end effector orientation relative to a slave reference frame, in response to:

said master actuator orientation signals (R MCURR ); and

master-slave misalignment signals (R Δ ), representing a product of previously saved values of said master actuator orientation signals (R MCURR ) and said end effector orientation signals (R EENEW );

produce control signals based on said end effector orientation signals, for transmission from the master to the slave;

receive an enablement signal for selectively enabling said control signals to be transmitted from the master to the slave whereby the master transmits said control signals to the slave when said enablement signal is active and does not transmit said control signals to the slave when said enablement signal is not active and such that when said enablement signal is active, changes in the orientation of the master actuator cause corresponding changes in the orientation of the end effector and such that when said enablement signal is not active, changes in the orientation of the master actuator do not cause corresponding changes in the orientation of the end effector;

in response to a transition of the enablement signal from said not active state to said active state:

compute the master-slave misalignment signals (R Δ ) as a difference between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the misalignment signals representing a difference in physical alignment between the tool and the master relative to their respective reference frames, the difference based on a difference rotation matrix that carries the end effector orientation signals into the master actuator orientation signals according to the relation:

R EE _ TO _ MASTER =R EENEW −1 R MCURR

Where: R EENEW −1 is an inverse matrix of the end effector orientation signal R EENEW represented by a 3×3 matrix; and

R MCURR is the master actuator orientation signal at a current time step represented by a 3×3 matrix;

to determine an angle of rotation associated with the difference rotation matrix according to the relation:

φ EE _ TO _ MASTER =a cos(0.5 trace( R EE _ TO _ MASTER )−1);

detect a second difference between the master actuator orientation signals (R MCURR ) and the end effector orientation signals (R EENEW ), the second difference representing the difference in physical alignment between the tool and the master relative to their respective reference frames; and

responsive to detecting said second difference, adjusting said master-slave misalignment signals (R Δ ) to reduce the alignment difference to satisfy an alignment criterion so that subsequent generations of said end effector orientation signals (R EENEW ) cause said control signals to cause said tool to be physically aligned with said master within the alignment criterion.

33. The apparatus of claim 32 wherein said processor is further configured to:

generate a correction rotation matrix for adjusting the current master-slave misalignment (R Δ ) signals by an incremental correction angle in the misalignment plane; and

adjust said current master-slave misalignment (R Δ ) signals with the correction matrix.

34. The apparatus of claim 33 wherein said processor is further configured to:

produce new end effector orientation signals (R EENEW ) using said adjusted master-slave misalignment (R Δ ) signals; and

generate said control signals using said new end effector orientation signals (R EENEW ).