CN111949030A

CN111949030A - Agricultural machinery positioning method, agricultural machinery vehicle and storage medium

Info

Publication number: CN111949030A
Application number: CN202010823768.4A
Authority: CN
Inventors: 罗尤春; 张子旭; 徐朝文; 宋鹏
Original assignee: Jiangsu Changfa Agricultural Equipment Co Ltd
Current assignee: Jiangsu Changfa Agricultural Equipment Co Ltd
Priority date: 2020-08-17
Filing date: 2020-08-17
Publication date: 2020-11-17
Anticipated expiration: 2040-08-17
Also published as: CN111949030B

Abstract

The invention provides an agricultural machinery positioning method, an agricultural machinery vehicle and a storage medium, wherein the method comprises the following steps: judging whether the receiving of the base station signal is interrupted; when the base station signal is determined to be received and interrupted at the current moment, judging whether the course is normal or not; and calculating to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the observation position coordinate, the course judgment result, the real-time vehicle speed, the time interval and the course angle at the current moment. The invention realizes low-cost high-precision positioning.

Description

Agricultural machinery positioning method, agricultural machinery vehicle and storage medium

Technical Field

The invention relates to the field of automatic driving control of agricultural machinery, in particular to an agricultural machinery positioning method, an agricultural machinery vehicle and a storage medium.

Background

The agricultural machinery automatic driving system consists of an agricultural machinery vehicle-mounted part and a ground base station part, wherein the agricultural machinery vehicle-mounted part comprises a vehicle-mounted satellite receiver, a control unit, a display unit, a hydraulic valve group, a wheel angle measuring device and the like, and the ground base station part can be a portable movable reference station, a fixed reference station or a foundation enhancement network (CORS) and the like.

The rapid development of the automatic driving automobile in recent years becomes the focus of the competition of the whole automobile enterprise and even the IT enterprise. Accurate vehicle position and heading are critical to automated driving performance. The method for estimating the position, the course and the position delay time by fusing multi-source information becomes a problem which needs to be solved urgently. The agricultural machinery automatic driving system guides the agricultural machinery automatic driving operation, the vehicle-mounted satellite receiver must receive a correction signal from the ground base station part, the correction signal generally adopts radio station communication (such as 410 MHz-470 MHz electromagnetic wave) or mobile communication network signals (such as 4G, 3G and 2G signals), and the updating frequency of the correction signal is generally 1 Hz. In a farm farmland area, a radio station signal or a network signal is frequently interrupted for a short time, after the signal is interrupted, an automatic driving system of the agricultural machine exits an automatic driving mode and does not control a vehicle any more, but if the signal is interrupted by a correction signal of a reference station in the automatic driving process of the agricultural machine, the efficiency of the automatic driving operation of the agricultural machine is greatly influenced.

Two solutions are generally adopted in the field for positioning agricultural vehicles, which are respectively described as follows:

firstly, estimating the navigation position, wherein the navigation position estimation algorithm is to start from the position of the agricultural vehicle at the previous moment, calculate the position of the agricultural vehicle at the current moment according to the current running course and the real-time vehicle speed, and then calculate the position of the agricultural vehicle at the next moment from the position of the current moment, and repeating the steps, thereby being the basic principle of the navigation position estimation method.

The basic formula of the dead reckoning algorithm is as follows:

Y_k＝Y_k-1+V·DT·cosψ_k

X_k＝X_k-1+V·DT·sinψ_k (1)

in the formula (X)_k，Y_k) Is an estimated position value of the current time k, (X)_k-1，Y_k-1) A presumed position value at the previous time k-1, V representing the real-time speed of the agricultural vehicle at the present time, DT being a time interval, psi_kIs the vehicle heading angle at time k. FIG. 1 shows a dead reckoning calculation method, and FIG. 2 showsError accumulation using dead reckoning positioning is used. After the base station signal is interrupted, the position of the agricultural vehicle is estimated by adopting a dead reckoning algorithm, high-precision positioning can be kept only for a short time, the effective time can only reach 3-5 s, the time is prolonged, and the dead reckoning error obviously drifts.

Inertial navigation, namely, using an inertial navigation unit IMU (inertial Measurement unit) to estimate the position. Inertial navigation units typically include a three-axis gyroscope and a three-axis accelerometer. The gyroscope is an instrument for measuring angular velocity, and works by means of inertia, and the rotating rate and the direction of a single-shaft rotating gyroscope are kept unchanged in space under the condition of no external torque. A single axis gyroscope may be used to measure angular velocity in one plane. The three-axis gyroscope refers to a gyroscope which is respectively arranged on three mutually vertical axes in space and used for measuring the three-dimensional rotation motion of the agricultural vehicle. The accelerometer measures the motion acceleration of the agricultural vehicle, and the general small accelerometer adopts a Micro Electro Mechanical System (MEMS), and the three-axis accelerometer can measure the motion acceleration of the agricultural vehicle in three vertical directions.

The inertial navigation unit IMU generally consists of a total of three gyroscopes and three accelerometers mounted on three mutually perpendicular axes, a pair of gyroscope and accelerometer on each axis measuring angular velocity and acceleration in the corresponding direction, respectively, and can determine three position coordinates and three attitude angles (heading angle Yaw, Pitch angle Pitch, Roll angle) of the agricultural vehicle in three-dimensional space by calculation.

After the base station signal is interrupted, the position and orientation can be calculated by adopting an inertial navigation unit configured on the agricultural vehicle. The inertial navigation unit has the defects that the cost is high, the pose error can drift along with the increase of time, so that the high positioning precision can be kept in a short time, and the inertial navigation unit with the service life of more than thousand yuan RMB can only maintain the high-precision positioning of 10-20 seconds in the industry.

Therefore, how to realize high-precision positioning with low cost after the interruption of the base station signal is a problem to be solved by those skilled in the art.

Disclosure of Invention

The invention aims to provide an agricultural machinery positioning method, an agricultural machinery vehicle and a storage medium, which can solve the problem of short-time signal interruption of an agricultural machinery automatic driving system only through an algorithm without increasing an inertial navigation unit after a base station signal is interrupted, and can realize the positioning of the position of the vehicle with high precision on the premise of saving the product cost.

The technical scheme provided by the invention is as follows:

the invention provides an agricultural machinery positioning method, which comprises the following steps:

judging whether the receiving of the base station signal is interrupted;

when the base station signal is determined to be received and interrupted at the current moment, judging whether the course is normal or not;

and calculating to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the observation position coordinate, the course judgment result, the real-time vehicle speed, the time interval and the course angle at the current moment.

Further, the determining whether the reception of the base station signal is interrupted includes:

reading a GPS navigation protocol statement output by the vehicle-mounted satellite receiver at the current moment;

if the positioning flag bit of the GPS navigation protocol statement is a first preset value, determining that the base station signal is normally received at the current moment;

and if the positioning mark bit of the GPS navigation protocol statement is a second preset value, determining that the receiving of the base station signal is interrupted at the current moment.

Further, the step of judging whether the heading is normal includes:

if the course flag bit of the GPS navigation protocol statement is a third preset numerical value, determining that the course is normal at the current moment;

and if the course flag bit of the GPS navigation protocol statement is a fourth preset numerical value, determining that the course is invalid at the current moment.

Further, the step of obtaining the optimal position coordinate estimation value of the agricultural vehicle at the current moment by calculation according to the observation position coordinate, the course judgment result, the real-time vehicle speed, the time interval and the course angle at the current moment comprises the following steps:

when the course judgment result is that the course is normal, reading a double-antenna course angle detected by the agricultural vehicle at the current moment, and calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the double-antenna course angle, the observation position coordinate and the real-time vehicle speed obtained at the current moment;

and when the course judgment result is that the course is invalid, calculating to obtain a presumed course angle at the current moment by adopting a course presumption algorithm through a real-time wheel rotation angle and a real-time vehicle speed, and calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the current presumed course angle, the observed position coordinate and the real-time vehicle speed.

Further, the step of obtaining an optimal position coordinate estimation value of the agricultural vehicle at the current moment by calculation according to the double-antenna course angle, the observation position coordinate, the real-time vehicle speed and the wheel angle detection value obtained at the current moment comprises the following steps:

when the course judgment result is that the course is normal, calculating to obtain a predicted position coordinate at the current moment by adopting a state equation according to the time interval, the real-time vehicle speed, the double-antenna course angle and the obtained last optimal position coordinate estimation value;

calculating according to the prediction variance and the previous covariance at the previous moment to obtain the prediction covariance at the current moment, and calculating according to the observation variance and the prediction covariance at the current moment to obtain the Kalman gain at the current moment;

calculating to obtain the observation position coordinates of the agricultural vehicle at the current moment by adopting an observation equation according to the Gaussian noise and the observation position coordinates at the current moment;

and calculating to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the current observation position coordinate, the current Kalman gain, the observation position coordinate at the current moment and the predicted position coordinate.

Further, the step of obtaining an optimal position coordinate estimation value of the agricultural vehicle at the current moment by calculation according to the double-antenna course angle, the observation position coordinate and the real-time vehicle speed obtained at the current moment comprises the following steps:

when the course judgment result is that the course is normal, calculating to obtain a predicted position coordinate at the current moment by adopting a state equation according to the time interval, the real-time vehicle speed, the current double-antenna course angle and the obtained last optimal position coordinate estimation value;

and calculating according to the prediction variance and the observation variance to obtain a steady-state Kalman gain value, and calculating according to the steady-state Kalman gain value, the observation position coordinate of the current moment and the prediction position coordinate to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment.

Further, the step of calculating the estimated course angle at the current moment by using a course estimation algorithm includes:

when the course judgment result is that the course is invalid, obtaining a current presumed course angle according to a last presumed course angle obtained at the last moment, real-time vehicle speed calculation, a wheel base, a real-time wheel corner and an integration time interval;

the step of obtaining the optimal position coordinate estimation value of the agricultural vehicle at the current moment by calculation according to the current presumed course angle, the observed position coordinate and the real-time vehicle speed comprises the following steps:

calculating to obtain a predicted position coordinate of the current moment by adopting a state equation according to the time interval, the real-time vehicle speed, the last presumed course angle and the obtained last optimal position coordinate estimation value;

The invention also provides an agricultural vehicle, which comprises a processor, a memory and a computer program stored in the memory and capable of running on the processor, wherein the processor is used for executing the computer program stored in the memory to realize the operation executed by the agricultural positioning method.

The invention also provides a storage medium, wherein at least one instruction is stored in the storage medium, and the instruction is loaded and executed by a processor to realize the operation executed by the agricultural machinery positioning method.

By the agricultural machinery positioning method, the agricultural machinery vehicle and the storage medium, a KF fusion algorithm can be carried out by adopting the satellite observation position coordinates and the dead reckoning to realize high-precision positioning under the condition of base station signal interruption, an inertial navigation unit is not added, and the product positioning cost is saved.

Drawings

The above features, technical features, advantages and implementations of an agricultural positioning method, an agricultural vehicle and a storage medium will be further explained in the following description of preferred embodiments in a clearly understandable manner with reference to the accompanying drawings.

FIG. 1 is a schematic diagram of a prior art dead reckoning calculation method;

FIG. 2 is a schematic diagram of error accumulation in a dead reckoning calculation method according to the prior art;

FIG. 3 is a flow chart of one embodiment of an agricultural machine positioning method of the present invention;

FIG. 4 is a flow chart of the present invention for observing position coordinates + measured dual antenna heading angle in combination with KF algorithm (5 formulas);

FIG. 5 is a flow chart of the present invention for observing position coordinates + measured dual antenna heading angle in combination with KF algorithm (2 formulas);

FIG. 6 is a flow chart of the combination of the observed position coordinates + inferred heading angle with KF algorithm (2 formulas) of the present invention;

FIG. 7 is a block diagram of the overall process of positioning in an agricultural machinery positioning method of the present invention;

FIG. 8 is a graph comparing errors in vehicle positioning using 2 prior art and the inventive method for dual antenna course angles;

FIG. 9 is a comparison of errors in wheel angle estimation for 2 prior art and the method of the present invention.

Detailed Description

In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. However, it will be apparent to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.

It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.

For the sake of simplicity, the drawings only schematically show the parts relevant to the present invention, and they do not represent the actual structure as a product. In addition, in order to make the drawings concise and understandable, components having the same structure or function in some of the drawings are only schematically illustrated or only labeled. In this document, "one" means not only "only one" but also a case of "more than one".

It should be further understood that the term "and/or" as used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

In addition, in the description of the present application, the terms "first", "second", and the like are used only for distinguishing the description, and are not intended to indicate or imply relative importance.

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following description will be made with reference to the accompanying drawings. It is obvious that the drawings in the following description are only some examples of the invention, and that for a person skilled in the art, other drawings and embodiments can be derived from them without inventive effort.

One embodiment of the present invention, as shown in fig. 1, is an agricultural machinery positioning method, comprising:

s100, judging whether the receiving of the base station signal is interrupted;

wherein, the step of S100 determining whether the reception of the base station signal is interrupted specifically comprises the steps of:

s110, reading a GPS navigation protocol statement output by the vehicle-mounted satellite receiver at the current moment;

s120, if the positioning flag bit of the GPS navigation protocol statement is a first preset value, determining that the base station signal is normally received at the current moment;

s130, if the positioning flag bit of the GPS navigation protocol statement is the second preset value, determining that the reception of the base station signal is interrupted at the current time.

Specifically, the NMEA protocol is intended to establish a unified RTCM (maritime radio technical commission) standard among different GPS (global positioning system) navigation devices. The agricultural vehicle on-board part comprises an on-board satellite receiver and a control unit, and the standard protocol NMEA is a protocol to which the on-board satellite receiver is adhered. The control unit (ECU) reads a GPS navigation protocol statement output by the vehicle-mounted satellite receiver at the current moment, finds a positioning marker bit from the GPS navigation protocol statement, and judges whether the positioning marker bit is a first preset value (the first preset value is 1 according to the NMEA protocol) or a second preset value (the second preset value is 4 according to the NMEA protocol), namely whether the positioning marker bit is equal to 1 or 4.

Illustratively, the following codes are used for finding out a positioning mark bit, and then whether the receiving of the base station signal is interrupted or not is determined according to the number of the positioning mark bit, and the control unit determines that the receiving of the base station signal of the vehicle-mounted satellite receiver at the current moment is normal when the positioning mark bit is equal to 4. Furthermore, the control unit determines that the reception of the base station signal by the in-vehicle satellite receiver at the present time is interrupted when the positioning flag bit is equal to 1.

S200, when the base station signal is determined to be received and interrupted at the current moment, judging whether the course is normal or not;

wherein, when the S200 determines that the receiving of the base station signal at the current moment is interrupted, the step of judging whether the course is normal specifically comprises the following steps:

s210, when the base station signal is determined to be received and interrupted at the current moment, if the course flag bit of the GPS navigation protocol statement is a third preset value, determining that the course is normal at the current moment;

s220, when the base station signal is determined to be received and interrupted at the current moment, if the course flag bit of the GPS navigation protocol statement is a fourth preset numerical value, determining that the course at the current moment is invalid;

specifically, the control unit (ECU) reads a GPS navigation protocol statement output by the vehicle-mounted satellite receiver at the current time, further searches for the heading flag bit when finding that the positioning flag bit is 1 from the GPS navigation protocol statement, and determines whether the heading flag bit is a third preset value (the third preset value is 4 according to the NMEA protocol) or a fourth preset value (the fourth preset value is 1 according to the NMEA protocol), that is, whether the heading flag bit is equal to 1 or 4.

And judging whether the course of the agricultural vehicle is normal at the current moment according to the course zone bit, and determining that the course of the agricultural vehicle is normal at the current moment when the heading zone bit is equal to 4 by the control unit. And the control unit determines that the course of the agricultural vehicle at the current moment is invalid when the heading flag bit is equal to 1.

S300, calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the observation position coordinate, the course judgment result, the real-time vehicle speed, the time interval and the course angle at the current moment;

the S300 specifically comprises the following steps of calculating to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the observation position coordinate, the course judgment result, the real-time vehicle speed, the time interval and the course angle at the current moment:

s310, when the course judgment result is that the course is normal, reading a double-antenna course angle detected by the agricultural vehicle at the current moment, and calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the double-antenna course angle, the observation position coordinate and the real-time vehicle speed obtained at the current moment;

specifically, the vehicle-mounted satellite receiver installed on the agricultural vehicle is provided with the double antennas, and the double-antenna vehicle-mounted satellite receiver can detect and provide a high-precision course angle, so that the agricultural vehicle can always keep the direction in the running operation process without large deviation, the safety of non-yawing running of the running operation of the agricultural vehicle is ensured, and the steering precision can be ensured. The double-antenna heading angle obtained at the current moment can be accurately output by adopting a double-antenna direction finding technology, and the automatic running operation of the agricultural vehicle can be effectively and reliably assisted.

The step S310 includes the steps of, when the course judgment result is that the course is normal, reading a double-antenna course angle detected by the agricultural vehicle at the current moment, and calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to a time interval, the double-antenna course angle at the current moment, an observation position coordinate and a real-time vehicle speed:

s3111, when the course judgment result is that the course is normal, calculating by using a state equation to obtain a predicted position coordinate of the current time according to the time interval, the real-time vehicle speed of the current time, the double-antenna course angle and the obtained last optimal position coordinate estimation value;

specifically, after the double-antenna course angle at the current moment is obtained through calculation in the above manner, the time interval, the real-time speed of the agricultural vehicle at the current moment, the double-antenna course angle at the current moment and the obtained last optimal position coordinate estimation value are substituted into the following formula (2), namely a state equation, so as to obtain the corresponding predicted position coordinate of the agricultural vehicle at the current moment through calculation.

Wherein,

representing the predicted position coordinates of the current time k, V representing the actual position of the agricultural vehicle at the current timeThe vehicle speed, DT represents the time interval,

indicating the last-time optimal position coordinate estimate, i.e., the last-optimal position coordinate estimate, ψ' (k) is the dual-antenna heading angle at the current time.

S3112, calculating a prediction covariance at the current moment according to the prediction variance and a previous covariance at the previous moment, and calculating a Kalman gain at the current moment according to the observation variance and the prediction covariance at the current moment;

specifically, the covariance formula is shown in the following formula (3), and the prediction covariance and the previous covariance at the previous time are substituted into the covariance formula to calculate the prediction covariance at the current time.

Wherein,

is the current time prediction covariance, (Px)_k-1，Py_k-1) Is the last covariance of the last time instant, (Q)_x，Q_y) Is the variance of the prediction, Qx and Qy are the variance values of the variance Q in the X and Y directions, respectively, and the variance values are equal, i.e., Qx-Qy-Q.

The kalman gain formula is shown in the following formula (4), and the current time prediction covariance and the observation variance are substituted into the following kalman gain formula to calculate the kalman gain at the current time.

Wherein,

is the current time prediction covariance, (Kx)_k，Ky_k) Is the current Kalman gain, (Q)_x，Q_y) The predicted variance and the observed variance are set in advance.

S3113, calculating a target observation position coordinate of the agricultural vehicle at the current moment by adopting an observation equation according to the Gaussian noise and the original observation position coordinate at the current moment;

specifically, the control unit may obtain an original observation position coordinate of the current time from the vehicle-mounted satellite receiver, and substitute a preset gaussian noise into the following formula (5), i.e., an observation equation, to calculate the target observation position coordinate of the current time.

X_k＝x_k+vx_k

Y_k＝y_k+vy_k (5)

Wherein (X)_k，Y_k) Is the target observed position coordinate of the current time k, (x)_k，y_k) Is the original observed position coordinate of the current time k, (Vxk, Vyk) is gaussian noise, Vxk and Vyk are the noise values of the gaussian noise in the X direction and the Y direction, respectively, and the noise values are both r.

S3114, calculating according to the target observation position coordinate, the current Kalman gain and the predicted position coordinate at the current moment to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment.

The control unit substitutes the current Kalman gain, the target observation position coordinate at the current moment and the predicted position coordinate into the following formula (6) to calculate and obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment.

Wherein,

representing the predicted position coordinates of the current time k,

represents the estimated value of the optimal position coordinate at the current moment, (Kx)_k，Ky_k) Is the current Kalman gain, (X)_k，Y_k) Is the observed position coordinate at the current time k.

In this embodiment, the process of S3111-S3114 is as shown in fig. 4, and 5 formulas including the observed position coordinate (i.e., single-point solution) + the measured dual-antenna course angle and the formula (2) -formula (6) are used to calculate an optimal position coordinate estimation value of the agricultural vehicle at the current time, so that high-precision positioning and automatic vehicle driving control can be effectively achieved under the condition of base station signal interruption, an inertial navigation unit is not added, the problem of short-time signal interruption of an agricultural automatic driving system is solved only through an algorithm, and the product cost is saved. The method has the advantages of stable continuous positioning precision, no accumulative error effect, no drift for a long time and improvement of positioning accuracy and reliability.

Wherein, when the course judgment result is that the course is normal, the step S310 reads the double-antenna course angle detected by the agricultural vehicle at the current moment, and calculates the optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the time interval, the double-antenna course angle at the current moment, the observation position coordinate and the real-time vehicle speed, further includes the steps of:

s3121, when the course judgment result is that the course is normal, calculating by using a state equation according to the time interval, the real-time vehicle speed at the current moment, the double-antenna course angle and the obtained last optimal position coordinate estimation value to obtain a predicted position coordinate at the current moment;

Wherein,

representing the predicted position coordinates of the current time k, V representing the real-time speed of the agricultural vehicle at the current time, DT representing the time interval,

S3122, calculating by adopting an observation equation according to the Gaussian noise and the original observation position coordinate of the current moment to obtain a target observation position coordinate of the agricultural vehicle at the current moment;

continuing the above embodiment, according to the gaussian noise and the original observation position coordinate of the current time, the observation equation is used to calculate the target observation position coordinate of the agricultural vehicle at the current time, and the process of specifically calculating and obtaining the target observation position coordinate of the current time is referred to above and will not be described in detail here.

S3123, calculating according to the prediction variance and the observation variance to obtain a steady-state Kalman gain value, and calculating according to the steady-state Kalman gain value, the target observation position coordinate of the current moment and the prediction position coordinate to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment;

specifically, the covariance formula is shown in the following formula (3):

the kalman gain formula is shown in equation (4) below:

wherein,

is the covariance of the current time, (Px)_k-1，Py_k-1) Is the covariance of the previous moment, the predicted variance (Q) since the predicted variance and the observed variance generally do not change over time_x，Q_y) And the observed variance R is a constant value. In this case, the current kalman gain, the previous covariance, and the predicted variance are substituted into the following equation (7), that is, a covariance update equation, to perform iterative computation, so that the kalman gain rapidly converges to a stable value to obtain a steady-state kalman gain value as shown in the following equation (8), and the covariance rapidly converges to a stable value to obtain a steady-state covariance as shown in the following equation (9).

Px_k＝(1-Kx_k)(Px_k-1+Q_x)

Py_k＝(1-Ky_k)(Py_k-1+Q_y) (7)

Wherein,

is the current time prediction covariance, (Q)_x，Q_y) Is the predicted variance, (Kx)_k，Ky_k) Is the current Kalman gain, (Px)_k-1，Py_k-1) Is the last covariance of the last time instant.

Wherein P is a steady-state covariance, K is a steady-state Kalman gain value, Q is a set value corresponding to a predicted variance, and R is a set value corresponding to an observed variance.

Therefore, the control unit carries out formula conversion calculation through the formula (3) to the formula (9) to obtain a steady-state Kalman gain value, and then directly substitutes the steady-state Kalman gain value, the target observation position coordinate of the current moment and the predicted position coordinate into the following formula (10) to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment through calculation.

Wherein,

represents the predicted position coordinates of the current time K, K being the steady state Kalman gain value, (X)_k,Y_k) Is the target observed position coordinate at the current time k.

In this embodiment, as shown in fig. 5, the process of S3121-S3122 is that 2 formulas including the observed position coordinates (i.e., single-point solution) + the measured dual-antenna heading angle and the above formula (2) and formula (10) are used to calculate an optimal position coordinate estimation value of the agricultural vehicle at the current time, so that high-precision positioning and automatic vehicle driving control can be effectively achieved under the condition of base station signal interruption, an inertial navigation unit is not added, the problem of short-time signal interruption of the agricultural automatic driving system is solved only by an algorithm, and the product cost is saved. The method has the advantages of stable continuous positioning precision, no accumulative error effect, no drift for a long time and improvement of positioning accuracy and reliability. Moreover, as shown in fig. 5, compared with the positioning method corresponding to fig. 4, the calculation amount is reduced, and the positioning accuracy and the positioning efficiency at the initial time are greatly improved.

S320, when the course judgment result is that the course is invalid, calculating to obtain a presumed course angle of the current moment by adopting a course presumption algorithm through a real-time wheel rotation angle and a real-time vehicle speed, and calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the time interval, the presumed course angle of the current moment, the observation position coordinate and the real-time vehicle speed;

when the course judgment result is that the course is invalid, the S320 calculates to obtain the estimated course angle of the current moment by adopting a course estimation algorithm through the real-time wheel rotation angle and the real-time vehicle speed, and calculates to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the time interval, the estimated course angle of the current moment, the observation position coordinate and the real-time vehicle speed, and specifically comprises the following steps:

s321, when the course judgment result is that the course is invalid, calculating to obtain the estimated course angle at the current moment according to the last estimated course angle, the real-time vehicle speed, the wheel base, the real-time wheel rotation angle and the integration time interval which are obtained at the last moment;

specifically, when the course at the current moment is invalid, the estimated course angle at the current moment needs to be calculated in a prediction mode. And when the heading flag bit is changed into 1, substituting the last estimated heading angle and the real-time speed of the agricultural vehicle into the following formula (11) to calculate and obtain the estimated heading angle at the current moment.

ψ_k＝ψ_k-1+(V*tan(-α)/L)*Dt (11)

Wherein psi_kIs the estimated heading angle, ψ, of the current time k_k-1The estimated course angle at the previous moment k-1 is the previous estimated course angle, V represents the real-time speed of the agricultural vehicle at the current moment, L is the wheelbase of the agricultural vehicle at the current moment, namely the distance between the front wheel axle and the rear wheel axle of the agricultural vehicle at the current moment, alpha is the real-time wheel rotation angle of the agricultural vehicle at the current moment, generally the steering angle of the front wheel, and Dt is the integration time interval.

S322, calculating to obtain a predicted position coordinate of the current moment by adopting a state equation according to the time interval, the real-time vehicle speed, the last presumed course angle and the obtained last optimal position coordinate estimation value;

specifically, after the estimated course angle at the current moment is obtained through calculation in the above manner, the time interval, the real-time speed of the agricultural vehicle at the current moment, the previous estimated course angle and the obtained previous optimal position coordinate estimation value are substituted into the following formula (3), i.e., a state equation, so as to obtain the predicted position coordinate corresponding to the agricultural vehicle at the current moment through calculation.

Wherein,

indicating the last optimum position coordinate estimate, psi_kIs the estimated heading angle at the current time.

S323, calculating to obtain a target observation position coordinate of the agricultural vehicle at the current moment by adopting an observation equation according to the Gaussian noise and the original observation position coordinate at the current moment;

S324, calculating according to the prediction variance and the observation variance to obtain a steady-state Kalman gain value, and calculating according to the steady-state Kalman gain value, the target observation position coordinate of the current moment and the prediction position coordinate to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment;

specifically, the manner of obtaining the steady-state kalman gain value is described in the above embodiments, and is not described in detail here. The control unit substitutes the steady-state Kalman gain value, the target observation position coordinate of the current moment and the predicted position coordinate obtained by calculation according to the last presumed double-antenna course angle into the following formula (10), and then the optimal position coordinate estimation value of the agricultural vehicle at the current moment can be obtained by calculation.

Wherein,

In the embodiment, the process of S3121-S3122 is shown in FIG. 6, the invention calculates to obtain the current estimated course angle when the heading is invalid, then adopts the observation position coordinate + the estimated course angle according to the current estimated course angle, and adopts 2 formulas in total of formulas (10) and (12), so as to calculate to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current time. The high-precision positioning and vehicle automatic driving control under the condition of base station signal interruption can be effectively realized, an inertial navigation unit is not added, the problem of short-time signal interruption of an agricultural machinery automatic driving system is solved only through an algorithm, and the product cost is saved. The method has the advantages of stable continuous positioning precision, no accumulative error effect, no drift for a long time and improvement of positioning accuracy and reliability. Moreover, as shown in fig. 6, compared with the positioning method corresponding to fig. 4, the calculation amount is reduced, and the positioning accuracy and the positioning efficiency at the initial time are greatly improved.

As shown in fig. 7, the present invention is a block diagram of the overall flow of positioning, and the automated driving control unit ECU shown in fig. 7 is the control unit of the present invention. According to the flow sequence of fig. 7, the optimal position coordinate estimation value of the agricultural vehicle at the current moment is obtained by calculation according to the contents shown in fig. 3-6. Specifically, when the receiving of the base station signal at the current moment is interrupted, the optimal position coordinate estimation value of the agricultural vehicle at the current moment is obtained through calculation through a double-antenna course angle, an observation position coordinate and a KF fusion algorithm (namely formulas 1-12 in the embodiment) so as to perform high-precision positioning on the agricultural vehicle, and more accurate driving can be realized. The invention makes the machine easier and easier to operate in darkness or under dusty conditions, so that the operator in the vehicle-mounted agricultural machine can ensure high-quality cultivation while reducing driving fatigue. By positioning the agricultural vehicle, replanting and missing plowing can be avoided, and the quantity of used fuel and chemicals can be reduced, thereby saving money and protecting the environment. In addition, as shown in fig. 8 and 9, it is obvious that the agricultural vehicle positioning is carried out by the invention, accumulated errors can be eliminated, real-time continuous position and heading information with high output frequency and good reliability is realized, accurate estimation of the position of the agricultural vehicle is realized, and the accuracy of vehicle positioning is further improved, so that the actual motion track of the agricultural vehicle does not deviate from the preset route in the automatic running process of the agricultural vehicle, the production efficiency is improved, the product cost is reduced, and the economic benefit is improved.

In addition, the embodiment of the invention adopts the double-antenna receiver to carry out course detection without inertial navigation IMU, thus the installation requirement on the control unit is not strict, the applicability is wider, the universality is stronger, and the cost is greatly reduced. The method provided by the embodiment of the invention is suitable for agricultural vehicles, including but not limited to tractors, harvesters, self-propelled sprayers, construction vehicles, mining vehicles and the like.

Illustratively, when a certain agricultural vehicle runs at a speed of 0.6m/s and runs in the east direction with a heading angle of about 80 degrees, the algorithm of the invention is used for automatic driving control, and the following signals are recorded under the condition of no base station:

(1) a vehicle single point location solution (i.e., the present invention single point solution or observation location coordinates);

(2) vehicle dead reckoning data (i.e., the estimated heading angle of the present invention);

(3) calculating a KF value (namely the optimal position coordinate estimation value at the current moment) by the vehicle single point and dead reckoning;

(4) wheel angle sensor values (i.e., the present invention real-time wheel rotation angles);

(5) and (5) real-time vehicle speed.

When the heading flag bit is 4, the data processing obtains the lateral deviation d deviating from the automatic driving working straight line, and a lateral deviation d-x graph under 3 methods is shown in the attached figure 7. The KF value in the graph has obvious advantages relative to two types, namely high positioning precision, good smoothness and no drift along with time.

When the heading flag bit is 1, the lateral deviation d deviating from the automatic driving working straight line is obtained by using heading estimation and data processing, and a lateral deviation d-x graph under 3 methods is shown as an attached figure 8. The KF value in the graph has obvious advantages relative to two or more KF values, namely high positioning accuracy, good smoothness and no drift along with time, but the linear accuracy is slightly lower than that of the course angle of a directly used double antenna, and the centimeter-level accuracy can be kept under the condition that the course and the position are not accurate in a short time.

It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-described division of program modules is illustrated, and in practical applications, the above-described distribution of functions may be performed by different program modules, that is, the internal structure of the apparatus may be divided into different program units or modules to perform all or part of the above-described functions. Each program module in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one processing unit, and the integrated unit may be implemented in a form of hardware, or may be implemented in a form of software program unit. In addition, the specific names of the program modules are only used for distinguishing the program modules from one another, and are not used for limiting the protection scope of the application.

In one embodiment of the invention, an agricultural vehicle comprises a processor, a memory, wherein the memory is used for storing a computer program; and the processor is used for executing the computer program stored on the memory to realize the agricultural machinery positioning method in the corresponding method embodiment.

The agricultural vehicle can be a desktop computer, a notebook computer, a palm computer, a tablet computer, a mobile phone, a man-machine interaction screen and other equipment. The agricultural vehicle may include, but is not limited to, a processor, a memory. It will be appreciated by those skilled in the art that the foregoing is merely exemplary of an agricultural vehicle and is not intended to be limiting and that the agricultural vehicle may include more or less components than those described above, or some components in combination, or different components, such as: the agricultural vehicle may also include input/output interfaces, display devices, network access devices, communication buses, communication interfaces, and the like. A communication interface and a communication bus, and may further comprise an input/output interface, wherein the processor, the memory, the input/output interface and the communication interface complete communication with each other through the communication bus. The storage stores a computer program, and the processor is used for executing the computer program stored on the storage to realize the agricultural machinery positioning method in the corresponding method embodiment.

The Processor may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic device, discrete hardware component, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.

The memory may be an internal memory unit of the agricultural vehicle, for example: hard disk or memory of agricultural vehicle. The memory may also be an external storage device of the agricultural vehicle, such as: the agricultural machinery vehicle is provided with a plug-in type hard disk, an intelligent memory Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card) and the like. Further, the memory may also include both an internal storage unit and an external storage device of the agricultural vehicle. The memory is used to store the computer program and other programs and data required by the agricultural vehicle. The memory may also be used to temporarily store data that has been output or is to be output.

A communication bus is a circuit that connects the described elements and enables transmission between the elements. For example, the processor receives commands from other elements through the communication bus, decrypts the received commands, and performs calculations or data processing according to the decrypted commands. The memory may include program modules such as a kernel (kernel), middleware (middleware), an Application Programming Interface (API), and applications. The program modules may be comprised of software, firmware or hardware, or at least two of the same. The input/output interface forwards commands or data entered by a user via the input/output interface (e.g., sensor, keyboard, touch screen). The communication interface connects the agricultural vehicle with other network equipment, user equipment and a network. For example, the communication interface may be connected to a network by wire or wirelessly to connect to external other network devices or user devices. The wireless communication may include at least one of: wireless fidelity (WiFi), Bluetooth (BT), Near Field Communication (NFC), Global Positioning Satellite (GPS) and cellular communications, among others. The wired communication may include at least one of: universal Serial Bus (USB), high-definition multimedia interface (HDMI), asynchronous transfer standard interface (RS-232), and the like. The network may be a telecommunications network and a communications network. The communication network may be a computer network, the internet of things, a telephone network. The agricultural vehicle may be connected to the network via a communication interface, and a protocol used by the agricultural vehicle to communicate with other network devices may be supported by at least one of an application, an Application Programming Interface (API), middleware, a kernel, and a communication interface.

In an embodiment of the present invention, a storage medium stores at least one instruction, and the instruction is loaded and executed by a processor to implement the operations performed by the corresponding embodiments of the agricultural machinery positioning method. For example, the storage medium may be a read-only memory (ROM), a Random Access Memory (RAM), a compact disc read-only memory (CD-ROM), a magnetic tape, a floppy disk, an optical data storage device, and the like.

They may be implemented in program code that is executable by a computing device such that it is executed by the computing device, or separately, or as individual integrated circuit modules, or as a plurality or steps of individual integrated circuit modules. Thus, the present invention is not limited to any specific combination of hardware and software.

In the above embodiments, the descriptions of the respective embodiments have respective emphasis, and reference may be made to the related descriptions of other embodiments for parts that are not described or recited in detail in a certain embodiment.

Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.

In the embodiments provided herein, it should be understood that the disclosed apparatus/agricultural vehicle and method may be implemented in other ways. For example, the above-described device/agricultural vehicle embodiments are merely illustrative, and for example, the division of the modules or units is merely a logical division, and other divisions may be realized in practice, for example, multiple units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of some interfaces, devices or units, and may be in an electrical, mechanical or other form.

The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.

In addition, functional units in the embodiments of the present application may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.

The integrated modules/units may be stored in a storage medium if they are implemented in the form of software functional units and sold or used as separate products. Based on such understanding, all or part of the flow of the method according to the embodiments of the present invention may also be implemented by sending instructions to relevant hardware through a computer program, where the computer program may be stored in a storage medium, and when the computer program is executed by a processor, the steps of the method embodiments may be implemented. Wherein the computer program may be in source code form, object code form, an executable file or some intermediate form, etc. The storage medium may include: any entity or device capable of carrying the computer program, recording medium, usb disk, removable hard disk, magnetic disk, optical disk, computer Memory, Read-Only Memory (ROM), Random Access Memory (RAM), electrical carrier wave signal, telecommunication signal, software distribution medium, etc. It should be noted that the content of the storage medium may be increased or decreased as appropriate according to the requirements of legislation and patent practice in the jurisdiction, for example: in certain jurisdictions, in accordance with legislation and patent practice, computer-readable storage media do not include electrical carrier signals and telecommunications signals.

It should be noted that the above embodiments can be freely combined as necessary. The foregoing is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, various modifications and decorations can be made without departing from the principle of the present invention, and these modifications and decorations should also be regarded as the protection scope of the present invention.

Claims

1. An agricultural machinery positioning method is characterized by comprising the following steps:

judging whether the receiving of the base station signal is interrupted;

2. The agricultural machinery positioning method according to claim 1, wherein the judging whether the reception of the base station signal is interrupted comprises the steps of:

3. The agricultural machinery positioning method according to claim 1, wherein the judging whether the heading is normal comprises the steps of:

4. The agricultural machinery positioning method according to any one of claims 1 to 3, wherein the step of obtaining the optimal position coordinate estimation value of the agricultural machinery vehicle at the current time by calculation according to the observation position coordinate, the course judgment result, the real-time vehicle speed, the time interval and the course angle at the current time comprises the steps of:

when the course judgment result is that the course is normal, reading a double-antenna course angle detected by the agricultural vehicle at the current moment, and calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to a time interval, the double-antenna course angle at the current moment, the observation position coordinate and the real-time vehicle speed;

and when the course judgment result is that the course is invalid, calculating to obtain a presumed course angle of the current moment by adopting a course presumption algorithm through the real-time wheel rotation angle and the real-time vehicle speed, and calculating to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the time interval, the presumed course angle of the current moment, the observation position coordinate and the real-time vehicle speed.

5. The agricultural machinery positioning method according to claim 4, wherein the step of obtaining the optimal position coordinate estimation value of the agricultural machinery vehicle at the current time by calculation according to the time interval, the double-antenna course angle at the current time, the observation position coordinate and the real-time vehicle speed comprises the following steps:

when the course judgment result is that the course is normal, calculating to obtain a predicted position coordinate of the current moment by adopting a state equation according to the time interval, the real-time vehicle speed of the current moment, the double-antenna course angle and the obtained last optimal position coordinate estimation value;

calculating to obtain a target observation position coordinate of the agricultural vehicle at the current moment by adopting an observation equation according to the Gaussian noise and the original observation position coordinate at the current moment;

and calculating to obtain the optimal position coordinate estimation value of the agricultural vehicle at the current moment according to the target observation position coordinate, the current Kalman gain and the predicted position coordinate at the current moment.

6. The agricultural machinery positioning method according to claim 4, wherein the step of obtaining the optimal position coordinate estimation value of the agricultural machinery vehicle at the current moment by calculation according to the double-antenna course angle, the observation position coordinate and the real-time vehicle speed obtained at the current moment comprises the following steps:

and calculating according to the prediction variance and the observation variance to obtain a steady-state Kalman gain value, and calculating according to the steady-state Kalman gain value, the target observation position coordinate at the current moment and the prediction position coordinate to obtain an optimal position coordinate estimation value of the agricultural vehicle at the current moment.

7. The agricultural machinery positioning method according to claim 4, wherein when the course judgment result is course failure, the step of calculating the estimated course angle at the current moment by adopting a course estimation algorithm through the real-time wheel rotation angle and the real-time vehicle speed comprises the following steps:

when the course judgment result is that the course is invalid, calculating to obtain a presumed course angle at the current moment according to a last presumed course angle, a real-time vehicle speed, a wheel base, a real-time wheel corner and an integration time interval which are obtained at the last moment;

the step of obtaining the optimal position coordinate estimation value of the agricultural vehicle at the current moment by calculation according to the time interval, the presumed course angle at the current moment, the observed position coordinate and the real-time vehicle speed comprises the following steps:

8. An agricultural vehicle comprising a processor, a memory, and a computer program stored in and executable on the memory, the processor configured to execute the computer program stored on the memory to perform operations performed by the agricultural positioning method of any one of claims 1 to 7.

9. A storage medium having stored therein at least one instruction, which is loaded and executed by a processor to perform the operations performed by the agricultural positioning method of any one of claims 1 to 7.