MPE Mathematical Problems in Engineering 1563-5147 1024-123X Hindawi 10.1155/2017/6835456 6835456 Research Article A Vision/Inertia Integrated Positioning Method Using Position and Orientation Matching http://orcid.org/0000-0001-6627-2250 Zhang Xiaoyue 1 http://orcid.org/0000-0001-9937-2063 Huo Liang 1 Gu Jason School of Instrumentation Science and Optoelectronics Engineering Beihang University Beijing 100191 China buaa.edu.cn 2017 1452017 2017 31 12 2016 26 02 2017 09 04 2017 1452017 2017 Copyright © 2017 Xiaoyue Zhang and Liang Huo. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

A vision/inertia integrated positioning method using position and orientation matching which can be adopted on intelligent vehicle such as automated guided vehicle (AGV) and mobile robot is proposed in this work. The method is introduced firstly. Landmarks are placed into the navigation field and camera and inertial measurement unit (IMU) are installed on the vehicle. Vision processor calculates the azimuth and position information from the pictures which include artificial landmarks with the known direction and position. Inertial navigation system (INS) calculates the azimuth and position of vehicle in real time and the calculated pixel position of landmark can be computed from the INS output position. Then the needed mathematical models are established and integrated navigation is implemented by Kalman filter with the observation of azimuth and the calculated pixel position of landmark. Navigation errors and IMU errors are estimated and compensated in real time so that high precision navigation results can be got. Finally, simulation and test are performed, respectively. Both simulation and test results prove that this vision/inertia integrated positioning method using position and orientation matching has feasibility and it can achieve centimeter-level autonomic continuous navigation.

1. Introduction

Intelligent vehicle has walked into human’s life and has shown more and more extensive value in the military and civil aspects. Navigation is one of the key technologies of mobile intelligent vehicle and it has been widely researched in recent two decades . With the development of science technology, navigation requirement of intelligent vehicle is higher and higher and the trend is more accurate, more flexible, and of lower cost. Especially for automated guided vehicle (AGV) which is adopted widely in factory field, high precision positioning is the key to determine whether it can finish other tasks excellently [4, 5].

Here, vision positioning method based on artificial landmarks is integrated with inertial navigation technology to achieve navigation. This method can effectively realize their complementary advantages. It has the characteristic of high accuracy, low cost, small calculation, and high real-time performance and it can meet the requirements of many complex tasks. The paper introduces this vision/inertia integrated positioning method using position and orientation matching first. Then the needed mathematical models are established and measurement model builds up the connection between vision and inertia. Finally, mathematical simulation and experiment are performed to verify the effectiveness of the proposed method.

2. Vision/Inertia Integrated Method

Vision positioning technology based on artificial landmarks is suitable for structured environment. Before using this method, it is necessary to carry out surveying and mapping on the site and place the landmarks in it. A kind of artificial landmark is shown in Figure 1. The direction of feature points of the artificial landmarks shows the orientation information and the absolute position of the artificial landmark can be embedded into pattern of landmark by encoding. Vision processor can output three parts of information which are the pixel coordinates of landmark in the picture, absolute position of landmark, and orientation. Vision navigation based on artificial landmarks is integrated with MEMS INS to achieve high precision navigation and its working principle diagram is shown in Figure 2. MEMS INS and camera are firmly fixed on the vehicle. INS outputs velocity, position, and attitude of vehicle continuously and camera collects images in real time when the vehicle is moving. If the camera has captured the image of landmark whose direction and position are known, vision processor extracts the orientation and position information from the picture by image processing and landmark recognition. Matching the information with INS outputs can realize high precision navigation positioning of vehicle.

A kind of artificial landmark.

The working principle diagram of the integrated system.

Kalman filter is used to realize integration of INS outputs and landmark information. The schematic diagram of this integration method is shown in Figure 3. The error state model of Kalman filter is established according to MEMS INS error model and measurement model is established from measurement difference of the same parameter between INS and vision. Here, observation consists of two parts. One is the orientation difference between INS output and vision measurement. The other is the difference between the calculated pixel position of landmark and the pixel coordinates collected directly by camera. The calculated pixel position of landmark comes from the position vector in the pixel coordinate system which is projected from the position vector from INS output position to absolute position of landmark. Kalman filter estimates the value of system error states and modifies the navigation errors by feedback correction. And IMU errors can be calibrated and compensated online so that the system can output optimal estimation of navigation parameter.

The schematic diagram of integration method.

As is shown in Figure 3, in order to realize integration, the needed mathematic models including MEMS INS error model, vision navigation error model, state-space model of Kalman filter, measurement model, and correction model of navigation errors and IMU errors should be established.

3. Integrated Mathematical Models 3.1. MEMS INS Error Model

When inertial navigation is integrated with other navigation systems through Kalman filter, the state equations of integrated navigation system are commonly established from INS error equations. The dynamic INS error model has long been established and researched in literature such as those in [14, 15], which can be referenced as follows:(1)δv˙n=-ϕn×fn+Cbnfb+δvn×2ωien+ωenn+vn×2δωien+δωenn+Cbnb,ϕ˙=ϕ×ωien+ωenn+δωien+δωenn-Cbnεb,δL˙=-vNRM+h2δh+1RM+hδvN,δλ˙=vEtanLsecLRN+hδL-vEsecLRN+h2δh+secLRN+hδvE,δh˙=δvU,where δvn is the velocity error vector in the navigation coordinate frame; ωien is the earth rotation rate in the navigation coordinate frame; ωenn is the rotation rate from the navigation coordinate frame with respect to the earth coordinate frame; ϕ is the attitude error vector; fn is the accelerometer’s output specific force in the navigation frame; b is the accelerometer’s output specific force error in the body frame; Cbn is the attitude matrix from body coordinate frame to navigation coordinate frame; εb is the gyro measurement error in the body frame; δL is the latitude position error; δλ is the longitude position error; δh is the height error; and RM and RN represent the radii of curvature along lines of constant longitude and latitude, respectively.

MEMS sensor cannot measure the earth rotation rate because of its low precision. And with the limit of artificial landmarks placement, integrated navigation method of INS and vision positioning based on artificial landmarks is only suitable for structure environment which means that the moving area of vehicle is considerably limited. Therefore, the global coordinate frame is selected as the navigation coordinate frame and MEMS INS error model can be expressed as(2)ϕ˙=-Cbnεb,δv˙n=-ϕn×fn+Cbnb,δp˙n=δvn,where δpn=(x,y,z); it is the position of vehicle in the navigation coordinate frame which is the global coordinate frame.

The errors of inertial components commonly include installation error, scale factor error, and random error. For the sake of this discussion, random error is only considered. Considering the land vehicle application condition, the dynamic range of vehicle is relatively smaller and both the accelerometer and gyroscope errors are considered as the composition of bias (random constant) and white noise. Supposing three direction error models are the same, they can be expressed as (3)εb=εb+ωg,b=b+ωa,where εb is arbitrary constant, ωg is white noise, b is arbitrary constant, and ωa is white noise.

Gyroscopes are directly installed on the vehicle in the strapdown inertial navigation system so the error εb is measured by gyroscopes in the body coordinate frame. It should be transformed into navigation coordinate frame. Transformations of accelerometers are the same as gyroscopes, and they can be expressed as(4)εn=Cbnεb,n=Cbnb.

3.2. State-Space Model of Integrated System

Error state equation of integration system can be got from (2)–(4) and it can be expressed as(5)X˙t=FtXt+Gtwt.

In the equation, Xt is a 15-dimension error state vector which is composed of the navigation errors and inertial sensors errors(6)Xt=ϕxϕyϕzδvxδvyδvzδpxδpyδpzεxbεybεzbxbybzb.

And ϕx, ϕy, and ϕz stand for platform error angle; δvx, δvy, and δvz stand for velocity error; δpx, δpy, and δpz stand for position error; xb, yb, and zb stand for the bias errors of the accelerometers; εxb, εyb, and εzb stand for the bias errors of the gyroscopes; wt is the system process noise which is composed of the white noises of the inertial sensors; G(t) is the system noise driving matrix; F(t) is the state matrix and it can be written as(7)Ft=03×303×303×3-Cbn03×3fn×03×303×303×3Cbn03×3I3×303×303×303×303×303×303×303×303×303×303×303×303×303×3.

The outputs of vision navigation system based on artificial landmarks are composed of absolute position of artificial landmark in the navigation frame, the pixel position of landmark in the pixel coordinate frame, and orientation information. The absolute position of landmarks is embedded into the patterns of landmarks which can be extracted from the picture including landmarks collected by camera. Its precision is affected by map precision of place points and here the map errors are not considered temporarily. The pixel coordinate of the landmark plp and orientation information φ are directly measured by vision camera and their precision is affected by camera manufacturing precision, camera shaking in vehicle moving process, the light conditions, and so on. If the distance from camera to landmark is short, the relative errors are not obvious. So we can think that the pixel position and orientation information measured by vision are just affected by white noise and vision navigation model can be written as(8)p^VNSp=plp+ωPVNS,φVNS=φ+ωφVNS,where p^VNSp is the pixel position of artificial landmark collected directly by vision and φVNS is vision output orientation. ωPVNS and ωφVNS are white noise.

3.4. Measurement Model

Measurement model reflects the relationship between observation and state and it is the link between inertial navigation and visual navigation. As discussed previously, both INS and vision can measure the pixel position of landmarks and orientation. And their difference is the observation of integrated system. Therefore, the observation Z(t) has two parts; one is the pixel position difference of artificial landmark Z1 between INS measurement and vision camera collection and the other is orientation difference Z2 between INS output and vision output, which can be expressed as(9)Zt=Z1Z2=p^INSp-p^VNSpφINS-φVNS,where p^INSp stands for the calculated pixel position of artificial landmark from INS measurement and φINS is INS output orientation.

Measurement model of integrated system can be given by(10)Zt=HtXt+Vt.

From (10) we can see that measurement matrix H(t) has established the relationship of orientation and pixel position of landmarks between INS and vision. The measurement matrix H(t) is derived in the following.

( 1) Position Observation. The position of landmarks in the pixel coordinate system can be calculated from the vehicle position in the navigation coordinate frame which is given by INS and landmark position in the navigation coordinate frame which is extracted from the image by vision processor. The goal is to get position observation by comparing it with the pixel position of landmarks collected by camera. Figure 4 shows the relative coordinate systems definition. Assume that l is the landmark point; f is the focus of camera; H is the distance from camera to ground; pn is the vehicle position in the navigation coordinate frame which is given by INS; pln is the landmark position which is encoded in the landmark pattern. Landmark position in the camera coordinate system plc can be got by transforming the position vector pln-pn with strapdown matrix Cnb and installation matrix Cbc, pcb between IMU and camera. Then, the pixel position of artificial landmark plp can be given by projecting plc to pixel coordinate system.

Definition of coordinate systems.

The relationship between the pixel position of landmark and the vehicle position of INS output can be given by(11)plp=1zlcM0M1plc=1zlcM0M1CbcCnbpln-pn-pcb.

In the equation, M0=100010, M1=ax0u00ayv0001 is the camera internal parameter matrix, ax, ay are constant, and (u0,v0) is the camera main point coordinate in the pixel coordinate system. The height between camera and landmark is unchanged basically after the camera is installed on the vehicle vertically. So zlc can be replaced with camera installation height H.

Various errors will be included in the actual navigation calculation. Assume that p^n is vehicle position including error which is calculated by INS; p^ln is landmark position including map error; C^nb is the strapdown matrix including error; C^bc, p^cb are the camera installation matrixes including error; M^1 is the camera internal parameter matrix including error; p^lc is the landmark position in the camera coordinate system including error and their relationship can be given by(12)p^INSp=1HM0M^1p^lc=1HM0M^1C^bcC^nbp^ln-p^n-p^cb.

Assume that the camera has been installed and been calibrated; we only consider the position error and attitude error of INS outputs and ignore the installation error between camera coordinate system and body coordinate frame, map error of landmark placement, and camera internal parameter error. So (12) can be simplified as(13)p^INSp=1HM0M1p^lc=1HM0M1CbcC^nbpln-p^n-pcb.

In the equation(14)p^n=pn+δpn,C^nb=CnbI+ϕ×.

Substituting (14) into (13), unfolding (13), and ignoring high-level minim, then (13) can be rewritten as(15)p^INSp=1HM0M1CbcC^nbpln-p^n-pcb=1HM0M1CbcCnbI+ϕ×pln-pn+δpn-pcb=1HM0M1CbcCnbpln-pn+δpn+Cnbϕ×pln-pn+δpn-BpC1HM0M1CbcCnbpln-pn-pcb-Cnbδpn+Cnbϕ×pln-pnplp+1HM0M1CbcCnbϕ×pln-pn-Cnbδpn.

Therefore, the pixel position error of landmark δpINSp measured by MEMS INS can be expressed as(16)δpINSp=p^INSp-plp=1HM0M1CbcCnbϕ×pln-pn-Cnbδpn.

The position observation can be written as(17)Z1=p^INSp-p^VNSp=δpINSp-ωPVNS=1HM0M1CbcCnbϕ×pln-pn-Cnbδpn-ωPVNS.

( 2) Orientation Observation. Both the vision positioning method based on artificial landmarks and MEMS INS can measure the orientation. And the orientation difference between them is the second part of observation of integrated navigation system. The orientation of INS output is ratio of elements from attitude matrix Cnb so the error of attitude matrix will be brought to orientation error. The real orientation of vehicle is φ and the INS output orientation error is δφINS so the INS output orientation can be expressed as (18)φINS=φ+δφINS.

Attitude matrix Cnb can be written as(19)Cnb=C11C21C31C12C22C32C13C23C33.

The attitude matrix including error C^nb unfolds and it can be written as (20)C^11C^21C^31C^12C^22C^32C^13C^23C^33=C11C21C31C12C22C32C13C23C331-ϕzϕyϕz1-ϕx-ϕyϕx1.

The relationship between attitude angles and elements in attitude matrix can be given by (21)tanφINS=tanφ+δφINS=C^12C^22=C12+C22ϕz-C32ϕyC22-C12ϕz+C32ϕx.

After unfolding by Taylor series and ignoring high-level minim, the following equation can be got from (21) :(22)δφINS=-C12C32C122+C222ϕx-C22C32C122+C222ϕy+ϕz.

Therefore the orientation observation can be expressed as(23)Z2=φINS-φVNS=δφINS-ωφVNS=-C12C32C122+C222ϕx-C22C32C122+C222ϕy+ϕz-ωφVNS.

Finally, measurement matrix H(t) can be expressed as (24)Ht=HpHφ=Hp102×3Hp2Hφ101×301×303×6,where(25)Hp1=1HM0M1CbcCnbpn-pln×,Hp2=-1HM0M1CbcCnb,Hφ1=-C12C32C122+C222-C22C32C122+C2221.

3.5. Correction Model

The navigation errors and inertial components errors can be estimated in the MEMS INS/vision integrated process and they can be used to compensate the system online.

( 1) IMU Errors Correction. ε~xb, ε~yb, ε~zb and εxb, εyb, εzb are gyroscope outputs before and after correction, respectively. ~xb, ~yb, ~zb and xb, yb, zb are accelerometer output before and after correction, respectively. ε^xb, ε^yb, ε^zb and ^xb, ^yb, ^zb are error estimation of gyroscope output and error estimation of accelerometer output, respectively. After estimating errors of inertial components, the inertial components outputs can be corrected by (26)εxb=ε~xb-ε^xb,εyb=ε~yb-ε^yb,εzb=ε~zb-ε^zb,xb=~xb-^xb,yb=~yb-^yb,zb=~zb-^zb.

( 2) Navigation Errors Correction. The relationship between real attitude matrix Cbn and calculated attitude matrix C~bn can be expressed as (27)Cbn=I+ϕ^×C~bn.

After estimating errors of platform angle, the attitude matrix can be corrected by the above equation and then corresponding angles can be calculated according to Cbn.

The relationship among real velocity vn, output velocity v~n, and velocity error δv^n can be expressed as(28)vn=v~n-δv^n.

After estimating errors of velocity, the velocity output can be corrected by the above equation.

The relationship among real position pn, output position p~n, and position error δp^n can be expressed as(29)pn=p~n-δp^n.

After estimating errors of position, the position output can be corrected by the above equation.

4. Simulation

The mathematical simulation is conducted to verify the proposed vision/inertia integrated positioning method using position and orientation matching. According to typical maneuvers of AGV, the movement track of the vehicle in the simulation is set as follows:

Accelerate with an acceleration rate of 0.05 m/s2: 10 s.

Move with a stable velocity: 10 s.

Turn 90 degrees left with an angular rate of 9°/s: 10 s.

Move with a stable velocity: 30 s.

Turn 90 degrees left with an angular rate of 9°/s: 10 s.

Move with a stable velocity: 60 s.

Accelerate with an acceleration rate of −0.05 m/s2: 10 s.

In the whole movement, the calculation frequency of INS is set as 100 Hz and the update cycle of vision information of artificial landmark is set as 3 s. In the simulation, initial errors, the IMU errors, and vision senor errors are set as follows:

Initial position errors:

x-axis position error: 3 cm;

y-axis position error: 3 cm.

Initial attitude errors:

orientation error: 1.5°;

pitch error: 0.08°;

roll error: 0.08°.

Gyroscope:

bias: 35°/h;

noise: 30°/√h.

Accelerometer:

bias: 1 mg;

noise: 500 ug/√Hz.

Vision sensor:

pixel noise: 5 pixel/√Hz;

angle noise: 0.4°/√Hz.

The related results of simulations are illustrated in Figures 517. Figures 57 show the estimates of gyroscope bias in integrated navigation. Figures 8-9 show the estimates of the x-axis and y-axis accelerometer bias in integrated navigation. Figures 10-11 show the x-axis and y-axis velocity error in the integrated navigation. Figures 1214 show the orientation error and level attitude errors in integrated navigation. Figures 1517 show the x-axis position error, the y-axis position error, and level position error in the integrated navigation.

Estimate of x-axis gyroscope.

Estimate of y-axis gyroscope.

Estimate of z-axis gyroscope.

Estimate of x-axis accelerometer.

Estimate of y-axis accelerometer.

x -axis velocity error.

y -axis velocity error.

Orientation error.

Pitch error.

Roll error.

x -axis position error.

y -axis position error.

Level position error.

From the simulation results, we can find the following useful results. As shown in Figures 59, the IMU errors can be estimated in the process of navigation. The estimation value of x-axis and y-axis gyroscopes bias is about 34°/h. The estimation value of z-axis gyroscopes bias is about 38°/h. The estimation value of x-axis accelerometer bias is about 950 ug. The estimation value of y-axis accelerometer bias is about 850 ug. It can be seen from above that this is a “deep-integrated” method and can estimate the IMU errors effectively. As shown in Figures 10-11, velocity error will be restrained in about 0.02 m/s by using this integrated method. As shown in Figures 1214, this method can effectively estimate and compensate initial attitude errors. Orientation error will be restrained in 0.3 degrees and level attitude error will be restrained in 0.03 degrees. This method can get high precision velocity and attitude. As shown in Figures 1517, level position error will be restrained in 4 centimeters. The current cycle of vision information correction is 3 s and it will get higher navigation precision if the distance of landmarks placement is reduced which means the cycle of vision information correction is reduced. From the simulation results, this vision/inertia integrated positioning method using position and orientation matching can realize the “deep-integration” of MEMS INS and vision positioning method based on artificial landmarks. It is a fully autonomous navigation method and it can realize the centimeter-level navigation positioning.

5. Experiment

A vehicle test is conducted to demonstrate the validity of the new integrated navigation method proposed in this work. The MEMS IMU and camera are simultaneously installed on the test vehicle, just as shown in Figure 18. The MEMS IMU can output three axes’ angular rate and accelerator at the same time and its update frequency is set at 100 Hz. The gyroscope error is 40°/h and the accelerator error is 800 ug. The resolution of the camera used in test is 640480 and image update frequency is 30 frames per second. The installation direction is vertically downward and the installation height is 50 cm. When the camera has captured the landmark, vision processor can extract the orientation, the pixel position of landmark, and the absolute position of landmark which is encoding in the landmark pattern from the picture. The side length of square contour of landmark is 7 cm and placement distance is 1.2 m. The length of the vehicle is about 55 cm and the width of vehicle is about 20 cm. The speed of vehicle is controlled at about 1.5 m/s. 15 landmarks are placed on the navigation path in a straight line. In order to get navigation errors, 6 datum marks are set on the running path of test vehicle. Navigation errors can be got from the difference between navigation output and datum mark position when the vehicle passes the datum mark.

Installation of MEMS IMU and camera.

When the vehicle reaches a datum mark, the navigation errors are recorded. The correlative results are listed in Table 1.

Mark number x -axis error (cm) y -axis error (cm) Level error (cm)
1 −1.0 3.1 3.2
2 1.1 2.0 2.3
3 1.8 −1.3 2.2
4 −2.3 4.8 5.3
5 −0.9 4.0 4.1
6 −2.6 4.6 5.3

From the experiment results of the vehicle tests, we can find that navigation errors are restrained in 5.5 cm. It means that the proposed integrated navigation method in this work is validated and this vision/inertia integrated positioning method using position and orientation matching can achieve the centimeter-level navigation positioning.

6. Conclusion

A vision/inertia integrated positioning method using position and orientation matching which can be adopted on intelligent vehicle such as automated guided vehicle (AGV) and mobile robot is proposed in this work. Vision processor extracts the azimuth and position information from the pictures which include artificial landmarks. Inertial navigation system (INS) calculates the azimuth and position of vehicle in real time and the calculated pixel position of landmark can be computed from the INS position results. Integrated navigation is implemented by Kalman filter with the observation of orientation and the calculated pixel position of landmark. Navigation errors and inertial measurement unit (IMU) errors are estimated and compensated online. Both simulation and test results prove that this vision/inertia integrated positioning method using position and orientation matching has feasibility and it can achieve the “deep-integration” of MEMS INS and vision positioning method based on artificial landmarks. It is a fully autonomous navigation method and it can realize the centimeter-level navigation positioning.

Conflicts of Interest

The authors declare that there are no conflicts of interest regarding the publication of this paper.