Mobile Robot Integrated with Gyroscope by Using IKF

Abstract

This paper mainly proposes absolute positioning instruments using camera positioning system and compass. The absolute positioning systems are used to estimate absolute position and orientation errors combined with estimated position and orientation from differential odometry integrated with gyroscope to calculate absolute position and orientation of mobile robot. In the method, the indirect Kalman filter is used to estimate absolute position and orientation errors and the estimated errors are fed back to odometry system, and also estimates some parameter errors to correct odometry system and gyroscope error. The simulation and experimental results show the estimated position and orientation of odometry system integrated with gyroscope, systematic errors of odometry system and gyroscope, absolute position from the camera positioning system and absolute orientation from compass compared with odometry system integrated with gyroscope.

1. Introduction

Typically, mobile robots behavior such as navigation, map building, the estimation of own position is very important. There are a lot of researches regarding the mobile robot in navigation (Maeyama, 1996; Borenstein, 1996; Hakyoung, 2001). The navigation system can be categorized in relative and absolute positioning systems. The relative positioning system estimates a current position by using the information about previous positions and velocities. Basically, the method of estimation for a wheel type mobile robot's position uses the wheel encoder called odometry system. However, in outdoor environment, the estimated position by encoder has the unpredictable error caused by traveling over an unexpected small obstacle or a bump under the wheels. In this case, the accuracy of the estimated robot's position is suddenly getting worse. The position error is detailed in differential equation either inertia measurement unit (IMU) or support sensors nowadays with some solutions in textbook (Britting, 1971). The analysis of navigation process from Kelly (Kelly, 2001) is under special consideration about systematic error and theory on navigation. Also the compensation of position's error with redundant sensors is discussed (Larsen, 1998; Chung, 2001), however always limits on some redundant support systems. Despite these limitations, most researchers agree that encoder is an important part of a robot navigation system and that navigation tasks will be simplified if encoder accuracy can be improved. For example, the map-based positioning system is the method in absolute positioning system by using environment as landmark. A mobile robot builds a local map by using onboard sensors and compares the local map with a global map. If the features from local map and the global map match, an absolute position of the mobile robot is computed. Researches on the map-based positioning have been mainly focused on creating an accurate local map and estimating the positions by a map matching techniques. Cameras are the most complex sensors recently utilized due to high computational and memory requirements, as well as high cost in robotics. On researches in robotic field are using either local or global vision. In global vision, the camera is used as external instrument to monitor area, which covers both of the robot and environment. The global vision is easy to realize and widely used such as in factory environments or in robotic filed, i.e. the popular competition of robot soccer. The absolute positioning system estimates a current position measuring the position from predefined positions. The position errors of the absolute positioning system are bounded, because the information on previous positions is not required. However the absolute positioning system cannot provide absolute position information along path because of absolute positioning instrument's cost and operation in large area. Therefore, a hybrid navigation system plays an important role in combination of the relative or absolute positioning system by selection one from each category. This paper proposes two main points. The first point is the integration of absolute positioning instruments in navigation process, which can support mainly differential odometry system. For the second point the navigation parameter with complementary support system based on indirect Kalman filter (KF) and with model of redundant error sources can compensate each other. The indirect Kalman filter can estimates the errors in the navigation and attitude information using the difference between inertial navigation system (INS) and external source data (Maybeck, 1979). The error model is separated between systematic and non-systematic. The different measurement of error level is performed by complement of redundant support system. For all relevant navigation parameters is with concept of relative and absolute support system in form of external redundant source. In scope of hybrid approach the differential odometry and support system for our purpose are combined that the navigation problem in real-time can be solved. By the experimental test sensor information are from real physical sensors. Section 2 describes the generally differential drive for mobile robot including encoder model. Section 3 explains the widely relative support of gyroscope. The absolute positioning systems, the CaPS and compass are explained in section 4. The hybrid navigation system is described in section 5. Simulations and experiments are presented in section 6 and finally concludes the paper.

2. Differential Drive for Mobile Robot as Main System

Generally, in navigation of mobile robot localization plays an important role. The robot's position is defined as P_k = [X_k, Y_k, θ_k]^T and the next position P_k+1 is calculated from current position plus the position's increment both translation ΔS_k = [ΔS_x,k ΔS_y,k]^T and rotation increment Δθ_k with reference to geometry as

${\mathbf{P}}_{\text{k+1}}={\mathbf{P}}_{\mathrm{k}}+\left[\begin{array}{c}\mathrm{\Delta }{\mathbf{S}}_{\text{x, k}}\\ \mathrm{\Delta }{\mathbf{S}}_{\text{y, k}}\\ \mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}\end{array}\right].$

(1)

With the increment ΔS_k and Δθ_k are used as couple function to construct mobile robot. In the differential drive system it may be considered that mobile robot move in curve path. Generally, the kinematics of differential drive consists of two wheels with wheel base B_k. In Fig. 1 mobile robots are widely integrated with encoders at the left and right wheels, which generate the increment translation ΔS_L,k and ΔS_R,k.

Figure 1.

Kinematics relation between ΔS_k, Δθ_k, B_k, ΔS_L,k and ΔS_R,k

2.1. Position error analysis

By the real robot path the parameter B_k, ΔS_L,k and ΔS_R,k behavior are not linear, because they are affected with error. These errors can not solved by exact equation, however their state-space model can be considered as linear. These errors can be separated in so called systematic and non systematic errors (Borenstein, 1997; Komoriya, 1994).

2.2. Wheel-base (distance between drive wheels) error

The wheel base is distance between both drive wheels. The distance B can not be exactly measured because it can change during running.

This generates wheel-base error ΔB_k and affects in Equation (3) especially during fast turn. Therefore the rotation increment is affect by errors and the robot's orientation error is unbounded too.

Figure 2.

Wheel base of a mobile robot

2.3. Wheel encoder model

The basic concept of odometry is the transformation from wheel revolution to linear translation on the floor. This transformation is affected with errors by wheel slippage, unequal wheel diameter, inaccurate wheel base distance, unknown factor and others. The real increment translation of wheel encoder is prone to systematic and some non-systematic errors.

2.4. Theoretical encoder model

The theoretical translation and rotation increment at the center of the robot is calculated by

$\mathrm{\Delta }{\mathbf{S}}_{\mathrm{k}}^{\mathrm{t}}=\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}}{2},$

(2)

$\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}=\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}-\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}.$

(3)

Position extrapolation with theoretical encoder model is written as

${\mathbf{P}}_{\text{k+1}}^{\mathrm{t}}={\mathbf{P}}_{\mathrm{k}}^{\mathrm{t}}+\mathrm{\Delta }{\mathbf{P}}_{\mathrm{k}}^{\mathrm{t}}$

(4)

and is detailed below

${\mathbf{X}}_{\text{k+1}}^{\mathrm{t}}={\mathbf{X}}_{\mathrm{k}}^{\mathrm{t}}+\mathrm{\Delta }{\mathbf{S}}_{\mathrm{k}}^{\mathrm{t}}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}},$

(5)

${\mathbf{Y}}_{\text{k+1}}^{\mathrm{t}}={\mathbf{Y}}_{\mathrm{k}}^{\mathrm{t}}+\mathrm{\Delta }{\mathbf{S}}_{\mathrm{k}}^{\mathrm{t}}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}},$

(6)

${\mathrm{\theta }}_{\text{k+1}}^{\mathrm{t}}={\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}+\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}.$

(7)

2.5. Practical encoder model

In practice encoders are prone to various systematic and non-systematic errors in the detection of ΔS_R,k and ΔS_L,k. The output of ideal wheel encoder is corrupted by systematic error from hardware imperfections as time and temperature varying scale factor errors (Yang H., et al., 2000; Chung H., et al., 2001). From practical system inputs

$\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}=\left(1+{\mathrm{\Omega }}_{\text{R, k}}\right)•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}=\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}},$

(8)

$\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}=\left(1+{\mathrm{\Omega }}_{\text{L, k}}\right)•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}=\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}},$

(9)

and from Equations (2), (3), (8) and (9) can be transformed in to a translation increment

$\begin{array}{l}\mathrm{\Delta }{\mathbf{S}}_{\mathrm{k}}^{\mathrm{p}}=\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}+\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{2}\\ =\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\text{)}+\text{(}\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}})}{2}\end{array}$

(10)

and rotation increment

$\begin{array}{l}\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}=\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}-\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{{\mathbf{B}}_{\mathrm{k}}+\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}}\\ =\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\text{)}-\text{(}\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}})}{{\mathbf{B}}_{\mathrm{k}}+\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}}.\end{array}$

(11)

Position with practical encoder model can be estimated as

$\begin{array}{l}{\mathbf{X}}_{\text{k+1}}^{\mathrm{p}}={\mathbf{X}}_{\mathrm{k}}^{\mathrm{p}}+\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\text{)}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}\\ +\frac{\text{(}\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}})\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}\end{array}$

(12)

$\begin{array}{l}{\mathbf{Y}}_{\text{k+1}}^{\mathrm{p}}={\mathbf{Y}}_{\mathrm{k}}^{\mathrm{p}}+\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\text{)}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}\\ +\frac{\text{(}\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}})\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2},\end{array}$

(13)

$\begin{array}{l}{\mathrm{\theta }}_{\text{k+1}}^{\mathrm{p}}={\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}+\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\text{)}}{{\mathbf{B}}_{\mathrm{k}}+\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}}\\ -\frac{\text{(}\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}+{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}})}{{\mathbf{B}}_{\mathrm{k}}+\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}}\end{array}.$

(14)

Firstly, the error state δX can be calculated by subtraction of the theoretical position values from practical position values yields the error propagation equations

$\begin{array}{l}\mathrm{\delta }{\mathbf{X}}_{\text{k+1}}=\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}+\frac{{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}+\frac{{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}\\ -\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}})\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}•\mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}}{2}\end{array},$

(15)

$\begin{array}{l}\mathrm{\delta }{\mathbf{Y}}_{\text{k+1}}=\mathrm{\delta }{\mathbf{Y}}_{\mathrm{k}}+\frac{{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}+\frac{{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}\\ +\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}+\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}})\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}•\mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}}{2}\end{array},$

(16)

$\begin{array}{l}\mathrm{\delta }{\mathrm{\theta }}_{\text{k+1}}=\mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}+\frac{{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}-\frac{{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}\\ +\frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}-\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}})•\mathrm{\Delta }B}{{\mathbf{B}}_{\mathrm{k}}^{\mathrm{2}}}\end{array},$

(17)

with the assumption that δθ_k is small then cos δθ_k ≈ 1, sin δθ_k ≈ δθ_k, δθ_k •Ω_R,k ≈ 0, δθ_k •Ω_L,k ≈ 0 and B_k ≫ ΔB_k. Also the encoder scale factor of both wheels and wheel-base distance are regarded as random constants varying slightly in practice, so Ω_R/L,k+1 = Ω_R/L,k, ΔB_k+1 = ΔB_k.

Figure 3.

Gyroscope CRS03 with temperature compensation.

2.6 State space of navigation system

A first-order linearization of Equations (15)–(17) can be evaluated by Jacobians method and the result of navigation system matrix is detailed as Equation (18) in appendix. The state variable, using X_k as the robot's position including some parameters of systematic errors is formulated as

${\mathbf{x}}_{\mathrm{k}}^{\mathrm{O}}={\left[\begin{array}{cccccc}{\hspace{0.17em}}_{\mathbf{X}}^{\mathrm{k}}& {\hspace{0.17em}}_{\mathbf{Y}}^{\mathrm{k}}& {\mathbf{\theta }}_{\mathrm{k}}^{\hspace{0.17em}}& {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k}}}^{\mathrm{O}}& {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k}}}^{\mathrm{O}}& \mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}\end{array}\right]}^{\mathrm{T}}.$

(19)

3. Differential Odometry Integrated with Gyroscope

The creation of relative support for differential odometry is the compensation both translation and rotation increment. The relative support can compensate error from odometry system for translation and rotation increment. With compensation the relative support system can reduce errors generated by noise. This paper presents also relative support system of rotation increment Δθ^G_k with gyroscope sensor. This work selects the commercial gyroscope CRS-03 (Silicon Sensing, 2000) working in measurement area ± 200°/s and Δθ^G = 3.2°±0.01° shown in Fig. 3.

3.1. Gyroscope model

Many researchers on robotic field work on about gyroscope sensor (Komoriya & Oyama, 1993). The theoretical gyroscope produces rotation increment in time interval k defined as Δθ^G,t_k. For position extrapolation with theoretical gyroscope is written as

${\mathrm{\theta }}_{\text{k+1}}^{\text{G, t}}={\mathrm{\theta }}_{\mathrm{k}}^{\text{G, t}}+\mathrm{\Delta }{\mathrm{\theta }}_k^{\text{G, t}}.$

(20)

The practical gyroscope output

$\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{G, P}}=\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{G, t}}+{\mathrm{\Omega }}_{\mathrm{\Omega }}^{\mathrm{k}}•\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{G, t}}$

(21)

is corrupted by the scale factor error Ω^G_k and bias Ψ^G_k. For position extrapolation with practical gyroscope can be formulated as

$\begin{array}{l}{\mathrm{\theta }}_{\text{k+1}}^{\text{G, p}}={\mathrm{\theta }}_{\mathrm{k}}^{\text{G, P}}+\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{G, P}}\\ ={\mathrm{\theta }}_{\mathrm{k}}^{\text{G, P}}+\mathrm{\Delta }{\mathbf{\theta }}_{\mathrm{k}}^{\text{G, t}}+{\mathrm{\Omega }}_{\mathrm{\Omega }}^{\mathrm{k}}•\mathrm{\Delta }{\mathbf{\theta }}_{\mathrm{k}}^{\text{G, t}}+{\mathrm{\Psi }}_{\mathrm{k}}^{\mathrm{G}}\end{array}$

(22)

The scale factor error Ω^G_k and bias Ψ^G_k can be regarded as random constants varying slightly in practical as

${\mathrm{\Omega }}_{\text{k+1}}^{\mathrm{G}}={\mathrm{\Omega }}_{\text{k }}^{\mathrm{G}}\hspace{0.17em},\hspace{0.17em}{\mathit{\Psi }}_{\text{k+1}}^{\mathrm{G}}={\mathit{\Psi }}_{\mathrm{k}}^{\mathrm{G}}$

The error state vector is calculated by subtraction of the theoretical orientation value from practical orientation value yields the error orientation propagation

$\mathrm{\delta }{\mathrm{\theta }}_{\text{k+1}}^{\mathrm{G}}=\mathrm{\delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}+{\mathrm{\Omega }}_{\mathrm{\Omega }}^{\mathrm{k}}•\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{G, t}}+{\mathrm{\Psi }}_{\mathrm{k}}^{\mathrm{G}}$

(23)

3.2. State-space model of gyroscope

From a first-order linearization equation of gyroscope, the resulting time-variant perturbation model can be obtained by

$\begin{array}{c}\left[\begin{array}{c}{\delta \theta }_{k+1}^G\\ {\Omega }_{\mathrm{k}+1}^{\mathrm{G}}\\ {\psi }_{\mathrm{k}+1}^{\mathrm{G}}\end{array}\right]=\left[\begin{array}{lll}1\hfill & \Delta {\theta }_k^{\mathrm{G},\mathrm{t}}\hfill & 1\hfill \\ 0\hfill & 1\hfill & 0\hfill \\ 0\hfill & 0\hfill & 1\hfill \end{array}\right]\left[\begin{array}{l}{\delta \theta }_k^G\hfill \\ {\Omega }_{\mathrm{k}}^{\mathrm{G}}\hfill \\ {\psi }_{\mathrm{k}}^{\mathrm{G}}\hfill \end{array}\right]+{\mathrm{W}}_{\mathrm{k}},\\ \mathrm{\delta }{\mathbf{X}}_{{\hspace{0.17em}}_{\text{k+1}}}^{\mathrm{G}}\text{ = }{\mathbf{A}}_{{\hspace{0.17em}}_{\mathrm{k}}}^{\mathrm{G}}\mathrm{\delta }{\mathbf{X}}_{{\hspace{0.17em}}_{\mathrm{k}}}^{\mathrm{G}}\text{ + }{\mathbf{w}}_{\mathrm{k}}.\end{array}$

(24)

The state variable, using x^G_k including parameters of systematic error is obtained as x^G_k = [θ^G_k Ω^G_k Ψ^G_k]^T.

3.3. Combination of state-space localization model between odometry and gyroscope

The combination of state space model can be written as

$\mathrm{\delta }{\mathbf{X}}_{{\hspace{0.17em}}_{\text{k+1}}}^{\text{OG}}\text{ = }{\mathbf{A}}_{{\hspace{0.17em}}_{\mathrm{k}}}^{\text{OG}}\mathrm{\delta }{\mathbf{X}}_{{\hspace{0.17em}}_{\mathrm{k}}}^{\text{OG}}\text{ + }{\mathbf{w}}_{\mathrm{k}}$

(25)

and detailed as Equation (26) in appendix.

3.4. The combination of odometry system and gyroscope by using different measurement model

The different measurement (Komoriya, 1994; Park, 1998) is calculated by

$\mathrm{\delta }{\mathbf{Z}}_{\mathrm{k}}^{\text{OG}}=\left[\mathrm{\delta }{\mathbf{Z}}_{\mathrm{k}}^{\mathrm{O}}-\mathrm{\delta }{\mathbf{Z}}_{\mathrm{k}}^{\mathrm{G}}\right]+{\mathbf{v}}_{\mathrm{k}}.$

(27)

Therefore from Equation (27) the different measurement of rotation increment from the encoders and gyroscope is written as

$\mathrm{\delta }{\mathbf{Z}}_{\mathrm{k}}^{\text{OG}}=\left[\mathrm{\delta }\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}-\mathrm{\delta }\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}\right]+{\mathbf{v}}_{\mathrm{k}},$

(28)

and detailed as Equation (29)–(30) in appendix. General form of different model between odometry system and gyroscope can be written as

$\mathrm{\delta }{\mathbf{Z}}_{\mathrm{k}}^{\text{OG}}={\mathbf{H}}_{\mathrm{k}}^{\text{OG}}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}^{\text{OG}}+{\mathbf{v}}_{\mathrm{k}}$

(31)

where v_k is the measurement noise.

4. Absolute positioning system

Absolute positioning systems use the geometry relation between the position and environment map. From the geometry measurement the Cartesian coordinates (X, Y, θ) can be specified under certain assumption. If the position of the geometry reference is known, the position of mobile robot can be calculated usually with good accuracy. Some methods have been proposed for robot position reference. One of them has designed landmarks placed at specified positions with respect to the reference coordinate system. The current position and orientation of the robot can be obtained by geometry transformations between robot sensors and these landmarks. Kabuka and Arenas (1987) researched on absolute positioning by using barcodes. Many papers on the evaluation of a low cost gyroscope for application estimation of mobile robot systems were presented. However, other absolute sensing information and accurate error model for inertial systems still needs. In this paper two absolute positioning systems are proposed.

4.1. The CaPS - Camera Positioning System

The design of navigation algorithm for mobile robot needs knowledge about ground truth. In order to quality of the estimated position P̂_k ≈ P^Ideal_k can be analyzed, P^Ideal_k must be known. In practice on mobile robot field lacks tool to measure P^Ideal_k and need more reference points to use P^Ref_k as P^Ideal_k. The robot is stopped and its position is measured with measurement tape, but this method has disadvantages. The position error from position measurement with measurement tape can be not avoided. Because the robot must be stopped to mark a reference points, this damages the continuous dynamic of trajectory. This is the motivation to make a tool, which can support the absolute reference position P^Ref_k for the mobile robot. The Camera Positioning System (CaPS) can calculate the robot's position P^CaPS_k in time interval Δ_t. The CaPS can be defined as

${\mathbf{P}}_{\mathrm{k}}^{\text{CaPS}}\triangleq {\mathbf{P}}_{\mathrm{k}}^{\mathrm{Ref}}$

(32)

The concept of absolute positioning system has more from experiment and commercial system. In closed environment, ultrasonic for robot's positioning can be used, but the system has some disadvantages (Werd 1997).

4.1.1. The CaPS Concept

Generally, machine vision is used to identify the position of objects to be manipulated by a robot, but this paper has purpose to identify the position of mobile robot by using overhead camera shown in Fig. 4.

Figure 4.

The CaPS system.

First principle dealt with the method to establish the relationship between image coordinate reference frame and the real-world coordinate reference frame. The information transformation is shown in Fig. 5. Point Q in a three-dimensional space, referenced to frame R, is projected onto the image plane of the single camera. The electrical signals, picked up by the imaging sensor of the camera, will be converted into an analogue image. If the camera's output is not digital, the image digitizer will convert analogue image into a corresponding digital image. Finally, the digital images from the image digitizer will be the input to the process of geometry measurement in the single camera system. If the image feature q can be detected, then the coordinates of point Q will be determined. This is called the inverse projective-mapping. It is necessary to know the coordinates of image point q are related to the coordinates of corresponding object point Q, which is referenced to frame R.

Figure 5.

Relationship between Camera frame, Image plane and Reference frame.

4.1.2. Forward Projective-Mapping

The forward projective-mapping needs to determine coordinates (u, v) of its image point q in the image array, which is referenced to index coordinate system of u and v. The coordinates (X^Ref, Y^Ref, Z^Ref) of point Q is referenced to frame R. The frame C is assigned to the camera, as shown in Fig. 5. If motion transformation ^CT_R describes the transformation from reference frame R to camera frame C, then the coordinates of point Q with respect to camera frame C are

$\left[\begin{array}{c}{\mathbf{X}}^{\text{Cam}}\\ {\mathbf{Y}}^{\text{Cam}}\\ {\mathbf{Z}}^{\text{Cam}}\\ 1\end{array}\right]={\hspace{0.17em}}^{\mathrm{C}}{\mathbf{T}}_{\mathrm{R}}•\left[\begin{array}{c}{\mathbf{X}}^{\text{Ref}}\\ {\mathbf{Y}}^{\text{Ref}}\\ {\mathbf{Z}}^{\text{Ref}}\\ 1\end{array}\right].$

(33)

The relationship between index coordinates (u, v) and the corresponding coordinates (X^C, Y^C, Z^C) with respect to camera frame C. This relationship is detailed by projective-mapping matrix ^IT_C as below

$\mathbf{S}•\left[\begin{array}{c}\mathbf{u}\\ \mathbf{v}\\ 1\end{array}\right]={\hspace{0.17em}}^{\mathrm{I}}{\mathbf{T}}_C•\left[\begin{array}{c}{\mathbf{X}}^{\text{Cam}}\\ {\mathbf{Y}}^{\text{Cam}}\\ {\mathbf{Z}}^{\text{Cam}}\\ 1\end{array}\right]$

(34)

where S is scaling factor.

The combination of these equations yields

$\mathbf{S}•\left[\begin{array}{c}\mathbf{u}\\ \mathbf{v}\\ 1\end{array}\right]=\left\{{\hspace{0.17em}}^I{\mathbf{T}}_c\right\}•\left\{{\hspace{0.17em}}^c{\mathbf{T}}_R\right\}•\left[\begin{array}{c}{\mathbf{X}}^{\text{Cam}}\\ {\mathbf{Y}}^{\text{Cam}}\\ {\mathbf{Z}}^{\text{Cam}}\\ 1\end{array}\right]$

(35)

This equation describes forward projective-mapping. The coefficients inside ^IT_C are called the camera's intrinsic parameters, while the elements in ^CT_R are called the camera's extrinsic parameters. It can be denoted as

$\mathbf{M}=\{{\hspace{0.17em}}^I{\mathbf{T}}_c\}•\{{\hspace{0.17em}}^c{\mathbf{T}}_R\},$

(36)

matrix M is known as the camera's calibration matrix, which is a 3×4 matrix.

4.1.3. Inverse Projective-Mapping

From the Equation (35)–(36) is written as below

$\mathbf{S}•\left[\begin{array}{c}\mathbf{u}\\ \mathbf{v}\\ 1\end{array}\right]=\mathbf{M}•\left[\begin{array}{c}{\mathbf{X}}^{\text{Ref}}\\ {\mathbf{Y}}^{\text{Ref}}\\ {\mathbf{Z}}^{\text{Ref}}\\ 1\end{array}\right],$

(37)

then the coordinates (X^Ref, Y^Ref, Z^Ref) can be determined, if image point (u, v) are given. In Equation (37), the scaling factor S is unknown. In order to obtain a solution, it is necessary to have at least four constraints. Otherwise, there will be an infinite number of solutions for the unknowns (X^Ref, Y^Ref, Z^Ref) and S. It is clear that one image point will impose only three constraints on the four unknowns. In order to have a unique solution, there are two possible ways to proceed: a) to increase the number of constraints, or b) to reduce the number of unknowns. The camera model is used to map a three-dimensional world point to its corresponding two-dimensional image point and on the contrary to identify the three-dimensional world point corresponding to a particular two-dimensional image point. For this paper, the camera model is considered as linear (Ballard and Brown, 1982).

4.1.4. The CaPS components

The DFW-SX91G is equipped with a 1/2 type 1,440,000-pixel CCD and offers SXGA resolution images. The model incorporate a variety of convenient features such as partial scan (16 × 16 zones), selectable output modes (image size/frame rate) and an external trigger function with an exposure range from 1/100,000 to 17.4 seconds, allowing for the clear capture of fast moving objects or still images in low light environments. The camera is placed in the overhead off-board position, 3m directly above the center of the workspace. The optical axis of the camera is perpendicular to the workspace of the robot. The robot workspace is an area of 3200mm × 2400mm. The camera is connected to a computer via IEEE 1394 interface. After choosing the camera, it is also important to check the availability of suitable lenses. The following formulas are used to calculate the focal length of the needed lens as proposed by (Sony Corporation, 2004).

$\text{Focal length of the width }=\frac{\text{Working distance * CCD width}}{\text{Object width + CCD width}}$

(38)

$\text{Focal length of the height}=\frac{\text{Working distance * CCD height}}{\text{Object height + CCD height}}$

(39)

With the 0.4 inch in CCD-format, CCD-width 6.4 mm and CCD-height 4.8 mm and Equation (38) and (39) yield

$\text{Focal length of the width }=\frac{3000*6.4}{3200+6.4}=5.99(\text{mm}),$

(40)

$\text{Focal length of the height }=\frac{3000*4.8}{2400+4.8}=5.99(\text{mm})$

(41)

, so the focal length of camera can be 6 mm. Finally, we select the H612A (KA) C-mount Pentax lens with the 6 mm of focal length for the DFW-SX910 camera.

4.1.5. Algorithms

The image processing is done using Intel Open Computer Vision (OpenCV) library. The OpenCV library is a cross-platform middle-to-high level Application Programming Interface (API) consisting of more than 300 C-functions and few C⁺⁺ classes that implement image processing operations and some popular image processing algorithms. The software algorithm of the image processing module is described in Fig. 6. The first step of the module is to acquire images by an overhead CCD camera sensor. The images are sent continuously from the camera to computer via IEEE 1394 interface. The second step is to perform a color threshold to identify static obstacles, moving obstacles and background. The images from camera are converted from RGB to HSV. The result is an image consisting of binary pixels. The third step is to create region of interest containing obstacles by removing noises and fill true regions. The fourth step is to correct data from pixel coordinates to real world coordinates. The fifth step is to determine parameters including locations, directions, and sizes. The last step is to send all resulting data to next module and user interface.

Figure 6.

Flowchart to determine parameters and find the center of marker on the robot

In Fig. 6 after threshold and classification, an image consists of binary pixels. The value of each pixel states if the pixel is of interest or not. So, the regions of interest are found easily. To reducing noises, only regions, that are large enough, are taken. After finding the regions of interest on the mobile robot, the regions are bounded in bounding boxes. From these bounding boxes, we can determine the parameters of interest in pixel coordinate such as the position of robot's marker.

4.1.6. Camera calibration

To transform the image coordinate to the real world coordinate, the camera needs to be calibrated. If some camera's extrinsic parameters are unknown, calibration matrix M will also not be known even if its intrinsic parameters are available. Without the calibration, it is not possible for monocular vision to make any interest geometry measurements. Thus, it is necessary to estimate calibration matrix M using a process commonly known as camera calibration. In our work, the coordinates Z^Ref in a reference frame are not importance, if all are equal. The robot runs on the smooth floor, which can be treated as two-dimensional (2-D). This is because their locations are uniquely determined by two coordinates with respect to a frame assigned to the smooth floor. These two coordinates are sufficient to find robot's position. Refer to Equation (37). If we set Z^Ref = 0, the equation becomes

$\mathbf{S}•\left[\begin{array}{c}\mathbf{u}\\ \mathbf{v}\\ 1\end{array}\right]=\mathbf{M}•\left[\begin{array}{c}{\mathbf{X}}^{\text{Ref}}\\ {\mathbf{Y}}^{\text{Ref}}\\ 0\\ 1\end{array}\right].$

(42)

The calibration matrix M, which can be normalized by its element m34, is expressed as

$\mathbf{M}=\left[\begin{array}{cccc}{\mathrm{m}}_{11}& {\mathrm{m}}_{12}& {\mathrm{m}}_{13}& {\mathrm{m}}_{14}\\ {\mathrm{m}}_{21}& {\mathrm{m}}_{22}& {\mathrm{m}}_{23}& {\mathrm{m}}_{24}\\ {\mathrm{m}}_{31}& {\mathrm{m}}_{32}& {\mathrm{m}}_{33}& 1\end{array}\right],$

(43)

and from Equation (41)–(42) can define a 3 × 3 matrix M' as

$M^{\prime }=\left[\begin{array}{ccc}{\mathrm{m}}_{11}& {\mathrm{m}}_{12}& {\mathrm{m}}_{14}\\ {\mathrm{m}}_{21}& {\mathrm{m}}_{22}& {\mathrm{m}}_{24}\\ {\mathrm{m}}_{31}& {\mathrm{m}}_{32}& 1\end{array}\right].$

(44)

From Equation (42)–(44) can be rewritten as

$\left[\begin{array}{c}\mathbf{S}•\mathbf{u}\\ \mathbf{S}•\mathbf{v}\\ \mathbf{S}\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{m}}_{11}& {\mathrm{m}}_{12}& {\mathrm{m}}_{14}\\ {\mathrm{m}}_{21}& {\mathrm{m}}_{22}& {\mathrm{m}}_{24}\\ {\mathrm{m}}_{31}& {\mathrm{m}}_{32}& 1\end{array}\right]•\left[\begin{array}{c}{\mathbf{X}}^{\text{Ref}}\\ {\mathbf{Y}}^{\text{Ref}}\\ 1\end{array}\right].$

(45)

From Equation (45) can be formulated in another form as

$\mathbf{A}•\mathbf{M}=\mathbf{B}$

(46)

where

$\mathbf{A}\text{ = }\left[\begin{array}{cccccccc}{\mathbf{X}}^{\text{Ref}}& {\mathbf{Y}}^{\text{Ref}}& 1& 0& 0& 0& -\mathbf{u}{\mathbf{X}}^{\text{Ref}}& -\mathbf{u}{\mathbf{Y}}^{\text{Ref}}\\ 0& 0& 0& {\mathbf{X}}^{\text{Ref}}& {\mathbf{Y}}^{\text{Ref}}& 1& -\mathbf{v}{\mathbf{X}}^{\text{Ref}}& -\mathbf{v}{\mathbf{Y}}^{\text{Ref}}\end{array}\right],$

(47)

$\mathbf{B}\text{ = }\left[\begin{array}{c}\mathbf{u}\\ \mathbf{v}\end{array}\right]$

(48)

and

$\mathbf{M}={\left[\begin{array}{cccccccc}{\mathrm{m}}_{11}& {\mathrm{m}}_{12}& {\mathrm{m}}_{14}& {\mathrm{m}}_{21}& {\mathrm{m}}_{22}& {\mathrm{m}}_{24}& {\mathrm{m}}_{31}& {\mathrm{m}}_{32}\end{array}\right]}^{{\rm T}}.$

(49)

The Equation (46) describes the linear relationship among vectors A, M and B. The coordinates (X^Ref, Y^Ref) of a point in a scene, and its image coordinates (u, v) are known, Equation (46) imposes two constraints on the eight unknowns in vector M. In order to derive a unique solution for the eight unknowns in M, we must have at least eight constraints. Since one pair of {(u, v), (X^Ref, Y^Ref)} provides two constraints, it is necessary to have at least four pairs of {(u, v), (X^Ref, Y^Ref)} in order to calibrate a camera. Assume that a set of n pairs of {(u, v), (X^Ref, Y^Ref)} are available (n ⩾ 6). The optimal solution, which minimizes the squared error of Equation (46), is obtained as

$\text{||}\mathbf{A}•\mathbf{M}-\mathbf{B}{\text{||}}^{\text{2 }}\text{is }\mathbf{M}={\text{(}{\mathbf{A}}^{\mathrm{T}}\mathbf{A}\text{)}}^{\text{-1}}•\text{ (}{\mathbf{A}}^{\mathrm{T}}\mathbf{B}\text{)}.$

(50)

In our work four points R₁ = (x₁, y₁), R₂ = (x₂, y₂), R₃ = (x₃, y₃), R₄ = (x₄, y₄) are selected in real world space to calibrate the camera. They correspond to four image points I₁ = (u₁, v₁), I₂ = (u₂, v₂), I₃ = (u₃, v₃) and I₄ = (u₄, v₄) in the image space, so the matrix equation can be defined as

$\mathbf{A}=\left[\begin{array}{cccccccc}{\mathbf{x}}_1& {\mathbf{y}}_1& 1& 0& 0& 0& -{\mathbf{u}}_1{\mathbf{x}}_1& -{\mathbf{u}}_1{\mathbf{y}}_1\\ 0& 0& 0& {\mathbf{x}}_1& {\mathbf{y}}_1& 1& -{\mathbf{v}}_1{\mathbf{x}}_1& -{\mathbf{v}}_1{\mathbf{y}}_1\\ {\mathbf{x}}_2& {\mathbf{y}}_2& 1& 0& 0& 0& -{\mathbf{u}}_2{\mathbf{x}}_2& -{\mathbf{u}}_2{\mathbf{y}}_2\\ 0& 0& 0& {\mathbf{x}}_2& {\mathbf{y}}_2& 1& -{\mathbf{v}}_2{\mathbf{x}}_2& -{\mathbf{v}}_2{\mathbf{y}}_2\\ {\mathbf{x}}_3& {\mathbf{y}}_3& 1& 0& 0& 0& -{\mathbf{u}}_3{\mathbf{x}}_3& -{\mathbf{u}}_3{\mathbf{y}}_3\\ 0& 0& 0& {\mathbf{x}}_3& {\mathbf{y}}_3& 1& -{\mathbf{v}}_3{\mathbf{x}}_3& -{\mathbf{v}}_3{\mathbf{y}}_3\\ {\mathbf{x}}_4& {\mathbf{y}}_4& 1& 0& 0& 0& -{\mathbf{u}}_4{\mathbf{x}}_4& -{\mathbf{u}}_4{\mathbf{y}}_4\\ 0& 0& 0& {\mathbf{x}}_4& {\mathbf{y}}_4& 1& -{\mathbf{v}}_4{\mathbf{x}}_4& -{\mathbf{v}}_4{\mathbf{y}}_4\end{array}\right]$

(51)

$\mathbf{M}={\left[\begin{array}{cccccccc}{\mathrm{m}}_{11}& {\mathrm{m}}_{12}& {\mathrm{m}}_{14}& {\mathrm{m}}_{21}& {\mathrm{m}}_{22}& {\mathrm{m}}_{24}& {\mathrm{m}}_{31}& {\mathrm{m}}_{32}\end{array}\right]}^{\mathrm{T}}$

(52)

and

$\mathbf{B}={\left[\begin{array}{cccccccc}{\mathbf{u}}_1& {\mathbf{v}}_1& {\mathbf{u}}_2& {\mathbf{v}}_2& {\mathbf{u}}_3& {\mathbf{v}}_3& {\mathbf{u}}_4& {\mathbf{v}}_4\end{array}\right]}^{\mathrm{{\rm T}}}.$

(53)

The Matrix M can be solved by using PMADE software as shown in Fig. 7.

Figure 7.

PMADE software developed for Matrix M parameter solving

After the calibration matrix can be determined, the real world coordinate from a pixel (u, v) in the image space can be calculated from Equation (45). If we eliminate unknown S in Equation (45), then the equation is written

$\left[\begin{array}{c}{\mathrm{m}}_{11}{\mathbf{X}}^{\text{Ref}}+{\mathrm{m}}_{12}{\mathbf{Y}}^{\text{Ref}}+{\mathrm{m}}_{14}-\mathbf{u}•{\mathrm{m}}_{31}{\mathbf{X}}^{\text{Ref}}-\mathbf{u}•{\mathrm{m}}_{32}{\mathbf{Y}}^{\text{Ref}}=\mathbf{u}\\ {\mathrm{m}}_{21}{\mathbf{X}}^{\text{Ref}}+{\mathrm{m}}_{22}{\mathbf{Y}}^{\text{Ref}}+{\mathrm{m}}_{24}-\mathbf{v}•{\mathrm{m}}_{31}{\mathbf{X}}^{\text{Ref}}-\mathbf{v}•{\mathrm{m}}_{32}{\mathbf{Y}}^{\text{Ref}}=\mathbf{v}\end{array}\right]$

(54)

and then

$\left[\begin{array}{cc}{\mathrm{m}}_{11}-{\mathrm{m}}_{31}\mathbf{u}& {\mathrm{m}}_{12}-{\mathrm{m}}_{32}\mathbf{u}\\ {\mathrm{m}}_{21}-{\mathrm{m}}_{31}\mathbf{v}& {\mathrm{m}}_{22}-{\mathrm{m}}_{32}\mathbf{v}\end{array}\right]\left[\begin{array}{c}{\mathbf{X}}^{\text{Ref}}\\ {\mathbf{Y}}^{\text{Ref}}\end{array}\right]\text{ = }\left[\begin{array}{c}\mathbf{u}-{\mathrm{m}}_{14}\\ \mathbf{v}-{\mathrm{m}}_{24}\end{array}\right].$

(55)

Finally, the robot's position (X^Ref, Y^Ref) can be determined

$\left[\begin{array}{c}{\mathbf{X}}^{\text{Ref}}\text{ = }\frac{\text{(}\mathbf{u}-{\mathrm{m}}_{14}{\text{)(m}}_{22}-{\mathrm{m}}_{32}\mathbf{u}\text{)}-\text{(}\mathbf{v}-{\mathrm{m}}_{24}{\text{)(m}}_{12}-{\mathrm{m}}_{32}\mathbf{u}\text{)}}{{\text{(m}}_{11}-{\mathrm{m}}_{31}\mathbf{u}{\text{)(m}}_{22}-{\mathrm{m}}_{32}\mathbf{v}\text{)}-{\text{(m}}_{21}-{\mathrm{m}}_{31}\mathbf{v}{\text{)(m}}_{12}-{\mathrm{m}}_{32}\mathbf{u}\text{)}}\\ {\mathbf{Y}}^{\text{Ref}}\text{ = }\frac{\text{(}\mathbf{v}-{\mathrm{m}}_{24}{\text{)(m}}_{11}-{\mathrm{m}}_{31}\mathbf{u}\text{)}-\text{(}\mathbf{u}-{\mathrm{m}}_{14}{\text{)(m}}_{21}-{\mathrm{m}}_{31}\mathbf{v}\text{)}}{{\text{(m}}_{11}-{\mathrm{m}}_{31}\mathbf{u}{\text{)(m}}_{22}-{\mathrm{m}}_{32}\mathbf{v}\text{)}-{\text{(m}}_{21}-{\mathrm{m}}_{31}\mathbf{v}{\text{)(m}}_{12}-{\mathrm{m}}_{32}\mathbf{u}\text{)}}\end{array}\right]$

(56)

In this section the CaPS is in IKF-algorithm integrated. The absolute position of mobile robot is calculated from the CaPS and then fed to IKF algorithm. From Equation (56) the absolute position (X^Ref, Y^Ref) of the robot can be generated from the CaPS. This absolute position from the CaPS will be compared with the absolute position from odometry integrated with gyroscope by using difference measurement of absolute position. The navigation method about absolute positioning system needs the environment map to localize the object's position. In case of the CaPS, the navigation system of mobile robot does not need environment information, because the robot's absolute position is directly measured as

${\mathbf{P}}_{\mathrm{k}}^{\text{Ref}}=\left[\begin{array}{c}{\mathbf{X}}_{\mathrm{k}}^{\text{Ref}}\\ {\mathbf{Y}}_{\mathrm{k}}^{\text{Ref}}\end{array}\right]={\mathbf{P}}_{\mathrm{k}}^{\text{CaPS}}=\left[\begin{array}{c}{\mathbf{X}}_{\mathrm{k}}^{\text{CaPS}}\\ {\mathbf{Y}}_{\mathrm{k}}^{\text{CaPS}}\end{array}\right].$

(57)

The CaPS can generate absolute position (P^CaPS_k) at center of mobile robot and the estimated position from odometry integrated with gyroscope can be calculated as

${\widehat{P}}_{\mathrm{k}}^{\text{OG}}=\left[\begin{array}{c}{\widehat{X}}_{\mathrm{k}}^{\text{OG}}\\ {\widehat{Y}}_{\mathrm{k}}^{\text{OG}}\end{array}\right].$

(58)

By subtraction Equation (58) from Equation (57), a different measurement equation for position error is obtained by

$\mathrm{\delta }{\mathbf{P}}_{\mathrm{k}}^{\text{OG, CaPS}}={\widehat{P}}_{\mathrm{k}}^{\text{OG}}-{\mathbf{P}}_{\mathrm{k}}^{\text{CaPS}}=\left[\begin{array}{c}{\widehat{X}}_{\mathrm{k}}^{\text{OG}}\\ {\widehat{Y}}_{\mathrm{k}}^{\text{OG}}\end{array}\right]-\left[\begin{array}{c}{\mathbf{X}}_{\mathrm{k}}^{\text{CaPS}}\\ {\mathbf{Y}}_{\mathrm{k}}^{\text{CaPS}}\end{array}\right].$

(59)

4.2 Absolute orientation support with electronic compass

Kim (1996) described the experiments on orientation recovery of mobile robot by using compass. Ojeda, L. (2000) discussed on the use of electronic compasses in mobile robots with different error sources and solutions to correct these errors. In this paper the CMPS03 compass module shown in Fig. 8 is used, specifically designed for mobile robot as an aid to navigation with resolution ± 0.1° (Surachai, P., 2008). The compass can generate direct the output of absolute orientation (θ^C_k) located at the mobile robot's center. From the odometry system integrated with gyroscope, the mobile robot's orientation is estimated as θ̂^OG_k. By subtraction of estimated absolute orientation θ̂^OG_k from compass θ^C_k, the absolute orientation error is obtained as

$\mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{OG, C}}={\widehat{\mathrm{\theta }}}_{\mathrm{k}}^{\text{OG}}-{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}$

(60)

Figure 8.

The CMPS03 compass module

5. Hybrid Navigation System from Absolute Positioning System

This paper presents the systems of main differential odometry integrated with gyroscope in mobile robot.

Figure 9.

IKF loop the combination of support system and odometry system in measurement step

Gyroscope generates rotation increment used to compensate rotation increment from differential odometry. The absolute positioning system for orientation θ^C with CMPS03 compass module (resolution ±0.1°) is used as orientation reference. The CaPS (Camera Positioning System) as the absolute positioning system is used as reference position for mobile robot. To construct the combination of the support system with odometry system of mobile robot is different measurement in the same physical variable, i.e. rotation or translation increment or absolute orientation or position between odometry and external support system. Then the estimate of error state δX from IKF (indirect Kalman filter) is calculated to correct the navigation state. The support systems are integrated in “Measurement Update” step of IKF algorithm in Fig. 9. If the connector A is only closed by software, the IKF loop to combine main odometry system and gyroscope is started. The IKF loop of odometry system integrated with gyroscope, which is combined with the CaPS, will start by only closed connector B. The compass will be combined with the odometry system integrated with gyroscope in IKF loop, if connector C is only close.

5.1. The combination of odometry integrated with gyroscope and the CaPS

A state equation from basic odometry integrated with gyroscope is obtained by Equation (26) in appendix. The measurement equation Z_k = HX_k + V_k between odometry integrated with gyroscope system and the CaPS can be written as

${\mathbf{Z}}_{\mathrm{k}}={\widehat{P}}_{\mathrm{k}}^{\text{OG}}-{\mathbf{P}}_{\mathrm{k}}^{\text{CaPS}}=\left[\begin{array}{c}{\widehat{X}}_{\mathrm{k}}^{\text{OG}}\\ {\widehat{Y}}_{\mathrm{k}}^{\text{OG}}\end{array}\right]-\left[\begin{array}{c}{\mathbf{X}}_{\mathrm{k}}^{\text{CaPS}}\\ {\mathbf{Y}}_{\mathrm{k}}^{\text{CaPS}}\end{array}\right]+{\mathbf{v}}_{\mathrm{k}}$

(61)

Then the measurement matrix of Equation (61) is

$\begin{array}{l}{\mathbf{Z}}_{\mathrm{k}}^{\text{OG, CaPS}}=\left[\begin{array}{cccccc}\begin{array}{l}1\\ 0\end{array}& \begin{array}{l}0\\ 1\end{array}& \begin{array}{l}0\\ 0\end{array}& \begin{array}{l}0\\ 0\end{array}& \begin{array}{l}0\\ 0\end{array}& \begin{array}{l}\begin{array}{cc}0& \begin{array}{cc}0& \begin{array}{cc}0& 0\end{array}\end{array}\end{array}\\ \begin{array}{cc}0& \begin{array}{cc}0& \begin{array}{cc}0& 0\end{array}\end{array}\end{array}\end{array}\end{array}\right]\left[\begin{array}{c}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{Y}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k}}}^{\mathrm{O}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k}}}^{\mathrm{O}}\\ \begin{array}{l}\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}^G\\ {\mathrm{\Omega }}_{\mathrm{k}}^{\mathrm{G}}\\ {\mathrm{\Psi }}_{\mathrm{k}}^{\mathrm{G}}\end{array}\end{array}\right]+{\mathbf{v}}_{\mathrm{k}}\\ ={\mathbf{H}}_{\mathrm{k}}^{\text{OG},\text{CaPS}}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}^{\text{OG}}+{\mathbf{v}}_{\mathrm{k}}\end{array}$

(62)

, where v_k is the measurement noise.

5.2 The combination of odometry integrated with gyroscope and compass

Same as the combination of the CaPS, the state equation is obtained by Equation (26) in appendix. The measurement equation Z_k = HX_k + v_k between odometry integrated with gyroscope system and compass can be given as

${\mathbf{Z}}_{\mathrm{k}}={\widehat{\mathrm{\theta }}}_{\mathrm{k}}^{\text{OG}}-{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}=\left[{\widehat{\mathrm{\theta }}}_{\mathrm{k}}^{\text{OG}}-{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}\right]+{\mathbf{v}}_{\mathrm{k}}.$

(63)

Then the measurement matrix of Equation (63) is

$\begin{array}{l}{\mathbf{Z}}_{\mathrm{k}}^{\text{OG, C}}=\left[\begin{array}{ccccccccc}0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{Y}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k}}}^{\mathrm{O}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k}}}^{\mathrm{O}}\\ \begin{array}{l}\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}^G\\ {\mathrm{\Omega }}_{\mathrm{k}}^{\mathrm{G}}\\ {\mathrm{\Psi }}_{\mathrm{k}}^{\mathrm{G}}\end{array}\end{array}\right]+{\mathbf{v}}_{\mathrm{k}}\\ ={\mathbf{H}}_{\mathrm{k}}^{\text{OG,C}}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}^{\text{OG}}+{\mathbf{v}}_{\mathrm{k}}\end{array}$

(64)

, where v_k is the measurement noise.

5.3. Overall structure of navigation system diagram

The main odometry integrated with gyroscope system and absolute positioning instruments is shown in Fig. 10 in appendix. The different measurement of absolute position between the CaPS and odometry system integrated with gyroscope is used as measurement value in IKF loop to estimate the absolute position error.

Figure 11.

Pioneer-II integrated with gyroscope and compass

To estimate the absolute orientation error, the different orientation between compass and odometry system integrated with gyroscope is used as a measurement value in IKF loop. In IKF loop error parameters of main system are also estimated and fed back to correct encoder error and gyroscope error.

6. Simulation and Experimental Results

The navigation error state from odometry δX^o occurring during long trajectory and high dynamic environment is reduced by linear state vector and estimated by IKF algorithm. In order to the position estimation of mobile robot can be kept with pinpoint accuracy, the reciprocal error compensation is based on between odometry error and gyroscope error.

6.1. The result of combination with gyroscope Support

The mobile robot (Pioneer-II from active media company) driven by differential odometry system integrated with gyroscope and compass shown in Fig. 11 is used for experiment in square shape 2.5×2.0 m. In experiment the robot starts at the position coordinate (x = 0, y = 0, θ = 0°) and runs from start point in right direction and returns at the start point again with speed 0.2 m/s. From experiment the robot's final position from odometry system are P̂^o (X̂^o = 24.4 cm, Ŷ^o = −36.1 cm) and from odometry system integrated with gyroscope P̂^OG (X̂^OG = 8.7 cm, Ŷ^OG = −12.4 cm) as shown in Fig. 12. In Fig. 13 shows the orientation estimation. The red line represents the orientation of the robot from gyroscope system, the cyan line from mainly odometry system and the blue line is the combination of odometry and gyroscope by using different measurement of increment rotation with IKF algorithm. In Fig. 14 with IKF algorithm can be minimized the different orientation between odometry system and gyroscope at maximum 14°. Also systematic errors of odometry system are shown in Fig. 15 and gyroscope in Fig. 16 and detail in Table 1

Figure 12.

Position generated by encoders is shown in cyan line and the blue line shows the position estimation of the robot by using different measurement of rotation increment between encoders and gyroscope with IKF algorithm

Figure 13.

Orientation of the robot in four corners with counter clockwise running

Figure 14.

Different orientation between odometry system and gyroscope

6.2 The result of combination with the CaPS support

Figure 15.

Systematic error of odometry system

In this section, details the powerful IKF algorithm with the CaPS support. The CaPS concept is more details in previous section. In Fig. 17 shows the graphic of robot's trajectory in square shape from the CaPS compared with the reference point.

Figure 16.

Systematic error of gyroscope

Table 1.

Details the systematic error of odometry system and gyroscope.

Systematic error of	Odometry	Gyroscope
Scale factor error of left encoder	0.9962–1.0004	—
Scale factor error of right encoder	0.9947–1.0010	—
Wheel-base error	0 – 9 mm.	—
Scale factor error	—	1.0000 – 1.0078
Bias error	—	(− 0.0004) – 0.0038

The different measurement of position P̂^OG,IKF and P^CaPSas shown in Fig. 18 will be send to the second measurement update as sh9own in Fig. 9 to reduce absolute position error. In Fig. 19 shows trajectory of the robot running from the calculated position P^o from odometry equation (cyan line), P^CaPS generated by the CaPS (red line) and estimated position P̂ with gyroscope and the CaPS support by using IKF algorithm (blue line).

Figure 17.

Absolute position generated by the CaPS compared with the reference position.

Figure 18.

Difference absolute position between estimate positions from odometry integrated with gyroscope and from the CaPS.

Figure 19.

Implement of the CaPS positioning system in IKF algorithm for estimate of robot position in global coordinate.

For the navigation algorithm the reference position plays an important role. To reduce position error of odometry, the reference position must be known to find the difference position between odometry system and reference position in time.

Figure 20.

Orientation of the robot from odometry, gyroscope, compass and the combination of main odometry system integrated with gyroscope and absolute positioning by using IKF algorithm.

Generally the reference positions are constructed by manual measurement, which is difficult and not possible to measure the reference position in time. But the CaPS can generate the reference or absolute position compared with position from odometry system in the same time. In Table 2 shows the results of final coordinate of robot trajectory compared with actual reference coordinate.

Table 2.

Results of the final position of mobile robot in comparison to ideal coordinate.

Final Coordinate	X(mm)	Y(mm)
Actual Position	40	50
Odometry	244	-361
CaPS	-88	75
Odo.+ Gyro.+ CaPS with IKF	-57	91

6.3 Result of combination with compass

This section explains the results of the orientation of mobile robot with gyroscope and compass support by using IKF algorithm as shown in Fig. 20. The cyan line represents the orientation from odometry, the red line from relative support of gyroscope, the blue line from absolute support of compass and the black line shows the combination of odometry system, gyroscope and compass by using IKF algorithm. The estimated orientation θ̂^OG,IKF_k is obtained from odometry with gyroscope support by using the first measurement update. The different measurement of θ̂^OG,IKF_k and θ^C_k shown in Fig. 21 will be passed to the third measurement update in Fig. 9 to reduce absolute orientation error.

Figure 21.

Different absolute orientation between from main odometry integrated with gyroscope by using IKF algorithm and from compass.

7. Conclusion

This paper proposed the hybrid absolute positioning system using the CaPS and compass for odometry system integrated with gyroscope for mobile robot in an indoor environment. The CaPS provides position information to reduce the position error due to the main odometry integrated with gyroscope error. To reduce the absolute orientation error compass is combined with odometry system integrated with gyroscope. The indirect feedback Kalman filter (IKF) is used as the combination of odometry system, gyroscope and the absolute positioning system to estimate and compensate errors. The IKF can estimate the error of estimated absolute position and orientation by using the information from the CaPS and compass. Then mobile robot's position and orientation are obtained from addition of the estimated position and orientation error with the estimated position and orientation from main system. Also the IKF estimated the scale factors of the encoders, the distance error between wheels and scale factor and bias errors of gyroscope to correct errors from encoders and gyroscope. The simulation results show that the IKF can give precisely estimated position and orientation by using absolute positioning system. Further research aims to support the translation increment for odometry system with an accelerometer. The complementary measurement between translation increment from encoder and accelerometer can be passed to the filter.

Appendix

$\begin{array}{l}\left[\begin{array}{c}\mathrm{\delta }{\mathbf{X}}_{\text{k+1}}\\ \mathrm{\delta }{\mathbf{Y}}_{\text{k+1}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\text{k+1}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k+1}}}^{\mathrm{O}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k+1}}}^{\mathrm{O}}\\ \mathrm{\Delta }{\mathbf{B}}_{\text{k+1}}\end{array}\right]=\left[\begin{array}{cccccc}1& 0& -\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}\text{ + }\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{2}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& 0\\ 0& 1& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}\text{ + }\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{2}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& 0\\ 0& 0& 1& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& -\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& -\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}\text{ + }\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{{\mathbf{B}}_{\mathrm{k}}^{\mathrm{2}}}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{Y}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k}}}^{\mathrm{O}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k}}}^{\mathrm{O}}\\ \mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}\end{array}\right]\\ \\ \end{array}$

(18)

$\left[\begin{array}{c}\mathrm{\delta }{\mathbf{X}}_{\text{k+1}}\\ \mathrm{\delta }{\mathbf{Y}}_{\text{k+1}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\text{k+1}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k+1}}}^{\mathrm{O}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k+1}}}^{\mathrm{O}}\\ \begin{array}{l}\mathrm{\Delta }{\mathbf{B}}_{\text{k+1}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\text{k+1}}^G\\ {\mathrm{\Omega }}_{\text{k+1}}^{\mathrm{G}}\\ {\mathrm{\Psi }}_{\text{k+1}}^{\mathrm{G}}\end{array}\end{array}\right]=\left[\begin{array}{ccccccccc}1& 0& -\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}\text{ + }\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{2}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& 0& 0& 0& 0\\ 0& 1& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}\text{ + }\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{2}\cos {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}\sin {\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}}{2}& 0& 0& 0& 0\\ 0& 0& 1& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& -\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& -\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{p}}\text{ + }\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{p}}}{{\mathbf{B}}_{\mathrm{k}}^{\mathrm{2}}}& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \mathrm{\Delta }{\mathrm{\theta }}_k^{\text{G, t}}& 1\\ 0& 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{Y}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k}}}^{\mathrm{O}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k}}}^{\mathrm{O}}\\ \begin{array}{l}\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}^G\\ {\mathrm{\Omega }}_{\mathrm{\Omega }}^{\mathrm{k}}\\ {\mathrm{\Psi }}_{\mathrm{k}}^{\mathrm{G}}\end{array}\end{array}\right]\text{+ }{\mathbf{w}}_{\mathrm{k}}$

(26)

$\begin{array}{l}\mathrm{\delta }{\mathbf{Z}}_{\mathrm{k}}^{\text{OG}}=\mathrm{\delta }\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}-\mathrm{\delta }\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{\theta }}^{\mathrm{k}}\\ =\left\{\left[\begin{array}{ccc}\frac{{\mathrm{\Omega }}_{\text{R, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& -\frac{{\mathrm{\Omega }}_{\text{L, k}}•\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& \frac{(\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}-\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}})•\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}}{{\mathbf{B}}_{\mathrm{k}}^{\mathrm{2}}}\end{array}\right]-\left[\begin{array}{cc}{\mathrm{\Omega }}_{\mathrm{\Omega }}^{\mathrm{k}}•\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{G, t}}& {\mathit{\Psi }}_{\mathrm{k}}^{\mathrm{G}}\end{array}\right]\right\}+{\mathbf{v}}_{\mathrm{k}}\end{array}$

(29)

$\mathrm{\delta }{\mathbf{Z}}_{\mathrm{k}}^{\text{OG}}=\left[\begin{array}{ccccccccc}0& 0& 0& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& -\frac{\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}}& \frac{\mathrm{\Delta }{\mathbf{S}}_{\text{L, k}}^{\mathrm{t}}-\mathrm{\Delta }{\mathbf{S}}_{\text{R, k}}^{\mathrm{t}}}{{\mathbf{B}}_{\mathrm{k}}^{\mathrm{2}}}& 0& -\mathrm{\Delta }{\mathrm{\theta }}_{\mathrm{k}}^{\text{G, t}}& -1\end{array}\right]\left[\begin{array}{c}\mathrm{\delta }{\mathbf{X}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathbf{Y}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{R, k}}}^{\mathrm{O}}\\ {\mathrm{\Omega }}_{{\hspace{0.17em}}_{\text{L, k}}}^{\mathrm{O}}\\ \begin{array}{l}\mathrm{\Delta }{\mathbf{B}}_{\mathrm{k}}\\ \mathrm{\delta }{\mathrm{\theta }}_{\mathrm{k}}^G\\ {\mathit{\Omega }}_{\mathrm{k}}^{\mathrm{G}}\\ {\mathit{\Psi }}_{\mathrm{k}}^{\mathrm{G}}\end{array}\end{array}\right]+{\mathbf{v}}_{\mathrm{k}}$

(30)

Figure 10.

Overall combinations of support system and odometry system