Methods for planning and controlling the dynamic motion of robots
The method addresses the limitations of existing motion planning by formulating a quadratic programming optimization with adaptive weighting for robots with redundant degrees of freedom, ensuring efficient and real-time obstacle avoidance and maintaining desired robot behavior.
Patent Information
- Authority / Receiving Office
- JP · JP
- Patent Type
- Patents
- Current Assignee / Owner
- FANUC LTD
- Filing Date
- 2022-02-07
- Publication Date
- 2026-06-17
Smart Images
Figure 0007874975000015 
Figure 0007874975000016 
Figure 0007874975000017
Abstract
Description
[Technical Field]
[0001] This disclosure relates, in general, to the field of motion planning for industrial robots, and more particularly to a method and system for motion planning for robots with redundant degrees of freedom, including the presence of dynamic obstacles. The motion plan is formulated as a quadratic programming optimization calculation with a multi-element objective function and collision avoidance constraint functions, wherein the weighting coefficients of the objective function terms can be changed for each control cycle calculation based on the proximity of obstacles at that point in time. [Background technology]
[0002] Industrial robots are well known to be used in a variety of tasks, including manufacturing, assembly, and material handling. In some applications, the robot employed has more degrees of freedom (DOF) than the task being performed. This can include situations where a conventional 6DOF robot is used for a 5DOF task, such as when the rotational position ("swivel") around a tool's local axis is not critical and can take on any value. Furthermore, 7DOF robots are designed to have more degrees of freedom than required for the task (e.g., spray painting), and the extra degrees of freedom improve the robot's short-range flexibility, such as allowing the robot arm to fold and fit into a narrow space between the vehicle body and the wall of a spray painting booth.
[0003] In the field of motion planning for robots with redundant degrees of freedom, several techniques have been developed. One known technique involves defining a plane to hold the robot's elbow joint. Adding this constraint reduces the robot's effective degrees of freedom by one, enabling analytical motion planning using inverse kinematics (IK) calculations. However, this technique artificially limits the robot's flexibility and requires an additional programming step by the operator to define the elbow joint plane.
[0004] Another known technique for planning the motion of robots with redundant degrees of freedom utilizes the redundant degrees of freedom in the zero space of the Jacobian matrix to calculate the robot's motion through optimization calculations. This technique has several drawbacks, including the fact that there are no constraints on the optimization calculations, that the optimization calculations are limited to a single objective function variable at a time, and that this technique is only applicable to the teaching mode of the robot. The limitation to the teaching mode also results in additional drawbacks, such as the inability to handle line tracking applications (such as a vehicle moving on a conveyor belt) and the inability to perform dynamic obstacle avoidance calculations in the motion planning.
[0005] Many robotic work environments contain obstacles, which can sometimes enter the robot's path. These obstacles may be permanent objects such as building structures or equipment, in which case the robot can easily avoid them with pre-planned movements due to the static nature of the objects. Alternatively, obstacles may be dynamic objects that randomly enter or pass through the robot's workspace. Dynamic objects must be considered in real-time calculations by the motion planning system, and the robot must be maneuvered to avoid objects while performing its operations. Collisions between any part of the robot and any obstacles must be avoided at all costs, and simply stopping the robot is not a sufficient solution when dynamic obstacles are present. [Overview of the project] [Problems that the invention aims to solve]
[0006] In light of the above circumstances, there is a need for improvements in dynamic motion planning techniques for redundant robots that allow for full flexibility of the robot, include obstacle avoidance in robot movements when dynamic objects are present in the workspace, and complete motion planning calculations quickly enough to be performed during each robot control cycle. [Means for solving the problem]
[0007] According to the teachings of the present disclosure, a method and system for motion planning of a robot with redundant degrees of freedom are described and shown. The present technology calculates the collision avoidance motion planning of a robot with redundant degrees of freedom without artificially restricting the extra degrees of freedom. The motion planning is formulated as a quadratic programming optimization calculation having a multi-element objective function and a collision avoidance constraint function. This formulation has sufficient efficiency to calculate the motion plan in real time for each control cycle of the robot. The collision avoidance constraint ensures that all parts of the robot are separated from static and dynamic obstacles. The terms of the objective function include minimization of path deviation, regularization of joint velocities, and regularization of the robot configuration or posture. The weighting coefficients for the terms of the objective function can be changed according to the proximity situation of the obstacles at that time for each control cycle calculation.
[0008] Additional features of the system and method of the present disclosure will become apparent from the following description and the appended claims in conjunction with the accompanying drawings.
Brief Description of the Drawings
[0009] [Figure 1] FIG. 1 is a block diagram of a dynamic motion planning system of a redundant robot including collision avoidance in motion planning according to an embodiment of the present disclosure. [Figure 2] FIG. 2 is a graph showing two different models for the adaptive adjustment of the weighting coefficients λ1 and λ2 of the objective function used in the dynamic motion optimization module of FIG. 1. [Figure 3] FIG. 3 is an explanatory diagram showing two different techniques for providing the reference position vector qref used in the dynamic motion optimization module of FIG. 1. [Figure 4A] FIGS. 4A and 4B are simplified diagrams showing how the posture or configuration of the robot is changed. [Figure 4B] FIGS. 4A and 4B are simplified diagrams showing how the posture or configuration of the robot is changed. [Figure 5] FIG. 5 is a flowchart diagram of a method for dynamic robot motion planning and control for a redundant robot according to an embodiment of the present disclosure. [Figure 6] Figure 6 is a block diagram of a dynamic motion planning system and control device for a redundant robot, which includes collision avoidance in its motion plan, according to a first embodiment of the present disclosure. [Figure 7] Figure 7 is a block diagram of a dynamic motion planning system and control device for a redundant robot, which includes collision avoidance in its motion plan, according to a second embodiment of the present disclosure. [Figure 8] Figure 8 is a graph relating to one embodiment of the present disclosure, showing the robot tool center point path in three-dimensional space and the modification of the robot elbow joint trace necessary to avoid static obstacles in the workspace. [Figure 9A] Figures 9A, 9B, and 9C illustrate a robot in a workspace with fixed overhead obstacles, including motion planning results with and without the technology of this disclosure. [Figure 9B] Figures 9A, 9B, and 9C illustrate a robot in a workspace with fixed overhead obstacles, including motion planning results with and without the technology of this disclosure. [Figure 9C] Figures 9A, 9B, and 9C illustrate a robot in a workspace with fixed overhead obstacles, including motion planning results with and without the technology of this disclosure. [Figure 10A] Figures 10A, 10B, and 10C show two robots operating together in a workspace, including the results of motion planning with and without the technology of this disclosure. [Figure 10B] Figures 10A, 10B, and 10C show two robots operating together in a workspace, including the results of motion planning with and without the technology of this disclosure. [Figure 10C] Figures 10A, 10B, and 10C show two robots operating together in a workspace, including the results of motion planning with and without the technology of this disclosure. [Modes for carrying out the invention]
[0010] The following considerations relating to embodiments of the present disclosure concerning dynamic motion planning and control of redundant robots are essentially illustrative and are not intended in any way to limit the disclosed apparatus and technology or their applications or uses.
[0011] Industrial robots are well known to be used in a variety of tasks, including manufacturing, assembly, and material handling. In some applications, robots have more degrees of freedom than the number of degrees of freedom of the task being performed. For example, a 7-axis robot might be used in a spray painting application where the position and orientation of the spray nozzle are fully defined by 6 degrees of freedom.
[0012] Furthermore, many robotic work environments contain obstacles that can sometimes enter the robot's path. This means that without adaptive motion planning, parts of the robot may collide with obstacles as it moves from its current position to its destination. Obstacles can be static structures such as equipment or tables, or dynamic (moving) objects such as people, forklifts, or other machinery. If dynamic objects are present, the robot's movements must be planned in real time for each control cycle.
[0013] Several technologies have been developed in the field of computing the motion of robots with redundant degrees of freedom. However, these technologies have various drawbacks, such as the need to define artificial constraints which limits the robot's flexibility, limitations on the optimization objective function and constraints which prevent sufficient control over the solution, and the fact that they can only be used in teaching mode and cannot be used for real-time motion planning, and as a result, they cannot be used for applications such as dynamic obstacle avoidance or line tracking.
[0014] The dynamic motion planning system of this disclosure overcomes the shortcomings of prior art systems by formulating the motion plan as a quadratic programming optimization calculation with a multi-element objective function and collision avoidance constraint function. This technology makes it possible to create a joint motion plan that satisfies dynamic object collision avoidance constraints while minimizing deviations from the planned path, balancing joint velocities, and avoiding changes in robot configuration / pose along the path. The optimization calculation is sufficiently efficient to be performed in real time during robot operation, and furthermore, the calculation is formulated so that the weighting coefficients for the objective function terms can be changed for each control cycle calculation based on the obstacle proximity situation at that time.
[0015] Figure 1 is a block diagram of a dynamic motion planning system for a redundant robot, which includes collision avoidance in its motion plan, according to one embodiment of the present disclosure. The planner module 110 calculates the planned robot motion based on the input target (destination) position. In one non-limiting embodiment, the robot tool is a gripper, and the robot's task is to move a part from its original (start) position to a target position. Naturally, there are many other examples of tasks, such as moving from one spot weld location to the next, or moving to a new location to begin spraying paint. In one embodiment, the planned robot motion u des is the acceleration vector that defines the "designed" (planned) robot motion in orthogonal space. Specifically, u des This can be defined as the tool's center point acceleration.
[0016]
number
[0017]
number
[0018] The input target (destination) position may be a moving target, such as a vehicle moving on a conveyor belt. In this type of "line tracking" application, the target position may be defined as a linear function of time based on the conveyor speed, or it may be calculated for each control cycle in applications where the conveyor speed may change. In either case, the target position of the robot tool is the position where the tool must be at a future point in time when the robot arm is expected to arrive.
[0019] Furthermore, the dynamic motion optimization module 120 receives obstacle data input from the perception module 130. The perception module 130 includes one or more cameras or sensors configured to provide data about obstacles that may be present in the robot's workspace. The obstacle data typically includes the minimum distance between robot obstacles and may also include other data about the position (including spatial shape) and velocity of any of the obstacles.
[0020] The dynamic motion optimization module 120 optimizes the robot's motion u des An optimization calculation is performed to minimize the tracking deviation from the target, and further, the robot's motion characteristics are optimized while including the robot's mechanical limitations and collision avoidance safety function as constraints. The result of this optimization calculation is the commanded robot motion q. cmd The commanded robot action q cmd This refers to the robot's movement in articulated space, guiding the robot tool to the target position while avoiding all obstacles within the robot's workspace. The optimization calculation is described in detail below. The feedback loop 140 receives the commanded robot movement q from the dynamic motion optimization module 120. cmd This is fed back to the planning module 110. The planning module 110 and the dynamic motion optimization module 120 repeat the above calculations for each robot control cycle.
[0021] Also, as is known in the art, the dynamic motion optimization module 120 provides the commanded robot motion q cmd to the robot control device, and the robot control device provides robot control commands to a robot (not shown), and the dynamic motion optimization module 120 receives the actual robot joint position q act in a feedback loop. The robot control device updates the robot control commands for each control cycle based on the actual robot joint position q act and the commanded robot motion q cmd . This will be further discussed below.
[0022] Box 150 shows how the aforementioned robot redundancy affects the system of FIG. 1. The operation u des of the tool center point is an element of the set R of real numbers having m dimensions. The robot joint motion vector q cmd is an element of the set of real numbers having n dimensions, and n > m. The redundancy of the DOF is (n - m). As described above, consider the case where a 7DOF robot performs a 6DOF task. In this case, n = 7, m = 6, and the redundancy is 1 degree of freedom (7 - 6). This redundant 1 degree of freedom allows the optimization solver to have some margin in finding a robot motion solution that efficiently moves the tool center point to the target position, ensures collision avoidance, and can also satisfy other objectives such as the "smoothness" of the robot motion described below.
[0023] Box 160 shows the basic formulation of the optimization calculation used in the dynamic motion optimization module 120. The optimization calculation includes an objective function 170 for minimizing a combination of some characteristics of the commanded robot motion. The objective function 170 will be described in detail below. The optimization calculation also includes at least one inequality constraint 180 corresponding to the structural / mechanical limitations of the robot. The inequality constraint 180 as shown is the robot joint speed
[0024]
Number
[0025]
number
[0026] Furthermore, the optimization calculation shown in box 160 includes a collision avoidance safety constraint 190. The safety constraint 190 is an embodiment of a collision avoidance inequality constraint that ensures the robot does not collide with any obstacles present in the robot workspace. The safety constraint 190 employs a model that defines a safety function h(X) based on the minimum distance between robot obstacles or a combination of the minimum distance between robot obstacles and the relative velocity between robot obstacles. The value of the safety function h(X) is equal to the minimum distance between robot obstacles when the robot and obstacles are moving away from each other, and equal to the minimum distance minus the relative velocity term when the robot and obstacles are moving towards each other. The inequality safety constraint 190 is as shown in Figure 1.
[0027]
number
[0028]
number
[0029] A complete consideration of the use of safety constraint 190 in a motion planning system based on safety constraint 190 and collision avoidance optimization is disclosed in U.S. Patent Application No. 17 / xxx,yyy, entitled "DYNAMIC MOTION PLANNING SYSTEM," filed on NN November 2021 and assigned together with this application, which is incorporated herein by reference in its entirety. Any other suitable type of collision avoidance safety constraint, including the use of a safety function based solely on the minimum distance between robot obstacles, may be employed in the optimization calculation of box 160.
[0030] Returning to the objective function 170 in Figure 1, we can see that it contains three terms: the tool center point tracking term 172, the joint velocity regularization term 174, and the posture regularization term 176. Terms 172, 174, and 176 are the squares of the norms of vectors representing different characteristics of the robot's motion.
[0031]
number
[0032]
number
[0033] In one embodiment, λ2||q ref -q|| 2 The posture regularization term 176, calculated as follows, is the reference posture (reference position vector) q of the robot posture (joint position vector) q ref This represents the deviation from the reference position. λ² is another weighting coefficient (which can be changed as described later). When the robot moves from the starting position to the target (destination) position, it is desirable for the robot to maintain a constant posture (overall configuration of joint and arm positions) such that the joint angles change slightly from one time process (control cycle) to the next. In other words, it is undesirable for one or more joints to suddenly "flip" or reverse position when moving from one process to the next. The posture regularization term 176 of the objective function 170 attempts 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 the posture regularization term will be described later.
[0034] A beneficial feature of the optimization calculation shown in box 160 of Figure 1 is that the weighting coefficients λ1 and λ2 in the objective function 170 can be changed "during execution" to achieve the desired robot behavior. That is, the values of λ1 and λ2 can be adaptively set for each control cycle calculation of the dynamic motion optimization module 120. For example, if there are no obstacles in the workspace, or if any obstacles are far from the robot, it is desirable to maintain a favorable robot posture while minimizing path deviation. In this case, the value of λ1 can be decreased while the value of λ2 can be increased (to emphasize the effect of the posture regularization term 176 of the objective function 170). On the other hand, if there are obstacles close to the robot and collision avoidance maneuvers are likely to be necessary, it may not be possible to maintain a favorable robot posture. In this case, it is conceivable to increase the value of λ1 while decreasing the value of λ2 (to suppress the emphasis of the effect of the posture regularization term 176 of the objective function 170). Other factors may be considered when determining the values of the weighting coefficients λ1 and λ2 in the objective function 170.
[0035] Figure 2 is a graph 200 showing two different models for the adaptive adjustment of the weighting coefficients λ1 and λ2 of the objective function 170 used in the dynamic motion optimization module 120 of Figure 1. Graph 200 plots the values of the weighting coefficient λ1 on the vertical axis 210 and the minimum distance between robot obstacles on the horizontal axis 220. The axes 210 and 220 are not labeled with specific values because their values can be adjusted to obtain the desired results using the concepts shown in graph 200.
[0036] Step function curve 230 illustrates a first technique that can be used to adaptively adjust the weighting coefficients λ1 and λ2 in the objective function 170 based on the minimum distance between robot obstacles. When the minimum distance between robot obstacles is small (left side of graph 200, i.e., when collision avoidance maneuvers are likely), λ1 is set to a high value (e.g., 1.0). The corresponding λ2 is set to a low value (e.g., 0.0). By combining the values of the weighting coefficients in the objective function 170 in this way, the optimization calculation will converge to a solution that prioritizes velocity regularization while minimizing tracking error and giving only slight consideration to attitude regularization.
[0037] When the minimum distance between robot obstacles is large, i.e., when the minimum distance exceeds a threshold such as 0.5m (right side of graph 200, i.e., when the probability of collision avoidance maneuvers is low), λ1 is set to a low value (e.g., 0.0). The corresponding λ2 is set to a high value (e.g., 1.0). By combining the weighting coefficient values of the objective function 170 in this way, the optimization calculation will converge to a solution that prioritizes attitude regularization while minimizing tracking error and considering velocity regularization to a small extent. If no obstacle is detected by the perception system 130, this is treated the same as when the minimum distance between robot obstacles is large (λ1 is set to a low value and λ2 is set to a high value). The transition from a high λ1 value to a low λ1 value in the step function curve 230 does not have to be a vertical line; for example, it may be a line with a negative slope.
[0038] The continuous curve 240 demonstrates a second technique that can be used to adaptively adjust the weighting coefficients λ1 and λ2 in the objective function 170 based on the minimum distance between robot obstacles. The continuous curve 240 sets λ1 to a high value when the minimum distance between robot obstacles is small, and to a low value when the minimum distance between robot obstacles is large. However, unlike the step function curve 230, the continuous curve 240 achieves a smooth transition from high to low values of λ1. The continuous curve 240 can be modeled, for example, as an algebraic function (such as a cubic or quintic function) or a trigonometric function (such as a cosine function). In some embodiments, after the continuous curve 240 or the step function curve 230 is used to determine the value of λ1, the value of λ2 may be determined by an equation in which the sum of λ1 and λ2 is equal to a constant, where λ1 is higher when λ2 is lower, and vice versa.
[0039] Here again, in Graph 200, the graph mid-distance or threshold distance may be a value other than 0.5m, and the maximum and minimum values of the weighting coefficients λ1 and λ2 may not be 1.0 and 0.0, respectively. For example, the value of the minimum weighting coefficient can be made slightly greater than 0 (e.g., 0.1) so that all terms of the objective function 170 are always considered. Also, the value of the maximum weighting coefficient may be higher or lower than 1.0, but this will affect the balance between the path-following deviation term 172 and the velocity and attitude regularization terms 174 and 176. The exact adaptive model used (one of the two shown in Graph 200, or any other that may be assumed), along with the values of λ1 and λ2 and the threshold minimum distance, may be selected to provide the best results for a particular application. In any case, the optimization formula shown in Box 160, with its objective function 170 featuring adaptive weighting for each calculation cycle, provides robust performance for real-time collision avoidance motion planning of robots with redundant degrees of freedom.
[0040] In objective function 170, the attitude regularization term 176(λ²||q ref -q|| 2 ) are the joint position vector q and the reference position vector q refThe aim is to provide the desired robot behavior by minimizing the norm of the difference between the reference position vector q. ref It is necessary to give it.
[0041] Figure 3 shows the reference position vector q used in the dynamic motion optimization module 120 of Figure 1. ref Two different techniques for achieving this are illustrated. The robot is required to move the tool center point from the starting point 310 (or P1) to the target point 312 (or P2). Each of points 310 and 312 (P1 and P2) is defined in orthogonal 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 points 310 and 312 (P1 and P2) in orthogonal space has a corresponding robot position in articulation space. The articulation position vector 320 (or q1) corresponds to the tool center point 310 (P1), and the articulation position vector 322 (or q2) corresponds to the tool center point 310 (P1). Each of the joint position vectors 320 and 322 (q1 and q2) is defined in joint space by six joint position degrees of freedom (J1, ..., J6) and an extra degree of freedom (E1).
[0042] Figure 3 shows the reference position vector q as the robot moves the tool center point from P1 to P2. ref Two different techniques are shown that provide this. In the first technique 300 (upper half of Figure 3), interpolation in joint space is performed in box 302. Using technique 300, the starting joint vector q1 is calculated from the starting tool center point P1, and the target joint vector q2 is calculated from the target tool center point P2. Then, interpolation between q1 and q2 is performed in joint space to obtain a set of points qi(330) in joint space that define a curved path in the space between tool center points P1 and P2. As will be understood by those skilled in the art, the most efficient robot joint motion for moving a tool center from one point to another is usually such that the tool center follows a curved path. In the case of technique 300, the reference position vector q when the robot moves the tool center from P1 to P2ref It is defined as (q1, ..., qi, ..., q2) using the joint vector qi(330).
[0043] In the second technique 350 (lower half of Figure 3), interpolation in orthogonal space is performed in box 352. Using technique 350, first, in box 352, the distance between the starting tool center point P1 and the target tool center point P2 is interpolated with a set of points Pi(360), obtaining a linear path in orthogonal space from P1 to P2. Next, the set of points Pi is transformed into a set of points qi(370) in joint space using inverse kinematics (IK) calculation in box 354. In technique 350, the reference position vector q when the robot moves the tool center point from P1 to P2 is... ref It is defined as (q1, ..., qi, ..., q2) using the joint vector qi(370).
[0044] In objective function 170, the attitude regularization term 176(λ²||q) described above is used. ref -q|| 2 ) are the joint position vector q and the reference position vector q ref The aim is to provide the desired behavior of the robot by minimizing the norm of the difference between the two. The following is a consideration of other techniques that may be used for attitude regularization in the objective function 170 of the optimization calculation.
[0045] Simply put, the purpose of the posture regularization term in the objective function is to maintain a constant overall posture as the robot moves a tool from a starting point to a target point. "Consistent overall posture" can also be expressed as avoiding sudden, large jumps or changes in the position of any joint and / or arm from one time step to the next, and instead moving smoothly within a range. A simple example of a sudden change in configuration or posture often seen in robot motion planning is when the wrist joint (at the end of the outer arm) "flipped" to a reversed position from one time step to the next in order to position the tool in the desired direction.
[0046] Figures 4A and 4B are simplified diagrams illustrating another example of how the posture or configuration of a robot changes. Robot 400 includes a fixed pedestal 410, a base 420 connected to the pedestal 410 via a vertical axis joint 412, an arm 430 connected to the base 420 via a joint 422, an arm 440 connected to the arm 430 via a joint 432, an arm 450 connected to the arm 440 via a joint 442, and a tool 460 connected to the arm 450 via a joint 452. The tool center point 462 is defined on the tool 460. Robot 400 is for illustrative purposes only and is depicted for the purposes of this study. Other robots may include different numbers of arms and joints, and joints that allow one arm to rotate axially relative to other arms.
[0047] In Figure 4A, the robot 400 is shown in a standard posture or configuration. Arm 430 is oriented substantially horizontally, and arm 440 is oriented substantially vertically, with joint position 432 at approximately 90°. The standard posture shown in Figure 4A can be considered a "down" posture because joint 432 is positioned considerably below joint 442.
[0048] In Figure 4B, the robot 400 is shown in an alternative posture or configuration. Arm 430 is oriented substantially vertically, and arm 440 is oriented substantially horizontally, with joint 432 at approximately -90°. The alternative posture shown in Figure 4B can be considered an "up" posture because joint 432 (the extra joint of the inner arm) is located considerably above joint 422. Despite the tool center point 462 being in exactly the same position in Figures 4A and 4B, the robot 400 can assume two very different postures or configurations that result in this tool center point position.
[0049] As described above, the purpose of the posture regularization term in the objective function is to maintain a constant overall posture as the robot moves the tool from the starting point to the target point. Regarding Figures 4A and 4B, it is undesirable for the robot 400 to switch from a "down" posture (Figure 4A) to an "up" posture (Figure 4B) midway through the tool movement. The posture regularization term included in the objective function is designed to prevent such mid-movement posture changes. One embodiment of the posture regularization term (proposed joint position and q) ref The comparison with the above is as described above. Another embodiment of the posture regularization term will be discussed later.
[0050] Furthermore, posture consistency can also be achieved by first calculating the destination configuration (the robot's posture or configuration when the tool's center point is at the target position) and then individually evaluating the joint angles for joints that are prone to significant posture changes during robot operation (for example, "reversal" of the wrist joint, "up" vs. "down" of the medial joint of the arm, etc.). As mentioned earlier, instead of formulating a posture regularization term in the objective function, (λ²||q ref -q|| 2 )), a cost function that penalizes solutions where the posture changes significantly from the destination configuration can be used as the posture regularization term. One method for constructing such a cost function is to detect the sign change of the position of any joint that is prone to large changes in posture. As described above with respect to Figures 4A and 4B, if the elbow joint has a position angle of approximately 90° in the destination (target tool center point) configuration, when the optimization calculation evaluates the solution vector for a time step in which the elbow joint has a position angle of approximately -90°, that solution vector will be heavily penalized by the cost function. By using the proposed dot product of the joint position and the joint's destination, the sign change can be detected and the cost function can be penalized.
[0051] Another type of posture regularization term can be constructed using a cost function that evaluates the position of each joint relative to the joint position at the time of destination configuration, but uses the change in angle rather than the change in sign. The posture regularization term of such a cost function is given by equation (1) below.
[0052]
number
[0053] Both of the posture regularization formulations described above have been shown to yield good results in detailed mathematical simulations of real-time robot motion planning when obstacles are present in the workspace. The results are shown in the following figures and discussed further.
[0054] Figure 5 is a flowchart 500 of a method for dynamic robot motion planning and control for a redundant robot according to one embodiment of the present disclosure. In box 502, the planned robot motion is calculated based on the tool center point position of the target or destination. In a typical embodiment, the planned robot motion is the tool center point acceleration vector in orthogonal space, and the "designed" (planned) robot motion u moves the tool center point to the target position. desThis defines the obstacles in the workspace provided by the perception module in box 504. The perception module includes at least one camera or sensor capable of detecting the location of any obstacles present in the workspace, such as a three-dimensional (3D) camera. The perception module may include an image processing device that calculates obstacle position data from camera images, or the perception module may simply provide raw camera images to a computer or control device that performs robot motion optimization calculations. The obstacle position data is preferably calculated in a workspace coordinate frame that can be easily compared with the robot position data. The final values to be obtained from the obstacle data are the minimum distance between robot obstacles and the relative velocity between robot obstacles.
[0055] In box 506, robot motion optimization calculations are performed based on the planned robot motion and obstacle data. The output of the robot motion optimization calculation is the commanded robot motion q as described above in relation to Figure 1. cmd Therefore, if there are no obstacles in the workspace, the commanded robot motion moves the tool center point according to the planned robot motion. The commanded robot motion is fed to box 502 in a feedback loop, and the planned robot motion is recalculated at each control cycle based on the target tool center point position and the commanded robot motion (which may have been modified by optimization to avoid any obstacles).
[0056] In box 508, the robot control unit provides the robot with the commanded robot movements. The robot control unit may perform calculations or transformations to provide the robot with appropriate robot joint movement commands. In box 510, the robot actually moves based on the joint movement commands from the control unit. The robot and control unit operate as a closed-loop feedback control system, and the actual robot state q act The joint position and velocity are fed back to the control unit, and the latest joint command is calculated. The robot and control unit operate in a control cycle with a specified time (i.e., a certain number of milliseconds).
[0057] In box 506, the operation optimization problem can be formulated as follows:
[0058]
number
[0059]
number
[0060]
number
[0061]
number
[0062]
number
[0063] The attitude regularization term (λ²||q) shown in equation (2) ref -q|| 2 ) can be replaced with a cost function attitude regularization term as shown in equation (1) above.
[0064] The obstacle avoidance constraint (Equation (4)) aims to maintain the safety function h(X)≧0, as previously mentioned with respect to Figure 1. The formulation of the obstacle avoidance constraint (Equation (4)) and the safety function h(X) is described in detail in the aforementioned cited specification No. 17 / xxx,yyy. Furthermore, as previously mentioned, a different safety function based solely on the minimum distance between robot obstacles may be used.
[0065] The optimization calculation using equations (2) to (4) is performed for each robot control cycle. The optimization calculation determines the commanded robot motion q, which represents the robot motion that minimizes the combination of objective function terms (tracking error, velocity regularization, and attitude regularization) while satisfying the inequality constraints. cmd It converges to . The weighting coefficients of the regularization terms (λ1 and λ2 or M) of the objective function may be adjusted for each optimization calculation (each control cycle) based on the obstacle conditions in the workspace, as described above.
[0066] In Figure 5, the calculation of the planned robot motion in box 502, the calculation of robot motion optimization in box 506, and the calculation of robot joint motion commands in box 508 may all be performed by a robot control unit that communicates with the robot in real time. Alternatively, the calculations in boxes 502 and 506 may be performed on a separate computer, and the robot motion commanded in box 508 for each control cycle may be provided to the control unit. Alternative implementations of these two hardware components are shown in the following two figures and discussed below.
[0067] Figure 6 is a block diagram of a dynamic motion planning system and control device for a redundant robot, which includes collision avoidance in its motion planning, according to a first embodiment of the present disclosure. In this embodiment, another computer 600, including a processor and memory, performs all calculations for the planner module 110 and the dynamic motion optimization module 120 in Figure 1. The perception module 130 communicates with the other computer 600 and provides obstacle data to the other computer 600 in the form of camera images and / or sensor data for use by the dynamic motion optimization module 120.
[0068] Another computer 600 communicates with the robot system 602, which includes the robot control device 650 and the robot 660. Specifically, it communicates with the commanded robot action q. cmd This is provided from the dynamic motion optimization module 120 to the control unit 650. The control unit 650 controls the movement of the robot 660 in a real-time control system that operates with a defined control period (a certain number of milliseconds), and the actual state of the robot q act The joint position and velocity are fed back to the control device 650 via the feedback loop 670. In the system shown in Figure 6, the other computer 600 may be a dedicated computer specifically for the robot system 600, or the other computer 600 may perform motion optimization calculations for multiple robot systems.
[0069] Figure 7 is a block diagram of a dynamic motion planning system and control device for a redundant robot, including collision avoidance in its motion plan, according to a second embodiment of the present disclosure. In this embodiment, there is no separate computer, and all motion optimization calculations and control of the redundant robot are self-contained within the robot system 700 itself. That is, calculations in the planner module 110, the dynamic motion optimization module 120, and the robot control device module 650 are all performed on a conventional robot control device (having a processor and memory) that communicates with the robot 660. The perception module 130 provides obstacle data to the robot system 700 in the form of camera images and / or sensor data for use by the dynamic motion optimization module 120. The control device module 650 controls the motion of the robot 660 in a real-time control system that operates at a defined control cycle, and the actual robot state q act The joint position and velocity are fed back to the control module 650 and the planning module 110 via the feedback loop 770.
[0070] The system in Figure 7 has the advantage of not requiring a separate computer 600 because all robot motion planning, optimization, and control calculations are performed on the robot control unit. On the other hand, the system in Figure 6 has the advantage of speeding up motion optimization calculations because the optimization calculations can be performed on a separate processor that is isolated from the real-time robot control unit. In either hardware implementation, motion optimization calculations must be performed at each robot feedback control cycle based on the robot configuration and obstacle data at that time in order to ensure collision avoidance from all dynamic obstacles and to provide optimal motion characteristics for redundant robots.
[0071] The dynamic motion planning techniques shown in Figures 1-7 demonstrate that reliable motion planning results, including obstacle avoidance, can be obtained in simulations of robot systems with redundant degrees of freedom. This includes effective motion planning that minimizes path deviations, maintains desirable joint velocities and robot posture, avoids all obstacles in the workspace, rapidly calculates safety functions, and optimizes the resulting motion.
[0072] Figure 8 is a graph relating to an embodiment of the present disclosure, showing the robot tool center point path in three-dimensional space and the modification of the robot's elbow joint path required to avoid static obstacles in the workspace. The workspace 800 is represented as a three-dimensional space with orthogonal X, Y, and Z axes as shown in the figure. A robot (not shown) operates within the workspace 800 and is required to perform a task related to moving the tool center point from a starting point 810 to a target (destination) point 812. The tool center point follows a TCP trajectory 820 from the starting point 810 to the destination point 812. Figure 8 shows data collected from simulations of a specific robot and control device using the redundant robot dynamic path planning technique of the present disclosure.
[0073] Figure 8 shows the robot's elbow joint point 830 in addition to the tool center point. Figure 8 depicts two scenarios of robot movement (elbow joint movement). In the first scenario, there are no obstacles in the workspace 800. When no obstacles are present, the robot moves the elbow joint point 830 along the elbow reference trajectory 840, while moving the tool center point from the starting point 810 along the TCP trace 820 to the target point 812.
[0074] In the second scenario, a fixed spherical obstacle 850 is positioned in the workspace 800 to obstruct the elbow reference trace 840. A buffer zone 860 defines a safe distance margin around the obstacle 850, and all parts of the robot should remain outside the buffer zone 860. Simulations of the second scenario were performed using the same tool center start point 810 and target point 812, but with the obstacle 850 present. The motion plan for the obstacle-avoiding robot was calculated using the dynamic path planning technique of this disclosure. Because the objective function has a tracking deviation term, when the obstacle 850 is in the workspace, the tool center point follows essentially the exact same TCP trace 820 as when there is no obstacle. However, when the obstacle 850 is present, the elbow joint point 830 follows a much different path, which is shown as the actual elbow trajectory 870. While the robot moves the tool center point along the desired path, the elbow trace 870 keeps the elbow joint point 830 and the inner robot arm outside the buffer zone 860. These simulation results confirmed the behavior expected from the dynamic motion planning calculation described above. Specifically, it was confirmed that the safety constraints for collision avoidance were satisfied while converging to a solution that produced almost no path deviation at the tool's center point.
[0075] Figures 9A, 9B, and 9C illustrate a robot in a workspace with a fixed overhead obstacle, including motion planning results with and without the technology of this disclosure. Figures 9A-9C illustrate another embodiment of collision avoidance from stationary obstacles by a redundant robot.
[0076] Robot 900 operates in a workspace with an overhead, fixed obstacle 910, which is illustrated as a ceiling angled downwards such that the right side of the workspace is lower in height. Robot 900 is required to perform tasks related to moving a tool from a starting point to a target (objective) point. Robot 900 is illustrated in the starting point configuration of Figure 9A.
[0077] Figure 9B shows robot 900 with a destination point configuration and represents simulation data of the robot and control unit using conventional path planning techniques for redundant robots, which cannot consider collision avoidance for parts of the robot other than the tool center point. In Figure 9B, robot 900 moved the tool center point from the starting point to the destination point along the tool center point trace 920 as instructed. However, the elbow joint of robot 900 collides with the overhead obstacle 910 below the ceiling, as shown by 930. This is clearly not acceptable robot behavior and illustrates the limitations of conventional redundant robot path planning, which only considers collisions of the tool center point with obstacles.
[0078] Figure 9C shows robot 900 in a target point configuration and represents data from simulations of the robot and control device using the redundant robot dynamic path planning technique of the Disclosure, including collision avoidance and motion characteristic quality in optimization calculations. In Figure 9C, robot 900 moved the tool center point along trace 940 which is substantially the same as trace 920. However, robot 900 flexes several arm joints to lower the position of the elbow joint so that the elbow joint is sufficiently separated from the overhead obstacle 910 below the ceiling, as shown in 950. This is clearly a better robot motion than that shown in Figure 9B and demonstrates the performance of the motion planning technique of the Disclosure, which uses redundant degrees of freedom of the robot to provide a desired tool path while ensuring collision-free robot motion.
[0079] Figures 10A, 10B, and 10C illustrate two robots operating in conjunction within a workspace, including the results of their operation plans with and without the technology of this disclosure. Figures 10A to 10C show an example of collision avoidance from dynamic obstacles using redundant robots. In this case, each robot becomes a potential obstacle that hinders the operation of the other robot.
[0080] Robots 1000 and 1010 are required to operate in the workspace, and each of them is required to perform tasks simultaneously. Robot 1000 is required to perform tasks related to moving a tool from a starting point 1002 to a target (destination) point (not shown). The tool center point of robot 1000 follows trace 1004 from the starting point to the destination point. Robot 1010 is required to perform tasks related to moving a tool from a starting point 1012 to a target (destination) point (not shown). The tool center point of robot 1010 follows trace 1014 from the starting point to the destination point. The tool center point traces 1004 and 1014 are shown in their entirety in Figure 10C, which will be described later.
[0081] Figure 10A shows the operation of robots 1000 and 1010 planned using a conventional method that does not include dynamic object collision avoidance. In Figure 10A, robots 1000 and 1010 are illustrated in configurations that are approximately intermediate in the operation of the task being performed. In this configuration, the tool of robot 1000 collides with the elbow joint of robot 1010, as indicated by arrow 1020. Any collision is not an acceptable outcome.
[0082] Figure 10B shows the operation of robots 1000 and 1010, planned using the redundant robot dynamic path planning technique of this disclosure, with optimization calculations including collision avoidance and operational characteristic quality. In Figure 10B, robots 1000 and 1010 are also illustrated in configurations approximately midway through the operation of the work being performed. However, in this embodiment, the tool of robot 1000 separates as indicated by arrow 1030 and does not collide with the elbow joint of robot 1010. This separation is made possible by robot 1010 flexing the extra joint of its inner arm as indicated by arrow 1032, resulting in the elbow joint being pulled slightly inward toward the base of the robot.
[0083] Figure 10C includes a bar drawing of the final configurations of robots 1000 and 1010, where the tool of robot 1000 is located at the destination point 1006, the endpoint of the tool center point trace 1004, and the tool of robot 1010 is located at the destination point 1016, the endpoint of the tool center point trace 1014. Also in Figure 10C is the elbow joint trace 1040 of robot 1010 for a motion planned in the prior art (Figure 10A). For the motion planned in the art of this disclosure (Figure 10B), the elbow joint of robot 1010 follows the same trace 1040 except in the area indicated by arrow 1042. As indicated by arrow 1042, the elbow joint of robot 1010 sinks downward and inward along the path to separate, as shown in Figure 10B, to avoid a collision.
[0084] The technology disclosed herein is noteworthy for its ability to avoid robot-to-robot collisions without causing deviations in the tool center point path of either robot 1000 or robot 1010. This is made possible by the redundant degrees of freedom of the robots and the aforementioned motion optimization calculations. Specifically, a weighted objective function can be used to find a solution that minimizes the deviation of the tool path while satisfying collision avoidance constraints.
[0085] Other simulations of the two robots' workspaces (similar to Figures 10A-10C) were also performed. In the first simulation, the attitude regularization term of the objective function was deactivated, and in the second simulation, the attitude regularization formula (λ²||q) was based on the reference attitude. ref -q|| 2 In the first simulation, the attitude regularization term of the objective function was activated using the ) formula, and in the third simulation, the attitude regularization term of the objective function was activated using the cost function attitude regularization formulation (equation (1) above). These simulations confirmed that an objective function using either adaptive weighting or the proposed attitude regularization term is effective in enabling the robot to maintain a desirable attitude during operation while avoiding collisions.
[0086] The results shown in Figures 8 to 10 demonstrate the effectiveness of the disclosed motion planning technology for redundant robots, including collision avoidance with static and dynamic obstacles, adherence to predetermined tool center point paths, and favorable robot motion characteristics (joint velocity and robot posture).
[0087] Furthermore, the time required for commanded motion calculations per control cycle (the time required to provide output from the dynamic motion optimization module 120) was measured to be less than 1 millisecond (ms) on average for the disclosed technology. The technology of this disclosure performs motion planning calculations at more than sufficient speed while providing significantly better collision avoidance and motion smoothness results compared to prior art motion calculation methods, when using a typical robot control cycle of 8 ms.
[0088] Throughout the discussion so far, various computers and control devices have been described and suggested. It is understood that the software applications and modules of these computers and control devices run on one or more electronic computing devices having processors and memory modules. In particular, this includes the processors in each of the robot control device 650 and optionally another computer 600 shown in Figures 6 and 7 above. Specifically, the processors in the control device 650 and / or the other computer 600 are configured to run the redundant robot collision avoidance motion planning function in box 506 (shown in Figure 5 and Figure 1) together with the robot feedback motion control function in box 508.
[0089] Many exemplary embodiments and models relating to methods and systems for dynamic motion planning and control of redundant robots have been described above, and those skilled in the art will be able to recognize variations, rearrangements, additions, and partial combinations thereof. Accordingly, the following appendices and the claims introduced below are intended to be construed as including all such variations, rearrangements, additions, and partial combinations that are in the true spirit and scope of the claims.
Claims
1. A method for planning the dynamic motion of a robot, wherein the method is executed on a computing device, Calculating the design robot motion based on the target position of the robot tool's center point, The system calculates the commanded robot motion based on the designed robot motion and obstacle data received from sensors, and the commanded robot motion is given as feedback for calculating the designed robot motion for the next control cycle, the calculation of the commanded robot motion includes an optimization calculation with a multi-item function whose coefficients can be adjusted for each control cycle, one or more robot structure constraints, and collision avoidance constraints, and This includes providing the commanded robot motion to a control module that controls the joint motion of the robot, A method wherein the multi-item function includes a path deviation minimization term relating to the deviation between the path in the designed robot motion and the path in the commanded robot motion, a joint velocity regularization term, and a robot posture regularization term, and the multi-item function is configured such that the sum of the terms is minimized.
2. The method according to claim 1, wherein the computing device is a robot control device that performs all of the following: calculation of the designed robot motion, calculation of the commanded robot motion, and control of the joint motion of the robot.
3. The method according to claim 1, wherein the computing device includes a computer that performs calculations of the designed robot motion and the commanded robot motion, and the computer provides the commanded robot motion to a robot control device that controls the joint motion of the robot.
4. The method according to claim 1, characterized in that the coefficient of the term of the multi-item function is adjustable for each control cycle based on the degree of proximity of the robot to any obstacles in the robot's workspace.
5. The method according to claim 1, wherein the joint velocity regularization term includes the square of the norm of the joint velocity vectors including all joints of the robot.
6. The method according to claim 1, wherein the robot posture regularization term includes, for all joints of the robot, the square of the norm of a vector defined by the difference between the joint position and the reference joint position.
7. The method according to claim 1, wherein the robot posture regularization term is a cost function that includes a norm of a vector defined by the difference between the joint position and the target joint position, with respect to one or more joints of the robot that are predetermined to be susceptible to large posture changes from one process to the next during robot operation.
8. The method according to claim 1, wherein the one or more robot structure constraints include an inequality constraint that defines one or more of the following: maintaining the robot joint position within a predetermined joint position range, maintaining the robot joint velocity below a predetermined joint velocity limit, and maintaining the robot joint acceleration below a predetermined joint acceleration limit.
9. The collision avoidance constraint is an inequality constraint calculated from a safety function, and the collision avoidance constraint is such that the rate of change of the safety function must be greater than or equal to a negative value obtained by multiplying the safety function by a coefficient.
10. The method according to claim 9, wherein the safety function is equal to the minimum distance between robot obstacles, or the safety function is equal to the minimum distance between robot obstacles minus a term calculated from the relative velocity between robot obstacles.
11. The method according to claim 1, wherein the target position is a moving target position defined as a function of time.
12. The method according to claim 1, wherein the design robot motion is a tool center point motion in orthogonal space, and the commanded robot motion includes robot joint rotation motion for all joints of the robot.
13. The method according to claim 12, wherein the robot has at least one redundant degree of freedom, and the number of degrees of freedom of the robot joint rotation motion exceeds the number of degrees of freedom of the tool center point motion.
14. A method for planning the dynamic motion of a robot, wherein the method is executed on a computing device, Calculating the design robot motion based on the target position of the robot tool's center point, The calculation of the commanded robot motion based on the design robot motion and obstacle data received from sensors, the commanded robot motion being provided as feedback for calculating the design robot motion for the next control cycle, the calculation of the commanded robot motion including an optimization calculation, the optimization calculation having a multi-item function whose coefficients can be adjusted for each control cycle, robot joint position, velocity and acceleration limits defined as inequality constraints, and collision avoidance constraints, and This includes providing the commanded robot motion to a control module that controls the joint motion of the robot, The design robot motion is a tool center point motion in orthogonal space, the commanded robot motion includes robot joint rotation motion of all joints of the robot, and the robot has at least one redundant degree of freedom such that the number of degrees of freedom of the robot joint rotation motion exceeds the number of degrees of freedom of the tool center point motion. A method wherein the multi-item function includes a path deviation minimization term relating to the deviation between the path in the designed robot motion and the path in the commanded robot motion, a joint velocity regularization term, and a robot posture regularization term, wherein the multi-item function is configured such that the sum of the terms is minimized, and the coefficients of the terms of the multi-item function are adjustable for each control cycle based on the degree of proximity of the robot to any obstacles in the robot workspace.
15. A dynamic motion planning system for an industrial robot, wherein the system is: A perception module including at least one sensor or camera configured to detect obstacles within the workspace of the industrial robot, and A computing device comprising a processor and memory, The aforementioned computing device A planning module programmed to calculate the design robot motion based on the target position of the tool center point on the tool attached to the industrial robot, and The system comprises a motion optimization module that receives obstacle data from the perception module, the motion optimization module being programmed to calculate a commanded robot motion based on the designed robot motion and the obstacle data, the commanded robot motion being provided as feedback to the planning module to calculate the designed robot motion for the next control cycle, the commanded robot motion being calculated using an iterative optimization calculation having a multi-item function with adjustable coefficients for each control cycle, one or more robot structure constraints, and collision avoidance constraints, and, The commanded robot motion is provided to a control module that controls the joint motion of the industrial robot. The multi-item function includes a path deviation minimization term relating to the deviation between the path in the designed robot motion and the path in the commanded robot motion, a joint velocity regularization term, and a robot posture regularization term, and the multi-item function is configured such that the sum of the terms is minimized.
16. The system according to claim 15, wherein the computing device is a robot control device that executes the planning module, the motion optimization module, and the control device module, and provides robot joint motion commands to the industrial robot.
17. The system according to claim 15, wherein the computing device executes the planning module and the motion optimization module, the robot control device communicates with the computing device, the robot control device executes the control device module and provides robot joint motion commands to the industrial robot.
18. The system according to claim 15, wherein the coefficient of the term of the multi-item function is adjustable for each control cycle based on the degree of proximity of the industrial robot to any obstacles in the robot workspace.
19. The system according to claim 15, wherein the robot posture regularization term is selected from a first formulation which includes the square of the norm of a vector defined by the difference between the joint position and the reference joint position for all joints of the industrial robot, or a second formulation which is a cost function that includes the norm of a vector defined by the difference between the joint position and the target joint position for one or more joints of the industrial robot that are predetermined to be susceptible to large posture changes from one process to the next during robot operation.
20. The system according to claim 15, wherein the one or more robot structure constraints include an inequality constraint that defines one or more of the following: maintaining the robot joint position within a predetermined joint position range, maintaining the robot joint velocity below a predetermined joint velocity limit, and maintaining the robot joint acceleration below a predetermined joint acceleration limit.
21. The collision avoidance constraint is an inequality constraint calculated from a safety function, and the collision avoidance constraint is such that the rate of change of the safety function is greater than or equal to a negative value obtained by multiplying the safety function by a coefficient, according to claim 15.
22. The system according to claim 21, wherein the safety function is equal to the minimum distance between robot obstacles, or the safety function is equal to the minimum distance between robot obstacles minus a term calculated from the relative velocity between robot obstacles.
23. The system according to claim 15, wherein the target position is a moving target position defined as a function of time.
24. The system according to claim 15, wherein the design robot motion is a tool center point motion in orthogonal space, and the commanded robot motion includes robot joint rotation motion for all joints of the industrial robot.
25. The system according to claim 24, wherein the industrial robot has at least one redundant degree of freedom, and the number of degrees of freedom of the robot joint rotation motion exceeds the number of degrees of freedom of the tool center point motion.