CN104697526A

Movatterモバイル変換

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

Translated fromChinese

本发明涉及一种用于农业机械的捷联惯导系统，所述的系统包括六轴惯性传感器、中央控制器；所述的六轴惯性传感器包括三个方向的加速度传感器以及三轴的陀螺仪传感器；所述的中央控制器包括：坐标变换模块、速度位置计算模块、姿态矩阵计算模块以及姿态计算模块；本发明还涉及一种基于捷联惯导系统的用于农业机械的控制方法。该种结构的用于农业机械的捷联惯导系统以及控制方法，采用了六轴惯性传感器，体积小，重量轻，结构简单，性价比高，模块化设计便于集成到农业机械的控制中心之中，同时，具有运行稳定和输出运动信息丰富等优点，控制精确度高，尤其满足农业机械等地面车辆辅助驾驶控制的要求，应用范围较为广泛。

The invention relates to a strapdown inertial navigation system for agricultural machinery. The system includes a six-axis inertial sensor and a central controller; the six-axis inertial sensor includes acceleration sensors in three directions and a three-axis gyroscope sensor; the central controller includes: a coordinate transformation module, a speed position calculation module, an attitude matrix calculation module and an attitude calculation module; the present invention also relates to a control method for agricultural machinery based on a strapdown inertial navigation system. The strapdown inertial navigation system and control method for agricultural machinery of this structure adopts a six-axis inertial sensor, which is small in size, light in weight, simple in structure, and high in cost performance. The modular design is easy to integrate into the control center of agricultural machinery , at the same time, it has the advantages of stable operation and rich output motion information, high control accuracy, especially meets the requirements of auxiliary driving control of ground vehicles such as agricultural machinery, and has a wide range of applications.

Description

Translated fromChinese

用于农业机械的捷联惯导系统以及控制方法Strapdown inertial navigation system and control method for agricultural machinery

技术领域technical field

本发明涉及机械控制领域，尤其涉及高精度机械控制，具体是指一种用于农业机械的捷联惯导系统以及控制方法。The invention relates to the field of mechanical control, in particular to high-precision mechanical control, in particular to a strapdown inertial navigation system and a control method for agricultural machinery.

背景技术Background technique

随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展以及国家对农业扶持力度的进一步加大，精准农业正在快速变成一种趋势，而在农业机械辅助驾驶控制过程中，车体的姿态(包括俯仰角、翻滚角和航向角)、速度和位置信息能够实时反映出车体的运动和位置信息，这些信息能够为高精度的组合导航和控制算法提供重要的数据输入。With the development of MEMS (Micro-Electro-Mechanical-System) sensors, navigation and control technology and the further increase of the country's support for agriculture, precision agriculture is rapidly becoming a trend, and in the process of auxiliary driving control of agricultural machinery , the attitude (including pitch angle, roll angle and heading angle), speed and position information of the vehicle body can reflect the motion and position information of the vehicle body in real time, and these information can provide important data input for high-precision integrated navigation and control algorithms .

在农业机械辅助驾驶控制过程中，车体在导航坐标系中的位置信息和航向信息是最重要的两个参数。农业机械辅助驾驶系统中常用的定位方式有：机械触觉、推算定位、机器视觉、激光定位和多传感器信息融合(IMU+GPS)。多传感器信息融合充分利用多个传感器资源，通过对这些传感器及其观测信息的合理支配和利用，把多个传感器在空间或时间上的冗余或互补信息依据某种准则来进行组合，以获得被测对象的一致性解释或描述，此种方法相对于其它几种具有定位精度高、体积小和较高的性价比。In the process of assisted driving control of agricultural machinery, the position information and heading information of the vehicle body in the navigation coordinate system are the two most important parameters. Commonly used positioning methods in agricultural machinery assisted driving systems include: mechanical haptics, dead reckoning, machine vision, laser positioning, and multi-sensor information fusion (IMU+GPS). Multi-sensor information fusion makes full use of multiple sensor resources. Through the reasonable control and utilization of these sensors and their observation information, the redundant or complementary information of multiple sensors in space or time is combined according to certain criteria to obtain Consistent interpretation or description of the measured object. Compared with other methods, this method has high positioning accuracy, small size and high cost performance.

多传感器信息融合中位置信息的获取主要依据高精度的GPS定位信息(如RTK)，但是GPS数据输出精度和输出频率与价格成正比，而且在有障碍物遮挡或者天气等原因时，GPS不能保证有效的数据输出，此时需要一套高频高精度的数据采集和计算系统为这种盲区实时提供数据填补。The acquisition of location information in multi-sensor information fusion is mainly based on high-precision GPS positioning information (such as RTK), but the accuracy and frequency of GPS data output are proportional to the price, and GPS cannot guarantee the accuracy of the location when there are obstacles or weather. Effective data output requires a set of high-frequency and high-precision data acquisition and calculation systems to provide real-time data filling for this blind area.

目前，惯性导航系统分为PINS(Platform Inertial Navigation System)和SINS(StrapdownInertial Navigation System)，SINS相比于PINS是采用IMU(Inertial Measuring Unit)传感器通过计算建立一个“数学平台”来代替PINS。SINS多使用在飞行器导航控制系统中，而针对农业机械控制领域的研究和应用则属于起步阶段，而且二者的应用对象和环境条件有较大的差别，飞行器控制系统中捷联惯导实现的方法在农业机械控制中不尽适用。At present, inertial navigation systems are divided into PINS (Platform Inertial Navigation System) and SINS (Strapdown Inertial Navigation System). Compared with PINS, SINS uses IMU (Inertial Measuring Unit) sensors to establish a "mathematical platform" through calculation to replace PINS. SINS is mostly used in aircraft navigation control systems, while the research and application in the field of agricultural machinery control is in its infancy, and the application objects and environmental conditions of the two are quite different. The method is not always applicable in the control of agricultural machinery.

发明内容Contents of the invention

本发明的目的是克服了上述现有技术的缺点，提供了一种能够实现的用于农业机械的捷联惯导系统以及控制方法。The purpose of the present invention is to overcome the above-mentioned shortcomings of the prior art, and to provide a strapdown inertial navigation system and control method for agricultural machinery that can be realized.

为了实现上述目的，本发明的用于农业机械的捷联惯导系统以及控制方法具有如下构成：In order to achieve the above object, the strapdown inertial navigation system and control method for agricultural machinery of the present invention have the following composition:

该用于农业机械的捷联惯导系统，其主要特点是，所述的系统包括六轴惯性传感器、中央控制器；所述的六轴惯性传感器包括三个方向的加速度传感器以及三轴的陀螺仪传感器；所述的中央控制器包括：The strapdown inertial navigation system for agricultural machinery is characterized in that the system includes a six-axis inertial sensor and a central controller; the six-axis inertial sensor includes acceleration sensors in three directions and a three-axis gyroscope instrument sensor; the central controller includes:

坐标变换模块，用以将所述的加速度传感器发送的所述的农业机械的载体坐标系的加速度转换为航天坐标系的加速度；A coordinate transformation module, used to convert the acceleration of the carrier coordinate system of the agricultural machinery sent by the acceleration sensor into the acceleration of the aerospace coordinate system;

速度位置计算模块，用以根据所述的坐标变换模块输出的航天坐标系的加速度计算获得所述的农业机械的速度信息以及位置信息；The speed and position calculation module is used to calculate and obtain the speed information and position information of the agricultural machinery according to the acceleration of the aerospace coordinate system output by the coordinate transformation module;

姿态矩阵计算模块，用以根据所述的陀螺仪传感器发送的所述的农业机械的角速度值以及所述的速度位置计算模块计算的位置角速度值计算获得更新后的姿态矩阵；以及The attitude matrix calculation module is used to calculate and obtain the updated attitude matrix according to the angular velocity value of the agricultural machinery sent by the gyroscope sensor and the position angular velocity value calculated by the velocity position calculation module; and

姿态计算模块，用以根据所述的姿态矩阵计算模块的更新后的姿态矩阵获得所述的农业机械的姿态角。The attitude calculation module is used to obtain the attitude angle of the agricultural machine according to the updated attitude matrix of the attitude matrix calculation module.

本发明还涉及一种实现农业机械的控制方法，其主要特点是，所述的方法包括以下步骤：The present invention also relates to a control method for realizing agricultural machinery, and its main feature is that the method includes the following steps:

(1)所述的加速度传感器实时地将所述的农业机械的加速度发送至所述的坐标变换模块，且同时所述的陀螺仪传感器实时地将所述的农业机械的角速度发送至所述的姿态矩阵计算模块；(1) The acceleration sensor sends the acceleration of the agricultural machinery to the coordinate transformation module in real time, and at the same time, the gyroscope sensor sends the angular velocity of the agricultural machinery to the Attitude matrix calculation module;

(2)所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度值以及所接收的陀螺仪传感器的角速度值计算获得更新后的姿态矩阵，并将更新后的姿态矩阵发送至坐标变换模块；(2) The attitude matrix calculation module calculates the 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 Coordinate transformation module;

(3)所述的坐标变换模块根据更新后的姿态矩阵将所述的加速度传感器发送的所述的农业机械的载体坐标系的加速度转换为航天坐标系的加速度；(3) The coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machinery sent by the acceleration sensor into the acceleration of the aerospace coordinate system according to the updated attitude matrix;

(4)所述的速度位置计算模块根据所述的坐标变换模块输出的航天坐标系的加速度输出所述的农业机械的位置信息以及速度信息，且所述的姿态计算模块根据所述的姿态矩阵计算模块的更新后的姿态矩阵输出所述的农业机械的姿态角；(4) The speed position calculation module outputs the position information and speed information of the agricultural machinery according to the acceleration of the aerospace coordinate system output by the coordinate transformation module, and the attitude calculation module outputs the position information and speed information according to the attitude matrix The updated attitude matrix of the calculation module outputs the attitude angle of the agricultural machinery;

(5)所述的农业机械的控制中心根据所述的位置信息、速度信息以及姿态角控制所述的农业机械。(5) The control center of the agricultural machinery controls the agricultural machinery according to the position information, speed information and attitude angle.

进一步地，导航坐标系为“东北天”地理坐标系，所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度值以及所接收的陀螺仪传感器的角速度值计算获得更新后的姿态矩阵，具体包括以下步骤：Further, the navigation coordinate system is the "northeast sky" geographic coordinate system, and the attitude matrix calculation module calculates the updated position angular velocity value calculated by the velocity position calculation module and the received angular velocity value of the gyroscope sensor. Attitude matrix, which specifically includes the following steps:

(2.1)所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度值以及所接收的陀螺仪传感器的角速度值计算姿态速率；(2.1) The attitude matrix calculation module calculates the attitude rate according to the position angular velocity value calculated by the velocity position calculation module and the angular velocity value of the received gyroscope sensor;

(2.2)所述的姿态矩阵计算模块根据所述的姿态速率求解四元数微分方程以获得第一姿态矩阵；(2.2) described attitude matrix calculation module solves quaternion differential equation according to described attitude rate to obtain the first attitude matrix;

(2.3)所述的姿态矩阵计算模块将所述的第一姿态矩阵归一化后获得更新后的姿态矩阵。(2.3) The attitude matrix calculation module normalizes the first attitude matrix to obtain an updated attitude matrix.

更进一步地，所述的步骤(2.1)具体为：Further, the described step (2.1) is specifically:

所述的姿态矩阵计算模块通过以下公式获得姿态速率：The attitude matrix calculation module obtains the attitude rate by the following formula:

${ω ω}_{nb nb}^{b b} = = {ω ω}_{ib ib}^{b b} - - {ω ω}_{in in}^{b b} = = {ω ω}_{ib ib}^{b b} - - {C C}_{n no}^{b b} {ω ω}_{in in}^{n no} = = {ω ω}_{ib ib}^{b b} - - {C C}_{n no}^{b b} (({ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) - - - - - - ((11))$

其中，是欧拉角表示的姿态矩阵，即in, is the attitude matrix represented by Euler angles, that is,

${C C}_{n no}^{b b} = = [\begin{matrix} cos cos γ γ cos cos ψ ψ + + sin sin γ γ sin sin θ θ sin sin ψ ψ & - - cos cos γ γ sin sin ψ ψ + + sin sin γ γ sin sin θ θ cos cos ψ ψ & - - sin sin γ γ cos cos θ θ \\ cos cos θ θ sin sin ψ ψ & cos cos θ θ cos cos ψ ψ & sin sin θ θ \\ sin sin γ γ cos cos ψ ψ - - cos cos γ γ sin sin θ θ sin sin ψ ψ & - - sin sin γ γ sin sin ψ ψ - - cos cos γ γ sin sin θ θ cos cos ψ ψ & cos cos γ γ cos cos θ θ \end{matrix}] - - - - - - ((22))$

为所述的陀螺仪传感器输出的所述的农业机械的角速度，且 $ω_{ib}^{b} = [\begin{matrix} ω_{bx}^{b} \\ ω_{by}^{b} \\ ω_{bz}^{b} \end{matrix}];$ 为地球角速度，是已知的，为 $ω_{ie}^{n} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}];$ 为导航坐标系相对地球的角速度，其可以由瞬时速度求得，且 $ω_{en}^{n} = [\begin{matrix} - \frac{v_{N}}{R_{M} + h} \\ ω_{e} \cos L + \frac{v_{E}}{R_{N} + h} \\ ω_{e} \sin L + \frac{v_{E}}{R_{N} + h} \tan L \end{matrix}];$ 为姿态速率，且 $ω_{nb}^{b} = [\begin{matrix} ω_{nb}^{bx} & ω_{nb}^{by} & ω_{nb}^{bz} \end{matrix}],$ ψ为方位角，θ为俯仰角，γ为横滚角，v_E、v_N、v_U为东北天的速度，由速度位置计算模块提供，R_M、R_N为地球子午面和卯酉面曲率半径，R_M≈R(1-2e+3esin²L)，R_N≈R(1+esin²L)，其中，R＝m，e＝1/298.257)，L、λ、h分别为纬度、经度、高度，是中间变量，为地球自转角速度与航天坐标系相对地球的角速度的合角速度在载体坐标系中的投影；ω_e地球自转合角速度。 is the angular velocity of the agricultural machine output by the gyro sensor, and $ω_{ib}^{b} = [\begin{matrix} ω_{bx}^{b} \\ ω_{by}^{b} \\ ω_{bz}^{b} \end{matrix}];$ is the angular velocity of the earth, which is known as $ω_{ie}^{no} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}];$ is the angular velocity of the navigation coordinate system relative to the earth, which can be determined by the instantaneous velocity obtain, and $ω_{en}^{no} = [\begin{matrix} - \frac{v_{N}}{R_{m} + h} \\ ω_{e} \cos L + \frac{v_{E.}}{R_{N} + h} \\ ω_{e} \sin L + \frac{v_{E.}}{R_{N} + h} the tan L \end{matrix}];$ is the attitude rate, and $ω_{nb}^{b} = [\begin{matrix} ω_{nb}^{bx} & ω_{nb}^{by} & ω_{nb}^{bz} \end{matrix}],$ ψ is the azimuth angle, θ is the pitch angle, γ is the roll angle, v_E , v_N , v_U are the velocities of the northeast sky, which are provided by the velocity and position calculation module, R_M ,_RN are the meridian plane and Maoyou plane of the earth Radius of curvature, R_M ≈ R(1-2e+3esin² L), R_N ≈ R(1+esin² L), where, R=m, e=1/298.257), L, λ, h are latitude , longitude, altitude, is an intermediate variable, which is the projection of the combined angular velocity of the earth's rotation angular velocity and the angular velocity of the aerospace coordinate system relative to the earth in the carrier coordinate system; ω_{e is} the combined angular velocity of the earth's rotation.

更进一步地，所述的步骤(2.2)具体包括以下步骤：Further, the step (2.2) specifically includes the following steps:

所述的姿态矩阵计算模块根据所述的姿态速率以及等效旋转矢量算法获得四元数更新微分方程以获得第一姿态矩阵，所述的四元数微分方程为：The attitude matrix calculation module obtains the quaternion update differential equation according to the attitude rate and the equivalent rotation vector algorithm to obtain the first attitude matrix, and the described quaternion differential equation is:

$\overset{\cdot \cdot}{Q Q} = = {ω ω}_{ib ib}^{b b} ((t t)) + + \frac{11}{22} Q Q \times \times {ω ω}_{ib ib}^{b b} ((t t)) - - - - - - ((33))$

所述的四元数更新微分方程为：The described quaternion update differential equation is:

$Q Q ((t t)) = = ((I I cos cos \frac{Δθ Δθ}{22} + + [[Δθ Δθ]] sin sin \frac{\frac{Δθ Δθ}{22}}{Δθ Δθ})) \cdot \cdot Q Q ((00)) - - - - - - ((44))$

其中，为在一个姿态周期内，所述的陀螺仪传感器输出的角速度，且t为时间刻度，其中，角增量根据有等效旋转矢量二子样算法， $Δ θ = (θ_{1} + θ_{2}) + \frac{2}{3} (θ_{1} \times θ_{2}),$ $Q (0) = [\begin{matrix} \cos \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} + \sin \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} \\ \sin \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} + \cos \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} \\ \cos \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} - \sin \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} \\ \cos \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} - \sin \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} \end{matrix}],$ I为单位矩阵；Q为四元数向量；为四元数导数；Δt为数据更新周期；第一姿态矩阵的结果是Q＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃为组成四元数向量的标量，i、j、k为三维坐标系单位向量，θ₁和θ₂分别为姿态更新周期内陀螺仪两次等间隔时间采样的角增量；θ₀、γ₀、ψ₀分别为初始状态下的俯仰角、翻滚角和航向角，Q(0)为利用上述初始姿态角计算出来的初始四元数值。in, is the angular velocity output by the gyroscope sensor within one attitude cycle, and t is the time scale, where the angular increment According to the two-sample algorithm with equivalent rotation vectors, $Δ θ = (θ_{1} + θ_{2}) + \frac{2}{3} (θ_{1} \times θ_{2}),$ $Q (0) = [\begin{matrix} \cos \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} + \sin \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} \\ \sin \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} + \cos \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} \\ \cos \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} - \sin \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} \\ \cos \frac{θ_{0}}{2} \cos \frac{γ_{0}}{2} \sin \frac{ψ_{0}}{2} - \sin \frac{θ_{0}}{2} \sin \frac{γ_{0}}{2} \cos \frac{ψ_{0}}{2} \end{matrix}],$ I is an identity matrix; Q is a quaternion vector; is the quaternion 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₂ , and q₃ are the composition four The scalar of the arity vector, i, j, k are the unit vectors of the three-dimensional coordinate system, θ₁ and θ₂ are the angular increments of the gyroscope at two equal intervals in the attitude update period; θ₀ , γ₀ , ψ₀ are the pitch angle, roll angle, and heading angle in the initial state, respectively, and Q(0) is the initial quaternion value calculated using the above initial attitude angle.

再进一步地，所述的姿态矩阵计算模块将所述的第一姿态矩阵归一化后获得更新后的姿态矩阵，具体为：Still further, the attitude matrix calculation module normalizes the first attitude matrix to obtain an updated attitude matrix, specifically:

所述的姿态矩阵计算模块根据以下公式将所述的第一姿态矩阵归一化后获得更新后的姿态矩阵：The attitude matrix calculation module obtains the updated attitude matrix after normalizing the first attitude matrix according to the following formula:

$Q Q = = \frac{{q q}_{00} + + {q q}_{11} i i + + {q q}_{22} j j + + {q q}_{33} k k}{\sqrt{{q q}_{00}^{22} + + {q q}_{11}^{22} + + {q q}_{22}^{22} + + {q q}_{33}^{22}}} - - - - - - ((55))$

所述的更新后的姿态矩阵为：The updated attitude matrix is:

${C C}_{b b}^{n no} = = [\begin{matrix} {q q}_{11}^{22} + + {q q}_{00}^{22} - - {q q}_{33}^{22} - - {q q}_{22}^{22} & 22 (({q q}_{11} {q q}_{22} - - {q q}_{00} {q q}_{33})) & 22 (({q q}_{11} {q q}_{33} + + {q q}_{00} {q q}_{22})) \\ 22 (({q q}_{11} {q q}_{22} + + {q q}_{00} {q q}_{33})) & {q q}_{22}^{22} - - {q q}_{33}^{22} + + {q q}_{00}^{22} - - {q q}_{11}^{22} & 22 (({q q}_{22} {q q}_{33} - - {q q}_{00} {q q}_{11})) \\ 22 (({q q}_{11} {q q}_{33} + + {q q}_{00} {q q}_{22})) & 22 (({q q}_{22} {q q}_{33} + + {q q}_{00} {q q}_{11})) & {q q}_{33}^{22} - - {q q}_{22}^{22} - - {q q}_{11}^{22} + + {q q}_{00}^{22} \end{matrix}] - - - - - - ((66))$

其中，Q为四元数向量，q₀、q₁、q₂、q₃为组成四元数向量的标量，i、j、k为三维坐标系单位向量，为载体坐标系到导航坐标系的旋转矩阵。Among them, Q is a quaternion vector, q₀ , q₁ , q₂ , and q₃ are scalars that make up a quaternion vector, i, j, and k are unit vectors in a three-dimensional coordinate system, is the rotation matrix from the carrier coordinate system to the navigation coordinate system.

更进一步地，导航坐标系为“东北天”地理坐标系，所述的坐标变换模块根据更新后的姿态矩阵将所述的加速度传感器发送的所述的农业机械的载体坐标系的加速度转换为航天坐标系的加速度，具体为：Furthermore, the navigation coordinate system is the "northeast sky" geographic coordinate system, and the coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machinery sent by the acceleration sensor into an aerospace coordinate system according to the updated attitude matrix. The acceleration of the coordinate system, specifically:

所述的坐标变换模块根据更新后的姿态矩阵并通过以下公式将所述的加速度传感器发送的所述的农业机械的载体坐标系的加速度转换为航天坐标系的加速度The coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machinery sent by the acceleration sensor into the acceleration of the aerospace coordinate system according to the updated attitude matrix and through the following formula

$[\begin{matrix} {f f}_{E E.} \\ {f f}_{N N} \\ {f f}_{U u} \end{matrix}] = = {C C}_{b b}^{n no} [\begin{matrix} {f f}_{bx bx} \\ {f f}_{by by} \\ {f f}_{bz bz} \end{matrix}] - - - - - - ((77))$

其中， $f_{b} = [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}]$ 为加速度传感器输出的加速度，f_E、f_N、f_U分别为沿地理坐标系在东、北、天方向上的比力分量，是更新后的姿态矩阵为：in, $f_{b} = [\begin{matrix} f_{bx} \\ f_{by} \\ f_{bz} \end{matrix}]$ is the acceleration output by the acceleration sensor, f_E , f_N , and f_U are the specific force components along the geographic coordinate system in the east, north, and sky directions respectively, is the updated attitude matrix as:

$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}],$ 其中，Q为四元数向量，q₀、q₁、q₂、q₃为组成四元数向量的标量，i、j、k为三维坐标系单位向量。 $C_{b}^{no} = [\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₂ , and q₃ are scalars composing the quaternion vector, and i, j, k are unit vectors of the three-dimensional coordinate system.

更进一步地，导航坐标系为“东北天”地理坐标系，所述的速度位置计算模块输出所述的农业机械的位置信息以及速度信息，具体包括以下步骤：Furthermore, the navigation coordinate system is the "northeast sky" geographic coordinate system, and the speed and position calculation module outputs the position information and speed information of the agricultural machinery, which specifically includes the following steps:

(4.1)所述的速度位置计算模块将所述的坐标变换模块所输出的加速度积分以获得所述的农业机械的速度信息；(4.1) The speed and position calculation module integrates the acceleration output by the coordinate transformation module to obtain the speed information of the agricultural machinery;

(4.2)所述的速度位置计算模块将速度积分后获得所述的农业机械的位置信息。(4.2) The speed and position calculation module integrates the speed to obtain the position information of the agricultural machinery.

再进一步地，所述的步骤(4.1)具体为：Still further, the described step (4.1) is specifically:

所述的速度位置计算模块根据以下公式将所述的坐标变换模块所输出的加速度积分以获得所述的农业机械的速度信息：The speed and 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 machinery:

${\overset{\cdot &Center Dot;}{v v}}^{n no} = = {f f}^{n no} - - (({ω ω}_{en en}^{n no} + + 22 {ω ω}_{ie ie}^{n no})) \times \times {v v}^{n no} + + g g - - - - - - ((88))$

其中， $ω_{ie}^{n} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}],$ $ω_{en}^{n} = [\begin{matrix} - \frac{v_{N}}{R_{M} + h} \\ \frac{v_{E}}{R_{N} + h} \\ \frac{v_{E}}{R_{N} + h} \tan L \end{matrix}],$ $f^{n} = [\begin{matrix} f_{E} \\ f_{N} \\ f_{U} \end{matrix}],$ ${\overset{\cdot}{v}}^{n} = [\begin{matrix} {\overset{\cdot}{v}}_{E} \\ {\overset{\cdot}{v}}_{N} \\ {\overset{\cdot}{v}}_{U} \end{matrix}],$ $v^{n} = [\begin{matrix} v_{E} \\ v_{N} \\ v_{U} \end{matrix}],$ $g = [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}]$ 公式(8)的矩阵表示为：in, $ω_{ie}^{no} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}],$ $ω_{en}^{no} = [\begin{matrix} - \frac{v_{N}}{R_{m} + h} \\ \frac{v_{E.}}{R_{N} + h} \\ \frac{v_{E.}}{R_{N} + h} the tan L \end{matrix}],$ $f^{no} = [\begin{matrix} f_{E.} \\ f_{N} \\ f_{u} \end{matrix}],$ ${\overset{&Center Dot;}{v}}^{no} = [\begin{matrix} {\overset{\cdot}{v}}_{E.} \\ {\overset{&Center Dot;}{v}}_{N} \\ {\overset{&Center Dot;}{v}}_{u} \end{matrix}],$ $v^{no} = [\begin{matrix} v_{E.} \\ v_{N} \\ v_{u} \end{matrix}],$ $g = [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}]$ The matrix of formula (8) is expressed as:

$[\begin{matrix} {\overset{\cdot \cdot}{v v}}_{E E.} \\ {\overset{\cdot &Center Dot;}{v v}}_{N N} \\ {\overset{\cdot &Center Dot;}{v v}}_{U u} \end{matrix}] = = [\begin{matrix} {f f}_{E E.} \\ {f f}_{N N} \\ {f f}_{U u} \end{matrix}] - - [\begin{matrix} 00 & - - ((22 {ω ω}_{iez iez}^{n no} + + {ω ω}_{enz enz}^{n no})) & 22 {ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no} \\ 22 {ω ω}_{iez iez}^{n no} + + {ω ω}_{enz enz}^{n no} & 00 & - - ((22 {ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no})) \\ - - ((22 {ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no})) & 22 {ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no} & 00 \end{matrix}] [\begin{matrix} {v v}_{E E.} \\ {v v}_{N N} \\ {v v}_{U u} \end{matrix}] + + [\begin{matrix} 00 \\ 00 \\ - - g g \end{matrix}]$

其中，为地球角速度，是已知的，为 $ω_{ie}^{n} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}];$ 为导航坐标系相对地球的角速度，其可以由瞬时速度求得，且 $ω_{en}^{n} = [\begin{matrix} - \frac{v_{N}}{R_{M} + h} \\ ω_{e} \cos L + \frac{v_{E}}{R_{N} + h} \\ ω_{e} \sin L + \frac{v_{E}}{R_{N} + h} \tan L \end{matrix}],$ L、λ、h分别为纬度、经度、高度；ψ为方位角，θ为俯仰角，γ为横滚角，v_E、v_N、v_U为航天坐标系的东、北、天的速度，由速度位置计算模块提供，R_M、R_N为地球子午面和卯酉面曲率半径，R_M≈R(1-2e+3esin²L)，R_N≈R(1+esin²L)，其中，R＝m，e＝1/298.257)，f_E、f_N、f_U分别为沿地理坐标系在东、北、天方向上的比力分量，ω_e地球自转合角速度。in, is the angular velocity of the earth, which is known as $ω_{ie}^{no} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}];$ is the angular velocity of the navigation coordinate system relative to the earth, which can be determined by the instantaneous velocity obtain, and $ω_{en}^{no} = [\begin{matrix} - \frac{v_{N}}{R_{m} + h} \\ ω_{e} \cos L + \frac{v_{E.}}{R_{N} + h} \\ ω_{e} \sin L + \frac{v_{E.}}{R_{N} + h} the tan L \end{matrix}],$ L, λ, and h are latitude, longitude, and altitude respectively; ψ is azimuth, θ is pitch angle, γ is roll angle, v_E , v_N , v_U are the speeds of east, north, and sky in the aerospace coordinate system, Provided by the velocity and position calculation module, R_M and R_N are the curvature radii of the earth's meridian plane and the unitary plane, R_M ≈ R(1-2e+3esin² L), R_N ≈R(1+esin² L), where , R=m, e=1/298.257), f_E , f_N , and f_U are the specific force components along the geographic coordinate system in the east, north, and sky directions respectively, and ω_{e is} the combined angular velocity of the earth's rotation.

再进一步地，所述的步骤(4.2)具体为：Still further, the described step (4.2) is specifically:

所述的速度位置计算模块通过以下公式将速度积分后获得所述的农业机械的位置信息：The speed and position calculation module obtains the position information of the agricultural machinery after integrating the speed by the following formula:

$\{\begin{matrix} λ λ ((k k)) = = λ λ ((k k - - 11)) + + \frac{{v v}_{E E.} ((k k - - 11))}{[[{R R}_{M m} ((k k - - 11)) + + h h ((k k - - 11))]] cos cos L L ((k k - - 11))} \cdot &Center Dot; T T \\ L L ((k k)) = = L L ((k k - - 11)) + + \frac{{v v}_{N N} ((k k - - 11))}{{R R}_{N N} ((k k - - 11)) + + h h ((k k - - 11))} \cdot &Center Dot; T T \\ h h ((k k)) = = h h ((k k - - 11)) + + {v v}_{U u} ((K K - - 11)) \cdot &Center Dot; T T \end{matrix} - - - - - - ((99))$

其中，L、λ、h分别为地面车辆的纬度、经度、高度，v_E、v_N、v_U为航天坐标系的东、北、天的速度，由速度位置计算模块提供，k为采样点，T为采样周期。Among them, L, λ, and h are the latitude, longitude, and height of the ground vehicle respectively; v_E , v_N , and v_U are the speeds of the east, north, and sky of the aerospace coordinate system, which are provided by the speed and position calculation module; k is the sampling point , T is the sampling period.

更进一步地，导航坐标系为“东北天”地理坐标系，所述的姿态计算模块根据所述的姿态矩阵计算模块的更新后的姿态矩阵输出所述的农业机械的姿态角，具体包括以下步骤：Furthermore, the navigation coordinate system is the "Northeast Sky" geographic coordinate system, 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, which specifically includes the following steps :

所述的姿态计算模块从所述的更新后的姿态矩阵中提取到包括俯仰角、翻滚角以及航向角的所述的农业机械的姿态角。The attitude calculation module extracts the attitude angle of the agricultural machine including pitch angle, roll angle and heading angle from the updated attitude matrix.

更进一步地，导航坐标系为“东北天”地理坐标系，所述的步骤(4)与步骤(5)之间还包括以下步骤：Furthermore, the navigation coordinate system is the "northeast sky" geographic coordinate system, and the following steps are also included between the steps (4) and (5):

(4.3)所述的中央控制器判断根据以下公式(10)计算出系统角误差，根据以下公式(11)计算出速度误差，根据以下公式(12)计算出位置误差，根据以下公式(13)计算出惯性仪表误差：(4.3) The central controller judges to calculate the system angle error according to the following formula (10), calculates the speed error according to the following formula (11), calculates the position error according to the following formula (12), and calculates the position error according to the following formula (13) Calculate the inertial instrument error:

$\{\begin{matrix} {\overset{\cdot \cdot}{φ φ}}_{E E.} = = - - \frac{δ δ {v v}_{N N}}{{R R}_{M m} + + h h} + + (({ω ω}_{ie ie} sin sin L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L)) {φ φ}_{N N} - - (({ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h})) {φ φ}_{U u} + + {ϵ ϵ}_{E E.} \\ {\overset{\cdot &Center Dot;}{φ φ}}_{N N} = = \frac{δ δ {v v}_{E E.}}{{R R}_{N N} + + h h} - - {ω ω}_{ie ie} sin sin LδL LδL - - (({ω ω}_{ie ie} sin sin L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L)) {φ φ}_{E E.} - - \frac{{v v}_{N N}}{{R R}_{M m} + + h h} {φ φ}_{U u} + + {ϵ ϵ}_{N N} \\ {\overset{\cdot &Center Dot;}{φ φ}}_{U u} = = \frac{δ δ {v v}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L + + (({ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} {sec sec}^{22} L L)) δL δ L + + (({ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h})) {φ φ}_{E E.} + + \frac{{v v}_{N N}}{{R R}_{M m} + + h h} {φ φ}_{N N} + + {ϵ ϵ}_{U u} \end{matrix} - - - - - - ((1010))$

$\{\begin{matrix} δ δ {\overset{\cdot &Center Dot;}{v v}}_{E E.} = = {f f}_{N N} {φ φ}_{U u} - - {f f}_{U u} {φ φ}_{N N} + + ((\frac{{v v}_{N N}}{{R R}_{M m} + + h h} tan the tan L L - - \frac{{v v}_{U u}}{{R R}_{M m} + + h h})) δ δ {v v}_{E E.} + + ((22 {ω ω}_{ie ie} sin sin L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L)) δ δ {v v}_{N N} \\ - - ((22 {ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h})) δ δ {v v}_{U u} + + ((22 {ω ω}_{ie ie} cos cos L L {v v}_{N N} + + \frac{{v v}_{E E.} {v v}_{N N}}{{R R}_{N N} + + h h} {sec sec}^{22} L L + + 22 {ω ω}_{ie ie} sin sin L L {v v}_{U u})) δL δL + + {&dtri; &dtri;}_{E E.} \\ δ δ {\overset{\cdot \cdot}{v v}}_{N N} = = {f f}_{U u} {φ φ}_{E E.} - - {f f}_{E E.} {φ φ}_{U u} - - 22 (({ω ω}_{ie ie} sin sin L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) δ δ {v v}_{E E.} - - \frac{{v v}_{U u}}{{R R}_{M m} + + h h} δ δ {v v}_{N N} - - \frac{{v v}_{N N}}{{R R}_{M m} + + h h} δ δ {v v}_{U u} \\ - - ((22 {ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} {sec sec}^{22} L L)) {v v}_{E E.} δL δ L + + {&dtri; &dtri;}_{N N} \\ δ δ {\overset{\cdot \cdot}{v v}}_{U u} = = {f f}_{E E.} {φ φ}_{N N} - - {f f}_{N N} {φ φ}_{E E.} + + 22 (({ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h})) δ δ {v v}_{E E.} - - 22 {ω ω}_{ie ie} sin sin L L {v v}_{E E.} δL δ L + + 22 \frac{{v v}_{N N}}{{R R}_{M m} + + h h} δ δ {v v}_{N N} + + {&dtri; &dtri;}_{U u} \end{matrix} - - - - - - ((1111))$

$\{\begin{matrix} δ δ \overset{\cdot &Center Dot;}{L L} = = \frac{δ δ {v v}_{N N}}{{R R}_{M m} + + h h} \\ δ δ \overset{\cdot &Center Dot;}{λ λ} = = \frac{δ δ {v v}_{E E.}}{{R R}_{N N} + + h h} sec sec L L + + \frac{{v v}_{E E.} sec sec L L}{{R R}_{N N} + + h h} tan the tan LδL LδL \\ δ δ \overset{\cdot &Center Dot;}{h h} = = δ δ {v v}_{U u} \end{matrix} - - - - - - ((1212))$

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

▽＝▽_b+▽_a+ω_a▽＝▽_b +▽_a +_ωa

其中，φ_E，φ_N，φ_U为东北天导航坐标系中三个姿态角，v_E、v_N、v_U为东北天的速度，L、λ、h分别为地面车辆的纬度、经度、高度，f_E、f_N、f_U分别为沿地理坐标系在东、北、天方向上的比力分量，R_M、R_N为地球子午面和卯酉面曲率半径，(X为φ，v，L、λ、h)为其对应的导数，(Y为v，L、λ、h)为其对应导数的误差，ε为陀螺仪总误差，ε_b、ε_r、ε_g分别为常值漂移、一阶马尔科夫过程和高斯白噪声，Δ为加速度计总误差，Δ_b、Δ_a、ω_a分别为常值漂移、一阶马尔科夫过程和高斯白噪声；ω_ie为地球角速度，是已知的，δv_E、δv_N、δv_U为导航坐标系下东北天速度误差，δL、δλ、δh为位置误差， $ϵ = [\begin{matrix} ϵ_{E} \\ ϵ_{N} \\ ϵ_{U} \end{matrix}]$ 和 $&dtri; = [\begin{matrix} {&dtri;}_{E} \\ {&dtri;}_{N} \\ {&dtri;}_{U} \end{matrix}]$ 分别陀螺仪和加速度计在导航坐标系下东北天的总误差，E、N、U下标分别为三个方向上的分量。Among them, φ_E , φ_N , φ_U are the three attitude angles in the northeast sky navigation coordinate system, v_E , v_N , v_U are the speeds of the northeast sky, L, λ, h are the latitude, longitude, Height, f_E , f_N , f_U are the specific force components along the geographic coordinate system in the east, north, and sky directions respectively, R_M , R_N are the curvature radii of the earth's meridian plane and Maoyou plane, (X is φ, v, L, λ, h) as its corresponding derivative, (Y is v, L, λ, h) is the error of its corresponding derivative, ε is the total error of the gyroscope, ε_b , ε_r , ε_g are constant value drift, first-order Markov process and Gaussian white noise, respectively, Δ is the total accelerometer error, Δ_b , Δ_a , and ω_a are constant value drift, first-order Markov process and Gaussian white noise respectively; ω_ie is the earth angular velocity, which is known, and δv_E , δv_N , δv_U is the speed error of the northeast sky in the navigation coordinate system, δL, δλ, and δh are the position errors, $ϵ = [\begin{matrix} ϵ_{E.} \\ ϵ_{N} \\ ϵ_{u} \end{matrix}]$ and $&dtri; = [\begin{matrix} {&dtri;}_{E.} \\ {&dtri;}_{N} \\ {&dtri;}_{u} \end{matrix}]$ Respectively, the total error of the gyroscope and accelerometer in the northeast sky in the navigation coordinate system, and the subscripts of E, N, and U are the components in the three directions respectively.

(4.4)所述的中央控制器根据上述所计算出的系统角误差、速度误差、位置误差以及惯性仪表误差，并根据卡尔曼滤波算法对所述的位置信息、速度信息以及姿态角进行误差补偿。(4.4) The central controller according to the above-mentioned calculated system angle error, velocity error, position error and inertial instrument error, and according to the Kalman filter algorithm, performs error compensation to the described position information, velocity information and attitude angle .

再进一步地，所述的根据卡尔曼滤波算法对所述的位置信息、速度信息以及姿态角进行误差补偿，具体为：Still further, the described position information, velocity information and attitude angle are compensated for errors according to the Kalman filter algorithm, specifically:

所述的根据卡尔曼滤波算法对所述的位置信息、速度信息以及姿态角进行补偿以获得十五维状态方程如下所示：The described position information, velocity information and attitude angle are compensated according to the Kalman filter algorithm to obtain the fifteen-dimensional state equation as follows:

$\overset{\cdot &Center Dot;}{X x} ((t t)) = = F f ((t t)) X x ((t t)) + + G G ((t t)) W W ((t t)) - - - - - - ((1414))$

其中，in,

X(t)＝[φ_E φ_N φ_U δv_E δv_N δv_U δL δλ δh ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T为系统的状态矢量，其中，下标E、N、U分别表示东北天地理坐标系的三个方向，φ_E、φ_N、φ_U为捷联惯导系统的误差角，δv_E、δv_N、δv_U为速度误差，δL、δλ、δh为位置误差，ε_bx、ε_by、ε_bz陀螺仪的随机漂移，▽_bx、▽_by、▽_bz加速度计的零位误差；W(t)＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T为系统过程白噪声矢量，其中，ω_gx、ω_gy、ω_gz为陀螺的白噪声，ω_ax、ω_ay、ω_az为加速度计的白噪声；F(t)为系统状态矩阵，G(t)为系统噪声传播矩阵。X(t)＝[φ_E φ_N φ_U δv_E δv_N δv_U δL δλ δh ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz ]^T is the state vector of the system, where the subscripts E, N, U Respectively represent the three directions of the northeast sky geographic coordinate system, φ_E , φ_N , φ_U are the error angles of the strapdown inertial navigation system, δv_E , δv_N , δv_U are the velocity errors, δL, δλ, δh are the position errors , ε_bx , ε_by , ε_bz gyroscope random drift, ▽_bx , ▽_by , ▽_bz accelerometer zero error; W(t)＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az ]^T is the system process white noise vector, where ω_gx , ω_gy , ω_gz are the white noise of the gyroscope, ω_ax , ω_ay , ω_az are the white noise of the accelerometer; F(t) is the system state matrix, G(t ) is the system noise propagation matrix.

采用了该发明中的用于农业机械的捷联惯导系统以及控制方法，与现有技术相比，具有以下有益效果：Adopting the strapdown inertial navigation system and control method for agricultural machinery in this invention, compared with the prior art, has the following beneficial effects:

(1)本发明采用的误差补偿和修正算法，大大减小了捷联惯导系统的算法误差和地球自转等干扰；(1) The error compensation and correction algorithm adopted in the present invention greatly reduce the interferences such as the algorithm error of the strapdown inertial navigation system and the earth's rotation;

(2)采用的六轴惯性传感器和捷联惯导系统的算法使得本发明用于农业机械的捷联惯导系统具有较高的性能参数，经拖拉机测试室外本装置输出航向角误差小于0.1°，俯仰角和翻滚角度误差小于0.01°，本发明的用于农业机械的捷联惯导系统输出的位置信息配合GPS实现的组合导航位置误差在cm级，数据输出频率达50HZ，满足农业机械辅助驾驶控制系统的要求；(2) The algorithm of the six-axis inertial sensor and the strapdown inertial navigation system adopted makes the strapdown inertial navigation system of the present invention used for agricultural machinery have higher performance parameters, and the output heading angle error of the device is less than 0.1° through the tractor test outdoor , the error of pitch angle and roll angle is less than 0.01°, the position information output by the strapdown inertial navigation system for agricultural machinery of the present invention cooperates with GPS to achieve a combined navigation position error of cm level, and the data output frequency reaches 50HZ, which meets the requirements of agricultural machinery assistance. requirements for driving control systems;

(3)本发明采用了六轴惯性传感器，其包括三个方向的加速度传感器以及三轴的陀螺仪传感器，体积小，重量轻，性价比高，模块化设计便于集成到农业机械辅助驾驶控制系统之中；(3) The present invention adopts a six-axis inertial sensor, which includes an acceleration sensor in three directions and a three-axis gyroscope sensor, which is small in size, light in weight, and high in cost performance. The modular design is convenient for integration into the auxiliary driving control system of agricultural machinery middle;

(4)本发明用于农业机械的捷联惯导系统具有运行稳定和输出运动信息丰富等优点，尤其满足农业机械等地面车辆辅助驾驶控制系统要求。(4) The strapdown inertial navigation system for agricultural machinery of the present invention has the advantages of stable operation and rich output motion information, and especially meets the requirements of auxiliary driving control systems for ground vehicles such as agricultural machinery.

附图说明Description of drawings

图1为本发明的用于农业机械的捷联惯导系统的结构示意图。Fig. 1 is a structural schematic diagram of a strapdown inertial navigation system for agricultural machinery according to the present invention.

图2为本发明的基于捷联惯导系统的用于农业机械的控制方法的步骤流程图。Fig. 2 is a flow chart of the steps of the control method for agricultural machinery based on the strapdown inertial navigation system of the present invention.

具体实施方式Detailed ways

为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。In order to describe the technical content of the present invention more clearly, further description will be given below in conjunction with specific embodiments.

本发明用于农业机械的捷联惯导系统以及控制方法采用较高精度的六轴惯性传感器，包括三个方向的加速度传感器以及三轴的陀螺仪传感器，传感器实时获取物体的运动的加速度和角速度，通过对加速度的积分可以计算出速度和再次积分可以计算出位置信息，通过对角速度的积分可以计算出车体当前姿态角(俯仰角、翻滚角和航向角)，然后将姿态换算成姿态矩阵，从而实现载体坐标系和导航坐标系的转换，此姿态矩阵起着是“数学平台”的作用。The strapdown inertial navigation system and control method for agricultural machinery of the present invention adopt a relatively high-precision six-axis inertial sensor, including three-direction acceleration sensors and three-axis gyroscope sensors, and the sensors can obtain the acceleration and angular velocity of the object's motion in real time , the velocity can be calculated by integrating the acceleration and the position information can be calculated by integrating again, the current attitude angle (pitch angle, roll angle and heading angle) of the car body can be calculated by integrating the angular velocity, and then the attitude is converted into an attitude matrix , so as to realize the conversion between the carrier coordinate system and the navigation coordinate system, and this attitude matrix plays the role of a "mathematical platform".

在SINS(Strapdown Inertial Navigation System)算法实现中，姿态矩阵尤为重要，由于农业机械时刻在运动，其姿态也在不停的变化，即姿态矩阵也要不停地进行重新计算和更新。常用的姿态更新算法有欧拉角、方向余弦和四元数，四元数与欧拉角算法相比没有奇异点，与方向余弦相比计算量小非常适合在嵌入式产品中使用。本发明在计算姿态矩阵时，采用角增量四元数姿态更新算法，但是这种算法也存在缺陷，即在利用角增量求解微分方程时由于不定轴转动会产生转动不可交换误差，所以本发明采用等效旋转矢量法求解微分方程即可对此误差进行修正。In the implementation of the SINS (Strapdown Inertial Navigation System) algorithm, the attitude matrix is particularly important. Since the agricultural machinery is always moving, its attitude is constantly changing, that is, the attitude matrix must be constantly recalculated and updated. Commonly used attitude update algorithms include Euler angle, direction cosine and quaternion. Quaternion has no singularity compared with Euler angle algorithm. Compared with direction cosine, quaternion has less calculation and is very suitable for use in embedded products. The present invention adopts the angle increment quaternion attitude update algorithm when calculating the attitude matrix, but this algorithm also has defects, that is, when the angle increment is used to solve the differential equation, the rotation of the indefinite axis will produce a non-commutable error of rotation, so the present invention The invention adopts the equivalent rotation vector method to solve the differential equation to correct the error.

针对地球自转、锥运动效应、划船效应和涡卷效应等误差干扰，本发明中采用多种补偿和修正算法对这些误差进行处理。Aiming at error interferences such as the earth's rotation, cone motion effect, rowing effect and vortex effect, the present invention adopts multiple compensation and correction algorithms to process these errors.

请参阅图1所示，为本发明的用于农业机械的捷联惯导系统的结构示意图，其中所述的系统包括六轴惯性传感器、中央控制器；所述的六轴惯性传感器包括三个方向的加速度传感器以及三轴的陀螺仪传感器；所述的中央控制器包括：Please refer to Fig. 1, which is a structural schematic diagram of the strapdown inertial navigation system for agricultural machinery of the present invention, wherein the system includes a six-axis inertial sensor and a central controller; the six-axis inertial sensor includes three The acceleration sensor of direction and the gyroscope sensor of three axes; Described central controller comprises:

请参阅图2所示，为本发明的基于捷联惯导系统的用于农业机械的控制方法的步骤流程图。其中，所述的方法包括以下步骤：Please refer to FIG. 2 , which is a flow chart of the steps of the control method for agricultural machinery based on the strapdown inertial navigation system of the present invention. Wherein, described method comprises the following steps:

首先，计算姿态矩阵，姿态矩阵是指从导航坐标系(n系)到载体坐标系(b系)的变换矩阵，当采用“东北天”地理坐标系为导航坐标系时，姿态矩阵为：First, calculate the attitude matrix. The attitude matrix refers to the transformation matrix from the navigation coordinate system (n system) to the carrier coordinate system (b system). When the "Northeast Sky" geographic coordinate system is used as the navigation coordinate system, the attitude matrix is:

${C C}_{n no}^{b b} = = [\begin{matrix} cos cos γ γ cos cos ψ ψ + + sin sin γ γ sin sin θ θ sin sin ψ ψ & - - cos cos γ γ sin sin ψ ψ + + sin sin γ γ sin sin θ θ cos cos ψ ψ & - - sin sin γ γ cos cos θ θ \\ cos cos θ θ sin sin ψ ψ & cos cos θ θ cos cos ψ ψ & sin sin θ θ \\ sin sin γ γ cos cos ψ ψ - - cos cos γ γ sin sin θ θ sin sin ψ ψ & - - sin sin γ γ sin sin ψ ψ - - cos cos γ γ sin sin θ θ cos cos ψ ψ & cos cos γ γ cos cos θ θ \end{matrix}]$

式中，ψ为方位角(航向角)，θ为俯仰角，γ为横滚角(翻滚角)，这三个角称为载体的姿态角(由初始化后的四元数提供，以后的姿态矩阵是由四元数计算提供的)。In the formula, ψ is the azimuth (heading angle), θ is the pitch angle, and γ is the roll angle (roll angle). These three angles are called the attitude angle of the carrier (provided by the quaternion after initialization, and the attitude matrices are provided by quaternion calculations).

当六轴惯性传感器固连的农业机械姿态发生变化时，六轴惯性传感器中的陀螺仪传感器就能敏感出相应的角速率，姿态矩阵随之发生了变化，其微分方程为：式中，为角速度 $ω_{nb}^{b} = {[\begin{matrix} ω_{nb}^{bx} & ω_{nb}^{by} & ω_{nb}^{bz} \end{matrix}]}^{T}$ 构成的反对称阵；x，y，z定义为右前上三个方向。When the attitude of the agricultural machinery connected to the six-axis inertial sensor changes, the gyroscope sensor in the six-axis inertial sensor can sense the corresponding angular rate, attitude matrix Then there is a change, and its differential equation is: In the formula, is the angular velocity $ω_{nb}^{b} = {[\begin{matrix} ω_{nb}^{bx} & ω_{nb}^{by} & ω_{nb}^{bz} \end{matrix}]}^{T}$ The anti-symmetric matrix formed; x, y, z are defined as the three directions of right front and upper.

捷联惯导系统的姿态矩阵的即时修正就是实时地给出姿态矩阵，它是捷联惯导的关键任务，而这要通过一定的算法来完成。由于四元数算数法计算量小，存储容量小，仅需要进行简单的四元数规范化处理便可以保证姿态矩阵的正交性。单位四元数可用如下形式的来描述：The real-time correction of the attitude matrix of the SINS is to give the attitude matrix in real time, which is the key task of the SINS, and this must be done through a certain algorithm. Due to the small amount of calculation and small storage capacity of the quaternion arithmetic method, only a simple quaternion normalization process is required to ensure the orthogonality of the attitude matrix. A unit quaternion can be described in the following form:

$Q Q = = {q q}_{00} + + {q q}_{11} i i + + {q q}_{22} j j + + {q q}_{33} k k = = [\begin{matrix} {q q}_{00} \\ {q q}_{11} \\ {q q}_{22} \\ {q q}_{33} \end{matrix}]$

在捷联导航中，要求载体系到导航系的转换矩阵，要解下列四元数的运动方程： $\overset{\cdot}{Q} = \frac{1}{2} ΩQ,$ 式中， $Ω = [\begin{matrix} 0 & - ω_{nb}^{bx} & - ω_{nb}^{by} & - ω_{nb}^{bz} \\ ω_{nb}^{bx} & 0 & ω_{nb}^{bz} & - ω_{nb}^{by} \\ ω_{nb}^{by} & - ω_{nb}^{bz} & 0 & ω_{nb}^{bx} \\ ω_{nb}^{bz} & ω_{nb}^{by} & ω_{nb}^{bx} & 0 \end{matrix}],$ $Q = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}],$ Q是从载体系到导航系的转动四元数。In strapdown navigation, the transformation matrix from the carrier system to the navigation system is required, and the following quaternion equations of motion must be solved: $\overset{\cdot}{Q} = \frac{1}{2} ΩQ,$ In the formula, $Ω = [\begin{matrix} 0 & - ω_{nb}^{bx} & - ω_{nb}^{by} & - ω_{nb}^{bz} \\ ω_{nb}^{bx} & 0 & ω_{nb}^{bz} & - ω_{nb}^{by} \\ ω_{nb}^{by} & - ω_{nb}^{bz} & 0 & ω_{nb}^{bx} \\ ω_{nb}^{bz} & ω_{nb}^{by} & ω_{nb}^{bx} & 0 \end{matrix}],$ $Q = [\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix}],$ Q is the rotation quaternion from the carrier frame to the navigation frame.

其中，导航坐标系为“东北天”地理坐标系，所述的姿态矩阵计算模块根据所述的速度位置计算模块计算的位置角速度值以及所接收的陀螺仪传感器的角速度值计算获得更新后的姿态矩阵，具体包括以下步骤：Wherein, the navigation coordinate system is the "northeast sky" geographic coordinate system, and the attitude matrix calculation module calculates and obtains the updated attitude according to the position angular velocity value calculated by the velocity position calculation module and the received angular velocity value of the gyroscope sensor matrix, including the following steps:

其中，在一种优选的实时方式中，所述的步骤(2.1)具体为：Wherein, in a preferred real-time manner, the step (2.1) is specifically:

这样，就可以求得，In this way, one can obtain,

$[\begin{matrix} {ω ω}_{nb nb}^{bx bx} \\ {ω ω}_{nb nb}^{by by} \\ {ω ω}_{nb nb}^{bz bz} \end{matrix}] = = [\begin{matrix} {ω ω}_{bx bx} \\ {ω ω}_{by by} \\ {ω ω}_{bz bz} \end{matrix}] - - {C C}_{n no}^{b b} (([\begin{matrix} 00 \\ {ω ω}_{e e} cos cos L L \\ {ω ω}_{e e} sin sin L L \end{matrix}] + + [\begin{matrix} - - \frac{{v v}_{N N}}{{R R}_{M m} + + h h} \\ \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} \\ \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L \end{matrix}])) = = [\begin{matrix} {ω ω}_{bx bx} \\ {ω ω}_{by by} \\ {ω ω}_{bz bz} \end{matrix}] - - {C C}_{b b}^{n no} [\begin{matrix} - - \frac{{v v}_{N N}}{{R R}_{M m} + + h h} \\ {ω ω}_{e e} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} \\ {ω ω}_{e e} sin sin L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L \end{matrix}]$

地球坐标系是固连在地球上的坐标系，随地球一起转动，它相对惯性坐标系以地球自转角速度ωie转动，ωie＝15*pi/180。The earth coordinate system is a coordinate system fixed on the earth and rotates with the earth. It rotates at the earth's rotation angular velocity ωie relative to the inertial coordinate system, ωie=15*pi/180.

陀螺仪传感器的输出通常是角速度，因此，为了计算运载体姿态，引入四元数微分方程。引入微分方程的好处是，根据上一时刻姿态四元数，通过定时采样载体坐标系的三轴角增量即可求出新的姿态四元数(即载体坐标系相对于导航坐标系的姿态矩阵)。通过等效旋转矢量法求得的角度增量能够消除转动的不可交换误差；所述的步骤(2.2)具体包括以下步骤：The output of the gyro sensor is usually angular velocity, therefore, in order to calculate the attitude of the vehicle, the quaternion differential equation is introduced. The advantage of introducing the differential equation is that according to the attitude quaternion at the last moment, the new attitude quaternion (that is, the attitude of the carrier coordinate system relative to the navigation coordinate system) can be obtained by regularly sampling the three-axis angle increment of the carrier coordinate system. matrix). The angle increment obtained by the equivalent rotation vector method can eliminate the non-exchangeable error of rotation; the described step (2.2) specifically includes the following steps:

所述的姿态矩阵计算模块根据所述的姿态速率以及等效旋转矢量算法获得四元数更新微分方程以获得第一姿态矩阵，所述的四元数微分方程为：The attitude matrix calculation module obtains the quaternion update differential equation according to the attitude rate and the equivalent rotation vector algorithm to obtain the first attitude matrix, and the quaternion differential equation is:

$Q Q ((t t)) = = ((I I cos cos \frac{Δθ Δθ}{22} + + [[Δθ Δθ]] sin sin \frac{\frac{Δθ Δθ}{22}}{Δθ Δθ})) \cdot &Center Dot; Q Q ((00)) - - - - - - ((44))$

所述的姿态矩阵计算模块将所述的第一姿态矩阵归一化后获得更新后的姿态矩阵，具体为：The attitude matrix calculation module obtains the updated attitude matrix after normalizing the first attitude matrix, specifically:

所述的更新后的姿态矩阵为：The updated attitude matrix is:

导航坐标系为“东北天”地理坐标系，所述的坐标变换模块根据更新后的姿态矩阵将所述的加速度传感器发送的所述的农业机械的载体坐标系的加速度转换为航天坐标系的加速度，具体为：The navigation coordinate system is the "northeast sky" geographic coordinate system, and the coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machinery sent by the acceleration sensor into the acceleration of the aerospace coordinate system according to the updated attitude matrix ,Specifically:

所述的坐标变换模块根据更新后的姿态矩阵并通过以下公式将所述的加速度传感器发送的所述的农业机械的载体坐标系的加速度转换为航天坐标系的加速度：The coordinate transformation module converts the acceleration of the carrier coordinate system of the agricultural machinery sent by the acceleration sensor into the acceleration of the aerospace coordinate system according to the updated attitude matrix and through the following formula:

导航坐标系为“东北天”地理坐标系，所述的速度位置计算模块输出所述的农业机械的位置信息以及速度信息，具体包括以下步骤：The navigation coordinate system is the "northeast sky" geographic coordinate system, and the speed and position calculation module outputs the position information and speed information of the agricultural machinery, which specifically includes the following steps:

9、根据权利要求8所述的实现农业机械的辅助驾驶的控制方法，其特征在于，所述的步骤(4.1)具体为：9. The control method for realizing assisted driving of agricultural machinery according to claim 8, characterized in that, the step (4.1) is specifically:

其中， $ω_{ie}^{n} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}],$ $ω_{en}^{n} = [\begin{matrix} - \frac{v_{N}}{R_{M} + h} \\ \frac{v_{E}}{R_{N} + h} \\ \frac{v_{E}}{R_{N} + h} \tan L \end{matrix}],$ $f^{n} = [\begin{matrix} f_{E} \\ f_{N} \\ f_{U} \end{matrix}],$ ${\overset{\cdot}{v}}^{n} = [\begin{matrix} {\overset{\cdot}{v}}_{E} \\ {\overset{\cdot}{v}}_{N} \\ {\overset{\cdot}{v}}_{U} \end{matrix}],$ $v^{n} = [\begin{matrix} v_{E} \\ v_{N} \\ v_{U} \end{matrix}],$ $g = [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}]$ 公式(8)的矩阵表示为：in, $ω_{ie}^{no} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}],$ $ω_{en}^{no} = [\begin{matrix} - \frac{v_{N}}{R_{m} + h} \\ \frac{v_{E.}}{R_{N} + h} \\ \frac{v_{E.}}{R_{N} + h} the tan L \end{matrix}],$ $f^{no} = [\begin{matrix} f_{E.} \\ f_{N} \\ f_{u} \end{matrix}],$ ${\overset{\cdot}{v}}^{no} = [\begin{matrix} {\overset{\cdot}{v}}_{E.} \\ {\overset{&Center Dot;}{v}}_{N} \\ {\overset{\cdot}{v}}_{u} \end{matrix}],$ $v^{no} = [\begin{matrix} v_{E.} \\ v_{N} \\ v_{u} \end{matrix}],$ $g = [\begin{matrix} 0 \\ 0 \\ - g \end{matrix}]$ The matrix of formula (8) is expressed as:

$[\begin{matrix} {\overset{\cdot \cdot}{v v}}_{E E.} \\ {\overset{\cdot \cdot}{v v}}_{N N} \\ {\overset{\cdot &Center Dot;}{v v}}_{U u} \end{matrix}] = = [\begin{matrix} {f f}_{E E.} \\ {f f}_{N N} \\ {f f}_{U u} \end{matrix}] - - [\begin{matrix} 00 & - - ((22 {ω ω}_{iez iez}^{n no} + + {ω ω}_{enz enz}^{n no})) & 22 {ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no} \\ 22 {ω ω}_{iez iez}^{n no} + + {ω ω}_{enz enz}^{n no} & 00 & - - ((22 {ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no})) \\ - - ((22 {ω ω}_{iey iey}^{n no} + + {ω ω}_{eny enny}^{n no})) & 22 {ω ω}_{iex iex}^{n no} + + {ω ω}_{enx enx}^{n no} & 00 \end{matrix}] [\begin{matrix} {v v}_{E E.} \\ {v v}_{N N} \\ {v v}_{U u} \end{matrix}] + + [\begin{matrix} 00 \\ 00 \\ - - g g \end{matrix}]$

其中，为地球角速度，是已知的，为 $ω_{ie}^{n} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}];$ 为导航坐标系相对地球的角速度，其可以由瞬时速度求得，且 $ω_{en}^{n} = [\begin{matrix} - \frac{v_{N}}{R_{M} + h} \\ ω_{e} \cos L + \frac{v_{E}}{R_{N} + h} \\ ω_{e} \sin L + \frac{v_{E}}{R_{N} + h} \tan L \end{matrix}],$ L、λ、h分别为纬度、经度、高度；ψ为方位角，θ为俯仰角，γ为横滚角，v_E、v_N、v_U为东北天的速度，由速度位置计算模块提供，R_M、R_N为地球子午面和卯酉面曲率半径，R_M≈R(1-2e+3esin²L)，R_N≈R(1+esin²L)，其中，R＝m，e＝1/298.257)，f_E、f_N、f_U分别为沿地理坐标系在东、北、天方向上的比力分量，ω_e地球自转合角速度。in, is the angular velocity of the earth, which is known as $ω_{ie}^{no} = [\begin{matrix} 0 \\ ω_{e} \cos L \\ ω_{e} \sin L \end{matrix}];$ is the angular velocity of the navigation coordinate system relative to the earth, which can be determined by the instantaneous velocity obtain, and $ω_{en}^{no} = [\begin{matrix} - \frac{v_{N}}{R_{m} + h} \\ ω_{e} \cos L + \frac{v_{E.}}{R_{N} + h} \\ ω_{e} \sin L + \frac{v_{E.}}{R_{N} + h} the tan L \end{matrix}],$ L, λ, and h are latitude, longitude, and altitude respectively; ψ is the azimuth angle, θ is the pitch angle, γ is the roll angle, v_E , v_N , and v_U are the speeds of the northeast sky, which are provided by the speed and position calculation module, R_M and R_N are the radii of curvature of the earth's meridian plane and Maoyou plane, R_M ≈ R(1-2e+3esin² L), R_N ≈R(1+esin² L), where R=m, e= 1/298.257), f_E , f_N , and f_U are the specific force components along the geographic coordinate system in the east, north, and sky directions, respectively, and ω_{e is} the resultant angular velocity of the earth's rotation.

为了提高计算速度，本发明中采用二阶龙格-库塔数值积分法计算速度更新，公式如下：In order to improve the calculation speed, the second-order Runge-Kutta numerical integration method is used to calculate the speed update in the present invention, and the formula is as follows:

式中，K₁和K₂为速度在t_m和t_m+1时的斜率；所述的步骤(4.2)具体为： In the formula, K₁ and K₂ are the slopes of the speed at t_m and t_m+1 ; the described step (4.2) is specifically:

其中，L、λ、h分别为地面车辆的纬度、经度、高度，v_E、v_N、v_U为东北天的速度，由速度位置计算模块提供，k为采样点，T为采样周期。Among them, L, λ, and h are the latitude, longitude, and height of the ground vehicle, respectively; v_E , v_N , and v_U are the speeds in the northeast sky, which are provided by the speed and position calculation module; k is the sampling point, and T is the sampling period.

在经度、纬度进行更新计算后，将计算结果L(k)代入计算kT时刻的R_M(k)、R_N(k)After updating the longitude and latitude, substitute the calculation result L(k) into the R_M (k) and R_N (k) at the time kT

R_M(k)＝R_e[1-2f+3fsin²L(k)]，R_N(k)＝R_e[1+fsin²L(k)]，式中，R_e为地球半径，f为椭圆度。重力随纬度和高度的变化规律近似为:R_M (k)＝R_e [1-2f+3fsin² L(k)], R_N (k)＝R_e [1+fsin² L(k)], where R_e is the radius of the earth, f is the ellipticity. The variation law of gravity with latitude and height is approximated as:

g＝g₀[l+0.00527094sin²(L)+0.0000232718sin⁴(L)]-0.000003086h，式中，g₀＝9.7803267714。g=g₀ [l+0.00527094sin² (L)+0.0000232718sin⁴ (L)]-0.000003086h, where g₀ =9.7803267714.

导航坐标系为“东北天”地理坐标系，所述的姿态计算模块根据所述的姿态矩阵计算模块的更新后的姿态矩阵输出所述的农业机械的姿态角，具体包括以下步骤：The navigation coordinate system is the "northeast sky" geographic coordinate system, 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, which specifically includes the following steps:

在一种优选的实施方式中，农业机械的姿态角可从更新计算后的姿态矩阵中提取，包括俯仰角、翻滚角和航向角，由于俯仰角θ定义在±90°区间，和反正弦函数的主值一致，不存在多值问题。而翻滚角γ定义在[-180°,180°]区间，航向角ψ定义在[0°,360°]区间，故γ、ψ都存在多值问题，在计算出主值之后，可由中的元素判断是在哪个象限。In a preferred embodiment, the attitude angle of the agricultural machinery can be calculated from the updated attitude matrix Extracted from , including pitch angle, roll angle and heading angle, since the pitch angle θ is defined in the ±90° interval, which is consistent with the main value of the arcsine function, there is no multi-value problem. The roll angle γ is defined in the interval [-180°, 180°], and the heading angle ψ is defined in the interval [0°, 360°], so both γ and ψ have multi-value problems. After calculating the main value, it can be determined by Which quadrant is judged by the elements in.

由于because

${C C}_{n no}^{b b} = = (\begin{matrix} {C C}_{1111} & {C C}_{1212} & {C C}_{1313} \\ {C C}_{21 twenty one} & {C C}_{22 twenty two} & {C C}_{23 twenty three} \\ {C C}_{3131} & {C C}_{3232} & {C C}_{3333} \end{matrix}) (\begin{matrix} cos cos γ γ cos cos ψ ψ + + sin sin γ γ sin sin ψ ψ sin sin θ θ & sin sin ψ ψ cos cos θ θ & sin sin γ γ cos cos ψ ψ - - cos cos γ γ sin sin ψ ψ sin sin θ θ \\ - - cos cos γ γ sin sin ψ ψ + + sin sin γ γ cos cos ψ ψ sin sin θ θ & cos cos ψ ψ cos cos θ θ & - - sin sin γ γ sin sin ψ ψ - - cos cos γ γ cos cos ψ ψ sin sin θ θ \\ - - sin sin γ γ cos cos θ θ & sin sin θ θ & cos cos γ γ cos cos θ θ \end{matrix})$

对于俯仰角：θ＝θ_主；For pitch angle: θ = θ_main ;

对于翻滚角：For roll angle:

对于航向角：For heading angle:

所以：so:

必须指出，由于纯惯导高度通道的高度不稳定，包括加速度传感器误差在内的一些误差源会形成积累性误差，高度误差会随时间的增加而加速增加。因此不能仅仅用速度来计算农业机械在较长时间内的高度信息，必须借助气压高度计或无线电高度表信号来进行修正。由于在纯惯导系统中高度通道是发散的，可用外来高度参考信息引入阻尼，在此对于高度以及和高度相关的项不予考虑。It must be pointed out that due to the altitude instability of the pure inertial navigation altitude channel, some error sources including acceleration sensor errors will form cumulative errors, and the altitude error will increase with time. Therefore, the height information of agricultural machinery in a long period of time cannot be calculated only by speed, and must be corrected with the help of barometric altimeter or radio altimeter signals. Since the altitude channel is divergent in the pure inertial navigation system, the external altitude reference information can be used to introduce damping, and the altitude and the items related to the altitude are not considered here.

误差分析：惯性导航系统的误差源有很多，主要有惯性仪表本身的误差，惯性仪表的安装误差和标度误差，系统的初始条件误差，系统的计算误差以及各种干扰引起的误差等。惯导误差可分为两类：确定性误差和随机误差。确定性误差包括平台角误差、速度误差以及位置误差，随机误差主要是陀螺仪传感器漂移和加速度传感器的零位偏置等。尽管惯导系统存在多种误差源，但其中部分误差源对惯导系统的影响很小。由于捷联惯性导航系统采用数学平台代替物理平台，即用陀螺仪传感器测量的角速度信息进行姿态矩阵计算，用加速度传感器测量的比力信息经姿态矩阵变换进行导航计算，所以惯性传感器的误差和初始条件误差通过姿态矩阵在系统中传播，对导航产生重要的影响。也就是说，陀螺仪传感器漂移，加速度传感器零偏误差和初始值误差这三种误差源对导航参数有一定程度的影响。在下述捷联惯导系统误差模型的描述中，捷联惯性导航坐标系(n系)采用“东北天”地理坐标系，导航信息误差为9维，三维平台误差角，三维速度误差和三维位置误差。Error analysis: There are many sources of error in the inertial navigation system, mainly including the error of the inertial instrument itself, the installation error and scale error of the inertial instrument, the initial condition error of the system, the calculation error of the system, and the errors caused by various disturbances. Inertial navigation errors can be divided into two categories: deterministic errors and random errors. Deterministic errors include platform angle errors, velocity errors, and position errors, and random errors are mainly gyroscope sensor drift and acceleration sensor zero offset. Although there are many error sources in the inertial navigation system, some of them have little influence on the inertial navigation system. Since the strapdown inertial navigation system uses a mathematical platform instead of a physical platform, that is, the angular velocity information measured by the gyroscope sensor is used to calculate the attitude matrix, and the specific force information measured by the acceleration sensor is transformed through the attitude matrix to perform navigation calculations, so the error of the inertial sensor and the initial The conditional error propagates through the system through the attitude matrix and has an important impact on navigation. That is to say, the three error sources of gyro sensor drift, acceleration sensor zero bias error and initial value error have a certain degree of influence on the navigation parameters. In the description of the error model of the strapdown inertial navigation system below, the strapdown inertial navigation coordinate system (n system) adopts the "northeast sky" geographic coordinate system, and the navigation information error is 9-dimensional, 3-dimensional platform error angle, 3-dimensional velocity error and 3-dimensional position error.

导航坐标系为“东北天”地理坐标系，所述的步骤(4)与步骤(5)之间还包括以下步骤：The navigation coordinate system is the "northeast sky" geographic coordinate system, and the following steps are also included between the described step (4) and the step (5):

$\{\begin{matrix} δ δ {\overset{\cdot &Center Dot;}{v v}}_{E E.} = = {f f}_{N N} {φ φ}_{U u} - - {f f}_{U u} {φ φ}_{N N} + + ((\frac{{v v}_{N N}}{{R R}_{M m} + + h h} tan the tan L L - - \frac{{v v}_{U u}}{{R R}_{M m} + + h h})) δ δ {v v}_{E E.} + + ((22 {ω ω}_{ie ie} sin sin L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tan the tan L L)) δ δ {v v}_{N N} \\ - - ((22 {ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h})) δ δ {v v}_{U u} + + ((22 {ω ω}_{ie ie} cos cos L L {v v}_{N N} + + \frac{{v v}_{E E.} {v v}_{N N}}{{R R}_{N N} + + h h} {sec sec}^{22} L L + + 22 {ω ω}_{ie ie} sin sin L L {v v}_{U u})) δL δ L + + {&dtri; &dtri;}_{E E.} \\ δ δ {\overset{\cdot &Center Dot;}{v v}}_{N N} = = {f f}_{U u} {φ φ}_{E E.} - - {f f}_{E E.} {φ φ}_{U u} - - 22 (({ω ω}_{ie ie} sin sin L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} tgL wxya)) δ δ {v v}_{E E.} - - \frac{{v v}_{U u}}{{R R}_{M m} + + h h} δ δ {v v}_{N N} - - \frac{{v v}_{N N}}{{R R}_{M m} + + h h} δ δ {v v}_{U u} \\ - - ((22 {ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h} {sec sec}^{22} L L)) {v v}_{E E.} δL δ L + + {&dtri; &dtri;}_{N N} \\ δ δ {\overset{\cdot &Center Dot;}{v v}}_{U u} = = {f f}_{E E.} {φ φ}_{N N} - - {f f}_{N N} {φ φ}_{E E.} + + 22 (({ω ω}_{ie ie} cos cos L L + + \frac{{v v}_{E E.}}{{R R}_{N N} + + h h})) δ δ {v v}_{E E.} - - 22 {ω ω}_{ie ie} sin sin L L {v v}_{E E.} δL δ L + + 22 \frac{{v v}_{N N}}{{R R}_{M m} + + h h} δ δ {v v}_{N N} + + {&dtri; &dtri;}_{U u} \end{matrix} - - - - - - ((1111))$

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

▽＝▽_b+▽_a+ω_a▽＝▽_b +▽_a +_ωa

其它误差，包括锥运动误差、划船误差和涡卷误差等，其中锥运动误差可通过对旋转矢量算法优化得到补偿，而划船误差和涡卷误差通过matlab仿真可得对捷联惯导系统的影响较小，可以忽略。Other errors include cone motion error, rowing error and vortex error, among which the cone motion error can be compensated by optimizing the rotation vector algorithm, while the rowing error and vortex error can be simulated by matlab to obtain the impact on the strapdown inertial navigation system Small and can be ignored.

通过以上对误差项公式，可通过建立准确数学模型经过卡尔曼滤波算法进行补偿。综合以上各式，可得组合导航系统在位置速度组合模式下的15维状态方程；所述的根据卡尔曼滤波算法对所述的位置信息、速度信息以及姿态角进行误差补偿，具体为：Through the above formula for the error term, it can be compensated by establishing an accurate mathematical model through the Kalman filter algorithm. Combining the above formulas, the 15-dimensional state equation of the integrated navigation system under the position-velocity combination mode can be obtained; the described position information, velocity information and attitude angle are compensated according to the Kalman filter algorithm, specifically:

$\overset{\cdot \cdot}{X x} ((t t)) = = F f ((t t)) X x ((t t)) + + G G ((t t)) W W ((t t)) - - - - - - ((1414))$

其中，in,

其中，卡尔曼滤波算法为通用技术，但是其数学模型和参数的选定为其重要技术也是区分点，本发明专利充分考虑到农用机械自动控制系统中的各类误差项，并对其进行数学建模，通过软件编程实现上述滤波算法，并经过实地拖拉机试验得出姿态误差角小于0.1°，航向角误差小于1°，位置偏差5cm左右的试验结果。Among them, the Kalman filter algorithm is a general technology, but the selection of its mathematical model and parameters is also a distinguishing point for its important technology. The patent of the invention fully considers various error items in the automatic control system of agricultural machinery, and performs mathematical Modeling, the above filtering algorithm is realized through software programming, and the test results that the attitude error angle is less than 0.1°, the heading angle error is less than 1°, and the position deviation is about 5cm are obtained through the field tractor test.

(2)本发明采用的误差补偿和修正算法，大大减小了捷联惯导系统的算法误差和地球自转等干扰；(2) The error compensation and correction algorithm adopted in the present invention greatly reduce the interferences such as the algorithm error of the strapdown inertial navigation system and the earth's rotation;

(2)采用的六轴惯性传感器和捷联惯导系统的算法使得本发明用于农业机械的捷联惯导系统具有较高的性能参数，经拖拉机测试室外本装置输出航向角误差小于1°，俯仰角和翻滚角度误差小于0.1°，本发明的用于农业机械的捷联惯导系统输出的位置信息配合GPS实现的组合导航位置误差在cm级，数据输出频率达50HZ，满足农业机械辅助驾驶控制系统的要求；(2) The algorithm of the six-axis inertial sensor and the strapdown inertial navigation system adopted makes the strapdown inertial navigation system for agricultural machinery of the present invention have higher performance parameters, and the output heading angle error of the device is less than 1° through the tractor test outdoor , the error of pitch angle and roll angle is less than 0.1°, the position information output by the strapdown inertial navigation system for agricultural machinery of the present invention cooperates with GPS to achieve a combined navigation position error of cm level, and the data output frequency reaches 50HZ, which meets the requirements of agricultural machinery assistance. requirements for driving control systems;

在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。In this specification, the invention has been described with reference to specific embodiments thereof. However, it is obvious that various modifications and changes can be made without departing from the spirit and scope of the invention. Accordingly, the specification and drawings are to be regarded as illustrative rather than restrictive.