Control system, control method, and control program

Abstract

To provide a control system enabling stable grip and arrangement of a grip target object by a mobile robot, a control method, and a control program.SOLUTION: A control system includes: a parameter optimization processing part optimizing a wheel rotation speed and an arm joint angular speed of a mobile robot 1 which are already calculated; and a time trajectory optimization processing part optimizing the trajectory of an end effector by the time trajectory optimization. The parameter optimization processing part minimizes the arm joint angular speed by adjusting the weight of the wheel rotation speed and the weight of the arm joint angular speed applied to the arm joint angular speed so that the weight of the wheel rotation speed applied to the wheel rotation speed becomes larger as the end effector approaches a target position of the end effector. The control system can adopt a machine-learned model.SELECTED DRAWING: Figure 1

Description

Technical Field

[0001] The present invention relates to a control system, a control method, and a control program for controlling a mobile robot.

Background Art

[0002] Conventionally, various mobile robots have been proposed. Regarding the technology related to such mobile robots, the omnidirectional mobile body trajectory generation device disclosed in Patent Document 1 calculates the maximum acceleration of joint parameters based on the Jacobian formula derived based on the current joint state of the moving body, and based on the acceleration limit vector including the maximum acceleration of the joint parameters, it includes a constraint calculation means for performing forward integration and backward integration on the plane of the trajectory parameters s and s(dot) to perform an optimization process and calculate a time trajectory.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] However, the trajectory generation device disclosed in Patent Document 1 aims to calculate a time trajectory at a higher speed, and does not aim to stably grasp and place a gripping object by a mobile robot.

[0005] The present disclosure is for solving such problems, and an object thereof is to provide a control system, a control method, and a control program that enable stable grasping and placement of a gripping object by a mobile robot.

Means for Solving the Problems

[0006] In a control system for controlling the operation of a mobile robot according to an embodiment, the mobile robot includes wheels for moving the mobile robot, an arm, and an end effector connected to the arm. The control system includes a parameter optimization processing unit that optimizes the rotation speed of the wheels and the joint angular velocity of the arm that have already been calculated, and a time trajectory optimization processing unit that optimizes the trajectory of the end effector by time trajectory optimization. As the end effector approaches the target position of the end effector, the parameter optimization processing unit adjusts the weight of the rotation speed of the wheels applied to the rotation speed of the wheels and the weight of the joint angular velocity of the arm applied to the joint angular velocity of the arm so that the weight of the rotation speed of the wheels increases, thereby minimizing the joint angular velocity of the arm.

[0007] The parameter optimization processing unit maximizes the manipulability of the arm using the Jacobian acting on the joint angular velocity of the arm. The Jacobian includes a value w representing the manipulability of the arm, a Jacobian J representing the relationship between the velocity of the end effector in the joint angular velocity of the arm arm and the transposed matrix H T of the transposed matrix (J arm H T ), T and the transposed matrix of the transposed matrix of the Jacobian J arm and the Jacobian J arm and the inverse matrix of the transposed matrix J arm T of J.

[0008] The target rotational speed of the end effector can be calculated by control that fills the deviation between the roll angle, pitch angle, and yaw angle that are the target orientation of the end effector and the roll angle, pitch angle, and yaw angle that are the current orientation of the end effector.

[0009] In a control method for controlling the operation of a mobile robot according to an embodiment, the mobile robot includes wheels for moving the mobile robot, an arm, and an end effector connected to the arm. The computer executes parameter optimization processing for optimizing the rotation speed of the wheel and the joint angular velocity of the arm that have already been calculated, executes time trajectory optimization processing for optimizing the trajectory of the end effector by time trajectory optimization, The parameter optimization processing includes processing for minimizing the joint angular velocity of the arm by adjusting the weight of the rotation speed of the wheel applied to the rotation speed of the wheel and the weight of the joint angular velocity of the arm applied to the joint angular velocity of the arm so that the weight of the rotation speed of the wheel increases as the end effector approaches the target position of the end effector.

[0010] In a control program for controlling the operation of a mobile robot according to an embodiment, the mobile robot includes a wheel for moving the mobile robot, an arm, and an end effector connected to the arm, the control program causes a computer to execute parameter optimization processing for optimizing the rotation speed of the wheel and the joint angular velocity of the arm that have already been calculated, execute time trajectory optimization processing for optimizing the trajectory of the end effector by time trajectory optimization, The parameter optimization processing includes processing for minimizing the joint angular velocity of the arm by adjusting the weight of the rotation speed of the wheel applied to the rotation speed of the wheel and the weight of the joint angular velocity of the arm applied to the joint angular velocity of the arm so that the weight of the rotation speed of the wheel increases as the end effector approaches the target position of the end effector.

Advantages of the Invention

[0011] According to the present invention, it is possible to provide a control system, a control method, and a control program that enable stable gripping and placement of an object to be gripped by a mobile robot.

Brief Description of the Drawings

[0012]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Embodiments for Carrying Out the Invention

[0013] FIG. 1 is a diagram showing the configuration of a mobile robot 1 according to an embodiment. The mobile robot 1 includes an arithmetic unit 10, a communication interface (I / F) 20, a storage device 21, imaging devices 22a and 22b, and an operating mechanism 23.

[0014] The arithmetic unit 10 is a device that performs overall control of the mobile robot 1. Specific examples of the arithmetic unit 10 include a CPU (Central Processing Unit), an MPU (Micro Processing Unit), an ECU (Electronic Control Unit), etc. The arithmetic unit 10 corresponds to a computer.

[0015] The arithmetic unit 10 realizes a control method by executing a control program for controlling the mobile robot 1. Note that a semiconductor device such as an FPGA (Field-Programmable Gate Array) or an ASIC (Application Specific Integrated Circuit) may execute the control program. These semiconductor devices also correspond to a computer.

[0016] The communication interface (I / F) 20 is a device that communicates data with an external device. The storage device 21 is a storage device that stores various information processed by the arithmetic unit 10.

[0017] The imaging devices 22a and 22b are devices that image the surroundings of the mobile robot 1. The imaging device 22a is arranged on the head of the mobile robot 1. The imaging device 22b is arranged on the end effector.

[0018] The motion mechanism 23 is a mechanism that performs the motion of the mobile robot 1. Specifically, the motion mechanism 23 includes a plurality of wheels attached to a carriage of the mobile robot 1, an arm connected to the main body of the mobile robot 1, and an end effector connected to the arm.

[0019] The control program executed by the arithmetic unit 10 includes an imaging image generation unit 11, an object detection unit 12, an attitude calculation unit 13, a whole body motion plan generation unit 14, a hand trajectory plan generation unit 15, and a mechanism control unit 16.

[0020] The imaging image generation unit 11 is a program that generates an imaging image using the imaging devices 22a and 22b that image the surroundings of the mobile robot 1. The imaging device 22a is arranged on the head of the mobile robot 1. The imaging device 22b is arranged on the end effector. Therefore, the imaging image generation unit 11 can generate an imaging image based on the head of the mobile robot 1. Also, the imaging image generation unit 11 can generate an imaging image based on the end effector of the mobile robot 1.

[0021] The object detection unit 12 is a program that detects a gripping object and a placement table on which the gripping object is placed using the imaging image generated by the imaging image generation unit 11. For the detection of the gripping object and the placement table, various image detection methods such as pattern matching can be adopted.

[0022] The posture calculation unit 13 is a program that calculates the posture of the end effector provided in the mobile robot 1. The posture of the end effector calculated by the posture calculation unit 13 includes the three-dimensional position coordinates and orientation (roll angle, pitch angle, and yaw angle) of the end effector.

[0023] The whole-body motion plan generation unit 14 is a program that generates a whole-body motion plan for the mobile robot 1. The whole-body motion plan targets the wheels and arms provided in the mobile robot 1 and operates them simultaneously. The whole-body motion plan generation unit 14 can generate a whole-body motion plan for the mobile robot 1 based on the quadratic programming method. In the whole-body motion plan, the three-dimensional position information of the carriage, the rotational speed of the wheels, the joint angles of the arms, and the joint angular velocities of the arms are calculated to realize the target hand position and target hand speed, which are the target position and target speed of the end effector. In generating the whole-body motion plan, the whole-body motion plan generation unit 14 optimizes the rotational speed of the wheels and the joint angular velocities of the arms calculated in the whole-body motion plan. The whole-body motion plan generation unit 14 corresponds to a parameter optimization processing unit.

[0024] The hand trajectory plan generation unit 15 is a program that generates a hand trajectory plan for the mobile robot 1. The hand trajectory plan targets the end effector provided in the mobile robot 1. In the hand trajectory plan, the trajectory of the end effector and the target hand position speed, which is the target position speed of the end effector, are calculated. In generating the hand trajectory plan, the hand trajectory plan generation unit 15 optimizes the trajectory of the end effector calculated in the hand trajectory plan by time trajectory optimization. The hand trajectory plan generation unit 15 corresponds to a time trajectory optimization processing unit.

[0025] The mechanism control unit 16 is a program that controls the wheels, arms, and end effector, which are the operating mechanisms provided in the mobile robot 1. The mechanism control unit 16 controls these operating mechanisms based on the whole-body motion plan generated by the whole-body motion plan generation unit 14 and the hand trajectory plan generated by the hand trajectory plan generation unit 15.

[0026] FIG. 2 is a flowchart showing an example of the process executed by the arithmetic unit 10 of the mobile robot 1. In step S1, the imaging image generation unit 11 generates an imaging image around the mobile robot 1 using the imaging device 22a installed on the head of the mobile robot.

[0027] In step S2, the object detection unit 12 detects the object to be grasped using the imaging image generated in step S1.

[0028] In step S3, the posture calculation unit 13 calculates a provisional target grasping posture and a target intermediate posture of the end effector based on the imaging image generated in step S1 and the position information of the object to be grasped in the imaging image detected in step S2.

[0029] As shown in FIG. 3, the provisional target grasping posture is a provisional posture for the end effector to grasp the object to be grasped. In the present embodiment, as the three-dimensional position coordinates included in the provisional target grasping posture, for example, the position coordinates of the center of the object to be grasped in the imaging image detected in step S2 can be adopted. The orientation (roll angle, pitch angle, and yaw angle) of the provisional target grasping posture can adopt a predetermined orientation. Note that the provisional target grasping posture may be calculated using a learned model learned by machine learning such as deep learning. In this case, the learned model can be learned using training data in which the imaging image and the position information of the object to be grasped in the imaging image are input information and the provisional target grasping posture is output information.

[0030] The target intermediate posture is in front of the position indicated by the provisional target grasping posture, that is, the posture of the end effector on the side of the mobile robot 1. In the present embodiment, as the three-dimensional position coordinates included in the target intermediate posture, for example, the position coordinates at a predetermined distance from the three-dimensional position coordinates included in the provisional target grasping posture can be adopted. As the orientation (roll angle, pitch angle, and yaw angle) of the target intermediate posture, a predetermined orientation can be adopted. Note that the target intermediate posture may be calculated using a learned model learned by machine learning such as deep learning. In this case, the learned model can be learned using training data in which a captured image and position information of the object to be grasped in the captured image are input information and the target intermediate posture is output information. Note that the learned model may be further learned using the provisional target grasping posture as input information.

[0031] In step S4, the whole-body motion plan generation unit 14 and the end-effector trajectory plan generation unit 15 generate a whole-body motion plan and an end-effector trajectory plan, respectively, based on the target intermediate posture calculated in step S2.

[0032] In step S5, the mechanism control unit 16 controls the wheels, arms, and end effectors, which are the motion mechanisms to be controlled, based on the whole-body motion plan and the end-effector trajectory plan generated in step S4. As a result, the posture of the end effector of the mobile robot 1 will coincide with the target intermediate posture as shown in FIG. 3.

[0033] In step S6, the captured image generation unit 11 generates a captured image around the mobile robot 1 using the imaging device 22a and the imaging device 22b installed on the end effector.

[0034] In step S7, the object detection unit 12 detects the object to be grasped and the placement table on which the object to be grasped is placed using the captured image generated in step S6. At this time, the object detection unit 12 detects the object to be grasped using the captured image generated using the imaging device 22b. Further, the object detection unit 12 detects the placement table using the captured image generated using the imaging device 22a.

[0035] In step S8, the posture calculation unit 13 calculates the final target grasping posture based on the captured image of the object to be grasped generated using the imaging device 22b in step S7 and the position information of the object to be grasped detected in step S7. As shown in FIG. 3, the final target grasping posture is the final posture for the end effector to grasp the object to be grasped. In the present embodiment, as the three-dimensional position coordinates included in the final target grasping posture, for example, the position coordinates of the center of the object to be grasped in the captured image detected in step S7 can be adopted. As for the orientation (roll angle, pitch angle, and yaw angle) of the final target grasping posture, a predetermined orientation can be adopted. Note that the final target grasping posture may be calculated using a learned model learned by machine learning such as deep learning. In this case, the learned model can be learned using training data in which the captured image and the position information of the object to be grasped in the captured image are input information and the final target grasping posture is output information.

[0036] In step S9, the posture calculation unit 13 calculates the target placement posture based on the captured image of the object to be grasped generated using the imaging device 22a in step S7 and the position information of the placement table detected in step S7. As shown in FIG. 3, the target placement posture is the posture for the end effector to place the object to be grasped on the placement table. In the present embodiment, as the three-dimensional position coordinates included in the target placement posture, for example, the position coordinates of a predetermined position of the placement table in the captured image detected in step S7 can be adopted. As for the orientation (roll angle, pitch angle, and yaw angle) of the target placement posture, a predetermined orientation can be adopted. Note that the target placement posture may be calculated using a learned model learned by machine learning such as deep learning. In this case, the learned model can be learned using training data in which the captured image and the position information of the placement table in the captured image are input information and the target placement posture is output information.

[0037] In step S10, the whole-body motion plan generation unit 14 and the end-effector trajectory plan generation unit 15 generate a whole-body motion plan and an end-effector trajectory plan, respectively, based on the final target grasping posture calculated in step S8.

[0038] In step S11, the whole-body motion plan generation unit 14 and the end-effector trajectory plan generation unit 15 generate a whole-body motion plan and an end-effector trajectory plan respectively based on the target placement posture calculated in step S9.

[0039] In step S12, the mechanism control unit 16 controls the wheels, arms, and end-effectors, which are the motion mechanisms to be controlled, based on the whole-body motion plan and end-effector trajectory plan generated in step S10 and the whole-body motion plan and end-effector trajectory plan generated in step S11. As a result, the posture of the end-effector is to hold the object to be grasped and place it on the placement table as shown in FIG. 3.

[0040] The whole-body motion plan generated by the whole-body motion plan generation unit 14 includes joint angle limit, joint angular velocity limit, and collision avoidance as the basic functions of the whole-body motion plan. Also, as a function of the whole-body motion plan, maximization of operability, although not essential, may also be included. When formulating each of the functions of joint angle limit, joint angular velocity limit, and collision avoidance as a quadratic programming problem, they can be expressed as shown in Formulas 1 to 5.

Equation

Equation

Equation

Equation

Equation

[0041] The x shown in Formulas 1 to 5 is a variable to be optimized, which is the rotational speed q of the wheels of the carriage · base and the joint angular velocity q of the arm · arm and is a vector including the slack variable s. The degree of freedom of q · base is l, and the degree of freedom of q· arm Define the degree of freedom as h. The slack variable is a variable that relaxes the equality constraint, and its degree of freedom is 6.

[0042] Next, Equation 2 will be described. f0 is a function that minimizes the joint angular velocity and maximizes the manipulability. The first term of the objective function in Equation 2 is the term for minimizing the joint angular velocity. x T is the transposed matrix composed of x described above. Q is a matrix that adjusts the weight for minimizing the value of x and is defined by Equation 6.

Number

Number

Number

[0043] Therefore, as the end effector approaches the target position of the end effector, the whole-body motion plan generation unit 14 adjusts the weight of the rotational speed of the wheel and the weight of the joint angular velocity of the arm applied to the rotational speed of the wheel so that the weight of the rotational speed of the wheel increases, thereby minimizing the joint angular velocity of the arm.

[0044] The second term of Equation 2 is the term for maximizing the manipulability. c T is the transposed matrix of c defined in Equation 9. c is composed of a term related to the movable range of the end effector, which is the hand, and a term related to maximizing the manipulability.

Number

Number

[0045] Therefore, the whole - body motion planning generation unit 14 maximizes the manipulability of the arm by using the Jacobian acting on the joint angular velocity of the arm. The Jacobian is the transpose matrix of the product of the Jacobian J arm representing the relationship between the value w representing the manipulability of the arm and the end - effector velocity (the velocity of the end - effector) in the joint angular velocity of the arm, the transpose matrix H T of the Hessian H, i.e., (J arm H T ), and the inverse matrix of the transpose matrix of the product of the Jacobian J T and the transpose matrix of the Jacobian J arm and the transpose matrix of the Jacobian J arm , i.e., J arm T .

[0046] Next, Equation 3 will be explained. Equation 3 is an equality constraint for end - effector velocity control. v is the target end - effector velocity, which is defined by Equation 12. [Number] Here, v p is the target end - effector position velocity, which is calculated by the time - trajectory optimization described later. The target end - effector position velocity corresponds to the target position velocity of the end - effector. v ris the target end - effector rotational speed, which is calculated by P - control that fills the deviation between the target end - effector orientation (roll angle, pitch angle, and yaw angle) and the current orientation (roll angle, pitch angle, and yaw angle). The target end - effector rotational speed corresponds to the target rotational speed of the end - effector.

[0047] J all is a matrix formed by combining the Jacobian between the target end - effector velocity and the joint angular velocities of the whole body seen from the mobile robot coordinate system and the identity matrix, and is defined by Equation 13. As shown in Figure 3, the mobile robot coordinate system is defined with the forward direction of the mobile robot as the x - axis, the lateral movement direction as the y - axis, and the direction perpendicular to the ground as the z - axis.

Number

Number

[0048] Next, Equation 4 will be explained. Equation 4 is an inequality constraint for performing joint angle limitation and collision avoidance. First, the joint angle limitation will be explained. A rad in Equation 4 is a matrix for applying the joint angle limitation only to the arm for which the joint angle limitation is desired, and is defined by Equation 15. A rad is composed of a combination of the identity matrix and the zero matrix. By this matrix A rad the rotational speed of the wheel and the slack variable are invalidated.

Number

[0049] b in Equation 4 rad is a matrix that functions as a velocity damper to slow down the joint angular velocity as the angle approaches the joint angle limit, and is defined by Equation 16.

Number

[0050] Next, collision avoidance will be described. Similar to the joint angle limit described above, collision avoidance is performed using the concept of a velocity damper. In this embodiment, when the distance between the link of the mobile robot and the obstacle becomes close, the joint angular velocity is restricted so that the distance is not made closer. A in Equation 4 ob is a matrix related to the constraint for collision avoidance with the obstacle, and is defined by Equation 17.

Number

[0051] B in Equation 4 ob is a matrix that functions as a velocity damper, and is defined by Equation 18.

Number

[0052] Next, Equation 5 will be explained. Equation 5 is an inequality constraint for joint angular velocity limitation and continuous movement. x in Equation 5 min and x max are defined by Equation 19.

Number

Number

[0053] Therefore, the maximum value of the rotational speed of the wheels of the carriage, as shown in Equation 20, is equal to the product of the target translational speed during the continuous movement of the carriage plus a predetermined speed ε x and the inverse matrix of the Jacobian J base (J base -1 ). Also, as shown in Equation 20, the minimum value of the rotational speed of the wheels of the carriage is equal to the product of the target translational speed during the continuous movement of the carriage minus a predetermined speed ε x and the inverse matrix of the Jacobian J base (J base -1 ).

[0054] Next, the end-effector trajectory planning will be described. In the present invention, an extended method of time trajectory optimization disclosed in the non-patent document "T. Kunz and M. Stilman: 'Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity'. Robotics: Science and Systems Conference-VII, pp. 209-216, (2013)." is used. First, the method of Kunz et al. will be described.

[0055] As shown in FIG. 4, when the i-th waypoint of the trajectory is p i the time trajectory optimization process is as follows. 1. Convert the trajectory into an expression of the trajectory length l (0.0 ≦ l ≦ L). (a) A straight-line segment from p0 to p1 (shown as A in the left figure of FIG. 4) (b) A straight-line segment from p1 to p2 (shown as B in the left figure of FIG. 4) (c) An arc segment continuous with the straight-line segments A and B (shown as C in the left figure of FIG. 4) (d)... 2. Starting from (l, l · ) = (0.0, 0.0), perform forward integration. The maximum speed and acceleration of l are derived from the maximum speed and acceleration of the end effector. 3. Starting from (l, l · ) = (L, 0.0), perform backward integration until it collides with the forward integration. 4. Derive the end effector's time trajectory from the time trajectory of l.

[0056] In the method of Kunz et al., the initial velocity direction is determined by p0 and p1, and an arbitrary initial velocity cannot be represented. Therefore, in order to set an arbitrary initial velocity, the following extension was made as disclosed in the non-patent document "Yasusuke Takeshita, Takashi Yamamoto: 'Proposal of a Mobile Manipulation System Using Periodic Whole-Body Trajectory Planning'. Robotics Symposium, (2023).". 1. Between p0 and p1, from p0 to p ·An appropriate trajectory point p in the direction of 0 v is placed (right figure in Fig. 4) 2. l during forward integration · Let 0 be the norm of p · 0 3. p v is obtained by search on the condition of successful forward integration (a) Search in ascending order from small values of |p v - p0| (b) The smaller |p v - p0| is, the shorter the trajectory length (operation time) becomes, so the smaller the better (c) If |p v - p0| is too small, the forward integration of p0, p v , p1 fails (d) Since it fails at the initial stage of forward integration, the impact on the total calculation time by search is small (e) After forward integration, backward integration is performed in the same way

[0057] Correction by position feedback To achieve the end - effector target velocity, the velocities of the wheels and the joint angular velocities of the arm are calculated by quadratic programming, but there is no guarantee that there is always a solution that can achieve the end - effector target velocity. When there is no solution that can achieve the end - effector target velocity, the value of the slack variable increases to relax the equality constraint, and the deviation from the target end - effector velocity occurs. In addition, the actual machine does not always follow the target joint velocity. Therefore, it is necessary to always feedback the deviation between the calculated target end - effector position and the current end - effector position, and a control law to reduce this deviation is required. Thus, the target end - effector velocity v p is calculated as follows

Equation

[0058] Experiment Experimental method To verify the effect of the method according to the present disclosure, a Pick&Place (grasp and placement) task was performed using a mobile manipulator. A force sensor manufactured by Leptrino was attached to the fingertip of the mobile robot, and when the force applied to the fingertip exceeded the threshold during Place, the gripper was released. 3D distance image sensors were incorporated into the head and wrist of the mobile robot. Initial recognition was performed with the camera on the head, and re-recognition was performed with the camera on the wrist. A notebook PC was mounted on the mobile robot for recognition, and YOLOv8-seg was used for object recognition. A separate PC was prepared to control the motion planning and the mobile robot, and its CPU was an Intel Core i9-12900E. The fingertip trajectory planning could be executed in about 3 ms. The whole-body motion planning could be executed in about 10 ms.

[0059] To evaluate the method according to the present disclosure, the Pick&Place task was performed 10 times each using the method according to the present disclosure and a conventional method. To evaluate the effectiveness of smooth fingertip trajectory generation, the object to be grasped was a cup in which a plurality of cubes were stacked, and it was measured whether the cubes fell from the cup due to the operation immediately after grasping. The mobile robot, the object to be grasped, and the table were arranged in a straight line. The cup was placed in the same position in all experiments.

[0060] Experimental results The experimental results are shown in Fig. 5. When a series of operations until the mobile robot grasps the cup and places it on the table is completed, it is considered a success, and the operation success rate indicates the ratio of success to the number of experiments. At this time, even if the cubes in the cup fell, it was regarded as a success, and the Pick&Place ability of the cup itself was evaluated. Looking at the results of the operation success rate, the method according to the present disclosure was 20% higher than the conventional method. Also, the failure cases of both methods were due to grasping failures caused by deviations in the estimated grasping position. These results suggest that the method according to the present disclosure reduces the vibration of the fingertip of the mobile robot and improves the estimation accuracy of the grasping position.

[0061] If the cube did not fall from the cup due to the movement of the fingertips immediately after grasping, it is considered a success, and the stability rate indicates the ratio of successful experiments in the above-mentioned definition. Looking at the results of the stability rate, the method according to the present disclosure was 80% higher. From this, it is considered that stable transportation was possible due to the smooth fingertip trajectory.

[0062] The average operation completion time is the average value of the time from the start of the operation to the placement of the object to be grasped when the operation is successful. When comparing the two methods, there is no significant difference. Therefore, it can be said that the method according to the present disclosure can perform smooth operations while maintaining speed.

[0063] Fig. 6 shows the speed command values of the target fingertip positions at each time by the method according to the present disclosure and the conventional method. Section C in Fig. 6 corresponds to immediately after grasping. Comparing the speed command values of each method, it can be seen that the method according to the present disclosure has less rapid speed changes overall. Also, looking at section C immediately after grasping, the speed changes rapidly in the conventional method, but changes smoothly in the method according to the present disclosure. From this, it can be seen that the smooth movement of the end effector immediately after grasping was realized by the above-mentioned time trajectory optimization.

[0064] In the above example, when the program is loaded into a computer, it includes a set of instructions (or software code) for causing the computer to perform one or more functions described in the embodiments. The program may be stored in a non-transitory computer-readable medium or a tangible storage medium. By way of example and not limitation, the computer-readable medium or tangible storage medium includes random-access memory (RAM), read-only memory (ROM), flash memory, solid-state drive (SSD), or other memory technologies, CD-ROM, digital versatile disk (DVD), Blu-ray (registered trademark) disk, or other optical disk storage, magnetic cassette, magnetic tape, magnetic disk storage, or other magnetic storage devices. The program may be transmitted on a transitory computer-readable medium or a communication medium. By way of example and not limitation, the transitory computer-readable medium or communication medium includes electrical, optical, acoustic, or other forms of propagated signals.

[0065] The present invention is not limited to the above-described embodiments, and can be appropriately modified without departing from the spirit of the present invention.

Explanation of Reference Numerals

[0066] 1: Mobile robot, 10: Arithmetic unit, 11: Captured image generation unit, 12: Object detection unit, 13: Posture calculation unit, 14: Whole-body motion plan generation unit, 15: End-effector trajectory plan generation unit, 16: Mechanism control unit, 20: Communication interface, 21: Storage device, 22a, 22b: Imaging device, 23: Motion mechanism

Claims

1. A control system for controlling the operation of a mobile robot, wherein the mobile robot includes wheels for moving the mobile robot, an arm, and an end effector connected to the arm, The control system A parameter optimization processing unit that optimizes the rotation speed of the wheel and the joint angular velocity of the arm that have already been calculated, And a time trajectory optimization processing unit that optimizes the trajectory of the end effector by time trajectory optimization, The parameter optimization processing unit adjusts the weight of the rotation speed of the wheel and the weight of the joint angular velocity of the arm applied to the joint angular velocity of the arm so that the weight of the rotation speed of the wheel increases as the end effector approaches the target position of the end effector, thereby minimizing the joint angular velocity of the arm. Control system.

2. The parameter optimization processing unit maximizes the manipulability of the arm using the Jacobian acting on the joint angular velocity of the arm, The Jacobian includes a value w representing the manipulability of the arm and a Jacobian J representing the relationship between the speed of the end effector and the joint angular velocity of the arm ａｒｍ And the transposed matrix H of the Hessian H Ｔ And the transposed matrix (J ａｒｍ H Ｔ ) Ｔ And the Jacobian J ａｒｍ And the transposed matrix of the transposed matrix of the Jacobian J ａｒｍ And the inverse matrix of J ａｒｍ Ｔ The control system according to claim 1, comprising:

3. The target rotation speed of the end effector is calculated by control to fill the deviation between the roll angle, pitch angle and yaw angle which are the target orientation of the end effector and the roll angle, pitch angle and yaw angle which are the current orientation of the end effector. The control system according to claim 1 or 2.

4. A control method for controlling the operation of a mobile robot, wherein the mobile robot includes wheels for moving the mobile robot, an arm, and an end effector connected to the arm, a computer executes a parameter optimization process for optimizing the rotation speed of the wheels and the joint angular velocity of the arm that have already been calculated, executes a time trajectory optimization process for optimizing the trajectory of the end effector by time trajectory optimization, wherein the parameter optimization process includes a process of minimizing the joint angular velocity of the arm by adjusting the weight of the rotation speed of the wheels applied to the rotation speed of the wheels and the weight of the joint angular velocity of the arm applied to the joint angular velocity of the arm such that the weight of the rotation speed of the wheels increases as the end effector approaches the target position of the end effector.

5. A control program for controlling the operation of a mobile robot, wherein the mobile robot includes wheels for moving the mobile robot, an arm, and an end effector connected to the arm, causes a computer to execute a parameter optimization process for optimizing the rotation speed of the wheels and the joint angular velocity of the arm that have already been calculated, execute a time trajectory optimization process for optimizing the trajectory of the end effector by time trajectory optimization, wherein the parameter optimization process includes a process of minimizing the joint angular velocity of the arm by adjusting the weight of the rotation speed of the wheels applied to the rotation speed of the wheels and the weight of the joint angular velocity of the arm applied to the joint angular velocity of the arm such that the weight of the rotation speed of the wheels increases as the end effector approaches the target position of the end effector.

Images