CN104697526A - Strapdown inertial navitation system and control method for agricultural machines - Google Patents

Strapdown inertial navitation system and control method for agricultural machines Download PDF

Info

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
Application number
CN201510134478.8A
Other languages
Chinese (zh)
Inventor
任强
王杰俊
沈雪峰
戴文鼎
Original Assignee
上海华测导航技术股份有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 上海华测导航技术股份有限公司 filed Critical 上海华测导航技术股份有限公司
Priority to CN201510134478.8A priority Critical patent/CN104697526A/en
Publication of CN104697526A publication Critical patent/CN104697526A/en

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/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

The invention relates to a strapdown inertial navitation system for agricultural machines. The system comprises a six-axis inertial sensor and a central control unit. The six-axis inertial sensor comprises acceleration sensors in three directions and a three-axis gyroscope sensor. The central control unit comprises a coordinate conversion module, a speed position calculating module, an attitude matrix calculating module and an attitude calculating module. The invention further relates to a control method for the agricultural machines based on the strapdown inertial navitation system. According to the strapdown inertial navitation system and control method for the agricultural machines of the structure, the six-axis inertial sensor is adopted, the size is small, weight is low, the structure is simple, the cost performance is high, the system can be integrated into a control center of the agricultural machines conveniently through the modular design, meanwhile, the advantages of being stable in running, rich in output motion information and the like are achieved, control precision is high, especially the requirement of the agricultural machines and other ground vehicles for aided driving control is met, and the application range is wide.

Description

For strapdown inertial navitation system (SINS) and the control method of agricultural machinery

Technical field

The present invention relates to Mechanical course field, particularly relate to high-accuracy 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 (Micro-Electro-Mechanical-System) 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 high-precision 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 multi-sensor information fusion (IMU+GPS).Multi-sensor 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 high-precision GPS locating information (as RTK) of the acquisition Main Basis of positional information in multi-sensor 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 high-frequency high-precision 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 above-mentioned 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:

ω nb b = ω ib b - ω in b = ω ib b - C n b ω in n = ω ib b - C n b ( ω ie n + ω en n ) - - - ( 1 )

Wherein, the attitude matrix that Eulerian angle represent, namely

C n b = cos γ cos ψ + sin γ sin θ sin ψ - cos γ sin ψ + sin γ sin θ cos ψ - sin γ cos θ cos θ sin ψ cos θ cos ψ sin θ sin γ cos ψ - cos γ sin θ sin ψ - sin γ sin ψ - cos γ sin θ cos ψ cos γ cos θ - - - ( 2 )

for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and ω ib b = ω bx b ω by b ω bz b ; for earth rate, be known, for ω ie n = 0 ω e cos L ω e sin L ; for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ω en n = - v N R M + h ω e cos L + v E R N + h ω e sin L + v E R N + h tan L ; for attitude speed, and ω nb b = ω nb bx ω nb by ω nb bz , ψ is position angle, and θ is the angle of pitch, and γ is roll angle, v e, v n, v ufor the speed in sky, northeast, provided by velocity location computing module, R m, R nfor earth meridian ellipse and prime plane radius-of-curvature, R m≈ R (1-2e+3esin 2l), R n≈ R (1+esin 2l), wherein, R=m, e=1/298.257), L, λ, h are respectively latitude, longitude, highly, being intermediate variable, is the projection of conjunction angular velocity in carrier coordinate system of the angular velocity of the rotational-angular velocity of the earth earth relative to space flight coordinate system; ω eearth rotation closes angular velocity.

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:

Q · = ω ib b ( t ) + 1 2 Q × ω ib b ( t ) - - - ( 3 )

Described hypercomplex number upgrades the differential equation:

Q ( t ) = ( I cos Δθ 2 + [ Δθ ] sin Δθ 2 Δθ ) · Q ( 0 ) - - - ( 4 )

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, Δ θ = ( θ 1 + θ 2 ) + 2 3 ( θ 1 × θ 2 ) , Q ( 0 ) = cos θ 0 2 cos γ 0 2 cos ψ 0 2 + sin θ 0 2 sin γ 0 2 sin ψ 0 2 sin θ 0 2 cos γ 0 2 cos ψ 0 2 + cos θ 0 2 sin γ 0 2 sin ψ 0 2 cos θ 0 2 sin γ 0 2 cos ψ 0 2 - sin θ 0 2 cos γ 0 2 sin ψ 0 2 cos θ 0 2 cos γ 0 2 sin ψ 0 2 - sin θ 0 2 sin γ 0 2 cos ψ 0 2 , I is unit matrix; Q is quaternionic vector; for hypercomplex number derivative; Δ t is the Data Update cycle; The result of the first attitude matrix is Q=q 0+ q 1i+q 2j+q 3k, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length, θ 1and θ 2be respectively the angle increment of gyroscope in the posture renewal cycle twice time-sampling at equal intervals; θ 0, γ 0, ψ 0be respectively the angle of pitch under original state, roll angle and course angle, the initial quaternary numerical value of Q (0) for utilizing above-mentioned initial attitude angle to calculate.

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:

Q = q 0 + q 1 i + q 2 j + q 3 k q 0 2 + q 1 2 + q 2 2 + q 3 2 - - - ( 5 )

Attitude matrix after described renewal is:

C b n = q 1 2 + q 0 2 - q 3 2 - q 2 2 2 ( q 1 q 2 - q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 2 2 - q 3 2 + q 0 2 - q 1 2 2 ( q 2 q 3 - q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 3 2 - q 2 2 - q 1 2 + q 0 2 - - - ( 6 )

Wherein, Q is quaternionic vector, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional 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

f E f N f U = C b n f bx f by f bz - - - ( 7 )

Wherein, f b = f bx f by f bz For the acceleration that acceleration transducer exports, f e, f n, f ube respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, be upgrade after attitude matrix be:

C b n = q 1 2 + q 0 2 - q 3 2 - q 2 2 2 ( q 1 q 2 - q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 2 2 - q 3 2 + q 0 2 - q 1 2 2 ( q 2 q 3 - q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 3 2 - q 2 2 - q 1 2 + q 0 2 , Wherein, Q is quaternionic vector, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length.

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:

v · n = f n - ( ω en n + 2 ω ie n ) × v n + g - - - ( 8 )

Wherein, ω ie n = 0 ω e cos L ω e sin L , ω en n = - v N R M + h v E R N + h v E R N + h tan L , f n = f E f N f U , v · n = v · E v · N v · U , v n = v E v N v U , g = 0 0 - g The matrix representation of formula (8) is:

v · E v · N v · U = f E f N f U - 0 - ( 2 ω iez n + ω enz n ) 2 ω iey n + ω eny n 2 ω iez n + ω enz n 0 - ( 2 ω iex n + ω enx n ) - ( 2 ω iey n + ω eny n ) 2 ω iex n + ω enx n 0 v E v N v U + 0 0 - g

Wherein, for earth rate, be known, for ω ie n = 0 ω e cos L ω e sin L ; for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ω en n = - v N R M + h ω e cos L + v E R N + h ω e sin L + v E R N + h tan L , L, λ, h are respectively latitude, longitude, highly; ψ is position angle, and θ is the angle of pitch, and γ is roll angle, v e, v n, v ufor the speed in the east of space flight coordinate system, north, sky, provided by velocity location computing module, R m, R nfor earth meridian ellipse and prime plane radius-of-curvature, R m≈ R (1-2e+3esin 2l), R n≈ R (1+esin 2l), wherein, R=m, e=1/298.257), f e, f n, f ube respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, ω eearth rotation closes angular velocity.

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:

λ ( k ) = λ ( k - 1 ) + v E ( k - 1 ) [ R M ( k - 1 ) + h ( k - 1 ) ] cos L ( k - 1 ) · T L ( k ) = L ( k - 1 ) + v N ( k - 1 ) R N ( k - 1 ) + h ( k - 1 ) · T h ( k ) = h ( k - 1 ) + v U ( K - 1 ) · T - - - ( 9 )

Wherein, L, λ, h are respectively the latitude of surface car, longitude, highly, v e, v n, v ufor 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):

φ · E = - δ v N R M + h + ( ω ie sin L + v E R N + h tan L ) φ N - ( ω ie cos L + v E R N + h ) φ U + ϵ E φ · N = δ v E R N + h - ω ie sin LδL - ( ω ie sin L + v E R N + h tan L ) φ E - v N R M + h φ U + ϵ N φ · U = δ v E R N + h tan L + ( ω ie cos L + v E R N + h sec 2 L ) δL + ( ω ie cos L + v E R N + h ) φ E + v N R M + h φ N + ϵ U - - - ( 10 )

δ v · E = f N φ U - f U φ N + ( v N R M + h tan L - v U R M + h ) δ v E + ( 2 ω ie sin L + v E R N + h tan L ) δ v N - ( 2 ω ie cos L + v E R N + h ) δ v U + ( 2 ω ie cos L v N + v E v N R N + h sec 2 L + 2 ω ie sin L v U ) δL + ▿ E δ v · N = f U φ E - f E φ U - 2 ( ω ie sin L + v E R N + h tgL ) δ v E - v U R M + h δ v N - v N R M + h δ v U - ( 2 ω ie cos L + v E R N + h sec 2 L ) v E δL + ▿ N δ v · U = f E φ N - f N φ E + 2 ( ω ie cos L + v E R N + h ) δ v E - 2 ω ie sin L v E δL + 2 v N R M + h δ v N + ▿ U - - - ( 11 )

δ L · = δ v N R M + h δ λ · = δ v E R N + h sec L + v E sec L R N + h tan LδL δ h · = δ v U - - - ( 12 )

ε=ε brg(13)

▽=▽ b+▽ aa

Wherein, φ e, φ n, φ ufor three attitude angle in sky, northeast navigational coordinate system, v e, v n, v ufor the speed in sky, northeast, L, λ, h are respectively the latitude of surface car, longitude, highly, f e, f n, f ube 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 nfor earth meridian ellipse and prime plane radius-of-curvature, (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, ε gbe respectively constant value drift, single order Markov process and white Gaussian noise, Δ is accelerometer total error, Δ b, Δ a, ω abe respectively constant value drift, single order Markov process and white Gaussian noise; ω iefor earth rate, be known, δ v e, δ v n, δ v ufor sky, northeast velocity error under navigational coordinate system, δ L, δ λ, δ h are site error, ϵ = ϵ E ϵ N ϵ U With ▿ = ▿ E ▿ N ▿ U The total error in gyroscope and accelerometer sky, northeast under navigational coordinate system respectively, E, N, U subscript is respectively the component on three directions.

(4.4) central controller described according to above-mentioned 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:

X · ( t ) = F ( t ) X ( t ) + G ( t ) W ( t ) - - - ( 14 )

Wherein,

X (t)=[φ eφ nφ uδ v eδ v nδ v uδ L δ λ δ h ε bxε byε bzbxbybz] tfor the state vector of system, wherein, subscript E, N, U represent three directions of sky, northeast geographic coordinate system respectively, φ e, φ n, φ ufor the error angle of strapdown inertial navitation system (SINS), δ v e, δ v n, δ v ufor velocity error, δ L, δ λ, δ h are site error, ε bx, ε by, ε bzgyrostatic random drift, ▽ bx, ▽ by, ▽ bzthe error of zero of accelerometer; W (t)=[ω gxω gyω gzω axω ayω az] tfor systematic procedure white noise vector, wherein, ω gx, ω gy, ω gzfor the white noise of gyro, ω ax, ω ay, ω azfor 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 Real-time 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 fixed-axis 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:

C n b = cos γ cos ψ + sin γ sin θ sin ψ - cos γ sin ψ + sin γ sin θ cos ψ - sin γ cos θ cos θ sin ψ cos θ cos ψ sin θ sin γ cos ψ - cos γ sin θ sin ψ - sin γ sin ψ - cos γ sin θ cos ψ cos γ cos θ

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 ω nb b = ω nb bx ω nb by ω nb bz T The antisymmetric matrix formed; X, y, z are defined as right front upper three directions.

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:

Q = q 0 + q 1 i + q 2 j + q 3 k = q 0 q 1 q 2 q 3

In strap-down navigation, require that carrier is tied to the transition matrix of navigation system, the equation of motion of following hypercomplex number be separated: Q · = 1 2 ΩQ , In formula, Ω = 0 - ω nb bx - ω nb by - ω nb bz ω nb bx 0 ω nb bz - ω nb by ω nb by - ω nb bz 0 ω nb bx ω nb bz ω nb by ω nb bx 0 , Q = q 0 q 1 q 2 q 3 , Q is the rotation hypercomplex number being tied to navigation system from carrier.

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 real-time mode of one, described step (2.1) is specially:

Described attitude matrix computing module obtains attitude speed by following formula:

ω nb b = ω ib b - ω in b = ω ib b - C n b ω in n = ω ib b - C n b ( ω ie n + ω en n ) - - - ( 1 )

Wherein, the attitude matrix that Eulerian angle represent, namely

C n b = cos γ cos ψ + sin γ sin θ sin ψ - cos γ sin ψ + sin γ sin θ cos ψ - sin γ cos θ cos θ sin ψ cos θ cos ψ sin θ sin γ cos ψ - cos γ sin θ sin ψ - sin γ sin ψ - cos γ sin θ cos ψ cos γ cos θ - - - ( 2 )

for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and ω ib b = ω bx b ω by b ω bz b ; for earth rate, be known, for ω ie n = 0 ω e cos L ω e sin L ; for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ω en n = - v N R M + h ω e cos L + v E R N + h ω e sin L + v E R N + h tan L ; for attitude speed, and ω nb b = ω nb bx ω nb by ω nb bz , ψ is position angle, and θ is the angle of pitch, and γ is roll angle, v e, v n, v ufor the speed in sky, northeast, provided by velocity location computing module, R m, R nfor earth meridian ellipse and prime plane radius-of-curvature, R m≈ R (1-2e+3esin 2l), R n≈ R (1+esin 2l), wherein, R=m, e=1/298.257), L, λ, h are respectively latitude, longitude, highly, being intermediate variable, is the projection of conjunction angular velocity in carrier coordinate system of the angular velocity of the rotational-angular velocity of the earth earth relative to space flight coordinate system; ω eearth rotation closes angular velocity.

Like this, just can be in the hope of,

ω nb bx ω nb by ω nb bz = ω bx ω by ω bz - C n b ( 0 ω e cos L ω e sin L + - v N R M + h v E R N + h v E R N + h tan L ) = ω bx ω by ω bz - C b n - v N R M + h ω e cos L + v E R N + h ω e sin L + v E R N + h tan L

Terrestrial coordinate system is connected in tellurian coordinate system, rotates with the earth, and its relative inertness coordinate system rotates with rotational-angular 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:

Q · = ω ib b ( t ) + 1 2 Q × ω ib b ( t ) - - - ( 3 )

Described hypercomplex number upgrades the differential equation:

Q ( t ) = ( I cos Δθ 2 + [ Δθ ] sin Δθ 2 Δθ ) · Q ( 0 ) - - - ( 4 )

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, Δ θ = ( θ 1 + θ 2 ) + 2 3 ( θ 1 × θ 2 ) , Q ( 0 ) = cos θ 0 2 cos γ 0 2 cos ψ 0 2 + sin θ 0 2 sin γ 0 2 sin ψ 0 2 sin θ 0 2 cos γ 0 2 cos ψ 0 2 + cos θ 0 2 sin γ 0 2 sin ψ 0 2 cos θ 0 2 sin γ 0 2 cos ψ 0 2 - sin θ 0 2 cos γ 0 2 sin ψ 0 2 cos θ 0 2 cos γ 0 2 sin ψ 0 2 - sin θ 0 2 sin γ 0 2 cos ψ 0 2 , I is unit matrix; Q is quaternionic vector; for hypercomplex number derivative; Δ t is the Data Update cycle; The result of the first attitude matrix is Q=q 0+ q 1i+q 2j+q 3k, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length, θ 1and θ 2be respectively the angle increment of gyroscope in the posture renewal cycle twice time-sampling at equal intervals; θ 0, γ 0, ψ 0be respectively the angle of pitch under original state, roll angle and course angle, the initial quaternary numerical value of Q (0) for utilizing above-mentioned initial attitude angle to calculate.

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:

Q = q 0 + q 1 i + q 2 j + q 3 k q 0 2 + q 1 2 + q 2 2 + q 3 2 - - - ( 5 )

Attitude matrix after described renewal is:

C b n = q 1 2 + q 0 2 - q 3 2 - q 2 2 2 ( q 1 q 2 - q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 2 2 - q 3 2 + q 0 2 - q 1 2 2 ( q 2 q 3 - q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 3 2 - q 2 2 - q 1 2 + q 0 2 - - - ( 6 )

Wherein, Q is quaternionic vector, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional 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:

f E f N f U = C b n f bx f by f bz - - - ( 7 )

Wherein, f b = f bx f by f bz For the acceleration that acceleration transducer exports, f e, f n, f ube respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, be upgrade after attitude matrix be:

C b n = q 1 2 + q 0 2 - q 3 2 - q 2 2 2 ( q 1 q 2 - q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 2 2 - q 3 2 + q 0 2 - q 1 2 2 ( q 2 q 3 - q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 3 2 - q 2 2 - q 1 2 + q 0 2 , Wherein, Q is quaternionic vector, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length.

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:

v · n = f n - ( ω en n + 2 ω ie n ) × v n + g - - - ( 8 )

Wherein, ω ie n = 0 ω e cos L ω e sin L , ω en n = - v N R M + h v E R N + h v E R N + h tan L , f n = f E f N f U , v · n = v · E v · N v · U , v n = v E v N v U , g = 0 0 - g The matrix representation of formula (8) is:

v · E v · N v · U = f E f N f U - 0 - ( 2 ω iez n + ω enz n ) 2 ω iey n + ω eny n 2 ω iez n + ω enz n 0 - ( 2 ω iex n + ω enx n ) - ( 2 ω iey n + ω eny n ) 2 ω iex n + ω enx n 0 v E v N v U + 0 0 - g

Wherein, for earth rate, be known, for ω ie n = 0 ω e cos L ω e sin L ; for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ω en n = - v N R M + h ω e cos L + v E R N + h ω e sin L + v E R N + h tan L , L, λ, h are respectively latitude, longitude, highly; ψ is position angle, and θ is the angle of pitch, and γ is roll angle, v e, v n, v ufor the speed in sky, northeast, provided by velocity location computing module, R m, R nfor earth meridian ellipse and prime plane radius-of-curvature, R m≈ R (1-2e+3esin 2l), R n≈ R (1+esin 2l), wherein, R=m, e=1/298.257), f e, f n, f ube respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, ω eearth rotation closes angular velocity.

In order to improve computing velocity, adopt second order Runge-Kutta numerical integration method computing velocity to upgrade in the present invention, formula is as follows:

in formula, K 1and K 2for speed is at t mand t m+1time 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:

λ ( k ) = λ ( k - 1 ) + v E ( k - 1 ) [ R M ( k - 1 ) + h ( k - 1 ) ] cos L ( k - 1 ) · T L ( k ) = L ( k - 1 ) + v N ( k - 1 ) R N ( k - 1 ) + h ( k - 1 ) · T h ( k ) = h ( k - 1 ) + v U ( K - 1 ) · T - - - ( 9 )

Wherein, L, λ, h are respectively the latitude of surface car, longitude, highly, v e, v n, v ufor 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[1-2f+3fsin 2l (k)], R n(k)=R e[1+fsin 2l (k)], in formula, R efor 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

C n b = C 11 C 12 C 13 C 21 C 22 C 23 C 31 C 32 C 33 cos γ cos ψ + sin γ sin ψ sin θ sin ψ cos θ sin γ cos ψ - cos γ sin ψ sin θ - cos γ sin ψ + sin γ cos ψ sin θ cos ψ cos θ - sin γ sin ψ - cos γ cos ψ sin θ - sin γ cos θ sin θ cos γ cos θ

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, strap-down inertial coordinate system (n system) adopts " sky, northeast " geographic coordinate system, and navigation information error is 9 dimensions, three-dimensional platform error angle, three-dimensional velocity error and three-dimensional 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):

φ · E = - δ v N R M + h + ( ω ie sin L + v E R N + h tan L ) φ N - ( ω ie cos L + v E R N + h ) φ U + ϵ E φ · N = δ v E R N + h - ω ie sin LδL - ( ω ie sin L + v E R N + h tan L ) φ E - v N R M + h φ U + ϵ N φ · U = δ v E R N + h tan L + ( ω ie cos L + v E R N + h sec 2 L ) δL + ( ω ie cos L + v E R N + h ) φ E + v N R M + h φ N + ϵ U - - - ( 10 )

δ v · E = f N φ U - f U φ N + ( v N R M + h tan L - v U R M + h ) δ v E + ( 2 ω ie sin L + v E R N + h tan L ) δ v N - ( 2 ω ie cos L + v E R N + h ) δ v U + ( 2 ω ie cos L v N + v E v N R N + h sec 2 L + 2 ω ie sin L v U ) δL + ▿ E δ v · N = f U φ E - f E φ U - 2 ( ω ie sin L + v E R N + h tgL ) δ v E - v U R M + h δ v N - v N R M + h δ v U - ( 2 ω ie cos L + v E R N + h sec 2 L ) v E δL + ▿ N δ v · U = f E φ N - f N φ E + 2 ( ω ie cos L + v E R N + h ) δ v E - 2 ω ie sin L v E δL + 2 v N R M + h δ v N + ▿ U - - - ( 11 )

δ L · = δ v N R M + h δ λ · = δ v E R N + h sec L + v E sec L R N + h tan LδL δ h · = δ v U - - - ( 12 )

ε=ε brg(13)

▽=▽ b+▽ aa

Wherein, φ e, φ n, φ ufor three attitude angle in sky, northeast navigational coordinate system, v e, v n, v ufor the speed in sky, northeast, L, λ, h are respectively the latitude of surface car, longitude, highly, f e, f n, f ube 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 nfor earth meridian ellipse and prime plane radius-of-curvature, (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, ε gbe respectively constant value drift, single order Markov process and white Gaussian noise, Δ is accelerometer total error, Δ b, Δ a, ω abe respectively constant value drift, single order Markov process and white Gaussian noise; ω iefor earth rate, be known, δ v e, δ v n, δ v ufor sky, northeast velocity error under navigational coordinate system, δ L, δ λ, δ h are site error, ϵ = ϵ E ϵ N ϵ U With ▿ = ▿ E ▿ N ▿ U The total error in gyroscope and accelerometer sky, northeast under navigational coordinate system respectively, E, N, U subscript is respectively the component on three directions.

(4.4) central controller described according to above-mentioned 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:

X · ( t ) = F ( t ) X ( t ) + G ( t ) W ( t ) - - - ( 14 )

Wherein,

X (t)=[φ eφ nφ uδ v eδ v nδ v uδ L δ λ δ h ε bxε byε bzbxbybz] tfor the state vector of system, wherein, subscript E, N, U represent three directions of sky, northeast geographic coordinate system respectively, φ e, φ n, φ ufor the error angle of strapdown inertial navitation system (SINS), δ v e, δ v n, δ v ufor velocity error, δ L, δ λ, δ h are site error, ε bx, ε by, ε bzgyrostatic random drift, ▽ bx, ▽ by, ▽ bzthe error of zero of accelerometer; W (t)=[ω gxω gyω gzω axω ayω az] tfor systematic procedure white noise vector, wherein, ω gx, ω gy, ω gzfor the white noise of gyro, ω ax, ω ay, ω azfor 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 above-mentioned 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)

1. for a strapdown inertial navitation system (SINS) for agricultural machinery, it is characterized in that, 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.
2. realize a control method for agricultural machinery based on system according to claim 1, it is characterized in that, 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.
3. the control method realizing agricultural machinery according to claim 2, it is characterized in that, 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.
4. the control method realizing agricultural machinery according to claim 3, is characterized in that, described step (2.1) is specially:
Described attitude matrix computing module obtains attitude speed by following formula:
ω nb b = ω ib b - ω in b = ω ib b - C n b ω in n = ω ib b - C n b ( ω ie n + ω en n ) - - - ( 1 )
Wherein, the attitude matrix that Eulerian angle represent, namely
C n b = cos γ cos ψ + sin γ sin θ sin ψ - cos γ sin ψ + sin γ sin θ cos ψ - sin γ cos θ cos θ sin ψ cos θ cos ψ sin θ sin γ cos ψ - cos γ sin θ sin ψ - sin γ sin ψ - cos γ sin θ cos ψ cos γ cos θ - - - ( 2 )
for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and ω ib b = ω bx b ω by b ω bz b ; for earth rate, be known, for ω ie n = 0 ω e cos L ω e sin L ; for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ω en n = - v N R M + h ω e cos L + v E R N + h ω e sin L + v E R N + h tan L ; for attitude speed, and ω nb b = ω nb bx ω nb by ω nb bz , ψ is position angle, and θ is the angle of pitch, and γ is roll angle, v e, v n, v ufor the speed in sky, northeast, provided by velocity location computing module, R m, R nfor earth meridian ellipse and prime plane radius-of-curvature, R m≈ R (1-2e+3esin 2l), R n≈ R (1+esin 2l), wherein, R=m, e=1/298.257), L, λ, h are respectively latitude, longitude, highly, being intermediate variable, is the projection of conjunction angular velocity in carrier coordinate system of the angular velocity of the rotational-angular velocity of the earth earth relative to space flight coordinate system; ω eearth rotation closes angular velocity.
5. the control method realizing agricultural machinery according to claim 3, is characterized in that, 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:
Q · = ω ib b ( t ) + 1 2 Q × ω ib b ( t ) - - - ( 3 )
Described hypercomplex number upgrades the differential equation:
Q ( t ) = ( I cos Δθ 2 + [ Δθ ] sin Δθ 2 Δθ ) · Q ( 0 ) - - - ( 4 )
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, Δθ = ( θ 1 + θ 2 ) + 2 3 ( θ 1 × θ 2 ) , Q ( 0 ) = cos θ 0 2 cos γ 0 2 cos ψ 0 2 + sin θ 0 2 sin γ 0 2 sin ψ 0 2 sin θ 0 2 cos γ 0 2 cos ψ 0 2 + cos θ 0 2 sin γ 0 2 sin ψ 0 2 cos θ 0 2 sin γ 0 2 cos ψ 0 2 - sin θ 0 2 cos γ 0 2 sin ψ 0 2 cos θ 0 2 cos γ 0 2 sin ψ 0 2 - sin θ 0 2 sin γ 0 2 cos ψ 0 2 , I is unit matrix; Q is quaternionic vector; for hypercomplex number derivative; Δ t is the Data Update cycle; The result of the first attitude matrix is Q=q 0+ q 1i+q 2j+q 3k, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length, θ 1and θ 2be respectively the angle increment of gyroscope in the posture renewal cycle twice time-sampling at equal intervals; θ 0, γ 0, ψ 0be respectively the angle of pitch under original state, roll angle and course angle, the initial quaternary numerical value of Q (0) for utilizing above-mentioned initial attitude angle to calculate.
6. the control method realizing agricultural machinery according to claim 3, is characterized in that, 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:
Q = q 0 + q 1 i + q 2 j + q 3 k q 0 2 + q 1 2 + q 2 2 + q 3 2 - - - ( 5 )
Attitude matrix after described renewal is:
C b n = q 1 2 + q 0 2 - q 3 2 - q 2 2 2 ( q 1 q 2 - q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 2 2 - q 3 2 + q 0 2 - q 1 2 2 ( q 2 q 3 - q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 3 2 - q 2 2 - q 1 2 + q 0 2 - - - ( 6 )
Wherein, Q is quaternionic vector, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length, for carrier coordinate system is to the rotation matrix of navigational coordinate system.
7. the control method realizing agricultural machinery according to claim 2, it is characterized in that, 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
f E f N f U = C b n f bx f by f bz - - - ( 7 )
Wherein, f b = f bx f by f bz For the acceleration that acceleration transducer exports, f e, f n, f ube respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, be upgrade after attitude matrix be:
C b n = q 1 2 + q 0 2 - q 3 2 - q 2 2 2 ( q 1 q 2 - q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 2 2 - q 3 2 + q 0 2 - q 1 2 2 ( q 2 q 3 - q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 3 2 - q 2 2 - q 1 2 + q 0 2 , Wherein, Q is quaternionic vector, q 0, q 1, q 2, q 3for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length.
8. the control method realizing agricultural machinery according to claim 2, it is characterized in that, 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:
v · n = f n - ( ω en n + 2 ω ie n ) × v n + g - - - ( 8 )
Wherein, ω ie n = 0 ω ie cos L ω ie sin L , ω en n = - v N R M + h v E R N + h v E R N + h tan L , f n = f E f N f U , v · n = v · E v · N v · U , v n = v E v N v U , g = 0 0 - g
The matrix representation of formula (8) is:
v · E v · N v · U = f E f N f U - 0 - ( 2 ω iez n + ω enz n ) 2 ω iey n + ω eny n 2 ω iez n + ω enz n 0 - ( 2 ω iex n + ω enx n ) - ( 2 ω iey n + ω eny n ) 2 ω iex n + ω enx n 0 v E v N v U + 0 0 - g
Wherein, for earth rate, be known, for ω ie n = 0 ω e cos L ω e sin L ; for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ω en n = - v N R M + h ω e cos L + v E R N + h ω e sin L + v E R N + h tan L , L, λ, h are respectively latitude, longitude, highly; ψ is position angle, and θ is the angle of pitch, and γ is roll angle, v e, v n, v ufor the speed in the east of space flight coordinate system, north, sky, provided by velocity location computing module, R m, R nfor earth meridian ellipse and prime plane radius-of-curvature, R m≈ R (1-2e+3esin 2l), R n≈ R (1+esin 2l), wherein, R=m, e=1/298.257), f e, f n, f ube respectively and tie up to east, north, old name for the Arabian countries in the Middle East ratio force component upwards along geographic coordinate, ω eearth rotation closes angular velocity.
10. the control method realizing agricultural machinery according to claim 8, is characterized in that, 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:
λ ( k ) = λ ( k - 1 ) + v E ( k - 1 ) [ R M ( k - 1 ) + h ( k - 1 ) ] cos L ( k - 1 ) · T L ( k ) = L ( k - 1 ) + v N ( k - 1 ) R N ( k - 1 ) + h ( k - 1 ) · T h ( k ) = h ( k - 1 ) + v U ( K - 1 ) · T - - - ( 9 )
Wherein, L, λ, h are respectively the latitude of surface car, longitude, highly, v e, v n, v ufor 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.
11. control methods realizing agricultural machinery according to claim 2, it is characterized in that, navigation coordinate is " sky, northeast " geographic coordinate system, 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.
12. control methods realizing agricultural machinery according to claim 2, is characterized in that, 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):
φ · E = - δ v N R M + h + ( ω ie sin L + v E R N + h tan L ) φ N - ( ω ie cos L + v E R N + h ) φ U + ϵ E φ · N = δ v E R N + h - ω ie sin LδL - ( ω ie sin L + v E R N + h tan L ) φ E - v N R M + h φ U + ϵ N φ · U = δ v E R N + h tan L + ( ω ie cos L + v E R N + h sec 2 L ) δL + ( ω ie cos L + v E R N + h ) φ E + v N R M + h φ N + ϵ U - - - ( 10 )
δ v · E = f N φ U - f U φ N + ( v N R M + h tan L - v U R M + h ) δ v E + ( 2 ω ie sin L + v E R N + h tan L ) δ v N - ( 2 ω ie cos L + v E R N + h ) δ v U + ( 2 ω ie cos L v N + v E v N R N + h sec 2 L + 2 ω ie sin L v U ) δL + ▿ E δ v · N = f U φ E - f E φ U - 2 ( ω ie sin L + v E R N + h tgL ) δ v E - v U R M + h δ v N - v N R M + h δ v U - ( 2 ω ie cos L + v E R N + h sec 2 L ) v E δL + ▿ N δ v · U = f E φ N - f N φ E + 2 ( ω ie cos L + v E R N + h ) δ v E - 2 ω ie sin L v E δL + 2 v N R M + h δ v N + ▿ U - - - ( 11 )
δ L · = δ v N R M + h δ λ · = δ v E R N + h sec L + v E sec L R N + h tan LδL δ h · = δ v U - - - ( 12 )
ε=ε brg(13)
▽=▽ b+▽ aa
Wherein, φ e, φ n, φ ufor three attitude angle in sky, northeast navigational coordinate system, v e, v n, v ufor the speed in sky, northeast, L, λ, h are respectively the latitude of surface car, longitude, highly, f e, f n, f ube 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 nfor earth meridian ellipse and prime plane radius-of-curvature, (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, ε gbe respectively constant value drift, single order Markov process and white Gaussian noise, Δ is accelerometer total error, Δ b, Δ a, ω abe respectively constant value drift, single order Markov process and white Gaussian noise; ω iefor earth rate, be known, δ v e, δ v n, δ v ufor sky, northeast velocity error under navigational coordinate system, δ L, δ λ, δ h are site error, ϵ = ϵ E ϵ N ϵ U With ▿ = ▿ E ▿ N ▿ U The total error in gyroscope and accelerometer sky, northeast under navigational coordinate system respectively, E, N, U subscript is respectively the component on three directions.
(4.4) central controller described according to above-mentioned 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.
13. control methods realizing agricultural machinery according to claim 12, is characterized in that, 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:
X · ( t ) = F ( t ) X ( t ) + G ( t ) W ( t ) - - - ( 14 )
Wherein,
X (t)=[φ eφ nφ uδ v eδ v nδ v uδ L δ λ δ h ε bxε byε bzbxbybz] tfor the state vector of system, wherein, subscript E, N, U represent three directions of sky, northeast geographic coordinate system respectively, φ e, φ n, φ ufor the error angle of strapdown inertial navitation system (SINS), δ v e, δ v n, δ v ufor velocity error, δ L, δ λ, δ h are site error, ε bx, ε by, ε bzgyrostatic random drift, ▽ bx, ▽ by, ▽ bzthe error of zero of accelerometer; W (t)=[ω gxω gyω gzω axω ayω az] tfor systematic procedure white noise vector, wherein, ω gx, ω gy, ω gzfor the white noise of gyro, ω ax, ω ay, ω azfor the white noise of accelerometer; F (t) is system state matrix, and G (t) is system noise propogator matrix.
CN201510134478.8A 2015-03-26 2015-03-26 Strapdown inertial navitation system and control method for agricultural machines CN104697526A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510134478.8A CN104697526A (en) 2015-03-26 2015-03-26 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) 2015-03-26 2015-03-26 Strapdown inertial navitation system and control method for agricultural machines

Publications (1)

Publication Number Publication Date
CN104697526A true CN104697526A (en) 2015-06-10

Family

ID=53344895

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510134478.8A CN104697526A (en) 2015-03-26 2015-03-26 Strapdown inertial navitation system and control method for agricultural machines

Country Status (1)

Country Link
CN (1) CN104697526A (en)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105203098A (en) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor
CN105509740A (en) * 2015-12-31 2016-04-20 广州中海达卫星导航技术股份有限公司 Measuring method and module for attitude of agriculture machinery vehicle
CN105607093A (en) * 2015-12-20 2016-05-25 上海华测导航技术股份有限公司 Integrated navigation system and method for acquiring navigation coordinate
CN106441275A (en) * 2016-09-23 2017-02-22 深圳大学 Method and device for updating planned path of robot
CN106595637A (en) * 2016-12-21 2017-04-26 上海华测导航技术股份有限公司 Visual navigation method for agricultural machine
CN106931965A (en) * 2015-12-31 2017-07-07 中国移动通信集团吉林有限公司 A kind of method and device for determining terminal attitude
CN107421494A (en) * 2017-04-01 2017-12-01 深圳市元征科技股份有限公司 Vehicle attitude detection method and device
CN107607113A (en) * 2017-08-02 2018-01-19 华南农业大学 A kind of two axle posture inclination angle measurement methods
CN109781096A (en) * 2017-11-15 2019-05-21 洛阳中科晶上智能装备科技有限公司 A kind of integrated navigation and location system and method for intelligent agricultural machinery
CN109927726A (en) * 2019-03-13 2019-06-25 深兰科技(上海)有限公司 A kind of method and apparatus adjusting target vehicle motion state
CN109987097A (en) * 2019-03-13 2019-07-09 深兰科技(上海)有限公司 A kind of method and apparatus adjusting target vehicle motion state

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (en) * 2008-01-18 2009-04-22 南京航空航天大学 Navigating and steady aiming method of navigation / steady aiming integrated system
CN103712623A (en) * 2014-01-20 2014-04-09 东南大学 Optical-fiber gyroscope inertial navigation system attitude optimization method based on angular rate input

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (en) * 2008-01-18 2009-04-22 南京航空航天大学 Navigating and steady aiming method of navigation / steady aiming integrated system
CN103712623A (en) * 2014-01-20 2014-04-09 东南大学 Optical-fiber gyroscope inertial navigation system attitude optimization method based on angular rate input

Non-Patent Citations (4)

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

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105203098A (en) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor
CN105607093A (en) * 2015-12-20 2016-05-25 上海华测导航技术股份有限公司 Integrated navigation system and method for acquiring navigation coordinate
CN105607093B (en) * 2015-12-20 2018-05-08 上海华测导航技术股份有限公司 A kind of integrated navigation system and the method for obtaining navigation coordinate
CN106931965A (en) * 2015-12-31 2017-07-07 中国移动通信集团吉林有限公司 A kind of method and device for determining terminal attitude
CN105509740A (en) * 2015-12-31 2016-04-20 广州中海达卫星导航技术股份有限公司 Measuring method and module for attitude of agriculture machinery vehicle
CN106441275A (en) * 2016-09-23 2017-02-22 深圳大学 Method and device for updating planned path of robot
CN106595637A (en) * 2016-12-21 2017-04-26 上海华测导航技术股份有限公司 Visual navigation method for agricultural machine
CN107421494B (en) * 2017-04-01 2019-12-10 深圳市元征科技股份有限公司 vehicle attitude detection method and device
CN107421494A (en) * 2017-04-01 2017-12-01 深圳市元征科技股份有限公司 Vehicle attitude detection method and device
CN107607113A (en) * 2017-08-02 2018-01-19 华南农业大学 A kind of two axle posture inclination angle measurement methods
CN107607113B (en) * 2017-08-02 2020-03-17 华南农业大学 Method for measuring inclination angles of two-axis attitude
CN109781096A (en) * 2017-11-15 2019-05-21 洛阳中科晶上智能装备科技有限公司 A kind of integrated navigation and location system and method for intelligent agricultural machinery
CN109927726A (en) * 2019-03-13 2019-06-25 深兰科技(上海)有限公司 A kind of method and apparatus adjusting target vehicle motion state
CN109987097A (en) * 2019-03-13 2019-07-09 深兰科技(上海)有限公司 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 low-cost MEMS IMU sensors
US8442703B2 (en) Turning-stabilized 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 single-shaft 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) Vehicle-mounted SINS/GPS combined navigation system performance reinforcement method
CN101907714B (en) GPS aided positioning system and method based on multi-sensor 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 nine-axis mems sensor
CN103389092B (en) A kind of kite balloon airship attitude measuring and measuring method
CN100541135C (en) Fiber-optic gyroscope strapdown inertial navigation system initial attitude based on Doppler is determined method
CN102980577B (en) Micro-strapdown altitude heading reference system and working method thereof
CN103776446B (en) A kind of pedestrian's independent navigation computation based on double MEMS-IMU
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
CN101082493B (en) Combined positioning method of agricultural machines navigation
Sun et al. MEMS-based 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