JP2019198948A - Contact mode estimation device - Google Patents

Abstract

To enable a contact mode to be estimated in real time so that an autonomous robot such as a satellite can reliably capture a moving free airborne object by using only a force sensor.SOLUTION: So as to capture an approaching object, a contact mode estimation device detects that the object collides with a robot hand, on the basis of a signal output from a force sensor, and performs an update of a particle state and evaluation of particles by executing a particle filter in response to the collision of the object with the object. Thus, the contact mode estimation device determines the timing when the object can be captured by closing a holding part of the robot hand.SELECTED DRAWING: Figure 7

Description

  The present invention relates to a contact mode estimation device, and more specifically, a contact mode estimation device that estimates a contact mode when an approaching object comes into contact with a robot hand and determines a timing for closing the robot hand to capture the object. About.

  Space debris, that is, space debris such as abandoned satellites, has become a problem, but there are cases where such space debris needs to be captured in space. In such a case, a robot arm equipped with a robot hand (gripper) is mounted on a chaser satellite or the like, thereby causing the chaser satellite to approach the space debris to be captured and operating the robot arm. After introducing a part of the space debris inside, the robot hand is closed to capture the space debris. At this time, it is necessary to determine the timing so that the robot hand is closed when the space debris comes to a position where it can be reliably captured.

  As a conventional technique, there is one that uses a contact sensor to determine that an object has contacted a robot hand (Non-Patent Document 1). In this technique, a contact sensor is divided into a plurality of places and attached to the inside of the robot hand, thereby sensing where the target object has come into contact. When contact is detected, estimation of the position of the target object is started by the particle filter. However, it is not realistic to arrange the contact sensor in every part including the movable part of the robot hand that may collide with the target object. Thus, there was a method using a plurality of contact sensors, but there was a problem in practical use.

Bayesian Estimation for Autonomous Object Manipulation Based on Tactile Sensors_Petrovskaya

  To remove space debris and repair broken satellites, it is necessary to capture free-flying objects. In particular, in a dynamically changing environment where the captured robot hand and the captured target object move relative to each other, the robot hand is instructed to close at an appropriate timing, and the relative relationship between the robot hand and the target object is determined. It is necessary to create physical constraints to make the speed zero. If this is not possible, the robot hand may not be able to capture the target object, for example, by flipping the target object.

  In order to overcome such problems, it is necessary to construct an appropriate control method including contact mode estimation. However, such contact mode estimation often has to be performed in a situation where an accurate collision prediction model cannot be performed in real time. Further, when a target object that is a moving rigid body is captured, an elastic collision or slip occurs between the target object and the robot hand. Here, the speed of the result of the collision of the rigid bodies is affected by the surface conditions including local tangential direction and material properties, and the result of the collision is often not stable. As a result, a prediction error usually occurs.

  For this purpose, the present invention proposes a real-time collision-based contact mode estimation using only a force sensor for capturing a moving rigid body. In order to achieve this, the present invention uses segmented collision positions in the robot hand as well as the mechanical properties in contact as the contact mode.

  Here, as a probabilistic approach, it is conceivable to use a particle filter for contact mode estimation. For the prediction, we adopt a collision model that can calculate and sample a rigid body (generate multiple samples). Although different collisions are simulated for each particle, the posterior distribution model is updated primarily according to the collision timing detected by the force sensor, with the most fundamental measurement during the capture operation. It is known that the particle filter can reduce the prediction error, but requires a large calculation resource that may impair the real-time property on the airframe.

  The present invention has been made to cope with the above-mentioned problem. Regarding the computational resource problem, a particle filter with a collision trigger filter is used to estimate a contact mode necessary for closing an object. In order to trigger the start of particle filter calculation by collision, detailed information can be provided using only a force sensor, and triggering calculation by collision contributes to a reduction in calculation amount. According to the present invention, an autonomous robot such as a satellite can estimate a contact mode in real time in order to reliably capture a moving free-flying object using only a force sensor.

  The above-described problems are solved by the present invention having the following features. That is, the present invention is a contact mode estimation device for determining the timing for closing a grip portion of a robot hand in order to capture an approaching object, and outputs a signal representing a force applied to the robot hand. It has a sensor, detects that an object has collided with the robot hand based on a signal output from the force sensor, generates a plurality of particle data for the particle filter, and determines the position and contact mode of each particle. In response to simulating and detecting the collision of the object with the robot hand, the particle is executed to update the particle state and evaluate the particle, thereby capturing the object by closing the gripping part of the robot hand. Determine possible timing. In addition, the present invention performs weighting for each particle based on the time difference from the actual collision time of the object by executing a particle filter when the object can be captured by closing the grip part of the robot hand. Each particle is transferred to the collision position, the contact mode with the robot hand is determined, the state of the particle is updated by re-sampling after normalization, and the contact mode corresponding to the closeable region is experienced. The simulation and the updating of the particle state are repeated until the total particle weight is equal to or greater than a predetermined value, and when the total particle weight is equal to or greater than the predetermined value, the timing of closing the grip portion of the robot hand is evaluated. It is characterized by.

  The present invention further includes a sensor for capturing the position of the object before the collision, estimates the position and motion of the object based on information obtained from the sensor, and generates a plurality of particles based on the estimated value. You may comprise as follows. Further, the present invention may be configured to use a camera as a sensor and estimate the position and motion of an object based on an image of the object obtained from the camera. Further, the present invention detects that an object is slipping on the robot hand based on a signal output from the force sensor, and in the simulation, when particles colliding with the robot hand move along the surface of the robot hand. You may comprise so that it may simulate as what is sliding on the robot hand. In the present invention, the robot hand has a robot arm attached to the tip, and impedance control is performed to operate the robot arm so as to absorb the impact of the collision when the collision of the object with the robot hand is detected. It may be configured.

  In the present invention, when the surface of the robot hand is defined as being composed of a plurality of segments, the contact mode indicates which segment the particle contacts with, and corresponds to a closeable region that can be captured by the robot hand. The contact mode may be configured to correspond to a segment facing the closeable region of the robot hand. Moreover, this invention can be grasped | ascertained as the contact mode estimation method which consists of a step implemented by each means of a contact mode estimation apparatus. In addition, the present invention can be understood as a program for causing a computer to implement the contact mode estimation device and a program for causing the computer to execute the contact mode estimation method, and as a computer-readable recording medium storing such a program. You can also

  In order to capture an approaching object, the present invention detects that the object has collided with the robot hand based on a signal output from the force sensor, and in response to detecting the collision of the object with the robot hand. Since it has a configuration in which the particle filter is executed to update the particle state and the particle is evaluated to determine when the object can be captured by closing the grip part of the robot hand, the output from the force sensor Based on this, it is possible to estimate the timing for closing the robot hand in real time. Further, the present invention obtains a weight based on the time difference from the actual collision time of the particle object by executing a particle filter when determining the timing at which the object can be captured by closing the grip part of the robot hand. , Each particle is transferred to the collision position, the contact mode with the robot hand is determined, the particle state is updated by re-sampling after normalization, and the contact corresponding to the closable area that can be captured by the robot hand Timing of closing the robot hand grip when the total particle weight exceeds the specified value by repeating the simulation and particle state update process until the total weight of the particles experiencing the mode exceeds the specified value When using a particle filter with a reduced calculation amount, the object is surely placed in the robot hand. Thus an effect that it is possible to estimate the timing of captured in real time.

  Further, the present invention estimates the position and motion of an object based on information obtained from a sensor for capturing the position of the object before the collision, and generates a plurality of particles based on the estimated value. Particles are generated on the basis of the actual state immediately before the collision, and accurate estimation can be performed. Further, according to the present invention, when the camera is used as the sensor, the position of the object can be estimated easily and with high accuracy by the camera and the image processing apparatus. Further, the present invention detects that an object is slipping on the robot hand based on a signal output from the force sensor, and in the simulation, when particles colliding with the robot hand move along the surface of the robot hand. If the simulation is performed on the assumption that the robot is slipping on the robot hand, it is possible to perform an accurate simulation with the particle filter when the object slips as well as the elastic collision. Slip is likely to occur when impedance control is performed, and a trigger for update processing by detecting slip is particularly suitable for impedance control. Further, according to the present invention, when the robot hand has a robot arm attached to the tip and is configured to perform impedance control for operating the robot arm so as to absorb the impact of the collision, the object can be reliably obtained by absorbing the impact. It has an effect that an object can be captured.

  The present invention also divides the surface of the robot hand into a plurality of segments, the contact mode indicates which segment the particle contacts, and the contact mode corresponding to the closeable region corresponds to the segment facing the closeable region. If it is configured to be a thing, it has an effect that it can be reliably estimated that the object is in the closeable region by estimating the contact mode.

It is a block diagram which shows the structure of the contact mode estimation apparatus 100 which concerns on embodiment of this invention. It is a figure which shows the structure of the robot hand 200 connected to the contact mode estimation apparatus 100 which concerns on embodiment of this invention. It is a figure showing the relationship between the position segment of a robot hand, and a contact mode. It is a figure which shows the contact mode estimation algorithm based on the particle | grain filter using a collision trigger filter. It is a figure which shows the dynamic relationship at the time of the collision of a rigid body. It is a figure showing the contact mode corresponding to the segment of a robot hand. It is a flowchart which shows operation | movement of a contact mode estimation apparatus. It is a figure showing a series of steps until object capture. It is a figure explaining the behavior at the time of an object collision (including slip at the time of impedance control). It is a figure explaining the behavior (elastic collision and slip) at the time of collision of an object. It is a figure which shows the prediction algorithm which enters slip mode. It is a figure which shows the example of the output of the force sensor of a normal direction. It is a figure which shows the example (including the collision in a slip) of the output of the force sensor of a normal direction. It is a figure which shows the estimation algorithm of all the contact modes which can cope under impedance control. It is a figure which shows a coordinate system. It is a figure showing the noise added to an initial particle. It is a figure which shows the sampling profile of the collision characteristic in the case of no impedance control. It is a figure showing the basic performance of CCME obtained by experiment in the case of no impedance control. It is a figure showing transition of contact mode estimation. It is a figure which shows the relationship between noise and a success rate. It is a figure which shows the profile of the sampling of the collision characteristic in the case with impedance control. It is a figure showing the basic performance of CCME obtained by the experiment using the slip trigger filter in the case of impedance control. It is a figure showing transition of contact mode estimation using a slip trigger filter. It is a figure showing transition of contact mode estimation using a slip trigger filter. It is a figure showing transition of contact mode estimation using a slip trigger filter. It is a figure showing transition of contact mode estimation which does not use a slip trigger filter. It is a figure showing transition of contact mode estimation which does not use a slip trigger filter.

(Configuration of contact mode estimation apparatus 100)
Now, the contact mode estimation apparatus 100 according to the embodiment of the present invention will be described. However, the present invention is not limited to the specific modes described below, and can take various modes within the scope of the technical idea of the present invention. Further, the configuration of the contact mode estimation device 100 is not limited to that shown in the figure, and any configuration can be adopted as long as a similar operation is possible. For example, operations performed by a plurality of components may be performed by a single component, or operations performed by a single component, such as distributing the functions of an information processing unit to a plurality of units. May be executed. Further, various data stored in the memory of the contact mode estimation apparatus 100 may be stored in a different location, and information recorded in the memory is also distributed to a plurality of types of one type of information. May be stored, or a plurality of types of information may be stored in one type. First, the configuration of the contact mode estimation apparatus 100 will be described. FIG. 1 is a block diagram showing a configuration of a contact mode estimation apparatus 100 according to an embodiment of the present invention. The contact mode estimation apparatus 100 is largely composed of an information processing unit 110, a controller 121, and an interface 131. The contact mode estimation apparatus 100 is connected to the motor 122, the force sensor 132, and the image processing unit 141. The information processing unit 110 includes a processor, a temporary memory (RAM), and the like, and includes a main arithmetic circuit 110c that performs various calculations and flow control, and a memory (not shown). The memory includes a contact mode estimation program. A program such as 110p is stored. Specifically, the memory is preferably a nonvolatile memory such as a flash memory, but an HDD or the like can also be used.

  The controller 121 is configured to control a motor 122 that controls opening and closing of the robot hand 200, and transmits a signal for closing and opening the robot hand 200 to the motor 122 based on a command from the information processing unit 110. . The interface 131 is a configuration that electrically connects the information processing unit 110 and the external configuration by converting the signal form so that the signal can be transmitted and received. Specifically, IC for interface. The interface 131 is connected to the force sensor 132 and the image processing unit 141, converts signals from the force sensor 132 and the image processing unit 141 into an appropriate format, and transmits them to the information processing unit 110. For convenience of explanation, the interface is described as one configuration in the drawing. However, depending on the type of configuration to be connected, a different interface may be used or the interface 131 may not be necessary.

  The image processing unit 141 is configured to recognize an image based on a series of image data acquired from the camera 142 and perform position determination, type determination, measurement, and the like of an object in the image. This unit is composed of a temporary memory, a ROM storing programs, and the like, and performs image processing. The image processing unit 141 is combined with the camera 142 to constitute a machine vision system. The image processing unit 141 specifies the position and speed of the target object to be captured based on the series of acquired image data, and outputs data representing the target object to the contact mode estimation apparatus 100. The camera 142 is a camera for capturing an image in front of the robot hand 200 of the satellite 300. The camera 142 acquires data of a series of images in the shooting range. The acquired image data is processed by the image processing unit 141, and the target object shown in the image is recognized.

  FIG. 2 is a diagram illustrating a configuration of the robot hand 200 connected to the contact mode estimation apparatus 100 according to the embodiment of the present invention. The robot hand 200 is configured to grip a target object such as space debris, and is typically a gripper mechanism having a pair of grip portions 202 at the tip of a base 201. The gripper 202 is a rod-shaped part for sandwiching an object, and it is preferable that the gripper 202 is inclined inward when at least the object is gripped. By reducing the distance between the pair of gripping portions 202, a closed region sandwiched between them can be formed, and an object existing therein can be gripped. The gripper 202 is formed integrally with the base 201 of the robot hand 200, and can be configured to form a closed region by translating the pair of grippers 202 and the base 201 inward. . Here, the closed region is a region when the gripper 202 is closed, and an object is captured here. A region where an object can be captured when the gripper 202 is closed is referred to as a closeable region. The grip 202 can also be pivotably attached to the base 201. In this case, the closed region is formed by turning the pair of grips 202 inward. The robot hand 200 is mechanically connected to a power source such as a motor 122 for adjusting the opening amount (angle or distance) of the gripper 202, and the gripper 202 is controlled by controlling the motor. Can be closed or opened. As for the inner shape of the robot hand 200, it is preferable that the bottom 203, which is the innermost part, is a concave portion near the center when viewed from the opening of the robot hand 200. For example, the bottom 203 can be formed from two equal sides having apex angles close to a right angle. With such a shape, when the target object enters the closeable region from the opening at the entrance of the robot hand 200, if it collides with either the left or right side of the bottom 203, the direction of movement of the target object Is changed by about 90 degrees and bounces back toward the center, so that multiple collisions are likely to occur, and it is easy to keep the target object in the closeable region.

  A force sensor 132 is attached to the rear end of the robot hand 200, and the force sensor 132 is further attached to the tip of the robot arm 250. The force sensor 132 is also called a force torque sensor, and is a sensor that outputs a signal indicating an applied force component or moment component. When an object collides with or comes into contact with the robot hand 200, a corresponding signal is output from the force sensor 132. With this signal, it can be detected that the target object has collided with the robot hand 200 or is sliding on it. The robot arm 250 has one or more joints, the angle of which can be adjusted by a power source such as a motor, and the tip of the robot arm 250 can be placed in a desired position and direction by controlling the motor. This is a configuration that gives a degree of freedom. The base of the robot arm 250 is attached to a satellite 300 (not shown), and the distance and direction of the robot hand 200 from the satellite 300 can be changed. The satellite 300 is typically a chaser satellite, and can move in outer space so as to approach the space debris. Since the contact mode estimation device 100 is an electronic device, it is preferably housed inside the satellite 300. The contact mode estimation device 100 is connected to a control line to the motor 122, a signal line from the force sensor 132, and the like.

  In the following embodiment, the target object to be captured is free-flying space debris, and the robot hand 200 is attached to the satellite 300 via the robot arm 250, but the present invention is not limited to such space applications. Even for the purpose, it can be applied to an object that captures a free flight in three dimensions or an object that freely moves in two dimensions by a gripper mechanism such as a robot hand. In this case, the target object is not limited to outer space, and may be an object existing in the air or in water. When the target object is in the air or underwater, the resistance of air or water may be considered in the simulation.

(Operation of contact mode estimation apparatus 100-without impedance control)
Next, operation | movement of the contact mode estimation apparatus 100 is demonstrated. The contact mode estimation is executed by reading the contact mode estimation program 110p into the main arithmetic circuit 110c and executing it. The action of moving the robot hand 200 when the target object collides to alleviate the impact is called impedance control, but such impedance control is not performed, and the robot hand 200 is fixed to the satellite 300 when the target object collides. Embodiments will be described.

  The present invention proposes real-time collision-based contact mode estimation using only a force sensor to capture a moving target object, typically a rigid body. The contact mode is defined to determine the timing for generating a signal for closing the robot hand 200 in order to establish and capture the closure of the target object. It shows how they are touching. In the present invention, the main calculation is triggered by the collision of the target object with the robot hand 200, which is called a “collision trigger filter” in order to determine the contact mode with a minimum amount of calculation using the particle filter method. In addition to the method and “collision trigger filter”, a method using a “slip trigger filter” was adopted. Brach's collision model is used to predict the position and motion of the target object. This is computationally lightweight, and can express a collision event using parameters of three collision characteristics, and thus is suitable for a rigid collision model-based simulation. In addition, the particle filter performs sampling by changing parameters of the three collision characteristics, so that it is possible to express collision events that are difficult to predict in various patterns. As a result, the collision event can be accurately simulated. The method of the present invention to be described uses a reasonable computational resource (average 3.9 milliseconds, worst case 6.1 milliseconds) to achieve a 100% success rate of target capture. The effect that it was able to be achieved was obtained.

(Estimation of contact mode by particle filter)
The present invention first uses a particle filter as a statistical estimation method. Intuitively speaking, a particle filter is used to estimate the state of a hidden object (a position that cannot be directly observed in the present invention) from an observed value (output of a force sensor in the present invention) that can be reliably obtained. After generating data of a plurality of particles (about several hundred in the present invention) in the initial state, each particle is changed based on the observation value while changing the state of each particle step by step according to a physical law or the like by simulation. Is given a weight (probability), and the state (position) of each particle is changed in accordance with the weight so that the distribution density of the particles continues to represent the probability, and then the process is repeated from the stepwise simulation described above. This is a mathematical method for estimating the state (position) of an object hidden by the distribution density of particles. From now on, after defining the contact mode, the mathematical basis of the contact mode estimation using the particle filter derived from the expression of Bayesian filtering will be described. The contact mode estimation function by the contact mode estimation apparatus 100 is implemented by executing the contact mode estimation program 110p by the main arithmetic circuit 110c in the algorithm described below.

(Definition of contact mode)
In order to capture the target object, in the capture phase, how the target object is in contact (or is not in contact) with a structure that captures by gripping, such as the robot hand 200. This information is necessary. Such information is called a contact mode. In order to reliably capture the target object, it is necessary to form a geometric object surrounding around the target object prior to grasping. Once the object is closed by closing the robot hand in a state of surrounding the object, the robot hand succeeds in capturing without missing the target object. This is a specific requirement for capture. First, as the operation required at that time, the robot hand 200 must be opened before the target object enters the closeable region of the robot hand. Second, when the target object enters the closeable region, the robot hand 200 must be closed immediately to establish the closed shape. In order to proceed from the first step to the second step, it is necessary to detect that the target object has entered the object closed region. This is the goal of the contact mode estimation of the present invention. For this reason, the contact mode estimation algorithm detects the position of the target object (ie, whether the target object is near or inside the robot hand 200, etc.) in order to detect that the target object has entered the closeable region. ) Must be estimated.

  As the contact mode in the present invention, first, (1) no contact, (2) ready-to-close (rtc), and (3) contact with a non-closable part, It is necessary to distinguish three categories. FIG. 3 is a diagram illustrating the relationship between the position segment of the robot hand and the contact mode. FIG. 3 shows a robot hand 200A. This robot hand 200 </ b> A has a structure in which a closed region is formed by a gripping part turning with respect to a base. With such a shape, in the initial state where the gripping part is not closed, the object is ready for closing when the object comes into contact with the bottom part, but can be closed when the object comes into contact with other parts, for example, the gripping part. It is not in the state. As described above, in FIG. 3, a portion indicated by a broken line is a portion where preparation for closing is completed, and a portion indicated by a one-dot chain line is a portion which cannot be closed. Then, a part that is ready for closing may be handled as one position segment, or may be divided into a plurality of position segments. The same applies to the part that cannot be closed. In this way, the robot hand 200A is divided into a plurality of position segments, each of which belongs to either a non-closable part or a ready-to-close part. The contact mode is defined by which position segment the target object collides with. Whether or not the target object is in a contact mode corresponding to the closeable region (typically facing the closeable region) (or the probability that the target object is in such a contact mode) Whether or not the target object is in the closable region. In this way, when an object collides with a position segment that is ready for closure, it is determined that “(2) the state is ready for closure”. When an object collides with a position segment that is not closeable, “(3 It is possible to determine that “there is a contact with a part that cannot be closed”. Then, the particle filter closes when it is estimated that the target object has collided with a position segment ready for closing with a predetermined probability (that is, the contact mode corresponding to the preparation for closing is entered with a predetermined probability). By determining that the preparation has been completed and instructing the robot hand 200 to close, the target object can be reliably captured.

  When a contact mode is set to realize a capture operation in a certain robot hand, other contact mode settings can be freely set as long as the above three categories can be identified. As such other contact modes, for example, a difference in contact dynamic characteristics or a position segment in the robot hand can be used. The collision mode (elastic collision or sliding), the number of collisions, and the like can also be used as the contact mode. As an example, it is conceivable to define the contact mode at a position such as the side and vertex of the robot hand 200 (which can be dealt with even when the boundary line of the contact mode is in the center of the side), and it has different material characteristics. It is conceivable to define a contact mode for each part. In other words, in order to realize the capture operation, if “(2) ready state for closing” and “(3) state that cannot be closed” can be clearly distinguished by the contact mode for the part in the robot hand, An additional mode can be freely set to "(2) state ready for closing" or "(3) state not closeable".

(Mathematical basis of particle filter)
A mathematical basis for estimating the contact mode of an object by simulation using a particle filter will be described below. Model-based approaches are commonly used for modeled state estimation by state-space equations using vectors representing continuous parameters of mechanics, but particle filters are hybrid It is commonly used for mode estimation involving systems of representation (continuous and discrete modes). A posteriori probability (posterior) for mode estimation based on Bayesian filtering
Is used. here,
Is a sequence of the target object's continuous state (specifically, location information)
Is a sequence of discrete contact mode (specifically, which segment of the robot hand 200 is in contact),
Is a set of sensor observation values (specifically, a sample of the output from the force sensor 132). For M unique discrete contact modes, the contact mode at time t is
It is represented by Next, the state transition is
Represented as:
Since the continuous state x is position information in the robot hand, a site in the robot hand can be uniquely specified by using the continuous state x. That is, continuous state
To contact mode
Can be calculated. Ie from them
(That is, since the shape and position of the outer shape of the robot hand 200 are known, the contact mode d can be directly obtained from the particle position x).

(Derivation of contact mode estimation)
In order to derive a contact mode estimate, a dynamic system that represents the dynamic state of a particle is first represented as:
Where ω is process noise or disturbance, f is a continuous state transition function of the system including high nonlinearity (specifically, from the previous position, contact mode, process noise, the law of flight in the space at the time of non-collision, A function that determines a new position according to the dynamics of elastic collision according to the equations (14) to (23) at the time of collision), v is measurement noise, and g is a contact mode determination function (a collision detection function for the purposes of the present invention). Specifically, from the position, a function that determines the segmented position of the robot hand when the position is on the robot hand 200), h is a measurement function that may also be a highly nonlinear system It is.

In the present invention, a first-order Markov formulation is applied in order to perform recursive iteration of posterior probability distribution calculation by Bayesian filtering. Hereinafter, in order to explain in a simple expression, the general state s that can include both the continuous state x and the discrete mode d is applied. Then, the recursive prediction step applies the Chapman-Kolmogorov equation, and the update step applies Bayes's law to the equation for estimating the motion, and the following equations (4), (5) and Become.
That is, in equation (4), the probability that the state s _t at state s _t-1 of the original, the state s _t-1 before one of the original sensor observations are present up to the time t-1 Is integrated in the state s _t-1 to obtain the probability (probability distribution function) of the state s _t in the presence of sensor observations up to time t-1. Desired. In equation (5), the probability (p (z _t | s _t )) that the sensor observation value z _t at time _t is obtained in the state _st at time t is multiplied by the probability obtained by equation (4). and what, by dividing the normalization constant obtained by integrating itself obtained by multiplying the state s _t, the probability that the state s _t of the original to the presence of sensor observations up to the time t (probability distribution function) is Desired. As mentioned above, the contact mode d _t can be calculated from the predicted continuous state x _{t of} equation (2), so the following replacement by statistical factorization can be applied.
Applying it to equations (1) and (2), equations (4) and (5) are as follows.

Since the posterior probability distribution of Bayesian filtering necessary for contact mode estimation cannot be explicitly calculated, a particle filter is generally used as an approximate solution for Bayesian filtering. Particle filters can provide a flexible theoretical framework for Bayesian state estimation to deal with non-linear processes, non-Gaussian noise, and multimodal cases as well as discrete modes (compared to Kalman filters, for example) it can. Each particle is a continuous dynamic state variable
And discrete contact modes
Where n in the N particles as a particle index is listed in square brackets. Next, a set of particles
Represented as: Each particle has
The weight, as defined by
There is an associated probability mass called, and using this, the probability distribution for Bayesian filtering is constructed as follows using a delta function:
This can also be understood as a representation of the probability of Bayesian filtering on the left side in terms of a particle filter on the right side. Here in fact, the posterior probability
Are recursively updated and maintained using the Markov assumption, but filtered estimates
Give only
It is useful to calculate
here,
Is called a user-defined importance density, and gives an appropriate distribution by prediction from a sufficiently distributed state. That is, the expression (10) indicates the likelihood that the output z _t of the force sensor at the time t with the position x _t at the time _t is obtained with respect to the weight at the time t−1. proportional to the weight of those particles at time t in the original is subjected to the probability that there are position x _t (a divided by severity density) at time t of the particles at t-1 is present at the position x _t-1 Represents what to do.

(Contact mode estimation by collision model with force sensor)
In the following, we will explain the theory of estimation based on a new model for capturing rigid bodies using only force sensors. Two basic components for contact mode estimation to create an object closure are a particle filter using a collision-triggered filter and a computable and sampleable collision of a rigid body. Model.

(Collision trigger filter)
FIG. 4 is a diagram illustrating a contact mode estimation algorithm based on a particle filter using a collision trigger filter. In the collision trigger filter proposed in the present invention, when a collision is detected by the force sensor, main estimation and evaluation processes are performed. In a general particle filter, updating and resampling are frequently performed, and considerable computational resources are required. In contrast, in the method of the present invention, both the update and resampling steps are triggered when a collision is detected, which can greatly reduce the amount of computation. The operation in each line of the algorithm will be described below.

  In line 1, using both translational and angular information given by prior knowledge before capture, particles are generated and initialized at various positions and velocities. More specifically, the relative position of the approaching target object is estimated by an external sensor such as the camera 142 at the stage before capturing. That is, the image processing unit 141 identifies a target object based on continuous images acquired by the camera 142, estimates its position and velocity in advance, and uses it for particle initialization. It is also possible to measure the position and speed of the target object using a sensor other than the camera. For that purpose, for example, a distance meter such as LIDAR (Light Detection and Ranging) can be used. Since both the chaser (specifically, its own satellite 300) and the target object are rigid bodies, the position of the center of gravity of the target object is sampled first, and the conversion matrix of the position of the gripped portion of the target object is obtained from the information. It is necessary to perform calculation using. Similarly, the chaser needs to calculate the position and speed of the center of gravity and the position and speed of the robot hand 200, and calculate the relative position from the robot hand 200 to the gripper of the chaser. The expected error can be modeled statistically (using the mean and variance parameters), which is expressed in the line 1 initialization stage. For particle filter implementation, the magnitude of the initial state error determines the magnitude of the dispersion in the initial state of the particles.

  Line 2 repeats the line steps existing from line 3 to line 4 while changing n from 1 to the number N of particles. Line 3 is a prediction of a continuous state that is position information. Before the particle collides with the robot hand 200, the particle simply moves in space by following the laws of physics (eg, free fall in the air, flight in orbital rendezvous docking). When the particle collides with any position of the robot hand 200 or the like, the velocity after the collision is obtained based on the velocity before the collision and the appropriately sampled collision parameters using the “Brac model”. In the collision of particles, the contact mode of the robot hand 200 is updated according to which part (that is, which segment) of the apex or side of the robot hand 200 collides with the gripped part (line 4).

The force sensor is too noisy to calculate the exact impulse and its direction, but can reliably detect the pulse signal due to the occurrence of a collision. If a collision is detected (line 6), the collision trigger filter is activated. That is, when a signal having a magnitude corresponding to a collision is detected in the signal from the force sensor 132, the collision trigger filter is activated. Line 7 repeats the steps of the lines existing from line 8 to line 10 while changing n from 1 to the number N of particles. The observation model for each particle in line 8 is the actual collision time here.
And the corresponding simulated collision time
Based on the time difference between
That is, as the simulated collision time t _coll is closer to the actual collision time t _cur , the contribution to the probability of the particle increases and the weight increases. Equation (11) is substituted into Equation (10) to calculate the weight of each particle.
here,
Is the variance of the collision time difference defined by the user and depends on the estimation error and speed of the movement of the gripped part. Among the group of particles that are each simulated, there is a particle that has already experienced a collision corresponding to the collision detected by the force sensor 132 at the time of the collision of the target object. For these particles, the collision time in the collision that has already occurred in the simulation is calculated.
Record as. For the remaining particles (Case B. Particles that have not yet experienced a collision), the prediction needs to be advanced until a simulated collision occurs. That is, for case B particles
Is calculated by advancing the time step of the simulation until the particle collides. In line 9, the continuous state, which is the position information of each particle, is moved to the collision state. That is, by rewinding in the case of case A particles, by moving forward in the case of case B,
Thus, the particles have the same position and velocity immediately before the collision. In other words, each particle is transferred to (immediately before) the collision position. That is, since the actual collision of the target object is detected by the force sensor 132, each particle is also moved to the collision position, and is made to correspond to the actual state. In case B, particles that do not collide even if the time is advanced to some extent are preferably erased by a process such as setting the weight to zero, assuming that there is almost no probability of collision in the first place. The weight is determined according to the equations (11) and (10) based on the collision time difference, and a stochastic difference is given to the particles. In this way, the collision of the transferred particles is simulated with different collision parameters in the prediction step (line 3) in the next iteration. In this way, different collisions are efficiently simulated with as few particles as possible.
Collision
Is recorded with the corresponding collision (line 10). Contact mode as soon as the particle leaves the collision
Indicates no collision. On the other hand, the contact mode at the time of collision was recorded.
Is used later to estimate contact mode for closing preparation (line 17). Line 12 normalizes each particle so that the evaluation value obtained in line 8 is 1 in total, as in a normal particle filter. Line 13 performs resampling, as is done with a normal particle filter, replacing each particle with a probable original particle location. Lines 15 and 16 are sanity checks for old particles that have not collided for a long time, and remove particles that have not collided for too long. The threshold for the time to remove is
It is preferable to make it several times larger. More specifically, for old particles
And the weights will be distributed over the active particles. Then, on line 17, the timing of completion of closing preparation is obtained as follows.
That is, in equation (12), the total of the weights w _t of the particles that have experienced a collision corresponding to the contact mode m is calculated for particles from 1 to N, and the probability of being ready for closure in equation (13). p (d _rtc ) is calculated by summing the weights of the contact modes belonging to the contact mode d _rtc that is ready for closing. When the probability p (d _rtc ) is equal to or higher than a predetermined probability (0.99, 1.00, etc.), it is determined that the closing preparation is complete. Line 18 advances time t by one step and returns to line 2 (t → t + 1 in FIG. 4 indicates that t is advanced by one cycle of the control loop frequency), where a control loop consisting of the aforementioned simulation and evaluation is performed. Will be repeated. Here, particle filters are generally well known for consuming computational power because each particle must go through all the steps of prediction and evaluation in order to generate the necessary states for the control loop. ing. Furthermore, more particles are needed to cover a larger space for estimation. In particular, the force control loop requires a high control loop frequency on the order of 1 kHz because the collision phenomenon typically lasts only a few milliseconds. Because of these facts, it is very difficult to execute real-time calculation. On the other hand, the algorithm of the present invention does not continuously output state estimates, which greatly reduces the computational requirements. The algorithm of the present invention only estimates the minimum contact mode required for the capture operation. Only particle advancement (Case B of line 8) and collision prediction after resampling can consume high computational resources, but in order to reduce computation, both actual collisions are force sensors Calculated only when detected by 132.

(Operation flow for contact mode estimation)
Although the mathematical basis for the contact mode estimation has been described above, the steps when the contact mode estimation apparatus 100 operates using such a mathematical basis are briefly summarized as follows. FIG. 7 is a flowchart showing the operation of the contact mode estimation apparatus.

  The satellite 300 which is a chaser is suitable for the target object to be captured so that the target object gripping part comes between the pair of gripping parts 202 of the robot hand 200 ahead of the robot arm 250 attached thereto. Approaching in different directions and speeds. The camera 142 continuously captures images of the space in front of the robot hand 200, and the image processing unit 141 identifies the target object from the image, and determines the target from the size and movement of the image portion of the target object. It is preferable to estimate the position and speed of the object. Thereby, it can be estimated to which position of the robot hand 200 the target object collides.

  The information processing unit 110 generates data representing a plurality of particles for the particle filter based on the estimated position and velocity of the target object (S101). This step corresponds to line 1 of the algorithm of FIG. Specifically, a data area is set in the information processing unit 110, and the position, contact mode, and the like of each generated particle at a certain time are stored therein. As the particles, a plurality of dispersed particles are generated using a value given appropriate dispersion around the estimated position. The timing at which the particles are generated is preferably a timing at which the camera 142 can confirm the movement of the target object most accurately. For example, it may be when the target object is close enough to be hidden behind the robot hand 200. The contact mode is information indicating which part of the robot hand 200 is in contact with, for example, the contact mode when the robot hand 200 is not in contact: 1, segment 2 of the robot hand 200 When touching, the contact mode is defined as 2 or the like. FIG. 6 is a diagram illustrating a contact mode corresponding to a segment of the robot hand. In the figure, a segmented position on the robot hand 200 is indicated by a reference numeral in which “d” is followed by a number, such as d2. The number after “d” is the contact mode when the segment is touched. Information on the external shape of the robot hand 200 and the robot arm 250 is known, and the degree of opening of the gripper 202 of the robot hand 200 and the position of the robot arm 250 are controlled on the satellite 300 side. From the position output from the encoder, the spatial position of each part of the robot hand 200 is known. Therefore, once the position of the particle is determined, it can be specified whether the particle is in contact with the robot hand 200, and if it is in contact, which segment is in contact with the robot hand 200. Can be identified.

  Next, the information processing unit 110 simulates the position and contact mode of each particle at time t (typically the current time) (step S102). This step corresponds to lines 2-4 of the algorithm of FIG. Here we simulate the continuous state describing the particles. A typical continuous state is a position, but it is preferable to include a velocity, which is a rate of change of the position, in the simulation target. The simulation needs to be executed in real time in the same way as the actual time passage. Therefore, it is necessary to use a calculation resource that can execute in real time a calculation amount that can accurately estimate the actual movement of the target object with the particle filter. Changes in the position of each particle are simulated according to physical laws. The laws of physics are typically constant-velocity linear motion when the robot hand 200 does not touch and freely fly in space, and when it collides with the robot hand 200, It is slipping. Since the shape and position of the outer shape of the robot hand 200 are known, it is calculated how particles collide with the robot hand 200 based on the shape and position. A rigid body collision model for determining how the motion changes when the target object collides with the robot hand 200 will be described in detail later. When it is determined No in step S107, which will be described later, t is advanced by one step and the process returns to step S102. Therefore, step S102 is repeatedly executed at a predetermined interval (for example, a cycle of 1 ms) to perform control. A loop is formed.

  The information processing unit 110 monitors the output of the force sensor 132, and detects that the target object has collided with any location of the robot hand 200 based on the output of the force sensor 132 (step S103). This step corresponds to line 6 of the algorithm of FIG. The output of the force sensor is repeatedly sampled at a predetermined interval, and when the absolute value of the output exceeds a predetermined value, it is determined that the target object has collided with any place of the robot hand 200. The predetermined interval may be the same as the control loop interval. In addition, when the absolute value of an output reduces gently from a peak, it can also be judged that the slip has occurred instead of the elastic collision. If no collision is detected, the process proceeds from step S104 to step S106, skipping step S105. On the other hand, when a collision is detected, the process proceeds to the next step S104.

  When the collision of the target object is detected in step S103, the information processing unit 110 calculates the weight based on the time difference between the collision time (collision time) of each particle and the actual collision time (collision time) (step S104). ). This step corresponds to lines 7, 8, and 11 of the algorithm of FIG. Specifically, the probability that the contribution of the particle becomes larger as the time difference between the collision time of each particle and the actual collision time is smaller in the equation (11) is calculated and substituted into the equation (10). In the equation (10), the current sensor observation value is obtained in the state where the particle exists at the current position with the probability that the particle moves from the position one step before to the current position with respect to the weight of the particle one step before. What is multiplied by the likelihood (further distributed by the importance density) is obtained, and the weight of the current (time t) particle is obtained. Since particles that have a collision time that is close to the actual collision time should have a movement that is closer to the actual movement of the target object, this step causes the movement to be closer to the actual movement of the target object. As the particles are larger, the weight (probability) becomes larger, and the motion of the target substance can be estimated more accurately as a whole particle.

  Next, the information processing unit 110 transfers each particle to the collision position, obtains the contact mode, resamples according to the weight after normalization (step S105). This step corresponds to lines 7, 9, 10, 11, 12, 13 of the algorithm of FIG. Here, as described above in the description of Expression (11), the time is rewinded for the particles that have already experienced a collision (Case A), and the particles that have not yet experienced the collision (Case B) collide. By proceeding with the simulation, each particle is transferred to a collision state. Then, which segment is in collision, that is, the contact mode is obtained. Furthermore, after normalizing all the particles so that the total probability of all the particles becomes 1, the particles are resampled according to the weight of each particle obtained in step S104. Resampling is a process of replacing each particle with the number of particles according to the high probability (weight) by replacing the original particle with a high probability (high weight). By resampling, the weight of each particle is equal, but the density of the particles represents the probability. At this time, particles having almost zero weight move to the position of any other particle having a large weight. By this step, the probability that the target object exists at the position of any one of the particles is represented by the existence density of the particles. In step S104 and step S105, the state (position, weight) of the particle is updated.

  Next, the information processing unit 110 sets the weight of the particles that do not collide for a long time (for example, several times the standard deviation of the collision time) to zero (step S106). This step corresponds to the lines 15 and 16 of the algorithm in FIG. 4 and is a process called sanity check. The particle whose weight is set to zero is replaced with the position of another particle having a larger weight in the next resampling. As a result, particles flying in a position where they are unlikely to collide can be eliminated, and wasteful calculation can be prevented. Position estimation can be continued with high accuracy. Note that the resampling in step S106 is not necessarily a step that needs to be performed. Depending on the situation in which particles that do not collide for a long time are unlikely to be generated, the position of the target object is accurately estimated without performing resampling. be able to.

Next, the information processing unit 110 determines whether or not the total of the weights of the particles that have experienced the closeable region contact mode is equal to or greater than a predetermined value (step S107). This step corresponds to line 17 of the algorithm of FIG. For this purpose, the probability for each contact mode of the entire particle is calculated from the weight of each particle and the contact mode due to the collision just before it. Specifically, the weight w _{d (m)} of all particles in the contact mode m is calculated by the equation (12). Then, in the closeable region, it is determined whether or not the probability of the object evaluated by the particles is greater than or equal to a predetermined value, that is, whether or not the object exists in the closeable region (greater than or equal to the predetermined probability). For that purpose, first, the weight p of all the particles in the contact mode corresponding to the state of preparation for closing is summed up by the equation (13) to obtain the probability p (d _rtc ) of being ready for closing. For example, in FIG. 6, contact modes 2, 3, 8, and 9 correspond to the closed preparation completion state. The contact modes 2, 3, 8, and 9 correspond to the segments facing the closeable region. If the robot hand 200 is closed when the target object is present in the vicinity thereof, the target object is surely secured. Can be captured. Next, it is evaluated whether or not the probability p (d _rtc ) for completing the closing preparation is equal to or higher than a predetermined probability (more than a predetermined numerical value close to 1.00 or 1.00). The target object is present in the closable area with the predetermined probability or more, and it is evaluated (determined) that the preparation for closing is completed, and the process proceeds to step S108. In response to detecting the collision of the target object with the robot hand 200 in steps S104, S105, S106 (not necessarily required) and S107, the particle filter is executed to update the particle state and evaluate the particle. By doing so, it is determined when the target object can be captured by closing the gripper 202 of the robot hand 200. In step S107, if the probability p (d _rtc ) that the closing preparation is completed is less than the predetermined probability, the time t is advanced by one step and the process returns to step S102 (t → t + 1 in FIG. This represents an advance by one cycle), where the control loop consisting of the above update and evaluation is repeated.

  When it is determined in step S107 that the preparation for closing is completed, the information processing unit 110 sends a signal via the controller 121 to the motor 122 that controls the opening / closing of the gripping part 202 of the robot hand 200 so that the gripping part 202 is closed. Send (step S108). As a result, the grip portion 202 of the robot hand 200 is closed to form a closed region, and the object to be handled in the closeable region is secured in the closed region and is gripped.

  By the processing from step S101 to step S108 described above, the approaching target object can be almost certainly captured by the robot hand 200. Although the case of one collision has been described so far, the simulation can be similarly performed using the particle filter even in the case of two or more collisions. In other words, in addition to the robot hand segment information, information about the number of collisions may be added to the contact mode. If the object collides with the robot hand for the second time, when the second collision is detected by the force sensor, a particle filter is used to extract the particles that make the second collision by simulation. Good (that is, particles that do not collide for the second time for a long time after the first collision are zeroed by sanity check and replaced with the positions of the particles that perform the second collision). At that time, as in the first collision, the weight of the particles is changed by the collision time difference from the actual collision time.

(Rigid body computable and sampleable collision model)
Here, how the motion changes in the elastic collision between the target object and the robot hand 200 in the above-described step S102 will be described. Here, a rigid body collision model used for the model-based method will be described. As a specific model, Brach's rigid collision model is used. Brach's rigid collision model has two advantages: low computational burden and particularly affinity for particle filters.

FIG. 5 is a diagram showing a dynamic relationship at the time of collision of rigid bodies. The rigid body {1} collides with the rigid body {2}. The collision point can be expressed as t in the normal direction (n) and the plane tangential direction (t. The figure shows the case of 2D, but it can be expressed using t ₁ and t ₂ in the case of 3D. Can). Since the center of gravity of each object is known or estimated before the collision, the position vector d from the center of gravity to the collision point can be calculated by both translation and rotation. Brach model represents in consideration of these factors, the collision characteristics e, e _m, and the collision phenomenon for simulating the speed after the collision by using the mu. The following equation is a 2D version of this.
Here, m is the mass, I is the moment of inertia of the rotating shaft, v is the translational speed, ω is the rotational speed, P is the impulse, M is the moment, and the suffixes i and f are before and after the collision, respectively. Indicates. The first three equations are momentum conservation laws. Equation (17) is derived from the restitution coefficient (e) in the translation direction. Brach model prescribes as shown in equation (18), the coefficient of restitution in the rotational direction (e _m). When e _m = + 1, it is elastic. When e _m = 0, it is an inelastic angle collisions. When e _m = -1, the moment the impulse is zero (i.e., M = 0). The moment M can be solved as follows by the angular motion equation of each object.
Finally, Brach defines μ, which usually represents the coefficient of friction, but extended the definition to include the impulse ratio from the normal direction to the tangential direction, as shown in Equation (19). Brach's μ can be used as a coefficient of friction when the object is rubbing. In such a case, its size depends on the properties of the material. However, with respect to collisions, it also depends on how the object collided and tends to be a small value. Negative values are also acceptable in terms of how the coefficients are defined in equation (1), where either or both of P _n and P _t are due to equations (20) and (21) In addition, it can be a negative value. The sign of the coefficient is determined to reduce the tangential relative velocity, which represents how the frictional force works. Thus, during the actual capture operation, the post-impact speed (v _jnf , v _jtf , ω _jf , where j = 1 and 2) is _calculated using the equations (14)-(19) It is possible to predict from (v _jni , v _jti , ω _ji ) and three collision parameters.

In addition, Brach's rigid collision model has a particular affinity for particle filters. To quantitatively evaluate the collision model, model Brach uses intuitive crashworthiness e, e _m, and mu, it is possible to express the relationship before and parameters after the collision impact. These collision characteristic parameters cannot be calculated explicitly. Still, since it uses a probabilistic model-based approach, it can be sampled using impact characteristics parameters expressed in statistical terms (ie, mean and variance) and experimentally estimate impact characteristics trends Can do. The collision parameters are sampled so that the calculated post-collision speed given the collision parameters satisfies:
Here, E _ji and E _jf represent kinetic energy. During actual operation, e, e _m, and appropriately sampling the mu, dissatisfaction of formula (24) will not happen.

(Experimental equipment)
An experiment was conducted to evaluate the performance of the contact mode estimation apparatus 100 of the present invention. To that end, we prepared a mock-up chaser and a target with air bearings to emulate motion without planar friction. The purpose of the experiment was to capture the space debris target using a chaser gripper (robot hand, shown in FIG. 6) with a 43 mm opening entrance (ie, the distance between vertices 10 and 11). is there. In this scale model, a target having a weight of 31.3 kg with an inertia moment of 1.16 kg-m ² has a 10 mm × 10 mm square gripped portion at a position 187 mm away from the center of gravity of the target. The test bed was equipped with a motion capture camera system and measured the actual 6 degrees of freedom position of each object at about 120 Hz for evaluation. The chaser robot hand 200 is equipped with a fixed contact shaft having a 6-axis force sensor manufactured by Wacoh (registered trademark) for measuring a collision impulse. In the experiment, the target was pushed carefully by hand and the chaser gripper was fixed to the background frame to avoid control problems including multi-object dynamics. For the calculation, a ROS-kinetic framework installed in Ubuntu 16.04 was used, and a laptop PC (2.9 GHz Intel Core i7-3520M (registered trademark) CPU and 8 GB memory) was used for the entire experiment. In this setup, a control loop frequency of 1 kHz was achieved, including force sensor data collection.

In the experiment, at a position about 350 mm away from the tip of the robot hand 200, the target is gently pushed by hand so that the speed is 29 to 99 / second (average 61 mm / second) which is a typical rendezvous docking operation range. It was. The rotation speed was manually added as an average of 1.5 degrees / second, as expected in actual space debris capture operations. As described above, the profile e of collision characteristics, e _m, and μ is used to predict, it depends on the initial translational velocity and rotational speed. In the case of a particle filter, the parameters of the collision characteristics are sampled with changes. The average and variance of these impact characteristics were determined as shown in FIG. In the particle filter used in the present invention, there is a particle filter that collides with a moving object in the prediction of particles (line 3 in the table of FIG. 4). Here, Brach's collision model is used. Brach's collision model equations are shown in equations (14)-(23), and as shown there, the three collision parameters that determine the escape velocity of the rigid contact are e (translational elastic modulus) , E _m (elastic modulus in the rotational direction), μ (ratio of impulse, friction when completely rubbed), and thus modeled. It has been understood that although there is a certain collision, the collision characteristics at that time cannot be accurately predicted, but there is a certain tendency. It is as follows.
(I) The contact time (t _hit ) depends on the relative velocity in the contact direction and the vertical direction.
(Ii) e depends on the contact time.
(Iii) μ depends on the relative velocity in the contact direction and the vertical direction.
FIG. 17 is a diagram illustrating a sampling profile of collision characteristics when there is no impedance control. In order to represent the above-mentioned tendency, when setting the impact characteristics for the prediction (or simulation) of particles, as shown in the graph of FIG. 17, numerical values are dispersed by the Monte Carlo method.

(Experiment 1: Basic performance evaluation)
The collision-based contact mode estimation (CCME) according to the present invention is evaluated by a test for measuring the basic performance of estimation accuracy and calculation time. Experimental data was collected so that the first contact mode (segment number of the first impacted edge or vertex) was distributed. In order to show robustness against noise, initial noise was artificially added to the true state measured by the motion capture system, as shown in the table of FIG. This is due to the following reasons. First, the particle filter requires an initial value of particles (line 1 in the table of FIG. 4). Specifically, it is necessary to disperse and arrange each particle with a position and speed estimated at a certain timing as a center and an estimation error assumed at that time. In this experiment, a high-accuracy value is measured by a motion capture camera, and based on such a system, a gripped portion represented by a white square surrounded by a black frame in FIG. 19 is plotted. Although it can be considered as a “true value”, there are still limitations on the measurement performance of the camera, and calibration errors and measurement errors are included. Furthermore, even if the contact phenomenon has an error of only 1 millimeter, the result is greatly different, such as rebounding in the air or indentation due to a collision. Therefore, it is important to perform a simulation including an error. Also in this experiment, it is necessary to set the “position and velocity estimated at timing”. Considering this as a “true value”, the position (p) and velocity (v) are shifted by μ (average) in FIG. 16, and the particles are diffused from that state with a variance of σ (dispersion). I am letting.

In the case of CCME, p (d _rtc ) becomes 1 when the gripped portion of the target object hits the side 2, 3, 8, or 9 before the gripped portion of the target object exits the opening entrance. A closure ready signal has been generated). When the gripped portion of the target object does not hit the side 2, 3, 8 or 9, p (d _rtc ) should be 0 instead of 1. FIG. 18 is a diagram showing the basic performance of CCME obtained by an experiment without impedance control. The table shown in FIG. 18 shows that CCME has a contact mode estimation accuracy of 100% with the initial particle distribution shown in the table shown in FIG. In FIG. 18, the “contact mode of the first collision” represents a portion where the gripped portion of the target first hits the gripper. 4-7 / 12/13 is a case that hits the outside of the gripper, which is a case where it cannot be captured, but such a case also needs to be detected. “10/11” is a case of hitting the apex (corner) of the gripper, and the gripper should not be closed only by hitting here. “8/9” is a case directly inside the gripper. “Number of trials” is the number of trials. The “number of trials that escaped” indicates that the target gripped part once entered the gripper, but the target gripped part went out of the gripper because it missed the closing timing, and capture failed. It is the number of times it has done. In this experiment, the actual capture operation was not performed, so such a case occurred. “Estimation accuracy” is the success rate of contact estimation. Everything is 1.00 and everything is successful. “Calculation time (ms) / worst” is the worst value of the processing time required for the operations of the lines 7 to 12 in FIG. Although the control period exceeds 1 ms, this estimation operation is not used for the control operation, and it is only necessary to detect the target gripped part before it comes out of the gripper. “Calculation time (ms) / worst” is an average value of the processing time required for the operations of the lines 7 to 12 in FIG.

FIG. 19 shows a transition of a typical trial mode estimation. The figure is a diagram in which the particle distribution estimated by the particle filter is associated with an image obtained from above where the target collides with the gripper. In the figure, time (unit: s) advances from the left. The upper, middle, and lower trials are the first collisions with side 8 (case 8), vertex 10 (case 10), and side 4 (case 4), respectively. A white rectangle surrounded by a black frame (the direction and speed are indicated by arrows) represents the position and orientation of the gripped portion, and the main body of the target object can be confirmed in the image. Gray squares represent active particles. The upper case 8 requires two collisions, but the particles track the gripped portion well. In the middle case 10, p (d _rtc ) decreases when it reaches the vertex 10, but when it reaches the edge 8, it immediately recovers and immediately indicates a closure ready signal. When hitting the vertex 10, subtle conditions are required as compared to the side, and therefore, when only a small number of particles are distributed, a solution may not be easily found. In the case 4 of the lower stage, as described in the lines 15 and 16 in the table of FIG. 4, the probability inside the robot hand 200 is erased by the sanity check of old particles (light gray squares in the figure). Finally, the closure ready signal must be provided at the appropriate time. For reference, many of the escaped trials in FIG. 18 suggest that the gripped portion exits from the entrance of the robot hand with the robot hand open. In the contact mode estimation, these cases are relieved.

  FIG. 18 also shows the result of calculation time of this estimation, and shows the main problems of the particle filter. The average / worst time of the CCME calculation (i.e., the calculation of lines 7-12 in FIG. Milliseconds. This is slower than the control loop frequency (1 millisecond), but this is not a problem because contact mode estimation can be performed in parallel with control. The calculation time requirement is the time during which the gripped part is still approaching the collision part of the robot hand, for example, 0.2 seconds based on the speed of the rendezvous docking operation. Satisfied. Thus, in the present invention, sufficient real-time estimation can be performed without excessive computational resources.

(Experiment 2: Evaluation of initial state error)
The magnitude of the initial state error is a factor that determines the magnitude of the initial state dispersion of the particles. In order to confirm the effect, the impact on the noise that was first predicted was evaluated. Since the initial particle distribution in FIG. 16 does not affect the estimation accuracy, different magnitudes of noise (both Gaussian distribution and DC bias) were added to the particles during initialization in the representative trial recorded. The result is shown in FIG. FIG. 20 is a diagram illustrating the relationship between noise and success rate. In this simulated experiment, the magnitude of the particle initialization noise in the previous experiment (FIG. 16) is multiplied by a noise factor. The success rate of the contact mode 10 of the initial collision starts to fail with a noise factor = 1, and drops more rapidly for 200 particles than for 400 particles. This indicates that as the initial state error increases, more particles are needed to cover the search space. And more calculations are needed (average 8.3 milliseconds, worst case 14.2 milliseconds). That is, it is twice or more compared with the previous experiment. In summary, the computational resource depends on the magnitude of the motion estimation error in the pre-capture stage.

(Operation of contact mode estimation apparatus 100-with impedance control)
Now, contact mode estimation for capturing rotating space debris under impedance control will be described. First, the control of force for capturing rotating space debris using contact mode estimation will be described. The contact mode estimation method in which impedance control is not performed has been described above, which is based on an environment in which the robot hand 200 is fixed to the ground. This embodiment uses and is associated with a robotic hand on an impedance-controlled robot arm to capture the free flight target required to capture rotating space debris. It also deals with contact mode issues.

  Removal of space debris and repair of broken satellites in space applications requires the capture of freely moving objects. After the range in which the collision avoidance operation of the Clohesy-Wiltshire system expressed by the Hill equation is possible, a one-shot operation (that is, an operation that cannot be performed again) is performed even in a (conventional) cooperative rendezvous operation. On the other hand, in the case of this rotating object capture, it is a one-shot operation, the target object is rotating, and even if it has 3D CAD information of the capture target without the target marker, Since the state is unknown, the one-shot operation becomes more difficult. This means that neither a complete rehearsal nor a complete test preparation on the ground can be performed in terms of camera sensor measurements and contact dynamics. In particular, the measurement is difficult, meaning that the capture is incomplete, and at the initial contact in capturing the rotating object, it is expected that some residual relative velocity will be affected that will affect subsequent operations. In such uncoordinated robot capture, it is necessary to consider collision between rigid bodies and subsequent offset of relative potential energy. In order to cope with the influence of the collision, it is more preferable to perform impedance control for controlling the mechanical impedance.

  The contact mode estimation without impedance control described above is an estimation result at the timing when the robot hand is closed in order to make the relative speed between the robot and the target zero at an appropriate timing. The problem of contact mode estimation under impedance control is that the predicted position of the particle becomes large because of the long collision duration compared to the elastic collision described above. The collision trigger filter uses information that can be clearly discriminated as a collision, but if impedance control is performed, the amount of information is limited and the accuracy of estimation may be reduced.

  In this paper, we propose contact mode estimation using only a force sensor in an impedance control method for a robot arm for uncoordinated free-flying object capture. More specifically, the slip phenomenon is modeled for contact mode estimation using the output of the Brach model to make the particle filter update process more complete. For this purpose, the contact mode estimation algorithm without impedance control described above is modified for this purpose. This technique makes it possible to more reliably capture the rotating rigid body by reliably detecting that the target object has entered the closeable region.

  From this, we first formulate the problem of contact mode estimation under impedance control, and then propose a freely rotating rigid body capture control method using contact mode estimation based on collision under impedance control.

(Contact mode estimation under impedance control)
From now on, contact mode estimation under impedance control will be described. Conventional capture procedures for a target object consisted of observation, approaching, tracking, capturing, and synchronizing. However, conventionally, even after the tracking step, it is impractical because it assumes full observation capability using the camera throughout the step. In order to overcome the above problems in achieving reliable capture, there is a way to capture even if the camera loses the ability to measure the relative position and state of the uncoordinated target at close range. Need to build. As a specific approach, the satellite will use a force sensor independent of the camera to estimate the contact mode in order to know when to close the robot hand to capture the object. In order for capture to be successful, the contact mode estimation needs to be successful. In addition, impedance control is performed to reduce the impact on the satellite due to the collision.

  Assuming that the camera misses the target and there is no input of relative position information in the approach phase where the camera can measure the non-cooperative target, the expected contact time and position and the path to the target gripping part are updated periodically. Need to be done. If the relative position data becomes unstable early, the satellite will leave the target once and start again from the beginning of the approach step. On the contrary, if the relative position data can be moved to the vicinity of the target while the input of the relative position data is stable as it is, the process proceeds to the next step. In this case, even if the camera information is interrupted, the satellite cannot return to the original position.

In this embodiment, another step called limp is added instead of the traditional tracking step (FIG. 8). FIG. 8 is a diagram illustrating a series of steps up to capturing an object. The steps after limp are as follows.
Limp: Based on the final prediction by the camera, the approach direction and speed of the target object are input to the satellite. The robot arm 250 also enters a collision posture so that impedance control can be performed in the next step. After this, the satellite waits in the limp state (no servo) at the given speed until the first collision with the gripped portion occurs. At this time, the speed of the satellite 300 with respect to the target object is about 100 mm / second.

And the next step is as follows:
Capture: After the first collision occurs, the satellite closes the object at the gripped part of the target object without a camera. Impedance control is performed to extinguish the effective energy due to the relative motion due to the collision. Along with this, when the robot hand 200 is closed is determined by the contact mode estimation.
• Synchronizing: The satellite robot hand 200 is now closed to close the object. The satellite is controlled to cancel the relative motion between the targets. When the object is closed, the satellite and the target object are not separated. There is enough time to navigate the satellite and robot arm.
It is necessary to design a robot hand that can afford a position error. In addition to the position error, an effective relative velocity at the time of initial collision is also expected. For such a situation, impedance control based on force sensor-based contact estimation capable of handling elastic collision as described below is performed.

(Impedance control including when there is no relative position information)
When capturing a rigid body that rotates freely, its rotational motion becomes a problem. Thus, some relative velocity between the rigid bodies is predicted and there is an energy interaction. In order to deal with this situation, impedance control theory was adopted. Here, the impedance control method when there is no relative position information will be described. The formula of the impedance control method is as follows. In the formula, if there is a dot at the top, it means that it has been differentiated once.
Here, x _gp is a vector of the position and orientation of the gripped part, and x _e is a vector of the position and orientation of the robot hand (instruction value of the operation to the robot hand and robot arm, or position encoder from them) F _e is an external force applied to the robot hand (detected by a torque sensor), and _De , _Me and _Ke are gain matrices. Solving for the desired robot hand acceleration xεe, the equation is:
In this case, at least in the capture step, there is no information about the relative position of the gripped portion. Therefore, it becomes as follows.
On the other hand, although the relative speed cannot be calculated in the same manner, the speed of the robot hand can be calculated from the change of the motor, and the decay time can be calculated accordingly.

The Jacobian matrix is defined as follows:
Here, J _m is a Jacobian. Φ _m is the angular position of the motor and is information acquired from the encoder of the motor. The desired motor angular position is derived as follows by differentiating both sides of the equation (5.4) with respect to time.
The dynamic equation of the robot robot arm (the equation of motion in the rotation direction of the robot arm) is as follows.
Here, H _m is an inertia matrix of the robot arm, and τ _m is a desired motor torque that becomes a command to the robot arm. From equations (5.2), (5.5), and (5.6), the command for the robot arm motor is calculated in the capture step. When impedance control is properly applied, the collision time is lengthened. As a result, the predicted position of the particles varies, and it becomes difficult to distinguish adjacent contact modes in the contact mode estimation.

(Contact mode estimation by impedance control)
Hereafter, impedance control including contact mode estimation using a force sensor is presented. Here, the contact mode estimation (CCME) theory based on the collision model without impedance control of the robot arm is applied. Here, the contact mode estimation by the slip trigger filter under impedance control will be described. This is because when impedance control is performed, slipping is likely to occur in an actual target object, and particles are likely to slip even in estimation. It is also possible to perform contact mode estimation using a slip trigger filter without performing impedance control.

  FIG. 9 is a diagram for explaining the behavior at the time of collision of an object (including slipping during impedance control). In the description so far, it is assumed that all the collisions are elastic and the contact portion is separated after the collision ((A) in FIG. 9). Impedance control includes elastic rigid collisions, but the contact portion may be separated or remain in contact after the collision. Further, when impedance control is performed, a phenomenon of “slip” may occur ((B) in FIG. 9). When the impedance control is completely performed, the coefficient of restitution becomes 0 and an inelastic collision occurs. However, since space debris capture involves rigid body collisions, impedance matching is almost impossible to achieve. Especially in space debris capture, the object is large and the gripped part is located outside the target object, so the gripped part is separated from the center of gravity of the target object. Therefore, perfect impedance matching is not expected, but slipping may occur if appropriate impedance control is performed. In order to implement a slip trigger filter, it is necessary to model for predicting the slip phenomenon and to detect for filtering.

(Prediction of slip phenomenon)
The rigid body slip phenomenon and its prediction will be described. In this case, Brach's rigid collision model is also used. FIG. 10 is a diagram for explaining the behavior (elastic collision and slip) at the time of collision of an object. In FIG. 10A, a collision occurs at the right end side of the lower object. Collision characteristics determined by the speed and _{(e, e m, μ)} , based on the inertial properties and microscopic material properties of both rigid, the speed output of both objects is determined. After the collision, the upper object moves away, but there is a space to leave. In the case of FIG. 10B, there is no room for movement. Therefore, the upper object must move along the surface of the lower object while applying some force to the lower object. This phenomenon is called slipping.

FIG. 11 shows a prediction algorithm for entering the slip mode in order to predict and calculate the phenomenon of slip. Whether or not slip occurs is determined by whether the two objects have geometric interference after calculating the output velocity using Brach's model. Specifically, in line 6, it is determined whether the contact point is moving along the surface. If so, it is determined that there is slip. In practice, the position is advanced according to the final velocity (k = 1 and 2) and checked for a collision.

(Slip detection)
When slipping occurs after a collision, a force is applied to the force sensor 132 attached to the tip of the robot arm, so that it can be easily detected. FIG. 12 is a diagram illustrating an example of the output of the force sensor 132 in the normal direction. In general, the duration of a collision is less than a few tens of milliseconds, and slipping occurs when a weaker force is applied for longer. If slip is detected in the particle filter calculation, particles moving in free space without slipping can be filtered out. That is, since the slip actually occurs, the estimation can be made accurate by setting the weight of the particle not causing the slip to zero. In addition, a new collision may occur during sliding as shown in FIG. FIG. 13 is a diagram illustrating an example of the output of the force sensor in the normal direction (including a collision during sliding). These give information for estimating the contact mode of the place where the gripped part is located.

(Overall algorithm)
The algorithm shown in FIG. 14 is an estimation algorithm for all contact modes that can be handled under impedance control. Lines 1 to 13 are similar to the algorithm shown in FIG. 4 and the update process is started when a collision is detected. The prediction process (line 3) in this embodiment includes the slip prediction described in the algorithm shown in FIG. The contact mode at time t is represented by d _t ∈ {d ₍₁₎ , d ₍₂₎ ,..., D _(K) }, and is used to determine whether the robot hand 200 is ready for closing. . Here, d _{(k ′)} is defined as a case where d _(k) slips. After the update process is performed, a slip trigger filter is employed. Whether slipping is occurring is tracked by the output of the force sensor (by detecting that a weak force is output for a long time) and filtered according to particles in sliding contact mode or non-slip mode ( Lines 16-22). Specifically, if slip is detected by the force sensor 132 at lines 16 to 17, resampling is performed for particles that are not in a slip state, and normalization is performed at line 18. Also, if a collision is detected on lines 19 to 20 but not slipping, resampling is performed on the slipped particles and normalization is performed on line 21. Lines 23 and 24 are sanity checks for old particles that have not collided for a long time, and remove particles that have not collided for too long. At line 25, the timing for the completion of the closure preparation is determined. Line 26 advances time t by one step and returns to line 2 (t → t + 1 in FIG. 14 indicates that t is advanced by one cycle of the control loop frequency). Will be repeated. Since the filter triggered by the detection of slip is executed only once at that timing, as shown in FIG. 13, when a collision is detected during the slip (line 6), the collision trigger filter is executed individually. (Lines 7-11). As described above, when summarizing the point about using the slip trigger filter, based on the signal output from the force sensor 132, it is detected that the target object is slipping on the robot hand 200, and in each of the plurality of particles, When the particle colliding with the robot hand 200 moves along the surface of the robot hand 200, it is simulated that the particle is sliding on the robot hand 200, and it is further detected that the target object is sliding on the robot hand 200. In response, by evaluating the contact mode by executing the particle filter, by considering the timing at which the target object can be captured by the closing of the grasping portion 202 of the robot hand 200, slipping as well as collision is considered. Simulation and estimation can be performed.

  The estimated estimated position error during visual servoing in the approach step is statistically modeled using the mean and variance parameters and reflected in the limp step shown in line 1 of the algorithm shown in FIG. In performing the particle filter, the magnitude of the initial state error determines the magnitude of the initial dispersion of the particles. The particle state needs to cover more space than expected initial error.

(Necessary computational resources)
Describe the amount of computational resources required. The amount of calculation is expected to be the same as that without impedance control. The slip trigger filtering step (line 17 and line 20) is expected to be computationally negligible compared to collision trigger filtering. A collision trigger filter involves a parameterized sampling process, but a slip trigger filter uses a certain velocity in a certain direction that continues from a previous collision and therefore does not have a parameterized sampling process. Sampling the magnitude of the velocity vector may be considered, but the effect of sampling is limited because it eventually collides with the same point of the opponent object. Thus, the actual slip trigger filter includes only resampling that requires limited computational resources.

(Experiment)
An experiment was conducted to verify the entire collision model-based contact mode estimation algorithm (including the algorithm of FIG. 14 and the algorithm of FIG. 11) with a particle filter under impedance control. More specifically, the following evaluation was performed.
・ Performance verification from different experimental tests using various collision schemes including calculation time for contact mode estimation (Experiment 3)
-Advantage of contact mode estimation using slip trigger filter (Experiment 4)

(Experimental device)
In the experiment, an air bearing robot system is used. Here, the chaser is fixed to the ground. FIG. 15 shows the coordinate system. The coordinate system is as follows.
World {W}: Ground data is acquired from the motion capture system.
Chaser {CH}: A coordinate system fixed to the chaser. Here, it is fixed to the ground.
・ Robot hand {EE}: Acquires forward mechanical data from {CH}. Contact mode estimation is performed in this frame.
Target {TA}: A coordinate system fixed to the target body for free flight.
-Grasped part {GP}: The grabbed part is a part of the target, and is slightly separated from the center of gravity of the target (185 mm).

In the chaser, the length of each link including the height of the force sensor is L ₀₁ = 110 mm (center of gravity of the main body to the joint 1), L ₁₂ = 100 mm (joint 1 to joint 2), L ₂₃ = 100 mm ( Joint 2 to joint 3), and L _3E = 188 mm (including the height of the force sensor, from joint 3 to {EE} base) and a three-degree-of-freedom arm on the plane. An actuator with a harmonic gear generated by a Harmonic Drive® system is used. The target has a weight of 31.3 kg, a moment of inertia of 1.16 kg-m ² in the scale model, and a 10 mm × 10 mm square gripped part at a distance of 185 mm from the target CoG.
The chaser is equipped with an on-board battery Shorai LFX series (12V, 7A). For the calculation, the ROS-kinetic kinetic framework installed on Ubuntu 16.04 (2.9 GHz Intel Core i7-3520M® CPU and 8 GB memory) with a laptop PC was used throughout the experiment. in use. The chaser robot hand is equipped with a fixed contact shaft equipped with a 6-axis force sensor Wacoh Tech DynPick, and can measure collision impact and slip phenomenon.

The experiment started with a limp step. The levitating target is gently pushed by hand at a speed of 70 to 133 mm / sec (average of 100 mm / sec) with respect to the relative speed between objects. The rotation speed is given by hand at an average of 2.4 degrees / second. The target is placed 350 mm away from the tip of the gripper. This is a situation similar to a limp step. At this point, the particles are assigned the noise profile shown in FIG. 16, corresponding to the noise variance of the state prediction based on camera measurements. This noise profile is the same as that used in the experiment without impedance control. The capture of the target object is successful when the contact mode estimation indicates that the closure preparation is complete at an appropriate time. For contact mode estimation, crashworthiness e, profile of e _m, and μ is dependent on the initial translational velocity and rotational speed. FIG. 21 shows a sampling profile of collision characteristics in the case of impedance control. This is because the longer collision time is expected as compared with that of FIG. 17 used in the experiment without impedance control, and is changed accordingly. When the target gripped part hits the side 2, 3, 8 or 9 before the target gripped part exits the opening, p _(drtc) becomes 1 (a closing preparation completion signal is also generated) ) And the experiment was successful. The number of particles is fixed at 200 throughout the experiment.

(Experiment 3: Evaluation of basic performance)
The performance is verified from different experiments with various collision schemes, and the results are shown in FIG. FIG. 22 is a diagram showing the basic performance of CCME using a slip trigger filter under impedance control. Experimental data was collected and recorded by changing the initial contact mode so that the behavior of the target after impact under impedance control was completely covered. This includes collisions with edges or vertices and non-sliding contact modes. These results show that the collision model-based contact mode estimation algorithm using a particle filter under impedance control is effective even considering the initial estimation error.

23, 24, and 25 show typical transition cases of contact mode estimation using a slip trigger filter. In these figures, the distribution of particles estimated by the particle filter is associated with an image taken from above where the target collides with the gripper. In the figure, time (unit: s) advances from the left. FIG. 23 is a diagram illustrating the transition of the contact mode estimation when the slip trigger filter is used and the contact mode of the first collision is 8. FIG. 24 is a diagram showing a transition of the contact mode estimation when the contact mode of the first collision is 8. FIG. 25 is a diagram illustrating a transition of the contact mode estimation when the contact mode of the first collision is 10. Dark gray squares represent colliding particles. The light gray square enters sliding sliding mode. The gray rectangle is toward the space (no collision), and the white rectangle surrounded by a black frame is data from the motion capture system and is the position of the actual gripped portion of the target. In the first case (FIG. 23), after the first collision, the contact mode enters the slip mode (d _t = d _{(8 ′)} ). When slipping is detected, the particles are collected into slipping particles, with the nonslipping particles being erased. This facilitates second contact mode estimation during sliding. The second collision determines the timing of the closure ready signal (ie, p _(drtc) = 1). In the second case (FIG. 24), the contact mode has not entered the slip mode after the first collision. Experimental results show that the timing of the closure ready signal can still be accurately determined. The result is the same as in the case of 8/9 in FIG. 18 when there is no impedance control. This indicates that the contact mode estimation was performed by utilizing the characteristic of impedance control advantageously. In the case of FIG. 25, p _(drtc) remains low when hitting vertex 10. At vertex 10, the particles that hit the top of the robot hand mainly survive. However, if some particles collide with the vertices, some resampled particles gather here and it is possible to simulate collisions at the vertices with various collision characteristics. After this, if more collisions are detected, the probability of p _(drtc) increases gradually. Immediately after hitting edge 8, the probability recovers, indicating that the closure is ready. The result is the same as the case of 10/11 in FIG. 18 when there is no impedance control.

  The calculation time for the contact mode estimation of the filtering step between trials is 4.2 ms on average and 8.3 ms in the worst case, which is well tolerated. As shown in Experiment 3, the performance verification of CCME using a slip trigger filter for contact mode estimation by impedance control verified that it has sufficient performance. Tests were performed using different collision schemes with different experimental cases, and 100% estimation accuracy was obtained. The calculation time for contact mode estimation is 4.2 milliseconds on average and 8.3 milliseconds at worst, which is sufficiently acceptable.

(Experiment 4: Evaluation of advantages of contact mode estimation using slip trigger filter)
Here, the advantages of contact mode estimation using a slip trigger filter are evaluated. The slip trigger filter makes some similar discrete mode estimations more accurate by adding another trigger to the contact mode estimation by collision trigger only (in the absence of impedance control). FIG. 26 and FIG. 27 are diagrams showing the transition of the contact mode when the recorded data obtained in the case of FIG. 23 is used but the slip trigger filter is not used. In the case of FIG. 26, the trigger information of two collision signals that are only collisions is used to accurately estimate the timing of the closing preparation completion signal. On the other hand, FIG. 27 shows a case where the operation has failed. A white square not surrounded by a black frame in FIG. 27 represents an old particle that has not collided. In the case of FIG. 27, since the ratio of w _d10 = p (d ₍₁₀₎ ) remains (= 0.0163) and p (d _rtc ) is not 1.00, accurate estimation cannot be performed. . This is because the impedance control induces a longer collision time, and some particles may reach the vertex 10 during the collision time. However, the slip trigger filter can be remedied from this situation by adding another condition of slip for filtering. The success rate of trials is 100% with slip trigger filter (33 out of 33 trials), and 87.9% without slip trigger filter (29 out of 33 trials) ). This clearly shows the advantage of contact mode estimation using a slip trigger filter.

  The present invention is used to estimate and capture the position of a target object by estimating the contact mode of the target object to a robot hand ahead of a robot arm attached to a satellite that captures space debris in outer space. It is possible. Furthermore, the present invention can be applied to an arbitrary system that captures an object that freely flies in three dimensions or moves freely in two dimensions by a gripper mechanism such as a robot hand.

100: Contact mode estimation device 110: Information processing unit 110c: Main arithmetic circuit 110p: Contact mode estimation program 121: Controller 122: Motor 131: Interface 132: Force sensor 141: Image processing unit 142: Camera 200: Robot hand 200A: Robot hand 201: Base 202: Grasping part 202A: Grasping part 203: Bottom part 250: Robot arm 300: Satellite

Claims (7)

A contact mode estimation device for determining a timing for closing a gripping part of a robot hand to capture an approaching object,
The robot hand is attached with a force sensor that senses a force applied to the robot hand and outputs a signal representing the force.
A collision detection unit for detecting that the object has collided with the robot hand based on the signal output from the force sensor;
A particle generator for generating data representing a plurality of particles for a particle filter;
A particle simulation unit for simulating each position of the particles and a contact mode with the robot hand;
In response to detecting the collision of the object with the robot hand, the particle filter is executed to update the state of the particle and to evaluate the particle, thereby closing the grip portion of the robot hand. A closing timing determination unit that determines a timing at which the object can be captured by:
The contact mode estimation apparatus characterized by having. The closing timing determination unit is responsive to the collision of the object with the robot hand,
By executing the particle filter, a weight is obtained for each of the particles based on a time difference between the collision time of the particle and the actual collision time of the object, and the particle is transferred to a collision position, and the robot Determine the contact mode with the hand, perform the particle state update by resampling after normalization,
The simulation and the update of the state of the particles are repeated until the sum of the weights of the particles that have experienced the contact mode corresponding to the closable area that can be captured by the robot hand is equal to or greater than a predetermined value. Evaluating the timing of closing the grip part of the robot hand when the total is a predetermined value or more,
The contact mode estimation apparatus according to claim 1. A sensor for capturing a pre-collision position of the object;
A target object position estimator that estimates the position and motion of the object based on information obtained from the sensor;
Further comprising
2. The particle generation unit according to claim 1, wherein the particle generation unit generates a plurality of particles for the particle filter in advance based on the position and movement of the object at the time of the collision estimated by the target object position estimation unit. Contact mode estimation device. The sensor is a camera;
The target object position estimation unit estimates the position and motion of the object based on the image of the object obtained from the camera.
The contact mode estimation apparatus according to claim 3. A slip detection unit that detects that the object is slipping on the robot hand based on the signal output from the force sensor;
The particle simulation unit simulates that each of the plurality of particles is sliding on the robot hand when the particles colliding with the robot hand move along the surface of the robot hand,
In response to detecting that the object is sliding on the robot hand, the closing timing determination unit further executes the particle filter to evaluate the contact mode, thereby The contact mode estimation apparatus according to claim 1, wherein timing that can be captured by closing a grip portion of the robot hand is estimated. A robot arm with the robot hand attached to the tip;
An impedance control unit that operates the robot arm to absorb the impact of the collision when the collision detection unit detects a collision of the object with the robot hand;
The contact mode estimation device according to claim 5, further comprising: The contact mode represents which segment the particle has contacted with when the surface of the robot hand is defined as being composed of a plurality of segments,
The contact mode estimation according to claim 1, wherein the contact mode corresponding to the closeable region that can be captured by the robot hand in the close timing determination unit corresponds to a segment facing the closeable region of the robot hand. apparatus.