WO2024085072A1 - 制御装置、制御方法、及び制御プログラム - Google Patents

制御装置、制御方法、及び制御プログラム Download PDF

Info

Publication number
WO2024085072A1
WO2024085072A1 PCT/JP2023/037119 JP2023037119W WO2024085072A1 WO 2024085072 A1 WO2024085072 A1 WO 2024085072A1 JP 2023037119 W JP2023037119 W JP 2023037119W WO 2024085072 A1 WO2024085072 A1 WO 2024085072A1
Authority
WO
WIPO (PCT)
Prior art keywords
contact
state
time
contact force
force
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2023/037119
Other languages
English (en)
French (fr)
Inventor
一敏 田中
竜典 谷合
想太郎 片山
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Omron Corp
Original Assignee
Omron Corp
Omron Tateisi Electronics Co
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Omron Corp, Omron Tateisi Electronics Co filed Critical Omron Corp
Priority to US19/115,604 priority Critical patent/US20260131459A1/en
Publication of WO2024085072A1 publication Critical patent/WO2024085072A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Program-controlled manipulators
    • B25J9/16—Program controls
    • B25J9/1628—Program controls characterised by the control loop
    • B25J9/1633—Program controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39001—Robot, manipulator control

Definitions

  • This disclosure relates to a control device, a control method, and a control program.
  • the contact state between the robot and the object (hereinafter simply referred to as the contact mode) changes successively.
  • the physical constraints on the robot's movements also change successively. For this reason, when a robot comes into frequent contact with objects, it is difficult to appropriately control the robot's movements.
  • the present disclosure has been made in consideration of the above points, and aims to control a control object so that a complementarity condition is satisfied regarding the contact force generated at the contact points of the control object and the distance between the contact points on the control object.
  • control device including: an acquisition unit that acquires the state of the controlled object at time t; a calculation unit that calculates, based on the state at time t, a state at time t+1 that satisfies a complementarity condition regarding the contact force generated at the contact point of the controlled object and the distance between the contact points on the controlled object and minimizes an objective function that represents the difference between the state of the controlled object at time t+1 and a target value; and a control unit that controls the controlled object so that it reaches the calculated state at time t+1.
  • the calculation unit can calculate the state at time t+1 based on the state at time t such that a complementarity condition regarding the contact force generated at the contact points of the controlled object and the distance between the contact points on the controlled object, and a complementarity condition regarding the tangential contact velocity on the contact surface with which the contact points are in contact, are satisfied, and an objective function representing the difference between the state of the controlled object at time t+1 and a target value is minimized.
  • the controlled object refers to a machine controlled by a control device and a target object acted upon by the machine, and the machine may include a robot.
  • the state may include the position and orientation of the machine and the target object.
  • Time t+1 refers to a calculated unit time ahead of time t, and may include the cycle time of the control device, the cycle time of a computer calculation, and the cycle time of a computer simulation.
  • the target value refers to a preset state that the machine and/or target object will reach, and may include coordinates and orientation. The minimum is a value calculated from an objective function, but may include an error depending on the calculation method.
  • control objects are a robot and a target object
  • the state data is calculated based on sensor data obtained by a sensor that senses the state of the robot and the object
  • the calculation unit can calculate the state at time t+1 based on the state at time t so that the complementarity condition regarding the contact force generated between any of the contact points of the robot, the contact point of the target object, and the contact point of the other object, and the distance between any of the contact points of the robot, the contact point of the target object, and the contact point of the other object is satisfied.
  • the complementarity conditions are as follows: for each of the plurality of contact points, the following complementarity condition ( 4a ) is related to a contact force ⁇ ni generated in the vertical direction of the contact surface with which the i-th contact point is in contact in the state q t at time t and the distance ⁇ i (q t ) between the i-th contact point and the contact surface; the following complementarity condition (4b) is related to a contact force ⁇ fi+ in the first horizontal direction on the contact surface with which the i-th contact point is in contact in the state q t at time t and at a generalized velocity v t at time t, a signed tangential contact velocity ⁇ i (q t , v t ) with the first horizontal direction of the contact surface being positive , and an absolute value ⁇ i of the tangential contact velocity ; ) and the absolute value ⁇ i of the tangential contact velocity at the contact surface with which the i-th contact point is in
  • the calculation unit can calculate, based on the state qt at time t, the state qt+1 at time t+1 that satisfies the complementarity conditions (4a) to (4d) and minimizes the following objective function (5):
  • g(q t+1 ) represents the physical quantity of the controlled object corresponding to the state q t+1
  • g ref represents the target value of the physical quantity of the controlled object corresponding to the target value of the state q t+1
  • Q g , Q v , and Q f are pre-given positive definite weighting matrices
  • ⁇ t+1 is a vector at time t+1 whose components are the contact force ⁇ ni generated in the vertical direction of the contact surface for each of the multiple contact points, the contact force ⁇ fi+ in the first horizontal direction, and the contact force ⁇ fi- in the second horizontal direction.
  • the complementarity condition is a linear complementarity condition ( 7a) for each of the plurality of contact points, which is related to a contact force ⁇ ni generated in the vertical direction of the contact surface with which the i-th contact point is in contact in the state qt at time t , a change amount ⁇ ni of the contact force ⁇ ni , the distance ⁇ i ( qt ) between the i-th contact point and the contact surface, ⁇ i ,q ( qt ) obtained by differentiating the distance ⁇ i ( qt ) with respect to the state qt , and a change amount ⁇ qt of the state qt :
  • the following linear complementarity condition (7b) is satisfied for ⁇ i,v (q t , v t ) obtained by differentiating the tangential contact velocity ⁇ i (q t , v t ) with respect to t , the absolute value ⁇ i of the tangential
  • the calculation unit can calculate the state qt+1 at time t +1 by calculating an amount of change ⁇ qt in the state that satisfies the complementarity conditions (7a) to (7d) and minimizes the following objective function ( 6 ) based on the state qt at time t.
  • gt represents the physical quantity of the controlled object corresponding to the state qt
  • gref represents a target value of the physical quantity of the controlled object corresponding to the target value of the state qt
  • Qg , Qv , and Qf are pre-given positive definite weighting matrices
  • ⁇ t is a vector at time t having as its components a contact force ⁇ ni generated in the vertical direction of the contact surface for each of the multiple contact points, a contact force ⁇ fi + in the first horizontal direction, and a contact force ⁇ fi ⁇ in the second horizontal direction
  • ⁇ t is the amount of change in ⁇ t .
  • the complementarity condition may include the following relaxation variables s i for relaxing the complementarity condition:
  • the calculation unit can calculate the state qt+1 at time t +1 by calculating, based on the state qt at time t, a change amount ⁇ qt in the state, a change amount ⁇ t in the contact force, and a vector s t having a relaxation variable s i as each component, which satisfy the complementarity conditions (9a) to ( 9d) and minimize the following objective function (10):
  • gt represents the physical quantity of the controlled object corresponding to the state qt
  • gref represents a target value of the physical quantity of the controlled object corresponding to the target value of the state qt
  • Qg , Qv , and Qf are pre-given positive definite weighting matrices
  • ⁇ t is a vector at time t having as its components a contact force ⁇ ni generated in the vertical direction of the contact surface for each of the multiple contact points, a contact force ⁇ fi + in the first horizontal direction, and a contact force ⁇ fi ⁇ in the second horizontal direction
  • ⁇ t is the amount of change in ⁇ t .
  • the control method disclosed herein is a control method in which a computer executes a process to acquire a state of a control object at time t, calculate a state of the control object at time t based on the state at time t such that a complementarity condition regarding the contact force generated at the contact point of the control object and the distance between the contact points of the control object are satisfied and an objective function representing the difference between the state of the control object at time t+1 and a target value is minimized, and control the control object so that the calculated state at time t+1 is achieved.
  • the control program disclosed herein is a control program for causing a computer to execute a process of acquiring the state of a controlled object at time t, calculating the state of the controlled object at time t+1 based on the state at time t such that a complementarity condition regarding the contact force generated at the contact point of the controlled object and the distance between the contact points of the controlled object are satisfied and an objective function representing the difference between the state of the controlled object at time t+1 and a target value is minimized, and controlling the controlled object so that the calculated state at time t+1 is achieved.
  • the control device, method, and program disclosed herein can control a control object so that a complementarity condition is satisfied regarding the contact force generated at the contact points of the control object and the distance between the contact points on the control object.
  • FIG. 1 is a diagram for explaining an overview of the present embodiment.
  • FIG. 2 is a diagram for explaining the flow of processing executed by the control system according to the present embodiment.
  • FIG. 13 is a diagram for explaining a state qt used in this embodiment.
  • 1A and 1B are diagrams for explaining the relationship between the distance between a contact point and a contact surface and the contact force.
  • 1 is a diagram for explaining the Coulomb friction coefficient between a contact point and a contact surface;
  • FIG. 2 is a block diagram showing a functional configuration of the control system according to the present embodiment. 2 is a block diagram showing a hardware configuration of a control device according to the embodiment.
  • FIG. 4 is a flowchart showing the flow of a control process in the present embodiment.
  • FIG. 1 is a diagram for explaining the present embodiment.
  • FIG. 13 is a diagram showing the results of this embodiment.
  • FIG. 13 is a diagram for explaining the results of the present embodiment.
  • FIG. 13 is a diagram showing the results of this
  • Fig. 13 is a diagram for explaining an example of the conventional technology.
  • control 1 according to the contact mode between the object B and the gripping part G of the robot at time t1 control 2 according to the contact mode at time t2 , and control 3 according to the contact mode at time t3 are executed.
  • the number of contact modes is enormous, there is a problem that it is difficult to continuously control the robot according to the enormous number of contact modes.
  • the robot's movements are continuously controlled so that the complementarity conditions regarding contact between objects are satisfied. This allows the robot's movements to be controlled in real time.
  • FIG. 1 is a diagram for explaining an overview of this embodiment. As shown in FIG. 1, in this embodiment, complementarity conditions regarding contact between a target object B, a gripping part G of the robot, and another object, a wall W or a floor surface FL, are taken into consideration.
  • the target object B and the gripping part G of the robot are examples of control targets of the present disclosure.
  • the gripping portion G of the robot is not in contact with the target object B, so the distance between the contact point pG of the gripping portion G and the target object B is greater than zero, and the contact force generated at the contact point pG of the gripping portion G of the robot is zero. Note that at time t1 , the contact point pG is not actually in contact with the target object B, so it is also a potential contact point.
  • the gripper G of the robot is in contact with the target object B, so the distance between the contact point pG of the gripper G and the target object B is zero, and a contact force ⁇ 4 greater than zero is generated at the contact point pG of the gripper G of the robot.
  • the gripper G of the robot is not in contact with the target object B, so the distance between the contact point pG of the gripper G and the target object B is greater than zero, and the contact force generated at the contact point pG of the gripper G of the robot is zero.
  • the target object B has a plurality of contact points p1 , p2 , p3 , and p4 .
  • a contact force ⁇ 1 occurs between the contact point p1 and the floor surface FL
  • a contact force ⁇ 2 occurs between the contact point p2 and the floor surface FL.
  • a contact force ⁇ 1 occurs between the contact point p1 and the floor surface FL
  • a contact force ⁇ 3 occurs between the contact point p3 and the wall W.
  • a contact force ⁇ 1 occurs between the contact point p1 and the floor surface FL
  • a contact force ⁇ 3 occurs between the contact point p3 and the floor surface FL.
  • the robot's motion is continuously controlled so that the complementarity condition regarding contact between objects is satisfied.
  • the complementarity condition between two scalar variables a and b is expressed by the following equation (1).
  • LCQP Linear Complementarity Quadratic Program
  • FIG. 2 is a diagram for explaining the flow of processing executed by the control system according to the present embodiment.
  • the control device 16 of the present embodiment acquires the state q t of the control object at time t, and calculates the state q t+1 of the control object at time t+1 such that the difference from the target value is small.
  • the control device 16 calculates the state q t +1 of the control object at time t+1 such that the complementarity condition regarding contact between objects is satisfied and the objective function representing the difference between the target value and the state q t+1 of the control object at time t+1 is small.
  • the control device 16 outputs the state q t + 1 of the control object at time t+1 as a control input to the robot 14.
  • FIG. 3 is a diagram for explaining the state qt used in this embodiment.
  • FIG. 3 is also a diagram showing an example of a quasi-static formulation in which the entire system is composed of a gripper G and a target object B.
  • a state vector qa is set having, as its components, coordinates ( xa , ya ) in a gripper coordinate system having a predetermined position of the gripper G as the origin, a posture angle ⁇ a representing the posture of the gripper G, and a distance r a between the grippers provided in the gripper G.
  • coordinates ( xa , ya ) having a predetermined position of the gripper G as the origin
  • ⁇ a representing the posture of the gripper G
  • a distance r a between the grippers provided in the gripper G.
  • a state vector qu is set having, as its components, coordinates ( x0 , y0 ) in a target object coordinate system having a predetermined position of the target object B as the origin, and a posture angle ⁇ 0 representing the posture of the target object B.
  • DOF Degree of freedom
  • the state vector qa and the state vector qu in the generalized coordinates are defined by the following formula.
  • the bold characters in the formulas explained below represent vectors or matrices.
  • the state q of the controlled object and the velocity v of each object are expressed by the following equation. Note that velocity v is also a generalized velocity.
  • h is a preset coefficient greater than or equal to 0.
  • a contact force that is a force generated between a contact point and a contact surface will be described.
  • a corner of the target object B and a corner of the gripper of the gripping unit G are set as contact points pG1 and pG2 .
  • a contact force ⁇ t , i generated at each time t between the i-th contact point and the contact surface is defined as follows:
  • ⁇ fi is the horizontal contact force on the contact surface with which the i-th contact point is in contact.
  • ⁇ ni is the vertical contact force on the contact surface with which the i-th contact point is in contact.
  • the target object B is a box, but the complementarity condition can be formulated in a similar manner for other types of objects (e.g., spherical objects).
  • An index i for identifying a pair of contact points and contact surfaces is defined by the following equation.
  • the index i for identifying a pair of a contact point and a contact surface is determined by the rotation matrix R i (q t ) of the contact surface defined by the following equation and the relative position [r o,i, x ( qt ),r o,i,y ( qt )] is associated with T.
  • the first and second dimensional components of the vector represented by the above formula (3) represent the force acting on object o in the world coordinate system.
  • the third dimensional component of the above formula (3) represents the moment.
  • the force and moment written in parentheses in the above formula (3) represent the contact force acting on object o.
  • the force F o defined by the following formula represents a force that is constant over time, such as gravity.
  • a contact force vector ⁇ t,i occurring at time t is defined for each index i for identifying a pair of a contact point and a contact surface.
  • this contact force vector ⁇ t,i has as its components a contact force ⁇ ni occurring in the vertical direction of the contact surface in the i-th contact point and contact surface pair, and a contact force ⁇ fi occurring in the horizontal direction of the contact surface in the i-th contact point and contact surface pair.
  • the contact force ⁇ ni becomes positive. Therefore, in this embodiment, the distance ⁇ i (q t ) between the i-th contact point and the contact surface at time t is introduced.
  • the distance ⁇ i (q t ) is expressed by the following equation.
  • the contact force vector ⁇ t,i In order to prevent the target object B or the gripping part G from slipping at the contact point, the contact force vector ⁇ t,i must be located within a friction cone formed by friction between the contact point and the contact surface. Therefore, in this embodiment, a contact velocity ⁇ i (q t , v t ) between the i-th contact point and the contact surface at time t is introduced.
  • the contact velocity ⁇ i (q t , v t ) is expressed by the following equation.
  • this embodiment introduces two non-negative contact forces ⁇ fi+ and ⁇ fi ⁇ that occur in the horizontal direction of the contact surface.
  • the two contact forces ⁇ fi+ and ⁇ fi ⁇ are expressed by the following equations.
  • FIG. 5 is a diagram for explaining the Coulomb friction coefficient between the contact point and the contact surface.
  • the black circle shown in FIG. 5 represents the contact point, the solid arrow represents the contact force, and the dashed arrow represents the contact velocity.
  • the contact force ⁇ ni generated in the vertical direction and the contact forces ⁇ fi+ and ⁇ fi- generated in the horizontal direction must have the relationship shown in FIG. 5.
  • the solid arrow shown in FIG. 5 represents the contact force
  • the dashed arrow represents the absolute value ⁇ i of the contact velocity ⁇ i (q t , v t ) generated by the target object B slipping.
  • ⁇ i represents the friction coefficient between the contact point and the contact surface, and ⁇ i >0.
  • the absolute value ⁇ i is defined by the following equation.
  • the contact force ⁇ ni is a positive value
  • the contact force ⁇ fi+ generated in the first horizontal direction is a positive value
  • the contact force ⁇ fi- generated in the second horizontal direction opposite to the first horizontal direction is zero
  • the absolute value of the contact velocity ⁇ i is positive.
  • the value obtained by subtracting the contact force ⁇ fi+ generated in the first horizontal direction and the contact force ⁇ fi- generated in the second horizontal direction from the friction force ⁇ i ⁇ ni generated at the contact point is zero.
  • slippage occurs at the contact point of the target object B.
  • the contact force ⁇ ni has a positive value
  • the contact force ⁇ fi+ generated in the first horizontal direction has a positive value
  • the contact force ⁇ fi- generated in the second horizontal direction is zero
  • the absolute value of the contact velocity ⁇ i is zero.
  • the value obtained by subtracting the contact force ⁇ fi+ generated in the first horizontal direction and the contact force ⁇ fi- generated in the second horizontal direction from the friction force ⁇ i ⁇ ni generated at the contact point is a positive value.
  • the contact force vector ⁇ t,i is located on the left boundary of the friction cone C, so slippage occurs at the contact point of the target object B. For this reason, in the state shown in FIG 5C, the following equation holds at the contact point where the target object B comes into contact with the floor surface FL.
  • the contact force ⁇ ni is a positive value
  • the contact force ⁇ fi+ generated in the first horizontal direction is zero
  • the contact force ⁇ fi- generated in the second horizontal direction is a positive value
  • the absolute value of the contact velocity ⁇ i is a positive value.
  • the value obtained by subtracting the contact force ⁇ fi+ generated in the first horizontal direction and the contact force ⁇ fi- generated in the second horizontal direction from the friction force ⁇ i ⁇ ni generated at the contact point is zero.
  • slippage occurs at the contact point of the target object B.
  • the above formula (4a) is a complementary condition that indicates that the contact force ⁇ ni is positive when the distance ⁇ i (q t ) between the objects is zero, and that the contact force ⁇ ni is zero when the distance ⁇ i (q t ) between the objects is a positive value.
  • the above formulas (4b) to (4d) are complementary conditions that indicate that the contact force vector ⁇ t,i is located inside the friction cone C.
  • the above formula (4d) is a complementary condition that indicates that the contact force vector ⁇ t,i is located on the boundary of the friction cone C, as shown in Figs. 5A and 5C.
  • complementary conditions regarding contact between the above-mentioned objects include complementary conditions (4a), (4b), (4c), and (4d).
  • Complementary condition (4a) is an example of a complementary condition regarding the contact force generated at the contact points of the controlled object and the distance between the contact points of the controlled object.
  • Complementary conditions (4b) to (4d) are examples of complementary conditions regarding the tangential contact velocity on the contact surface where the contact points come into contact.
  • the above equation (4a) is a complementarity condition for each of the multiple contact points, regarding the contact force ⁇ ni generated in the vertical direction of the contact surface with which the i-th contact point is in contact, in state qt at time t, and the distance ⁇ i ( qt ) between the i-th contact point and the contact surface.
  • the above equation (4b) is a complementarity condition for each of the multiple contact points, with respect to the contact force ⁇ fi + in the first horizontal direction on the contact surface with which the i-th contact point is in contact, in the state qt at time t and the generalized velocity vt at time t , the signed tangential contact velocity ⁇ i ( qt , vt ) with the first horizontal direction of the contact surface being positive, and the absolute value ⁇ i of the tangential contact velocity.
  • Equation (4c) is a complementarity condition for the second horizontal contact force ⁇ fi ⁇ , the signed tangential contact velocity ⁇ i (q t , v t ), and the absolute value ⁇ i of the tangential contact velocity on the contact surface contacted by the i-th contact point, for each of the multiple contact points, in state q t at time t and generalized velocity v t at time t .
  • the above formula (4d) is a complementarity condition for each of the multiple contact points, regarding the absolute value ⁇ i of the tangential contact velocity on the contact surface with which the i-th contact point is in contact, the friction coefficient ⁇ i of the contact surface, the contact force ⁇ ni generated in the vertical direction of the contact surface, the first horizontal contact force ⁇ fi+ , and the second horizontal contact force ⁇ fi ⁇ .
  • the contact force vector ⁇ t is a vector having the following components ⁇ fi+ , ⁇ fi ⁇ , and ⁇ ni .
  • g in the above formula (5) is a general differentiable function, and is a function for mapping the state q onto the space of the position and orientation of the gripper G of the robot 14 and the position and orientation of the target object B. Therefore, g(q) is the physical quantity of the control object corresponding to the state q. g ref represents the target value vector of the physical quantity. Furthermore, Q g , Q v , and Q f are positive definite weight matrices that are given in advance. Q g represents the priority of the task, and Q v and Q f are normalization terms.
  • the constrained nonlinear objective function shown in the above formula (5) is optimized based on a concept similar to that of the known gradient descent method. Specifically, in this embodiment, a descent step is generated such that the objective function of the above formula (5) is reduced, thereby executing control to operate the robot 14. Note that each descent step is calculated by solving the linearized objective function of formula (5). As a result, a state q t+1 and a contact force vector ⁇ t+1 representing the positions and forces of the gripper G and the target object B at the next time t+1 are obtained.
  • the complementarity conditions regarding contact between the above-mentioned objects include linear complementarity condition (7a), complementarity condition (7b), linear complementarity condition (7c), and linear complementarity condition (7d).
  • the above formula (7a) is a linear complementarity condition for each of the multiple contact points, the contact force ⁇ ni generated in the vertical direction of the contact surface with which the i-th contact point is in contact in the state qt at time t , the change ⁇ ni in the contact force ⁇ ni , the distance ⁇ i ( qt ) between the i-th contact point and the contact surface, ⁇ i ,q ( qt ) obtained by differentiating the distance ⁇ i ( qt ) with respect to the state qt , and the change ⁇ qt in the state qt .
  • the above formula (7b) expresses, for each of the multiple contact points, the contact force ⁇ fi + in the first horizontal direction on the contact surface with which the i-th contact point is in contact, the change amount ⁇ fi + in the contact force ⁇ fi+ , the signed tangential contact velocity ⁇ i (q t , v t ) with the first horizontal direction of the contact surface being positive , and the tangential contact velocity ⁇ i (q
  • the linear complementarity condition is related to ⁇ i,v (q t , v t ) obtained by differentiating the state q t , the absolute value ⁇ i of the tangential contact velocity, and the change amount ⁇ q t of the state q t .
  • the above formula (7c) is a linear complementarity condition for each of the multiple contact points, with respect to the second horizontal contact force ⁇ fi- on the contact surface with which the i-th contact point is in contact, in the state qt at time t and the generalized velocity vt at time t , the change amount ⁇ fi- in the contact force ⁇ fi- , the signed tangential contact velocity ⁇ i (q t , v t ), the absolute value ⁇ i of the tangential contact velocity, and ⁇ i,v (q t , v t ) obtained by differentiating the tangential contact velocity ⁇ i (q t , v t ) by the generalized velocity vt.
  • the above formula (7d) is the following linear complementarity condition for the absolute value ⁇ i of the tangential contact velocity on the contact surface with which the i-th contact point is in contact, for each of the multiple contact points, the friction coefficient ⁇ i of the contact surface, the contact force ⁇ ni generated in the vertical direction of the contact surface, the amount of change ⁇ ni in the contact force ⁇ ni , the contact force ⁇ fi+ in the first horizontal direction, the amount of change ⁇ fi+ in the contact force ⁇ fi+ in the first horizontal direction, the contact force ⁇ fi ⁇ in the second horizontal direction, and the amount of change ⁇ fi ⁇ in the contact force ⁇ fi ⁇ in the second horizontal direction.
  • the complementarity condition regarding contact between objects is relaxed by introducing a relaxation variable s i into the above equations (7a) to (7d).
  • the complementarity condition with the relaxation variable s i introduced is expressed by the following equation.
  • p i s i 2 is added to the objective function of the above formula (6).
  • the objective function to which p i s i 2 is added is expressed by the following formula (10).
  • p i is a coefficient that is set in advance.
  • ⁇ qt , ⁇ t , and st at time t are calculated so as to minimize the above formula (10).
  • st is a vector having the relaxation variable s i as each component. Since qt+1 and ⁇ t +1 contain the position and force of the actuation system (in this embodiment, the gripper G of the robot 14) as elements, the operation of the gripper G of the robot 14 is controlled based on this.
  • the value calculated by LCQP is converted into a control signal q ⁇ t+1 related to the position of the gripper G by the following equation.
  • K>0 is a gain defined by the user.
  • the above transformation prevents the robot 14 from actually executing the force command signal calculated by LCQP accurately.
  • FIG. 6 is a block diagram showing the schematic configuration of the control system 10 of this embodiment controlled by LCQP.
  • the control system 10 includes a camera 12, a robot 14, and a control device 16.
  • the control device 16 continuously controls the operation of the robot 14 so that the complementarity conditions regarding contact between objects (e.g., between the gripper G of the robot 14, the target object B, and the wall W and floor surface FL) are satisfied.
  • the camera 12 sequentially captures the gripping part G of the robot 14, which is the object to be controlled, and the target object B.
  • the camera 12 then outputs the image data obtained by capturing the images of the gripping part G of the robot 14 and the target object B to the control device 16.
  • FIG. 7 is a block diagram showing the hardware configuration of the control device 16 according to this embodiment.
  • the control device 16 has a CPU (Central Processing Unit) 42, a memory 44, a storage device 46, an input/output I/F (Interface) 48, a storage medium reading device 50, and a communication I/F 52.
  • Each component is connected to each other so as to be able to communicate with each other via a bus 54.
  • the storage device 46 stores control programs for executing the various processes described below.
  • the CPU 42 is a central processing unit, and executes various programs and controls each component. That is, the CPU 42 reads the programs from the storage device 46 and executes the programs using the memory 44 as a working area.
  • the CPU 42 controls each of the components and performs various calculation processes according to the programs stored in the storage device 46.
  • Memory 44 is made up of RAM (Random Access Memory) and serves as a working area to temporarily store programs and data.
  • Storage device 46 is made up of ROM (Read Only Memory), HDD (Hard Disk Drive), SSD (Solid State Drive), etc., and stores various programs including the operating system, and various data.
  • the input/output I/F 48 is an interface that inputs data from the camera 12 and the robot 14, and outputs data to the camera 12 and the robot 14.
  • input devices for various inputs such as a keyboard or a mouse, and output devices for outputting various information, such as a display or a printer, may be connected.
  • a touch panel display may be used as the output device to function as an input device.
  • the storage medium reader 50 reads data stored in various storage media such as CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray Disc, and USB (Universal Serial Bus) memory, and writes data to the storage media.
  • CD Compact Disc
  • DVD Digital Versatile Disc
  • USB Universal Serial Bus
  • the communication I/F 52 is an interface for communicating with other devices, and uses standards such as Ethernet (registered trademark), FDDI, and Wi-Fi (registered trademark).
  • control device 16 functionally includes an acquisition unit 24, a calculation unit 26, and a control unit 28.
  • a data storage unit 18, a complementarity condition storage unit 20, and an objective function storage unit 22 are provided in a predetermined storage area of the control device 16.
  • Each functional configuration is realized by the CPU 42 reading out each program stored in the storage device 46, expanding it in the memory 44, and executing it.
  • the data storage unit 18 stores image data captured by the camera 12.
  • the complementarity condition storage unit 20 stores data on the complementarity conditions related to contact between the objects described above.
  • the objective function storage unit 22 stores data related to the objective functions described above.
  • the acquisition unit 24 sequentially acquires image data, which is an example of sensor data sensed from the gripping unit G of the robot 14, which is the control target, and the target object B. The acquisition unit 24 then stores the acquired image data in the data storage unit 18.
  • the acquisition unit 24 acquires the state qt of the robot 14 and the target object B at time t. Specifically, the acquisition unit 24 reads out the image data at time t stored in the data storage unit 18, and calculates data representing the state qt at time t. The acquisition unit 24 calculates the data representing the state qt from the image data using a known technique.
  • the calculation unit 26 calculates a state q t +1 at time t+1 that satisfies the above-mentioned complementarity condition regarding contact between objects and minimizes an objective function that represents a difference between the state q t+1 of the robot 14 and the target object B at time t+1 and a target value, based on the state q t at time t calculated by the acquisition unit 24. Note that at this time, the calculation unit 26 performs the calculation process by referring to data on the complementarity condition regarding contact between objects stored in the complementarity condition storage unit 20 and data on the objective function stored in the objective function storage unit 22.
  • the complementarity condition regarding contact between objects is a complementarity condition regarding the contact force generated between the contact point of the gripper G of the robot 14, the contact point of the target object B, and the contact point of the wall W or the floor FL, which is another object, and the distance between the contact point of the gripper G of the robot 14, the contact point of the target object B, and the contact point of the wall W or the floor FL.
  • the complementarity condition regarding contact between objects is any of the above-mentioned formulas (4a) to (4d), (7a) to (7d), and (9a) to (9d).
  • the control unit 28 controls the operation of the gripper G of the robot 14, which is the control target, so as to achieve the state q t+1 at the time t+1 calculated by the calculation unit 26.
  • the control unit 28 controls the operation of the gripper G of the robot 14 by using a known technique.
  • the CPU 42 of the control device 16 reads out a control program from the storage device 46, expands it in the memory 44, and executes it. As a result, the CPU 42 functions as each functional component of the control device 16, and the control process shown in FIG. 8 is executed. Note that the control process shown in FIG. 8 is executed repeatedly.
  • step S100 the acquisition unit 24 reads out the image data at time t stored in the data storage unit 18, and acquires data representing a state qt at time t.
  • step S102 the calculation unit 26 reads out data relating to the above formulas (9a) to (9d) from the complementarity condition storage unit 20. Also, in step S102, the calculation unit 26 reads out data relating to the above formula (10) from the objective function storage unit 22. Then, in step S102, based on the state q t at time t acquired in step S100, a state q t+1 at time t+1 is calculated that satisfies the complementarity conditions relating to contact between objects in the above formulas (9a) to (9d) and minimizes the objective function of the above formula (10) that represents the difference between the state q t+1 of the robot 14 and the target object B at time t+ 1 and the target value.
  • step S104 the control unit 28 controls the operation of the gripper G of the robot 14, which is the control target, so as to attain the state q t+1 at time t+1 calculated in step S102.
  • the control process shown in FIG. 8 is repeated, and a control signal is repeatedly output to the robot 14, so that the target object B is brought into the desired position and posture.
  • the control device of the control system acquires the state of the controlled object at time t, and calculates, based on the state at time t, a state at time t+1 that satisfies the complementarity condition relating to the contact force generated at the contact point of the controlled object and the distance between the contact points on the controlled object, and that minimizes the objective function that represents the difference between the state of the controlled object at time t+1 and a target value.
  • the control device then controls the controlled object so that it reaches the calculated state at time t+1. This makes it possible to control the controlled object so that the complementarity condition relating to the contact force generated at the contact point of the controlled object and the distance between the contact points on the controlled object is satisfied.
  • control device of this embodiment can continuously control the robot that is the object of control.
  • the complementary conditions of the above formulas (4a) to (4d) are linearized to generate the linear complementary conditions of the above formulas (7a) to (7d). This reduces the computational cost required to calculate the complementary conditions, and allows the robot's motion command to be calculated online according to the state of the gripping part G of the robot 14 and the target object B. This makes it possible to realize, for example, the motion of pushing the target object B with the tip of the gripping part G of the robot 14 to raise it up (without gripping it).
  • a relaxation variable s i is introduced to the linear complementarity conditions of the above formulas (7a) to (7d). This makes it possible to calculate whether or not the complementarity conditions regarding contact between objects are satisfied even if there is a discrepancy between contact between objects in the real world and contact between objects detected by sensor data, and thus makes it possible to appropriately control the operation of the gripper G of the robot 14.
  • FIG. 9 is a diagram for explaining this embodiment. As shown in FIG. 9, in this embodiment, a simulation was performed in which the tip of the gripping part G of the robot was brought into contact with a lying target object B, which is a box, and the box was made to stand up. Note that this embodiment utilizes complementarity conditions that introduce relaxation variables into the above equations (7a) to (7d).
  • FIGS. 10 and 12 show the results of this embodiment.
  • FIG. 11 is a diagram for explaining the results of this embodiment.
  • FIG. 10 it can be seen that the gripping part G of the robot touches the box, which is the target object B, and can make the target object B stand upright.
  • FIG. 12 is a diagram showing the results of monitoring the parameters related to each contact point shown in FIG. 11 at each time.
  • the complementary condition for contact is satisfied in each parameter of the contact points p1 to p4.
  • the contact point p3 contacts the wall surface W between (a) and (b).
  • ⁇ n and ⁇ f are generated between (a) and (b).
  • LCQP is considered to be applicable to targets that require real-time optimization in a control system with many parameters. For example, it can be applied to object manipulation in which the contact state is switched, as well as the movement of legged robots or wheeled robots. Robots that perform object manipulation in which the contact state is switched often require the omission of changing the grip of the object and pick-and-place in a narrow environment, so LCQP can be applied. LCQP can also be applied to cases in which a manipulator autonomously performs work, such as in automating experiments and assembling parts. In addition, wheeled robots run in contact with walls or floor surfaces, and frictional forces are generated between the wall or floor surface, so LCQP that utilizes the complementarity condition regarding contact can be applied.
  • processors in this case include PLDs (Programmable Logic Devices) such as FPGAs (Field-Programmable Gate Arrays) whose circuit configuration can be changed after manufacture, and dedicated electric circuits such as ASICs (Application Specific Integrated Circuits), which are processors with circuit configurations designed exclusively to execute specific processes.
  • PLDs Programmable Logic Devices
  • FPGAs Field-Programmable Gate Arrays
  • ASICs Application Specific Integrated Circuits
  • each process may be executed by one of these various processors, or may be executed by a combination of two or more processors of the same or different types (for example, multiple FPGAs, or a combination of a CPU and an FPGA, etc.).
  • the hardware structure of these various processors is, more specifically, an electric circuit that combines circuit elements such as semiconductor elements.
  • the programs are described as being stored (e.g., installed) in advance in a storage device, but this is not limiting.
  • the programs may be provided in a form stored in a storage medium such as a CD-ROM, DVD-ROM, Blu-ray disc, or USB memory.
  • the programs may also be downloaded from an external device via a network.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)
  • Feedback Control In General (AREA)

Abstract

制御装置は、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の状態を計算する。そして、制御装置は、計算された時刻ｔ＋１の状態となるように、制御対象を制御する。

Description

制御装置、制御方法、及び制御プログラム
　本開示は、制御装置、制御方法、及び制御プログラムに関する。
　従来、ロボットが外界と多様な物理インタラクションを行なった際にも、体の任意箇所における接触状態と作用力をともに計測する技術が知られている（例えば、特開２００７－３００５４号公報を参照）。
　また、ロボットが物体に接触する際の目標軌道を達成するために、どのように物体に接触すべきかに関する技術が知られている（例えば、文献１：Bernardo Aceituno-Cabezas and Alberto Rodriguez, "A Global Quasi-Dynamic Model for Contact-Trajectory Optimization", Robotics: Science and Systems 2020 Corvalis, Oregon, USA, July 12-16, 2020, ［令和4年9月27日検索］，インターネット＜URL：https://www.researchgate.net/publication/342882025>を参照）。この文献１では、ロボットが物体に接触する際の大域的な最適化モデルが提案されている。
　ところで、例えば、ロボットが物体と接触するような場面においては、ロボットと物体との間の接触態様（以下、単に接触モードとも称する。）が逐次変化する。これにより、ロボットの動作に関する物理的な制約も逐次変化する。このため、ロボットと物体との接触が多い場合、そのロボットの動作を適切に制御することは難しい。
　本開示は、上記の点に鑑みてなされたものであり、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件が満たされるように、制御対象を制御することを目的とする。
　上記目的を達成するために、本開示に係る制御装置は、時刻ｔの制御対象の状態を取得する取得部と、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算する計算部と、計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する制御部と、を含む制御装置である。
　また、前記計算部は、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件、及び前記接触点が接する接触面における接線接触速度に関する相補条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算するようにすることができる。
　なお、制御対象とは、制御装置によって制御される機械と、その機械により作用を及ぼされる対象物体のことを示し、機械はロボットを含み得る。また、状態とは、前記機械および前記対象物の位置および姿勢を含み得る。また、時刻ｔ＋１とは、時刻ｔからの計算上の単位時間進んだ時刻を示し、制御装置のサイクルタイム、コンピュータの計算のサイクルタイム、コンピュータによるシミュレーションのサイクルタイムのことを含み得る。また、目標値とは、前記機械または／および対象物体が到達するあらかじめ設定した状態であり、座標および姿勢を含みうる。また、最小とは、目的関数から計算される値であるが、計算の手法により誤差を含み得る。
　また、前記制御対象は、ロボット及び対象物体であり、前記状態のデータは、前記ロボット及び前記物体の状態をセンシングするセンサによって得られたセンサデータに基づき計算され、前記計算部は、時刻ｔの前記状態に基づいて、前記ロボットの接触点、前記対象物体の接触点、及び他の物体の接触点の何れかの間において発生する接触力と、前記ロボットの接触点、前記対象物体の接触点、及び前記他の物体の接触点の何れかの間の距離とに関する前記相補性条件が満たされるように、時刻ｔ＋１の前記状態を計算するようにすることができる。
　また、前記相補性条件は、複数の前記接触点の各々についての、時刻ｔの前記状態ｑｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φｉ（ｑｔ）とに関する以下の相補性条件（４ａ）と、時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における、第１水平方向の接触力λｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、前記接線接触速度の絶対値γｉとに関する以下の相補性条件（４ｂ）と、時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における、第２水平方向の接触力λｆｉ－と、前記符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）及び前記接線接触速度の絶対値γｉとに関する以下の相補性条件（４ｃ）と、ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γｉと、前記接触面の摩擦係数μｉと、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第２水平方向の接触力λｆｉ－とに関する以下の相補性条件（４ｄ）とを含んで構成されるようにすることができる。
　また、前記計算部は、時刻ｔの前記状態ｑｔに基づいて、前記相補性条件（４ａ）～（４ｄ）を満たし、かつ以下の目的関数（５）が最小となるような時刻ｔ＋１の前記状態ｑｔ＋１を計算する、ようにすることができる。
　ただし、ｇ（ｑｔ＋１）は前記状態ｑｔ＋１に対応する前記制御対象の物理量を表し、ｇｒｅｆは前記状態ｑｔ＋１の目標値に対応する前記制御対象の物理量の目標値を表し、Ｑｇ，Ｑｖ，及びＱｆは予め与えられる正定重み行列であり、λｔ＋１は、複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第２水平方向の接触力λｆｉ－とを各成分として持つ時刻ｔ＋１のベクトルである。
　また、前記相補性条件は、複数の前記接触点の各々についての、時刻ｔの前記状態ｑｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λｎｉと、前記接触力λｎｉの変化量Δλｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φｉ（ｑｔ）と、前記状態ｑｔにより前記距離φｉ（ｑｔ）を微分することにより得られるΦｉ，ｑ（ｑｔ）と、前記状態ｑｔの変化量Δｑｔとに関する以下の線形相補性条件（７ａ）と、時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における第１水平方向の接触力λｆｉ＋と、前記接触力λｆｉ＋の変化量Δλｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、前記一般化速度ｖｔにより前記接線接触速度ψｉ（ｑｔ，ｖｔ）を微分することにより得られるΨｉ，ｖ（ｑｔ，ｖｔ）と、前記接線接触速度の絶対値γｉと、前記状態ｑｔの変化量Δｑｔとに関する以下の線形相補性条件（７ｂ）と、時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における第２水平方向の接触力λｆｉ－と、前記接触力λｆｉーの変化量Δλｆｉーと、前記符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、前記接線接触速度の絶対値γｉと、前記一般化速度ｖｔにより前記接線接触速度ψｉ（ｑｔ，ｖｔ）を微分することにより得られるΨｉ，ｖ（ｑｔ，ｖｔ）と、に関する以下の線形相補性条件（７ｃ）と、ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γｉと、前記接触面の摩擦係数μｉと、前記接触面の鉛直方向に発生する接触力λｎｉと、前記接触力λｎｉの変化量Δλｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第１水平方向の接触力λｆｉ＋の変化量Δλｆｉ＋と、前記第２水平方向の接触力λｆｉ－と、前記第２水平方向の接触力λｆｉ－の変化量Δλｆｉ－と、に関する以下の線形相補性条件（７ｄ）とを含んで構成されるようにすることができる。

 
　また、前記計算部は、時刻ｔの前記状態ｑｔに基づいて、前記相補性条件（７ａ）～（７ｄ）を満たし、かつ以下の目的関数（６）が最小となるような前記状態の変化量Δｑｔを計算することにより、時刻ｔ＋１の前記状態ｑｔ＋１を計算する、ようにすることができる。

 

 

 

 
　ただし、ｇｔは前記状態ｑｔに対応する前記制御対象の物理量を表し、ｇｒｅｆは前記状態ｑｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑｇ，Ｑｖ，及びＱｆ予め与えられる正定重み行列であり、λｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前
記第２水平方向の接触力λｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλｔは前記λｔの変化量である。
　また、前記相補性条件には、前記相補性条件を緩和するための以下の緩和変数ｓｉが含まれている、ようにすることができる。

 
　また、前記計算部は、時刻ｔの前記状態ｑｔに基づいて、前記相補性条件（９ａ）～（９ｄ）を満たし、かつ以下の目的関数（１０）が最小となるような前記状態の変化量Δｑｔと前記接触力の変化量Δλｔと緩和変数ｓｉを各成分として持つベクトルｓｔとを計算することにより、時刻ｔ＋１の前記状態ｑｔ＋１を計算する、ようにすることができる。

 

 

 

 
　ただし、ｇｔは前記状態ｑｔに対応する前記制御対象の物理量を表し、ｇｒｅｆは前記状態ｑｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑｇ，Ｑｖ，及びＱｆ予め与えられる正定重み行列であり、λｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第２水平方向の接触力λｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλｔは前記λｔの変化量である。
　また、本開示の制御方法は、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、処理をコンピュータが実行する制御方法である。
　また、本開示の制御プログラムは、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、処理をコンピュータに実行させるための制御プログラムである。
　本開示の制御装置、方法、及びプログラムによれば、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件が満たされるように、制御対象を制御することができる。
本実施形態の概要を説明するための図である。 本実施形態に係る制御システムが実行する処理の流れを説明するための図である。 本実施形態で用いる状態ｑｔを説明するための図である。 接触点と接触面との間の距離と接触力との間の関係を説明するための図である。 接触点と接触面との間のクーロン摩擦係数を説明するための図である。 本実施形態に係る制御システムの機能構成を示すブロック図である。 本実施形態に係る制御装置のハードウェア構成を示すブロック図である。 本実施形態における制御処理の流れを示すフローチャートである。 本実施例を説明するための図である。 本実施例の結果を表す図である。 本実施例の結果を説明するための図である。 本実施例の結果を表す図である。 従来技術を説明するための図である。
　以下、本開示の実施形態の一例を、図面を参照しつつ説明する。本実施形態では、本開示に係る制御装置を搭載した制御システムを例に説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。
＜実施形態の概要＞
　図１３は、従来技術の一例を説明するための図である。図１３に示されるように、従来技術では、時刻ｔ１における物体Ｂとロボットの把持部Ｇとの間の接触モードに応じた制御１、時刻ｔ２における接触モードに応じた制御２、及び時刻ｔ３における接触モードに応じた制御３が実行される。この場合には、対象物体Ｂとロボットの把持部Ｇとの間の接触モードに応じてロボットの動作を制御する必要がある。しかし、その接触モードの数は膨大なものとなるため、膨大な数の接触モードに応じてロボットを連続的に制御することは難しい、という課題がある。
　そこで、本実施形態では、物体間の接触に関する相補性条件が満たされるように、ロボットの動作を連続的に制御する。これにより、ロボットの動作をリアルタイムに制御することができる。
　図１は、本実施形態の概要を説明するための図である。図１に示されるように、本実施形態では、対象物体Ｂとロボットの把持部Ｇと他の物体である壁Ｗ又は床面ＦＬとの接触に関する相補性条件が考慮される。対象物体Ｂとロボットの把持部Ｇとは、本開示の制御対象の一例である。
　具体的には、図１に示されるように、時刻ｔ１においては、ロボットの把持部Ｇは対象物体Ｂと接触していないため把持部Ｇの接触点ｐＧと対象物体Ｂとの間の距離はゼロよりも大きく、ロボットの把持部Ｇの接触点ｐＧにおいて発生する接触力はゼロである。なお、時刻ｔ１においては、接触点ｐＧは対象物体Ｂと実際に接触していないため、接触候補点でもある。
　一方、図１に示されるように、時刻ｔ２においては、ロボットの把持部Ｇは対象物体Ｂと接触しているため把持部Ｇの接触点ｐＧと対象物体Ｂとの間の距離はゼロとなり、ロボットの把持部Ｇの接触点ｐＧにおいてゼロよりも大きい接触力λ４が発生する。
　更に、図１に示されるように、時刻ｔ３においては、ロボットの把持部Ｇは対象物体Ｂと接触していないため把持部Ｇの接触点ｐＧと対象物体Ｂとの間の距離はゼロよりも大きく、ロボットの把持部Ｇの接触点ｐＧにおいて発生する接触力はゼロとなる。
　なお、図１に示されるように、対象物体Ｂには複数の接触点ｐ１，ｐ２，ｐ３，ｐ４がある。例えば、時刻ｔ１においては、接触点ｐ１と床面ＦＬとの間において接触力λ１が発生し、接触点ｐ２と床面ＦＬとの間において接触力λ２が発生する。また、時刻ｔ２においては、接触点ｐ１と床面ＦＬとの間において接触力λ１が発生し、接触点ｐ３と壁Ｗとの間において接触力λ３が発生する。また、時刻ｔ３においては、接触点ｐ１と床面ＦＬとの間において接触力λ１が発生し、接触点ｐ３と床面ＦＬとの間において接触力λ３が発生する。
　図１に示されるように、ロボットの把持部Ｇと対象物体Ｂと壁Ｗ又は床面ＦＬとの間において、各々の距離がゼロよりも大きい場合には接触力がゼロとなる。一方、ロボットの把持部Ｇと対象物体Ｂと壁Ｗ又は床面ＦＬとの間において、各々の距離がゼロである場合には接触力がゼロよりも大きくなる。このため、物体間の距離と物体間で発生する接触力
との間には相補性条件が成立する。
　そこで、本実施形態では、物体間の接触に関する相補性条件が満たされるように、ロボットの動作を連続的に制御する。これにより、図１３の従来技術のような複数の接触モードに基づくロボットの制御をすることなく、ロボットの動作をリアルタイムに制御することができる。なお、２つのスカラー変数であるａとｂとの相補性条件は以下の式（１）によって表現される。
　なお、ａ，ｂは次式を満たす。

 

 
　なお、以下では、本実施形態において提案される手法を、ＬＣＱＰ（Ｌｉｎｅａｒ　ｃｏｍｐｌｅｍｅｎｔａｒｉｔｙ　ｑｕａｄｒａｔｉｃ　ｐｒｏｇｒａｍ）とも称する。
　図２は、本実施形態に係る制御システムが実行する処理の流れを説明するための図である。図１に示されるように、本実施形態の制御装置１６は、時刻ｔの制御対象の状態ｑｔを取得し、目標値との差異が小さくなるような時刻ｔ＋１の制御対象の状態ｑｔ＋１を計算する。その際、制御装置１６は、物体間の接触に関する相補性条件が満たされ、かつ目標値と時刻ｔ＋１の制御対象の状態ｑｔ＋１との間の差異を表す目的関数が小さくなるように、時刻ｔ＋１の制御対象の状態ｑｔ＋１を計算する。そして、制御装置１６は、時刻ｔ＋１の制御対象の状態ｑｔ＋１を制御入力としてロボット１４へ出力する。これにより、制御対象の変化に応じて接触モードを逐次設定することなく、ロボットの動作をリアルタイムに制御することができる。また、本実施形態では、物体間の接触に関する相補性条件が満たされているか否かを判定するのみでよいため、ロボットの動作をリアルタイムに制御する際の計算コストを低減させることができる。
　図３は、本実施形態で用いる状態ｑｔを説明するための図である。図３は、システム全体が把持部Ｇと対象物体Ｂとにより構成される準静的定式化の一例を示す図でもある。図３に示されるように、本実施形態では、把持部Ｇの所定位置を原点とする把持部座標系における座標（ｘａ，ｙａ）と、把持部Ｇの姿勢を表す姿勢角θａと、把持部Ｇが備えるグリッパー間の距離ｒａとを各成分として持つ状態ベクトルｑａが設定される。また、図３に示されるように、本実施形態では、対象物体Ｂの所定位置を原点とする対象物体座標系における座標（ｘ０，ｙ０）と、対象物体Ｂの姿勢を表す姿勢角θ０とを各成分として持つ状態ベクトルｑｕが設定される。なお、把持部Ｇの全自由度（DOF : Degree of freedom）をＮｕとし、対象物体Ｂの全自由度をＮａとした場合、一般化座標における状態ベク
トルｑａと状態ベクトルｑｕとは、以下の式によって定義される。なお、以下で説明する数式内の太字はベクトル又は行列を表す。

 
　制御対象の状態ｑと各物体の速度ｖは、次式によって表される。なお、速度ｖは一般化速度でもある。

 
　なお、準静的モデルを離散時間へ拡張することにより、以下の式が導出される。ｈは０以上の予め設定される係数である。
　次に、接触点と接触面との間において発生する力である接触力について説明する。図３に示されるように、対象物体Ｂの隅と把持部Ｇが備えるグリッパーの隅を接触点ｐＧ１，ｐＧ２として設定する。ここで、ｉ番目の接触点と接触面との間において発生する各時刻ｔの接触力λｔ，ｉが以下のように定義される。

 
　λｆｉは、ｉ番目の接触点が接する接触面における水平方向の接触力である。λｎｉは、ｉ番目の接触点が接する接触面の鉛直方向に発生する接触力である。なお、本実施形態では、対象物体Ｂがボックスである場合を例に説明するが、他の種類の物体（例えば、球状の物体）に関しても同様に相補性条件を定式化することができる。
　次に、対象物体Ｂに相当するオブジェクトｏにかかる力とモーメントとの釣り合いを導出するために、接触点と接触面とのペアの集合Ｃｏを導入する。接触点と接触面とのペアを識別するためのインデックスｉは、次式によって定義される。
　また、接触点と接触面とのペアを識別するためのインデックスｉは、次式によって定義される接触面の回転行列Ｒｉ（ｑｔ）及び接触面の質量中心からの相対位置［ｒｏ，ｉ，
ｘ（ｑｔ），ｒｏ，ｉ，ｙ（ｑｔ）］Ｔに関連付けられる。

 
　準静的な仮定下（例えば、速度と加速度とが十分に小さい）において、オブジェクトｏに関するニュートンの第２法則は、以下の式（３）によって表される。
　なお、上記式のｒ～ ｏ，ｉ（ｑｔ）は次式によって表される。
　上記式（３）が表すベクトルの１次元目の成分と２次元目の成分とは、世界座標系においてオブジェクトｏに作用する力を表す。同様に、上記式（３）の３次元目の成分がモーメントを表す。また、上記式（３）のかっこ内に記載されている力とモーメントとはオブジェクトｏにかかる接触力を表す。また、次式によって定義される力Ｆｏは重力等の時間的に一定の力を表す。上記式（３）に示されている各値は、既知の技術によって計算可能である。
［接触に関する相補性条件］
　本実施形態では、接触点と接触面とのペアを識別するためのインデックスｉの各々において、時刻ｔに発生する接触力ベクトルλｔ，ｉが定義される。上述したように、この接触力ベクトルλｔ，ｉは、ｉ番目の接触点と接触面とのペアにおける接触面の垂直方向に発生する接触力λｎｉと、ｉ番目の接触点と接触面とのペアにおける接触面の水平方向に発生する接触力λｆｉとを各成分として持つ。図４は、ｉ番目の接触点と接触面との間の距離φｉ（ｑ）と接触力λｎｉとの間の関係を説明するための図である。図４に示されるように、距離φｉ（ｑ）＝０である場合には接触力λｎｉ＞０となり、距離φｉ（ｑ）＞０である場合には接触力λｎｉ＝０となる。
　また、上記図１に示されるように、ロボット１４の把持部Ｇの接触点と対象物体Ｂとが接触し、把持部Ｇの接触点と対象物体Ｂとの間の距離がゼロである場合に接触力λｎｉが正となる。そのため、本実施形態では、時刻ｔにおけるｉ番目の接触点と接触面との間の
距離φｉ（ｑｔ）を導入する。距離φｉ（ｑｔ）は次式によって表される。
　また、対象物体Ｂ又は把持部Ｇが接触点において滑らないためには、接触力ベクトルλｔ，ｉが、接触点と接触面との摩擦によって形成される摩擦円錐内に位置している必要がある。そのため、本実施形態では、時刻ｔにおけるｉ番目の接触点と接触面とにおける接触速度ψｉ（ｑｔ，ｖｔ）を導入する。接触速度ψｉ（ｑｔ，ｖｔ）は、次式によって表される。
　更に、物体間の接触に関する相補性条件を表現するために、本実施形態では、接触面の水平方向に発生する非負の２つの接触力λｆｉ＋，λｆｉ－を導入する。２つの接触力λｆｉ＋，λｆｉ－は、次式によって表される。

 
　図５は、接触点と接触面との間のクーロン摩擦係数を説明するための図である。図５に示される黒丸は接触点を表し、実線の矢印は接触力を表し、破線の矢印は接触速度を表す。対象物体Ｂ又は把持部Ｇが接触点において滑らないためには、鉛直方向に発生する接触力λｎｉと水平方向に発生する接触力λｆｉ＋，λｆｉ－とが、図５に示されるような関係である必要がある。図５に示される実線の矢印は接触力を表し、破線の矢印は対象物体Ｂが滑ることにより生じる接触速度ψｉ（ｑｔ，ｖｔ）の絶対値γｉを表す。また、μｉは接触点と接触面との間の摩擦係数を表し、μｉ＞０である。なお、絶対値γｉは次式によって定義される。
　図５の（Ａ）の場合、接触力ベクトルλｔ，ｉが摩擦円錐Ｃの右側の境界に位置しているため、対象物体Ｂの接触点において滑りが発生する。このため、図５の（Ａ）のような状態下においては、対象物体Ｂと床面ＦＬとが接触する接触点において次式が成立する。
　上記式に示されているように、図５の（Ａ）の場合、接触力λｎｉが正の値となり、第１水平方向に発生する接触力λｆｉ＋が正の値となり、第１水平方向とは逆向きの第２水平方向に発生する接触力λｆｉ－がゼロとなり、第１水平方向を正とする接触速度ψｉ（ｑ，ｑ・）＝ψｉ（ｑｔ，ｖｔ）が負となり、接触速度の絶対値γｉが正となる。また、図５の（Ａ）の場合、接触点において発生する摩擦力μｉλｎｉから第１水平方向に発生する接触力λｆｉ＋と第２水平方向に発生する接触力λｆｉ－とを差し引いた値がゼロとなる。これにより、図５の（Ａ）の場合、対象物体Ｂの接触点において滑りが発生する。
　また、図５の（Ｂ）の場合、接触力ベクトルλｔ，ｉが摩擦円錐Ｃの内側に位置しているため、対象物体Ｂに滑りは発生しない。このため、図５の（Ｂ）のような状態下においては、対象物体Ｂと床面ＦＬとが接触する接触点において次式が成立する。
　上記式に示されているように、図５の（Ｂ）の場合、接触力λｎｉが正の値をとり、第１水平方向に発生する接触力λｆｉ＋が正の値をとり、第２水平方向に発生する接触力λｆｉ－がゼロとなり、第１水平方向を正とする接触速度ψｉ（ｑ，ｑ・）＝ψｉ（ｑｔ，ｖｔ）がゼロとなり、接触速度の絶対値γｉがゼロとなる。また、図５の（Ｂ）の場合、接触点において発生する摩擦力μｉλｎｉから第１水平方向に発生する接触力λｆｉ＋と第２水平方向に発生する接触力λｆｉ－とを差し引いた値が正の値となる。これにより、図５の（Ｂ）の場合、対象物体Ｂに滑りは発生しない。
　また、図５の（Ｃ）の場合、接触力ベクトルλｔ，ｉが摩擦円錐Ｃの左側の境界に位置しているため、対象物体Ｂの接触点において滑りが発生する。このため、図５の（Ｃ）のような状態下においては、対象物体Ｂと床面ＦＬとが接触する接触点において次式が成立する。
　上記式に示されているように、図５の（Ｃ）の場合、接触力λｎｉが正の値となり、第１水平方向に発生する接触力λｆｉ＋がゼロとなり、第２水平方向に発生する接触力λｆｉ－が正の値となり、第１水平方向を正とする接触速度ψｉ（ｑ，ｑ・）＝ψｉ（ｑｔ，ｖｔ）が正の値となり、接触速度の絶対値γｉが正の値となる。また、図５の（Ｃ）の場合、接触点において発生する摩擦力μｉλｎｉから第１水平方向に発生する接触力λｆｉ＋と第２水平方向に発生する接触力λｆｉ－とを差し引いた値がゼロとなる。これにより、図５の（Ｃ）の場合、対象物体Ｂの接触点において滑りが発生する。
　図５に示されるように、接触速度ψｉ（ｑｔ，ｖｔ）がゼロではない場合に、第１水平方向に発生する接触力λｆｉ＋及び第２水平方向に発生する接触力λｆｉ－の何れか一方がゼロとなる。これは、接触速度ψｉ（ｑｔ，ｖｔ）がゼロではない場合に、γｉ＋ψｉ（ｑｔ，ｖｔ）の値及びγｉ－ψｉ（ｑｔ，ｖｔ）の値の何れか一方がゼロではないためである。なお、前述したように、ψｉ（ｑｔ，ｖｔ）は、第１方向を正とする符号付きの接触速度である。また、図５に示されるように、接触速度ψｉ（ｑｔ，ｖｔ）は、第１水平方向に発生する接触力λｆｉ＋又は第２水平方向に発生する接触力λｆｉ－とは逆向きの符号となる。
　このため、物体間の接触に関する相補性条件として、次式が成立する。
　上記式（４ａ）は、物体間の距離φｉ（ｑｔ）がゼロである場合には接触力λｎｉが正となり、物体間の距離φｉ（ｑｔ）が正の値である場合には接触力λｎｉがゼロとなる、ということを表す相補性条件の式である。また、上記式（４ｂ）～（４ｄ）は、接触力ベクトルλｔ，ｉが摩擦円錐Ｃの内側に位置している、ということを表す相補性条件である。また、上記式（４ｄ）は、図５の（Ａ）及び（Ｃ）にも示されているように、接触力ベクトルλｔ，ｉが摩擦円錐Ｃの境界に位置している、ということを表す相補性条件である。
　このため、上述した物体間の接触に関する相補性条件は、相補性条件（４ａ）と、相補性条件（４ｂ）と、相補性条件（４ｃ）と、相補性条件（４ｄ）とを含んで構成される。相補性条件（４ａ）は、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件の一例である。相補性条件（４ｂ）～（４ｄ）は、接触点が接する接触面における接線接触速度に関する相補条件の一例である。
　上記式（４ａ）は、複数の接触点の各々についての、時刻ｔの状態ｑｔにおける、ｉ番目の接触点が接する接触面の鉛直方向に発生する接触力λｎｉと、ｉ番目の接触点と接触面との間の距離φｉ（ｑｔ）とに関する相補性条件である。
　上記式（４ｂ）は、複数の接触点の各々についての、時刻ｔの状態ｑｔ及び時刻ｔの一
般化速度ｖｔにおける、ｉ番目の接触点が接する接触面における、第１水平方向の接触力λｆｉ＋と、接触面の第１水平方向を正とする符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、接線接触速度の絶対値γｉとに関する相補性条件である。
　上記式（４ｃ）は、複数の接触点の各々についての、時刻ｔの状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の接触点が接する接触面における、第２水平方向の接触力λｆｉ－と、符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）及び接線接触速度の絶対値γｉとに関する相補性条件である。
　上記式（４ｄ）は、複数の接触点の各々についての、ｉ番目の接触点が接する接触面における、接線接触速度の絶対値γｉと、接触面の摩擦係数μｉと、接触面の鉛直方向に発生する接触力λｎｉと、第１水平方向の接触力λｆｉ＋と、第２水平方向の接触力λｆｉ－とに関する相補性条件である。
　なお、上記の提案手法を３次元運動の場合に適用すると、多面体円錐の対角線毎に上記式（４ｂ）と（４ｃ）のペアが導入される。
　次に、本実施形態のＬＣＱＰを定式化する。ここで、接触力ベクトルλｔが定義される。接触力ベクトルλｔは、次式のλｆｉ＋，λｆｉ－，λｎｉを各成分として持つベクトルである。
　また、以下の式（５）が導入される。本実施形では、上記式（２）～（４ｄ）が表す条件を満たしつつ、以下の式（５）が表す目的関数が最小となるような時刻ｔ＋１の状態ｑｔ＋１及び接触力ベクトルλｔ＋１が計算される。
　ここで、上記式（５）のｇは、一般微分可能関数であり、状態ｑを、ロボット１４の把持部Ｇの位置及び姿勢と対象物体Ｂの位置及び姿勢の空間へ写像するための関数である。このため、ｇ（ｑ）は、状態ｑに対応する制御対象の物理量である。ｇｒｅｆは、物理量の目標値ベクトルを表す。また、Ｑｇ，Ｑｖ，Ｑｆは、予め与えられる正定重み行列である。Ｑｇはタスクの優先度を表し、Ｑｖ，Ｑｆは、正規化項である。
　本実施形態のＬＣＱＰでは、既知の勾配降下法と同様の考え方に基づき、上記式（５）に示されている制約付き非線形目的関数を最適化する。具体的には、本実施形では、上記式（５）の目的関数が小さくなるような降下ステップを生成することにより、ロボット１４を動作させる制御を実行する。なお、各降下ステップは、線形化された式（５）の目的関数を解くことによって計算される。これにより、次の時間ｔ＋１の把持部Ｇ及び対象物体Ｂの位置と力とを表す状態ｑｔ＋１及び接触力ベクトルλｔ＋１が得られる。
　上記式（５）の目的関数が小さくなるような降下ステップの生成を繰り返すことにより、最終的に式（５）が最小化されるような目標状態ｑ＊に到達する。
　本実施形態では、問題を扱いやすくするために、Δｑｔ及びΔλｔに基づいて定義される以下の式を導入する。

 
　Δｑｔ及びΔλｔが十分に小さい場合、上記式（５）の目的関数はΔｑｔとΔλｔの２次関数によって以下の式（６）のように近似される。
　ここで、上記式（６）のうちの各変数は以下の式によって定義される。

 

 
　上記式（２）を上記式（６）へ組み込むと、上記式（３）～（４ｄ）は以下のように線形化することができる。
　具体的には、時刻ｔにおけるΔｑｔ，Δλｎｉ，Δλｆｉ＋，及びΔλｆｉ－を用いて時刻ｔ＋１におけるｑｔ＋１，λｎｉ，λｆｉ＋，及びλｆｉ－を近似することにより、以下の式（７ａ）～（７ｄ）が導出される。
　なお、上記式（７ａ）～（７ｄ）における未定義の変数は次式によって定義される。

 
　なお、ＬＣＱＰが実行される際には、次式の関係式が上記式（７ａ）～（７ｄ）に代入される。
　このため、上述した物体間の接触に関する相補性条件は、線形相補性条件（７ａ）と、相補性条件（７ｂ）と、線形相補性条件（７ｃ）と、線形相補性条件（７ｄ）とを含んで構成される。
　上記式（７ａ）は、複数の接触点の各々についての、時刻ｔの状態ｑｔにおける、ｉ番目の接触点が接する接触面の鉛直方向に発生する接触力λｎｉと、接触力λｎｉの変化量Δλｎｉと、ｉ番目の接触点と接触面との間の距離φｉ（ｑｔ）と、状態ｑｔにより距離φｉ（ｑｔ）を微分することにより得られるΦｉ，ｑ（ｑｔ）と、状態ｑｔの変化量Δｑｔとに関する線形相補性条件である。
　上記式（７ｂ）は、複数の接触点の各々についての、時刻ｔの状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の接触点が接する接触面における第１水平方向の接触力λｆｉ＋と、接触力λｆｉ＋の変化量Δλｆｉ＋と、接触面の第１水平方向を正とする符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、一般化速度ｖｔにより接線接触速度ψｉ（ｑ
ｔ，ｖｔ）を微分することにより得られるΨｉ，ｖ（ｑｔ，ｖｔ）と、接線接触速度の絶対値γｉと、状態ｑｔの変化量Δｑｔとに関する線形相補性条件である。
　上記式（７ｃ）は、複数の接触点の各々についての、時刻ｔの状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の接触点が接する接触面における第２水平方向の接触力λｆｉ－と、接触力λｆｉーの変化量Δλｆｉーと、符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、接線接触速度の絶対値γｉと、一般化速度ｖｔにより接線接触速度ψｉ（ｑｔ，ｖｔ）を微分することにより得られるΨｉ，ｖ（ｑｔ，ｖｔ）とに関する線形相補性条件である。
　上記式（７ｄ）は、複数の接触点の各々についての、ｉ番目の接触点が接する接触面における、接線接触速度の絶対値γｉと、接触面の摩擦係数μｉと、接触面の鉛直方向に発生する接触力λｎｉと、接触力λｎｉの変化量Δλｎｉと、第１水平方向の接触力λｆｉ＋と、第１水平方向の接触力λｆｉ＋の変化量Δλｆｉ＋と、第２水平方向の接触力λｆｉ－と、第２水平方向の接触力λｆｉ－の変化量Δλｆｉ－とに関する以下の線形相補性条件である。
（力とモーメントの線形化）
　上記式（３）が表す力とモーメントとの間の関係式は、次式（８）のように線形化される。
（物体間の接触に関する相補性条件の緩和）
　なお、実際には、物体間の接触に関する相補性条件（７ａ）～（７ｃ）を満たすような解が存在しない場合があり得る。例えば、センサ等によって測定された状態がノイズを含んでいるような場合又は対象物体Ｂを表すモデルが現実の対象物体Ｂを表現しきれていない場合（例えば、対象物体Ｂの形状モデルのモデリングエラー又は対象物体Ｂの変形等）により、物体間の接触に関する相補性条件（７ａ）～（７ｃ）が満たされないという場合もあり得る。
　また、物体間の接触に関する相補性条件（７ａ）～（７ｄ）により、作動するオブジェクトであるロボット１４の把持部Ｇが、対象物体Ｂと接触することできないという状況も発生し得る。例えば、非ゼロの鉛直方向の接触力λｎｉが計算された場合であっても、その接触点の実際の位置は接触面からわずかにずれているという場合もあり得る。
　そこで、本実施形態では、これらの問題を解決するために、物体間における小さな貫通を許可する。具体的には、本実施形態では、上記式（７ａ）～（７ｄ）に対して、緩和変数ｓｉを導入することにより、物体間の接触に関する相補性条件を緩和させる。緩和変数ｓｉが導入された相補性条件は次式によって表される。
　なお、上記式（９ａ）～（９ｄ）を利用する場合には、ｐｉｓｉ ２を上記式（６）の目的関数へ追加する。ｐｉｓｉ ２が追加された目的関数は、以下の式（１０）によって表される。なお、以下の式におけるｐｉは予め設定される係数である。
　本実施形態では、上記式（１０）を最小化するような時刻ｔのΔｑｔ，Δλｔ，及びｓｔを計算する。時刻ｔのΔｑｔ及びΔλｔは、次の時刻ｔ＋１におけるｑｔ＋１＝ｑｔ＋Δｑｔ及びλｔ＋１＝λｔ＋Δλｔを計算する際に用いられる。ｓｔは、緩和変数ｓｉを各成分として持つベクトルである。ｑｔ＋１及びλｔ＋１には、作動システム（本実施形態では、ロボット１４の把持部Ｇ）の位置と力とが要素として含まれているため、これに基づいてロボット１４の把持部Ｇの動作が制御される。
　なお、ＬＣＱＰによって計算された値は、次式によって把持部Ｇの位置に関する制御信号ｑ～ ｔ＋１へと変換される。
　ここで、Ｋ＞０はユーザによって定義されるゲインである。上記の変換により、実際にはロボット１４がＬＣＱＰによって計算された力の指示信号を正確に実行できないという事態の発生が抑制される。
　図６は、ＬＣＱＰによって制御される本実施形態の制御システム１０の概略構成を表すブロック図である。図６に示されるように、制御システム１０は、カメラ１２と、ロボット１４と、制御装置１６とを備えている。本実施形態に係る制御装置１６は、物体間（例えば、ロボット１４の把持部Ｇと対象物体Ｂと壁Ｗ及び床面ＦＬとの間）の接触に関する相補性条件が満たされるように、ロボット１４の動作を連続的に制御する。
　カメラ１２は、制御対象であるロボット１４の把持部Ｇと対象物体Ｂとを逐次撮像する。そして、カメラ１２は、ロボット１４の把持部Ｇと対象物体Ｂとを撮像することにより得られた画像データを制御装置１６へ出力する。
　図７は、本実施形態に係る制御装置１６のハードウェア構成を示すブロック図である。図７に示されるように、制御装置１６は、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）４２、メモリ４４、記憶装置４６、入出力Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）４８、記憶媒体読取装置５０、及び通信Ｉ／Ｆ５２を有する。各構成は、バス５４を介して相互に通信可能に接続されている。
　記憶装置４６には、後述する各処理を実行するための制御プログラムが格納されている。ＣＰＵ４２は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ４２は、記憶装置４６からプログラムを読み出し、メモリ４４を作業領域としてプログラムを実行する。ＣＰＵ４２は、記憶装置４６に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。
　メモリ４４は、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）により構成され、作業領域として一時的にプログラム及びデータを記憶する。記憶装置４６は、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、及びＨＤＤ（Ｈａｒｄ　Ｄｉｓｋ　Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）等により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。
　入出力Ｉ／Ｆ４８は、カメラ１２とロボット１４とからのデータの入力、及びカメラ１２とロボット１４とへのデータの出力を行うインタフェースである。また、例えば、キーボードやマウス等の、各種の入力を行うための入力装置、及び、例えば、ディスプレイやプリンタ等の、各種の情報を出力するための出力装置が接続されてもよい。出力装置として、タッチパネルディスプレイを採用することにより、入力装置として機能させてもよい。
　記憶媒体読取装置５０は、ＣＤ（Ｃｏｍｐａｃｔ　Ｄｉｓｃ）－ＲＯＭ、ＤＶＤ（Ｄｉｇｉｔａｌ　Ｖｅｒｓａｔｉｌｅ　Ｄｉｓｃ）－ＲＯＭ、ブルーレイディスク、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ　Ｓｅｒｉａｌ　Ｂｕｓ）メモリ等の各種記憶媒体に記憶されたデータの読み込みや、記憶媒体に対するデータの書き込み等を行う。
　通信Ｉ／Ｆ５２は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。
　次に、制御装置１６の機能構成について説明する。図６に示されるように、制御装置１６は、機能的には、取得部２４と、計算部２６と、制御部２８とを含む。また、制御装置
１６の所定の記憶領域には、データ記憶部１８と、相補性条件記憶部２０と、目的関数記憶部２２とが設けられている。各機能構成は、ＣＰＵ４２が記憶装置４６に記憶された各プログラムを読み出し、メモリ４４に展開して実行することにより実現される。
　データ記憶部１８には、カメラ１２によって撮像された画像データが格納される。相補性条件記憶部２０には、上述した物体間の接触に関する相補性条件のデータが格納されている。目的関数記憶部２２には、上述した目的関数に関するデータが格納されている。
　取得部２４は、制御対象であるロボット１４の把持部Ｇと対象物体Ｂからセンシングされたセンサデータの一例である画像データを逐次取得する。そして、取得部２４は、取得した画像データを、データ記憶部１８へ格納する。
　また、取得部２４は、時刻ｔのロボット１４及び対象物体Ｂの状態ｑｔを取得する。具体的には、取得部２４は、データ記憶部１８に格納されている時刻ｔの画像データを読み出し、時刻ｔの状態ｑｔを表すデータを計算する。なお、取得部２４は、既知の技術を用いて、画像データから状態ｑｔを表すデータを計算する。
　計算部２６は、取得部２４によって計算された時刻ｔの状態ｑｔに基づいて、上述した物体間の接触に関する相補性条件を満たし、かつ時刻ｔ＋１のロボット１４及び対象物体Ｂの状態ｑｔ＋１と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の状態ｑｔ＋１を計算する。なお、この際、計算部２６は、相補性条件記憶部２０に格納されている物体間の接触に関する相補性条件のデータと、目的関数記憶部２２に格納されている目的関数に関するデータとを参照して計算処理を実行する。
　なお、本実施形態では、物体間の接触に関する相補性条件は、ロボット１４の把持部Ｇの接触点、対象物体Ｂの接触点、及び他の物体である壁Ｗ又は床面ＦＬの接触点の何れかの間において発生する接触力と、ロボット１４の把持部Ｇの接触点、対象物体Ｂの接触点、及び壁Ｗ又は床面ＦＬの接触点の何れかの間の距離とに関する相補性条件である。物体間の接触に関する相補性条件は、上述した式（４ａ）～（４ｄ）、式（７ａ）～（７ｄ）、及び式（９ａ）～（９ｄ）の何れかである。
　このため、例えば、計算部２６は、物体間の接触に関する相補性条件式（９ａ）～（９ｄ）を満たし、かつ状態ｑｔを引数とするｇｔとｇｒｅｆとの差を要素として含む上記式（１０）の目的関数が最小となるような状態Δｑｔと接触力Δλｔとのペアを計算する。そして、計算部２６は、状態Δｑｔと接触力Δλｔとのペアに基づいて、時刻ｔ＋１のｑｔ＋１＝ｑｔ＋Δｑｔ及びλｔ＋１＝λｔ＋Δλｔを計算する。
　制御部２８は、計算部２６によって計算された時刻ｔ＋１の状態ｑｔ＋１となるように、制御対象であるロボット１４の把持部Ｇの動作を制御する。例えば、制御部２８は、既知の技術を利用することにより、ロボット１４の把持部Ｇの動作を制御する。
　次に、本実施形態に係る制御システム１０の作用について説明する。
　まず、対象物体Ｂが置かれている環境下において、制御システム１０が所定の指示信号を受け付けると、制御装置１６のＣＰＵ４２は記憶装置４６から制御プログラムを読み出して、メモリ４４に展開して実行する。これにより、ＣＰＵ４２が制御装置１６の各機能構成として機能し、図８に示す制御処理が実行される。なお、図８に示される制御処理は繰り返し実行される。
　なお、以下では、物体間の接触に関する相補性条件として上記式（９ａ）～（９ｄ）を
用い、目的関数として上記式（１０）を用いる場合を例に説明する。このため、相補性条件記憶部２０には、上記式（９ａ）～（９ｄ）に関するデータが格納されている。また、目的関数記憶部２２には、上記式（１０）に関するデータが格納されている。
　ステップＳ１００において、取得部２４は、データ記憶部１８に格納されている時刻ｔの画像データを読み出し、時刻ｔの状態ｑｔを表すデータを取得する。
　ステップＳ１０２において、計算部２６は、相補性条件記憶部２０から上記式（９ａ）～（９ｄ）に関するデータを読み出す。また、ステップＳ１０２において、計算部２６は、目的関数記憶部２２から上記式（１０）に関するデータを読み出す。そして、ステップＳ１０２において、ステップＳ１００で取得された時刻ｔの状態ｑｔに基づいて、上記式（９ａ）～（９ｄ）の物体間の接触に関する相補性条件を満たし、かつ時刻ｔ＋１のロボット１４及び対象物体Ｂの状態ｑｔ＋１と目標値との差を表す上記式（１０）の目的関数が最小となるような時刻ｔ＋１の状態ｑｔ＋１を計算する。
　ステップＳ１０４において、制御部２８は、ステップＳ１０２で計算された時刻ｔ＋１の状態ｑｔ＋１となるように、制御対象であるロボット１４の把持部Ｇの動作を制御する。
　図８に示される制御処理が繰り返され、ロボット１４に対して制御信号が繰り返し出力されることにより、対象物体Ｂが所望の位置及び姿勢となる。
　以上説明したように、本実施形態に係る制御システムの制御装置は、時刻ｔの制御対象の状態を取得し、時刻ｔの前記状態に基づいて、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の状態を計算する。そして、制御装置は、計算された時刻ｔ＋１の状態となるように、制御対象を制御する。これにより、制御対象の接触点において発生する接触力と制御対象における接触点間の距離とに関する相補性条件が満たされるように、制御対象を制御することができる。
　また、本実施形態の制御装置によれば、制御対象であるロボットを連続的に制御することができる。
　また、本実施形態では、上記式（４ａ）～（４ｄ）の相補性条件を線形化し、上記式（７ａ）～（７ｄ）の線形相補性条件を生成する。これにより、相補性条件の演算に要する計算コストを低減させることができ、ロボット１４の把持部Ｇと対象物体Ｂの状態に応じたロボットの動作命令をオンラインで演算することができる。これにより、例えば、ロボット１４の把持部Ｇの先端で対象物体Ｂを押して（把持せずに）立ち上げるという動作を実現させることができる。
　また、本実施形態では、上記式（７ａ）～（７ｄ）の線形相補性条件に対して緩和変数ｓｉを導入する。これにより、現実世界における物体間の接触とセンサデータによって検知された物体間の接触とに齟齬があったとしても、物体間の接触に関する相補性条件を満たしているか否かを計算することが可能となり、ロボット１４の把持部Ｇの動作を適切に制御することができる。
［実施例］
　次に、実施例について説明する。本実例では、提案したＬＣＱＰの有効性を検証するため、PyBullet 3D physics simulator（E. Coumans and Y. Bai, “Pybullet, a python m
odule for physics simulation for games, robotics and machine learning,” 2016-2021. [Online]. Available: http://pybullet.org）を利用したシミュレーションを行う。
　図９は、本実施例を説明するための図である。図９に示されるように、本実施例では、ロボットの把持部Ｇの先端を、横たわっている対象物体Ｂであるボックスに接触させ、ボックスを立たせるようなシミュレーションを行った。なお、本実施例では、上記式（７ａ）～（７ｄ）に緩和変数を導入した相補性条件を利用する。
　図１０，図１２は、本実施例の結果を表す図である。図１１は、本実施例の結果を説明するための図である。
　図１０に示されるように、ロボットの把持部Ｇが対象物体Ｂであるボックスに触れ、対象物体Ｂを立たせることができていることがわかる。また、図１２は、図１１に示す各接触点に関連するパラメータを各時刻でモニタリングした際の結果を表す図である。図１２（１）～（５）に示されるように、接触点ｐ１～ｐ４の各パラメータにおいて、接触に関する相補性条件が満たされていることがわかる。例えば、図１０及び図１１に示されているように、接触点ｐ３は（ａ）～（ｂ）の間において壁面Ｗと接触する。図１２の（１）を参照すると、（ａ）～（ｂ）の間においてλｎ及びλｆが発生する。その一方で、図１２の（１）の（ａ）～（ｂ）の間においては、Φ及びψはゼロに近い値となる。このように、図１２（１）～（５）に示されているように、接触点ｐ１～ｐ４の各パラメータにおいて、接触に関する相補性条件が満たされていることがわかる。
　なお、上記実施形態では、本開示のＬＣＱＰをロボット制御に適用した例について説明したが、これに限定されるものではない。ＬＣＱＰはパラメータの多い制御系においてリアルタイムでの最適化が求められる対象に応用可能と考えられる。例えば、接触状態の切り替わる物体操作の他、脚式ロボット、又は車輪ロボットの移動に応用することが可能である。接触状態の切り替わる物体操作を行うロボットは、物体の持ち替えの省略、狭隘環境でのピックアンドプレイス等が必要な場面も多いため、ＬＣＱＰを適用することが可能である。また、実験自動化、部品組立など、自律してマニピュレータが作業を実行する場合にもＬＣＱＰを適用させることができる。また、車輪ロボットは、壁又は床面等と接触して走行するため、壁又は床面との間で摩擦力が発生するため、接触に関する相補性条件を利用するＬＣＱＰを適用させることが可能である。
　また、上記実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した各処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ　Ｌｏｇｉｃ　Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、各処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。
　また、上記実施形態では、各プログラムが記憶装置に予め記憶（例えば、インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、ブルーレイディスク、ＵＳＢメモリ等の記憶媒体に記憶された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。
　２０２２年１０月２０日に出願された日本国特許出願２０２２－１６８７１３号の開示は、その全体が参照により本明細書に取り込まれる。本明細書に記載された全ての文献、特許出願、および技術規格は、個々の文献、特許出願、および技術規格が参照により取り込まれることが具体的かつ個々に記された場合と同程度に、本明細書中に参照により取り込まれる。

Claims (11)

  1. 　時刻ｔの制御対象の状態を取得する取得部と、
    　時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算する計算部と、
    　計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する制御部と、
    　を含む制御装置。
  2. 　前記計算部は、時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件、及び前記接触点が接する接触面における接線接触速度に関する相補条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算する、
    　請求項１に記載の制御装置。
  3. 　前記制御対象は、ロボット及び対象物体であり、
    　前記状態のデータは、前記ロボット及び前記対象物体の状態をセンシングするセンサによって得られたセンサデータに基づき計算され、
    　前記計算部は、時刻ｔの前記状態に基づいて、前記ロボットの接触点、前記対象物体の接触点、及び他の物体の接触点の何れかの間において発生する接触力と、前記ロボットの接触点、前記対象物体の接触点、及び前記他の物体の接触点の何れかの間の距離とに関する前記相補性条件が満たされるように、時刻ｔ＋１の前記状態を計算する、
    　請求項１又は請求項２に記載の制御装置。
  4. 　前記相補性条件は、
    　複数の前記接触点の各々についての、
    　時刻ｔの状態ｑｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φｉ（ｑｔ）とに関する以下の相補性条件（４ａ）と、
    　時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における、第１水平方向の接触力λｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、前記接線接触速度の絶対値γｉとに関する以下の相補性条件（４ｂ）と、
    　時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における、第２水平方向の接触力λｆｉ－と、前記符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）及び前記接線接触速度の絶対値γｉとに関する以下の相補性条件（４ｃ）と、
    　ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γｉと、前記接触面の摩擦係数μｉと、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第２水平方向の接触力λｆｉ－とに関する以下の相補性条件（４ｄ）とを含んで構成されている、
    　請求項３に記載の制御装置。
  5. 　前記計算部は、時刻ｔの前記状態ｑｔに基づいて、前記相補性条件（４ａ）～（４ｄ）を満たし、かつ以下の目的関数（５）が最小となるような時刻ｔ＋１の前記状態ｑｔ＋１を計算する、
    　請求項４に記載の制御装置。

     
    　ただし、ｇ（ｑｔ＋１）は前記状態ｑｔ＋１に対応する前記制御対象の物理量を表し、ｇｒｅｆは前記状態ｑｔ＋１の目標値に対応する前記制御対象の物理量の目標値を表し、Ｑｇ，Ｑｖ，及びＱｆは予め与えられる正定重み行列であり、λｔ＋１は、複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第２水平方向の接触力λｆｉ－とを各成分として持つ時刻ｔ＋１のベクトルである。
  6. 　前記相補性条件は、
    　複数の前記接触点の各々についての、
    　時刻ｔの状態ｑｔにおける、ｉ番目の前記接触点が接する接触面の鉛直方向に発生する接触力λｎｉと、前記接触力λｎｉの変化量Δλｎｉと、ｉ番目の前記接触点と前記接触面との間の前記距離φｉ（ｑｔ）と、前記状態ｑｔにより前記距離φｉ（ｑｔ）を微分することにより得られるΦｉ，ｑ（ｑｔ）と、前記状態ｑｔの変化量Δｑｔとに関する以下の線形相補性条件（７ａ）と、
    　時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における第１水平方向の接触力λｆｉ＋と、前記接触力λｆｉ＋の変化量Δλｆｉ＋と、前記接触面の前記第１水平方向を正とする符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、前記一般化速度ｖｔにより前記接線接触速度ψｉ（ｑｔ，ｖｔ）を微分することにより得られるΨｉ，ｖ（ｑｔ，ｖｔ）と、前記接線接触速度の絶対値γｉと、前記状態ｑｔの変化量Δｑｔとに関する以下の線形相補性条件（７ｂ）と、
    　時刻ｔの前記状態ｑｔ及び時刻ｔの一般化速度ｖｔにおける、ｉ番目の前記接触点が接する前記接触面における第２水平方向の接触力λｆｉ－と、前記接触力λｆｉーの変化量Δλｆｉーと、前記符号付きの接線接触速度ψｉ（ｑｔ，ｖｔ）と、前記接線接触速度の絶対値γｉと、前記一般化速度ｖｔにより前記接線接触速度ψｉ（ｑｔ，ｖｔ）を微分す
    ることにより得られるΨｉ，ｖ（ｑｔ，ｖｔ）と、に関する以下の線形相補性条件（７ｃ）と、
    　ｉ番目の前記接触点が接する前記接触面における、前記接線接触速度の絶対値γｉと、前記接触面の摩擦係数μｉと、前記接触面の鉛直方向に発生する接触力λｎｉと、前記接触力λｎｉの変化量Δλｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第１水平方向の接触力λｆｉ＋の変化量Δλｆｉ＋と、前記第２水平方向の接触力λｆｉ－と、前記第２水平方向の接触力λｆｉ－の変化量Δλｆｉ－と、に関する以下の線形相補性条件（７ｄ）とを含んで構成されている、
    　請求項３に記載の制御装置。

     
  7. 　前記計算部は、時刻ｔの前記状態ｑｔに基づいて、前記相補性条件（７ａ）～（７ｄ）を満たし、かつ以下の目的関数（６）が最小となるような前記状態の変化量Δｑｔを計算することにより、時刻ｔ＋１の前記状態ｑｔ＋１を計算する、
    　請求項６に記載の制御装置。

     

     

     


     
    　ただし、ｇｔは前記状態ｑｔに対応する前記制御対象の物理量を表し、ｇｒｅｆは前記状態ｑｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑｇ，Ｑｖ，及びＱｆ予め与えられる正定重み行列であり、λｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第２水平方向の接触力λｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλｔは前記λｔの変化量である。
  8. 　前記相補性条件には、前記相補性条件を緩和するための以下の緩和変数ｓｉが含まれている、
    　請求項５に記載の制御装置。

     
  9. 　前記計算部は、時刻ｔの前記状態ｑｔに基づいて、前記相補性条件（９ａ）～（９ｄ）を満たし、かつ以下の目的関数（１０）が最小となるような前記状態の変化量Δｑｔと前記接触力の変化量Δλｔと緩和変数ｓｉを各成分として持つベクトルｓｔとを計算することにより、時刻ｔ＋１の前記状態ｑｔ＋１を計算する、
    　請求項８に記載の制御装置。


     

     

     

     
    　ただし、ｇｔは前記状態ｑｔに対応する前記制御対象の物理量を表し、ｇｒｅｆは前記状態ｑｔの目標値に対応する前記制御対象の物理量の目標値を表し、Ｑｇ，Ｑｖ，及びＱｆ予め与えられる正定重み行列であり、λｔは複数の前記接触点の各々についての、前記接触面の鉛直方向に発生する接触力λｎｉと、前記第１水平方向の接触力λｆｉ＋と、前記第２水平方向の接触力λｆｉ－とを各成分として持つ時刻ｔのベクトルであり、Δλｔは前記λｔの変化量である。
  10. 　時刻ｔの制御対象の状態を取得し、
    　時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、
    　計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、
    　処理をコンピュータが実行する制御方法。
  11. 　時刻ｔの制御対象の状態を取得し、
    　時刻ｔの前記状態に基づいて、前記制御対象の接触点において発生する接触力と前記制御対象における接触点間の距離とに関する相補性条件を満たし、かつ時刻ｔ＋１の前記制御対象の状態と目標値との差を表す目的関数が最小となるような時刻ｔ＋１の前記状態を計算し、
    　計算された時刻ｔ＋１の前記状態となるように、前記制御対象を制御する、
    　処理をコンピュータに実行させるための制御プログラム。
PCT/JP2023/037119 2022-10-20 2023-10-12 制御装置、制御方法、及び制御プログラム Ceased WO2024085072A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US19/115,604 US20260131459A1 (en) 2022-10-20 2023-10-12 Control device, control method, and control program

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022168713A JP2024061046A (ja) 2022-10-20 2022-10-20 制御装置、制御方法、及び制御プログラム
JP2022-168713 2022-10-20

Publications (1)

Publication Number Publication Date
WO2024085072A1 true WO2024085072A1 (ja) 2024-04-25

Family

ID=90737588

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2023/037119 Ceased WO2024085072A1 (ja) 2022-10-20 2023-10-12 制御装置、制御方法、及び制御プログラム

Country Status (3)

Country Link
US (1) US20260131459A1 (ja)
JP (1) JP2024061046A (ja)
WO (1) WO2024085072A1 (ja)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06138930A (ja) * 1992-10-30 1994-05-20 Nippon Telegr & Teleph Corp <Ntt> 加工・組立装置の倣い制御方法および倣い制御装置
JP2009099082A (ja) * 2007-10-19 2009-05-07 Sony Corp 力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラム
CN113059561A (zh) * 2021-03-12 2021-07-02 华中科技大学 吸附机器人的控制方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US12397425B2 (en) * 2022-03-18 2025-08-26 Mitsubishi Electric Research Laboratories, Inc. System and method for robust pivoting for re-orienting parts during robotic assembly
US12097619B2 (en) * 2022-09-26 2024-09-24 Fanuc Corporation Predictive control method for torque-rate control and vibration suppression

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06138930A (ja) * 1992-10-30 1994-05-20 Nippon Telegr & Teleph Corp <Ntt> 加工・組立装置の倣い制御方法および倣い制御装置
JP2009099082A (ja) * 2007-10-19 2009-05-07 Sony Corp 力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラム
CN113059561A (zh) * 2021-03-12 2021-07-02 华中科技大学 吸附机器人的控制方法

Also Published As

Publication number Publication date
US20260131459A1 (en) 2026-05-14
JP2024061046A (ja) 2024-05-07

Similar Documents

Publication Publication Date Title
Kulakov et al. Methods of remote control over space robots
Odhner et al. The smooth curvature model: An efficient representation of Euler–Bernoulli flexures as robot joints
JP6007636B2 (ja) ロボット制御システム及びロボット制御装置
CN107548336B (zh) 控制和/或调节机器人的电机
US8386213B2 (en) Device, method and computer program for simulation of collision events between physical objects
JP2022076024A (ja) ロボットハンド、ロボットハンド制御方法及びプログラム
Moisio et al. Model of tactile sensors using soft contacts and its application in robot grasping simulation
JP6003312B2 (ja) ロボットシステム
Wirnshofer et al. State estimation in contact-rich manipulation
Halm et al. A quasi-static model and simulation approach for pushing, grasping, and jamming
Han et al. Static model analysis and identification for serial articulated manipulators
Li et al. Stable grasp planning based on minimum force for dexterous hands
Zapolsky et al. Inverse dynamics with rigid contact and friction
Smith et al. Application of neural networks in inverse dynamics based contact force estimation
Han et al. Quasi-static modeling and controlling for planar pushing of deformable objects
Doulgeri et al. A position/force control for a robot finger with soft tip and uncertain kinematics
Nguyen et al. FIGAROH: a Python toolbox for dynamic identification and geometric calibration of robots and humans
Moisio et al. Simulation of tactile sensors using soft contacts for robot grasping applications
Galaiya et al. Grasp approach under positional uncertainty using compliant tactile sensing modules and reinforcement learning
JP2024061046A (ja) 制御装置、制御方法、及び制御プログラム
Pang et al. A robust time-stepping scheme for quasistatic rigid multibody systems
Cetin et al. Cooperative control of a human and a robot manipulator for positioning a cart on a frictionless plane
Ueki et al. Adaptive coordinated control of multi-fingered robot hand
Kulakov Active force-torque robot control without using wrist force-torque sensors
Al Mashagbeh et al. Virtual performance evaluation of an industrial SCARA robot prior to real-world task

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23879715

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 23879715

Country of ref document: EP

Kind code of ref document: A1