Patent Application
20220009101
The present invention relates to a control device, a control method, and a recording medium.
A control device for a robot controls driving of joint mechanisms provided in the robot such that predetermined parts configuring the robot are caused to move to target positions. As an example of robot control, Patent Document 1 discloses a technique of quickly performing calculation in order to avoid interference with a robot arm. Also, a technique of avoiding collision of body segments with a plurality of degrees of freedom in an articulated system is disclosed in Patent Document 2.
Japanese Unexamined Patent Application, First Publication No. 2017-131990
Japanese Patent No. 5261495
A control device for a robot as described above may calculate all trajectories of moving parts configuring the robot. The posture of the robot is set by calculating the trajectories of the moving parts such as joints. In a case in which a predetermined part such as a robot joint does not move due to an obstacle or a load when driving of the predetermined part is controlled, it is necessary for the control device to recalculate positions and trajectories of all moving parts again in order to set information of a posture state including the position of each joint of the robot as described above.
Thus, an object of the present invention is to provide a control device, a control method, and a recording medium to solve the aforementioned problem.
According to a first aspect of the present invention, there is provided a control device that is connected to a robot arm with a plurality of joints, the control device including: a system information acquisition unit that is configured to acquire system information indicating at least a posture state of the robot arm or a driving state of the robot arm; a control target acquisition unit that is configured to acquire control target information indicating a control target of the robot arm; a joint contribution information calculation unit that is configured to calculate, on the basis of the system information, contribution information that is an index indicating contributions of the joints to the driving of the robot arm on the basis of the control target information; and a control command unit that is configured to calculate control command information to be used to control driving of each of the joints on the basis of the contribution information.
According to a second aspect of the present invention, there is provided a control method performed by a control device connected to a robot arm with a plurality of joints, the control method including: acquiring system information indicating at least a posture state of the robot arm and a drive state of the robot arm; acquiring control target information indicating a control target of the robot arm; calculating, on the basis of the system information, contribution information that is an indicator indicating contribution of the joints to driving of the robot arm on the basis of the control target information; and calculating control command information to be used to control driving of each of the joints on the basis of the contribution information.
According to a third aspect of the present invention, there is provided a recording medium recording a program that is configured to cause a control device connected to a robot arm with a plurality of joints to implement: a system information acquisition function of acquiring system information indicating at least a posture state of the robot arm and a drive state of the robot arm; a control target acquisition function of acquiring control target information indicating a control target of the robot arm; a joint contribution information calculation function of calculating, on the basis of the system information, contribution information that is an indicator indicating contribution of the joints to driving of the robot arm on the basis of the control target information; and a control command function of calculating control command information to be used to control driving of each of the joints on the basis of the contribution information.
According to the present invention, it is possible to cause a predetermined part to move a target position through control of a robot.
Hereinafter, a control device according to an embodiment of the present invention will be described with reference to the drawings.
As illustrated in the drawing, a robot system 100 is configured to include a control device 1 and a robot arm 2. The robot arm 2 includes a plurality of links 21 and joint mechanisms 22(ji) that couple the plurality of links 21 to a coupling target such that the links 21 can freely turn. Note that j in the mention of the joint mechanism 22(ji) denotes the joint mechanism 22 and i denotes a number. In other words, in a case in which the robot arm is configured with n joint mechanisms 22(ji), i=1, 2, . . . , n. The joint mechanisms 22 (ji) denote any one or a plurality of joint mechanisms 22 configuring the robot arm 2. The control device 1 controls at least driving of the joint mechanisms 22(ji) of the robot arm 2. In this manner, the control device 1 causes a predetermined part such as the joint mechanisms 22(j′i) or an arm tip position to move to a predetermined target position.
Each joint mechanism 22(ji) is configured to incorporate a first rotation motor that causes the coupled link 21 to rotate about an axis of the link 21, a second rotation motor that inclines the link 21 in a perpendicularly intersecting direction that perpendicularly intersects the axis of the link 21 coupled to the joint mechanism 22(ji), and the like. The distal end portion of the robot arm 2 may be provided with a grasping mechanism 23 for grasping an object and the like. Also, the control device 1 is electrically connected to a motor of each joint mechanism 22(ji) and controls driving of the joint mechanism 22(ji).
As illustrated in the drawing, the control device 1 is a computer provided with hardware such as a central processing unit (CPU) 101 that is a control unit, a read only memory (ROM) 102, a random access memory (RAM) 103, a solid state drive (SSD) 104, and a communication module 105.
The CPU 101 of the control device 1 is activated when power is turned on, and then executes a control program stored in advance. In this manner, the control device 1 exhibits each of functions of a control target acquisition unit 10, a joint contribution information calculation unit 11, a control command unit 12, a system information acquisition unit 13, a state calculation unit 14, and an obstruction detection unit 15.
The control target acquisition unit 10 acquires control target information indicating a control target of the robot arm 2.
The joint contribution information calculation unit 11 calculates contribution information that is an indicator indicating contributions of each joint mechanism 22(ji) to the control target information. The contribution information may be an indicator representing the degree of contribution or may be information representing the presence/absence of a contribution.
The control command unit 12 calculates control command information to be used to control driving of each joint mechanism 22(j′i) on the basis of the contribution information.
The system information acquisition unit 13 acquires system information including information indicating a state of the robot system 100 and information on an environment.
The state calculation unit 14 calculates information on a current state regarding the robot arm 2 such as each joint mechanism 22(ji) and arm tip positions using information and the like acquired by the system information acquisition unit 13. The obstruction detection unit 15 detects the position of an obstacle in the vicinity of each of joint mechanisms 22(j′i), the position of an obstacle predicted to enter the vicinity of the joint mechanisms 22(j′i), and occurrence of obstruction to the joint mechanism 22(ji). Here, the vicinity means that the distance from the joint mechanism 22(ji) (or the grasping mechanism 23 or the link 21) is equal to or less than a predetermined distance or is less than a predetermined distance.
Through processing of the control device 1 that exhibits each of such functions, it is possible to more flexibly control the robot arm 2 without calculating the entire posture of the robot arm 2 and to cause the distal end portion to move to a target position.
Next, the processing flow of the control device according to the first embodiment will be described in order.
First, if the control command unit 12 of the control device starts control, then the control device provides a command to acquire control target information to the control target acquisition unit 10 and provides a command to acquire system information to the system information acquisition unit 13.
The control target information indicates at least a control target of the robot arm 2. The control target information in one example may be a target arm tip position Pd={xd, yd, zd} of the robot arm 2. The target arm tip position Pd{xd, yd, zd} indicates three-dimensional coordinates of a movement destination of a current position of the arm tip of the robot arm 2. Alternatively, the control target information may be a target position, a target speed, a target torque, a speed vector, a force vector, or an acceleration vector of a part or a plurality of parts of the links 21. The plurality of parts may be a smaller number of joint mechanisms 22 than the total number of joint mechanisms 22, for example. The control target information may be time-series information of such values.
The system information may include so-called Denavit-Hartenberg (DH) parameters such as a joint angle, an angular speed, a torque, an inter-joint distance, a joint position (coordinates), and a joint attachment angle of each joint mechanism 22, a variable range, and the like. The system information may include information on an obstacle detected by the obstruction detection unit 15. For example, the information on the obstacle may be three-dimensional coordinates indicating the center of the obstacle, three-dimensional coordinates of a discretized surface of the obstacle, vector data of the surface of the obstacle, polygonal approximation data of the surface of the obstacle, functional approximation data of the surface of the obstacle, or the like. Also, coordinates of the robot arm 2 may be included as the information on the obstacle. It is possible to perform control to prevent different positions on the robot arm 2 from coming into contact with each other by the coordinates of the robot arm 2 being included in the information on the obstacle.
Here, the control target acquisition unit 10 acquires the target arm tip position Pd={xd, yd, zd} in an example (Step S101). Also, the system information acquisition unit 13 acquires, as system information, a joint angle θ={θ1, θ2, . . . θn} of each joint mechanism 22, an inter-joint distance (the length of each link 21) l={l1, l2, . . . ln}, and the like.
The state calculation unit 14 acquires the system information from the system information acquisition unit 13 (Step S102). Then, the state calculation unit 14 calculates a current arm tip position Pc (Step S103). The current arm tip position Pc may be calculated from the system information in accordance with forward kinematics, for example. For example, it is possible to obtain the arm tip position if the base point of the coordinates of the robot arm 2 and the Denavit-Hartenberg (DH) parameters are known. Also, the state calculation unit 14 calculates a joint position Pi of each joint mechanism 22(ji) that is one or a plurality of operation targets that can be caused to operate to move the current arm tip position Pc to the target arm tip position Pd (Step S104).
Note that the state calculation unit 14 may acquire a captured image from an external camera adapted to image the robot arm 2 itself and calculate the current arm tip position Pc and the joint position Pi of each joint mechanism 22(ji) on the basis of the captured image.
The state calculation unit 14 outputs the current arm tip position Pc and each joint position Pi of the joint mechanism 22(ji) that is a current operation target to the joint contribution information calculation unit 11. The joint contribution information calculation unit 11 acquires the system information, the arm tip position Pc, and each joint position Pi that is an operation target.
The joint contribution information calculation unit 11 calculates a moving direction and a moving amount θdi of each joint mechanism 22(ji) that is an operation target in a case in which the arm tip position Pc is caused to move to the target arm tip position Pd (Step S105). For a certain joint mechanism 22(ji), for example, the moving direction and the moving amount θdi when a line connecting the joint position Pi of the joint mechanism 22(ji) to the arm tip position Pc conforms to a line connecting the joint position Pi of the joint mechanism 22(ji) to the target arm tip position Pd are determined. For example, it is possible to obtain θdi using a geometric relationship between Pd, Pi, and Pc. For example, the coordinates are represented by vectors, and θdi is obtained as an angle formed between the vectors represented by Pd−Pi and Pc−Pi. Moreover, it is also possible to obtain θdi using an inner product of the vectors, for example. The joint contribution information calculation unit 11 may obtain the moving direction and the moving amount Δθdi of each joint mechanism 22(ji) through simulation processing.
Δθdi=Gc·θdi (1)
Here, in a case in which movement at the same angle is added to a moving direction identified for each joint mechanism 22(ji) that is an operation target, the arm tip position Pc significantly varies even if the joint mechanism 22 far from the arm tip position Pc slightly moves. Thus, the joint contribution information calculation unit 11 calculates contribution information ki indicating the degree of contribution of the identified joint mechanism 22(ji) to the moving amount when the arm tip position is caused to move from Pc to the target arm tip position Pd as follows on the basis of the current posture of the robot arm 2. When the contribution information ki is calculated, the joint contribution information calculation unit 11 first calculates a Pc−Pi distance di of the identified joint mechanism 22(ji) using Expression (2).
di=√(Pc2−Pi2) (2)
Also, the joint contribution information calculation unit 11 calculates contribution information ki of the identified joint mechanism 22(ji) using Expression (3) (Step S107).
Ki=f(di)=b·di (3)
In Expression (3), b denotes a constant. f(di) may be a function of being proportional to a square of the Pd−Pi distance di or may be an arbitrary function using the Pd−Pi distance di as an argument. Alternatively, f(di) may be a polynomial using the distance di as a variable or a trigonometric function, a logarithmic function, or an exponential function, using the distance di as a variable, or an arbitrary function represented by a sum, a product, or a quotient of the aforementioned functions. Also, the contribution information ki may be calculated on the basis of a similar equation using values d′i determined in accordance with the order of the position of each joint mechanism 22(ji) in the robot arm 2 (the order of the position from the arm tip or the like) instead of the Pd−Pi distance di. Then, the joint contribution information calculation unit 11 outputs the contribution information ki of each joint mechanism 22 to the control command unit 12.
The control command unit 12 calculates control command information θcontrol(i) on the basis of Expression (4) using the contribution information ki of the identified joint mechanism 22(ji) (Step S108). Alternatively, the control command information θcontrol(i) may be an arbitrary function represented by ki and Δθdi.
θcontrol(i)=ki·Δθdi (4)
The control command unit 12 outputs the control command information θcontrol(i) to the identified joint mechanism 22(ji) (Step S109). The control command unit 12 determines whether or not there are joint mechanisms 22(ji) for which control command information has not been calculated from among joint mechanisms 22(ji) that are operation targets (Step S110). In a case in which there are joint mechanisms 22(ji) for which control command information has not been calculated, the control command unit 12 provides a command to perform processing on the next joint mechanism 22(ji) to the joint contribution information calculation unit 11. The joint contribution information calculation unit 11 sets a value indicating the joint mechanism 22(ji) for which the contribution information ki has not been calculated from among the joint mechanisms 22(ji) that are operation targets to i and repeats the processing from Step S106 for each of minute time periods. In this manner, each joint mechanism 22(ji) that is an operation target is operated, and the arm tip position moves to the target arm tip position Pd. The control command unit 12 determines whether or not the arm tip position Pc has reached a final target position and the processing is to be ended (Step S111). The control device 1 can cause the arm tip position to gradually move and reach the final target position by successively executing the aforementioned processing while gradually shifting the target arm tip position Pd to reach the final arm tip position.
Next, a processing flow of the control device according to the second embodiment will be described in order.
First, if the control command unit 12 of the control device starts control, the control command unit 12 provides a command to acquire control target information to the control target acquisition unit 10 and provides a command to acquire system information to the system information acquisition unit 13.
The control target information indicates at least a control target of a part or a plurality of parts of the robot arm 2. For example, the control target may be an arm tip target speed vector Vc={vxd, vyd, vzd} of the robot arm 2, The arm tip target speed vector Vc{vxd, vyd, vzd} indicates a speed vector in a three-dimensional space of the arm tip of the robot arm 2. The control target may be a force vector or an acceleration vector instead of the arm tip target speed vector Vc.
The system information may include so-called Denavit-Hartenberg (DH) parameters such as a joint angle, an angular speed, a torque, an inter-joint distance, a joint position (coordinates), and a joint attachment angle of each joint mechanism 22(ji), a movable range, and the like. The system information may include information on an obstacle detected by the obstruction detection unit 15. For example, the information on the obstacle may be three-dimensional coordinates indicating the center of the obstacle, three-dimensional coordinates of discretized surface of the obstacle, vector data of the surface of the obstacle, polygonal approximation data of the surface of the obstacle, functional approximation data of the surface of the obstacle, or the like. Also, the information on the obstacle may include coordinates of the robot arm 2. It is possible to perform control to prevent different positions on the robot arm 2 from coming into contact with each other by the coordinates of the robot arm 2 being included in the information on the obstacle.
Here, in one example, the control target acquisition unit 10 acquires the arm tip target speed vector Vc={vxd, vyd, vzd} (Step S201). Also, the system information acquisition unit 13 acquires system information such as a joint angle θ={θ1, θ2, . . . θn} of each joint mechanism 22(ji) and an inter-joint distance (the length of each link 21) l={l1, l2, ln}.
The state calculation unit 14 acquires the system information from the system information acquisition unit 13 (Step S202). Then, the state calculation unit 14 calculates the current arm tip position Pc (Step S203). The current arm tip position Pc may be calculated from the system information, for example, in accordance with forward kinematics. For example, the arm tip position Pc can be obtained if the base point of the coordinates of the robot arm 2 and the Denavit-Hartenberg (DH) parameters are known. Also, the state calculation unit 14 calculates the joint position Pi of each joint mechanism 22(ji) that is one or a plurality of operation targets that can be caused to operate to cause the current arm tip position Pc to move to the target arm tip position Pd (Step S204).
Note that the state calculation unit 14 may acquire a captured image from an external camera adapted to image the robot arm 2 itself and calculate the current arm tip position Pc and the joint position Pi of each joint mechanism 22(j′i) on the basis of the captured image similarly to the first embodiment.
The state calculation unit 14 outputs the current arm tip position Pc and each joint position Pi of the joint mechanism 22(ji) that is a current operation target to the joint contribution information calculation unit 11. The joint contribution information calculation unit 11 acquires the system information, the arm tip position Pc, and the joint position Pi.
The joint contribution information calculation unit 11 calculates a moving direction and a moving amount θdi of each joint mechanism 22(ji) that is an operation target in a case in which the arm tip position Pc is caused to move to the target arm tip position Pd (Step S205). For a joint mechanism 22(ji), for example, the moving direction and the moving amount θdi when a line connecting the joint position Pi of the joint mechanism 22(ji) to the arm tip position Pc conforms to a line connecting the joint position Pi of the joint mechanism 22(ji) to the target arm tip position Pd are determined. For example, it is possible to obtain θdi using a geometric relationship of Pd, Pi, and Pc. For example, the coordinates are represented as vectors, and θdi is obtained as an angle formed between the vectors represented as Pd−Pi and Pc−Pi. In addition, it is also possible to obtain θdi using an inner product of the vectors, for example. The joint contribution information calculation unit 11 may obtain the moving direction and the moving amount θdi of each joint mechanism 22(ji) through simulation processing.
The joint contribution information calculation unit 11 derives a speed vector component Vci that perpendicularly intersects a straight line connecting the joint position Pi of a certain joint mechanism 22(ji) and the arm tip position Pc and has the arm tip position Pc as a base point using the arm tip target speed vector Vc for calculating contribution information of the certain joint mechanism 22(ji). The joint contribution information calculation unit 11 sets a numerical value indicating the joint mechanism 22(ji) that is an operation target in i and identifies the joint mechanism 22(ji). Then, the joint contribution information calculation unit 11 calculates, by Expression (5), the angular speed movement instruction Δθ′ di of the identified joint mechanism 22(ji) using the speed vector component Vci and the predetermined gain Gc (Step S206).
Δθ′di=Gc·(Vci*e_x) (5)
In Expression (5), “*” indicates an inner product of the vectors. Also, “e_x” in Expression (5) indicates a unit vector of the speed vector component Vci. Note that the joint contribution information calculation unit 11 derives a force vector component fci that perpendicularly intersects a straight line connecting the joint position Pi of the identified joint mechanism 22(ji) and the arm tip position Pc and has the arm tip position Pc as a base point using a force vector f for calculating the contribution information of the identified joint mechanism 22(ji). Then, the joint contribution information calculation unit 11 may calculate the angular speed movement instruction Δθ′di on the basis of Expression (6) using the force vector component fci and the predetermined gain Gc. In Expression (6) as well, “*” indicates an inner product of the vectors. Also, “e_x′” in Expression (6) indicates a unit vector of the force vector component fci.
Δθ′di=Gc·(fci*e_x′) (6)
Here, in a case in which control is applied to each joint mechanism 22(ji) that is an operation target such that each joint mechanism 22(ji) moves at the same angular speed in the identified moving direction, the arm tip position pc significantly varies even if the joint mechanism 22 that is far from the arm tip position Pc slightly moves. Thus, the joint contribution information calculation unit 11 calculates the contribution information ki indicating the degree of contribution of the identified joint mechanism 22(ji) to the moving amount when the arm tip position Pc is caused to move to the target arm tip position Pd, on the basis of the current posture of the robot arm 2 (Step S207) as follows.
ki=√(Pc2−Pi2) (7)
Alternatively, it is possible to reduce a control torque of each joint mechanism 22(ji) that is an operation target by further increasing the moving amount of the joint mechanism 22(ji) that is close to the arm tip position Pc. In this case, since the joint mechanism 22(ji) that is an operation target close to the arm tip position Pc further contributes to the movement of the arm tip position Pc, the contribution information ki may be calculated by Expression (8).
ki=1≥√(Pc2−Pi2) (8)
Note that “√i(Pd2−Pi2)” in Expressions (7) and (8) above may be replaced with a function including “√i(Pd2−Pi2)” as a variable. Then, the joint contribution information calculation unit 11 outputs the contribution information ki of the identified joint mechanism 22 to the control command unit 12.
The control command unit 12 calculates control command information θ′control(i) on the basis of Expression (9) using the contribution information of the identified joint mechanism 22(ji) (Step S208).
θ′control(i)=ki·Δθ′di (9)
The control command information θ′control(i) may be an arbitrary function including “ki·Δθ′di”. The control command unit 12 outputs the control command information θ′control(i) to the identified joint mechanism 22(ji) (Step S209). The control command unit 12 determines whether or not there are joint mechanisms 22(ji) for which control command information has not been calculated from among the joint mechanisms 22(ji) that are operation targets (Step S210). In a case in which there are joint mechanisms 22(ji) for which control command information has not been calculated, the control command unit 12 provides a command to perform processing on the next joint mechanism 22(ji) to the joint contribution information calculation unit 11. The joint contribution information calculation unit 11 sets a value indicating the joint mechanism 22(ji) for which the contribution information ki has not been calculated from among the joint mechanisms 22(j′i) that are operation targets to i and repeats the processing from Step S206 for each minute time. In this manner, each joint mechanism 22(ji) operates, and the arm tip position moves to the target arm tip position Pd. The control command unit 12 determines whether or not the arm tip position Pc has reached a final target position and the processing is to be ended (Step S211). The control device 1 can gradually moves the arm tip position and cause the arm tip position to reach the final target position by successively executing the aforementioned processing while gradually shifting the target arm tip position Pd such that the arm tip position reaches the final arm tip position.
In the first embodiment and the second embodiment, the contribution information ki is calculated on the basis of the degree of contribution of motion of each joint mechanism 22(ji) that contributes to movement of the target part of the robot arm 2 (arm tip position Pc), which is to be moved to a certain target position, to the target position in the calculation of the contribution information. However, the joint contribution information calculation unit 11 may calculate the contribution information ki indicating the degree of contribution to movement of the target part that is to be moved to the target position on the basis of an indicator indicating easiness of the operation, such as a magnitude of a force when the joint mechanism 22(ji) operates by a predetermined amount or an inertial torque, for calculating the contribution information.
Specifically, the joint contribution information calculation unit 11 calculates, as the contribution information ki, a proportion of an angle θactual(i) by which the joint mechanism 22(ji) has actually operated within a defined control time in response to actual control performed on a joint mechanism 22(J) based on the control command information θcontrol(i) calculated in the first embodiment, on the basis of Expression (10).
ki=θactual(i)÷θcontrol(i) (10)
In Expression (10) above, the joint contribution information calculation unit 11 may use the control command information θ′control(i) calculated in the second embodiment instead of the control command information θcontrol(i) calculated in the first embodiment. Also, although the contribution information ki calculated in the first embodiment is obtained on the basis of a ratio of the angle θactual(i) by which the joint mechanism 22(ji) has operated within a control time with respect to the control command information θcontrol(i) in Expression (10) above, the contribution information may be calculated on the basis of a difference therebetween or may be an arbitrary function including the ratio or the difference. The joint contribution information calculation unit 11 may use a function including the control command information θcontrol(i) and the angle θactual(i) by which the joint mechanism 22(ji) has operated within the control time for calculating the contribution information ki. Also, the control command unit 12 calculates the control command information θcontrol(i) using the contribution information ki obtained by Expression (10).
Note that the contribution information may be a value obtained by a sum or a product of the contribution information ki calculated in the first embodiment or the second embodiment and the contribution information ki calculated in the third embodiment or a value calculated by a function including the contribution information.
The joint contribution information calculation unit 11 may use values of the contribution information ki of a certain joint mechanism 22(ji) itself calculated in the aforementioned embodiments and the contribution information kj of another joint mechanism for calculating the contribution information k′i of the certain joint mechanism 22(ji). For example, it is assumed that contribution information of another j-th joint mechanism 22(Jj) is denoted as kj. In this case, the contribution information k′i is calculated by Expression (11) in one example. Another joint mechanism 22(Jj) may be all joint mechanisms 22 configuring the robot arm 2 or may be one or a plurality of joint mechanisms 22 in the vicinity of the joint mechanism 22 that is a target of calculation of the contribution information ki. Note that n in Expression (11) indicates the number of joint mechanisms 22 configuring the robot arm.
k′i=ki−(1÷(n−1)Σkj) (11)
Although the exemplary case in which the arm tip position Pc is caused to move to the target arm tip position Pd has been described in the aforementioned embodiments, the control device 1 may perform processing of causing a current position Pe of an arbitrary part of the robot arm 2 to move to the target arm tip position Pd. In this case, the processing may be performed by replacing the arm tip position Pc in the aforementioned processing with the current position Pe of the arbitrary part.
In the aforementioned processing, contribution information in which the degree of contribution of a joint mechanism 22 closer to the obstacle is set to be lower may be calculated on the basis of the position of the obstacle that disturbs motion of some joint mechanism 22(ji). For example, the obstruction detection unit 15 acquires sensing information from an object detection sensor provided at a predetermined position of the robot arm 2. In a case in which the object detection sensor is a proximity sensor, coordinates of an object are calculated on the basis of the installation position of the proximity sensor, the sensing information form the proximity sensor (the distance to the object), and the like. In a case in which the object detection sensor is a camera, the obstruction detection unit 15 calculates coordinates of the object on the basis of the installation position of the camera and a captured image obtained from the camera. In a case in which the object detection sensor is a distance measurement sensor, the obstruction detection unit 15 calculates coordinates of the object on the basis of the installation position of the distance measurement sensor, sensing information obtained from the distance measurement sensor (the distance to the object, a distance image), and the like. The obstruction detection unit 15 acquires the current position of each joint mechanism 22(ji) from the state calculation unit 14. The obstruction detection unit 15 calculates coordinates of each link 21 on the basis of the current coordinates of each joint mechanism 22(ji). In a case in which the distance between the position of the link 21 or the joint mechanism 22(ji) and the position of the object is equal to or less than a predetermined distance, the obstruction detection unit 15 determines that the object is an obstacle. The obstruction detection unit 15 outputs the position of the obstacle to the joint contribution information calculation unit 11.
The joint contribution information calculation unit 11 calculates a distance dsi between the position of each joint mechanism 22(ji) and the position of the obstacle. Then, the joint contribution information calculation unit 11 calculates the contribution information ki using Expression (12) in which the degree of contribution decreases as the distance dsi between the position of the joint mechanism 22(ji) and the position of the obstacle is shorter. Note that in Expression (12), the contribution information calculated according to any one of the first to fifth embodiments is defined as Ki using an uppercase letter K.
ki=Ki−f(dsi) (12)
Note that the contribution information ki may be calculated by Expression (13) using the distance dsi between the position of the joint mechanism 22(j′i) and the position of the obstacle or a function g using the contribution information Ki calculated according to any of the first to fifth embodiments.
ki=g(Ki,dsi) (13)
According to the aforementioned processing, there is a likelihood that the robot arm 2 is interfered with obstruction when the robot arm 2 moves in a case in which the obstacle is present in the vicinity of the joint mechanism 22(ji). In this case, it is possible to cause the joint mechanism 22(ji) that is not present in the vicinity of the obstacle to more largely move to avoid the obstacle and to cause a predetermined part such as the arm tip position Pc to move to the target arm tip position Pd, by calculating the control command information for each joint mechanism 22(ji) while setting the degree of contribution of the joint mechanism 22(ji) located in the vicinity of the obstacle to be low. In other words, this is an example of a processing aspect in which the joint contribution information calculation unit 11 recalculates the contribution information on the basis of a change in position information of the obstacle.
Note that the obstruction detection unit 15 may detect contact of the obstacle with the joint mechanism 22(ji) or the link 21 on the basis of the system information. For example, the system information acquisition unit 13 acquires system information from the sensor of each joint mechanism 22(ji) when the robot arm 2 is controlled as described above. The sensor may be a torque sensor, a motor angle sensor, a motor angular speed sensor, or the like. In a case in which the joint mechanism 22(j′i) is provided with a torque sensor, the system information acquisition unit 13 acquires a torque value as the sensing information from the torque sensor. In a case in which the joint mechanism 22(ji) is provided with an angle sensor, the system information acquisition unit 13 acquires a rotation angle from a reference position of each motor included in the joint mechanism 22(ji) as the sensing information from the angle sensor. In a case in which the joint mechanism 22(ji) is provided with an angular speed sensor, the system information acquisition unit 13 acquires an angular speed of each motor included in the joint mechanism 22(ji) as the sensing information from the angular speed sensor. The system information acquisition unit 13 outputs the acquired sensing information to the obstruction detection unit 15.
The obstruction detection unit 15 determines whether or not some obstruction for driving of the joint mechanism 22(ji) that is a drive target has occurred, on the basis of a difference between system information indicating a drive state of the joint mechanism 22(ji) that is a drive target and control command information for the joint mechanism 22(ji) that is a drive target. For example, the control command information includes a torque τdi of the joint mechanism 22(ji) that is a drive target. The obstruction detection unit 15 determines whether or not there is a difference between the torque τdi designated as a command for the joint mechanism 22(ji) that is a drive target and a torque value obtained from the joint mechanism 22(ji) through comparison therebetween. In a case in which there is a difference that is equal to or greater than a predetermined torque value between the torque τdi designated as a command for the joint mechanism 22(ji) that is a drive target and the torque value obtained from the joint mechanism 22(ji), the obstruction detection unit 15 determines that some obstruction has occurred in the joint mechanism 22(ji). The obstruction detection unit 15 outputs an identifier (ID) or the like of the joint mechanism 22(ji) in which the obstruction has occurred to the joint contribution information calculation unit 11. The joint contribution information calculation unit 11 performs updating such that the contribution information (ki) of the joint mechanism 22(ji) with the ID decreases. The width by which the contribution information (ki) is caused to decrease may be a value in accordance with how large the difference which is equal to or greater than the predetermined torque value between the torque τdi designated as a command for the joint mechanism 22(ji) that is a drive target and the torque value obtained from the joint mechanism 22(j′i). Alternatively, the width by which the contribution information (ki) is caused to decrease may be calculated by an arbitrary function including the torque τdi designated as a command for the joint mechanism 22(ji) that is a drive target. The control command unit 12 calculates the control command information on the basis of the new contribution information (ki) of each joint mechanism 22(ji).
There may be a case in which the control command information includes an angular speed with respect to each joint mechanism 22(ji) and a rotation angle. In this case, the obstruction detection unit 15 determines whether or not there are differences between an angular speed and a rotational angle designated as a command for the joint mechanism 22(ji) that is a drive target and an angular speed and a rotational angle obtained from the joint mechanism 22(ji) through comparison therebetween. In a case in which there are differences that are equal to or greater than predetermined values between the angular speed and the rotational angle designated as a command for the joint mechanism 22(ji) that is a drive target and the angular speed and the rotational angle obtained from the joint mechanism 22(ji), the obstruction detection unit 15 determines that some obstruction has occurred in the joint mechanism 22(j′i). Similarly to the above description, the obstruction detection unit 15 outputs an identifier such as an ID of the joint mechanism 22(ji) in which the obstruction has occurred to the joint contribution information calculation unit 11. The joint contribution information calculation unit 11 performs updating such that the contribution information (ki) of the joint mechanism 22(ji) with the ID approaches zero, and the control command unit 12 calculates the control command information on the basis of the new contribution information (ki) of each joint mechanism 22(ji).
The robot arm 2 illustrated in
At the timing when the clock time t=1, the control device 1 controls the robot arm 2 similarly to
At the timing when the clock time t=1, the control device 1 controls the robot arm 2 similarly to
In the aforementioned example, the exemplary case in which the control device 1 controls driving of the joint mechanisms 22 of the robot arm 2 including the plurality of links 21 and the joint mechanisms 22 that couples the plurality of links 21 to the coupling target such that the links 21 can freely rotate has been described. However, the control device 1 may be adapted to control driving of drive mechanisms for a robot including the plurality of drive mechanisms that can couple a plurality of parts such that a positional relationship therebetween can be moved through processing similar to that described above.
In this case, the control device 1 is connected to the drive mechanisms with a plurality of joints and acquires system information indicating a state of the drive mechanisms and an environment. The control device 1 acquires a control target of predetermined parts such as joints of the drive mechanisms and calculates information indicating degrees of contribution to movement of the predetermined parts to the control target. Then, the control device 1 calculates control command information on the basis of the information indicating the degrees of contribution and controls the drive mechanisms on the basis of the control command information.
The control device 1 connected to the robot arm 2 with the plurality of joints exhibits, at least, functions of the control target acquisition unit 10, the joint contribution information calculation unit 11, the control command unit 12, and the system information acquisition unit 13.
The system information acquisition unit 13 acquires system information indicating at least a posture state of the robot arm 2 or a drive state of the robot arm 2 (Step S301).
The control target acquisition unit 10 acquires control target information indicating the control target of the robot arm 2 (Step S302).
The joint contribution information calculation unit 11 calculates, on the basis of the system information, contribution information that is an indicator indicating contribution of the joints to the driving of the robot arm 2 based on the control target information (Step S303).
The control command unit 12 calculates control command information to be used to control the driving of each joint on the basis of the contribution information (Step S304).
The aforementioned control device 1 has a computer system therein. Also, the aforementioned process of processing is stored in the form of a program in a computer-readable recording medium, and the aforementioned processing is performed by a computer reading and executing the program. Here, the computer-readable recording medium is a magnetic disk, a magneto-optical disc, a semiconductor memory, or the like. Also, the computer program may be distributed to the computer through a communication line, and the computer that has received the distribution may execute the program.
Also, the aforementioned program may be for realizing a part of the aforementioned functions. Further, the aforementioned function may be realized in combination with a program that has already been recorded in the computer system, that is, a so-called differential file (differential program) may be employed.
Although the invention of the present application has been described with reference to the embodiments (and the examples), the invention of the present application is not limited to the aforementioned embodiments (and the examples). Various modifications that can be understood by those skilled in the art can be added to the configurations and the details of the invention of the present application within the scope of the invention of the present application.
| Filing Document | Filing Date | Country | Kind |
|---|---|---|---|
| PCT/JP2019/004710 | 2/8/2019 | WO | 00 |
