Agricultural vehicle positioning method and device, electronic equipment and storage medium

Abstract

The embodiment of the invention discloses an agricultural vehicle positioning method and device, electronic equipment and a storage medium, and the method comprises the steps: determining the posture, position and speed of an agricultural vehicle under a geographic coordinate system according to inertial navigation data obtained by an inertial navigation system; establishing an initial observation equation according to a satellite observation equation corresponding to a global satellite positioning system and a visual observation equation corresponding to a visual sensor; when it is detected that satellite data collected by a global satellite positioning system is valid, the initial observation equation serves as a target observation equation; when it is detected that the satellite data collected by the global satellite positioning system is invalid, the initial observation equation with the satellite observation equations removed serves as a target observation equation; the current error state quantity of the inertial navigation system is corrected according to the target observation equation, and the attitude, position and speed of the agricultural vehicle are corrected according to the updated error state quantity, so that the actual positioning information of the agricultural vehicle is obtained, and the robustness of the positioning result of the agricultural vehicle is improved.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to a method, device, electronic equipment and storage medium for positioning an agricultural vehicle. Background Art

[0002] With the rapid development of information technology, agriculture and high-precision positioning technology are becoming increasingly integrated. In modern agricultural production, whether agricultural vehicles can be positioned stably and with high precision has become an important factor affecting the accuracy of agricultural vehicle operation and scheduling.

[0003] In the prior art, an inertial navigation system and a global satellite positioning system are usually combined to obtain an integrated navigation system, and the agricultural vehicle is positioned by the integrated navigation system.

[0004] However, when agricultural vehicles are positioned using the existing combined navigation system in hilly and mountainous areas with complex terrain and large elevation differences, the satellite signal is often weak, interfered with, or even rejected due to the obstruction of mountains and trees, which makes the positioning results of the global satellite positioning system unreliable, and the positioning results after combined navigation fluctuate greatly, become disconnected, or even diverge. Therefore, the existing combined navigation system cannot stably and accurately position agricultural vehicles in hilly and mountainous areas. Summary of the invention

[0005] The present invention provides an agricultural vehicle positioning method, device, electronic equipment and storage medium, which improve the anti-disturbance capability of the combined navigation system and enhance the robustness of the agricultural vehicle positioning result.

[0006] In a first aspect, an embodiment of the present invention provides an agricultural vehicle positioning method, which is applied to a visually assisted integrated navigation system, wherein the visually assisted integrated navigation system includes an inertial navigation system, a global satellite positioning system, and a visual sensor, and the method includes: obtaining inertial navigation data of the agricultural vehicle through the inertial navigation system, and determining the attitude, position, and speed of the agricultural vehicle in a geographic coordinate system according to the inertial navigation data; establishing an initial observation equation that integrates satellite data and visual data according to a satellite observation equation corresponding to the global satellite positioning system and a visual observation equation corresponding to the visual sensor; when it is detected that the satellite data collected by the global satellite positioning system is valid, using the initial observation equation as a target observation equation; when it is detected that the satellite data collected by the global satellite positioning system is invalid, eliminating the satellite observation equation in the initial observation equation, and using the initial observation equation from which the satellite observation equation has been eliminated as the target observation equation; correcting a current error state quantity of the inertial navigation system according to the target observation equation to obtain an updated error state quantity, and correcting the attitude, position, and speed of the agricultural vehicle according to the updated error state quantity to obtain actual positioning information of the agricultural vehicle; wherein the error state quantity at least includes an attitude error, a position error, and a speed error of the agricultural vehicle.

[0007] Optionally, determining the attitude, position and speed of the agricultural vehicle in the geographic coordinate system based on inertial navigation data includes: acquiring satellite data of the agricultural vehicle through a global satellite positioning system, and determining the absolute initial position of the agricultural vehicle based on the satellite data; establishing an association relationship between the inertial coordinate system and the geographic coordinate system with the absolute initial position as the coordinate origin; wherein the inertial coordinate system is a coordinate system corresponding to the inertial navigation system; performing strapdown solution on the inertial navigation data to obtain the attitude, position and speed of the agricultural vehicle in the inertial coordinate system; and converting the attitude, position and speed of the agricultural vehicle in the inertial coordinate system into the attitude, position and speed of the agricultural vehicle in the geographic coordinate system based on the association relationship between the inertial coordinate system and the geographic coordinate system.

[0008] Optionally, an initial observation equation for fusing satellite data and visual data is established based on a satellite observation equation corresponding to a global satellite positioning system and a visual observation equation corresponding to a visual sensor, including: establishing a satellite observation equation corresponding to a global satellite positioning system based on a navigation state error of an inertial navigation system, a preset measurement coefficient matrix and a preset observation noise; wherein the navigation state error includes at least an attitude error, a velocity error, a position error, a gyroscope bias and an accelerometer bias of the agricultural vehicle; acquiring an image of the vehicle surrounding environment through a visual sensor on the agricultural vehicle, determining a current estimated pose of the visual sensor based on feature points on the image of the vehicle surrounding environment, and optimizing the three-dimensional point coordinates corresponding to the feature points based on the current estimated pose of the visual sensor to obtain optimized three-dimensional point coordinates; constructing a visual reprojection error equation based on the coordinates of the feature points on the image of the vehicle surrounding environment in a pixel coordinate system, the current estimated pose of the visual sensor, the camera intrinsic parameters of the visual sensor and the optimized three-dimensional point coordinates, and linearizing and expanding the visual reprojection error equation to obtain a visual observation equation corresponding to the visual sensor; splicing the satellite observation equation and the visual observation equation to obtain an initial observation equation for fusing satellite data and visual data.

[0009] Optionally, the current estimated pose of the visual sensor is determined based on feature points on the vehicle surrounding environment image, including: obtaining a target circle on the vehicle surrounding environment image with each candidate pixel point on the vehicle surrounding environment image as the center and a preset value as the radius, and obtaining pixel points to be compared corresponding to each candidate pixel point on the circumference of each target circle; taking the candidate pixel points whose pixel differences with a preset number of pixel points to be compared are all greater than a first gradient threshold as the first target pixel points; taking the candidate pixel points whose pixel differences with a preset number of pixel points to be compared are all greater than a second gradient threshold as the second target pixel points; taking the candidate pixel points whose pixel differences with a preset number of pixel points to be compared are all greater than a third gradient threshold as the third target pixel points; wherein the first gradient threshold is greater than the second gradient threshold, and the second gradient threshold is greater than the third gradient threshold; selecting feature points from the first target pixel points, the second target pixel points or the third target pixel points, and tracking the position information of the feature points on adjacent vehicle surrounding environment images to obtain the current estimated pose of the visual sensor.

[0010] Optionally, selecting feature points from the first target pixel points, the second target pixel points or the third target pixel points includes: filtering each first target pixel point to obtain a first remaining pixel point corresponding to each first target pixel point; if the number of the first remaining pixel points is greater than a preset number threshold, using the first remaining pixel point as a feature point on the vehicle surrounding environment image; if the number of the first remaining pixel points is less than or equal to the preset number threshold, filtering each second target pixel point to obtain a second remaining pixel point corresponding to each second target pixel point; if the number of the second remaining pixel points is greater than the preset number threshold, using the second remaining pixel point as a feature point on the vehicle surrounding environment image; if the number of the second remaining pixel points is less than or equal to the preset number threshold, filtering each third target pixel point to obtain a third remaining pixel point corresponding to each third target pixel point, and using the third remaining pixel point as a feature point.

[0011] Optionally, the three-dimensional point coordinates corresponding to the feature points are optimized according to the current estimated posture of the visual sensor to obtain optimized three-dimensional point coordinates, including: determining the three-dimensional point coordinates of the feature points according to the transformation matrix between adjacent vehicle surrounding environment images, and projecting the three-dimensional point coordinates according to the current estimated posture of the visual sensor to obtain normalized coordinates of the three-dimensional point coordinates; projecting the pixel coordinates of the feature points through the camera intrinsic parameters to obtain the normalized coordinates of the feature points; optimizing the three-dimensional point coordinates of the feature points according to the normalized coordinates of the feature points and the normalized coordinates of the three-dimensional point coordinates to obtain optimized three-dimensional point coordinates.

[0012] Optionally, the current error state quantity of the inertial navigation system is corrected according to the target observation equation, including: establishing a state equation corresponding to the global satellite positioning system according to the navigation state error quantity of the inertial navigation system and the posture error of the visual sensor; determining the Jacobian matrix of the visual sensor state for the error quantity to be estimated, and the initial covariance matrix corresponding to the state equation, and establishing a fusion covariance matrix according to the initial covariance matrix and the Jacobian matrix; wherein the error quantity to be estimated includes the navigation state error quantity of the inertial navigation system and the posture error of the visual sensor; updating the fusion covariance matrix according to the Kalman state update formula to obtain an updated fusion covariance matrix, and determining the Kalman filter gain according to the updated fusion covariance matrix; and correcting the current error state quantity of the inertial navigation system according to the Kalman filter gain and the target observation equation.

[0013] In a second aspect, an embodiment of the present invention further provides an agricultural vehicle positioning device, which is applied to a visually assisted integrated navigation system, wherein the visually assisted integrated navigation system includes an inertial navigation system, a global satellite positioning system, and a visual sensor. The device includes: a positioning information acquisition module, which is used to acquire inertial navigation data of the agricultural vehicle through the inertial navigation system, and determine the posture, position, and speed of the agricultural vehicle in a geographic coordinate system according to the inertial navigation data; an initial observation equation establishment module, which is used to establish an initial observation equation that fuses satellite data and visual data according to a satellite observation equation corresponding to the global satellite positioning system and a visual observation equation corresponding to the visual sensor; a first observation equation selection module, which is used to select an initial observation equation when a global satellite is detected. When the satellite data collected by the positioning system is valid, the initial observation equation is used as the target observation equation; the second observation equation selection module is used to eliminate the satellite observation equation in the initial observation equation when it is detected that the satellite data collected by the global satellite positioning system is invalid, and the initial observation equation from which the satellite observation equation has been eliminated is used as the target observation equation; the positioning information correction module is used to correct the current error state quantity of the inertial navigation system according to the target observation equation to obtain an updated error state quantity, and correct the attitude, position and speed of the agricultural vehicle according to the updated error state quantity to obtain the actual positioning information of the agricultural vehicle; wherein the error state quantity at least includes the attitude error, position error and speed error of the agricultural vehicle.

[0014] In a third aspect, an embodiment of the present invention further provides an electronic device, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute the agricultural vehicle positioning method provided by any embodiment of the present invention.

[0015] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, which stores computer instructions, and the computer instructions are used to enable a processor to implement the agricultural vehicle positioning method provided by any embodiment of the present invention when executed.

[0016] The technical solution of the embodiment of the present invention establishes an initial observation equation that integrates satellite data and visual data based on the satellite observation equation corresponding to the global satellite positioning system and the visual observation equation corresponding to the visual sensor, and when it is detected that the satellite data collected by the global satellite positioning system is valid, the initial observation equation is used as the target observation equation, thereby avoiding the situation in which the positioning results of the existing combined navigation system are inaccurate due to occasional failures of the global satellite positioning system in semi-obstructed scenes of hilly terraces, improving the anti-disturbance capability of the combined navigation system and enhancing the robustness of the positioning results of agricultural vehicles.

[0017] It should be understood that the contents described in this section are not intended to identify the key or important features of the embodiments of the present invention, nor are they intended to limit the scope of the present invention. Other features of the present invention will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0019] Figure 1 is a flow chart of a method for positioning an agricultural vehicle provided according to Embodiment 1 of the present invention;

[0020] Figure 2 is a flow chart of another agricultural vehicle positioning method provided according to Embodiment 2 of the present invention;

[0021] Figure 3 is a structural schematic diagram of an agricultural vehicle positioning device provided according to Embodiment 3 of the present invention;

[0022] Figure 4 It is a schematic diagram of the structure of an electronic device provided by Embodiment 4 of the present invention. DETAILED DESCRIPTION

[0023] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0024] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0025] Embodiment 1

[0026] Figure 1 This is a flow chart of an agricultural vehicle positioning method provided according to Embodiment 1 of the present invention. This embodiment is applicable to the situation of positioning agricultural vehicles. The method can be executed by an agricultural vehicle positioning device. The agricultural vehicle positioning device can be implemented in the form of hardware and / or software. The agricultural vehicle positioning device can be configured in an electronic device.

[0027] like Figure 1 As shown, a method for positioning an agricultural vehicle disclosed in this embodiment is applied to a visually assisted integrated navigation system, wherein the visually assisted integrated navigation system includes an inertial navigation system, a global satellite positioning system, and a visual sensor, which includes:

[0028] S110, acquiring inertial navigation data of the agricultural vehicle through an inertial navigation system, and determining the posture, position and speed of the agricultural vehicle in a geographic coordinate system according to the inertial navigation data.

[0029] In this embodiment, the inertial navigation data may include acceleration and angular velocity of the agricultural vehicle, etc. There may be many types of agricultural vehicles, such as tractors, etc.

[0030] In this step, specifically, the inertial navigation data can be strapdown solved to obtain the posture, position and speed of the agricultural vehicle in the inertial coordinate system. Among them, the inertial coordinate system is a coordinate system with the head direction as the horizontal axis, and the inertial navigation data is the data in the inertial coordinate system. Then, according to the correlation between the inertial coordinate system and the geographic coordinate system, the posture, position and speed of the agricultural vehicle in the inertial coordinate system can be converted into the posture, position and speed of the agricultural vehicle in the geographic coordinate system. Among them, there can be multiple geographic coordinate systems, such as the northeast sky coordinate system.

[0031] S120: Establish an initial observation equation for fusing satellite data and visual data according to a satellite observation equation corresponding to the global satellite positioning system and a visual observation equation corresponding to the visual sensor.

[0032] In this embodiment, the global satellite positioning system can provide global positioning services through satellite signals. Specifically, a real-time kinematic (RTK) technique based on the global satellite positioning system can be used to improve the positioning accuracy to centimeter level through differential correction between a base station and a mobile station. The satellite data can be data collected by the global satellite positioning system for agricultural vehicles. The visual data can be data collected by a visual sensor for agricultural vehicles.

[0033] In this step, specifically, the satellite observation equation can be established based on the linear difference of the position, velocity or heading angle between the global satellite positioning system and the inertial navigation system, and the visual observation equation can be established based on the current estimated pose of the visual sensor and the camera internal parameters. Then, the satellite observation equation and the visual observation equation can be spliced ​​to obtain the initial observation equation. Among them, the current estimated pose of the visual sensor can be the result of estimating the position of the feature point on the vehicle surrounding environment image using the optical flow method.

[0034] S130: When it is detected that the satellite data collected by the global satellite positioning system is valid, the initial observation equation is used as the target observation equation.

[0035] In this step, specifically, whether the satellite data collected by the global satellite positioning system is valid can be determined according to the real-time dynamic carrier phase differential state. If the real-time dynamic carrier phase differential state is carrier dual-frequency differential, it is determined that the satellite data collected by the global satellite positioning system is valid. If the real-time dynamic carrier phase differential state is not carrier dual-frequency differential, it is determined that the satellite data collected by the global satellite positioning system is invalid.

[0036] S140: When it is detected that the satellite data collected by the global satellite positioning system is invalid, the satellite observation equation in the initial observation equation is eliminated, and the initial observation equation from which the satellite observation equation has been eliminated is used as the target observation equation.

[0037] S150. Correct the current error state quantity of the inertial navigation system according to the target observation equation to obtain an updated error state quantity, and correct the posture, position and speed of the agricultural vehicle according to the updated error state quantity to obtain actual positioning information of the agricultural vehicle.

[0038] In this embodiment, the error state quantity at least includes the posture error, position error and speed error of the agricultural vehicle. The actual positioning information of the agricultural vehicle may include the corrected posture, position and speed of the agricultural vehicle.

[0039] In this step, specifically, the current error state quantity of the inertial navigation system can be corrected according to the target observation equation and the Kalman filter gain to obtain an updated error state quantity.

[0040] The technical solution of this embodiment obtains the inertial navigation data of the agricultural vehicle through the inertial navigation system, and determines the posture, position and speed of the agricultural vehicle in the geographic coordinate system according to the inertial navigation data; establishes an initial observation equation that integrates the satellite data and the visual data according to the satellite observation equation corresponding to the global satellite positioning system and the visual observation equation corresponding to the visual sensor; when it is detected that the satellite data collected by the global satellite positioning system is valid, the initial observation equation is used as the target observation equation; when it is detected that the satellite data collected by the global satellite positioning system is invalid, the satellite observation equation in the initial observation equation is eliminated, and the satellite observation equation from which the satellite observation equation has been eliminated is used as the target observation equation. The initial observation equation is used as the target observation equation; the current error state quantity of the inertial navigation system is corrected according to the target observation equation to obtain the updated error state quantity, and the attitude, position and speed of the agricultural vehicle are corrected according to the updated error state quantity to obtain the actual positioning information of the agricultural vehicle; wherein the error state quantity at least includes the technical means of attitude error, position error and speed error of the agricultural vehicle, which solves the problem that in the semi-obstructed scene of hilly terraces, the occasional failure of the global satellite positioning system causes the positioning results of the existing combined navigation system to be discontinuous or even divergent, improves the anti-disturbance capability of the combined navigation system, and improves the robustness of the positioning results of the agricultural vehicle.

[0041] Embodiment 2

[0042] Figure 2 It is a flow chart of another agricultural vehicle positioning method provided according to the second embodiment of the present invention. This embodiment is a further optimization and expansion based on the above embodiments, and can be combined with various optional technical solutions in the above implementation modes.

[0043] like Figure 2 As shown, a method for positioning an agricultural vehicle disclosed in this embodiment is applied to a vision-assisted integrated navigation system, which includes an inertial navigation system, a global satellite positioning system, and a visual sensor, including:

[0044] S210: Acquire inertial navigation data of the agricultural vehicle through an inertial navigation system, and determine the posture, position and speed of the agricultural vehicle in a geographic coordinate system according to the inertial navigation data.

[0045] Optionally, determining the attitude, position and speed of the agricultural vehicle in the geographic coordinate system based on inertial navigation data includes: acquiring satellite data of the agricultural vehicle through a global satellite positioning system, and determining the absolute initial position of the agricultural vehicle based on the satellite data; establishing an association relationship between the inertial coordinate system and the geographic coordinate system with the absolute initial position as the coordinate origin; wherein the inertial coordinate system is a coordinate system corresponding to the inertial navigation system; performing strapdown solution on the inertial navigation data to obtain the attitude, position and speed of the agricultural vehicle in the inertial coordinate system; and converting the attitude, position and speed of the agricultural vehicle in the inertial coordinate system into the attitude, position and speed of the agricultural vehicle in the geographic coordinate system based on the association relationship between the inertial coordinate system and the geographic coordinate system.

[0046] Specifically, the absolute initial position of the agricultural vehicle can be determined in the satellite data, and the absolute initial position is used as the coordinate origin to establish the correlation between the inertial coordinate system and the geographic coordinate system. Wherein, the satellite data may include the initial heading angle and the absolute initial position of the agricultural vehicle. Then, the strapdown inertial navigation algorithm can be used to perform strapdown solution on the inertial navigation data based on the zero bias error of the inertial navigation system and the initial horizontal attitude angle and initial heading angle of the agricultural vehicle to obtain the attitude, position and speed of the agricultural vehicle in the inertial coordinate system. Wherein, the zero bias error of the inertial navigation system and the initial horizontal attitude angle of the agricultural vehicle can be determined according to the output data of the inertial navigation system when the agricultural vehicle is stationary.

[0047] The advantage of this setting is that by converting the posture, position and speed of the agricultural vehicle in the inertial coordinate system into the posture, position and speed of the agricultural vehicle in the geographic coordinate system, it is convenient for subsequent unified correction of the posture, position and speed of the agricultural vehicle.

[0048] S220: Establish a satellite observation equation corresponding to the global satellite positioning system according to the navigation state error of the inertial navigation system, a preset measurement coefficient matrix and a preset observation noise.

[0049] In this embodiment, the navigation state error includes at least the attitude error, speed error, position error, gyroscope zero bias and accelerometer zero bias of the agricultural vehicle.

[0050] In this step, specifically, after multiplying the navigation state error amount and the preset measurement coefficient matrix, the multiplication result and the preset observation noise are added to obtain the satellite observation equation corresponding to the global satellite positioning system.

[0051] Exemplarily, the satellite observation equation can be determined by the following specific calculation formula:

[0052] δzr=Hr·δXb+nr

[0053] Among them, δzr represents the satellite observation equation, δXb is the navigation state error of the inertial navigation system, Hr is the preset measurement coefficient matrix, and nr is the preset observation noise.

[0054] S230, obtaining an image of the vehicle's surrounding environment through a visual sensor on the agricultural vehicle, determining a current estimated position and posture of the visual sensor based on feature points on the image of the vehicle's surrounding environment, and optimizing the three-dimensional point coordinates corresponding to the feature points based on the current estimated position and posture of the visual sensor to obtain optimized three-dimensional point coordinates.

[0055] In this embodiment, the vehicle surrounding environment image may be an environment image around the agricultural vehicle. The three-dimensional point coordinates may be the three-dimensional coordinates of the feature points.

[0056] In this step, specifically, the position information of the feature points on different vehicle surrounding environment images can be tracked by using the optical flow method, feature matching method or direct method to obtain the current estimated pose of the visual sensor. Then, the three-dimensional point coordinates corresponding to the feature points can be determined by using the triangulation method, and the three-dimensional point coordinates can be optimized according to the current estimated pose of the visual sensor and the camera internal parameters to obtain the optimized three-dimensional point coordinates.

[0057] Optionally, the current estimated pose of the visual sensor is determined based on feature points on the vehicle surrounding environment image, including: obtaining a target circle on the vehicle surrounding environment image with each candidate pixel point on the vehicle surrounding environment image as the center and a preset value as the radius, and obtaining pixel points to be compared corresponding to each candidate pixel point on the circumference of each target circle; taking the candidate pixel points whose pixel differences with a preset number of pixel points to be compared are all greater than a first gradient threshold as the first target pixel points; taking the candidate pixel points whose pixel differences with a preset number of pixel points to be compared are all greater than a second gradient threshold as the second target pixel points; taking the candidate pixel points whose pixel differences with a preset number of pixel points to be compared are all greater than a third gradient threshold as the third target pixel points; wherein the first gradient threshold is greater than the second gradient threshold, and the second gradient threshold is greater than the third gradient threshold; selecting feature points from the first target pixel points, the second target pixel points or the third target pixel points, and tracking the position information of the feature points on adjacent vehicle surrounding environment images to obtain the current estimated pose of the visual sensor.

[0058] Specifically, the pixel difference between each candidate pixel and the corresponding pixel to be compared can be calculated, and when the pixel difference between the candidate pixel and a preset number of pixels to be compared is greater than the first gradient threshold, the candidate pixel is used as the first target pixel. When the pixel difference between the candidate pixel and a preset number of pixels to be compared is greater than the second gradient threshold, the candidate pixel is used as the second target pixel. When the pixel difference between the candidate pixel and a preset number of pixels to be compared is greater than the third gradient threshold, the candidate pixel is used as the third target pixel. The pixel difference is the difference in pixel values, and the candidate pixel can be the first target pixel, the second target pixel, and the third target pixel at the same time.

[0059] Further, selecting feature points from the first target pixel point, the second target pixel point or the third target pixel point includes: filtering each first target pixel point to obtain a first remaining pixel point corresponding to each first target pixel point; if the number of the first remaining pixel points is greater than a preset number threshold, the first remaining pixel point is used as a feature point on the vehicle surrounding environment image; if the number of the first remaining pixel points is less than or equal to the preset number threshold, filtering each second target pixel point to obtain a second remaining pixel point corresponding to each second target pixel point; if the number of the second remaining pixel points is greater than the preset number threshold, the second remaining pixel point is used as a feature point on the vehicle surrounding environment image; if the number of the second remaining pixel points is less than or equal to the preset number threshold, filtering each third target pixel point to obtain a third remaining pixel point corresponding to each third target pixel point, and using the third remaining pixel point as a feature point. Among them, the preset number threshold can be determined based on historical experience, and this embodiment does not limit this. The filtering processing method can include non-maximum suppression processing and edge point filtering. There can be multiple edge point filtering methods, such as Harris edge point filtering.

[0060] Specifically, after performing non-maximum suppression processing on each first target pixel point to obtain unclustered first target pixel points, edge point filtering can be performed on the unclustered first target pixel points to obtain first residual pixel points. Then, when the number of first residual pixel points is less than or equal to a preset number threshold, non-maximum suppression processing and edge point filtering can be performed on each second target pixel point to obtain second residual pixel points corresponding to each second target pixel point. Finally, when the number of second residual pixel points is less than or equal to a preset number threshold, non-maximum suppression processing and edge point filtering can be performed on each third target pixel point to obtain third residual pixel points corresponding to each third target pixel point, and the third residual pixel points can be used as feature points.

[0061] The advantage of this setting is that by setting three gradient thresholds, the number of image feature points in hilly and mountainous environments can be avoided to be unstable, resulting in too few selected feature points, which provides a basis for accurately determining the current estimated pose of the visual sensor. Secondly, by performing non-maximum suppression processing on each first target pixel point, the clustering of corner points can be reduced. Finally, by performing Harris edge point filtering on the first target pixel points that are not clustered, the density of edge points at the far end of the land-sky border that is unique to the farmland environment can be reduced, thereby improving the effectiveness of the feature points.

[0062] Optionally, the three-dimensional point coordinates corresponding to the feature points are optimized according to the current estimated posture of the visual sensor to obtain optimized three-dimensional point coordinates, including: determining the three-dimensional point coordinates of the feature points according to the transformation matrix between adjacent vehicle surrounding environment images, and projecting the three-dimensional point coordinates according to the current estimated posture of the visual sensor to obtain normalized coordinates of the three-dimensional point coordinates; projecting the pixel coordinates of the feature points through the camera intrinsic parameters to obtain the normalized coordinates of the feature points; optimizing the three-dimensional point coordinates of the feature points according to the normalized coordinates of the feature points and the normalized coordinates of the three-dimensional point coordinates to obtain optimized three-dimensional point coordinates.

[0063] Specifically, a nonlinear least squares solution method may be used to determine the optimal three-dimensional point coordinates according to the normalized coordinates of the feature points and the normalized coordinates of the three-dimensional point coordinates.

[0064] In a specific implementation, the optimized three-dimensional point coordinates can be determined by the following specific calculation formula:

[0065]

[0066] Among them, j is the vehicle environment image of the jth frame, pi is the three-dimensional point coordinate corresponding to the feature point, is the normalized coordinate of the feature point (that is, the normalized coordinate in the camera coordinate system of the jth frame obtained by projecting the pixel coordinate of the feature point through the camera intrinsic parameter), is the normalized coordinates of the 3D point coordinates (that is, the 3D point coordinates through the j-th frame camera pose The projected C j Normalized coordinates in the frame coordinate system).

[0067] S240, construct a visual reprojection error equation based on the coordinates of the feature points on the vehicle surrounding environment image in the pixel coordinate system, the current estimated position and posture of the visual sensor, the camera intrinsic parameters of the visual sensor and the optimized three-dimensional point coordinates, and linearize the visual reprojection error equation to obtain a visual observation equation corresponding to the visual sensor.

[0068] In this step, specifically, after the optimized three-dimensional point coordinates are projected into the image using the current estimated pose of the visual sensor and the camera intrinsic parameters, the visual reprojection error equation can be constructed by combining the projection result with the coordinates of the feature points in the pixel coordinate system.

[0069] In a specific implementation, the visual reprojection error equation can be expressed by the following specific formula:

[0070]

[0071] Among them, δzk is the visual reprojection error, μk is the coordinate of the feature point in the pixel coordinate system, K is the camera intrinsic parameter of the visual sensor, is the current estimated pose of the visual sensor, and pi is the optimized three-dimensional point coordinate.

[0072] Furthermore, the visual observation equation can be expressed by the following specific formula:

[0073] δzk=Hk·δX+nk

[0074] Wherein, Hk is the Jacobian matrix of the visual reprojection error to be estimated, δX is the error to be estimated, and nk is the observed random noise. Specifically, the error to be estimated may include the navigation state error of the inertial navigation system and the position and posture error of the visual sensor.

[0075] S250, combining the satellite observation equation and the visual observation equation to obtain an initial observation equation that integrates the satellite data and the visual data.

[0076] In this step, specifically, the initial observation equation can be expressed by the following specific formula:

[0077]

[0078] Among them, δz represents the initial observation equation, δzr is the satellite observation equation, δzk is the visual observation equation, δzk1...δzkn are different columns of the visual observation equation, and n is the observation noise.

[0079] Optionally, it is possible to determine whether the agricultural vehicle is stationary based on the parallax of matching pixels on adjacent vehicle surrounding environment images, the integrated linear acceleration and angular velocity of the agricultural vehicle, and the speed of the satellite. Among them, the integrated linear acceleration and angular velocity of the agricultural vehicle can be determined by an inertial navigation system. If the agricultural vehicle is stationary, the initial position information and initial heading angle of the agricultural vehicle when it is in a zero-speed stationary state can be used as input values ​​of the satellite observation equation, and the value of the observation error matrix can be reduced. It is worth noting that since the noise of the data collected by the visual sensor when the agricultural vehicle is stationary is very small, the value of the observation error matrix can be reduced. Since the observation error matrix is ​​the covariance matrix of the observation noise, the value of the observation noise can be reduced by reducing the value of the observation error matrix. Among them, the observation error matrix can be used to reflect the uncertainty of the observation value, and the observation error matrix can be determined based on actual measurement data or modeling prediction noise.

[0080] S260: When it is detected that the satellite data collected by the global satellite positioning system is valid, the initial observation equation is used as the target observation equation.

[0081] S270: When it is detected that the satellite data collected by the global satellite positioning system is invalid, the satellite observation equation in the initial observation equation is eliminated, and the initial observation equation from which the satellite observation equation has been eliminated is used as the target observation equation.

[0082] In this step, specifically, the initial observation equation from which the satellite observation equation has been eliminated can be expressed as the target observation equation by the following specific formula:

[0083]

[0084] S280. Correct the current error state quantity of the inertial navigation system according to the target observation equation to obtain an updated error state quantity, and correct the posture, position and speed of the agricultural vehicle according to the updated error state quantity to obtain actual positioning information of the agricultural vehicle.

[0085] The error state quantity may include the posture error, position error and speed error of the agricultural vehicle.

[0086] Optionally, the current error state quantity of the inertial navigation system is corrected according to the target observation equation, including: establishing a state equation corresponding to the global satellite positioning system according to the navigation state error quantity of the inertial navigation system and the posture error of the visual sensor; determining the Jacobian matrix of the visual sensor state for the error quantity to be estimated, and the initial covariance matrix corresponding to the state equation, and establishing a fusion covariance matrix according to the initial covariance matrix and the Jacobian matrix; wherein the error quantity to be estimated includes the navigation state error quantity of the inertial navigation system and the posture error of the visual sensor; updating the fusion covariance matrix according to the Kalman state update formula to obtain an updated fusion covariance matrix, and determining the Kalman filter gain according to the updated fusion covariance matrix; and correcting the current error state quantity of the inertial navigation system according to the Kalman filter gain and the target observation equation.

[0087] In a specific implementation, after establishing the fusion covariance matrix, the state equation can be updated according to the first Kalman state update formula to obtain an updated state equation. Then, the updated covariance matrix corresponding to the updated state equation can be determined, and the fusion covariance matrix can be updated according to the updated covariance matrix and the Jacobian matrix of the estimated error amount of the visual sensor state to obtain an updated fusion covariance matrix.

[0088] Specifically, the state equation can be established by the following specific formula:

[0089]

[0090] Wherein, δX is the error amount to be estimated, F is the first preset system parameter, G is the second preset system parameter, and nc is the control amount of the vision-assisted integrated navigation system.

[0091] The expanded form of the state equation is expressed by the following specific formula:

[0092]

[0093] Among them, δXb is the navigation state error of the inertial navigation system, is the posture error of the visual sensor. Specifically, the navigation state error may include attitude error, velocity error, position error, gyroscope bias and accelerometer bias, etc. The posture error may include attitude error and position error, etc.

[0094] The fusion covariance matrix is ​​established by the following specific formula:

[0095]

[0096] Wherein, Pk′ represents the fusion covariance matrix, Pk is the Jacobian matrix of the visual sensor state to be estimated error, and J is the initial covariance matrix corresponding to the state equation. Specifically, the visual sensor state may include the current posture information and current position information of the visual sensor.

[0097] After establishing the fusion covariance matrix, the updated state equation is established through the following specific formula:

[0098] δXk+1|k=F·δXk|k-1+G·nc+1

[0099] Among them, δXk+1|k represents the updated state equation, δXk|k-1 represents the state equation before updating, and nc+1 represents the updated control amount of the visual-assisted integrated navigation system.

[0100] The updated covariance matrix corresponding to the updated state equation is determined by the following specific formula:

[0101]

[0102] Among them, Pk|k-1 represents the updated covariance matrix, and Pk′ represents the fused covariance matrix before updating.

[0103] After determining the updated covariance matrix, the updated fusion covariance matrix is ​​determined by the following specific formula:

[0104] Pk+1|k=F·Pk|k-1·FT

[0105] Among them, Pk+1|k represents the updated fusion covariance matrix.

[0106] The Kalman filter gain is determined by the following specific formula:

[0107] K F =Pk+1|kHkT(HkPk+1|kHkT+Rk)-1

[0108] Among them, K F is the Kalman filter gain, Hk is the Jacobian matrix of the estimated error of the visual sensor state, and Rk is the measurement noise covariance matrix.

[0109] After updating the fusion covariance matrix, the updated error state is determined by the following specific formula:

[0110] δXk+1=δXk+K(δzk-HkδXk)

[0111] Among them, δXk+1 is the updated error state quantity, and δXk is the current error state quantity.

[0112] Optionally, the updated fusion covariance matrix may be further updated, so that the Kalman filter gain may be updated according to the further updated fusion covariance matrix, so as to quickly determine the updated error state quantity at the next moment.

[0113] Specifically, the further updated fusion covariance matrix can be determined by the following specific formula:

[0114] Pk+1|k′=Pk+1|k-KHkPk+1|k

[0115] Among them, Pk +1| k′ is the fusion covariance matrix after further update.

[0116] Optionally, the technical solution of this embodiment can be implemented by using a sensor acquisition thread, an image processing thread, a navigation thread, and a fusion filter thread. Among them, the sensor acquisition thread can be used to receive the vehicle surrounding environment image collected by the visual sensor, and store the vehicle surrounding environment image in a buffer area. The image processing thread can be used to extract the feature points on the vehicle surrounding environment image, track the position information of the feature points on the adjacent vehicle surrounding environment images, determine the three-dimensional point coordinates corresponding to the feature points, and optimize the three-dimensional point coordinates. The navigation thread can be used to jointly initialize the data collected by the inertial navigation system, the global satellite positioning system, and the visual sensor and perform inertial strapdown solution. The fusion filter thread can be used to determine the fusion covariance matrix, determine whether the satellite data collected by the global satellite positioning system is valid, determine the target observation equation based on whether the satellite data is valid, and correct the posture, position and speed of the agricultural vehicle according to the target observation equation.

[0117] The technical solution of this embodiment, by converting the posture, position and speed of the agricultural vehicle in the inertial coordinate system into the posture, position and speed of the agricultural vehicle in the geographic coordinate system, facilitates the subsequent unified correction of the posture, position and speed of the agricultural vehicle. Secondly, by setting three gradient thresholds, it is possible to avoid the situation where the number of image feature points in the hilly and mountainous environment is unstable, resulting in too few selected feature points, and provides a basis for accurately determining the current estimated posture of the visual sensor. Finally, by performing non-maximum suppression processing on each first target pixel point, the clustering of corner points can be reduced. By performing Harris edge point filtering on the first target pixel points that are not clustered, the density of edge points at the far end of the land-sky border that is unique to the farmland environment can be reduced, and the effectiveness of the feature points can be improved.

[0118] Embodiment 3

[0119] Figure 3It is a structural schematic diagram of an agricultural vehicle positioning device provided according to Embodiment 3 of the present invention. This embodiment can be applied to the situation of positioning agricultural vehicles. The agricultural vehicle positioning device can be implemented in the form of hardware and / or software and can be configured in an electronic device.

[0120] like Figure 3 As shown, the agricultural vehicle positioning device disclosed in this embodiment is applied to a visually assisted integrated navigation system, which includes an inertial navigation system, a global satellite positioning system and a visual sensor. The device includes: a positioning information acquisition module 31, which is used to acquire the inertial navigation data of the agricultural vehicle through the inertial navigation system, and determine the posture, position and speed of the agricultural vehicle in the geographic coordinate system according to the inertial navigation data; an initial observation equation establishment module 32, which is used to establish an initial observation equation that integrates satellite data and visual data according to the satellite observation equation corresponding to the global satellite positioning system and the visual observation equation corresponding to the visual sensor; a first observation equation selection module 33, which is used to select the first observation equation after detecting the global satellite positioning system. When the satellite data collected by the system is valid, the initial observation equation is used as the target observation equation; the second observation equation selection module 34 is used to eliminate the satellite observation equation in the initial observation equation when it is detected that the satellite data collected by the global satellite positioning system is invalid, and the initial observation equation from which the satellite observation equation has been eliminated is used as the target observation equation; the positioning information correction module 35 is used to correct the current error state quantity of the inertial navigation system according to the target observation equation to obtain an updated error state quantity, and to correct the attitude, position and speed of the agricultural vehicle according to the updated error state quantity to obtain the actual positioning information of the agricultural vehicle; wherein the error state quantity at least includes the attitude error, position error and speed error of the agricultural vehicle.

[0121] The technical solution in this embodiment solves the problem that in semi-obstructed scenarios of hilly terraces, occasional failures of the global satellite positioning system lead to discontinuous or even divergent positioning results of the existing combined navigation system, through the mutual cooperation of the positioning information acquisition module, the initial observation equation establishment module, the first observation equation selection module, the second observation equation selection module and the positioning information correction module. It improves the anti-disturbance capability of the combined navigation system and the robustness of the positioning results of agricultural vehicles.

[0122] Optionally, the positioning information acquisition module 31 is specifically used to obtain satellite data of the agricultural vehicle through the global satellite positioning system, and determine the absolute initial position of the agricultural vehicle based on the satellite data; using the absolute initial position as the coordinate origin, establish an association relationship between the inertial coordinate system and the geographic coordinate system; wherein the inertial coordinate system is a coordinate system corresponding to the inertial navigation system; perform strapdown solution on the inertial navigation data to obtain the posture, position and speed of the agricultural vehicle in the inertial coordinate system; based on the association relationship between the inertial coordinate system and the geographic coordinate system, convert the posture, position and speed of the agricultural vehicle in the inertial coordinate system into the posture, position and speed of the agricultural vehicle in the geographic coordinate system.

[0123] Optionally, the initial observation equation establishment module 32 includes: a satellite observation equation establishment unit, which is used to establish a satellite observation equation corresponding to the global satellite positioning system according to the navigation state error of the inertial navigation system, a preset measurement coefficient matrix and a preset observation noise; wherein the navigation state error includes at least the attitude error, speed error, position error, gyroscope zero bias and accelerometer zero bias of the agricultural vehicle; a three-dimensional point coordinate optimization unit, which is used to obtain the vehicle surrounding environment image through the visual sensor on the agricultural vehicle, determine the current estimated posture of the visual sensor according to the feature points on the vehicle surrounding environment image, and optimize the three-dimensional point coordinates corresponding to the feature points according to the current estimated posture of the visual sensor to obtain the optimized three-dimensional point coordinates; a visual observation equation establishment unit, which is used to construct a visual reprojection error equation according to the coordinates of the feature points on the vehicle surrounding environment image in the pixel coordinate system, the current estimated posture of the visual sensor, the camera intrinsic parameters of the visual sensor and the optimized three-dimensional point coordinates, and linearize the visual reprojection error equation to obtain the visual observation equation corresponding to the visual sensor; an initial observation equation establishment unit, which is used to splice the satellite observation equation and the visual observation equation to obtain the initial observation equation that integrates the satellite data and the visual data.

[0124] Optionally, the three-dimensional point coordinate optimization unit includes: a pixel point acquisition subunit for obtaining a target circle on the vehicle surrounding environment image with each candidate pixel point on the vehicle surrounding environment image as the center and a preset value as the radius, and obtains the pixel points to be compared corresponding to each candidate pixel point on the circumference of each target circle; a first pixel point determination subunit for selecting the candidate pixel points whose pixel difference values ​​with a preset number of pixel points to be compared are all greater than a first gradient threshold as the first target pixel points; a second pixel point determination subunit for selecting the candidate pixel points whose pixel difference values ​​with a preset number of pixel points to be compared are all greater than a first gradient threshold as the first target pixel points. The candidate pixel points whose values ​​are all greater than the second gradient threshold are taken as the second target pixel points; the third pixel point determination subunit is used to take the candidate pixel points whose pixel difference values ​​with a preset number of pixel points to be compared are all greater than the third gradient threshold as the third target pixel points; wherein the first gradient threshold is greater than the second gradient threshold, and the second gradient threshold is greater than the third gradient threshold; the actual posture determination subunit is used to select feature points from the first target pixel points, the second target pixel points or the third target pixel points, and track the position information of the feature points on the adjacent vehicle surrounding environment images to obtain the current estimated posture of the visual sensor.

[0125] Optionally, the actual posture determination subunit is specifically used to filter each first target pixel point to obtain a first remaining pixel point corresponding to each first target pixel point; if the number of the first remaining pixel points is greater than a preset number threshold, the first remaining pixel points are used as feature points on the vehicle surrounding environment image; if the number of the first remaining pixel points is less than or equal to the preset number threshold, each second target pixel point is filtered to obtain a second remaining pixel point corresponding to each second target pixel point; if the number of the second remaining pixel points is greater than the preset number threshold, the second remaining pixel points are used as feature points on the vehicle surrounding environment image; if the number of the second remaining pixel points is less than or equal to the preset number threshold, each third target pixel point is filtered to obtain a third remaining pixel point corresponding to each third target pixel point, and the third remaining pixel point is used as a feature point.

[0126] Optionally, the three-dimensional point coordinate optimization unit also includes: a first coordinate projection subunit, used to determine the three-dimensional point coordinates of the feature point according to the transformation matrix between adjacent vehicle surrounding environment images, and project the three-dimensional point coordinates according to the current estimated posture of the visual sensor to obtain the normalized coordinates of the three-dimensional point coordinates; a second coordinate projection subunit, used to project the pixel coordinates of the feature point through the camera intrinsic parameters to obtain the normalized coordinates of the feature point; a three-dimensional coordinate optimization subunit, used to optimize the three-dimensional point coordinates of the feature point according to the normalized coordinates of the feature point and the normalized coordinates of the three-dimensional point coordinates to obtain the optimized three-dimensional point coordinates.

[0127] Optionally, the positioning information correction module 35 is specifically used to establish a state equation corresponding to the global satellite positioning system based on the navigation state error of the inertial navigation system and the posture error of the visual sensor; determine the Jacobian matrix of the visual sensor state for the estimated error amount, and the initial covariance matrix corresponding to the state equation, and establish a fusion covariance matrix based on the initial covariance matrix and the Jacobian matrix; wherein the estimated error amount includes the navigation state error amount of the inertial navigation system and the posture error of the visual sensor; update the fusion covariance matrix according to the Kalman state update formula to obtain an updated fusion covariance matrix, and determine the Kalman filter gain according to the updated fusion covariance matrix; and correct the current error state amount of the inertial navigation system according to the Kalman filter gain and the target observation equation.

[0128] The agricultural vehicle positioning device provided in the embodiment of the present invention can execute the agricultural vehicle positioning method provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method. For the contents not described in detail in this embodiment, reference can be made to the description in any method embodiment of the present application.

[0129] Embodiment 4

[0130] Figure 4 FIG. 1 is a schematic diagram showing the structure of an electronic device 10 that can be used to implement an embodiment of the present invention. Figure 4 As shown, the electronic device 10 includes at least one processor 11, and a memory connected to the at least one processor 11, such as a read-only memory (ROM) 12, a random access memory (RAM) 13, etc., wherein the memory stores a computer program that can be executed by at least one processor, and the processor 11 can perform various appropriate actions and processes according to the computer program stored in the read-only memory (ROM) 12 or the computer program loaded from the storage unit 18 to the random access memory (RAM) 13. In the RAM 13, various programs and data required for the operation of the electronic device 10 can also be stored. The processor 11, the ROM 12, and the RAM 13 are connected to each other through a bus 14. An input / output (I / O) interface 15 is also connected to the bus 14.

[0131] A number of components in the electronic device 10 are connected to the I / O interface 15, including: an input unit 16, such as a keyboard, a mouse, etc.; an output unit 17, such as various types of displays, speakers, etc.; a storage unit 18, such as a disk, an optical disk, etc.; and a communication unit 19, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 19 allows the electronic device 10 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0132] The processor 11 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any appropriate processor, controller, microcontroller, etc. The processor 11 executes the various methods and processes described above, such as the agricultural vehicle positioning method.

[0133] In some embodiments, the agricultural vehicle positioning method can be implemented as a computer program, which is tangibly contained in a computer-readable storage medium, such as a storage unit 18. In some embodiments, part or all of the computer program can be loaded and / or installed on the electronic device 10 via the ROM 12 and / or the communication unit 19. When the computer program is loaded into the RAM 13 and executed by the processor 11, one or more steps of the agricultural vehicle positioning method described above can be performed. Alternatively, in other embodiments, the processor 11 can be configured to perform the agricultural vehicle positioning method in any other suitable manner (e.g., by means of firmware).

[0134] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), complex programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0135] Computer programs for implementing the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that when the computer program is executed by the processor, the functions / operations specified in the flow chart and / or block diagram are implemented. The computer program may be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.

[0136] In the context of the present invention, a computer-readable storage medium may be a tangible medium that may contain or store a computer program for use by or in combination with an instruction execution system, device or equipment. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or equipment, or any suitable combination of the foregoing. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0137] To provide interaction with a user, the systems and techniques described herein may be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and a pointing device (e.g., a mouse or trackball) through which the user can provide input to the electronic device. Other types of devices may also be used to provide interaction with the user; for example, the feedback provided to the user may be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user may be received in any form (including acoustic input, voice input, or tactile input).

[0138] The systems and techniques described herein may be implemented in a computing system that includes backend components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes frontend components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such backend components, middleware components, or frontend components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), a blockchain network, and the Internet.

[0139] A computing system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The client and server relationship is generated by computer programs running on the corresponding computers and having a client-server relationship with each other. The server may be a cloud server, also known as a cloud computing server or cloud host, which is a host product in the cloud computing service system to solve the defects of difficult management and weak business scalability in traditional physical hosts and VPS services.

[0140] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps described in the present invention can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solution of the present invention can be achieved, and this document does not limit this.

[0141] The above specific implementations do not constitute a limitation on the protection scope of the present invention. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for positioning an agricultural vehicle, characterized in that: Applied to a vision-assisted integrated navigation system, the vision-assisted integrated navigation system includes an inertial navigation system, a global satellite positioning system and a visual sensor, and the method includes: Acquire inertial navigation data of the agricultural vehicle through an inertial navigation system, and determine the attitude, position and speed of the agricultural vehicle in a geographic coordinate system according to the inertial navigation data; According to the satellite observation equation corresponding to the global satellite positioning system and the visual observation equation corresponding to the visual sensor, an initial observation equation for fusing satellite data and visual data is established; When it is detected that the satellite data collected by the global satellite positioning system is valid, the initial observation equation is used as the target observation equation; When it is detected that the satellite data collected by the global satellite positioning system is invalid, the satellite observation equation in the initial observation equation is eliminated, and the initial observation equation from which the satellite observation equation has been eliminated is used as the target observation equation; The current error state quantity of the inertial navigation system is corrected according to the target observation equation to obtain an updated error state quantity, and the attitude, position and speed of the agricultural vehicle are corrected according to the updated error state quantity to obtain the actual positioning information of the agricultural vehicle; The error state quantity at least includes the posture error, position error and speed error of the agricultural vehicle.

2. The method according to claim 1, characterized in that Determine the attitude, position and speed of agricultural vehicles in geographic coordinates based on inertial navigation data, including: Acquire satellite data of the agricultural vehicle through a global satellite positioning system, and determine the absolute initial position of the agricultural vehicle based on the satellite data; Taking the absolute initial position as the coordinate origin, establishing an association relationship between the inertial coordinate system and the geographic coordinate system; wherein the inertial coordinate system is a coordinate system corresponding to the inertial navigation system; The inertial navigation data is strapdown solved to obtain the attitude, position and speed of the agricultural vehicle in the inertial coordinate system; According to the relationship between the inertial coordinate system and the geographic coordinate system, the posture, position and speed of the agricultural vehicle in the inertial coordinate system are converted into the posture, position and speed of the agricultural vehicle in the geographic coordinate system.

3. The method according to claim 1, characterized in that According to the satellite observation equation corresponding to the global satellite positioning system and the visual observation equation corresponding to the visual sensor, the initial observation equation for fusing satellite data and visual data is established, including: According to the navigation state error of the inertial navigation system, the preset measurement coefficient matrix and the preset observation noise, a satellite observation equation corresponding to the global satellite positioning system is established; Wherein, the navigation state error amount includes at least the attitude error, speed error, position error, gyroscope zero bias and accelerometer zero bias of the agricultural vehicle; The visual sensor on the agricultural vehicle acquires the image of the vehicle environment, determines the current estimated position and posture of the visual sensor according to the feature points on the image of the vehicle environment, and optimizes the three-dimensional point coordinates corresponding to the feature points according to the current estimated position and posture of the visual sensor to obtain the optimized three-dimensional point coordinates; According to the coordinates of the feature points on the vehicle environment image in the pixel coordinate system, the current estimated pose of the visual sensor, the camera intrinsic parameters of the visual sensor and the optimized three-dimensional point coordinates, a visual reprojection error equation is constructed, and the visual reprojection error equation is linearly expanded to obtain the visual observation equation corresponding to the visual sensor; The satellite observation equation and the visual observation equation are spliced ​​to obtain the initial observation equation that integrates the satellite data and the visual data.

4. The method according to claim 3, characterized in that The current estimated position and posture of the visual sensor is determined based on the feature points on the vehicle environment image, including: Taking each candidate pixel point on the vehicle surrounding environment image as the center of the circle and a preset value as the radius, a target circle is obtained on the vehicle surrounding environment image, and pixels to be compared corresponding to each candidate pixel point are obtained on the circumference of each target circle; A candidate pixel point whose pixel difference with a preset number of pixels to be compared is greater than a first gradient threshold is used as a first target pixel point; The candidate pixel whose pixel difference with a preset number of to-be-compared pixel points is greater than the second gradient threshold is used as the second target pixel point; A candidate pixel point whose pixel difference with a preset number of pixels to be compared is greater than a third gradient threshold is used as a third target pixel point; Wherein, the first gradient threshold is greater than the second gradient threshold, and the second gradient threshold is greater than the third gradient threshold; A feature point is selected from the first target pixel point, the second target pixel point or the third target pixel point, and the position information of the feature point on the adjacent vehicle surrounding environment image is tracked to obtain the current estimated position and posture of the visual sensor.

5. The method according to claim 4, characterized in that Selecting a feature point from the first target pixel point, the second target pixel point, or the third target pixel point includes: Performing filtering processing on each first target pixel point to obtain first residual pixel points corresponding to each first target pixel point; If the number of the first remaining pixel points is greater than a preset number threshold, the first remaining pixel points are used as feature points on the vehicle surrounding environment image; If the number of the first remaining pixel points is less than or equal to the preset number threshold, filtering is performed on each second target pixel point to obtain second remaining pixel points corresponding to each second target pixel point; If the number of the second remaining pixel points is greater than the preset number threshold, the second remaining pixel points are used as feature points on the vehicle surrounding environment image; If the number of the second remaining pixel points is less than or equal to the preset number threshold, each third target pixel point is filtered to obtain a third remaining pixel point corresponding to each third target pixel point, and the third remaining pixel point is used as a feature point.

6. The method according to claim 4, characterized in that The three-dimensional point coordinates corresponding to the feature points are optimized according to the current estimated position and posture of the visual sensor to obtain the optimized three-dimensional point coordinates, including: Determine the three-dimensional point coordinates of the feature point according to the transformation matrix between the adjacent vehicle surrounding environment images, and project the three-dimensional point coordinates according to the current estimated position and posture of the visual sensor to obtain the normalized coordinates of the three-dimensional point coordinates; The pixel coordinates of the feature points are projected through the camera intrinsic parameters to obtain the normalized coordinates of the feature points; According to the normalized coordinates of the feature point and the normalized coordinates of the three-dimensional point coordinates, the three-dimensional point coordinates of the feature point are optimized to obtain optimized three-dimensional point coordinates.

7. The method according to claim 1, characterized in that According to the target observation equation, the current error state of the inertial navigation system is corrected, including: According to the navigation state error of the inertial navigation system and the position error of the visual sensor, the state equation corresponding to the global satellite positioning system is established; Determine the Jacobian matrix of the visual sensor state to be estimated error and the initial covariance matrix corresponding to the state equation, and establish a fusion covariance matrix based on the initial covariance matrix and the Jacobian matrix; The error to be estimated includes the navigation state error of the inertial navigation system and the position and posture error of the visual sensor; The fusion covariance matrix is ​​updated according to the Kalman state update formula to obtain an updated fusion covariance matrix, and the Kalman filter gain is determined according to the updated fusion covariance matrix; The current error state of the inertial navigation system is corrected according to the Kalman filter gain and the target observation equation.

8. An agricultural vehicle positioning device, characterized in that: Applied to a visually assisted integrated navigation system, the visually assisted integrated navigation system includes an inertial navigation system, a global satellite positioning system and a visual sensor, and the device includes: A positioning information acquisition module is used to acquire inertial navigation data of the agricultural vehicle through an inertial navigation system, and determine the posture, position and speed of the agricultural vehicle in a geographic coordinate system based on the inertial navigation data; An initial observation equation establishment module is used to establish an initial observation equation that integrates satellite data and visual data based on a satellite observation equation corresponding to a global satellite positioning system and a visual observation equation corresponding to a visual sensor; A first observation equation selection module is used to use the initial observation equation as the target observation equation when it is detected that the satellite data collected by the global satellite positioning system is valid; The second observation equation selection module is used to remove the satellite observation equation in the initial observation equation when it is detected that the satellite data collected by the global satellite positioning system is invalid, and use the initial observation equation from which the satellite observation equation has been removed as the target observation equation; A positioning information correction module is used to correct the current error state quantity of the inertial navigation system according to the target observation equation to obtain an updated error state quantity, and to correct the posture, position and speed of the agricultural vehicle according to the updated error state quantity to obtain the actual positioning information of the agricultural vehicle; The error state quantity at least includes the posture error, position error and speed error of the agricultural vehicle.

9. An electronic device, characterized in that: The electronic device comprises: At least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute the agricultural vehicle positioning method described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a processor to implement the agricultural vehicle positioning method according to any one of claims 1 to 7 when executed.

Images