Method of controlling a robot through a singularity

[00002] This patent application claims priority to and all advantages of U.S. Provisional Patent Application No. 60/362,353, which was filed on Mar. 7, 2002.

[00003] 1. Field of the Invention

[00004] The subject invention relates to a method for controlling a robot such that a tool attached to the robot may follow a Cartesian path without excessive joint speed while encountering a singularity.

[00005] 2. Description of the Related Art

[00006] An industrial robot can be viewed as a chain of stiff links, or arms. Referring to FIGS. 2A, 3, and 6, a robot is shown having varying numbers of axes. FIG. 2A show a robot 20 having a base 22, a first arm 23 connected to the base 22 and rotatable about a first axis A1, and a second arm 24 connected to the first arm 23 and being rotatable about a second axis A2, a third link 25 connected to the second arm and being rotatable about a third axis A3. This robot 20 is commonly referred to as a three-axis robot. The robot 20 in FIGS. 3 and 6 has the base 22, the first arm or link 23 connected to the base 22 that is rotatable about a first axis A1, the second arm or link 24 connected to the first arm 23 that is rotatable about a second axis A2, the third arm or link 25 connected to the second arm 24 that is rotatable about a third axis A3, a fourth arm or link 26 connected to the third arm 25 rotatable about a fourth axis A4, a fifth arm or link 27 connected to the fourth arm 26 rotatable about a fifth axis A5, a sixth arm or link 28 connected to the fifth arm 27 rotatable about a sixth axis A6 and a tool 29 solidly attached to the sixth arm 28.

[00007] In general, two links are joined to each other such that they are rotatable in relation to each other around a rotary axis. Each rotary axis is provided with a position transducer which measures the angle of rotation θi, where “i” stands for the axis number. The configuration of the robot is described by the angles of rotation of the axes. Normally, an industrial robot has five or six rotary axes, but may have as few as three axes, as described above. The final link in the chain includes the tool which, depending on the field of application, may be, a gripper, a glue gun, a sprayer, spot welding equipment, and the like. The position of the tool is defined by a tool center point (TCP). The axes of the robot may also be referred to as joints between the links.

[00008] Motion of the robot is created by providing a drive signal to a motor coupled to the joint to effect movement of the robot about the axis of rotation. Many applications require the control of the tool on specific trajectories in Cartesian space. One of the most common trajectories of the tool is a path in a straight line. This path is indicated as 30 in FIGS. 2A, 3, and 6. In order to move the robot tool along the path 30, the line is divided into a large number of positions and the robot joint angles are calculated at each position which specifies the location and orientation requirement on the path. Thus, by commanding the joint positions of the robot at a large number of points on the straight line, the desired motion of the robot tool is generated.

[00009] Presently, there are two methods of implementing a desired motion: a programmed motion method or a continuous controlled motion method from a teach pendant, i.e., a linear jog method, in which the destination position is continuously updated “on the fly” or in real-time to control the trajectory. In the programmed motion method, the trajectory of the tool is specified within the program. In the continuous controlled motion (jogging) method, the trajectory is generated in real-time.

[00010] The process of transforming a position in Cartesian space to joint angles is commonly called the inverse kinematics. The converse is the forward kinematics which, given a set of joint angles, defines a unique tool position. The forward and inverse kinematics are the key transformations in processing robot motions through computer control.

[00011] The inverse kinematics for the robot is not usually unique. For a given location and orientation of the tool in Cartesian space, there is more than one way of configuring the robot arms to place the tool at the desired position. The forward kinematics is always unique, since for a given set of joint angles, the tool can only be at one location and orientation in space.

[00012] A common articulated robot geometry has six degrees of freedom. The joint angles starting from the base of the robot are commonly termed the base rotation A1, shoulder rotation A2, elbow rotation A3, wrist roll A4, wrist pitch A5, and wrist yaw A6. The complexity of determining the six joint angles of the six degrees of freedom for a given location component and orientation component directly depends on the robot geometry. Some robot geometries yield a closed form inverse kinematics. In a closed form inverse kinematics, the joint angles are calculated by solving simultaneous equations, which do not require iterations or checks for convergence. Alternatively, some robot geometries do not yield a closed form solution, and iterative methods are generally employed to solve the inverse kinematics by iterating until the solution converges. In either case, when a singular configuration is encountered, the inverse kinematics solution typically degenerates into an infinite number of joint solutions. Consequently, when controlling a robot along a Cartesian path that passes through a singularity, a finite Cartesian speed typically demands an infinite joint speed. This is a longstanding issue in robotics, that is, when the robot approaches a singularity, considerable reconfigurations of certain joint axes are required and these axes also attain very high axis speeds. These reconfigurations may cause the robot to deteriorate over time. Also, any supply lines (electrical, paint, etc.) connected to the tool may tangle or even break as the reconfigurations occur.

[00013] For the robot to be able to follow a rectilinear path which passes through a singular point, two problems have to be solved. For one thing, the robot must detect that it approaches a singularity and it must be guided through the singularity such that the rectilinear path is followed. For the reasons described above, the passage through the singularity must not entail the considerable reconfigurations of the axes.

[00014] One method of detecting that the robot approaches a singularity, and of passing through the singularity without unnecessary reconfigurations of certain of the robot axes is illustrated in U.S. Pat. No. 5,590,034. The method comprises factorizing the Jacobian matrix of the robot, which is converting the matrix into a product of several factors, wherein each of the factors may be inverted in a simple manner. The Jacobian matrix is factorized in such a way that one of the factors becomes a diagonal matrix. A diagonal matrix is inverted by inverting the diagonal elements. If any of the diagonal elements is smaller than a preset value, this is an indication that the robot is approaching a singularity. On the basis of the diagonal elements which are smaller than this value, it is determined which axes run the risk of attaining high axis velocities and these axes are locked. When the axes are locked this means that the robot movement can only take place in certain directions. A major disadvantage of this solution is that the robot cannot follow a given path through the singularity since the tool does not retain its position through the singularity. Furthermore, the method requires processors with large capacity to factor the matrices and has to deal with numerical stability of the algorithm involved.

[00015] U.S. Pat. No. 4,680,519 discloses another method of passing through singularity along a Cartesian path without unnecessary major reconfigurations of certain robot axes. The main object of this patent is to present an iterative method of solving the inverse kinematics of robots with offset wrist where closed form solution does not exist. However, this iterative method breaks down at or near robot singularities. Hence as a side objective, this patent present a different method of solving the inverse kinematics when the robot is at/near singularity, which requires that the wrist joint angles joint angles 4, 5 and 6) be given along with the Cartesian location as input. In moving through the singularity region, for each interpolation point, it needs to know the wrist joint angles and the Cartesian location. The resultant smooth robot motion is achieved in the expense of giving up the Cartesian orientation control along the path as it moves through the singularity region. Furthermore, this patent addresses only the wrist singularity and does not address other singularities.

[00016] One common approach to identify the singularities of a given robot structure, is to compute its Jacobian matrix as described below. The conditions that cause the determinant of this Jacobian matrix to be zero are the singularity conditions for this robot.

[00017] The Jacobian matrix relates the joint speed to the Cartesian speed of the robot $\left[\begin{array}{c}\underset{\_}{v}\\ \underset{\_}{\omega }\end{array}\right]=J\underset{\_}{\stackrel{.}{\theta }}$

[00018] Hence for a given Cartesian path with a certain program Cartesian speeds, its joint speeds are given by $\underset{\_}{\stackrel{.}{\theta }}=J^{-1}\left[\begin{array}{c}\underset{\_}{v}\\ \underset{\_}{\omega }\end{array}\right]$

[00019] So when the determinant of the Jacobian matrix is equal to 0, i.e. det(J)=0, the inverse of J does not exists and a singularity is reached. At such conditions, the joint speed for certain axes are infinite.

[00020] For purpose of describing the most common types of singularities encountered in industrial robots, let us consider a 6 axes robot with an inline wrist, as shown in FIG. 1., where the wrist axes (aces 4, 5 and 6) intersect at point called the wrist center point (WCP). For the purpose of exposing the singularity conditions of this robot, the Jacobian matrix of this robot for relating frame 6 velocities to the joint speeds, expressed in frame 3 coordinate frame, can be derived as. ${\hspace{0.17em}}^{3}J=\left[\begin{array}{cccccc}0& l_{12}& l_{13}& 0& 0& 0\\ l_{21}& 0& 0& 0& 0& 0\\ 0& l_{32}& l_{33}& 0& 0& 0\\ -s_3& 0& 0& 1& 0& c_5\\ 0& 0& 1& 0& c_4& s_4{s}_5\\ c_3& 0& 0& 0& s_4& -c_4{s}_5\end{array}\right]$
where
l₂₁=d₁+d₂s₂+d₃₁c₃+d₃₂s₃
l₁₂32 d₂c₂c₃+d₂s₂s₃
l₃₂32 d₂c₂s₃−d₂s₂c₃
l₁₃=c₃(−d₃₁s₃+d₃₂c₃)+s₃(d₃₁c₃+d₃₂s₃)
l₃₃=s₃(−d₃₁s₃+d₃₂c₃)−c₃(d₃₁c₃+d₃₂s₃)
Denoting ${\hspace{0.17em}}^{3}J=\left[\begin{array}{cc}A& 0\\ B& C\end{array}\right]$

• • [00028] det(J)=det(³J)=det(A)·det(C) and
  • [00029] det(A)=l₂₁(l₁₃l₃₂−l₁₂l₃₃)
  • [00030] det(C)=−s₅
  • [00031] where det( ) indicates the determinant of a matrix

[00032] Note that J is singular when the determinant of matrix A is equal to zero and the determinant of matrix C is equal to zero. All together, there are three distinct conditions in which J is singular giving rise to three distinct singularities. A first type of singularity occurs in the wrist when det(C)=0, that is when sin(θ₅)=0, where the fourth axis A4 and the sixth axis A6 are aligned, as shown in FIGS. 6 and 7. This is by far the most common type of singularity encountered in the industry. This is commonly referred to as a wrist singularity. A second type of singularity occurs when the distance of a wrist center point (WCP) from the first axis A1 is zero, that is when l₂₁=0, causing det(A) to be 0. In other words, when the WCP is on the line passing through the first axis A1. This type of singularity occurs only when the first axis A1, the second axis A2, and the third axis A3 have sufficient motion ranges, as shown in FIGS. 3 and 4. This is commonly referred to as an overhead singularity. A third type of singularity occurs when the second axis A2 and the third axis A3, are inline, that is when (l₁₃l₃₂−l₁₂l₃₃)=0, causing det(A) to be 0. This type of singularity occurs when the angle between the link axes is 180 degrees, then the second axis A2 and the third axis A3 are fully stretched out, or when the angle between the link axes is zero degree, as shown in FIGS. 2A and 2B. This is commonly referred to as an inline singularity.

[00033] For a given Cartesian location and orientation, there are in general eight possible sets of joint angles to reach the position. These correspond to three possible independent configuration sets: a first configuration set having an up configuration and a down configuration, which are related to the second axis A2 and the third axis A3, a second configuration set having a flip configuration and a no-flip configuration, which are related to the wrist axes A4, A5, A6 and a third configuration set having a front configuration and a back configuration, which are related to the sign of the distance of WCP to the first axis A1. A boundary condition of each configuration set represents one type of singularity for the robot. This is another way of identifying the robot singularities besides using the Jacobian matrix, as described above. To put it another way, the singularity occurs when the arms are in a position where the configuration cannot be determined. Hence the up/down configuration set is associated with the inline singularity, the flip/no-flip configuration set is associated with the wrist singularity and finally the front/back configuration set is associated with the overhead singularity.

[00034] For robots with an inline wrist, flip/no-flip configurations involve only the wrist axes A4, A5, A6. That is, angles of rotation (θ₁,θ₂,θ₃) about axis A1, A2, and A3 remains the same. If the flip configuration solution for the wrist axes is (θ₄,θ₅,θ₆), then the no-flip solution is given by (θ₄±180,−θ₅,θ₆∓180), and vice versa.

[00035] Hence the flip/no-flip configuration can be identified by the sign of θ₅. When θ₅ is positive, it is in the flip configuration. When θ₅ is negative, it is in the no-flip configuration.

[00036] At the boundary point of flip and no-flip configuration, i.e. θ₅=0, this is the singularity condition. This is by far the most common singularity. It is present in all five and six axes robots and for both inline and offset wrist.

[00037] Consider a straight line path that passes through wrist singularity, i.e., with θ₅=0 which is the boundary between flip and no-flip configurations, as shown in FIG. 6. As shown in FIG. 7, by maintaining the same “no-flip” configuration along this path, axis A4 has to move 180 degrees in one direction and axis A6 has to move 180 degrees in the opposite direction in one interpolation period just before and after the singular point. Therefore, the fourth axis A4 and sixth axis A6 has to maintain speeds of 18,000 deg/sec (for 10 msec interpolation time) in one interpolation period. Hence, the TCP will have to slowdown, because both axes A4 and A6 are limited by their respective joint speed limits as the robot moves through the singular point. Referring to FIG. 8, the TCP speed slow down is graphically represented. The TCP speed was traveling at a slow speed of 500 mm/s before and after the singularity and drops to less than 100 mm/s during the period where the fourth axis A4 and the sixth axis A6 are limiting to their respective joint speed limits when the robot passes through the wrist singularity.

[00038] For a given Cartesian location and orientation, there are two solutions corresponding to the robot's up/down configuration. With reference to FIG. 2C, a line from the start of the second link to the wrist center point (WCP), there are two ways to position the wrist center point (WCP) to reach the desired location. In the up configuration, the elbow is above the line, whereas in the down configuration, the elbow is below the line. Note that the up/down configuration is when the second link and the third link moves in a vertical plane and is referred to as a left/right configuration when these links moves in a horizontal plane as depicted in FIGS. 2A and 2B.

[00039] Without loss of generality, the effect of this Inline singularity can be explained with reference to FIGS. 2A and 2B for the horizontal case. The singularity occurs in this type of robot when the first link and the second link are inline, i.e. fully stretched out or fully folded in. Referring to FIG. 2A, if the path passes through the exact singularity, i.e. the boundary point between “right” and “left where the configuration is indeterminable. While moving along this path 30, the first link has to move 180 degrees as it moves through the singular point in order to remain at “right” configuration for the rest of the motion. For an interpolation time of 10 msec, this corresponds to requiring the first link to move at a very high speed of 18,000 deg/sec. By limiting the first link to its joint speed limit, it takes a longer amount of time to pass through this short distance across the singular point, thus slowing down the robot's TCP speed.

[00040] Similarly, for a given Cartesian location and orientation there are two solutions corresponding to the robot's front/back configuration. Referring to FIGS. 3 and 4, this involves mainly the first link and indirectly the second link and the third link. If a coordinate frame were attached to the first link, as shown in FIG. 4, the distance of the WCP in the positive x direction is in the front configuration. For the same WCP location, the back configuration is obtained by moving θ₁ 180 degrees from its current position, and bending θ₂,θ₃, backwards to reach the same WCP location.

[00041] The boundary of front and back configuration is when the distance of WCP to the first axis A1 is zero. At this point, the sign of the distance is neither positive nor negative, which results in a singularity. The first link has to move 180 degrees in one interpolation period, which means a first link joint speed of 18,000 deg/sec (for 10 msec interpolation period). Again TCP speed will be slowed to limit the first link's joint speed within its limit, as is shown in FIG. 5. The TCP speed before and after the singularity is 500 mm/s, but slows to about 100 mm/s due to this joint speed limit.

[00042] Accordingly, the related art methods are characterized by one or more inadequacies as described above. Specifically, the related art methods are unable to maintain the TCP speed through the singularity and are unable to maintain the TCP on the path through all the common types of singularities. The related art methods also do not suggest minimizing any errors between the location and orientation components while moving along the path that passes through or near the singularity. Furthermore, none of the related art methods change configuration of the robot arms to move through the singularity.

[00043] The subject invention provides a method of controlling a robot. The robot has a plurality of arms rotatable about a plurality of axes that define a plurality of angles therebetween. The robot also supports a tool having a tool center point (TCP) for relative movement of the tool along a path. The path has a starting point and an ending point that are defined in Cartesian coordinates.

[00044] The robot has at least one of a first, a second, and a third configuration sets. The first configuration set has an up configuration and a down configuration, the second configuration set having a flip configuration and a no-flip configuration, and the third configuration set having a front configuration and a back configuration. The robot approaches a singularity associated with the configuration set along the path while moving about the plurality of axes. The singularity occurs between a first point and a second point.

[00045] The method includes 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 with the angles of the arms in an initial configuration. The method of the subject invention includes the step of selecting the other configuration of at least one of the sets to position the TCP at the ending point with the angles of the arms in a final configuration. To move the TCP from the starting position along the path while maintaining the initial configuration, the arms are rotated about the axes by changing the angles between the axes. The robot approaches the singularity which occurs between a first point and a second point along the path and selects one of the axes in the initial configuration in response to reaching the first point. The angle of the selected axis is then interpolated from the first point to the second point in a predetermined number of steps between the first point and the second point. The angles about the remaining axes are determined in relationship to the aforesaid interpolation at each of the steps to position the arms in the final configuration when the TCP reaches the second point. To move the TCP along the path to the ending point while maintaining the final configuration, the arms are rotated about the axes by changing the angles therebetween.

[00046] Accordingly, the subject invention overcomes the inadequacies of the related art systems. Specifically, the subject invention controls the robot to move through and near a singularity region without slowing TCP speed down because there are no excessive motions about the joints. Furthermore, the location component and the orientation component of the Cartesian path are optimized, which improves path accuracy of the TCP. The method is computationally efficient with numerical stability and convergence. The method is adaptable to avoid all three types of singularities common to almost all industrial robots: wrist singularity, overhead singularity and inline singularity. The method works for both inline wrist robots, with closed form inverse kinematics solution, and offset wrist robots, without closed form inverse kinematics solution. The method can also be applied to all common types of industrial robots with different number of joint axes, such four, five, and six axes and different combination of rotary and sliding axes. The method works equally well for the path that encounters an exact singularity or near a singularity.

[00063] Referring to the Figures, wherein like numerals indicate like or corresponding parts throughout the several views, a method of controlling a robot 32 is disclosed. The robot 32 has a plurality of arms that are rotatable about a plurality of axes defining a plurality of angles therebetween. The axes may also be referred to herein as joint axes.

[00064] The robot 32 supports a tool 49 having a tool center point (TCP) for relative movement of the tool 49 along a path 33. One example of a robot 32 for carrying out the subject invention is illustrated in FIG. 9. The robot 32, shown in FIG. 9, has six arms or links altogether, it has a base 34, a first arm or link 35 connected to the base 34 that is rotatable about a first axis A1, a second arm or link 36 connected to the first arm 35 that is rotatable about a second axis A2, a third arm or link 38 connected to the second arm 36 that is rotatable about a third axis A3, a fourth arm or link 40 connected to the third arm 38 rotatable about a fourth axis A4, a fifth arm or link 42 connected to the fourth arm 40 rotatable about a fifth axis A5, a sixth arm or link 43 connected to the fifth arm 42 rotatable about a sixth axis A6 and a tool 49 solidly attached to the sixth arm 43. For ease of description, the robot 32 is described below in terms of the 6-axis robot, even thought the subject invention is equally applicable to other robots.

[00065] The path 33 has a starting point 44 and an ending point 46 defined in Cartesian coordinates. Path planning is carried out to determine a set of interpolation points along the path 33. The set of interpolation points includes a desired Cartesian location component and a desired orientation component. The robot 32 has at least one of a first, a second, and a third configuration sets. The first configuration set has an up configuration and a down configuration, the second configuration set has a flip configuration and a no-flip configuration, and the third configuration set has a front configuration and a back configuration, as described above. The robot 32 approaches a singularity associated with the configuration set along the path 33 while moving about the plurality of axes. For descriptive purposes, the singularity occurs between a first point 48 and a second point 50 along the path 33. As understood by those skilled in the art, there are three types of singularities, a wrist singularity, an overhead singularity, and an inline singularity.

[00066] The method of controlling the robot 32 includes 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. The other configuration of at least one of the sets is also selected. This positions the TCP at the ending point 46 with the angles of the arms in a final configuration.

[00067] Generally, normal inverse kinematics is used to control the robot 32 from point 44 to point 48 with this initial configuration. Between point 48 and point 50, that is around singularity region 52, the method includes the step of commanding the robot 32 to pass through exact singularity, thereby changing the configuration related to the singularity to the other configuration in the set, resulting in the final configuration. This is always possible since each type of singularity occurs at the boundary of its corresponding configuration set. The preferred embodiment of this method of passing through the singularity with configuration change between point 48 and point 50 also includes the steps of selecting one of the joint axes related to the singularity, computing its joint angle at point 48 with the initial configuration using normal inverse kinematics, computing its joint angle at point 50 with the final configuration using normal inverse kinematics, and interpolating this joint axis from its starting joint angle at point 48 to its ending joint angle at point 50 in a predetermined number of steps. Furthermore at each interpolation step, the method further includes the step of solving the remaining joint angles such that the resultant Cartesian location and Cartesian orientation matches the desired Cartesian location and the Cartesian orientation as close as possible. After reaching the second point 50 at the final configuration, normal inverse kinematics is once again used to control the robot 32 to move to the ending point 46 with this final configuration. All of the steps generally described above are described, where necessary, in greater detail hereinbelow.

[00068] To provide examples associated with selecting the initial and final configurations, the user may selected the up configuration, the no-flip configuration, and the front configuration to define the initial configuration and subject method selects the up configuration, the flip configuration, and the front configuration to define the final configuration to overcome the wrist singularity. For different applications, especially where the robot is mounted to a ceiling, instead of a floor, the user may select the up configuration, the no-flip configuration, and the front configuration to define the initial configuration and the subject method selects the up configuration, the no-flip configuration, and the back configuration to define the final configuration to overcome the overhead singularity. Yet for another application, the user may select the up configuration, the no-flip configuration, and the front configuration to define an initial configuration and the subject method selects the down configuration, the no-flip configuration, and the front configuration to define the final configuration to overcome the inline singularity. It is to be understood that by selecting the configuration, while being described as selected by a user for ease of understanding, a computer, a controller, or the like may select the configuration of the robot.

[00069] The arms are rotated 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. In response to reaching the first point 48, one of the axes in the initial configuration is selected and the angle for the selected axis is interpolated from the first point 48 to the second point 50. More specifically, the angle is interpolated in a predetermined number of steps 52 between the first point 48 and the second point 50. After the interpolation, the angles about the remaining axes are determined at each of the steps 52. By determining the angles at each of the steps 52, the arms are positioned in the final configuration when the TCP reaches the second point 50. After reaching the second point 50, the arms are rotated about the axes by changing the angles therebetween to move the TCP along the path 33 to the ending point 46 while the final configuration is maintained.

[00070] The initial configuration of the arm at the starting point 44 is normally specified by the user by selecting the configuration of at least one of the first, second and third configuration sets. For example, the user may select the up configuration, the no-flip configuration and the front configuration to define the initial configuration. The arms are rotated 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 up to the first point 48. Depending on the type of singularity encountered in the region 52 from the first point 48 to the second point 50, this invention selects the other configuration in the configuration set that is associated with that particular type of singularity.

[00071] Continuing with the examples introduced above, when wrist singularity is encountered, the initial configuration at point 48 (up, no-flip, front), will be changed to (up, flip, front) at point 50, because the no-flip/flip configuration set is associated with the wrist singularity. When the overhead singularity is encountered, the initial configuration at point 48 (up, no-flip, front), will be changed to (up, no-flip, back) at point 50, because the front/back configuration set is associated with the overhead singularity. When the inline singularity is encountered, the initial configuration at point 48 (up, no-flip, front), will be changed to (down, no-flip, front) at point 50, because the up/down configuration set is associated with the inline singularity. This final configuration at point 50 will be maintained in moving the TCP from point 50 to the ending point 46 along the path 33. This change in configuration from point 48 to point 50 is achieved by selecting one (or more) of the joint axes associated with the particular type of singularity, and the joint angle for the selected joint axis is interpolated from the first point 48 to the second point 50. More specifically, this joint angle is interpolated in a predetermined number of steps 52 between the fist point 48 and the second point 50. After each joint interpolation, the joint angles of the remaining joint axes are determined at each of the steps 52. By determining the angles at each of the steps 52, the arms are positioned in the final configuration when the TCP reaches the second point 50.

[00072] At each of the points along the path 33, such as at the starting point 44, the ending point 46, the first point 48, the second point 50, and each of the steps 52, the points are further defined by the desired location component and the desired orientation component. In region 52, as the angles are determined about the remaining axes, an actual, or resultant, location component and an actual orientation component are determined. In other words, by interpolating one of the angles, the actual location component and the actual orientation component may not exactly match the desired components.

[00073] In region 52, because the actual location component and the actual orientation component may be different than the desired components, the method further includes 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. The step of minimizing the difference is based upon the type of robot applications.

[00074] For certain types of robots 32 having certain structures, 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. For example, in paint applications and welding applications, it is important to maintain the location component. The applications are not significantly effected by the actual orientation component being different than the desired orientation component.

[00075] In order to minimize the difference between the orientation components, an orientation equation is selected that is a function of at least two of the angles. The orientation equation is solved by manipulating the angles until the orientation equation results in a minimum value. It is preferable that the minimum value is zero, however, depending upon the configuration and type of robot 32, the minimum value may be near zero and may be specified by the user. It is to be understood that different orientation equations are selected based upon the number of axes for the robot 32 and the type of singularity encountered in the selected configurations.

[00076] For ease of illustrating the tradeoffs in minimizing location error and minimizing orientation error, we consider the following specific cases related to different type of singularities and where appropriate a six axes robot is used. It is to be understood that different tradeoffs may be utilized in minimizing location and orientation errors and for different number of robot axes, while still practicing the subject invention.

[00077] When wrist singularity is encountered, we illustrate a method of equating the actual location component to the desired location component and minimizing the difference between the actual orientation component and the desired orientation component. In order to minimize the difference between the orientation components, an orientation equation is selected that is a function of at least two of the wrist angles. The orientation equation is solved by manipulating the angles until the orientation equation results in a minimum value. It is preferable that the minimum value is zero. However, the actual value depends on how far the path 33 is away from wrist singularity. The minimum value is zero when the path 33 passes through exact wrist singularity, and increases as the path 33 is farther away from wrist singularity.

[00078] Once the orientation equation is selected, in order to solve for the minimum values, the orientation equation is iteratively solved to minimize the difference between the actual orientation component and the desired orientation component. It is preferable to only have to perform two iterations that result in the minimum value. However, depending upon the orientation equation selected and the number of angles involved, more iterations may be necessary to reach the minimum value.

[00079] 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, preferably wrist angles, as will be described more below, and applying a Newton-Raphson method to the determined derivative. The iterations continue by selecting the angles that result in the derivative equaling zero. If zero is not obtainable, based upon the orientation equation or the number of axes of the robot 32, then the iterations are complete when the minimum value is obtained. This is always possible, hence the method always converges. The zero value of this derivative corresponds to the minimum value of the orientation equation.

[00080] Since the desired location component is known and because the path 33 is known and the wrist angles are determined, equating the actual location component to the desired location component is further defined as manipulating the angles, preferably the major axes, about the remaining axes to equate the actual location component to the desired location component. If one angle has been interpolated and two other angles are solved by the orientation equation, any of the remaining major joint axes angles may be manipulated so that the actual location component matches the desired location component.

[00081] Alternately, it may be preferable to equate the actual orientation component to the desired orientation component, while minimizing the difference between the actual location component and the desired location component. For different types of singularities that occur in the different configurations, it may be desirable to equate orientation components. An example of this situation arises where the robot 32 is being used to transport a part, and if the orientation were changed, the part would fall from the robot 32, such as a stack of material. However, the location component can vary for the path 33 without dropping the part.

[00082] In order to minimize the difference between the actual and desired location components, a location equation is selected that is a function of at least two of the angles. The location equation is solved by manipulating the at least two angles until the location equation results in a minimum value. It is preferable that the minimum value is zero, however, depending upon the configuration and type of robot 32, the minimum value may be greater than zero, as illustrated in the following sections. It is to be understood that different location equations are selected based upon the number of axes for the robot 32 and the type of singularity encountered in the configurations.

[00083] Because the desired orientation component is known and because the path 33 is known, 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. If one angle has been interpolated and two other angles are solved by the location equation, any of the remaining angles may be manipulated so that the actual orientation component matches the desired orientation component.

[00084] The subject invention provides another method of controlling the robot 32 as described above. This method is most applicable with six axis robots, which encounters the wrist singularity. The method includes 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. Next, the final configuration is selected to be in the up configuration, the flip configuration, and the front configuration from the sets to position the TCP at the ending point 46. The arms are rotated 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 and approaching the singularity which occurs between a first point 48 and a second point 50 along the path 33.

[00085] In response to reaching the first point 48, one of the axes in the initial configuration is selected and the angle for the selected axis is interpolated 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. After the interpolation, the angles about the remaining axes are determined at each of the steps 52. By determining the angles at each of the steps 52, the arms are positioned in the final configuration when the TCP reaches the second point 50. After reaching the second point 50, the arms are rotated 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. The subject invention provides yet another method of controlling the robot 32 described above. This method is most applicable to six axis robots mounted to either a floor or a ceiling, which encounters the overhead singularity. The method includes 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. The final configuration is selected to be in the up configuration, the no-flip configuration, and the back configuration from the sets to position the TCP at the ending point 46. The arms are rotated 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 and approaching the singularity which occurs between a first point 48 and a second point 50 along the path 33.

[00086] In response to reaching the first point 48, one of the axes in the initial configuration is selected and the angle for the selected axis is interpolated 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. After the interpolation, the angles about the remaining axes are determined at each of the steps 52. By determining the angles at each of the steps 52, the arms are positioned in the final configuration when the TCP reaches the second point 50. After reaching the second point 50, the arms are rotated 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.

[00087] The subject invention provides still another method of controlling the robot 32 described above. The method includes 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, and 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. The robot 32 approaches the singularity, which occurs between a first point 48 and a second point 50 along the path 33.

[00088] The angles about the axes are determined to move the TCP between the first point 48 and the second point 50 and through the singularity, and the arms are rotated about the axes accordingly by changing the angles therebetween to move the TCP along the path 33 while maintaining the final configuration to the ending point 46. It is to be understood that different steps may be utilized to position the arms in the final configuration when moving between the first point 48 and the second point 50, while still practicing the subject invention. The remaining steps associated with this embodiment of the method have already been described above.

[00089] When overhead singularity is encountered, the method of equating the actual orientation component to the desired orientation component and minimizing the difference between the actual location component and the desired location component is utilized. Since joint axis 1 is involved in overhead singularity, it may be chosen as the selected axis to joint interpolate from point 48 to point 50, (to change the configuration from front to back using the example above). Hence at each interpolated point along 52, the joint value of axis 1 is known. Next, the other major axes, i.e. axes 2 and 3 are manipulated such that the wrist center point WCP, which is solely a function of axes 1, 2, and 3, is closest to the desired location. This can always be solved. As shown in FIG. 12, the trace of the WCP will pass through the exact overhead singularity. And the location error in region 52 is a function of how far the path 33 is from overhead singularity. The location error can be zero when the path passes through exact overhead singularity and increases as the path moves farther away. Once the WCP is determined, the wrist axes 4, 5 and 6 can be manipulated such that the actual orientation component matches the desired orientation component.

[00090] When inline singularity is encountered, we illustrate a method of equating the actual orientation component to the desired orientation component and minimizing the difference between the actual location component and the desired location component. Since joint axis 2 is involved in inline singularity, it may be chosen as the selected axis to joint interpolate from point 48 to point 50, (to change the configuration from up to down using the example above). Hence at each interpolated point along 52, the joint value of axis 2 is known. Next, the other major axes, i.e. axes 1 and 3 are manipulated such that the wrist center point WCP, which is solely a function of axes 1, 2, and 3, is closest to the desired location. This can always be solved. FIG. 13 shows the trace of the WCP, it passes through the exact inline singularity. And the location error in region 52 is a function of how far the path 33 is from inline singularity. The location error can be zero when the path passes through exact inline singularity and increases as the path moves farther away. Once the WCP is determined, the wrist axes 4, 5 and 6 can be manipulated such that the actual orientation component matches the desired orientation component.

[00091] For the robot 32 having the first axis A1, the second axis A2, the third axis A3, the fourth axis A4, the fifth axis A5, and the sixth axis A6 respectively defining the first angle, the second angle, the third angle, the fourth angle, the fifth angle, and the sixth angle, an example according to the method is described below. The initial configuration can be any of eight possible configurations, hence without loss of generality, for the following description, assume the initial configuration is up, noflip, front. The wrist singularity is associated with the second configuration set of noflip and flip. At point 50, the step of selecting the other configuration in the set is then defined as selecting the flip configuration, resulting in the final configuration of up, flip, front at point 50. In the region 52, the step of selecting one of the axes is further defined as selecting the fourth axis A4. Normal inverse kinematics is used to solve for joint 4 with up, noflip, front configuration at point 48 and with up, flip, front configuration at point 50. The step of interpolating the angle of the selected axis is further defined as interpolating the fourth angle from its starting value at point 48 to its ending value at point 50. The step of minimizing the Cartesian orientation error while matching the Cartesian location within the region 52 with the fourth angle known is further illustrated as follows. p The desired orientation component is denoted by the 3×3 rotation matrix R. Let us further denote R as [noa], where n is a normal vector, o is an orient vector, and a is an approach vector, as is known in the robotic arts. An orientation equation represents the error between the desired orientation and the actual orientation. Different orientation equation and different weighting can be applied to each of these vectors based upon the robot 32. In the following, for ease of illustration, each vector is weighted the same and the angle between the approach vectors and the angle between the orient vectors are used. It is to be understood that one skilled in the art would select different vectors to solve for different singularity and for different types of robot 32.
ƒ=(1.0−a_desired·a_tcp)+1.0−o_desired·o_tcp)

• • [00093] where
  • [00094] · indicates dot product
  • [00095] a_desired,o_desired are the desired approach and orient vectors respectively
  • [00096] a_tcp,o_tcp are the actual approach and orient vectors respectively

[00097] The function f is a representation of the orient equation mentioned in the method. When there is an exact match between the desired orientation and the actual orientation, the value of f evaluates to zero. If zero is not obtainable, then f is greater than zero. Furthermore, it is preferable that f is a function of θ₅,θ₆.

[00098] The orientation equation, f, is smooth and continuous, and its first partial derivatives $\frac{\partial f}{\partial {\theta }_5}\mathrm{and}\frac{\partial f}{\partial {\theta }_6}$
continuously change from negative to zero to positive around its minimum point. Hence, applying the Newton-Raphson method on $F=\left[\begin{array}{c}\frac{\partial f}{\partial {\theta }_5}\\ \frac{\partial f}{\partial {\theta }_6}\end{array}\right]$
and search θ₅,θ₆ for its zero values, which corresponds to minimum f, quadratic convergence can be achieved.

[00101] Note that each of the partial derivatives $\frac{\partial f}{\partial {\theta }_5}\mathrm{and}\frac{\partial f}{\partial {\theta }_6}$
are scalars and are given by $\begin{array}{c}\frac{\partial f}{\partial {\theta }_5}=-{\underset{\_}{a}}_{\mathrm{desired}}·\frac{\partial }{\partial {\theta }_5}\left({\underset{\_}{a}}_{\mathrm{tcp}}\right)-{\underset{\_}{o}}_{\mathrm{desired}}·\frac{\partial }{\partial {\theta }_5}\left({\underset{\_}{o}}_{\mathrm{tcp}}\right)\\ \frac{\partial f}{\partial {\theta }_6}=-{\underset{\_}{a}}_{\mathrm{desired}}·\frac{\partial }{\partial {\theta }_6}\left({\underset{\_}{a}}_{\mathrm{tcp}}\right)-{\underset{\_}{o}}_{\mathrm{desired}}·\frac{\partial }{\partial {\theta }_6}\left({\underset{\_}{o}}_{\mathrm{tcp}}\right)\end{array}$

[00103] For a given robot structure, the 3 by 1 vectors $\frac{\partial }{\partial {\theta }_5}\left(a_{\mathrm{tcp}}\right),\frac{\partial }{\partial {\theta }_6}\left(a_{\mathrm{tcp}}\right),\frac{\partial }{\partial {\theta }_5}\left(o_{\mathrm{tcp}}\right),$
and $\frac{\partial }{\partial {\theta }_6}\left(o_{\mathrm{tcp}}\right)$
can be symbolically derived in a straightforward manner.

[00106] Newton-Raphson Formulation on F:

• • [00107] Find θ₅,θ₆ such that $F=\left[\begin{array}{c}\frac{\partial f}{\partial {\theta }_5}\\ \frac{\partial f}{\partial {\theta }_6}\end{array}\right]$
    is as close to $\left[\begin{array}{c}0\\ 0\end{array}\right]\hspace{1em}$
    as possible
  • [00110] Starting from the initial ${\underset{\_}{\theta }}_k=\left[\begin{array}{c}{\theta }_5\\ {\theta }_6\end{array}\right]$
  • [00111] compute the next θ_k+1 as
  • [00112] θ_k+1=θ_k−J_e⁻¹F(θ_k)
  • [00113] where $J_e=\left[\frac{\partial F}{\partial {\theta }_5}\frac{\partial F}{\partial {\theta }_6}\right]=\left[\begin{array}{cc}\frac{{\partial }^{2}f}{\partial {\theta }_5^2}& \frac{{\partial }^{2}f}{\partial {\theta }_6\partial {\theta }_5}\\ \frac{{\partial }^{2}f}{\partial {\theta }_5\partial {\theta }_6}& \frac{{\partial }^{2}f}{\partial {\theta }_6^2}\end{array}\right]$

[00114] Knowing the symbolic form of the first partial derivatives, it is straightforward to derive the second derivative in J_e. Therefore, J_e is a 2×2 matrix, whose inverse can similarly be symbolically computed.

[00115] Since f is smooth, so is F, the method is numerically stable with quadratic convergence.

[00116] Once θ₄,θ₅,θ₆ are known, the step of equating the actual location component to the desired location component can be achieved as follows. Knowing the wrist angles θ₄,θ₅,θ₆, the location of the WCP can be computed. Next, the major axes can be solved to match this WCP location, thereby matching the actual location component to the desired location component. Since the major axes will affect the actual orientation, if the orientation error is not acceptable, the above two steps can be repeated iteratively as summarized in the flowchart of FIG. 11. For a majority of cases, the above method converges in 2 iteration steps.

[00117] Utilizing the method of the subject invention allows the TCP speed to be maintained while minimizing the location and orientation components. As shown in FIG. 10, the TCP speed remains constant at 500 mm/s as the robot 32 moves through the singularity.

[00118] The methods described above are applicable for robots 32 operating under program motion, where both the starting 44 and the ending points 46 are known. The method is equally applicable for robot 32 operating under jogging motion. For jogging motion, the ending point 46 is a short distance in the jogged direction from the current position. The same equations above can be used for the jogging motion. Basically, if the ending point 46 is between the first point 48 and the second point 50, the same procedure is used as that of interpolating the steps 52, wherein the ending point 46 is one of the steps 52. Hence, the difference in the fourth axis A4 from the first point 48 to the ending point 46 is known. Then, the fourth axis A4 is determined at the second point 50 to determine what the fourth axis A4 needs to be to change configuration. Hence, as the robot 32 is jogged through the singularity region, the wrist configuration of the robot 32 is changed and high axis speed about the fourth axis A4 and the sixth axis A6 is avoided.

[00119] For the robot 32 having the first axis A1, the second axis A2, the third axis A3, the fourth axis A4, the fifth axis A5, and the sixth axis A6 respectively defining the first angle, the second angle, the third angle, the fourth angle, the fifth angle, and the sixth angle, an example according to the method is described below. Again, the initial configuration is immaterial. For this example, up, noflip, front is the initial configuration. The overhead singularity is associated with the third configuration set of front and back. At point 50, the step of selecting the other configuration in the set is then defined as selecting the back configuration, resulting in the final configuration up, noflip, back at point 50. In the region 52, the step of selecting one the axes is further defined as selecting the first axis A1. Normal inverse kinematics is used to solve for joint 1 with up, noflip, front configuration at point 48 and with up, noflip, back configuration at point 50. The step of interpolating the angle of the selected axis is further defined as interpolating the first angle from its starting value at point 48 to its ending value at point 50. The step of minimizing the Cartesian location error while matching the Cartesian orientation within the region 52 with the first angle known is illustrated as follows.

[00120] As the robot 32 moves along the path 33, the singularity is detected, by checking if the WCP is close to the first axis A1, i.e. the x- and y-components of the WCP in Cartesian space are both close to zero. By using a predefined tolerance, the singularity region can easily be characterized as a set of points that has a distance from the first axis A1 less than the tolerance.

[00121] In region 52, based on the interpolated joint 1 value and the desired Cartesian location component, we solved for joint 2 and joint 3 to position the WCP such that the location error is minimized. As shown in FIG. 12, this can be best described by considering the top view of the trace of WCP in region 52. At each step, with the interpolated joint 1 value defined, the WCP is constrained to move in a plane formed by link 2 and link 3. By projecting the desired location onto this plane, we can then solve for joint 2 and joint 3 with a small location error. Consequently, the WCP trace will pass through the exact overhead singularity following this procedure, and the maximum location error is the distance d_wcp as shown in FIG. 12. Note that if the path 33 passes through exact overhead singularity, the location error is zero. Finally, knowing the first three joint angles, the remaining joint angles can be solved to maintain the exact orientation. The end result is that, the subject invention allows the TCP speed to be maintained in region 52 without high joint axis A1 speed while encountering overhead singularity.

[00122] To further illustrate the subject invention, a three axes robot will be used in this section. For the robot 20 in FIG. 2A having the first axis A1, the second axis A2 and the third axis A3, respectively defining the first angle, the second angle and the third angle, an example according to the method is described below. Since the robot has only three degrees of freedom, the position of the tool attached to the robot can be characterized as x, y and r—three components in Cartesian space with an appropriate world coordinate frame. The x- and y-components define the location of the tool while r-component denotes the orientation of the tool. This robot has only one configuration set which is the right configuration and the left configuration. Again the initial configuration is immaterial, and for this example, the right configuration is the initial configuration. The inline singularity occurs at the boundary of this right and left configuration set, when the first and second joint axes are inline, either fully stretched out or folded in. The more interesting case, which is described here, is when they are folded in. At point 50, the step of selecting the other configuration in the set is then defined as selecting the left configuration. In the region 52, the step of selecting one the axes is further defined as selecting the first axis A1. Normal inverse kinematics is used to solve for joint 1 with right configuration at point 48 and with left configuration at point 50. The step of interpolating the angle of the selected axis is further defined as interpolating the first angle from its starting value at point 48 to its ending value at point 50. The step of minimizing the Cartesian location error while matching the Cartesian orientation within the region 52 with the first angle known is illustrated as follows.

[00123] With reference to FIG. 13, as the robot 32 moves along the path 33, the inline singularity is detected by checking how close the angle between the first axis A1 and the second axis A2 is to 180 degrees. In region 52, based on the interpolated joint 1 value and the desired Cartesian location, we solved for joint 2 to position the WCP such that the location error is minimized. This can be achieved as follows: Still referring to FIG. 13, given the joint angle of the first axis A1, the possible location of WCP forms a circle centered at the second axis A2 with the radius of the length of link 2. By projecting the desired location onto this circle, we can then solve for joint 2 with a small location error. Consequently, the WCP trace will pass through exact inline singularity following this procedure and the maximum location error is the distance d_wcp as shown in FIG. 13. Subsequently, the third joint angle can be solved to maintain the exact Cartesian orientation. The end result is that, the subject invention allows the TCP speed to be maintained in region 52 without high joint axis A1 speed while encountering inline singularity.

[00124] Obviously, many modifications and variations of the present invention are possible in light of the above teachings. The invention may be practiced otherwise than as specifically described within the scope of the appended claims. In addition, the reference numerals in the claims are merely for convenience and are not to be read in any way as limiting.