Method of robot dynamic motion planning and control

A method and system for motion planning for robots with a redundant degree of freedom. The technique computes a collision avoidance motion plan for a robot with a redundant degree of freedom, without artificially constraining the extra degree of freedom. The motion planning is formulated as a quadratic programming optimization calculation having a multi-component objective function and a collision avoidance constraint function. The formulation is efficient enough to compute the motion plan in real time at every robot control cycle. The collision avoidance constraint ensures clearance of all parts of the robot from both static and dynamic obstacles. Objective function terms include minimizing path deviation, joint velocity regularization and robot configuration or pose regularization. Weighting factors on the terms of the objective function are changeable for each control cycle calculation based on obstacle proximity conditions at the time.

BACKGROUND

Field

The present disclosure relates generally to the field of industrial robot motion planning and, more particularly, to a method and system for motion planning for robots with a redundant degree of freedom, including the presence of dynamic obstacles, where the motion planning is formulated as a quadratic programming optimization calculation having a multi-component objective function and a collision avoidance constraint function, and where weighting factors on the terms of the objective function are changeable for each control cycle calculation based on obstacle proximity conditions at the time.

Discussion of the Related Art

The use of industrial robots to perform a wide range of manufacturing, assembly and material movement operations is well known. In some applications, the robot being employed has more degrees of freedom (DOF) than the task being performed. This is the case when a conventional six-DOF robot is used for a five-DOF task, such as where the rotational (“spin”) position of the tool about its local axis is not important and can take on any value. Seven-DOF robots have also been designed to intentionally have more degrees of freedom than the required task (e.g., spray painting), where the extra degree of freedom improves the near-reach flexibility of the robot—such as enabling the robot arms to fold and fit in a narrow space between a vehicle body and a wall of a spray paint booth.

Techniques have been developed in the art for motion planning of robots with a redundant degree of freedom. One known technique involves defining a plane within which the robot elbow joint must remain. This extra constraint reduces the effective degrees of freedom of the robot by one, and enables motion planning to be computed analytically using inverse kinematics (IK) calculations. However, this technique artificially limits the flexibility of the robot, and requires an extra programming step for the operators to define the elbow joint plane.

Another known technique for motion planning of robots with a redundant degree of freedom exploits the redundant degree of freedom in the null space of the Jacobian matrix, then uses an optimization computation to compute robot motion. This technique has several drawbacks—including the lack of any constraints on the optimization computation, the optimization computation being limited to a single objective function variable at a time, and the technique being applicable only for robot teaching mode. The teaching mode limitation in turn introduces additional drawbacks—including the inability to handle line tracking applications (such as vehicle bodies moving on a conveyor), and the inability to perform dynamic obstacle avoidance calculations in the motion planning.

In many robot workspace environments, obstacles are present and may sometimes be in the path of the robot's motion. The obstacles may be permanent objects such as building structures and fixtures, which can easily be avoided by the robot with pre-planned motions due to the static nature of the object. The obstacles may also be dynamic objects which move into or through the robot workspace at random. Dynamic objects must be accounted for in real-time calculations by the motion planning system, where the robot must maneuver to avoid the objects while performing an operation. Collisions between any part of the robot and any obstacle must absolutely be avoided, and simply stopping the robot in the presence of dynamic obstacles is not a satisfactory solution.

In light of the circumstances described above, there is a need for an improved dynamic motion planning technique for redundant robots which permits full flexibility of the robot, includes obstacle avoidance in the robot motion when dynamic objects exist in the workspace, and completes the motion planning computations rapidly enough to be performed during each robot control cycle.

SUMMARY

In accordance with the teachings of the present disclosure, a method and system for motion planning for robots with a redundant degree of freedom is described and illustrated. The technique computes a collision avoidance motion plan for a robot with a redundant degree of freedom, without artificially constraining the extra degree of freedom. The motion planning is formulated as a quadratic programming optimization calculation having a multi-component objective function and a collision avoidance constraint function. The formulation is efficient enough to compute the motion plan in real time at every robot control cycle. The collision avoidance constraint ensures clearance of all parts of the robot from both static and dynamic obstacles. Objective function terms include minimizing path deviation, joint velocity regularization and robot configuration or pose regularization. Weighting factors on the terms of the objective function are changeable for each control cycle calculation based on obstacle proximity conditions at the time.

Additional features of the presently disclosed systems and methods will become apparent from the following description and appended claims, taken in conjunction with the accompanying drawings.

DETAILED DESCRIPTION OF THE EMBODIMENTS

The following discussion of the embodiments of the disclosure directed to dynamic motion planning and control for redundant robots is merely exemplary in nature, and is in no way intended to limit the disclosed devices and techniques or their applications or uses.

It is well known to use industrial robots for a variety of manufacturing, assembly and material movement operations. In some applications, the robot has more degrees of freedom of motion than the number of degrees of freedom of the task being performed. This is the case, for example, when a seven-axis robot is used for a spray painting application in which the spray nozzle position and pose are fully defined by six degrees of freedom.

Furthermore, in many robot workspace environments, obstacles may be present and may at times be in the path of the robot's motion. That is, without adaptive motion planning, some part of the robot may collide with some part of an obstacle when the robot moves from its current position to a destination position. The obstacles may be static structures such as fixtures and tables, or the obstacles may be dynamic (moving) objects such as people, forklifts and other machines. When dynamic objects may be present, the robot's motion must be planned in real time for each control cycle.

Techniques have been developed in the art for computing motions for robots having a redundant degree of freedom. However, these techniques exhibit a variety of shortcomings—including limiting the flexibility of the robot by requiring an artificial constraint to be defined, lack of sufficient control of the solution due to limitations in the optimization objective function and constraints, the capability to be used only in teaching mode and not in real time motion planning, and the resultant inability to handle dynamic obstacle avoidance or line tracking applications.

The dynamic motion planning system of the present disclosure overcomes the shortcomings of prior art systems by formulating the motion planning as a quadratic programming optimization calculation having a multi-component objective function and a collision avoidance constraint function. This technique results in a joint motion plan which seeks to minimize deviation from the planned path, balance joint velocities, and avoid mid-path robot configuration/pose changes—all while meeting a dynamic object collision avoidance constraint. The optimization computation is efficient enough to be performed in real time during robot motion, and the computation is further formulated such that weighting factors on the terms of the objective function are changeable for each control cycle calculation based on obstacle proximity conditions at the time.

FIG.1is a block diagram illustration of a dynamic motion planning system for a redundant robot, including collision avoidance in the motion planning, according to an embodiment of the present disclosure. A planner module110computes a planned robot motion based on an input target (destination) location. In one non-limiting example, the robot tool is a gripper, and the robot's task is to move a part from a source (starting) location to the target location. Of course, there are many other examples of tasks—such as moving from one spot-welding location to the next, moving to a new location to begin a spray-painting stroke, etc. In one embodiment, the planned robot motion udesis an acceleration vector defining “design” (planned) robot motion in Cartesian space. Specifically, udesmay be defined as tool center point acceleration, which may also be represented as {umlaut over (x)}des, where x is the tool center point position and orientation in six degrees of freedom. Motion planning may also be performed using velocities rather than accelerations, in which case, udeswould be defined as tool center point velocity, {dot over (x)}des. The planner module110provides the planned robot motion udesto a dynamic motion optimization module120.

The input target (destination) location may be a moving target—such as a vehicle body moving along a conveyor. In this type of “line tracking” application, the target location may be defined as a linear function of time based on conveyor velocity, or may even be computed for each control cycle for applications where the conveyor speed may vary. In any case, the target location for the robot tool is the location where the tool needs to be, at the time in the future when the robot arm is projected to arrive.

The dynamic motion optimization module120also receives obstacle data input from a perception module130. The perception module130includes one or more cameras or sensors configured to provide data about obstacles which may exist in the robot workspace. The obstacle data typically includes a minimum robot-obstacle distance, and may also include other data about the position (including spatial shape) and velocity of any obstacles.

The dynamic motion optimization module120performs an optimization computation which minimizes tracking deviation from the planned robot motion udes, and also optimizes robot motion characteristics, while including robot mechanical limitations and a collision avoidance safety function as constraints. This optimization computation results in a commanded robot motion qcmd. The commanded robot motion qcmdis the robot motion in joint space which will take the robot tool to the target location while avoiding any obstacles in the robot workspace. The optimization computation is discussed in detail below. A feedback loop140provides the commanded robot motion qcmdfrom the dynamic motion optimization module120back to the planner module110. The planner module110and the dynamic motion optimization module120repeat the calculations described above at each robot control cycle.

The dynamic motion optimization module120also provides the commanded robot motion qcmdto a robot controller, which provides robot control commands to a robot (not shown), and receives actual robot joint positions qacton a feedback loop, as known in the art. The robot controller updates the robot control commands at each control cycle based on the actual robot joint positions qactand the commanded robot motion qcmd. This is discussed further below.

Box150shows how the robot redundancy discussed previously affects the system ofFIG.1. The motion of the tool center point, udes, is an element of the set of real numbers R having a dimension m. The robot joint motion vector, qcmd, is an element of the set of real numbers having a dimension n, where n >m. The DOF redundancy is (n−m). Consider the case of a 7-DOF robot performing a 6-DOF task, as discussed earlier. In this case, n=7, m=6, and the redundancy is (7-6)=1 degree of freedom. This single redundant degree of freedom allows the optimization solver some latitude in finding a robot motion solution which efficiently moves the tool center point to the target location, while ensuring collision avoidance and also meeting other robot motion “smoothness” objectives as discussed below.

Box160shows the fundamental formulation of the optimization computation used in the dynamic motion optimization module120. The optimization computation includes an objective function170which seeks to minimize a combination of several properties of the commanded robot motion. The objective function170is discussed in detail below. The optimization computation also includes at least one inequality constraint180which addresses structural/mechanical limitations of the robot. The inequality constraint180as shown ensures that robot joint velocities {dot over (q)} remain below predefined maximum joint velocities {dot over (q)}maxwhich are predetermined based on robot mechanical limitations. In some embodiments, one or more other inequality constraints (not shown onFIG.1) may be added which ensure that robot joint accelerations and jerk remain below predefined maximum values which are also based on robot mechanical limitations. An additional inequality constraint may be added (not shown above) which requires that the joint positions q remain within predefined joint position ranges.

A collision avoidance safety constraint190is also included in the optimization computations shown in the box160. The safety constraint190is one embodiment of collision avoidance inequality constraint which ensures that the robot does not collide with any obstacles which are present in the robot workspace. The safety constraint190employs a model which defines a safety function h(X) based on robot-obstacle minimum distance or a combination of robot-obstacle minimum distance and robot-obstacle relative velocity, where the value of the safety function h(X) is equal to the robot-obstacle minimum distance when the robot and obstacle are moving away from each other, and the value of the safety function h(X) is equal to the minimum distance minus a relative velocity term when the robot and obstacle are moving toward each other. The inequality safety constraint190is then defined as {dot over (h)}(X)≥−γh(X), as shown onFIG.1. In simple terms, the safety constraint190dictates that the larger the robot-obstacle minimum distance d, the larger the allowable rate of change of distance ({dot over (d)}). The value of the coefficient γ can be established to achieve the desired system behavior, where a smaller value of γ results in more conservative robot behavior (increased maneuvering to provide greater obstacle avoidance distances). The robot-obstacle distance and relative velocity are determined from the obstacle data from the perception module130and known robot configuration data.

A complete discussion of the safety constraint190and its use in a collision avoidance optimization-based motion planning system was disclosed in U.S. patent application Ser. No. 17/455,676, titled DYNAMIC MOTION PLANNING SYSTEM, filed -19 Nov. 2021 and commonly assigned with the present application, and hereby incorporated by reference in its entirety. Any other suitable type of collision avoidance safety constraint may be employed in the optimization computation of the box160, including the use of a safety function which is based on robot-obstacle minimum distance only.

Returning to the objective function170onFIG.1, it can be observed to contain three terms—a tool center point tracking term172, a joint velocity regularization term174, and a pose regularization term176. Each of the terms172,174and176is a square of a norm of a vector representing a different characteristic of robot motion. The tool center point tracking term172, computed as ∥udes−j{dot over (q)}∥2, represents the deviation of the commanded joint motions q from the planned tool center point path udes, where J is the Jacobian (a derivative of robot configuration). In the absence of any obstacles in the robot workspace, the commanded joint motions q will cause the tool center point to follow the planned path udes. However, when an obstacle is present in the workspace, the safety constraint190might require that the commanded joint motions q cause the tool center point to deviate from the planned path udesin order to avoid a collision. The tool center point tracking term172in the objective function170seeks to minimize this deviation. Sometimes, when an obstacle is present in the path of some part of the robot other than the tool, a robot motion plan can be found which does not affect the tool center point path, but rather results in flexing the arms of the robot into a shape that avoids the obstacle. Examples of this are discussed below.

The joint velocity regularization term174, computed as λ1∥{dot over (q)}∥2, represents the overall magnitude of the robot joint velocity vector including all joints in the robot, where λ1is a weighting factor (which is changeable, as discussed later). In general, it is desirable for the rotations of the robot joints to be as small and balanced as possible while moving the tool center point to the target location. The joint velocity regularization term174in the objective function170seeks to provide this behavior by minimizing the norm of the joint velocity vector.

The pose regularization term176, computed in one embodiment as λ2∥qref−q|2, represents the deviation of the robot pose (joint position vector) q from a reference pose (reference position vector) qref, where λ2is another weighting factor (which is changeable, as discussed later). As a robot moves from a start location to a target (destination) location, it is desirable for the robot to maintain a consistent pose (overall configuration of joint and arm positions), where each joint angle increments a small amount from one time step (control cycle) to the next. That is, it is undesirable for one or more of the joints to suddenly “flip” to an inverted or opposite position from one step to the next. The pose regularization term176in the objective function170seeks to provide the desired behavior by minimizing the norm of the difference between the joint position vector and the reference position vector. Other formulations of a pose regularization term are discussed later.

A beneficial feature of the optimization computations shown in the box160ofFIG.1is that the weighting factors λ1and λ2in the objective function170can be changed “on the fly” to achieve the desired robot behavior. That is, the values of λ1and λ2can be adaptively set for each control cycle computation of the dynamic motion optimization module120. For example, when no obstacle is present in the workspace or any obstacle is far from the robot, it is desirable to maintain the preferred robot pose while minimizing path deviation; in this case, the value of λ2may be increased (to emphasize the effect of the pose regularization term176in the objective function170) while the value of λ1is decreased. On the other hand, when an obstacle is close to the robot and collision avoidance maneuvering is likely to be necessary, it may not be possible to maintain the preferred robot pose, in this case, the value of λ2may be decreased (to deemphasize the effect of the pose regularization term176in the objective function170) while the value of λ1is increased. Other factors may be considered in the determination of the values of the weighting factors λ1and λ2in the objective function170.

FIG.2is a graph200showing two different models for adaptive adjustment of the weighting factors λ1and λ2in the objective function170used in the dynamic motion optimization module120ofFIG.1. The graph200plots the value of the weighting factors λ1on a vertical axis210versus a minimum robot-obstacle distance on a horizontal axis220. The axes210and220are not labelled with specific values, as the values can be adjusted to achieve desired results, using the concepts shown on the graph200.

A step function curve230illustrates a first technique which can be used for adaptively adjusting the weighting factors λ1and λ2in the objective function170based on minimum robot-obstacle distance. When the minimum robot-obstacle distance is low (at the left of the graph200, meaning that evasive collision avoidance maneuvering is likely), λ1is set to a high value (such as 1.0). The corresponding λ2is set to a low value (such as 0.0). This combination of weighting factor values in the objective function170causes the optimization computation to converge to a solution which emphasizes velocity regularization while minimizing tracking deviation, and considers pose regularization only slightly if at all.

When the minimum robot-obstacle distance is high—that is, the minimum distance exceeds a threshold such as 0.5 meters (at the right of the graph200, meaning that evasive collision avoidance maneuvering is not likely), λ1is set to a low value (such as 0.0). The corresponding λ2is set to a high value (such as 1.0). This combination of weighting factor values in the objective function170causes the optimization computation to converge to a solution which emphasizes pose regularization while minimizing tracking deviation, and considers velocity regularization only slightly if at all. When no obstacle is detected by the perception system130, this is treated the same as a high minimum robot-obstacle distance (λ1is set to a low value; λ2is set to a high value). The transition from a high λ1to a low λ1in the step function curve230need not be a vertical line; it could be a line with a negative slope, for example.

A continuous curve240illustrates a second technique which can be used for adaptively adjusting the weighting factors λ1and λ2in the objective function170based on minimum robot-obstacle distance. The continuous curve240also sets λ1to a high value when minimum robot-obstacle distance is low, and sets λ1to a low value when minimum robot-obstacle distance is high. However, unlike the step function curve230, the continuous curve240provides a smooth transition from high to low values of λ1. The continuous curve240could be modeled as an algebraic function (such as a cubic or quintic), or as a trigonometric function (such as a cosine), for example. In some embodiments, after the continuous curve240or the step function curve230is used to determine the value of λ1, the value of λ2may be determined by an equation in which the sum of λ1and λ2is equal to a constant—such that λ1is higher when λ2is lower, and vice versa.

Again, in the graph200, the mid-graph or threshold distance could be some value other than 0.5 meters, and the maximum and minimum values of the weighting factors λ1and λ2need not be 1.0 and 0.0, respectively. For example, the lowest weighting factor value could be slightly greater than zero (such as 0.1), so that all terms in the objective function170are always considered. Also, the maximum weighting factor value could be higher or lower than 1.0, which would affect the balance between the path tracking deviation term172and the velocity and pose regularization terms174and176. The exact adaptation model which is used (of the two shown on the graph200, or others that can be envisioned), along with the values of λ1and λ2and the threshold minimum distance, can be selected to provide the best results in a particular application. In any case, the optimization computation formulation shown in the box160, with the objective function170featuring adaptive weighting at each computation cycle, provides powerful capability for real time collision avoidance motion planning of robots with a redundant degree of freedom.

In the objective function170, the pose regularization term176(λ2∥qref−q|) seeks to provide the desired robot behavior by minimizing the norm of the difference between the joint position vector q and the reference position vector qref. This requires that the reference position vector qrefbe provided for each step of the robot motion from the start point to the target point.

FIG.3is an illustration of two different techniques for providing the reference position vector qreffor use in the dynamic motion optimization module120ofFIG.1. A robot is required to move a tool center point from a start point310(or P1) to a target point312(or P2). Each of the points310and312(P1and P2) is defined in Cartesian space by three positional degrees of freedom (x, y, z), three rotational degrees of freedom (yaw, pitch and roll, or w, p, r), and an extra degree of freedom (α). Each of the tool center points310and312(P1and P2) in Cartesian space has a corresponding robot position in joint space. A joint position vector320(or q1) corresponds to the tool center point310(P1), and a joint position vector322(or q2) corresponds to the tool center point310(P1). Each of the joint position vectors320and322(q1and q2) is defined in joint space by six joint position degrees of freedom (J1, . . . , J6), and an extra degree of freedom (E1).

FIG.3illustrates two different techniques for providing the reference position vector qrefas the robot moves the tool center point from P1to P2. In a first technique300(top half ofFIG.3), the interpolation is done in joint space at box302. Using the technique300, the start joint vector q1is computed from the start tool center point P1, and the target joint vector q2is computed from the target tool center point P2. Then the interpolation between q1and q2is done in joint space, resulting in a set of points q1(330) in joint space which define a curved path in space between the tool center points P1and P2. As understood by those skilled in the art, the most efficient robot joint motion to move a tool center from one point to another usually causes the tool center point to follow a curved path. For the technique300, the reference position vector qrefas the robot moves the tool center point from P1to P2is defined by (q1, . . . , q1, . . . , q2) using the joint vectors q1(330).

In a second technique350(bottom half ofFIG.3), the interpolation is done in Cartesian space at box352. Using the technique350, a set of points Pi (360) are first interpolated, at the box352, between the start tool center point P1and the target tool center point P2, resulting in a linear path in Cartesian space from P1to P2. The set of points Pi are then converted to a set of points q1(370) in joint space using an inverse kinematics (IK) calculation at box354. For the technique350, the reference position vector qrefas the robot moves the tool center point from P1to P2is defined by (q1, . . . , q1, . . . , q2) using the joint vectors q1(370).

In the objective function170, the pose regularization term176(λ2∥qref−q∥) discussed above seeks to provide the desired robot behavior by minimizing the norm of the difference between the joint position vector q and the reference position vector qref. Following is a discussion of another technique which may be used for pose regularization in the objective function170of the optimization computations.

In simple terms, the purpose of the pose regularization term in the objective function is to maintain a consistent overall pose of the robot as it moves the tool from the start point to the target point. The “consistent overall pose” may be described as joint positions and arm positions moving smoothly within a range, avoiding any joint positions and/or arm positions suddenly making a large jump or change from one time step to the next. One simple example of a sudden configuration or pose change, commonly encountered in robot motion planning, is where the wrist joint (at the end of the outer arm) “flips” to an inverted position from one time step to the next in order to put the tool in a desired orientation.

FIGS.4A and4Bare simplified illustrations of another example of a robot undergoing a pose or configuration change. A robot400includes a fixed pedestal410, a base420coupled to the pedestal410via a vertical-axis joint412, an arm430coupled to the base420via a joint422, an arm440coupled to the arm430via a joint432, an arm450coupled to the arm440via a joint442, and a tool460coupled to the arm450via a joint452. A tool center point462is defined on the tool460. The robot400is simply illustrative, and is drawn to suit the purpose of the present discussion. Other robots may include different numbers of arms and joints, joints which permit axial rotation of one arm relative to another, etc.

InFIG.4A, the robot400is shown in a standard pose or configuration—where the arm430is oriented substantially horizontally, and the arm440is oriented substantially vertically, resulting in a joint position at the joint432of about 90°. The standard pose shown inFIG.4Amay be considered the “down” pose, because the joint432is located well below the joint442.

InFIG.4B, the robot400is shown in an alternate pose or configuration—where the arm430is oriented substantially vertically, and the arm440is oriented substantially horizontally, resulting in a joint position at the joint432of about −90°. The alternate pose shown inFIG.4Bmay be considered the “up” pose, because the joint432(extra joint in the inner arm) is located well above the joint422. Even though the tool center point462is in exactly the same location inFIGS.4A and4B, the robot400can take on two very different poses or configurations which result in this tool center point location.

As mentioned above, the purpose of the pose regularization term in the objective function is to maintain a consistent overall pose of the robot as it moves the tool from the start point to the target point. In terms ofFIGS.4A and4B, it is undesirable for the robot400to switch from the “down” pose (FIG.4A) to the “up” pose (FIG.4B) in the middle of a tool movement. The pose regularization term in the objective function is designed to prevent this type of pose change mid-motion. One embodiment of pose regularization term (comparing proposed joint positions to qref) was discussed above. Another embodiment of pose regularization term is discussed below.

Consistency of pose may also be achieved by initially computing a destination configuration (the robot pose or configuration when the tool center point is at the target location), and separately evaluating individual joint angles for joints which are prone to significant pose change during the robot task (e.g., wrist joint “flip”, or extra inner arm joint “up” vs. “down”). Rather than formulating the pose regularization term in the objective function as discussed earlier (λ2∥qref−q∥2), a cost function can be used in a pose regularization term which penalizes any solution with a significant pose change from the destination configuration. One approach to building such a cost function would be to detect a change in sign of the position of any of the joints which are prone to significant pose change. As discussed above with respect toFIGS.4A and4B, if the elbow joint has a position angle of about 90° at the destination (target tool center point) configuration, and the optimization computation evaluates a solution vector for a time step which includes the elbow joint at a position angle around −90°, that solution vector would be heavily penalized by the cost function. The change of sign could be detected and penalized in the cost function by using an inner product of the proposed position of a joint with the destination position of the joint.

Yet another type of pose regularization term may be constructed using a cost function which evaluates individual joint positions relative to their position at a destination configuration, but uses the amount of angular change rather than a change of sign. A cost function pose regularization term of this type is shown in Equation (1) below.
M·(∥θd,wrist−θwrist∥+∥+∥θd,ext-inn−θext-inn)  (1)
where θd,wristis the wrist joint angle at the destination configuration, θwristis the wrist joint angle at the configuration currently being evaluated by the optimization solver, and similarly for the extra inner arm joint, and where M is a number which can be tailored to achieved the desired results, and may be changeable at each control cycle calculation. In other words, Equation (1) will take on a larger value for configurations which have large joint position differences from the destination configuration. When Equation (1) is used as the pose regularization term (instead of λ2∥qref−q∥2) in the objective function, the optimization computation will tend to move away from configurations or poses which are significantly different from the destination pose.

Both of the pose regularization formulations discussed above have been shown to provide good results in detailed mathematical simulations of real-time robot motion planning in the presence of obstacles in the workspace. Results are shown in later figures and discussed further below.

FIG.5is a flowchart diagram500of a method for dynamic robot motion planning and control for redundant robots, according to an embodiment of the present disclosure. At box502, a planned robot motion is computed based on a target or destination tool center point location. In a representative embodiment, the planned robot motion is a tool center point acceleration vector in Cartesian space defining “design” (planned) robot motions udeswhich move the tool center point to the target location. At box504, workspace obstacle data is provided by a perception module. The perception module includes at least one camera or sensor, such as a three dimensional (3D) camera, which can detect the location of any obstacles which are present in the workspace. The perception module may include an image processor which computes obstacle location data from the camera images, or the perception module may simply provide raw camera images to a computer or controller which performs robot motion optimization computations. The obstacle location data is preferably computed in a workspace coordinate frame where it can be readily compared to robot position data. Minimum robot-obstacle distance and robot-obstacle relative velocity are ultimately required from the obstacle data.

At box506, robot motion optimization computations are performed based on the planned robot motion and the obstacle data. The output of the robot motion optimization computations is the commanded robot motion qcmddiscussed above with respect toFIG.1. If no obstacles are present in the workspace, then the commanded robot motion moves the tool center point according to the planned robot motion. The commanded robot motion is provided in a feedback loop to the box502, where the planned robot motion is recomputed at every control cycle based on the target tool center point location and the commanded robot motion (which may have been modified during optimization to avoid any obstacles).

At box508, a robot controller provides the commanded robot motion to a robot. The robot controller may perform computations or transformations in order to provide suitable robot joint motion commands to the robot. At box510, the robot actually moves based on the joint motion commands from the controller. The robot and controller operate as a closed loop feedback control system, where the actual robot state qact(joint positions and velocities) is fed back to the controller for computation of updated joint commands. The robot and controller operate on a control cycle having a designated time period (i.e., a certain number of milliseconds).

In the box506, the motion optimization problem can be formulated as:

The pose regularization term (λ2∥qref−q|2) shown in Equation (2) may be replaced by the cost function pose regularization term of the type shown in Equation (1) and discussed earlier.

For the obstacle avoidance constraint (Equation (4)), the goal is to keep the safety function h(X)≥0, as discussed above with respect toFIG.1. The obstacle avoidance constraint (Equation (4)) and the formulation of the safety function h(X) are described in detail in the incorporated application Ser. No. 17/455,676 discussed above. A different safety function, based on robot-obstacle minimum distance only, may also be used as discussed earlier.

The optimization computations using Equations (2)-(4) are performed at each robot control cycle. Upon convergence, the optimization computations yield the commanded robot motion qcmdwhich represents the robot motion having the minimum combination of objective function terms (tracking deviation, velocity regularization, and pose regularization) while satisfying the inequality constraints. The weighting factor coefficients on the regularization terms of the objective function (λ1and λ2or M) may be adjusted for each optimization computation (each control cycle) based on workspace obstacle conditions, as discussed earlier.

InFIG.5, the computation of the planned robot motion at the box502, the robot motion optimization computations at the box506and the computation of robot joint motion commands at the box508may all be performed on a robot controller which is in real-time communication with the robot. Alternately, the computations of the boxes502and506may be performed on a separate computer and the commanded robot motion for each control cycle provided to the controller at the box508. These two alternate hardware implementations are shown in the following two figures and discussed below.

FIG.6is a block diagram illustration of a dynamic motion planning system and controller for a redundant robot, including collision avoidance in the motion planning, according to a first embodiment of the present disclosure. In this embodiment, a separate computer600, including a processor and memory, performs all of the computations in the planner module110and the dynamic motion optimization module120ofFIG.1. The perception module130is in communication with the separate computer600, and provides obstacle data in the form of camera images and/or sensor data to the separate computer600for use in the dynamic motion optimization module120.

The separate computer600communicates with a robot system602, including a robot controller650and a robot660. Specifically, the commanded robot motion qcmais provided from the dynamic motion optimization module120to the controller650. The controller650controls the motion of the robot660in a real-time control system operating on a defined control cycle (a certain number of milliseconds), with actual robot state qact(joint positions and velocities) provided back to the controller650on a feedback loop670. In the system ofFIG.6, the separate computer600could be a dedicated computer specifically for the robot system600, or the separate computer600could perform motion optimization computations for multiple robot systems.

FIG.7is a block diagram illustration of a dynamic motion planning system and controller for a redundant robot, including collision avoidance in the motion planning, according to a second embodiment of the present disclosure. In this embodiment, there is no separate computer; rather, all of the motion optimization computations and control of the redundant robot are self-contained in a robot system700itself. That is, the computations in the planner module110, the dynamic motion optimization module120and the robot controller module650are all performed on a traditional robot controller (having a processor and memory) in communication with the robot660. The perception module130provides obstacle data in the form of camera images and/or sensor data to the robot system700for use in the dynamic motion optimization module120. The controller module650controls the motion of the robot660in a real-time control system operating on a defined control cycle, with actual robot state qact(joint positions and velocities) provided back to the controller module650and to the planner module110on a feedback loop770.

The system ofFIG.7, with all robot motion planning, optimization and control computations performed on the robot controller, offers the advantage of avoiding the use of the separate computer600. On the other hand, the system ofFIG.6allows the optimization computations to be performed on a separate processor, decoupled from the real-time robot controller, which offers the advantage of faster motion optimization computations. In either hardware implementation, the motion optimization computations—ensuring collision avoidance from any dynamic obstacles, and providing optimal motion characteristics of the redundant robot—must be performed for each robot feedback control cycle, based on robot configuration and obstacle data at the time.

The dynamic motion planning techniques ofFIGS.1-7have been demonstrated to produce reliable motion planning results including obstacle avoidance in simulated robot systems having a redundant degree of freedom. This includes effective motion planning to avoid any obstacles in the workspace while minimizing path deviation and maintaining desired joint velocities and robot pose, and rapid computation of the safety function and the resulting motion optimization.

FIG.8is a graph of a robot tool center point path in three-dimensional space, along with robot elbow joint path modifications needed to avoid a static obstacle in the workspace, according to an embodiment of the present disclosure. A workspace800is represented in 3D space by orthogonal X, Y and Z axes as shown. A robot (not shown) operates in the workspace800and is required to perform a task which involves moving a tool center point from a start point810to a target (destination) point812. The tool center point follows a TCP trace820from the start point810to the destination point812.FIG.8represents data collected from simulations of a particular robot and controller, using the dynamic path planning techniques for redundant robots of the present disclosure.

In addition to the tool center point, a robot elbow joint point830is shown onFIG.8. Two scenarios of robot motion (elbow joint motion) are depicted inFIG.8. In a first scenario, no obstacles are present in the workspace800. In the absence of any obstacles, the robot will move the tool center point from the start point810to the target point812along the TCP trace820, while the elbow joint point830moves along an elbow reference trace840.

In a second scenario, a fixed spherical obstacle850is placed in the workspace800in a location obstructing the elbow reference trace840. A buffer zone860defines a safe distance margin around the obstacle850, such that all parts of the robot should remain outside of the buffer zone860. The second scenario simulation was run with the obstacle850, using the same tool center start point810and target point812. The dynamic path planning techniques of the present disclosure were used to compute an obstacle avoidance robot motion plan. Because of the tracking deviation term in the objective function, the tool center point follows essentially exactly the same TCP trace820when the obstacle850is in the workspace as when no obstacle is present. However, with the obstacle850present, the elbow joint point830follows a much different path—shown as an actual elbow trace870. The elbow trace870causes the elbow joint point830and the inner robot arm to remain outside of the buffer zone860, while the robot still moves the tool center point along the desired path. These simulation results confirm the behavior that is expected from the dynamic motion planning computations described above—satisfying the collision avoidance safety constraint, while converging to a solution with virtually no tool center point path deviation.

FIGS.9A,9B and9Care illustrations of a robot in a workspace with a fixed overhead obstruction, including motion planning results with and without the techniques of the present disclosure.FIGS.9A-9Cillustrate another example of collision avoidance from a static obstacle by a redundant robot.

A robot900operates in a workspace having a fixed overhead obstruction910—illustrated as a ceiling which angles downward to a lower height at the right side of the workspace. The robot900is required to perform a task which involves moving a tool from a start point to a target (destination) point. The robot900is illustrated in the start point configuration inFIG.9A.

FIG.9Bshows the robot900in the destination point configuration, and represents data from a simulation of the robot and controller using a conventional path planning technique for redundant robots, which cannot consider collision avoidance of any part of the robot other than the tool center point. InFIG.9B, the robot900has moved the tool center point along a tool center point trace920as instructed, from the start point to the destination point. However, the elbow joint of the robot900has collided with the overhead obstruction910in the lower portion of the ceiling, as indicated at930. This is obviously not an acceptable robot motion, and illustrates the limitations of conventional path planning for redundant robots which can only consider tool center point obstacle collisions.

FIG.9Cshows the robot900in the destination point configuration, and represents data from a simulation of the robot and controller using the dynamic path planning techniques for redundant robots of the present disclosure, including collision avoidance and motion characteristic quality in an optimization computation. InFIG.9C, the robot900has moved the tool center point along a trace940which is virtually the same as the trace920. However, the robot900has articulated several of the arm joints to lower the elbow joint location, such that the elbow joint has sufficient clearance from the overhead obstruction910in the lower portion of the ceiling, as indicated at950. This is obviously a much better robot motion than shown inFIG.9B, and illustrates the capabilities of the motion planning techniques of the present disclosure—which use the redundant degree of freedom of the robot to provide a desired tool path while also ensuring collision-free robot motion.

FIGS.10A,10B and10Care illustrations of two robots operating in tandem in a workspace, including motion planning results with and without the techniques of the present disclosure.FIGS.10A-10Cillustrate an example of collision avoidance from dynamic obstacles by redundant robots. In this case, each robot represents a potential obstacle to the motion of the other robot.

A robot1000and a robot1010operate in the workspace, where each of the robots1000and1010is required to perform a task at the same time. The robot1000is required to perform a task which involves moving a tool from a start point1002to a target (destination) point (not shown). The tool center point for the robot1000follows a trace1004from the start point to the destination point. The robot1010is required to perform a task which involves moving a tool from a start point1012to a target (destination) point (not shown). The tool center point for the robot1010follows a trace1014from the start point to the destination point. The tool center point traces1004and1014are shown in their entirety inFIG.10C, discussed below.

FIG.10Aillustrates motions of the robots1000and1010which were planned with conventional methods not including dynamic object collision avoidance. InFIG.10A, the robots1000and1010are illustrated in a configuration roughly midway through the motion of the task they are performing. In this configuration, the tool of the robot1000collides with the elbow joint of the robot1010, as indicated at arrow1020. A collision of any kind is obviously not an acceptable result.

FIG.10Billustrates motions of the robots1000and1010which were planned using the dynamic path planning techniques for redundant robots of the present disclosure, including collision avoidance and motion characteristic quality in an optimization computation. InFIG.10B, the robots1000and1010are again illustrated in a configuration roughly midway through the motion of the task they are performing. In this instance, however, the tool of the robot1000does not collide with the elbow joint of the robot1010, with the clearance indicated at arrow1030. This clearance is possible because the robot1010flexed its inner-arm extra joint as shown at arrow1032, resulting in the elbow joint being pulled inward toward the robot base slightly.

FIG.10Cincludes stick-figure illustrations of the robots1000and1010in their final configuration, with the tool of the robot1000positioned at a destination point1006at the end of the tool center point trace1004, and the tool of the robot1010positioned at a destination point1016at the end of the tool center point trace1014. Also shown inFIG.10Cis an elbow joint trace1040for the robot1010for the motion planned with conventional techniques (FIG.10A). For the motion planned with the techniques of the present disclosure (FIG.10B), the elbow joint of the robot1010follows the same trace1040except for an area indicated at arrow1042. As shown at the arrow1042, the elbow joint of the robot1010dips downward and inward mid-path in order to create the clearance shown inFIG.10B, thus avoiding a collision.

It is noteworthy that the techniques of the present disclosure avoid a robot-robot collision without causing a deviation in the tool center point paths of either the robot1000or the robot1010. This is made possible by the redundant degree of freedom of the robots along with the motion optimization computations discussed above—with the weighted objective function which can seek a solution which minimizes tool path deviation while also meeting the collision avoidance constraint.

Other simulations of a two-robot workspace (similar toFIGS.10A-10C) were also performed, where the pose regularization term of the objective function was deactivated in one simulation, activated using the reference-pose-based pose regularization formulation (λ2∥qref−q|) in a second simulation, and activated using the cost function pose regularization formulation (Equation (1) discussed earlier) in a third simulation. These simulations confirmed that the objective function with adaptive weighting and either of the proposed pose regularization terms is effective in causing the robots to maintain a preferred pose throughout their motion while avoiding collisions.

The results shown inFIGS.8-10and discussed above illustrate the effectiveness of the disclosed motion planning techniques for redundant robots, including collision avoidance with static and dynamic obstacles, adherence to a prescribed tool center point path, and preferable robot motion characteristics (joint velocities and robot poses).

In addition, the commanded motion computation time for each control cycle (the time to provide an output from the dynamic motion optimization module120) was measured to have an average of less than one millisecond (ms) for the disclosed techniques. Using a typical robot control cycle of 8 ms, the techniques of the present disclosure perform motion planning computations more than fast enough, while providing far superior collision avoidance and motion smoothness results compared to prior art motion computation methods.

Throughout the preceding discussion, various computers and controllers are described and implied. It is to be understood that the software applications and modules of these computer and controllers are executed on one or more electronic computing devices having a processor and a memory module. In particular, this includes a processor in each of the robot controller650and the optional separate computer600ofFIGS.6and7discussed above. Specifically, the processor in the controller650and/or the separate computer600are configured to perform the redundant robot collision avoidance motion planning functions of the box506ofFIG.5(and shown onFIG.1), along with the robot feedback motion control function of the box508.

While a number of exemplary aspects and embodiments of the methods and systems for dynamic motion planning and control for redundant robots have been discussed above, those of skill in the art will recognize modifications, permutations, additions and sub-combinations thereof. It is therefore intended that the following appended claims and claims hereafter introduced are interpreted to include all such modifications, permutations, additions and sub-combinations as are within their true spirit and scope.