Whisker-Inspired Tactile Sensing for Contact Localization on Robot Manipulators

Fig. 2A shows an overview of our sensor design and components. The sensor consists of a thin super-elastic nitinol wire bonded to a compliant silicone base that senses bending moment on the wire. Two variations of our sensor design are used in this work, one uses a straight nitinol wire (0.2 mm diameter and 55 mm long) and a second one uses a custom-shaped curved wire (0.2 mm diameter, 20 mm in arc radius and 60 mm in arc length). These wires are attached at one end to a neodymium permanent magnet through a 3D printed coupling. The magnet is axially magnetized with a field direction coinciding with the axis. The magnet and wire are supported by a hollow dome of silicone rubber that promotes rotation over translation as the wire is loaded in contact. The dome is centered over a triaxial Hall effect sensor (Melexis MLX90393) mounted to a PCB.

Contacts along the whisker produce rotations at the base, which change the magnetic flux measured by the Hall effect sensor (Fig. 2B). The transduction principle is similar to that in [26] but with substantially lower stiffness (0.17 mNm/rad v.s. 3.2 mNm/rad) and designed for incorporation into an array.

We performed a COMSOL Multiphysics analysis to investigate the effects of magnet motion on the Hall effect sensor. As seen in Fig. 2C, the change in magnetic flux in the Y sensing axis is nearly linear when the motion of the magnet is pure rotation. In contrast, if the rotation is coupled with translation in the direction of applied force then the change in flux is non-linear and flux density change is substantially smaller. We therefore designed the compliant silicone base to favor rotational displacements over translation when the radial loads are applied to the wire. An advantage of this design is that it allows one to sense deflections even when using a very thin and flexible wire, which promotes non-intrusive sensing. Figure 2D shows that the center of rotation (center of blue region indicating low displacement) is very close to point $P_{r}$.

The chosen sensor design parameters are: dome thickness: 0.6 mm; dome height: 3.25 mm; whisker length: 55 mm. While we cannot claim they are optimal, they appear to provide a favorable combination of monotonic and approximately linear sensor response and large signal/noise ratio without saturation for anticipated contacts.

The Hall effect sensor (MLX90393) is sampled at 250 Hz through SPI communication with a Teensy 4.0 (ARM Cortex-M7 at 600 MHz). It is possible to sample an array of 20 sensors (3-axis each) at this frequency. Data arrays are sent to a computer through USB serial communication at the same rate. We also designed each array with a common-mode reference (Fig. 4A) to compensate for the geomagnetic field.

In order to determine how to filter the sensors we performed an experiment with sensors mounted on a link attached to a Universal Robot UR10e arm. The link is commanded to move the sensor in free-space following an arbitrary trajectory – in part, to test the effects of induced vibrations from robot actuators. At the end of the trajectory, the whisker is moved to make contact with an object causing a large deflection and later breaking contact, causing the whisker to return to its nominal shape, as it would happen in a use-case. Figure 4 shows a typical time series plot of sensor signal, with small vibrations due to robot motion, followed by a large increase due to contact and a resonant vibration as the whisker breaks contact. Vibrations from robot motion can be mostly ignored as they account for less than 3% of the maximum sensor signal. The ringing oscillation on breaking contact is due to the nitinol wire, and occurred at 26 Hz. We applied a 6th order Butterworth band-pass IIR filter with a passband frequency of 20 to 32 Hz, and used a threshold of the filtered output to detect these lost-contact events.

We begin by summarizing the assumptions made to limit the scope of the problem.

1. 1.

   Objects that come into contact are immobile in the world reference frame. In other words, if the robot does not move then contact locations do not change.

2. 2.

   There is at most one contact point on a whisker with the environment at any time. This will be true for convex objects.

3. 3.

   Frictional forces along the whisker are negligible in terms of their effect on the sensor. This again will generally be true given the nitinol whisker material.

4. 4.

   Motion of the robot as well as deflections of whiskers are in a 2D plane. This is generally true for objects that are flat in the direction of gravity, which we limit to in this work.

In the following derivation we follow the conventions in Murray et al. [29] with lowercase boldface fonts for vectors, uppercase boldface fonts for matrices and $\{F\}$ denoting a reference frame F. Lowercase letters also refer to reference frames. For example, $\bm{v}^{a}_{ab}$ is the linear velocity of reference frame A relative to B (subscript ${ab}$) as viewed in reference frame A (super-script $a$).

Let $\{B\}$ be the reference frame of the sensor base and $\{S\}$ be the spatial (world-fixed) reference frame. We define our process and sensor model as follows

	$\displaystyle\bm{x}_{k+1}$	$\displaystyle=\bm{A}\bm{x}_{k}+\bm{B}\begin{bmatrix}\bm{v}^{b}_{sb}\\ \bm{\omega}^{b}_{sb}\end{bmatrix}+\bm{w}_{k}$		(1)
	$\displaystyle y_{k}$	$\displaystyle=g(\bm{x}_{k})+\bm{\nu}_{k}$		(2)

Where $\bm{x}_{k}=\bm{p}_{c}$ is the position vector of the contact point relative to the origin of $\{B\}$ at time-step $k$. $y_{k}$ is the sensor measurement at time-step $k$. $\bm{v}^{b}_{sb}$ and $\bm{\omega}^{b}_{sb}$ are the linear and angular velocity, respectively, of sensor base $\{B\}$ relative to world-fixed frame $\{S\}$ as viewed in the body frame. Process and sensor noise are modelled as Gaussian white noise where $\bm{w}_{k},\bm{\nu}_{k}\sim\mathcal{N}(0,\,\sigma^{2})$. $g:\mathbb{R}^{2}\to\mathbb{R}$ is our sensor model that maps contact location to moment measurements which we will elaborate on in Section IV-B.

To define the process model, we first find the velocity of point $p_{c}$ relative to $\{S\}$ as viewed in the body frame $\{B\}$ denoted as $\bm{v}^{b}_{sp_{c}}$. Let’s consider $p_{c}$ as a point with respect to $\{B\}$, and define a point $b_{p}$ that is instantaneously coincident to $p_{c}$, but fixed in $\{B\}$. We can find the linear velocity of $p_{c}$ with respect to the spatial frame as

	$\displaystyle\bm{v}^{b}_{sp_{c}}$	$\displaystyle=\bm{v}^{b}_{sb_{p}}+\bm{v}^{b}_{bp_{c}}$		(3)
		$\displaystyle=\bm{v}^{b}_{sb}+\bm{\omega}^{b}_{sb}\times\bm{b}_{p}+\bm{v}^{b}_{bp_{c}}$		(4)

where $\bm{v}^{b}_{sb_{p}}$ is the linear velocity of point $b_{p}$ relative to $\{S\}$ as viewed in the body frame, $\bm{v}^{b}_{sb}$ is the linear velocity of $\{B\}$ relative to $\{S\}$ as viewed in the body frame, $\bm{\omega}^{b}_{sb}$ is the angular velocity of $\{B\}$ with respect to $\{S\}$. $\bm{p}_{c}$ is the position vector of point $p_{c}$ relative to $\{B\}$’s origin.

Given our assumption that the contact point remains static in the spatial frame (i.e. $\bm{v}^{b}_{sp_{c}}=\bm{0}$), the contact point velocity relative to $\{B\}$ is

	$\displaystyle\bm{v}^{b}_{bp_{c}}$	$\displaystyle=-\bm{v}^{b}_{sb}+-\bm{\omega}^{b}_{sb}\times\bm{b}_{p}$		(5)
		$\displaystyle=-\begin{bmatrix}\bm{I}&[\bm{p}_{c}]\end{bmatrix}\begin{bmatrix}\bm{v}^{b}_{sb}\\ \bm{\omega}^{b}_{sb}\end{bmatrix}$		(6)

where $[\bm{p}_{c}]$ is the skew-symmetric matrix of vector $\bm{p}_{c}$ and $\bm{I}$ is the identity matrix.

As the sensor is attached on a robot link, $\bm{v}^{b}_{sb}$ and $\bm{\omega}^{b}_{sb}$ can be found through

$\displaystyle\begin{bmatrix}\bm{v}^{b}_{sb}\\ \bm{w}^{b}_{sb}\end{bmatrix}=\bm{J}^{b}_{sb}(\bm{q})\bm{\dot{q}}$

(7)

where $\bm{J}_{sb}^{b}$ is the body Jacobian of $\{B\}$ relative to $\{S\}$, and q is the vector of manipulator joint angles.

With the velocity of the contact point with respect to $\{B\}$ defined, we can express the process model as

$\displaystyle\bm{x}_{k+1}$

$\displaystyle=\bm{x}_{k}+\delta_{t}\bm{v}^{b}_{sp_{c}}=\bm{x}_{k}-\delta_{t}\begin{bmatrix}\bm{I}&[\bm{p}_{c}]\end{bmatrix}\bm{\xi}^{b}_{sb}+\bm{w}_{k}$

(8)

which gives as $\bm{A}=I$ and $\bm{B}=-\delta_{t}\begin{bmatrix}\bm{I}&[\bm{p}_{c}]\end{bmatrix}$ in Eq. 1.

In the case when contacts are not static, such as when contact travel along the contour of large radius of curvature objects, the velocity of the contact point relative to the world $\bm{v}^{b}_{sp_{c}}$ is non-zero. However, since we assume no prior knowledge of the object shape and location, the contact velocity is unpredictable. As inferences are executed very fast (250 Hz) contact points cannot move far between iterations, so we choose to make the static assumption and correct for errors using the sensor model.

In order to find the sensor model, $g:\mathbb{R}^{2}\to\mathbb{R}$, which maps contact point locations, $\bm{x}_{k}$, to predicted moments measurements, $y_{k}$), we develop a calibration platform and procedure. To gather data for this mapping we used a calibration stage composed of two orthogonal prismatic joints and a thin rod (steel dowel pin of 0.4 mm diameter) attached vertically at the tip of the end-effector (illustrated schematically in Fig. 5A). The position of the dowel pin is measured with linear optical encoders (US Digital EM-2) at a resolution of 0.0064 mm/count. During data collection, position and magnetic sensor data are recorded as the end-effector of the stage is driven to deflect the whisker with the dowel pin. This procedure is then repeated for different positions of the dowel pin by tracing an arbitrary trajectory that spans the sensing region. For straight whisker sensors, data are collected for contacts on the left and right side of the whisker. For curved whiskers, data are collected for contacts that deflect the whisker towards the center of the arc.

We used a 5th-order bivariate polynomial model to fit the calibration data for both straight and curved sensors. Fig. 5B shows level set plots of the models for both straight and curved whisker sensors. Polynomial regression for the straight whisker data resulted in a R-squared value of 0.9987 and Root-Mean Squared Error (RMSE) of 0.503 (max. signal of 32). Regression for the curved whisker data resulted in a R-squared value of 0.9895 and RMSE of 1.348 (max. signal of 82).

While our 2D sensor calibration model works for the scenarios we consider here, in some real world cases deflections of the whisker out of the plane may occur due to contact with surfaces that are not vertical. As discussed in Sec. VI we can extend the calibration to include such effects.

To track contact locations from a sequence of base moment measurements, we use Bayesian filtering in a recursive algorithm that infers the state distribution from a history of sensor data and control inputs. The posterior distribution is defined as

	$\displaystyle b(\bm{x}_{t})$	$\displaystyle=p(\bm{x}_{t}\mid\bm{z}_{1:t},\bm{u}_{1:t})$		(9)
		$\displaystyle=\eta\;p(\bm{z}_{t}\mid\bm{x}_{t},\bm{u}_{t})\int p(\bm{x}_{t}\mid\bm{x}_{t-1},\bm{u}_{t})b(\bm{x}_{t-1})d\bm{x}_{t-1}$		(10)

where $\eta$ is a normalization factor, $b(\bm{x}_{t-1})$ is the prior distribution, and $p(\bm{z}_{t}\mid\bm{x}_{t},\bm{u}_{t})$ and $p(\bm{x}_{t}\mid\bm{x}_{t-1},\bm{u}_{t})$ can be obtained from the sensor and process model, respectively.

We implemented three different non-linear Bayesian filters to compare their performance: Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF) and Particle Filter (PF). A process noise covariance of $1e^{-5}I_{2}$ was empirically found to work well for EKF and UKF. This low noise is reasonable given the accuracy of the optical encoders and calibration system. The sensor noise variance was chosen to be 0.25 which was found through the RMSE of the calibration results reported previously. The process and sensor noise covariance for PF $1e^{-3}I_{2}$ and 1 respectively, which were also found empirically to track with low error. We used N=1000 particles for PF.

As mentioned previously, when the actual contact location travels along the surface of an object, our process model described in Sec. IV-A will be inaccurate which may lead to divergence of the estimate. We cannot model this behavior, but we can use known techniques for compensating for model errors. These include adding fictitious process noise or using a Fading Memory (FM) filter [30, p. 139]. Both are used to increase the predicted covariance, but do so differently. FM scales the prior covariance while fictitious process noise adds a constant positive variance to the diagonals. FM was empirically found to produce better results in our application. We implemented this method on the Bayesian filters by scaling the prior covariance by a factor $\alpha=1.004$ at every time-step.

In addition to implementing these filters, we compared tracking performance to a baseline by Solomon et al. [13]. Similar to our method, this baseline estimates contact points at every time-step but with estimates that are deterministic rather than probabilistic (i.e. single values rather than a distribution). Another distinction between our method and this baseline is that our sensor model is calibrated, while the baseline uses a small deflection Euler-Bernoulli beam model. In our implementation of this baseline, we used our calibrated sensor model to obtain predicted moments ($M_{i+1}$) and the arc-length to torque ratio ($\frac{ds}{M_{i}}$) which are used for the estimation correction step. For more detail on the baseline please refer to [13].

To compare the tracking performance of all algorithms, we collected data from two experiments, one using straight whiskers and one using curved whiskers. Each experiment consisted of 10 trials of data collected at 250 Hz using the 2-DOF calibration stage. In each trial, the whisker makes contact and stays in contact with the thin dowel pin as it traces an arbitrary trajectory. Duration of each trial was on average 15 seconds.

Initial conditions used for localization were the same across all conditions. The initial prior mean $\mu_{0}$ was set to be a constant 5 mm offset in the $\hat{b}_{y}$ direction (illustrated in Fig. 5) from the ground truth. This is done to replicate error in initial estimate of the contact point along the whisker, which is realistic in practice due to sensor noise, hysteresis, model error, etc. Offset in the $\hat{b}_{x}$ direction is less important as, for a straight whisker, it can be captured accurately at the instance contact is detected. For the Bayesian filters, the initial prior covariance matrix was set to be $Q=\sigma^{2}I_{2}$, where $\sigma^{2}=10$. This large initial variance is a conservative choice to indicate high uncertainty in the initial contact location estimate. Inference for all methods is executed at sensor sampling rate of 250 Hz.

Results with the three Bayesian filtering and baseline methods are compared in Table I with tracking error over time for one trial shown in Fig. 6. While we show EKF, UKF and PF results for a straight and curved sensor, the baseline was executed only for the straight whisker case as this was an assumption in that model.

In the straight whisker experiments, all Bayesian filters tracked the ground truth contact location with a lower Euclidean distance compared to the baseline on average. UKF had the lowest average error of 0.838 mm with a std. dev. of 0.603. In all trials, UKF converged to within 1 mm of the true location within 0.7 seconds for both straight or curved whisker experiments. Fig. 6 shows this convergence for one trial.

The results show that our Bayesian filtering methods are able to integrate sensor measurements and proprioceptive input to quickly infer contact location with relatively high accuracy, with EKF and UKF tracking with sub-millimeter accuracy on average. High accuracy tracking on both straight and curved whiskers shows that the Bayesian filtering method generalizes to both sensor designs even though they have different sensor models. These methods outperform the baseline which, to the authors’ knowledge, is the closest work to that presented here. As mentioned in the cited paper, the baseline method requires an accurate estimate of the initial contact to perform well. However, in practice it is difficult to obtain accurate priors on contact location along the whisker. Some work has investigated using small angle deflection during initial contact to estimate initial location [11] but this can still introduce errors due to object curvature, sensor noise or model errors. Obtaining good priors is even more challenging when using non-straight whiskers. Our curved whisker tracking results show that average errors are close to those in the straight whisker experiments.

To test the Bayesian filters on moving contacts, we collected data for contacts between an initially straight whisker and objects of different shapes and sizes. This experiment was performed with the same setup as the previous experiment, however, objects of different shapes were moved into contact with the whisker, instead of the thin rod. In each trial, a whisker was deflected by bringing it into contact with an object. The whisker was moved laterally back and forth three times, such that the contact location traveled over the surface, tracing the contour of the object.

We selected the filter with the lowest mean error (UKF) to execute the contact localization for these experiments. Four objects were used: a rectangular prism (width: 30 mm, length: 40 mm), a small cylinder (diameter: 30 mm), a large cylinder (diameter: 100 mm) and an octagonal prism (side length: 12.4 mm). As ground truth we use the known location and shape of the object, allowing us to compare the contour predicted by our tracking algorithm. Tracking results can be visualized by how closely the contact location traces the object contour sweeping back and forth three times. Estimated contact locations are shown as a scatter plot in Fig. 7 overlaid with the ground truth object contour. We computed the minimum distance of estimated contact points to the surface of each object contour. Average distance for each shape was: large cylinder = 0.460 mm (std. dev. 0.389), small cylinder = 0.378 mm (std. dev. 0.290), octagonal prism = 0.521 mm (std. dev. 0.436) and rectangular prism = 0.744 mm (std. dev. 0.670).

It can be seen in Fig. 7 that, for the cylinders and octagonal prism, the filter produces three distinguishable sweeps along the object contour. For the case of the cuboid, the filter correctly estimates the contact location to remain on one corner. These results show that our tracking method is able to extract object contours in a real application and allow for object shape and location identification as a robot navigates an unstructured environment.