4WD Robot Posture Estimation by Radial Multi-View Visual Odometry

Abstract

This chapter presents a four-wheel robot’s trajectory tracking model by an extended Kalman filter (EKF) estimator for visual odometry using a divergent trinocular visual sensor. The trinocular sensor is homemade and a specific observer model was developed to measure 3D key-points by combining multi-view cameras. The observer approaches a geometric model and the key-points are used as references for estimating the robot’s displacement. The robot’s displacement is estimated by triangulation of multiple pairs of environmental 3D key-points. The four-wheel drive (4WD) robot’s inverse/direct kinematic control law is combined with the visual observer, the visual odometry model, and the EKF. The robot’s control law is used to produce experimental locomotion statistical variances and is used as a prediction model in the EKF. The proposed dead-reckoning approach models the four asynchronous drives and the four damping suspensions. This chapter presents the deductions of models, formulations and their validation, as well as the experimental results on posture state estimation comparing the four-wheel dead-reckoning model, the visual observer, and the EKF with an external global positioning reference.

1. Introduction

Autonomous robots obtain precise information about their surroundings by deploying their sensing devices and developing perceptual tasks to accomplish useful missions. Intelligent robots require to concurrently execute multiple functions such as path planning, collision avoidance, self-localization, tasks scheduling, trajectory control, map building, environment recognition, kinematic/dynamic control, and so forth. Autonomous robots depend on multisensor fusion, which is the process of combining data from the physical sensors into a homogeneous data space.

This chapter presents robot’s visual odometry using sensor data obtained from a homemade radial multi-view device (Figure 1a). For this case, trinocular sensing is divergent; hence, an inherent problem refers to different perspectives in each camera. Besides, the partial overlap between adjacent cameras allows sharing approximately $25\%$ of the total sensing angles, which is too reduced and limits extracting numerous relevant features for data fusion affecting to infer consistent information. Besides perspective, radial cameras yield differences of scale, skew, rotation, and lighting intensities. To cope with this problem, this chapter deduces a geometric trinocular sensor model to directly measure 3D data by combining divergent pairs, the central camera with one of the two lateral cameras (Figure 1b). The robot’s state vector (posture) is recursively estimated by a visual odometry model that triangulates multiple pairs of key-points. Thus, an EKF uses the 3D odometry model and estimates the robot’s position. The mobile robot is a four-wheel drive (4WD) modeled by a differential control law involving the four passive damping suspensions to infer accurate positions.

Figure 1.

Robot’s trinocular sensor. (a) Trinocular sensor onboard. (b) Camera’s geometric divergence (top view).

Parallel trinocular stereo systems had been deployed either to detect the ground [1], or to estimate motion [2]. There are reported works on motion estimation with binocular divergent systems [3], trinocular divergence for visual odometry [4], and divergent visual simultaneous localization and mapping (SLAM) [5]. As a difference from the active sensing modalities for localization [6], and concurrent localization and mapping with parallel multi-view [7], this chapter intends to estimate the posture of a rolling vehicle by exploiting feedback of the rich data fusion that a divergent trinocular sensor provides. Numerous visual odometry algorithms had been reported, using stereo cameras [8], matching multi-frame features [9] and 3D point cloud [10]. Some outdoor visual odometry approaches for urban [11] environments estimate motion tracking by extraction of visual feature points. There are numerous works combining the benefits of visual SLAM algorithms [12, 13, 14] with visual odometry [15], detecting geometrical features [16].

This chapter is organized into to the following sections. Section 2 deduces the sensor fusion observer modeling the trinocular system geometry. Section 3 models the 4WD direct/inverse kinematic solutions. Section 4 deduces the visual odometry formulation and EKF-based control state and estimation. Finally, conclusions are provided in Section 5.

2. Trinocular sensing model

This section describes the divergent multi-view geometric model, which basically combines the data of a pair of cameras radially arranged. In addition, this section presents an algebraic analysis of the lateral cameras’ alignment and correction w.r.t. the central camera. The fundamental geometrical relationship of the system divergence was experimentally studied by deploying a homemade prototype onboard a mobile robot, see Figure 2a. Cameras with homogeneous intrinsic parameters are assumed, and cameras are mechanically fixed epipolar. The sensor model’s purpose is to determine the depth information of a point in the scene $\mathbf{p}={\left(x,y,z\right)}^{\top }$, which is projected onto the overlapping area of a divergent pair. The proposed multi-view geometric model combines data using the central camera as the reference (Figure 1b). The focal plane in cameras $A,C$ are perspective transformed, in order to align them epipolar and coplanar w.r.t. the central reference $B$. As seen in Figure 1b, a point $\mathbf{p}$ is projected over two focal planes, for instance, at column $x_C$ of lateral camera $C$, and at $x_B$ of camera $B$. Thus, an isosceles triangle $\mathit{PBC}$ is formed. For the triangle $\mathit{OBC}$, let $\beta$ be the angle between the cameras’ centers $B$ and $C$, as deployed by expression (1). Let $\varphi /2$ be the remaining angle of $\mathit{OBC}$, where the inner angles’ total sum is $\pi$. By expressing $\beta +\varphi =\pi$ and dropping off $\varphi =\pi -\beta$, we easily deduce that $\frac{\varphi }{2}=\frac{\pi }{2}-\frac{\beta }{2}$. The geometrical distance $\overline{\mathrm{BC}}$ is calculated by triangulation using the law of sines, with known distance $l$ that converges to $O$. The linear distance between adjacent sensors $\mathit{BC}$ commonly oriented w.r.t. the center $O$ is

$$\overline{\mathrm{BC}}=\frac{lsin\beta }{sin\left(\frac{\pi }{2}-\frac{\beta }{2}\right)}.$$E1

Figure 2.

4WD kinematics. (a) Deployed robot. (b) Damping system. (c) Robot’s view from below.

To calculate the Cartesian coordinates of $\mathbf{p}$, let us state that the point $\mathbf{p}$ is projected through the horizontal coordinate $x_B$, and on camera $B$ angle’s ${\theta }_B$, and focal distance $f_B$ as expressed by

$${\theta }_B={tan}^{-1}\left(\frac{x_B}{{\mathrm{f}}_B}\right)\text{and}{\theta }_C={tan}^{-1}\left(\frac{x_C}{{\mathrm{f}}_C}\right).$$E2

The complementary angles $B$ and $C$ are modeled by

$$\angle B=\frac{\pi }{2}-{\theta }_B+\frac{\beta }{2}\text{and}\angle C=\frac{\pi }{2}-{\theta }_C+\frac{\beta }{2}.$$E3

In the triangle $\mathit{BCO}$, the angle at point $\mathbf{p}$ is obtained by equivalence of similar triangles $\angle P={\theta }_B+{\theta }_C-\beta$. Thus, to estimate the range of the radial system $B$ and $C$ w.r.t. $\mathbf{p}$, the linear distance is calculated by the law of sines:

$$\frac{\overline{\mathrm{BC}}}{sin\angle P}=\frac{\overline{\mathrm{CP}}}{sin\angle B}\text{and}\overline{\mathrm{CP}}=\frac{\overline{\mathrm{BC}}sin\angle B}{sin\angle P}.$$E4

Thus, for the other cameras’ pair, similar expressions are stated

$$\frac{\overline{\mathrm{BC}}}{sin\angle P}=\frac{\overline{\mathrm{BP}}}{sin\angle C}\text{and}\overline{\mathrm{BP}}=\frac{\overline{\mathrm{BC}}sin\angle C}{sin\angle P}.$$E5

Hence, the model to express depth information is given by $z_B=\overline{\mathrm{BP}}cos{\theta }_B$. By substituting $\overline{\mathrm{BP}}$ and ${\theta }_B$, the model is further specified by

$${\mathrm{\Lambda }}_1=\frac{lsin\beta }{sin\left(\frac{\pi -\beta }{2}\right)sin\left({\theta }_B-{\theta }_C-\beta \right)},$$

where,

$$z_B={\mathrm{\Lambda }}_{1}sin\left(\frac{\pi +\beta }{2}-{\theta }_C\right)cos\left({tan}^{-1}\left(\frac{x_B}{{\mathrm{f}}_B}\right)\right).$$E6

In addition, the range between camera $C$ and $\mathbf{p}$ is defined by $z_C=\overline{\mathrm{C}\mathbf{p}}cos{\theta }_C$. Thus, substituting $\overline{\mathrm{C}\mathbf{p}}$ and ${\theta }_C$, we define

$$z_C={\mathrm{\Lambda }}_{1}sin\left(\frac{\pi +\beta }{2}-{\theta }_B\right)cos\left({tan}^{-1}\left(\frac{x_C}{{\mathrm{f}}_C}\right)\right).$$E7

Using the depth models $z_B$ and $z_C$, the distances $d_B$ and $d_C$ w.r.t. $\mathbf{p}$ are estimated, such that $d_B=z_{B}tan\left({\theta }_B\right)$. Hence,

$$d_B=z_{B}tan\left({tan}^{-1}\left(\frac{x_B}{{\mathrm{f}}_B}\right)\right)\text{or}{d}_B=\frac{z_B{x}_B}{{\mathrm{f}}_B}$$E8

and being $d_C=z_{C}tan\left({\theta }_C\right)$, by substituting θ_C from expression (2) we have

$$d_C=z_{C}tan\left({tan}^{-1}\left(\frac{x_C}{{\mathrm{f}}_C}\right)\right)\text{or}{d}_C=\frac{z_C{x}_C}{{\mathrm{f}}_C}.$$E9

Furthermore, the algebraic deduction along the $Y$ component for the equalities $h_B{\mathrm{f}}_B=z_B{y}_B$ and $h_C{\mathrm{f}}_C=z_C{y}_C$, w.r.t $\mathbf{p}$ using distances $h_B$ and $h_C$, is obtained by

$$h_B=\frac{z_B{y}_B}{{\mathrm{f}}_B}\text{and}{h}_C=\frac{z_C{y}_C}{{\mathrm{f}}_C},$$

thus the following term is stated as

$$\mathrm{\Psi }=\frac{1}{{\mathrm{f}}_B}\left(\frac{lsin\beta }{sin\left(\frac{\pi }{2}-\frac{\beta }{2}\right)}\right)cos\left({tan}^{-1}\left(\frac{x_B}{{\mathrm{f}}_B}\right)\right).$$

Therefore, the geometry vector model for camera $B$ w.r.t. camera $C$, with substitution of the terms $\mathrm{\Psi }$, $z_B$, and $z_C$ in robot’s inertial frame ${}^R$, produce the next expression:

$${\mathbf{p}}_{\mathit{BC}}^R=\frac{\mathrm{\Psi }sin\left(\frac{\pi +\beta }{2}-{\theta }_C\right)}{sin\left({\theta }_B+{\theta }_C-\beta \right)}\left(\begin{array}{c}{x}_B\\ y_B\\ {\mathrm{f}}_B\end{array}\right)$$E10

and the same model is enhanced for camera $B$, using the geometry of cameras $A$ and $B$ by

$${\mathbf{p}}_{\mathit{AB}}^R=\frac{\mathrm{\Psi }sin\left(\frac{\pi +\beta }{2}-{\theta }_A\right)}{sin\left({\theta }_A+{\theta }_B-\beta \right)}\left(\begin{array}{c}{x}_B\\ y_B\\ {\mathrm{f}}_B\end{array}\right).$$E11

Hence, the arbitrary point ${\mathbf{p}}_{\mathit{ABC}}\in {\mathrm{ℝ}}^3$ is projected onto cameras $\mathit{AB}$, or onto cameras $\mathit{BC}$. In order to express a general formula, let us define the following theorem.

Theorem 1 (Trinocular depth model). Let camera $B$ be the reference for either divergent camera $A$ or $C$. A point coordinates w.r.t. camera $A$ is ${\mathbf{p}}_{\mathit{AB}}={\left(x_A,y_A,z\right)}^{{\rm T}}$, and w.r.t. camera $C$ is ${\mathbf{p}}_{\mathit{AC}}={\left(x_C,y_C,z\right)}^{{\rm T}}$. Hence, the general depth coordinate model for $x$ and $y$ for any divergent pair is

$$x,y_{A,C}=\frac{\mathrm{\Psi }{x}_{B}sin\left(\frac{\pi +\beta }{2}-{\theta }_{A,C}\right)}{sin\left({\theta }_{A,C}+{\theta }_B-\beta \right)},$$E12

for coordinate $z$,

$$z_{A,C}=\frac{\mathrm{\Psi }{f}_B^{2}sin\left(\frac{\pi +\beta }{2}-{\theta }_{A,C}\right)}{sin\left({\theta }_{A,C}+{\theta }_B-\beta \right)}.$$E13

The four points shown by the three cameras may illustrate their transformation, experimentally developed at 1-m distance between the robot and the marks.

3. 4WD dead-reckoning controller

Since the visual trinocular approach uses an exteroceptive sensor, we decided to challenge its detection and tracking capabilities with a robot having high holonomic properties. A 4WD robot’s locomotion is prone to experience frequent swift turns resulting in numerous slippages. Thus, a 4WD has to depend more on exteroceptive rather than inner measurements. Comparatively, inner 4WD odometry differs greatly from external visual measurement to infer posture. The proposed dead-reckoning system obtains speed measurements by deploying odometer readings of the four asynchronous drives (Figure 2). A 4WD system is considerably different from a conventional differential dual approach. Moreover, four passive mass-spring-damper suspensions are included in this system (Figure 2b), which varies the inter-wheel distances over time. Particularly, the robot’s 4WD and passive suspensions make the posture observations challenging.

The robot’s dead-reckoning model is fundamental to sense and control position used as feedback, providing motion description as a kinematic reference to match the visual observations when estimating the robot’s motion. The positioning and trajectory control [17], as well as the type of kinematic analysis [18] and the dynamic suspension [19] in this type of robot have been previously reported. The robot’s instantaneous speed $v_t$ (m/s) and yaw rate ${\omega }_t$ (rad/s) depend on the four wheels’ asynchronous rolling motion, ${\dot{\phi }}_1,{\dot{\phi }}_2,{\dot{\phi }}_3,{\dot{\phi }}_4$. For a wheel’s encoder, the velocity model approaches measurements by high-precision numerical derivatives (central divided differences) of the rotary angle ${\phi }_t$ (rad) w.r.t. time $t$, such that

$${\dot{\phi }}_t=\frac{\pi }{6\mathit{Rt}}\left[{\eta }_{t+2}+7{\eta }_{t+1}+7{\phi }_{t-1}-{\eta }_{t-2}\right],$$E14

where the wheel’s angular speed $\dot{\phi }$ is measured through pulse detection $\eta$ (dimensionless) of encoder’s resolution $R$ (pulses/rev); thus, the robot’s instantaneous velocity is modeled by the averaged wheel speed, with wheels of nominal radius $r$ (m)

$$v_t=\frac{r}{4}\sum _{i=1}^4{\dot{\phi }}_i.$$E15

Further, the differential velocity ${\widehat{v}}_t$ expresses the lateral speeds’ difference that yields ${\omega }_t$. Thus, ${\widehat{v}}_t$ is formulated by the expression

$${\widehat{v}}_t=r\left({\dot{\phi }}_1+{\dot{\phi }}_2-{\dot{\phi }}_3-{\dot{\phi }}_4\right).$$E16

This model describes that the rotary motion of ${\dot{\phi }}_1$ and ${\dot{\phi }}_2$ contributes to robot’s $+{\omega }_t$ (counterclockwise sense). Likewise, ${\dot{\phi }}_3$ and ${\dot{\phi }}_4$ contribute to robot’s $-{\omega }_t$ (clockwise sense). Therefore, ${\omega }_t$ is yielded by the lateral speed component ${\widehat{v}}_{t}cos\left({\alpha }_i\right)$ (see Figure 2b) modeled by

$${\omega }_t=\frac{\widehat{v}cos\left(cos\left({\alpha }_i\right)\right)}{l_i}.$$E17

The previous equation expresses the conservation of angular motion, and the wheel’s contact point turns w.r.t. the robot’s center,

$$cos\left({\alpha }_i\right)=\frac{W}{2l_i},$$E18

where for each length $l_i$ there is an asynchronous model,

$$l_1=\frac{\sqrt[2]{W^2+{\left(L_1+L_4\right)}^2}}{2},l_2=\frac{\sqrt[2]{W^2+{\left(L_2+L_3\right)}^2}}{2},$$

as well as

$$l_3=\frac{\sqrt[2]{W^2+{\left(L_3+L_2\right)}^2}}{2},l_4=\frac{\sqrt[2]{W^2+{\left(L_4+L_1\right)}^2}}{2}.$$

Thus, substituting $cos\left({\alpha }_i\right)$ and $l_i$ into ${\omega }_t$ and by considering both clockwise and counterclockwise motions, the robot’s angular speed is

$${\omega }_t=\sum _{i=1}^2\frac{\mathit{rW}{\dot{\phi }}_i}{l_i^2}-\sum _{i=3}^4\frac{\mathit{rW}{\dot{\phi }}_i}{l_i^2}.$$E19

The longitudinal contact point’s distance $l_i$ (m) takes as reference the robot’s geometric center. When $l_i$ varies, the contact point’s position $L_i$ changes.

$$L_i=dcos\left(arcsin\left({\gamma }_i\right)\right),$$E20

where ${\gamma }_i$ represents the vertical motion along the suspension,

$${\gamma }_i=arcsin\left(\frac{{\mathrm{\Delta }}_y}{d_1}\right).$$E21

From Figure 2a, the vertical motion $y_d$ is modeled assuming critical damping motion for a general spring-mass-damper system. The suspension is modeled by the following second-order homogeneous differential equation:

$$m{\ddot{y}}_d+{\kappa }_2{\dot{y}}_d+{\kappa }_1{y}_d=0,$$E22

where the elastic spring restitution coefficient is ${\kappa }_1$ (kg/s²). The damping coefficient is ${\kappa }_2$ (kg/s). The restitution force $m{\ddot{y}}_d$ counteracts the vertical oscillatory damping effects. The oscillatory velocity and acceleration are denoted by ${\dot{y}}_d$ and ${\ddot{y}}_d$, respectively. Thus, solving the second-order differential equation as a first-order equation such that ${\kappa }_2{\dot{y}}_d=-{\kappa }_1{y}_d$,

$${\int }_{y_d}\frac{\mathrm{d}{y}_d}{y_d}=-\frac{{\kappa }_1}{{\kappa }_2}{\int }_t\mathrm{d}t,$$E23

hence

$$ln\left(y_d\right)=-\frac{{\kappa }_1}{{\kappa }_2}t+c,\zeta =-\frac{{\kappa }_1}{{\kappa }_2},$$E24

with integration constant $c=0$ for analysis purpose. The suspension’s elongation derivatives as functions of time are

$$y_d={\mathrm{e}}^{\mathit{\zeta t}},{\dot{y}}_d=\zeta {\mathrm{e}}^{\mathit{\zeta t}},{\ddot{y}}_d={\zeta }^2{\mathrm{e}}^{\mathit{\zeta t}}.$$E25

Substituting the previous expression in (22),

$$m{\zeta }^2{\mathrm{e}}^{\mathit{\zeta t}}+{\kappa }_2\zeta {\mathrm{e}}^{\mathit{\zeta t}}+{\kappa }_1{\mathrm{e}}^{\mathit{\zeta t}}=0,$$E26

and by algebraically simplifying, the characteristic equation is

$${\zeta }^2+\frac{{\kappa }_2}{m}\zeta +\frac{{\kappa }_1}{m}=0,$$E27

and its analytic solution is

$${\zeta }_{1,2}=\frac{-{\kappa }_2}{2m}\pm \frac{\sqrt[2]{{\left(\frac{{\kappa }_1}{m}\right)}^2-4\frac{{\kappa }_1}{m}}}{2}.$$E28

As we assume a critically damped system, ${\left(\frac{{\kappa }_2}{m}\right)}^2=4\left(\frac{{\kappa }_1}{m}\right)$ and there is only one real root solution, such that

$$\zeta =-\frac{{\kappa }_2}{2m}.$$E29

Therefore, the damping motion is analytically solved by

$$y_d\left(t\right)=A{\mathrm{e}}^{\mathit{\zeta t}},$$E30

where $A$ (m) is the elongation amplitude (m) parameter for the suspension system. Moreover, in this type of robotic platform, the four asynchronous drives simultaneously produce slip/skid motions that are advantageously used to maneuver the robot. This approach proposes inferring the instantaneous $Z$-turn axis location ${\left(x_R,y_R\right)}^{\top }$. The $Z$-turn axis is movable in the square region bounded by the wheels’ contact point. The Z-turn axis is expressed by the first-order derivatives

$${\dot{x}}_R=\frac{\mathit{rW}}{4v_{\mathit{max}}}\left({\ddot{\phi }}_1-{\ddot{\phi }}_2-{\ddot{\phi }}_3+{\ddot{\phi }}_4\right)$$E31

and

$${\dot{y}}_R=\frac{\mathit{rL}}{4v_{\mathit{max}}}\left(-{\ddot{\phi }}_1-{\ddot{\phi }}_2+{\ddot{\phi }}_3+{\ddot{\phi }}_4\right).$$E32

There is a maximal allowable Z-turn displacement speed $v_{\mathit{max}}$. Hereafter, with four independent equations, the control positioning system is instantaneously computed. The robot control vector is ${\dot{\mathbf{u}}}_R={\left({\dot{v}}_t,{\dot{\omega }}_t,{\dot{x}}_R,{\dot{y}}_R\right)}^{\top }$, and the control transition matrix ${\mathbf{\Lambda }}_t$ has the elements ${\lambda }_1=r/4$, ${\lambda }_{2i}=\mathit{rW}/l_i^2$, ${\lambda }_3=\mathit{rW}/\left(4v_{\mathit{max}}\right)$, and ${\lambda }_4=\mathit{rL}/\left(4v_{\mathit{max}}\right)$. Thus, the forward kinematics solution is ${\dot{\mathbf{u}}}_R={\mathbf{\Lambda }}_t\cdot \dot{\mathbf{\Omega }}$ or

$$\left(\begin{array}{c}{\dot{v}}_t\\ {\dot{\omega }}_t\\ {\dot{x}}_R\\ {\dot{y}}_R\end{array}\right)=\left(\begin{array}{cccc}{\lambda }_1& {\lambda }_1& {\lambda }_1& {\lambda }_1\\ {\lambda }_{21}& {\lambda }_{22}& -{\lambda }_{23}& -{\lambda }_{24}\\ {\lambda }_3& -{\lambda }_3& -{\lambda }_3& {\lambda }_3\\ -{\lambda }_4& -{\lambda }_4& {\lambda }_4& {\lambda }_4\end{array}\right)\cdot \left(\begin{array}{c}{\ddot{\phi }}_1\\ {\ddot{\phi }}_2\\ {\ddot{\phi }}_3\\ {\ddot{\phi }}_4\end{array}\right).$$E33

In addition, to inversely solve this matrix system, the analytical solution represents the vector of independent control rotary variables ${\dot{\mathrm{\Omega }}}_t={\left({\ddot{\phi }}_1,{\ddot{\phi }}_2,{\ddot{\phi }}_3,{\ddot{\phi }}_4\right)}^{\top }$. Thus, let us define ${\lambda }_w={\lambda }_1{\lambda }_3{\lambda }_4$, ${\lambda }_A={\lambda }_{23}-{\lambda }_{24}$, ${\lambda }_B={\lambda }_{22}-{\lambda }_{21}$, ${\lambda }_C={\lambda }_{21}-{\lambda }_{23}$, ${\lambda }_D={\lambda }_{24}-{\lambda }_{22}$, ${\lambda }_E={\lambda }_{21}+{\lambda }_{24}$, and ${\lambda }_F={\lambda }_{22}-{\lambda }_{23}$.

$${\ddot{\phi }}_1=\frac{{\lambda }_3{\lambda }_4{\lambda }_D\dot{v}+2{\lambda }_w\dot{\omega }-{\lambda }_1{\lambda }_4{\lambda }_A{\dot{x}}_R+{\lambda }_1{\lambda }_3{\lambda }_F{\dot{y}}_R}{2{\lambda }_w\left({\lambda }_{21}-{\lambda }_{22}-{\lambda }_{23}+{\lambda }_{24}\right)},$$E34

$${\ddot{\phi }}_2=\frac{{\lambda }_3{\lambda }_4{\lambda }_C\dot{v}-2{\lambda }_w\dot{\omega }+{\lambda }_1{\lambda }_4{\lambda }_A{\dot{x}}_R-{\lambda }_1{\lambda }_3{\lambda }_E{\dot{y}}_R}{2{\lambda }_w\left({\lambda }_{21}-{\lambda }_{22}-{\lambda }_{23}+{\lambda }_{24}\right)},$$E35

$${\ddot{\phi }}_3=\frac{{\lambda }_3{\lambda }_4{\lambda }_D\dot{v}+2{\lambda }_w\dot{\omega }+{\lambda }_1{\lambda }_4{\lambda }_B{\dot{x}}_R+{\lambda }_1{\lambda }_3{\lambda }_E{\dot{y}}_R}{2{\lambda }_w\left({\lambda }_{21}-{\lambda }_{22}-{\lambda }_{23}+{\lambda }_{24}\right)}$$E36

and

$${\ddot{\phi }}_4=\frac{{\lambda }_3{\lambda }_4{\lambda }_C\dot{v}-2{\lambda }_w\dot{\omega }-{\lambda }_1{\lambda }_4{\lambda }_B{\dot{x}}_R-{\lambda }_1{\lambda }_3{\lambda }_F{\dot{y}}_R}{2{\lambda }_w\left({\lambda }_{21}-{\lambda }_{22}-{\lambda }_{23}+{\lambda }_{24}\right)}.$$E37

The matrix form of the inverse analytical solution for all wheels’ speed under damping variations is stated as $\dot{\mathbf{\Omega }}={\mathbf{\Lambda }}_t^{-1}\cdot {\dot{\mathbf{u}}}_t$ or

$${\dot{\mathrm{\Omega }}}_t=\left(\begin{array}{cccc}\frac{{\lambda }_3{\lambda }_4{\lambda }_D}{2{\lambda }_w{\lambda }_G}& \frac{2{\lambda }_w}{2{\lambda }_w{\lambda }_G}& \frac{-{\lambda }_1{\lambda }_4{\lambda }_A}{2{\lambda }_w{\lambda }_G}& \frac{{\lambda }_1{\lambda }_3{\lambda }_F}{2{\lambda }_w{\lambda }_G}\\ \frac{{\lambda }_3{\lambda }_4{\lambda }_C}{2{\lambda }_w{\lambda }_G}& \frac{-2{\lambda }_w}{2{\lambda }_w{\lambda }_G}& \frac{{\lambda }_1{\lambda }_4{\lambda }_A}{2{\lambda }_w{\lambda }_G}& \frac{-{\lambda }_1{\lambda }_3{\lambda }_E}{2{\lambda }_w{\lambda }_G}\\ \frac{{\lambda }_3{\lambda }_4{\lambda }_D}{2{\lambda }_w{\lambda }_G}& \frac{2{\lambda }_w}{2{\lambda }_w{\lambda }_G}& \frac{{\lambda }_1{\lambda }_4{\lambda }_B}{2{\lambda }_w{\lambda }_G}& \frac{{\lambda }_1{\lambda }_3{\lambda }_E}{2{\lambda }_w{\lambda }_G}\\ \frac{{\lambda }_3{\lambda }_4{\lambda }_C}{2{\lambda }_w{\lambda }_G}& \frac{-2{\lambda }_w}{2{\lambda }_w{\lambda }_G}& \frac{-{\lambda }_1{\lambda }_4{\lambda }_B}{2{\lambda }_w{\lambda }_G}& \frac{-{\lambda }_1{\lambda }_3{\lambda }_F}{2{\lambda }_w{\lambda }_G}\end{array}\right)\cdot \left(\begin{array}{c}{\dot{v}}_t\\ {\dot{\omega }}_t\\ {\dot{x}}_R\\ {\dot{y}}_R\end{array}\right),$$E38

where ${\lambda }_G={\lambda }_{21}-{\lambda }_{22}-{\lambda }_{23}+{\lambda }_{24}$.

4. State estimation and feedback position control

This section formulates a deterministic geometric model for visual odometry and the state estimation by an EKF. The proposed model combines pairs of key-points at times $t$ and $t-1$. The robot’s displacements are deduced by inverse geometric triangulations to feed forward an EKF and estimate the robot’s posture.

In Figure 3a, the instantaneous angle ${\alpha }_{t-1}$ is formed by a pair of key-points defined by

$${\alpha }_{t-1}=\mid {\theta }_{t-1}^a\mid +\mid {\theta }_{t-1}^b\mid$$E39

and such key-points’ distance $c_{t-1}$ is defined by

$$c_{t-1}=\sqrt{{\left({\delta }_{t-1}^a\right)}^2+{\left({\delta }_{t-1}^b\right)}^2-2{\delta }_{t-1}^a{\delta }_{t-1}^{b}cos{\alpha }_{t-1}}.$$E40

Figure 3.

Robot’s visual odometry. (a) Robot’s key-point pair observed at $t-1$. (b) Same pair observed at $t$. (c) Robot’s displacement ${\mathrm{\Delta }}_s$ by triangulation of key-points ${\mathbf{p}}_a$.

The angle ${\beta }_{t-1}^{a,b}$ of either key-point $a$ or $b$ is calculated by the law of sines,

$${\beta }_{t-1}^{a,b}=arcsin\left(\frac{{\delta }_{t-1}^{b,a}sin{\alpha }_{t-1}}{c_{t-1}}\right).$$E41

However, at time $t$ in Figure 3b, the instantaneous angle ${\alpha }_t$ is obtained by

$${\alpha }_t=\mid {\theta }_t^a\mid +\mid {\theta }_t^b\mid ,$$E42

with the key-point’s distance $c_t$ as

$$c_t=\sqrt{{\left({\delta }_t^a\right)}^2+{\left({\delta }_t^b\right)}^2-2{\delta }_t^a{\delta }_t^{b}cos\left({\alpha }_t\right)},$$E43

which is used to obtain the angle ${\beta }_t^{a,b}$ of the key-point $a$ or $b$ at actual time

$${\beta }_t^{a,b}=arcsin\left(\frac{{\delta }_t^{b,a}sin{\alpha }_t}{c_t}\right).$$E44

Further, the differential angle $\widehat{\beta }$ is defined by triangulation of previous and actual poses and an arbitrary 3D point ${\mathbf{p}}_{a,b}$ (Figure 3c),

$$\widehat{\beta }={\beta }_{t-1}^{a,b}-{\beta }_t^{a,b}.$$E45

Proposition 1 (Triangulation odometric displacement). The robot’s displacement ${\mathrm{\Delta }}_s$ (Figure 3c) that is inferred by triangulation of visual key-points over time is

$${\mathrm{\Delta }}_s=\sqrt{{\delta }_{t-1}^a+{\delta }_t^a-2{\delta }_{t-1}^a{\delta }_t^{a}cos\left(\widehat{\beta }\right)}.$$E46

The triangulation angle $\lambda$ is calculated by the law of sines,

$${\lambda }^{a,b}=arcsin\left(\frac{{\delta }_t^{a,b}sin\left(\widehat{\beta }\right)}{{\mathrm{\Delta }}_s^{a,b}}\right)$$E47

and the orientation angle for each reference $a$ is

$${\varphi }^{a,b}={\lambda }^{a,b}+\left(\frac{\pi }{2}-{\theta }_{t-1}^{a,b}\right),$$E48

which is required to know the $X$ displacement

$${\mathrm{\Delta }}_x={\mathrm{\Delta }}_s^{a,b}cos\left({\varphi }^{a,b}\right),$$E49

as well as the $Y$ displacement

$${\mathrm{\Delta }}_y={\mathrm{\Delta }}_s^{a}sin{\varphi }^a.$$E50

When obtaining numerous key-point pairs simultaneously, the total robot’s displacement is an averaged value of the displacements yielded by all key-point pairs,

$${\mathrm{\Delta }}_{x_t}=\frac{{\sum }_{i=1}^n{\mathrm{\Delta }}_{x_i}}{n}\text{and}{\mathrm{\Delta }}_{y_t}=\frac{{\sum }_{i=1}^n{\mathrm{\Delta }}_{y_i}}{n}.$$E51

Therefore, without loss of generality, for state estimation, let us assume a nonlinear robot’s model state vector

$${\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k\right),$$E52

where the state vector is $\mathbf{x}=\left(x,y,\theta ,v,\omega \right)$, and combined with a nonstationary state transition matrix ${\mathbf{A}}_t$, such that ${\mathbf{x}}_k={\mathbf{A}}_t\cdot {\mathbf{x}}_{k-1}$ or

$${\mathbf{x}}_k=\left(\begin{array}{ccccc}1& 0& 0& cos\left(\theta \right)\mathrm{\Delta }t& 0\\ 0& 1& 0& sin\left(\theta \right)\mathrm{\Delta }t& 0\\ 0& 0& 1& 0& \mathrm{\Delta }t\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right)\cdot \left(\begin{array}{c}x\\ y\\ \theta \\ v\\ \omega \end{array}\right),$$E53

by developing the dot product from previous expression, we obtain

$${\mathbf{x}}_k=\left(\begin{array}{c}{x}_{k-1}+vcos\left(\theta \right)\mathrm{\Delta }t\\ x_{k-1}+vsin\left(\theta \right)\mathrm{\Delta }t\\ {\theta }_{k-1}+\omega \mathrm{\Delta }t\\ v\\ \omega \end{array}\right).$$E54

The measurement model requires the displacements that were inferred through key-point triangulation

$${\mathbf{z}}_k=h\left({\mathbf{x}}_k,{\mathbf{v}}_k\right),$$E55

where ${\mathbf{w}}_k$ and ${\mathbf{v}}_k$ are the process and measurement noise models, respectively. These are statistically independent and supposed to have a Gauss distribution with zero average value and known variance. To approximate the nonlinear robot’s measurement model, a linearized first-order approximation by the expansion of the Taylor series is used, and a linear approximation of a function is built, with slope obtained through partial derivatives by

$$f^{\prime }\left(u_t,x_{t-1}\right)=\frac{\partial f\left(u_t,x_{t-1}\right)}{\partial x_{t-1}}.$$E56

Thus, the linearized models of the process and measurement are defined next in (54) and (55), such that

$$f\left(\mathbf{x}\right)=f\left(\widehat{\mathbf{x}}\right)+\underset{=J}{\underbrace{f^{\prime }\left(\widehat{\mathbf{x}}\right)}}\left(\mathbf{x}-\widehat{\mathbf{x}}\right)$$E57

and

$$h\left(\mathbf{x}\right)=h\left(\widehat{\mathbf{x}}\right)+\underset{=H}{\underbrace{h^{\prime }\left(\widehat{\mathbf{x}}\right)}}\left(\mathbf{x}-\widehat{\mathbf{x}}\right).$$E58

In addition, the EKF’s prediction models (56) and the correction models (57) are formulated and linearized as

$${\widehat{\mathbf{x}}}_k^{-}=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_{k-1},0\right)$$E59

and

$${\mathbf{P}}_k^{-}={\mathbf{A}}_k{\mathbf{P}}_{k-1}{\mathbf{A}}_k^{\top }+{\mathbf{W}}_k{\mathbf{Q}}_{k-1}{\mathbf{W}}_k^{\top }.$$E60

Moreover, the recursive Kalman gain for system convergence is

$${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\top }{\left({\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\top }+{\mathbf{V}}_k{\mathbf{R}}_k{\mathbf{V}}_k^{\top }\right)}^{-1}$$E61

and the state vector of the system is described by

$${\widehat{\mathbf{x}}}_k={\widehat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k\left({\mathbf{z}}_k-\mathbf{H}{\overline{\mathbf{x}}}_k\right),$$E62

with covariance matrix of the system

$${\mathbf{P}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_k^{-}.$$E63

Thus, hereafter, the vector and matrix models describing the proposed robot’s system are formulated and incorporated into the conventional EKF. Let us define the robot’s pose vector $\mathbf{x}={\left(x_t,y_t,{\theta }_t\right)}^{\top }$. The control vector is comprised of the robot’s absolute and angular speeds, ${\mathbf{u}}_k={\left(\upsilon ,\omega \right)}^{\top }$. Furthermore, the observation vector with sensor measurement ${\mathbf{z}}_k={\left(X_p,Y_p,Z_p\right)}^{\top }$. Eventually, the process noise vector ${\mathbf{w}}_k={\left(w_x,w_y,w_{\theta }\right)}^{\top }$. The measurement noise vector ${\mathbf{v}}_k={\left(v_{X_p},v_{Y_p},v_{Z_p}\right)}^{\top }$.

Therefore, from the displacement equation (46), which arises from exteroceptive observations, the robot’s Cartesian displacements are

$$\mathrm{\Delta }x=\left(\mathrm{\Delta }s\right)cos\left({\theta }_{k-1}+\lambda +\left(\frac{\pi }{2}-{\theta }_{t-1}^a\right)\right)$$E64

and

$$\begin{array}{l}\mathrm{\Delta }y=\left(\mathrm{\Delta }s\right)sin\left({\theta }_{k-1}+\lambda +\left(\frac{\pi }{2}-{\theta }_{t-1}^a\right)\right),\end{array}$$E65

as well as $\mathrm{\Delta }\theta$ is given by

$$\begin{array}{l}\mathrm{\Delta }\theta =\lambda +\left(\frac{\pi }{2}-{\theta }_{t-1}^a\right).\end{array}$$E66

By substituting an averaged Cartesian displacement, one considers $n$ key-point observations to calculate the recursive noisy state vector ${\mathbf{x}}_k$.The Jacobian matrix ${\mathbf{J}}_k$ of the robot’s temporal matrix state transition ${\mathbf{A}}_k\cdot {\mathbf{x}}_k$, where $x_k=x_{k-1}+vcos\left(\theta \right)\mathrm{\Delta }t$, $y_k=y_{k-1}+vcos\left(\theta \right)\mathrm{\Delta }t$, and ${\theta }_k={\theta }_{k-1}+\omega \mathrm{\Delta }t$ is stated by

$$\mathbf{J}=\frac{\partial {\mathbf{A}}_k\cdot {\mathbf{x}}_k}{\partial {\mathbf{x}}_{k-1}}=\left(\begin{array}{ccccc}\frac{\partial x}{\partial x}& \frac{\partial x}{\partial y}& \frac{\partial x}{\partial \theta }& \frac{\partial x}{\partial v}& \frac{\partial x}{\partial \omega }\\ \frac{\partial y}{\partial x}& \frac{\partial y}{\partial y}& \frac{\partial y}{\partial \theta }& \frac{\partial y}{\partial v}& \frac{\partial y}{\partial \omega }\\ \frac{\partial \theta }{\partial x}& \frac{\partial \theta }{\partial y}& \frac{\partial \theta }{\partial \theta }& \frac{\partial \theta }{\partial v}& \frac{\partial \theta }{\partial \omega }\\ \frac{\partial v}{\partial x}& \frac{\partial v}{\partial y}& \frac{\partial v}{\partial \theta }& \frac{\partial v}{\partial v}& \frac{\partial v}{\partial \omega }\\ \frac{\partial \omega }{\partial x}& \frac{\partial \omega }{\partial y}& \frac{\partial \omega }{\partial \theta }& \frac{\partial \omega }{\partial v}& \frac{\partial \omega }{\partial \omega }\end{array}\right)=\left(\begin{array}{ccccc}1& 0& -vsin\left(\theta \right)\mathrm{\Delta }t& cos\left(\theta \right)\mathrm{\Delta }t& 0\\ 0& 1& vcos\left(\theta \right)\mathrm{\Delta }t& sin\left(\theta \right)\mathrm{\Delta }t& 0\\ 0& 0& 1& 0& t\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right).$$E67

Thus, a measurement is a 3D point arising from either divergent pair Eq. (10) or (11) and deployed by Proposition 1.1. Thus, the robot’s measurement vector model $\mathbf{z}$ includes noise measurements. The Jacobian matrix $\mathbf{H}$ of the expected state model w.r.t. measurements is defined by

$$\mathbf{H}=\frac{\partial \mathbf{h}}{\partial \mathbf{x}}==\left(\begin{array}{ccc}\frac{\mathrm{\partial \Delta }x}{\partial x_{k-1}}& \frac{\mathrm{\partial \Delta }x}{\partial y_{k-1}}& \frac{\mathrm{\partial \Delta }x}{\partial {\theta }_{k-1}}\\ \frac{\mathrm{\partial \Delta }y}{\partial x_{k-1}}& \frac{\mathrm{\partial \Delta }y}{\partial y_{k-1}}& \frac{\mathrm{\partial \Delta }y}{\partial {\theta }_{k-1}}\\ \frac{\mathrm{\partial \Delta }\theta }{\partial x_{k-1}}& \frac{\mathrm{\partial \Delta }\theta }{\partial y_{k-1}}& \frac{\mathrm{\partial \Delta }\theta }{\partial {\theta }_{k-1}}\end{array}\right).$$E68

The process noise covariance matrix ${\mathbf{Q}}_k$ is defined by

$${\mathbf{Q}}_k\dot{=}{\left|2\pi \mathbf{\Sigma }\right|}^{-1/2}exp\left\{0.5{\left(\mathbf{z}-\mathit{\mu }\right)}^{\top }{\mathbf{\Sigma }}^{-1}\left(\mathbf{z}-\mathit{\mu }\right)\right\}.$$E69

Let us define the nonstationary covariance matrix $\mathbf{P}$,

$$\mathbf{P}=\left(\begin{array}{ccc}{\sigma }_x^2& 0& 0\\ 0& {\sigma }_y^2& 0\\ 0& 0& {\sigma }_{\theta }^2\end{array}\right),$$E70

the matrix diagonal variances are experimental measurements that describe the trend of the robot motion’s error.

The robot’s motion covariance matrix was obtained experimentally through 500 tests—straight motion, right turns, left turns, clockwise and counterclockwise rotations, with $N=100$ tests for each type of motion. Exteroceptive sensing devices onboard are tied to the robot’s geometry of motion, and with their observations the robot’s posture can be estimated and therefore matched with the robot’s deterministic kinematic model. From Section 3, the inverse (38) and direct (33) models were used experimentally to obtain the following statistical covariance about the measurement model

$${\sigma }_x^2=\frac{1}{N}\sum _{i=1}^N\left({\int }_t{\left({\lambda }_1,{\lambda }_1,{\lambda }_1,{\lambda }_1\right)}^{\top }\cdot \dot{\mathrm{\Omega }}\mathrm{d}t\right.\cdot {\left.cos\left({\int }_t{\left(\frac{\mathit{rW}}{l_1^2}\frac{\mathit{rW}}{l_2^2}\frac{-\mathit{rW}}{l_3^2}\frac{-\mathit{rW}}{l_4^2}\right)}^{\top }\cdot \dot{\mathbf{\Omega }}\mathrm{d}t\right)-\overline{x}\right)}^2$$E71

and

$${\sigma }_y^2=\frac{1}{N}\sum _{i=1}^N\left({\int }_t{\left({\lambda }_1,{\lambda }_1,{\lambda }_1,{\lambda }_1\right)}^{\top }\cdot \dot{\mathrm{\Omega }}\mathrm{d}t\right.\cdot {\left.sin\left({\int }_t{\left(\frac{\mathit{rW}}{l_1^2}\frac{\mathit{rW}}{l_2^2}\frac{-\mathit{rW}}{l_3^2}\frac{-\mathit{rW}}{l_4^2}\right)}^{\top }\cdot \dot{\mathbf{\Omega }}\mathrm{d}t\right)-\overline{y}\right)}^2,$$E72

as well as the robot’s yaw statistical measurement model

$${\sigma }_{\theta }^2=\frac{1}{N}\sum _{i=1}^N{\left({\int }_t{\left(\frac{\mathit{rW}}{l_1^2}\frac{\mathit{rW}}{l_2^2}\frac{-\mathit{rW}}{l_3^2}\frac{-\mathit{rW}}{l_4^2}\right)}^{\top }\cdot \dot{\mathbf{\Omega }}\mathrm{d}t-\overline{\theta }\right)}^2.$$E73

Furthermore, the measurement noise covariance matrix is

$${\mathbf{R}}_k=\left(\begin{array}{cccccc}{\sigma }_{x_p}^2& 0& 0& 0& 0& 0\\ 0& {\sigma }_{y_p}^2& 0& 0& 0& 0\\ 0& 0& {\sigma }_{y_p}^2& 0& 0& 0\end{array}\right),$$E74

and the matrix ${\mathbf{W}}_k$ which is the partial derivative of the process model w.r.t. the process noise vector is

$${\mathbf{W}}_k=\frac{\partial f}{\partial {\mathbf{w}}_k}=\left(\begin{array}{ccc}\frac{\partial x_k}{\partial w_x}& \frac{\partial x_k}{\partial w_y}& \frac{\partial x_k}{\partial w_{\theta }}\\ \frac{\partial y_k}{\partial w_x}& \frac{\partial y_k}{\partial w_y}& \frac{\partial y_k}{\partial w_{\theta }}\\ \frac{\partial {\theta }_k}{\partial w_x}& \frac{\partial {\theta }_k}{\partial w_y}& \frac{\partial {\theta }_k}{\partial w_{\theta }}.\end{array}\right).$$E75

The matrix ${\mathbf{V}}_k=\partial h/\partial {\mathbf{v}}_k$ is the partial derivative w.r.t. the measurement noise vector,

$${\mathbf{V}}_k=\left(\begin{array}{ccc}\frac{\partial x_k}{\partial v_{X_p}}& \frac{\partial x_k}{\partial v_{Y_p}}& \frac{\partial x_k}{\partial v_{Z_p}}\\ \frac{\partial y_k}{\partial v_{X_p}}& \frac{\partial y_k}{\partial v_{Y_p}}& \frac{\partial y_k}{\partial v_{Z_p}}\end{array}\right).$$E76

Let us summarize the 3D points ${\mathbf{p}}_{\mathit{AB}}$ and ${\mathbf{p}}_{\mathit{BC}}$ obtained by Theorem 1.

4.1. State feedback position control

This section describes in six general steps the combined use of the visual observers and the EKF geometric odometer as a recursive feedback for the robot’s positioning control. The robot’s deterministic kinematic model conveys predictions about the robot’s geometry of motion and its observations. Therefore, the deterministic model is used to infer the robot’s motion observations implicitly by the trinocular sensor. The following formulation illustrates how the EKF and the visual odometry model are fed back for the 4WD kinematics.

4.1.1. Kalman gain

The initial estimate of Kalman gain is

$${\mathbf{k}}_k={\mathbf{P}}_k{\mathbf{H}}_k^{\top }{\left({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{V}}_k{\mathbf{R}}_x{\mathbf{V}}_k^T\right)}^{-1}$$E77

4.1.2. Observation

From Proposition 1.1, the visual observers provide $m$ 3D key-points from Theorem 1, ${\mathbf{p}}_{\mathit{AB},\mathit{BC}}$

$$\mathrm{\Delta }s\left(\mathbf{p}\right)=\sqrt[2]{\parallel {\mathbf{p}}_{t-1}\parallel +\parallel {\mathbf{p}}_t\parallel -2\parallel {\mathbf{p}}_{t-1}\parallel \parallel {\mathbf{p}}_t\parallel cos\left({\beta }_{t-1}-{\beta }_t\right)}$$

The angle of each key-point $\mathbf{p}$ or $\mathbf{q}$ w.r.t. to the robot in actual time is

$${\lambda }_t=arcsin\left(\frac{\parallel {\mathbf{p}}_{t}sin\left(\widehat{\beta }\right)\parallel }{\mathrm{\Delta }s}\right),$$

and the local angle of the robot w.r.t. the robot’s previous position is

$$\varphi ={\lambda }_t+\left(\frac{\pi }{2}-{\theta }_{t-1}\right),$$

thus the inferred displacement is

$${\mathbf{x}}_k=\left(\begin{array}{c}\frac{1}{m+n}{\sum }_i\mathrm{\Delta }{s}_i^a\left(\mathbf{p}\right)cos\left({\varphi }_i^a\right)\\ \frac{1}{m+n}{\sum }_i\mathrm{\Delta }{s}_i^a\left(\mathbf{p}\right)sin\left({\varphi }_i^a\right)\\ \lambda +\left(\frac{\pi }{2}-{\theta }_{t-1}^a\right)\end{array}\right).$$E78

Therefore, the observation vector with Gauss noise $\mathbf{w}$ is

$${\mathbf{z}}_k=\mathbf{H}\cdot {\mathbf{x}}_k+\left(\begin{array}{c}{w}_x\\ w_y\\ w_{\theta }\end{array}\right).$$E79

4.1.3. Update estimate

The update estimate is obtained by

$${\widehat{\mathbf{x}}}_k={\widehat{\mathbf{x}}}_{k-1}^{-}+{\mathbf{K}}_k\left({\mathbf{z}}_k-{\mathbf{Hx}}_{k-1}\right).$$E80

4.1.4. Update error covariance

The covariance matrix error dispersion of the system is updated

$${\widehat{\mathbf{P}}}_k={\mathbf{P}}_k-{\mathbf{K}}_k{\mathbf{H}}_k{\mathbf{P}}_k.$$E81

4.1.5. Deterministic control model

Therefore, the prediction is firstly obtained through the robot’s inverse position control model, from the inverse kinematics equation, Eq. (38)

$${\mathbf{\Omega }}_{t+1}={\mathbf{\Omega }}_t+{\mathbf{\Lambda }}_t^{-1}\cdot \left({\mathbf{u}}_R^{\mathit{ref}}-{\widehat{\mathbf{x}}}_k\right),$$

where, in the previous expression, ${\mathbf{u}}_R={\left(s,\theta ,x_R,y_R\right)}^{\top }$. Thus, ${\widehat{\mathbf{\Omega }}}_t={\left(\frac{2\pi }{R}\mathrm{\Delta }{\eta }_1,\frac{2\pi }{R}\mathrm{\Delta }{\eta }_2,\frac{2\pi }{R}\mathrm{\Delta }{\eta }_3,\frac{2\pi }{R}\mathrm{\Delta }{\eta }_4\right)}^{\top }$ is the vector of the wheels’ instantaneous measurements

$${\mathbf{u}}_{t+1}={\mathbf{u}}_t-{\mathbf{\Lambda }}_t^{-1}\cdot \left({\mathbf{\Omega }}_{t+1}-{\widehat{\mathbf{\Omega }}}_t\right).$$

This step converges until $\left({\mathbf{u}}_R^{\mathit{ref}}-{\widehat{\mathbf{u}}}_t\right)<{\epsilon }_u$, where ${\epsilon }_u$ is the convergence error. Then, the robot’s prediction model is

$${\overline{\mathbf{x}}}_k={\overline{\mathbf{x}}}_{k-1}+\mathbf{B}\cdot {\mathbf{u}}_{t+1}$$

$\mathbf{B}$ being a control transition matrix.

4.1.6. State prediction

It follows that state prediction is

$${\overline{\mathbf{x}}}_{k+1}={\mathbf{\Phi }}_k+{\overline{\mathbf{x}}}_k+{\mathbf{q}}_k$$E82

and the error dispersion covariance matrix is also predicted at $t+1$

$${\mathbf{P}}_{k+1}={\mathbf{P}}_k+\left(\mathbf{A}+{\mathbf{A}}^{\top }\right){\mathbf{P}}_k\mathrm{\Delta }t+\left({\mathbf{AP}}_k{\mathbf{A}}^{\top }-{\mathbf{\Sigma }}_W\right)\mathrm{\Delta }{t}^2$$E83

From the previous step, the estimation process repeats again, going to step one. The previous Kalman process is performed until the robot reaches the goal and the estimation error converges by numerical approximation according to $\left({\mathbf{x}}_k-{\widehat{\mathbf{x}}}_k\right)\le {\epsilon }_{\mathbf{x}}$.

Therefore, Figure 4a shows the robot’s trajectory obtained by the different comparative approaches conducted in this study. The postures measured by an external visual global reference system are the main references to be compared with. The EKF estimation was obtained by the use of Theorem 1, Proposition 1.1, and Eqs. (71)–(77). In addition, the trinocular key-points used as inputs of the visual odometry model inferred the robot’s displacements, which are shown in same Figure 4a. Furthermore, the dead-reckoning robot system was deployed to infer the robot’s postures and is also shown in Figure 4a. Raw odometry refers to the robot’s dead-reckoning kinematic model used as a mean for direct posture observation through direct kinematics (33) and inverse kinematics (38), but using direct encoder readings by (14).

Figure 4.

Positions and errors. (a) Cartesian positions. (b) Errors vs. global reference. (c) EKF’s Cartesian errors’ convergence. (d) EKF’s angle error convergence.

Figure 4b shows the dead-reckoning and the EKF Cartesian absolute errors, taken as main reference for the visual global reference system. As for the direct dead-reckoning measurements, the absolute error grows exponentially, where the position observation starts diverging before the robot reaches the third turn. As for the EKF model, the Cartesian error w.r.t. the global reference does not diverge but preserves bounded error magnitudes.

As for Figure 4c and d, the EKF’s Cartesian and angular absolute errors w.r.t. the global visual tracker are shown. In Figure 4d, the local minimums and maximums determine the Cartesian regions where the robot performed its turns.

Finally, Figure 5a shows the covariance error behavior obtained at each control loop during the EKF recursive calculations. Figure 5b is a mapping of the measured key-points registered using the state vector (posture) of a robot’s turn to illustrate the map’s divergence.

Figure 5.

Errors’ convergence behavior. (a) EKF variances over time. (b) Key-point map’s divergence using state vectors.

5. Conclusion

This chapter presented a visual odometry scheme for a trinocular divergent visual system that was combined with an EKF for visual odometry estimation. The proposed trinocular geometric model observer geometrically combined adjacent radial views. About $20\%$ of adjacent multi-view overlapping data allowed inference of small volumes of depth information. In measuring 3D key-points, the X-axis metrical error was reported to be lower than 7 cm, with error less than 10 cm in $+Y$ component and less than 3 cm in $-Y$ (vertical). Likewise, we found an averaged Z-axis error less than 5 cm (depth). Such errors were mostly produced by the angular divergence of the object w.r.t. the central camera, rather than linear distances. Indoor experiments, measuring distances up to 10 m were developed. In addition, a set of experimental results in convergent robot’s course gave closed loops, and as the robot moved, the trinocular sensor incrementally stored environmental 3D key-points.

The robot’s trajectory was obtained by different comparative approaches conducted in this study. The postures were measured by an external visual global reference system, which was the main reference system to be compared with. The robotic platform’s kinematics was modeled in terms of a dead-reckoning approach. The direct and the inverse solutions were combined to produce a recursive linearized control model and this was used as the prediction model for EKF estimator. The dead-reckoning robot system was deployed to infer the robot’s postures using directly the four encoders’ readings, with good results obtained only for very short paths. As a comparative perspective, using only the 4WD dead-reckoning system the posture exponentially diverged.

We found bounded Cartesian error for this 4WD robot by deploying the EKF. The trinocular 3D key-points were used as inputs of the visual odometry model that inferred the robot’s displacements by geometrical triangulations.