Program, storage medium, information processing device, system, and method for controlling operation of robot

Abstract

This program causes a computer to execute a process in which: singular value decomposition of a first Jacobian matrix is carried out, and a second Jacobian matrix is generated, the second Jacobian matrix including a first matrix, a second matrix, and a first singular matrix including a singular value in a diagonal element; on the basis of the result of comparing the singular value and a first threshold value, execution of a first process is determined when the singular value is a singular point or close to the singular point, and execution of a second process is determined when the singular value is distant from the singular point; in the first process, a third Jacobian matrix is generated by converting diagonal elements in the first singular matrix, and operation command values for a plurality of joints (JT) of a robot (10) are determined and outputted using the third Jacobian matrix and a movement command value for a moving part (12) of the robot (10); and in the second process, the operation command values are determined and outputted using the movement command value and either the first Jacobian matrix or the second Jacobian matrix.

Description

Program, storage medium, information processing device, system, and method for controlling robot operation

[0001] The present disclosure relates to a program, a storage medium, an information processing device, a system, and a method for controlling the operation of a robot.

[0002] For example, Patent Document 1 discloses a system for dealing with singularities during the operation of a multi-axis machine including a kinematic linkage with multiple joints. In this system, numerical control is performed using a Jacobian matrix. The Jacobian matrix is ​​subjected to singular value decomposition into an orthonormal matrix including singular values, left singular vectors, and right singular vectors. The singular value decomposition results are used to determine joints that are singular or close to singular points, and the Jacobian matrix is ​​modified by transforming columns of the Jacobian matrix associated with the determined joints using the singular values ​​and left singular vectors.

[0003] Patent No. 5393461

[0004] In Patent Document 1, a virtual joint obtained by calculating a left singular vector using a singular value is replaced with a column of a Jacobian matrix associated with the determined joint. As a result, the relationship between the joint's operating speed and the operating speed of the tip of the link mechanism, which is inherent in the Jacobian matrix, may be lost. The present disclosure provides a program, a storage medium, an information processing device, a system, and a method for realizing novel robot operation control that corresponds to singular points using singular value decomposition of a Jacobian matrix.

[0005] A program according to one aspect of the present disclosure is a program for controlling the movement of a robot including a plurality of joints and a moving part that is moved by movement of the plurality of joints, the program performing singular value decomposition on a first Jacobian matrix that relates movement speeds of the plurality of joints to the movement speed of the moving part to generate a second Jacobian matrix that includes a first matrix including as elements a first basis vector that represents a movement speed space of the joints, a second matrix including as elements a second basis vector that represents a movement speed space of the moving part, and a first singular matrix including as diagonal elements a singular value; and determining whether the robot is at a singular point or whether the robot is in contact with the singular point based on a comparison result between the singular value and a first threshold. The present invention causes a computer to execute the following steps: determining and executing a process to be executed from a first process that is executed when the robot is close to the singular point and a second process that is executed when the robot is away from the singular point; in the first process, generating a third Jacobian matrix by transforming diagonal elements in the first singular matrix; in the first process, determining and outputting motion command values ​​that command motion of the plurality of joints using the third Jacobian matrix and a motion command value that commands movement of the moving unit; and in the second process, determining and outputting the motion command value using the first Jacobian matrix or the second Jacobian matrix and the motion command value.

[0006] FIG. 1 is a diagram illustrating an example of the configuration of a robot system according to an illustrative embodiment. FIG. 2 is a conceptual diagram illustrating the configuration of a joint of the robot arm of FIG. 1. FIG. 3 is a block diagram illustrating an example of the configuration of a robot system according to an embodiment. FIG. 4 is a conceptual diagram illustrating an example of the posture of a robot arm at a singular point. FIG. 5 is a conceptual diagram illustrating an example of the posture of a robot arm at a singular point. FIG. 6 is a conceptual diagram illustrating an example of the posture of a robot arm at a singular point. FIG. 7 is a conceptual diagram illustrating an example of the posture of a robot arm at a singular point. FIG. 8 is a flowchart illustrating an example of processing related to a singular point of an information processing device according to an embodiment. FIG. 9 is a flowchart illustrating an example of processing related to a singular point of an information processing device according to an embodiment. FIG. 10 is a flowchart illustrating an example of processing related to a singular point of an information processing device according to an embodiment. FIG. 11 is a flowchart illustrating an example of processing related to a singular point of an information processing device according to an embodiment. FIG. 12 is a flowchart illustrating an example of processing related to a singular point of an information processing device according to an embodiment. FIG. 13A is a conceptual diagram illustrating an example of self-motion of a robot arm in the case of a shoulder singular posture. FIG. 13B is a conceptual diagram illustrating an example of self-motion of a robot arm in the case of a shoulder singular posture. Fig. 13C is a conceptual diagram showing an example of self-motion of the robot arm in the case of a shoulder singular posture Fig. 14 is a diagram showing an example of the configuration of a robot system according to a modified example.

[0007] Hereinafter, exemplary embodiments of the present disclosure will be described with reference to the drawings. The embodiments described below are all comprehensive or specific examples. Among the components in the following embodiments, components that are not recited in the independent claims showing the highest concepts will be described as optional components. Each figure in the accompanying drawings is a schematic diagram and is not necessarily an exact drawing. In each figure, substantially identical components are assigned the same reference numerals, and duplicated descriptions may be omitted or simplified. In this specification and claims, the term "device" may refer not only to one device but also to a system including multiple devices.

[0008] [Configuration of Robot System] The configuration of a robot system 1 according to an embodiment will be described. FIG. 1 is a schematic diagram showing an example of the configuration of a robot system 1 according to an embodiment. As shown in FIG. 1, the robot system 1 according to the embodiment includes a robot 10, a control device 20, and a user interface 30. Although not limited to this, in this embodiment, the robot system 1 functions as a system for manually controlling the robot 10, the user interface 30 functions as a manual controller, and the robot 10 operates in accordance with operations input to the manual controller. The control device 20 controls the overall operation of the robot system 1.

[0009] The robot 10 includes a link structure 11 and a moving unit 12. The link structure 11 includes a plurality of links L and a plurality of joints JT that operatively connect the plurality of links L. The moving unit 12 is attached to the link structure 11. In this embodiment, the moving unit 12 is attached to the tip of the link structure 11. The link structure 11 can move the moving unit 12 to various positions and postures by operating the plurality of joints JT.

[0010] In the present embodiment, the link structure 11 is a multi-joint robot arm, for example, an industrial robot arm, but is not limited thereto. The link structure 11 may be, for example, a robot leg, as long as it has a structure in which a plurality of links L are connected by a plurality of joints JT.

[0011] Furthermore, the link structure 11 has a vertical multi-joint arm structure, and includes six links L1 to L6 and six joints JT1 to JT6 as multiple links L and multiple joints JT. The link structure 11 as a robot arm is not limited to the above, and may have other types of arm structures, such as a horizontal multi-joint type. The number of links L and the number of joints JT may also be five or less, or seven or more. Hereinafter, the link structure 11 may also be referred to as a robot arm 11. "Joints" are represented using "joint JT1" to "joint JT6" when the joints need to be distinguished, and are represented as "joint JT" when the joints are not distinguished. "Links" are represented using "link L1" to "link L6" when the links need to be distinguished, and are represented as "link L" when the links are not distinguished.

[0012] FIG. 2 is a conceptual diagram showing the configuration of the joints of the robot arm 11 of FIG. 1. As shown in FIGS. 1 and 2, the robot arm 11 is fixed at its base to a base 13. Links L1 to L6 are arranged in order from the base to the tip of the robot arm 11. Joint JT1 connects link L1 to the base 13 rotatably around axis A1. Joint JT2 connects link L2 to link L1 rotatably around axis A2. The direction of axis A2 intersects, for example, is perpendicular to, the direction of axis A1. Joint JT3 connects link L3 to link L2 rotatably around axis A3. The direction of axis A3 is along, for example, parallel to, the direction of axis A2. Joint JT4 connects link L4 to link L3 rotatably around axis A4. The direction of axis A4 intersects, for example, is perpendicular to, the direction of axis A3. Joint JT5 connects link L5 to link L4 rotatably around axis A5. The direction of axis A5 intersects, for example, is perpendicular to, the direction of axis A4. Joint JT6 connects link L6 to link L5 rotatably around axis A6. The direction of axis A6 intersects, for example, is perpendicular to, the direction of axis A5.

[0013] All of the joints JT1 to JT6 of the vertically articulated robot arm 11 are rotary joints, but at least one of the joints JT1 to JT6 may be a linear joint. For example, the joints JT1 to JT3 may be linear joints. In this case, the singular points of the robot arm 11, which will be described later, will be different.

[0014] The link L6 includes a mechanical interface at its tip and is connected to the moving unit 12 via the mechanical interface. The moving unit 12 can send and receive signals and receive power via the mechanical interface. The moving unit 12 has a structure that can apply an action to an object, and is also called an end effector. The action of the moving unit 12 is not particularly limited, but in this embodiment, it is the action of grasping an object. Hereinafter, the moving unit 12 may also be called the end effector 12.

[0015] The robot arm 11 includes drive units D1 to D6 that drive joints JT1 to JT6, respectively. As shown in FIG. 3 , drive units D1 to D6 each include motors M1 to M6 as drive sources and may further include reducers that transmit the rotational drive forces of the motors M1 to M6. FIG. 3 is a block diagram showing an example of the configuration of a robot system 1 according to an embodiment. In this embodiment, each of the motors M1 to M6 is a servo motor, and includes an electric motor and a rotation sensor E, such as an encoder, that detects the amount of rotation of the electric motor. The operation of the motors M1 to M6 is controlled by a control device 20. The control device 20 feedback-controls the motors M1 to M6 using the current values ​​applied to the motors M1 to M6 and the detection results of the respective rotation sensors E as feedback information.

[0016] As shown in FIGS. 1 and 2 , the user interface 30 is connected to the control device 20 via wired communication, wireless communication, or a combination thereof. Any wired or wireless communication may be used. The user interface 30 accepts input from an operator and outputs a signal based on the input to the control device 20. For example, the user interface 30 accepts an input of an operation for manually controlling the robot arm 11. The input is related to at least a target velocity of the end effector 12 moved by the robot arm 11. The target velocity includes a target movement direction of the end effector 12 and a target speed of the end effector 12 in the movement direction. The user interface 30 outputs a signal to the control device 20 commanding a target velocity of the end effector 12 according to the input for manual control.

[0017] For example, the user interface 30 accepts input of information related to the specifications of the robot 10 and outputs a signal representing the information to the control device 20. The information may include the number and lengths of the links L, the number and types of the joints JT, the joints JT connected to each link L, and the position and direction of the axis A of the joints JT connected to each link L.

[0018] The length of the link L may be the length of the link L itself, or may be the distance between two joints JT connected to the link L. The type of the joint JT may represent a revolute joint or a prismatic joint. The position of the axis A of the joint JT may represent the position of the rotation axis of the revolute joint or the position of the prismatic axis of the prismatic joint with respect to the link L to which the joint JT is connected. The position of the axis A of the joint JT may represent the relative position between the two axes A of the two joints JT connected to the link L, for example, may represent the distance between the two axes A. The direction of the axis A of the joint JT may represent the direction of the rotation axis of the revolute joint or the direction of the prismatic axis of the prismatic joint with respect to the link L to which the joint JT is connected. The direction of the axis A of the joint JT may represent the relative direction between the two axes A of the two joints JT connected to the link L, for example, may represent the angle between the two axes A.

[0019] The user interface 30 may be a control arm having a structure similar to the robot arm 11, a known operating device for a robot, a known teaching device for a robot such as a teach pendant, a computer such as a personal computer, a smart device such as a smartphone or tablet, a game terminal, other operating device, other terminal device, a device using these, an improved device of these, or a combination of two or more of the above devices. The user interface 30 may include, as input means, a device that receives input through operation by an operator, such as a button, a lever, a dial, a joystick, a mouse, keys, a touch panel, motion capture, or a combination of two or more of these.

[0020] In this embodiment, the user interface 30 includes a joystick. The direction of tilt of the joystick represents the direction of movement of the target of the end effector 12, and the angle of tilt of the joystick can represent the speed of the target of the end effector 12 in that direction of movement.

[0021] The control device 20 is connected to the robot 10 via wired communication, wireless communication, or a combination thereof. Any wired or wireless communication may be used. The control device 20 controls the movement of the robot 10 according to commands contained in signals received from the user interface 30. The control device 20 determines elements to be included in the control program for the robot 10 using information about the specifications of the robot 10 contained in the signals received from the user interface 30. In this embodiment, the control device 20 also controls the power supply to the robot 10, although this is not limited thereto.

[0022] 3, the control device 20 includes an information processing device 100 and a power control circuit 200. In this embodiment, the information processing device 100 and the power control circuit 200 are included in one control device 20, but are separate devices that are separate from each other and may be connected to each other via wired communication, wireless communication, or a combination thereof. Any wired communication or wireless communication may be used.

[0023] The power control circuit 200 includes one or more of an amplifier, a converter, and an inverter, and controls the power supplied to the robot 10. The power control circuit 200 supplies current to the motors M1 to M6 of the drive devices D1 to D6 of the robot arm 11 in accordance with a current value included in a command received from the information processing device 100.

[0024] The information processing device 100 includes a circuit C, which includes a processor P and a memory M. The circuit C may include a processing circuit. For example, the information processing device 100 may be an electronic circuit board, an electronic control unit, a microcomputer, a personal computer, a workstation, a smart device such as a smartphone or a tablet, or other electronic device. The memory M stores programs executed by the processor P and various data. The memory M stores a control program for the robot 10.

[0025] The information processing device 100 may further include a hard disk drive (HDD) or a solid state drive (SSD) as a storage unit. The memory M includes a random access memory (RAM) which is a volatile memory and a read-only memory (ROM) which is a non-volatile memory.

[0026] The processor P, together with the RAM and ROM, forms a computer system. The computer system may realize the functions of the information processing device 100 by the processor P using the RAM as a work area to execute a program recorded in the ROM. Some or all of the functions of the information processing device 100 may be realized by the computer system, by a dedicated hardware circuit such as an electronic circuit or an integrated circuit, or by a combination of the computer system and the hardware circuit. The information processing device 100 may be configured to execute each process under centralized control by a single device, or may be configured to execute each process under distributed control through the cooperation of multiple devices.

[0027] For example, the processor P may include, but is not limited to, a CPU (Central Processing Unit), an MPU (Micro Processing Unit), a GPU (Graphics Processing Unit), a microprocessor, a processor core, a multiprocessor, an ASIC (Application-Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), etc. The processor P may implement each process by a logic circuit or a dedicated circuit formed on an IC (Integrated Circuit) chip, an LSI (Large Scale Integration), etc. Multiple processes may be implemented by one or multiple integrated circuits, or may be implemented by a single integrated circuit.

[0028] The information processing device 100 converts the target velocity of the end effector 12, which is included in the signal received from the user interface 30, into a target velocity for each of the joints JT1 to JT6. By operating each of the joints JT1 to JT6 at the target velocity, the robot arm 11 can move the end effector 12 at the target velocity. The velocity of the joint JT includes the direction of motion of the joint JT and the speed of motion of the joint JT in that direction. The rotation angle of a rotary joint and the linear position of a linear joint are both expressed by a phase relative to the joint, and the velocity of the joint is obtained by differentiating the phase with respect to time. Hereinafter, the phase of the joint JT is represented by θ, and the velocity of the joint JT is represented by a velocity θva. The velocities θva1 to θva6 of the joints JT1 to JT6 are represented by a velocity vector θv (θv = [θva1, θva2, θva3, θva4, θva5, θva6]).

[0029] The control program stored in the memory M includes a matrix J called the Jacobian matrix. The conversion of the target velocity is performed using the inverse matrix J of the Jacobian matrix J. -1 The Jacobian matrix J is a matrix that converts the minute displacements of the joints JT1 to JT6 into minute displacements of the position and orientation of the end effector 12.

[0030] 1 and 2 , the position and orientation of the mechanical interface surface of the link L6 are expressed as the position and orientation of the end effector 12. The position and orientation of the mechanical interface surface are expressed in coordinates in a robot coordinate system Σ(X, Y, Z) based on the robot 10. Although not limited thereto, in this embodiment, the robot coordinate system Σ(X, Y, Z) has its origin at the connection portion between the base 13 and the support surface S of the base 13, and includes an X-axis and a Y-axis that extend perpendicular to each other along the support surface S, and a Z-axis that is perpendicular to the X-axis and Y-axis.

[0031] The position of the mechanical interface surface is expressed by x, y, and z, which are coordinates in the robot coordinate system Σ(X, Y, Z) of the origin of the mechanical interface coordinate system Σ(Xm, Ym, Zm), which is based on the mechanical interface surface. For example, the origin of the mechanical interface coordinate system Σ(Xm, Ym, Zm) is located at the center of the mechanical interface surface. The orientation of the mechanical interface surface is expressed by the angle r that the mechanical interface coordinate system Σ(Xm, Ym, Zm) makes with the robot coordinate system Σ(X, Y, Z). O , r A and r T The angle r is expressed as O , r A and r T may be expressed in terms of roll, pitch, and yaw angles, or in terms of Euler angles. The velocity of the end effector 12 is expressed by the element x v , y v , z v , ω ｘ , ω y and ω z For example, the velocity vector V=[x v , y v , z v , ω x , ω y , ω z ] is expressed as [x v , y v , z v ] represents the translational velocity vector, and x v , y v and z v represent the velocities in the X-axis, Y-axis, and Z-axis directions in the robot coordinate system Σ(X, Y, Z), respectively.x , ω y , ω z ] represents the angular velocity vector, and ω x , ω y and ω z represent the angular velocities around the X-axis, Y-axis, and Z-axis in the robot coordinate system Σ(X, Y, Z), respectively. The velocity vector V of the end effector 12, the velocity vectors θv of the joints JT1 to JT6, and the Jacobian matrix J are expressed as follows: θv = J -1 The relational expression of V is satisfied.

[0032] The information processing device 100 operates in accordance with a control program and determines the target speed of each of the joints JT1 to JT6 using the above relational expressions and the target speed of the end effector 12. Furthermore, the information processing device 100 determines the current values ​​to be applied to the motors M1 to M6 in order to operate the joints JT1 to JT6 at the target speeds, and outputs the current values ​​to the power control circuit 200. The information processing device 100 calculates the current value θv = J -1 Numerical control based on the relational expression of V is performed on the robot 10. That is, the information processing device 100 performs decomposition speed control using a Jacobian matrix.

[0033] The Jacobian matrix J includes the phase θ of the joint JT as a variable in its elements. The information processing device 100 calculates the phases θ1, θ2, θ3, θ4, θ5, and θ6 of the joints JT1 to JT6 using the detection result of the rotation sensor E included in the feedback information, and uses the calculated phases to calculate the Jacobian matrix J.

[0034] Furthermore, the Jacobian matrix J is a matrix that depends on the specifications of the robot 10, and can be calculated using information about the specifications of the robot 10. When the information processing device 100 receives information about the specifications of the robot 10 from another device such as the user interface 30, it stores the information in the memory M. The information processing device 100 operates according to a control program, and calculates the Jacobian matrix J using the phases θ1, θ2, θ3, θ4, θ5, and θ6 of the joints JT1 to JT6 based on feedback information and information about the specifications of the robot 10, and stores the calculated Jacobian matrix J in the memory M. Furthermore, the information processing device 100 calculates the inverse matrix J of the Jacobian matrix J. -1 The target velocity of the end effector 12 is applied to determine the target velocities of the joints JT1 to JT6.

[0035] Therefore, even if the robot 10 connected to the control device 20 is changed, the information processing device 100 can obtain information regarding the specifications of the changed robot 10, calculate the Jacobian matrix J that is compatible with the changed robot 10, and control the decomposition rate according to the changed robot 10.

[0036] Furthermore, the robot arm 11 of the robot 10 has a singularity, which is a posture at which the robot arm 11 cannot be controlled according to a calculation using the Jacobian matrix J. When the posture of the robot arm 11 is at a singularity, the robot arm 11 cannot move the end effector 12 at a speed according to the target speed output from the user interface 30. For example, at a singularity, the robot arm 11 cannot generate a velocity for the end effector 12 in a certain direction. Therefore, near the singularity, the robot arm 11 requires excessive speeds for any or all of the joints JT1 to JT6 to generate the velocity of the end effector 12 in that direction. Therefore, the information processing device 100 determines the excessive speeds required for the joints JT1 to JT6 as an error and stops the operations of the joints JT1 to JT6. Alternatively, near a singular point, the robot arm 11 may not be able to achieve a sufficient velocity of the joints JT1 to JT6 to generate a velocity of the end effector 12 in the above direction, and the trajectory of the end effector 12 may deviate from the originally commanded trajectory. Also, at a singular point, the robot arm 11 may perform self-motion, stopping the movement of a portion of the end effector 12, etc., and operating while maintaining the position of the stopped portion. Near a singular point, the robot arm 11 may move the end effector 12 at a speed that suddenly changes with respect to the target speed as the robot arm approaches the singular point.

[0037] 4 to 7 are conceptual diagrams showing examples of postures of the robot arm 11 at singular points. FIG. 4 shows a posture of the robot arm 11 when fully extended, in which joints JT1 to JT6 are aligned in a straight line. FIGS. 5 to 7 show postures of the robot arm 11 in which two or more of joints JT1 to JT6 are aligned in a straight line. In FIG. 5, joints JT2 to JT4, including joint JT3 corresponding to the elbow joint of the robot arm 11, are aligned in a straight line. This posture of the robot arm 11 is also called an elbow singular posture. In FIG. 6, joints JT3 to JT6, including joint JT5 corresponding to the wrist bending joint of the robot arm 11, are aligned in a straight line. This posture of the robot arm 11 is also called a wrist singular posture. In FIG. 7, joints JT1 and JT2 corresponding to the shoulder joint of the robot arm 11 and joint JT5 are aligned in a straight line. This posture of the robot arm 11 is also called a shoulder singular posture.

[0038] At a singular point, the inverse matrix of the Jacobian matrix J does not exist, and the determinant detJ of the Jacobian matrix J becomes 0. As the singular point is approached, the determinant detJ approaches 0. Because the inverse matrix of the Jacobian matrix J contains 1 / detJ in all elements, as the determinant detJ approaches 0, the velocity of the joint JT diverges, and the velocity of the end effector 12 may become an unintended velocity.

[0039] [Operation of Information Processing Device 100] The information processing device 100 performs control to reduce unintended movements of the robot arm 11 at and near the singular point. Figures 8 to 12 are flowcharts showing an example of processing related to the singular point of the information processing device 100 according to the embodiment.

[0040] The following steps S1 and S2 are preparatory steps for controlling the robot 10 in accordance with a command for the speed of the end effector 12 output from the user interface 30. The steps from step S3 onwards are steps related to the control of the robot 10.

[0041] In step S1, a user of the robot system 1 inputs information about the specifications of the robot 10 to the user interface 30. The user interface 30 transmits the information, and the control device 20 receives the information.

[0042] Next, in step S2, the information processing device 100 of the control device 20 stores information about the specifications of the robot 10 in the memory M.

[0043] In step S3, the operator of the robot system 1 inputs an operation to the user interface 30 to manually control the robot 10. The user interface 30 transmits a signal indicating a speed command value for the end effector 12 commanded by the operation to the control device 20. The information processing device 100 acquires the speed command value, which is a target speed of the end effector 12, from the signal. In this example, the speed command value is represented by a speed vector V. If the speed command value is not a speed vector V, the information processing device 100 may convert the speed command value into a speed vector V. For example, the information processing device 100 may acquire the speed command value for the end effector 12 at a predetermined sampling period from the signal received from the user interface 30.

[0044] Next, in step S4, the information processing device 100 receives, as feedback information, the current values ​​applied to the motors M1 to M6 of the joints JT1 to JT6 and the detection results of the rotation sensors E from the motors M1 to M6 from the power control circuit 200. For example, the information processing device 100 may acquire the feedback information at every predetermined sampling period.

[0045] Next, in step S5, the information processing device 100 calculates the phases θ1, θ2, θ3, θ4, θ5, and θ6 of the joints JT1 to JT6 using the detection results of the rotation sensor E included in the feedback information. The information processing device 100 calculates the Jacobian matrix J using the phases θ1, θ2, θ3, θ4, θ5, and θ6 and information related to the specifications of the robot 10 stored in the memory M. The Jacobian matrix J is an example of a first Jacobian matrix. Therefore, steps S4 and S5 may be performed in the reverse order.

[0046] Next, in step S6, the information processing device 100 normalizes the Jacobian matrix J.

[0047] Here, the velocity vector V of the end effector 12 is expressed as V=[x v , y v, z v , ω x , ω y , ω z ] is expressed by the velocity vector V Ne The velocity vectors θ of joints JT1 to JT6 can be normalized to v = [θva1, θva2, θva3, θva4, θva5, θva6] is the velocity vector θ vN can be normalized to vi max is the maximum value preset for the i-th element v of the velocity vector V. max is the velocity vector θ v is the maximum value that is preset for the i-th element θvai of max and θvai max may be set based on the specifications of the robot 10. ｖ and d θv are respectively 1≦d ｖ ≦m and 1≦d θv ≦n, m is the number of dimensions of the velocity vector V, which is 6 in this example, and n is the velocity vector θ v The number of dimensions is 6 in this example. The robot arm 11 is a non-redundant manipulator. Hereinafter, m=n, and both "m" and "n" are represented by "n". The velocity vector V and the velocity vector V Ne is an example of a movement command value, and the velocity vector θ v and velocity vector θ vN is an example of an operation command value.

[0048]

[0049] In addition, in the velocity vector V, [x v , y v , z v ] can be expressed as a translation vector vl, and [ω x , ω y , ω z ] can be expressed as an angular velocity vector ω. The translation vector vl and the angular velocity vector ω are in different units. Therefore, as shown in the following equation 3, the velocity vector V is calculated by multiplying the maximum norm ∥vl∥ of the translation vector vl maxand the maximum norm of the angular velocity vector ω, ||ωl|| max Using the velocity vector V N In this example, the correction coefficient d ｖ is 1≦d ｖ ≦2. I3 is a 3 row by 3 column identity matrix.

[0050]

[0051] Then, the normalized velocity vector V Ne or V N and the normalized velocity vector θ vN The Jacobian matrix J can be normalized using V = Jθ v Using the above relationship and equations 2 and 3, the Jacobian matrix J can be expressed as the Jacobian matrix J N The normalized Jacobian matrix J N Is W vN J.W. θvN -1 Hereinafter, the normalized speed vector may be expressed as a normalized speed command value, a normalized speed command, a speed command value, or a speed command.

[0052]

[0053] When the upper limit value of the velocity vector V of the end effector 12 is set appropriately, the normalized velocity vector V N The norm of ||V N || is ||V N Since ||≦1 is satisfied, if the maximum value of the joint velocity of each joint is set appropriately, the normalized velocity vector θ vN Norm of ||θ vN || is ||θ vN 1 is satisfied, the robot arm 11 can be operated in accordance with the velocity command value of the end effector 12 output from the user interface 30.

[0054] Therefore, in step S6, the information processing device 100 normalizes the velocity command value V of the end effector 12 according to Equation 1 or Equation 3 to obtain a normalized velocity vector V Ne or V NFurthermore, the information processing device 100 calculates the normalized velocity vector V Ne or V N and the Jacobian matrix J, N =W vN J.W. θvN -1 Applying this to the relationship, the normalized Jacobian matrix J N is calculated and stored in memory M.

[0055] Next, in step S7, the information processing device 100 calculates the normalized Jacobian matrix J N is decomposed into three matrices U, Σ and V θ The matrix Uv is an orthonormal system, and the column vector u of the matrix Uv is vi (i=1, . . . , n) are basis vectors of the velocity space of the end effector 12 defined by singular value decomposition. θ is an orthonormal system, and the matrix V θ The transposed matrix V is a column vector of θ Ｔ row vector v θi (i = 1, ..., n) are basis vectors of the velocity space of multiple joints JT defined by singular value decomposition. The matrix Σ is a diagonal matrix, and its diagonal elements are singular values ​​σi (i = 1, ..., n). In the matrix Σ, the diagonal elements are in the order σ1 > σ2 > ... > σ n The column vector u vi where "n" is the number of dimensions of the velocity of the end effector 12, and the row vector v θi The "n" in (1) is the number of dimensions of the velocity of the joints JT. The closer the posture of the robot arm 11 is to a singular point, the smaller the minimum singular value σ n The matrix set M1 is an example of the second Jacobian matrix. θ Ｔ is an example of a first matrix, the matrix U is an example of a second matrix, and the matrix Σ is an example of a first singular matrix. θi is an example of a first basis vector, and the vector u vi is an example of a second basis vector.

[0056]

[0057] In addition, when the posture of the robot arm 11 is in a non-singular state where it does not correspond to a singular point, the normalized Jacobian matrix J N The inverse matrix of the matrix set M1 is expressed as in the following equation 6. The inverse matrix of the matrix set M1 is an example of a second inverse Jacobian matrix. θ is an example of a first transformation matrix, and the matrix U V Ｔ is an example of a second transformation matrix, and the matrix Σ -1 is an example of a first transformation singular matrix.

[0058]

[0059] The velocity command value V of the end effector 12 is calculated using the velocity vector V N The normalized velocity vector V N is its norm ||V N || and the normalized velocity vector V N The unit vector u N Using the above, it can be expressed as shown in the following equation 7.

[0060]

[0061] Velocity vectors θ of the joints JT1 to JT6 for moving the end effector 12 according to the velocity command value V v The velocity vector θ obtained by normalizing vN is expressed as the following Equation 8 using Equations 6 and 7.

[0062]

[0063] When the posture of the robot arm 11 approaches a singular point, the normalized velocity vector θ vN Norm of ||θ vN || is ||θ vN Therefore, the normalized velocity vector θ in Equation 8 may not satisfy ||≦1. vN is expressed as a normalized velocity vector θ vN1 When changing to ||θ vN1 Therefore, in order to generate a command value for the operation of the robot arm 11, the normalized velocity vector θ vNInstead of the normalized velocity vector θ vN1 can be used, whereby decomposition rate control according to Equation 8 can be changed to decomposition rate control according to Equation 9.

[0064]

[0065] Next, in step S8, the information processing device 100 calculates the normalized command speed V N The norm of ||V N || is the threshold ε V The information processing device 100 determines whether the norm ∥V N || is the threshold ε V If it is equal to or greater than this (Yes in step S8), the process proceeds to step S9, and the norm ∥V N || is the threshold ε V If it is less than (No in step S8), proceed to step S10. N || is the threshold ε V The normalized velocity vector V is N If the magnitude of is too small, the unit vector u N u is a normalization calculation that calculates N =V N / ||V N Therefore, the information processing device 100 determines whether the norm ∥V N || is the threshold ε V Any speed command value less than this value will not be accepted and will be regarded as zero.

[0066] In step S10, the information processing apparatus 100 sets the speed command value V of the end effector 12 to 0, and performs disassembly speed control in accordance with state 0. As shown in Fig. 9, step S10 includes steps S1001 to S1008, the details of which will be described later.

[0067] Furthermore, in step S9, the information processing device 100 determines whether the posture of the robot arm 11 is at a singularity or in an area near a singularity. If the posture of the robot arm 11 is at a singularity or in an area near a singularity (Yes in step S9), the information processing device 100 proceeds to step S11, and if the posture of the robot arm 11 is not at a singularity or in an area near a singularity (No in step S9), the information processing device 100 proceeds to step S12.

[0068] As the posture of the robot arm 11 approaches the singular point, 1 / σ in Equation 8 n becomes large, and eventually it becomes impossible to numerically calculate the inverse matrix of the Jacobian matrix. For this reason, in step S9, the information processing device 100 n and threshold ε σn The information processing device 100 performs processing based on the comparison result with σ n <ε σn If σ is σ, it is determined that the posture of the robot arm 11 is in a singular point or in a region near the singular point. n ≧ε σn If the threshold ε is 0, it is determined that the posture of the robot arm 11 is not at a singular point or in a region near the singular point. σn is an example of the first threshold value.

[0069] Threshold ε σn is the smallest singular value σ n For example, the threshold ε σn is the normalized velocity vector θ expressed as in Equation 8. vN Norm of ||θ vN ||, ||θ vN The smallest singular value σ such that ||>1 n It may be set based on the rule of thumb.

[0070] Threshold ε σn is κ = σ1 / σ n For example, the reciprocal of the condition number κ, 1 / κ (=σ n A threshold value 1 / κ0 can be set for the threshold ε σn can be set based on 1 / κ0, for example, ε σn =σ1 / κ0.

[0071] In step S12, the information processing apparatus 100 performs decomposition speed control in accordance with the non-singular (NS) state, which is decomposition speed control in a normal state. As shown in Fig. 10, step S12 includes steps S1201 to S1209, the details of which will be described later.

[0072] In step S11, the information processing device 100 calculates the normalized command velocity vector V of the end effector 12 based on Equation 8. N The basis vector u of the velocity space of the end effector 12 vn Contribution in the direction of w n is calculated as shown in the following equation 10. N is the normalized velocity vector V N is a unit vector of the contribution w n The range that can be taken is 0≦w n ≦1. Contribution w n indicates the degree to which the direction of the command velocity of the end effector 12 contributes to the directional component of the velocity of the end effector 12 that cannot be realized by the robot arm 11 in a singular point state. n The closer to 1, the greater the contribution.

[0073]

[0074] Next, in step S13, the information processing device 100 calculates the contribution w n is the threshold ε wn The information processing device 100 determines whether the contribution degree w n is the threshold ε wn If it is equal to or greater than this (Yes in step S13), the process proceeds to step S14, and the contribution w n is the threshold ε wn If it is less than ε (No in step S13), the process proceeds to step S15. wn is 0 < ε wn The threshold value ε is set in the range of 1. wn is an example of the third threshold value.

[0075] In step S14, the information processing device 100 performs decomposition speed control in accordance with a singularity consistent (SC) state. The SC state is decomposition speed control in a state where the contribution at a singularity point or in a region near the singularity point is equal to or greater than a threshold. As shown in FIG. 11 , step S14 includes steps S1401 to S1429, the details of which will be described later.

[0076] In step S15, the information processing device 100 performs decomposition speed control in accordance with state P (Perpendicular). State P is decomposition speed control in a state where the contribution at a singular point or an area near the singular point is smaller than a threshold. As shown in FIG. 12, step S15 includes steps S1501 to S1511, the details of which will be described later.

[0077] 9 will be used to describe the details of step S10 relating to state 0. First, in step S1001, the information processing device 100 sets the speed command value V of the end effector 12 to 0, determines a speed command value for stopping the operations of the joints JT1 to JT6, and outputs to the power control circuit 200 a command value for stopping the rotation of the motors M1 to M6 of the joints JT1 to JT6.

[0078] Next, in step S1002, the power control circuit 200 stops the rotation of the motors M1 to M6, thereby stopping the operation of the robot arm 11.

[0079] Next, in step S1003, the information processing apparatus 100 acquires a new speed command value for the end effector 12 from a signal received from the user interface 30, similar to step S3.

[0080] Next, in step S1004, the information processing device 100 acquires new feedback information of the robot 10, similar to step S4.

[0081] Next, in step S1005, the information processing apparatus 100 calculates the Jacobian matrix J using the new speed command value and the new feedback information, similarly to step S5.

[0082] Next, in step S1006, the information processing apparatus 100 normalizes the Jacobian matrix J in the same manner as in step S6.

[0083] Next, in step S1007, the information processing device 100 calculates the normalized Jacobian matrix J N is subjected to singular value decomposition.

[0084] Next, in step S1008, the information processing device 100 calculates the normalized command speed V N The norm of ||V N || is the threshold ε V The information processing device 100 determines whether the norm ∥V N || is the threshold ε V If it is equal to or greater than this (Yes in step S1008), the process proceeds to step S9, and the norm ∥V N || is the threshold ε V If it is less than the predetermined value (No in step S1008), the process returns to step S1001 and the subsequent processes are repeated.

[0085] Details of step S12 relating to state NS will be described with reference to FIG. 10. First, in step S1201, the information processing device 100 generates velocity command values ​​for the joints JT1 to JT6 through processing in accordance with the resolved velocity control of state NS. The information processing device 100 calculates the velocity command value V of the end effector 12 by dividing the velocity vector V by Equation 3. N Furthermore, the information processing device 100 normalizes the normalized velocity vector V N 9 to obtain the normalized velocity vectors θ of the joints JT1 to JT6. vN1 The information processing device 100 calculates the normalized velocity vector θ vN1 By returning to the non-normal form, the velocity command values ​​θ v This ensures that the speeds of the joints JT1 to JT6 are equal to or lower than the upper limit speeds set for the joints JT1 to JT6.

[0086] The information processing device 100 v =J -1According to the relational expression of V, the velocity command value V of the end effector 12 is applied to the inverse matrix of the Jacobian matrix J, and the velocity command values ​​θ of the joints JT1 to JT6 are calculated. v may be generated.

[0087] Next, in step S1202, the information processing apparatus 100 calculates the velocities of the joints JT1 to JT6 obtained from the detection results of the rotation sensor E included in the feedback information and the velocity command value θ v and the current values ​​of the motors M1 to M6 included in the feedback information, the speed command value θ v The information processing device 100 determines the current values ​​to be applied to the motors M1 to M6 to operate the joints JT1 to JT6 according to the current values. The information processing device 100 outputs a command for the determined current value to the power control circuit 200, and the power control circuit 200 drives the motors M1 to M6 according to the determined current value, thereby operating the robot arm 11.

[0088] Next, in step S1203, the information processing apparatus 100 acquires a new speed command value for the end effector 12 from a signal received from the user interface 30, similar to step S1003.

[0089] Next, in steps S1204 to S1207, the information processing apparatus 100 performs the same processing as in steps S1004 to S1007 using the new speed command value.

[0090] Next, in step S1208, the information processing apparatus 100 calculates the normalized command speed V N The norm of ||V N || is the threshold ε V The information processing device 100 determines whether the norm ∥V N || is the threshold ε V If it is equal to or greater than this (Yes in step S1208), the process proceeds to step S1209, and the norm ∥V N || is the threshold ε V If it is less than the predetermined value (No in step S1208), the process proceeds to step S1001 of step S10.

[0091] Next, in step S1209, the information processing device 100 determines whether the posture of the robot arm 11 is at a singularity or in an area near a singularity, similar to step S9. If the posture of the robot arm 11 is at a singularity or in an area near a singularity (Yes in step S1209), the information processing device 100 proceeds to step S11, and if the posture of the robot arm 11 is not at a singularity or in an area near a singularity (No in step S1209), the information processing device 100 returns to step S1201 and repeats the subsequent processes.

[0092] Details of step S14 relating to the state SC will be described with reference to FIG. 11. First, in step S1401, the information processing device 100 performs processing in accordance with the decomposition rate control of the state SC. The information processing device 100 calculates the normalized Jacobian matrix J N Inverse matrix J N -1 is transformed into a new matrix J as shown in the following Equation 11: εN Calculate the matrix J εN is also a normalized matrix. Specifically, the information processing device 100 calculates the inverse matrix J N -1 The singular value elements 1 / σ1, ..., 1 / σ n In order to maintain the ratio between N -1 matrix Σ in -1 All elements of σn / ε σn Multiply by the element 1 / σ i (i=1,...,n) is σ n / σ i ε σn Furthermore, σ n / σ i ε σn to 1 / σ i ’ The matrix J εN is an example of the third inverse Jacobian matrix, and the matrix J εN The inverse matrix of is an example of the third Jacobian matrix. N -1 From matrix J εN is an example of the first transformation, and σn is an example of a first ratio.

[0093]

[0094] Here, for example, the information processing device 100 can perform processing according to the decomposition speed control shown in the following equation 12 based on equation 11, similar to equation 9. The information processing device 100 calculates the normalized velocity vector V corresponding to the velocity command value of the end effector 12 at the singular point or its vicinity. N In response to the above, a normalized velocity vector θ corresponding to the velocity command values ​​of the joints JT1 to JT6 is calculated. vNA Then, the normalized velocity vector θ vNA Norm of ||θ vNA || is ||θ vNA ||≦1 is guaranteed.

[0095]

[0096] In addition, when the posture of the robot arm 11 is in a singular state, the matrix J in Equation 12 is εN is a matrix as shown in the following equation 13. This corresponds to the null space that causes the robot arm 11 to perform self-motion at the singular point.

[0097]

[0098] In the decomposition rate control according to Equation 12, the norm ||J εN u N If || becomes too small, the normalization may become numerically unstable. This is because the robot arm 11 approaches a singular state, and 1 / σ in Equation 12 i ’ (i=1,...,n-1) approaches 0, and the contribution w n corresponds to the state where the contribution w approaches 0. For example, in a singular state, n If is 0, then the norm ||J εN u N || becomes 0.

[0099] Therefore, in step S1402, the information processing device 100 calculates the normalized matrix J εN and the normalized command velocity vector V of the end effector 12 N Norm with ||J εN u N || is the threshold κθ It is determined whether the threshold value κ is equal to or greater than the threshold value κ. θ is the normalized velocity vector V of the end effector 12 N The unit vector u N and normalization matrix J εN The information processing device 100 determines the norm of the product of the norm ∥J εN u N || is the threshold κ θ If it is equal to or greater than this (Yes in step S1402), the process proceeds to step S1410, and the norm ∥J εN u N || is the threshold κ θ If it is less than the threshold value κ (No in step S1402), the process proceeds to step S1420. θ is an example of the second threshold, and the norm ||J εN u N || is an example of a second norm.

[0100] In step S1410, the information processing device 100 performs processing in accordance with the decomposition speed control of the state SC-1. εN Similarly to step S1201, the information processing device 100 generates velocity command values ​​for the joints JT1 to JT6 according to the resolved velocity control shown in Equation 12 using the velocity vector V N Furthermore, the information processing device 100 normalizes the normalized velocity vector V N 12 to obtain the normalized velocity vectors θ of the joints JT1 to JT6. vNA The matrix J in Equation 11 is calculated. εN The singular value elements, which are diagonal elements in the vector θ, do not diverge even when the robot arm 11 approaches a singular state. vNA Norm of ||θ vNA || is ||θ vNA ||≦1 is satisfied.

[0101] Furthermore, the matrix J in Equation 11 εN In this case, the ratio between the diagonal singular value elements is the normalized Jacobian matrix J N Inverse matrix J N -1Therefore, the ratio between the singular value elements in the normalized velocity vector θ vNA The direction in which the robot arm 11 moves the end effector 12 by operating the joints JT1 to JT6 in accordance with the normalized velocity vector θ calculated in step S1201 of the resolved velocity control of the state NS is vN By operating the joints JT1 to JT6 in accordance with the above, the direction in which the robot arm 11 moves the end effector 12 does not change. Therefore, the robot arm 11 can move the end effector 12 in the direction of the command speed of the end effector 12 without causing the end effector 12 to deviate from the target trajectory near the singular point and without suddenly stopping due to an error in which the joint speed exceeds the upper limit at the singular point.

[0102] Next, in step S1411, the information processing device 100 determines the current values ​​to be applied to the motors M1 to M6 for operating the joints JT1 to JT6 in accordance with the velocity command value, as in step S1202. The information processing device 100 outputs commands for the determined current values ​​to the power control circuit 200, and the power control circuit 200 drives the motors M1 to M6 in accordance with the determined current values, causing the robot arm 11 to operate.

[0103] Next, in steps S1412 to S1416, the information processing apparatus 100 acquires a new velocity command value for the end effector 12 from the signal received from the user interface 30, similarly to steps S1203 to S1207, and calculates the normalized Jacobian matrix J N Calculate and perform singular value decomposition.

[0104] Next, in step S1417, the information processing apparatus 100 calculates the normalized command speed V N The norm of ||V N || is the threshold ε V The information processing device 100 determines whether the norm ∥V N || is the threshold ε V If it is equal to or greater than this (Yes in step S1417), the process proceeds to step S1418, and the norm ∥V N || is the threshold ε VIf it is less than the predetermined value (No in step S1417), the process proceeds to step S1001 of step S10.

[0105] In step S1418, similarly to step S1401, the information processing apparatus 100 calculates the normalized Jacobian matrix J calculated in steps S1412 to S1416. N Inverse matrix J N -1 is transformed into a new matrix J εN Calculate.

[0106] Next, in step S1419, the information processing device 100 calculates the normalized matrix J εN and the normalized command velocity vector V of the end effector 12 N Norm with ||J εN u N || is the threshold κ θ The information processing device 100 determines whether the norm ∥J εN u N || is the threshold κ θ If it is less than (Yes in step S1419), the process returns to step S1402, and the norm ∥J εN u N || is the threshold κ θ If so (No in step S1419), the process returns to step S1410 and the subsequent processes are repeated.

[0107] In step S1420, the information processing device 100 performs processing in accordance with the decomposition speed control of the state SC-2. The information processing device 100 performs processing in accordance with the decomposition speed control shown in the following equation 14, instead of equation 12. In equation 14, the norm ∥J εN u N || is the threshold κ θ This fixes the norm ||J εN u N The information processing device 100 suppresses instability in the numerical calculation of the joint velocity when || becomes too small. εN and the norm ||J εN u N Velocity command values ​​for the joints JT1 to JT6 are generated according to the resolved velocity control shown in Equation 14 with ∥ fixed.

[0108]

[0109] Similarly to step S1410, the information processing apparatus 100 calculates the normalized velocity vector V of the velocity command value V of the end effector 12. N 14 to obtain the normalized velocity vectors θ of the joints JT1 to JT6. vNB Calculate the normalized velocity vector θ vNB Each element of is the norm ||J εN u N The velocity corresponding to the magnitude of || is shown. Normalized velocity vector θ vNB Each element of is the norm ||J εN u N As || becomes smaller, the velocities of joints JT1 to JT6 are successively and gradually decreased.

[0110] And the normalized velocity vector θ vNB Norm of ||θ vNB || is ||θ vNB ∥≦1. Furthermore, similarly to step S1410, the normalized velocity vector θ vNB The direction in which the robot arm 11 moves the end effector 12 by operating the joints JT1 to JT6 in accordance with the normalized velocity vector θ calculated in step S1201 of the resolved velocity control of the state NS is vN Therefore, the robot arm 11 can move the end effector 12 in the direction of the command speed of the end effector 12 without causing the end effector 12 to deviate from the target trajectory near the singular point and without suddenly stopping due to an error in which the joint speed exceeds the upper limit at the singular point.

[0111] Next, in step S1421, the information processing device 100 determines the current values ​​to be applied to the motors M1 to M6 for operating the joints JT1 to JT6 in accordance with the velocity command value, as in step S1411. The information processing device 100 outputs commands for the determined current values ​​to the power control circuit 200, and the power control circuit 200 drives the motors M1 to M6 in accordance with the determined current values, causing the robot arm 11 to operate.

[0112] Next, in steps S1422 to S1426, the information processing apparatus 100 acquires a new velocity command value for the end effector 12 from the signal received from the user interface 30, similarly to steps S1412 to S1416, and calculates the normalized Jacobian matrix J N Calculate and perform singular value decomposition.

[0113] Next, in step S1427, the information processing apparatus 100 calculates the normalized command speed V N The norm of ||V N || is the threshold ε V If it is equal to or greater than this (Yes in step S1427), the process proceeds to step S1428, and the norm ∥V N || is the threshold ε V If it is less than the predetermined value (No in step S1427), the process proceeds to step S1001 of step S10.

[0114] In step S1428, similarly to step S1418, the information processing apparatus 100 calculates the normalized Jacobian matrix J calculated in steps S1422 to S1426. N Inverse matrix J N -1 is transformed into a new matrix J εN Calculate.

[0115] Next, in step S1429, the information processing device 100 calculates the normalized matrix J εN and the normalized command velocity vector V of the end effector 12 N Norm with ||J εN u N || is the threshold κ θ The information processing device 100 determines whether the norm ∥J εN u N || is the threshold κ θ If it is equal to or greater than this (Yes in step S1429), the process returns to step S1402, and the norm ∥J εN u N || is the threshold κ θ If it is less than the predetermined value (No in step S1429), the process returns to step S1420 and the subsequent processes are repeated.

[0116] 12, the details of step S15 regarding the state P will be described. First, in step S1501, the information processing apparatus 100 calculates the normalized matrix J in Equation 11 in step S1401. εN is a matrix obtained by transforming εNA Generate the matrix J εNA In the above transformation, the normalized matrix J εN element 1 / ε σn is replaced by 0. Matrix J εNA is an example of the fourth inverse Jacobian matrix.

[0117]

[0118] Contribution rate w n is the threshold ε wn In the state where the velocity is sufficiently small, the directional component of the command velocity of the end effector 12 that cannot be generated due to the singular state is very small. Such a directional component of the velocity is expressed by the singular value element 1 / ε σn For example, the normalized matrix J in Equation 11 in step S1401 is a velocity component related to εN Normalized velocity vector θ vNA If you generate a normalized velocity vector θ vNA can realize the movement of the end effector 12 in the direction of the command velocity of the end effector 12, but the magnitude of the velocity of the end effector 12 is very small.

[0119] On the other hand, contribution w n is 0, there is no directional component of the command velocity of the end effector 12 that cannot be generated due to a singular state. σn Therefore, the above transformation is performed by n When ε is sufficiently small, the element 1 / ε is related to the directional component of the velocity of the end effector 12 that cannot be generated due to a singular state. σn By setting θ to 0, such directional components are discarded, thereby enabling the commanded velocity of the end effector 12 to be realized as is.

[0120] According to Equation 15, the singular value element 1 / ε σn This means that there is no command velocity in the direction of the normalized velocity vector V N is converted into a new velocity vector V as shown in the following equation 16. NA is equivalent to changing it to

[0121]

[0122] Next, in step S1502, the information processing apparatus 100 calculates the normalized matrix J εNA Similarly to step S1201, the information processing device 100 generates the velocity command values ​​of the joints JT1 to JT6 according to the decomposed velocity control shown in the following equation 17 using N Applying Equation 17 to obtain the normalized velocity vectors θ of the joints JT1 to JT6. vNC Calculate.

[0123]

[0124] Normalized velocity vector θ vNC is the singular value element 1 / ε in the command velocity (hand velocity) of the end effector 12. σn By regarding the velocity component in the direction as non-existent, the number of dimensions of the hand velocity is reduced by one, and as a result, the dimension is reduced and the normalized velocity vector θ corresponds to the velocity command value for the robot arm 11 that is no longer in a singular state. vNC Even if the robot arm 11 is in a singular state, the command velocity of the end effector 12 can be approximately realized by the robot arm 11. Furthermore, the normalized velocity vector θ vNC Norm of ||θ vNC || is ||θ vNC||≦1 is satisfied. Therefore, the robot arm 11 can move the end effector 12 in the direction of the command velocity of the end effector 12 without causing the end effector 12 to deviate from the target trajectory near the singular point and without suddenly stopping due to an error in which the joint velocity exceeds the upper limit value at the singular point. After step S1502, the information processing device 100 proceeds to step S1503.

[0125] Here, the direction of the command velocity of the end effector 12 changes according to the operation of the user interface 30 by the operator, and the contribution w n is the threshold ε wn The threshold ε wn Contribution w n is the threshold ε wn In the above state, for example, the robot arm 11 moves in accordance with the normalized velocity vector θ of Equation 12 in step S1401. vNA or the normalized velocity vector θ of Equation 14 in step S1420 vNB The end effector 12 is moved at a speed according to the contribution w n As the contribution w decreases, the velocity of the end effector 12 decreases. n is the threshold ε wn When the normalized velocity vector θ vNC The end effector 12 is moved at a speed according to the

[0126] As mentioned above, the normalized velocity vector θ vNC corresponds to a velocity command value for the robot arm 11 in which the number of dimensions of the velocity is reduced by one and which is not in a singular state, and can approximately realize the command velocity of the end effector 12. Therefore, the contribution w n is the threshold ε wnWhen the speed becomes less than 1 / 2, the robot arm 11 may transition to state P, and the end effector 12 of the robot arm 11 may suddenly move at a certain speed. However, as can be seen from Figures 10 to 12, in order to transition from state SC to state P, it is necessary to first transition to state 0 or state NS, so that the end effector 12 does not suddenly start moving. Furthermore, the information processing device 100 can notify the operator of the speed change and prompt them to adjust the operation of the user interface 30.

[0127] In step S1503, similarly to step S1202, the information processing device 100 determines the current values ​​to be applied to the motors M1 to M6 for operating the joints JT1 to JT6 in accordance with the velocity command value. The information processing device 100 outputs commands for the determined current values ​​to the power control circuit 200, and the power control circuit 200 drives the motors M1 to M6 in accordance with the determined current values ​​to operate the robot arm 11.

[0128] Next, in steps S1504 to S1508, the information processing apparatus 100 acquires a new velocity command value for the end effector 12 from the signal received from the user interface 30, similarly to steps S1203 to S1207, and calculates the normalized Jacobian matrix J N Calculate and perform singular value decomposition.

[0129] Next, in step S1509, the information processing apparatus 100 calculates the normalized command speed V N The norm of ||V N || is the threshold ε V If it is equal to or greater than this (Yes in step S1509), the process proceeds to step S1510, and the norm ∥V N || is the threshold ε V If it is less than the predetermined value (No in step S1509), the process proceeds to step S1001 of step S10.

[0130] In step S1510, the information processing apparatus 100 calculates the normalized command velocity vector V of the end effector 12 for the velocity command value V of the end effector 12 acquired in steps S1504 to S1508, in the same manner as in step S11. N The smallest singular value σn The basis vector u associated with vn Contribution in the direction of w n Calculate.

[0131] Next, in step S1511, the information processing apparatus 100 calculates the smallest singular value σ obtained by the singular value decomposition in steps S1504 to S1508. n is the threshold ε σn or more and contribution w n is the threshold ε wn If the condition is satisfied (Yes in step S1511), the information processing apparatus 100 proceeds to step S12, and if the condition is not satisfied (No in step S1511), the information processing apparatus 100 returns to step S1501 and repeats the subsequent processes. If the above condition is satisfied, the smallest singular value σ n The velocity component in the direction related to is small, and the smallest singular value σ n is not a singular state. Therefore, the robot arm 11 can get out of state P.

[0132] By steps S3 to S15, the information processing device 100 can control the robot arm 11 to move the end effector 12 in the direction of the command speed of the end effector 12 without causing the end effector 12 to deviate from the target trajectory near the singular point, without causing an error in which the joint speed exceeds the upper limit value at the singular point, and while transitioning the decomposition speed control method between state 0, state NS, state SC, and state P.

[0133] State 0 is a state in which the command velocity of the end effector 12 is less than the threshold value, and the velocity command value of the end effector 12 is regarded as 0, causing the robot arm 11 to stop. State NS is a state in which a velocity command value of the joint JT can be generated by normal resolved velocity control. State SC is a state in which a velocity command value of the joint JT can be generated by resolved velocity control corresponding to a state in which the robot arm 11 is close to or in a singular state. State P is a state in which the command velocity of the end effector 12 is greater than the singular value σ n The command direction u corresponding to vnThis is a state in which a velocity command value for the joint JT can be generated by decomposition velocity control corresponding to a state in which the component of

[0134] The information processing device 100 controls the transition of the decomposition rate control between the state NS and the state SC based on the minimum singular value σ n and threshold ε σn The relationship between n ≧ε σn Is it σ? n <ε σn For example, σ n <ε σn And the contribution w n But w n ≧ε wn If the above condition is satisfied, the information processing device 100 determines a transition to the decomposition speed control of the state SC. Therefore, the continuity of the transition between the state NS and the state SC is maintained. Furthermore, the information processing device 100 determines a transition to the decomposition speed control of the state SC by using the norm ∥J εN u N || and threshold κ θ The relationship between ||J εN u N ||≧κ θ Is it ||J εN u N ||<κ θ The information processing device 100 determines the transition of the decomposition speed control between the two states depending on whether the norm ∥J εN u N || is the threshold κ θ Since the decomposition rate is controlled so that the temperature does not fall below , the continuity of the transition between the two states is maintained.

[0135] The information processing device 100 calculates the transition between the state NS and the state P using the contribution w n and threshold ε wn The relationship with n ≧ε wn Or so lol n <ε wn It can be determined depending on whether n <ε wn and the smallest singular value σ n and threshold ε σn The relationship between n <ε σnIf the above condition is satisfied, the information processing apparatus 100 transitions from the state NS to the decomposition speed control of the state P, and calculates the singular value σ n The command direction u corresponding to vn components are removed, but the threshold ε wn By setting r to a small value, continuity of the transition between the two states is maintained.

[0136] In the state P, the command velocity of the end effector 12 is changed to the command direction u vn Since the component of is removed, the contribution w n But w n ≧ε wn Even if the above relationship is satisfied, the velocity of the end effector 12 actually commanded to the robot arm 11 is in the direction u vn Therefore, the information processing device 100 removes the component w n <ε wn And σ n ≧ε σn If the following is satisfied, the state transitions from state P to state NS, which is decomposition rate control. In this case, continuity of the transition between the two states is maintained.

[0137] The information processing device 100 does not allow a direct transition between state SC and state P. To transition from state SC to state P, the command speed of the end effector 12 is temporarily set to 0 to transition to state 0, and the robot arm 11 is temporarily stopped. Then, the contribution w n But w n <ε wn It is possible to transition to state P by suddenly issuing a speed command value for the end effector 12 that satisfies the relationship above. Note that the information processing device 100 can also transition to the disassembly speed control of state P after once transitioning to the disassembly speed control of state NS.

[0138] Conversely, to transition from state P to state SC, the command speed of the end effector 12 is temporarily set to 0 to transition to state 0, the robot arm 11 is temporarily stopped, and then the contribution w n But w n ≧ε wnThe state can be transitioned to state SC by issuing a speed command value for the end effector 12 that satisfies the relationship shown below. Note that the information processing device 100 can also transition from state P to the disassembly speed control of state NS, and then to the disassembly speed control of state SC.

[0139] [Verification of Guarantee of Upper Limit of Movement Speed ​​of End Effector 12] In steps S1 to S15, the information processing device 100 calculates a normalized velocity vector θ corresponding to the velocity command value of the joint JT. vN Norm of ||θ vN The magnitude of || is ||θ vN The normalized velocity vector θ is vN However, depending on the posture of the robot arm 11, the normalized velocity vector θ vN It is possible that the magnitude of the velocity of the end effector 12 realized by the velocity command value in a non-normalized form may exceed the upper limit of the command velocity of the end effector 12. Therefore, the normalized velocity vector of the joint JT obtained by Equation 9, Equation 12, Equation 14, and Equation 17 will be verified to verify whether the upper limit of the moving velocity of the end effector 12 is guaranteed.

[0140] First, in the state SC, the normalized velocity vector θ obtained by Equation 9 is vN1 In the case of , the normalized velocity vector θ vN1 The normalized velocity vector V of the end effector 12 is realized by NR is obtained as shown in the following equation 18. Furthermore, as shown in the following equation 19, the normalized velocity vector V NR The norm of ||V NR || is the normalized command velocity vector V of the end effector 12 N The norm of ||V N It may be larger than ||.

[0141]

[0142] In this case, the normalized velocity vector θ is calculated by the following equation 20: vN1 can be corrected. The norm ||V NR is an example of the first norm, and the corrected normalized velocity vector θ vN1 is an example of a correction operation command value.

[0143]

[0144] Note that the following equation 21 is obtained from equation 9, and the following equation 22 is obtained from equation 21. Furthermore, by applying equations 22 and 9 to equation 20, the following equation 23 is obtained.

[0145]

[0146] Therefore, the normalized velocity vector θ corrected by Equation 20 vN1 is the normalized command velocity vector V of the end effector 12 N corresponds to the normalized velocity vector of the joint JT obtained by applying normal decomposition velocity control to the norm ||V NR || is the norm ||V N ||It can be confirmed that the following is true.

[0147] Next, in the state SC, the normalized velocity vector θ obtained by Equation 12 is vNA In the case of , the normalized velocity vector θ vNA The normalized velocity vector V of the end effector 12 is realized by NRA is obtained as shown in the following equation 24. Furthermore, as shown in the following equation 25, the normalized velocity vector V NRA The norm of ||V NRA || is the normalized command velocity vector V of the end effector 12 N The norm of ||V N It may be larger than ||.

[0148]

[0149] In this case, the normalized velocity vector θ vNA can be corrected. The norm ||V NRA is an example of the first norm, and the corrected normalized velocity vector θ vNA is an example of a correction operation command value.

[0150]

[0151] The normalized matrix J in Equation 12 εN is the normalized Jacobian matrix J N Inverse matrix JN -1 , the relationship of the following Equation 27 is satisfied. Then, the following Equation 28 is obtained from Equation 26 and Equation 27. Therefore, the normalized velocity vector θ corrected by Equation 26 vNA is the normalized command velocity vector V of the end effector 12 N corresponds to the normalized velocity vector of the joint JT obtained by applying normal decomposition velocity control to the norm ||V NRA || is the norm ||V N ||It can be confirmed that the following is true.

[0152]

[0153] Next, in the state SC, the normalized velocity vector θ obtained by Equation 14 is vNB In the case of , the normalized velocity vector θ vNB The normalized velocity vector V of the end effector 12 is realized by NRB is obtained as shown in the following equation 29. Furthermore, as shown in the following equation 30, the normalized velocity vector V NRB The norm of ||V NRB || is the normalized command velocity vector V of the end effector 12 N The norm of ||V N It may be larger than ||.

[0154]

[0155] In this case, the normalized velocity vector θ is expressed as follows: vNB can be corrected. The norm ||V NRB is an example of the first norm, and the corrected normalized velocity vector θ vNB is an example of a correction operation command value.

[0156]

[0157] The normalized matrix J in Equation 14 εN is the normalized Jacobian matrix J N Inverse matrix J N -1 , the relationship of Equation 27 is satisfied. Then, from Equation 27 and Equation 31, the following Equation 32 is obtained. Therefore, the normalized velocity vector θ corrected by Equation 31vNB is the normalized command velocity vector V of the end effector 12 N corresponds to the normalized velocity vector of the joint JT obtained by applying normal decomposition velocity control to the norm ||V NRB || is the norm ||V N ||It can be confirmed that the following is true.

[0158]

[0159] Next, in state P, the normalized velocity vector θ obtained by Equation 17 is vNC In the case of , the normalized velocity vector θ vNC The normalized velocity vector V of the end effector 12 is realized by NRC is obtained as shown in the following equation 33. Furthermore, as shown in the following equation 34, the normalized velocity vector V NRC The norm of ||V NRC || is the normalized command velocity vector V of the end effector 12 N The norm of ||V N It may be larger than ||.

[0160]

[0161] In this case, the normalized velocity vector θ is expressed as in the following equation 35. vNC can be corrected. The norm ||V NRC is an example of the first norm, and the corrected normalized velocity vector θ vNC is an example of a correction operation command value.

[0162]

[0163] Then, the following equation 36 is obtained from equations 16, 17, and 27. Furthermore, the norm ∥J N θ vNC || is obtained as shown in the following equation 37.

[0164]

[0165] Therefore, by substituting Equations 17 and 37 into Equation 35, the following Equation 38 is obtained.

[0166]

[0167] In Equation 38, the normalized matrix J ’ εNA is the usual normalized Jacobian matrix J N Inverse matrix J N -1 In this case, the smallest singular value σ n element 1 / σ n The inverse matrix J N -1 The normalized matrix J is the pseudo-inverse matrix of ’ εNA is the normalized matrix J εNA The normalized velocity vector θ of the joint JT corrected by Equation 38 is similar to vNC corresponds to the normalized velocity vector of the joint JT obtained by applying the decomposition velocity control using such a pseudo-inverse matrix.

[0168] normalization matrix J ’ εNA The state P to which the n is the threshold ε wn Since it applies when it is less than ||V| NA || is the norm ||V N asymptotically equal to ||. Therefore, ||V N || / ||V NA The effect of || is negligible.

[0169] pseudo inverse matrix J ’ εNA The solution is the normalized command velocity vector V of the end effector 12. N Therefore, the normalized velocity vector θ of the joint JT obtained by Equation 38 is vNC The norm of the velocity of the end effector 12 ||V NRC The magnitude of || is the normalized command velocity vector V of the end effector 12. N The norm of ||V N It never exceeds the magnitude of ||.

[0170] From the above, it can be confirmed that in both the state SC and the state P, the magnitude of the velocity of the end effector 12 realized by the velocity command values ​​of the joints JT1 to JT6 output by the information processing device 100 does not exceed the magnitude of the command velocity of the end effector 12.

[0171] [Processing for Self-Motion in Singular State] In the Jacobian matrix J, the smallest singular value σ as shown in Equation 5 is n may become 0. In such a case, the robot arm 11 is in a completely singular state. The normalized Jacobian matrix J N is expressed as the following Equation 39.

[0172]

[0173] The basis vector v of the velocity space of the joint JT θn In the singular state, the vector u contained in the velocity command value V of the end effector 12 becomes a null space in the velocity space of the joint JT that does not affect the velocity of the end effector 12. Vn In response to a velocity command value in the direction of the arrow, a velocity command value for the joint JT in the null space is output. Self-motion occurs in the operation of the robot arm 11 in accordance with such a velocity command value for the joint JT.

[0174] The robot arm 11 can continue the self-motion in the wrist singular posture and shoulder singular posture shown in Figures 6 and 7. In the elbow singular posture shown in Figure 5, the robot arm 11 may escape from the singular point due to the self-motion. In the elbow singular posture, the vector u VnWhen a velocity command value for the end effector 12 in the direction continues to be given, the robot arm 11 reaches a singular point once as shown in Figures 13A and 13B, and then escapes from the singular point by self-motion as shown in Figure 13C, moving the end effector 12 in the direction opposite to the target direction. After that, the robot arm 11 again moves the end effector 12 toward the singular point as shown in Figure 13C, and reaches the state of Figure 13B. Thereafter, the robot arm 11 repeats the states from Figure 13A to Figure 13C. This induces a vibration phenomenon in the robot arm 11. Figures 13A to 13C are conceptual diagrams showing an example of self-motion of the robot arm 11 in the case of an elbow singular posture.

[0175] Therefore, in order to prevent such vibration, the information processing device 100 temporarily stops all movements of the joints JT1 to JT6 of the robot arm 11 when the robot arm 11 reaches the state shown in FIG. 13B . For example, if only the joint JT related to self-motion among the joints JT1 to JT6 of the robot arm 11 is stopped to prevent vibration, the robot arm 11 may move the end effector 12 in a direction different from the commanded speed of the end effector 12. The information processing device 100 prevents the end effector 12 from moving in such an unintended direction. Then, the information processing device 100 operates the robot arm 11 to deviate from the elbow peculiar posture in accordance with a command from the user interface 30 or autonomously. Note that in a singular state other than the elbow peculiar posture, the information processing device 100 may temporarily stop the robot arm 11 and then deviate from the singular state.

[0176] Furthermore, the information processing device 100 can identify the type of singular state of the robot arm 11 from the geometric parameters used to calculate the Jacobian matrix. v and the Jacobian matrix J, V = Jθ v By further differentiating this equation with respect to time, the following equation 40 is obtained.

[0177]

[0178] During self-motion, the above element (dJ / dt) θ v Norm of ||(dJ / dt)θ v If || has a value other than zero, the robot arm 11 will not be able to remain in the singular state due to self-motion, such as the elbow singular point, and will escape from it.

[0179] In the Jacobian matrix J, the smallest singular value σn as shown in Equation 5 is σn When the posture of the robot arm 11 approaches a singular point, the basis vector v θn We can focus on the joint velocity vector v θn In a singular posture where the singular value σn=0, indicates the joint velocity corresponding to the self-motion as shown in Equation 39.

[0180] In the case of singular points where self-motion can continue, such as the wrist singular position and the shoulder singular position, the joint velocity vector v θn indicates a velocity close to the self-motion. Therefore, the acceleration of the end effector 12 moved by the robot arm 11 is sufficiently small. Therefore, the smallest singular value σ n and threshold ε σn is σ n <ε σn In the vicinity of the singular point satisfying the above equation, the element (dJ / dt)v θn Absolute value of |(dJ / dt)v θn | is the threshold ε a If the condition that the threshold ε is less than ε is satisfied, the singular point can be determined as a singular point that can continue the self-motion. a is a threshold value of the magnitude of acceleration of the end effector 12. dJ / dt is an example of the eighth Jacobian matrix, and (dJ / dt)v θn is an example of an acceleration vector, and the threshold ε a is an example of the fourth threshold value.

[0181]

[0182] Therefore, when the condition of Expression 41 is satisfied, the information processing device 100 determines the singular point as a singular point at which the self-motion of the robot arm 11 can be continued. When the condition of Expression 41 is not satisfied, the information processing device 100 determines the singular point as a singular point at which the self-motion of the robot arm 11 cannot be continued. In addition to or instead of the condition of Expression 41, the information processing device 100 determines the singular point as a singular point at which the self-motion of the robot arm 11 cannot be continued. v The information processing device 100 may determine the singular point as a singular point where the self-motion can be continued, provided that || is a non-zero value. θn By using the above formula, the type of the singular point can be determined regardless of the current velocity command value of the end effector 12.

[0183] When the information processing device 100 determines that the singular point is one at which the self-motion cannot be continued, it stops the robot arm 11 as follows. The information processing device 100 uses information acquired at each sampling period to calculate the absolute value |(dJ / dt)v θn The information processing device 100 calculates the absolute value |(dJ / dt)v θn The robot arm 11 may be stopped when the absolute value |(dJ / dt)v θn The timing when the fluctuation of | changes from increasing to decreasing may be determined as the timing to stop the robot arm 11. Although not limited thereto, in the case of an elbow peculiar posture, the above timing is particularly suitable as the timing to stop the robot arm 11.

[0184] Furthermore, the information processing device 100 may determine whether the direction of the command velocity of the end effector 12 has changed due to a reversal or the like at the above timing. θn It is possible to determine whether or not the change from an increase to a decrease in | is caused by a change in the direction of the command velocity of the end effector 12. n The unit vector u of the command velocity of the end effector 12 used to calculate the joint velocity vector v θnThe velocity vector u of the end effector 12 corresponding to vn The inner product u vn T The information processing device 100 calculates u using information acquired at each sampling period and stores the calculated value in the memory M. θn At or around the time when the fluctuation of | changes from increasing to decreasing, the inner product u vn T If the sign of u is not inverted, the robot arm 11 is stopped.

[0185] The information processing device 100 determines the timing to restart the stopped robot arm 11 by calculating the inner product u vn T For example, the information processing device 100 may determine the inner product u vn T From the sign of u, the inner product u vn T The timing when the sign of u is inverted may be determined as the timing to move the robot arm 11 again. vn T When u is approximately 0, the velocity vector u vn In this case, when the information processing device 100 moves the robot arm 11, the end effector 12 moves along the boundary of the motion area of ​​the robot arm 11.

[0186] [Case in which the robot arm falls into multiple singular states] The robot arm 11 may fall into multiple singular states. In this case, if two or more singular values ​​σi are greater than or equal to the threshold ε σn The normalized Jacobian matrix J in Equation 5 is N In this case, the singular value σ n and σ n-1 is the threshold ε σn The process of the information processing device 100 will be described for an example where the singular value σ n and σ n-1 But σ n <σ n-1 <ε σn The singular value σ n is an example of the first singular value, and the singular value σn-1 is an example of the second singular value.

[0187] The information processing device 100 calculates the contribution w n is the threshold ε wn For the above-mentioned state SC, similarly to step S1401 described above, the normalized Jacobian matrix J shown in Equation 6 is N Inverse matrix J N -1 matrix Σ in -1 All elements of σ n / ε σn , and a new matrix J is obtained as shown in Equation 42 below. εN However, the singular value σ n-1 is the threshold ε σn Since the singular value σ n-1 element 1 / σ n-1 For σ, the information processing device 100 performs processing to prevent cancellation of digits in the numerical calculation as shown in the following equation 43. n <σ n-1 Therefore, the element 1 / σ n-1 ’ does not diverge. εN The decomposition rate is controlled based on the contribution w n is an example of a first contribution.

[0188]

[0189] Contribution rate w n is the threshold ε wn In the state P where σ is less than σ, the information processing device 100 n-1 is regarded as the smallest singular value. N Inverse matrix J N -1 matrix Σ in -1 All elements of σ n-1 / ε σn Multiply by the element 1 / σ i (i=1,...,n-2) is σ n-1 / σ i ε σn Furthermore, σ n-1 / σ i ε σn to 1 / σ iFurthermore, the information processing apparatus 100 calculates the singular value σ n By this, the information processing device 100 generates a new matrix J as shown in the following equation 44. εNA The information processing device 100 calculates the matrix J εNA The decomposition speed is controlled based on the singular value σ n The processing is performed on the complementary space excluding the space corresponding to the matrix J εNA is an example of the fifth inverse Jacobian matrix, and σ n-1 / ε σn is an example of the second ratio.

[0190]

[0191] Two singular values ​​σ n and σ n-1 Each has a threshold ε σn In the singular state below σ n and σ n-1 The corresponding contribution w n and w n-1 The information processing device 100 controls the disassembly speed of the robot arm 11 in response to the four cases regarding the state of the contribution w n is as shown in Equation 10. Contribution w n-1 is expressed by the following equation 45, and the normalized command velocity vector V of the end effector 12 is N The singular values ​​σ n-1 The basis vectors u in the velocity space corresponding to vn-1 Furthermore, the contribution w n and w n-1 For both of these, the threshold ε wn is set. The threshold ε wn is 0 < ε wn Contribution w is set in the range of <1. n-1 is an example of a second contribution.

[0192]

[0193] (1) w n ≧ε wn and w n-1 ≧ε wn The first case of contribution wn is the threshold ε wn As described above, the information processing device 100 calculates the normalized matrix J εN The decomposition speed of the state SC is controlled using the singular value σ n << Singular value σ n-1 As such, the singular value σ n and the singular value σ n-1 When the difference between the singular values ​​σ and σ is large, the singular value σ is small when the robot arm 11 performs self-motion. n The self-motion corresponding to the singular value σ n and the singular value σ n-1 As the difference between the singular values ​​σ n The self-motion and singular value σ corresponding to n-1 It is possible to perform a combined motion with the corresponding self-motion.

[0194] (2) w n ≧ε wn and w n-1 <ε wn The second case of contribution w n is the threshold ε wn As described above, the information processing device 100 calculates the normalized matrix J εN The decomposition speed of the state SC is controlled using the singular value σ n << Singular value σ n-1 As such, the singular value σ n and the singular value σ n-1 When the difference between the singular values ​​σ and σ is large, the singular value σ is small when the robot arm 11 performs self-motion. n Therefore, the self-motion corresponding to the contribution w n-1 is the threshold ε wn The effect of the robot arm 11 being below this level on the self-motion is small.

[0195] (3) w n <ε wn and w n-1 ≧ε wn The third case of contribution w n is the threshold ε wn Therefore, the information processing device 100 calculates the normalized matrix J εNAThe decomposition rate of state P is controlled using the contribution w n-1 is the threshold ε wn Therefore, when the robot arm 11 performs self-motion, the singular value σ n-1 The corresponding self-motion becomes dominant.

[0196] (4) w n <ε wn and w n-1 <ε wn The fourth case of contribution w n is the threshold ε wn Therefore, the information processing device 100 calculates the normalized matrix J εNA The decomposition rate of the state P is controlled based on the contribution w n-1 is the threshold ε wn Since it is less than the smallest singular value σ n The normalized matrix J of Equation 15 is recursively calculated for the complementary space excluding the space corresponding to εNA That is, the information processing device 100 performs processing to generate the normalized matrix J εNA For element 1 / ε σn is replaced with 0, and a new normalized matrix J εNA1 The information processing device 100 generates the normalized matrix J εNA1 In other words, the decomposition speed is controlled in a four-dimensional space in which the two-dimensional space corresponding to the two singular values ​​is contracted. εNA1 are examples of the sixth inverse Jacobian matrix and the seventh inverse Jacobian matrix.

[0197]

[0198] Also, two singular values ​​σ n and σ n-1 Each has a threshold ε σn When the robot arm 11 operates in a singular state where the singular value σ is less than n and σ n-1 is the threshold ε σn The two singular values ​​σ n and σ n-1 Even in such a case, the information processing device 100 can calculate the singular value σn and σ n-1 The corresponding contribution w n and w n-1 The disassembly speed of the robot arm 11 is controlled in response to the four cases regarding the state of the robot arm 11.

[0199] (1) w n ≧ε wn and w n-1 ≧ε wn First Case: The information processing device 100 controls the decomposition speed of the state SC. For example, the information processing device 100 controls the decomposition speed of the normalized matrix J εN Decomposition rate control according to Equation 12 using εN However, the decomposition rate can be controlled according to Equation 14 using two singular values ​​σ n and σ n-1 Just before the magnitude relationship of is reversed, the singular value σ n and σ n-1 Since the difference is small, the contribution w n Even if the norm ||J εN u N || is the threshold κ θ Therefore, the information processing device 100 does not perform the decomposition speed control according to the formula 14. Therefore, the robot arm 11 does not perform the decomposition speed control according to the formula 14. n The self-motion and singular value σ corresponding to n-1 Furthermore, the singular value σ n and σ n-1 Even if the magnitude relationship between the speed command values ​​is reversed, the speed command value of the joint JT does not change, and the operation of the robot arm 11 can transition smoothly.

[0200] (2) w n ≧ε wn and w n-1 <ε wn The second case of contribution w n is the threshold ε wn For the above reasons, the information processing device 100 controls the decomposition speed of the state SC. For example, the information processing device 100 controls the decomposition speed of the normalized matrix J εNDecomposition rate control according to Equation 12 using εN When the robot arm 11 performs self-motion, the singular value σ n In this state, the self-motion corresponding to the singular value σ n and σ n-1 When the magnitude relationship of is reversed, the singular value σ n The contribution of is below the threshold, and therefore the information processing apparatus 100 controls the decomposition speed of the state P. However, the singular value σ n The state in which the self-motion corresponding to the current state is dominant is maintained.

[0201] (3) w n <ε wn and w n-1 ≧ε wn The third case of contribution w n is the threshold ε wn Therefore, the information processing device 100 calculates the normalized matrix J εNA When the robot arm 11 performs self-motion, the singular value σ n-1 In this state, the self-motion corresponding to the singular value σ n and σ n-1 When the magnitude relationship between the two is reversed, the singular value σ after the change occurs in the command velocity of the end effector 12. n-1 The state in which it is assumed that there is no command velocity in the direction corresponding to state P is maintained. The information processing device 100 may exit state P by setting the command velocity of the end effector 12 to zero, either in response to a command from the user interface 30 or autonomously.

[0202] (4) w n <ε wn and w n-1 <ε wn In the fourth case, the information processing device 100 calculates the normalized matrix J εNA1 The decomposition rate of the state P is controlled using the normalized matrix J εNA1 Now, the singular value σ n and σ n-1Therefore, the singular value σ n and σ n-1 Even if the magnitude relationship between the speeds of the end effector 12 and the speed of the end effector 12 is reversed, the disassembly speed control of the information processing device 100 is not affected. The information processing device 100 may exit state P by setting the command speed of the end effector 12 to zero, either in response to a command from the user interface 30 or autonomously.

[0203] Therefore, the information processing device 100 determines the threshold value ε σn Even if the magnitude relationship between two singular values ​​in a singular state where the value of the signal is less than 1 / 2 is reversed, there is no need to perform any special processing to deal with the change. Each time the feedback information from the robot 10 is updated, the information processing device 100 determines the Jacobian matrix, performs singular value decomposition, and performs the above-described processing based on the magnitude relationship between the singular values, thereby controlling the decomposition speed for each state.

[0204] In addition, when three or more singular values ​​are below the threshold, or when multiple contributions corresponding to the singular values ​​are below the threshold, the matrix Σ -1 This can be handled by appropriately changing the elements in . In this way, this method has the generality to handle cases where multiple singular states occur simultaneously. For example, if three or more singular values ​​each fall within the threshold ε σn In a singular state where the magnitude relationship between the singular values ​​is reversed, the information processing device 100 may control the decomposition speed of each state by performing the same processes as in the first to fourth cases described above. Even when the magnitude relationship between three or more singular values ​​is reversed, the information processing device 100 may control the decomposition speed of each state by performing the same processes as in the first to fourth cases described above.

[0205] For example, the first singular value σ n and the second singular value σ n-1 and the second singular value σ n-1 and one or more singular values ​​next smallest to n to the k-th singular value σ n-k+1 All of these are within the threshold ε σn In a singular state where the first singular value σ is less than n to the k-th singular value σ n-k+1The corresponding first contribution w n From the kth contribution w n-k+1 Here, k is a natural number between 3 and n.

[0206] The information processing device 100 calculates the first contribution w n From the kth contribution w n-k+1 The contribution gradually increases toward the threshold ε wn The information processing device 100 determines whether the threshold value ε wn If we can find a contribution that is equal to or greater than the threshold ε wn The i-th contribution w n-i+1 The i-th singular value σ corresponding to n-i+1 Based on this, the normalized Jacobian matrix J shown in Equation 6 N Inverse matrix J N -1 is converted. i is a natural number between 1 and k. n-i+1 The ordinal number i of wn It is the smallest of the ordinal numbers of the contribution degrees that are equal to or greater than this.

[0207] In the above transformation, the information processing device 100 calculates the inverse matrix J N -1 matrix Σ in -1 The first singular value σ n to the (i-1)th singular value σ n-i Furthermore, the information processing device 100 performs a zero transformation to replace the (i+1)-th singular value σ n-i to the k-th singular value σ n-k+1 The (i+1)th contribution w corresponding to n-i From the kth contribution w n-k+1 For each, the contribution is the threshold ε wn The information processing device 100 determines whether the contribution degree is equal to or greater than a threshold ε wn A zero transformation is performed to replace singular values ​​that are less than .times. ...

[0208] Furthermore, the information processing device 100 converts the two zero-transformed matrices Σ -1 All elements of the third ratio σ n-i+1 / ε σn By multiplying, the inverse matrix J N -1Calculate a new normalized matrix transformed from the i-th singular value σ n-i+1 is the i-th singular value σ n-i+1 to the k-th singular value σ n-k+1 The information processing device 100 controls the decomposition rate of the state P using the normalized matrix, as in the third case above. The normalized matrix is ​​an example of a seventh inverse Jacobian matrix.

[0209] For example, the first contribution w n From the kth contribution w n-k+1 Both of these are within the threshold ε wn If the ratio is equal to or greater than the first ratio σ, the information processing device 100 sets the third ratio as the first ratio σ n / ε σn The information processing device 100 uses the normalized Jacobian matrix J shown in Equation 6. N Inverse matrix J N -1 matrix Σ in -1 All elements of the first ratio σ n / ε σn , and the normalized matrix J shown in Equation 42 is obtained. εN Similarly to the first or second case, the information processing device 100 calculates the normalized matrix J εN The decomposition rate of state SC is controlled using

[0210] For example, the first contribution w n From the kth contribution w n-k+1 Both of these are within the threshold ε wn If it is less than 1, the information processing device 100 calculates the normalized Jacobian matrix J N Inverse matrix J N -1 The first singular value σ in n to the k-th singular value σ n-k+1 Then, the information processing device 100 performs zero transformation to replace all of the matrices Σ -1 All elements of the third ratio σ n-k+1 / ε σn The information processing device 100 controls the decomposition rate of the state P using the normalized matrix, similarly to the fourth case described above. The normalized matrix is ​​an example of a seventh inverse Jacobian matrix.

[0211] For example, three singular values ​​σ n , σ n-1 and σ n-2 Each has a threshold ε σn In the singular state below n , w n-1 and w n-2 The threshold ε wn The information processing device 100 may control the disassembly speed of the robot arm 11 in response to eight cases relating to the state of the robot arm 11. The processing in each case can be performed in the same manner as the processing in the first to fourth cases.

[0212] In addition, the operation of the robot arm 11 involves three singular values ​​σ n , σ n-1 and σ n-2 At the same time, the threshold ε σn Rather than leading to a singular state below the threshold ε σn After reaching a singular state where the singular value is below the threshold ε σn often reaches a state below

[0213] For example, the robot arm 11 shown in FIG. 4 has three singular values ​​that are smaller than the threshold ε σn 5 to 7, the robot arm 11 shown in FIG. 5 to FIG. 7 is in a singular state where one singular value is below the threshold ε σn The robot arm 11 first reaches the shoulder singular posture shown in FIG. 7, then reaches the elbow singular posture shown in FIG. 5 by extending the elbow, and then or simultaneously reaches the wrist singular posture shown in FIG. 6, whereby the three singular values ​​as shown in FIG. 4 fall below the threshold ε σn This can lead to singular positions below

[0214] Also, two singular values ​​σ n and σ n-1 Each has a threshold ε σn In a singular state where the singular value σ is less than n and σ n-1 The information processing device 100 may perform processing that does not depend on the magnitude relationship between the normalized Jacobian matrix J N Inverse matrix J N -1 matrix Σ in -1All elements of σ n / ε σn , and a new matrix J shown in Equation 42 is obtained. εN Furthermore, the information processing device 100 calculates the singular value σ n-１ The elements containing the singular values ​​σ n The same value 1 / ε as the element containing σn By replacing εND This calculates the singular value σ n , σ n-１ The information processing device 100 can prevent divergence of each element including the normalized matrix J εND The decomposition rate can be controlled using

[0215]

[0216] [Modification of Robot System] A modification of the robot system 1 according to the embodiment will be described. The robot system 1A according to the modification differs from the embodiment in that it is a remote-controlled robot system in which the user interface 30 and the robot 10 are connected via a communication network N. The following description of this modification will focus on the differences from the embodiment, and will omit a description of the same points as the embodiment as appropriate.

[0217] FIG. 14 is a diagram illustrating an example of the configuration of a robot system 1A according to a modified example. As illustrated in FIG. 14 , the robot system 1A includes a robot 10 and a control device 20 arranged in each of one or more robot areas AR, a user interface 30 arranged in each of one or more user areas AU, and an intermediary device 300 that matches the robot area AR with the user area AU and connects the control device 20 and user interface 30 included in each area AR to enable data communication via a communication network N. At least some of the functions of the information processing device 100 are realized by one or both of the user interface 30 and the intermediary device 300. The control device 20 includes at least a power control circuit 200. Although not limited thereto, in this modified example, multiple robot areas AR and multiple user areas AU exist as targets of the robot system 1A.

[0218] The robot system 1A further includes imaging devices 14a and 14b and a robot communication device 15 in the robot area AR. The imaging devices 14a and 14b include cameras and are connected to the control device 20. The imaging device 14a is disposed on the robot 10, for example, on the robot arm 11, and captures images of the end effector 12 and its processing object W. The imaging device 14b is disposed at a position other than the robot 10 so as to capture images of the entire robot 10. The robot communication device 15 includes a modem, an ONU (Optical Network Unit), a router, or a mobile data communication device. The robot communication device 15 connects the control device 20 to a communication network N. The control device 20 can transmit and receive information, commands, and data to and from the user interface 30 and the intermediary device 300 via the communication network N. The control device 20 can transmit image data acquired by the imaging devices 14a and 14b.

[0219] The robot system 1A further includes a presentation device 31 and a user communication device 32 in the user area AU. The presentation device 31 is connected to the user interface 30 and presents information to the operator P in a perceptible manner. The presentation device 31 includes at least a display and may further include a speaker, etc. The user communication device 32 includes a modem, an ONU, a router, or a mobile data communication device. The user communication device 32 connects the user interface 30 to the communication network N. The user interface 30 can send and receive information, commands, and data to and from the control device 20 and the intermediary device 300 via the communication network N. The presentation device 31 presents image data received by the user interface 30 from the imaging devices 14a and 14b as images. The user interface 30 may include the presentation device 31, the user communication device 32, or both.

[0220] The intermediary device 300 manages communications via the communication network N. The intermediary device 300 may have the configuration of a server including a computer. The intermediary device 300 manages authentication of the user communication device 32, connection and disconnection between the user communication device 32 and the robot communication device 15, etc. For example, the intermediary device 300 connects the user communication device 32 to the robot communication device 15 of the robot 10 specified by the user interface 30 connected to the authenticated user communication device 32. In other words, the intermediary device 300 matches and connects the user communication device 32 and the robot communication device 15. The intermediary device 300 manages transmission and reception of data between the robot communication device 15 and the user communication device 32, and the data may pass through the intermediary device 300.

[0221] The communication network N is not particularly limited and may include, for example, a local area network (LAN), a wide area network (WAN), the Internet, or a combination of two or more of these. The communication network N may be configured to use short-range wireless communication such as Bluetooth (registered trademark) and ZigBee (registered trademark), a network dedicated line, a dedicated line of a telecommunications carrier, a public switched telephone network (PSTN), a mobile communication network, the Internet network, satellite communication, or a combination of two or more of these. The mobile communication network may use a fourth-generation mobile communication system, a fifth-generation mobile communication system, or the like. The communication network N may include one or more networks. In this modification, the communication network N is the Internet.

[0222] In this modification, the intermediary device 300 includes the functions of the information processing device 100 according to the embodiment. The intermediary device 300 receives information relating to the specifications of the robot 10 connected to the control device 20 from the control device 20 or a user interface 30 connected to the control device 20. The intermediary device 300 receives feedback information about the robot 10 from the control device 20, and generates a Jacobian matrix using the feedback information and the information relating to the specifications of the robot 10. The intermediary device 300 performs the processing relating to the singularity, which is performed by the information processing device 100 according to the embodiment, based on the Jacobian matrix.

[0223] Even if the control device 20 in the robot area AR does not have the functionality of the information processing device 100, the intermediary device 300 can function as the information processing device 100 and cause the robot 10 to perform an action corresponding to the singular point. The intermediary device 300 can function as the information processing device 100 if it can obtain information regarding the specifications of the robot 10. The intermediary device 300 eliminates the need for the operator P in the user area AU to operate the robot 10 to avoid the singular point based on an image of a limited area presented by the presentation device 31. The operator P can operate the robot 10 without considering the singular point. The intermediary device 300 makes it easy for various operators P to operate various robots 10.

[0224] The user interface 30 may include some or all of the functions of the information processing device 100 according to the embodiment. The user interface 30 may incorporate the functions of the information processing device 100 as application software. For example, if the user interface 30 includes all of the functions of the information processing device 100 according to the embodiment, the intermediary device 300 may match the user interface 30 with the control device 20 and mediate the transmission and reception of information, commands, and data between the user interface 30 and the control device 20. For example, the user interface 30 may include some of the functions of the information processing device 100 according to the embodiment, and the intermediary device 300 may include other or all of the functions of the information processing device 100. The functions of the information processing device 100 included in the user interface 30 and the functions of the information processing device 100 included in the intermediary device 300 may or may not overlap. For example, the intermediary device 300 may execute a process that the user interface 30 cannot execute as the information processing device 100 and provide the processing results to the user interface 30.

[0225] The control device 20 may have some of the functions of the information processing device 100. In this case, the intermediary device 300 may have other or all of the functions of the information processing device 100, and the user interface 30 may have other or all of the functions of the information processing device 100. The control device 20, the user interface 30, and the intermediary device 300 may each have some of the functions of the information processing device 100. In this case, the intermediary device 300 may have all of the functions of the information processing device 100. The functions of the information processing device 100 provided in the control device 20, the functions of the information processing device 100 provided in the user interface 30, and the functions of the information processing device 100 provided in the intermediary device 300 may or may not overlap.

[0226] [Others] Illustrative embodiments and modifications of the present disclosure have been described above, but the present disclosure is not limited to the above-described embodiments and modifications. That is, various modifications and improvements are possible within the scope of the present disclosure. For example, various modifications made to the embodiments or modifications, and forms constructed by combining components of different embodiments and modifications, are also included within the scope of the present disclosure.

[0227] For example, in the embodiment and the modified example, the information processing device 100 performs processing related to a singularity when the robot 10 is manually controlled, but is not limited to this. The information processing device 100 may also perform processing related to a singularity when the robot 10 is automatically controlled, that is, when the robot 10 operates autonomously according to preset data such as teaching data. As a result, even if the teaching data does not include data on the operation of the robot 10 that avoids a singularity, the information processing device 100 can perform processing related to a singularity when causing the robot 10 to operate according to the teaching data, and cause the robot 10 to operate in accordance with the singularity.

[0228] Examples of aspects of the technology of the present disclosure are as follows: A program according to a first aspect of the present disclosure is a program for controlling the operation of a robot including a plurality of joints and a moving part that is moved by the operation of the plurality of joints, the program performing singular value decomposition on a first Jacobian matrix that correlates the operation speeds of the plurality of joints with the movement speed of the moving part to generate a second Jacobian matrix that includes a first matrix including, as elements, a first basis vector that represents a movement speed space of the joints, a second matrix including, as elements, a second basis vector that represents a movement speed space of the moving part, and a first singular matrix including, as diagonal elements, a singular value; The method causes a computer to execute the following: determining and executing a process to be executed from a first process that is executed when the robot is approaching the singular point and a second process that is executed when the robot is away from the singular point; in the first process, generating a third Jacobian matrix by transforming diagonal elements in the first singular matrix; in the first process, determining and outputting motion command values ​​that command movements of the plurality of joints using the third Jacobian matrix and a motion command value that commands movement of the moving unit; and in the second process, determining and outputting the motion command value using the first Jacobian matrix or the second Jacobian matrix and the motion command value.

[0229] According to the above aspect, in the first process, the diagonal elements of the first singular matrix are transformed to generate the third Jacobian matrix from the second Jacobian matrix. In this case, the diagonal elements may be transformed to correspond to a state in which the robot is at a singularity and a state in which the robot is approaching a singularity. For example, the diagonal elements may be transformed so that the elements of the inverse matrix of the second Jacobian matrix do not diverge when the robot is at a singularity and a state in which the robot is approaching a singularity. The above transformation can prevent sudden changes in joint velocity when the robot is at a singularity and a state in which the robot is approaching a singularity. In this way, the novel method of transforming the diagonal elements of the first singular matrix enables robot motion control that corresponds to singularities.

[0230] Here, near singularities, normal decomposition velocity control can cause joint velocities to become excessive. Methods for solving this problem include singularity insensitive decomposition and singularity conforming. Singularity insensitive decomposition can prevent joint velocities from becoming excessive, but at the cost of causing the end effector to deviate from the originally commanded trajectory. The singularity conforming method prevents joint velocities from becoming excessive, while also preventing the end effector from deviating from the originally commanded trajectory. However, the singularity conforming method requires analytical calculation of the cofactor matrix of the Jacobian matrix, which requires a great deal of effort to apply to a specific arm.

[0231] While the conventional singularity fitting method requires the calculation of the cofactor matrix of the Jacobian matrix, the inventors of the present invention have discovered that the Jacobian matrix is ​​subjected to singular value decomposition and the singular values ​​are scaled so that the ratio of the reciprocals of the singular values ​​is preserved. As a result, the inventors have discovered that a matrix having the same effect as the cofactor matrix of the Jacobian matrix in the conventional singularity fitting method can be obtained by numerical calculation rather than analytical calculation.

[0232] In the first aspect above, a program according to a second aspect of the present disclosure may cause a computer to calculate a first norm, which is the norm of a vector obtained by applying the motion command value to a Jacobian matrix used to determine the motion command value; if the first norm is greater than the movement command value, correcting the motion command value using the first norm to determine a corrected motion command value and outputting the corrected motion command value as the motion command value; and if the first norm is less than or equal to the movement command value, outputting the motion command value.

[0233] According to the above aspect, an upper limit of the speed at which the moving unit of the robot moves can be set, and the movement command values ​​of the multiple joints can be determined so that the moving unit moves at a speed equal to or less than the upper limit, thereby preventing the moving speed of the moving unit from increasing excessively.

[0234] In the first or second aspect described above, a program according to a third aspect of the present disclosure may cause a computer to receive first information, which is information on the specifications of the robot, and generate the first Jacobian matrix using the first information and information on the states of the plurality of joints.

[0235] According to the above aspect, the first information can be acquired and the first Jacobian matrix can be generated using the acquired first information, thereby enabling the first Jacobian matrix to be set in accordance with the robot.

[0236] In any of the first to third aspects above, a program according to a fourth aspect of the present disclosure may cause a computer to determine to execute the first process when the minimum value of the singular values ​​is smaller than the first threshold, and to determine to execute the second process when the minimum value of the singular values ​​is equal to or greater than the first threshold.

[0237] According to the above aspect, the minimum singular value decreases as the robot approaches the singular point. Whether the minimum singular value is smaller than the first threshold value can be used as a condition for determining whether the robot is approaching a singular point, i.e., a condition for determining whether or not to execute the first process. Therefore, it is possible to reliably determine whether or not to execute the first process.

[0238] In any of the first to fourth aspects described above, a program according to a fifth aspect of the present disclosure may cause a computer to execute the following steps: generate a second inverse Jacobian matrix that is an inverse matrix of the second Jacobian matrix, the second inverse Jacobian matrix including a first transformation matrix, a second transformation matrix, and a first transformation singular matrix that are transformed from the first matrix, the second matrix, and the first singular matrix, respectively; generate a third inverse Jacobian matrix that is an inverse matrix of the third Jacobian matrix by performing a first transformation that transforms diagonal elements in the first transformation singular matrix; determine the motion command value by applying the movement command value to the third inverse Jacobian matrix; and determine the motion command value by applying the movement command value to the first inverse Jacobian matrix or the second inverse Jacobian matrix that are the inverse matrix of the first Jacobian matrix.

[0239] According to the above aspect, the inverse Jacobian matrix, which is the inverse matrix of the Jacobian matrix, can be applied to the movement speed of the moving part to obtain the movement speeds of multiple joints. By using the inverse Jacobian matrix, it is possible to determine the movement command value for the movement command value.

[0240] In the above fifth aspect, a program according to a sixth aspect of the present disclosure may cause a computer to execute the first transformation to transform each of the diagonal elements including the singular values ​​in the first transformed singular matrix so as to maintain a ratio between the diagonal elements.

[0241] According to the above aspect, the first transformation maintains the ratio between diagonal elements containing singular values ​​in the first transformation singular matrix. Therefore, the first transformation maintains the relationship between the movement command value and the motion command value inherent in the inverse Jacobian matrix. When the same movement command value is applied to the first inverse Jacobian matrix, the second inverse Jacobian matrix, and the third inverse Jacobian matrix, a motion command value that moves the moving unit in the same direction can be obtained. The movement of the moving unit in the commanded direction can be maintained regardless of whether the robot is near a singular point.

[0242] In the above-mentioned sixth aspect, a program according to a seventh aspect of the present disclosure may cause a computer to execute the following: in the first transformation, each of the diagonal elements is multiplied by a first ratio between the smallest value of the singular values ​​in the first transformed singular matrix and the first threshold value.

[0243] According to the above aspect, in the first transformed singular matrix, the diagonal element including the minimum singular value does not diverge even when the minimum singular value approaches 0. Therefore, a sudden change in the velocity of the joint corresponding to the diagonal element in the vicinity of the singular point is prevented.

[0244] In any of the above-described fifth to seventh aspects, a program according to an eighth aspect of the present disclosure may cause a computer to execute the following steps: in the first processing, calculating a second norm, which is a norm of a vector, obtained by applying a direction vector representing a direction of a command velocity of the moving unit, included in the movement command value, to the third inverse Jacobian matrix; in the first processing, if the second norm is equal to or greater than a second threshold, applying the movement command value to the third inverse Jacobian matrix normalized using the second norm to determine the movement command value; and in the first processing, if the second norm is less than the second threshold, applying the movement command value to an inverse Jacobian matrix obtained by changing the second norm in the third inverse Jacobian matrix normalized using the second norm to the second threshold, to determine the movement command value.

[0245] According to the above aspect, the second norm indicates the magnitude of the moving speed of the moving unit corresponding to the directional component of the command speed of the moving unit. When the second norm becomes small, the normalized third inverse Jacobian matrix may diverge. When the second norm is less than the second threshold, the normalized third inverse Jacobian matrix is ​​prevented from diverging by changing the second norm to the second threshold. Therefore, a sudden change in the speed of the moving unit near the singular point is prevented.

[0246] In any of the above-described fifth to eighth aspects, a program according to a ninth aspect of the present disclosure may cause a computer to execute the following steps: in the first processing, determining a contribution rate, which is a rate at which a command velocity of the moving unit included in the movement command value contributes to the direction of the second basis vector; in the first processing, if the contribution rate is equal to or greater than a third threshold, determining the movement command value using the third inverse Jacobian matrix; in the first processing, if the contribution rate is less than the third threshold, transforming the third inverse Jacobian matrix into a fourth inverse Jacobian matrix so as to set to 0 a value of a diagonal element corresponding to a smallest singular value in the first transformed singular matrix after the first transformation; and in the first processing, if the contribution rate is less than the third threshold, applying the movement command value to the fourth inverse Jacobian matrix to determine the movement command value.

[0247] According to the above aspect, when the command velocity of the moving unit includes a component that does not contribute to the direction of the second basis vector representing the movement velocity space of the moving unit, the third inverse Jacobian matrix is ​​transformed into a fourth inverse Jacobian matrix so that the value of the diagonal element corresponding to the smallest singular value in the first transformed singular matrix after the first transformation is set to 0. By using the fourth inverse Jacobian matrix, it is possible to reduce an error in the velocity of the moving unit realized by the operation command value relative to the movement command value.

[0248] In any one of the fifth to ninth aspects described above, a program according to a tenth aspect of the present disclosure includes, in the first processing, determining a first contribution corresponding to a first singular value that is the smallest among the singular values ​​in the first transformed singular matrix, among contributions that are a rate at which the command velocity of the moving unit included in the movement command value contributes to the direction of the second basis vector; and, in the first processing, when the first singular value and a second singular value that is the next smallest after the first singular value among the singular values ​​in the first transformed singular matrix are both smaller than the first threshold value and the first contribution is equal to or greater than a third threshold value, assigning a value between the first singular value and the first threshold value to each of diagonal elements in the first transformed singular matrix. and if, in the first processing, both the first singular value and the second singular value are smaller than the first threshold and the first contribution of the first singular value is less than the third threshold, setting a value of a diagonal element in the first transformed singular matrix corresponding to the first singular value to 0, multiplying each diagonal element in the first transformed singular matrix by a second ratio between the second singular value and the first threshold to generate a fifth inverse Jacobian matrix, and applying the movement command value to the fifth inverse Jacobian matrix to determine the movement command value.

[0249] According to the above aspect, when the first singular value and the second singular value are smaller than the first threshold, an inverse Jacobian matrix is ​​determined according to the relationship between the first contribution corresponding to the first singular value and the third threshold, and a motion command value can be determined using the determined inverse Jacobian matrix. Thus, it is possible to control the motion of the robot in accordance with a state where the robot is at or approaching a singular point, in which the two singular values ​​are smaller than the first threshold.

[0250] In the above-mentioned tenth aspect, a program according to an eleventh aspect of the present disclosure may cause a computer to execute the following steps: if, in the first processing, both the first singular value and the second singular value are smaller than the first threshold and the first contribution and the second contribution corresponding to the second singular value are less than the third threshold, set values ​​of diagonal elements in the first transformed singular matrix corresponding to the first singular value and the second singular value to 0, multiply each of the diagonal elements in the first transformed singular matrix by the second ratio to generate a sixth inverse Jacobian matrix, and apply the movement command value to the sixth inverse Jacobian matrix to determine the motion command value.

[0251] According to the above aspect, when the command velocity of the moving unit includes components that do not contribute to the directions of the second basis vectors corresponding to the first singular value and the second singular value, respectively, the sixth inverse Jacobian matrix is ​​generated so that the values ​​of the diagonal elements corresponding to the first singular value and the second singular value in the first transformed singular matrix are set to 0. By using the sixth inverse Jacobian matrix, a motion command value is generated from which the components corresponding to the first singular value and the second singular value have been removed. Therefore, it is possible to reduce an error in the velocity of the moving unit realized by the motion command value relative to the motion command value.

[0252] In the fifth to eleventh aspects described above, a program according to a twelfth aspect of the present disclosure includes the steps of: in the first processing, when all of the first singular value to k-th singular values ​​(k is a natural number equal to or greater than 3) including a first singular value that is a smallest among the singular values ​​in the first transformed singular matrix and two or more singular values ​​in the first transformed singular matrix that are next smallest to the first singular value are smaller than the first threshold, determining first to k-th contributions which are rates at which a command velocity of the moving unit included in the movement command value contributes to directions corresponding to the first singular value to the k-th singular value in the second basis vector; and in the first processing, when the first to k-th contributions are equal to or greater than a third threshold, determining the third inverse Jacobian matrix by multiplying each of the diagonal elements in the first transformed singular matrix by a first ratio between the first singular value and the first threshold. and applying the movement command value to the third inverse Jacobian matrix to determine the operation command value; and, in the first processing, when at least one of the first contribution to the kth contribution is less than the third threshold, performing a zero transformation in the first transformation to set to zero values ​​of diagonal elements corresponding to the singular values ​​in the first transformed singular matrix whose contribution is less than the third threshold, and multiplying the diagonal elements of the first transformed singular matrix after the zero transformation by a third ratio between the first threshold and a smallest singular value, excluding the singular values ​​in the first transformed singular matrix whose contribution is less than the third threshold, in the first transformed singular matrix after the zero transformation; and, in the first processing, when at least one of the first contribution to the kth contribution is less than the third threshold, applying the movement command value to the seventh inverse Jacobian matrix to determine the operation command value.

[0253] According to the above aspect, even when three or more singular values ​​are smaller than the first threshold, it is possible to determine an inverse Jacobian matrix according to the contribution corresponding to each singular value, and to determine a motion command value using the determined inverse Jacobian matrix, in the same way as when two singular values ​​are smaller than the first threshold. Thus, it is possible to control the motion of the robot in a manner that corresponds to a state in which the robot is at or approaching a singular point, such that three singular values ​​are smaller than the first threshold.

[0254] In any of the first to twelfth aspects described above, a program according to a thirteenth aspect of the present disclosure may cause a computer to execute the following steps: generate an eighth Jacobian matrix by time-differentiating the first Jacobian matrix; generate an acceleration vector by applying, to the eighth Jacobian matrix, a velocity in the second basis vector corresponding to self-motion of the robot, among the movement velocities of the plurality of joints during the first processing; and, when the norm of the acceleration vector is equal to or greater than a fourth threshold value in the first processing, determine whether the norm of the acceleration vector indicates a maximum value; and, when the norm of the acceleration vector indicates a maximum value, output a command to stop the movement of the plurality of joints.

[0255] According to the above aspect, when the norm of the acceleration vector is less than a fourth threshold, the robot generates a small acceleration that allows it to continue self-motion. When the norm of the acceleration vector is equal to or greater than the fourth threshold, the robot generates an acceleration that allows it to escape from a state near a singularity through self-motion. When the norm of the acceleration vector is equal to or greater than the fourth threshold and the norm of the acceleration vector is at its maximum value, the robot may reach a singularity and may experience an oscillation phenomenon in which the robot repeatedly deviates from and reaches the singularity through self-motion. In such a case, the oscillation phenomenon can be prevented by temporarily stopping the movement of multiple joints and then restarting the movement.

[0256] A storage medium according to a fourteenth aspect of the present disclosure stores the program according to any one of the first to thirteenth aspects. According to the above aspect, the same effect as the program according to each aspect of the present disclosure can be obtained. The storage medium may be a non-transitory, tangible computer-readable medium. The storage medium may be a non-transitory, tangible computer-readable recording medium. Examples of storage media may include one or more semiconductor-based or other integrated circuits (ICs), ROMs, EEPROMs (Electrically Erasable and Programmable Read Only Memory), EPROMs, flash memories, CD-ROMs (Compact Disc Read Only Memory), CD-RWs (Compact Disc Rewritable), DVDs (Digital Versatile Disks), hard disk drives (HDDs), hybrid hard drives (HHDs), optical disks, optical disk drives (ODDs), magneto-optical disks, magneto-optical drives, floppy disks, floppy disk drives (FDDs), magnetic tapes, solid-state drives (SSDs), RAM drives, Secure Digital Cards, Secure Digital Drives, any other suitable storage media, or a combination of two or more thereof. Examples of integrated circuits may include field programmable gate arrays (FPGAs), application specific ICs (ASICs), etc. Storage media may be volatile, non-volatile, or a combination of volatile and non-volatile.

[0257] An information processing device according to a fifteenth aspect of the present disclosure includes a memory that stores the program according to any one of the first to thirteenth aspects, and a processor that executes the program stored in the memory. According to the above aspect, the same effects as those of the programs according to the respective aspects of the present disclosure can be obtained.

[0258] A system according to a sixteenth aspect of the present disclosure includes an information processing device including a computer that executes the program according to any one of the first to thirteenth aspects, and the robot, wherein the information processing device is connected to the robot and a user interface through which robot operations are input, the information processing device receives from the user interface a command to move the moving unit, determines the operation command value using the movement command value based on the command to move the moving unit, and transmits the operation command value to the robot, and the information processing device receives first information that is information on the specifications of the robot, and generates the first Jacobian matrix using the first information. According to the above aspect, the same effects as those of the program according to each aspect of the present disclosure can be obtained.

[0259] A method for controlling the movement of a robot according to a seventeenth aspect of the present disclosure is a method for controlling the movement of a robot including a plurality of joints and a moving part that is moved by the movement of the plurality of joints, the method comprising: performing singular value decomposition on a first Jacobian matrix that relates the movement speeds of the plurality of joints to the movement speed of the moving part, to generate a second Jacobian matrix that includes a first matrix including as elements a first basis vector that represents a movement speed space of the joints; a second matrix including as elements a second basis vector that represents a movement speed space of the moving part; and a first singular matrix including as diagonal elements a singular value; and determining whether the robot is at a singular point or not based on a comparison result between the singular value and a first threshold value. The program includes determining and executing a process to be executed from a first process to be executed when the robot is approaching the singular point and a second process to be executed when the robot is away from the singular point, generating a third Jacobian matrix by transforming diagonal elements in the first singular matrix in the first process, determining and outputting motion command values ​​to command motions of the plurality of joints using the third Jacobian matrix and a motion command value that commands movement of the moving unit in the first process, and determining and outputting the motion command values ​​using the first Jacobian matrix or the second Jacobian matrix and the motion command value in the second process.

[0260] The program of the present disclosure may be, for example, a program recorded on a non-transitory, tangible computer-readable recording medium, and may be configured to be read from the recording medium using a recording medium drive device and installed on a computer. The program may be, for example, a program that can be distributed via a transmission medium such as the Internet, and may be configured to be downloaded and installed on a computer.

[0261] A part or all of the method of the present disclosure may be realized by, for example, a circuit such as a CPU or an LSI, an IC card, or a stand-alone module, etc. Multiple elements included in the method of the present disclosure may be realized by one device, or may be realized by two or more devices in a shared manner.

[0262] The functions of the elements disclosed herein can be performed using circuits or processing circuits, including general-purpose processors, special-purpose processors, integrated circuits, ASICs, conventional circuits, and / or combinations thereof, configured or programmed to perform the disclosed functions. A processor is considered a processing circuit or circuit because it includes transistors and other circuitry. In this disclosure, a circuit, unit, or means is hardware that performs the recited functions or hardware that is programmed to perform the recited functions. The hardware may be hardware disclosed herein or other known hardware that is programmed or configured to perform the recited functions. Where the hardware is a processor, which is considered a type of circuit, the circuit, means, or unit is a combination of hardware and software, and the software is used to configure the hardware and / or processor.

[0263] All numbers such as ordinal numbers and quantities used in this specification are provided as examples to specifically explain the technology of the present disclosure, and the present disclosure is not limited to the illustrated numbers. The connection relationships between components are provided as examples to specifically explain the technology of the present disclosure, and the connection relationships that realize the functions of the present disclosure are not limited to these.

[0264] Because the present disclosure may be embodied in various forms without departing from the scope of its essential characteristics, the scope of the present disclosure is defined by the appended claims rather than the description in the specification, and therefore the exemplary embodiments and modifications are intended to be illustrative and not limiting. All modifications within the scope of the claims and their equivalents are intended to be embraced by the claims.

Claims

1. A program for controlling the movement of a robot including a plurality of joints and a moving part that is moved by the movement of the plurality of joints, comprising: performing singular value decomposition on a first Jacobian matrix that relates the movement speeds of the plurality of joints and the movement speed of the moving part to generate a second Jacobian matrix including a first matrix whose elements include a first basis vector that represents the movement speed space of the joints, a second matrix whose elements include a second basis vector that represents the movement speed space of the moving part, and a first singular matrix whose diagonal elements include a singular value; determining and executing a process to be executed from among a first process that is executed when the robot is at a singular point and when the robot is approaching the singular point, and a second process that is executed when the robot is away from the singular point, based on a comparison result between the singular value and a first threshold; and generating a third Jacobian matrix by transforming diagonal elements in the first singular matrix in the first process. A program causing a computer to execute the following steps: in the first processing, determining and outputting motion command values ​​that command motion of the plurality of joints using the third Jacobian matrix and a motion command value that commands motion of the moving part; and in the second processing, determining and outputting the motion command value using the first Jacobian matrix or the second Jacobian matrix and the motion command value.

2. The program according to claim 1, which causes a computer to execute the following: calculating a first norm, which is the norm of a vector obtained by applying the motion command value to the Jacobian matrix used to determine the motion command value; if the first norm is greater than the movement command value, correcting the motion command value using the first norm to determine a corrected motion command value and outputting the corrected motion command value as the motion command value; and if the first norm is less than or equal to the movement command value, outputting the motion command value.

3. The program according to claim 1, which causes a computer to receive first information which is information on the specifications of the robot, and generate the first Jacobian matrix using the first information and information on the states of the multiple joints.

4. The program according to claim 1, which causes a computer to determine execution of the first process when the minimum value of the singular values ​​is smaller than the first threshold value, and to determine execution of the second process when the minimum value of the singular values ​​is equal to or greater than the first threshold value.

5. The program according to claim 1, which causes a computer to execute the following steps: generate a second inverse Jacobian matrix which is an inverse matrix of the second Jacobian matrix, the second inverse Jacobian matrix including a first transformation matrix, a second transformation matrix and a first transformation singular matrix which are respectively transformed from the first matrix, the second matrix and the first singular matrix; in the first processing, generate a third inverse Jacobian matrix which is an inverse matrix of the third Jacobian matrix by performing a first transformation that transforms diagonal elements in the first transformation singular matrix; in the first processing, apply the movement command value to the third inverse Jacobian matrix to determine the operation command value; and in the second processing, apply the movement command value to the first inverse Jacobian matrix or the second inverse Jacobian matrix which are the inverse matrix of the first Jacobian matrix to determine the operation command value.

6. The program according to claim 5, which causes a computer to execute the steps of: transforming each of the diagonal elements in the first transformation so as to maintain a ratio between the diagonal elements containing the singular values ​​in the first transformation singular matrix.

7. The program according to claim 6, which causes a computer to execute a step of multiplying each of the diagonal elements by a first ratio between the minimum value of the singular values ​​in the first transformed singular matrix and the first threshold value in the first transformation.

8. The program according to claim 5, which causes a computer to execute the following steps: in the first processing, calculating a second norm, which is a norm of a vector, obtained by applying a directional vector representing a direction of a command velocity of the moving part included in the movement command value to the third inverse Jacobian matrix; in the first processing, when the second norm is equal to or greater than a second threshold, applying the movement command value to the third inverse Jacobian matrix normalized using the second norm to determine the operation command value; and in the first processing, when the second norm is less than the second threshold, applying the movement command value to an inverse Jacobian matrix obtained by changing the second norm to the second threshold in the third inverse Jacobian matrix normalized using the second norm to determine the operation command value.

9. The program according to claim 5, which causes a computer to execute the following steps: in the first processing, determining a contribution rate which is a rate at which the command speed of the moving part included in the movement command value contributes to the direction of the second basis vector; in the first processing, if the contribution rate is equal to or greater than a third threshold, determining the movement command value using the third inverse Jacobian matrix; in the first processing, if the contribution rate is less than the third threshold, converting the third inverse Jacobian matrix into a fourth inverse Jacobian matrix so as to set to 0 a value of a diagonal element corresponding to the minimum value of the singular values ​​in the first transformed singular matrix after the first transformation; and in the first processing, if the contribution rate is less than the third threshold, applying the movement command value to the fourth inverse Jacobian matrix to determine the movement command value.

10. In the first process, among the contributions which are the proportion of the command speed of the moving part included in the movement command value that contributes to the direction of the second basis vector, determining a first contribution corresponding to a first singular value which is the smallest among the singular values ​​in the first transformed singular matrix; in the first process, when both the first singular value and a second singular value which is the second smallest among the singular values ​​in the first transformed singular matrix after the first singular value are smaller than the first threshold value and the first contribution is equal to or greater than a third threshold value, multiplying each diagonal element in the first transformed singular matrix by a first ratio between the first singular value and the first threshold value to generate the third inverse Jacobian matrix, and applying the movement command value to the third inverse Jacobian matrix to determine the motion command value; 6. The program according to claim 5, further comprising: if, in the first processing, both the first singular value and the second singular value are smaller than the first threshold and the first contribution of the first singular value is less than the third threshold, setting a value of a diagonal element in the first transformed singular matrix corresponding to the first singular value to 0, multiplying each diagonal element in the first transformed singular matrix by a second ratio between the second singular value and the first threshold to generate a fifth inverse Jacobian matrix, and applying the movement command value to the fifth inverse Jacobian matrix to determine the motion command value.

11. The program described in claim 10, which causes a computer to execute the following steps: if, in the first processing, both the first singular value and the second singular value are smaller than the first threshold value, and the first contribution and the second contribution corresponding to the second singular value are less than the third threshold value, set values ​​of diagonal elements in the first transformed singular matrix corresponding to the first singular value and the second singular value to 0, multiply each diagonal element in the first transformed singular matrix by the second ratio to generate a sixth inverse Jacobian matrix, and apply the movement command value to the sixth inverse Jacobian matrix to determine the motion command value.

12. In the first process, when all of the first singular value to the k-th singular value (k is a natural number equal to or greater than 3) including a first singular value that is the smallest among the singular values ​​in the first transformed singular matrix and two or more singular values ​​in the first transformed singular matrix that are next to the first singular value are smaller than the first threshold value, determining a first contribution to a k-th contribution that is a ratio of a command speed of the moving part included in the movement command value to a direction corresponding to each of the first singular value to the k-th singular value in the second basis vector; and in the first process, when the first contribution to the k-th contribution is equal to or greater than a third threshold value, in the first transformation, multiplying each of the diagonal elements in the first transformed singular matrix by a first ratio between the first singular value and the first threshold value to generate the third inverse Jacobian matrix, and applying the movement command value to the third inverse Jacobian matrix to determine the motion command value; 6. The program according to claim 5, further comprising: causing a computer to execute: in the first processing, when at least one of the first contribution to the kth contribution is less than the third threshold, a zero transformation is performed in the first transformation to set values ​​of diagonal elements in the first transformed singular matrix corresponding to the singular values ​​whose contribution is less than the third threshold to zero, and multiplying the diagonal elements in the first transformed singular matrix after the zero transformation by a third ratio between the first threshold and a smallest singular value, excluding the singular values ​​in the first transformed singular matrix whose contribution is less than the third threshold, to convert to a seventh inverse Jacobian matrix; and in the first processing, when at least one of the first contribution to the kth contribution is less than the third threshold, applying the movement command value to the seventh inverse Jacobian matrix to determine the motion command value.

13. The program according to claim 1, which causes a computer to execute the following steps: time-differentiating the first Jacobian matrix to generate an eighth Jacobian matrix; applying to the eighth Jacobian matrix, among the movement velocities of the multiple joints during the first process, a velocity in the second basis vector corresponding to the self-motion of the robot, to generate an acceleration vector; when the norm of the acceleration vector is equal to or greater than a fourth threshold in the first process, determining whether the norm of the acceleration vector indicates a maximum value; and when the norm of the acceleration vector indicates a maximum value, outputting a command to stop the movement of the multiple joints.

14. A storage medium storing the program according to any one of claims 1 to 13.

15. An information processing device comprising: a memory that stores the program according to any one of claims 1 to 13; and a processor that executes the program stored in the memory.

16. A system comprising: an information processing device including a computer that executes the program described in any one of claims 1 to 13; and the robot, wherein the information processing device is connected to a user interface through which robot operations are input and to the robot, wherein the information processing device receives a command to move the moving part from the user interface, determines the operation command value using the movement command value based on the command to move the moving part, and transmits the operation command value to the robot, and wherein the information processing device receives first information which is information on the specifications of the robot, and generates the first Jacobian matrix using the first information.

17. A method for controlling the movement of a robot including a plurality of joints and a moving part that is moved by the movement of the plurality of joints, comprising: performing singular value decomposition on a first Jacobian matrix that relates the movement speeds of the plurality of joints and the movement speed of the moving part to generate a second Jacobian matrix including a first matrix whose elements include a first basis vector that represents the movement speed space of the joints, a second matrix whose elements include a second basis vector that represents the movement speed space of the moving part, and a first singular matrix whose diagonal elements include a singular value; determining and executing a process to be executed from among a first process that is executed when the robot is at a singular point and when the robot is approaching the singular point, and a second process that is executed when the robot is away from the singular point, based on a comparison result between the singular value and a first threshold; and generating a third Jacobian matrix by transforming diagonal elements in the first singular matrix in the first process. in the first processing, determining and outputting motion command values ​​that command motion of the plurality of joints using the third Jacobian matrix and a motion command value that commands motion of the moving part; and in the second processing, determining and outputting the motion command value using the first Jacobian matrix or the second Jacobian matrix and the motion command value.

Images