Infrastructure-Aided Localization and State Estimation for Autonomous Mobile Robots

Abstract

A slip-aware localization framework is proposed for mobile robots experiencing wheel slip in dynamic environments. The framework fuses infrastructure-aided visual tracking data (via fisheye lenses) and proprioceptive sensory data from a skid-steer mobile robot to enhance accuracy and reduce variance of the estimated states. The slip-aware localization framework includes: the visual thread to detect and track the robot in the stereo image through computationally efficient 3D point cloud generation using a region of interest; and the ego motion thread which uses a slip-aware odometry mechanism to estimate the robot pose utilizing a motion model considering wheel slip. Covariance intersection is used to fuse the pose prediction (using proprioceptive data) and the visual thread, such that the updated estimate remains consistent. As confirmed by experiments on a skid-steer mobile robot, the designed localization framework addresses state estimation challenges for indoor/outdoor autonomous mobile robots which experience high-slip, uneven torque distribution at each wheel (by the motion planner), or occlusion when observed by an infrastructure-mounted camera. The proposed system is real-time capable and scalable to multiple robots and multiple environmental cameras.

1. Introduction

Navigating mobile robots in dynamic environments with human presence makes visual odometry challenging due to occlusion and dynamic features. This necessitates multi-modal (e.g., camera, LiDAR, inertial) data fusion to identify and remove the dynamic features for feature-based localization [1,2], address disturbance and model mismatch challenges for LiDAR based localization [3,4], or tackle perceptually degraded conditions through distributed estimation [5,6]. In this regard, multi-modal state estimation approaches for mobile robots [7,8] are revolutionizing accurate navigation for indoor applications (e.g., warehouse robotics or service robots using on-board sensors) where the loss of reception and low bandwidth of commercial Global Navigation Satellite Systems (GNSS), inhibit reliable robot state measurements.

One of main challenges for the the existing multi-modal state estimators that utilize on-board inertial measurement unit (IMU) data and visual odometry through monocular/stereo cameras is the wheel slip in the longitudinal and lateral directions. This is due to: (i) Model uncertainties caused by the wheel force saturation in the robot dynamical model (by various robot payloads, changing surface conditions, or harsh cornering scenarios) impacting estimation error and update frequency in real-time [9,10,11]; and (ii) The real-time performance of state estimators for safe motion planning and controls in a scene with dynamic features [12,13]. Infrastructure-aided state estimation approaches which leverage visual/radar data measured by fixed sensors and communication with the robot are proposed in the literature to deal with perceptually degraded conditions and dynamic features for navigation of mobile autonomous systems [14,15,16]. This is cost effective as it reduces the number of on-board sensors, specially for large-scale networked robotic systems. In [17], cameras installed on the ceiling detect multiple robots with unique markers and determine their position and heading states based on the distance to fixed markers on the ground and known marker sizes. A stationary fisheye camera installed on the ceiling is used in [14] for indoor robot localization, in which the pose is determined based on the azimuth and elevation of the line of view (to the center of the segmentation). Multiple fixed surveillance cameras are used in [18] to detect the robot and static objects to construct a 2D map. Pose data from low-cost cameras mounted on ceiling is fused with on-board LiDAR odometry data for robot state estimation in [19] where the fusion of camera and odometry is performed in a map with an adaptive Monte Carlo approach. The existing infrastructure-aided localization approaches require visual markers or free line of sight to the robot [17,19], heavily rely on robot model, and are challenged by occluded scenes and model uncertainties due to the wheel slip.

In order to compensate for the wheel longitudinal/lateral slip in robots with nonholonomic constraints, kinematic- or dynamic-based slip estimation/compensation methods have been adopted in the literature [20,21] using on-board sensory data. The dynamic-based approaches require wheel stiffness properties and vertical forces that may change due to various payloads and road surface conditions [22]. Kinematic-based methods, on the other hand, use wheel odometry and inertial data to estimate the slip with upper bounded mean square estimation error (MSE) through nonlinear or stochastic observers [12,23,24]. A high-gain observer is designed in [25] to deal with unknown model parameters. To avoid model complexities due to tire force nonlinearities (and the combined-slip effect), an empirical parameterized kinematic model is proposed in [26] for robot state estimation. An event-based Kalman observer is designed in [27] to fuse IMU data and wheel odometry for heading and speed estimation. However, the information from on-board state observers has not been used for fusion with infrastructure sensing units to enhance reliability of the pose estimation. In addition, the computational efficiency and accuracy are main challenges for the existing infrastructure-mounted visual tracking and localization methods that use low-cost wide-angle lenses.

To address computational time and accuracy challenges of the existing visual and kinematic/dynamic model based localization methods (to be executed on embedded systems and robot’s on-board processing units), this paper develops and experimentally verifies a cooperative state estimator using: (i) Proprioceptive data from low-cost odometry sensors of a skid-steer mobile robot; and (ii) Region of Interest (ROI)-based processing and visual tracking on the 3D point clouds obtained from fixed sensing units. The main contributions of the paper are summarized as:

• Design of a computationally efficient ROI-based pose estimator using 3D point clouds from a stationary stereo camera with a wide-angle (fisheye) lens.
• Developing an infrastructure-aided localization framework which is scalable for large systems with multiple robots using communication between a slip-aware onboard observer and the stationary sensing unit.

2. Background and System Overview

The localization framework includes visual tracking through forming an ROI for computationally enhanced processing at the edge (e.g., embedded Jetson Xavier) and a slip-aware state observer at the robot using proprioceptive data. The visual tracking is through a fixed low-cost stereo camera, Intel Realsense T265. As illustrated in Figure 1, the system has independent visual tracking thread and ego motion thread.

The state vector is defined by $\xi \left(t\right)={[x\left(t\right),y\left(t\right),\theta \left(t\right)]}^{\top }$ for the proposed framework, where the longitudinal position, lateral position, and heading of the robot in the reference fixed frame $\left\{W\right\}$ is denoted by x, y, and $\theta$, respectively. The local robot body frame is denoted by $\left\{b\right\}$, which is at the geometrical center of the robot and is depicted in Figure 2. The reference coordinate system $\left\{W\right\}$ is derived from $\left\{b\right\}$ at time zero $t_0$.

The visual tracking thread estimates the robot pose ${\widehat{\xi }}^v$ based on the captured images of the stationary stereo camera in the environment. The occlusion cases, in which visual-based pose estimates are intermittent (or not available), will be addressed by the Covariance Intersection (CI) fusion with the estimated states ${\widehat{\xi }}^p$ from the slip-aware motion model. The updated pose by CI is then used as a corrected pose for the relative motion prediction in the next sample time. The robot pose is a time-varying transformation ${}^{W}T{}_b\left(t\right)=\left[\begin{array}{cc}{}^{W}R{}_b& {}^{W}p{}_b\\ \mathbf{0}& 1\end{array}\right]$ where the rotation matrix ${}^{W}R{}_b$ with $\theta \left(t\right)$ is about the Z-axis of the $\left\{W\right\}$, and the position vector ${}^{W}p{}_b={[x,y,0]}^{\top }$ with $x,y$ is the longitudinal and lateral robot position in the reference frame $\left\{W\right\}$.

2.1. Visual Tracking Thread

The visual tracking thread includes frontend and backend modules as illustrated in Figure 3. The frontend performs image processing and object detection. In the image processing step, the stereo image pair is undistorted and rectified. The object detection generates a boundingbox for the robot within the rectified stereo images. The area in the images enclosed with the boundingbox is termed as region of interest. The undistorted and rectified images, and image coordinates of the corresponding bounding box are used in the backend to localize the robot using the 3D position of points on the robot.

With the assumption of the pinhole model and known extrinsic parameters, the constraints for the projection of point clouds in $\left\{W\right\}$ onto the two image planes are derived. These constraints are described with epipolar geometry, and determine the area in the image planes where the same point in $\left\{W\right\}$ is mapped on. Figure 4 illustrates the epipolar geometry for two non rectified images. The projection of the point m into the camera centers $C_1$ and $C_2$ defines the epipolar plane which intersects the image plane $P_1$ and $P_2$ forming epipoles $e_1$ and $e_2$ for the left/right images. The homogeneous transformation $T=[R,p]\in SO\left(3\right)$ with the rotation matrix R and translation vector p between the camera centers describes the extrinsic parameters [28].

The position of a point in $\left\{W\right\}$ is determined with the intersection of the projection ray in 3D from the left and right image plane for the same mapped world point. The mapping of the x,y and z coordinate of a point from $\left\{W\right\}$ onto the left and right rectified image (Figure 5 and  Figure 6) plane as $\overline{\mathbf{u}}={[u,v,1]}^{\top }$ is described as $z\overline{\mathbf{u}}=K_j\overline{\mathbf{x}},j\in \{l,r\}$ ($l,r$ denotes the left and right sides, respectively) where $\overline{\mathbf{x}}={[x,y,z,1]}^{\top }$ and

$$K_l=\left[\begin{array}{cccc}f& 0& c_{x1}& 0\\ 0& f& c_y& 0\\ 0& 0& 1& 0\end{array}\right],K_r=\left[\begin{array}{cccc}f& 0& c_{x2}& b·f\\ 0& f& c_y& 0\\ 0& 0& 1& 0\end{array}\right]$$

(1)

are the extended camera matrix for the left and right image planes. The images have the same focal length in X and Y direction as well as the same principal point in Y direction; they are geometrically shifted with the baseline b in X direction. The radial distance r for perspective pinhole projection between the principal point and image coordinates of incoming ray of the point m is $r=\sqrt{u^2+v^2}$ and the angle $\mathrm{\Psi }$ between the principal axis and the ray is $\mathrm{\Psi }={tan}^{-1}\left(r\right)$. The radial fisheye distortion factor ${\mathrm{\Psi }}_d$ is modeled [29] as ${\mathrm{\Psi }}_d=\mathrm{\Psi }(1+k_1{\mathrm{\Psi }}^2+k_2{\mathrm{\Psi }}^4+k_3{\mathrm{\Psi }}^6+k_4{\mathrm{\Psi }}^8)$ with the individual lens distortion parameter $k_i,i\in \{1,\cdots ,4\}$. The distorted image coordinates $u^{{}^{\prime }}$ and $v^{{}^{\prime }}$ are

$$\begin{array}{c}\hfill u^{{}^{\prime }}=\frac{{\mathrm{\Psi }}_d}{r}u{v}^{{}^{\prime }}=\frac{{\mathrm{\Psi }}_d}{r}v,\end{array}$$

(2)

which are then converted into undistorted image coordinates

$$\begin{array}{c}\hfill u=f_x(u^{{}^{\prime }}+\alpha v^{{}^{\prime }})+c_x,v=f_y{v}^{{}^{\prime }}+c_y,\end{array}$$

(3)

Subsequent to this, a Yolov4 object detector [30] is used for 2D detection of the robot in the undistorted left image. The Yolov4 model is trained on a custom collected dataset of the robot for identification of the robot as a class label since the state-of-the-art COCO class labels have no training data corresponding to the robot.

Remark 1.

The output of the Yolov4 custom training detector at $k-$th step is a bounding box ${\mathcal{B}}_d\left(k\right)$ around the robot in the image yielding the extents of the box in the horizontal and vertical directions of the image. This enables an ROI which will be used to extract a frustum of the point cloud representing the robot. Point cloud processing will then be applied exclusively to the ROI-based frustum, i.e., interior $Int\left({\mathcal{B}}_d\left(k\right)\right)$. This bounding box-informed frustum significantly reduces the computational cost compared to processing the point cloud as a whole.

2.2. Point Cloud Computation and Post Processing

The feature extraction is restricted to the ROI, $\mathrm{Int}\left({\mathcal{B}}_d\left(k\right)\right)$, and is scalable for visual tracking in multi-robot settings. The robot is depicted inside the ROI in the left and right image plane of the undistorted and rectified images. The aim is to find the image coordinates $u_l$ and v (in the left image) and $u_r$ (in the right image plane) of the world point m, as see in Figure 5 and Figure 6.

For feature extraction, ORB features [31,32] were used, where the extracted features are matched within the stereo image pair and between subsequent captured image pairs. It is assumed that the remaining image coordinates represent the same point on the robot platform, then, these points’ 3D coordinates are reconstructed. Based on the epipolar geometry, the depth $z=\frac{bf}{u_l-u_r}$ is computed for each match with the horizontal image coordinates $u_l$ and $u_r$ of the left and right stereo image and the baseline b, as well as the focal length f of the camera, then the depth is used for $x=\frac{u_l}{f}z$ and $y=\frac{v_l}{f}z$ with the vertical image coordinate $v_l$ of the left stereo image plane as illustrated in Figure 7. The coordinates are computed for every match and transformed into $\left\{W\right\}$. All points lead to a point cloud assumed to be derived from the surface of the robot. The point cloud is processed with the PCL library [33,34] and a statistical outlier filter. The filter rejects points that are further away from their neighbors compared to the average of the point cloud. The input parameters are the number of neighbors to calculate the average distance for a given point and a ratio to set the threshold based on the standard deviation across the point cloud.

The 2D projection of the point cloud is used to enhance the reliability of the 3D point clouds for navigating the robot far from the stationary sensing unit (i.e., the stereo visual node). The Euclidean center of the 2D points (which is less sensitive to outliers) is considered as an estimate of the position, i.e., ${\widehat{\xi }}^p\left(k\right)$ at time step $t_k$ and will be corrected using the slip-aware motion model, which is described in the next section.

For orientation estimation, a linear regressor is used for a moving horizon $N_h$ of the estimated states. The angle between the estimated linear function and the world frame’s longitudinal axis is then considered as the orientation of the robot. To cope with situations when the robot is not driving or turning with zero radius, a plausibility check is applied. The plausibility check rejects estimates if the linear regression is too short or the distance between the position estimates and the line is greater than a threshold.

3. Infrastructure-Aided State Estimation

A kinematic model is introduced and parametrized to predict the motion in presence of wheel skidding and slipping. A covariance intersection (CI) method is then used to update the prediction.

3.1. Slip-Aware Motion Model

The autonomous mobile robot used to evaluate the localization approach is the skid-steer Clearpath’s Jackal robot, which is subject to the large wheel longitudinal slip in various cornering scenarios. The kinematic motion model in the following predicts the robot states using the heading and wheel rotational speed in the robot body frame $\left\{b\right\}$. The robot’s motion is defined based on the instantaneous center of rotation (IC) as shown in Figure 8, assuming that the robot is a rigid body and has a planar motion with nonholonomic constraints.

The longitudinal velocity, lateral velocity, and yaw rate are denoted by ${\nu }_x$, ${\nu }_y$, and $\dot{\theta }$ in the body frame $\left\{b\right\}$ and are expressed in terms of the left/right wheel rotational speeds ${\omega }_l,{\omega }_r$ as

$$\begin{array}{c}\hfill \mathbf{v}\left(t\right)=G(\Lambda )w\left(t\right)=G(\Lambda )\left[\begin{array}{c}{R}_e{\omega }_l\left(t\right)\\ R_e{\omega }_r\left(t\right)\end{array}\right]\end{array}$$

(4)

where $\mathbf{v}\left(t\right)={[{\nu }_x\left(t\right),{\nu }_y\left(t\right),\dot{\theta }\left(t\right)]}^{\top }$, the wheel rolling radius is denoted by $R_e$, and $G(\Lambda )$ includes the model parameter vector $\Lambda =\left[x_{\mathrm{IC}},y_{\mathrm{IC},l},y_{\mathrm{IC},r},{\alpha }_l,{\alpha }_r\right]$ as follows

$$\begin{array}{c}\hfill G(\Lambda )=\frac{1}{\tilde{y}}\left[\begin{array}{cc}-y_{\mathrm{IC},r}{\alpha }_l& y_{\mathrm{IC},l}{\alpha }_r\\ -x_{\mathrm{IC}}{\alpha }_l& x_{\mathrm{IC}}{\alpha }_r\\ -{\alpha }_l& {\alpha }_r\end{array}\right],\tilde{y}=y_{\mathrm{IC},l}-y_{\mathrm{IC},r},\end{array}$$

(5)

where $\mathrm{IC},l$ is the instantaneous center of the front-left and rear-left tires of the robot and $\mathrm{IC},r$ denotes the instantaneous center of the front-right and rear-right tires of the robot. In the schematic provided in Figure 8, due to nonholonomic constraints and since the longitudinal speed on the right side (i.e., rotational speed multiplied by the effective rolling radius $R_e$) is larger than the robot speed $v_x$, the instantaneous center $\mathrm{IC},r$ is located on the right side (i.e., $y_{\mathrm{IC},r}<0$ in the body frame).

The instantaneous center is expressed in $\left\{b\right\}$ as $(x_{\mathrm{IC},v},y_{\mathrm{IC},v})\in {\mathbb{R}}^2$, where $y_{\mathrm{IC},v}=\frac{{\nu }_x}{\dot{\theta }}$ [26]. The IC locations for the left and right wheels are expressed in $\left\{b\right\}$ as $(x_{\mathrm{IC},l},y_{\mathrm{IC},l})$ and $(x_{\mathrm{IC},r},y_{\mathrm{IC},r})$, respectively. It is assumed that the longitudinal position of ICs along the x-axis lie all on a parallel line to the Y-axis, i.e., $x_{\mathrm{IC}}=x_{\mathrm{IC},v}=x_{\mathrm{IC},j}=\frac{{\nu }_y}{\dot{\theta }},j\in \{l,r\}$ and have the same angular velocity. The lateral IC locations, which are bounded variables, are expressed as [21]:

$$y_{\mathrm{IC},j}=\frac{{\nu }_x-R_e{\omega }_j{\alpha }_j}{\dot{\theta }},\dot{\theta }=\frac{R_e({\omega }_r-{\omega }_l)}{y_{\mathrm{IC},l}-y_{\mathrm{IC},r}}$$

(6)

where ${\alpha }_l$ and ${\alpha }_r$ are parameters accounting for model uncertainties (tire inflation and longitudinal slip ratios at each corner of the robot) and $R_e$ is the tire rolling radius. The location of IC is bounded, i.e., $|x_{\mathrm{IC},v}|<\overline{x}$ and $|y_{\mathrm{IC},v}|<\overline{y}$ even reached in the proximity of straight trajectories where the numerator and denominator in (6) are of the same infinitesimal order which leads to finite values for $x_{\mathrm{IC}}$, $y_{\mathrm{IC},j}$.

The boundedness of $y_{\mathrm{IC},v}$ need to be guaranteed for lateral stability and minimizing the robot’s sideslip angle in harsh turning. Using the transformation between $\left\{b\right\}$ and the world frame, the robot states in $\left\{W\right\}$ are expressed as

$$\dot{\xi }\left(t\right)={}^{W}R{}_b\left(t\right)·\mathbf{v}\left(t\right)+\varrho ,{}^{W}R{}_b=\left[\begin{array}{ccc}cos\left(\theta \right)& -sin\left(\theta \right)& 0\\ sin\left(\theta \right)& cos\left(\theta \right)& 0\\ 0& 0& 1\end{array}\right],$$

(7)

where $\theta \left(t\right)$ is the robot heading and $\varrho \in {\mathbf{R}}^3$ represents model uncertainties. Then, the parameter identification process consists of two steps: gathering representative data from on-board and infrastructure-mounted sensory data; and developing an optimization program to find the optimal parameter vector ${\Lambda }^{*}$ through data set. The data collection consists of typically fast maneuvers on various surfaces in different trajectories based on the operational envelope of the mobile robot maintaining the lateral stability. The lateral stability is defined by a bounded sideslip angle $\left|\beta \right|<\overline{\beta }$ where $\beta \triangleq {tan}^{-1}\left(\frac{{\nu }_y}{{\nu }_x}\right)$ on various surface conditions. The wheel rotational speed measurement at each front-left, front-right, rear-left, and rear-right corners of the robot is used for the motion model by compensating the slip ratio component. The training data set (i.e., 12 different step-steer to the left and right, 18 random cornering, and 10 full/circular rotations in large and small path curvatures in indoor settings and on various surfaces) includes $N_t$ independent segments with the training horizon $d_t$. The measured wheel speeds of each segment are used to predict the robot speeds in the body frame using (4) and determine the robot pose in $\left\{W\right\}$ using (7). The predicted pose $\widehat{\xi }\in {\mathbb{R}}^3$ and the ground truth at the end of each segment are included in the cost function

$${\Lambda }^{*}=\underset{\Lambda }{min}J(\Lambda ),J(\Lambda )=\sum _{i=1}^{N_h}\left|\right|\xi -\widehat{\xi }{(\Lambda )}_i{\left|\right|}_2,$$

(8)

where $\widehat{\xi }(\Lambda )$ is the ground truth and $\xi \left(k\right)$ is the predicted state based on the linearized slip-aware motion model in discrete times. Minimizing J results in the optimal parameter vector ${\Lambda }^{*}$. The trained model is evaluated over different data sets with the evaluation horizon $d_e$. In this context, the evaluation horizon represents the prediction horizon for specific applications. The evaluation horizon is the indication of the prediction horizon of the model in the application. Assessing variable evaluation horizons with respect to variable training horizons is reveals the impact of different prediction horizons in the application compared to the parameter identification process.

To analyse the impact of different training and evaluation horizons, the mean relative translation/rotation errors are provided in Figure 9 and Figure 10. The analysis reveals that the best performance is achieved if the evaluation horizon is equal to (or less than) $0.5$ m. The error increases for larger deviation but remains bounded and lower than 5%.

3.2. Pose Prediction

The prediction model in (7), with elements from (4)–(6), is linearized around the operating point $({\xi }^p\left(k\right),\mathbf{w}\left(k\right))$ at each time step k in discrete times, where ${\xi }^p\left(k\right)={[x\left(k\right),y\left(k\right),\theta \left(k\right)]}^{\top }$ is the robot’s pose by the ego motion thread. The linear affine prediction model can be written as:

$$\begin{array}{c}\hfill {\xi }^p(k+1)=A\left(k\right){\xi }^p\left(k\right)+B\left(k\right)\mathbf{w}\left(k\right)+\varrho \left(k\right),\end{array}$$

(9)

whereas the zero-mean term $\varrho$ is due to model uncertainties. The discrete-time realization is approximated by

$$\begin{array}{c}\hfill A\left(k\right):={\varphi }_{t_{k+1},t_k}^{A^c}\approx e^{A^c\left(t_k\right)T_s}\in {\mathbb{R}}^{3\times 3}\end{array}$$

(10)

and

$$\begin{array}{c}\hfill B\left(k\right):={\int }_{t_k}^{t_{k+1}}{\varphi }^{A^c\left(t_{k+1}\right),\tau }{B}^c\left(\tau \right)d\tau \approx {\int }_{t_k}^{t_{k+1}}{e}^{A^c\left(t_k\right)(t_{k+1}-\tau )}d\tau B^c\left(t_k\right)\end{array}$$

(11)

whereas $A^c,B^c$ are the continuous-time system and input matrices of the linearized prediction model, and ${\varphi }_{t_i,t_j}^{A^c}$ for $t_i>t_j$ is the continuous-time state transition matrix expressed by the Peano-Baker series; the realization is assumed to not vary a lot in each interval $[t_k,t_{k+1}]$, which is valid for the proposed cooperative mobile robot localization model with the sampling time $T_s=25$ ms. As a result, the bound of uncertainty due to the sampling time for discretization (in the slip-aware motion model) at the maximum speed of 1 m/s, at which the robot may experience wheel longitudinal slip, is 25 mm. Then, the expected state prediction from the ego motion thread is

$$\begin{array}{c}\hfill {\overline{\xi }}^p(k+1)=A\left(k\right){\overline{\xi }}^p\left(k\right)+B\left(k\right)\overline{\mathbf{w}}\left(k\right),\end{array}$$

(12)

whereas ${\overline{\xi }}^p\left(k\right)=\mathbb{E}\left\{{\xi }^p\left(k\right)\right\}$ and $\overline{\mathbf{w}}\left(k\right)=\mathbb{E}\left\{\mathbf{w}\left(k\right)\right\}$; the joint covariance for $\mathbf{x}={[{\xi }^p\left(k\right),\mathbf{w}\left(k\right)]}^{\top }$ is then given by

$$\mathrm{cov}\left(\mathbf{x}\right)=\left[\begin{array}{cc}{Q}_{\xi }\left(k\right)& 0\\ 0& Q_w\left(k\right)\end{array}\right]=\mathbb{E}\left\{\left[\begin{array}{c}{\xi }^p\left(k\right)-{\overline{\xi }}^p\left(k\right)\\ \mathbf{w}\left(k\right)-\overline{\mathbf{w}}\left(k\right)\end{array}\right]\left[{({\xi }^p\left(k\right)-{\overline{\xi }}^p\left(k\right))}^{\top },{(\mathbf{w}\left(k\right)-\overline{\mathbf{w}}\left(k\right))}^{\top }\right]\right\}.$$

(13)

The predicted covariance is

$$\begin{array}{c}\hfill Q_{\xi }(k+1)=\mathbb{E}\left\{[{\xi }^p(k+1)-{\overline{\xi }}^p(k+1)]{[{\xi }^p(k+1)-{\overline{\xi }}^p(k+1)]}^{\top }\right\}\end{array}$$

(14)

in which

$$\begin{array}{c}\hfill {\xi }^p(k+1)-{\overline{\xi }}^p(k+1)=A\left(k\right)[{\xi }^p\left(k\right)-{\overline{\xi }}^p\left(k\right)]+\varrho \left(k\right).\end{array}$$

(15)

Then, by using $\mathrm{cov}\left(\mathbf{x}\right)$, the predicted covariance from the slip-aware motion model yields:

$$\begin{array}{c}\hfill Q_{\xi }(k+1)=A\left(k\right)Q_{\xi }(k+1)A^{\top }\left(k\right)+B\left(k\right)Q_w\left(k\right)B^{\top }\left(k\right).\end{array}$$

(16)

3.3. Augmented Localization

The visual thread and the ego motion thread communicate within the ROS framework through WiFi for the specific mobile robot test platform. To ensure proper data synchronization, time stamps are used to associate the visual-based localization (i.e., state estimation of ${\widehat{\xi }}^v\left(k\right)$) to the corresponding pose estimation ${\widehat{\xi }}_k^p$ by the slip-aware model description. Delay in the communication, which is less than 20 ms for the tests conducted within 10 m of the stationary visual node (i.e., infrastructure-mounted stereo camera with the fisheye lens), is ignored in this section for the CI fusion. This is a valid assumption considering the sampling time $T_s=25$ ms for the pose prediction in the slip-aware motion model, the fusion part’s sampling time (i.e., 50 ms), and the maximum robot speed of 1 m/s at which the robot may experience wheel’s longitudinal slip. Denoting the estimation error in the slip-aware motion model at time step k by ${\tilde{\xi }}^p\left(k\right)={\xi }^p\left(k\right)-{\widehat{\xi }}^p\left(k\right)$, and the visual thread by ${\tilde{\xi }}^v\left(k\right)={\xi }^v\left(k\right)-{\widehat{\xi }}^v\left(k\right)$, we utilize the covariance intersection method having the upper bound of the mean square estimation error and the consistency condition.

Remark 2.

The asymptotic stable state transition matrix of the error dynamics ${\tilde{\xi }}^p$ in the motion model (9), and the geometrical filters for the visual-based depth estimation guarantee that the mean square estimation error (MSE) for the pose prediction model and the visual localization are upper bounded, i.e., ${\tilde{Q}}_p\left(k\right):=\mathbb{E}\left\{{\tilde{\xi }}^p\left(k\right){\tilde{\xi }}^{p\top }\left(k\right)\right\}\le Q_p\left(k\right)$ and ${\tilde{Q}}_v\left(k\right):=\mathbb{E}\left\{{\tilde{\xi }}^v\left(k\right){\tilde{\xi }}^{v\top }\left(k\right)\right\}\le Q_v\left(k\right)$. As a result, the error covariance ${\tilde{Q}}_v\left(k\right)$ and ${\tilde{Q}}_p\left(k\right)$ of the estimated states from the two threads are consistent.

The estimated states from the ego motion thread and the visual thread are then fused using CI which is a convex combination of the covariances of the estimated states and guarantees a consistent error covariance (i.e., ${\tilde{Q}}_f\le Q_f$). The CI is a geometric interpretation of

$$\begin{array}{c}\hfill {\tilde{Q}}_f=W_p{\tilde{Q}}_p{W}_p^{\top }+W_p{\tilde{Q}}_{pv}{W}_v^{\top }+W_v{\tilde{Q}}_{vp}{W}_p^{\top }+W_v{\tilde{Q}}_v{W}_v^{\top },\end{array}$$

(17)

in which for all choices of ${\tilde{Q}}_{pv}$, the covariance ellipses of the bound $Q_f$ at level c,

$$\begin{array}{c}\hfill {\mathcal{E}}_{Q_f}^c:=\{z\in \mathbb{R}:z^{\top }{Q}_f^{-1}z<c\},\end{array}$$

(18)

lies within the intersection of covarinace ellipses of $Q_p$ and $Q_v$, i.e., ${\mathcal{E}}_{Q_f}^c\subset {\mathcal{E}}_{Q_p}^c\cap {\mathcal{E}}_{Q_v}^c$.

The weights $W_p,W_v$ will be obtained by minimizing a performance index on the bound $Q_f$, e.g., $\mathrm{tr}\left(Q_f\right)$ or $\det \left(Q_f\right)$, and consequently the covariance ${\tilde{Q}}_f$. The CI update strategy finds $Q_f$ which encloses the intersection area ${\mathcal{E}}_{Q_p}^c\cap {\mathcal{E}}_{Q_v}^c$ and is consistent, although no knowledge about $Q_{pv}$ is available. The upper bounds of the covariance matrix elements for visual pose estimates is set to constant values derived from the error analysis (discussed in the next section) For the case where ${\tilde{Q}}_{pv}\ne 0$, the covariance $Q_f$ can be given by

$$Q_f=[W_p,W_v]\underset{\mathbf{Q}}{\underbrace{\left[\begin{array}{cc}{Q}_p& {\tilde{Q}}_{pv}\\ {\tilde{Q}}_{vp}^{\top }& Q_v\end{array}\right]}}\left[\begin{array}{c}{W}_p^{\top }\\ W_v^{\top }\end{array}\right],$$

(19)

in which the optimal $W_p,W_v$ that minimize $\mathrm{tr}\left(Q_f\right)$ is obtained from the following constrained optimization program

$$\begin{array}{cc}\hfill & \underset{W}{min}\mathrm{tr}\left(Q_f\right)\hfill \\ \hfill & \mathrm{s}.\mathrm{t}.\text{:}{W}_p,+W_v=\mathbf{I},\hfill \end{array}$$

(20)

where $\mathbf{I}$ is the identity matrix with the proper dimension. The trace minimization program in (20) yields ${\left(Q_f\right)}^{-1}=(Q_p^{-1}{\tilde{Q}}_{pv}-\mathbf{I}){(Q_v-{\tilde{Q}}_{pv}^{\top }{Q}_p^{-1}{\tilde{Q}}_{pv})}^{-1}({\tilde{Q}}_{pv}^{\top }{Q}_p^{-1}-\mathbf{I})+Q_p^{-1}$. As a result, the fusion of the estimated states from the ego motion and the visual threads is

$$\begin{array}{cc}\hfill {\widehat{\xi }}^f\left(k\right)& =Q_f\left(k\right)[W_p{\left(Q_p\left(k\right)\right)}^{-1}{\widehat{\xi }}^p\left(k\right)\hfill \\ \hfill & +(1-W_p){\left(Q_v\left(k\right)\right)}^{-1}{\widehat{\xi }}^v\left(k\right)],\hfill \\ \hfill {}_f{\left(k\right)]}^{-1}& =W_p{\left(Q_p\left(k\right)\right)}^{-1}+(1-W_p){\left(Q_v\left(k\right)\right)}^{-1},\hfill \end{array}$$

(21)

where $W_p\in [0,1]$ adjusts the assigned weights to ${\widehat{\xi }}^p$ and ${\widehat{\xi }}^v$ minimizing the performance index $\mathrm{tr}\left(Q^f\right)$ of the updated covariance.

According to the consistency in Remark 2 and the property of CI, it holds that

$$\begin{array}{c}\hfill \mathbb{E}\left\{({\widehat{\xi }}^f\left(k\right)-\overline{\xi }\left(k\right)){({\widehat{\xi }}^f\left(k\right)-\overline{\xi }\left(k\right))}^{\top }\right\}\le Q_f\left(k\right).\end{array}$$

(22)

The heading of the robot is fused once the robot is close to the camera, thus, measurements are more accurate and reliable. The slip-aware observer and fusion is described in Algorithm 1.

Algorithm 1: Augmented Slip-Aware Localization

4. Experiments and Discussion

The proposed infrastructure-aided localization framework is experimentally evaluated in this section through tests with harsh turning, cornering with acceleration/deceleration, and accelerated straight maneuvers which all include longitudinal slip at each wheel. The reference measurement and system setup is first discussed, then the experimental evaluations are provided. The wheel slip during harsh cornering, with nonholonomic constraints, results is reduced pose estimation accuracy for the existing odometry-based motion models which rely on wheel rotational speed. This has been addressed in this paper by the proposed slip-aware motion model (considering instantaneous centers of rotation) and the a multi-modal data fusion with the visual thread (even with distortion challenges imposed by low-cost fisheye lens).

The ground truth trajectory is recorded with the optical motion capture system Vicon Vantage V5. The test setup is composed of the Vicon system, the autonomous mobile robot (Jackal AGV), and the stationary stereo camera T265. The T265 is fixed mounted on a tripod at a height of 2 m and capturing the whole area where the tests are conducted. The robot is operating under the normal path-tracking mode and starting in front of the tripod of T265, with the speed between $0.4$ and 1 m/s, and mild and harsh cornering in tight and wide trajectories. In the proposed motion model, the wheel slip is indirectly quantified as a kinematic model parameter.

To detect the robot and initial setup of the stereo camera in the environment, passive markers are mounted on top of the robot and the stationary stereo camera, as shown in Figure 11, having sufficient distance for a rotation invariant geometry which is essential to ensure a unique pose and proper localization results using the Vicon system.

Figure 12 shows the visual point cloud of the robot detected under occlusion (by a human/user) in a long distance.

The ROI-based point cloud processing, which generates point cloud within the 2D bounding box of the detected robot, reduced computation time up to $67\%$ as has experimentally been tested with the robot in dynamic indoor environments with human presence. The processing time for the pose prediction based on the slip-aware motion model is almost <5 ms. There is no exhaustive recursive algorithm associate with the motion model part. The visual thread with the ROI-based processing takes up to 16 ms in various harsh turn and random cornering maneuvers. The fusion part with the trace optimization program on the visual and motion threads take up to 35 ms on the utilized embedded system in dynamic environments with human presence.

The position estimation error by the stereo visual thread is shown in Figure 13 for a maneuver with several tight cornering. The largest error of 21 cm is for the situation in which the robot is occluded (by a human/user in a shared working indoor environment) in a far (i.e., $7.8$ m) distance. The slip-aware motion model helps CI to recover the robot pose guaranteeing consistency of the estimation error covariance, i.e., $\mathbb{E}\{\left({\tilde{\xi }}^f\left(k\right){\left[{\tilde{\xi }}^f\left(k\right)\right]}^{\top }\right\}\le Q_f\left(k\right)$.

The heading fusion result is depicted in Figure 14, where the heading prediction by the ego motion thread (without visual thread updates) is shown in dotted lines; this heading has large estimation error due to the harsh cornering scenarios and inaccuracies in the position of instantaneous center for the slip-aware ego motion model. The prediction fused with pose update from the visual thread in Figure 14 confirms better performance even with occlusion in this perceptually degraded test. This is due to the fact that the heading estimator (by the visual thread) employs multiple geometrical and nonholonomic constraints for the robot motion.

The position fusion results are illustrated in Figure 15 which confirms improvements in the estimation error when CI is applied using the visual thread to address uncertainties in the slip-aware ego motion thread in such arduous scenarios. The position prediction is fused with visual thread data, intentionally at each 200 ms to evaluate the effectiveness in large sample time updates or possible packet drop. Once the heading estimates are corrected by CI, the localization data is accurate with the root mean square error (RMSE) ≤$17\%$ for several tests even with intermittent CI updates. The triangular shapes show the effect of the fusion process in which the kinematic motion model has been corrected and fused with the visual thread data. The kinematic model, a dead reckoning system, suffers from fault propagation and has an higher uncertainty as well as biased position prediction. Once the position is corrected with the visual localization, the corrected position and new initial position for the dead reckoning system moves close to the ground truth. Increasing the frequency of the update by the CI fusion will smooth the final estimates.

5. Conclusions

An augmented state estimation framework was proposed for localization of autonomous mobile robots in dynamic environments using infrastructure-mounted visual sensors and on-board data. The proposed system is composed of a visual tracking thread based on a stationary low-cost fisheye stereo camera mounted in the environment and a slip-aware ego motion thread that uses proprioceptive sensory data from a skid-steer mobile robot to enhance accuracy and reduce variance of the estimated states. The position and heading of the robot was estimated using the visual thread with a region of interest-based 3D point cloud processing which reduced the computation up to $67\%$ in dynamic indoor environments with human presence. This significantly enhances the real-time processing capability of the infrastructure-mounted sensing unit for localization and tracking of multi robots in indoor settings. A slip-aware kinematic model was developed for the ego motion thread to predict the robot pose, then, covariance intersection with guaranteed consistency was used to update the pose prediction with visual estimates, addressing slippage and occlusion for wheel odometry based state estimators and visual based localization in dynamic environments. The experimental results confirmed RMSE ≤17% and an average position accuracy of 7 cm for various tests even with intermittent (e.g., $0.2$ s) CI updates. The real time capability of the state estimation framework was confirmed by the computation time 35 ms for ROI-based visual processing and the fusion (through trace minimization). The future avenues include: (i) Using a motion model in the visual thread to enhance the consistency of the pose estimation; (ii) Integrating the IMU data into the ego motion thread and developing a motion model connecting wheel speeds, longitudinal slips, and robot dynamics within an optimization problem constrained to the robot kinematic/dynamic constrains to enhance orientation estimation.