Method of controlling a robot through a singularity

• US 6,845,295 B2
• Filed: 03/06/2003
• Issued: 01/18/2005
• Est. Priority Date: 03/07/2002
• Status: Active Grant

• Current Assignee: Fanuc Robotics America Incorporated (Fanuc Corporation)
• Original Assignee: Fanuc Robotics America Incorporated (Fanuc Corporation)
• Inventors: Xiao, Di, McGee, H. Dean, Cheng, Sai-Kai, Tsai, Chi-Keng, Jean, Min-Ren
• Primary Examiner(s): Black, Thomas G.
• Assistant Examiner(s): Marc, McDieunel, Marc, McDieunel
• Application Number:
  US10/382,387
• Publication Number:
  US 20030171847A1
• Time in Patent Office: 684 Days
• Field of Search: 701/1, 701/11, 701/21, 701/23, 901/2, 901/8, 901/9, 901/16, 901/23, 901/32, 414/729, 700245-264, 318/568.1, 318/568.11, 318/568.12, 318/568.13, 318/568.14, 318/568.15, 318/568.16, 219/121.61, 219/121.62, 219/121.63, 219/121.64, 219/125.11, 219/125.12
• US Class Current: 700/245
• CPC Class Codes: G05B 19/4103 Digital interpolation
• CPC Class Codes: G05B 2219/34169 Coarse interpolator, path calculator delivers position, speed, acceleration blocks
• CPC Class Codes: G05B 2219/40333 Singularity, at least one movement not possible, kinematic redundancy

Abstract

A method of controlling a robot (32) includes the steps of selecting an initial configuration from at least one of a first, second, and third sets to position a TCP at a starting point (44) along a path (33) and selecting a final configuration different than the initial configuration to position the TCP at an ending point (46). Next, the TCP moves from the starting point (44) while maintaining the initial configuration, approaches the singularity between a first point (48) and a second point (50), and selects one of the axes in response to reaching the first point (48). The angle for the selected axis is interpolated from the first point (48) to the second point (50). After the interpolation, the angles about the remaining axes are determined and positions the arms in the final configuration when the TCP reaches the second point (50) and moves to the ending point (46) while maintaining the final configuration.

Claims

1. A method of controlling a robot (32) having a plurality of arms rotatable about a plurality of axes defining a plurality of angles therebetween and supporting a tool (49) having a tool point (TCP) for relative movement of the tool (49) along a path (33) having a starting point (44) and an ending point (46) defined in Cartesian coordinates, wherein the robot (32) has at least one of a first configuration set having an up configuration and a down configuration, a second configuration set having a flip configuration and a no-flip configuration, and a third configuration set having a front configuration and a back configuration, and wherein the robot (32) approaches a singularity associated with the configuration set along the path (33) while moving about the plurality of axes, said method comprising the steps of:

selecting the up configuration, the no-flip configuration, and the front configuration from the first, second, and third sets to position the TCP at the starting point (44) with the angles of the arms in an initial configuration;

selecting the up configuration, the flip configuration, and the front configuration from the sets to position the TCP at the ending point (46) with the angles of the arms in a final configuration;

rotating the arms about the axes by changing the angles therebetween to move the TCP from the starting point (44) along the path (33) while maintaining the initial configuration;

approaching the singularity which occurs between a first point (48) and a second point (50) along the path (33);

selecting one of the axes in the initial configuration in response to reaching the first point (48);

interpolating the angle for the selected axis from the first point (48) to the second point (50) in a predetermined number of steps (52) between the first point (48) and the second point (50);

determining the angles about the remaining axes in relationship to the aforesaid interpolation at each of the steps (52) to position the arms in the final configuration when the TCP reaches the second point (50) in response to determining the angles; and

rotating the arms about the axes by changing the angles therebetween to move the TCP along the path (33) to the ending point (46) while maintaining the final configuration.

2. A method of controlling a robot (32) having a plurality of arms rotatable about a plurality of axes defining a plurality of angles therebetween and supporting a tool (49) having a tool point (TCP) for relative movement of the tool (49) along a path (33) having a starting point (44) and an ending point (46) defined in Cartesian coordinates, wherein the robot (32) has at least one of a first configuration set having an up configuration and a down configuration, a second configuration set having a flip configuration and a no-flip configuration, and a third configuration set having a front configuration and a back configuration, and wherein the robot (32) approaches a singularity associated with the configuration set along the path (33) while moving about the plurality of axes, said method comprising the steps of:

selecting the up configuration, the no-flip configuration, and the front configuration from the first, second, and third sets to position the TCP at the starting point (44) with the angles of the arms in an initial configuration;

selecting the up configuration, the no-flip configuration, and the back configuration from the sets to position the TCP at the ending point (46) with the angles of the arms in a final configuration;

rotating the arms about the axes by changing the angles therebetween to move the TCP from the starting point (44) along the path (33) while maintaining the initial configuration;

approaching the singularity which occurs between a first point (48) and a second point (50) along the path (33);

selecting one of the axes in the initial configuration in response to reaching the first point (48);

interpolating the angle for the selected axis from the first point (48) to the second point (50) in a predetermined number of steps (52) between the first point (48) and the second point (50);

determining the angles about the remaining axes in relationship to the aforesaid interpolation at each of the steps (52) to position the arms in the final configuration when the TCP reaches the second point (50) in response to determining the angles; and

rotating the arms about the axes by changing the angles therebetween to move the TCP along the path (33) to the ending point (46) while maintaining the final configuration.

3. A method of controlling a robot (32) having a plurality of arms rotatable about a plurality of axes defining a plurality of angles therebetween and supporting a tool (49) having a tool point (TCP) for relative movement of the tool (49) along a path (33) having a starting point (44) and an ending point (46) defined in Cartesian coordinates, wherein the robot (32) has at least one of a first configuration set having an up configuration and a down configuration, a second configuration set having a flip configuration and a no-flip configuration, and a third configuration set having a front configuration and a back configuration, and wherein the robot (32) approaches a singularity associated with the configuration set along the path (33) while moving about the plurality of axes, said method comprising the steps of:

selecting the up configuration, the no-flip configuration, and the front configuration from the first, second, and third sets to position the TCP at the starting point (44) with the angles of the arms in an initial configuration;

selecting the down configuration, the no-flip configuration, and the front configuration from the sets to position the TCP at the ending point (46) with the angles of the arms in a final configuration;

rotating the arms about the axes by changing the angles therebetween to move the TCP from the starting point (44) along the path (33) while maintaining the initial configuration;

approaching the singularity which occurs between a first point (48) and a second point (50) along the path (33);

selecting one of the axes in the initial configuration in response to reaching the first point (48);

interpolating the angle for the selected axis from the first point (48) to the second point (50) in a predetermined number of steps (52) between the first point (48) and the second point (50);

determining the angles about the remaining axes in relationship to the aforesaid interpolation at each of the steps (52) to position the arms in the final configuration when the TCP reaches the second point (50) in response to determining the angles; and

rotating the arms about the axes by changing the angles therebetween to move the TCP along the path (33) to the ending point (46) while maintaining the final configuration.

4. A method of controlling a robot (32) having a plurality of arms rotatable about a plurality of axes defining a plurality of angles therebetween and supporting a tool (49) having a tool point (TCP) for relative movement of the tool (49) along a path (33) having a starting point (44) and an ending point (46) defined in Cartesian coordinates, wherein the robot (32) has at least one of a first configuration set having an up configuration and a down configuration, a second configuration set having a flip configuration and a no-flip configuration, and a third configuration set having a front configuration and a back configuration, and wherein the robot (32) approaches a singularity associated with the configuration set along the path (33) while moving about the plurality of axes, said method comprising the steps of:

selecting the configuration of at least one of the first, second, and third sets to position the TCP at the starting point (44) with the angles of the arms in an initial configuration;

selecting the other configuration of at least one of the sets to position the TCP at the ending point (46) with the angles of the arms in a final configuration;

rotating the arms about the axes by changing the angles therebetween to move the TCP from the starting point (44) along the path (33) while maintaining the initial configuration;

approaching the singularity which occurs between a first point (48) and a second point (50) along the path (33);

selecting one of the axes in the initial configuration in response to reaching the first point (48);

interpolating the angle for the selected axis from the first point (48) to the second point (50) in a predetermined number of steps (52) between the first point (48) and the second point (50);

determining the angles about the remaining axes in relationship to the aforesaid interpolation at each of the steps (52) to position the arms in the final configuration when the TCP reaches the second point (50) in response to determining the angles; and

rotating the arms about the axes by changing the angles therebetween to move the TCP along the path (33) to the ending point (46) while maintaining the final configuration.

5. A method as set forth in claim 4 wherein each of the points along the path (33) are further defined by a desired location component and a desired orientation component and wherein the step of determining the angles about the remaining axes further includes determining an actual location component and an actual orientation component.

6. A method as set forth in claim 5 further including the step of minimizing a difference between at least one of the actual location component and the desired location component and the actual orientation component and the desired orientation component.

7. A method as set forth in claim 6 wherein the step of minimizing the difference between the location and orientation components is further defined as equating the actual location component to the desired location component and minimizing the difference between the actual orientation component and the desired orientation component.

8. A method as set forth in claim 7 wherein the step of minimizing the difference between the orientation components is further defined as selecting an orientation equation that is a function of at least two of the angles and manipulating the two angles to solve the orientation equation for a minimum value.

9. A method as set forth in claim 8 wherein the step of equating the actual location component to the desired location component is further defined as manipulating the angles about the remaining axes to equate the actual location component to the desired location component.

10. A method as set forth in claim 9 wherein the plurality of axes is further defined as a first axis A1, a second axis A2, a third axis A3, a fourth axis A4, a fifth axis A5, and a sixth axis A6 respectively defining a first angle, a second angle, a third angle, a fourth angle, a fifth angle, and a sixth angle, and wherein the step of selecting one of the axes is further defined as selecting the fourth axis A4, and wherein the step of interpolating the angle for the selected axis is further defined as interpolating the fourth angle, and wherein the step of manipulating the two angles is further defined as manipulating the fifth and the sixth angles to solve the orientation equation for the minimum value.

11. A method as set forth in claim 10 wherein the step of manipulating the angles about the remaining axes is further defined as manipulating the first angle, the second angle, and the third angle to equate the actual location component to the desired location component.

12. A method as set forth in claim 9 further including the step of iteratively solving the orientation equation to minimize the difference between the actual orientation component and the desired orientation component.

13. A method as set forth in claim 10 wherein the step of iteratively solving the orientation equation is further defined as determining the derivative of the orientation equation with respect to each of the at least two angles, applying a Newton-Raphson method to the determined derivative, and selecting the angles that result in the derivative equaling zero.

14. A method as set forth in claim 6 wherein the step of minimizing the difference between the location and orientation components is further defined as equating the actual orientation component to the desired orientation component and minimizing the difference between the actual location component and the desired location component.

15. A method as set forth in claim 14 wherein the step of minimizing the difference between the location components is further defined as selecting a location equation that is a function of at least two of the angles and manipulating the at least two angles to solve the location equation for a minimum value.

16. A method as set forth in claim 15 wherein the step of equating the actual orientation component to the desired orientation component is further defined as manipulating the angles about the remaining axes to equate the actual orientation component to the desired orientation component.

17. A method as set forth in claim 16 wherein the plurality of axes is further defined as a first axis A1, a second axis A2, a third axis A3, a fourth axis A4, a fifth axis A5, and a sixth axis A6 respectively defining a first angle, a second angle, a third angle, a fourth angle, a fifth angle, and a sixth angle, and wherein the step of selecting one of the axes is further defined as selecting the first axis A1, and wherein the step of interpolating the angle for the selected axis is further defined as interpolating the first angle, and wherein the step of manipulating the two angles is further defined as manipulating the second and the third angles to solve the location equation for the minimum value.

18. A method as set forth in claim 17 wherein the step of manipulating the angles about the remaining axes is further defined as manipulating the fourth angle, the fifth angle, and the sixth angle to equate the actual orientation component to the desired orientation component.

19. A method as set forth in claim 16 further including the step of solving the location equation to minimize the difference between the actual location component and the desired location component.

20. A method as set forth in claim 16 wherein the plurality of axes is further defined as a first axis A1, a second axis A2, a third axis A3, a fourth axis A4, a fifth axis A5, and a sixth axis A6 respectively defining a first angle, a second angle, a third angle, a fourth angle, a fifth angle, and a sixth angle, and wherein the step of selecting one of the axes is further defined as selecting the second axis A2, and wherein the step of interpolating the angle for the selected axis is further defined as interpolating the second angle, and wherein the step of manipulating the two angles is further defined as manipulating the first and the third angles to solve the location equation for the minimum value.

21. A method as set forth in claim 4 wherein the step of selecting the configuration of at least one of the sets is further defined as selecting the configuration from each of the sets to define the initial configuration and wherein the step of selecting the other configuration of the at least one of the sets is further defined as selecting at least one other configuration of the sets to define the final configuration.

22. A method as set forth in claim 21 wherein the step of selecting the configuration from each of the sets is further defined as selecting the up configuration, the no-flip configuration, and the front configuration to define the initial configuration and wherein selecting at least one other configuration of the sets is further defined as selecting the up configuration, the flip configuration, and the front configuration to define the final configuration.

23. A method as set forth in claim 21 wherein the step of selecting the configuration from each of the sets is further defined as selecting the up configuration, the no-flip configuration, and the front configuration to define the initial configuration and wherein the step of selecting at least one other configuration of the sets is further defined as selecting the up configuration, the no-flip configuration, and the back configuration to define the final configuration.

24. A method as set forth in claim 21 wherein the step of selecting the configuration from each of the sets is further defined as selecting the up configuration, the no-flip configuration, and the front configuration to define the initial configuration and wherein the step of selecting at least one other configuration of the sets is further defined as selecting the down configuration, the no-flip configuration, and the front configuration to define the final configuration.

25. A method of controlling a robot (32) having a plurality of arms rotatable about a plurality of axes defining a plurality of angles therebetween and supporting a tool (49) having a tool point (TCP) for relative movement of the tool (49) along a path (33) having a starting point (44) and an ending point (46) defined in Cartesian coordinates, wherein the robot (32) has at least one of a first configuration set having an up configuration and a down configuration, a second configuration set having a flip configuration and a no-flip configuration, and a third configuration set having a front configuration and a back configuration, and wherein the robot (32) approaches a singularity associated with the configuration set along the path (33) while moving about the plurality of axes, said method comprising the steps of:

selecting the configuration of at least one of the first, second, and third sets to position the TCP at the starting point (44) with the angles of the arms in an initial configuration;

selecting the other configuration of at least one of the sets to position the TCP at the ending point (46) with the angles of the arms in a final configuration;

rotating the arms about the axes by changing the angles therebetween to move the TCP from the starting point (44) along the path (33) while maintaining the initial configuration;

approaching the singularity which occurs between a first point (48) and a second point (50) along the path (33);

determining the angles about the axes to move the TCP between the first point (48) and the second point (50) and through the singularity; and

rotating the arms about the axes by changing the angles therebetween to move the TCP along the path (33) while maintaining the final configuration to the ending point (46).

26. A method as set forth in claim 25 wherein each of the points along the path (33) are further defined by a desired location component and a desired orientation component and wherein the step of determining the angles about the axes further includes determining an actual location component and an actual orientation component.

27. A method as set forth in claim 26 further including the step of minimizing a difference between at least one of the actual location component and the desired location component and the actual orientation component and the desired orientation component.

28. A method as set forth in claim 27 wherein the step of minimizing the difference between the location and orientation components is further defined as equating the actual location component to the desired location component and minimizing the difference between the actual orientation component and the desired orientation component.

29. A method as set forth in claim 26 wherein the step of minimizing the difference between the orientation components is further defined as selecting an orientation equation that is a function of at least two of the angles and manipulating the at least two angles to solve the orientation equation for a minimum value.

30. A method as set forth in claim 29 wherein the step of equating the actual location component to the desired location component is further defined as manipulating the angles about the axes to equate the actual location component to the desired location component.

31. A method as set forth in claim 30 further including the step of iteratively solving the orientation equation to minimize the difference between the actual orientation component and the desired orientation component.

32. A method as set forth in claim 31 wherein the step of iteratively solving the orientation equation is further defined as determining the derivative of the orientation equation with respect to each of the at least two angles, applying a Newton-Raphson method to the determined derivative, and selecting the angles that result in the derivative equaling zero.

33. A method as set forth in claim 29 wherein the step of minimizing the difference between the location and orientation components is further defined as equating the actual orientation component to the desired orientation component and minimizing the difference between the actual location component and the desired location component.

34. A method as set forth in claim 33 wherein the step of minimizing the difference between the location components is further defined as selecting a location equation that is a function of at least two of the angles and manipulating the at least two angles to solve the location equation for a minimum value.

35. A method as set forth in claim 34 wherein the step of equating the actual orientation component to the desired orientation component is further defined as manipulating the angles about the axes to equate the actual orientation component to the desired orientation component.

36. A method as set forth in claim 35 further including the step of solving the location equation to minimize the difference between the actual location component and the desired location component.