Multi-Contact Particle Filter with Exploration Particles (MCP-EP). Contact locations ($\mathbf{r}_{c,i}$) and forces ($\mathbf{F}_{ext,i}$) are estimated using proprioceptive sensors (JTS, base F/T sensor), with a motion-model using preprocessed mesh data and a measurement-model with the base F/T sensor.

Challenges

The goal is to estimate contact locations ($\mathbf{r}_{c,i} \in \mathbb{R}^3$) and forces ($\mathbf{F}_{ext,i} \in \mathbb{R}^3$) using proprioceptive sensors, the joint torque sensors (JTSs) and a base F/T sensor. The external joint torque and the wrench at the base caused by the contact forces are

Finding the contact locations and forces that best explain $\hat{\mathbf{W}}$ is a nonlinear optimization problem, since $\mathbf{r}_c$ and $\mathbf{F}_c$ appear as a cross product. Particle filter (PF)-based approaches have been proposed for this problem, but two challenges remain:

• Run-time: the run-time is typically slow, mainly due to the computation time in the motion model of the PF (e.g., about 10 Hz for a 30-DOF humanoid robot, and about 200 Hz for a 7-DOF robot arm).
• Singularity: multiple solutions that explain all sensor measurements may exist. Such singularities may appear quite frequently when only JTSs are considered. For example, if a contact is on link 1 and the robot is equipped with only JTSs, the sensing DOF is 1, and the contact force and location cannot be estimated.

Key idea: Multi-Contact Particle Filter with Exploration Particles (MCP-EP)

• Run-time: we improve the run-time of the PF with a mesh preprocessing that computes mesh data offline (e.g., distance between all face pairs). This can only be done for each link separately, and consequently, particles cannot be updated across links. Therefore, we modified the PF by introducing exploration particles, which are forced to investigate adjacent links.
• Singularity: we mitigate the singularity by introducing an F/T sensor at the base of the robot. With this setup, we can estimate a contact at any link including links 1 and 2, and can estimate multiple contacts even for a collaborative robot which has only a few DOF.

Technical components

1. QP-based contact force identification

Assuming a point contact, the $i$th contact force is constrained to lie inside a friction cone, which is approximated as a polyhedral cone, $\mathbf{F}_{c,i} = \mathbf{A}_{c,i}\mathbf{f}_{c,i}$. If, in addition, $\mathbf{r}_c$ is fixed, the problem becomes a convex QP in which the contact force is the only decision variable:

$$\min_{\mathbf{f}_c} \big\| \hat{\mathbf{W}} - \mathbf{Q}^{\top}\mathbf{f}_c \big\|^2 \;\; \text{s.t. } f_{c,i,1}, \ldots, f_{c,i,4} \le 0$$

By virtue of the base F/T sensor, 6 sensing DOFs are added to the QP regardless of the contact location.

Point contact assumption realized by the friction cone formulation, approximated as a polyhedral cone with support vectors $\mathbf{a}_{c,i,j}$.

2. Motion-model with mesh preprocessing

Mesh preprocessing. Non-contact parts are eliminated, and an isotropic remesh algorithm makes the size of the faces almost uniform. The position of a particle is represented using the link and face index number, (link#, face#).

Motion-model with preprocessed mesh data. The geodesic distances for all face pairs are computed offline and stored in ascending order. Using this data structure, the random walk is applied simply by changing the face index of a particle (about 0.06 µs to update one particle).

3. Measurement-model

A smaller QP error implies that the particle well-explains the sensor measurements, so the weight of a particle is defined using the QP error, $w^{[m]}_i \propto \exp\!\big(-\alpha\,\mathrm{QP}(\hat{\mathbf{W}} \mid \mathbf{x}^{[m]}_i, \mathbf{x}^{*})\big)$. In the multi-contact case, instead of evaluating all possible pairs of particles, particles $\mathbf{x}^{*}$ are sampled from the other particle sets considering the associated weights.

4. Exploration particles

While the distance between faces in the same link is constant, the distance between faces in different links changes as the joint rotates, so it cannot be precomputed. Consequently, the motion model changes face#, but not link#.

Exploration particles (yellow) are forced to investigate adjacent links. If the maximum weight of a particle set is lower than a threshold, one particle is randomly discarded from the basic set (pink), and a new particle with an adjacent link# is added to the explore set. Exploration particles do not undergo resampling, so basic particles can be resampled around them on the correct link.

5. Manage-particle-set

Results

Simulation validation

The algorithm is implemented in the MuJoCo simulator with a 7-DOF Franka Emika Panda robot arm equipped with JTSs, and tested as a single-thread C++ program. Single, dual, and triple contact scenarios are simulated 10000 times each. At each run, the robot configuration was initialized randomly, and a 20 N force in the friction cone was applied at a random position (up to 1 contact per link). A case with ≤ 2.25 cm localization error (roughly 5 faces) is classified as a success.

Real-world experiments (supplementary)

Real-world single, dual, and triple contact experiments on a Kinova Gen3 robot arm. These are not part of the simulation validation reported in the paper.

MCP-EP was later employed to establish real-time multi-contact feedback in Multi-Contact Feedback MPC (ICRA 2024), and used as a baseline in ContactFlowFilter (T-RO 2026).

Full video

BibTeX

@inproceedings{han2023proprioceptive,
  title={Proprioceptive Sensor-Based Simultaneous Multi-Contact Point Localization and Force Identification for Robotic Arms},
  author={Han, Seo Wook and Kim, Min Jun},
  booktitle={IEEE International Conference on Robotics and Automation (ICRA)},
  pages={12099--12105},
  year={2023}
}