Pose Estimation of a Container with Contact Sensing Based on Discrete State Discrimination

Abstract

In cases where vision is not sufficiently reliable for robots to recognize an object, tactile sensing can be a promising alternative for estimating the object’s pose. In this paper, we consider the task of a robot estimating the pose of a container aperture in order to select an object. In such a task, if the robot can determine whether its hand with equipped contact sensor is inside or outside the container, estimation of the object’s pose can be improved by reflecting the discrimination to the robotic hand’s exploration strategy. We propose an exploration strategy and an estimation method using discrete state recognition on the basis of a particle filter. The proposed method achieves superior estimation in terms of the number of contact actions, operation time, and stability of estimation efficiency. The pose is estimated with sufficient accuracy that the hand can be inserted into the container.

1. Introduction

Robots need to estimate object pose for manipulation when the task is not a simple repetitive operation. In such cases, visual sensors are generally used to estimate the pose. For example, the pose of an object can be estimated by matching the 3D model of an object with point clouds obtained by lidar sensors (e.g., [1,2,3]). Deep learning techniques, such as convolutional neural networks, are also widely used to directly estimate the pose of objects from images [4,5,6,7]. Image processing techniques using vision transformers instead of convolutional neural networks have been studied as well [8]. Such methods using visual sensors are available, but are not applicable when a transparent object is located in a deep-bottomed box. A method that does not rely on visual sensors is required when robots operate in environments where such sensors are likely to become dirty, such as outdoor areas.

For such cases, tactile and force information is effective for estimating the pose of objects. Contact-based object recognition can be categorized into deterministic and probabilistic approaches. As examples of the deterministic approach, a multimodal method that uses tactile, force, and vision information [9] and a method that estimates pose by local contact force based on geometric models [10] have been proposed. Methods with geometric rendering of objects [11] and high-resolution tactile sensors [12] have been proposed as well. However, all of these require expensive and highly accurate sensors (e.g., [13]). The probabilistic approach, on the other hand, is available even with inexpensive sensors, as even if a single measurement is inaccurate, the accuracy can be improved by repeating the contact measurement.

The probabilistic approach ranges from methods based on Kalman filters [14,15,16], in which the estimated or observed distribution is represented by a Gaussian distribution, to those based on particle filters [17,18,19,20,21,22] that allow arbitrary probability distributions to be represented using particles. For object pose estimation with contact sensor information, a manifold particle filter [23,24,25] samples particles from a low-dimensional manifold. This approach has been proposed for solving the unique problems of particle filters that arise when estimating the contact base, and is used in the method proposed in this paper.

While contact-based object pose estimation has been applied to various objects, robotic systems often require the insertion of a robotic hand into concave objects such as bags and containers. Ichiwara et al. constructed a network to unzip a bag by integrating the visual and tactile senses [26]. Gu et al. used dynamic manipulation to widen the aperture of a bag and place an object into a bag [27]. In order to place an object into a concave container or remove it from the container, it may be necessary to widen or open the aperture of the container in addition to estimating the pose of the object. In cases where vision information is not reliable due to transparency or occlusion, the aperture should be estimated by the contact-based probabilistic approach mentioned above. In this paper, we propose an efficient method for estimating the aperture by utilizing the property of contact between the robot hand and the concave object. The idea behind this method is that while the hand can be either inside or outside of a concave object, contact information is generally richer when it is inside. We focus on discriminating the discrete inside/outside state and investigate its effect on estimation efficiency. Although strategies for improving the efficiency of contact search generally include contact selection to maximize information gain [28,29], contacting points with large uncertainty [30,31], and using Bayesian optimization [32], we show that the efficiency of pose estimation can be improved by selecting appropriate actions to move inside the concave object and obtain richer contact information.

The contributions of this paper are twofold:

• We propose a manifold particle filter to estimate the discrete state when the robot hand is positioned inside or outside the concave object.
• Improved estimation efficiency is achieved by selecting actions according to the estimated discrete states.

The remainder of this paper is organized as follows: Section 2 describes the conditions and objectives of the problem; Section 3 describes the details of the methodology; the experimental results are shown in Section 4; finally, the conclusions are provided in Section 5.

2. Problem Definition

We consider the task of inserting a robot hand along with attached force sensors into a concave object, such as a bag or container.

For simplicity, we assume that the aperture of the concave object is oriented upward against the horizontal plane and that the object’s 2D shape on the plane is invariant along the vertical axis, as shown in Figure 1. The objective of the robot system is to estimate the object aperture pose by repeating contacts between the force sensor and the object, given that the pose is initially known roughly as a probability distribution. The estimation is considered as sufficient if it is precise enough that the hand can be inserted into the aperture.

One application of this problem is to improve the efficiency of estimating the aperture pose for inserting a hand into a transparent container such as a beaker, which has a relatively small aperture, in order to pick up an object within. The system can accommodate containers with various aperture shapes by collecting observational data offline and constructing a observation model for arbitrary shapes.

2.1. Conditions

The geometrical shape of the object, including its height, is known; however, the reaction force observed by the force sensor at contact with the object is unknown. Thus, the relation should be obtained by experiment. The object is assumed to be rigid and does not deform by contact or move during the estimation process.

2.2. Object Property and Basic Strategy of Contact Motion

Based on the assumptions mentioned above, exploration of the robot hand is designed as a combination of planar motion and up/down motion. In the up-motion plane, the hand can freely move horizontally without any contact. In the down-motion plane, on the other hand, the hand will stop its horizontal motion when the force sensor detects that it has contacted the object.

Because the object edge has a certain thickness, the hand can contact the object at the edge when the hand moves from the upper plane to the bottom plane. It is assumed that the contact at the edge can be detected by the force sensor. When the hand is at the bottom plane, it is either inside or outside the object. We define this discrimination as the discrete state.

2.3. Notation

Let ${}^W\mathit{x}={[x,y,\theta ]}^T\subset \mathcal{X}$ denote the pose of the aperture of the object in the world coordinate system. The force sensor information is denoted by $\mathit{s}\in {\mathbb{R}}^n$, where n denotes the dimension of the force sensor. The discrete state is denoted by $d\in \{\mathrm{in},\mathrm{out}\}$. Let ${}^H\mathit{x}={[{}^{H}x,{}^{H}y,{}^H\theta ]}^T$ denote the pose of the object in the hand coordinate system. The relation between the hand and the object on the horizontal plane is characterized by a boundary function $B_i({}^{H}x,{}^{H}y,\theta ),i=0,1$, as shown in the right panel of Figure 2. The correspondence between discrete state d and $B_i({}^{H}x,{}^{H}y,\theta )$ is provided as follows:

$$\left\{\begin{array}{cc}{B}_1({}^{H}x,{}^{H}y,\theta )<\parallel [{}^{H}x,{}^{H}y]\parallel \hfill & \iff d=\mathrm{out}\hfill \\ B_0({}^{H}x,{}^{H}y,\theta )\le \parallel [{}^{H}x,{}^{H}y]\parallel \le B_1({}^{H}x,{}^{H}y,\theta )\hfill & \iff d\mathrm{not}\mathrm{defined}\hfill \\ B_0({}^{H}x,{}^{H}y,\theta )>\parallel [{}^{H}x,{}^{H}y]\parallel \hfill & \iff d=\mathrm{in}.\hfill \end{array}\right.$$

(1)

3. Estimation of Aperture Based on Discrete State Recognition

An overview of the proposed estimation method is shown in Figure 3. Prior to the estimation, the observation model is constructed as shown in the left frame of the figure. The observation model consists of: (1) the force sensing model; (2) the edge collision model; and (3) the non-existence region model. The first model estimates the object’s pose based on the contact force information obtained during planar exploration and constructs the pose based on samples of $\mathit{s}$ and pose ${}^H\mathit{x}$ collected in the preliminary experiment, where $G_{\mathrm{in}}={\{{}^H\mathit{x}_i,{\mathit{s}}_i\}}_{i=1,\cdots ,N_{\mathrm{in}}}$ and $G_{\mathrm{out}}={\{{}^H\mathit{x}_i,{\mathit{s}}_i\}}_{i=1,\cdots ,N_{\mathrm{out}}}$ denote sets of samples with the hand inside and outside the object, respectively, while $N_{\mathrm{in}}$ and $N_{\mathrm{out}}$ denote the respective number of data samples.

The second model deals with collisions with the object at the edge during downward motion. During planer exploration, non-contact information can also be utilized to refine the pose estimation, which is realized by the third model. The second and third models are constructed based on the geometric information of the object.

In the estimation phase, shown in the right frame, The initial distribution is generated a priori. An initial distribution is generated under the assumption that an initial visual observation has been obtained. The robot descends to a height at which it can contact an object, then performs a planar motion. Force sensing and updating of the estimated distribution are performed using the method described in the following subsection. While repeating this process, the robot performs up/down motions under certain conditions. The object aperture pose is estimated by the particle filter until it terminates based on a predefined condition. The well-known Kalman Filter expresses the estimated distribution using a Gaussian distribution. In our research, the estimated distribution may be multimodal, making a particle filter more suitable. A histogram filter can also express multimodal distributions; however, the amount of calculation increases when the estimation target becomes high-dimensional. Therefore, both a particle filter (PF) [17,18,19,20,21,22] and manifold particle filter (MPF) [23,24,25] were used for pose estimation, as described in the following sections. The object of pose estimation consists of the following components:

• Particle set with weight and discrete state $\left\{X\right(t),W(t),D(t\left)\right\}$.

  -

  Set of particles representing object pose: $X\left(t\right)={\left\{{\mathit{x}}_k\left(t\right)\right\}}_{k=1,\cdots ,N_p}$

  -

  Set of weights corresponding to particles: $W\left(t\right)={\left\{w_k\left(t\right)\right\}}_{k=1,\cdots ,N_p}$

  -

  Set of discrete states corresponding to particles: $D\left(t\right)={\left\{d_k\left(t\right)\right\}}_{k=1,\cdots ,N_p}$

• Non-existence region of object $\mathcal{A}$.

Here, $N_p$ denotes the total number of particles and time step $t\in \mathbb{N}$ is incremented after each exploration action. Each particle has a weight $w_k\ge 0$ and a discrete state $d_k\in \{\mathrm{in},\mathrm{out}\}$.

3.1. Particle Filter Update for Non-Contact Observation

The PF was applied for pose estimation as depicted in Algorithm 1 in order to reflect non-contact sensing information during planar exploration. In a general PF, particles are sampled based on the state transition probability. In our problem, however, the pose of the object is assumed to be fixed and there is no state transition. Therefore, in our method we use the distribution $X(t-1)$ of the particles at one step prior, without sampling at step 1. The discrete states $D(t-1)$ of the particles are directly inherited from one step prior as well.

Algorithm 1 Particle filter for non-contact information processing without state transition

Input:  Particle sets $X(t-1)$ and $D(t-1)$ and sensor observation $\mathit{s}\left(t\right)$ (non-contact) Output:  Particle sets $X\left(t\right)$, and $D\left(t\right)$  1: $\{X^{\prime },D^{\prime }\}\leftarrow$ {$X(t-1)$, $D(t-1)$}  2: $W^{\prime }\leftarrow$ weighted by non-contact model $P_{non}\left(\mathit{s}\right|\mathit{x})$  3: $\left\{X\right(t),D(t\left)\right\}\leftarrow$ Resample$(X^{\prime },W^{\prime },D^{\prime })$

When the hand has moved along a linear trajectory in the lower plane without force sensation, it means that the object does not exist in the corresponding region when considering the object as a 2D shape. Thus, in the case where the hand moves from ${\mathit{x}}_0$ to ${\mathit{x}}_1$, as shown in Figure 4, the gray region should be omitted from the probability distribution.

The particles are weighted based on the non-contact model $P_{non}\left(\mathit{s}\right|\mathit{x})$. Let ${\mathcal{A}}_t\subset \mathcal{X}$ denote the non-existence region where no object is supposed to exist based on the non-contact information and the hand motion trajectory, and let $X_{\mathrm{non}}$ denote the set of particles that are located in ${\mathcal{A}}_t$, with $X_{\mathrm{non}}=\left\{{\mathit{x}}_k|{\mathit{x}}_k\in {\mathcal{A}}_t\right\}$. The weights of particles are reset as follows:

$$\begin{array}{c}\hfill w_k=P_{\mathrm{non}}\left(\mathit{s}\right|{\mathit{x}}_k)=\left\{\begin{array}{cc}\frac{1}{N_p-\left|X_{\mathrm{non}}\right|},\hfill & {\mathit{x}}_k\notin {\mathcal{A}}_t\hfill \\ 0,\hfill & {\mathit{x}}_k\in {\mathcal{A}}_t.\hfill \end{array}\right.\end{array}$$

(2)

This process corresponds to Step 2 in the algorithm. Because the number of active particles with positive weights decreases during the process, active particles are recovered by resampling at Step 3 while preserving the discrete state $D^{\prime }$.

Note that the estimated non-existence region is accumulated through the process. Let $\mathcal{A}\subset \mathcal{X}$ denote the accumulated non-existence region, which is updated using the non-contact motion at time step t as

$$\mathcal{A}\leftarrow \mathcal{A}\cup {\mathcal{A}}_t.$$

(3)

3.2. Manifold Particle Filter (MPF) with Discrete State Discrimination

When contact is observed by the force sensor, the pose of the object can be constrained on a low-dimensional manifold (surface of the object). Under the original PF framework, particles that are not located on the manifold should be deleted, as they are not consistent with the contact observation. To prevent such particle loss, an MPF directly samples particles from the low-dimensional manifold. Furthermore, the discrete state discrimination as to whether the hand is inside or outside the object when the hand descends is used for pose estimation and the exploration strategy. The MPF algorithm applied to the problem is shown in Algorithm 2.

Algorithm 2 Manifold particle filter with discrete state discrimination

Input:  Particle sets $X(t-1)$, $W(t-1)$ and $D(t-1)$ and sensor observation $\mathit{s}\left(t\right)$ (contact) Output:  Particle sets $X\left(t\right)$, $W\left(t\right)$ and $D\left(t\right)$  1: $\{X^{\prime },D^{\prime }\}\leftarrow$particle sampled from $P(\mathit{x},d|\mathit{s}\left(t\right))$  2: $W^{\prime }\leftarrow$weights calculated by EstimateDensity$(X^{\prime },X(t-1))$  3: $\left\{X\right(t),D(t\left)\right\}\leftarrow$Resample$(X^{\prime },W^{\prime },D^{\prime })$

3.2.1. Methods for Updating the Estimated Distribution

At Step 1 in the algorithm, particles are sampled from probability distribution $P\left(\mathit{x}\right|\mathit{s})$. There are two kinds of contact models: the edge collision model for the hand’s descending motion, and the force sensing model for planar (horizontal) exploration. For construction of the edge model, $P_{\mathrm{edge}}\left(\mathit{s}\right|{}^H\mathit{x})$ is constructed by extracting ${}^H\mathit{x}$ satisfying $B_0({}^{H}x,{}^{H}y,\theta )\le \parallel ({}^{H}x,{}^{H}y)\parallel \le B_1({}^{H}x,{}^{H}y,\theta )$ and the appropriate sensor information $\mathit{s}$.

For the force sensing model, the sample sets $G_{\mathrm{in}}$ and $G_{\mathrm{out}}$ are used to construct $P_d\left(\mathit{x}\right|\mathit{s}),d\in \left\{\mathrm{in},\mathrm{out}\right\}$. The following procedures are repeated for both cases, $d=\mathrm{in}$ and $d=\mathrm{out}$. First, $P\left(\mathit{s}\right|\mathit{x})$ is regressed using a Gaussian process (GP) with a Gaussian kernel. Note that the superscript H is omitted in the following for simplicity of notation. Based on the regression, the average sensor value $\widehat{\mathit{s}}\left(\mathit{x}\right)$ is determined and $P\left(\mathit{x}\right|\mathit{s})$ is approximated in discrete multi-dimensional grids following the Bayes rule. Suppose that the pose space $\mathcal{X}$ is discretized into $N_g$ grids with boundary vector ${\mathit{a}}_{\min }^{\left(l\right)}$ and ${\mathit{a}}_{\max }^{\left(l\right)}$ by

$$\mathrm{Grid}l\leftrightarrow \left\{\mathit{x}|{\mathit{a}}_{\min }^{\left(l\right)}\le \mathit{x}<{\mathit{a}}_{\max }^{\left(l\right)}\right\},l=1,\cdots ,N_g.$$

(4)

Using the mean of the boundary ${\overline{\mathit{a}}}^{\left(l\right)}\equiv \frac{{\mathit{a}}_{\min }^{\left(l\right)}+{\mathit{a}}_{\max }^{\left(l\right)}}{2}$, $P_d^H\left(\mathit{x}\right|\mathit{s}),d\in \left\{\mathrm{in},\mathrm{out},\mathrm{edge}\right\}$ (with $\mathit{x}$ expressed in the hand coordinate) is approximated as follows:

$$\begin{array}{cc}\hfill P_d^H\left(\mathit{x}\right|\mathit{s})& =\left\{\begin{array}{cc}\frac{{\rho }_x^{\left(l\right)}\left(\mathit{s}\right)}{{\sum }_{j\in I\left(\mathcal{A}\right)}{\rho }_x^{\left(j\right)}\left(\mathit{s}\right)},\mathrm{s}.\mathrm{t}.{\mathit{a}}_{\min }^{\left(l\right)}\le \mathit{x}<{\mathit{a}}_{\max }^{\left(l\right)},\hfill & {\overline{\mathit{a}}}^{\left(l\right)}\notin \mathcal{A}\hfill \\ 0\hfill & {\overline{\mathit{a}}}^{\left(l\right)}\in \mathcal{A}\hfill \end{array}\right.\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill {\rho }_x^{\left(l\right)}\left(\mathit{s}\right)& \equiv exp\{-\frac{1}{2}{(\mathit{s}-\widehat{\mathit{s}}\left({\overline{\mathit{a}}}^{\left(l\right)}\right))}^T{\Sigma }^{-1}(\mathit{s}-\widehat{\mathit{s}}\left({\overline{\mathit{a}}}^{\left(l\right)}\right)\},\Sigma =\mathrm{diag}[{\sigma }_1^2,\cdots ,{\sigma }_m^2]\hfill \end{array}$$

(6)

where $I\left(\mathcal{A}\right)=\left\{i\right|{\overline{\mathit{x}}}^{\left(i\right)}\notin \mathcal{A}\}$ denotes the set of indices of the grid that is not contained in region $\mathcal{A}$.

Because the particles are sampled in the world coordinate, the probability of $P_d^H\left(\mathit{x}\right|\mathit{s})$ in (5), which is expressed in the hand coordinate, should be transformed to the world coordinate. Let $T\left(t\right)$ denote a homogeneous transformation matrix from the world coordinate to the hand coordinate at time step t. Using this transformation, the probability distribution for Step 1 is provided by

$$\begin{array}{cc}\hfill P(\mathit{x},d|\mathit{s})=P({}^W\mathit{x},d|\mathit{s})& =P({}^H\mathit{x},d|\mathit{s})\hfill \\ & =P(T\left(t\right){}^W\mathit{x},d|\mathit{s})=P_d^H\left(\mathit{x}\right|\mathit{s}),d\in \left\{\mathrm{in},\mathrm{out},\mathrm{edge}\right\}.\hfill \end{array}$$

(7)

Using the distribution, the particles are sampled with discrete state information as $X^{\prime }$ and $D^{\prime }$.

Step 2 of Algorithm 2 assigns weights to the set of particles $X^{\prime }$ based on the kernel density estimation of particles $X(t-1)$. EstimateDensity() is represented as

$$\begin{array}{cc}\hfill w_k\left(t\right)& =\frac{{\sum }_{\ell =1}^{N_p}{\rho }_h({\mathit{x}}_k,{\mathit{x}}_{\ell })}{{\sum }_{k=1}^{N_p}{\sum }_{\ell =1}^{N_p}{\rho }_h({\mathit{x}}_k,{\mathit{x}}_{\ell })},\hfill \\ \hfill {\rho }_h({\mathit{x}}_1,{\mathit{x}}_2)& =exp\left\{-\frac{1}{2}{({\mathit{x}}_1-{\mathit{x}}_2)}^T{\Sigma }_{\mathrm{density}}^{-1}({\mathit{x}}_1-{\mathit{x}}_2)\right\}\hfill \\ \hfill {\mathit{x}}_k& \in X^{\prime },{\mathit{x}}_{\ell }\in X(t-1),\hfill \end{array}$$

(8)

where ${\Sigma }_{\mathrm{density}}\in {\mathbb{R}}^{3\times 3}$ denotes the parameter matrix used for density estimation.

At Step 3, particles are resampled based on $W^{\prime }$ and all weights are reset to $1/N_p$ while preserving the discrete state $D^{\prime }$. The final output of the algorithm is provided by $X\left(t\right)$ and $D\left(t\right)$, as $W\left(t\right)$ is uniform with the same weights over the particles.

3.2.2. Usage of Particle Output

The object’s pose is estimated using $X\left(t\right)$. Instead of a weighted average over all particles, an estimation (denoted by ${\mathit{x}}_{\mathrm{est}}$) is determined using kernel density estimation, as follows:

$$\begin{array}{cc}\hfill v_k\left(t\right)& =\frac{{\sum }_{m=1}^{N_p}h({\mathit{x}}_k,{\mathit{x}}_m)}{{\sum }_{k=1}^{N_p}{\sum }_{m=1}^{N_p}h({\mathit{x}}_k,{\mathit{x}}_m)},{\mathit{x}}_k,{\mathit{x}}_m\in X\left(t\right)\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill {\mathit{x}}_{\mathrm{est}}\left(t\right)& ={\mathit{x}}_{k_{\mathrm{est}}},k_{\mathrm{est}}=\underset{k=1,\cdots ,N_p}{argmax}{v}_k\left(t\right)\hfill \end{array}$$

(10)

The discrete state is then probabilistically estimated from the set of particles after resampling, as follows:

$$\begin{array}{cccc}\hfill p(d=\mathrm{in})& =\frac{|D_{\mathrm{in}}|}{N_p},\hfill & \hfill D_{\mathrm{in}}& =\{d_k\in D\left(t\right)|d_k=\mathrm{in}\},\hfill \end{array}$$

(11)

$$\begin{array}{cccc}\hfill p(d=\mathrm{out})& =\frac{|D_{\mathrm{out}}|}{N_p},\hfill & \hfill D_{\mathrm{out}}& =\{d_k\in D\left(t\right)|d_k=\mathrm{out}\}.\hfill \end{array}$$

(12)

3.3. Exploration Strategy Based on Discrete State

The pseudo-code of the robot operation is depicted in Algorithm 3. Initially, a rough estimation of the object pose is provided and the first target position on the 2D plane is decided according to the initial estimation. When the hand descends to the bottom with the target 2D position, the hand may collide with the object at the edge. In such a case, the distribution is updated by the MPF using the edge collision model $P_{\mathrm{edge}}^H\left(\mathit{x}\right|\mathit{s})$. For Update-hand-target( ) at Step 7, $(x,y)$ is updated by adding random values. Note that the discrete state is not considered in the case of collision during descending motion, as it is defined only when the hand is at the bottom plane during horizontal exploration.

Algorithm 3 Exploration of hand with pose/discrete state estimation

1: Initialization   2: Set initial particle sets $X\left(0\right)$, $W\left(0\right)$ and $D\left(0\right)$ based on initial estimation   3: Set initial hand target $(x,y)$   4: while true do   5:    Descend-hand($x,y$)   6:    if Collision then   7:      $(x,y)\leftarrow$Update-hand-target( )   8:      Update distribution by MPF   9:    else  10:      repeat  11:         Contact-action( )  12:         Update distribution by MPF/PF  13:         if $p\left(\mathrm{out}\right)\ge p_{\mathrm{th}}$ then  14:           $(x,y)\leftarrow$Update-hand-target( )  15:           break  16:         end if  17:      until Estimation-convergence( )  18:    end if  19:    Raise-hand( )  20: end while

When the descent motion is complete, Contact action is executed. In Contact action, the target position of the exploration is determined by the estimated value and the position information of randomly extracted contact data. If the hand contacts the object, the distribution is updated based on the MPF (Algorithm 2), while if the hand finishes the motion without contact, the distribution is updated by the PF (Algorithm 1).

If the hand is expected to be outside the object, then the hand is relocated, aiming at another target 2D position. Equation (12) with the p-th threshold value is used for this judgment. For Update-hand-target( ) at Step 14, ${\mathit{x}}_{\mathrm{est}}\left(t\right)$ in (10) is used. When the robot raises its hand, the discrete state estimation is reset. This is naturally done at Step 1 of Algorithm 2 at the next contact sensing, as the discrete states are sampled based only on the new contact observation. This exploration strategy allows the robot to collect useful contact information more efficiently, thereby improving the efficiency of the pose estimation.

The total exploration and estimation process is finished when the following termination conditions are satisfied:

$$\begin{array}{cc}\hfill {\overline{\mathit{x}}}_{\mathrm{est}}\left(t\right)& <\mathit{x}\mathrm{th},{\overline{\mathit{x}}}_{\mathrm{est}}\left(t\right)=\frac{1}{T}\sum _{i=1}^T|{\mathit{x}}_{\mathrm{est}}(t-i)-{\mathit{x}}_{\mathrm{est}}(t-i+1)|\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill \mathit{V}\left[X\left(t\right)\right]& <{\mathit{V}}_{\mathrm{th}}\hfill \end{array}$$

(14)

where ${\overline{\mathit{x}}}_{\mathrm{est}}\left(t\right)$ represents the moving average of the estimate after the t-th contact action, T is the size of the average, $\mathit{x}$-th is the threshold value, $\mathit{V}\left[X\right(t\left)\right]$ represents the variance of the set of particles, and $\mathit{V}$-th denotes the variance threshold.

4. Experiment

4.1. Condition

A 5-DOF manipulator manufactured by HEBI Robotics [33] was used in the experiment, with a small force sensor manufactured by Touchence [34] mounted on the hand tip. This force sensor is equipped with Micro-Electro-Mechanical Systems (MEMS) to achieve miniaturization. The piezoresistive layer detects force by measuring the change in resistance. The experimental system and force sensor are shown in Figure 5. A force sensor was attached to the center of the hand to obtain contact information and conduct experiments. Gras** would require changing the position of the force sensor, which would not very difficult; however, this is left to future work. This force sensor can measure six axes (fx, fy, fz, Mx, My, Mz); the rated loads are ±7.0 N for fx and fy, 18.0 N for fz, ±15.0 Nmm for Mx and My, and ±10 Nmm for Mz. The resolution is 0.5 N for fx, fy, and fz and 0.2 Nmm for Mx, My, and Mz. The measurement accuracy is ±7.5%, and a measurement cycle of 10 ms was used. The force sensor was connected via spring to a contactor with a length of 30 mm in order prevent any sudden increases in the reaction force. As the rigid bodies came into contact with each other, an elastic body was installed between the sensor and the contact to prevent damage to either the robot or the object.

The objects consisted of a rectangular container 140 mm in length and 85 mm in width, a regular triangle of 160 mm per side, and an irregular quadrangle with the sizes shown in the Figure 6 The heights of the objects were all 65 mm. The pose of the object’s aperture (x, y, and $\theta$) in the world coordinate system is denoted by ${}^W\mathit{x}={[x\left[\mathrm{mm}\right],y\left[\mathrm{mm}\right],\theta \left[\mathrm{deg}\right]]}^T$, while the hand pose is denoted by ${}^W\mathit{h}=[h_x\left[\mathrm{mm}\right],h_y\left[\mathrm{mm}\right],h_{\theta }=0\left[\mathrm{deg}\right]]$, as the hand is in a constant posture. The robot estimated the aperture pose of the object ${}^W\mathit{x}$ in the world coordinate system from the hand tip pose ${}^W\mathit{h}$ calculated from the kinematics of the manipulator and the sensor information $\mathit{s}={[\mathrm{fx}\left[\mathrm{N}\right],\mathrm{fy}\left[\mathrm{N}\right],\mathrm{Mx}\left[\mathrm{Nmm}\right],\mathrm{My}\left[\mathrm{Nmm}\right]]}^T\in {\mathbb{R}}^4$.

Initially, the robotic hand was positioned at the origin in the x and y planes and began to move from a height of 195 mm above the steel table. Assuming that the visual sensor can estimate the approximate location of the object, the hand then transitioned to the initial distribution estimates $x_v$ and $y_v$. After that, the robot executed “Descend hand” with $(x,y)=(x_v,y_v)$. If the contact condition was met while descending the hand, the robot recognized that it had collided with the edge of the container, executed the estimation process using the MPF as $\mathit{s}={[1,1,1,1]}^T$, and moved the hand upward. The contact condition was realized when either fx, fy, or fz became larger than 0.6 N or when either Mx or My became larger than 0.8 Nmm. If the hand was successfully lowered, a contact action was executed. When the contact condition was met, the hand retreated 10 mm in the direction opposite to that of the action, then executed the estimation process using the MPF. If the contact condition could not be satisfied during the action, the hand stopped at the selected position and the estimation process was executed by the PF as $\mathit{s}={[0,0,0,0]}^T$. The contact condition was the same as in the above condition. When the MPF was executed, the sensor information s was normalized. The contact action was then executed again from the position at which it stopped for the MPF/PF estimation process, and the contact action was repeated; if the robot recognized that the hand was positioned outside, then it executed “Raise hand” and moved to the estimated value.

Next, $P_d^H\left(\mathit{x}\right|\mathit{s})$ was calculated in $xy\theta$ space with $-110\le x\le 110$, $-110\le y\le 110$, $-90\le \theta \le 90$; the total number of grids for discretization was $N_g=45\times 45\times 19=$ 38,475. The kernel parameters of the kernel $k({\mathit{x}}_i,{\mathit{x}}_j)={\theta }_1\exp (-{\theta }_2\parallel {\mathit{x}}_i-{\mathit{x}}_j{\parallel }^2)+{\theta }_3{\delta }_{ij}$ were ${\theta }_1=5$, ${\theta }_2=0.01$, and ${\theta }_3=0.3$. The variances ${\sigma }_i^2,i=1,2,3,4$ were determined from the observed data as follows:

$$\left\{\begin{array}{cc}{\widehat{s}}_i<0.6\hfill & {\sigma }_i^2=0.15\hfill \\ 0.6\le {\widehat{s}}_i<0.8\hfill & {\sigma }_i^2=0.08\hfill \\ 0.8\le {\widehat{s}}_i\hfill & {\sigma }_i^2=0.01\hfill \end{array}\right.$$

(15)

where ${\widehat{s}}_i$ denotes the i-th component of $\widehat{\mathit{s}}\left({\overline{\mathit{a}}}^{\left(l\right)}\right)$. The number of particles $N_p=1000$ was set to represent the probability distribution in the MPF/PF. The parameter used for kernel density estimation in the MPF was set to ${\sum }_{\mathrm{density}}^{-1}=\mathrm{diag}[100,100,1000]$. The MPF/PF termination conditions were set as $\mathit{x}\mathrm{th}={[5,5,20]}^T$, $T=3$, and $\mathit{V}\mathrm{th}={[300,300,2000]}^T$. The robot recognized that convergence was difficult for $t>20$; thus, the termination conditions were set to $\mathit{x}\mathrm{th}={[10,10,40]}^T$, $T=3$, and $\mathit{V}\mathrm{th}={[600,600,4000]}^T$. Moreover, the robot recognized that convergence was very difficult for $t>35$; thus, the termination conditions were set to $\mathit{x}\mathrm{th}={[15,15,60]}^T$, $T=3$, and $\mathit{V}\mathrm{th}={[10,000,10,000,10,000]}^T$.

The following explanation covers implementation of the non-existence region ${\mathcal{A}}_t$ when the robot executes the t-th contact motion and finds no contact. Let $S_0$ denote the non-existence region of the object at the position of the hand before the contact motion. Let $S_l$ denote the non-existence region of the object at the position of the hand where the hand moves l[mm] horizontally. If the total moving distance is L [mm], then the non-existence area ${\mathcal{A}}_t$ is as follows:

$$\begin{array}{cc}\hfill {\mathcal{A}}_t& =(S_0\cup S_1\cup \cdots \cup S_L)\cap \left(\overline{S_0\cap S_L}\right),\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill S_l& =\{\mathit{x}={[x,y,\theta ]}^T|\mathit{F}\left(\mathit{x}\right)\le 0\},\hfill \end{array}$$

(17)

$$\mathit{F}(\mathit{x})=\left[\begin{array}{cc}sin(\theta +b_1)& cos(\theta +b_1)\\ sin(\theta +b_2)& cos(\theta +b_2)\\ sin(\theta +b_3)& cos(\theta +b_3)\\ sin(\theta +b_4)& cos(\theta +b_4)\end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}{c}_1\\ c_2\\ c_3\\ c_4\end{array}\right].$$

(18)

Here, $b_i,c_i,i=1,2,3,4$ were determined by the size and posture of each of the three objects and the moving distance l of the hand.

To verify the effectiveness of the proposed method, we experimented with two methods that did not discriminate between the discrete states. In the first “SD:stay-down” method the hand descends one time, then contact search is executed using only horizontal movements. When the hand descends to the outside of the container, there is no inside transition; if the hand descends to the inside of the container, then there is no raising or lowering of the hand. In the second “LR:lift-random” method, the hand is raised with a probability of 1/2 for each contact action, then descends to the estimated value. The proposed method (DSB:discrete state-based lift strategy) raises the hand when the discrete state is recognized as outside, then lowers the hand to the estimated value. The threshold for recognition as outside was set to $p\mathrm{th}=0.7$.

4.2. Preliminary Experiment for Building the Contact Model

In the preliminary experiment, the true pose of the object’s aperture was known and samples of the input ${}^H\mathit{x}$ and output $\mathit{s}$ were collected. Contact samples were collected from both inside and outside the objects. In the former case, the hand was set to start at the center of gravity of the aperture at a height that would allow the contactor to contact the rectangular container edge. The robot moved horizontally in the direction of angle $\alpha$ along the x-axis, and samples of ${}^H\mathit{x}$ and $\mathit{s}$ were collected at the time of contact. Contact information was obtained at 36 positions with $\alpha =0,10,20,\cdots ,350$. To collect samples from outside the objects, the hand was set to start at a position of 90 mm in the direction of $\alpha$ from the center of gravity of the aperture at a height that allowed the contactor to contact the edge of the rectangular container. The samples of ${}^H\mathit{x}$ and $\mathit{s}$ were collected at the time of contact by horizontal movement toward the position of the aperture’s center of gravity. As in the case of contact from the inside, $\alpha =0,10,20,\cdots ,350$ was set to obtain the contact information at 36 positions. The robot was required to obtain contact samples from different postures in a rectangular container. In this case, the sample was collected by changing the joint angle of the fifth axis of the manipulator (${\theta }_5$) while maintaining the fixed posture of the container. The experiment was conducted assuming ${\theta }_5=0,10,20,\cdots ,170$ and 18 posture patterns of the container. A total of 36 × 18 × 2 = 1296 inside and outside samples were collected. For the regular triangular object, the experiment was conducted assuming ${\theta }_5=0,10,20,\cdots ,110$ and 12 posture patterns of the container. Therefore, a total of 36 × 12 × 2 = 864 inside and outside samples were collected. Due to the lack of symmetry, for the irregular quadrangle object the experiment was conducted assuming ${\theta }_5=0,10,20,\cdots ,350$ and 36 posture patterns of the container. Therefore, a total of 36 × 36 × 2 = 2592 inside and outside samples were collected.

4.3. Results

The experimental results for the three types of objects are shown in

Table 1. Results of the discrete state-based lift strategy (DSB), stay-down method (SD), and lift-random method (LR) on the rectangular object.

	${\mathit{N}}_{\mathit{c}}$	${\mathit{T}}_{\mathit{o}}$	${\mathit{x}}_{\mathit{e}}$	${\mathit{y}}_{\mathit{e}}$	${\mathit{\theta }}_{\mathit{e}}$
DSB
Max	13	91.6	17.1	12.0	55.4
Min	5	39.6	0.9	0.5	0.7
Ave	7.9	62.9	5.1	5.9	10.9
Var	7.7	192.7	19.3	16.9	256.8
SD
Max	19	117.1	16.0	14.7	49.4
Min	5	37.3	1.6	0.4	1.8
Ave	13.6	77.6	6.4	7.0	14.4
Var	24.8	772.4	26.4	18.5	195.7
LR
Max	21	227.1	13.3	9.8	40.5
Min	5	58	1.0	1.7	0.7
Ave	10.4	123.2	6.5	5.6	13.4
Var	30.6	2783.0	17.8	5.4	129.7

,

Table 2. Results of the discrete state-based lift strategy (DSB), stay-down method (SD), and lift-random method (LR) on the regular triangle object.

	${\mathit{N}}_{\mathit{c}}$	${\mathit{T}}_{\mathit{o}}$	${\mathit{x}}_{\mathit{e}}$	${\mathit{y}}_{\mathit{e}}$	${\mathit{\theta }}_{\mathit{e}}$
DSB
Max	14	116	24.4	26.1	31.6
Min	6	55	0.3	1.7	5.6
Ave	8.6	80.5	8.5	10.7	16.1
Var	6.6	348.25	43.8	55.32	75.3
SD
Max	27	133	20.1	25.6	51.5
Min	9	65	0.9	0.6	0.4
Ave	18.9	99.9	10.5	12.7	12.7
Var	44.3	649.3	51.9	77.8	259.7
LR
Max	21	307	16.7	25.6	28.4
Min	5	75	1.26	1.5	1
Ave	10.8	150.8	7.6	7.3	10.1
Var	32.4	5552.2	25.2	49.3	73.6

and

Table 3. Results of the discrete state-based lift strategy (DSB), stay-down method (SD), and lift-random method (LR) on the irregular quadrangle object.

	${\mathit{N}}_{\mathit{c}}$	${\mathit{T}}_{\mathit{o}}$	${\mathit{x}}_{\mathit{e}}$	${\mathit{y}}_{\mathit{e}}$	${\mathit{\theta }}_{\mathit{e}}$
DSB
Max	23	183	21.8	37.9	140.7
Min	7	68	1.1	0.7	4.4
Ave	13.3	108.6	10.1	9.6	48.6
Var	39.5	1387.8	46.9	100.3	2365.6
SD
Max	35	183	38.3	23.3	113.2
Min	9	60	1.2	0.9	3.1
Ave	17.7	99.5	9.4	9.9	44.4
Var	56	1191.7	128.7	53.6	1558.4
LR
Max	29	434	25.5	15.8	109.6
Min	6	97	1.6	0.3	0.5
Ave	16.9	227.5	13.8	7.8	27.6
Var	53.1	11121.3	41.5	16.5	1025.9

. In the tables, $N_c$ and $T_o$ denote the total number of contact actions $\left[\mathrm{times}\right]$ and the operation time $\left[\mathrm{s}\right]$ until the termination condition was satisfied. The $x,y,\theta$ errors of the convergence values of the estimation were $x_e\left[\mathrm{mm}\right]$,$y_e\left[\mathrm{mm}\right]$,${\theta }_e\left[\mathrm{deg}\right]$. We experimented ten times with ${}^W\mathit{x}_{\mathrm{true}1}={[0\left[\mathrm{mm}\right],90\left[\mathrm{mm}\right],0\left[\mathrm{deg}\right]]}^T$ and ${}^W\mathit{x}_{\mathrm{true}2}={[5\left[\mathrm{mm}\right],80\left[\mathrm{mm}\right],30\left[\mathrm{deg}\right]]}^T$ for the true pose of the rectangular container five times each. For regular triangular objects, the true pose was ${}^W\mathit{x}_{\mathrm{true}3}={[-30\left[\mathrm{mm}\right],50\left[\mathrm{mm}\right],0\left[\mathrm{deg}\right]]}^T$ and ${}^W\mathit{x}_{\mathrm{true}4}=[-5\left[\mathrm{mm}\right],60\left[\mathrm{mm}\right],$ ${30\left[\mathrm{deg}\right]]}^T$. For irregular quadrilateral objects, the true pose was ${}^W\mathit{x}_{\mathrm{true}5}=[-20\left[\mathrm{mm}\right],$ ${25\left[\mathrm{mm}\right],0\left[\mathrm{deg}\right]]}^T$ and ${}^W\mathit{x}_{\mathrm{true}6}={[5\left[\mathrm{mm}\right],40\left[\mathrm{mm}\right],30\left[\mathrm{deg}\right]]}^T$. The initial distribution was changed in all ten experiments. The results for rectangular objects are described below. The robot was able to perform estimation with an average error of less than 15 degrees and an average positional error of less than 7 mm. Comparison of the operation times shows that the DSB method is the most efficient, followed by the SD and LR methods. The DSB method has a smaller variance than both other methods, indicating that its estimation efficiency was more stable. The errors for the regular triangular objects were slightly larger than those for the rectangular objects; however, the superiority of the results with respect to the estimation efficiency remained the same. The posture error was larger in the case of irregular quadrangular objects. The operation time was shorter for SD than for DSB. Finally, the irregular quadrangle objects required more contact motions to converge.

Figure 7 shows the updated distribution for the experiment with DSB and the distribution satisfying the termination condition. The initial distribution $X\left(0\right)$ was artificially generated under the assumption that it was obtained from visual sensors. The robot descended its hand to the estimated initial distribution and moved to contact, but was unable to contact the object. The result of updating the distribution from the non-contact information using the particle filter is represented as $X\left(1\right)$. Compared to the initial distribution, resampling that considers non-existence regions shows that the distribution of the particles is missing. In the next contact action, the robot contacted the container and sampled particles from the observation model. Here, the blue particles have “out” information and the red particles have “in” information. Kernel density estimation was based on the distribution of X(1) results in regions where particles were present with a large weight, and particles close to these regions were resampled. Because the discrete state estimate resulted in p(d=out) = 1, the robot raised the hand and descended to the estimated position, causing the hand to transition to the inside of the container. After three repeated contact actions, the estimates converged and the robot stopped operating. The variance of $\theta$ in the converged $X\left(5\right)$ is relatively large, which is discussed in the next subsection.

Figure 8 shows the results of updating the distribution using the MPF from the third to the fifth contact information in the experiment with the SD strategy. Under SD, when the hand descends to the outside of the container, it does not transition to the inside, and often repeats the contact at the same position. In this case, the sampled distributions are similar, and the distributions for weighting are similar as well; therefore, the estimates and variances do not change significantly when the distributions are updated. Therefore, the contact action must be repeated many times before convergence is achieved, which degrades the estimation efficiency.

Table 4. Number of up/down motions.

Experiment	1	2	3	4	5	6	7	8	9	10	Ave
Up/Down times	6	4	2	1	3	3	4	7	7	7	4.4

shows the number of times the hand up/down motion was performed during the ten experiments for the LR strategy along with the average number of such motions. DSB often required only one up/down motion when the hand descended outside the container based on the estimated initial distribution. However, because this method did not discriminate between the discrete states and could not determine whether the hand was inside or outside the container, the robot performed an average of 4.4 hand up/down motions. As shown in Figure 9, the robot often made up/down motions with the hand even when the hand was positioned inside, resulting in wasted operation time. On the other hand, owing to stochastic up/down motions, estimation was sometimes completed in a short time, leading to a large variance in the total operation time. Because this up/down motion represents a combination of linear motions, the total motion time can be optimized by designing the program to perform circular motions; however, the superiority of the results compared to the proposed method would not change.

4.4. Discussion

Figure 10 shows the kernel density-estimated distribution of only the $\theta$ component of particles sampled at the last contact before convergence (shown in Figure 7). This figure shows one large peak at approximately −60 and a slightly smaller peak at approximately 30. When the postures of the rectangular container are 30 and −60, as shown in Figure 11, the robot cannot distinguish these two postures from the force information when contacting the edge indicated by the arrow from the inside. For this reason, the distribution of the $\theta$ component of the particles sampled from the observation model became multimodal even though the true posture of the object was 30, causing the variance of the updated $\theta$ distribution to increase. If the object has a characteristic shape at the aperture and the sensor can obtain observation information that reflects this characteristic, then the variance of $\theta$ can be reduced.

Figure 12 shows the progress of the particle distribution over five contact actions in an experiment with the SD strategy. This experiment satisfied the termination condition, as the estimates converged after five contact motions. Based on the initial distribution, the hand descended outside the container; the randomness of the contact action allowed the robot to search a wide region, providing useful contact/non-contact information to refine the estimate. The current contact motion selection is effective when the hand is positioned inside the container; coincidentally, however, this result also worked well when the hand was positioned outside the container. Based on this result, efficient estimation can be achieved by selecting a contact motion that searches a large region when the hand is positioned outside the container. In such cases, discrete state estimation is necessary because the action strategy can differ from that of the inside case.

Figure 13 shows the transition of the variance of $\theta$ by repeated contact motions in one experiment with each object. For the irregular quadrangle object, the variance of $\theta$ decreased after the eighth contact motion, although it was larger than that of the other objects. This is because the four posture patterns could not be distinguished from the sensor information, as shown in Figure 14. Therefore, the experiment with irregular quadrangles required a large number of contact motions, resulting in a large error in $\theta$.

The experimental results for irregular quadrilateral objects showed smaller operation times for SD than for DSB. In the case of SD, the robot repeatedly performs contacts from the outside in a similar position, resulting in a shorter operating distance and shorter operating time. However, this requires a greater number of contact motions than the DSB strategy. In the experiment with irregular quadrilateral objects, about 45 s was required for one contact information processing calculation, compared to about 15 s for one non-contact information processing calculation. Therefore, it is more important to reduce the number of contact motions than the operation time.

5. Conclusions

To realize the task of inserting a robotic hand into a container, the pose of the aperture was estimated using a probabilistic state estimation method. In the case of rectangular and regular triangular objects, the following conclusions were obtained. Although the estimation error of the posture was sometimes large, the robot was able to estimate with an average error of less than 17 degrees and an average positional error of less than 13 mm. With this accuracy, the hand can be inserted close to the center of gravity of the container aperture. In the proposed method, the robot estimates the discrete positional relationship between the hand and container and selects the action according to the discrete state, thereby improving the estimation efficiency. Compared with other methods, the proposed method was superior in terms of the number of contact motions and total operation time. Furthermore, the proposed method showed stable estimation efficiency with small variance in the number of contact motions and total operation time. Although the experiment with irregular objects resulted in a larger estimation error for $\theta$, the superiority of the proposed method remained clear. Even with the proposed method, at least one up-down motion is required when the contact motion is started from the outside, and at least five contacts are required. It would be possible to achieve more efficient estimation by changing the hardware, for instance by increasing the number of force sensors.

The SD method repeats contact actions from outside the container, and the estimation sometimes converged after a small number of contact motions. This means that if a strategy involving contact action from outside the container were properly designed, it could provide efficient estimates without needing to transition to the inside. In this case, it would be necessary to estimate the discrete state in which the hand was positioned outside the container, further highlighting the superiority of the proposed method. In future work, we would like to verify our proposed system with an object featuring a deformable aperture, such as a bag, and to utilize a sensor that can acquire rich information. By integrating the visual and tactile senses, it should be possible to realize more accurate and shorter estimation times for transparent bags. When the aperture exists in a three-dimensional space, the pose representation becomes a high-dimensional vector, and the total number of particles used in the estimation method must be increased. In principle, it would be possible to increase the dimensionality in the proposed system, although a method for reducing the amount of calculation would be required. Although we estimated the pose in this paper through a wide range of contacts with the object, it would also be possible to estimate the pose in a partial space that allows the hand to be inserted, potentially reducing the amount of calculation.