WO2023025105A1 - 一种耙地作业的路径规划方法及装置 - Google Patents

一种耙地作业的路径规划方法及装置 Download PDF

Info

Publication number
WO2023025105A1
WO2023025105A1 PCT/CN2022/114023 CN2022114023W WO2023025105A1 WO 2023025105 A1 WO2023025105 A1 WO 2023025105A1 CN 2022114023 W CN2022114023 W CN 2022114023W WO 2023025105 A1 WO2023025105 A1 WO 2023025105A1
Authority
WO
WIPO (PCT)
Prior art keywords
path
point
plot
slope
planned
Prior art date
Application number
PCT/CN2022/114023
Other languages
English (en)
French (fr)
Inventor
李晓宇
梅军辉
郭向明
Original Assignee
上海联适导航技术股份有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 上海联适导航技术股份有限公司 filed Critical 上海联适导航技术股份有限公司
Publication of WO2023025105A1 publication Critical patent/WO2023025105A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications

Definitions

  • the invention relates to the technical field of path planning, in particular to a path planning method and device for harrowing operations.
  • the automatic driving system of agricultural machinery is a kind of steering mechanism of traditional agricultural machinery as the controlled object, performs precise position calculation according to GNSS (Global Navigation Satellite System), and uses the planned driving path in the system as the target path for walking.
  • GNSS Global Navigation Satellite System
  • the automatic driving system of agricultural machinery can greatly liberate productivity, improve work efficiency, and provide convenience for agricultural operations.
  • Harrowing is a type of topsoil cultivation with a harrow (a farm tool), usually after plowing, before planting, or when moisture is preserved in early spring.
  • the large soil clods produced after the plowing of the cultivated land can be mechanically broken by harrowing.
  • the harrowing has the functions of loosening the soil, retaining water, and increasing the soil temperature.
  • One of the objectives of the present invention is to provide a path planning method and device for raking land, which is used to solve the problem that the current path planning method can only plan regular plots.
  • a path planning method for land raking operations comprising: obtaining boundary data of a land plot; obtaining a circumscribed rectangle of the land plot according to the boundary data of the land plot; planning a land raking operation within the circumscribed rectangle according to preset rules
  • a route is obtained to obtain a first planned route; the first planned route is clipped according to the boundary of the plot to obtain a second planned route; according to the second planned route, a third planned route that meets the driving requirements of agricultural machinery is obtained.
  • the obtaining the circumscribed rectangle of the plot according to the boundary data of the plot includes: performing convex hull calculation on the boundary data of the plot to obtain the smallest Convex polygon; calculate the minimum circumscribed rectangle of the convex polygon, and use the minimum circumscribed rectangle as the circumscribed rectangle of the plot.
  • the planning of the raking operation path within the circumscribed rectangle according to preset rules includes: when a path line segment with a slope of the first slope intersects with a side of the circumscribed rectangle of the plot in the advancing direction , then turn around and make the next path line segment according to the second slope from the intersection point; The next path segment is made at the intersection point according to the first slope; the first slope is different from the second slope.
  • one side of the circumscribed rectangle of the plot is set as a sliding side, and the remaining sides are non-sliding sides;
  • the planning of the raking operation path in the circumscribed rectangle according to preset rules further includes: when an effective next path segment cannot be obtained at an intersection point, and the intersection point is located on the sliding edge of the circumscribed rectangle, then Slide the sliding edge for a preset distance, and then make the next path line segment according to the second slope or the first slope.
  • the clipping the first planned path according to the plot boundary to obtain the second planned path includes: retracting the path line beyond the plot boundary in the first planned path to the On the boundary of the plot, delete the path line that has no intersection with the boundary of the plot to obtain the second planned path.
  • the obtaining a third planned route that meets the driving requirements of agricultural machinery according to the second planned route includes: the second planned route is a set of points; and the third planned route is initialized according to the second planned route ; Select a point to be processed except the first point and the last point from the second planning path; obtain the point corresponding to the point to be processed from the third planning path, and convert the previous point As the starting point, the latter point is used as the end point; calculate the curve point set that meets the agricultural machinery driving requirements from the starting point to the end point; replace the third planning path with the curve point set that removes the starting point and the end point.
  • the point corresponding to the point to be processed select the next point to be processed in the second planned path, obtain the point corresponding to the next point to be processed from the third planned path, and repeat the above process until the first All points in the second planning path except the first point and the last point are processed.
  • calculating a set of curve points from the start point to the end point that meets the driving requirements of the agricultural machinery includes: according to the point corresponding to the point to be processed in the third planned route, the start point, the end point , to obtain the starting point direction and the ending point direction; according to the steering radius of the agricultural machine, the starting point position, the ending point position, the starting point direction and the end point direction, use the dubins curve algorithm to obtain a curve point set that meets the driving requirements of the agricultural machine.
  • the present invention also provides a path planning device for harrowing operations, including:
  • the boundary acquisition module is used to obtain the boundary data of the plot; the rectangle generation module is used to obtain the circumscribed rectangle of the plot according to the boundary data of the plot; Plan the raking operation path in the circumscribed rectangle to obtain the first planned path; the second planning module is used to cut the first planned path according to the plot boundary to obtain the second planned path; the third planning module is used to obtain the second planned path according to The second planned route obtains a third planned route that meets the driving requirements of the agricultural machinery.
  • the rectangle generation module is also used to perform convex hull calculation on the boundary data of the plot to obtain the smallest convex polygon including all points in the boundary data; calculate the smallest circumscribed rectangle of the convex polygon , using the smallest circumscribing rectangle as the circumscribing rectangle of the plot.
  • the first planning module is further configured to: when a path line segment with a slope of the first slope has an intersection with a side of the circumscribed rectangle of the plot in the advancing direction, turn around and press The second slope is used as the next path line segment; when a path line segment with a slope of the second slope has an intersection point with a side of the circumscribed rectangle of the plot in the forward direction, then turn around and make the next path line segment from the intersection point according to the first slope.
  • a path segment; the first slope is different from the second slope.
  • a path planning method and device for raking operations provided by the present invention can bring at least the following beneficial effects: the present invention plans the path through the circumscribed rectangle of the plot, then cuts the planned path, and then The cut path is optimized according to the driving requirements of agricultural machinery, so as to realize the path planning of irregular plots, expand the application scenarios of the automatic driving system of agricultural machinery, thereby improving the efficiency of raking land; Path planning improves the effect of harrowing.
  • Fig. 1 is a flowchart of an embodiment of a path planning method for raking operations of the present invention
  • Fig. 2 is a schematic diagram of clipping the first planned path in Fig. 1;
  • Fig. 3 is a flow chart of another embodiment of a path planning method for raking operations of the present invention.
  • Fig. 4 is a schematic diagram of the harrowing operation path planned in the circumscribed rectangle
  • Fig. 5 is a flow chart of another embodiment of a path planning method for raking operations of the present invention.
  • Fig. 6 is another schematic diagram of the harrowing operation path planned in the circumscribed rectangle
  • Fig. 7 is a structural schematic diagram of an embodiment of a path planning device for raking operations according to the present invention.
  • Fig. 8 is a structural schematic diagram of an automatic driving system for agricultural machinery in a specific application embodiment of the present invention.
  • Fig. 9 is a schematic diagram of a plot boundary figure in a specific application embodiment of the present invention.
  • Fig. 10 is a schematic diagram of the smallest circumscribed rectangle of a plot in a specific application embodiment of the present invention.
  • Fig. 11 is a schematic diagram of the operation of path planning within the minimum circumscribed rectangle in a specific application embodiment of the present invention.
  • Fig. 12 is a schematic diagram of a first planned path in a specific application embodiment of the present invention.
  • Fig. 13 is a schematic diagram of a second planned path in a specific application embodiment of the present invention.
  • Fig. 14 is a schematic diagram of a third planned path in a specific application embodiment of the present invention.
  • a kind of path planning method of harrowing operation comprises:
  • Step S100 obtains the boundary data of the plot
  • Step S200 obtains the circumscribed rectangle of the plot according to the boundary data of the plot
  • Step S300 Plan the raking operation path within the circumscribed rectangle according to preset rules to obtain the first planned path;
  • Step S400 clipping the first planned route according to the plot boundary to obtain the second planned route
  • Step S500 obtains a third planned route that meets the driving requirements of the agricultural machinery.
  • the parcel's boundary data is a set of data points that reflect the parcel's boundary.
  • the agricultural machinery can be manually driven to drive around the border of the plot to collect the border data of the plot.
  • the shape of the plot may be irregular, so the plot boundary graph obtained according to the boundary data of the plot may also be an irregular graph.
  • Graham scanning method or Jarvis stepping method can be used to calculate the convex hull of the boundary data of the plot to obtain the smallest convex polygon.
  • the following methods can be used to calculate the minimum circumscribed rectangle of a convex polygon: calculate the center of the convex polygon; rotate the convex polygon step by step according to the preset angle (for example, rotate in the range of 0-90°, the interval is set to 1°), and calculate the convex polygon after each rotation A simple circumscribed rectangle of a polygon; select the circumscribed rectangle with the smallest area from all circumscribed rectangles, and rotate the circumscribed rectangle by the same degree in the opposite direction to obtain the smallest circumscribed rectangle of a convex polygon.
  • the preset angle for example, rotate in the range of 0-90°, the interval is set to 1°
  • the area contained in the circumscribed rectangle/minimum circumscribed rectangle is a regular plot, and the path planning for the circumscribed rectangle/minimum circumscribed rectangle can be planned by using the path planning technology for regular plots.
  • path planning technology for regular plots.
  • straight line harrowing path planning diagonal harrowing path planning.
  • a number of parallel line segments are obtained through straight-line raking path planning, and a number of intersecting line segments are obtained through diagonal raking path planning.
  • the first planned path is clipped according to the plot boundary to obtain the second planned path. Specifically, it includes: retracting the path lines exceeding the boundary of the plot in the first planning path to the boundary of the plot, deleting the path lines that do not intersect with the boundary of the plot, and obtaining the second planning path.
  • the dotted line is the plot boundary
  • there are two line segments AB and BC in the first planned path and these two line segments intersect the plot boundary at points A1, A2, C1, and C. Cut the first planned path according to the plot boundary, then the A1B line segment is retracted to the A1 point, the BC1 line segment is retracted to the C1 point, and the AA2 line segment is retracted to the A2 point.
  • the first planned path is expressed by a set of points arranged in order, such as ⁇ A, B, C ⁇
  • the corresponding second planned path is ⁇ A2, A1, C1, C ⁇ , which is also arranged in order.
  • the first planning path is expressed by a set of line segments arranged in order, such as ⁇ AB, BC ⁇
  • the corresponding second planning path is ⁇ A2A1, A1C1, C1C ⁇ , which is also arranged in order.
  • step S500 can be further refined as:
  • Step S510 initializes the third planned path according to the second planned path.
  • the points in the third planned path correspond one-to-one to the points in the second planned path; for example, the point set of the third planned path is equal to the point set of the second planned path.
  • Step S520 selects a point P x to be processed except the first point and the last point from the second planning path;
  • Step S530 obtains the point corresponding to the pending point P x from the third planning path, using the previous point as the starting point, and the subsequent point as the end point;
  • Step S540 calculates the set of curve points meeting the agricultural machinery driving requirements from the start point to the end point;
  • Step S550 replaces the point corresponding to the point P x to be processed in the third planning path with the curve point set that removes the start point and the end point;
  • Step S560 judges whether all points in the second planning path except the first point and the last point have been processed; if not, select the next point to be processed in the second planning path, and jump to step S530. If so, end.
  • Step S540 may further include:
  • the starting point direction and the ending point direction are obtained; according to the turning radius of the agricultural machine, the starting point position, the end point position, the starting point direction, and the end point direction, the direction that meets the driving requirements of the agricultural machine is obtained Set of curve points.
  • the driving requirements of agricultural machinery include the requirement of turning radius of agricultural machinery when turning.
  • the direction of the starting point is the direction from the starting point to the point corresponding to the point P x to be processed in the third planned path;
  • the direction of the end point is the direction from the point corresponding to the point P x to be processed in the third planned path to the end point.
  • the dubins curve algorithm can be used to obtain the curve point set that meets the driving requirements of the agricultural machinery.
  • the shortest path from the start point to the end point can be obtained, which is conducive to shortening the distance when the agricultural machine harrows the ground according to the planned path, and reduces the energy consumption of the agricultural machine.
  • the second planning path is ⁇ A2, A1, C1, C ⁇ .
  • Initialize the third planning path as ⁇ A2, A1, C1, C ⁇ . Excluding the first point A2 and the last point C, there are two pending points A1 and C1. First select point A1, the previous point A2 of A1 is the starting point, and the last point C1 of A1 is the end point.
  • the dubins curve algorithm to obtain the curve point set M from A2 to C1, such as ⁇ A2,A11,A12 ,A13,C1 ⁇ ; remove the starting point and end point from M, get the set M′ ⁇ A11,A12,A13 ⁇ , replace the point A1 in the third planning path with the set M′, and get the updated third planning path ⁇ A2, A11,A12,A13,C1,C ⁇ .
  • the next point C1 to be processed on the second planned path is processed.
  • the previous point A13 of C1 is the starting point
  • the next point C of C1 is the end point
  • the curve point set M from A13 to C that meets the requirements of agricultural machinery driving is obtained, such as ⁇ A13,C11,C12 , C ⁇ ; remove the starting point and end point, get the set M′ ⁇ C11,C12 ⁇ , replace the C1 point in the third planning path with the set M′, and get the updated third planning path
  • any polyline segment can be taken from the second planned path, and converted into a curved segment that meets the driving requirements of agricultural machinery; the polyline segment is composed of two adjacent line segments.
  • a third planned path is obtained from all curve segments.
  • the second planned path is ⁇ A2A1, A1C1, C1C ⁇ .
  • A2A1 and A1C1 constitute a broken line segment A2C1, A2 and C1 are the starting point and end point of the broken line segment, and according to the steering radius of the agricultural machinery, the corresponding curve segment is obtained by using the dubins curve algorithm; similarly, A1C1 and C1C form another broken line segment A1C, and we get The curve segment corresponding to the A1C polyline segment; combine these two curve segments to obtain the third planning path.
  • the third planned path is a path that satisfies the driving requirements of the agricultural machinery.
  • path planning is carried out on the circumscribed rectangle of the plot, and then the planned path is cut, and then the cut path is optimized according to the driving requirements of agricultural machinery, so as to realize the path planning of irregular plots and expand the scope of agricultural machinery.
  • the automatic driving system is used in the scene of raking, thus improving the efficiency of raking.
  • step S300 includes:
  • Step S310 When a path line segment with a slope of the first slope K1 intersects with a side of the circumscribed rectangle of the plot in the forward direction, turn around and make the next path line segment according to the second slope K2 from the intersection point;
  • Step S311 When a path line segment with the second slope K2 has an intersection with a side of the circumscribed rectangle of the plot in the forward direction, turn around and make the next path line segment according to the first slope K1 from the intersection point.
  • the first slope K1 is different from the second slope K2.
  • the meaning of valid can be determined according to the design needs. For example, if the distance between the next path line segment and the adjacent parallel line segment is smaller than the width of the agricultural tool, the next path line segment is invalid; if the next path line segment is completely outside the circumscribed rectangle , the next path segment is invalid.
  • the path planning within the circumscribed rectangle is carried out with the vertex D as the starting point, and according to the above rules, a series of path segments where the slopes K1 and K2 intersect are obtained in sequence, These path segments constitute the first planned path. It can be seen that the planned path meets the requirements of diagonal harrowing, and the agricultural machine can improve the harrowing effect according to the planned path.
  • step S300 includes:
  • Step S320 When a path line segment with a slope of the first slope K1 has an intersection point with a side of the circumscribed rectangle of the plot in the forward direction, turn around and make the next path line segment according to the second slope K2 from the intersection point;
  • Step S321 When a path line segment with a slope of the second slope K2 has an intersection point with a side of the circumscribed rectangle of the plot in the forward direction, turn around and make the next path line segment according to the first slope K1 from the intersection point;
  • Step S322 When an effective next path line segment cannot be obtained at an intersection point, and the intersection point is located on the sliding side of the circumscribed rectangle, slide the preset distance along the sliding side, and then make the next path line segment according to the second slope or the first slope .
  • the first slope K1 is different from the second slope K2.
  • the path planning according to steps S320 and S321 cannot obtain the first planned path that meets the requirements of diagonal raking. For example, when the intersection point with the circumscribed rectangle is a vertex of a rectangle, effective The next path segment of , leading to the end of path planning.
  • the dotted line box represents the circumscribed rectangle ABCD of the plot, and the path planning in the circumscribed rectangle is carried out with the vertex D as the starting point, and the path planning is carried out according to steps S320 and S321.
  • the intersection point is point A, because point A A valid next path segment cannot be obtained, and the path planning ends.
  • the path obtained at this time does not meet the requirements of diagonal raking.
  • a path planning device for raking operations includes:
  • a boundary acquisition module 100 configured to acquire boundary data of plots
  • the rectangle generation module 200 is used to obtain the circumscribed rectangle of the plot according to the boundary data of the plot;
  • the first planning module 300 is configured to plan a raking operation path within a circumscribed rectangle according to preset rules to obtain a first planned path;
  • the second planning module 400 is configured to cut the first planned path according to the plot boundary to obtain the second planned path;
  • the third planning module 500 is configured to obtain a third planned route that meets the driving requirements of the agricultural machinery according to the second planned route.
  • the agricultural machine can be manually driven for a circle along the boundary of the plot to obtain the boundary data of the plot.
  • the shape of the plot may be irregular, so the plot boundary graph obtained according to the boundary data of the plot may also be an irregular graph.
  • the area contained in the circumscribed rectangle/minimum circumscribed rectangle is a regular plot, and the path planning for the circumscribed rectangle/minimum circumscribed rectangle can be planned by using the path planning technology for regular plots.
  • the second planning module 400 includes: retracting the path lines exceeding the boundary of the plot in the first planning path to the boundary of the plot, deleting the path lines that do not intersect with the boundary of the plot, and obtaining the second planning path.
  • the third planning module 500 can be further used for:
  • the point corresponding to the point to be processed P x select the next point to be processed in the second planning path, obtain the point corresponding to the next point to be processed from the third planning path, repeat the above process until the second planning path except All points other than the first and last are processed.
  • the third planning module 500 can also be further used to: obtain the starting point direction and the ending point direction according to the point, starting point, and ending point corresponding to the point P x to be processed in the third planning path; direction and the direction of the end point to obtain a set of curve points that meet the driving requirements of agricultural machinery.
  • the dubins curve algorithm can be used to obtain the curve point set that meets the driving requirements of the agricultural machinery.
  • the shortest path from the start point to the end point of the broken line segment can be obtained, which is beneficial to shorten the distance when the agricultural machine rakes the land according to the planned path, and reduces the energy consumption of the agricultural machine.
  • path planning is carried out on the circumscribed rectangle of the plot, and then the planned path is cut, and then the cut path is optimized according to the driving requirements of agricultural machinery, so as to realize the path planning of irregular plots and expand the scope of agricultural machinery.
  • the application scenario of the automatic driving system thereby improving the efficiency of harrowing.
  • FIG. 6 Another embodiment of the present invention, as shown in Figure 6, a path planning device for raking operations, compared with the embodiment shown in Figure 1, the difference lies in:
  • the first planning module 300 is used to make a U-turn from the intersection point to make the next path segment according to the second slope K2 when a path line segment with a slope of the first slope K1 intersects with a side of the circumscribed rectangle of the plot in the forward direction. ;
  • a path line segment with a slope of the second slope K2 has an intersection point with a side of the circumscribed rectangle of the plot in the forward direction, then turn around and make the next path line segment by the first slope K1 from the intersection point.
  • the first slope K1 is different from the second slope K2.
  • the effective meaning can be determined according to design needs.
  • FIG. 6 Another embodiment of the present invention, as shown in Figure 6, a path planning device for raking operations, compared with the embodiment shown in Figure 1, the difference lies in:
  • the first planning module 300 is used to make a U-turn from the intersection point to make the next path segment according to the second slope K2 when a path line segment with a slope of the first slope K1 intersects with a side of the circumscribed rectangle of the plot in the advancing direction ;
  • a path line segment with a slope of the second slope K2 has an intersection point with a side of the circumscribed rectangle of the plot in the advancing direction, then turn around and make the next path line segment by the first slope K1 from the intersection point;
  • a valid next path line segment is obtained, and the intersection point is located on the sliding side of the circumscribed rectangle, slide the preset distance along the sliding side, and then make the next path line segment according to the second slope or the first slope.
  • the first slope K1 is different from the second slope K2.
  • the path planning according to steps S320 and S321 cannot obtain the first planned path that meets the requirements of diagonal raking. For example, when the intersection point with the circumscribed rectangle is a vertex of a rectangle, effective The next path segment of , leading to the end of path planning.
  • the embodiment of the path planning device for harrowing operation provided by the present invention and the aforementioned embodiment of the path planning method for harrowing operation are based on the same inventive concept and can achieve the same technical effect. Therefore, for other details of the embodiment of the path planning device for raking operations, reference may be made to the content of the embodiments of the foregoing path planning method for raking operations.
  • the present invention also provides an embodiment of a specific application scenario, in which the above-mentioned path planning method and device for harrowing operations are applied to path planning for harrowing operations.
  • This embodiment requires an agricultural machine equipped with an automatic driving system for the agricultural machine.
  • the agricultural machinery can be four-wheeled agricultural machinery or crawler-type agricultural machinery, and the turning radius of the agricultural machinery is R.
  • the agricultural machine can hang the raking implements, and the width of the raking implements is width.
  • the automatic driving system for agricultural machinery includes a GNSS antenna 60, a GNSS receiver 50, a display terminal 10 for human-computer interaction, a control motor 30 for controlling the steering mechanism, and a control motor 30 for feedback A contact or non-contact angle sensor 40 for the front wheel angle.
  • the display terminal 10 incorporates a controller 20 .
  • the GNSS receiver 50 can be built in the display terminal 10 or externally installed; FIG. 8 is an externally installed situation.
  • the GNSS antenna is installed on the roof of the agricultural machinery and connected to the GNSS receiver through a cable to receive satellite signals and transmit the signals to the GNSS receiver.
  • the GNSS receiver is connected to the display terminal through a cable, and can receive signals from multiple satellite systems, perform high-precision positioning or orientation, and transmit positioning and orientation information to the display terminal at the same time.
  • the control motor 30 is installed on the steering shaft of the agricultural machinery, or installed in the power steering mechanism of the agricultural machinery in the form of an electromagnetic hydraulic valve, and is connected to the controller of the display terminal through a cable to receive control signals and control the steering system of the agricultural machinery .
  • the display terminal is installed in the cab of the agricultural machinery and is used for functions such as human-computer interaction, automatic driving algorithm operation, path planning, etc., and controls the steering wheel motor or hydraulic valve (ie, the control motor 30 ) through the controller 20 .
  • the contact angle sensor is installed on the front wheel shaft, or the non-contact angle sensor is installed on the chassis of the agricultural machine, connected to the controller 20 through a cable, used to calculate the front wheel angle value and feed it back to the controller 20 for closed-loop control.
  • the path planning scheme of harrowing operation is realized according to the following steps:
  • Step 1 Manually drive the agricultural machine around the land that needs to be harrowed and drive along the border of the land to collect the border data of the land. After the data collection is completed, the coordinates of a series of points are obtained, which are used as basic plot data for subsequent steps.
  • the plot boundary graphics are shown in Figure 9.
  • Step 2 Process the obtained coordinate points of the plot boundary. First remove the repeated coordinate points, calculate the convex hull of the remaining coordinate points, obtain the minimum circumscribed polygon of these points (that is, the smallest convex polygon containing the boundary data of the plot), and calculate the minimum circumscribed polygon of the minimum circumscribed polygon rectangle.
  • the minimum circumscribed rectangle is shown in Figure 10.
  • Step 3 Set the sliding edge and two slopes.
  • the four vertices of the rectangle are A, B, C, and D, where the distance between AB and CD is equal, AB and CD are the short side, and the length is W; the distance between AD and BC is equal, and AD and BC are the long side , of length H.
  • E Take the midpoint of the long side BC as E, make a line segment with A as the starting point and E as the end point, and record the slope as K1.
  • K Take E as the starting point and D as the end point to make a line segment, and record the slope as K2.
  • Set CD as the sliding edge
  • AB, BC, AD as the non-sliding edge.
  • the definition of the sliding edge is: when a path line segment intersects with the sliding edge, slide the next path point to the DC direction for a certain distance d, and the slope of the next path line segment is K2.
  • the angle between the path line segment and the sliding edge that has an intersection point with the sliding edge is recorded as ⁇ , and the calculation method of the distance d is: width is the width of the farm tool.
  • the spacing between the parallel path line segments will be greater than the width of the farm tool. In this way, the spacing of the raking land is too large, resulting in some plots being missed and not being raked. If the distance d is less than Then the distance between the parallel path line segments will be smaller than the width of the agricultural implement. In this way, if the distance between the raking fields is too small, some plots will be repeatedly raked, and the whole raking time will be long, and the agricultural machinery will consume more oil. So it is preferable to set the distance d equal to Then the distance between the parallel path line segments will be equal to the width of the farm tool, so that there will be no plot omission, and the harrowing time will be controlled.
  • non-sliding edge when a path line segment intersects with the non-sliding edge, if the slope of the line segment is K1, then the slope of the next path line segment must be K2. If the slope of this line segment is K2, then the slope of the next path segment needs to be K1.
  • Step 4 According to the requirements of diagonal raking operations, the operation path is mainly composed of the above-mentioned line segments with two slopes of K1 and K2, and the path planning is carried out with A as the starting point.
  • the planning method is as follows: starting from A, the slope is A straight line of K1, which intersects BC at a point PP2, then the second path point is PP2. Make a straight line with slope K2 through PP2, and this straight line intersects CD at a point PP3, then the third path point is PP3. Since CD is a sliding edge, according to the above setting, slide PP3 along the DC direction for a distance d to obtain the fourth path point PP4.
  • Step 5 Cut the path line according to the plot outline (i.e., the plot boundary), retract the path line beyond the plot boundary to the plot boundary, delete the path line that has no intersection with the plot outline, and obtain the clipping
  • the final set of path points is denoted as P ⁇ x, y ⁇ , that is, the second planned path is obtained, as shown in Fig. 13 .
  • Step 6 Traverse the set of path points P ⁇ x,y ⁇ , take any point P x except the first and last point, the previous path point of this point is P x-1 , and the next path point The point is P x+1 , and the azimuth from P x-1 to P x is Record the azimuth angle from P x to P x+1 as According to P x-1 , P x+1 ,
  • the turning radius R of agricultural machinery uses the dubins curve algorithm to calculate that the agricultural machinery walks from P x-1 to P x+1 , and at the same time the vehicle body is oriented from turn to
  • the set of ideal path points denoted as M ⁇ x, y ⁇ , removes P x from the set P ⁇ x, y ⁇ , and inserts the set M ⁇ x, y ⁇ into P of the set P ⁇ x, y ⁇ Between x-1 and P x+1 .
  • the dubins curve is the shortest path connecting two two-dimensional planes under the condition of satisfying the curvature constraint and the tangent direction of the specified start point and end point, and restricts the target to only move forward. Since the dubins curve is used to replace the original planned path, the harrowing path is not only the third planned path that meets the requirements of agricultural machinery driving, but also the shortest path that meets the requirements of harrowing operations.
  • Step 7 The agricultural machinery automatic driving system controls the agricultural machinery to operate according to the third planned path, and then the automatic raking operation can be realized.
  • a method for planning a raking path is proposed.
  • This method can plan a raking path for land plots of any terrain, which greatly enriches the scene of the automatic driving system of agricultural machinery for raking operations, and can It meets the requirements of most users for harrowing operations.
  • the method fully considers the user's requirements for energy saving and consumption reduction during the path planning process, and the planned path is the shortest path that meets the requirements of the land harrowing operation, which is more energy-saving and environment-friendly.
  • the data method is used for path planning, there are no uncertain factors and nonlinear factors, so this method can complete the path planning of large-area plots in a very short time.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Guiding Agricultural Machines (AREA)
  • Soil Working Implements (AREA)

Abstract

一种耙地作业的路径规划方法及装置，耙地作业的路径规划方法包括：获取地块的边界数据（S100）；根据地块的边界数据得到地块的外接矩形（S200）；按照预设规则在外接矩形内规划耙地作业路径，得到第一规划路径（S300）；按照地块边界对第一规划路径进行裁剪，得到第二规划路径（S400）；根据第二规划路径，得到符合农机行驶要求的第三规划路径（S500）。耙地作业的路径规划方法可以用于不规则地块的耙地路径规划，扩展了农机自动驾驶系统用于耙地的场景，从而提高了耙地效率；通过对不规则地块提供对角耙地的路径规划，提高了耙地效果。

Description

一种耙地作业的路径规划方法及装置 技术领域
本发明涉及路径规划技术领域，尤指一种耙地作业的路径规划方法及装置。
背景技术
农机自动驾驶系统是一种以传统农机的转向机构作为被控对象，根据GNSS(Global Navigation Satellite System，全球导航卫星系统)进行精确的位置计算，以系统中规划好的行驶路径作为目标路径进行行走方向控制的系统。农机自动驾驶系统可以极大的解放生产力，提高工作效率，为农事作业提供便利。
耙地是用耙(一种农具)进行的一种表土耕作，通常在犁耕后、播种前或早春保墒时进行。通过耙地可以把耕地经过犁地后产生的大土块机械破碎，耙地有疏松土壤、保蓄水分、提高土温等作用。
耙地有直线耙地、对角耙地等。直线耙地较为简单，按照犁沟的方向进行耙地作业即可，到达地边界掉头。对角耙地较为复杂，需要把耕地内每个区域经过两次交叉的路径对角耙过，这样有助于将犁沟填平和大土块破碎的更小。
无论是直线耙地或对角耙地，目前多是对规则地块进行耙地的路径规划，如长方形、正方形地块，无法对不规则地块，如不规则四边形、多边形等进行路径规划。然而在实际的农业场景中，仅有少部分规则地块，绝大多数地块都是不规则图形，所以当前的耙地路径规划方法极大地限制了农机自动驾驶系统用于耙地的场景，无法满足用户对耙地作业的要求。
发明内容
本发明的目的之一是提供一种耙地作业的路径规划方法及装置，用于解决目前路径规划方法只能对规则地块进行规划的问题。
本发明提供的技术方案如下：
一种耙地作业的路径规划方法，包括：获取地块的边界数据；根据所述地块的边界数据得到所述地块的外接矩形；按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。
可选地，所述的根据所述地块的边界数据得到所述地块的外接矩形包括：对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。
可选地，所述按照预设规则在所述外接矩形内规划耙地作业路径包括：当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率作下一路径线段；当一斜率为第二斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；所述第一斜率与所述第二斜率不同。
可选地，设置所述地块的外接矩形的一条边为滑动边，其余边为非滑动边；
所述按照预设规则在所述外接矩形内规划耙地作业路径还包括：当在一交点处无法得到有效的下一路径线段，且该交点位于所述外接矩形的滑动边时，则沿所述滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。
可选地，所述的按照所述地块边界对所述第一规划路径进行裁剪，得到第二规划路径，包括：将所述第一规划路径中超出地块边界的路径线缩回到所述地块边界上，将与所述地块边界无交点的路径线删除，得到第二规划路径。
可选地，所述的根据所述第二规划路径，得到符合农机行驶要求的第三规 划路径，包括：所述第二规划路径为点集；按照所述第二规划路径初始化第三规划路径；从所述第二规划路径中选取除第一个点和最后一个点外的一个待处理点；从所述第三规划路径中获取与所述待处理点对应的点，将其前一个点作为起点，后一个点作为终点；计算从所述起点到所述终点的符合农机行驶要求的曲线点集；用去除所述起点和所述终点的曲线点集取代所述第三规划路径中与所述待处理点对应的点；选择所述第二规划路径中下一个待处理点，从所述第三规划路径中获取与所述下一个待处理点对应的点，重复上述过程，直至第二规划路径中除第一个点和最后一个点外的所有点都得到处理。
可选地，计算从所述起点到所述终点的符合农机行驶要求的曲线点集，包括：根据所述第三规划路径中与所述待处理点对应的点、所述起点、所述终点，得到起点方向和终点方向；根据农机转向半径、起点位置、终点位置、所述起点方向和所述终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。
本发明还提供一种耙地作业的路径规划装置，包括：
边界获取模块，用于获取地块的边界数据；矩形生成模块，用于根据所述地块的边界数据得到所述地块的外接矩形；第一规划模块，用于按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；第二规划模块，用于按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；第三规划模块，用于根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。
可选地，所述矩形生成模块，还用于对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。
可选地，所述第一规划模块，还用于当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率作下一路径线段；当一斜率为第二斜率的路径线段在前进方向与所述地块 的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；所述第一斜率与所述第二斜率不同。
通过本发明提供的一种耙地作业的路径规划方法及装置，至少能够带来以下有益效果：本发明通过通过对地块的外接矩形进行路径规划，再对规划出来的路径进行裁剪，再对裁剪后的路径按农机行驶要求进行优化，从而实现不规则地块的路径规划，扩展了农机自动驾驶系统的应用场景，从而提高了耙地效率；通过对不规则地块提供对角耙地的路径规划，提高了耙地效果。
附图说明
下面将以明确易懂的方式，结合附图说明优选实施方式，对一种耙地作业的路径规划方法及装置的上述特性、技术特征、优点及其实现方式予以进一步说明。
图1是本发明的一种耙地作业的路径规划方法的一个实施例的流程图；
图2是对图1中第一规划路径进行裁剪的一种示意图；
图3是本发明的一种耙地作业的路径规划方法的另一个实施例的流程图；
图4是在外接矩形内规划的耙地作业路径的一种示意图；
图5是本发明的一种耙地作业的路径规划方法的另一个实施例的流程图；
图6是在外接矩形内规划的耙地作业路径的另一种示意图；
图7是本发明的一种耙地作业的路径规划装置的一个实施例的结构示意图；
图8是本发明一个具体应用实施例中农机自动驾驶系统的结构示意图；
图9是本发明一个具体应用实施例中一种地块边界图形的示意图；
图10是本发明一个具体应用实施例中一种地块的最小外接矩形的示意图；
图11是本发明一个具体应用实施例中在最小外接矩形内进行路径规划的操作示意图；
图12是本发明一个具体应用实施例中第一规划路径的示意图；
图13是本发明一个具体应用实施例中第二规划路径的示意图；
图14是本发明一个具体应用实施例中第三规划路径的示意图。
具体实施方式
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。
为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘制了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。
本发明的一个实施例，如图1所示，一种耙地作业的路径规划方法，包括：
步骤S100获取地块的边界数据；
步骤S200根据地块的边界数据得到地块的外接矩形；
步骤S300按照预设规则在外接矩形内规划耙地作业路径，得到第一规划路径；
步骤S400按照地块边界对第一规划路径进行裁剪，得到第二规划路径；
步骤S500根据第二规划路径，得到符合农机行驶要求的第三规划路径。
具体地，地块的边界数据是一数据点集，其反映了地块的边界。可以由人工驾驶农机沿着地块边界行驶一圈，以采集地块的边界数据。
地块的形状可能是不规则的，所以根据地块的边界数据得到的地块边界图形也可能是不规则图形。
计算包含地块边界图形的外接矩形。比如，在一XOY坐标系中，找到地块边界图形在X轴方向的最小坐标和最大坐标，Y轴方向的最小坐标和最大坐标，根据这4个坐标值就可确定该地块边界图形的外接矩形的四个顶点，从而确定外接矩形。
上面仅是一种简单外接矩形的计算方法，并不是面积最小的外接矩形。为了得到面积最小的矩形，可采用下面方法：
对地块的边界数据进行凸包计算，得到包含该边界数据中所有点的最小的凸多边形；计算该凸多边形的最小外接矩形，将该最小外接矩形作为地块的外接矩形。
可采用Graham扫描法或Jarvis步进法计算地块的边界数据的凸包，得到最小的凸多边形。
可采用以下方法计算凸多边形的最小外接矩形：计算凸多边形的中心；按预设角度逐步旋转凸多边形(比如在0-90°范围旋转，间距设为1°)，计算每次旋转后的凸多边形的简单外接矩形；从所有外接矩形中选择面积最小的外接矩形，将该外接矩形朝相反的方向旋转相同度数，得到凸多边形的最小外接矩形。
外接矩形/最小外接矩形所包含区域是一规则地块，可采用对规则地块的路径规划技术对外接矩形/最小外接矩形区域进行耙地作业路径规划。比如，直线耙地路径规划、对角耙地路径规划。通过直线耙地路径规划得到若干平行线段，通过对角耙地路径规划得到若干交叉线段。
在得到第一规划路径后，按照地块边界对第一规划路径进行裁剪，得到第二规划路径。具体包括：将第一规划路径中超出地块边界的路径线缩回到地块边界上，将与地块边界无交点的路径线删除，得到第二规划路径。
比如，如图2所示，假设虚线部分为地块边界，第一规划路径中有AB、BC两条线段，这两条线段与地块边界交于A1、A2、C1、C点。按照地块边界对第 一规划路径进行裁剪，则A1B线段被缩回到A1点,BC1线段被缩回到C1点，AA2线段被缩回到A2点。若第一规划路径是用按序排列的点集来表达，比如{A,B,C}，则得到对应的第二规划路径为{A2,A1,C1,C}，其也是按序排列。若第一规划路径是用按序排列的线段集来表达，比如{AB,BC},则得到对应的第二规划路径为{A2A1,A1C1,C1C}，其也是按序排列。
根据第二规划路径，得到符合农机行驶要求的第三规划路径。假设第二规划路径为按序排列的点集，步骤S500可以进一步细化为：
步骤S510按照第二规划路径初始化第三规划路径。
第三规划路径中的点与第二规划路径中的点一一对应；比如，第三规划路径的点集等于第二规划路径的点集。
步骤S520从第二规划路径中选取除第一个点和最后一个点外的一个待处理点P x；
步骤S530从第三规划路径中获取与该待处理点P x对应的点，将其前一个点作为起点，其后一个点作为终点；
步骤S540计算从起点到终点的符合农机行驶要求的曲线点集；
步骤S550用去除起点和终点的曲线点集取代第三规划路径中与该待处理点P x对应的点；
步骤S560判断第二规划路径中除第一个点和最后一个点外的所有点是否都得到处理；若否，则选择第二规划路径中下一个待处理点，跳转到步骤S530。若是，则结束。
步骤S540又可以进一步包括：
根据第三规划路径中与待处理点P x对应的点、起点、终点，得到起点方向和终点方向；根据农机转向半径、起点位置、终点位置、起点方向和终点方向，得到符合农机行驶要求的曲线点集。
农机行驶要求包括农机转向时符合农机转向半径要求。起点方向为从起点 至第三规划路径中与待处理点P x对应的点的方向；终点方向为从第三规划路径中与待处理点P x对应的点至终点的方向。
可以根据农机转向半径、起点位置、终点位置、起点方向和终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。这样可以得到从起点到终点的最短路径，有利于农机按规划路径耙地时缩短路程，降低农机的能耗。
如图2所示，假设第二规划路径为{A2,A1,C1,C}。初始化第三规划路径为{A2,A1,C1,C}。排除第一个点A2和最后一个点C有两个待处理点A1、C1。首先选取A1点，A1的前一个点A2为起点，A1的后一个点C1为终点，根据农机转向半径，采用dubins曲线算法得到从A2到C1的曲线点集M，比如{A2,A11,A12,A13,C1}；从M中去除起点和终点，得到集合M′{A11,A12,A13}，用集合M′取代第三规划路径中的A1点，得到更新的第三规划路径{A2,A11,A12,A13,C1,C}。
接着处理第二规划路径的下一个待处理点C1。在更新的第三规划路径中，C1的前一个点A13为起点，C1的后一个点C为终点，得到从A13到C的符合农机行驶要求的曲线点集M，比如{A13,C11,C12，C}；去除起点和终点，得到集合M′{C11,C12}，用集合M′取代第三规划路径中的C1点，得到更新的第三规划路径
{A2,A11,A12,A13,C11,C12，C}。
第二规划路径中需要处理的点都已处理完毕，所以处理结束，得到符合农机行驶要求的第三规划路径为{A2,A11,A12,A13,C11,C12，C}。
假设第二规划路径为线段集，可以从第二规划路径中取出任意一条折线段，将折线段转换成符合农机行驶要求的曲线段；折线段由两条相邻的线段构成。根据所有曲线段得到第三规划路径。
如图2所示，假设第二规划路径为{A2A1,A1C1,C1C}。A2A1、A1C1构成一条折线段A2C1，A2、C1为该折线段的起点和终点，根据农机转向半径，采用dubins曲线算法得到对应的曲线段；类似的，A1C1、C1C构成另一条折线段A1C，得到 A1C折线段对应的曲线段；将这两条曲线段组合得到第三规划路径。第三规划路径为满足农机行驶要求的路径。
本实施例，通过对地块的外接矩形进行路径规划，再对规划出来的路径进行裁剪，再对裁剪后的路径按农机行驶要求进行优化，从而实现不规则地块的路径规划，扩展了农机自动驾驶系统用于耙地的场景，从而提高了耙地效率。
本发明的另一个实施例，如图3所示，一种耙地作业的路径规划方法，与图1所示实施例相比，区别在于步骤S300包括：
步骤S310当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；
步骤S311当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段。
第一斜率K1与第二斜率K2不同。
具体地，可以选择地块的外接矩形上的任意一点为起点，按照第一斜率K1的直线向前移动，当该直线遇到外接矩形的边时，则调头从交点处作第二斜率K2的路径线段，将其作为下一路径线段；若第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；如此循环，直至无法得到有效的下一路径线段。有效的含义可以根据设计需要确定，比如，若下一路径线段与相邻的平行线段之间的间距小于农具幅宽，则该下一路径线段为无效；若下一路径线段完全在外接矩形外，则该下一路径线段为无效。
如图4所示，假设虚线框代表地块的外接矩形ABCD，以顶点D点为起点进行外接矩形内的路径规划，按照上述规则，按序得到斜率K1、K2交叉出现的一系列路径线段，这些路径线段构成了第一规划路径。可以看出，该规划路径符合对角耙地要求，农机按此规划路径耙地可以提高耙地效果。
本发明的另一个实施例，如图5所示，一种耙地作业的路径规划方法，与图1所示实施例相比，区别在于步骤S300包括：
设置地块的外接矩形的一条边为滑动边，其余边为非滑动边。
步骤S320当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；
步骤S321当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；
步骤S322当在一交点处无法得到有效的下一路径线段，且该交点位于外接矩形的滑动边时，则沿滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。
具体地，第一斜率K1与第二斜率K2不同。
在有些场景下，根据步骤S320、步骤S321进行路径规划，不能得到符合对角耙地要求的第一规划路径，比如，当与外接矩形的交点为一矩形的顶点时，由于顶点处无法得到有效的下一路径线段，导致路径规划结束。
如图6所示，虚线框代表地块的外接矩形ABCD，以顶点D点为起点进行外接矩形内的路径规划，按照步骤S320、步骤S321进行路径规划，当交点为A点时，由于A点无法得到有效的下一路径线段，路径规划结束。此时得到的路径不符合对角耙地要求。
为了解决这个问题，引入滑动边。若交点在滑动边上可以通过滑动预设距离，尝试得到有效的下一路径线段。若滑动后能得到一有效的下一路径线段，则继续规划；若滑动后仍无法得到有效的下一路径线段，则规划结束；或无法滑动，则规划结束。所谓无法滑动是指在滑动过程中遇到已有的交点。
如图6所示，假设AB为滑动边，当交点为A点时，在滑动边上滑动预设距离到E点，再在E点处按斜率K2或斜率K1(未示出)作下一路径线段，若该路径线段有效，则继续路径规划，如此得到图6所示符合对角耙地要求的第 一规划路径。
建议从与起点相对的两条外接矩形边中，选择不与第一条路径线段相交的边作为滑动边。
本发明的一个实施例，如图6所示，一种耙地作业的路径规划装置，包括：
边界获取模块100，用于获取地块的边界数据；
矩形生成模块200，用于根据地块的边界数据得到地块的外接矩形；
第一规划模块300，用于按照预设规则在外接矩形内规划耙地作业路径，得到第一规划路径；
第二规划模块400，用于按照地块边界对第一规划路径进行裁剪，得到第二规划路径；
第三规划模块500，用于根据第二规划路径，得到符合农机行驶要求的第三规划路径。
具体地，可以由人工驾驶农机沿着地块边界行驶一圈，以获得地块的边界数据。
地块的形状可能是不规则的，所以根据地块的边界数据得到的地块边界图形也可能是不规则图形。
通过矩形生成模块计算包含地块边界图形的外接矩形；或，对地块的边界数据进行凸包计算，得到包含该边界数据中所有点的最小的凸多边形，再计算该凸多边形的最小外接矩形，将最小外接矩形作为地块的外接矩形。
外接矩形/最小外接矩形所包含区域是一规则地块，可采用对规则地块的路径规划技术对外接矩形/最小外接矩形区域进行耙地作业路径规划。
在得到第一规划路径后，按照地块边界对第一规划路径进行裁剪，得到第二规划路径。第二规划模块400包括：将第一规划路径中超出地块边界的路径线缩回到地块边界上，将与地块边界无交点的路径线删除，得到第二规划路径。
根据第二规划路径，得到符合农机行驶要求的第三规划路径。假设第二规划路径为按序排列的点集，第三规划模块500可进一步用于：
按照第二规划路径初始化第三规划路径；从第二规划路径中选取除第一个点和最后一个点外的一个待处理点P x；从第三规划路径中获取与该待处理点P x对应的点，将其前一个点作为起点，其后一个点作为终点；计算从起点到终点的符合农机行驶要求的曲线点集；用去除起点和终点的曲线点集取代第三规划路径中与该待处理点P x对应的点；选择第二规划路径中下一个待处理点，从第三规划路径中获取与下一个待处理点对应的点，重复上述过程，直至第二规划路径中除第一个点和最后一个点外的所有点都得到处理。
第三规划模块500还可进一步用于：根据第三规划路径中与待处理点P x对应的点、起点、终点，得到起点方向和终点方向；根据农机转向半径、起点位置、终点位置、起点方向和终点方向，得到符合农机行驶要求的曲线点集。
可以根据农机转向半径、起点位置、终点位置、起点方向和终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。这样可以得到从折线段的起点到终点的最短路径，有利于农机按规划路径耙地时缩短路程，降低农机的能耗。
本实施例，通过对地块的外接矩形进行路径规划，再对规划出来的路径进行裁剪，再对裁剪后的路径按农机行驶要求进行优化，从而实现不规则地块的路径规划，扩展了农机自动驾驶系统的应用场景，从而提高了耙地效率。
本发明的另一个实施例，如图6所示，一种耙地作业的路径规划装置，与图1所示实施例相比，区别在于：
第一规划模块300，用于当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段。
具体地，第一斜率K1与第二斜率K2不同。可以选择地块的外接矩形上的任意一点为起点，按照第一斜率K1的直线向前移动，当该直线遇到外接矩形的边时，则调头从交点处作第二斜率K2的路径线段，将其作为下一路径线段；若第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；如此循环，直至无法得到有效的下一路径线段。有效的含义可以根据设计需要确定。
本发明的另一个实施例，如图6所示，一种耙地作业的路径规划装置，与图1所示实施例相比，区别在于：
设置地块的外接矩形的一条边为滑动边，其余边为非滑动边。
第一规划模块300，用于当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；当在一交点处无法得到有效的下一路径线段，且该交点位于外接矩形的滑动边时，则沿滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。
具体地，第一斜率K1与第二斜率K2不同。
在有些场景下，根据步骤S320、步骤S321进行路径规划，不能得到符合对角耙地要求的第一规划路径，比如，当与外接矩形的交点为一矩形的顶点时，由于顶点处无法得到有效的下一路径线段，导致路径规划结束。
为了解决这个问题，引入滑动边。若交点在滑动边上可以通过滑动预设距离，尝试得到有效的下一路径线段。若滑动后能得到一有效的下一路径线段，则继续规划；若滑动后仍无法得到有效的下一路径线段，则规划结束；或无法滑动，则规划结束。所谓无法滑动是指在滑动过程中遇到已有的交点。
需要说明的是，本发明提供的耙地作业的路径规划装置的实施例与前述提供的耙地作业的路径规划方法的实施例均基于同一发明构思，能够取得相同的 技术效果。因而，耙地作业的路径规划装置的实施例的其它具体内容可以参照前述耙地作业的路径规划方法的实施例内容的记载。
本发明还提供一种具体应用场景实施例，将前述的耙地作业的路径规划方法及装置应用于耙地作业的路径规划中。
本实施例要求一台安装有农机自动驾驶系统的农机。农机可以是四轮农机也可以是履带式农机，记该农机的转弯半径为R。农机可以悬挂耙地农具，记耙地农具的幅宽为width。
农机自动驾驶系统，如图8所示，包括GNSS天线60、一个GNSS接收机50、一个用于人机交互的显示终端10、一个用于对转向机构进行控制的控制电机30、一个用于反馈前轮转角的接触式或非接触式角度传感器40。显示终端10内置控制器20。GNSS接收机50可以内置在显示终端10中，也可以外置；图8是一种外置情况。
GNSS天线安装在农机车顶，通过线缆与GNSS接收机连接，用于接收卫星信号并且将信号传输到GNSS接收机中。
GNSS接收机通过线缆与显示终端连接，可以接收多个卫星系统的信号，并且进行高精度定位或定向，同时将定位定向信息传输到显示终端内。
控制电机30安装在农机转向轴上，或以电磁液压阀形式安装在农机的转向助力机构内，同时通过线缆连接到显示终端的控制器上，用于接收控制信号并对农机转向系统进行控制。
显示终端安装在农机驾驶室内，用于人机交互、自动驾驶算法运行、路径规划等功能，通过控制器20对方向盘电机或液压阀(即控制电机30)进行控制。
接触式角度传感器安装在前轮转轴上，或非接触式角度传感器安装在农机底盘上，通过线缆与控制器20连接，用于计算前轮转角值并反馈给控制器20 进行闭环控制。
耙地作业的路径规划方案按如下步骤实现：
第一步：人工驾驶农机在需要进行耙地作业的地块周围沿着地块边界行驶一圈，用于采集地块的边界数据。该数据采集完成后，得到一系列点的坐标，作为基本的地块数据用于后续步骤。地块边界图形如图9所示。
第二步：对获得的地块边界的坐标点进行处理。首先去除重复的坐标点，对剩下的坐标点进行凸包计算，获取这些点的最小外接多边形(即包含地块的边界数据的最小的凸多边形)，并计算得到该最小外接多边形的最小外接矩形。最小外接矩形如图10所示。
第三步：设置滑动边和两种斜率。
记矩形四个顶点为A、B、C、D，其中AB与CD之间的距离相等，AB与CD为短边，长度为W；AD与BC之间的距离相等，AD与BC为长边，长度为H。取长边BC的中点记为E，以A为起点，E为终点做线段，斜率记为K1。以E为起点,D为终点做线段，斜率记为K2。将CD设置为滑动边，AB、BC、AD为非滑动边。
滑动边的定义为：当某一路径线段与滑动边有交点时，则将下一个路径点向DC方向滑动一定的距离d，且下一路径线段的斜率为K2。记与滑动边有交点的路径线段与滑动边的夹角记为δ，该距离d的计算方式为：
Figure PCTCN2022114023-appb-000001
width为农具幅宽。
若距离d大于
Figure PCTCN2022114023-appb-000002
则平行路径线段之间的间距将大于农具幅宽，这样，耙地的间距过大，导致有一些地块被遗漏，未被耙到。若距离d小于
Figure PCTCN2022114023-appb-000003
则平行路径线段之间的间距将小于农具幅宽，这样，耙地的间距过小，一些地块被多次耙到，整个耙地时间长，农机比较费油。所以优选将距离d设为等于
Figure PCTCN2022114023-appb-000004
则平行路径线段之间的间距将等于农具幅宽，这样不会出现地块遗漏，又控制了耙地时长。
非滑动边的定义为：当某一路径线段与非滑动边有交点时，如果该线段的斜率为K1，则下一路径线段的斜率需为K2。如果该线段的斜率为K2，则下一路径线段的斜率需为K1。
第四步：根据对角耙地作业的要求，作业路径主要由上述斜率为K1和K2两种斜率的线段构成，以A为起点进行路径规划，规划方法如下：以A为起点，做斜率为K1的直线，该直线与BC交于一点PP2，则第二个路径点为PP2。过PP2做斜率为K2的直线，该直线与CD交于一点PP3，则第三个路径点为PP3。由于CD为滑动边，根据上述设定，将PP3沿DC方向滑动距离d，得到第四个路径点PP4。过PP4做斜率为K2的直线，该直线与BC交于一点PP5，则第五个路径点为PP5，如图11。依上述描述执行下去，直到当某一路径线段与滑动边有交点且该交点到的距离小于滑动距离d时，规划任务停止，得到路径点的集合，即得到最小外接矩形内的第一规划路径，如图12所示。
第五步：根据地块轮廓(即地块边界)对路径线进行裁剪，将超出地块边界的路径线缩回到地块边界上，将与地块轮廓无交点的路径线删除，得到裁剪后的路径点的集合，记为P {x,y}，即得到第二规划路径，如图13所示。
第六步：遍历路径点的集合P {x,y}，取其中除第一个和最后一个点外的任意一个点P x，该点的前一个路径点为P x-1，后一个路径点为P x+1，记P x-1到P x的方位角为
Figure PCTCN2022114023-appb-000005
记P x到P x+1的方位角为
Figure PCTCN2022114023-appb-000006
根据P x-1、
Figure PCTCN2022114023-appb-000007
P x+1、
Figure PCTCN2022114023-appb-000008
农机转向半径R使用dubins曲线算法，计算得到农机从P x-1行走到P x+1，同时车身朝向从
Figure PCTCN2022114023-appb-000009
转向到
Figure PCTCN2022114023-appb-000010
的理想路径点集合，记为M {x,y}，将P x从集合P {x,y}中移除，同时将集合M {x,y}插入到集合P {x,y}的P x-1与P x+1之间。对集合P {x,y}中除第一个和最后一个点外的每一个点执行以上步骤，最终得到集合P′ {x,y}，记为规划完成的耙地路径， 为方便用户寻找入口，将集合中的第一个点反向延长20米，如图14所示。
dubins曲线是在满足曲率约束和规定的起点和终点的切线方向的条件下，连接两个二维平面的最短路径，而且限制目标只能向前行进。由于采用dubins曲线替换原始规划路径，所以该耙地路径不仅是满足农机行驶要求的第三规划路径，还为满足耙地作业要求的最短路径。
第七步：农机自动驾驶系统根据第三规划路径控制农机进行作业，即可实现自动耙地作业。
本实施例，提出一种用于耙地路径的规划方法，该方法可以对任意地形的地块进行耙地路径的规划，极大的丰富了农机自动驾驶系统用于耙地作业的场景，可以满足绝大多数用户对耙地作业的要求。本方法在进行路径规划的过程中充分考虑用户对节能降耗的要求，规划出来的路径为满足耙地作业要求的最短路径，更加节能环保。同时由于使用数据方法进行路径规划，不存在不确定性因素与非线性因素，所以本方法能在极短的时间内完成大面积地块的路径规划。
应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

  1. 一种耙地作业的路径规划方法，其特征在于，包括：
    获取地块的边界数据；
    根据所述地块的边界数据得到所述地块的外接矩形；
    按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；
    按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；
    根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。
  2. 根据权利要求1所述的路径规划方法，其特征在于，所述的根据所述地块的边界数据得到所述地块的外接矩形包括：
    对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；
    计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。
  3. 根据权利要求1、2任一项所述的路径规划方法，其特征在于，所述按照预设规则在所述外接矩形内规划耙地作业路径包括：
    当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率作下一路径线段；
    当一斜率为第二斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；
    所述第一斜率与所述第二斜率不同。
  4. 根据权利要求3所述的路径规划方法，其特征在于：
    设置所述地块的外接矩形的一条边为滑动边，其余边为非滑动边；
    所述按照预设规则在所述外接矩形内规划耙地作业路径还包括：
    当在一交点处无法得到有效的下一路径线段，且该交点位于所述外接矩形的滑动边时，则沿所述滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。
  5. 根据权利要求1所述的路径规划方法，其特征在于，所述的按照所述地块边界对所述第一规划路径进行裁剪，得到第二规划路径，包括：
    将所述第一规划路径中超出地块边界的路径线缩回到所述地块边界上，将与所述地块边界无交点的路径线删除，得到第二规划路径。
  6. 根据权利要求1所述的路径规划方法，其特征在于，所述的根据所述第二规划路径，得到符合农机行驶要求的第三规划路径，包括：
    所述第二规划路径为点集；
    按照所述第二规划路径初始化第三规划路径；
    从所述第二规划路径中选取除第一个点和最后一个点外的一个待处理点；
    从所述第三规划路径中获取与所述待处理点对应的点，将其前一个点作为起点，后一个点作为终点；
    计算从所述起点到所述终点的符合农机行驶要求的曲线点集；
    用去除所述起点和所述终点的曲线点集取代所述第三规划路径中与所述待处理点对应的点；
    选择所述第二规划路径中下一个待处理点，从所述第三规划路径中获取与所述下一个待处理点对应的点，重复上述过程，直至所述第二规划路径中除第一个点和最后一个点外的所有点都得到处理。
  7. 根据权利要求6所述的路径规划方法，其特征在于，计算从所述起点到所述终点的符合农机行驶要求的曲线点集，包括：
    根据所述第三规划路径中与所述待处理点对应的点、所述起点、所述终点，得到起点方向和终点方向；
    根据农机转向半径、起点位置、终点位置、所述起点方向和所述终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。
  8. 一种耙地作业的路径规划装置，其特征在于，包括：
    边界获取模块，用于获取地块的边界数据；
    矩形生成模块，用于根据所述地块的边界数据得到所述地块的外接矩形；
    第一规划模块，用于按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；
    第二规划模块，用于按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；
    第三规划模块，用于根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。
  9. 根据权利要求8所述的路径规划装置，其特征在于：
    所述矩形生成模块，还用于对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。
  10. 根据权利要求8所述的路径规划装置，其特征在于：
    所述第一规划模块，还用于当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率 作下一路径线段；当一斜率为第二斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；所述第一斜率与所述第二斜率不同。
PCT/CN2022/114023 2021-08-23 2022-08-22 一种耙地作业的路径规划方法及装置 WO2023025105A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110967426.4 2021-08-23
CN202110967426.4A CN113804212B (zh) 2021-08-23 2021-08-23 一种耙地作业的路径规划方法及装置

Publications (1)

Publication Number Publication Date
WO2023025105A1 true WO2023025105A1 (zh) 2023-03-02

Family

ID=78893854

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/114023 WO2023025105A1 (zh) 2021-08-23 2022-08-22 一种耙地作业的路径规划方法及装置

Country Status (2)

Country Link
CN (1) CN113804212B (zh)
WO (1) WO2023025105A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117308965A (zh) * 2023-11-28 2023-12-29 华南农业大学 基于滑动窗口算法的收获机器人自主卸粮路径规划方法

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113804212B (zh) * 2021-08-23 2022-07-26 上海联适导航技术股份有限公司 一种耙地作业的路径规划方法及装置
CN114459487B (zh) * 2022-02-25 2024-02-09 上海联适导航技术股份有限公司 一种对角耙地的路径规划方法、装置、设备及存储介质
CN115617030A (zh) * 2022-04-29 2023-01-17 丰疆智能软件科技(南京)有限公司 对角耙地路径规划方法、电子设备和计算机可读存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101592957A (zh) * 2009-06-23 2009-12-02 北京市农林科学院 一种分布式农机作业辅助直线跟踪控制系统
CN104757909A (zh) * 2015-01-22 2015-07-08 深圳市银星智能科技股份有限公司 一种清洁机器人的清洁模式
US20170144702A1 (en) * 2015-11-19 2017-05-25 Agjunction Llc K-turn path controller
CN109238298A (zh) * 2018-09-26 2019-01-18 上海联适导航技术有限公司 一种无人驾驶带避障的路径规划方法
CN112033404A (zh) * 2020-07-31 2020-12-04 重庆智行者信息科技有限公司 一种无人清扫车多连通区域全覆盖清扫路径规划方法
CN112306050A (zh) * 2019-07-15 2021-02-02 苏州宝时得电动工具有限公司 自主机器人及其行走路径规划方法、装置和存储介质
CN112686424A (zh) * 2020-12-03 2021-04-20 广州极飞科技有限公司 平地路径范围确定方法、平地路径规划方法及相关装置
CN113804212A (zh) * 2021-08-23 2021-12-17 上海联适导航技术股份有限公司 一种耙地作业的路径规划方法及装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102079890B1 (ko) * 2016-05-10 2020-02-20 얀마 가부시키가이샤 자율 주행 경로 생성 시스템
US10143126B2 (en) * 2016-06-10 2018-12-04 Cnh Industrial America Llc Planning and control of autonomous agricultural operations
CN107340768B (zh) * 2016-12-29 2020-08-28 珠海市一微半导体有限公司 一种智能机器人的路径规划方法
WO2020100810A1 (ja) * 2018-11-15 2020-05-22 株式会社クボタ 収穫機および経路設定システム
CN109634276B (zh) * 2018-12-13 2021-05-25 中联重科股份有限公司 农用车辆无人驾驶控制方法、系统及该农用车辆

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101592957A (zh) * 2009-06-23 2009-12-02 北京市农林科学院 一种分布式农机作业辅助直线跟踪控制系统
CN104757909A (zh) * 2015-01-22 2015-07-08 深圳市银星智能科技股份有限公司 一种清洁机器人的清洁模式
US20170144702A1 (en) * 2015-11-19 2017-05-25 Agjunction Llc K-turn path controller
CN109238298A (zh) * 2018-09-26 2019-01-18 上海联适导航技术有限公司 一种无人驾驶带避障的路径规划方法
CN112306050A (zh) * 2019-07-15 2021-02-02 苏州宝时得电动工具有限公司 自主机器人及其行走路径规划方法、装置和存储介质
CN112033404A (zh) * 2020-07-31 2020-12-04 重庆智行者信息科技有限公司 一种无人清扫车多连通区域全覆盖清扫路径规划方法
CN112686424A (zh) * 2020-12-03 2021-04-20 广州极飞科技有限公司 平地路径范围确定方法、平地路径规划方法及相关装置
CN113804212A (zh) * 2021-08-23 2021-12-17 上海联适导航技术股份有限公司 一种耙地作业的路径规划方法及装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117308965A (zh) * 2023-11-28 2023-12-29 华南农业大学 基于滑动窗口算法的收获机器人自主卸粮路径规划方法
CN117308965B (zh) * 2023-11-28 2024-03-12 华南农业大学 基于滑动窗口算法的收获机器人自主卸粮路径规划方法

Also Published As

Publication number Publication date
CN113804212A (zh) 2021-12-17
CN113804212B (zh) 2022-07-26

Similar Documents

Publication Publication Date Title
WO2023025105A1 (zh) 一种耙地作业的路径规划方法及装置
US11044842B2 (en) Path planning for area coverage
US10209714B2 (en) Line acquisition path generation
EP3518647A1 (en) A system and a method for determining a trajectory to be followed by an agricultural work vehicle
US11814074B2 (en) Headland-following turns
US20060178820A1 (en) System and method for guiding an agricultural vehicle through a recorded template of guide paths
Eaton et al. Autonomous farming: Modelling and control of agricultural machinery in a unified framework
US20060178823A1 (en) System and method for propagating agricultural vehicle guidance paths that have varying curvature along their length
CN104781633A (zh) 农田导航系统和农田导航方法及其软件和软件的存储设备
US20090071667A1 (en) Method and system for optimising land levelling designs
JP6318805B2 (ja) 圃場形状決定装置
CN104322173B (zh) 山地丘陵区缓坡地改造方法
Bakker An autonomous robot for weed control: design, navigation and control
Hameed A coverage planner for multi-robot systems in agriculture
JP2017131199A (ja) 植林システム、植林方法、耕作機械、植付機械及び管理装置
AU2007240239B2 (en) Method and system for shaping furrows in soil
JP4909661B2 (ja) 等高線に基づいて作成された農作業用作業線に沿って走行位置を明示する表示装置及び農作業方法
Jing et al. Path tracking control with slip compensation of a global navigation satellite system based tractor-scraper land levelling system
DE102013202075A1 (de) Bewegungsstrategieerarbeitungs- und/oder Navigationsvorrichtung
CN107646211A (zh) 一种基于分形理论的仿生多尺度锯齿结构
CN113313784B (zh) 基于无人驾驶农机的农田画制作方法及装置
EP3518649A1 (en) A method for determining placement of new obstacles in an agricultural field
CN107864711B (zh) 耙地方法、控制装置及耙地系统
EP3903555A1 (en) Method and system for path planning for headland-following turns
US20180264954A1 (en) Autonomous electric agricultural vehicles

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22860451

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE