# US 2024/0316766 A1 · System and Method for Controlling an Operation of a Manipulation System

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2024/0316766 A1
- Document ID: US20240316766A1
- Status: Published application
- Kind: A1
- Application number: 18/188,217
- Filed: 2023-03-22
- Published: 2024-09-26
- Earliest claimed: 2023-03-22
- Assignees: Mitsubishi Electric Research Laboratories, Inc.
- Applicants: Mitsubishi Electric Research Laboratories, Inc.
- Inventors: Devesh Jha; Arvind Raghunathan; Yuki Shirai
- Web page: https://patentgrep.com/patent/US20240316766A1

## CPC

- B25J 9/1664: {characterised by motion, path, trajectory planning}
- B25J 9/1605: {Simulation of manipulator lay-out, design, modelling of manipulator}
- B25J 9/1628: {characterised by the control loop}
- G05B 2219/42065: Feedforward combined with pid feedback
- B25J: MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES ({manipulators specially adapted for use in surgery A61B34/70; manipulators used in cleaning hollow articles B08B9/04}; manipulators associated with rolling mills B21B39/20; manipulators associated with forging machines B21J13/10; {manipulators associated with picking-up and placing mechanisms B23P19/007}; means for holding wheels or parts thereof B60B30/00; {vehicles with ground-engaging propulsion means, e.g. walking members B62D57/02, B62D57/032; devices for picking-up and depositing articles or materials between conveyors B65G47/90, B65G47/91; manipulators with gripping or holding means for transferring packages B65H67/065}; cranes B66C; {manipulators used in the protection or supervision of pipe-line installations F17D5/00; walking equipment adapted for nuclear steam-generators F22B37/006}; manipulators specially adapted for, or associated with, nuclear reactors G21C; {apparatus used for handling wafers during manufacture or treatment of semiconductor H10P72/50})
- G05B: CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS (systems for controlling or regulating non-electric variables G05D; systems for regulating electric or magnetic variables G05F; control devices or systems insofar as characterised by mechanical features only G05G)

## Abstract

The present disclosure provides a system and a method for controlling an operation of a manipulation system. The method comprises formulating an optimization problem based on a Stochastic Discrete-time Linear Complementarity Model (SDLCM) of a manipulation task, and a sample average approximation; solving the formulated optimization problem using an important-particle algorithm to compute an optimal state trajectory, an optimal feedforward control trajectory, an optimal complementarity variable trajectory, a state feedback gain, and a complementarity feedback gain; collecting, measurements indicative of a current state trajectory and a current complementarity variable trajectory; determining an online control input based on the optimal feedforward control trajectory, a deviation of the current state trajectory from the optimal state trajectory, a deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory, the state feedback gain, and the complementarity feedback gain; and controlling actuators of the manipulation system according to the online control input.

## Claims

### Claim 1

A control system for controlling an operation of a manipulation system, wherein the manipulation system includes a robotic arm for performing a manipulation task of manipulating an object, the control system comprising: at least one processor; and a memory having instructions stored thereon that cause the at least one processor to:
formulate a particle based optimization problem for covariance control of the manipulation system, based on a Stochastic Discrete-time Linear Complementarity Model (SDLCM) of the manipulation task, and a sample average approximation;
solve the formulated particle based optimization problem using an important-particle algorithm to compute an optimal state trajectory, an optimal feedforward control trajectory, an optimal complementarity variable trajectory, a state feedback gain, and a complementarity feedback gain;
collect, from one or more sensors associated with the manipulation system, measurements indicative of a current state trajectory and a current complementarity variable trajectory;
compute a deviation of the current state trajectory from the optimal state trajectory and a deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory;
determine an online control input based on the optimal feedforward control trajectory and a feedback control to control a covariance of states and complementarity variables of the manipulation system, wherein the feedback control is based on the deviation of the current state trajectory from the optimal state trajectory, the deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory, the state feedback gain, and the complementarity feedback gain;
produce control commands for actuators of the robotic arm based on the determined online control input; and
operate the actuators of the robotic arm according to the determined online control input by transmitting control signals of the control commands to the robotic arm.

### Claim 2 (depends on 1)

The control system of claim 1, wherein the particle based optimization problem corresponds to a chance constrained optimization problem subject to stochastic complementarity constraints and a chance constraint.

### Claim 3 (depends on 2)

The control system of claim 2, wherein the chance constraint is approximated using the sample average approximation.

### Claim 4 (depends on 2)

The control system of claim 2, wherein the important-particle algorithm is iteratively executed until a termination condition is met, and wherein, to execute an iteration, the processor is further configured to:
solve the particle based optimization problem with a number of particles to determine a controller;
execute, based on the controller, Monte-Carlo simulation for each test particle of a number of test particles sampled from an uncertainty distribution, to determine particles that violate the chance constraint;
select, from the determined particles, a number of worst particles; and
add the number of worst particles to the number of particles.

### Claim 5 (depends on 1)

The control system of claim 1, wherein the current state trajectory includes a current pose of the object, and wherein the current complementarity variable trajectory includes a current contact force between the robotic arm and the object.

### Claim 6 (depends on 1)

The control system of claim 1, wherein the state of the manipulation system corresponds to a pose of the object, and wherein the complementarity variables of the manipulation system correspond to a contact force between the robotic arm and the object.

### Claim 7 (depends on 1)

The control system of claim 1, wherein the manipulation task corresponds a pushing the object, reorientation of the object, gripping the object, or handling the object to assemble the object for manufacturing a product using a combination of pushing, reorienting, and gripping the object.

### Claim 8 (depends on 1)

The control system of claim 1, wherein the one or more sensors associated with the manipulation system correspond to tactile sensors, force sensors, or torque sensors.

### Claim 9

A method for controlling an operation of a manipulation system, wherein the manipulation system includes a robotic arm for performing a manipulation task of manipulating an object, the method comprising:
formulating a particle based optimization problem for covariance control of the manipulation system, based on a Stochastic Discrete-time Linear Complementarity Model (SDLCM) of the manipulation task, and a sample average approximation;
solving the formulated particle based optimization problem using an important-particle algorithm to compute an optimal state trajectory, an optimal feedforward control trajectory, an optimal complementarity variable trajectory, a state feedback gain, and a complementarity feedback gain;
collecting, from one or more sensors associated with the manipulation system, measurements indicative of a current state trajectory and a current complementarity variable trajectory;
computing a deviation of the current state trajectory from the optimal state trajectory and a deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory;
determining an online control input based on the optimal feedforward control trajectory and a feedback control to control a covariance of states and complementarity variables of the manipulation system, wherein the feedback control is based on the deviation of the current state trajectory from the optimal state trajectory, the deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory, the state feedback gain, and the complementarity feedback gain; and
producing control commands for actuators of the robotic arm based on the determined online control input; and
operating the actuators of the robotic arm according to the determined online control input by transmitting control signals of the control commands to the robotic arm.

### Claim 10 (depends on 9)

The method of claim 9, wherein the particle based optimization problem corresponds to a chance constrained optimization problem subject to stochastic complementarity constraints and a chance constraint.

### Claim 11 (depends on 10)

The method of claim 10, wherein the chance constraint is approximated using the sample average approximation.

### Claim 12 (depends on 10)

The method system of claim 10, wherein the important-particle algorithm is iteratively executed until a termination condition is met, and wherein execution of an iteration comprises:
solving the particle based optimization problem with a number of particles to determine a controller;
executing, based on the controller, Monte-Carlo simulation for each test particle of a number of test particles sampled from an uncertainty distribution, to determine particles that violate the chance constraint;
selecting, from the determined particles, a number of worst particles; and
adding the number of worst particles to the number of particles.

### Claim 13 (depends on 9)

The method of claim 9, wherein the current state trajectory includes a current pose of the object, and wherein the current complementarity variable trajectory includes a current contact force between the robotic arm and the object.

### Claim 14 (depends on 9)

The method of claim 9, wherein the state of the manipulation system corresponds to a pose of the object, and wherein the complementarity variables of the manipulation system correspond to a contact force between the robotic arm and the object.

### Claim 15 (depends on 9)

The method of claim 9, wherein the manipulation task corresponds to one of a pushing the object, and reorientation of the object.

### Claim 16 (depends on 9)

The method of claim 9, wherein the one or more sensors associated with the manipulation system correspond to tactile sensors, force sensors, or torque sensors.

### Claim 17

A non-transitory computer-readable storage medium embodied thereon a program executable by a processor for performing a method for controlling an operation of a manipulation system, wherein the manipulation system includes a robotic arm for performing a manipulation task of manipulating an object, the method comprising:
formulating a particle based optimization problem for covariance control of the manipulation system, based on a Stochastic Discrete-time Linear Complementarity Model (SDLCM) of the manipulation task, and a sample average approximation;
solving the formulated particle based optimization problem using an important-particle algorithm to compute an optimal state trajectory, an optimal feedforward control trajectory, an optimal complementarity variable trajectory, a state feedback gain, and a complementarity feedback gain;
collecting, from one or more sensors associated with the manipulation system, measurements indicative of a current state trajectory and a current complementarity variable trajectory;
computing a deviation of the current state trajectory from the optimal state trajectory and a deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory;
determining an online control input based on the optimal feedforward control trajectory and a feedback control to control a covariance of states and complementarity variables of the manipulation system, wherein the feedback control is based on the deviation of the current state trajectory from the optimal state trajectory, the deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory, the state feedback gain, and the complementarity feedback gain; and
producing control commands for actuators of the robotic arm based on the determined online control input; and
operating the actuators of the robotic arm according to the determined online control input by transmitting control signals of the control commands to the robotic arm.

### Claim 18 (depends on 17)

The non-transitory computer-readable storage medium of claim 17, wherein the particle based optimization problem corresponds to a chance constrained optimization problem subject to stochastic complementarity constraints and a chance constraint.

### Claim 19 (depends on 18)

The non-transitory computer-readable storage medium of claim 18, wherein the chance constraint is approximated using the sample average approximation.

### Claim 20

A control system for controlling an operation of a manipulation system, wherein the manipulation system includes a robotic arm for performing a manipulation task of manipulating an object, the control system comprising: at least one processor; and a memory having instructions stored thereon that cause the at least one processor to:
collect, from one or more sensors associated with the manipulation system, measurements indicative of a current state trajectory and a current complementarity variable trajectory;
compute a deviation of the current state trajectory from an optimal state trajectory and a deviation of the current complementarity variable trajectory from an optimal complementarity variable trajectory;
wherein a particle based optimization problem is solved using an important-particle algorithm to obtain the optimal state trajectory, the optimal complementarity variable trajectory, an optimal feedforward control trajectory, a state feedback gain, and a complementarity feedback gain, and wherein the particle based optimization problem is based on a Stochastic Discrete-time Linear Complementarity Model (SDLCM) of the manipulation task and a sample average approximation;
determine an online control input based on the optimal feedforward control trajectory and a feedback control to control a covariance of states and complementarity variables of the manipulation system, wherein the feedback control is based on the deviation of the current state trajectory from the optimal state trajectory, the deviation of the current complementarity variable trajectory from the optimal complementarity variable trajectory, the state feedback gain, and the complementarity feedback gain; and
produce control commands for actuators of the robotic arm based on the determined online control input; and
operate the actuators of the robotic arm according to the determined online control input by transmitting control signals of the control commands to the robotic arm.
