Applying workspace limitations in a velocity-controlled robotic mechanism

A robotic system includes a robotic mechanism responsive to velocity control signals, and a permissible workspace defined by a convex-polygon boundary. A host machine determines a position of a reference point on the mechanism with respect to the boundary, and includes an algorithm for enforcing the boundary by automatically shaping the velocity control signals as a function of the position, thereby providing smooth and unperturbed operation of the mechanism along the edges and corners of the boundary. The algorithm is suited for application with higher speeds and/or external forces. A host machine includes an algorithm for enforcing the boundary by shaping the velocity control signals as a function of the reference point position, and a hardware module for executing the algorithm. A method for enforcing the convex-polygon boundary is also provided that shapes a velocity control signal via a host machine as a function of the reference point position.

TECHNICAL FIELD

The present invention relates to the control of a wrist assembly or other robotic mechanism within a robotic system.

BACKGROUND OF THE INVENTION

Dexterous robots are able to precisely grasp and manipulate objects using a series of linkages, which in turn are interconnected via one or more motor-driven robotic joints. End-effectors are the particular linkages used to perform a given task at hand, such as grasping and maneuvering a work tool or other object. Humanoid robots are a particular type of dexterous robot having an approximately human structure, e.g., a full body, torso, hand(s), and/or another appendage(s). The structural and control complexity of any robotic system is largely dependent upon commanded work tasks, and therefore dexterous robots present a substantially greater control challenge relative to the control of conventional robots.

Dexterous robots may include one or more end effectors, such as a robotic wrist assembly having an open wrist joint, the proper control of which can enable a more precise handling of a grasped object. An open wrist joint results in wrist degrees of freedom that are actuated by indirect drives in a closed-chain mechanism. The mapping between the actuator space of any velocity-controlled wrist actuators and the joint space itself is thus coupled and nonlinear, as is understood in the art. Therefore, such a wrist joint can end up having an irregularly-shaped permissible workspace. Stable operation of a robotic mechanism operating within such a workspace is paramount to the optimal functioning of the robotic system.

SUMMARY OF THE INVENTION

Accordingly, a software-based method is provided herein for enforcing a complex-polygon workspace boundary of a velocity-controlled robotic mechanism, e.g., a robotic wrist assembly that is responsive to velocity control signals. The present method is applicable to any coupled and complex workspace wherein the degrees of freedom (DOF) of the robotic mechanism are actuated by coupled and non-direct joint actuators. Due to the coupled mapping between the DOFs and the joint actuators, the boundaries of the workspace are implemented in software rather than as simple hard stops. Although a closed-chain wrist mechanism is used herein for illustrative purposes, the present method may also be applied to the operational space of any serial chain manipulator.

The complex workspace of the robotic mechanism is defined herein by a convex-polygon, i.e., a convex set defined by n corners in real vector space, as is well understood in the mathematical arts. A convex-polygon workspace may provide the robotic mechanism with a greatly increased range of motion of relative to a conventional rectangular workspace. Additionally, the method eliminates software-induced sticking, slipping, and chattering along the boundary, i.e., along the edges and in the corners of the workspace.

In particular, a robotic system is provided herein that includes a robotic mechanism. The robotic mechanism is responsive to velocity control signals, and has a permissible workspace, i.e., a workspace that is simultaneously dependent on multiple joints of the mechanism. Within the system, a host machine determines a position of a reference point on the robotic mechanism with respect to the boundary. The host machine includes an algorithm for enforcing the boundary by automatically shaping the velocity control signals as a function of the reference point position with respect to the boundary, thereby providing a smooth and unperturbed operation of the robotic mechanism along the edges and corners of the boundary.

A host machine is also provided that is adapted for use within a robotic system. As noted above, the system includes a robotic mechanism that is responsive to velocity control signals. The host machine includes a hardware module that is electrically connected to the robotic mechanism, and that determines a position of reference point on the robotic mechanism with respect to the complex-polygon boundary, e.g., using joint sensors or other sensors. The host machine includes an algorithm for enforcing the boundary. Execution of the algorithm by the hardware module automatically shapes the velocity control signals as a function of the reference point position with respect to the boundary, thereby providing a smooth and unperturbed operation of the robotic mechanism along the edges and within the corners of the boundary. Transitional buffers applied near the edges and corners of the boundary ensure stable, non-chattering performance, especially with high speeds and/or external forces.

A method is also provided for enforcing the convex-polygon boundary noted above. The method includes determining whether the reference point on the mechanism lies within the boundary, and automatically shaping the velocity control signal(s) as a function of the distance of the reference point relative to the boundary, thereby ensuring a smooth and unperturbed operation of the robotic mechanism along the edges and corners of the boundary.

DETAILED DESCRIPTION

With reference to the drawings, wherein like reference numbers refer to the same or similar components throughout the several views, and beginning withFIG. 1, a robotic system10is adapted to perform one or more dexterous and automated tasks. The robotic system10is configured with independently and/or interdependently-moveable robotic joints, such as but not limited to a shoulder joint, the position of which is generally indicated by arrow A. Robotic system10may also include an elbow joint (arrow B), a wrist joint (arrow C), a neck joint (arrow D), a waist joint (arrow E), and various finger/thumb joints (arrow F).

Additionally, the robotic system10includes a lower arm assembly12having one or more anthropomorphic and dexterous hands14, each moveable via a wrist assembly16. Each hand14may includes an opposable thumb and one or more fingers as shown, which when operated together are capable of grasping an object20in the same hand, or in a cooperative grasp between different hands. The wrist assembly16forms a closed-chain robotic mechanism that may be controlled via the method or algorithm set forth herein. While control of wrist assembly16is described below, the present method may also be applied to control other robotic mechanisms within the robotic system10, as will be understood by those of ordinary skill in the art.

A host machine (HOST)18is adapted, via execution of a control algorithm100, for enforcing certain defined workspace limits on the wrist assembly16by shaping and applying velocity control signals (arrow11) as set forth below. Such enforcement provides an irregularly-shaped or convex-polygon workspace, and a smooth, stable operation of the wrist assembly16along the edge segments or edges and corners of its boundary, as described extensively below with reference toFIGS. 3-9.

Host machine18includes a suitably configured hardware module22that is electrically connected to the mechanism being controlled, e.g., the wrist assembly16. Hardware module22may include a digital computer(s) or data processing device(s) having one or more microprocessors or central processing units (CPU), sufficient read only memory (ROM), and sufficient random access memory (RAM). Hardware module22may also include erasable electrically-programmable read only memory (EEPROM), a high-speed clock, analog-to-digital (A/D) circuitry, digital-to-analog (D/A) circuitry, and required input/output (I/O) circuitry and devices, as well as appropriate signal conditioning and buffer electronics. Individual control algorithms resident in hardware module22or readily accessible thereby, including algorithm100as described below with reference toFIG. 4, may be automatically executed by the module as needed to provide the required control functionality.

Referring toFIG. 2, the lower arm assembly12is shown in more detail to include the wrist assembly16. As noted above, the wrist assembly16is a robotic mechanism suitable for control per the present method, and therefore the wrist assembly ofFIG. 2will be used hereafter for illustrative purposes. Movement of wrist assembly16is provided via one or more actuators24. In one embodiment, the actuators24, e.g., push-pull linear actuators such as tension and/or force-generating joint motors, may be embedded within the structure of the lower arm assembly12as shown.

Wrist assembly16is thus moveable in different directions, and therefore is moveable along different pitch (θ) and yaw (ψ) axes. The position of the wrist assembly16can be determined via sensors15, with the measurements from the sensors transmitted to the host machine18ofFIG. 1. Sensors15may include joint sensors adapted for determining joint angles along the pitch and yaw axes, and/or other devices suitable for determining the position of a reference point on the wrist assembly16as set forth below, and for relaying the position information to the host machine18ofFIG. 1. The wrist assembly16may be moved via the actuators24via indirect drives in a closed-chain mechanism, with required mapping between the actuator spaces and the joint spaces thus coupled and nonlinear, i.e., the mapping between the joint and actuator degrees-of-freedom are coupled such that the velocity of a joint is a function of the velocity of its multiple actuators. Because of this, the wrist assembly16has an irregularly-shaped or convex-polygon boundary of a permissible workspace, as will now be explained with reference toFIG. 3.

Referring toFIG. 3, a permissible workspace30for the wrist assembly16shown inFIG. 2may be defined by a convex-polygon boundary40, a term that is understood and the art and described above. Workspace30may be plotted on the pitch (θ) and yaw (φ) axes as shown. The workspace30may be physically limited by many different factors. Shown schematically inFIG. 3, workspace30may be bounded in part by close proximity of delicate wires (feature32), mathematical singularities (feature34) such as pitch limits, and hard stops (feature36), e.g., contact with a casing (not shown) for the lower arm assembly12ofFIG. 2. Other variables, whether physical or virtual, may likewise define and/or limit the workspace30.

The joint limits on a conventional robot are typically defined independently, which inFIG. 3would appear as an axis-aligned rectangle. The conventional workspace may therefore be greatly expanded into a convex-polygon, for example a polygon as shown inFIG. 3, to thereby optimize performance of the wrist assembly16. However, the expanded complex nature of the workspace results in a greatly increased level of required control complexity.

Referring toFIG. 4, the algorithm100of the present invention automatically allows for velocity-based control to occur within an irregularly-shaped workspace, e.g., the workspace30noted above, to provide a smooth, stable, and unperturbed operation of the controlled robotic mechanism along the boundary, e.g., the boundary40shown inFIG. 3. Algorithm100therefore has two key features: (1) convex-polygon management techniques, and (2) a velocity distribution map or logic, each of which are explained further below. While two degrees of freedom (DOF) are used for conceptual illustration throughout the present work, three or more DOF may also be used as understood by those of ordinary skill in the art.

Execution of algorithm100automatically identifies the boundary40of workspace30using an arc segment test, determines if a reference point (P) in the arc of the mechanism lies within the workspace30, and shapes the velocity control signals11ofFIG. 1as a function of the distance of the reference point (P) from the boundary40. In one embodiment, the host machine18ofFIG. 1shapes and applies an output velocity via signals11to the mechanism in one manner if the reference point (P) lies outside of the workspace30, and applies the output velocity in another manner if the reference point (P) lies inside the workspace.

Referring briefly toFIG. 5, the workspace30may be defined by its multiple corner points, which are progressively numbered 1-5 in the example shown. The workspace30can be divided into different adjacent arc segments50(seeFIG. 6), as defined by the lines drawn radially-outward from centroid52. The identification of the particular arc segment50in which reference point (P) lies plays an important function in the subsequent velocity shaping logic set forth below.

Referring again toFIG. 4, algorithm100begins with step102, wherein a desired velocity is input into the host machine18shown inFIG. 1as velocity control signals11, and the current position of the wrist assembly16is determined, e.g., using sensors15or other suitable means. The current position is the reference point (P) noted above. The algorithm100then proceeds to step104.

At step104, the arc segment50of wrist assembly16is identified. Referring toFIG. 6, the angles (φ1-φ5) subtending the vectors from a reference point (P) on the mechanism to each other are shown. The algorithm100determines within which particular arc segment50the reference point (P) lies. The answer determines which line segment of boundary40, and which corresponding normal vector, to use for the velocity shaping steps described below.

θimay be considered the angle between {circumflex over (r)}CPand {circumflex over (r)}C1, as shown inFIG. 5, with C referring to the centroid52. To test if a reference point (P) lies in the segment “12” ofFIG. 5, for example, the following two conditions must be satisfied:

(rCP×rC1)·k^⁡(rCP×rC2)·k^<0θ1+θ2<π
where {circumflex over (k)} represents the unit vector out of the plane. The first condition ensures that reference point (P) lies between either of the two corner vectors, rC1and rC2, or the negatives of the two corner vectors, −rC1and −rC2. The second condition eliminates the latter possibility. While inside the envelope of workspace30one might simply find the closest line segment, outside of the envelope doing so might identify an incorrect border line. For illustrative purposes it is assumed herein that reference point (P) lies in the segment ‘12’, and that it lies closer to the right edge than the left, i.e., θ2<θ1. Accordingly, n1is the primary normal and n2is the secondary normal as shown inFIG. 5. In the same sense, d1refers to the distance from reference point (P) to a primary boundary, and d2is the distance to a secondary boundary, as set forth below.

Referring again toFIG. 4, at step106the host machine18ofFIG. 1determines whether reference point (P) lies within the envelope of boundary40. If so, the algorithm100proceeds to step108. If reference point (P) does not lie within the boundaries40, the algorithm100proceeds to step107.

Step106may be accomplished in two ways. First, consider the vectors drawn from reference point (P) to each other, rPi, as shown inFIG. 6. φirepresents the angle between two consecutive vectors. Angles between vectors may be defined in the range [0, π]. Reference point (P) will lie in the envelope of the workspace30if and only if:

The second way, which is scalable to higher DOFs, involves looking at the normal vectors for each line segment of the boundary40. Let nirepresent the unit vector in the normal direction for segment i. This normal must be defined pointing into the workspace30. The normal vectors will thus be derived with respect to the centroid52, since the centroid is guaranteed to lie inside the workspace30. Thus:

n~i≐riC-(riC·ri⁡(i+1)riC·riC)⁢riCni=n~in~i
Note that the count (i+1) rolls over to 1 when i=n. Accordingly, reference point (P) lies within the envelope if and only if:
riP·ni>0,∀i.

At step107, the host machine18, having determined the correct arc at step104and that reference point (P) lies outside of the boundary of workspace30at step106, projects and scales or shapes the velocity control signals11, and then proceeds to step109. Velocity control signals11for the wrist assembly16need to be automatically shaped so as to enforce the boundaries40in software. As will be understood by those of ordinary skill in the art, due to the coupled mapping between the DOFs and the actuators24of the wrist assembly16ofFIG. 2, the boundary40cannot be implemented in hardware using simple joint limits.

Referring toFIG. 7, a snapshot is shown of allowable velocity distribution patterns56,58with respect to the boundary40. These distribution patterns56,58reflect two key considerations. First, when the reference point (P) lies outside of the workspace30, any normal component pointing away from the boundary40should be automatically zeroed by the host machine18. This step keeps the reference point (P) from moving away from the boundary40while still permitting tangential velocities along the boundary. These tangential velocities are important to allow the controlled robotic mechanism, e.g., the wrist assembly16, to move freely without software-induced stick and/or slip, and to allow the reference point (P) to find the point on the boundary40that is closest to a desired point. Second, any transitions from one region of allowable velocities to another should be continuous and gradual in order to avoid instabilities and chatter. These transitions occur at both the edges and the corners of the boundary40, as shown inFIG. 7.

Therefore, if the reference point (P) is determined to lie outside of the envelope of boundary40, any velocity component pointing away from the boundary is automatically zeroed. This results in the semi-circular velocity distribution patterns56as shown inFIG. 7. In addition, a velocity component pointing back to the boundary40and proportional to the distance from the boundary is superimposed.

At step108, and as shown inFIG. 7, when operating within the workspace30the algorithm100ofFIG. 4determines whether reference point (P) lies within a predetermined buffer55, which is defined as the area between the boundary40and a predetermined inner boundary40A. If this buffer55is defined by a distance (dmax), the following equations may be used:

d^i≐1-ⅆiⅆmaxvo={v-d^1⁡(v·n1)⁢n1,(v·n1<0)^(d1<dmax)v,else
If the reference point (P) is outside of the buffer55, the algorithm100proceeds to step112, otherwise proceeding to step110when the reference point (P) is within the buffer.

At step109, the host machine18determines whether the reference point (P) is within buffer55ofFIG. 7, and proceeds to step111if it is. Otherwise, the algorithm100proceeds to step113.

At step110, having determined at step108that reference point (P) lies within the buffer55, the host machine18ofFIG. 1either blends the normal component along the boundary40, or it accounts for blended transitions at each corner of the workspace30, depending on where the reference point (P) is located. That is, when reference point (P) is within the buffer55and the velocity is pointed towards the boundary40, the normal component of the velocity is gradually dropped to zero to provide a continuous transition from inside to outside.

When operating in a corner of the workspace30, as a point inside the envelope approaches the primary boundary or boundary40, any normal component of velocity is scaled down by the host machine18. That is, the component of a desired velocity pointing outwards in the normal direction with respect to a neighboring segment of the boundary40is gradually decreased. In addition, as the point enters a corner and approaches inner boundary40A, the tangential velocity is also scaled down. Using a two-stage projection, the first stage scales down the normal component with respect to the inner boundary40A. The second projection scales down the normal component with respect to the primary boundary, i.e., boundary40. The new rule for the points inside the envelope at the corners is:

At step111, the host machine18projects and scales the velocity control signals11, and proceeds to step113.

At step112, the host machine20maintains the desired velocity, i.e., it outputs a velocity control signals11with no changes. The algorithm100is finished.

At step113, a virtual or software-based “spring” is added that is proportional to the distance of the reference point (P) from the boundary40, i.e., f=kΔx. The wrist assembly16is forced back to the boundary40in software, and velocity is scaled to zero at the boundaries.

Referring toFIG. 8, in understanding the transition needed for a virtual spring component, consider the vector from a corner to reference point (P), or r2P, and project it onto a unit circle60. An ideal solution would apply the spring definition, vs=kd1n1, to all cases except where r2Plies in region I. In region I, the spring would be based on the vector to the corner: vs=−kr2P. k is the proportional gain. The result is a continuous and smooth spring value as reference point (P) moves from one segment to the next. Unfortunately, trying to determine the relative location of reference point (P) to the unit circle60would introduce a non-trivial amount of extra computation. The location of the unit circle60is independent of the results from the arc segment test noted above, and requires several new angle computations.

Alternately, an approximate solution may be determined that performs satisfactorily and that utilizes the previously computed data. This solution considers the corner angle, θ2, and blends the two adjacent normals to provide a continuous change in direction from one arc segment to the next. The new rule for the spring component is as follows:

Virtual springs70, i.e., software generated to systematically enforce the rules set forth above, are thus employed by the host machine18to enforce the boundary40, with the velocity component vs=kd1n1. In particular, the spring component provided by virtual springs70is needed for back-drivable systems, as that term is understood in the art. Let v refer to the initially desired velocity, and voto the output commanded after velocity shaping. The following rule defines velocity inside and outside the envelope:

v′={v-(v·n1)⁢n1+vS,v·n1<0v+vS,else⁢⁢vo={v′-d^1⁡(v′·n1)⁢n1,(v′·n1<0)^(d1<dmax)v′,else
This rule provides a continuous transition to the sides and corners of the workspace30, however, the transition across the arc segment may be discontinuous. As the reference point (P) crosses from one arc segment to the next, the primary and secondary borders swap places. If the two are orthogonal, n1·n2=0, the transition will be continuous. Otherwise, a minor discontinuity will exist according to the degree of non-orthogonality.

Therefore, at step115, the algorithm100outputs the velocity control signals11to the wrist assembly16. The algorithm100is finished.