Brief introduction

 

As the development that serves robot market and technology, navigation already became a heat in consider and applying. Compare with car, shipping or plane photograph, service robot volume is minor, cost is low, because their navigation system should have the characteristic of nimble couplet and low cost. The accelerometer that traditional stable platform navigation system wants to use independence normally and fiber-optic or laser gyroscope, all sensor are mechanical and on the stable platform that rigid installation is keeping apart with the car that moving. This brought about dimension the defect with big, dependability difference, high cost. Contrary, guide in nimble couplet be used to in the system, inertial sensor is secured directly go up in car noumenon, this means sensor to be able to rotate together with car. Method of couplet of this kind of nimble eliminated the defect that stable platform be used to guides. However, the accuracy that platform be used to guides normally prep above SINS. Platform be used to guides often can reach strategic level (the gyroscope when 0.0001°/ slants buy, the accelerator of 1μg slants buy) or class of for military use (the gyroscope when 0.005°/ slants buy, the accelerator of 30μg slants buy) , and most SINS can reach navigation level only (the gyroscope when 0.01°/ slants buy, the accelerator of 50μg slants buy) or tactical class (the gyroscope when 10°/ slants buy, the accelerator of 1mg slants buy) . Serve robot or AGV navigation application to great majority, this one precision is enough.

 

Navigation method is very much, include machine vision, GPS, UWB, SLAM lidar to wait. The inertial navigation that is based on IMU is the main component of navigation from beginning to end. However, because this is planted,the limitative —— of sensor slants for example the buy error, error between the axis, noise, especially 0 slant instability —— inertial navigation needs to use a companionate sensor normally, offer reference or calibration for it regularly, the article calls this kind of situation sensor confluence. A lot of sensor are OK as shirt-sleeve as IMU, photograph for example like head and odograph, but in these sensor, geomagnetic sensor is the plan of a kind of low cost, can cooperate to obtain attitude information with IMU.

 

In the article, we use the IMU ADIS16470 of ADI and geomagnetic sensor to develop platform and algorithm, implementation nimble couplet is inertial navigation system. But, geomagnetic sensor can provide attitude information only. To boat computative or the distance is measured, we can use the acceleration sensor in IMU only.

 

ADIS16470 IMU brief introduction

 

The ADIS16470 of ADI company is MEMS IMU of a miniature, compositive 3 axes gyroscope and 3 axes accelerometer. Its gyroscope 0 slant when stability is 8°/ , accelerometer 0 slant stability is parameter of 13μg its key to be passed leave factory calibration. In addition, the low price of ADIS16470 has appeal in the product that be the same as class, use extensively what got a lot of clients. In the article, we use small controller and ADIS16470 to have communication through SPI interface.

 

Geomagnetic sensor introduces

 

Geomagnetic sensor is to be used at measuring compass body coordinate (namely coordinate is fastened) the sensor of medium geomagnetic field, can provide absolutely reference for course. Its X, Y and Z heft value by this locality terrestrial magnetism field is umbriferous and come. This kind of sensor has —— of two main drawback is precision and resolution not tall —— for example, the resolution of HMC5883L of commonly used sensor of compass of Er of Huo Ni Wei is 12 only. Another defect is sensor gets easily the interference of surroundings, because geomagnetic field is very weak, intensity limits is fine long hair gauss arrives 8 gauss.

 

Although have these drawback, still can use below a lot of circumstances, for example environment of outdoors, low EMI. Will geomagnetic sensor and IMU undertake loose coupling, can use this kind of sensor in most environment.

 

In the article, we use the RM3100 of sensor of high-powered electron compass of PNI sensor company, it offerred 24 resolution. PNI uses active and incentive Falaidi to fight noise ability high.

 

The calibration of compass sensor

 

Before use compass sensor, need undertakes calibration to its in order to eliminate two main errors. One is maladjusted error, this is caused by the maladjusted error of sensor and circuit originally. Another is scale error. These two kinds of errors get easily all round the interference of magnetism environment. For example, if the exterior magnetic field of axial of a X brings to bear on to go up to sensor, can give out exterior X axis is maladjusted error. In the meantime, x axis scale also will differ with Y axis and Z axis.

 

The method that is used at calibration magnetism sensor normally is rotational on Xy plane sensor circles a circle, smoke access to occupy next. The geomagnetic field intensity of the three unities is an invariable value, because the data of this scale should be a circle; However, in fact, we will see elliptic, this means us to need mobile ellipse to shrink afresh put with the 0 circles that are a center.

 

Afore-mentioned 2D calibration methods have a few drawback, and need measure its gradient with accelerator. We use 3D spherical plan to add up to a method to come calibration compass sensor. Above all, we need to rotate sensor every direction in X-y-z space, and in the scale in 3D coordinate its are worth. Next we need to use least square error (MSE) method drafts data to close for ellipsoid face.

 

Ellipsoid equation can express to be

 

 

Among them, x, Y and Z are compass output the geomagnetic heft on 3 direction. Draft these values to close to be meant for ellipsoid face, we need to get a group of best coefficient are solved. We are coefficient definition:

 

 

When planning to close, we define vector:

 

 

So we need consideration best σ , use formula 2 will find out the least value:

 

 

Such we can get 1 is shown pursueing plan to combine a result.

 

Graph 1. Primitive compass data distributings (left) the compass data after planning to close with use ellipsoid (right) .

 

For calibration sensor, we need drawing or compress the ellipsoid face that plans to close to move its to with 0 those who be a center is spherical on. We use matrix bizarre value is decomposed (SVD) method will undertake this kind of calibration. The sphere after calibration is shown 2 times like the graph. 1, 2

 

Graph 2. Have the compass data after sphere calibration with SVD method.

 

After calibration, we can see, measures magnetic field intensity (ball radius) almost constant and changeless, if pursue,3 are shown.

 

Graph 3. With calibration the magnetic field after is compared before calibration.

 

The attitude of use ADIS16470 and compass and course reference system

 

AHRS is comprised by the sensor on 3 axes, provide attitude information, include roll horn, pitch angle and yaw angle. AHRS is a concept that comes from plane navigation. We describe way with it, namely attitude.

 

Before the method that introduces us, be necessary to explain above all why to decide attitude needs to undertake confluence. In fact, our system has 3 kinds of sensor now: Gyroscope, accelerator and compass (geomagnetic sensor) .

 

Gyroscope offers the rate of corner coming back around each axis. Pass horny rate integrating meter to calculate, we can get corner is spent coming back. If we know initiative course, pass angle to be able to get course attitude from beginning to end. Integral does not stabilize those who accumulate gyroscope 0 slant, this will cause angle error. In addition, the gauss that comes from gyroscope distributings noise becomes integral process of a Brownian movement, bring about wander randomly error. Accordingly, we use gyroscope for long very hard, gyroscope needs fixed calibration.

 

Accelerometer offers the mobile acceleration of every axis direction. Below static condition, we can get the point of view between every axis and gravitational acceleration. Go up in direction and value as a result of gravitational acceleration constant and changeless, we can be obtained relative to the course attitude at gravitational direction. However, this method uses gravitational acceleration to serve as reference, because this cannot dismiss a point of view that rotates around gravitational acceleration.

 

Compass offers the value from every axis with geomagnetic umbriferous field. The relation derivation between the geomagnetic field direction that we can be invariable vector from every axis and constant gives angle to be worth. If a place is narrated, because the sex fighting faze to exterior magnetic field is poorer, compass needs the environment of a low interference.

 

From inside this one explanation, we can see, rely on a sensor very hard to find attitude, our need combination uses two or 3 sensor rise information confluence. The article searchs attitude with accelerometer, gyroscope and geomagnetic compass. This kind of confluence also is called rate of magnetism, horn and gravity (MARG) system.

 

The design of filter of patulous Ka Erman and sensor are shirt-sleeve

 

It is OK to have a variety of methods rise IMU and compass data confluence, for example complementary filter, statistical ARMA filter, ka Erman filter. What we use in the article is filter of patulous Ka Erman.

 

Above all, we need to introduce a few definitions that use in the article.

 

Coordinate is defined

 

T course or direction are two coordinate (namely coordinate is fastened) the relation between. A coordinate always is changing, another coordinate keeps changeless. Define a method to coordinate, we use navigation coordinate and body coordinate. With northeast ground (NED) coordinate department or geographical method are contrary, we are worth metrical initiative body coordinate the definition to be navigation coordinate department, this coordinate is after this constant coordinate. Arrive from body coordinate the map of navigation coordinate (umbriferous) matrix definition is

 

 

Attitude definition

 

Pull horn or directional cosine matrix with Europe (DCM) is different, we use quaternion here, the definition is

 

 

Commonly used surprise in order to avoid at navigation the opposite sex.

 

With Kaerman filter updates attitude

 

The kinematic equation that we use in the article (namely condition transfers equation) equation of dispute linear differential, because this needs to use an EKF, use at differential to this equation to undertake first-order and approximate. To EKF design, we are defined

 

 

Vector of a 1×7 regards condition as variable, among them

 

 

For horny rate;

 

 

For attitude quaternion.

 

 

Vector of a 1×7 regards observation as variable, have identical weight with condition variable.

 

 

Matrix of a 7×7 transfers matrix as condition, among them, the first part of A is the digitlization differential equation of horny rate, the 2nd part is to digitlize quaternion to update equation, latter comes from kinematic equation derivation.

 

 

Matrix of a 7×7 regards observation as matrix.

 

 

For error covariance matrix, this is matrix of a 7×7, among them

 

 

Estimate vector X? We are checking the error between real value Xx initiative error sets the lieutenant general for relatively lesser value. Should be worth can automatic astringent to arrive a little value.

 

 

Be set to be condition to transfer the covariance matrix of noise and observation noise. We get their initiative value,

 

 

In maintain while IMU and compass are in static condition, through measuring the communicates mean effective value square of gyroscope and accelerator to get. We are set

 

 

According to above definition, filter will pass Ka Erman the following 5 measure finish:

 

Measure 1: Use formula K of gain of 3 computation Ka Erman

 

 

Measure 2: Matrix of computational error covariance, p:

 

 

Measure 3: Output estimation condition X? :

 

 

Measure 4: Update condition X? – :

 

 

Measure 5: Update P– of error covariance matrix:

 

 

This procedure is OK and simple ground described as pursues 4 medium block diagram.

 

Graph 4. Use at updating the flow chart of Ka Erman filter of attitude.

 

The sensor that is based on MSE is shirt-sleeve

 

In getting on, observation variable is

 

 

Did not come from the information of compass among them. Because ω is horny rate, we can use quaternion to guide only compass data Q. We use MSE method to obtain Q, namely the constituent in observation variable.

 

We define each variable as follows:

 

M_BWith A_B: Body coordinate ties the compass magnetism value in and acceleration cost.

 

M_NWith A_N: Navigation coordinate ties the compass magnetism value in and acceleration cost.

 

M_N0With A_N0: Navigation coordinate ties the value of magnetism of compass of initiative static state in and acceleration cost.

 

 

The attitude that fastens to fasten navigation coordinate from body coordinate changes matrix, express with quaternion, can write into

 

 

Its gave out the error ε between initiative value and the tie map to navigation coordinate from body coordinate in real time cost in navigation coordinate department.

 

According to the definition before, MSE method can be used at begging to take best cost.

 

 

Through begging equation the least value of 8:

 

 

Beg to F(q) guide and make its are equal to 0,

 

 

We will obtain the best Q on variance meaning. We use gauss – Newtonian method, will seek solution with first-order gradient astringent above is nonlinear equation.

 

Through setting horny rate, we will get observation variable

 

 

Among them shirt-sleeve the compass data in Ka Erman filter and IMU data.

 

This procedure is OK and simple ground described as pursues 5 medium block diagram.

 

Graph 5. The sensor confluence block diagram of use MSE method.

 

Loose coupling

 

The place before be like is narrated, we often encounter the case that cannot use compass sensor. If magnetism data is disturbed, when precision will compare the attitude that seeks solution to use IMU only poorer. Accordingly, we use loose coupling to judge magnetism sensor to whether can be used. When magnetism sensor is not usable, we beg solution attitude only with IMU; When magnetism sensor is usable, we will use shirt-sleeve algorithm to find attitude, if pursue,6 are shown.

 

Graph 6. Attitude computation flow chart.

 

Perhaps be in after obtaining new data when begging the attitude with new solution (in certain system, sampling cycle and attitude solution consider cycle different, but what our herein undertakes is solution of odd sampling cycle calculates) , the size of our computation acceleration, if the result is not equal to 1g, we undertake with respect to the output that won’t use accelerator attitude is calculated. Next the size that our computation compass outputs has its and initiative value comparative. If they each other are unequal, we won’t use the data of geomagnetic sensor in this cycle. When satisfying two requirements, we can use Ka Erman filter and carry out MSE confluence.

 

Use ADIS16470 has boat computative (DR)

 

In navigation, boat computative it is computation the process of current position, use the position that determines before first, consider cycle medium to be based on in solution next foregone or the speed of estimation or acceleration are newer this position. Here will use the accelerometer in ADIS16470. Be based on the attitude that a solution gives, we can get nimble contacts the mobile direction of all, need to calculate this to be apart from to what go up next, determine the position finally.

 

Nimble couplet boat computative need uses those who be based on acceleration to measure to dog than power equation the position of INS. The equation that compare force is OK and simple described as equality 10, equality 11 with equality 12:

 

 

Among them A_EIt is the acceleration in earthly coordinate department, A_B It is the acceleration in body coordinate department, V_EIt is the speed in earthly coordinate department, S_EIt is the distance in earthly coordinate department, G_E It is the gravity in earthly coordinate department is quickly [0 0 1] , the unit is G. Those who need ambitious shift is, earthly coordinate department and navigation coordinate fasten coordinate of different —— earth is based on NED. This δtt is solution calculates cycle.

 

The acceleration map that can get with the first equality coordinate is fastened fastening the earth from IMU body coordinate, be like a format

 

 

Integral or acceleration of the 2nd equality general is cumulative for speed; However, included gravitational weight as a result of metrical acceleration, need subtractive gravity so.

 

With equality 11 similar, equality 12 become speed integral the distance.

 

Traditional method is put in a few problems.

 

● accelerometer output always has slant buy, after be united in wedlock with gravitational photograph, hard from formula 10 in subtractive, because this more accurate expression should be:

 

 

Unless be to use a few professional equipment to measure this,slant buy, for example dividing head.

 

● is based on the implementation means of numeric integral, use normally 0 rank maintain implement method (a value) undertake integral. But, to successive shift, this will bring gross error. For example, we will compare the following method:

 

Method 1:

 

 

(0 rank maintain implement)

 

Method 2:

 

 

(linear interpolation)

 

Inside 5 seconds acceleration is for 0.5 M/s²When, displacement is highest will differ 4m. Emulation result is shown 7 times like the graph.

 

Graph 7. In speed computation 0 rank maintain with first-order integral the method is compared.

 

Be based on the discussion in front, be based on application, we revised two places in traditional scale equation:

 

We do not use X earthly coordinate is fastened as navigation coordinate. Contrary, we are calculating no less than what do when previous stance in that way, we use initiative attitude

 

 

Regarding navigation as coordinate is. Carry this kind of kind, slant buy and gravity can cancel easily, if formula shows 14 times:

 

 

Although was included at the same time in attitude,slant buy and gravitational heft, but such we do not need to become them alone heft apart, subtract together directly however they.

Be based on 0 rank maintain implement with the comparison between first-order interpolation, we use first-order method to achieve more accurate integral result.

Kinematic mode and 0 fast replace a technique (ZUPT)

 

The initiative cost that passes use IMU regards navigation coordinate as the department, we can eliminate one part acceleration slant buy influence. However, although we can use dividing head before use equipment accurate measure slant buy, cancel very hard still, unless use another accurate sensor,come fixed calibration it. This basically causes by two reasons: It is to slant buy is not stable, this means what measure before us to slant buy is not now is actual slant buy. 2 it is speed wanders randomly, come by acceleration noise integral. The undesirable character that mentions in front can make our calculative is apart from remarkable drift. Although we stop to move and keep dormant, the speed that comes from acceleration integral still exists, the distance still can increase.

 

Want to solve this problem, we need to find a kind of method that carries rate of replacement of use ZUPT technology. Closely related ZUPT technology and specific application, accordingly we need to achieve the kinematic trait of system and application, give out next a few algorithm are regular. The kinematic mode that we discover is more, the result is more accurate.

 

We contain the revolving chair of SINS system to have a test through shift. The research as a result of us not be confined to is specific application, we use the following kinematic hypothesis:

 

● to boat computative, without Z the axis moves in navigation coordinate department. This limitation applies to boat only computative, do not apply to attitude to seek solution. Apparent, we are mobile in 2 dimension space systems. This conduces to eliminate Z axis error.

 

● is all turn to happen after stop. If happen when shift,turn, outside because introduce the specified number,be being met, be quickened and disturb attitude to seek solution.

 

● if the system is moving, acceleration is irretentive changeless more than 500 millisecond. Speed is irretentive changeless more than 2 seconds. Be being driven as a result of us or pull use revolving chair, because this very hard hand is moved,make force keeps accurately changeless more than 500 millisecond, and individual very hard divide evenly fast drive revolving chair continuously 2 seconds above. In fact, we apply this one regulation to carry out ZUPT just about.

 

● acceleration cannot be more than ±1 M/s2. This regulation is used at a few noise to filter, latter is based on us to bring to bear on to go up at the chair, very won’t large pulling force or thrust.

 

If the graph is shown 8 times, be in when the system on X direction when shift (umbriferous after be being fastened to navigation coordinate) , y direction also can produce acceleration; After integral, y direction speed won’t is 0, although we are mobile on X direction only,this is meant, boat computative system still can fetch Y weight to us.

 

Graph 8. The acceleration of 3 direction in navigation coordinate department.

 

Be based on the 3rd kinematic hypothesis, we can use ZUPT to eliminate this error. If the integral speed after classics ZUPT is handled pursues 9 are shown.

 

Graph 9. The speed of 3 direction in navigation coordinate department.

 

Although we used the 3rd hypothesis, the place before be like is shown, the error still cannot cancel completely. The error is eliminated depend on the 0 acceleration of set and the judgement threshold value of 0 speed. But, most error already got correction.

 

Although used ZUPT, but still cannot achieve sometimes 0 fast. This brings about by two elements:

 

● we cannot be eliminated completely with ZUPT slant buy does not stabilize speed of sum of errors to wander randomly.

We beg ● the attitude that give has a few errors, the result will be brought about umbriferous (fastening navigation coordinate from body coordinate is) the acceleration error after.

 

To in an attempt to 10 for exemple. Graph the primitive data that 10 medium different pictures are ADIS16470 (body coordinate is fastened) , graph 10 medium right graphs are umbriferous the acceleration that fastens to navigation coordinate. Can see, when stopping shift, umbriferous acceleration is 0. Because it always is changing, we say here for base line drift.

 

Graph 10. Body coordinate is (left) fasten with navigation coordinate (right) acceleration.

 

To eliminate base line drift, we need to obtain deflection to slant continuously in real time buy subtracts from inside umbriferous acceleration this value. The result is shown 11 times like the graph

 

Figure pursues 11. Before base line drift is eliminated (go up) after mixing (below) acceleration.

 

The graph is the acceleration before base line drift is eliminated on, the green contrail in pursueing below is deflection of our calculative base line, gules contrail is the acceleration after base line deflection is eliminated.

 

Can use a figure 12 medium block diagram are brief descriptive boat computative process. We fasten body coordinate acceleration Ab and attitude move matrix (come from AHRS) input

 

 

To DR system. After finishing, we will obtain navigation coordinate to fasten medium seat.

 

Graph 12. Boat computative flow chart

 

Experimental result and conclusion

 

Experimental result

 

Use SPI port, we evaluate ADIS16470 board evaluate with RM3100 compass board the ADuCM4050 circuit that receives ADI company repeatedly board, compose builds the system that gives us, if pursue,13 are shown. ADuCM4050 attune integer keeps abreast of according to the format a time is synchronous (because IMU is mixed,the data rate of compass differs) . Use UART to transmit captured data the computer next. All computation (include calibration, AHRS and DR to be in MATLAB) all carry out in MATLAB® .

 

Graph 13. Experimental platform is installed.

 

Will evaluate board put on revolving chair with the computer, pushing revolving chair to circle a circle in the lab.

 

● AHRS outputs: Attitude expresses with quaternion format and DCM format, if pursue,14 are shown.

 

Graph 14. Quaternion format (left) with DCM format (right) attitude.

 

● DR outputs: The boat that takes XYZ place computative result and three-dimensional graph are shown 15 times like the graph.

 

Graph 15. Positional computation result.

 

Conclusion

 

Compose builds the IMU ADIS16470 that the article introduced to use ADI firm and geomagnetic sensor RM3100 nimble couplet be used to guides basic course of the system, introduced the calibration that we use, AHRS and DR method. In the condition such as platform and experimental environment finite circumstance falls, check platform and algorithm very hard further.

 

A lot of methods can be used at improving a result, for example:

 

The accelerometer in ● use odograph or UWB distance measurement technique and IMU is shirt-sleeve, obtain exacter distance value in order to be in DR.

 

● uses more complex kinematic model, mix in sensor in AHRS and DR thereby systematic administrative levels introduces more character, for example systematic vibration, wait with flatness of decelerate model, ground quickly. This is meant to raise the accuracy of navigation result, need gives out more attrib border conditions.

 

● X uses more accurate numeric calculation method, use Xin Pusen regulation for instance or 3 batten interpolation undertakes integral in DR, perhaps use gauss of Newtonian method and rather than – Newtonian method begs equation of MSE of Jie Fei linear to wait.

 

Finally but also be the most important a bit, we discover in the experiment INS and application or kinematic model are close together and relevant. For example, we had a test in two places: The lab that did not spread carpet and shop have the office of carpet. If we use same parameter part, DR result can show vast difference. Accordingly, no matter which kinds apply, for example the patient dogs, AGV navigation or jockey fixed position, perhaps use medium different condition to be the same as all, we need to understand its in the round kinematic model.

 

Referenced circuit

 

1Long Li and Zhang He, “Automatic And Adaptive Calibration Method Of Tri-axial Magnetometer” (of 3 axes magnetometer mix automatically get used to calibration method oneself) , ” journal of Chinese instrument appearance ” , 2013.

 

2Timothy Sauer, numerical Analysis (numeric analysis (the 2nd edition) , pearson, 2011.

 

3David H. Titterton, strapdown Inertial Navigation Technology (nimble couplet is inertial navigation technology (the 2nd edition) , electric engineer learns, 2004.

 

Gongmin, yan, “Research On Vehicle Autonomous Positioning And Orientation System” (car locates directional system studies independently) , doctoral paper, university of Chinese northwest industry, 2006.

 

Marins, joão Luís, “An Extended Kalman Filter For Quaternion-Based Orientation Estimation Using MARG Sensors” (the filter of patulous Ka Erman that faces the application of quaternion direction estimation that is based on MARG sensor) , IEEE, 2001.

 

Prikhodko, Igor P. With Brock Bearss, “Towards Self-Navigating Cars Using MEMS IMU: Challenges And Opportunities” (use MEMS IMU marchs toward drive automatically car: Challenge and good luck) , IEEE, 2018.

 

RM3100.PNI sensor company, 2018.

 

Woodman, oliver J. “An Introduction To Inertial Navigation. ”(inertial navigation brief introduction) , cantabrigian university, in August 2007.