CN104697526A  Strapdown inertial navitation system and control method for agricultural machines  Google Patents
Strapdown inertial navitation system and control method for agricultural machines Download PDFInfo
 Publication number
 CN104697526A CN104697526A CN201510134478.8A CN201510134478A CN104697526A CN 104697526 A CN104697526 A CN 104697526A CN 201510134478 A CN201510134478 A CN 201510134478A CN 104697526 A CN104697526 A CN 104697526A
 Authority
 CN
 China
 Prior art keywords
 amp
 described
 omega
 sin
 cos
 Prior art date
Links
 230000001133 acceleration Effects 0.000 claims abstract description 73
 230000001131 transforming Effects 0.000 claims description 31
 230000005540 biological transmission Effects 0.000 claims description 12
 238000001914 filtration Methods 0.000 claims description 12
 238000000034 methods Methods 0.000 claims description 12
 238000010606 normalization Methods 0.000 claims description 9
 238000005070 sampling Methods 0.000 claims description 7
 230000000875 corresponding Effects 0.000 claims description 4
 239000000284 extracts Substances 0.000 claims description 3
 230000000694 effects Effects 0.000 description 5
 238000004458 analytical methods Methods 0.000 description 1
 239000000203 mixtures Substances 0.000 description 1
Classifications

 G—PHYSICS
 G01—MEASURING; TESTING
 G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
 G01C21/00—Navigation; Navigational instruments not provided for in preceding groups G01C1/00G01C19/00
 G01C21/10—Navigation; Navigational instruments not provided for in preceding groups G01C1/00G01C19/00 by using measurements of speed or acceleration
 G01C21/12—Navigation; Navigational instruments not provided for in preceding groups G01C1/00G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
 G01C21/16—Navigation; Navigational instruments not provided for in preceding groups G01C1/00G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
Abstract
Description
Technical field
The present invention relates to Mechanical course field, particularly relate to highaccuracy mechanical and control, specifically refer to a kind of strapdown inertial navitation system (SINS) for agricultural machinery and control method.
Background technology
Along with the development of MEMS (MicroElectroMechanicalSystem) sensor, navigation and control technology and country are to the further increasing of agriculture support dynamics, precision agriculture becomes a kind of trend fast, and assist in Driving control process at agricultural machinery, the attitude (comprising the angle of pitch, roll angle and course angle) of car body, speed and positional information can reflect motion and the positional information of car body in real time, and these information can provide important data input for highprecision integrated navigation and control algolithm.
Assist in Driving control process at agricultural machinery, the positional information of car body in navigational coordinate system and course information are most important two parameters.Locator meams conventional in agricultural machinery DAS (Driver Assistant System) has: Mechanical Touch, piloting, machine vision, laser positioning and multisensor information fusion (IMU+GPS).Multisensor information fusion makes full use of multiple sensor resource, by to the reasonable domination of these sensors and observation information thereof and utilization, multiple sensor is combined according to certain criterion in space or temporal redundancy or complementary information, explain with the consistance obtaining measurand or describe, this kind of method is several relative to other has the cost performance that positioning precision is high, volume is little and higher.
The highprecision GPS locating information (as RTK) of the acquisition Main Basis of positional information in multisensor information fusion, but gps data output accuracy and output frequency are directly proportional to price, and have barrier to block or the reason such as weather time, GPS can not ensure that effective data export, and now needs the data acquisition of a set of highfrequency highprecision and computing system to provide data filling in real time for this blind area.
At present, inertial navigation system is divided into PINS (Platform Inertial Navigation System) and SINS (StrapdownInertial Navigation System), SINS to be adopt IMU (Inertial Measuring Unit) sensor to set up one " mathematical platform " by calculating to replace PINS compared to PINS.SINS is used in aircraft navigation control system more, investigation and application for agricultural machinery control field then belongs to the starting stage, and the application of the two and environmental baseline have larger difference, the method that in flight control system, inertial navigation realizes is applicable not to the utmost in agricultural machinery controls.
Summary of the invention
The object of the invention is the shortcoming overcoming abovementioned prior art, provide a kind of strapdown inertial navitation system (SINS) for agricultural machinery that can realize and control method.
To achieve these goals, the strapdown inertial navitation system (SINS) for agricultural machinery of the present invention and control method have following formation:
This is used for the strapdown inertial navitation system (SINS) of agricultural machinery, and its principal feature is, described system comprises six axle inertial sensors, central controller; Six described axle inertial sensors comprise the acceleration transducer in three directions and the gyro sensor of three axles; Described central controller comprises:
Coordinate transformation module, the acceleration in order to the carrier coordinate system by the agricultural machinery described in the transmission of described acceleration transducer is converted to the acceleration of space flight coordinate system;
Velocity location computing module, the velocity information of the agricultural machinery described in the acceleration calculation of space flight coordinate system in order to export according to described coordinate transformation module obtains and positional information;
Attitude matrix computing module, the position angle velocity amplitude that magnitude of angular velocity and described velocity location computing module in order to the agricultural machinery according to the transmission of described gyro sensor calculate calculates and obtains the attitude matrix after upgrading; And
Attitude Calculation module, in order to the attitude angle of the agricultural machinery according to the attitude matrix acquisition after the renewal of described attitude matrix computing module.
The invention still further relates to a kind of control method realizing agricultural machinery, its principal feature is, described method comprises the following steps:
(1) acceleration of described agricultural machinery is sent to described coordinate transformation module by acceleration transducer described in real time, and simultaneously described in gyro sensor in real time the angular velocity of described agricultural machinery is sent to described attitude matrix computing module;
(2) the position angle velocity amplitude that the attitude matrix computing module described in calculates according to described velocity location computing module and the magnitude of angular velocity of gyro sensor received calculate and obtain the attitude matrix after upgrading, and the attitude matrix after upgrading is sent to coordinate transformation module;
(3) acceleration of the carrier coordinate system of the agricultural machinery described in the transmission of described acceleration transducer is converted to the acceleration of space flight coordinate system by the coordinate transformation module described according to the attitude matrix after renewal;
(4) positional information of the agricultural machinery described in acceleration output of the space flight coordinate system that the velocity location computing module described in exports according to described coordinate transformation module and velocity information, and the attitude angle of the agricultural machinery of described Attitude Calculation module according to the attitude matrix output after the renewal of described attitude matrix computing module;
(5) agricultural machinery of control center according to the control of described positional information, velocity information and attitude angle of the agricultural machinery described in.
Further, navigation coordinate is " sky, northeast " geographic coordinate system, the position angle velocity amplitude that described attitude matrix computing module calculates according to described velocity location computing module and the magnitude of angular velocity of gyro sensor received calculate and obtain the attitude matrix after upgrading, and specifically comprise the following steps:
(2.1) the position angle velocity amplitude that the attitude matrix computing module described in calculates according to described velocity location computing module and the magnitude of angular velocity of gyro sensor received calculate attitude speed;
(2.2) the attitude matrix computing module described in solves quaternion differential equation to obtain the first attitude matrix according to described attitude speed;
(2.3) the attitude matrix computing module described in will obtain the attitude matrix after upgrading after the first described attitude matrix normalization.
Further, described step (2.1) is specially:
Described attitude matrix computing module obtains attitude speed by following formula:
Wherein, the attitude matrix that Eulerian angle represent, namely
for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and
Further, described step (2.2) specifically comprises the following steps:
Described attitude matrix computing module obtains hypercomplex number according to described attitude speed and equivalent rotating vector algorithm and upgrades the differential equation to obtain the first attitude matrix, and described quaternion differential equation is:
Described hypercomplex number upgrades the differential equation:
Wherein,
for within an attitude cycle, the angular velocity that described gyro sensor exports, and
t is time scale, wherein, and angle increment
according to there being equivalent rotating vector two increment algorithm,
Again further, described attitude matrix computing module will obtain the attitude matrix after upgrading after the first described attitude matrix normalization, be specially:
Described attitude matrix computing module will obtain the attitude matrix after upgrading according to following formula after the first described attitude matrix normalization:
Attitude matrix after described renewal is:
Wherein, Q is quaternionic vector, q _{0}, q _{1}, q _{2}, q _{3}for forming the scalar of quaternionic vector, i, j, k are threedimensional system of coordinate vector of unit length, for carrier coordinate system is to the rotation matrix of navigational coordinate system.
Further, navigation coordinate is " sky, northeast " geographic coordinate system, the acceleration of the carrier coordinate system of the agricultural machinery described in the transmission of described acceleration transducer is converted to the acceleration of space flight coordinate system by described coordinate transformation module according to the attitude matrix after renewal, be specially:
The acceleration of carrier coordinate system of the agricultural machinery described in described acceleration transducer to send according to the attitude matrix after upgrading and by following formula by described coordinate transformation module is converted to the acceleration of space flight coordinate system
Wherein,
Further, navigation coordinate is " sky, northeast " geographic coordinate system, and the positional information of the agricultural machinery described in described velocity location computing module exports and velocity information, specifically comprise the following steps:
(4.1) integrated acceleration that described coordinate transformation module exports by the velocity location computing module described in is to obtain the velocity information of described agricultural machinery;
(4.2) the velocity location computing module described in will obtain the positional information of described agricultural machinery after rate integrating.
Again further, described step (4.1) is specially:
The integrated acceleration that described coordinate transformation module exports according to following formula by described velocity location computing module is to obtain the velocity information of described agricultural machinery:
Wherein,
Wherein,
for earth rate, be known, for
Again further, described step (4.2) is specially:
Described velocity location computing module will obtain the positional information of described agricultural machinery by following formula after rate integrating:
Wherein, L, λ, h are respectively the latitude of surface car, longitude, highly, v _{e}, v _{n}, v _{u}for the speed in the east of space flight coordinate system, north, sky, provided by velocity location computing module, k is sampled point, and T is the sampling period.
Further, navigation coordinate is " sky, northeast " geographic coordinate system, and the attitude angle of the agricultural machinery described in described Attitude Calculation module exports according to the attitude matrix after the renewal of described attitude matrix computing module, specifically comprises the following steps:
Described Attitude Calculation module extracts the attitude angle of the agricultural machinery comprised described in the angle of pitch, roll angle and course angle from the attitude matrix after described renewal.
Further, navigation coordinate is " sky, northeast " geographic coordinate system, further comprising the steps of between described step (4) and step (5):
(4.3) central controller described in judges to calculate system angle error according to following formula (10), velocity error is calculated according to following formula (11), calculate site error according to following formula (12), calculate inertia type instrument error according to following formula (13):
ε＝ε _{b}+ε _{r}+ω _{g}(13)
▽＝▽ _{b}+▽ _{a}+ω _{a}
Wherein, φ
_{e}, φ
_{n}, φ
_{u}for three attitude angle in sky, northeast navigational coordinate system, v
_{e}, v
_{n}, v
_{u}for the speed in sky, northeast, L, λ, h are respectively the latitude of surface car, longitude, highly, f
_{e}, f
_{n}, f
_{u}be respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, R
_{m}, R
_{n}for earth meridian ellipse and prime plane radiusofcurvature,
(X is φ, v, L, λ, h) be the derivative of its correspondence,
(Y is v, L, λ, h) be the error of its corresponding derivative, ε is gyroscope total error, ε
_{b}, ε
_{r}, ε
_{g}be respectively constant value drift, single order Markov process and white Gaussian noise, Δ is accelerometer total error, Δ
_{b}, Δ
_{a}, ω
_{a}be respectively constant value drift, single order Markov process and white Gaussian noise; ω
_{ie}for earth rate, be known, δ v
_{e}, δ v
_{n}, δ v
_{u}for sky, northeast velocity error under navigational coordinate system, δ L, δ λ, δ h are site error,
(4.4) central controller described according to abovementioned calculated system angle error, velocity error, site error and inertia type instrument error, and carries out error compensation according to Kalman filtering algorithm to described positional information, velocity information and attitude angle.
Again further, described carries out error compensation according to Kalman filtering algorithm to described positional information, velocity information and attitude angle, is specially:
It is described that to compensate to obtain 15 dimension state equations according to Kalman filtering algorithm to described positional information, velocity information and attitude angle as follows:
Wherein,
X (t)=[φ _{e}φ _{n}φ _{u}δ v _{e}δ v _{n}δ v _{u}δ L δ λ δ h ε _{bx}ε _{by}ε _{bz}▽ _{bx}▽ _{by}▽ _{bz}] ^{t}for the state vector of system, wherein, subscript E, N, U represent three directions of sky, northeast geographic coordinate system respectively, φ _{e}, φ _{n}, φ _{u}for the error angle of strapdown inertial navitation system (SINS), δ v _{e}, δ v _{n}, δ v _{u}for velocity error, δ L, δ λ, δ h are site error, ε _{bx}, ε _{by}, ε _{bz}gyrostatic random drift, ▽ _{bx}, ▽ _{by}, ▽ _{bz}the error of zero of accelerometer; W (t)=[ω _{gx}ω _{gy}ω _{gz}ω _{ax}ω _{ay}ω _{az}] ^{t}for systematic procedure white noise vector, wherein, ω _{gx}, ω _{gy}, ω _{gz}for the white noise of gyro, ω _{ax}, ω _{ay}, ω _{az}for the white noise of accelerometer; F (t) is system state matrix, and G (t) is system noise propogator matrix.
Have employed the strapdown inertial navitation system (SINS) for agricultural machinery in this invention and control method, compared with prior art, there is following beneficial effect:
(1) error compensation that adopts of the present invention and correction algorithm, substantially reduce the interference such as the Algorithm Error of strapdown inertial navitation system (SINS) and earth rotation;
(2) the six axle inertial sensors adopted and the algorithm of strapdown inertial navitation system (SINS) make the present invention have higher performance parameter for the strapdown inertial navitation system (SINS) of agricultural machinery, export course angle error through tractor test cabinet this device outer and be less than 0.1 °, the angle of pitch and rolling angular error are less than 0.01 °, the integrated navigation site error that the positional information that strapdown inertial navitation system (SINS) for agricultural machinery of the present invention exports coordinates GPS to realize is in cm level, data output frequencies reaches 50HZ, meets the requirement that Ride Control System assisted by agricultural machinery;
(3) present invention employs six axle inertial sensors, it comprises the acceleration transducer in three directions and the gyro sensor of three axles, and volume is little, lightweight, cost performance is high, and modular design is convenient to be integrated into agricultural machinery and is assisted among Ride Control System;
(4) the present invention has stable for the strapdown inertial navitation system (SINS) of agricultural machinery and exports the advantages such as movable information enriches, and especially meets the surface cars such as agricultural machinery and assists Ride Control System requirement.
Accompanying drawing explanation
Fig. 1 is the structural representation of the strapdown inertial navitation system (SINS) for agricultural machinery of the present invention.
Fig. 2 is the flow chart of steps of the control method for agricultural machinery based on strapdown inertial navitation system (SINS) of the present invention.
Embodiment
In order to more clearly describe technology contents of the present invention, conduct further description below in conjunction with specific embodiment.
The present invention is used for the strapdown inertial navitation system (SINS) of agricultural machinery and six axle inertial sensors of control method employing degree of precision, comprise the acceleration transducer in three directions and the gyro sensor of three axles, the acceleration of the motion of sensor Realtime Obtaining object and angular velocity, by can calculate the integration of acceleration speed and again integration can calculate positional information, by the car body current pose angle (angle of pitch can be calculated to the integration of angular velocity, roll angle and course angle), then attitude is converted into attitude matrix, thus realize the conversion of carrier coordinate system and navigational coordinate system, this attitude matrix plays a part to be " mathematical platform ".
In SINS (Strapdown Inertial Navigation System) algorithm realization, attitude matrix is particularly important, be engraved in motion due to during agricultural machinery, its attitude is also ceaselessly changing, and namely attitude matrix also will ceaselessly carry out recalculating and upgrading.Conventional attitude updating algorithm has Eulerian angle, direction cosine and hypercomplex number, and hypercomplex number does not have singular point compared with Euler algorithm, and little being highly suitable in embedded product of calculated amount uses compared with direction cosine.The present invention is when calculating attitude matrix, adopt angle increment quaternion attitude updating algorithm, but this algorithm also existing defects, namely when utilizing angle increment to solve the differential equation due to not fixedaxis rotation can produce rotation can not exchange error, can revise this error so the present invention adopts equivalent rotating vector method to solve the differential equation.
For the interference of earth rotation, coning motion effect, sculling effect and scrollwork effect equal error, multiple compensation and correction algorithm in the present invention, is adopted to process these errors.
Refer to shown in Fig. 1, be the structural representation of the strapdown inertial navitation system (SINS) for agricultural machinery of the present invention, wherein said system comprises six axle inertial sensors, central controller; Six described axle inertial sensors comprise the acceleration transducer in three directions and the gyro sensor of three axles; Described central controller comprises:
Coordinate transformation module, the acceleration in order to the carrier coordinate system by the agricultural machinery described in the transmission of described acceleration transducer is converted to the acceleration of space flight coordinate system;
Velocity location computing module, the velocity information of the agricultural machinery described in the acceleration calculation of space flight coordinate system in order to export according to described coordinate transformation module obtains and positional information;
Attitude matrix computing module, the position angle velocity amplitude that magnitude of angular velocity and described velocity location computing module in order to the agricultural machinery according to the transmission of described gyro sensor calculate calculates and obtains the attitude matrix after upgrading; And
Attitude Calculation module, in order to the attitude angle of the agricultural machinery according to the attitude matrix acquisition after the renewal of described attitude matrix computing module.
Referring to shown in Fig. 2, is the flow chart of steps of the control method for agricultural machinery based on strapdown inertial navitation system (SINS) of the present invention.Wherein, described method comprises the following steps:
(1) acceleration of described agricultural machinery is sent to described coordinate transformation module by acceleration transducer described in real time, and simultaneously described in gyro sensor in real time the angular velocity of described agricultural machinery is sent to described attitude matrix computing module;
(2) the position angle velocity amplitude that the attitude matrix computing module described in calculates according to described velocity location computing module and the magnitude of angular velocity of gyro sensor received calculate and obtain the attitude matrix after upgrading, and the attitude matrix after upgrading is sent to coordinate transformation module;
(3) acceleration of the carrier coordinate system of the agricultural machinery described in the transmission of described acceleration transducer is converted to the acceleration of space flight coordinate system by the coordinate transformation module described according to the attitude matrix after renewal;
(4) positional information of the agricultural machinery described in acceleration output of the space flight coordinate system that the velocity location computing module described in exports according to described coordinate transformation module and velocity information, and the attitude angle of the agricultural machinery of described Attitude Calculation module according to the attitude matrix output after the renewal of described attitude matrix computing module;
(5) agricultural machinery of control center according to the control of described positional information, velocity information and attitude angle of the agricultural machinery described in.
First, calculate attitude matrix, attitude matrix refers to the transformation matrix from navigational coordinate system (n system) to carrier coordinate system (b system), and when adopting " sky, northeast " geographic coordinate to be navigational coordinate system, attitude matrix is:
In formula, ψ is position angle (course angle), and θ is the angle of pitch, and γ is roll angle (roll angle), these three angles are called the attitude angle (provided by the hypercomplex number after initialization, later attitude matrix is calculated by hypercomplex number and provides) of carrier.
When the agricultural machinery attitude that six axle inertial sensors are connected changes, the gyro sensor in six axle inertial sensors just sensitivity can go out corresponding angular speed, attitude matrix
there occurs change, its differential equation is thereupon:
in formula,
for angular velocity
The instant correction of the attitude matrix of strapdown inertial navitation system (SINS) is exactly provide attitude matrix in real time, and it is the mission critical of inertial navigation, and this will have been come by certain algorithm.Because the hypercomplex number method calculated amount that counts is little, memory capacity is little, only needs to carry out the orthogonality that simple hypercomplex number standardization processing just can ensure attitude matrix.Unit quaternion can describing by following form:
In strapdown navigation, require that carrier is tied to the transition matrix of navigation system, the equation of motion of following hypercomplex number be separated:
Wherein, navigation coordinate is " sky, northeast " geographic coordinate system, the position angle velocity amplitude that described attitude matrix computing module calculates according to described velocity location computing module and the magnitude of angular velocity of gyro sensor received calculate and obtain the attitude matrix after upgrading, and specifically comprise the following steps:
(2.1) the position angle velocity amplitude that the attitude matrix computing module described in calculates according to described velocity location computing module and the magnitude of angular velocity of gyro sensor received calculate attitude speed;
(2.2) the attitude matrix computing module described in solves quaternion differential equation to obtain the first attitude matrix according to described attitude speed;
(2.3) the attitude matrix computing module described in will obtain the attitude matrix after upgrading after the first described attitude matrix normalization.
Wherein, in the preferred realtime mode of one, described step (2.1) is specially:
Described attitude matrix computing module obtains attitude speed by following formula:
Wherein, the attitude matrix that Eulerian angle represent, namely
for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and
Like this, just can be in the hope of,
Terrestrial coordinate system is connected in tellurian coordinate system, rotates with the earth, and its relative inertness coordinate system rotates with rotationalangular velocity of the earth ω ie, ω ie=15*pi/180.
The output normally angular velocity of gyro sensor, therefore, in order to calculate carrier attitude, introduces quaternion differential equation.The benefit introducing the differential equation, according to a upper moment attitude quaternion, can obtain new attitude quaternion (namely carrier coordinate system is relative to the attitude matrix of navigational coordinate system) by three shaft angle increments of timing sampling carrier coordinate system.What the angle step of being tried to achieve by equivalent rotating vector method can eliminate rotation can not exchange error; Described step (2.2) specifically comprises the following steps:
Described attitude matrix computing module obtains hypercomplex number according to described attitude speed and equivalent rotating vector algorithm and upgrades the differential equation to obtain the first attitude matrix, and described quaternion differential equation is:
Described hypercomplex number upgrades the differential equation:
Wherein,
for within an attitude cycle, the angular velocity that described gyro sensor exports, and
t is time scale, wherein, and angle increment
according to there being equivalent rotating vector two increment algorithm,
Described attitude matrix computing module will obtain the attitude matrix after upgrading after the first described attitude matrix normalization, be specially:
Described attitude matrix computing module will obtain the attitude matrix after upgrading according to following formula after the first described attitude matrix normalization:
Attitude matrix after described renewal is:
Wherein, Q is quaternionic vector, q _{0}, q _{1}, q _{2}, q _{3}for forming the scalar of quaternionic vector, i, j, k are threedimensional system of coordinate vector of unit length, for carrier coordinate system is to the rotation matrix of navigational coordinate system.
Navigation coordinate is " sky, northeast " geographic coordinate system, the acceleration of the carrier coordinate system of the agricultural machinery described in the transmission of described acceleration transducer is converted to the acceleration of space flight coordinate system by described coordinate transformation module according to the attitude matrix after renewal, be specially:
The acceleration of carrier coordinate system of the agricultural machinery described in described acceleration transducer to send according to the attitude matrix after upgrading and by following formula by described coordinate transformation module is converted to the acceleration of space flight coordinate system:
Wherein,
Navigation coordinate is " sky, northeast " geographic coordinate system, and the positional information of the agricultural machinery described in described velocity location computing module exports and velocity information, specifically comprise the following steps:
(4.1) integrated acceleration that described coordinate transformation module exports by the velocity location computing module described in is to obtain the velocity information of described agricultural machinery;
(4.2) the velocity location computing module described in will obtain the positional information of described agricultural machinery after rate integrating.
9, the control method realizing the auxiliary driving of agricultural machinery according to claim 8, is characterized in that, described step (4.1) is specially:
The integrated acceleration that described coordinate transformation module exports according to following formula by described velocity location computing module is to obtain the velocity information of described agricultural machinery:
Wherein,
Wherein,
for earth rate, be known, for
In order to improve computing velocity, adopt second order RungeKutta numerical integration method computing velocity to upgrade in the present invention, formula is as follows:
in formula, K _{1}and K _{2}for speed is at t _{m}and t _{m+1}time slope; Described step (4.2) is specially:
Described velocity location computing module will obtain the positional information of described agricultural machinery by following formula after rate integrating:
Wherein, L, λ, h are respectively the latitude of surface car, longitude, highly, v _{e}, v _{n}, v _{u}for the speed in sky, northeast, provided by velocity location computing module, k is sampled point, and T is the sampling period.
After longitude, latitude carry out renewal calculating, result of calculation L (k) is substituted into the R calculating the kT moment _{m}(k), R _{n}(k)
R _{m}(k)=R _{e}[12f+3fsin ^{2}l (k)], R _{n}(k)=R _{e}[1+fsin ^{2}l (k)], in formula, R _{e}for earth radius, f is ovality.Gravity is approximately with the Changing Pattern of latitude and height:
G=g _{0}[l+0.00527094sin ^{2}(L)+0.0000232718sin ^{4}(L)]0.000003086h, in formula, g _{0}=9.7803267714.
Navigation coordinate is " sky, northeast " geographic coordinate system, and the attitude angle of the agricultural machinery described in described Attitude Calculation module exports according to the attitude matrix after the renewal of described attitude matrix computing module, specifically comprises the following steps:
Described Attitude Calculation module extracts the attitude angle of the agricultural machinery comprised described in the angle of pitch, roll angle and course angle from the attitude matrix after described renewal.
In a preferred embodiment, the attitude angle of agricultural machinery can from upgrading the attitude matrix after calculating middle extraction, comprises the angle of pitch, roll angle and course angle, because pitching angle theta is defined in ± 90 ° of intervals, consistent with the main value of arcsin function, there is not multivalue problem.And roll angle γ to be defined in [180 °, 180 °] interval, it is interval that course angle ψ is defined in [0 °, 360 °], therefore γ, ψ exist multivalue problem, after calculating main value, can be by in element judge be at which quadrant.
Due to
For the angle of pitch: θ=θ _{main};
For roll angle:
For course angle:
So:
Must be pointed out, highly unstable due to pure inertial navigation altitude channel, some error sources comprising acceleration transducer error can form accumulation property error, and height error can increase in time and accelerate to increase.Therefore only can not calculate agricultural machinery elevation information in a long time by speed, must revise by barometric altimeter or radio altimeter signal.Because in pure inertial navigation system, altitude channel is dispersed, available external elevation references information introduces damping, does not consider for height and with the item of height correlation at this.
Error analysis: the error source of inertial navigation system has a lot, mainly contains the error of inertia type instrument itself, the alignment error of inertia type instrument and Calibration errors, the starting condition error of system, the error etc. that the error of calculation of system and various interference cause.Ins error can be divided into two classes: ascertainment error and stochastic error.Ascertainment error comprises mesa corners error, velocity error and site error, and stochastic error is gyro sensor drift and the zero offset etc. of acceleration transducer mainly.Although inertial navigation system exists multiple error source, wherein fractional error source is very little on the impact of inertial navigation system.Because strapdown inertial navigation system adopts mathematical platform sub platform, namely attitude matrix calculating is carried out with the angular velocity information that gyro sensor is measured, the ratio force information measured with acceleration transducer carries out navigation through attitude matrix conversion and calculates, so the error of inertial sensor and starting condition error are propagated in systems in which by attitude matrix, important impact is produced on navigation.That is, gyro sensor drifts about, and acceleration transducer zero partially error and these three kinds of error sources of initial value error has impact to a certain degree to navigational parameter.In the description of following strapdown inertial navitation system (SINS) error model, strapdown inertial coordinate system (n system) adopts " sky, northeast " geographic coordinate system, and navigation information error is 9 dimensions, threedimensional platform error angle, threedimensional velocity error and threedimensional position error.
Navigation coordinate is " sky, northeast " geographic coordinate system, further comprising the steps of between described step (4) and step (5):
(4.3) central controller described in judges to calculate system angle error according to following formula (10), velocity error is calculated according to following formula (11), calculate site error according to following formula (12), calculate inertia type instrument error according to following formula (13):
ε＝ε _{b}+ε _{r}+ω _{g}(13)
▽＝▽ _{b}+▽ _{a}+ω _{a}
Wherein, φ
_{e}, φ
_{n}, φ
_{u}for three attitude angle in sky, northeast navigational coordinate system, v
_{e}, v
_{n}, v
_{u}for the speed in sky, northeast, L, λ, h are respectively the latitude of surface car, longitude, highly, f
_{e}, f
_{n}, f
_{u}be respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, R
_{m}, R
_{n}for earth meridian ellipse and prime plane radiusofcurvature,
(X is φ, v, L, λ, h) be the derivative of its correspondence,
(Y is v, L, λ, h) be the error of its corresponding derivative, ε is gyroscope total error, ε
_{b}, ε
_{r}, ε
_{g}be respectively constant value drift, single order Markov process and white Gaussian noise, Δ is accelerometer total error, Δ
_{b}, Δ
_{a}, ω
_{a}be respectively constant value drift, single order Markov process and white Gaussian noise; ω
_{ie}for earth rate, be known, δ v
_{e}, δ v
_{n}, δ v
_{u}for sky, northeast velocity error under navigational coordinate system, δ L, δ λ, δ h are site error,
(4.4) central controller described according to abovementioned calculated system angle error, velocity error, site error and inertia type instrument error, and carries out error compensation according to Kalman filtering algorithm to described positional information, velocity information and attitude angle.
Other error, comprise coning motion error, sculling algorithms and scrollwork error etc., wherein coning motion error is by being compensated rotating vector algorithm optimization, and sculling algorithms and scrollwork error can be less on the impact of strapdown inertial navitation system (SINS) by matlab emulation, can ignore.
By above to error term formula, compensate through Kalman filtering algorithm by setting up accurate mathematical model.Comprehensive above various, the 15 dimension state equations of integrated navigation system under the velocity composition pattern of position can be obtained; Described carries out error compensation according to Kalman filtering algorithm to described positional information, velocity information and attitude angle, is specially:
It is described that to compensate to obtain 15 dimension state equations according to Kalman filtering algorithm to described positional information, velocity information and attitude angle as follows:
Wherein,
X (t)=[φ _{e}φ _{n}φ _{u}δ v _{e}δ v _{n}δ v _{u}δ L δ λ δ h ε _{bx}ε _{by}ε _{bz}▽ _{bx}▽ _{by}▽ _{bz}] ^{t}for the state vector of system, wherein, subscript E, N, U represent three directions of sky, northeast geographic coordinate system respectively, φ _{e}, φ _{n}, φ _{u}for the error angle of strapdown inertial navitation system (SINS), δ v _{e}, δ v _{n}, δ v _{u}for velocity error, δ L, δ λ, δ h are site error, ε _{bx}, ε _{by}, ε _{bz}gyrostatic random drift, ▽ _{bx}, ▽ _{by}, ▽ _{bz}the error of zero of accelerometer; W (t)=[ω _{gx}ω _{gy}ω _{gz}ω _{ax}ω _{ay}ω _{az}] ^{t}for systematic procedure white noise vector, wherein, ω _{gx}, ω _{gy}, ω _{gz}for the white noise of gyro, ω _{ax}, ω _{ay}, ω _{az}for the white noise of accelerometer; F (t) is system state matrix, and G (t) is system noise propogator matrix.
Wherein, Kalman filtering algorithm is current techique, but its important technology that is chosen to be of its mathematical model and parameter is also diacritical point, patent of the present invention fully takes into account all kinds of error terms in farm machinery automatic control system, and mathematical modeling is carried out to it, realize abovementioned filtering algorithm by software programming, and show that attitude error angle is less than 0.1 ° through tractor test on the spot, course angle error is less than 1 °, the test findings of about position deviation 5cm.
Have employed the strapdown inertial navitation system (SINS) for agricultural machinery in this invention and control method, compared with prior art, there is following beneficial effect:
(2) error compensation that adopts of the present invention and correction algorithm, substantially reduce the interference such as the Algorithm Error of strapdown inertial navitation system (SINS) and earth rotation;
(2) the six axle inertial sensors adopted and the algorithm of strapdown inertial navitation system (SINS) make the present invention have higher performance parameter for the strapdown inertial navitation system (SINS) of agricultural machinery, export course angle error through tractor test cabinet this device outer and be less than 1 °, the angle of pitch and rolling angular error are less than 0.1 °, the integrated navigation site error that the positional information that strapdown inertial navitation system (SINS) for agricultural machinery of the present invention exports coordinates GPS to realize is in cm level, data output frequencies reaches 50HZ, meets the requirement that Ride Control System assisted by agricultural machinery;
(3) present invention employs six axle inertial sensors, it comprises the acceleration transducer in three directions and the gyro sensor of three axles, and volume is little, lightweight, cost performance is high, and modular design is convenient to be integrated into agricultural machinery and is assisted among Ride Control System;
(4) the present invention has stable for the strapdown inertial navitation system (SINS) of agricultural machinery and exports the advantages such as movable information enriches, and especially meets the surface cars such as agricultural machinery and assists Ride Control System requirement.
In this description, the present invention is described with reference to its specific embodiment.But, still can make various amendment and conversion obviously and not deviate from the spirit and scope of the present invention.Therefore, instructions and accompanying drawing are regarded in an illustrative, rather than a restrictive.
Claims (13)
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201510134478.8A CN104697526A (en)  20150326  20150326  Strapdown inertial navitation system and control method for agricultural machines 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CN201510134478.8A CN104697526A (en)  20150326  20150326  Strapdown inertial navitation system and control method for agricultural machines 
Publications (1)
Publication Number  Publication Date 

CN104697526A true CN104697526A (en)  20150610 
Family
ID=53344895
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201510134478.8A CN104697526A (en)  20150326  20150326  Strapdown inertial navitation system and control method for agricultural machines 
Country Status (1)
Country  Link 

CN (1)  CN104697526A (en) 
Cited By (11)
Publication number  Priority date  Publication date  Assignee  Title 

CN105203098A (en) *  20151013  20151230  上海华测导航技术股份有限公司  Whole attitude angle updating method applied to agricultural machinery and based on nineaxis MEMS (microelectromechanical system) sensor 
CN105509740A (en) *  20151231  20160420  广州中海达卫星导航技术股份有限公司  Measuring method and module for attitude of agriculture machinery vehicle 
CN105607093A (en) *  20151220  20160525  上海华测导航技术股份有限公司  Integrated navigation system and method for acquiring navigation coordinate 
CN106441275A (en) *  20160923  20170222  深圳大学  Method and device for updating planned path of robot 
CN106595637A (en) *  20161221  20170426  上海华测导航技术股份有限公司  Visual navigation method for agricultural machine 
CN106931965A (en) *  20151231  20170707  中国移动通信集团吉林有限公司  A kind of method and device for determining terminal attitude 
CN107421494A (en) *  20170401  20171201  深圳市元征科技股份有限公司  Vehicle attitude detection method and device 
CN107607113A (en) *  20170802  20180119  华南农业大学  A kind of two axle posture inclination angle measurement methods 
CN109781096A (en) *  20171115  20190521  洛阳中科晶上智能装备科技有限公司  A kind of integrated navigation and location system and method for intelligent agricultural machinery 
CN109927726A (en) *  20190313  20190625  深兰科技（上海）有限公司  A kind of method and apparatus adjusting target vehicle motion state 
CN109987097A (en) *  20190313  20190709  深兰科技（上海）有限公司  A kind of method and apparatus adjusting target vehicle motion state 
Citations (2)
Publication number  Priority date  Publication date  Assignee  Title 

CN101413800A (en) *  20080118  20090422  南京航空航天大学  Navigating and steady aiming method of navigation / steady aiming integrated system 
CN103712623A (en) *  20140120  20140409  东南大学  Opticalfiber gyroscope inertial navigation system attitude optimization method based on angular rate input 

2015
 20150326 CN CN201510134478.8A patent/CN104697526A/en not_active Application Discontinuation
Patent Citations (2)
Publication number  Priority date  Publication date  Assignee  Title 

CN101413800A (en) *  20080118  20090422  南京航空航天大学  Navigating and steady aiming method of navigation / steady aiming integrated system 
CN103712623A (en) *  20140120  20140409  东南大学  Opticalfiber gyroscope inertial navigation system attitude optimization method based on angular rate input 
NonPatent Citations (4)
Title 

汪懋华 等，: "《现代精细农业理论与实践》", 31 October 2012 * 
蒋黎星: ""捷联惯性导航算法及半实物仿真系统研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * 
陈勇: ""基于等效旋转矢量法的捷联惯导系统仿真"", 《淮阴师范学院学报（自然科学版）》 * 
陈曙光: ""捷联惯性导航系统的仿真研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * 
Cited By (14)
Publication number  Priority date  Publication date  Assignee  Title 

CN105203098A (en) *  20151013  20151230  上海华测导航技术股份有限公司  Whole attitude angle updating method applied to agricultural machinery and based on nineaxis MEMS (microelectromechanical system) sensor 
CN105607093A (en) *  20151220  20160525  上海华测导航技术股份有限公司  Integrated navigation system and method for acquiring navigation coordinate 
CN105607093B (en) *  20151220  20180508  上海华测导航技术股份有限公司  A kind of integrated navigation system and the method for obtaining navigation coordinate 
CN106931965A (en) *  20151231  20170707  中国移动通信集团吉林有限公司  A kind of method and device for determining terminal attitude 
CN105509740A (en) *  20151231  20160420  广州中海达卫星导航技术股份有限公司  Measuring method and module for attitude of agriculture machinery vehicle 
CN106441275A (en) *  20160923  20170222  深圳大学  Method and device for updating planned path of robot 
CN106595637A (en) *  20161221  20170426  上海华测导航技术股份有限公司  Visual navigation method for agricultural machine 
CN107421494B (en) *  20170401  20191210  深圳市元征科技股份有限公司  vehicle attitude detection method and device 
CN107421494A (en) *  20170401  20171201  深圳市元征科技股份有限公司  Vehicle attitude detection method and device 
CN107607113A (en) *  20170802  20180119  华南农业大学  A kind of two axle posture inclination angle measurement methods 
CN107607113B (en) *  20170802  20200317  华南农业大学  Method for measuring inclination angles of twoaxis attitude 
CN109781096A (en) *  20171115  20190521  洛阳中科晶上智能装备科技有限公司  A kind of integrated navigation and location system and method for intelligent agricultural machinery 
CN109927726A (en) *  20190313  20190625  深兰科技（上海）有限公司  A kind of method and apparatus adjusting target vehicle motion state 
CN109987097A (en) *  20190313  20190709  深兰科技（上海）有限公司  A kind of method and apparatus adjusting target vehicle motion state 
Similar Documents
Publication  Publication Date  Title 

CN104198765B (en)  The coordinate system conversion method of vehicle acceleration of motion detection  
CN106289246B (en)  A kind of flexible link arm measure method based on position and orientation measurement system  
Ahmed et al.  Accurate attitude estimation of a moving land vehicle using lowcost MEMS IMU sensors  
US8442703B2 (en)  Turningstabilized estimation of the attitude angles of an aircraft  
CN101413800B (en)  Navigating and steady aiming method of navigation / steady aiming integrated system  
CN103314274B (en)  The evaluation method of the track of moving element or object and system  
CN101514899B (en)  Optical fibre gyro strapdown inertial navigation system error inhibiting method based on singleshaft rotation  
KR101168100B1 (en)  Systems and methods for estimating position, attitude and/or heading of a vehicle  
Phuong et al.  A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass  
EP1582840B1 (en)  Inertial navigation system error correction  
US6876926B2 (en)  Method and system for processing pulse signals within an inertial navigation system  
CN101476894B (en)  Vehiclemounted SINS/GPS combined navigation system performance reinforcement method  
CN101907714B (en)  GPS aided positioning system and method based on multisensor data fusion  
DE4029215C2 (en)  
CN103323026B (en)  The attitude reference estimation of deviation of star sensor and useful load and modification method  
CN100593689C (en)  Gasture estimation and interfusion method based on strapdown inertial nevigation system  
US9026263B2 (en)  Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis  
RU2662460C1 (en)  Method of upgrading angular position of agricultural machine based on the nineaxis mems sensor  
CN103389092B (en)  A kind of kite balloon airship attitude measuring and measuring method  
CN100541135C (en)  Fiberoptic gyroscope strapdown inertial navigation system initial attitude based on Doppler is determined method  
CN102980577B (en)  Microstrapdown altitude heading reference system and working method thereof  
CN103776446B (en)  A kind of pedestrian's independent navigation computation based on double MEMSIMU  
US6459990B1 (en)  Selfcontained positioning method and system thereof for water and land vehicles  
CN101082493B (en)  Combined positioning method of agricultural machines navigation  
Sun et al.  MEMSbased rotary strapdown inertial navigation system 
Legal Events
Date  Code  Title  Description 

PB01  Publication  
C06  Publication  
SE01  Entry into force of request for substantive examination  
C10  Entry into substantive examination  
RJ01  Rejection of invention patent application after publication 
Application publication date: 20150610 

RJ01  Rejection of invention patent application after publication 