Agricultural robot positioning method, system and apparatus

Abstract

The application discloses an agricultural robot positioning method, system and device, and relates to the field of robot navigation positioning. The method comprises the following steps: calculating a primary correction heading angle of a marked mobile laser receiver according to a laser emission rotation period, a fixed laser receiving time, a preset relative position of a plurality of mobile laser receivers, a mobile laser receiving time group and current inertial navigation data; determining a corrected position of the marked laser receiver according to previous inertial navigation data based on a Kalman filtering algorithm, and then calculating a mobile end laser receiving position group at the current time and a mobile end laser receiving position group at the next time; judging whether an obstruction appears between the plurality of mobile laser receivers; if no obstruction appears, a secondary correction heading angle is calculated; the secondary correction heading angle and the mobile end laser receiving position group at the current time constitute current position information of the agricultural robot; and if an obstruction appears, the position of the obstructed mobile laser receiver is calculated. The application realizes higher-precision positioning.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot navigation positioning, in particular to a laser and inertial sensor fusion agricultural robot positioning method, system and device. BACKGROUND

[0002] With the advent of the intelligent era, great changes have taken place in various industries, and unmanned farm technology will be a strong support for modern agriculture, and agricultural robots will replace most manual operations. Robot positioning and navigation technology is the core technology for the autonomous movement of agricultural robots, and there are a large number of GNSS information blockages and missing scenarios in agricultural operation environments, such as greenhouses, hangars, bridges and other facilities. Solving the problem of accurate positioning in the absence of (weak) GNSS signals and realizing continuous, accurate and stable positioning of robots entering and exiting facilities is a key technology for the autonomous movement of unmanned farm agricultural robots. The existing commonly used positioning and navigation technologies can be divided into GNSS positioning technology, laser positioning technology, SLAM technology, etc. according to the main sensors used.

[0003] (1) Global Navigation Satellite System (GNSS) can provide 3D coordinates and velocity and time information for users at any location on or near the earth's surface, a space-based radio navigation positioning system that is all-weather, unaffected by weather factors, including one or more satellite constellations and the enhancement system required to support specific work. GNSS positioning technology has the advantages of high positioning accuracy, short observation time, no need for communication between stations, simple instrument operation, strong adaptability, etc., but it is affected by many factors, including buildings, viaducts, radio waves, etc., resulting in incomplete positioning and inability to achieve accurate positioning indoors.

[0004] (2) Laser positioning technology is to obtain surrounding environment information through laser radar, and some auxiliary units are used to position the target, which is suitable for indoor use. The triangular positioning method is the mainstream method of laser positioning technology, which is implemented by arranging a certain number of reflective plates in the environment. During the scanning process of the laser, the distance between the laser and all the reflective plates is calculated, and according to the sensing time and scanning period, the distance between any two reflective plates can be calculated using the triangular formula. The measured distance is compared with the offline theoretical value to match the number and position information of each reflective plate. Laser positioning technology has the characteristics of stability, reliability, high performance continuity, long service life, and low post-modification cost, but laser is easily blocked, and in the case of many obstacles, it is difficult to analyze the work load of obstacle recognition through point cloud.

[0005] (3) Inertial navigation technology is based on inertial sensors to predict the state, specifically, using accelerometers, gyroscopes and magnetometers and other sensors to process the position information at the previous time, to obtain the relative position at the current time. Inertial navigation system is based on dead reckoning method to realize the positioning of the terminal, has strong autonomy, the positioning accuracy and continuity in a short time is very high; But the positioning and navigation accuracy is greatly limited by the cost of device, and inevitably with the passage of time produces cumulative error, need to rely on external positioning information source constantly calibrating position calculation.

[0006] (4) SLAM technology is in unknown environment, robot in the process of movement observes map features in real time, and then matches the map features according to the change of its own position, so as to complete positioning and map construction technology. The mainstream SLAM technology is mainly divided into laser SLAM and visual SLAM. As early as 2005, laser SLAM has been studied thoroughly, and the framework has been preliminarily determined. It is the most stable and mainstream positioning and navigation method at present.

[0007] At present, there is no effective solution to the problems existing in laser positioning technology. SUMMARY

[0008] The purpose of the present application is to provide an agricultural robot positioning method, system and device, which can achieve high-precision positioning by fusing laser positioning and inertial data at different times.

[0009] To achieve the above purpose, the present application provides the following scheme:

[0010] An agricultural robot positioning method comprises:

[0011] acquiring a laser emission rotation period, a fixed laser receiving time, a preset plurality of mobile laser receiving relative positions, a mobile laser receiving time group, previous time inertial navigation data and current time inertial navigation data; the mobile laser receiving time group comprises mobile laser receiving times collected by a plurality of mobile laser receivers arranged on the agricultural robot according to the preset plurality of mobile laser receiving relative positions;

[0012] According to the laser emission rotation period, the fixed laser receiving time, the preset plurality of mobile laser receiving relative positions, the mobile laser receiving time group and the current time inertial navigation data, the initial correction heading angle of the marked mobile laser receiver is calculated; the marked mobile laser receiver is any mobile laser receiver;

[0013] Based on Kalman filtering algorithm, the corrected position of the marked laser receiver is determined according to the previous time inertial navigation data;

[0014] According to the corrected position of the marker laser receiver and the preset relative positions of the multiple mobile laser receivers, a current mobile laser receiver position group is calculated; the mobile laser receiver position group includes positions of multiple mobile laser receivers arranged according to the preset relative positions of the multiple mobile laser receivers;

[0015] Based on the current mobile laser receiver position group and the initial correction heading angle of the marker mobile laser receiver, a next mobile laser receiver position group is calculated;

[0016] According to the next mobile laser receiver position group, it is determined whether there is an occlusion between the multiple mobile laser receivers;

[0017] If there is no occlusion between the multiple mobile laser receivers, a secondary correction heading angle is calculated according to the corrected position of the marker laser receiver and the mobile laser receiving time obtained by the marker laser receiver at the corrected position; the secondary correction heading angle and the current mobile laser receiver position group constitute the current position information of the agricultural robot;

[0018] If there is an occlusion between the multiple mobile laser receivers, a receiver position calculation instruction is generated; the receiver position calculation instruction is used to calculate the position of the occluded mobile laser receiver according to the position of the non-occluded mobile laser receiver and the preset relative positions of the multiple mobile laser receivers at the next time.

[0019] Optionally, the mobile laser receiving time group includes a first mobile receiving time corresponding to the mobile laser receiver, a second mobile receiving time corresponding to the first auxiliary mobile laser receiver, and a third mobile receiving time corresponding to the second auxiliary mobile laser receiver; the mobile laser receiver, the first auxiliary mobile laser receiver, the second auxiliary mobile laser receiver, and the fixed laser receiver are all in a coordinate system established with the laser transmitter as the coordinate origin; the fixed laser receiver is used to collect the fixed laser receiving time;

[0020] According to the laser emission rotation period, the fixed laser receiving time, the preset relative positions of the multiple mobile laser receivers, the mobile laser receiving time group, and the current inertial navigation data, an initial correction heading angle of the marker mobile laser receiver is calculated, specifically including:

[0021] According to the laser emission rotation period, the fixed laser receiving time, and the first mobile receiving time, a first origin included angle is calculated; the first origin included angle is an included angle between a line connecting the mobile laser receiver and the coordinate origin and a line connecting the fixed laser receiver and the coordinate origin;

[0022] According to the laser emission rotation period, the fixed laser receiving time, and the second mobile receiving time, a second origin included angle is calculated; the second origin included angle is an included angle between a line connecting the first auxiliary mobile laser receiver and the coordinate origin and a line connecting the fixed laser receiver and the coordinate origin;

[0023] According to the laser emission rotation period, the fixed laser receiving time, and the third mobile receiving time, a third origin included angle is calculated; the third origin included angle is an included angle between a line connecting the second auxiliary mobile laser receiver and the coordinate origin and a line connecting the fixed laser receiver and the coordinate origin;

[0024] According to the first origin included angle, the second origin included angle, the third origin included angle, and the preset multiple mobile laser receiving relative positions, an included angle between a body axis of the agricultural robot and a Y axis of the coordinate system is calculated;

[0025] Based on a Kalman filtering algorithm, according to the included angle between the body axis of the agricultural robot and the Y axis of the coordinate system and the current time inertial navigation data, a first correction heading angle of a marked mobile laser receiver is determined.

[0026] To achieve the above purpose, the present application also provides the following technical solutions:

[0027] An agricultural robot positioning system comprises:

[0028] A data acquisition module is configured to acquire a laser emission rotation period, a fixed laser receiving time, preset multiple mobile laser receiving relative positions, a mobile laser receiving time group, previous time inertial navigation data, and current time inertial navigation data; the mobile laser receiving time group comprises mobile laser receiving times collected by multiple mobile laser receivers arranged on the agricultural robot according to the preset multiple mobile laser receiving relative positions;

[0029] A heading angle first correction module is configured to calculate a first correction heading angle of a marked mobile laser receiver according to the laser emission rotation period, the fixed laser receiving time, the preset multiple mobile laser receiving relative positions, the mobile laser receiving time group, and the current time inertial navigation data; the marked mobile laser receiver is any one of the mobile laser receivers;

[0030] A laser receiver position correction module is configured to determine a corrected position of the marked laser receiver according to the previous time inertial navigation data based on a Kalman filtering algorithm;

[0031] A current receiving position group determination module is configured to calculate a mobile laser receiver position group at a current time according to the corrected position of the marker laser receiver and the preset relative positions of the multiple mobile laser receivers; the mobile laser receiver position group includes positions of multiple mobile laser receivers arranged according to the preset relative positions of the multiple mobile laser receivers;

[0032] A next-time receiving position group determination module is configured to calculate a mobile laser receiver position group at a next time based on the mobile laser receiver position group at the current time and the initial correction heading angle of the marker mobile laser receiver;

[0033] An occlusion judgment module is configured to:

[0034] judge whether occlusion occurs between the multiple mobile laser receivers according to the mobile laser receiver position group at the next time;

[0035] If no occlusion occurs between the multiple mobile laser receivers, a secondary correction heading angle is calculated according to the corrected position of the marker laser receiver and the mobile laser receiving time obtained by the marker laser receiver at the corrected position; the secondary correction heading angle and the mobile laser receiver position group at the current time constitute the current position information of the agricultural robot;

[0036] If occlusion occurs between the multiple mobile laser receivers, a receiver position calculation instruction is generated; the receiver position calculation instruction is used to calculate the position of a mobile laser receiver that is occluded according to the position of a mobile laser receiver that is not occluded and in combination with the preset relative positions of the multiple mobile laser receivers at a next time.

[0037] An agricultural robot positioning device includes at least one set of fixed-end laser components, at least one set of mobile-end laser receiving components, an inertial navigation component, and a processor.

[0038] The fixed-end laser components include a laser emitter and a fixed laser receiver; the mobile-end laser receiving components include a mobile laser receiver, a first auxiliary mobile laser receiver, and a second auxiliary mobile laser receiver.

[0039] The mobile laser receiver, the first auxiliary mobile laser receiver, and the second auxiliary mobile laser receiver are arranged on the agricultural robot according to preset relative positions of the multiple mobile laser receivers; a line connecting the mobile laser receiver and the first auxiliary mobile laser receiver is parallel to the body axis direction of the agricultural robot.

[0040] The laser emitter is configured to emit a rotating laser signal according to a laser emission rotation period;

[0041] The fixed laser receiver is configured to receive the rotating laser signal and record a fixed laser receiving time;

[0042] The mobile laser receiver, the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver are all used for receiving a rotating laser signal and recording corresponding mobile laser receiving time;

[0043] The inertial navigation component is arranged on the agricultural robot, and is used for collecting inertial navigation data of the agricultural robot at different moments;

[0044] The processor is connected with the laser emitter, the fixed laser receiver, the mobile laser receiver, the first auxiliary mobile laser receiver, the second auxiliary mobile laser receiver and the inertial navigation component respectively, and is used for positioning the agricultural robot based on an agricultural robot positioning method.

[0045] According to the specific embodiments of the present application, the following technical effects are disclosed:

[0046] The present application discloses an agricultural robot positioning method, system and device, calculates a first correction heading angle of a marked mobile laser receiver according to a laser emission rotating period, a fixed laser receiving time, a preset plurality of mobile laser receiving relative positions, a mobile laser receiving time group and current moment inertial navigation data, so as to obtain laser data; determines a corrected position of the marked laser receiver according to previous moment inertial navigation data based on a Kalman filtering algorithm; calculates a current moment mobile end laser receiving position group according to the corrected position of the marked laser receiver and the preset plurality of mobile laser receiving relative positions, so as to obtain more accurate laser receiver position data by fusing the laser data and the inertial navigation data; on this basis, calculates a next moment mobile end laser receiving position group, and judges whether shielding occurs between the plurality of mobile laser receivers, if no shielding occurs, the heading angle is corrected again, and the final agricultural robot position information is output; if shielding occurs, a receiver position calculation instruction is generated, so that the position of the shielded laser receiver is calculated according to the position of the unshielded laser receiver when shielding occurs, avoiding the situation that the positioning data is missing due to shielding of the laser light path, and further improving the positioning accuracy. BRIEF DESCRIPTION OF DRAWINGS

[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0048] Figure 1 It is a flowchart of the agricultural robot positioning method of the present application;

[0049] Figure 2 This is a schematic diagram illustrating the calculation of the angle between the body axis of the agricultural robot in Example 1 and the Y-axis of the coordinate system.

[0050] Figure 3 This is a schematic diagram of the positioning points at different times in Example 1;

[0051] Figure 4 This is a schematic diagram illustrating the calculation of the heading angle at different times in Example 1;

[0052] Figure 5 This is a schematic diagram of the agricultural robot positioning system of the present invention.

[0053] Figure 6 This is a schematic diagram of a device with multiple laser emitters.

[0054] Symbol explanation:

[0055] 1-Mobile laser receiver, 2-First auxiliary mobile laser receiver, 3-Second auxiliary mobile laser receiver, 4-Laser emitter, 5-Fixed laser receiver, 6-Agricultural robot, 7-Agricultural robot body axis, 8-Laser emitter No. 2, 9-Fixed laser receiver No. 2. Detailed Implementation

[0056] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0057] The purpose of this invention is to provide an agricultural robot positioning method, system, and device that eliminates the need for a costly laser transmitter with scanning and ranging capabilities. Instead, it uses a combined navigation system that integrates laser sensing and inertial navigation to position the agricultural robot, resulting in low computational load and high positioning accuracy.

[0058] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0059] Example 1

[0060] like Figure 1 As shown, this embodiment provides a method for locating an agricultural robot, including:

[0061] Step 100: Obtain the laser emission rotation period, fixed laser reception time, preset relative positions of multiple moving laser receivers, moving laser reception time group, inertial navigation data of the previous moment, and inertial navigation data of the current moment.

[0062] The mobile laser receiving time group includes a first mobile receiving time corresponding to the mobile laser receiver, a second mobile receiving time corresponding to the first auxiliary mobile laser receiver, and a third mobile receiving time corresponding to the second auxiliary mobile laser receiver. The mobile laser receiver, the first auxiliary mobile laser receiver, the second auxiliary mobile laser receiver, and the fixed laser receiver are in a coordinate system established with the laser transmitter as the coordinate origin; and the fixed laser receiver is used to collect the fixed laser receiving time. Generally, the projections of the mobile laser receiver, the first auxiliary mobile laser receiver, and the second auxiliary mobile laser receiver on the horizontal plane form a right triangle.

[0063] The preset three-receiver position data includes a first distance, a second distance, a third distance, a first angle, a second angle, and a third angle. The first distance d yf2 is the installation distance of the mobile laser receiver and the second auxiliary mobile laser receiver; the second distance d ff is the installation distance of the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver; the third distance d yf1 is the installation distance of the mobile laser receiver and the first auxiliary mobile laser receiver. The first angle θ s2 is the included angle between the line connecting the mobile laser receiver and the first auxiliary mobile laser receiver and the line connecting the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver; the second angle is the included angle between the line connecting the mobile laser receiver and the second auxiliary mobile laser receiver and the line connecting the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver, and is 90°; and the third angle θ s1 is the included angle between the line connecting the mobile laser receiver and the first auxiliary mobile laser receiver and the line connecting the mobile laser receiver and the second auxiliary mobile laser receiver.

[0064] The inertial navigation data is the three-axis acceleration, three-axis angular velocity, and three-axis magnetic field of the inertial navigation unit after coordinate conversion and other operations, and the heading angle θ yaw is the included angle between the body axis of the agricultural robot (in the direction of the head) and the north pole of the earth, the heading angle acceleration a yaw is the acceleration of the heading angle, the pitch angle θ pitch is the included angle between the body axis of the agricultural robot and the horizontal plane, the pitch angle acceleration a pitch is the acceleration of the pitch angle, and the roll angle θ roll, roll angle acceleration a roll . The acceleration of the navigation target in the X, Y, Z axis direction projection is a x , a y , a z ; the data output frequency of the inertial navigation unit is greater than that of the mobile laser receiver.

[0065] Step 200, according to the laser emission rotation period, the fixed laser receiving time, the preset multiple mobile laser receiving relative positions, the mobile laser receiving time group and the current time inertial navigation data, calculate the initial correction heading angle of the marked mobile laser receiver; the marked mobile laser receiver is any of the mobile laser receivers.

[0066] Wherein, the laser emission rotation period is the time for the laser emitter to rotate one round, t0, and the rotation direction is known; the fixed laser receiving time t g is the time when the silicon photodiode array of the fixed laser receiver responds to receive the laser signal of the laser emitter; two columns of silicon photodiode arrays with known interval distance Dis and parallel are arranged on each mobile laser receiver, so that the time when each mobile laser receiver can receive the laser signal of the laser emitter is t y1 and t y2 , and t y = (t y1 +t y2 ) / 2, that is, each mobile laser receiving time is t y .

[0067] As shown in Figure 2 , step 200 specifically includes:

[0068] (1) according to the laser emission rotation period, the fixed laser receiving time, the first mobile receiving time, calculate the first origin angle θ j ; the first origin angle is the angle between the line connecting the mobile laser receiver 1 and the coordinate origin and the line connecting the fixed laser receiver and the coordinate origin; the coordinate origin is the laser emitter 4, and the specific calculation formula is as follows:

[0069]

[0070] Wherein, the mobile time parameters t y1 and t y2 are greater than the fixed time parameter t g , and

[0071] (2) According to the laser emission rotation period, the fixed laser receiving time, and the second mobile receiving time, a second origin included angle is calculated; the second origin included angle is the included angle between the line connecting the first auxiliary mobile laser receiver 2 and the coordinate origin and the line connecting the fixed laser receiver 5 and the coordinate origin. Similarly, θ jf1 .

[0072] (3) According to the laser emission rotation period, the fixed laser receiving time, and the third mobile receiving time, a third origin included angle is calculated; the third origin included angle is the included angle between the line connecting the second auxiliary mobile laser receiver 3 and the coordinate origin and the line connecting the fixed laser receiver 5 and the coordinate origin. Similarly, θ jf2 .

[0073] (4) As shown in Figure 2 , according to the first origin included angle, the second origin included angle, the third origin included angle, and the preset multiple mobile laser receiving relative positions, an angle between the body axis 7 of the agricultural robot and the Y-axis of the coordinate system is calculated; the calculation formula is:

[0074]

[0075] wherein, θ represents the angle between the body axis of the agricultural robot and the Y-axis of the coordinate system, d yf2 represents the first distance, d ff represents the second distance, θ s2 represents the first angle, θ jy represents the first origin included angle, θ jf1 represents the second origin included angle, and θ jf2 represents the third origin included angle. In addition, Figure 2 6 in the figure is an agricultural robot, and the Z-axis is not shown in the figure, and the positive direction of the Z-axis points to the sky.

[0076] (5) Based on the Kalman filtering algorithm, according to the angle between the body axis of the agricultural robot and the Y-axis of the coordinate system and the current time inertial navigation data, a first-time correction heading angle of the marked mobile laser receiver is determined.

[0077] Step 300, based on the Kalman filtering algorithm, according to the previous time inertial navigation data, a corrected position of the marked laser receiver is determined.

[0078] Before processing the Kalman filtering algorithm, it is first assumed that the initial speed of the navigation target is 0, and the mobile laser receiver is the positioning point of the agricultural robot, and the positioning points of the mobile laser receiver at two different times are determined: as shown in Figure 3The first positioning point p1(x1, y1) and the second positioning point p2(x2, y2) are shown. The heading angle is the included angle between the straight line connecting the first positioning point and the second positioning point and the positive direction of the Y axis of the coordinate system, which is calculated according to the heading angle data output by the inertial navigation unit at two different time points.

[0079] The difference between the coordinates of the first positioning point and the second positioning point is Δx = x2 - x1 and Δy = y2 - y1, and Δx and Δy are obtained by integrating a x , a y The distance between the first positioning point and the second positioning point is obtained by integrating twice The included angle between the line connecting the first positioning point and the origin and the line connecting the second positioning point and the origin is Δθ1 = θ j2 - θ j1 , θ j1 is the included angle of the first positioning point, and θ j2 is the included angle of the second positioning point. The first positioning point, the second positioning point and the origin form a triangle, and the distance between the first positioning point and the origin is:

[0080]

[0081] Thus, the coordinates of the first positioning point p1(d1cosθ j1 , d1sinθ j1 ) and the coordinates of the second positioning point p2(d1cosθ j1 + Δx, d1sinθ j1 + Δy) are obtained.

[0082] Step 300 specifically includes:

[0083] (1) Based on the inertial navigation data at the previous time point, the moving acceleration and the actual heading angle of the marked laser receiver are determined.

[0084] (2) According to the moving acceleration of the marked laser receiver, the to-be-verified position of the marked laser receiver at the current time point is calculated; specifically, the to-be-verified coordinates at the current time point are p k (x k , y k ), where Δx k-1 is the x-coordinate difference of the to-be-verified positioning coordinates at the previous two time points, Δx k-1 is the x-coordinate difference of the to-be-verified positioning coordinates at the previous two time points, Δx k-1 = x k-1 - x k-2 , and Δy k-1 is the y-coordinate difference of the to-be-verified positioning coordinates at the previous two time points, Δy k-1 = y k-1 - yk-2 , t0 is a sampling step (s), is the speed of the agricultural robot (mm / s) at the sampling moment (k-1) t0, a x(k-1) and a y(k-1) is the free acceleration of the agricultural robot (mm / s 2 ) at the sampling moment (k-1) t0.

[0085] (3) According to the actual heading angle of the marker laser receiver, the predicted position of the marker laser receiver at the current moment is calculated; specifically, the coordinates of the last moment p k-1 (x k-1 , y k-1 ) and the heading angle θ yaw k at the current moment, the last moment angle θ j(k-1) , the current moment angle θ j(k) , the predicted coordinates p yk (x yk , y yk ) at the current moment are obtained.

[0086]

[0087]

[0088] (4) Based on the Kalman filtering algorithm, the to-be-tested position and the predicted position are fused to obtain the corrected position of the marker laser receiver.

[0089] Step 400: According to the corrected position of the marker laser receiver and the preset relative positions of the multiple mobile laser receivers, a mobile laser receiver position group at the current moment is calculated; the mobile laser receiver position group includes the positions of multiple mobile laser receivers arranged according to the preset relative positions of the multiple mobile laser receivers. Specifically, based on the corrected position of the marker laser receiver, the positions of the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver are calculated in combination with the triangular position relationship among the three laser receivers.

[0090] Step 500: Based on the mobile laser receiver position group at the current moment and the initial corrected heading angle of the marker mobile laser receiver, a mobile laser receiver position group at the next moment is calculated.

[0091] Step 600: According to the mobile laser receiver position group at the next moment, it is judged whether there is an occlusion among the multiple mobile laser receivers.

[0092] Step 600 specifically includes:

[0093] (1) determining a plurality of line directions according to the mobile laser receiver position set at the next moment; the plurality of line directions include a first line direction, a second line direction, and a third line direction; the first line direction is the line direction of the mobile laser receiver and the first auxiliary mobile laser receiver at the next moment; the second line direction is the line direction of the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver at the next moment; and the third line direction is the line direction of the mobile laser receiver and the second auxiliary mobile laser receiver at the next moment.

[0094] Generally, at the positioning starting moment, the two auxiliary mobile laser receivers and the mobile laser receiver are in a state of not shielding the laser emitter laser rays. When the triangle formed by the three lasers moves from the known endpoint coordinate position to the predicted endpoint coordinate position, the three points move at the same time, and during the movement of the triangle from the known position to the predicted position, the three endpoints are translated from the known endpoint coordinates to the predicted endpoint coordinates.

[0095] (2) judging whether the extension of each of the line directions passes through the coordinate origin and generating a judgment result; when the judgment result is that the extension of any of the line directions passes through the coordinate origin, it indicates that shielding occurs; and when the judgment result is that the extension of all of the line directions does not pass through the coordinate origin, it indicates that no shielding occurs.

[0096] That is, the above-mentioned step can judge whether the next moment is the moment when the three sides of the triangle coincide with the rays of the laser receiver. If yes, when the auxiliary laser receiver and the mobile laser receiver shield each other during the positioning of the agricultural robot, the missing data is supplemented by the data in the process of translating the corresponding three endpoints from the known endpoint coordinates to the predicted endpoint coordinates; if no, the judgment of the next moment is continued. The specific steps are as follows:

[0097] Step 700, if no shielding occurs between the plurality of mobile laser receivers, then calculating a secondary correction heading angle according to the corrected position of the marker laser receiver and the mobile laser receiving time obtained by the marker laser receiver at the corrected position; the secondary correction heading angle and the mobile end laser receiving position set at the current moment constitute the current position information of the agricultural robot.

[0098] The secondary correction heading angle is calculated according to the corrected position of the marker laser receiver and the mobile laser receiving time obtained by the marker laser receiver at the corrected position, and specifically includes:

[0099] (1) obtaining the first laser receiving parameter t y1 and the second laser receiving parameter t y2The two laser receiving parameters were acquired because two parallel arrays of silicon photodiodes with a known spacing of Dis were set on the moving laser receiver.

[0100] (2) Obtain the distance of the preset laser receiving sensing component on the marked laser receiver; that is, the distance Dis between the two columns of silicon photodiode arrays.

[0101] (3) Figure 4 As shown, according to the corrected position p of the marked laser receiver k (x k ,y k The secondary corrected heading angle is calculated using the first laser receiving parameters, the second laser receiving parameters, and the preset distance to the laser receiving sensor, as shown in the following formula:

[0102]

[0103] Preferably, the first and second laser receiving parameters are collected at the corrected position of the marked laser receiver at time k-1. Similarly, the corrected heading angle θ at time k-1 can be obtained. yaw(k-1) By changing the heading θ at two different times yaw(k) -θ yaw(k-1) The heading angle is further corrected. This corrected heading angle can then be used for positioning calculations at the next moment.

[0104] Step 800: If there is obstruction between multiple mobile laser receivers, a receiver position estimation instruction is generated; the receiver position estimation instruction is used to calculate the position of the obstructed mobile laser receiver at the next moment based on the position of the unobstructed mobile laser receiver and the preset relative positions of the multiple mobile laser receivers.

[0105] In summary, this embodiment processes fixed time parameters, multiple movement time parameters, and inertial navigation data. Specifically, it fuses the difference between the fixed time parameters and the movement time parameters using Kalman filtering, combined with the inertial navigation data, to obtain the position data of the laser receiver center. The heading angle is corrected using the time parameters and auxiliary time parameters of the mobile laser receiver. More accurate predicted coordinates are obtained by illuminating the mobile laser receiver with multiple laser emitters. This invention eliminates the need for costly scanning and ranging laser emitters, resulting in low computational load and high positioning accuracy.

[0106] The specific implementation object in this example is an unmanned agricultural machine based on GNSS navigation. When the unmanned agricultural machine enters the hangar or encounters an obstruction such as entering the bottom of a bridge during the operation, the GNSS navigation signal is poor or even interrupted. This positioning method is used to supplement the positioning data.

[0107] Example 2

[0108] As Figure 5 shown in the technical solutions in Embodiment One, this embodiment provides an agricultural robot positioning system, comprising:

[0109] The data acquisition module 101 is configured to acquire a laser emission rotation period, a fixed laser receiving time, a preset plurality of mobile laser receiving relative positions, a mobile laser receiving time group, previous time inertial navigation data, and current time inertial navigation data. The mobile laser receiving time group includes mobile laser receiving times collected by a plurality of mobile laser receivers arranged on the agricultural robot according to the preset plurality of mobile laser receiving relative positions.

[0110] The heading angle initial correction module 201 is configured to calculate an initial correction heading angle of a marked mobile laser receiver according to the laser emission rotation period, the fixed laser receiving time, the preset plurality of mobile laser receiving relative positions, the mobile laser receiving time group, and the current time inertial navigation data. The marked mobile laser receiver is any one of the mobile laser receivers.

[0111] The laser receiver position correction module 301 is configured to determine a corrected position of the marked laser receiver based on a Kalman filtering algorithm and the previous time inertial navigation data.

[0112] The current receiving position group determination module 401 is configured to calculate a mobile end laser receiving position group at the current time according to the corrected position of the marked laser receiver and the preset plurality of mobile laser receiving relative positions. The mobile end laser receiving position group includes positions of a plurality of mobile lasers arranged according to the preset plurality of mobile laser receiving relative positions.

[0113] The next time receiving position group determination module 501 is configured to calculate a mobile end laser receiving position group at the next time based on the mobile end laser receiving position group at the current time and the initial correction heading angle of the marked mobile laser receiver.

[0114] The occlusion judgment module 601 is configured to:

[0115] determine whether occlusion occurs between the plurality of mobile laser receivers according to the mobile end laser receiving position group at the next time;

[0116] If no occlusion occurs between the plurality of mobile laser receivers, a secondary correction heading angle is calculated according to the corrected position of the marked laser receiver and a mobile laser receiving time obtained by the marked laser receiver at the corrected position. The secondary correction heading angle and the mobile end laser receiving position group at the current time constitute current position information of the agricultural robot.

[0117] If the multiple mobile laser receivers are blocked, a receiver position calculation instruction is generated; the receiver position calculation instruction is used to calculate the position of the blocked mobile laser receiver according to the position of the mobile laser receiver that is not blocked and in combination with the preset multiple mobile laser receiver relative position at the next time.

[0118] Embodiment three

[0119] The embodiment provides an agricultural robot positioning device to which the agricultural robot positioning method in the embodiment one is applied, and the agricultural robot positioning device comprises at least one set of fixed-end laser assembly, at least one set of mobile-end laser receiving assembly, inertial navigation component and processor.

[0120] The fixed-end laser assembly comprises a laser transmitter and a fixed laser receiver; the mobile-end laser receiving assembly comprises a mobile laser receiver, a first auxiliary mobile laser receiver and a second auxiliary mobile laser receiver. In a positioning space, the fixed-end laser assembly is arranged on a fixed base station, and a three-dimensional coordinate system is established with the position of the laser transmitter as the origin.

[0121] The mobile laser receiver, the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver are arranged on the agricultural robot according to a preset multiple mobile laser receiver relative position; and the line connecting the mobile laser receiver and the first auxiliary mobile laser receiver is parallel to the body axis direction of the agricultural robot. The preset multiple mobile laser receiver relative position is that the projection three-point line of the mobile laser receiver, the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver on the horizontal plane forms a right triangle.

[0122] The laser transmitter is used to emit a rotating laser signal according to a laser emission rotation period.

[0123] The fixed laser receiver is used to receive the rotating laser signal and record a fixed laser receiving time. The fixed laser receiving time is the chip time of receiving the signal of sensing the laser transmitted by the fixed-end laser receiver.

[0124] The mobile laser receiver, the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver are all used to receive the rotating laser signal and record corresponding mobile laser receiving times, i.e. the times of sensing the laser signal.

[0125] The inertial navigation component is arranged on the agricultural robot, and the inertial navigation component is used to collect inertial navigation data of the agricultural robot at different times.

[0126] The processor is connected to the laser emitter, the fixed laser receiver, the mobile laser receiver, the first auxiliary mobile laser receiver, the second auxiliary mobile laser receiver, and the inertial navigation component, respectively, and is used to locate the agricultural robot based on the agricultural robot positioning method described in Embodiment 1.

[0127] Preferably, the mobile laser receiver responds quickly to the rotating laser signal from the laser transmitter via a silicon photodiode array, which consists of silicon photodiodes with known numbers and is mounted perpendicular to the ground; the fixed laser receiver and the two auxiliary mobile laser receivers each have a column of silicon photodiode arrays, and the mobile laser receivers have two columns of parallel silicon photodiode arrays with a known spacing distance Dis.

[0128] The mobile laser receiver can also output elevation parameters, which include the numbers of the silicon photodiode-induced lasers in the two columns of silicon photodiode arrays and the calculated elevation.

[0129] Furthermore, such as Figure 6 As shown, there can be multiple laser transmitters. A fixed-end laser receiver is paired one-to-one with each laser transmitter. A polar coordinate system is constructed with one of the laser transmitters as the origin. The coordinates of each laser transmitter and the fixed-end laser receiver are known. The angles between the lines connecting the mobile-end laser receiver to each laser transmitter, and between the lines connecting the laser transmitters and the fixed-end laser receivers, are also known. From these angles, the positioning coordinates can be calculated. Specifically... Figure 6 Laser emitter 4 corresponds to fixed laser receiver 5, and laser emitter 8 corresponds to fixed laser receiver 9. A three-dimensional coordinate system is established with laser emitter 4 as the origin to facilitate subsequent data calculation.

[0130] Compared with the prior art, the present invention also has the following advantages:

[0131] (1) The present invention has a small amount of computation, a short algorithm time, a reliable calculation method, high positioning accuracy, strong anti-interference ability, and low implementation cost.

[0132] (2) By installing and setting up multiple laser receivers, the problem of blocked laser light path can be solved and the positioning accuracy can be improved.

[0133] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0134] The principles and implementation manners of the present application are described by using specific examples in the present application, and the above examples are only used to help understand the method of the present application and its core idea; meanwhile, for the general technical personnel in the art, the specific implementation manners and application ranges will be changed according to the idea of the present application. In conclusion, the content of the present specification should not be understood as the limitation of the present application.

Claims

1. An agricultural robot positioning method, characterized by, The agricultural robot positioning method comprises the following steps: acquiring a laser emission rotation period, a fixed laser receiving time, a preset plurality of mobile laser receiving relative positions, a mobile laser receiving time group, previous time inertial navigation data and current time inertial navigation data; the mobile laser receiving time group comprises mobile laser receiving times collected by a plurality of mobile laser receivers arranged on the agricultural robot according to the preset plurality of mobile laser receiving relative positions; calculating a primary correction heading angle of a marked mobile laser receiver according to the laser emission rotation period, the fixed laser receiving time, the preset plurality of mobile laser receiving relative positions, the mobile laser receiving time group and the current time inertial navigation data; the marked mobile laser receiver is any one of the mobile laser receivers; determining a corrected position of the marked mobile laser receiver according to the previous time inertial navigation data based on a Kalman filtering algorithm; calculating a current time mobile end laser receiving position group according to the corrected position of the marked mobile laser receiver and the preset plurality of mobile laser receiving relative positions; the mobile end laser receiving position group comprises positions of a plurality of mobile lasers arranged according to the preset plurality of mobile laser receiving relative positions; calculating a next time mobile end laser receiving position group based on the current time mobile end laser receiving position group and the primary correction heading angle of the marked mobile laser receiver; judging whether shielding occurs between the plurality of mobile laser receivers according to the next time mobile end laser receiving position group; if no shielding occurs between the plurality of mobile laser receivers, calculating a secondary correction heading angle according to the corrected position of the marked mobile laser receiver and a mobile laser receiving time obtained by the marked mobile laser receiver at the corrected position; the secondary correction heading angle and the current time mobile end laser receiving position group constitute current position information of the agricultural robot; if shielding occurs between the plurality of mobile laser receivers, generating a receiver position calculation instruction; the receiver position calculation instruction is used to calculate positions of the shielded mobile laser receivers according to positions of the unshielded mobile laser receivers and the preset plurality of mobile laser receiving relative positions at the next time.

2. The agricultural robot positioning method according to claim 1, characterized in that, The mobile laser receiving time group comprises a first mobile receiving time corresponding to a mobile laser receiver, a second mobile receiving time corresponding to a first auxiliary mobile laser receiver and a third mobile receiving time corresponding to a second auxiliary mobile laser receiver; the mobile laser receiver, the first auxiliary mobile laser receiver, the second auxiliary mobile laser receiver and a fixed laser receiver are all in a coordinate system established with the laser emitter as a coordinate origin; the fixed laser receiver is used to collect the fixed laser receiving time; calculating a primary correction heading angle of a marked mobile laser receiver according to the laser emission rotation period, the fixed laser receiving time, the preset plurality of mobile laser receiving relative positions, the mobile laser receiving time group and the current time inertial navigation data, specifically comprising: According to the laser emission rotation period, the fixed laser receiving time, and the first mobile receiving time, a first origin included angle is calculated; the first origin included angle is an included angle between a line connecting the mobile laser receiver and the coordinate origin and a line connecting the fixed laser receiver and the coordinate origin; According to the laser emission rotation period, the fixed laser receiving time, and the second mobile receiving time, a second origin included angle is calculated; the second origin included angle is an included angle between a line connecting the first auxiliary mobile laser receiver and the coordinate origin and a line connecting the fixed laser receiver and the coordinate origin; According to the laser emission rotation period, the fixed laser receiving time, and the third mobile receiving time, a third origin included angle is calculated; the third origin included angle is an included angle between a line connecting the second auxiliary mobile laser receiver and the coordinate origin and a line connecting the fixed laser receiver and the coordinate origin; According to the first origin included angle, the second origin included angle, the third origin included angle, and a preset relative position of multiple mobile laser receivers, an included angle between a body axis of the agricultural robot and a Y axis of the coordinate system is calculated; Based on a Kalman filtering algorithm, according to the included angle between the body axis of the agricultural robot and the Y axis of the coordinate system and current time inertial navigation data, a first correction heading angle of a marked mobile laser receiver is determined.

3. The method of claim 2, wherein, The preset position data of the three receivers include a first distance, a second distance, and a first angle; the first distance is an installation distance of the mobile laser receiver and the second auxiliary mobile laser receiver; the second distance is an installation distance of the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver; and the first angle is an included angle between a line connecting the mobile laser receiver and the first auxiliary mobile laser receiver and a line connecting the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver; The calculation formula of the included angle between the body axis of the agricultural robot and the Y axis of the coordinate system is: wherein θ represents an included angle between a body axis of the agricultural robot and a Y-axis of the coordinate system, d yf2 represents the first distance, d ff represents a second distance, θ s2 represents a first angle, θ jy represents a first origin included angle, θ jf1 represents a second origin included angle, θ jf2 represents a third origin included angle.

4. The agricultural robot positioning method of claim 1, wherein, Based on the Kalman filtering algorithm, according to the previous time inertial navigation data, a corrected position of the marked mobile laser receiver is determined, and the method specifically includes: Based on the previous time inertial navigation data, a moving acceleration and an actual heading angle of the marked mobile laser receiver are determined; According to the moving acceleration of the marked mobile laser receiver, a to-be-verified position of the marked mobile laser receiver at the current time is calculated; According to the actual heading angle of the marked mobile laser receiver, a predicted position of the marked mobile laser receiver at the current time is calculated; Based on the Kalman filtering algorithm, the to-be-verified position and the predicted position are fused to obtain the corrected position of the marked mobile laser receiver.

5. The agricultural robot positioning method of claim 2, wherein, According to the next time mobile end laser receiving position group, whether shielding occurs between multiple mobile laser receivers is determined, and the method specifically includes: According to the next time mobile end laser receiving position group, a plurality of connection line directions are determined; the plurality of connection line directions include a first connection line direction, a second connection line direction and a third connection line direction; the first connection line direction is a connection line direction of the mobile laser receiver and the first auxiliary mobile laser receiver at the next time; the second connection line direction is a connection line direction of the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver at the next time; and the third connection line direction is a connection line direction of the mobile laser receiver and the second auxiliary mobile laser receiver at the next time; It is judged whether the extension of each connection line direction passes through the coordinate origin, and a judgment result is generated; when the judgment result is that the extension of any connection line direction passes through the coordinate origin, it is represented that an occlusion appears; and when the judgment result is that the extensions of all connection line directions do not pass through the coordinate origin, it is represented that no occlusion appears.

6. The agricultural robotic positioning method of claim 1, wherein, According to the corrected position of the marker mobile laser receiver and the mobile laser receiving time obtained by the marker mobile laser receiver at the corrected position, a secondary correction heading angle is calculated, specifically including: The first laser receiving parameter and the second laser receiving parameter collected by the marker mobile laser receiver at the corrected position are acquired; The distance of the preset laser receiving sensing component on the marker mobile laser receiver is acquired; According to the corrected position of the marker mobile laser receiver, the first laser receiving parameter, the second laser receiving parameter and the preset laser receiving sensing component distance, a secondary correction heading angle is calculated.

7. An agricultural robot positioning system characterized by, The agricultural robot positioning system includes: A data acquisition module is configured to acquire a laser emission rotation period, a fixed laser receiving time, a preset plurality of mobile laser receiving relative positions, a mobile laser receiving time group, previous time inertial navigation data and current time inertial navigation data; the mobile laser receiving time group includes mobile laser receiving times collected by a plurality of mobile laser receivers arranged on an agricultural robot according to the preset plurality of mobile laser receiving relative positions; A heading angle primary correction module is configured to calculate a primary correction heading angle of a marker mobile laser receiver according to the laser emission rotation period, the fixed laser receiving time, the preset plurality of mobile laser receiving relative positions, the mobile laser receiving time group and the current time inertial navigation data; the marker mobile laser receiver is any of the mobile laser receivers; A laser receiver position correction module is configured to determine a corrected position of the marker mobile laser receiver according to the previous time inertial navigation data based on a Kalman filtering algorithm; A current receiving position group determination module is configured to calculate a current time mobile end laser receiving position group according to the corrected position of the marker mobile laser receiver and the preset plurality of mobile laser receiving relative positions; the mobile end laser receiving position group includes positions of a plurality of mobile lasers arranged according to the preset plurality of mobile laser receiving relative positions. A next moment receiving position group determination module is configured to calculate a next moment mobile end laser receiving position group based on the current moment mobile end laser receiving position group and the initial correction heading angle of the marker mobile laser receiver; An occlusion judgment module is configured to: Judge whether occlusion occurs among the plurality of mobile laser receivers based on the next moment mobile end laser receiving position group; If no occlusion occurs among the plurality of mobile laser receivers, calculate a secondary correction heading angle based on the corrected position of the marker mobile laser receiver and the mobile laser receiving time obtained by the marker mobile laser receiver at the corrected position; the secondary correction heading angle and the current moment mobile end laser receiving position group constitute the current position information of the agricultural robot; If occlusion occurs among the plurality of mobile laser receivers, generate a receiver position calculation instruction; the receiver position calculation instruction is used to calculate the position of the occluded mobile laser receiver based on the position of the mobile laser receiver that is not occluded and in combination with the preset plurality of mobile laser receiving relative positions at the next moment.

8. An agricultural robot positioning device, characterized by The agricultural robot positioning device comprises at least one set of fixed end laser assembly, at least one set of mobile end laser receiving assembly, inertial navigation component and processor; The fixed end laser assembly comprises a laser emitter and a fixed laser receiver; the mobile end laser receiving assembly comprises a mobile laser receiver, a first auxiliary mobile laser receiver and a second auxiliary mobile laser receiver; The mobile laser receiver, the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver are arranged on the agricultural robot according to a preset plurality of mobile laser receiving relative positions; the line connecting the mobile laser receiver and the first auxiliary mobile laser receiver is parallel to the body axis direction of the agricultural robot; The laser emitter is used to emit a rotating laser signal according to a laser emission rotation period; the fixed laser receiver is used to receive the rotating laser signal and record a fixed laser receiving time; the mobile laser receiver, the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver are all used to receive the rotating laser signal and record corresponding mobile laser receiving times; The inertial navigation component is arranged on the agricultural robot and is used to collect inertial navigation data of the agricultural robot at different moments; The processor is connected with the laser emitter, the fixed laser receiver, the mobile laser receiver, the first auxiliary mobile laser receiver, the second auxiliary mobile laser receiver and the inertial navigation component respectively and is used to position the agricultural robot based on the agricultural robot positioning method of claim 1.

9. The agricultural robotic positioning device of claim 8, wherein, The preset plurality of mobile laser receiving relative positions are three points connected in a straight angle triangle formed by the projections of the mobile laser receiver, the first auxiliary mobile laser receiver and the second auxiliary mobile laser receiver on a horizontal plane.

Images