CN104697526A

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

Info

Publication number: CN104697526A
Application number: CN201510134478.8A
Authority: CN
Inventors: 任强; 王杰俊; 沈雪峰; 戴文鼎
Original assignee: Shanghai Huace Navigation Technology Ltd
Current assignee: Shanghai Huace Navigation Technology Ltd
Priority date: 2015-03-26
Filing date: 2015-03-26
Publication date: 2015-06-10

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

Strapdown inertial navigation system for agricultural machinery and control method

Technical Field

The invention relates to the field of machine control, in particular to high-precision machine control, and specifically relates to a strapdown inertial navigation system and a control method for agricultural machinery.

Background

With the development of MEMS (Micro-Electro-Mechanical-System) sensors, navigation and control technologies and the further increase of agricultural supporting strength of China, precision agriculture is rapidly becoming a trend, and in the auxiliary driving control process of agricultural machinery, the attitude (including pitch angle, roll angle and course angle), speed and position information of a vehicle body can reflect the motion and position information of the vehicle body in real time, and the information can provide important data input for high-precision combined navigation and control algorithms.

In the auxiliary driving control process of the agricultural machine, the position information and the heading information of the vehicle body in the navigation coordinate system are the two most important parameters. The common positioning modes in the agricultural machinery auxiliary driving system are as follows: mechanical haptics, dead reckoning, machine vision, laser positioning, and multi-sensor information fusion (IMU + GPS). The multi-sensor information fusion fully utilizes a plurality of sensor resources, redundant or complementary information of a plurality of sensors in space or time is combined according to a certain criterion through reasonable domination and utilization of the sensors and observation information thereof, so as to obtain consistent explanation or description of a measured object.

The position information is mainly acquired according to high-precision GPS positioning information (such as RTK) in multi-sensor information fusion, but the GPS data output precision and the output frequency are in direct proportion to the price, and when obstacles are shielded or weather and the like exists, the GPS cannot ensure effective data output, and a set of high-frequency high-precision data acquisition and calculation system is needed to provide data filling for the blind area in real time.

Currently, Inertial Navigation systems are divided into PINS (platform Inertial Navigation system) and SINS (strapdown Inertial Navigation system), and compared with PINS, SINS uses an imu (Inertial measurement unit) sensor to establish a "mathematical platform" by calculation to replace PINS. The SINS is mostly used in an aircraft navigation control system, research and application in the field of agricultural machine control belong to a starting stage, application objects and environmental conditions of the SINS and the agricultural machine control are greatly different, and a method for realizing the strapdown inertial navigation in the aircraft control system is not suitable for agricultural machine control.

Disclosure of Invention

The invention aims to overcome the defects of the prior art and provides a strap-down inertial navigation system for agricultural machinery and a control method.

In order to achieve the above object, the strapdown inertial navigation system for agricultural machine and the control method according to the present invention are configured as follows:

the strapdown inertial navigation system for the agricultural machine is mainly characterized by comprising a six-axis inertial sensor and a central controller; the six-axis inertial sensor comprises acceleration sensors in three directions and a three-axis gyroscope sensor; the central controller comprises:

the coordinate transformation module is used for converting the acceleration of the carrier coordinate system of the agricultural machine sent by the acceleration sensor into the acceleration of an aerospace coordinate system;

the speed and position calculation module is used for calculating and obtaining the speed information and the position information of the agricultural machine according to the acceleration of the aerospace coordinate system output by the coordinate transformation module;

the attitude matrix calculation module is used for calculating and obtaining an updated attitude matrix according to the angular velocity value of the agricultural machine sent by the gyroscope sensor and the position angular velocity value calculated by the velocity position calculation module; and

and the attitude calculation module is used for obtaining the attitude angle of the agricultural machine according to the updated attitude matrix of the attitude matrix calculation module.

The invention also relates to a control method for realizing the agricultural machinery, which is mainly characterized by comprising the following steps:

(1) the acceleration sensor sends the acceleration of the agricultural machine to the coordinate transformation module in real time, and the gyroscope sensor sends the angular velocity of the agricultural machine to the attitude matrix calculation module in real time;

(2) the attitude matrix calculation module calculates to obtain an updated attitude matrix according to the position angular velocity value calculated by the velocity position calculation module and the received angular velocity value of the gyroscope sensor, and sends the updated attitude matrix to the coordinate transformation module;

(3) the coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machine, which is sent by the acceleration sensor, into the acceleration of an aerospace coordinate system according to the updated attitude matrix;

(4) the speed and position calculation module outputs the position information and the speed information of the agricultural machine according to the acceleration of the aerospace coordinate system output by the coordinate transformation module, and the attitude calculation module outputs the attitude angle of the agricultural machine according to the updated attitude matrix of the attitude matrix calculation module;

(5) and the control center of the agricultural machine controls the agricultural machine according to the position information, the speed information and the attitude angle.

Further, the navigation coordinate system is a geographical coordinate system of northeast, the attitude matrix calculation module calculates an updated attitude matrix according to the position angular velocity value calculated by the velocity position calculation module and the received angular velocity value of the gyroscope sensor, and the method specifically comprises the following steps:

(2.1) the attitude matrix calculation module calculates an attitude rate according to the position angular velocity value calculated by the velocity position calculation module and the received angular velocity value of the gyro sensor;

(2.2) the attitude matrix calculation module solving a quaternion differential equation according to the attitude rate to obtain a first attitude matrix;

and (2.3) normalizing the first attitude matrix by the attitude matrix calculation module to obtain an updated attitude matrix.

Further, the step (2.1) is specifically:

the attitude matrix calculation module obtains an attitude rate through the following formula:

wherein,is an Euler angle representationThe attitude matrix of, i.e.

An angular velocity of the agricultural machine output for the gyro sensor, and

is the angular velocity of the earth, is known as

To navigate the angular velocity of the coordinate system relative to the earth, it may be determined from the instantaneous velocityIs obtained, and

is the attitude rate, and

psi is azimuth angle, theta is pitch angle, gamma is roll angle, v_E、v_N、v_UFor northeast speed, provided by a speed position calculation module, R_M、R_NRadius of curvature R of meridian plane and unitary plane of earth_M≈R(1-2e+3esin²L)，R_N≈R(1+esin²L), where R ═ m and e ═ 1/298.257), L, λ and h are latitude, longitude and altitude, respectively,the intermediate variable is the projection of the total angular velocity of the earth rotation and the angular velocity of the space coordinate system relative to the earth in a carrier coordinate system; omega_eThe earth's rotation makes an angular velocity.

Further, the step (2.2) specifically comprises the following steps:

the attitude matrix calculation module obtains a quaternion updating differential equation according to the attitude rate and the equivalent rotation vector algorithm to obtain a first attitude matrix, wherein the quaternion updating differential equation is as follows:

the quaternion updating differential equation is as follows:

wherein,is the angular velocity output by the gyro sensor in an attitude period, andt is a time scale in which the angle is increasedAccording to the two-subsample algorithm with equivalent rotation vectors,

i is an identity matrix; q is a quaternion vector;is the quaternion derivative; Δ t is a data update period; the result of the first attitude matrix is Q ═ Q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃Is a scalar quantity forming a quaternion vector, i, j, k are three-dimensional coordinate system unit vectors, theta₁And theta₂Respectively carrying out angle increment of twice equal-interval time sampling on the gyroscope in the attitude updating period;θ₀、γ₀、ψ₀the pitch angle, the roll angle and the course angle in the initial state are respectively, and Q (0) is an initial quaternion value calculated by using the initial attitude angle.

Further, the attitude matrix calculation module normalizes the first attitude matrix to obtain an updated attitude matrix, specifically:

the attitude matrix calculation module normalizes the first attitude matrix according to the following formula to obtain an updated attitude matrix:

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}}} - - - (5)

the updated attitude matrix is as follows:

C_{b}^{n} = [\begin{matrix} 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{matrix}] - - - (6)

wherein Q is a quaternion vector, Q₀、q₁、q₂、q₃I, j, k are unit vectors of a three-dimensional coordinate system,a rotation matrix from the carrier coordinate system to the navigation coordinate system.

Furthermore, the navigation coordinate system is a geographical coordinate system of northeast, the coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machine, which is sent by the acceleration sensor, into an acceleration of an aerospace coordinate system according to the updated attitude matrix, and the method specifically comprises the following steps:

the coordinate transformation module converts the acceleration of the agricultural machine carrier coordinate system sent by the acceleration sensor into the acceleration of an aerospace coordinate system according to the updated attitude matrix and through the following formula

[\begin{matrix} f_{E} \\ f_{N} \\ f_{U} \end{matrix}] = C_{b}^{n} [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}] - - - (7)

Wherein,

f_{b} = [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}]

acceleration output from an acceleration sensor, f_E、f_N、f_URespectively are the specific force components along the geographic coordinate system in the east, north and sky directions,the updated attitude matrix is:

C_{b}^{n} = [\begin{matrix} 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{matrix}],

wherein Q is a quaternion vector, Q₀、q₁、q₂、q₃I, j, k are three-dimensional coordinate system unit vectors, which are scalars constituting a quaternion vector.

Furthermore, the navigation coordinate system is a geographical coordinate system of northeast, the speed and position calculation module outputs the position information and the speed information of the agricultural machine, and the method specifically comprises the following steps:

(4.1) the speed position calculation module integrates the acceleration output by the coordinate transformation module to obtain the speed information of the agricultural machine;

(4.2) the speed and position calculation module integrates the speed to obtain the position information of the agricultural machine.

Still further, the step (4.1) is specifically:

the speed position calculation module integrates the acceleration output by the coordinate transformation module according to the following formula to obtain the speed information of the agricultural machine:

wherein,

f^{n} = [\begin{matrix} f_{E} \\ f_{N} \\ f_{U} \end{matrix}],

v^{n} = [\begin{matrix} v_{E} \\ v_{N} \\ v_{U} \end{matrix}],

g = [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}]

the matrix of equation (8) is represented as:

wherein,is the angular velocity of the earth, is known as

l, lambda and h are respectively latitude, longitude and altitude; psi is azimuth angle, theta is pitch angle, gamma is roll angle, v_E、v_N、v_UThe speed of east, north and sky of the space coordinate system is provided by a speed position calculation module, R_M、R_NRadius of curvature R of meridian plane and unitary plane of earth_M≈R(1-2e+3esin²L)，R_N≈R(1+esin²L), where R ═ m, e ═ 1/298.257), f_E、f_N、f_UAre respectively sitting along the geographyThe scale is the specific force component in the east, north and sky directions, omega_eThe earth's rotation makes an angular velocity.

Still further, the step (4.2) is specifically:

the speed and position calculation module integrates the speed through the following formula to obtain the position information of the agricultural machine:

wherein L, lambda and h are respectively latitude, longitude and altitude of the ground vehicle, v_E、v_N、v_UThe speed of east, north and sky of the space coordinate system is provided by a speed position calculation module, k is a sampling point, and T is a sampling period.

Furthermore, the navigation coordinate system is a geographical coordinate system of northeast, the attitude calculation module outputs the attitude angle of the agricultural machine according to the updated attitude matrix of the attitude matrix calculation module, and the method specifically comprises the following steps:

and the attitude calculation module extracts the attitude angle of the agricultural machine comprising a pitch angle, a roll angle and a course angle from the updated attitude matrix.

Furthermore, the navigation coordinate system is a geographical coordinate system of northeast, and the following steps are further included between step (4) and step (5):

(4.3) the central controller determines that the system angle error is calculated according to the following equation (10), the velocity error is calculated according to the following equation (11), the position error is calculated according to the following equation (12), and the inertial instrument error is calculated according to the following equation (13):

＝_b+_r+ω_g (13)

▽＝▽_b+▽_a+ω_a

wherein phi is_E，φ_N，φ_UFor navigating three attitude angles, v, in a coordinate system in the northeast_E、v_N、v_UThe speed in northeast, L, lambda and h are respectively latitude, longitude and altitude of the ground vehicle, f_E、f_N、f_UAre the specific force components in the east, north and sky directions along the geographical coordinate system, R_M、R_NThe curvature radius of meridian plane and unitary plane of earth,(X is phi, v, L, lambda, h) is the corresponding derivative,(Y is v, L, lambda, h) is the error of its corresponding derivative, which is the total error of the gyroscope,_b、_r、_grespectively, constant drift, first order Markov process and white Gaussian noise, wherein delta is the total error of the accelerometer, and delta is_b、Δ_a、ω_aRespectively constant drift, first order Markov process and white Gaussian noise; omega_ieIs the angular velocity of the earth, v is known_E、v_N、v_UIs the speed error of northeast in navigation coordinate system, L, lambda and h are the position errors,

and

the total error of the gyroscope and accelerometer in the northeast of the navigation coordinate system, respectively, is denoted by the E, N, U subscripts as the components in the three directions, respectively.

And (4.4) the central controller performs error compensation on the position information, the speed information and the attitude angle according to the calculated system angle error, the calculated speed error, the calculated position error and the calculated inertial instrument error and according to a Kalman filtering algorithm.

Further, the performing error compensation on the position information, the speed information and the attitude angle according to a kalman filtering algorithm specifically comprises:

the position information, the speed information and the attitude angle are compensated according to a Kalman filtering algorithm to obtain a fifteen-dimensional state equation as follows:

wherein,

X(t)＝[φ_E φ_N φ_U v_E v_N v_U L λ h _bx _by _bz ▽_bx ▽_by ▽_bz]^Tis a state vector of the system in which,the subscripts E, N, U denote the three directions, φ, of the northeast geographic coordinate system, respectively_E、φ_N、φ_UIs an error angle, v, of a strapdown inertial navigation system_E、v_N、v_UIs a speed error, L, lambda and h are position errors,_bx、_by、_bzrandom drift of gyroscope +_bx、▽_by、▽_bzZero error of the accelerometer; w (t) ═ ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^TIs a system process white noise vector, where ω is_gx、ω_gy、ω_gzWhite noise, omega, of the gyro_ax、ω_ay、ω_azWhite noise for an accelerometer; f (t) is the system state matrix, and G (t) is the system noise propagation matrix.

Compared with the prior art, the strapdown inertial navigation system for agricultural machinery and the control method have the following beneficial effects:

(1) the error compensation and correction algorithm adopted by the invention greatly reduces the algorithm error of the strapdown inertial navigation system and the interference of earth rotation and the like;

(2) the algorithm of the six-axis inertial sensor and the strapdown inertial navigation system is adopted, so that the strapdown inertial navigation system for the agricultural machine has high performance parameters, the error of a course angle output by the device outside a tractor test chamber is less than 0.1 degrees, the error of a pitch angle and a rolling angle is less than 0.01 degrees, the position information output by the strapdown inertial navigation system for the agricultural machine is matched with a GPS (global position system) to realize that the combined navigation position error is in the cm level, the data output frequency reaches 50HZ, and the requirement of an auxiliary driving control system of the agricultural machine is met;

(3) the invention adopts six-axis inertial sensors which comprise acceleration sensors in three directions and three-axis gyroscope sensors, has small volume, light weight and high cost performance, and is convenient to integrate into an agricultural machinery auxiliary driving control system due to modular design;

(4) the strapdown inertial navigation system for the agricultural machinery has the advantages of stable operation, rich output motion information and the like, and particularly meets the requirements of ground vehicle auxiliary driving control systems of the agricultural machinery and the like.

Drawings

Fig. 1 is a schematic structural diagram of a strapdown inertial navigation system for agricultural machinery according to the present invention.

FIG. 2 is a flow chart illustrating the steps of the control method for the agricultural machine based on the strapdown inertial navigation system according to the present invention.

Detailed Description

In order to more clearly describe the technical contents of the present invention, the following further description is given in conjunction with specific embodiments.

The invention relates to a strapdown inertial navigation system for agricultural machinery and a control method thereof, wherein a six-axis inertial sensor with higher precision is adopted, the six-axis inertial sensor comprises acceleration sensors in three directions and three-axis gyroscope sensors, the acceleration sensors acquire the acceleration and the angular velocity of the motion of an object in real time, the acceleration and the angular velocity can be calculated through integration of the acceleration, the position information can be calculated through second integration, the current attitude angle (a pitch angle, a roll angle and a course angle) of a vehicle body can be calculated through integration of the angular velocity, and then the attitude is converted into an attitude matrix, so that the conversion of a carrier coordinate system and a navigation coordinate system is realized, and the attitude matrix plays a role of a mathematical platform.

In the implementation of the sins (strapdown Inertial Navigation system) algorithm, the attitude matrix is particularly important, and the attitude of the agricultural machine constantly changes due to the movement of the agricultural machine at any moment, i.e., the attitude matrix is also constantly recalculated and updated. The commonly used attitude updating algorithm comprises an Euler angle, a direction cosine and a quaternion, the quaternion has no singular point compared with the Euler angle algorithm, and the calculation amount is small compared with the direction cosine, so that the method is very suitable for being used in embedded products. When the attitude matrix is calculated, the angular increment quaternion attitude updating algorithm is adopted, but the algorithm has the defect that the rotation irreplaceable error is generated due to the rotation of an indefinite axis when the angular increment is used for solving the differential equation, so that the error can be corrected by adopting an equivalent rotation vector method to solve the differential equation.

Aiming at the error interferences such as earth rotation, cone motion effect, rowing effect, scroll effect and the like, the invention adopts a plurality of compensation and correction algorithms to process the errors.

Referring to fig. 1, a schematic structural diagram of a strapdown inertial navigation system for agricultural machinery according to the present invention is shown, wherein the system includes six-axis inertial sensors and a central controller; the six-axis inertial sensor comprises acceleration sensors in three directions and a three-axis gyroscope sensor; the central controller comprises:

Fig. 2 is a flowchart illustrating steps of a control method for an agricultural machine based on a strapdown inertial navigation system according to the present invention. Wherein, the method comprises the following steps:

Firstly, calculating a posture matrix, wherein the posture matrix refers to a transformation matrix from a navigation coordinate system (n system) to a carrier coordinate system (b system), and when a geographical coordinate system of 'northeast sky' is adopted as the navigation coordinate system, the posture matrix is as follows:

where ψ is the azimuth angle (heading angle), θ is the pitch angle, and γ is the roll angle (roll angle), these three angles are referred to as the attitude angle of the carrier (provided by the initialized quaternion, and the attitude matrix thereafter is provided by the quaternion calculation).

When the attitude of the agricultural machine fixedly connected with the six-axis inertial sensor changes, the gyroscope sensor in the six-axis inertial sensor can sense the corresponding angular rate and attitude matrixWith consequent change, the differential equation is:in the formula,is angular velocity

An antisymmetric array is formed; x, y, z are defined as the three directions of the front right and the top right.

The instant correction of the attitude matrix of the strapdown inertial navigation system is to give the attitude matrix in real time, which is a key task of strapdown inertial navigation and is completed by a certain algorithm. Because the quaternion arithmetic method has small calculated amount and small storage capacity, the orthogonality of the attitude matrix can be ensured only by carrying out simple quaternion normalization processing. The unit quaternion can be described in the form:

Q = q_{0} + q_{1} i + q_{2} j + q_{3} k = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}]

in strapdown navigation, a carrier system to navigation system conversion matrix is required, and the following quaternion motion equation is solved:

in the formula,

Q = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}],

q is the rotational quaternion from the carrier system to the navigation system.

The navigation coordinate system is a geographical coordinate system of northeast, the attitude matrix calculation module calculates and obtains an updated attitude matrix according to the position angular velocity value calculated by the velocity position calculation module and the received angular velocity value of the gyroscope sensor, and the method specifically comprises the following steps:

Wherein, in a preferred real-time mode, the step (2.1) is specifically:

wherein,is a matrix of poses represented by Euler angles, i.e.

An angular velocity of the agricultural machine output for the gyro sensor, and

is the angular velocity of the earth, is known as

is the attitude rate, and

Thus, it can be obtained that,

the earth coordinate system is a coordinate system fixedly connected to the earth and rotates along with the earth, and the earth coordinate system rotates relative to the inertial coordinate system at the rotational angular velocity of the earth, wherein the rotational angular velocity of the earth is omega ie, and the omega ie is 15 pi/180.

The output of the gyro sensor is usually an angular velocity, and therefore, in order to calculate the vehicle attitude, a quaternion differential equation is introduced. The differential equation is introduced, so that a new attitude quaternion (namely an attitude matrix of the carrier coordinate system relative to the navigation coordinate system) can be obtained by sampling the triaxial angle increment of the carrier coordinate system at regular time according to the attitude quaternion at the last moment. The angle increment obtained by the equivalent rotation vector method can eliminate the irreplaceable error of rotation; the step (2.2) specifically comprises the following steps:

the quaternion updating differential equation is as follows:

i is an identity matrix; q is a quaternion vector;is the quaternion derivative; Δ t is a data update period; the result of the first attitude matrix is Q ═ Q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃Is a scalar quantity forming a quaternion vector, i, j, k are three-dimensional coordinate system unit vectors, theta₁And theta₂Respectively carrying out angle increment of twice equal-interval time sampling on the gyroscope in the attitude updating period; theta₀、γ₀、ψ₀The pitch angle, the roll angle and the course angle in the initial state are respectively, and Q (0) is an initial quaternion value calculated by using the initial attitude angle.

The attitude matrix calculation module normalizes the first attitude matrix to obtain an updated attitude matrix, and specifically comprises:

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}}} - - - (5)

the updated attitude matrix is as follows:

C_{b}^{n} = [\begin{matrix} 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{matrix}] - - - (6)

The navigation coordinate system is a geographical coordinate system of 'northeast-earth', the coordinate transformation module converts the acceleration of the agricultural machine carrier coordinate system sent by the acceleration sensor into the acceleration of an aerospace coordinate system according to the updated attitude matrix, and the method specifically comprises the following steps:

the coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machine, which is sent by the acceleration sensor, into the acceleration of an aerospace coordinate system according to the updated attitude matrix and by the following formula:

[\begin{matrix} f_{E} \\ f_{N} \\ f_{U} \end{matrix}] = C_{b}^{n} [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}] - - - (7)

wherein,

f_{b} = [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}]

C_{b}^{n} = [\begin{matrix} 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{matrix}],

The navigation coordinate system is a geographical coordinate system of northeast, the speed and position calculation module outputs the position information and the speed information of the agricultural machine, and the method specifically comprises the following steps:

9. The control method for assisting driving of an agricultural machine according to claim 8, wherein the step (4.1) is specifically:

wherein,

f^{n} = [\begin{matrix} f_{E} \\ f_{N} \\ f_{U} \end{matrix}],

v^{n} = [\begin{matrix} v_{E} \\ v_{N} \\ v_{U} \end{matrix}],

g = [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}]

the matrix of equation (8) is represented as:

wherein,is the angular velocity of the earth, is known as

l, lambda and h are respectively latitude, longitude and altitude; psi is azimuth angle, theta is pitch angle, gamma is roll angle, v_E、v_N、v_UFor northeast speed, provided by a speed position calculation module, R_M、R_NRadius of curvature R of meridian plane and unitary plane of earth_M≈R(1-2e+3esin²L)，R_N≈R(1+esin²L), where R ═ m, e ═ 1/298.257), f_E、f_N、f_UAre the specific force components, omega, in the east, north and sky directions, respectively, along the geographical coordinate system_eThe earth's rotation makes an angular velocity.

In order to improve the calculation speed, the invention adopts a second-order Runge-Kutta numerical integration method to calculate the speed update, and the formula is as follows:

in the formula, K₁And K₂At a speed t_mAnd t_m+1The slope of time; the step (4.2) is specifically as follows:

wherein L, lambda and h are respectively latitude, longitude and altitude of the ground vehicle, v_E、v_N、v_UThe speed of the northeast is provided by a speed position calculation module, k is a sampling point, and T is a sampling period.

After updating the longitude and latitude, substituting the calculation result L (k) into R for calculating kT time_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 the formula, R_eIs the earth radius and f is the ellipticity. The change rule of gravity along with latitude and height is approximately as follows:

g＝g₀[l+0.00527094sin²(L)+0.0000232718sin⁴(L)]0.000003086h, wherein g₀＝9.7803267714。

The navigation coordinate system is a geographical coordinate system of northeast, the attitude calculation module outputs the attitude angle of the agricultural machine according to the updated attitude matrix of the attitude matrix calculation module, and the method specifically comprises the following steps:

In a preferred embodiment, the attitude angle of the agricultural machine may be calculated from the updated attitude matrixMedium extraction, including pitch angle, roll angle and yawAnd the azimuth angle is defined in a range of +/-90 degrees by the pitch angle theta and is consistent with the main value of the arcsine function, so that the multi-value problem does not exist. And the roll angle gamma is defined at-180 DEG and 180 DEG]The heading angle psi is defined at [0 deg. ], 360 deg. ]]The interval, so there is a multi-value problem for both gamma and psi, after calculating the main value, it can be calculated fromThe element in (1) determines which quadrant is in.

Due to the fact that

For pitch angle: theta is equal to theta_{Master and slave}；

For the roll angle:

for the heading angle:

therefore:

it must be noted that due to the highly unstable pure inertial navigation altitude channel, some error sources including acceleration sensor error may form an accumulative error, and the altitude error may increase with time. Therefore, the height information of the agricultural machine in a long time cannot be calculated only by using the speed, and the correction must be carried out by using a barometric altimeter or a radio altimeter signal. Since the altitude channel is divergent in a pure inertial navigation system, damping can be introduced with extraneous altitude reference information, where altitude and altitude-related terms are not considered.

And (3) error analysis: the inertial navigation system has many error sources, mainly including the error of the inertial instrument itself, the installation error and the scale error of the inertial instrument, the initial condition error of the system, the calculation error of the system, the error caused by various interferences, and the like. Inertial navigation errors can be divided into two categories: deterministic errors and random errors. The deterministic errors include platform angle errors, speed errors and position errors, and the random errors mainly include gyroscope sensor drift, zero offset of an acceleration sensor and the like. Although various error sources exist in the inertial navigation system, part of the error sources have little influence on the inertial navigation system. Because the strapdown inertial navigation system adopts a mathematical platform to replace a physical platform, namely, angular velocity information measured by a gyroscope sensor is used for carrying out attitude matrix calculation, and specific force information measured by an acceleration sensor is used for carrying out navigation calculation through attitude matrix transformation, errors of the inertial sensor and initial condition errors are propagated in the system through an attitude matrix, and important influence is generated on navigation. That is, the three error sources, i.e., the gyroscope sensor drift, the acceleration sensor zero offset error and the initial value error, have a certain degree of influence on the navigation parameters. In the following description of an error model of a strapdown inertial navigation system, a geographical coordinate system of northeast China is adopted as a strapdown inertial navigation coordinate system (n system), the navigation information error is 9-dimensional, and the three-dimensional platform error angle, the three-dimensional speed error and the three-dimensional position error are adopted.

The navigation coordinate system is a geographical coordinate system of northeast, and the following steps are further included between the step (4) and the step (5):

＝_b+_r+ω_g (13)

▽＝▽_b+▽_a+ω_a

and

Other errors comprise cone motion error, rowing error, scroll error and the like, wherein the cone motion error can be compensated by optimizing a rotating vector algorithm, and the rowing error and the scroll error have small and negligible influence on the strapdown inertial navigation system by matlab simulation.

By the error term formula, compensation can be performed through a Kalman filtering algorithm by establishing an accurate mathematical model. By combining the above formulas, a 15-dimensional state equation of the combined navigation system in a position and speed combined mode can be obtained; the error compensation is carried out on the position information, the speed information and the attitude angle according to a Kalman filtering algorithm, and the method specifically comprises the following steps:

wherein,

X(t)＝[φ_E φ_N φ_U v_E v_N v_U L λ h _bx _by _bz ▽_bx ▽_by ▽_bz]^Tis a state vector of the system, wherein the subscripts E, N, U respectively represent the three directions, φ, of the northeast geographic coordinate system_E、φ_N、φ_UIs an error angle, v, of a strapdown inertial navigation system_E、v_N、v_UIs a speed error, L, lambda and h are position errors,_bx、_by、_bzrandom drift of gyroscope +_bx、▽_by、▽_bzZero error of the accelerometer; w (t) ═ ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^TIs a system process white noise vector, where ω is_gx、ω_gy、ω_gzWhite noise, omega, of the gyro_ax、ω_ay、ω_azWhite noise for an accelerometer; f (t) is the system state matrix, and G (t) is the system noise propagation matrix.

The invention fully considers various error items in an automatic control system of agricultural machinery, performs mathematical modeling on the error items, realizes the filter algorithm through software programming, and obtains a test result that an attitude error angle is less than 0.1 degrees, a course angle error is less than 1 degree and a position deviation is about 5cm through a field tractor test.

(2) the error compensation and correction algorithm adopted by the invention greatly reduces the algorithm error of the strapdown inertial navigation system and the interference of earth rotation and the like;

(2) the algorithm of the six-axis inertial sensor and the strapdown inertial navigation system is adopted, so that the strapdown inertial navigation system for the agricultural machine has high performance parameters, the error of a course angle output by the device outside a tractor test chamber is less than 1 degree, the error of a pitch angle and a rolling angle is less than 0.1 degree, the position information output by the strapdown inertial navigation system for the agricultural machine is matched with a GPS to realize that the combined navigation position error is in the level of cm, the data output frequency reaches 50HZ, and the requirement of an auxiliary driving control system of the agricultural machine is met;

In this specification, the invention has been described with reference to specific embodiments thereof. It will, however, be evident that various modifications and changes may be made thereto without departing from the broader spirit and scope of the invention. The specification and drawings are, accordingly, to be regarded in an illustrative rather than a restrictive sense.

Claims

1. A strapdown inertial navigation system for agricultural machinery, comprising a six-axis inertial sensor, a central controller; the six-axis inertial sensor comprises acceleration sensors in three directions and a three-axis gyroscope sensor; the central controller comprises:

2. A method for controlling an agricultural machine based on the system of claim 1, comprising the steps of:

3. The control method for realizing agricultural machinery according to claim 2, wherein the navigation coordinate system is a geographical coordinate system of "northeast sky", and the attitude matrix calculation module calculates an updated attitude matrix according to the angular position value calculated by the speed and position calculation module and the received angular position value of the gyro sensor, and specifically comprises the following steps:

4. The control method for realizing an agricultural machine according to claim 3, wherein the step (2.1) is specifically as follows:

wherein,is a matrix of poses represented by Euler angles, i.e.

An angular velocity of the agricultural machine output for the gyro sensor, and

is the angular velocity of the earth, is known as

is the attitude rate, and

5. A control method for realizing an agricultural machine according to claim 3, wherein the step (2.2) comprises the following steps:

the quaternion updating differential equation is as follows:

6. The control method for realizing agricultural machinery according to claim 3, wherein the attitude matrix calculation module normalizes the first attitude matrix to obtain an updated attitude matrix, and specifically comprises:

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}}} - - - (5)

the updated attitude matrix is as follows:

C_{b}^{n} = [\begin{matrix} 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{matrix}] - - - (6)

7. The control method for realizing agricultural machinery according to claim 2, wherein the navigation coordinate system is a geographical coordinate system of "northeast", and the coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machinery, which is sent by the acceleration sensor, into the acceleration of the aerospace coordinate system according to the updated attitude matrix, specifically:

[\begin{matrix} f_{E} \\ f_{N} \\ f_{U} \end{matrix}] = C_{b}^{n} [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}] - - - (7)

Wherein,

f_{b} = [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}]

C_{b}^{n} = [\begin{matrix} 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{matrix}],

8. The method of claim 2, wherein the navigation coordinate system is a geographic coordinate system of "northeast", and the speed and position calculation module outputs the position information and the speed information of the agricultural machine, and the method specifically includes the following steps:

wherein,

the matrix of equation (8) is represented as:

wherein,is the angular velocity of the earth, is known as

l, lambda and h are respectivelyLatitude, longitude, altitude; psi is azimuth angle, theta is pitch angle, gamma is roll angle, v_E、v_N、v_UThe speed of east, north and sky of the space coordinate system is provided by a speed position calculation module, R_M、R_NRadius of curvature R of meridian plane and unitary plane of earth_M≈R(1-2e+3esin²L)，R_N≈R(1+esin²L), where R ═ m, e ═ 1/298.257), f_E、f_N、f_UAre the specific force components, omega, in the east, north and sky directions, respectively, along the geographical coordinate system_eThe earth's rotation makes an angular velocity.

10. The control method for realizing an agricultural machine according to claim 8, wherein the step (4.2) is specifically as follows:

11. The control method for realizing agricultural machinery according to claim 2, wherein the navigation coordinate system is a geographical coordinate system of "northeast sky", and the attitude calculation module outputs the attitude angle of the agricultural machinery according to the updated attitude matrix of the attitude matrix calculation module, specifically comprising the following steps:

12. The control method for realizing agricultural machinery according to claim 2, wherein the navigation coordinate system is a geographical coordinate system of "northeast", and the following steps are further included between the step (4) and the step (5):

＝_b+_r+ω_g (13)

▽＝▽_b+▽_a+ω_a

and

13. The control method for realizing agricultural machinery according to claim 12, wherein the error compensation is performed on the position information, the speed information and the attitude angle according to a kalman filter algorithm, and specifically comprises:

wherein,