Program, storage medium, information processing apparatus, system, and method for controlling the operation of a robot

Abstract

The program causes a computer to perform the following operations: perform singular value decomposition on a first Jacobian matrix to generate a second Jacobian matrix including a first matrix, a second matrix, and a first singular matrix having singular values as diagonal elements; determine whether to execute a first process when at or near a singular point or a second process when away from the singular point based on a comparison result between the singular values and a first threshold value; in the first process, generate a third Jacobian matrix by converting diagonal elements in the first singular matrix, and determine and output operation command values for a plurality of joints (JT) of the robot using the third Jacobian matrix and a movement command value of a moving part (12) of the robot (10); and in the second process, determine and output the operation command values using the first Jacobian matrix or the second Jacobian matrix and the movement command value.

Description

Technical Field

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

Background Art

[0002] For example, Patent Document 1 discloses a system for dealing with singularities during the operation of a multi-axis machine including a kinematic link mechanism having a plurality of joints. In this system, numerical control using a Jacobian matrix is performed. The Jacobian matrix is decomposed into a unitary matrix including singular values, left singular vectors, and right singular vectors. Using the singular value decomposition result, joints in a singular point or a state close to a singular point are determined, and the columns of the Jacobian matrix associated with the determined joints are transformed using the singular values and the left singular vectors, thereby correcting the Jacobian matrix.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] In Patent Document 1, a virtual joint obtained by calculating a left singular vector using singular values is replaced with the column of the Jacobian matrix associated with the determined joint. For this reason, the relationship between the operating speed of the joint originally possessed by the Jacobian matrix and the operating speed of the tip of the link mechanism may be lost. The present disclosure provides a program, a storage medium, an information processing apparatus, a system, and a method for realizing a novel operation control of a robot corresponding to a singular point using singular value decomposition of a Jacobian matrix.

Means for Solving the Problems

[0005] A program according to an 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 decomposes a first Jacobian matrix that associates the operation speeds of the plurality of joints with the moving speed of the moving part into singular value decomposition, and includes a first matrix including, as elements, first basis vectors representing the operation speed space of the joints, a second matrix including, as elements, second basis vectors representing the moving speed space of the moving part, and a first singular matrix including singular values as diagonal elements, to generate a second Jacobian matrix. Based on the comparison result between the singular values and a first threshold value, a first process executed when the robot is at a singular point and when the robot is approaching the singular point, and a second process executed when the robot is away from the singular point are determined, and the process to be executed is determined and executed. In the first process, diagonal elements in the first singular matrix are converted to generate a third Jacobian matrix. In the first process, using the third Jacobian matrix and a movement command value for commanding the movement of the moving part, an operation command value for commanding the operation of the plurality of joints is determined and output. In the second process, using the first Jacobian matrix or the second Jacobian matrix and the movement command value, the operation command value is determined and output, and the computer is caused to execute these operations.

Brief Description of the Drawings

[0006]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10

Figure 11

Figure 12

Figure 13A

Figure 13B

Figure 13C

Figure 14

DETAILED DESCRIPTION OF THE INVENTION

[0007] Hereinafter, exemplary embodiments of the present disclosure will be described with reference to the drawings. The embodiments described below are all illustrative or specific examples. Among the components in the following embodiments, components not described in the independent claims indicating the highest-level concept are described as optional components. Each figure in the accompanying drawings is a schematic diagram and is not necessarily drawn precisely. In each figure, substantially the same components are denoted by the same reference numerals, and duplicate descriptions may be omitted or simplified. In this specification and the claims, "device" may mean not only one device but also a system including a plurality of devices.

[0008] [Configuration of Robot System] The configuration of the robot system 1 according to the embodiment will be described. FIG. 1 is a schematic diagram showing an example of the configuration of the robot system 1 according to the 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, in the present embodiment, the robot system 1 functions as a system for manually operating the robot 10, the user interface 30 functions as a manual controller, and the robot 10 operates according to an operation 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 part 12. The link structure 11 includes a plurality of links L and a plurality of joints JT that operably connect the plurality of links L. The moving part 12 is attached to the link structure 11. In the present embodiment, the moving part 12 is attached to the tip of the link structure 11. The link structure 11 can move the moving part 12 to various positions and postures by operating the plurality of joints JT.

[0010] Although not limited, in the present embodiment, the link structure 11 is a multi-joint robot arm, for example, an industrial robot arm. The link structure 11 may have a structure in which a plurality of links L are connected by a plurality of joints JT, and may be, for example, a robot leg.

[0011] Furthermore, the link structure 11 has a structure of a vertically articulated arm and includes six links L1 to L6 and six joints JT1 to JT6 as the plurality of links L and the plurality of joints JT. The link structure 11 as a robot arm is not limited to the above and may have another type of arm structure such as a horizontally articulated 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 the robot arm 11. "Joint" is represented by "joint JT1" to "joint JT6" when distinguishing joints, and is represented by "joint JT" when not distinguishing joints. "Link" is represented by "link L1" to "link L6" when distinguishing links, and is represented by "link L" when not distinguishing links.

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

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

[0014] The link L6 includes a mechanical interface at its tip and is connected to the moving part 12 via the mechanical interface. The moving part 12 can transmit and receive signals and receive power supply via the mechanical interface. The moving part 12 has a structure capable of acting on an object and is also called an end effector. The action of the moving part 12 is not particularly limited, but in this embodiment, it is an action of gripping an object. In the following, the moving part 12 may also be referred to as the end effector 12.

[0015] The robot arm 11 includes drive devices D1 to D6 that drive the joints JT1 to JT6, respectively. As shown in FIG. 3, the drive devices D1 to D6 each include motors M1 to M6 as drive sources, and may further include a speed reducer that transmits the rotational driving force of the motors M1 to M6. FIG. 3 is a block diagram showing an example of the configuration of the robot system 1 according to the embodiment. In the present embodiment, the motors M1 to M6 are each servo motors, and include an electric motor and a rotation sensor E such as an encoder that detects the rotation amount of the electric motor. The operations of the motors M1 to M6 are controlled by the control device 20. The control device 20 performs feedback control on 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 FIG. 1 and Figure 3 the user interface 30 is connected to the control device 20 via wired communication, wireless communication, or a combination thereof. Any wired communication and wireless communication may be used. The user interface 30 receives an input from an operator and outputs a signal based on the input content to the control device 20. For example, the user interface 30 receives an input for manually operating the robot arm 11. The content of the input is at least related to the target speed of the end effector 12 moved by the robot arm 11. The target speed includes the target moving direction of the end effector 12 and the target speed of the end effector 12 in the moving direction. The user interface 30 outputs a signal instructing the target speed of the end effector 12 according to the input content for manual operation to the control device 20.

[0017] For example, the user interface 30 receives an input of information regarding 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 length of the links L, the number and type 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 the distance between two joints JT connected to the link L. The type of the joint JT may represent a rotational joint or a linear motion joint. The position of the axis A of the joint JT may represent the position of the rotation axis of the rotational joint or the position of the linear motion axis of the linear motion 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 rotational joint or the direction of the linear motion axis of the linear motion 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 have a structure similar to that of the robot arm 11, a known operating device of the robot, a known teaching device of the robot such as a teach pendant, a computer such as a personal computer, a smart device such as a smartphone and a tablet, a game terminal, other operating devices, other terminal devices, 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 input via an operation of an operator such as a button, a lever, a dial, a joystick, a mouse, a key, a touch panel, a motion capture, or a combination of two or more of these.

[0020] In the present embodiment, the user interface 30 includes a joystick. The tilting direction of the joystick represents the target moving direction of the end effector 12, and the tilting angle of the joystick may represent the target speed of the end effector 12 in the moving direction.

[0021] The control device 20 is connected to the robot 10 via wired communication, wireless communication, or a combination thereof. Any wired communication and wireless communication may be used. The control device 20 controls the operation of the robot 10 according to a command included in a signal received from the user interface 30. The control device 20 determines elements included in the control program of the robot 10 using information regarding the specifications of the robot 10 included in a signal received from the user interface 30. Although not limited, in the present embodiment, the control device 20 also controls the power supply to the robot 10.

[0022] As shown in FIG. 3, the control device 20 includes an information processing device 100 and a power control circuit 200. In the present embodiment, the information processing device 100 and the power control circuit 200 are included in one control device 20, but are separate devices separated from each other and may be connected to each other via wired communication, wireless communication, or a combination thereof. Any wired communication and 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 according to a current value included in a command received from the information processing device 100.

[0024] The information processing device 100 includes a circuit C, and the circuit C 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 equipment. The memory M stores a program executed by the processor P and various data. The memory M stores the control program of the robot 10.

[0025] The information processing apparatus 100 may further include a hard disk drive (HDD) or a solid state drive (SSD) as a storage means. 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 forms a computer system together with the RAM and the ROM. The computer system may realize the functions of the information processing apparatus 100 by the processor P executing a program recorded in the ROM using the RAM as a work area. Some or all of the functions of the information processing apparatus 100 may be realized by the above computer system, may be realized by a dedicated hardware circuit such as an electronic circuit or an integrated circuit, or may be realized by a combination of the above computer system and hardware circuit. The information processing apparatus 100 may be configured to execute each process by centralized control by a single apparatus, or may be configured to execute each process by distributed control by cooperation of a plurality of apparatuses.

[0027] Although not limited, for example, the processor P may include a central processing unit (CPU), a micro processing unit (MPU), a graphics processing unit (GPU), a microprocessor, a processor core, a multiprocessor, an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), etc. The processor P may realize each process by a logic circuit or a dedicated circuit formed on an integrated circuit (IC) chip, a large scale integration (LSI), etc. A plurality of processes may be realized by one or a plurality of integrated circuits, or may be realized by one integrated circuit.

[0028] The information processing apparatus 100 converts the target speed of the end effector 12 included in the signal received from the user interface 30 into the target speeds of the joints JT1 to JT6 respectively. By operating the joints JT1 to JT6 at the respective target speeds, the robot arm 11 can move the end effector 12 at the target speed. The speed of the joint JT includes the direction of the operation of the joint JT and the speed of the operation of the joint JT in that direction. Both the rotation angle of the rotary joint and the linear movement position of the linear joint are represented in the phase with respect to the joint, and the speed 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 speed of the joint JT is represented by the speed θva. The speeds θva1 to θva6 of the joints JT1 to JT6 are represented by the speed 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 inverse matrix J of the Jacobian matrix J is used for the conversion of the target speed. -1 The Jacobian matrix J is a matrix that converts the minute displacements of the joints JT1 to JT6 into the minute displacements of the position and orientation of the end effector 12.

[0030] As shown in FIGS. 1 and 2, the position and orientation of the mechanical interface surface of the link L6 are represented as the position and orientation of the end effector 12. The position and orientation of the mechanical interface surface are represented by the coordinates in the robot coordinate system Σ (X, Y, Z) with respect to the robot 10. Although not limited, in the present embodiment, the robot coordinate system Σ (X, Y, Z) has an 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 extending perpendicular to each other along the support surface S, and a Z axis perpendicular to the X axis and the Y axis.

[0031] The position of the mechanical interface surface is represented by x, y, and z, which are the coordinates in the robot coordinate system Σ(X, Y, Z) of the origin of the mechanical interface coordinate system Σ(Xm, Ym, Zm) with respect to 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 the angle r O , r A and r T represented by. The angle r O , r A and r T may be represented by the roll angle, pitch angle, and yaw angle, or may be represented by Euler angles. The velocity of the end effector 12 is represented by the elements x v , y v , z v , ω x , ω y and ω z , and for example, the velocity vector V = [x v , y v , z v , ω x , ω y , ω z . [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 vector θv of the joints JT1 to JT6, and the Jacobian matrix J satisfy the relational expression θv = J -1 V.

[0032] The information processing device 100 operates according to a control program, and uses the above relational expression and the target speed of the end effector 12 to determine the target speed of each of the joints JT1 to JT6. Further, 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 speed, and outputs them to the power control circuit 200. The information processing device 100 performs numerical control on the robot 10 based on the relational expression of θv = J -1 V. That is, the information processing device 100 performs decomposition speed control using the Jacobian matrix.

[0033] Note that the elements of the Jacobian matrix J include the phase θ of the joint JT as a variable. 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, and uses the calculated phases for the calculation of 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 the information on the specifications of the robot 10. When the information processing device 100 receives information on 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 the feedback information and the information on the specifications of the robot 10, and stores it in the memory M. Further, the information processing device 100 applies the target speed of the end effector 12 to the inverse matrix J -1 of the Jacobian matrix J to determine the target speed 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 calculate the Jacobian matrix J that conforms to the changed robot 10 by acquiring the information on the specifications of the changed robot 10, and perform decomposition speed control according to the changed robot 10.

[0036] In addition, there are singular points in the robot arm 11 of the robot 10 where the robot arm 11 cannot be controlled according to the calculation using the Jacobian matrix J. When the posture of the robot arm 11 is at a singular point, 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 singular point, the robot arm 11 cannot generate the speed of the end effector 12 in a certain direction. Therefore, in the vicinity of the singular point, the robot arm 11 requires an excessive speed for any one or all of the joints JT1 to JT6 in order to generate the speed of the end effector 12 in the said direction. Thus, the information processing device 100 determines the excessive speed required for the joints JT1 to JT6 as an error and stops the operations of the joints JT1 to JT6. Or, in the vicinity of the singular point, the robot arm 11 cannot achieve a sufficient speed of the joints JT1 to JT6 to generate the speed of the end effector 12 in the said 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 can perform self-motion such that it stops the movement of a part such as the end effector 12 and operates while maintaining the position of the stopped part. In the vicinity of the singular point, the robot arm 11 may move the end effector 12 at a speed that changes rapidly with respect to the target speed as it approaches the singular point.

[0037] Figures 4 to 7 are conceptual diagrams showing an example of the posture of the robot arm 11 at the singularity. Figure 4 shows the posture in which the robot arm 11 with the joints JT1 to JT6 aligned in a straight line is fully extended. Figures 5 to 7 show the postures of the robot arm 11 in which two or more of the joints JT1 to JT6 are aligned on a straight line. In Figure 5, the joints JT2 to JT4 including the joint JT3 corresponding to the elbow joint of the robot arm 11 are aligned on a straight line. Such a posture of the robot arm 11 is also called an elbow singularity posture. In Figure 6, the joints JT3 to JT6 including the joint JT5 corresponding to the wrist bending joint of the robot arm 11 are aligned on a straight line. Such a posture of the robot arm 11 is also called a wrist singularity posture. In Figure 7, the joints JT1 and JT2 corresponding to the shoulder joints of the robot arm 11 and the joint JT5 are aligned on a straight line. Such a posture of the robot arm 11 is also called a shoulder singularity posture.

[0038] At the singularity, the inverse matrix of the Jacobian matrix J does not exist and the determinant detJ of the Jacobian matrix J becomes 0. As it approaches the singularity, the determinant detJ approaches 0. Since the inverse matrix of the Jacobian matrix J includes 1 / detJ in all elements, as the determinant detJ approaches 0, the speed of the joint JT diverges and the speed of the end effector 12 can become an unintended speed.

[0039] [Operation of the information processing apparatus 100] The information processing apparatus 100 performs control to reduce the unintended operation of the robot arm 11 at the singularity and in its vicinity. Figures 8 to 12 are flowcharts showing an example of the processing related to the singularity of the information processing apparatus 100 according to the embodiment.

[0040] The following processing of steps S1 and S2 is the pre-stage processing of the control of the robot 10 according to the command of the speed of the end effector 12 output from the user interface 30. The processing after step S3 is the processing related to the control of the robot 10.

[0041] In step S1, a user of the robot system 1 inputs information regarding the specifications of the robot 10 into 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 the information regarding the specifications of the robot 10 in the memory M.

[0043] In step S3, an operator of the robot system 1 inputs an operation into the user interface 30 to manually control the robot 10. The user interface 30 transmits a signal indicating the speed command value of 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 the target speed of the end effector 12, from the signal. In this example, the speed command value is represented by the speed vector V. If the speed command value is not the speed vector V, the information processing device 100 may convert the speed command value into the speed vector V. For example, the information processing device 100 may acquire the speed command value of the end effector 12 at every 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 from the power control circuit 200 and the detection results of the rotation sensors E from the motors M1 to M6 respectively. 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 apparatus 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 apparatus 100 calculates the Jacobian matrix J using the phases θ1, θ2, θ3, θ4, θ5, and θ6 and the information regarding the specifications of the robot 10 stored in the memory M. The Jacobian matrix J is an example of the first Jacobian matrix. Therefore, steps S4 and S5 may be in reverse order.

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

[0047] Here, the velocity vector V = [x v , y v , z v , ω x , ω y , ω z of the end effector 12 can be normalized to the velocity vector V Ne as shown in Equation 1 below. The velocity vector θ v = [θva1, θva2, θva3, θva4, θva5, θva6] of the joints JT1 to JT6 can be normalized to the velocity vector θ vN as shown in Equation 2 below. vi max is the maximum value preset for the i-th element vi of the velocity vector V. θvai max is the maximum value preset for the i-th element θvai of the velocity vector θ v . vi max and θvai max may be set based on the specifications of the robot 10. d v and d θv are correction coefficients that satisfy 1 ≤ d v ≤ m and 1 ≤ d θv ≤ n, respectively. m is the dimension of the velocity vector V, which is 6 in this example. n is the dimension of the velocity vector θ v , which 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 VNe is an example of a movement command value, the velocity vector θ v and the velocity vector θ vN is an example of an operation command value.

[0048]

Number

[0049] Note that in the velocity vector V, [x v , y v , z v can be represented as the translational vector vl, and [ω x , ω y , ω z can be represented as the angular velocity vector ω. The translational vector vl and the angular velocity vector ω are different in unit systems from each other. Therefore, as shown in Equation 3 below, the velocity vector V is the maximum norm ||vl|| of the translational vector vl max and the maximum norm ||ωl|| of the angular velocity vector ω max and may be normalized using the velocity vector V N . In this example, the correction coefficient d v satisfies 1 ≤ d v ≤ 2. I3 is a 3-row × 3-column identity matrix.

[0050]

Number

[0051] And, using the normalized velocity vector V Ne or V N and the normalized velocity vector θ vN , the Jacobian matrix J can be normalized. The Jacobian matrix J satisfies the relationship V = Jθ v . Using the above relationship, Equation 2, and Equation 3, as shown in Equation 4 below, the Jacobian matrix J can be normalized to the Jacobian matrix J N . The normalized Jacobian matrix J N is W vN JW θvN -1It is as follows. In the following, the normalized velocity vector may also be expressed as a normalized velocity command value, a normalized velocity command, a velocity command value, or a velocity command.

[0052]

Number

[0053] When the upper limit value of the velocity vector V of the end effector 12 is appropriately set, the normalized velocity vector V N of the norm ||V N || satisfies ||V N || ≤ 1. Therefore, if the maximum value of the joint velocity of each joint is appropriately set, the norm ||θ vN of the normalized velocity vector θ vN || satisfies ||θ vN || ≤ 1, and the robot arm 11 can be operated according to 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 the normalized velocity vector V Ne or V N . Further, the information processing device 100 applies the normalized velocity vector V Ne or V N and the Jacobian matrix J to a relationship such as J N =W vN JW θvN -1 to calculate the normalized Jacobian matrix J N and stores it in the memory M.

[0055] Next, in step S7, as shown in Equation 5 below, the information processing device 100 performs singular value decomposition on the normalized Jacobian matrix J N to generate a matrix set M1 including three matrices Uv, Σ, and V θ . The matrix Uv is an orthonormal system, and the column vector u of the matrix Uv vi(i = 1, ···, n) is the basis vector of the velocity space of the end effector 12 defined by singular value decomposition. Matrix V θ is an orthonormal system, and the transposed matrix V θ of the column vectors of matrix V θ T The row vectors v θi (i = 1, ···, n) are the basis vectors of the velocity space of a plurality of joints JT defined by singular value decomposition. Matrix Σ is a diagonal matrix, and its diagonal components are singular values σi (i = 1, ···, n). In matrix Σ, the diagonal components are arranged in descending order as σ1 > σ2 > ··· > σ n For the column vector u vi "n" is the dimension of the velocity of the end effector 12, and for the row vector v θi "n" is the dimension of the velocity of a plurality of joints JT. The closer the posture of the robot arm 11 is to the singular point, the smaller the minimum singular value σ n becomes. The matrix set M1 is an example of the second Jacobian matrix. Matrix V θ T is an example of the first matrix, matrix Uv is an example of the second matrix, and matrix Σ is an example of the first singular matrix. Vector v θi is an example of the first basis vector, and vector u vi is an example of the second basis vector.

[0056]

Number

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

[0058] [Number]

[0059] The speed command value V of the end effector 12 can be normalized using Equation 1 to the speed vector V N The normalized speed vector V N is given by its norm ||V N || and the unit vector u N of the normalized speed vector V N and can be expressed as shown in Equation 7 below.

[0060] [Number]

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

[0062] [Number]

[0063] When the posture of the robot arm 11 approaches a singularity, the norm ||θ vN || of the normalized speed vector θ vN || may not satisfy ||θ vN || ≤ 1. Therefore, when the normalized speed vector θ vN in Equation 8 is changed to the normalized speed vector θ vN1 as in Equation 9 below, ||θ vN1 || ≤ 1 is guaranteed. Thus, in order to generate a command value for the operation of the robot arm 11, instead of the normalized speed vector θ vN the normalized speed vector θ vN1 can be used, and thereby the decomposition speed control according to Equation 8 can be changed to the decomposition speed control according to Equation 9.

[0064] [Number]

[0065] Next, in step S8, the information processing apparatus 100 normalizes the commanded speed V N and determines whether the norm ||V N || is greater than or equal to the threshold value ε V . The information processing apparatus 100 proceeds to step S9 if the norm ||V N || is greater than or equal to the threshold value ε V (Yes in step S8), and proceeds to step S10 if the norm ||V N || is less than the threshold value ε V (No in step S8). If the magnitude of the normalized speed vector V N is less than the threshold value ε V so that the normalization calculation for calculating the unit vector u N based on Equation 7, u N =V N / ||V N || becomes numerically unstable. Therefore, the information processing apparatus 100 regards it as 0 without accepting a speed command value such that the norm ||V N || is less than the threshold value ε N . V In step S10, the information processing apparatus 100 performs decomposition speed control according to state 0 by setting the speed command value V of the end effector 12 to 0. As shown in FIG. 9, step S10 includes steps S1001 to S1008, the details of which will be described later.

[0066]

[0067] ​Also, in step S9, the information processing apparatus 100 determines whether the posture of the robot arm 11 is at a singular point or in a region near the singular point. When the posture of the robot arm 11 is at a singular point or in a region near the singular point (Yes in step S9), the information processing apparatus 100 proceeds to step S11. When the posture of the robot arm 11 is neither at a singular point nor in a region near the singular point (No in step S9), the information processing apparatus 100 proceeds to step S12.

[0068] As the posture of the robot arm 11 approaches a singular point, 1 / σ in Equation 8 n becomes large, and eventually, the numerical calculation of the inverse matrix of the Jacobian matrix becomes impossible. Therefore, in step S9, the information processing apparatus 100 performs processing based on the comparison result between the minimum singular value σ n and the threshold value ε σn . The information processing apparatus 100 determines that the posture of the robot arm 11 is at a singular point or in a region near the singular point when σ n < ε σn , and determines that the posture of the robot arm 11 is neither at a singular point nor in a region near the singular point when σ n ≧ ε σn . The threshold value ε σn is an example of a first threshold value.

[0069] The threshold value ε σn is a threshold value set to a non-zero value with respect to the minimum singular value σ n . For example, the threshold value ε σn may be set based on an empirical rule of the minimum singular value σ vN such that the norm ||θ vN || of the normalized velocity vector θ vN represented by Equation 8 satisfies ||θ n ||>1.

[0070] The threshold value ε σn may be set based on the condition number κ defined by κ = σ1 / σ n . For example, a threshold value 1 / κ0 can be set for the reciprocal 1 / κ (= σ n / σ1) of the condition number κ. The threshold value ε σn can be set based on 1 / κ0. For example, ε σncan be set to σ1 / κ0.

[0071] In step S12, the information processing apparatus 100 performs decomposition rate control according to the state NS (Non Singular) which is the decomposition rate control in the normal state. As shown in FIG. 10, step S12 includes steps S1201 to S1209, and the details will be described later.

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

[0073]

Equation

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

[0075] In step S14, the information processing apparatus 100 performs decomposition rate control according to state SC (Singularity Consistent). State SC is decomposition rate control for a state in which the contribution degree in the region of a singularity or the vicinity of a singularity is equal to or greater than a threshold value. As shown in FIG. 11, step S14 includes steps S1401 to S1429, and the details thereof will be described later.

[0076] In step S15, the information processing apparatus 100 performs decomposition rate control according to state P (Perpendicular). State P is decomposition rate control for a state in which the contribution degree in the region of a singularity or the vicinity of a singularity is smaller than a threshold value. As shown in FIG. 12, step S15 includes steps S1501 to S1511, and the details thereof will be described later.

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

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

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

[0080] Next, in step S1004, the information processing apparatus 100 acquires new feedback information of the robot 10, similarly 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, in the same manner as in 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 apparatus 100 performs singular value decomposition on the normalized Jacobian matrix J N in the same manner as in step S7.

[0084] Next, in step S1008, the information processing apparatus 100 determines whether the norm ||V N || of the normalized commanded speed V N is greater than or equal to the threshold value ε V . If the norm ||V N || is greater than or equal to the threshold value ε V (Yes in step S1008), the process proceeds to step S9. If the norm ||V N || is less than the threshold value ε V (No in step S1008), the process returns to step S1001 and the subsequent processing is repeated.

[0085] Using FIG. 10, the details of step S12 regarding the state NS will be described. First, in step S1201, the information processing apparatus 100 generates speed command values for joints JT1 to JT6 by a process according to the decomposition speed control of the state NS. The information processing apparatus 100 normalizes the speed command value V of the end effector 12 into the speed vector V N . Further, the information processing apparatus 100 applies the normalized speed vector V N to Equation 9 to calculate the normalized speed vector θ vN1 of joints JT1 to JT6. The information processing apparatus 100 returns the normalized speed vector θ vN1 to a non-normal form to obtain the speed command values θ v of joints JT1 to JT6.Calculate this. As a result, the speeds of joints JT1 to JT6 are guaranteed to be below the upper limit speeds set for joints JT1 to JT6.

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

[0087] Next, in step S1202, the information processing device 100 uses the difference between the speeds of joints JT1 to JT6 obtained from the detection results of the rotation sensors E included in the feedback information and the speed command value θ v and the current values of motors M1 to M6 included in the feedback information to determine the current values to be applied to motors M1 to M6 to operate joints JT1 to JT6 according to the speed command value θ v . The information processing device 100 outputs a command of the determined current value to the power control circuit 200, and the power control circuit 200 drives motors M1 to M6 according to the current value and operates the robot arm 11.

[0088] Next, in step S1203, the information processing device 100 obtains a new speed command value of the end effector 12 from the signal received from the user interface 30 in the same manner as in step S1003.

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

[0090] Next, in step S1208, the information processing device 100 determines whether the norm ||V N of the normalized command speed V N || is greater than or equal to the threshold value ε V . The information processing device 100 determines whether the norm ||V N || is greater than or equal to the threshold value ε VIf the above is the case (Yes in step S1208), proceed to step S1209, and the norm ||V N || is less than the threshold value ε V If not (No in step S1208), proceed to step S1001 of step S10.

[0091] Next, in step S1209, the information processing apparatus 100 determines, in the same manner as in step S9, whether or not the posture of the robot arm 11 is in a singular point or a region near a singular point. If the posture of the robot arm 11 is in a singular point or a region near a singular point (Yes in step S1209), proceed to step S11. If the posture of the robot arm 11 is neither in a singular point nor in a region near a singular point (No in step S1209), return to step S1201 and repeat the subsequent processing.

[0092] Using FIG. 11, the details of step S14 regarding the state SC will be described. First, in step S1401, the information processing apparatus 100 performs processing according to the decomposition rate control of the state SC. The information processing apparatus 100 converts the inverse matrix J N of the N -1 normalized Jacobian matrix J shown in Equation 6 and calculates a new matrix J εN as shown in the following Equation 11. The matrix J εN is also a normalized matrix. Specifically, the information processing apparatus 100 multiplies all elements of the matrix Σ N -1 included in the inverse matrix J n in the inverse matrix J of Equation 6 by σn / ε N -1 so as to maintain the ratio between the singular value elements 1 / σ1, ···, 1 / σ -1 . The element 1 / σ σn (i = 1, ···, n) is changed to σ i / σ n ε i . Furthermore, σ σn / σ n ε i is expressed as 1 / σ σn . The matrix J i ’ is εNis an example of the third inverse Jacobian matrix, and the inverse matrix of matrix J εN is an example of the third Jacobian matrix. The inverse matrix J N -1 to matrix J εN is an example of the first transformation, and σn / ε σn is an example of the first ratio.

[0093]

Number

[0094] Here, for example, the information processing apparatus 100 can perform processing according to the decomposition speed control shown in the following Equation 12, similar to Equation 9, based on Equation 11. In the vicinity of a singular point or the singular point, the information processing apparatus 100 can generate a normalized speed vector θ N corresponding to the speed command value of the end effector 12 according to the normalized speed vector V vNA corresponding to the speed command values of joints JT1 to JT6. Then, the norm ||θ vNA || of the normalized speed vector θ vNA || is guaranteed to be ||θ vNA || ≤ 1.

[0095]

Number

[0096] Also, in the case of a singular state where the posture of the robot arm 11 corresponds to a singular point, the matrix J εN in Equation 12 becomes a matrix as shown in the following Equation 13. This corresponds to the null space that causes self-motion at the singular point in the robot arm 11.

[0097]

Number

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

[0099] Therefore, in step S1402, the information processing apparatus 100 determines whether the norm ||J εN of the normalization matrix J and the normalized command velocity vector V N of the end effector 12 is greater than or equal to a threshold value κ εN u N ||. The threshold value κ θ is a threshold value set for the norm of the product of the unit vector u θ of the normalized velocity vector V N of the end effector 12 and the normalization matrix J N . The information processing apparatus 100, if the norm ||J εN u εN u N || is greater than or equal to the threshold value κ θ (Yes in step S1402), proceeds to step S1410, and if the norm ||J εN u N || is less than the threshold value κ θ (No in step S1402), proceeds to step S1420. The threshold value κ θ is an example of a second threshold value, and the norm ||J εN u N || is an example of a second norm.

[0100] In step S1410, the information processing apparatus 100 performs processing according to the decomposition velocity control of state SC - 1. The information processing apparatus 100 generates velocity command values for joints JT1 to JT6 according to the decomposition velocity control shown in Equation 12 using the matrix J εN . The information processing apparatus 100, in the same manner as in step S1201, sets the velocity command value V of the end effector 12 as the velocity vector VN is normalized. Further, the information processing apparatus 100 applies the normalized velocity vector V N to Equation 12 to calculate the normalized velocity vectors θ vNA of joints JT1 to JT6. The singular value elements, which are diagonal elements in the matrix J εN in Equation 11, do not diverge even when the robot arm 11 approaches a singular state. Then, the norm ||θ vNA || of the velocity vector θ vNA satisfies ||θ vNA || ≦ 1.

[0101] Furthermore, in the matrix J εN of Equation 11, the ratio between the singular value elements, which are diagonal elements, maintains the ratio between the singular value elements included in the inverse matrix J N of the normalized Jacobian matrix J N -1 . Therefore, the direction in which the robot arm 11 moves the end effector 12 as joints JT1 to JT6 operate according to the normalized velocity vector θ vNA does not change from the direction in which the robot arm 11 moves the end effector 12 as joints JT1 to JT6 operate according to the normalized velocity vector θ vN calculated in step S1201 of the decomposition velocity control in state NS. Thus, the robot arm 11 can move the end effector 12 in the direction of the commanded velocity of the end effector 12 without deviating from the target trajectory of the end effector 12 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.

[0102] Next, in step S1411, the information processing apparatus 100 determines, in the same manner as in step S1202, the applied current values of motors M1 to M6 for operating joints JT1 to JT6 according to the velocity command value. The information processing apparatus 100 outputs a command of the determined current value to the power control circuit 200, and the power control circuit 200 drives motors M1 to M6 according to the current value to operate the robot arm 11.

[0103] Next, in steps S1412 to S1416, the information processing apparatus 100 obtains a new speed command value for the end effector 12 from the signal received from the user interface 30, in the same manner as in steps S1203 to S1207, and calculates the normalized Jacobian matrix J N and performs singular value decomposition on it.

[0104] Next, in step S1417, the information processing apparatus 100 determines, in the same manner as in step S1208, whether the norm ||V N of the normalized commanded speed V N is greater than or equal to the threshold value ε V . The information processing apparatus 100 proceeds to step S1418 when the norm ||V N is greater than or equal to the threshold value ε V (Yes in step S1417), and proceeds to step S1001 of step S10 when the norm ||V N is less than the threshold value ε V (No in step S1417).

[0105] In step S1418, the information processing apparatus 100 calculates a new matrix J N by converting the inverse matrix J N -1 of the normalized Jacobian matrix J εN calculated in steps S1412 to S1416, in the same manner as in step S1401.

[0106] Next, in step S1419, the information processing apparatus 100 determines whether the norm ||J εN u N of the normalized Jacobian matrix J εN and the normalized commanded speed vector V N of the end effector 12 is less than the threshold value κ θ . The information processing apparatus 100 returns to step S1402 when the norm ||J εN u N is less than the threshold value κ θ (Yes in step S1419), and when the norm ||J εN u N is greater than or equal to the threshold value κ θIf the above is the case (No in step S1419), the process returns to step S1410, and the subsequent processes are repeated.

[0107] In step S1420, the information processing apparatus 100 performs processing according to the decomposition rate control in state SC-2. Instead of Equation 12, the information processing apparatus 100 performs processing according to the decomposition rate control shown in the following Equation 14. In Equation 14, the norm ||J εN u N || is fixed to a constant by being replaced with the threshold value κ θ . Thereby, the numerical instability of the joint speed calculation when the norm ||J εN u N || becomes too small is suppressed. The information processing apparatus 100 uses the matrix J εN and generates speed command values for joints JT1 to JT6 according to the decomposition rate control shown in Equation 14 with the norm ||J εN u N || fixed.

[0108]

Number

[0109] Similar to step S1410, the information processing apparatus 100 applies the normalized speed vector V N of the speed command value V of the end effector 12 to Equation 14 to calculate the normalized speed vector θ vNB of joints JT1 to JT6. Each element of the normalized speed vector θ vNB indicates a speed corresponding to the magnitude of the norm ||J εN u N ||. Each element of the normalized speed vector θ vNB continuously and gradually decreases the speeds of joints JT1 to JT6 as the norm ||J εN u N || becomes smaller.

[0110] And the norm ||θ vNB || of the normalized speed vector θ vNB is ||θ vNBsatisfies ||≦1. Further, in the same manner as in step S1410, the joint JT1 to JT6 operate according to the normalized velocity vector θ vNB The direction in which the robot arm 11 moves the end effector 12 by the operation of the joints JT1 to JT6 according to is the normalized velocity vector θ calculated in step S1201 of the decomposition velocity control in the state NS vN does not change from the moving direction of the end effector 12 according to. Therefore, the robot arm 11 does not deviate from the trajectory targeting the end effector 12 in the vicinity of the singularity, and does not suddenly stop due to an error in which the joint velocity exceeds the upper limit value at the singularity, and the end effector 12 can be moved in the direction of the commanded velocity of the end effector 12

[0111] Next, in step S1421, the information processing apparatus 100 determines, in the same manner as in step S1411, the applied current values of the motors M1 to M6 for operating the joints JT1 to JT6 according to the velocity command value. The information processing apparatus 100 outputs a command of 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 current value to operate the robot arm 11

[0112] Next, in steps S1422 to S1426, the information processing apparatus 100 obtains a new velocity command value of the end effector 12 from the signal received from the user interface 30, and calculates and performs singular value decomposition on the normalized Jacobian matrix J N in the same manner as in steps S1412 to S1416

[0113] Next, in step S1427, the information processing apparatus 100, in the same manner as in step S1417, calculates the norm ||V N of the normalized commanded velocity V N If || is greater than or equal to the threshold value ε V (Yes in step S1427), the process proceeds to step S1428. If the norm ||V N || is less than the threshold value ε V (No in step S1427), the process proceeds to step S1001 of step S10

[0114] In step S1428, the information processing apparatus 100, similar to step S1418, calculates the inverse matrix J N of the normalized Jacobian matrix J N -1 calculated in steps S1422 to S1426, and calculates a new matrix J εN .

[0115] Next, in step S1429, the information processing apparatus 100 determines whether the norm ||J εN u N || of the normalized command velocity vector V of the end effector 12 is greater than or equal to a threshold value κ εN u N ||. If the norm ||J θ u εN u N || is greater than or equal to the threshold value κ θ (Yes in step S1429), the process returns to step S1402. If the norm ||J εN u N || is less than the threshold value κ θ (No in step S1429), the process returns to step S1420 and the subsequent processing is repeated.

[0116] Using FIG. 12, the details of step S15 regarding the state P will be described. First, in step S1501, the information processing apparatus 100 generates a new matrix J εN which is a matrix obtained by converting the normalized matrix J of Equation 11 in step S1401 and is shown in the following Equation 15 εNA . The matrix J εNA is also a normalized matrix. In the above conversion, the element 1 / ε εN of the normalized matrix J σn is replaced with 0. The matrix J εNA is an example of the fourth inverse Jacobian matrix.

[0117]

Equation

[0118] The contribution degree w n is greater than or equal to a threshold value ε wnWhen it is less than and in a sufficiently small state, among the directional components of the commanded velocity of the end effector 12, the directional components of the velocity of the end effector 12 that cannot be generated by the singular state are very small. Such directional components of velocity are the velocity components related to the singular value element 1 / ε σn For example, the normalization matrix J of Equation 11 in step S1401 εN is used to generate the normalized velocity vector θ vNA When generated, the normalized velocity vector θ vNA can realize the movement of the end effector 12 according to the direction of the commanded velocity of the end effector 12, but it will be realized in a state where the magnitude of the velocity of the end effector 12 is very small.

[0119] On the other hand, when the contribution degree w n is 0, among the directional components of the commanded velocity of the end effector 12, the directional components of the velocity of the end effector 12 that cannot be generated by the singular state will not exist. That is, the commanded velocity in the direction of the singular value element 1 / ε σn will not exist. Therefore, the above conversion cuts off such directional components by setting to 0 the element 1 / ε n related to the directional components of the velocity of the end effector 12 that cannot be generated by the singular state in a state where the contribution degree w σn is sufficiently small, so that the commanded velocity of the end effector 12 can be realized as it is.

[0120] According to Equation 15, the commanded velocity in the direction of the singular value element 1 / ε σn is regarded as not existing. This corresponds to changing the normalized velocity vector V N corresponding to the commanded velocity of the end effector 12 to a new velocity vector V NA as shown in the following Equation 16.

[0121]

Equation

[0122] Next, in step S1502, the information processing apparatus 100 normalizes the matrix JεNA In the same manner as in 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 the velocity command value V of the end effector 12. N Applying Equation 17, the normalized velocity vectors θ vNC Calculate.

[0123]

number

[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 nonexistent, the number of dimensions of the hand velocity is reduced by one, and as a result, the dimension is reduced and it corresponds to the velocity command value for the robot arm 11 that is no longer in a singular state. For this reason, the normalized velocity vector θ 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 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. 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 is changed by the operation of the user interface 30 by the operator, and the contribution w n is the threshold ε wn The threshold value ε wn The contribution ratio w may change to a state less than n is the threshold ε wnIn the above state, for example, the robot arm 11 moves the end effector 12 at a speed according to the normalized velocity vector θ of Equation 12 in Step S1401 vNA or the normalized velocity vector θ of Equation 14 in Step S1420 vNB At this time, as the contribution degree w n decreases, the speed of the end effector 12 decreases. Furthermore, as the contribution degree w n becomes less than the threshold value ε wn the robot arm 11 moves the end effector 12 at a speed according to the normalized velocity vector θ of Equation 17 vNC

[0126] As described above, the normalized velocity vector θ vNC corresponds to the speed command value for the robot arm 11 with one less dimension of speed and no singular state, and can approximately realize the commanded speed of the end effector 12. Therefore, at the timing when the contribution degree w n becomes less than the threshold value ε wn the state transitions to state P, and there is a possibility that the end effector 12 of the robot arm 11 suddenly moves at a certain speed. However, as can be seen from FIGS. 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 the adjustment of the operation of the user interface 30

[0127] In Step S1503, the information processing device 100 determines the applied current values of the motors M1 to M6 for operating the joints JT1 to JT6 according to the speed command value in the same manner as in Step S1202. The information processing device 100 outputs a command of 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 current value to operate the robot arm 11

[0128] ​Next, in steps S1504 to S1508, the information processing apparatus 100 obtains a new speed command value for the end effector 12 from the signal received from the user interface 30, in the same manner as in steps S1203 to S1207, and calculates the normalized Jacobian matrix J N and performs singular value decomposition on it.

[0129] Next, in step S1509, the information processing apparatus 100, in the same manner as in step S1208, determines that if the norm ||V N of the normalized commanded speed V N is greater than or equal to the threshold value ε V (Yes in step S1509), it proceeds to step S1510, and if the norm ||V N is less than the threshold value ε V (No in step S1509), it proceeds to step S1001 of step S10.

[0130] In step S1510, the information processing apparatus 100, in the same manner as in step S11, calculates the contribution degree w N in the direction of the base vector u n associated with the minimum singular value σ vn of the normalized commanded speed vector V n for the speed command value V of the end effector 12 obtained in steps S1504 to S1508.

[0131] Next, in step S1511, the information processing apparatus 100 determines whether or not the condition that the minimum singular value σ n obtained by the singular value decomposition in steps S1504 to S1508 is greater than or equal to the threshold value ε σn and the contribution degree w n is less than the threshold value ε wn is satisfied. If the information processing apparatus 100 determines that the condition is satisfied (Yes in step S1511), it proceeds to step S12, and if the condition is not satisfied (No in step S1511), it returns to step S1501 and repeats the subsequent processing. When the above condition is satisfied, among the speed command values V of the end effector 12, the minimum singular value σ nThe velocity component in the direction related to is small, and the minimum singular value σ n is not in a singular state. Therefore, the robot arm 11 can escape from the state P.

[0132] From step S3 to S15, the information processing device 100 transitions the decomposition velocity control method among the states 0, NS, SC, and P, and without deviating the end effector 12 from the target trajectory near the singular point, without suddenly stopping due to an error where the joint velocity exceeds the upper limit value at the singular point, and moves the end effector 12 in the direction of the commanded velocity of the end effector 12, the robot arm 11 can be controlled.

[0133] In state 0, the commanded velocity of the end effector 12 is less than the threshold value, and the robot arm 11 is stopped by regarding the velocity command value of the end effector 12 as 0. State NS is a state where the velocity command value of the joint JT can be generated by normal decomposition velocity control. State SC is a state where the robot arm 11 is close to or in a singular state, and the velocity command value of the joint JT can be generated by decomposition velocity control corresponding to the state. State P is a state where the commanded velocity of the end effector 12 corresponds to the singular value σ n and does not include the component of the command direction u vn corresponding to it, and the velocity command value of the joint JT can be generated by decomposition velocity control corresponding to the state.

[0134] The information processing device 100 determines the transition of the decomposition velocity control between the state NS and the state SC according to whether the relationship between the minimum singular value σ n and the threshold value ε σn is σ n ≧ε σn or σ n <ε σn For example, σ n <ε σn and the contribution degree w n is w n ≧ε wnIf the condition is satisfied, the information processing apparatus 100 determines the transition to the decomposition rate control of the state SC. Thus, the continuity of the transition between the state NS and the state SC is maintained. Further, in the state SC, the information processing apparatus 100 determines the transition of the decomposition rate control between two states according to whether the relationship between the norm ||J εN u N || and the threshold value κ θ is ||J εN u N ||≧κ θ or ||J εN u N ||<κ θ . In either of the two states, the information processing apparatus 100 performs the decomposition rate control so that the norm ||J εN u N || does not fall below the threshold value κ θ , thus maintaining the continuity of the transition between the two states.

[0135] The information processing apparatus 100 can determine the transition between the state NS and the state P according to whether the relationship between the contribution degree w n and the threshold value ε wn is w n ≧ε wn or w n <ε wn . When w n <ε wn and the relationship between the minimum singular value σ n and the threshold value ε σn satisfies σ n <ε σn , the information processing apparatus 100 transitions from the state NS to the decomposition rate control of the state P and removes the component of the command direction u n corresponding to the singular value σ vn from the command speed of the end effector 12. By setting the threshold value ε wn to be small, the continuity of the transition between the two states is maintained.

[0136] In the state P, since the component of the command direction u vn is removed from the command speed of the end effector 12, the contribution degree w n in the command speed of the end effector 12 becomes w n ≧ε wnEven if the relationship is satisfied, the actual speed of the end effector 12 commanded to the robot arm 11 is the one from which the component in the direction u vn is removed. Therefore, the information processing apparatus 100, w n <ε wn and σ n ≧ε σn transitions to the decomposition speed control from state P to state NS when satisfied. In this case, the continuity of the transition between the two states is maintained.

[0137] The information processing apparatus 100 does not allow a direct transition between state SC and state P. To transition from state SC to state P, first set the commanded speed of the end effector 12 to 0 and transition to state 0 to stop the robot arm 11 once, and then, the contribution w n is w n <ε wn It can be transitioned to state P by immediately outputting a speed command value of the end effector 12 that satisfies the relationship. Note that the information processing apparatus 100 can also transition to the decomposition speed control of state P after once transitioning to the decomposition speed control of state NS.

[0138] Conversely, to transition from state P to state SC, first set the commanded speed of the end effector 12 to 0 and transition to state 0 to stop the robot arm 11 once, and then, the contribution w n is w n ≧ε wn It can be transitioned to state SC by outputting a speed command value of the end effector 12 that satisfies the relationship. Note that the information processing apparatus 100 can also transition to the decomposition speed control of state SC after once transitioning to the decomposition speed control of state NS from state P.

[0139] [Verification of guarantee of upper limit of movement speed of end effector 12] In steps S1 to S15, the information processing apparatus 100 normalizes the speed vector θ corresponding to the speed command value of the joint JT vN so that the norm ||θ vN || of is such that ||θ vN ||≦1 is satisfied, and the normalized speed vector θ vNProcess it. However, depending on the posture of the robot arm 11, the magnitude of the speed of the end effector 12 realized by the speed command value that makes the normalized speed vector θ vN may exceed the upper limit of the commanded speed of the end effector 12. Therefore, for the normalized speed vector of joint JT obtained by Equations 9, 12, 14, and 17, verify the guarantee of the upper limit of the moving speed of the end effector 12.

[0140] First, in state SC, for the normalized speed vector θ vN1 obtained by Equation 9, the normalized speed vector V vN1 of the end effector 12 realized by the normalized speed vector θ NR is obtained as shown in the following Equation 18. Furthermore, as shown in the following Equation 19, the norm ||V NR || of the normalized speed vector V NR may be larger than the norm ||V N || of the normalized commanded speed vector V N of the end effector 12.

[0141]

Equation

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

[0143]

Equation

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

[0145]

Number

[0146] Therefore, the normalized velocity vector θ corrected by Equation 20 vN1 corresponds to the normalized velocity vector of joint JT obtained by applying normal decomposition velocity control to the normalized commanded velocity vector V of the end effector 12 N . Therefore, it can be confirmed that the norm ||V NR || is less than or equal to the norm ||V N ||.

[0147] Next, in the state SC, for the normalized velocity vector θ obtained by Equation 12 vNA , the normalized velocity vector V of the end effector 12 realized by the normalized velocity vector θ vNA is obtained as shown in Equation 24 below. Furthermore, as shown in Equation 25 below, the norm ||V NRA || of the normalized velocity vector V NRA may be larger than the norm ||V NRA || of the normalized commanded velocity vector V of the end effector 12 N . N ||.

[0148]

Number

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

[0150]

Number

[0151] The normalization matrix J of Equation 12​εN is the normalized Jacobian matrix J N and its inverse matrix J N -1 satisfies the relationship of the following Equation 27. And from Equations 26 and 27, the following Equation 28 is obtained. Therefore, the normalized velocity vector θ vNA corrected by Equation 26 is equivalent to the normalized velocity vector of joint JT obtained by applying normal decomposition velocity control to the normalized command velocity vector V N of the end effector 12. Therefore, it can be confirmed that the norm ||V NRA || is less than or equal to the norm ||V N ||.

[0152]

Equation

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

[0154]

Equation

[0155] In this case, the normalized velocity vector θ vNB can be corrected as shown in the following Equation 31. The norm ||V NRB || is an example of the first norm, and the corrected normalized velocity vector θ vNB is an example of the correction operation command value.

[0156]

Number

[0157] The normalization matrix J of Equation 14 εN is the normalization Jacobian matrix J N and its inverse matrix J N -1 satisfies the relationship of Equation 27. Then, from Equations 27 and 31, the following Equation 32 is obtained. Therefore, the normalized velocity vector θ vNB corrected by Equation 31 corresponds to the normalized velocity vector of joint JT obtained by applying normal decomposition velocity control to the normalized command velocity vector V N of the end effector 12. Therefore, it can be confirmed that the norm ||V NRB || is less than or equal to the norm ||V N ||.

[0158]

Number

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

[0160]

Number

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

[0162]

Equation

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

[0164]

Equation

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

[0166]

Equation

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

[0168] The normalization matrix J ’ εNA ​The state P to which it is applied is the contribution degree w n is less than the threshold value ε wn and is applied. Therefore, the norm ||V NA || is asymptotically equal to the norm ||V N ||. Thus, the influence of ||V N || / ||V NA || can be ignored.

[0169] The solution by the pseudo-inverse matrix J ’ εNA is the one with the smallest norm among those that best realize the normalized commanded velocity vector V N of the end effector 12. Therefore, the norm ||V vNC || of the velocity of the end effector 12 realized by the normalized velocity vector θ NRC of the joint JT obtained by Equation 38 N does not exceed the norm ||V N || of the normalized commanded velocity vector V of the end effector 12.

[0170] From the above, in both cases of the state SC and the state P, it can be confirmed that 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 apparatus 100 does not exceed the magnitude of the commanded velocity of the end effector 12.

[0171] [Processing for Self-Motion in Singular States] In the Jacobian matrix J, the minimum singular value σ n as shown in Equation 5 may become 0. In such a case, the robot arm 11 enters a complete singular state. The normalized Jacobian matrix J N at this time is shown as in Equation 39 below.

[0172] [Equation]]

[0173] The base vector v θnIn the singular state, it becomes a null space that does not affect the velocity of the end effector 12 in the velocity space of the joint JT. Therefore, in the singular state, the information processing device 100 outputs a velocity command value of the joint JT in the null space for the velocity command value of the vector u Vn included in the velocity command value V of the end effector 12 in the direction. Self-motion occurs in the operation of the robot arm 11 according to such a velocity command value of the joint JT.

[0174] The robot arm 11 can continue self-motion in the wrist singular posture and the shoulder singular posture shown in FIGS. 6 and 7. In the elbow singular posture shown in FIG. 5, the robot arm 11 may escape from the singular point due to self-motion. In the case of the elbow singular posture, if the velocity command value of the end effector 12 in the direction of the vector u Vn is continuously given, the robot arm 11 once reaches the singular point as shown in FIGS. 13A to 13B, and then escapes from the singular point by self-motion and moves the end effector 12 in the direction opposite to the target direction as shown in FIG. 13C. Thereafter, the robot arm 11 moves the end effector 12 toward the singular point again as shown in FIG. 13C and reaches the state of FIG. 13B. Thereafter, the robot arm 11 repeats the state of FIG. 13C from the state of FIG. 13A. Thereby, the vibration phenomenon of the robot arm 11 is induced. FIGS. 13A to 13C are conceptual diagrams showing an example of the self-motion of the robot arm 11 in the case of the elbow singular posture.

[0175] Therefore, in order to prevent such vibration phenomena, when the robot arm 11 reaches the state shown in FIG. 13B, the information processing apparatus 100 temporarily stops all operations of joints JT1 to JT6 of the robot arm 11. For example, if only the joints JT related to self-motion among the joints JT1 to JT6 of the robot arm 11 are stopped in order to prevent vibration phenomena, 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 apparatus 100 prevents the end effector 12 from moving in such an unintended direction. Then, the information processing apparatus 100 operates the robot arm 11 so as to deviate from the elbow-singular posture according to a command from the user interface 30 or autonomously. Note that in a singular state other than the elbow-singular posture, the information processing apparatus 100 may cause the robot arm 11 to deviate from the singular state after temporarily stopping it.

[0176] In addition, the information processing apparatus 100 can identify the type of singular state of the robot arm 11 from the geometric parameters used for calculating the Jacobian matrix. The speed command value V of the end effector 12, the speed command value θ of the joint JT v and the Jacobian matrix J satisfy V = Jθ v This equation is further differentiated with respect to time to obtain the following equation 40.

[0177]

Equation

[0178] During self-motion, when the norm ||(dJ / dt)θ v of the element (dJ / dt)θ v is not zero, the robot arm 11 cannot stay in its singular state due to self-motion like an elbow singularity point and will escape.

[0179] In the Jacobian matrix J, the minimum singular value σn as shown in Equation 5 is a threshold value ε σnWhen approaching and the posture of the robot arm 11 approaches a singularity, the base vector v, which is the joint velocity vector corresponding to the minimum singular value σn θn can be focused on. The joint velocity vector v θn represents the joint velocity corresponding to self-motion as shown in Equation 39 in the singular posture where the singular value σn = 0.

[0180] In the case of a singularity where self-motion can be continued, such as the wrist singular posture and the shoulder singular posture, even in the vicinity of the singularity, the joint velocity vector v θn represents a velocity close to self-motion. Therefore, the acceleration of the end effector 12 moved by the robot arm 11 is sufficiently small. For this reason, the minimum singular value σ n and the threshold value ε σn where σ n < ε σn in the vicinity of the singularity that satisfies the condition, as shown in Equation 41, the absolute value |(dJ / dt)v θn | of the element (dJ / dt)v θn | is less than the threshold value ε a If the condition is satisfied, the singularity can be regarded as a singularity where self-motion can be continued. The threshold value ε a is the threshold value of the magnitude of the 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 the acceleration vector, and the threshold value ε a is an example of the fourth threshold value.

[0181]

Equation

[0182] Therefore, when the condition of Equation 41 is satisfied, the information processing device 100 determines the singularity as a singularity where the self-motion of the robot arm 11 can be continued. When the condition of Equation 41 is not satisfied, the information processing device 100 determines the singularity as a singularity where the self-motion of the robot arm 11 cannot be continued. In addition to or instead of the condition of Equation 41, the information processing device 100 uses the norm ||(dJ / dt)θ vOn the condition that || is a non-zero value, the singularity may be determined as a singularity at which self-motion can be continued. The information processing apparatus 100 uses the joint velocity vector v θn to be able to discriminate the type of singularity regardless of the velocity command value of the current end effector 12.

[0183] When the information processing apparatus 100 determines the singularity as a singularity at which self-motion cannot be continued, the information processing apparatus 100 stops the robot arm 11 as follows. The information processing apparatus 100 calculates the absolute value |(dJ / dt)v θn | using the information acquired for each sampling period. During the process in which the robot arm 11 operates, the information processing apparatus 100 may stop the robot arm 11 when the absolute value |(dJ / dt)v θn | reaches its maximum. For example, the information processing apparatus 100 may use the timing at which the variation of the absolute value |(dJ / dt)v θn | changes from an increase to a decrease as the timing for stopping the robot arm 11. Although not limited, in the case of the elbow singular posture, the above timing is particularly suitable for the timing of stopping the robot arm 11.

[0184] Furthermore, the information processing apparatus 100 may determine whether the direction of the commanded velocity of the end effector 12 has changed, such as by reversal, at the above timing. Thereby, it is possible to determine whether the change in the variation of the absolute value |(dJ / dt)v θn | from an increase to a decrease is caused by a change in the direction of the commanded velocity of the end effector 12. The information processing apparatus 100 calculates the inner product u n of the unit vector u of the commanded velocity of the end effector 12 used for calculating the contribution degree w in Equation 10 and the velocity vector u θn of the end effector 12 corresponding to the joint velocity vector v vn indicating self-motion, and stores it in the memory M using the information acquired for each sampling period. The information processing apparatus 100 calculates the inner product u vn T u at the timing when the variation of the absolute value |(dJ / dt)v θn | changes from an increase to a decrease or in the vicinity thereof, andvn T When the sign of u is not inverted, stop the robot arm 11.

[0185] The information processing apparatus 100 determines the timing to move the stopped robot arm 11 again based on the inner product u vn T The information processing apparatus 100 may determine based on the sign of u. For example, when the information processing apparatus 100 determines to stop the robot arm 11, the inner product u vn T From the sign of u, the inner product u vn T The information processing apparatus 100 may determine the timing when the sign of u is inverted as the timing to move the robot arm 11 again. The inner product u vn T When u is approximately 0, the contribution degree of the commanded speed of the end effector 12 in the direction of the velocity vector u vn can be ignored. In this case, when the information processing apparatus 100 moves the robot arm 11, the end effector 12 will move along the boundary of the operation area of the robot arm 11.

[0186] [Case where the robot arm falls into a plurality of singular states] The robot arm 11 may fall into a plurality of singular states. In this case, two or more singular values σi are less than the threshold value ε σn In the normalized Jacobian matrix J of Equation 5 N Regarding an example where the singular values σ n and σ n-1 are less than the threshold value ε σn the processing of the information processing apparatus 100 will be described. In this example, the singular values σ n and σ n-1 satisfy the relationship of σ 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 apparatus 100 determines that the contribution degree w n is less than the threshold value ε wnFor the state SC as described above, similar to the above-described step S1401, the normalized Jacobian matrix J shown in Equation 6 N of the inverse matrix J N -1 within the matrix Σ -1 of all elements to σ n / ε σn times, to calculate a new matrix J as shown in the following Equation 42. However, since the singular value σ εN is below the threshold value ε n-1 , for the element 1 / σ σn including the singular value σ n-1 , the information processing apparatus 100 performs a process to prevent digit loss in numerical calculation as shown in the following Equation 43. Since σ n-1 <σ n , the element 1 / σ n-1 does not diverge. The information processing apparatus 100 performs decomposition speed control based on the matrix J n-1 ’ . The contribution degree w εN is an example of the first contribution degree. n is an example of the first contribution degree.

[0188]

Number

[0189] In the state P where the contribution degree w n is less than the threshold value ε wn , the information processing apparatus 100 regards the singular value σ n-1 as the minimum singular value. The information processing apparatus 100 multiplies all elements of the matrix Σ N in the inverse matrix J N -1 of the normalized Jacobian matrix J shown in Equation 6 by σ -1 / ε n-1 . The element 1 / σ σn i (i = 1, ···, n - 2) is changed to σ n-1 / σ i ε σn . Further, σ n-1 / σ i ε σn is changed to 1 / σ i . i". Furthermore, in the same manner as in step S1501 described above, the information processing device 100 calculates the singular value σ n In this way, the information processing device 100 generates a new matrix J ε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 a second ratio.

[0190]

number

[0191] Two singular values σ n and σ n-1 Each has a threshold ε σn In the singular state below the singular value σ 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 of the state of the contribution degree w n is as shown in Equation 10. Contribution w n-1 is as shown in the following equation 45, and the normalized command velocity vector V N The singular values σ n-1 The basis vector u of the velocity space corresponding to vn-1 is the contribution in the direction of n and w n-1 For each of these, the threshold ε wn is set. The threshold value ε wn , 0<ε wn Contribution w is set in the range of <1. n-1 is an example of a second contribution.

[0192]

number

[0193] (1) w n ≥ ε wn and w n-1 ≥ ε wn in the first case The contribution degree w n is greater than or equal to the threshold value ε wn Therefore, the information processing apparatus 100 performs the decomposition rate control of the state SC using the normalization matrix J shown in Equation 42 and Equation 11 εN For example, when the difference between the singular value σ n << singular value σ n-1 is large, when the robot arm 11 performs self-motion, the self-motion corresponding to the singular value σ n and the singular value σ n-1 becomes dominant. As the difference between the singular value σ n and the singular value σ n and the singular value σ n-1 decreases, the robot arm 11 can perform a combined operation of the self-motion corresponding to the singular value σ n and the self-motion corresponding to the singular value σ n-1

[0194] (2) w n ≥ ε wn and w n-1 < ε wn in the second case The contribution degree w n is greater than or equal to the threshold value ε wn Therefore, the information processing apparatus 100 performs the decomposition rate control of the state SC using the normalization matrix J shown in Equation 42 and Equation 11 εN For example, when the difference between the singular value σ n << singular value σ n-1 is large, when the robot arm 11 performs self-motion, the self-motion corresponding to the singular value σ n and the singular value σ n-1 becomes dominant. Therefore, the contribution degree w n being less than the threshold value ε n-1 has a small influence on the self-motion of the robot arm 11 wn

[0195] ​​ (3) w n <ε wn and w n-1 ≥ ε wn in the third case The contribution degree w n is below the threshold value ε wn Therefore, the information processing apparatus 100 performs control of the decomposition rate of the state P using the normalization matrix J of Equation 44 εNA The contribution degree w n-1 is above the threshold value ε wn Therefore, when the robot arm 11 performs self - motion, the self - motion corresponding to the singular value σ n-1 becomes dominant.

[0196] (4) w n <ε wn and w n-1 <ε wn in the fourth case The contribution degree w n is below the threshold value ε wn Therefore, the information processing apparatus 100 performs control of the decomposition rate of the state P based on the normalization matrix J of Equation 44 εNA Furthermore, since the contribution degree w n-1 is below the threshold value ε wn a process for generating the normalization matrix J of Equation 15 recursively is performed for the complementary space excluding the space corresponding to the minimum singular value σ n That is, the information processing apparatus 100 performs a transformation that replaces the element 1 / ε εNA with 0 for the normalization matrix J of Equation 44 to generate a new normalization matrix J shown in the following Equation 46 εNA The information processing apparatus 100 performs control of the decomposition rate of the state P using the normalization matrix J of Equation 46 σn That is, it is control of the decomposition rate in a 4 - dimensional space where the 2 - dimensional space corresponding to two singular values is reduced. The matrix J εNA1 is an example of the sixth inverse Jacobian matrix and the seventh inverse Jacobian matrix. εNA1 εNA1 εNA1 is an example of the sixth inverse Jacobian matrix and the seventh inverse Jacobian matrix.

[0197]

Equation

[0198] Also, when the robot arm 11 operates in a singular state where two singular values σ n and σ n-1 each fall below the threshold value ε σn there may be a case where the magnitude relationship between the two singular values σ n and σ n-1 remains below the threshold value ε σn and the magnitude relationship between the two singular values σ n and σ n-1 is reversed. Even in such a case, the information processing apparatus 100 controls the decomposition speed of the robot arm 11 corresponding to four cases regarding the states of the contribution degrees w n and w n-1 respectively. n and w n-1 For the four cases regarding the states of the contribution degrees w

[0199] (1) w n ≥ ε wn and w n-1 ≥ ε wn In the first case the information processing apparatus 100 performs decomposition speed control for the state SC. For example, the information processing apparatus 100 can perform decomposition speed control according to Equation 12 using the normalization matrix J εN shown in Equation 42 and Equation 11, or perform decomposition speed control according to Equation 14 using the normalization matrix J εN shown in Equation 42 and Equation 11. However, immediately before the magnitude relationship between the two singular values σ n and σ n-1 is reversed, since the difference between the singular values σ n and σ n-1 is small, even if the contribution degree w n decreases due to the reversal, the norm ||J εN u N || will not be less than the threshold value κ θ Therefore, the information processing apparatus 100 does not perform decomposition speed control according to Equation 14. Thus, the robot arm 11 can perform a combined motion between the self-motion corresponding to the singular value σ n and the self-motion corresponding to the singular value σ n-1 Furthermore, the singular values σ n and σ n-1Even if the magnitude relationship changes, there is no change in the speed command value of the joint JT, 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 degree w n is greater than or equal to the threshold value ε wn Therefore, the information processing apparatus 100 performs decomposition speed control of the state SC. For example, the information processing apparatus 100 performs decomposition speed control according to Equation 12 using the normalization matrix J shown in Equations 42 and 11, or decomposition speed control according to Equation 14 using the normalization matrix J shown in Equations 42 and 11. When the robot arm 11 performs self-motion, the self-motion corresponding to the singular value σ εN becomes dominant. In this state, when the magnitude relationship between the singular values σ εN and σ n is reversed, the contribution degree of the singular value σ n and σ n-1 after the reversal is below the threshold value. Therefore, the information processing apparatus 100 performs decomposition speed control of the state P. However, the state in which the self-motion corresponding to the singular value σ n is dominant is maintained. n

[0201] (3)w n <ε wn and w n-1 ≧ε wn the third case of contribution degree w n is below the threshold value ε wn Therefore, the information processing apparatus 100 performs decomposition speed control of the state P using the normalization matrix J of Equation 44. When the robot arm 11 performs self-motion, the self-motion corresponding to the singular value σ εNA becomes dominant. In this state, when the magnitude relationship between the singular values σ n-1 and σ n is reversed, at the commanded speed of the end effector 12, the singular value σ n-1 after the reversal n-1 ​The state in which there is no commanded velocity in the corresponding direction is maintained. The information processing apparatus 100 may exit the state P by setting the commanded velocity of the end effector 12 to zero according to a command from the user interface 30 or autonomously.

[0202] (4)w n <ε wn and w n-1 <ε wn for the fourth case The information processing apparatus 100 performs decomposition velocity control of the state P using the normalization matrix J of Equation 46. The normalization matrix J εNA1 removes the direction components corresponding to the singular values σ εNA1 and σ n and σ n-1 are removed. Therefore, even if the magnitude relationship between the singular values σ n and σ n-1 is swapped, the decomposition velocity control of the information processing apparatus 100 is not affected. The information processing apparatus 100 may exit the state P by setting the commanded velocity of the end effector 12 to zero according to a command from the user interface 30 or autonomously.

[0203] Thus, the information processing apparatus 100 does not need to perform special processing corresponding to the swap even if the magnitude relationship between two singular values in a singular state below the threshold ε σn is swapped. The information processing apparatus 100 can perform decomposition velocity control for each state by determining the Jacobian matrix, performing singular value decomposition, and performing the above-described processing based on the magnitude relationship of the singular values every time the feedback information from the robot 10 is updated.

[0204] Also, when three or more singular values are below the threshold, or when a plurality of contributions corresponding to the singular values are below the threshold, it is possible to cope by appropriately changing the elements in the matrix Σ -1 in the same manner as above. Thus, this method has generality such that it can also cope with the case of falling into a plurality of singular states at the same time. For example, when each of three or more singular values is below the threshold ε σnIn a singular state where the singular value falls below 0, 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 is switched between three or more singular values, 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 σ n to the k-th singular value σ n-k+1 The 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 determines the first contribution degree w n From the kth contribution w n-k+1 The contribution rate 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 that is first determined to be equal to or greater than n-i+1 The i-th singular value σ n-i+1 Based on this, the normalized Jacobian matrix J shown in Eq. N Inverse matrix J N -1 Convert i to k, where i is a natural number between 1 and k. n-i+1 The ordinal number i of wn It is the smallest among the ordinals of the contribution degrees that are equal to or greater than 1.

[0207] In the above transformation, the information processing device 100 calculates the inverse matrix J N -1 Matrix Σ in -1Among the singular values included in, the first singular value σ n to the (i - 1)-th singular value σ n-i is replaced with 0 by zero transformation. Further, the information processing apparatus 100 performs zero transformation on the (i + 1)-th singular value σ n-i to the k-th singular value σ n-k+1 For each of the corresponding (i + 1)-th contribution degree w n-i to the k-th contribution degree w n-k+1 it is determined whether the contribution degree is equal to or greater than the threshold value ε wn The information processing apparatus 100 performs zero transformation on the singular values whose contribution degree is less than the threshold value ε wn by replacing them with 0.

[0208] Furthermore, the information processing apparatus 100 multiplies all elements of the matrix Σ -1 after the two zero transformations by the third ratio σ n-i+1 / ε σn times to calculate a new normalized matrix transformed from the inverse matrix J N -1 The i-th singular value σ n-i+1 is the minimum singular value among the i-th singular value σ n-i+1 to the k-th singular value σ n-k+1 The information processing apparatus 100 performs decomposition speed control of the state P using the normalized matrix in the same manner as in the above third case. The normalized matrix is an example of the seventh inverse Jacobian matrix.

[0209] For example, when all of the first contribution degree w n to the k-th contribution degree w n-k+1 are equal to or greater than the threshold value ε wn the information processing apparatus 100 uses, as the third ratio, the first ratio σ n / ε σn The information processing apparatus 100 multiplies all elements of the matrix Σ N in the inverse matrix J N -1 of the normalization Jacobian matrix J -1 shown in Equation 6 by the first ratio σ n / ε σn times to calculate the normalization matrix J εN shown in Equation 42. The information processing apparatus 100 uses the normalization matrix J εNThe decomposition rate of state SC is controlled using

[0210] For example, the first contribution w n From the kth contribution w n-k+1 All 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 In the new normalization matrix, the information processing device 100 performs zero conversion to replace all of the matrices Σ -1 All elements of are divided by the third ratio σ n-k+1 / ε σn The information processing device 100 performs decomposition speed control of the state P using the normalization matrix, similarly to the above-described fourth case. The normalization matrix is an example of a seventh inverse Jacobian matrix.

[0211] For example, the 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 Threshold ε wn The information processing device 100 may control the disassembly speed of the robot arm 11 in response to eight cases related 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 motion of the robot arm 11 has 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 below the threshold ε σn often reaches a state below

[0213] For example, the robot arm 11 shown in FIG. 4 is in a singular state where three singular values are below the threshold value ε σn The robot arms 11 shown in FIGS. 5 to 7 are in singular states where one singular value is below the threshold value ε σ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, thereby reaching a singular posture where three singular values are below the threshold value ε σn as shown in FIG. 4.

[0214] Also, in a singular state where two singular values σ n and σ n-1 are each below the threshold value ε σn the information processing apparatus 100 may perform processing independent of the magnitude relationship between the singular values σ n and σ n-1 The information processing apparatus 100 multiplies all elements of the matrix Σ N in the inverse matrix J N -1 of the normalized Jacobian matrix J -1 shown in Equation 6 by σ n / ε σn times to calculate a new matrix J εN shown in Equation 42. Further, the information processing apparatus 100 replaces the elements including the singular value σ n-1 with the same value 1 / ε n as the elements including the singular value σ σn to calculate a new matrix J εND shown in the following Equation 47. Thereby, it is possible to prevent divergence of each of the elements including the singular values σ n and σ n-1 The information processing apparatus 100 can perform decomposition speed control using the normalized matrix J εND of Equation 47.

[0215] [Modification Example of Robot System]

[0216] ​A modification example of the robot system 1 according to the embodiment will be described. The robot system 1A according to the modification example is different from the embodiment in that the user interface 30 and the robot 10 are connected via the communication network N in a remote operation robot system. Hereinafter, this modification example will be described centering on the differences from the embodiment, and the description of the same points as the embodiment will be omitted as appropriate.

[0217] FIG. 14 is a diagram showing an example of the configuration of the robot system 1A according to the modification example. As shown 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 a mediation device 300 that matches the robot area AR and the user area AU and connects the control device 20 and the user interface 30 included therein to be capable of data communication via the communication network N. At least a part of the functions of the information processing device 100 is realized by one or both of the user interface 30 and the mediation device 300. The control device 20 includes at least a power control circuit 200. Although not limited, in this modification example, a plurality of robot areas AR and a plurality of user areas AU exist as targets of the robot system 1A.

[0218] The robot system 1A further includes an imaging device 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 arranged on the robot 10, for example, on the robot arm 11, and images the end effector 12 and the object W to be processed thereof. The imaging device 14b is arranged at a position other than the robot 10 so as to image the entire robot 10. The robot communication device 15 includes a modem, an ONU (Optical Network Unit), a router, or mobile data communication equipment. The robot communication device 15 connects the control device 20 to the communication network N. The control device 20 can transmit and receive information, commands, and data to and from the user interface 30 and the mediation device 300 via the communication network N. The control device 20 can transmit the 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 perceptibly presents information to the operator P. The presentation device 31 includes at least a display, and may further include a speaker or the like. The user communication device 32 includes a modem, an ONU, a router, or mobile data communication equipment. The user communication device 32 connects the user interface 30 to the communication network N. The user interface 30 can transmit and receive information, commands, and data to and from the control device 20 and the mediation device 300 via the communication network N. The presentation device 31 presents the image data of the imaging devices 14a and 14b received by the user interface 30 as an image. The user interface 30 may include the presentation device 31, the user communication device 32, or both of them.

[0220] The mediation device 300 manages communication via the communication network N. The mediation device 300 may have a server configuration including a computer device. The mediation 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 mediation device 300 connects the robot communication device 15 of the robot 10 specified by the user interface 30 connected to the authenticated user communication device 32 and the user communication device 32. That is, the mediation device 300 matches and connects the user communication device 32 and the robot communication device 15. The mediation 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 mediation device 300.

[0221] The communication network N is not particularly limited, and can 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 can 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 communications 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 and a fifth-generation mobile communication system, etc. The communication network N can include one or more networks. In this modification example, the communication network N is the Internet.

[0222] In this modified example, the mediation device 300 includes the functions of the information processing device 100 according to the embodiment. The mediation device 300 receives information regarding the specifications of the robot 10 connected to the control device 20 from the control device 20 or the user interface 30 connected to the control device 20. The mediation device 300 receives the feedback information of the robot 10 from the control device 20, and generates a Jacobian matrix using the feedback information and the information regarding the specifications of the robot 10. The mediation device 300 performs the processing regarding singularities 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 functions of the information processing device 100, the mediation device 300 can function as the information processing device 100 to cause the robot 10 to perform an operation corresponding to a singularity. If the mediation device 300 can obtain the information regarding the specifications of the robot 10, it can function as the information processing device 100. The mediation device 300 eliminates the need for the operator P in the user area AU to operate the robot 10 so as to avoid singularities based on the image of the limited area presented by the presentation device 31. The operator P can operate the robot 10 without considering singularities. The mediation device 300 facilitates various operators P to operate various robots 10.

[0224] Note that the user interface 30 may include some or all of the functions of the information processing apparatus 100 according to the embodiment. The user interface 30 may incorporate the functions of the information processing apparatus 100 as application software. For example, when the user interface 30 includes all of the functions of the information processing apparatus 100 according to the embodiment, the mediation apparatus 300 may perform matching between the user interface 30 and the control apparatus 20 and mediate the transmission and reception of information, commands, and data between the user interface 30 and the control apparatus 20. For example, the user interface 30 may include some of the functions of the information processing apparatus 100 according to the embodiment, and the mediation apparatus 300 may include the other part or all of the functions of the information processing apparatus 100. The functions of the information processing apparatus 100 included in the user interface 30 and the functions of the information processing apparatus 100 included in the mediation apparatus 300 may or may not overlap. For example, the mediation apparatus 300 may execute a process that the user interface 30 cannot execute as the information processing apparatus 100 and provide the processing result to the user interface 30.

[0225] The control apparatus 20 may include some of the functions of the information processing apparatus 100. In this case, the mediation apparatus 300 may include the other part or all of the functions of the information processing apparatus 100, and the user interface 30 may include the other part or all of the functions of the information processing apparatus 100. Each of the control apparatus 20, the user interface 30, and the mediation apparatus 300 may include some of the functions of the information processing apparatus 100. In this case, the mediation apparatus 300 may include all of the functions of the information processing apparatus 100. The functions of the information processing apparatus 100 included in the control apparatus 20, the functions of the information processing apparatus 100 included in the user interface 30, and the functions of the information processing apparatus 100 included in the mediation apparatus 300 may or may not overlap.

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

[0227] For example, in the embodiments and modifications, the information processing apparatus 100 performs processing related to singular points in the case of manual operation of the robot 10, but is not limited thereto. The information processing apparatus 100 may perform processing related to singular points in the case of automatic operation of the robot 10, that is, when the robot 10 operates autonomously according to preset data such as teaching data. Thereby, even if the teaching data does not include data on the operation of the robot 10 that avoids singular points, the information processing apparatus 100 can perform processing related to singular points when operating the robot 10 according to the teaching data, and cause the robot 10 to operate corresponding to singular points.

[0228] Examples of each aspect 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, wherein a first Jacobian matrix associating the operation speeds of the plurality of joints and the moving speed of the moving part is subjected to singular value decomposition to generate a second Jacobian matrix including a first matrix having, as elements, first basis vectors representing the operation speed space of the joints, a second matrix having, as elements, second basis vectors representing the moving speed space of the moving part, and a first singular matrix having singular values as diagonal elements; determining and executing a process to be executed from among a first process executed when the robot is at a singularity and when the robot is approaching the singularity, and a second process executed when the robot is away from the singularity, based on a comparison result between the singular value and a first threshold value; in the first process, converting diagonal elements in the first singular matrix to generate a third Jacobian matrix; in the first process, using the third Jacobian matrix and a movement command value for commanding movement of the moving part to determine and output an operation command value for commanding the operation of the plurality of joints; and in the second process, using the first Jacobian matrix or the second Jacobian matrix and the movement command value to determine and output the operation command value, to be executed by a computer.

[0229] According to the above aspect, in the first process, by converting the diagonal elements in the first singular matrix, a third Jacobian matrix is generated from the second Jacobian matrix. In this case, the diagonal elements may be converted so as to correspond to a state where the robot is at a singularity and a state where the robot is approaching the singularity. For example, in a state where the robot is at a singularity and a state where the robot is approaching the singularity, the diagonal elements may be converted so that the elements of the inverse matrix of the second Jacobian matrix do not diverge. By the above conversion, it is possible to prevent the speed of the joints from changing rapidly in a state where the robot is at a singularity and a state where the robot is approaching the singularity. Thus, it is possible to control the operation of the robot corresponding to the singularity by a novel method of converting the diagonal elements in the first singular matrix.

[0230] Here, in the vicinity of a singularity, the joint speed can become excessive with normal decomposition rate control. As methods for solving such problems, there are singularity-insensitive decomposition and the singularity adaptation method. Singularity-insensitive decomposition can suppress the joint speed from becoming excessive, but instead, there is a problem that the end effector deviates from the originally commanded trajectory. The singularity adaptation method suppresses the joint speed from becoming excessive and also prevents the end effector from deviating from the originally commanded trajectory. However, the singularity adaptation method requires analytical calculation of the cofactor matrix of the Jacobian matrix and is very laborious to apply to a specific arm.

[0231] While the conventional singularity adaptation method required calculation of the cofactor matrix of the Jacobian matrix, the inventor of the present application found that by performing singular value decomposition on the Jacobian matrix and scaling the singular values so that the ratio of the reciprocals of the singular values is preserved. As a result, the inventor of the present application found that a matrix having the same effect as the cofactor matrix of the Jacobian matrix in the conventional singularity adaptation method can be obtained by numerical calculation instead of analytical calculation.

[0232] In the first aspect described above, the program according to the 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 the Jacobian matrix used to determine the motion command value, and when the first norm is greater than the movement command value, correct the motion command value using the first norm to determine a corrected motion command value, output the corrected motion command value as the motion command value, and when the first norm is less than or equal to the movement command value, output the motion command value.

[0233] According to the above aspect, it is possible to set an upper limit on the speed at which the robot moves the moving part and determine the motion command values of a plurality of joints so that the moving part moves at a speed equal to or lower than the upper limit. Therefore, an excessive increase in the moving speed of the moving part can be prevented.

[0234] In the above-described first aspect or second aspect, the program according to the third aspect of the present disclosure may cause a computer to receive first information that 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, first information can be acquired, and the first Jacobian matrix can be generated using the acquired first information. Therefore, it is possible to set the first Jacobian matrix according to the robot.

[0236] In any one of the above-described first aspect to third aspect, the program according to the 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 determine to execute the second process when the minimum value of the singular values is greater than or equal to the first threshold.

[0237] According to the above aspect, as the robot approaches a singular point, the minimum singular value becomes smaller. The condition of whether the minimum singular value is smaller than the first threshold can be a condition for determining whether the robot is in a state of approaching a singular point, that is, a condition for determining whether to execute the first process. Therefore, it is possible to surely determine whether to execute the first process.

[0238] In any of the first to fourth aspects described above, the program according to the fifth aspect of the present disclosure is to generate a second inverse Jacobian matrix that is the 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 each transformed from the first matrix, the second matrix, and the first singular matrix, and in the first process, performing a first transformation for transforming diagonal elements in the first transformation singular matrix to generate a third inverse Jacobian matrix that is the inverse matrix of the third Jacobian matrix, and in the first process, applying the movement command value to the third inverse Jacobian matrix to determine the operation command value, and in the second process, applying the movement command value to the first inverse Jacobian matrix that is the inverse matrix of the first Jacobian matrix or the second inverse Jacobian matrix to determine the operation command value, which may be executed by a computer.

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

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

[0241] According to the above aspect, in the first transformation, the ratio between the diagonal elements including the singular values in the first transformation singular matrix is maintained. Therefore, the first transformation maintains the relationship between the movement command value and the operation command value that the inverse Jacobian matrix originally has. 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, operation command values for moving the moving part in the same direction can be obtained. Regardless of whether the robot is near a singular point, the operation of the moving part in the commanded direction can be maintained.

[0242] In the sixth aspect described above, the program according to the seventh aspect of the present disclosure may cause a computer to multiply, in the first conversion, a first ratio of the minimum value of the singular values in the first conversion specific matrix and the first threshold value by each of the diagonal elements.

[0243] According to the above aspect, in the first conversion specific matrix, even if the minimum singular value approaches 0, the diagonal elements including the minimum singular value do not diverge. Therefore, in the vicinity of the singular point, a rapid change in the speed of the joint corresponding to the diagonal element can be prevented.

[0244] In any one of the fifth to seventh aspects described above, the program according to the eighth aspect of the present disclosure calculates, in the first process, a second norm, which is a norm of a vector obtained by applying a direction vector representing the direction of the commanded speed of the moving unit included in the movement command value to the third inverse Jacobian matrix; in the first process, when the second norm is greater than or equal to a second threshold value, applies 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 process, when the second norm is less than the second threshold value, applies the movement command value to an inverse Jacobian matrix obtained by changing the second norm to the second threshold value in the third inverse Jacobian matrix normalized using the second norm to determine the operation command value, may cause a computer to execute.

[0245] According to the above aspect, the second norm indicates the magnitude of the moving speed of the moving unit corresponding to the direction component of the commanded 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 value, by changing the second norm to the second threshold value in the normalized third inverse Jacobian matrix, divergence of the normalized third inverse Jacobian matrix can be prevented. Therefore, in the vicinity of the singular point, a rapid change in the speed of the moving unit can be prevented.

[0246] In any one of the fifth to eighth aspects described above, the program according to the ninth aspect of the present disclosure, in the first process, determines a contribution degree which is a ratio of the command speed of the moving part included in the movement command value contributing in the direction of the second base vector, and in the first process, when the contribution degree is equal to or greater than a third threshold value, determines the operation command value using the third inverse Jacobian matrix, and in the first process, when the contribution degree is less than the third threshold value, converts the third inverse Jacobian matrix into a fourth inverse Jacobian matrix such that a value of a diagonal element corresponding to a minimum value of the singular values in the first transformed first transformation singular matrix becomes zero, and in the first process, when the contribution degree is less than the third threshold value, applies the movement command value to the fourth inverse Jacobian matrix to determine the operation command value, may be executed by a computer.

[0247] According to the above aspect, when the command speed of the moving part includes a component that does not contribute in the direction of the second base vector representing the movement speed space of the moving part, the third inverse Jacobian matrix is converted into the fourth inverse Jacobian matrix such that the value of the diagonal element corresponding to the minimum singular value in the first transformed first transformation singular matrix becomes zero. By using the fourth inverse Jacobian matrix, the error of the speed of the moving part realized by the operation command value with respect to the movement command value can be reduced.

[0248] In any one of the fifth to ninth aspects described above, in the first process of the program according to the tenth aspect of the present disclosure, among the degrees of contribution in which the commanded speed of the moving part included in the movement command value contributes in the direction of the second base vector, a first degree of contribution corresponding to a first singular value that is the smallest among the singular values in the first transformation singular matrix is determined; in the first process, when both 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 transformation singular matrix are smaller than the first threshold value and the first degree of contribution is equal to or greater than a third threshold value, each diagonal element in the first transformation singular matrix is multiplied by a first ratio between the first singular value and the first threshold value to generate the third inverse Jacobian matrix, and the movement command value is applied to the third inverse Jacobian matrix to determine the operation command value; and in the first process, when both the first singular value and the second singular value are smaller than the first threshold value and the first degree of contribution of the first singular value is less than the third threshold value, the value of the diagonal element corresponding to the first singular value in the first transformation singular matrix is set to 0, each diagonal element in the first transformation singular matrix is multiplied by a second ratio between the second singular value and the first threshold value to generate a fifth inverse Jacobian matrix, and the movement command value is applied to the fifth inverse Jacobian matrix to determine the operation command value, which may be executed by a computer.

[0249] According to the above aspect, when the first singular value and the second singular value are smaller than the first threshold value, an inverse Jacobian matrix corresponding to the relationship between the first degree of contribution corresponding to the first singular value and the third threshold value can be determined, and the operation command value can be determined using the determined inverse Jacobian matrix. Therefore, it is possible to control the operation of the robot corresponding to a state where the robot is at or approaching a singular point such that the two singular values are smaller than the first threshold value.

[0250] In the above-described tenth aspect, in the first process, when both the first singular value and the second singular value are smaller than the first threshold value, and the first contribution degree and the second contribution degree corresponding to the second singular value are less than the third threshold value, 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, each diagonal element in the first transformed singular matrix is multiplied by the second ratio to generate a sixth inverse Jacobian matrix, and the movement command value is applied to the sixth inverse Jacobian matrix to determine the operation command value. This may be executed by a computer.

[0251] According to the above aspect, when the commanded speed 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, an operation command value with the components corresponding to the first singular value and the second singular value removed is generated. Therefore, the error in the speed of the moving unit realized by the operation command value with respect to the movement command value can be reduced.

[0252] In the fifth to eleventh aspects described above, in the twelfth aspect of the present disclosure, in the first process, among the singular values in the first transformation singular matrix, a first singular value that is the smallest and two or more singular values that are the next smallest after the first singular value in the singular values in the first transformation singular matrix are included. When any of the first singular value to the k-th singular value (k is a natural number of 3 or more) is smaller than the first threshold value, the command speed of the moving unit included in the movement command value is the first contribution degree that is the ratio of contribution to the directions corresponding to the first singular value to the k-th singular value in the second basis vector. Determining the k-th contribution degree from the first contribution degree; in the first process, when the first contribution degree to the k-th contribution degree is equal to or greater than a third threshold value, in the first transformation, each diagonal element in the first transformation singular matrix is multiplied by a first ratio of the first singular value to 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 operation command value; in the first process, when at least one of the first contribution degree to the k-th contribution degree is less than the third threshold value, in the first transformation, a zero transformation is performed to set the value of the diagonal element corresponding to the singular value whose contribution degree in the first transformation singular matrix is less than the third threshold value to 0, and a third ratio of the smallest singular value excluding the singular value whose contribution degree in the first transformation singular matrix after the zero transformation is less than the third threshold value to the first threshold value is multiplied by the diagonal elements in the first transformation singular matrix after the zero transformation to convert it into a seventh inverse Jacobian matrix; in the first process, when at least one of the first contribution degree to the k-th contribution degree is less than the third threshold value, applying the movement command value to the seventh inverse Jacobian matrix to determine the operation command value may be executed by a computer.

[0253] According to the above aspect, even when three or more singular values are smaller than the first threshold value, in the same manner as when two singular values are smaller than the first threshold value, an inverse Jacobian matrix corresponding to the contribution degree of each singular value can be determined, and the operation command value can be determined using the determined inverse Jacobian matrix. Therefore, it is possible to control the operation of the robot corresponding to the state where the robot is at or approaching a singular point such that three singular values are smaller than the first threshold value.

[0254] In any of the first to twelfth aspects described above, the program according to the thirteenth aspect of the present disclosure generates an eighth Jacobian matrix by time-differentiating the first Jacobian matrix, applies the velocity within the second basis vector corresponding to the self-motion of the robot among the operating velocities of the plurality of joints during the first process to the eighth Jacobian matrix to generate an acceleration vector, determines whether the norm of the acceleration vector indicates a maximum value when the norm of the acceleration vector is greater than or equal to a fourth threshold in the first process, and outputs a command to stop the operation of the plurality of joints when the norm of the acceleration vector indicates a maximum value, and may cause a computer to execute.

[0255] According to the above aspect, when the norm of the acceleration vector is less than the fourth threshold, the robot generates a small acceleration that can continue self-motion. When the norm of the acceleration vector is greater than or equal to the fourth threshold, the robot generates an acceleration that can escape from a state near a singular point due to self-motion. When the norm of the acceleration vector is greater than or equal to the fourth threshold and the norm of the acceleration vector indicates a maximum value, the robot may reach a singular point and generate a vibration phenomenon in which deviation from the singular point and arrival at the singular point due to self-motion are repeated. In such a case, the vibration phenomenon can be prevented by once stopping the operation of the plurality of joints and then operating.

[0256] The storage medium according to the 14th aspect of the present disclosure stores the program according to any one of the 1st to 13th aspects described above. According to the above aspect, the same effects as the programs according to the respective aspects of the present disclosure can be obtained. The storage medium may be a non-transitory and tangible computer-readable medium. The storage medium may be a non-transitory and tangible computer-readable recording medium. Examples of the storage medium can include one or more semiconductor-based or other integrated circuits (ICs), ROM, EEPROM (Electrically Erasable and Programmable Read Only Memory), EPROM, flash memory, CD-ROM (Compact Disc Read Only Memory), CD-RW (Compact Disc Rewritable), DVD (Digital Versatile Disk), hard disk drive (HDD), hybrid hard drive (HHD), optical disk, optical disk drive (ODD), magneto-optical disk, magneto-optical drive, floppy disk, floppy disk drive (FDD), magnetic tape, solid state drive (SSD), RAM drive, secure digital card, secure digital drive, any other suitable storage medium, or a combination of two or more of these. Examples of the integrated circuit can include a field programmable gate array (FPGA), an application specific integrated circuit (ASIC), etc. The storage medium may be volatile, non-volatile, or a combination of volatile and non-volatile.

[0257] The information processing apparatus according to the 15th aspect of the present disclosure includes a memory that stores the program according to any one of the 1st to 13th aspects described above, and a processor that executes the program stored in the memory. According to the above aspect, the same effects as the programs according to the respective aspects of the present disclosure can be obtained.

[0258] The system according to the 16th aspect of the present disclosure includes an information processing apparatus including a computer that executes any one of the programs according to the 1st aspect to the 13th aspect described above, and the robot. The information processing apparatus includes a user interface to which a robot operation is input and is connected to the robot. The information processing apparatus 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. The information processing apparatus is input with 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 the programs according to the respective aspects of the present disclosure can be obtained.

[0259] A method for controlling the operation of a robot according to the 17th aspect of the present disclosure is a method 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 method includes performing singular value decomposition on a first Jacobian matrix that associates the operating speeds of the plurality of joints with the moving speed of the moving part, to generate a first matrix including, as elements, first basis vectors representing the operating speed space of the joints, a second matrix including, as elements, second basis vectors representing the moving speed space of the moving part, and a first singular matrix including singular values as diagonal elements, to generate a second Jacobian matrix; determining and executing a process to be executed from among a first process executed when the robot is at or approaching a singularity point and a second process executed when the robot is away from the singularity point, based on a comparison result between the singular values and a first threshold value; in the first process, converting diagonal elements in the first singular matrix to generate a third Jacobian matrix; in the first process, determining and outputting an operation command value for commanding the operation of the plurality of joints, using the third Jacobian matrix and a movement command value for commanding the movement of the moving part; and in the second process, determining and outputting the operation command value, using the first Jacobian matrix or the second Jacobian matrix and the movement command value. According to the above aspect, the same effects as the programs according to the respective aspects of the present disclosure can be obtained.

[0260] The program of the present disclosure may be, for example, a program recorded on a non-transitory and tangible computer-readable recording medium, and may be configured to be read from the recording medium using a drive device of the recording medium and installed in 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 in a computer.

[0261] Part or all of the method of the present disclosure may be implemented, for example, by a circuit such as a CPU or LSI, an IC card, or a single module. A plurality of elements included in the method of the present disclosure may be implemented by one device, or may be implemented in a shared manner by two or more devices.

[0262] The functions of the elements disclosed in this specification can be executed using a circuit or a processing circuit including a general-purpose processor, a dedicated processor, an integrated circuit, an ASIC, a conventional circuit, and / or a combination thereof configured or programmed to execute the disclosed functions. Since a processor includes transistors and other circuits, it is regarded as a processing circuit or a circuit. In the present disclosure, a circuit, a unit, or a means is hardware that executes the listed functions, or is hardware programmed to execute the listed functions. The hardware may be the hardware disclosed in this specification, or may be other known hardware programmed or configured to execute the listed functions. When the hardware is a processor considered to be a type of circuit, the circuit, the means, or the unit is a combination of hardware and software, and the software is used for the configuration of the hardware and / or the processor.

[0263] The ordinal numbers, numbers such as quantities, etc. used in this specification are all for illustrative purposes to specifically describe the technology of the present disclosure, and the present disclosure is not limited to the illustrated numbers. The connection relationships between the components are for illustrative purposes to specifically describe the technology of the present disclosure, and the connection relationships for realizing the functions of the present disclosure are not limited thereto.

[0264] The present disclosure can be implemented in various forms without departing from the scope of its essential features. Since the scope of the present disclosure is defined by the appended claims rather than the description in the specification, exemplary embodiments and modifications are illustrative and not restrictive. All changes within the claims and their scope, or equivalents of the claims and their scope, are intended to be encompassed by the claims.

Claims

1. 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, performing singular value decomposition on a first Jacobian matrix that associates the operating speeds of the plurality of joints with the moving speed of the moving part, to generate a second Jacobian matrix including a first matrix having as elements first basis vectors representing the operating speed space of the joints, a second matrix having as elements second basis vectors representing the moving speed space of the moving part, and a first singular matrix having singular values as diagonal elements; determining and executing a process to be executed from among a first process executed when the robot is at or approaching a singularity point based on a comparison result between the singular value and a first threshold value, and a second process executed when the robot is away from the singularity point; in the first process, converting diagonal elements in the first singular matrix to generate a third Jacobian matrix; in the first process, using the third Jacobian matrix and a movement command value for commanding movement of the moving part to determine and output an operation command value for commanding the operation of the plurality of joints; causing a computer to execute, in the second process, determining and outputting the operation command value using the first Jacobian matrix or the second Jacobian matrix and the movement command value. A program.

2. Calculating a first norm which is the norm of a vector obtained by applying the operation command value to the first Jacobian matrix or the second Jacobian matrix; when the first norm is greater than the movement command value, correcting the operation command value using the first norm to determine a corrected operation command value, and outputting the corrected operation command value as the operation command value; when the first norm is less than or equal to the movement command value, causing a computer to execute outputting the operation command value. The program according to claim 1.

3. Causing 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. The program according to claim 1.

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

5. Generating a second inverse Jacobian matrix that is the inverse matrix of the second Jacobian matrix, wherein the second inverse Jacobian matrix includes a first transformation matrix, a second transformation matrix, and a first transformation singular matrix that are respectively transformed from the first matrix, the second matrix, and the first singular matrix; In the first process, generating a third inverse Jacobian matrix that is the inverse matrix of the third Jacobian matrix by performing a first transformation for transforming diagonal elements in the first transformation singular matrix; In the first process, applying the movement command value to the third inverse Jacobian matrix to determine the operation command value; In the second process, causing a computer to apply the movement command value to a first inverse Jacobian matrix that is the inverse matrix of the first Jacobian matrix or the second inverse Jacobian matrix to determine the operation command value; The program according to claim 1.

6. In the first transformation, causing a computer to transform each of the diagonal elements so as to maintain a ratio between diagonal elements including the singular value in the first transformation singular matrix; The program according to claim 5.

7. In the first transformation, causing a computer to multiply each of the diagonal elements by a first ratio between a minimum value of the singular values in the first transformation singular matrix and a first threshold value; The program according to claim 6.

8. In the first process, calculating a second norm that is a norm of a vector obtained by applying a direction vector representing a direction of a commanded speed of the moving unit included in the movement command value to the third inverse Jacobian matrix; In the first process, when the second norm is greater than or equal to a second threshold value, normalizing the direction vector included in the movement command value using the second norm, and applying the movement command value including the normalized direction vector to the third inverse Jacobian matrix to determine the operation command value; In the first process, when the second norm is less than the second threshold value, dividing the direction vector included in the movement command value by the second threshold value, and applying the movement command value including the divided direction vector to the third inverse Jacobian matrix to determine the operation command value, and causing a computer to execute the above; The program according to claim 5.

9. In the first process, determining a contribution degree that is a ratio of the commanded speed of the moving unit included in the movement command value contributing to the direction of the second basis vector; In the first process, when the contribution degree is equal to or greater than a third threshold value, determining the operation command value by using the third inverse Jacobian matrix; In the first process, when the contribution degree is less than the third threshold value, converting the third inverse Jacobian matrix into a fourth inverse Jacobian matrix such that the value of the diagonal element corresponding to the minimum value of the singular values in the first transformed first transformation singular matrix is set to 0; In the first process, when the contribution degree is less than the third threshold value, causing a computer to apply the movement command value to the fourth inverse Jacobian matrix to determine the operation command value; The program according to claim 5.

10. In the first process, determining a first contribution degree corresponding to a first singular value that is the smallest among the singular values in the first transformation singular matrix, where the contribution degree is a ratio of the command speed of the moving part included in the movement command value that contributes in the direction of the second basis vector; In the first process, when both the first singular value and a second singular value that is the next smallest to the first singular value among the singular values in the first transformation singular matrix are smaller than the first threshold value and the first contribution degree is equal to or greater than the third threshold value, multiplying each of the diagonal elements in the first transformation 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 operation command value; In the first process, when both the first singular value and the second singular value are smaller than the first threshold value and the first contribution degree of the first singular value is less than the third threshold value, setting the value of the diagonal element corresponding to the first singular value in the first transformation singular matrix to 0, multiplying each of the diagonal elements in the first transformation singular matrix by a second ratio between the second singular value and the first threshold value to generate a fifth inverse Jacobian matrix, and applying the movement command value to the fifth inverse Jacobian matrix to determine the operation command value, and causing a computer to execute this; The program according to claim 5.

11. In the first process, when both the first singular value and the second singular value are smaller than the first threshold value, and the first contribution degree and the second contribution degree corresponding to the second singular value are less than the third threshold value, set the values of the diagonal elements corresponding to the first singular value and the second singular value in the first transformation singular matrix to 0, multiply each of the diagonal elements in the first transformation singular matrix by the second ratio to generate a sixth inverse Jacobian matrix, and cause a computer to apply the movement command value to the sixth inverse Jacobian matrix to determine the operation command value. The program according to claim 10.

12. In the first process, when any of the first singular value, which is the smallest among the singular values in the first transformation singular matrix, and two or more of the singular values in the first transformation singular matrix that are next smaller than the first singular value, i.e., the first singular value to the k-th singular value (k is a natural number of 3 or more), are all smaller than the first threshold value, determine the first contribution degree to the k-th contribution degree, which is the ratio of the command speed of the moving part included in the movement command value that contributes to the direction corresponding to each of the first singular value to the k-th singular value in the second basis vector. In the first process, when the first contribution degree to the k-th contribution degree is equal to or greater than the third threshold value, in the first transformation, multiply each of the diagonal elements in the first transformation singular matrix by the first ratio of the first singular value to the first threshold value to generate a third inverse Jacobian matrix, and apply the movement command value to the third inverse Jacobian matrix to determine the operation command value. In the first process, when at least one of the first contribution degree to the k-th contribution degree is less than the third threshold value, in the first transformation, perform a zero transformation of setting the value of the diagonal element corresponding to the singular value corresponding to the contribution degree less than the third threshold value among the first contribution degree to the k-th contribution degree to 0, and multiply the smallest singular value among those excluding the singular value corresponding to the contribution degree less than the third threshold value among the first contribution degree to the k-th contribution degree and the first threshold value by the third ratio, and multiply it by the diagonal element in the first transformation singular matrix after the zero transformation to convert it into a seventh inverse Jacobian matrix. In the first process, when at least one of the first contribution degree to the k-th contribution degree is less than the third threshold value, causing a computer to apply the movement command value to the seventh inverse Jacobian matrix to determine the operation command value The program according to claim 5.

13. Differentiating the first Jacobian matrix with respect to time to generate an eighth Jacobian matrix, and Among the operating speeds of the plurality of joints during the first process, applying the speed within the second base vector corresponding to the self-motion of the robot to the eighth Jacobian matrix to generate an acceleration vector, and In the first process, when the norm of the acceleration vector is equal to or greater than a fourth threshold value, determining whether the norm of the acceleration vector indicates a maximum value, and When the norm of the acceleration vector indicates a maximum value, causing a computer to output a command to stop the operation of the plurality of joints The program according to claim 1.

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

15. An information processing apparatus including a memory storing the program according to any one of claims 1 to 13, and a processor executing the program stored in the memory.

16. An information processing apparatus including a computer executing the program according to any one of claims 1 to 13, and including the robot, wherein the information processing apparatus is connected to a user interface for inputting a robot operation and the robot, the information processing apparatus receives a command to move the moving unit from the user interface, 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, the information processing apparatus is input with first information which is information on specifications of the robot, and generates the first Jacobian matrix using the first information System.

17. A method for controlling the operation of a robot including a plurality of joints and a moving unit moved by the operation of the plurality of joints, Performing singular value decomposition on a first Jacobian matrix that associates the operating speeds of the plurality of joints with the moving speed of the moving part to obtain a first matrix including as elements first basis vectors representing the operating speed space of the joints, a second matrix including as elements second basis vectors representing the moving speed space of the moving part, and a first singular matrix including singular values as diagonal elements, and generating a second Jacobian matrix; Based on the comparison result between the singular values and a first threshold value, determining and executing a process to be executed from among a first process executed when the robot is at or approaching a singularity point and a second process executed when the robot is away from the singularity point; In the first process, generating a third Jacobian matrix by transforming diagonal elements in the first singular matrix; In the first process, using the third Jacobian matrix and a movement command value for commanding movement of the moving part to determine and output an operation command value for commanding operations of the plurality of joints; In the second process, determining and outputting the operation command value using the first Jacobian matrix or the second Jacobian matrix and the movement command value, the method comprising the above steps.

Images