# Autonomous Human Tracking using UWB sensors for mobile robots: An Observer-Based approach to follow the human path

Mathieu Deremetz, Roland Lenain, Jean Laneurit, Christophe Debain, Thierry Peynot

► To cite this version:

Mathieu Deremetz, Roland Lenain, Jean Laneurit, Christophe Debain, Thierry Peynot. Autonomous Human Tracking using UWB sensors for mobile robots: An Observer-Based approach to follow the human path. 2020 IEEE Conference on Control Technology and Applications (CCTA), Aug 2020, Montreal, France. pp.372-379, 10.1109/CCTA41146.2020.9206153. hal-05009002

HAL Id: hal-05009002

https://hal.inrae.fr/hal-05009002v1

Submitted on 27 Mar 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
<figure><img src="image_2.png" /></figure>

Queensland University of Technology
Brisbane Australia

This may be the author's version of a work that was submitted/accepted for publication in the following source:

Deremetz, Mathieu, Lenain, Roland, Laneurit, Jean, Debain, Christophe, & Peynot, Thierry (2020)

Autonomous human tracking using UWB sensors for mobile robots: An observer-based approach to follow the human path.

In *Proceedings of the 2020 IEEE Conference on Control Technology and Applications (CCTA 2020)*.

Institute of Electrical and Electronics Engineers Inc., United States of America, pp. 372-379.

This file was downloaded from: https://eprints.qut.edu.au/210210/

© 2020 IEEE

© 2020 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.

**License:** Creative Commons: Attribution-Noncommercial 4.0

**Notice:** Please note that this document may not be the Version of Record (i.e. published version) of the work. Author manuscript versions (as Submitted for peer review or as Accepted for publication after peer review) can be identified by an absence of publisher branding and/or typeset appearance. If there is any doubt, please refer to the published source.

https://doi.org/10.1109/CCTA41146.2020.9206153
# Autonomous Human Tracking using UWB sensors for mobile robots:
An Observer-Based approach to follow the human path

Mathieu Deremetz<fnref n="1" />, Roland Lenain<fnref n="1" />, Jean Laneurit<fnref n="1" />, Christophe Debain<fnref n="1" /> and Thierry Peynot<fnref n="2" />

**Abstract**— Assistance robots are rising as a promising solution to help workers in everyday life, for many applications. In particular, many outdoor tasks, such as manual harvesting or carrying heavy loads, may benefit from the use of a mobile robot following a human worker. The literature offers various frameworks and systems for target tracking, using several kinds of sensors and control algorithms. Most of them are based on vision, which is sensitive to lighting conditions, or LIDAR. Both of these sensors require to keep the human in line of sight to be tracked, and have limitations in some challenging environmental conditions encountered outdoors, such as in rough weather or in the presence of airborne dust or fog. Moreover, existing approaches are often focused on direct tracking and may lead the robot to cross the human trajectory. This is penalising when considering off-road assistance, especially in agriculture, where the environment is often narrow (because of crop rows), and require many obstacle avoidance situations. In this paper an algorithm based on Ultra-Wide-Band technology is proposed in order to ensure a relative localisation without the need for a direct view of the target. The method approximates locally the trajectory of the human leader as a circle in order to follow the path achieved by the human, without direct communication nor absolute localization system. This is made possible thanks to the on-line reconstruction of the leader's variables (namely, its velocity and angular course), using a state observer. The effectiveness of this adaptive approach is demonstrated through full-scale experiments.

## I. INTRODUCTION

Numerous areas may benefit from the tremendous progress achieved recently in the field of autonomous driving [1]. Examples include transportation, defense, or agriculture [2]. In this latter area, beyond transportation of people or automation of machines, mobile robots can be very helpful to farmers and even provide them with some comfort [3]. This is especially the case of assistance robots, such as the one developed in [4] for domestic use.

In the framework of agriculture, the objective of assistance robots is mainly focused on helping farmers to carry heavy loads (during manual harvesting for instance). Contrarily to harvesting robots, such as those proposed in [5], this work proposes to use human-following robots. Several target tracking approaches exist to achieve such a task, using different kinds of technology, such as vision [6] or TOF-Camera [7]. This has given interesting results indoors. Nevertheless, the sensitivity to changing lighting conditions makes it difficult to use vision outside. Moreover, if Lidar solutions [8] provide better results outdoors, they require continuing visual contact

between the robot and the user, which can be difficult to ensure in agriculture, where the robot may be hidden from the view of the operator, e.g. in a different crop row.

In order to avoid this drawback, a solution based on Ultra-Wide-Band (UWB) beacons (such as used in [9] for formation control of vehicles) is proposed in this paper. It allows to track the trajectory of the target, without recording explicitly its successive positions, such as in [10]. Contrarily to many prior approaches, the control architecture proposed in this paper does not regulate directly an error in a local framework, but computes a desired lateral position, allowing to follow a trajectory, instead of regulating directly a relative position. This can prevent the robot from cutting corners when the target is performing harsh maneuvers.

However, this requires the knowledge of the relative orientation and the speed of the target, which are difficult to obtain by direct measurement using a single UWB beacon (on a human, it is not practical to consider setting up several antennas, similarly to what was done on a leader vehicle in [9]). As a result, an observer, computed using a similar methodology to that proposed in [11] has been designed in order to estimate the target behavior. This allows to obtain accurate target tracking, following a similar trajectory to the human that is being tracked. The efficiency of the proposed approach is investigated and demonstrated in this paper through full-scale experiments.

The paper is composed as follows. First the considered framework for human/robot tracking is proposed, including notations, modeling and assumptions. The measurement system using UWB is then detailed, allowing to highlight the benefits of this technology in outdoor application. Then the indirect estimation of unmeasured variables (representative of the target behavior) is proposed. The control law allowing for the longitudinal and the lateral regulation is then derived. Finally, experimental results show the efficiency of the proposed control strategy.

## II. HUMAN FOLLOWING FRAMEWORK

### A. *Assumptions and notations*

In this paper, the objective is to follow the trajectory followed by a human walking without recording its successive positions, which imposes an absolute localization system and explicit communication. For that purpose, we consider a leader-follower framework, such as depicted in Fig. 1. In this framework, both the pedestrian and the robot are considered to move on flat ground. The robot is assumed to be symmetric and is considered to be rolling without sliding condition. As a result, the robot can be viewed as a bicycle

<footnote n="1">¹Université Clermont Auvergne, INRAE, UR TSCF, 63178 Aubière, France `FirstName.Name@inrae.fr`</footnote>

<footnote n="2">²Queensland University of Technology (QUT), 2 George St., Brisbane QLD 4001, Australia `t.peynot@qut.edu.au`</footnote>
<figure><img src="image_3.png" /><figcaption>Fig. 1. Modelling of a generic mobile robot in Frénet frame</figcaption></figure>

(see [11]) and the orientation of its speed vector is supported by its wheels. In particular, the speed vector, denoted $v$ at the center of the rear axle, defines the robot's heading. The second control variable is the steering angle, denoted by $\delta$ on the figure. The position of the pedestrian $P$ (depicted in blue) is denoted by $x$ and $y$ in the mobile robot's frame. The pedestrian is assumed to be moving at a speed of $v_P$, and the direction of its motion makes an angle $\theta$ with the robot's orientation.

Contrarily to the control strategy proposed in [9], where a follower robot has to regulate its position directly with respect to a leader (i.e. regulates $x$ to a desired distance and $y$ to 0), here we consider that the follower has to be on a circle depicted in red on Fig. 2, defined by the following elements:

* the center $C$ is the intersection of normals to the pedestrian's direction and to the robot heading crossing at the middle of the rear axle,

* it passes through point $P$ (pedestrian), which implies that the radius $R$ is equal to distance $[CP]$.

Using these notations, one can define the objective of the pedestrian tracking in the framework of the mobile robot. From a longitudinal point of view, the objective is simply to ensure the convergence and the regulation of $x$ to some desired distance $x^d$. From a lateral point of view, we consider that the robot follows the pedestrian if the rear axle is on the circle previously defined ($\epsilon = 0$). In terms of lateral position, this condition can be expressed as:

$$ \epsilon = y - x \tan \frac{\theta}{2}. \qquad (1) $$

For more generality, one may want that the robot be laterally on a side of the pedestrian. Then one can define a desired lateral position $y^d$. In such condition the objective is to ensure the convergence of $\epsilon \to y^d$.

## B. Motion equations

Thanks to the notations introduced previously, the motion model of the robot with respect to the pedestrian may be simply obtained from kinematics considerations [11]. One

can express the evolution of $x$ and $y$ with respect to the angular velocity of the robot $\omega$, such that:

$$ \begin{cases} \dot{x} = v + v_P \cos \theta + y \omega \\ \dot{y} = v_P \sin \theta + x \omega \end{cases} \qquad (2) $$

This model links the variation of state variables $x$ and $y$ to the control variable $v$ explicitly, and $\delta$ implicitly. The yaw rate of the robot $\omega$ relies on the steering angle $\delta$ via the following relationship:

$$ \omega = v \frac{\tan \delta}{L}. \qquad (3) $$

Using this model, the objective is then to find control expressions for $v$ and $\delta$ that ensure the previously defined convergences: $x \to x^d$ and $y \to y^d + x \tan \frac{\theta}{2}$. In the model (2), the latter can be used to know the state vector $\xi = [x \ y \ v_P \ \theta]^T$. The knowledge of $X$ is addressed in the following two sections.

# III. RELATIVE POSITION MEASUREMENT

## A. Background on UWB

The aim of this section is to compute the position $X$ of the leader (the human to follow) in the robot frame. To do this some authors have proposed to use Radio Frequency technology because it is not sensitive to illumination conditions, weather conditions or shape of the ground [12][13]. Furthermore this technology is often inexpensive. In such approach, RF modules (tags) are used, one worn by the human operator and the others embedded on the robot. The leader is located by 2D trilateration principles based on measurement of distances between the leader tag and robot tags. This kind of systems is sensitive to signal propagation problems such as multipath phenomena. These problems are mitigated when using Ultra-Wide-Band technology. However, distance measurement between tags cannot be done simultaneously because communications are usually made on a single channel. Our system, developed in the Baudet-Rob project, provides a solution to deal with “asynchronous” trilateration by combining distance measurements with proprioceptive data coming from the robot in order to obtain a robust and accurate localization of the human operator.

## B. Measurement of the human position

The aim of UWB System is to locate the human who is being followed, relatively to the robot. To that end, four UWB tags are used: one on the person to follow (the leader) and the other three on the robot (the vehicle follower). The calculation of the leader's position relative to the robot is usually solved by a 2D trilateration using the measurements of the three distances between the robot tags and the leader tag. However, the communication between our tags can only be done on a single channel. Consequently, measurements of distance between the tags cannot be done simultaneously. Several milliseconds of latency can be seen between each measure to avoid interference. Due to the displacement of the robot this problem involves unacceptable errors on the relative position estimation because distance measurements have no spatial coherence. Our system proposes to use
odometry of the vehicle and an extended Kalman filter to improve spatial data consistency and to obtain an optimal localization of the leader.

### C. Kalman state vector

The goal of the Kalman filter is to estimate the location of the leader **l** with respect to the body reference frame **b** of the follower vehicle. Therefore, the state vector **X** of the system is the 2D Cartesian position of the leader:

$$X = (x_l^b, y_l^b)^T. \quad (4)$$

Location uncertainties are also computed by the Kalman filter and are represented by a 2x2 covariance matrix **P**.

### D. Kalman prediction step

In this article we assume that the leader is followed by a steering vehicle. Odometry data are used to compute the displacement of this vehicle between two distance measurements and to provide a good estimation of the a priori position of the leader. An Ackermann kinematic model is typically considered for this kind of vehicle:

$$\left\{ \begin{aligned} \dot{x}_r^w &= v_r \cos \theta \\ \dot{y}_r^w &= v_r \sin \theta \\ \dot{\theta}_r^w &= v_r \frac{\tan \delta_r}{L} \end{aligned} \right. \quad (5)$$

where:

* $v_r$, $\delta_r$ and $L$ are the linear speed, the steering angle and the wheel base of the robot, respectively,

* $(x_r^w, y_r^w, \theta_r^w)^T$ is the pose of the follower robot in the world reference frame $w$.

In discrete time, the robot's displacement in the world reference frame between two distance measurements is given by:

$$\left\{ \begin{aligned} x_r^w(k+1) &= x_r^w(k) - \Delta_D \sin(\theta_r^w(k) + \Delta_\theta) \\ y_r^w(k+1) &= y_r^w(k) + \Delta_D \cos(\theta_r^w(k) + \Delta_\theta) \\ \theta_r^w(k+1) &= \theta_r^w(k) + \Delta_\theta \end{aligned} \right. \quad (6)$$

with:

$$\Delta_D = v_r(k)\Delta_T \quad \text{and} \quad \Delta_\theta = v_r(k) \frac{\Delta_T \tan \delta_r(k)}{L}. \quad (7)$$

$\Delta_T$ is the elapsed time between step $k$ and $k+1$, namely the sampling period. These equations describe the movement of the vehicle in the world. However, our problem is to determine the movement of the leader in the reference frame of the vehicle. Assuming that the leader does not move between two distance measurements, the a priori position of the leader can be computed as:

$$\left( \begin{array}{c} x_l^b(k+1) \\ y_l^b(k+1) \end{array} \right) = \mathbf{R}(\Delta_\theta)^T \left( \begin{array}{c} x_l^b(k) - (x_r^w(k+1) - x_r^w(k)) \\ y_l^b(k) - (y_r^w(k+1) - y_r^w(k)) \end{array} \right) \quad (8)$$

Combining Equation 6 into Equation 8, the a priori position can be simply derived from odometry information:

$$\left( \begin{array}{c} x_l^b(k+1) \\ y_l^b(k+1) \end{array} \right) = \mathbf{R}(\Delta_\theta)^T \left( \begin{array}{c} x_l^b(k) + \Delta_D \sin(\Delta_\theta) \\ y_l^b(k) - \Delta_D \cos(\Delta_\theta) \end{array} \right) \quad (9)$$

where $\mathbf{R}(\Delta_\theta)$ is the rotation matrix for the rotation by an angle $\Delta_\theta$ around the z-axis. Of course, the leader may move at the same time. However, his displacement is unknown. Since human displacements are non-holonomic they are usually described by a kinematic stop model:

$$\left\{ \begin{aligned} \dot{x}_l^w &= x_l^b = 0 \\ \dot{y}_l^w &= y_l^b = 0 \end{aligned} \right. \quad (10)$$

This kind of model is available in both world and follower reference frames. Taking into account this constraint, Equation 9 can be rewritten as:

$$\left( \begin{array}{c} x_l^b(k+1) \\ y_l^b(k+1) \end{array} \right) = \mathbf{R}(\Delta_\theta)^T \left( \begin{array}{c} x_l^b(k) + \Delta_D \sin(\Delta_\theta) \\ y_l^b(k) - \Delta_D \cos(\Delta_\theta) \end{array} \right) + \mathcal{N}(\mathbf{0}, \mathbf{Q}_{xy}) \quad (11)$$

where $\mathcal{N}(\mathbf{0}, \mathbf{Q}_{(xy)})$ is a 2D normal distribution with zero mean and the covariance matrix $\mathbf{Q}_{xy}$ is defined by:

$$\mathbf{Q}_{xy} = I(\sigma_{xy}^b \cdot \Delta_T)^2. \quad (12)$$

$\sigma_{xy}^b$ represents the standard deviation of the movement that could be performed by the leader over one second in both $x$ and $y$ directions, and $\Delta_T$ the elapsed time between times $k$ and $k+1$. The equation of the prediction step of the Kalman filter can be easily derived from Equation 11. Then, the a priori state vector $X_{k+1/k}$ at time $k+1$ is given by:

$$X_{k+1|k} = \mathbf{R}(\Delta_\theta)^T \left( X_{k|k} - \begin{pmatrix} -\Delta_D \sin(\Delta_\theta) \\ \Delta_D \cos(\Delta_\theta) \end{pmatrix} \right) \quad (13)$$

and its associated covariance matrix by:

$$\mathbf{P}_{k+1|k} = \mathbf{R}(\Delta_\theta)^T (\mathbf{P}_{k|k} + \mathbf{G}_k \mathbf{Q}_u \mathbf{G}_k^T) \mathbf{R}(\Delta_\theta) + \mathbf{Q}_{xy} \quad (14)$$

where $\mathbf{Q}_u$ is the covariance matrix describing the uncertainties of the proprioceptive data:

$$\mathbf{Q}_u = \begin{pmatrix} \sigma_{\nu_r}^2 & 0 \\ 0 & \sigma_{\delta_r}^2 \end{pmatrix} \quad (15)$$

$\sigma_{\nu_r}$ and $\sigma_{\delta_r}$ are the standard deviation on the linear speed and the steering of the robot, respectively, and

$$\mathbf{G}_k = \begin{pmatrix} -\Delta_T \sin(\Delta_\theta) & -\Delta_D \cos(\Delta_\theta) \\ \Delta_T \cos(\Delta_\theta) & -\Delta_D \sin(\Delta_\theta) \end{pmatrix} \quad (16)$$

### E. Kalman update step

Using Equations 13 and 14 a rough estimation of the position of the leader is computed at each time step. In order to improve this estimation, trilateration principles are kept but using a single distance measurement at each step as they cannot be done simultaneously. Then at time $k$ a distance observation between the follower tag $n$ and the leader tag can be established. Assuming that the leader's tag and leader's locations are the same we obtain:

$$d_{n,k+1} = \sqrt{(x_{l,k+1/k}^b - x_n^b)^2 + (y_{l,k+1/k}^b - y_n^b)^2} \quad (17)$$

where $d_{n,k+1}$ is the distance between the leader and the tag $n$ located on the robot at position $(x_n^b, y_n^b)^T$. Update equations of the Kalman filter can be easily deduced from Equation 17:

$$\begin{align} K_{n,k+1} &= P_{k+1/k} H_{n,k+1}^T (H_{n,k+1} P_{k+1/k} H_{n,k+1}^T + \sigma_d^2)^{-1} \nonumber \\ X_{k+1/k+1} &= X_{k+1/k+1} + K_{n,k+1} (d_{n,k+1} - d_{n,k+1/k}) \tag{18} \\ P_{k+1/k+1} &= (I - K_{n,k+1} H_{n,k+1}) P_{k+1/k} \nonumber \end{align}$$
knowing that Jacobian matrix $H_{n,k+1}$ is:

$$H_{n,k+1} = \left( \frac{X_{k+1/k} Y_{k+1/k}}{d_{n,k+1/k} d_{n,k+1/k}} \right). \quad (19)$$

In order to achieve the “trilateration”, three tags are embedded on the robot. The distance between the first tag and the leader is measured at time $k$ and the distance between the second tag and the leader is measured at time $k+1$, and so on. In this way spatial coherency of distance measurement is ensured and a precise localization of the leader can be obtained.

## IV. OBSERVATION OF THE HUMAN VELOCITY AND RELATIVE HEADING

### A. Observer equation

Using the previously described sensor system, one can easily measure the relative position of the pedestrian. Nevertheless, the model (2) needs to be fed with the pedestrian velocity $v_P$, as well as the relative orientation $\theta$. In order to have an estimation of these variables, let us first decompose the state vector into two parts:

$$\xi = \begin{bmatrix} \xi_{pos} \\ \xi_P \end{bmatrix} \quad (20)$$

where $\xi_{pos} = [x \ y]$ is composed of the known position of the pedestrian in the framework of the robot, while $\xi_{pos} = [\theta \ v_P]$ is constituted of the unknown variables of the pedestrian behavior, which are to be estimated. This estimation is achieved thanks to the observer defined by (21):

$$\left\{ \begin{aligned} \dot{\xi}_{pos} &= f(\xi_{pos}, \hat{\xi}_P) + \alpha_{pos}(\xi_{pos}) \\ \hat{\xi}_P &= \alpha_P(\xi_{pos}) \end{aligned} \right. \quad (21)$$

where $\alpha_{pos}$ and $\alpha_P$ are feedback functions pending on the observed error $\hat{\xi}_P = \xi_{pos} - \xi_{pos}$ and $f(\xi_{pos}, \hat{\xi}_P)$ constitutes the model (2), such that:

$$f(\xi_{pos}, \hat{\xi}_P) = \begin{bmatrix} v + \hat{v}_P \cos \hat{\theta} + y\omega \\ \hat{v}_P \sin \hat{\theta} + x\omega \end{bmatrix} \quad (22)$$

One can show that the observer (21) can ensure the convergence $\hat{\xi} \to \xi$ using the expression defined by (23) for the feedback function. In particular, the convergence $\hat{\xi}_P \to \xi_P$ is ensured, providing a relevant estimation for the two unmeasured variables $v_P$ and $\theta$, required to use the evolution model (2):

$$\left\{ \begin{aligned} \alpha_{pos} &= K_{pos} \xi_{pos} \\ \hat{\xi}_P &= K_P \frac{\partial f}{\partial \xi_{pos}} (\xi_{pos}, \hat{\xi}_P) \xi_{pos} \end{aligned} \right. \quad (23)$$

$K_{pos}$ and $K_P$ are positive diagonal matrices, tuned manually, and defining the settling time of the proposed observer.

### B. Sketch of proof

Let us consider the Lyapunov candidate function $V$ defined by:

$$V = V_1 + V_2 \quad (24)$$

with:

$$\left\{ \begin{aligned} V_1 &= \frac{1}{2} \hat{\xi}_{pos}^T K_P \hat{\xi}_{pos} \\ V_2 &= \frac{1}{2} \hat{\xi}_P^T \hat{\xi}_P \end{aligned} \right. \quad (25)$$

Functions $V_1$ and $V_2$ (and consequently $V$) are clearly positive and their time derivatives are:

$$\left\{ \begin{aligned} \dot{V}_1 &= K_P \hat{\xi}_{pos}^T \hat{\xi}_{pos} \\ \dot{V}_2 &= \hat{\xi}_P^T \dot{\hat{\xi}}_P \end{aligned} \right. \quad (26)$$

From (2) and (21), it can also be derived that:

$$\left\{ \begin{aligned} \dot{\hat{\xi}}_{pos} &= f(\xi_{pos}, \xi_P) - f(\xi_{pos}, \hat{\xi}_P) - \alpha_{pos} \\ \dot{\hat{\xi}}_{\beta} &= \hat{\xi}_P - \alpha_P \end{aligned} \right. \quad (27)$$

At this step, the two following assumptions are introduced.

1. The observation error on the pedestrian behavior (i.e. $\hat{\xi}_P$) is small enough that a first-order Taylor series expansion of function $f(\xi_{pos}, \hat{\xi}_P)$ around $\hat{\xi}_P$ can be considered:

$$f(\xi_{pos}, \xi_P) = f(\xi_{pos}, \hat{\xi}_P) + \frac{\partial f}{\partial \hat{\xi}_P}(\xi_{pos}, \hat{\xi}_P) \hat{\xi}_P.$$

2. The time derivative of the actual pedestrian behavior $\hat{\xi}_P$ is negligible, or at least compensated by the corresponding observer dynamics $\hat{\xi}_P$ imposed by $\alpha_P$.

Considering these assumptions in Equations (27) and replacing also $\alpha_{pos}$ and $\alpha_P$ by their expressions (23) lead to:

$$\left\{ \begin{aligned} \dot{\hat{\xi}}_{pos} &= \frac{\partial f}{\partial \hat{\xi}_P}(\xi_{pos}, \hat{\xi}_P) \hat{\xi}_P - K_{pos} \hat{\xi}_{pos} \\ \dot{\hat{\xi}}_P &= -K_P \left[ \frac{\partial f}{\partial \hat{\xi}_P}(\xi_{pos}, \hat{\xi}_P) \right]^T \hat{\xi}_{pos} \end{aligned} \right. \quad (28)$$

Injecting these expressions into the time derivative of the Lyapunov functions shown in (26) ensures that:

$$\left\{ \begin{aligned} \dot{V}_1 &= -K_P \hat{\xi}_{pos}^T K_{pos} \hat{\xi}_{pos} + \dots \\ & \quad \dots K_P \hat{\xi}_P^T \left[ \frac{\partial f}{\partial \hat{\xi}_P}(\xi_{pos}, \hat{\xi}_P) \right]^T \hat{\xi}_{pos} \\ \dot{V}_2 &= -K_P \hat{\xi}_P^T \left[ \frac{\partial f}{\partial \hat{\xi}_P}(\xi_{pos}, \hat{\xi}_P) \right]^T \hat{\xi}_{pos} \end{aligned} \right. \quad (29)$$

and, in view of (24), the time derivative of the complete Lyapunov function is:

$$\dot{V} = -K_P \hat{\xi}_{pos}^T K_{pos} \hat{\xi}_{pos} \quad (30)$$

Since $K_P$ is a positive scalar and $K_{pos}$ is a positive definite diagonal matrix, $\dot{V}$ is negative and equal to zero if and only if $\hat{\xi}_{pos} = 0$. It then follows that $\hat{\xi}_{pos}$ converges on zero, i.e. the observed position and orientation converge on their corresponding measure. Next, in order to investigate the behavior of $\hat{\xi}_P$, let us inject $\hat{\xi}_{pos} = 0$ into the observer error dynamics (28). It can be established that:

$$\frac{\partial f}{\partial \hat{\xi}_P}(\xi_{pos}, \hat{\xi}_P) \hat{\xi}_P \to 0 \quad (31)$$

As a consequence, when the robot is moving, the observer (21) with the chosen functions (23) ensures that the overall observation error tends to zero: $\hat{\xi} = 0$. In particular, this demonstrates that the estimated pedestrian behavior variables converge on the actual (but unmeasured) ones: $\hat{\xi}_P \to \xi_P$.
As a result, a relevant estimation of pedestrian velocity and relative heading ($\hat{v}_p, \hat{\theta}$) is available and representative of the actual robot motion.

## V. MOBILE ROBOT CONTROL

Using the previously described measurement system and observer, the model (2) is entirely known and can be used to derive control laws for the pedestrian following. One can decompose the control approach into two control laws: one dedicated to the longitudinal behavior, and the second for the lateral motion. The objective of the longitudinal control is to impose the convergence of the longitudinal distance $x$ to some desired offset $x^d$. In order to ensure such convergence, let us consider the longitudinal error $\epsilon_x$ defined such as:

$$ \epsilon_x = x - x^d \quad (32) $$

The regulation of $\epsilon_x$ to 0 may be ensured by imposing the following differential equation:

$$ \dot{\epsilon}_x = K_x \epsilon_x \quad (33) $$

where $K_x$ is a positive scalar defining the settling time for the convergence of $\epsilon_x$.

From a lateral point of view, the objective is to ensure the convergence of $y$ to $x \tan \frac{\theta}{2} + y^d$. If we consider the error

$$ \epsilon_y = y - x \tan \frac{\theta}{2} + y^d \quad (34) $$

this objective may be achieved by finding a control law imposing:

$$ \dot{\epsilon}_y = K_y \epsilon_y \quad (35) $$

with $K_x$ a positive scalar defining the settling time for the convergence of $\epsilon_y$.

The two set points $x^d$ and $y^d$ are considered as slowly variables. As a result, considering the model (2), fed with the observer (21), one can then rewrite the two conditions (33) and (35), such as:

$$ \left\{ \begin{array}{l} v + \hat{v}_p \cos \hat{\theta} + y\omega = K_x \epsilon_x \\ \hat{v}_p \sin \hat{\theta} + x\omega = K_y \epsilon_y \end{array} \right. \quad (36) $$

Considering the two lines of the system (36), one can extract the two control variables ensuring the exponential convergence of $\epsilon_x$ and $\epsilon_y$ to zero. The control law for velocity can indeed be expressed as:

$$ v = \frac{y K_y \epsilon_y - x K_x \epsilon_x - \hat{v}_p (y \sin \hat{\theta} - x \cos \hat{\theta})}{x} \quad (37) $$

This control supposes that $x$ is non null, i.e. that the robot is not on the side of the pedestrian, which is the case in the problem considered as soon as the robot is properly initialized.

For the longitudinal point of view, the control variable is the robot's yaw rate $\omega$, which can be expressed as:

$$ \omega = \frac{K_y \epsilon_y + \hat{v}_p \sin \hat{\theta}}{x} \quad (38) $$

which exists under the same condition as previously expressed. In order to compute the steering angle $\delta$, the invert

of the relationship (3), may be used, leading to the control law:

$$ \delta = \frac{L}{v} \arctan \left\{ \frac{K_y \epsilon_y + \hat{v}_p \sin \hat{\theta}}{x} \right\} \quad (39) $$

This control law may be computed only if the robot velocity is non null. In practice, one can define a minimal velocity $v_{min}$, close to zero. When $v < v_{min}$, the actual control applied to the steering angle then becomes:

$$ \delta = \frac{L}{v_{min}} \arctan \left\{ \frac{K_y \epsilon_y + \hat{v}_p \sin \hat{\theta}}{x} \right\} \quad (40) $$

This singularity does not affect skid steered mobile robots, and is not limiting in practice for car-like mobile robot, thanks to Equation (40). The second conditions $x \neq 0$, cannot be reached in practice when robot the desired lateral offset is null (i.e. $y^d = 0$) as the robot would then be superimposed with the target. Nevertheless, when the desired offset is not null, the application of such an algorithm means that the robot cannot move on the human side.

## VI. EXPERIMENTAL RESULTS

### A. Experimental set-up

In order to investigate the performance of the proposed approach of human robot following, the experimental robot depicted in Fig. 2 was used<fnref n="1" />. It is a fully electric four-wheel-driven vehicle that weighs 550kg, has a wheelbase of 1.2m and may reach the maximal speed of 3m/s.

<figure><img src="image_4.png" /><figcaption>Fig. 2. Experimental setup</figcaption></figure>

The robot is equipped with three UWB beacons. The human to be tracked holds a fourth beacon in order to measure its relative position, while its velocity and the relative orientation are estimated using the observer (21). The parameters applied for the experiments are summarized in Table I.

In order to illustrate the efficiency of the proposed approach, two experiments are shown in this paper. The first one consists in following a human without lateral distance, while the second consists in following the human in the same conditions, with a lateral offset of 1m.

### B. Robot and pedestrian trajectories

The data obtained from the robot's sensors allow to compute the trajectory achieved by the robot (using odometry), and by the human (thanks to UWB). During both experiments, the trajectory are compared in Fig. 3. During

<footnote n="1">¹A video highlighting the proposed results is available at https://stratus.irstea.fr/f/f938acf69f2a4c6b883b/</footnote>
<table><caption>TABLE I
CONTROL PARAMETER USED DURING EXPERIMENTS</caption><thead><tr><td>Parameter</td><td>value</td></tr></thead><tbody><tr><td>Maximal speed</td><td>1m/s</td></tr><tr><td>Desired lateral deviation y<sup>d</sup></td><td>0 and 1m</td></tr><tr><td>Gains K<sub>x</sub> and K<sub>y</sub></td><td>0.5</td></tr><tr><td>Gain K<sub>pos</sub> (reactive control)</td><td>diag(10,10)</td></tr><tr><td>Gain K<sub>P</sub></td><td>5</td></tr></tbody></table>

<figure><img src="image_5.png" /><figcaption>(a) Experiment 1</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>(b) Experiment 2</figcaption></figure>

<caption>Fig. 3. Trajectory followed by the human (in black) and robot (in red) in each experiment.</caption>

both experiments, the pedestrian moves at an almost constant speed, and achieves two half-turns, with two different radius of curvature (8m and 5m), separated by a short straight line. In view of Fig. 3, one can see that the robot properly follows the trajectory achieved by the human. Thanks to the proposed approach described in Fig. 1, consisting in defining a circle based on the estimation of the human motion, the trajectory achieved by the robot is smoothed with respect to the one achieved by the human. This allows to address the fact that the robot cannot achieve some human motions, because of its non-holonomy, without cutting curves, as it is the case when considering classical leader-follower approaches.

### C. Pedestrian variable estimation

Interesting results obtained on the trajectory are obtained thanks to the control law (39), fed with the observer (21). Since the results on observation are quite similar from an observer point of view, only the results related to the first experiments are shown for the reconstruction of pedestrian variables. Thanks to the proposed observer, it is possible to estimate in real time the longitudinal speed of the pedestrian $v_P$, as well as the relative orientation between his course and the robot heading $\theta$. Figure 4 shows the results of the pedestrian estimated thanks to the observer (black line). In order to have a comparison, the actual speed of the robot is also depicted on the same figure.

One can see that an estimation of the pedestrian's speed is available, while it is an unmeasured variable. Thanks to the velocity control law (37), this velocity is of course similar to the one applied and measured on the robot, in order to regulate the desired longitudinal distance $x^d$. The forthcoming results, related to the longitudinal servoing, will show the relevance of such results on the estimation.

<figure><img src="image_7.png" /><figcaption>Fig. 4. Estimated pedestrian speed and measured robot velocity</figcaption></figure>

The observer also allows to estimate the angle $\theta$ between the direction of pedestrian motion and the robot heading. The estimate $\hat{\theta}$ of this angle during the first experiments is reported on Fig. 5 showing the capability of our approach to compute this variable.

<figure><img src="image_8.png" /><figcaption>Fig. 5. Estimated angle between the robot heading and the human's speed vector</figcaption></figure>

One can notably see that during the curves (between 40-80s and 110-180s) the relative orientation stabilizes around 25°, since the robot is following a circle deduced from this orientation. This enables to preserve the lateral servoing as it will be shown in the next section

### D. longitudinal and lateral errors

Figure 3 shows good results from a trajectory following point of view. To go further, one can investigate the results of the lateral and longitudinal error regulation during the first experiment, as the second experiment shows similar results. Figure 6 shows the results obtained on the longitudinal distance $x$ in red, with respect to the desired one $x^d$, depicted in black on the same figure.

<figure><img src="image_9.png" /><figcaption>Fig. 6. Longitudinal distances $x$ and $x^d$</figcaption></figure>

It can be noticed that the robot converges well to the desired distance of 5m during the almost-straight-line section of the trajectory. Nevertheless, during the curve section
(between 40-80s and between 110-180s), the robot tends to get closer. This is due to the neglected variation of the human direction. In the model (2), the yaw rate $\omega$ is in fact the derivative of $\theta$, but is considered in this paper as the robot's angular velocity, since there is no way to measure or estimate properly this variation. As the robot has to follow a circle, the longitudinal distance $y$ is non null and affects the accuracy of the longitudinal regulation. Nevertheless, this lack of accuracy is quite limited (error of about 6%), since it relies on the curvature and the curvature is quite important here.

As it has been mentioned in the modeling section, from a lateral point of view, the objective is to ensure the convergence $y \rightarrow y^d + y^s$, with $y^s = x \tan \frac{\theta}{2}$. Results of the lateral regulation are reported on Fig. 7. The lateral position $y$ is reported in red, while the desired position is depicted in black and the desired set point for the robot to be on the desired circle is reported in blue.

<figure><img src="image_10.png" /><figcaption>Fig. 7. Lateral distances $y$ and $y^d$, and set point $y^s$</figcaption></figure>

As expected, when the pedestrian goes straight, the set point $y^s$ stays close to zero. During the curves (between 40-80s and 110-180s), this value reaches about $1.2m$ in order to ensure the position of the robot on the desired circle. One can see that the lateral position of the robot $y$ actually converges to the desired set point $y^s$, within a few centimeters accuracy. This permits to obtain a smooth trajectory tracking of the pedestrian, without using an absolute localization for both the pedestrian and the robot, and then avoiding an explicit communication.

# VII. CONCLUSIONS

In this paper, an adaptive approach for autonomous human tracking using a car-like mobile robot was proposed. It allows a robot to track the trajectory of a moving target using relative localization sensors (Ultra Wide Band beacons). The method avoids the use of absolute localization sensors, such as GPS, allowing to move in different areas, without considering satellite signals reception. Instead of collecting successive absolute points, the proposed algorithm computes a desired lateral position to regulate the robot on a circle pending on the target direction. The target orientation, as well as its velocity, cannot be measured directly using the relative sensor used. As a result, in this paper an observer is proposed to estimate the target motion, allowing for accurate tracking.

The efficiency of the proposed approach was investigated through full-scale experiments, showing high accuracy of the

robot position with respect to the human motion. Such an approach uses low-cost sensors and may then be used for many fields of applications, such as industry, defense, or agriculture. The latter is the main targeted field application of this work, in order to help farmers achieving painful task, such as harvesting or carrying heavy loads.

As mentioned in the experimental section, some limitations of the current approach may be pointed out. The angular velocity of the target is neglected, while a singularity exists on the control law to control the robot on the side of the target. An extension of both the observer and the control law, using a backstepping approach, is under investigation in order to limit these two drawbacks. Moreover, the addition of a predictive control layer is under investigation in order to anticipate for fast variations of the targeted speed.

Beyond the perception and control purpose, the democratization of such robots in every-day life requires the development of efficient human-robot interfaces, including a self-adaptation of the human behavior. Therefore, future work may focus on the use of human motion observation to adapt the parameters of control (such as the gain), with respect to the human feeling and actions. This is an important point of consideration to design robots actually able to meet societal needs and expectations.

# ACKNOWLEDGMENTS

This work has been sponsored by the French National Research Agency under the grant number ANR-14-CE27-0004 attributed to Adap2E project (adap2e.irstea.fr). It has also been sponsored by the French government research program “Investissements d’Avenir” through the IDEX-ISITE initiative 16-IDEX-0001 (CAP 20-25), the IMobS3 Laboratory of Excellence (ANR-10-LABX-16-01) and the RobotEx Equipment of Excellence (ANR-10-EQPX-44). This research was also funded by the European Union through the Regional Competitiveness and Employment program -2014-2020- (ERDF - AURA region) and by the AURA region. Thierry Peynot also acknowledges support from the QUT Centre for Robotics.

# REFERENCES

[1] F. Marmoiton and M. Slade, “Toward Smart Autonomous Cars,” in *Intelligent Transportation Systems: From Good Practices to Standards*. CRC Press, 2016, pp. 84–109.

[2] M. Bergerman, J. Billingsley, J. Reid, and E. Van Henten, “Robotics in Agriculture and Forestry,” in *Springer Handbook of Robotics*. Springer International Publishing, 2016, pp. 1463–1492.

[3] S. Blackmore, “Towards robotic agriculture,” in *SPIE Commercial+Scientific Sensing and Imaging*. International Society for Optics and Photonics, 2016, pp. 986 603–986 603.

[4] K. Zsiga, A. Tóth, T. Pilissy, O. Péter, Z. Dénes, and G. Fazekas, “Evaluation of a companion robot based on field tests with single older adults in their homes,” *Assistive Technology*, vol. 30, no. 5, pp. 259–266, 2018.

[5] C. W. Bac, J. Hemming, B. Van Tuijl, R. Barth, E. Wais, and E. J. van Henten, “Performance evaluation of a harvesting robot for sweet pepper,” *Journal of Field Robotics*, vol. 34, no. 6, pp. 1123–1139, 2017.

[6] M. Gupta, S. Kumar, L. Behera, and V. K. Subramanian, “A novel vision-based tracking algorithm for a human-following mobile robot,” *IEEE Transactions on Systems, Man, and Cybernetics: Systems*, vol. 47, no. 7, pp. 1415–1427, 2016.
[7] V. L. Popov, S. A. Ahmed, N. G. Shakev, and A. V. Topalov, "Detection and following of moving targets by an indoor mobile robot using microsoft Kinect and 2d lidar data," in *2018 15th International Conference on Control, Automation, Robotics and Vision (ICARCV)*. IEEE, 2018, pp. 280-285.

[8] S. A. Ahmed, A. V. Topalov, N. G. Shakev, and V. L. Popov, "Model-free detection and following of moving objects by an omnidirectional mobile robot using 2d range data," *IFAC-PapersOnLine*, vol. 51, no. 22, pp. 226-231, 2018.

[9] T. Tourrette, M. Deremetz, O. Naud, R. Lenain, J. Laneurit, and V. De Rudnicki, "Close coordination of mobile robots using radio beacons: A new concept aimed at smart spraying in agriculture," in *2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2018, pp. 7727-7734.

[10] A. Guillet, R. Lenain, B. Thuilot, and V. Rousseau, "Formation control of agricultural mobile robots: A bidirectional weighted constraints approach," *Journal of Field Robotics*, vol. 34, no. 7, pp. 1260-1274, 2017.

[11] C. Samson, P. Morin, and R. Lenain, "Modeling and control of wheeled mobile robots," in *Springer Handbook of Robotics*. Springer, 2016, pp. 1235-1266.

[12] G. Y. Ohya A., Nagumo Y., "Intelligent escort robot moving together with human-methods for human position recognition," in *1st International Conference on Soft Computing and Intelligent Systems and 3rd International Symposium on Advanced Intelligent Systems*, 2002.

[13] R. C. E. R. S. P. J. Sales, J.; Marín, "Multi-sensor person following in low-visibility scenarios," *Sensors*, vol. 10, no. 12, pp. 10953-10966, 2010.