Validating observer based on-line slip estimation for improved navigation by a mobile robot

Abstract

Wheel-slippage is a crucial but one of the marginally attended subjects regarding indoor navigation. Uncompensated slippage has the potential to introduce serious consequences in the form of safety-violation while manipulating obstacles and degraded performance when the vehicle is subjected to tracking and interception. This paper aims at establishing an alternative approach to dynamic modeling and robust control by proposing online estimation of slip parameters and modifying the kinematic model such that it is capable to accommodate slip-disturbance inputs. This approach works in a minimally invasive way, without interfering with or replacing the existing controller. The proposed approach has a low computational requirement and can be easily implemented without any major changes in the control architecture.

1 Introduction

Kinematic models are widely used in indoor navigation mainly because of a moderately uniform terrain that supports pure rolling motion. Apparently, this assumption holds good in most cases and kinematics make controller design easier (Taghia and Katupitiya 2013) for highly complex path arbitration problems. Practically however, indoor manoeuvres are not entirely free from violation of the said assumption. In most cases, mechanical vibrations due to tire deformation, unbalanced loading, variations in tractive forces induced by ground texture, etc. are the primary contributors of unmodeled disturbances. A wet floor, or the difference between a tiled corridor and a carpeted lobby or simply a sudden change in speed and direction may lead to kinematic disturbance like wheel slippage.

In this paper, our interest is to detect the slip-angle and the slip-ratio due to sudden dynamicity of motion. It is important to understand the premises of such dynamicity in the context of indoor navigation (target-tracking, for an example) and why the violation of non-holonomicity may be misleading with respect to safety and performance evaluation. Classically, a vehicle localizes itself with the help of odometry. In the event of slippage, the odometer generates faulty readings and this ‘apparently correct’ feedback further affects the successive actions. An incorrect state feedback may create an illusion of safety from an imminent collision and it may also indicate a false convergence of tracking error wherein the vehicle is actually diverging.

The commonly adopted remedial measures include replacing the kinematics with a detailed dynamics of the vehicle and substituting the existing controller by a more robust or adaptive scheme (Liao and Berrelli 2019). Both of these measures are undoubtedly effective in addressing the present concern, but they introduce additional cost and complexity (Wang et al. 2009). In this paper, our objective is to investigate two things—whether (1) the existing controller can be retained and (2) the existing kinematic model can be upgraded to incorporate slippage, with the help of additional sensors. In this regard, experiment-based dynamics-free sideslip calculation of four-wheeled vehicles have been reported by Selmanaj et al. (2017). Motivated by the precedent work, we have developed a modified slip-kinematic model and validated the same using a time-sequence of ‘estimated’ values of the slip parameters. The idea of slip ‘forecast’ also appears in association with anti-lock braking systems reported by Oniz et al. (2009). The technique proposed in the current paper can be executed easily and with a non-invasive perspective towards the existing controller.

2 Related work

Wheel slippage acts instantaneously. Hence, a regular pattern cannot be ascribed to it. Most of the related literature direct the readers to common faults like—non-ideal contact patch instead of a contact point and deformation of tires (Gillespie 1992; Lhomme-Desages et al. 2009) under shear force. Studies on anti-lock braking and traction control strategies (Zhang et al. 2016) have investigated the effects of different tire models. Probable reasons behind wheel slippage have also been discussed in relation to vehicle stability (Karnopp 2013). Change in coefficient of friction due to surface texture is also possible if the vehicle travels from hardwood-surface to concrete or climbs an incline with uneven load distribution on wheels. These problems are similar in nature and treatment to an outdoor navigation system. But slippage due to sudden dynamic motion still remains one of the least studied areas. Although an examination of ‘wheelie’ in two-wheelers (Corno et al. 2013) lightly touches this subject, lack of a detailed investigation is a motivation for this paper.

State estimation is usually the first step to tackle this issue. An adaptive learning method that has been proposed by Lai and Le (2021) for observation of the coupled dynamics of a helicopter promises robust estimation. Another landmark assisted robust localization with disturbance estimation and cancellation scheme has been discussed by Shimada (2018). Use of deep learning techniques (Chang 2019) for localization of high-speed objects is a recent advancement in estimation literature. A unified nonlinear observation approach combining both Luenberger and parameter-estimation observers has been presented by Yi and Zhang (2019). Recently, a non-Bayesian filter using wideband beacons has been proposed for accurate state estimation and prediction of uncertainties in Fontanelli et al. (2021). Observers have also proved to be efficient in analysing responsiveness of multi-agent systems over classical $L_1$ gain approach (Shang et al. 2021). Events like, sudden acceleration (or braking), turning manoeuvres at high speed (cornering and lane-change) introduce dynamic forces lateral to the direction of motion and result in slippage. The terminology have been adopted from automobile technology, but are equally relevant for indoor navigation problems like tracking, collision avoidance and formation control. Applications thereof present a typical scope for the proposed work. Unlike direct estimation of body-slip angle for yaw rate control presented by Geng et al. (2007), our aim is to improve the reliability of the state feedback. Our approach is aligned with the complementary sensing topology discussed by Zhu and Lamarche (2007) and Fusini et al. (2015), following which, a slip-propagation model has been derived.

Popular disturbance handling techniques for dynamic systems include disturbance rejection (Davison et al. 1999), robust controller design (Zwierzewicz 2020) and adaptive approaches (Cui 2019). However, introduction of new controllers to achieve improved performance is often non-economical. Robust controllers are difficult to design and extremely hard to manifest (Pretagostini et al. 2020). Reactive controllers are adaptive but require extensive parameter tuning, whereas, optimal model predictive controllers demonstrate reliable dynamicity and disturbance rejection, but are computationally intensive. The idea is to point out that each of the existing advanced controller designs come with additional constraints and limitations which can make cost of controller replacement unjustified. In contrast, our solution is easier to implement and can bypass the need to replace existing controllers until absolutely necessary. For example, a proportional–integral–derivative (PID) control strategy, which is known to suffer from poor response with nonlinearities and disturbances but show easily tuneable behaviour and reliable implementation can benefit from the proposed technique as a low-cost, complementary addition.

The key contributions of our work may be summarized as follows.

• (1) Slip estimation and prediction: Slip-angle and slip-ratio (disturbance parameters) have been estimated and propagation models of these parameters have been derived for quantitatively predicting slippage at a later instant.

• (2) Slip-kinematic vehicle model: Modified kinematic-based state equations for a mobile robot have been proposed, which have been configured to accept ‘expected slip’ as disturbance input.

• (3) Observer based state estimation using low-cost camera: Linear/nonlinear observers have been studied for estimating pose and velocity based on optical flow from a low-cost vision sensor and inertial sensors. Estimated pose and velocity are used to further estimate slippage. No other absolute position, velocity or yaw rate measurements have been used to validate our claim of improving the accuracy of state feedback.

• (4) Non-invasive approach to existing controller: The proposed state estimation strategy for reliable navigation is completely exclusive of the existing control method, which means the proposed method can be applied without needing to replace any existing stable control strategy. Tracking performance is improved in presence of disturbance by introducing corrective measures generated by state estimation through secondary observation. The focus is to provide accurate state feedback when subjected to slippage, as the latter goes overlooked by wheel encoders.

The paper is organized in the following manner. Section 3 explains the slip parameters relevant to the present problem. Section 4 discusses the design of a linear observer and an extended Kalman filter (EKF) which have been used to validate the proposed approach. Section 5 presents the proposed slip propagation model and the modified robot kinematics that accepts slip inputs. The experimental results for four case studies have been analysed in Sect. 6, along with a demonstration of how the proposed slip estimation helps to identify safety critical situations. Concluding remarks summarize the paper and give directions to future work.

3 Wheel-slip as disturbance

Slippage occurs when the patch of tire in contact with the ground is subjected to dynamic forces, that act in directions other than forward. As a result, the actual trajectory deviates from the intended path. As lateral forces develop at the contact patch, pure rolling motion is compromised (Karnopp 2013) and the wheel encoders fail to record this deviation, thereby giving an incorrect localization response. Any control action taken upon this ‘incorrect’ pose feedback can lead to serious errors.

Instead of treating longitudinal and lateral slip in each wheel independently (Gillespie 1992), this paper considers an effective slip-angle and slip-ratio about the geometric center of the vehicle and maintains this definition throughout the entire discussion.

$$\begin{aligned} \alpha \,=\,\,&\theta _{commanded}- \theta _{observed} \nonumber \\ \sigma \,=\,&\frac{|v_{encoder}-v_{observed}|}{\max \{v_{encoder},v_{observed}\}} \end{aligned}$$

(1)

Slippage can be quantified by two parameters. The first one $\alpha$, in Eq. (1) is the lateral deviation of heading, $\theta$ from the commanded direction. The second parameter, $\sigma$ (see Eq. (1)), is defined as the ratio of forward velocity error between the data recorded by wheel encoders and the observation (v is forward velocity). The angular shift, $\alpha$ is called the slip-angle and $\sigma$ is known as the slip-ratio. The mobile vehicle is assumed to have no backward motion. Therefore, $\sigma$ is a positive, dimensionless variable ranging between 0 and 1. A perfect non-holonomic motion corresponds to $\sigma =0$, indicating a complete agreement between the odometry and observed pose. Conversely, $\sigma =1$ indicates a complete disagreement implying either a skid or a false-rolling condition, depending on whichever is greater between the driving torque and traction.

4 Observer design

The objective of this paper is to validate the concept of on-line slip estimation through observation and thereby to advocate in favour of a modified vehicle kinematics that can accommodate slippage. Therefore, the claim does not rest on the novelty or efficiency of the observer design. For a fair understanding of the proposed approach, we have investigated the effects of both linear (Zhu and Lamarche 2007) and nonlinear observers (Katriniok and Abel 2016; Fusini et al. 2015).

4.1 Linear observer

The system to be observed can be explained with the help of Eq. (2), where (A, B) is the system and input matrix pair, q is the state vector, u is the input vector and n is Gaussian process noise.

$$\begin{aligned} {\dot{q}}=A q + B u + n \end{aligned}$$

(2)

where,

$$\begin{aligned} A=\begin{bmatrix} 0 &{} 1 &{} 0\\ 0 &{} 0 &{} 0\\ 0 &{} 0 &{}0\\ \end{bmatrix},B=\begin{bmatrix} 0 &{} 0 \\ 1 &{} 0 \\ 0 &{} 1 \\ \end{bmatrix},q=\begin{bmatrix} p\\ v\\ \theta \\ \end{bmatrix},u=\begin{bmatrix} a-c_{a}\\ \omega -c_{\omega }\\ \end{bmatrix} \end{aligned}$$

(3)

Here the states are, forward travelled distance: p, forward velocity: v and heading: $\theta$. The input pair, $(a,\omega )$ are the linear acceleration along the forward direction and angular velocity about the center of mass of the vehicle. The parameters, $c_{a}$ and $c_{\omega }$ are the accelerometer and gyroscope biases having Gaussian distribution. Assuming there is no absolute-position sensor and dedicated velocity sensor like GPS/GNSS and tachometer, we have used optical flow from an overhead camera for observation. In absence of expensive commercial sensors, techniques and utility of deploying low-cost vision sensor in velocity-estimation of a vehicle have been explained by Sun et al. (2013). The output z, is related to the state vector q (see Eq. (4)) through the observation matrix C (identity matrix of rank 3) and is supposed to represent the actual trajectories. Measurement noise m is assumed Gaussian.

$$\begin{aligned} z=C q + m \end{aligned}$$

(4)

The system (2) defined above is observable and pertaining to (4) a generalized state equation for a Luenberger observer (Frazzoli and Daleh 2011) can be defined as in (5).

$$\begin{aligned} \dot{{\hat{q}}}=A{\hat{q}}+Bu+L(z-{\hat{z}}) \end{aligned}$$

(5)

The pair, $({\hat{q}},{\hat{z}})$ denote the estimated state and output vectors and L represents the observer gain matrix of full rank. The observer error, e is a difference vector of the actual and the estimated states and is represented by $q-{\hat{q}}$. It may be recalled that, the elements of the observer gain matrix, L can be determined independent of any controller (which drives the vehicle along a reference trajectory) by virtue of the ‘Separation Principle’. For the same reason, the observer error can be designed to converge asymptotically by selective pole placement of the matrix $(A-LC)$, irrespective of the controller design, $(A-BK)$ (K being the controller gain). Using Ackerman’s method or otherwise, the elements of L can be placed such that the eigenvalues of the closed loop observer are located in the negative half of the s-plane. However, it may be noted that L may not be unique in this case. In the light of the correlated observation gain designed for a non-linear observer by Fusini et al. (2015), the matrix $(A-LC)$ in this paper is block-diagonal at best. This is subject to an assumption that there exists little or no correlation between heading and forward travelled distance, and heading and forward velocity. Thereby, the observer error dynamics can be rearranged into Eq. (6), which guarantees global asymptotic stability to the designed observer. Convergence proof of Luenberger observer of similar construction can be found in literature by Cui (2019) in addition to pole-placement methods discussed by Ogata (2009).

$$\begin{aligned} {\dot{e}} = (A-LC)e \end{aligned}$$

(6)

The process and measurement noises have no effect on the designed observer. In Eq. (6), $(A-LC)$ is a $3 \times 3$ matrix. Since odometry cannot be trusted in presence of slippage, the states must be estimated from additional observations, like a camera in this case. Optical flow, most certainly, does not give a very accurate measurement due to its sensitivity to light, processing delay and low sampling rate in response to high-speed motion segments of short duration. If available, it is recommended to use a sensor network for facilitating accurate observation over a distributed space comprising of multiple mobile units. A combination of optical and proximity sensors would be preferable for fast and accurate localization. Unified sensing using onboard sensors and beacons have recently been found to yield good results in state estimation in presence of uncertainties (Fontanelli et al. 2021). While high-speed motion capture cameras are best suited to this case, with proper static and dynamic calibration prior to the experiments, satisfactory performance can be achieved with a low-cost, moderately efficient vision sensor (Amarante and Fujarra 2020) paired with advanced image recognition techniques.

4.2 Nonlinear observer

In order to strengthen our investigations, we have also studied an Extended Kalman Filter (EKF) assuming that the system to be observed is governed by the non-linear kinematics given in Eq. (7), where k is the sampling instant and dt is the sampling time.

$$\begin{aligned} \begin{aligned} q_k\, =\,&f_k(q_{k-1},u_{k-1}, n_{k-1}) \\ \begin{bmatrix} x_{k}\\ y_{k}\\ v_{k}\\ \theta _{k} \end{bmatrix} =&\begin{bmatrix} x_{k-1} + v_{k-1}\cos ({\theta _{k-1}}) dt \\ y_{k-1} +v_{k-1}\sin ({\theta _{k-1}}) dt\\ v_{k-1} + (a_{k} - c_a) dt\\ \theta _{k-1}+ (\omega _{k} -c_{\omega }) dt \end{bmatrix} + \begin{bmatrix} n_{k-1} \end{bmatrix} \end{aligned} \end{aligned}$$

(7)

The states are, Cartesian positions: x and y, forward linear velocity: v and heading: $\theta$; and n is the vector of zero-mean Gaussian white state-noises. The observation is given by $z_k=H q_k + m_k$, where the observation matrix, H is basically an identity matrix of rank 4 and m is a zero-mean Gaussian measurement-noise vector. The apriori estimation covariance matrix is initialized by identity matrix and we assume that $det(R) < det(Q)$, where, Q and R are respectively the process and measurement noise covariance matrices. The filter equations for prediction and correction are given in (8). Predictions are driven by odometry while corrections are driven by optical flow.

$$\begin{aligned} \begin{aligned} Prediction:&q_{k|k-1}= f(q_{k-1|k-1}, u_{k-1})\\ {}&P_{k|k-1} = J_{k|k-1} P_{k-1|k-1} J_{k|k-1}^{\top } + {\bar{Q}} \\ Kalman Gain:&K_k = P_{k|k-1} H^{\top }_k (H_k P_{k|k-1} H^{\top }_k + {\bar{R}})^{-1} \\ Correction:&q_{k|k} = q_{k|k-1} + K_k ({\bar{z}}_k-z_k)^{\top }\\&P_{k|k} =[I-K_k H_k]P_{k|k-1} \end{aligned} \end{aligned}$$

(8)

The elements ${\bar{Q}}$ and ${\bar{R}}$ represent the noise terms $n_k Q_{k-1} n_k^{\top }$ and $m_k R_k m^{\top }_k$ respectively. The Jacobian, $J=\frac{\partial f}{\partial q}$ locally linearizes the state model. The estimated (prior) and corrected (posterior) state covariances are expressed as $P_{k|k-1}$ and $P_{k|k}$ respectively. The Kalman gain at the $k^{th}$ instant is given as $K_k$ and ${\bar{z}}_k$ is the observed state from the camera.

The acceleration and angular velocity inputs are obtained from a 6 DOF inertial measurement unit (IMU) attached to the vehicle. However, the deviation of inertial measurement builds up incremental errors over time and need to be updated regularly. In absence of a yaw-rate measure (magnetometer), heading provided by the optical flow has been assumed to be the reference for the update. For velocity estimation, the observer uses both integration and differentiation, representing the core operations of the IMU and the camera and are performed simultaneously. These complementary operations cancel out the lead and lag errors introduced by them (Zhu and Lamarche 2007).

Ideally, the accelerometer and the gyro biases also need to be estimated online. In contrast to the complicated bias models presented  by Zhu and Lamarche (2007), we have assumed a ‘random walk’, inspired by the bias calculation for a state partitioning particle filter demonstrated by Berntorp (2015). The time-varying ‘estimated’ bias terms, ${\hat{c}}_{a}$ and ${\hat{c}}_{\omega }$ are effectively zero-mean Gaussian white noises. Stability and local convergence properties of EKF, in the role of an observer for deterministic nonlinear dynamics have been well established in existing literatures (Boutayeb et al. 1997; Bonnabel and Barrau 2017) and hence proof of the same have been omitted here.

5 Slip propagation and modified kinematics

Following the pose and velocity estimation (by using a linear or a non-linear observer), the evolution of the expected slip-angle and the slip-ratio can be modeled according to the differential equations given in (9), which have been formulated by differentiating equation (1) and using estimated states instead of the observed states, wherever applicable. The ± signs in the ODE governing the slip-ratio are decided by the ‘skid’ and ‘false-rolling’ conditions (see Appendix). Slip-dynamics illustrated in Eq. (9) can be represented by Euler discretization with a fixed sampling interval, wherein at each sampling instant, k, current estimated states can be obtained from the observer. Using the estimated state at k in the discretized version of Eq. (9), ‘anticipated’ discrete-time propagation models of slip-angle and slip-ratio can be computed for instants $(k+1)$ and beyond. At $(k+1)$, these anticipated values can be revised based on a new set of estimates (if available) provided by the observer. Expected slippage projected by the propagation models and the actually observed values computed from the camera and the wheel encoders have been found to be in close agreement (see Sect. 6).

$$\begin{aligned} \begin{aligned} {\dot{\alpha }}(t)&= \omega _{commanded}(t)- \dot{{\hat{\theta }}}(t) \\ {\dot{\sigma }}(t)&= (1 \pm \sigma (t))\bigg (\frac{\dot{{\hat{v}}}(t)}{{\hat{v}}(t)}- \frac{{\dot{v}}_{encoder}(t)}{v_{encoder}(t)} \bigg ) \end{aligned} \end{aligned}$$

(9)

Using the current expected slip-angle as an input, a modified vehicle kinematics can be constructed according to Eq. (10). The longitudinal and the lateral components of velocity (due to slip disturbance) about the originally desired direction of motion are defined as $v_L={\hat{v}}\cos {\alpha }$ and $v_{\delta }= {\hat{v}}\sin {\alpha }$ respectively. The estimated angular velocity, ${\hat{\omega }} = \dot{{\hat{\theta }}}$ can also be computed from the observer equations (5) or (8).

$$\begin{aligned} \begin{aligned} {\dot{x}} \,=\,&v_L\cos ({\theta _{commanded}}) - v_{\delta }\sin ({\theta _{commanded}})\\ {\dot{y}} \,=\,&v_L\sin ({\theta _{commanded}}) + v_{\delta }\cos ({\theta _{commanded}})\\ {\dot{\theta }} \,=\,&{\hat{\omega }} \end{aligned} \end{aligned}$$

(10)

A discrete-time linearization of (10) can now correctly predict the reachable state in the next instant. It may again be recalled that the proposed method aims to facilitate an accurate estimation of state in presence of slip-disturbance and has no direct bearing with the controller used or the quality of measurement.

6 Experiments and simulations

In this section we shall discuss how the proposed method works, with the help of different navigation scenarios. We have demonstrated four case studies, which are prone to undergo wheel slippage. Observation of the vehicle’s states using optical flow has been considered to be a true representation of ground truth. For validation purpose we have used a single overhead camera monitoring a 3 m $\times$ 3 m arena. We have used a low-cost commercial grade camera with limitations on response time and frame rate. This has been done in an attempt to identify the lower limit of achievable performance in a tracking robot, as explained in Sect. 6.2 and a collision avoidance situation which will be explained further in Sect. 6.4. Efficiency of the ‘estimation’ process discussed in this paper is not controlled by the quality of measurement. This means that a better vision sensor will yield measurements closer to ground truth, but does not imply a reduced estimation error.

6.1 Experimental set-up

Fig. 1

Data flow between the system, observer and controller

The experiments reported in this paper have been implemented on Patrolbot, a programmable autonomous mobile robot by MobileRobotics Inc. (MobileRobotics 2020). The mobile robot is equipped with two symmetrically placed differential drive wheels, controlled by an in-built PID controller. Differential encoders attached to the wheels provide in-built dead-reckoning capabilities and a 6-DOF inertial measurement unit (MPU6050) has been attached close to the base along the centre of gravity. The accelerometer operates in the $\pm 2$ g mode and the gyroscope at $\pm 250^{\circ }$/s. The IMU communicates with the Arduino at 115,200 baud rate. Swing radius of the vehicle is 29 cm and velocity saturation limit is 1.8 m/s. Unique and asymmetric pre-registered visual marker has been attached to the top of the robot, which carries a piggybacked computer configured to run in the Server mode. The server performs slip estimation and generates motion commands for the vehicle through in-built PID control action. A schematic representation of the connections between the key functional blocks of the experimental setup has been illustrated in Fig. 1. A camera has been attached to  the ceiling of the designated workspace so that it receives an aerial view of the movement of the robot in the arena. The camera is a fixed focus 720 p/30 fps (maximum resolution) webcam with $60^{\circ }$ field of view. A remote computer connected to the camera runs in the client mode and computes closed loop optimal tracking trajectory based on commanded path and state feedback from the robot. The client executes an image processing application program aided by Augmented Reality toolkit (Unity3d 2020). This application is enabled to recognize the visual marker on the robot and correspondingly generate a stream of position and orientation data. The data is received by the client, transformed using a suitable transformation matrix and maps observed data onto the workspace. The client sends the localization data to the server via wireless serial transmission using User Datagram Protocol (Masirap et al. 2016) which aids fast communication.

Prior to the experiments, we perform two calibrations to ensure correctness of data obtained from the camera and the IMU. First, a commanded trajectory is physically marked on the workspace and the camera data is verified by displacing the visual marker through a predetermined sequence of points on the commanded path. Linear and angular displacement offsets of the transformation matrix are tuned till desired accuracy is obtained. Secondly, the IMU is calibrated by static placement and independent axial motion experiments. The IMU offsets have been calibrated with an Arduino-Uno programmer using the open-source ‘I2Cdevlib’ libraries (InvenSense 2020). Calibrations ensure reliability of the experimental observations and help to minimize modeling and measurement errors, which means, deviations can be safely attributed to slippage and not to the designed observer. A better camera is undoubtedly capable of generating more accurate observations, but our experiments show that even a low-quality camera can also produce useful outcomes by adopting the proposed method and using standardized image recognition techniques.

The experimental trajectories have been designed to challenge the kinematic limits of the Patrolbot. The observation time has been kept short, typically within 3s, pertaining to the limitations imposed by the vision sensor’s field of view. Accordingly, sudden acceleration, sudden deceleration, cornering and lane-change are the four types of trajectories that have been commanded to the robot. In the first two cases, the effect of sudden changes in linear acceleration and sudden braking have been studied. Ideally, slippage in these examples indicate contrasting phenomena—wheels rotating in-place without forward movement and motion due to inertia without actual rolling of the wheels, respectively. The third and the fourth cases deal with turning manoeuvres. A high-speed turn and a series of quick sharp turns respectively represent situations like cornering and lane-change.

Fig. 2

Contrasting effects of sudden braking and sudden acceleration gives rise to skidding and false-rolling. Estimated forward velocities depict the correct trajectories

6.2 Case study: changes in linear velocity

Fig. 3

Path and heading profiles of the vehicle on event of sudden braking. Estimation errors for slip-angle (blue) and slip-ratio (black) has been shown. EE estimation error, OdH odometry heading, CH commanded heading, EH: estimated heading, OH: observed heading (colour figure online)

Separate experiments have been designed for a tracking robot where—(1) the robot brakes due to an unanticipated and fast movement of an obstacle and (2) the robot speeds up to avoid a slow-moving object. In the former situation (braking), the instantaneous drop in the driving torque stalls the motors, but the vehicle continues to move due to inertia. Whereas, the second event (acceleration) is followed by a ‘phase’, in which, the driving torque fails to exceed the tractive force, and the wheels rotate in-place without making an actual forward motion. In both cases, dead-reckoning by the wheel encoders infer incorrect results.

Figure 2 illustrates the mismatch between the velocities computed from odometry and optical flow in the said experiments, the reasons being explained before. In the first experiment (braking), the commanded velocity demonstrates a sharp decline between 2.2 and 3 s. While the odometry seems to follow suit, the actual (observed) velocity is found to keep increasing for a few more seconds due to inertia and starts decreasing at a comparatively slower rate around 2.3 s. In the second experiment (acceleration) the desired velocity involves a sharp rise between 1.5 and 2 s, but the observed velocity fails to catch up with the desired trajectory. The under-achieved velocity profile is indicative of a false-rolling condition, contrary to the odometry readings. Figure 3a shows a comparison of path profiles due to sudden braking. The estimated time-evolution of the slip parameters, $\alpha$ and $\sigma$ have been computed from the estimated states of the observer. The actual slip-ratio and slip-angle have been computed from the observed states, commanded states and corresponding odometer readings according to Eq. (1). Figure 3b shows the slip-angle estimation error over commanded heading and the slip-ratio estimation error against observation time. The errors are bounded and their low magnitudes verify the efficiency of the estimation, given that slippage does not evolve in a regular pattern. The desired, observed and estimated heading profiles in Fig. 3c help to correlate the foregoing comparisons in Fig. 3a and b.

Fig. 4

Comparison of velocity and heading estimation by a linear observer and extended Kalman filter for sudden acceleration of the vehicle. LO: linear observer, EKF: Extended Kalman Filter

In Fig. 4, we have compared the performances of a linear observer (described in Sect. 4.1) and an extended Kalman filter (non-linear) (described in Sect. 4.2) in estimating the forward velocity and heading for a sudden acceleration scenario. The EKF and the linear filter have almost similar symmetric mean average estimation errors, as can also be inferred from Table 1. The only disadvantage of the linear observer is that it requires nine gain parameters to be designed and tuned to achieve the desired result. However, this tuning needs to be done offline and only once before the experiment is performed. It is clear from this study that a suitably designed observer can be used to estimate slippage, irrespective of whether it is a linear filter or a nonlinear one. It is important to note that estimation is necessary in case the observations are unavailable due to a temporary period of occlusion or communication delay. During that time, the vehicle can be driven by the modified kinematics with projected slip-angle estimates from the last iteration.

Table 1 Mean average estimation error: SA

6.3 Case study: turning manoeuvres

Fig. 5

Comparison of path trajectories due to cornering. Also shows the estimated slip parameters. ET: estimated trajectory, DT: desired trajectory, OdT: odometry trajectory, OT: observed trajectory

The next set of experiments is aimed at studying slippage during turning manoeuvres. The first case studies a high speed motion along an arc, wherein the vehicle is subjected to slippage causing understeer or oversteer or both, depending on its rate of acceleration. This phenomenon is known as ‘cornering’. The second case investigates motion along sharp turns, which are constrained by the swing radius of the vehicle. Figure 5a illustrates the estimated path traced out by the vehicle compared to the desired path. While the path computed from odometry preserves the spiral shape due to linearly increasing velocity (see Fig. 6b), the deviation from the desired path is only due to a miscalculated initial heading (see Fig. 6c for the heading from odometry) deliberated by inertia. However, odometry fails to recognize the actual slippage when the vehicle gains enough speed and oversteers between 1 and 1.8 s. During understeer, the wheel spins in-place and the observed velocity lags the commanded velocity. Whereas, once the vehicle gains momentum, the vehicle steers inwards across the arc. At this point, the wheel skids in an effort to maintain the commanded motion and the estimated velocity leads the desired velocity. The slip-angle attains the maximum value in the said interval (see Fig. 5b) and changes sign due to the switch from understeer to oversteer. Figure 6c clearly shows that the transition of estimated heading is stimulated by the said slippage and the result concurs with the present analysis. The cornering effects gradually subside after 1.8 s and can be observed from the decline of the slip-ratio curve in Fig. 5c.

Fig. 6

Comparison of estimated distance travelled, forward velocity and heading with commanded, observed and odometry readings in relation to slippage-induced over and understeer. OdV/H: odometry velocity/heading, DV/H: desired velocity/heading, EV/H: estimated velocity/heading, OV/H: observed velocity/heading, EDT: estimated distance travelled, OdDT: odometry distance travelled, ODT: observed distance travelled, CFD: commanded forward distance

Fig. 7

Trajectory, forward velocity and heading with lane change manoeuver. LO: linear observer, EKF: Extended Kalman Flter, EV/H/TD: estimated velocity/heading/travelled distance, OV/H/TD: observed velocity/heading/travelled distance, OdV/H/TD: odometry velocity/heading/travelled distance, DH/TD: desired heading/travelled distance, CV: commanded velocity

In the second experiment of this set, slippage is encountered as the robot tries to make a sharp turn at a constant acceleration. A ‘lane change’ can be interpreted as a sequence of sharp turns in quick succession, thus challenging the robot’s ability to stabilize itself along the desired heading. With the velocity increasing linearly, the initial lag in observed velocity and heading are caused by slippage due to false-rolling, as we can see in Fig. 7b and c. After 1.7s, both the estimated velocity and estimated heading make an attempt to stabilize about the reference values. Figure 7 also provides a performance comparison of the linear observer and Extended Kalman Filter in estimating pose and velocity. The preceding analysis can be further confirmed by the time-evolution of the slip parameters demonstrated by Fig. 8. The close agreement between the observed slip parameters (slip angle in Fig. 8a and slip-ratio in Fig. 8b) and the estimated slip parameters validates the efficiency of the estimation process.

Fig. 8

Demonstration of actual and predicted slip parameters for motion with lane change. SA: slip-angle, SR: slip-ratio

6.4 Effectiveness of slip-kinematic model

Fig. 9

An example showing the effectiveness of the slip-kinematic model in detecting safety violation condition. ET: estimated trajectory, UST: undetected slippage in trajectory

An experiment concerning collision avoidance has been performed for validating the effectiveness of the slip-kinematic model. In Fig. 9, the desired trajectory represents a path constraint, designed to limit the vehicle’s motion to a safe distance from the obstacle. The vehicle is commanded to follow the desired trajectory at a constant linear acceleration (wall-following manoeuver), thereby making the situation a classic example of cornering. Odometry, as expected, fails to detect slippage and gives a false idea of safety. Whereas, the proposed estimation approach shows the possibility of an imminent collision, as the vehicle slips, oversteers and crosses the safety boundary. A suitable controller (like an optimal controller, PID controller or otherwise) can be benefitted by a correct state feedback in replanning the trajectory and avoiding the collision.

In a subsequent trial of this experiment, we have selectively blocked some samples of data obtained from the camera to imitate a case of temporary occlusion or data loss. At those instants, the modified kinematic model (10) applies a reverse engineering using the time-sequence of estimated slip-angle and slip-ratio given in (9) along with Eq. (1) to predict the future states. The estimated trajectory computed therein is shown in blue in Fig. 9. It may be noted that this trajectory also indicates the violation and hence confirms that the proposed method can be utilized by a controller for taking necessary actions for improved navigation.

Table 2 Eigenvalue assignment in linear observer

6.5 Selection of observer gain

The main challenge in designing the Luenberger observer is the number of parameters that need to be tuned in order to achieve the desired error convergence. While we can safely rule out the dependence of forward travelled distance and forward velocity on the vehicle’s heading (and vice versa), there exists a correlation between the estimated p and v (Fusini et al. 2015). In fact, it has been experimentally confirmed that it is best to design the gain matrix in a block diagonal form. The dependence on velocity term is crucial because, it is the only state which is not directly measured but computed either by differentiating position information or integrating acceleration or a combination thereof. Experimental results indicate that $l_2$ and $l_4$ must be smaller or atmost equal to $l_1$ and $l_5$ respectively, $l_i$ ($i \in {\mathbb {Z}}, i \in [1,9]$) being the elements of observer gain matrix. It has been found that poles placed further away from the origin enhances the observer’s efficiency, which typically means that better results can be expected if the gain is high. This is especially true for the heading, wherein camera data is the sole reference. For turning manoeuvres, however, the gain can be chosen as a diagonal matrix. The primary diagonal elements, which determine the stability of the observer, have been selected close in numerical values, typically in the order of $\sim$ 20–25 in the above experiments. For case studies reported in Sect. 6.2, $l_2$ and $l_4$ have been numerically assigned half of the values compared to $l_1$ and $l_5$ respectively. Typical eigenvalue assignment for the four cases discussed above in selecting the gain parameters of the Luenberger observer have been shown in Table 2. These eigenvalues have been experimentally validated to yield acceptable convergence rate and were found to produce realisable gain with the hardware used.

The advantage of using an Extended Kalman Filter is that, a rigorous gain tuning procedure is inessential, but requires well-conditioned covariance matrices to achieve convergence. Having said so, our investigations do not recognize any significant difference and/or inconsistency in the estimation errors incurred by both the filters. Both linear and non-linear observers have been tested on the four dynamic cases discussed above, and the number of iterations taken by the respective observers for the observer errors to reach 95% of steady-state value (5% tolerance band) has been shown in Table 3. EKF exhibits a faster convergence in accordance with its exponential convergence properties, but increases computational complexity by 66%. The only practical disadvantage of the linear observer is gain-tuning, which is a single-time procedure and can be carried out offline before execution of the task. In terms of implementation, it can be concluded that either of the estimators will work well in the present application.

Table 3 Comparison of observer error convergence

7 Conclusions

This paper proposes a strategy demonstrating slip detection, estimation and prediction to facilitate accurate state feedback for improved navigation. The proposed method can be used to complement an existing controller in presence of slip-disturbance. Slippage can be attributed to a variety of sources, types and pattern of occurrence, based on which, rejection or adaptation strategies can be widely different. Besides, many of similar such disturbances are un-modelable, which makes our approach highly relevant and useful. We have shown that an online observer can be designed to achieve accurate tracking performance, without interfering with the existing controller and without using any dynamical equation (force-torque etc.) of the vehicle. A modified slip-kinematic model has been derived which, along with the proposed slip propagation model can approximate the reachable states fairly accurately. Both linear and nonlinear observers have been tested and they yield similar mean average estimation errors. In future this work may be extended to formulate a robust estimation problem with disturbance rejection.

Appendix

1.1 Derivation of slip-ratio propagation model

Representing observed velocity by estimated velocity of the vehicle, ${\hat{v}}$, we can write:

$$\begin{aligned}&\sigma =\frac{|v_{encoder}(t)-{\hat{v}}(t)|}{\max \{v_{encoder}(t),{\hat{v}}(t)\}} \\&\left. \begin{aligned} {\dot{\sigma }}(t)= \frac{d}{dt}\Bigg (\frac{|v_{encoder}(t)-{\hat{v}}(t)|}{v_{encoder}(t)}\Bigg ) ~\text {if} (a)\\ =\frac{d}{dt}\Bigg (\frac{|v_{encoder}(t)-{\hat{v}}(t)|}{{\hat{v}}(t)}\Bigg ) ~\text {if} (b) \end{aligned}\right\} \\&\left. \begin{aligned} {\dot{\sigma }}(t)= \frac{-{\hat{v}}(t)}{v_{encoder}(t)}\frac{{\dot{v}}_{encoder}(t)}{v_{encoder}(t)}+ \frac{\dot{{\hat{v}}}(t)}{v_{encoder}(t)} ~\text {if} (a) \\ = \frac{v_{encoder}(t)}{{\hat{v}}(t)}\frac{\dot{{\hat{v}}}(t)}{{\hat{v}}(t)}- \frac{{\dot{v}}_{encoder}(t)}{{\hat{v}}(t)} ~\text {if} (b) \end{aligned}\right\} \\&\left. \begin{aligned} {\dot{\sigma }}(t)= (\sigma (t)-1)\Bigg ( \frac{{\dot{v}}_{encoder}(t)}{v_{encoder}(t)}-\frac{\dot{{\hat{v}}}(t)}{{\hat{v}}(t)}\Bigg ) ~\text {if} (a)\\ = (\sigma (t)+1)\Bigg (\frac{\dot{{\hat{v}}}(t)}{{\hat{v}}(t)} - \frac{{\dot{v}}_{encoder}(t)}{v_{encoder}(t)}\Bigg ) ~\text {if} (b) \end{aligned}\right\} \end{aligned}$$

where, $(a) : v_{encoder}(t)>{\hat{v}}(t)$ and $(b) : v_{encoder}(t)<{\hat{v}}(t)$.