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

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:

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)---\left(1\right)$

Wherein, the attitude matrix that Eulerian angle represent, namely

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]---\left(2\right)$

for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and ${\mathrm{\ω}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}^b\\ {\mathrm{\ω}}_{\mathrm{by}}^b\\ {\mathrm{\ω}}_{\mathrm{bz}}^b\end{array}\right];$ for earth rate, be known, for ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right];$ for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_e\cos L+\frac{v_E}{R_N+h}\\ {\mathrm{\ω}}_e\sin L+\frac{v_E}{R_N+h}\tan L\end{array}\right];$ for attitude speed, and ${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right],$ ψ 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 ²l), R _n≈ R (1+esin ²l), 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:

$\stackrel{\·}{Q}={\mathrm{\ω}}_{\mathrm{ib}}^b\left(t\right)+\frac{1}{2}Q\×{\mathrm{\ω}}_{\mathrm{ib}}^b\left(t\right)---\left(3\right)$

Described hypercomplex number upgrades the differential equation:

$Q\left(t\right)=(I\cos \frac{\mathrm{\Δ\θ}}{2}+[\mathrm{\Δ\θ}\left]\sin \frac{\frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}\right)\·Q\left(0\right)---\left(4\right)$

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, $\mathrm{\Δ}\mathrm{\θ}=({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+\frac{2}{3}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_2),$ $Q\left(0\right)=\left[\begin{array}{c}\cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}-\sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}-\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}\end{array}\right],$ 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 ₀+ q ₁i+q ₂j+q ₃k, q ₀, q ₁, q ₂, q ₃for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length, θ ₁and θ ₂be respectively the angle increment of gyroscope in the posture renewal cycle twice time-sampling at equal intervals; θ ₀, γ ₀, ψ ₀be 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=\frac{q_0+q_{1}i+q_{2}j+q_{3}k}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}---\left(5\right)$

Attitude matrix after described renewal is:

$C_b^n=\left[\begin{array}{ccc}{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\end{array}\right]---\left(6\right)$

Wherein, Q is quaternionic vector, q ₀, q ₁, q ₂, q ₃for 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

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=C_b^n\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]---\left(7\right)$

Wherein, $f_b=\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]$ 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=\left[\begin{array}{ccc}{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\end{array}\right],$ Wherein, Q is quaternionic vector, q ₀, q ₁, q ₂, q ₃for 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:

${\stackrel{\·}{v}}^n=f^n-({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)\×v^n+g---\left(8\right)$

Wherein, ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ \frac{v_E}{R_N+h}\\ \frac{v_E}{R_N+h}\tan L\end{array}\right],$ $f^n=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right],$ ${\stackrel{\·}{v}}^n=\left[\begin{array}{c}{\stackrel{\·}{v}}_E\\ {\stackrel{\·}{v}}_N\\ {\stackrel{\·}{v}}_U\end{array}\right],$ $v^n=\left[\begin{array}{c}{v}_E\\ v_N\\ v_U\end{array}\right],$ $g=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$ The matrix representation of formula (8) is:

$\left[\begin{array}{c}{\stackrel{\·}{v}}_E\\ {\stackrel{\·}{v}}_N\\ {\stackrel{\·}{v}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]-\left[\begin{array}{ccc}0& -(2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n)& 2{\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n\\ 2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n& 0& -(2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enx}}^n)\\ -(2{\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n)& 2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enx}}^n& 0\end{array}\right]\left[\begin{array}{c}{v}_E\\ v_N\\ v_U\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$

Wherein, for earth rate, be known, for ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right];$ for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_e\cos L+\frac{v_E}{R_N+h}\\ {\mathrm{\ω}}_e\sin L+\frac{v_E}{R_N+h}\tan L\end{array}\right],$ 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 ²l), R _n≈ R (1+esin ²l), 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:

$\left\{\begin{array}{c}\mathrm{\λ}\left(k\right)=\mathrm{\λ}(k-1)+\frac{v_E(k-1)}{[R_M(k-1)+h(k-1)]\cos L(k-1)}\·T\\ L\left(k\right)=L(k-1)+\frac{v_N(k-1)}{R_N(k-1)+h(k-1)}\·T\\ h\left(k\right)=h(k-1)+v_U(K-1)\·T\end{array}\right.---\left(9\right)$

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

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_E=-\frac{\mathrm{\δ}{v}_N}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_N-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_U+{\mathrm{\ϵ}}_E\\ {\stackrel{\·}{\mathrm{\φ}}}_N=\frac{\mathrm{\δ}{v}_E}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_E-\frac{v_N}{R_M+h}{\mathrm{\φ}}_U+{\mathrm{\ϵ}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_U=\frac{\mathrm{\δ}{v}_E}{R_N+h}\tan L+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_E+\frac{v_N}{R_M+h}{\mathrm{\φ}}_N+{\mathrm{\ϵ}}_U\end{array}\right.---\left(10\right)$

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_E=f_N{\mathrm{\φ}}_U-f_U{\mathrm{\φ}}_N+(\frac{v_N}{R_M+h}\tan L-\frac{v_U}{R_M+h})\mathrm{\δ}{v}_E+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L)\mathrm{\δ}{v}_N\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_U+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{v}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_U)\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{v}}_N=f_U{\mathrm{\φ}}_E-f_E{\mathrm{\φ}}_U-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\mathrm{tgL})\mathrm{\δ}{v}_E-\frac{v_U}{R_M+h}\mathrm{\δ}{v}_N-\frac{v_N}{R_M+h}\mathrm{\δ}{v}_U\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E\mathrm{\δL}+{\▿}_N\\ \mathrm{\δ}{\stackrel{\·}{v}}_U=f_E{\mathrm{\φ}}_N-f_N{\mathrm{\φ}}_E+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_E-2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_E\mathrm{\δL}+2\frac{v_N}{R_M+h}\mathrm{\δ}{v}_N+{\▿}_U\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{\mathrm{\δ}{v}_N}{R_M+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\mathrm{\δ}{v}_E}{R_N+h}\sec L+\frac{v_E\sec L}{R_N+h}\tan \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_U\end{array}\right.---\left(12\right)$

ε＝ε _b+ε _r+ω _g(13)

▽＝▽ _b+▽ _a+ω _a

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, $\mathrm{\ϵ}=\left[\begin{array}{c}{\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_N\\ {\mathrm{\ϵ}}_U\end{array}\right]$ With $\▿=\left[\begin{array}{c}{\▿}_E\\ {\▿}_N\\ {\▿}_U\end{array}\right]$ 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:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(14\right)$

Wherein,

X (t)=[φ _eφ _nφ _uδ v _eδ v _nδ v _uδ L δ λ δ h ε _bxε _byε _bz▽ _bx▽ _by▽ _bz] ^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=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

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 ${\mathrm{\ω}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right]}^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=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

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: $\stackrel{\·}{Q}=\frac{1}{2}\mathrm{\ΩQ},$ In formula, $\mathrm{\Ω}=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],$ $Q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right],$ 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:

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)---\left(1\right)$

Wherein, the attitude matrix that Eulerian angle represent, namely

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]---\left(2\right)$

for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and ${\mathrm{\ω}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}^b\\ {\mathrm{\ω}}_{\mathrm{by}}^b\\ {\mathrm{\ω}}_{\mathrm{bz}}^b\end{array}\right];$ for earth rate, be known, for ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right];$ for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_e\cos L+\frac{v_E}{R_N+h}\\ {\mathrm{\ω}}_e\sin L+\frac{v_E}{R_N+h}\tan L\end{array}\right];$ for attitude speed, and ${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right],$ ψ 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 ²l), R _n≈ R (1+esin ²l), 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,

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]-C_n^b(\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right]+\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ \frac{v_E}{R_N+h}\\ \frac{v_E}{R_N+h}\tan L\end{array}\right])=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]-C_b^n\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_e\cos L+\frac{v_E}{R_N+h}\\ {\mathrm{\ω}}_e\sin L+\frac{v_E}{R_N+h}\tan L\end{array}\right]$

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:

$\stackrel{\·}{Q}={\mathrm{\ω}}_{\mathrm{ib}}^b\left(t\right)+\frac{1}{2}Q\×{\mathrm{\ω}}_{\mathrm{ib}}^b\left(t\right)---\left(3\right)$

Described hypercomplex number upgrades the differential equation:

$Q\left(t\right)=(I\cos \frac{\mathrm{\Δ\θ}}{2}+[\mathrm{\Δ\θ}\left]\sin \frac{\frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}\right)\·Q\left(0\right)---\left(4\right)$

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, $\mathrm{\Δ}\mathrm{\θ}=({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+\frac{2}{3}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_2),$ $Q\left(0\right)=\left[\begin{array}{c}\cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}-\sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}-\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}\end{array}\right],$ 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 ₀+ q ₁i+q ₂j+q ₃k, q ₀, q ₁, q ₂, q ₃for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length, θ ₁and θ ₂be respectively the angle increment of gyroscope in the posture renewal cycle twice time-sampling at equal intervals; θ ₀, γ ₀, ψ ₀be 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=\frac{q_0+q_{1}i+q_{2}j+q_{3}k}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}---\left(5\right)$

Attitude matrix after described renewal is:

$C_b^n=\left[\begin{array}{ccc}{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\end{array}\right]---\left(6\right)$

Wherein, Q is quaternionic vector, q ₀, q ₁, q ₂, q ₃for 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:

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=C_b^n\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]---\left(7\right)$

Wherein, $f_b=\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]$ 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=\left[\begin{array}{ccc}{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\end{array}\right],$ Wherein, Q is quaternionic vector, q ₀, q ₁, q ₂, q ₃for 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:

${\stackrel{\·}{v}}^n=f^n-({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)\×v^n+g---\left(8\right)$

Wherein, ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ \frac{v_E}{R_N+h}\\ \frac{v_E}{R_N+h}\tan L\end{array}\right],$ $f^n=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right],$ ${\stackrel{\·}{v}}^n=\left[\begin{array}{c}{\stackrel{\·}{v}}_E\\ {\stackrel{\·}{v}}_N\\ {\stackrel{\·}{v}}_U\end{array}\right],$ $v^n=\left[\begin{array}{c}{v}_E\\ v_N\\ v_U\end{array}\right],$ $g=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$ The matrix representation of formula (8) is:

$\left[\begin{array}{c}{\stackrel{\·}{v}}_E\\ {\stackrel{\·}{v}}_N\\ {\stackrel{\·}{v}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]-\left[\begin{array}{ccc}0& -(2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n)& 2{\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n\\ 2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n& 0& -(2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enx}}^n)\\ -(2{\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n)& 2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enx}}^n& 0\end{array}\right]\left[\begin{array}{c}{v}_E\\ v_N\\ v_U\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$

Wherein, for earth rate, be known, for ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right];$ for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_e\cos L+\frac{v_E}{R_N+h}\\ {\mathrm{\ω}}_e\sin L+\frac{v_E}{R_N+h}\tan L\end{array}\right],$ 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 ²l), R _n≈ R (1+esin ²l), 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 ₁and K ₂for 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:

$\left\{\begin{array}{c}\mathrm{\λ}\left(k\right)=\mathrm{\λ}(k-1)+\frac{v_E(k-1)}{[R_M(k-1)+h(k-1)]\cos L(k-1)}\·T\\ L\left(k\right)=L(k-1)+\frac{v_N(k-1)}{R_N(k-1)+h(k-1)}\·T\\ h\left(k\right)=h(k-1)+v_U(K-1)\·T\end{array}\right.---\left(9\right)$

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 ²l (k)], R _n(k)=R _e[1+fsin ²l (k)], in formula, R _efor earth radius, f is ovality.Gravity is approximately with the Changing Pattern of latitude and height:

G=g ₀[l+0.00527094sin ²(L)+0.0000232718sin ⁴(L)]-0.000003086h, in formula, g ₀=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=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}\\ -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right)$

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

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_E=-\frac{\mathrm{\δ}{v}_N}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_N-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_U+{\mathrm{\ϵ}}_E\\ {\stackrel{\·}{\mathrm{\φ}}}_N=\frac{\mathrm{\δ}{v}_E}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_E-\frac{v_N}{R_M+h}{\mathrm{\φ}}_U+{\mathrm{\ϵ}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_U=\frac{\mathrm{\δ}{v}_E}{R_N+h}\tan L+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_E+\frac{v_N}{R_M+h}{\mathrm{\φ}}_N+{\mathrm{\ϵ}}_U\end{array}\right.---\left(10\right)$

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_E=f_N{\mathrm{\φ}}_U-f_U{\mathrm{\φ}}_N+(\frac{v_N}{R_M+h}\tan L-\frac{v_U}{R_M+h})\mathrm{\δ}{v}_E+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L)\mathrm{\δ}{v}_N\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_U+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{v}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_U)\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{v}}_N=f_U{\mathrm{\φ}}_E-f_E{\mathrm{\φ}}_U-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\mathrm{tgL})\mathrm{\δ}{v}_E-\frac{v_U}{R_M+h}\mathrm{\δ}{v}_N-\frac{v_N}{R_M+h}\mathrm{\δ}{v}_U\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E\mathrm{\δL}+{\▿}_N\\ \mathrm{\δ}{\stackrel{\·}{v}}_U=f_E{\mathrm{\φ}}_N-f_N{\mathrm{\φ}}_E+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_E-2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_E\mathrm{\δL}+2\frac{v_N}{R_M+h}\mathrm{\δ}{v}_N+{\▿}_U\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{\mathrm{\δ}{v}_N}{R_M+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\mathrm{\δ}{v}_E}{R_N+h}\sec L+\frac{v_E\sec L}{R_N+h}\tan \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_U\end{array}\right.---\left(12\right)$

ε＝ε _b+ε _r+ω _g(13)

▽＝▽ _b+▽ _a+ω _a

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, $\mathrm{\ϵ}=\left[\begin{array}{c}{\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_N\\ {\mathrm{\ϵ}}_U\end{array}\right]$ With $\▿=\left[\begin{array}{c}{\▿}_E\\ {\▿}_N\\ {\▿}_U\end{array}\right]$ 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:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(14\right)$

Wherein,

X (t)=[φ _eφ _nφ _uδ v _eδ v _nδ v _uδ L δ λ δ h ε _bxε _byε _bz▽ _bx▽ _by▽ _bz] ^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:

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)---\left(1\right)$

Wherein, the attitude matrix that Eulerian angle represent, namely

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]---\left(2\right)$

for the angular velocity of the agricultural machinery described in the output of described gyro sensor, and ${\mathrm{\ω}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}^b\\ {\mathrm{\ω}}_{\mathrm{by}}^b\\ {\mathrm{\ω}}_{\mathrm{bz}}^b\end{array}\right];$ for earth rate, be known, for ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right];$ for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_e\cos L+\frac{v_E}{R_N+h}\\ {\mathrm{\ω}}_e\sin L+\frac{v_E}{R_N+h}\tan L\end{array}\right];$ for attitude speed, and ${\mathrm{\ω}}_{\mathrm{nb}}^b=\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right],$ ψ 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 ²l), R _n≈ R (1+esin ²l), 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:

$\stackrel{\·}{Q}={\mathrm{\ω}}_{\mathrm{ib}}^b\left(t\right)+\frac{1}{2}Q\×{\mathrm{\ω}}_{\mathrm{ib}}^b\left(t\right)---\left(3\right)$

Described hypercomplex number upgrades the differential equation:

$Q\left(t\right)=(I\cos \frac{\mathrm{\Δ\θ}}{2}+[\mathrm{\Δ\θ}\left]\sin \frac{\frac{\mathrm{\Δ\θ}}{2}}{\mathrm{\Δ\θ}}\right)\·Q\left(0\right)---\left(4\right)$

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, $\mathrm{\Δ\θ}=({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+\frac{2}{3}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_2),Q\left(0\right)=\left[\begin{array}{c}\cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}-\sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}-\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}\end{array}\right],$ 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 ₀+ q ₁i+q ₂j+q ₃k, q ₀, q ₁, q ₂, q ₃for forming the scalar of quaternionic vector, i, j, k are three-dimensional system of coordinate vector of unit length, θ ₁and θ ₂be respectively the angle increment of gyroscope in the posture renewal cycle twice time-sampling at equal intervals; θ ₀, γ ₀, ψ ₀be 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=\frac{q_0+q_{1}i+q_{2}j+q_{3}k}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}---\left(5\right)$

Attitude matrix after described renewal is:

$C_b^n=\left[\begin{array}{ccc}{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\end{array}\right]---\left(6\right)$

Wherein, Q is quaternionic vector, q ₀, q ₁, q ₂, q ₃for 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

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=C_b^n\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]---\left(7\right)$

Wherein, $f_b=\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]$ 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=\left[\begin{array}{ccc}{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\end{array}\right],$ Wherein, Q is quaternionic vector, q ₀, q ₁, q ₂, q ₃for 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:

${\stackrel{\·}{v}}^n=f^n-({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)\×v^n+g---\left(8\right)$

Wherein, ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right],{\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ \frac{v_E}{R_N+h}\\ \frac{v_E}{R_N+h}\tan L\end{array}\right],f^n=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right],{\stackrel{\·}{v}}^n=\left[\begin{array}{c}{\stackrel{\·}{v}}_E\\ {\stackrel{\·}{v}}_N\\ {\stackrel{\·}{v}}_U\end{array}\right],v^n=\left[\begin{array}{c}{v}_E\\ v_N\\ v_U\end{array}\right],g=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$

The matrix representation of formula (8) is:

$\left[\begin{array}{c}{\stackrel{\·}{v}}_E\\ {\stackrel{\·}{v}}_N\\ {\stackrel{\·}{v}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]-\left[\begin{array}{ccc}0& -(2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n)& 2{\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n\\ 2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n& 0& -(2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enx}}^n)\\ -(2{\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n)& 2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enx}}^n& 0\end{array}\right]\left[\begin{array}{c}{v}_E\\ v_N\\ v_U\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$

Wherein, for earth rate, be known, for ${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_e\cos L\\ {\mathrm{\ω}}_e\sin L\end{array}\right];$ for the angular velocity of the relative earth of navigational coordinate system, it can by instantaneous velocity try to achieve, and ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_e\cos L+\frac{v_E}{R_N+h}\\ {\mathrm{\ω}}_e\sin L+\frac{v_E}{R_N+h}\tan L\end{array}\right],$ 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 ²l), R _n≈ R (1+esin ²l), 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:

$\left\{\begin{array}{c}\mathrm{\λ}\left(k\right)=\mathrm{\λ}(k-1)+\frac{v_E(k-1)}{[R_M(k-1)+h(k-1)]\cos L(k-1)}\·T\\ L\left(k\right)=L(k-1)+\frac{v_N(k-1)}{R_N(k-1)+h(k-1)}\·T\\ h\left(k\right)=h(k-1)+v_U(K-1)\·T\end{array}\right.---\left(9\right)$

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

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_E=-\frac{\mathrm{\δ}{v}_N}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_N-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_U+{\mathrm{\ϵ}}_E\\ {\stackrel{\·}{\mathrm{\φ}}}_N=\frac{\mathrm{\δ}{v}_E}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_E-\frac{v_N}{R_M+h}{\mathrm{\φ}}_U+{\mathrm{\ϵ}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_U=\frac{\mathrm{\δ}{v}_E}{R_N+h}\tan L+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_E+\frac{v_N}{R_M+h}{\mathrm{\φ}}_N+{\mathrm{\ϵ}}_U\end{array}\right.---\left(10\right)$

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_E=f_N{\mathrm{\φ}}_U-f_U{\mathrm{\φ}}_N+(\frac{v_N}{R_M+h}\tan L-\frac{v_U}{R_M+h})\mathrm{\δ}{v}_E+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L)\mathrm{\δ}{v}_N\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_U+(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{v}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_U)\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{v}}_N=f_U{\mathrm{\φ}}_E-f_E{\mathrm{\φ}}_U-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\mathrm{tgL})\mathrm{\δ}{v}_E-\frac{v_U}{R_M+h}\mathrm{\δ}{v}_N-\frac{v_N}{R_M+h}\mathrm{\δ}{v}_U\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E\mathrm{\δL}+{\▿}_N\\ \mathrm{\δ}{\stackrel{\·}{v}}_U=f_E{\mathrm{\φ}}_N-f_N{\mathrm{\φ}}_E+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_E-2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_E\mathrm{\δL}+2\frac{v_N}{R_M+h}\mathrm{\δ}{v}_N+{\▿}_U\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{\mathrm{\δ}{v}_N}{R_M+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\mathrm{\δ}{v}_E}{R_N+h}\sec L+\frac{v_E\sec L}{R_N+h}\tan \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_U\end{array}\right.---\left(12\right)$

ε＝ε _b+ε _r+ω _g(13)

▽＝▽ _b+▽ _a+ω _a

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, $\mathrm{\ϵ}=\left[\begin{array}{c}{\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_N\\ {\mathrm{\ϵ}}_U\end{array}\right]$ With $\▿=\left[\begin{array}{c}{\▿}_E\\ {\▿}_N\\ {\▿}_U\end{array}\right]$ 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:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(14\right)$

Wherein,

X (t)=[φ _eφ _nφ _uδ v _eδ v _nδ v _uδ L δ λ δ h ε _bxε _byε _bz▽ _bx▽ _by▽ _bz] ^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.