A full path coverage planning method for unmanned lawn mowing vehicles based on geographic coordinate system

Abstract

The present invention discloses a full-path coverage planning method for an unmanned lawn mower based on a geographic coordinate system, and relates to the technical field of path planning, including calculating a rectangular frame of a working area; constructing grid lines; calculating the intersections of the grid lines with the polygons of the working area and the obstacle area; grouping the grid line intersections to construct a line segment group set; calculating the shortest distance line segment group at the current position; storing the line segment group endpoints in sequence; completing the line segment group set traversal; and outputting the results, thereby improving the mowing efficiency and coverage rate of the unmanned lawn mower in a large range of grassland areas.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a full path coverage planning method for an unmanned lawn mower based on a geographic coordinate system. Background Art

[0002] Full path coverage planning requires that the path of the unmanned vehicle should cover all set working areas and avoid obstacle areas. For example, Chinese patent CN2019102343776 discloses a path planning method for an intelligent lawn mower, which establishes a coordinate system with an initial position as the origin; controls the intelligent lawn mower to walk around the working area along the boundary line, uses a gyroscope sensor to obtain the travel angle value of the intelligent lawn mower in real time, and records the real-time moving distance value, so as to obtain the real-time position coordinates of the intelligent lawn mower; constructs a global area map based on the full position coordinates recorded during the walking process of the intelligent lawn mower from the starting point to the end point of the boundary line; and controls the intelligent lawn mower to plan and cover the working area according to the global area map and the real-time position coordinates of the intelligent lawn mower.

[0003] However, in the prior art, full-path coverage planning methods are usually based on relative coordinate systems of smaller areas. In large grassland environments, the navigation of unmanned lawn mowers requires the use of geographic coordinate systems in units of longitude and latitude. Currently, there is no suitable full-path coverage planning method. Summary of the invention

[0004] To solve the above problems, the present invention provides a full-path coverage planning method for an unmanned lawn mower based on a geographic coordinate system, which can improve the mowing efficiency and coverage rate of the unmanned lawn mower in a large-scale grassland environment.

[0005] A full path coverage planning method for an unmanned lawn mower based on a geographic coordinate system comprises the following steps:

[0006] A1: Calculate the rectangular frame of the working area;

[0007] A2: Construct grid lines;

[0008] A 3: Calculate the intersection points between the grid lines and the polygons of the working area and obstacle area;

[0009] A 4: Grid line intersections are grouped to construct line segment group sets;

[0010] A5: Calculate the shortest distance line segment group at the current position;

[0011] A 6: Store the endpoints of the line segment group in order;

[0012] A 7: The traversal of the line segment group set is completed;

[0013] A 8: Output the results.

[0014] As a further improvement of the above technical solution, in step A1, the working area and the obstacle area are set to be constructed in the form of polygonal vertices, the number of working areas is unique, and there are multiple obstacle areas; according to the coordinates of all polygonal vertices in the set working area, the minimum and maximum coordinates in the longitude and latitude directions of the working area are calculated to form a rectangular frame surrounding the working area.

[0015] As a further improvement of the above technical solution, the steps for calculating the minimum and maximum longitude and latitude coordinates of the working area are as follows:

[0016] a) Set the initial values ​​of the latitude and longitude maximum and minimum variables LngMaxX, LatMaxX, LngMinX, and LatMinX to 180.0, 90.0, 0.0, and 0.0 respectively;

[0017] b) Traverse all polygon vertices and compare the vertex longitude and latitude coordinates with the maximum and minimum longitude and latitude variables

[0018] Compare the values ​​of LngMax, LatMax, LngMin, and LatMin;

[0019] c) If the vertex latitude coordinate value is less than LatMin, LatMin is set as the coordinate value; if the vertex latitude coordinate value is greater than LatMax, LatMax is set as the coordinate value; if the vertex longitude coordinate value is less than LngMin, LngMin is set as the coordinate value; if the vertex longitude coordinate value is greater than LngMax, LngMax is set as the coordinate value;

[0020] d) After the traversal is completed, the values ​​of the variables LngMin, LatMin, LngMax, and LatMax are the required minimum and maximum longitude and latitude values.

[0021] As a further improvement of the above technical solution, in step A2, the diagonal length of the rectangular frame calculated above is used as the grid length L, the point with the smallest longitude of the rectangular frame is used as the starting point, the set initial angle A is used as the grid line angle, and the working area is initially grid-divided using the set path spacing D as the grid line spacing to form n grid lines covering the entire working area.

[0022] As a further improvement of the above technical solution, in step A 3, a line segment set S1 is constructed with the edges of the polygons of the working area and the obstacle area, and the intersections of the above n grid lines with the line segments in the line segment set are calculated respectively, and the intersection points are obtained respectively, and the index of the line segment where the intersection point is located in the set is recorded.

[0023] As a further improvement of the above technical solution, the line segment intersection is obtained by the following steps:

[0024] a) Represent two line segments using the two - point form. The four points are (x1, y1), (x2, y2) and (x3, y3), (x4, y4) respectively;

[0025] b) Calculate the denominator term D1, with the formula as follows:

[0026] D1 = (x2 - x1)*(y4 - y3)-(y2 - y1)*(x4 - x3)

[0027] c) Judge the denominator term D1. If D1 is equal to 0, the two line segments are parallel and have no intersection point;

[0028] d) Calculate the numerator term N1, with the formula as follows:

[0029] N1 = (y1 - y3)*(x4 - x3)-(x1 - x3)*(y4 - y3)

[0030] e) Calculate the result term R, R = N1 / D1;

[0031] f) Calculate the numerator term N2, with the formula as follows:

[0032] N2 = (y1 - y3)*(x2 - x1)-(x1 - x3)*(y2 - y1)

[0033] g) Calculate the result term S, S = N2 / D1;

[0034] h) If 0 < R < 1 and 0 < S < 1, the intersection point of the line segments can be calculated (R x , R y ), with the formula as follows:

[0035] R x = x1+(R*(x2 - x1))

[0036] R y = y1+(R*(y2 - y1)).

[0037] As a further improvement of the above - mentioned technical solution, in step A4, the intersection points on the same grid line are grouped as a set. The intersection points in the same group are pairwise constructed into a line segment according to the calculation order and rules, and all the newly - constructed line segments form a line - segment set S2. At the same time, record the indices of the line segments where the two vertices of the line segment are located in the set S1.

[0038] As a further improvement of the above - mentioned technical solution, the rules for constructing line segments are as follows:

[0039] a) Judge the number of intersection points between the grid line and the polygon;

[0040] b) If the number of intersection points between the grid line and the polygon is 0, no processing is performed;

[0041] c) If the number of intersections between the grid line and the polygon is 1, it means that the intersection of the grid line and the vertex of the polygon is on the vertex of the polygon, then the intersection is ignored and the two vertices of the grid line are used to directly form a line segment;

[0042] d) If the number of intersections between the grid line and the polygon is 2, the grid line passes through the two sides of the polygon, and the vertices p1 and p2 at both ends of the grid line and the two intersection points i1 and i2 are sequentially connected to form two grid lines (p1, i1)

[0043] and (p2,i2);

[0044] e) If the number of intersections between the grid line and the polygon is greater than 2 and is an even number, and the vertices at both ends of the grid line are p1 and p2, traverse the intersection set, calculate the distance between all intersections and the line segment vertex p1, sort the intersections from near to far to get a set (i1, i2, i3…in), and use p1 as the starting point to form line segments in pairs;

[0045] If the number of intersections between the grid line and the polygon is greater than 2 and is an odd number, it means that some of the intersections between the grid line and the polygon overlap with the polygon vertices. The intersection set is traversed to remove the intersections that overlap with the polygon vertices. The subsequent processing is the same as the above steps.

[0046] As a further improvement of the above technical solution, the line segment set S2 is traversed, and adjacent line segments with the same two vertex indexes are grouped together to form a set Z of line segment groups; according to the starting position of the vehicle, the group closest to the first and last line segment endpoints of each line segment group in the set Z is calculated respectively.

[0047] As a further improvement of the above technical solution, in steps A5 and A6, a result set is constructed, in which all path points of the shortest path are stored; a line segment is constructed with the current position Ps and the target position Pe, and it is calculated whether there is an intersection between the line segment and all obstacle area polygons. If there is an intersection, the intersection is added to the result set, and then the distances between the two endpoints of the edge of the polygon where the intersection is located and the target point Pe are calculated respectively, and the endpoint with the smaller distance is stored as the next path point in the result set; a line segment is constructed with the endpoint and the target point Pe, and the above process is repeated until there is no intersection between the line segment and the obstacle area polygon, and the distance between all path points in the result set is calculated as the shortest path distance; a path point set is constructed, and after finding the line segment group closest to the starting position of the vehicle, the endpoints of the line segment group are stored in the set in order, in a head-to-tail order, and then the shortest path distances with other line segment groups are calculated with the last stored endpoint, and the previous process is repeated until all line segment group endpoints are stored in the set and the construction is completed.

[0048] It can be seen from the above technical solutions that the beneficial effects of the present invention are: simple calculation, easy implementation, fast calculation speed, and strong reliability of the automatically planned path; the entire mowing area is traversed with the optimal path; and the mowing efficiency and coverage rate of the unmanned mowing vehicle in a large range of grassland areas are improved. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0050] Figure 1 The figure is a flowchart of a planning method according to a specific embodiment of the present invention. DETAILED DESCRIPTION

[0051] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical scheme of the present invention will be clearly and completely described below in conjunction with the drawings in this specific embodiment. Obviously, the embodiments described below are only part of the embodiments of the present invention, not all of them. Based on the embodiments in this patent, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this patent.

[0052] See also Figure 1 As shown, the present invention provides: a full path coverage planning method for an unmanned lawn mower based on a geographic coordinate system, characterized in that it includes the following steps: A1: calculating a rectangular frame of a working area;

[0053] A2: Construct grid lines;

[0054] A 3: Calculate the intersection points between the grid lines and the polygons of the working area and obstacle area;

[0055] A 4: Grid line intersections are grouped to construct line segment group sets;

[0056] A5: Calculate the shortest distance line segment group at the current position;

[0057] A 6: Store the endpoints of the line segment group in order;

[0058] A 7: The traversal of the line segment group set is completed;

[0059] A 8: Output the results.

[0060] Specifically, the set working area and obstacle area are constructed in the form of polygonal vertices, the number of working areas is unique, and the number of obstacle areas can be multiple; according to the coordinates of all polygonal vertices of the set working area, the minimum and maximum coordinates in the longitude and latitude directions are calculated to form a rectangular frame surrounding the working area;

[0061] The steps for calculating the minimum and maximum longitude and latitude coordinates of the working area are as follows:

[0062] a: Set the initial values ​​of the latitude and longitude maximum and minimum variables LngMaxX, LatMaxX, LngMinX, and LatMinX to 180.0, 90.0, 0.0, and 0.0 respectively;

[0063] b: Traverse all polygon vertices and compare the vertex longitude and latitude coordinates with the values ​​of the maximum and minimum longitude and latitude variables LngMax, LatMax, LngMin, and LatMin;

[0064] b: If the vertex latitude coordinate value is less than LatMin, LatMin is set as the coordinate value; if the vertex latitude coordinate value is greater than LatMax, LatMax is set as the coordinate value; if the vertex longitude coordinate value is less than LngMin, LngMin is set as the coordinate value; if the vertex longitude coordinate value is greater than LngMax, LngMax is set as the coordinate value;

[0065] d: After the traversal is completed, the values ​​of variables LngMin, LatMin, LngMax, and LatMax are the minimum and maximum longitude and latitude values ​​required;

[0066] The diagonal length of the rectangular frame calculated above is used as the grid length L, the point with the smallest longitude in the rectangular frame is used as the starting point, the set initial angle A is used as the grid line angle, and the working area is initially grid-divided using the set path spacing D as the grid line spacing to form n grid lines covering the entire working area.

[0067] Construct a line segment set S1 with each edge of the polygons of the working area and the obstacle area, calculate the intersections of the n grid lines and the line segments in the line segment set, obtain the intersection points, and record the index of the line segment where the intersection point is located in the set;

[0068] The steps for finding the intersection of line segments are as follows:

[0069] a: Use two-point form to represent two line segments, the four points are (x1, y1), (x2, y2) and (x3, y3), (x4, y4); b: Calculate the denominator D1, the formula is as follows:

[0070] D1=(x2-x1)*(y4-y3)-(y2-y1)*(x4-x3)

[0071] c: Judge the denominator term D1. If D1 equals 0, the two line segments are parallel and have no intersection point.

[0072] d: Calculate the numerator term N1. The formula is as follows:

[0073] N1 = (y1 - y3) * (x4 - x3) - (x1 - x3) * (y4 - y3)

[0074] e: Calculate the result term R, R = N1 / D1;

[0075] f: Calculate the numerator term N2. The formula is as follows:

[0076] N2 = (y1 - y3) * (x2 - x1) - (x1 - x3) * (y2 - y1)

[0077] g: Calculate the result term S, S = N2 / D1;

[0078] h: If 0 < R < 1 and 0 < S < 1, the intersection point of the line segments can be calculated (R x , R y ), and the formula is as follows:

[0079] R x = x1 + (R * (x2 - x1))

[0080] R y = y1 + (R * (y2 - y1))

[0081] The intersection points on the same grid line form a group. The intersection points in the same group are pairwise constructed into a line segment according to the calculation order and rules. All the newly constructed line segments form a line segment set S2, and at the same time, record the indices of the line segments where the two vertices of the line segment are located in the set S1;

[0082] The rules for constructing line segments are as follows:

[0083] a: Judge the number of intersection points between the grid line and the polygon;

[0084] b: If the number of intersection points between the grid line and the polygon is 0, no processing is performed;

[0085] c: If the number of intersection points between the grid line and the polygon is 1, which means the intersection point of the grid line and the vertex of the polygon is on the vertex of the polygon, then ignore this intersection point and directly form a line segment with the two vertices of the grid line;

[0086] d: If the number of intersection points between the grid line and the polygon is 2, the grid line passes through two sides of the polygon. According to the order, the two end vertices p1, p2 of the grid line and the two intersection points i1, i2 are used to form two grid lines (p1, i1) and (p2, i2);

[0087] e: If the number of intersections between the grid line and the polygon is greater than 2 and is an even number, and the vertices at both ends of the grid line are p1 and p2, traverse the intersection set, calculate the distance between all intersections and the line segment vertex p1, sort the intersections from near to far to get a set (i1, i2, i3…in), and use p1 as the starting point to form line segments in pairs;

[0088] f: If the number of intersections between the grid line and the polygon is greater than 2 and is an odd number, it means that the intersections between the grid line and the polygon overlap with the vertices of the polygon. Traverse the intersection set and remove the intersections that overlap with the vertices of the polygon. The subsequent processing is the same as step e above.

[0089] Traverse the line segment set S2, group the adjacent line segments with the same two vertex indexes into a group to form a line segment group set Z;

[0090] According to the starting position of the vehicle, the group of segments closest to the first and last endpoints of each segment group in set Z is calculated respectively. Since there are obstacle areas, the vehicle needs to bypass them, and the shortest path calculation method is used here.

[0091] The shortest path calculation method is as follows: construct a result set, which stores all the path points of the shortest path; construct a line segment with the current position Ps and the target position Pe, calculate whether there is an intersection between the line segment and all the obstacle area polygons, and if there is an intersection, add the intersection to the result set, and then calculate the distance between the two endpoints of the edge of the polygon where the intersection is located and the target point Pe, and store the endpoint with the smaller distance as the next path point in the result set; then construct a line segment with the endpoint and the target point Pe, and repeat the above process until there is no intersection between the line segment and the obstacle area polygon, then the distance between all the path points in the result set is the shortest path distance.

[0092] Construct a path point set, find the line segment group closest to the vehicle's starting position, and store the endpoints of the line segment group in the set in order. The order is end-to-end, that is, the end point of the previous line segment is connected to the start point of the next line segment. Then, the shortest path distance with other line segment groups is calculated with the last stored endpoint, and the previous process is repeated until all the line segment group endpoints are stored in the set, and the construction is completed.

[0093] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same and similar parts between the various embodiments can be referenced to each other.

[0094] The terms "upper", "lower", "outer side", "inner side", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish the relative relationship in position if they exist, and do not need to be qualitative. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the present invention described here can be implemented in an order other than those illustrated or described here. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions.

[0095] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A full path coverage planning method for an unmanned lawn mower based on a geographic coordinate system, characterized in that: The steps include: A1: Calculate the rectangular frame of the working area; A2: Construct grid lines; In step A2, the diagonal length of the rectangular frame calculated above is used as the grid length L, the point with the smallest longitude of the rectangular frame is used as the starting point, the set initial angle A is used as the grid line angle, and the working area is initially grid-divided using the set path spacing D as the grid line spacing to form n grid lines covering the entire working area; A3: Calculate the intersection points between the grid lines and the polygons of the working area and obstacle area; In step A3, a line segment set S1 is constructed with each edge of the polygons of the working area and the obstacle area, and the intersections of the n grid lines and the line segments in the line segment set are calculated, and the intersection points are obtained respectively, and the index of the line segment where the intersection point is located in the set is recorded; A4: Grid line intersections are grouped to construct line segment group sets; In step A4, the intersection points of the same grid line are grouped together, and the intersection points in the same group are used to construct a line segment in pairs according to the calculation order and rules. All the newly created line segments form a line segment set S2, and the indexes of the line segments where the two vertices of the line segments are located in the set S1 are recorded at the same time; The rules for constructing line segments are as follows: a) Determine the number of intersections between grid lines and polygons; b) If the number of intersections between the grid lines and the polygon is 0, no processing is done; c) If the number of intersections between the grid line and the polygon is 1, it means that the intersection of the grid line and the vertex of the polygon is on the vertex of the polygon, then the intersection is ignored and the two vertices of the grid line are used to directly form a line segment; d) If the number of intersections between the grid line and the polygon is 2, the grid line passes through two sides of the polygon, and the vertices p1 and p2 at both ends of the grid line and the two intersection points i1 and i2 are sequentially combined to form two grid lines (p1, i1) and (p2, i2); e) If the number of intersections between the grid line and the polygon is greater than 2 and is an even number, and the vertices at both ends of the grid line are p1 and p2, traverse the intersection set, calculate the distance between all intersections and the line segment vertex p1, sort the intersections from near to far to get a set (i1, i2, i3…in), and use p1 as the starting point to form line segments in pairs; If the number of intersections between the grid line and the polygon is greater than 2 and is an odd number, it means that the intersections between the grid line and the polygon overlap with the vertices of the polygon. The intersection set is traversed to remove the intersections that overlap with the vertices of the polygon. The subsequent processing is the same as the above steps. A5: Calculate the shortest distance line segment group at the current position; A6: Store the endpoints of the line segment groups in order; A7: The traversal of the line segment group set is completed; A8: Output the result.

2. The full path coverage planning method of an unmanned lawn mower based on a geographic coordinate system according to claim 1, characterized in that: The steps to find the intersection of line segments are as follows: a) Use two-point form to represent two line segments. The four points are (x 1, y1), (x2,y2) and (x3,y3), (x4,y4); b) Calculate the denominator D1 using the following formula: D1=(x2-x1)*(y4-y3)-(y2-y1)*(x4-x3) c) Determine the denominator D1. If D1 is equal to 0, the two line segments are parallel and have no intersection. d) Calculate the numerator N1 using the following formula: N1=(y1-y3)*(x4-x3)-(x1-x3)*(y4-y3) e) Calculate the result term R, R = N1 / D1; f) Calculate the numerator N2 using the following formula: N2=(y1-y3)*(x2-x1)-(x1-x3)*(y2-y1) g) Calculate the result item S, S = N2 / D1; h) If 0 < R < 1 and 0 < S < 1, then the intersection point of the line segment (R x , R y ) can be calculated, and the formula is as follows: R x =x1+(R*(x2-x1)) R y =y1+(R*(y2-y1)).

3. The full path coverage planning method of the unmanned lawn mower based on the geographic coordinate system according to claim 1 is characterized in that: In step A1, the working area and obstacle area are set to be constructed in the form of polygonal vertices, the number of working areas is unique, and there are multiple obstacle areas; according to the coordinates of all polygonal vertices in the set working area, the minimum and maximum coordinates in the longitude and latitude directions of the working area are calculated to form a rectangular frame surrounding the working area.

4. The full path coverage planning method of the unmanned lawn mower based on the geographic coordinate system according to claim 3 is characterized in that: The steps to calculate the minimum and maximum longitude and latitude coordinates of the working area are as follows: a) Set the initial values ​​of the latitude and longitude maximum and minimum variables LngMaxX, LatMaxX, LngMinX, and LatMinX to 180.0, 90.0, 0.0, and 0.0 respectively; b) Traverse all polygon vertices and compare the vertex longitude and latitude coordinates with the values ​​of the maximum and minimum longitude and latitude variables LngMax, LatMax, LngMin, and LatMin; c) If the vertex latitude coordinate value is less than LatMin, LatMin is set as the coordinate value; if the vertex latitude coordinate value is greater than LatMax, LatMax is set as the coordinate value; if the vertex longitude coordinate value is less than LngMin, LngMin is set as the coordinate value; if the vertex longitude coordinate value is greater than LngMax, LngMax is set as the coordinate value; d) After the traversal is completed, the values ​​of the variables LngMin, LatMin, LngMax, and LatMax are the minimum and maximum longitude and latitude values ​​required.

5. The full path coverage planning method of the unmanned lawn mower based on the geographic coordinate system according to claim 1, characterized in that: Traverse the line segment set S2, group the adjacent line segments with the same two vertex indexes into a group to form a line segment group set Z; according to the starting position of the vehicle, calculate the group closest to the first and last line segment endpoints of each line segment group in set Z.

6. The full path coverage planning method of the unmanned lawn mower based on the geographic coordinate system according to claim 5, characterized in that: In steps A5 and A6, a result set is constructed, in which all path points of the shortest path are stored; a line segment is constructed with the current position Ps and the target position Pe, and it is calculated whether there is an intersection between the line segment and all obstacle area polygons. If there is an intersection, the intersection is added to the result set, and then the distances between the two endpoints of the edge of the polygon where the intersection is located and the target point Pe are calculated respectively, and the endpoint with the smaller distance is stored as the next path point in the result set; a line segment is constructed with the endpoint and the target point Pe, and the above process is repeated until there is no intersection between the line segment and the obstacle area polygon, and the distance between all path points in the result set is calculated as the shortest path distance; a path point set is constructed, and after finding the line segment group closest to the vehicle's starting position, the endpoints of the line segment group are stored in the set in order, in a head-to-tail order, and then the shortest path distances with other line segment groups are calculated with the last stored endpoint, and the previous process is repeated until all line segment group endpoints are stored in the set and the construction is completed.

Images