JP2015016536A - Robot control system, robot, path creation device, path creation method and path creation program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot control system, a robot, a path creation device, a path creation method, a path creation program etc. in which path calculation processing speed can be made fast.SOLUTION: A robot control system 10 includes a moving path calculation part 112 which calculates a moving path of an end point of an arm 320 of a robot 30 and a robot control part 120. The moving path calculation part 112 acquires the present point CP which is the present position and the present posture of the end point, a target point TP which is a target position and a target posture of the end point, and an allowance range (RGi) of a relay point RPi which is a position and a posture of the end point that relays the path from the present point CP to the target point TP. And, the moving path calculation part 112 implements path determining processing for determining the joint angle of the joint of the arm 320 so that the relay point RPi is positioned in the allowance range (RGi).

Description

  The present invention relates to a robot control system, a robot, a route creation device, a route creation method, a route creation program, and the like.

  Using a robot arm composed of a link / joint mechanism, assembly work and inspection / measurement work performed in a product production line are automated. In such a robot, the target position / target posture (target point) of the arm tip to be realized, the current position / current posture (current point) of the arm tip, and the relay position / relay posture (relay point) to relay them Is set as the teaching point. Then, the joint angle of the arm that can realize the teaching point is calculated. It is desirable that the arm joint angle at each teaching point can be calculated in as short a time as possible.

  As a technique for creating the path of the arm tip, for example, there is a technique disclosed in Patent Document 1. In this method, an intermediate point candidate is generated between a preset initial position and final position, an intermediate point is determined from the intermediate point candidate, and a candidate route is created from the determined intermediate point. Then, this process is repeated to create a plurality of candidate routes, the amount of movement of the moving unit of the robot is evaluated for each candidate route, and the candidate route with the highest movement efficiency is selected as the movement route.

JP 2008-269021 A

  When a relay point is generated between the current point and the target point, it is necessary that a joint angle that realizes the relay point exists. Creating such a relay point uniquely has a problem of increasing the processing load.

  For example, in the method of Patent Document 1, in particular, when an intermediate point is generated in a work space, an intermediate point having a joint angle in the work space is uniquely generated (three degrees of freedom related to position and three degrees of freedom related to posture). It is cumbersome. That is, when determining an intermediate point, it is necessary to calculate whether or not there is a joint angle that realizes the intermediate point, and it is necessary to repeat the calculation for many candidate paths. Such processing may cause a case where processing time and calculation amount are increased.

  According to some aspects of the present invention, it is possible to provide a robot control system, a robot, a route creation device, a route creation method, a route creation program, and the like that can speed up route calculation processing.

  One aspect of the present invention includes a movement path calculation unit that calculates a movement path of an end point of an arm of a robot, and a robot control unit that controls the robot based on information on the movement path. A current point that is the current position and posture of the end point; a target point that is the target position and target posture of the end point; and a path of the end point that relays a route from the current point to the target point. The present invention relates to a robot control system that obtains an allowable range of a relay point, which is a position and an orientation, and performs a route determination process for obtaining a joint angle of the joint of the arm so that the relay point is located within the allowable range.

  According to one aspect of the present invention, the allowable range of the relay point is acquired, and the joint angle of the robot arm is determined so that the relay point is located within the allowable range. This makes it possible to speed up the route calculation process.

  In the aspect of the invention, the movement route calculation unit obtains the first to Nth allowable ranges (N is a natural number of N ≦ 2), and allows each of the first to Nth allowable ranges. You may perform the said route determination process which calculates | requires the said joint angle so that the said relay point may be located in the range.

  In this way, relay points can be determined for each allowable range of the first to Nth allowable ranges, so a plurality of relay points are determined between the current point and the target point, and the movement path of the end point of the arm is determined. Can be calculated.

  In one aspect of the present invention, the movement route calculation unit includes the route determination process for determining the relay point from the current point side, and the route determination process for determining the relay point from the target point side. It may be executed in parallel.

  In this way, since the relay point determination process can be performed in parallel from both the current point side and the target point side, the processing time can be shortened compared to the case where the relay point determination process is performed from one side. It becomes possible.

  In the aspect of the invention, the movement route calculation unit may include the current point side and the target in the kth allowable range (k is a natural number of k ≦ N) of the first to Nth allowable ranges. When the relay point determined from the point side exists, the relay point determined from the current point side and the target point side may be connected.

  In this way, two relay points determined by parallel processing from the current point side and the target point side are connected within the allowable range in which the two relay points are obtained, and are obtained from the current point side and the target point side. Can be connected to one route.

  Also, in one aspect of the present invention, the movement path calculation unit is configured to determine one of the relay points determined from the current point side and the target point side as a final relay point, or the current point side and the target You may perform the process which determines the average value of the said relay point determined from the point side to the said last relay point as said connection process.

  In this way, when the relay point determined from the current point side and the target point side is within the allowable range, the two relay points are connected within the allowable range, and the final relay point of the allowable range Can be determined.

  In the aspect of the invention, the movement route calculation unit may determine the relay point where the joint angle exists in each of the allowable ranges.

  In this way, by determining the relay point where the joint angle exists within the allowable range, the joint angle can be obtained so that the relay point is located in each allowable range, and the movement path of the arm can be determined.

  Moreover, in one aspect of the present invention, the movement route calculation unit sets the relay points in the first to Nth allowable ranges as first to Nth relay points, and sets the first to Nth relay points. When the i-th relay point (i is a natural number of i ≦ N−1) is determined, the i + 1-th relay point candidate points of the first to N-th relay points are defined as the first to N-th relay points. If the change in the joint angle corresponding to the movement from the i-th relay point to the candidate point exists, the candidate point is selected from the i + 1-th allowable range of the allowable range When the i + 1th relay point is determined and it is determined that there is no change in the joint angle, the next candidate point may be selected from the i + 1th allowable range.

  In this way, relay points in which joint angles exist are determined by sequentially setting points within the allowable range as relay point candidate points and determining whether there is a joint angle change that realizes the candidate points. Points can be determined at each tolerance.

  Moreover, in one aspect of the present invention, a display unit that displays a setting screen for setting the allowable range may be included.

  In this way, for example, when the user teaches the arm route, the user can set the allowable range via the setting screen.

  In one embodiment of the present invention, a display unit that displays the allowable range may be included.

  In this way, for example, when the robot control system generates an allowable range, the user can confirm the generated allowable range.

  According to another aspect of the present invention, an arm having a joint, a movement path calculation unit that calculates a movement path of an end point of the arm, a robot control unit that controls the arm based on information on the movement path, The movement path calculation unit includes a current point that is the current position and posture of the end point, a target point that is the target position and posture of the end point, and a path from the current point to the target point And a route determination process for obtaining a joint angle of the joint of the arm so that the relay point is located within the tolerance range. Related to the robot to perform.

  Still another aspect of the present invention includes a movement path calculation unit that calculates a movement path of the end point of the arm of the robot, and an output unit that outputs information of the movement path, and the movement path calculation unit includes: The current point and the current posture of the end point, the target point of the target position and the target posture of the end point, and the position and posture of the end point that relays the path from the current point to the target point This is related to a route creation device that obtains the allowable range of the relay point, and performs a route determination process for obtaining the joint angle of the joint of the arm so that the relay point is located within the allowable range.

  In still another aspect of the present invention, a display unit that displays a setting screen for setting the allowable range may be included.

  In still another aspect of the present invention, a display unit that displays the allowable range may be included.

  According to still another aspect of the present invention, a current point that is a current position and a current posture of an end point of a robot arm, a target point that is a target position and a target posture of the end point, and the target point from the current point To obtain the allowable range of the relay point, which is the position and posture of the end point that relays the route up to, and the joint angle of the joint of the arm so that the relay point is located in the allowable range The present invention relates to a route creation method in which a route determination process to be obtained is performed, and a process for controlling the robot is performed based on information on a movement route obtained by the route determination process.

  According to still another aspect of the present invention, a current point that is a current position and a current posture of an end point of a robot arm, a target point that is a target position and a target posture of the end point, and the target point from the current point To obtain the allowable range of the relay point, which is the position and posture of the end point that relays the route up to, and the joint angle of the joint of the arm so that the relay point is located in the allowable range The present invention relates to a route creation program for causing a computer to execute a step of performing a route determination process to be performed and performing a process of controlling the robot based on information of a movement route determined by the route determination process.

The 1st structural example of a robot system. The 2nd structural example of a robot system. A detailed configuration example of a robot control system and a robot system. Explanatory drawing about a relay point range. Explanatory drawing about a relay point range. The flowchart of the determination process of a relay point. Explanatory drawing about the determination process of a relay point. The flowchart of the search process of a relay point. The flowchart of the calculation process of a joint angle. The flowchart of a 1st connection process. The flowchart of a 2nd connection process. The flowchart of a 3rd connection process.

  Hereinafter, preferred embodiments of the present invention will be described in detail. The present embodiment described below does not unduly limit the contents of the present invention described in the claims, and all the configurations described in the present embodiment are indispensable as means for solving the present invention. Not necessarily.

1. Robot System FIG. 1 shows a configuration example of a robot system that performs route calculation processing of the present embodiment. The robot system includes a robot control system 10 (information processing apparatus) and a robot 30.

  The robot control system 10 generates a control signal and controls the robot 30 based on the control signal. Details of the robot control system 10 will be described later. Part or all of the functions of the robot control system 10 are realized by an information processing apparatus such as a PC. Note that some or all of the functions of the robot control system 10 may be realized by the robot 30 or an electronic device different from the information processing apparatus. In addition, some or all of the functions of the robot control system 10 according to the present embodiment may be realized by a server connected to the information processing apparatus or the robot 30 through communication.

  The robot 30 includes an arm 320 and an end effector 330 (for example, a hand), and performs processing according to a control signal from the robot control system 10. The robot 30 performs processing such as gripping and processing on the workpiece 40 that is a processing target in the robot 30.

  Here, the arm 320 refers to a partial region of the robot 30 and a movable region including one or more joints. Further, the end point of the arm 320 is a region of the tip portion of the arm 320 and is not connected to any region other than the end effector 330 of the robot 30. Further, the end effector 330 refers to a component that is attached to the end point of the arm 320 in order to grip the workpiece 40 or process the workpiece 40. Note that the position of the end point of the arm may be the position of the end effector 330.

  FIG. 1 is an example of a robot system in which the robot 30 and the robot control system 10 exist as separate bodies. However, in the present embodiment, the robot control system 10 may be built in the robot 30.

  FIG. 2 shows a configuration example of such a robot system. The robot system includes a robot main body 30 (having an arm 320 and an end effector 330) and a base unit portion that supports the robot main body 30, and the robot control system 10 is stored in the base unit portion. In the robot system of FIG. 2, wheels and the like are provided in the base unit section, and the entire robot is movable. Although FIG. 1 shows an example of a single arm type, the robot may be a multi-arm type robot such as a double arm type as shown in FIG.

2. Robot Control System FIG. 3 shows a detailed configuration example of the robot control system 10 and a robot system including the same. The robot control system 10 includes a processing unit 110, a robot control unit 120, a storage unit 130, and an I / F unit 140 (input unit).

  The processing unit 110 performs various processes based on data from the storage unit 130, information from the robot 30 received by the I / F unit 140, and the like. The function of the processing unit 110 can be realized by hardware such as various processors (for example, CPU, GPU, etc.), ASIC (gate array, etc.), a program, and the like.

  The processing unit 110 includes a movement path calculation unit 112 that calculates the movement path of the arm 320. The movement path calculation unit 112 determines a relay point that can realize the joint angle of the arm 320 within the relay point range set between the current point and the target point. That is, the route from the current point to the target point through the relay point is determined. The relay point range is a range allowed for the position and posture of the end point of the arm 320 at the relay point. Details of this route determination processing will be described later.

  The robot control unit 120 controls the robot 30 based on the control signal output from the processing unit 110. For example, the movement route calculation unit 112 outputs the position and posture of the end point at the current point, the target point, and the relay point. Alternatively, the joint angle at the current point, the target point, and the relay point is output. Based on this information, the robot controller 120 outputs a signal for controlling the operation of the arm 320 from the current point through the relay point to the target point.

  The storage unit 130 stores a database and serves as a work area for the processing unit 110 and the like, and its function can be realized by a memory such as a RAM or an HDD (hard disk drive).

  The I / F unit 140 is an interface for performing input from the user to the robot control system 10 and receiving information from the robot 30. Regarding input from the user, etc., it may be constituted by a switch, a button, a keyboard, a mouse, or the like.

  The robot 30 includes a control unit 310 in addition to the arm 320 and the end effector 330. The control unit 310 receives information from the robot control system 10 and controls each unit (such as the arm 320 and the end effector 330) of the robot 30.

3. Route determination process 3.1. Relay Point Range Next, details of the route determination process will be described. In the following description, a robot arm having three degrees of freedom (three joints) that moves in a two-dimensional plane (XY plane) will be described as an example.

  First, the relay point range will be described. As shown in FIG. 4, a relay point range RGi is set on a path along which the arm end point moves from the current point CP to the target point TP. This relay point range RGi may be given by teaching, or may be generated by the movement route calculation unit 112. The current point CP and the target point TP are represented by the position and posture of the end point. The position is an XY coordinate (X, Y), and the posture represents the direction of the end point by a clockwise angle Θ with respect to the X axis, for example. The end point moves from the current point CP to the target point TP via the relay point RPi, and the range allowed when determining the relay point RPi is the relay point range RGi. The relay point RPi is represented by a position (coordinates (X, Y)) and a posture (angle Θ), and the relay point range RGi is represented by a position tolerance range (position range) and a posture tolerance range (posture range). The The position range is specified by coordinates (Xi, Yi) and ranges (Wi, Hi) in the X-axis direction and the Y-axis direction. The coordinates (Xi, Yi) are, for example, the coordinates of the center of the position range. The posture range is an end point direction (angle) range Θimin ≦ Θi ≦ Θimax.

  Although one relay point range RGi is illustrated in FIG. 4, a plurality of relay point ranges RG1 to RGN are set in the present embodiment as shown in FIG. That is, N is a natural number of 2 or more, and i is a natural number of N or less. FIG. 5 shows an example where N = 5.

  The position and posture of the end point are determined by the joint angles θ1 to θ3 (joint angles) of the joints JT1 to JT3 (joints). Conversely, when the position and orientation of the end point are designated, the joint angles θ1 to θ3 that realize the position and orientation are not necessarily present. In the present embodiment, by setting the relay point range RGi as the allowable range of the relay point RPi, the relay point RPi where the joint angles θ1 to θ3 exist within the relay point range RGi can be determined. Further, by limiting the route search range with the relay point range RGi, the processing load can be reduced as compared with the case of determining an optimum route from any route in the work space. For example, when the movement path of the arm is roughly determined, the relay point RPi for realizing the joint angles θ1 to θ3 can be determined by teaching the relay point range RGi in advance to the movement path.

3.2. Relay Point Determination Processing Next, processing for determining the relay point RPi within the relay point range RGi will be described. FIG. 6 shows a flowchart of the process, and FIG. 7 shows an explanatory diagram of the process.

  When the process is started, the position and orientation of the current point CP are acquired (step S1). Next, the position and orientation of the target point TP are acquired (step S2). Next, the position range and posture range of the relay point ranges RG1 to RG5 are acquired (step S3). Next, the relay points RP1 to RP5 are determined. At this time, steps S4 to S11 determined from the current point CP side and steps S12 to S19 determined from the target point TP side are processed in parallel.

  First, a process for determining a relay point from the current point CP side will be described. The current point CP is set to the latest point (step S4). Here, the notation such as “latest point (current point side)” indicates that the point is determined from the current point CP side. Next, i = 1 is set (step S5). Next, when i <N (step S6), the relay point range RGi is acquired (step S7). Next, the relay point RPi is obtained in the relay point range RGi using the latest point as a reference (step S8). For example, when i = 1, the relay point RP1 for moving the end point of the arm from the current point CP to the relay point RP1 is obtained, and when i = 2, the relay point from the obtained relay point RP1 is obtained. A relay point RP2 for moving to RP2 is obtained. That is, the relay points RP1, RP2,... Are obtained sequentially from the current point CP side. At this time, a relay point RPi that can realize joint angle changes (Δθ1, Δθ2, Δθ3) with respect to changes in position and orientation between two points (ΔX, ΔY, ΔΘ) is searched from the relay point range RGi. . Details of this search process will be described later.

  Next, it is determined whether or not the relay point RPi is obtained from the target point TP side (step S9). For example, when a relay point is not obtained from the target point TP as in the relay points RP1 and RP2 (i = 1, 2) shown in FIG. 7, the relay point RPi is set to the latest point (step S10). I = i + 1 (step S11), and the process returns to step S6. On the other hand, when the relay point RP3 ″ is also obtained from the target point TP side like the relay points RP3 ′ and RP3 ″ (i = 3) shown in FIG. 7, the process proceeds to step S20.

  Next, processing for determining a relay point from the target point TP side will be described. First, the target point TP is set to the latest point (step S12). Here, the notation such as “latest point (target point side)” indicates that the point is determined from the target point TP side. Next, j = N-1 is set (step S13). Next, when j ≧ 1 (step S14), the relay point range RGj is acquired (step S15). Next, the relay point RPj is obtained in the relay point range RGj using the latest point as a reference (step S16). That is, relay points RP5, RP4,... Are sequentially obtained from the target point TP side. As in the case of obtaining from the current point CP side, the relay point RPj capable of realizing the joint angle change (Δθ1, Δθ2, Δθ3) is obtained.

  Next, it is determined whether or not the relay point RPj is obtained from the current point CP side (step S17). For example, when the relay point is not obtained from the current point CP as in the relay points RP5 and RP4 (i = 5, 4) shown in FIG. 7, the relay point RPj is set to the latest point (step S18). , J = j−1 (step S19), and the process returns to step S14. On the other hand, when the relay point RP3 'is obtained also from the current point CP side as in the relay points RP3' and RP3 "(j = 3) shown in FIG. 7, the process proceeds to step S20.

  In step S20, the relay point range RGk for which relay points are determined from both the current point CP side and the target point TP side is selected from the relay point ranges RG1 to RG5. Next, in the relay point range RGk, the relay points obtained from the current point CP side and the target point TP side are connected (step S21). In the example of FIG. 7, the relay point range RG3 (k = 3) is selected, the relay points RP3 ′ and RP3 ″ obtained from the current point CP side and the target point TP side are connected, and the final relay point RP3 is determined. Details of the connection process will be described later.

  In the embodiment described above, the robot control system 10 includes the movement path calculation unit 112 that calculates the movement path of the end point of the arm 320 of the robot 30 and the robot control unit 120 that controls the robot 30 based on the movement path information. And including. The movement path calculation unit 112 relays a current point CP that is the current position and posture of the end point, a target point TP that is the target position and target posture of the end point, and a route from the current point CP to the target point TP. An allowable range (relay point range RGi) of the relay point RPi, which is the position and orientation of the end point, is acquired. And the movement path | route calculating part 112 performs the path | route determination process which calculates | requires the joint angles (joint angles (theta) 1- (theta) 3) of the arm 320 so that the relay point RPi may be located in the tolerance | permissible_range.

  In this way, it is possible to reduce the processing time and the calculation amount of the process for determining the path of the robot arm. For example, when a route candidate is created by designating each relay point as one point, the joint angle that realizes each relay point may not be obtained. In such a case, it is necessary to generate a new route candidate and confirm the existence of the joint angle. However, if this is performed in a work space where there are no restrictions on the route, there are innumerable route candidates, and the amount of processing is reduced. May increase. In this respect, according to the present embodiment, since the allowable ranges of the relay points RP1 to RPN are determined by the relay point ranges RG1 to RGN, the route range can be limited to some extent, and the processing amount can be reduced. Further, by determining the relay point RPi within the allowable range (relay point range RGi) instead of one point, it is possible to set the relay point RPi that can realize the joint angle.

  Here, the relay point RPi and the allowable range (relay point range RGi) are set in a space (or a virtual space corresponding thereto) in which the arm 320 of the robot 30 operates. In the above embodiment, the case of a two-dimensional space has been described, but generally a three-dimensional space. In other words, the relay point RPi has three degrees of freedom (for example, XYZ coordinates) and three degrees of freedom (for example, roll angle, pitch angle, and yaw angle). The allowable range (relay point range RGi) is set for each of the 6 degrees of freedom of the relay point RPi. The three-dimensional space is a world coordinate system set by the robot control system 10 corresponding to the work space, for example. Alternatively, when the operation of the robot 30 is simulated, it is a virtual three-dimensional space in which a model of the robot 30 is installed.

  Moreover, in this embodiment, the movement path | route calculating part 112 acquires the 1st-Nth permissible range (1st-Nth relay point range RG1-RGN), and the 1st-Nth permissible range (N is N). A route determination process for obtaining the joint angle is performed so that the relay point RPi is positioned in each allowable range (RGi) of N ≦ natural number.

  In this way, since the relay point RPi can be determined for each of the first to Nth allowable ranges (RGi), a plurality of relay points RP1 to RPN are determined between the current point CP and the target point TP. The movement path of the arm end point can be calculated.

  Further, in the present embodiment, the movement route calculation unit 112 determines the relay point RPi from the current point CP side (steps S4 to S11 in FIG. 6) and the route to determine the relay point RPi from the target point TP side. The determination process (steps S12 to S19) is executed in parallel.

  In this way, it is possible to shorten the processing time by determining the relay points RP1 to RPN from both sides of the current point CP side and the target point TP side. For example, as will be described later, a method is conceivable in which a relay point is determined in order one by one by obtaining a joint angle change from the latest point to the next relay point. In such a method, the relay points are obtained in order from both sides in parallel processing, and calculation is performed up to the relay point range (RG3 in FIG. 7) from which the relay points are obtained, so the relay point is determined from one side. The processing time can be shortened compared with the case where it goes.

  Further, in the present embodiment, the movement path calculation unit 112 has a relay point (determined from the current point CP side and the target point TP side) in the kth allowable range (relay point range RGk) of the first to Nth allowable ranges. When RP3 ′, RP3 ″) in FIG. 7 exists, the relay points determined from the current point CP side and the target point TP side are connected (step S21 in FIG. 6).

  In this way, it is possible to connect paths determined by parallel processing from the current point CP side and the target point TP side. That is, as shown in steps S9 and S17 in FIG. 6, when there is a relay point (that is, a joint angle that realizes it) obtained from both sides in one allowable range (RGk), the relay points thereof are reached. By connecting the points within an allowable range, one path can be generated.

  In the present embodiment, the robot control system 10 may include a display unit that displays a setting screen for setting an allowable range (RGi). For example, a screen for setting the reference position (Xi, Yi), position range (Hi, Wi), maximum value and minimum value (Θimin, Θimax) of the posture range of each relay point range RGi described in FIG. 4 is displayed.

  In this way, for example, when the user teaches the arm route, the user can set the allowable range (RG1 to RGN) via the setting screen. The display unit may be provided, for example, in the robot control system 10 (information processing apparatus) separate from the robot 30 as shown in FIG. 1, or provided in the robot control system 10 integrated with the robot 30 as shown in FIG. May be.

  In the present embodiment, the robot control system 10 may include a display unit that displays the allowable range RGi.

  In this way, for example, when the robot control system 10 generates the allowable range (RG1 to RGN), the generated allowable range can be confirmed by the user.

  In the above embodiment, the case where the robot control system 10 performs the route determination process has been described as an example. However, the present embodiment is not limited to this. For example, the route determination process may be performed by a route creation device (for example, an information processing device such as a PC) separate from the robot 30.

  The route creation device includes a movement route calculation unit 112 and an output unit that outputs information on the movement route obtained by the movement route calculation unit 112. The information on the movement path is, for example, the position and posture of the current point, the relay point, and the target point, the joint angle at those points, or a control signal for instructing the robot of these joint angles. The output unit can be realized by, for example, a communication interface such as a USB or an interface of an external storage device such as a memory card.

3.3. Relay Point Search Processing Next, processing in steps S8 and S16 in FIG. 6, that is, processing for obtaining a relay point RPi capable of realizing joint angles (θ1, θ2, θ3) will be described. FIG. 8 shows a flowchart of the processing.

  When the process is started, information on the relay point range for obtaining the relay point is acquired (step S31). The acquired information includes the XY coordinates (X, Y) of the reference position, the width W in the X-axis direction, the width H in the Y-axis direction, and the maximum angle Θmax and the minimum value Θmin representing the posture. Here, the XY coordinates (X, Y) of the reference position are the lower left corner of the relay point range (the position where the coordinate values X and Y are the smallest). Next, the latest point information is acquired (step S32). The information to be acquired is the XY coordinates (Xc, Yc) of the latest point and the angle Θc representing the posture of the latest point.

  Next, a relay point candidate is selected from the relay point range, and a joint angle capable of realizing the relay point candidate is calculated. Then, the process is repeated while sequentially changing candidates until a relay point that can calculate the joint angle is found.

  Specifically, the relay point range is divided by a predetermined natural number M (step S33). That is, δx = W / (M−1), δy = H / (M−1), and δθ = (Θmax−Θmin) / (M−1) are obtained. Next, i = 0 is set (step S34). Next, it is determined whether i <M is satisfied (step S36). If i <M, the coordinates Xa = X + δx × i within the relay point range are acquired (step S37). Next, j = 0 is set (step S38). Next, it is determined whether j <M is satisfied (step S40). If j <M, the coordinates Ya = Y + δy × j within the relay point range are acquired (step S41). Next, k = 0 is set (step S42). Next, it is determined whether or not k <M (step S43). If k <M, the angle Θa = Θmin + δθ × k within the relay point range is acquired (step S44).

  Next, joint angles (θ1, θ2, θ3) that realize relay point candidates (Xa, Ya, Θa) are obtained (step S45). Details of the processing in step S45 will be described later. Next, it is determined whether or not the joint angles (θ1, θ2, θ3) have been obtained (step S46). If the joint angle cannot be obtained, the relay point candidate is changed and the calculation is repeated. That is, k = k + 1 is set (step S47), and the process returns to step S43. If k <M is not satisfied, j = j + 1 is set (step S39), and the process returns to step S40. If j <M is not satisfied, i = i + 1 is set (step S35), and the process returns to step S36. When a joint angle that realizes a relay point candidate is obtained by this iterative calculation (step S46), the relay point candidate (Xa, Ya, Θa) is set as a relay point, and the joint angle ( (θ1, θ2, θ3) are held (step S48), and the process is terminated.

  If i <M is not satisfied in step S36, an error notification indicating that the relay points included in the relay point range could not be determined is made (step S49), and the process is terminated.

3.4. Next, the processing in step S45 of FIG. 8, that is, the processing for obtaining the joint angle θ = (θ1, θ2, θ3) for realizing the relay point candidate Pa = (Xa, Ya, Θa) will be described. . FIG. 9 shows a flowchart of the process.

  In this process, the joint angle θ = θc + Δθ is obtained by iterative calculation. For example, when determining the relay point RP1 from the current point CP side in FIG. 7, θc = (θ1c, θ2c, θ3c) is the joint angle at the current point CP (latest point), and the joint angle θ at the relay point RP1. = (Θ1, θ2, θ3). Δθ = (Δθ1, Δθ2, Δθ3) is a joint angle change between θ and θc.

When the processing of FIG. 9 is started, first, an initial value θ0 = (θ1_0, θ2_0, θ3_0) of the joint angle is set (step S61). Next, n = 0 is set (step S62). Next, it is determined whether n <nmax is satisfied (step S63). nmax is the upper limit of the number of iterations. If n <nmax, the Jacobian matrix [J] _n at the joint angle θn = (θ1_n, θ2_n, θ3_n) is obtained (step S64). Next, an inverse matrix [J ⁻¹ ] _n of the Jacobian matrix [J] _n is obtained (step S65).

Next, Δθn = (Δθ1_n, Δθ2_n, Δθ3_n) is obtained from Δθn = [J ⁻¹ ] _n · ΔP, and θn + 1 = θn + Δθn is obtained (step S66). Here, ΔP = Pa−Pc = (Xa−Xc, Ya−Yc, Θa−Θc) is a change in position and posture from the latest point Pc to the relay point candidate Pa. Δθn is the joint angle change Δθ obtained at the nth iteration. Since the initial value θ0 is generally different from the solution, Δθn changes as the calculation is repeated.

  Next, the difference θn + 1−θn is obtained (step S67). Next, it is determined whether or not the difference θn + 1−θn is sufficiently small (for example, smaller than a predetermined threshold) (step S68). If the difference θn + 1−θn is not sufficiently small, n = n + 1 is set (step S69), and the process returns to step S63. On the other hand, if the difference θn + 1−θn is sufficiently small, θ = θn + 1 is set. That is, θn + 1 = (θ1_n + 1, θ2_n + 1, θ3_n + 1) is held as the joint angle θ = (θ1, θ2, θ3) for realizing the relay point candidate Pa = (Xa, Ya, Θa) (step S70), and the process is performed. finish.

  If n has reached the upper limit nmax of the number of iterations in step S63, an error notification is given to the effect that the joint angle θ for realizing the relay point candidate Pa cannot be obtained within the relay point range, and the process ends. (Step S71).

  In the above embodiment, the movement path calculation unit 112 determines a relay point where the joint angle (joint angle θ = (θ1, θ2, θ3)) exists in each allowable range (each relay point range).

  In order for the movement path of the arm to be established, a joint angle that realizes the path needs to exist. In this embodiment, by determining the relay point where the joint angle exists within the allowable range, the joint angle is obtained so that the relay point is located in each allowable range, and the movement path of the arm can be generated.

  Further, in the present embodiment, when the i-th relay point RPi of the 1st to N-th relay points RP1 to RPN is determined, the movement route calculation unit 112 determines the candidate point Pa = i + 1-th relay point RPi + 1. (Xa, Ya, Θa) is selected from the (i + 1) th allowable range (relay point range RGi + 1) (steps S34 to S44 in FIG. 8). If the movement path calculation unit 112 determines that there is a change in joint angle Δθ = (Δθ1, Δθ2, Δθ3) corresponding to the movement from the i-th relay point RPi to the candidate point Pa, the candidate point Pa is determined as the i + 1th relay point RPi + 1 (steps S45 to S48 in FIG. 8, flow in FIG. 9). On the other hand, when it is determined that there is no joint angle change Δθ, the next candidate point is selected from the (i + 1) th allowable range (RGi + 1) (steps S35, S39, and S47 in FIG. 8).

  In this way, a relay point where the joint angle θ = (θ1, θ2, θ3) exists can be determined in each allowable range. That is, a relay point can be determined by sequentially setting points within the allowable range as relay point candidates and determining whether or not there is a joint angle θ that realizes the candidate.

For example, as described with reference to FIG. 9, the joint angle θ may be determined by starting the joint angle θ from the initial value θ0 and converging the joint angle θn by an iterative operation using the inverse Jacobian matrix [j ⁻¹ ] _n. Good.

  According to this method, when the joint angle θn by the repetitive calculation becomes sufficiently small, it can be determined that there is a solution for the joint angle θ (a solution for realizing the relay point). Also, the initial value θ0 can be set based on the joint angle of the latest point (the relay point determined in the previous relay point range). Since the joint angle of the desired relay point is considered to be close to the joint angle of the previous relay point, setting such an initial value allows the joint angle θn to converge quickly and reduce the number of iterations. it can.

3.5. Relay Point Connection Processing Next, the processing of step S21 in FIG. 6, that is, the relay points obtained from the current point CP side and the target point TP side for the same relay point range (for example, relay points RP3 ′ and RP3 ″ in FIG. 7). ) Will be described.

  FIG. 10 shows a flowchart when the first connection process is performed. In this process, the relay point obtained from the current point CP side is adopted. That is, when the processing is started, a joint angle that realizes the relay point obtained from the current point CP side is acquired for the relay point range for which the relay point obtained from the current point CP side and the target point TP side is obtained (step) S81). Next, the acquired joint angle is held as a joint angle in the relay point range (step S82).

  FIG. 11 shows a flowchart when the second connection process is performed. In this process, the relay point obtained from the target point TP side is adopted. That is, when the process is started, a joint angle that realizes the relay point obtained from the target point TP side is obtained for the relay point range for which the relay point obtained from the current point CP side and the target point TP side is obtained (step) S91). Next, the acquired joint angle is held as a joint angle in the relay point range (step S92).

  FIG. 12 shows a flowchart when the third connection process is performed. In this process, the average of the relay points obtained from the current point CP side and the target point TP side is adopted. That is, when the processing is started, a joint angle that realizes the relay point obtained from the current point CP side is acquired for the relay point range for which the relay point obtained from the current point CP side and the target point TP side is obtained (step) S101). Next, for the relay point range, a joint angle for realizing the relay point obtained from the target point TP side is acquired (step S102). Next, an average value of the acquired joint angles is obtained, and the average value is held as a joint angle in the relay point range (step S103).

  In the above embodiment, the movement route calculation unit 112 determines one of the relay points (RP3 ′ and RP3 ″ in FIG. 7) determined from the current point CP side and the target point TP side as the final relay point (FIG. 10, FIG. 11), or the process of determining the average value of the relay points determined from the current point CP side and the target point TP side as the final relay point is performed as a connection process.

  In this way, when the relay point determined from the current point CP side and the target point TP side is within one allowable range (relay point range), the relay points are connected within the allowable range, The final relay point of the range can be determined. In addition, a connection process is not limited to the said method, A various method is employable. For example, a relay point determined from the current point CP side and the target point TP side may be employed together to determine a joint angle change between the two relay points.

  Although the present embodiment has been described in detail as described above, it will be easily understood by those skilled in the art that many modifications can be made without departing from the novel matters and effects of the present invention. Accordingly, all such modifications are intended to be included in the scope of the present invention. For example, a term described at least once together with a different term having a broader meaning or the same meaning in the specification or the drawings can be replaced with the different term in any part of the specification or the drawings. All combinations of the present embodiment and the modified examples are also included in the scope of the present invention. In addition, the configuration and operation of the robot control system, the robot, and the route creation device, the relay point determination method, the robot control method, and the like are not limited to those described in the present embodiment, and various modifications can be made. .

10 robot control system, 30 robot (robot body),
40 workpieces, 110 processing unit, 112 movement path calculation unit,
120 robot control unit, 130 storage unit, 140 I / F unit,
310 control unit, 320 arm, 330 end effector,
H, W position range, JT1-JT3 joint,
RG1-RG5 relay point range (allowable range), RP1-RP5 relay point,
TP target point, X and Y coordinates (position), Θ angle (posture),
Θimax Maximum value of posture range, Θimin: Minimum value of posture range,
θ1-θ3 Joint angle (joint angle)

Claims (15)

A movement path calculation unit for calculating the movement path of the end point of the robot arm;
A robot controller that controls the robot based on the information of the movement path;
Including
The travel route calculation unit
The current point and the current posture of the end point, the target point of the target position and the target posture of the end point, and the position and posture of the end point that relays the path from the current point to the target point And the allowable range of the relay point is
A robot control system that performs a route determination process for obtaining a joint angle of the joint of the arm so that the relay point is located within the allowable range. In claim 1,
The travel route calculation unit
The first to Nth allowable ranges are acquired, and the joint angle is set so that the relay point is located in each allowable range of the first to Nth allowable ranges (N is a natural number of N ≦ 2). A robot control system that performs the route determination process to be obtained. In claim 2,
The travel route calculation unit
The robot control system, wherein the route determination process for determining the relay point from the current point side and the route determination process for determining the relay point from the target point side are executed in parallel. In claim 3,
The travel route calculation unit
When the relay point determined from the current point side and the target point side exists in the kth allowable range (k is a natural number of k ≦ N) of the first to Nth allowable ranges, A robot control system characterized in that the relay point determined from the current point side and the target point side is connected. In claim 4,
The travel route calculation unit
A process of determining one of the relay points determined from the current point side and the target point side as a final relay point, or an average value of the relay points determined from the current point side and the target point side A robot control system characterized in that a process for determining a relay point is performed as the connection process. In any of claims 2 to 5,
The travel route calculation unit
The robot control system characterized in that the relay point where the joint angle exists is determined in each allowable range. In claim 6,
The travel route calculation unit
The first to Nth relay points in the first to Nth allowable ranges are the first to Nth relay points, and the i-th relay point (i is a natural number where i ≦ N−1). ) Is determined,
A candidate point of the (i + 1) th relay point of the first to Nth relay points is selected from the (i + 1) th allowable range of the first to Nth allowable ranges;
When it is determined that there is a change in the joint angle corresponding to the movement from the i-th relay point to the candidate point, the candidate point is determined as the i + 1-th relay point;
If it is determined that there is no change in the joint angle, the next candidate point is selected from the (i + 1) th allowable range. In any one of Claims 1 thru | or 7,
A robot control system comprising a display unit for displaying a setting screen for setting the allowable range. In any one of Claims 1 thru | or 7,
A robot control system comprising a display unit for displaying the allowable range. An arm having a joint;
A movement path calculation unit for calculating a movement path of the end point of the arm;
A robot controller that controls the arm based on the information of the movement path;
Including
The travel route calculation unit
The current point and the current posture of the end point, the target point of the target position and the target posture of the end point, and the position and posture of the end point that relays the path from the current point to the target point And the allowable range of the relay point is
A robot that performs a route determination process for obtaining a joint angle of the joint of the arm so that the relay point is located within the allowable range. A movement path calculation unit for calculating the movement path of the end point of the robot arm;
An output unit for outputting information of the travel route;
Including
The travel route calculation unit
The current point and the current posture of the end point, the target point of the target position and the target posture of the end point, and the position and posture of the end point that relays the path from the current point to the target point And the allowable range of the relay point is
A route creation device that performs route determination processing for obtaining a joint angle of the joint of the arm so that the relay point is located within the allowable range. In claim 11,
A route creation device comprising a display unit for displaying a setting screen for setting the allowable range. In claim 11,
A route creation device including a display unit for displaying the allowable range. A current point that is the current position and posture of the end point of the arm of the robot, a target point that is the target position and posture of the end point, and the end point that relays the path from the current point to the target point. Perform the process of obtaining the allowable range of the relay point that is the position and orientation,
A route determination process for obtaining a joint angle of the joint of the arm so that the relay point is located in the allowable range,
Performing a process of controlling the robot based on the information of the movement path obtained by the path determination process;
A route creation method characterized by that. A current point that is the current position and posture of the end point of the arm of the robot, a target point that is the target position and posture of the end point, and the end point that relays the path from the current point to the target point. Perform the process of obtaining the allowable range of the relay point that is the position and orientation,
A route determination process for obtaining a joint angle of the joint of the arm so that the relay point is located in the allowable range,
Performing a process of controlling the robot based on the information of the movement path obtained by the path determination process;
A route creation program that causes a computer to execute steps.

Images