Mobile Robot Integrated with Gyroscope by Using IKF

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.

OPEN IN VIEWER

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

Δ S k t = Δ S R, k t + Δ S L, k t 2 ,

(2)

Δ θ θ k = Δ S R, k t − Δ S L, k t B k .

(3)

Position extrapolation with theoretical encoder model is written as

P k+1 t = P k t + Δ P k t

(4)

and is detailed below

X k+1 t = X k t + Δ S k t cos θ θ k ,

(5)

Y k+1 t = Y k t + Δ S k t sin θ θ k ,

(6)

θ k+1 t = θ θ k + Δ θ θ 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

Δ S R, k p = ( 1 + Ω R, k ) • Δ S R, k t = Δ S R, k t + Ω R, k • Δ S R, k t ,

(8)

Δ S L, k p = ( 1 + Ω L, k ) • Δ S L, k t = Δ S L, k t + Ω L, k • Δ S L, k t ,

(9)

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

Δ S k p = Δ S R, k p + Δ S L, k p 2 = ( Δ S R, k t + Ω R, k • Δ S R, k t ) + ( Δ S L, k t + Ω L, k • Δ S L, k t ) 2

(10)

and rotation increment

Δ θ θ k = Δ S R, k p − Δ S L, k p B k + Δ B k = ( Δ S R, k t + Ω R, k • Δ S R, k t ) − ( Δ S L, k t + Ω L, k • Δ S L, k t ) B k + Δ B k .

(11)

Position with practical encoder model can be estimated as

X k+1 p = X k p + ( Δ S R, k t + Ω R, k • Δ S R, k t ) cos θ θ k 2 + ( Δ S L, k t + Ω L, k • Δ S L, k t ) cos θ θ k 2

(12)

Y k+1 p = Y k p + ( Δ S R, k t + Ω R, k • Δ S R, k t ) sin θ θ k 2 + ( Δ S L, k t + Ω L, k • Δ S L, k t ) sin θ θ k 2 ,

(13)

θ k+1 p = θ θ k + ( Δ S R, k t + Ω R, k • Δ S R, k t ) B k + Δ B k − ( Δ S L, k t + Ω L, k • Δ S L, k t ) B k + Δ B k .

(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

δ X k+1 = δ X k + Ω R, k • Δ S R, k t cos θ θ k 2 + Ω L, k • Δ S L, k t cos θ θ k 2 − ( Δ S R, k t + Δ S L, k t ) sin θ θ k • δ θ k 2 ,

(15)

δ Y k+1 = δ Y k + Ω R, k • Δ S R, k t sin θ θ k 2 + Ω L, k • Δ S L, k t sin θ θ k 2 + ( Δ S R, k t + Δ S L, k t ) cos θ θ k • δ θ k 2 ,

(16)

δ θ k+1 = δ θ k + Ω R, k • Δ S R, k t B k − Ω L, k • Δ S L, k t B k + ( Δ S L, k t − Δ S R, k t ) • Δ B B k 2 ,

(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.

OPEN IN VIEWER

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

x k O = [ X k Y k θ k Ω R, k O Ω L, k O Δ B k ] T .

(19)

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

θ k+1 G, t = θ k G, t + Δ θ k G, t .

(20)

The practical gyroscope output

Δ θ k G, P = Δ θ k G, t + Ω Ω k • Δ θ k 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

θ k+1 G, p = θ k G, P + Δ θ k G, P = θ k G, P + Δ θ k G, t + Ω Ω k • Δ θ k G, t + Ψ k G

(22)

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

Ω k+1 G = Ω k G , Ψ k+1 G = Ψ k G

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

δ θ k+1 G = δ θ θ k + Ω Ω k • Δ θ k G, t + Ψ k 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

[ δ θ k + 1 G Ω k + 1 G ψ k + 1 G ] = [ 1 Δ θ k G , t 1 0 1 0 0 0 1 ] [ δ θ k G Ω k G ψ k G ] + W k , δ X k+1 G = A k G δ X k G + w k .

(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

δ X k+1 OG = A k OG δ X k OG + w 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

δ Z k OG = [ δ Z k O − δ Z k G ] + v k .

(27)

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

δ Z k OG = [ δ Δ θ θ k − δ Δ θ θ k ] + v k ,

(28)

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

δ Z k OG = H k OG δ X k OG + v k

(31)

where v_k is the measurement noise.

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

P k CaPS ≜ P k 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.

OPEN IN VIEWER

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.

OPEN IN VIEWER

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

[ X Cam Y Cam Z Cam 1 ] = C T R • [ X Ref Y Ref Z Ref 1 ] .

(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

S • [ u v 1 ] = I T C • [ X Cam Y Cam Z Cam 1 ]

(34)

where S is scaling factor.

The combination of these equations yields

S • [ u v 1 ] = { I T c } • { c T R } • [ X Cam Y Cam Z Cam 1 ]

(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

M = { I T c } • { c 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

S • [ u v 1 ] = M • [ X Ref Y Ref Z Ref 1 ] ,

(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).

Focal length of the width = Working distance * CCD width Object width + CCD width

(38)

Focal length of the height = Working distance * CCD height 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

Focal length of the width = 3000 * 6 . 4 3200 + 6 . 4 = 5 . 99 ( mm ) ,

(40)

Focal length of the height = 3000 * 4 . 8 2400 + 4 . 8 = 5 . 99 ( 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.

OPEN IN VIEWER

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

S • [ u v 1 ] = M • [ X Ref Y Ref 0 1 ] .

(42)

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

M = [ m 11 m 12 m 13 m 14 m 21 m 22 m 23 m 24 m 31 m 32 m 33 1 ] ,

(43)

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

M ′ = [ m 11 m 12 m 14 m 21 m 22 m 24 m 31 m 32 1 ] .

(44)

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

[ S • u S • v S ] = [ m 11 m 12 m 14 m 21 m 22 m 24 m 31 m 32 1 ] • [ X Ref Y Ref 1 ] .

(45)

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

A • M = B

(46)

where

A = [ X Ref Y Ref 1 0 0 0 − u X Ref − u Y Ref 0 0 0 X Ref Y Ref 1 − v X Ref − v Y Ref ] ,

(47)

B = [ u v ]

(48)

and

M = [ m 11 m 12 m 14 m 21 m 22 m 24 m 31 m 32 ] Τ .

(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

|| A • M − B || 2 is M = ( A T A ) -1 • ( A T B ) .

(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

A = [ x 1 y 1 1 0 0 0 − u 1 x 1 − u 1 y 1 0 0 0 x 1 y 1 1 − v 1 x 1 − v 1 y 1 x 2 y 2 1 0 0 0 − u 2 x 2 − u 2 y 2 0 0 0 x 2 y 2 1 − v 2 x 2 − v 2 y 2 x 3 y 3 1 0 0 0 − u 3 x 3 − u 3 y 3 0 0 0 x 3 y 3 1 − v 3 x 3 − v 3 y 3 x 4 y 4 1 0 0 0 − u 4 x 4 − u 4 y 4 0 0 0 x 4 y 4 1 − v 4 x 4 − v 4 y 4 ]

(51)

M = [ m 11 m 12 m 14 m 21 m 22 m 24 m 31 m 32 ] T

(52)

and

B = [ u 1 v 1 u 2 v 2 u 3 v 3 u 4 v 4 ] Τ .

(53)

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

OPEN IN VIEWER

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

[ m 11 X Ref + m 12 Y Ref + m 14 − u • m 31 X Ref − u • m 32 Y Ref = u m 21 X Ref + m 22 Y Ref + m 24 − v • m 31 X Ref − v • m 32 Y Ref = v ]

(54)

and then

[ m 11 − m 31 u m 12 − m 32 u m 21 − m 31 v m 22 − m 32 v ] [ X Ref Y Ref ] = [ u − m 14 v − m 24 ] .

(55)

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

[ X Ref = ( u − m 14 )(m 22 − m 32 u ) − ( v − m 24 )(m 12 − m 32 u ) (m 11 − m 31 u )(m 22 − m 32 v ) − (m 21 − m 31 v )(m 12 − m 32 u ) Y Ref = ( v − m 24 )(m 11 − m 31 u ) − ( u − m 14 )(m 21 − m 31 v ) (m 11 − m 31 u )(m 22 − m 32 v ) − (m 21 − m 31 v )(m 12 − m 32 u ) ]

(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

P k Ref = [ X k Ref Y k Ref ] = P k CaPS = [ X k CaPS Y k CaPS ] .

(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

P ^ k OG = [ X ^ k OG Y ^ k OG ] .

(58)

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

δ P k OG, CaPS = P ^ k OG − P k CaPS = [ X ^ k OG Y ^ k OG ] − [ X k CaPS Y k CaPS ] .

(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

δ θ k OG, C = θ ^ k OG − θ θ k

(60)

OPEN IN VIEWER

Figure 8.

The CMPS03 compass module

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

Z k = P ^ k OG − P k CaPS = [ X ^ k OG Y ^ k OG ] − [ X k CaPS Y k CaPS ] + v k

(61)

Then the measurement matrix of Equation (61) is

Z k OG, CaPS = [ 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] [ δ X k δ Y k δ θ k Ω R, k O Ω L, k O Δ B k δ θ k G Ω k G Ψ k G ] + v k = H k OG , CaPS δ X k OG + v k

(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

Z k = θ ^ k OG − θ θ k = [ θ ^ k OG − θ θ k ] + v k .

(63)

Then the measurement matrix of Equation (63) is

Z k OG, C = [ 0 0 1 0 0 0 0 0 0 ] [ δ X k δ Y k δ θ k Ω R, k O Ω L, k O Δ B k δ θ k G Ω k G Ψ k G ] + v k = H k OG,C δ X k OG + v k

(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.

OPEN IN VIEWER

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.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

OPEN IN VIEWER

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

OPEN IN VIEWER

Figure 13.

Orientation of the robot in four corners with counter clockwise running

OPEN IN VIEWER

Figure 14.

Different orientation between odometry system and gyroscope

6.2 The result of combination with the CaPS support

OPEN IN VIEWER

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.

OPEN IN VIEWER

Figure 16.

Systematic error of gyroscope

OPEN IN VIEWER

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).

OPEN IN VIEWER

Figure 17.

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

OPEN IN VIEWER

Figure 18.

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

OPEN IN VIEWER

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.

OPEN IN VIEWER

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.

OPEN IN VIEWER

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.

OPEN IN VIEWER

Figure 21.

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