Method and system for path deformation to avoid obstacles

Abstract

A method and system for path deformation to avoid obstacles includes the steps of determining an obstacle is located along an initial spline trajectory and determining an elementary section of the initial spline trajectory on which the obstacle is located. One or more of a plurality of control points of the elementary section based on the obstacle to generate a modified spline trajectory and a machine is controlled based on the modified spline trajectory.

Description

METHOD AND SYSTEM FOR PATH DEFORMATION TO AVOID OBSTACLESBACKGROUND

[0001] Automation of machinery can improve efficiency since machines can be controlled to perform operations that only experienced operators could otherwise perform. The use of automated machinery can include a number of applications such as the agricultural operations of sowing, spraying, fertilizing, mowing, and harvesting. These applications require determining a path that results in coverage of a desired area. Paths can be generated that facilitate travel of a machine over an area so that the coverage of the area is appropriate with respect to the agricultural operation. However, path generation requires obstacle avoidance which is not easy to account for and often requires significant processing power. What is needed is a method for path deformation to avoid obstacles that is quick and efficient.FIELD OF THE INVENTION

[0002] The present disclosure relates generally to machine control, and more particularly to path deformation to avoid obstacles.SUMMARY

[0003] A method for deforming an initial spline trajectory based on an obstacle located along the initial spline trajectory includes the step of determining that an obstacle is located along an initial spline trajectory. An elementary section of the initial spline trajectory on which the obstacle is located is then determined. One or more of a plurality of control points of the elementary section are modified based on the obstacle to generate a modified spline trajectory. A machine is then controlled based on the modified spline trajectory. In one embodiment, the initial spline trajectory is generated to cover an area over which the machine will travel. In one embodiment, a Euclidian norm of shifts vector is used to minimize skipped areas of the area over which the machine will travel. The initial spline trajectory can be generated using homogeneous cubic B-splines. In one embodiment, the elementary section is generated using four control points using equations described herein. In one embodiment, one or more of the plurality of control points is modified based on a maximum steering angle of the machine. In one embodiment, one of the one or more of the plurality of control points is modified based on how close the one of the one or more of the plurality of control points is located to the obstacle.

[0004] An apparatus having memory storing computer program instructions and a computer readable medium storing instructions for deforming an initial spline trajectory based on an obstacle located along the spline trajectory are also described herein.BRIEF DESCRIPTION OF THE DRAWINGS

[0005] FIG. 1 shows an agricultural machine traversing a field;

[0006] FIG. 2 shows a path comprising a plurality of swaths located in a coordinate system;

[0007] FIG. 3 shows the result of path deformation in a coordinate system according to one embodiment;

[0008] FIG. 4 shows routes for two different vehicles according to one embodiment;

[0009] FIG. 5 show a flow chart of a method for deforming an initial spline trajectory based on an obstacle located along the initial spline trajectory according to one embodiment; and

[0010] FIG. 6 shows a high-level schematic of a computer for implementing methods and systems described herein.DETAILED DESCRIPTION

[0011] A method and system for path deformation to avoid obstacles is described herein. The method and system use a path that was planned for a machine to traverse an area without taking obstacles into account and modify that planned path so that obstacles are accounted for and avoided.

[0012] A path for agricultural operations of a machine in an area (e.g., a field) is typically planned so that every portion of the field is covered by a portion of the path. For example, when treating an area with a liquid agricultural material (e.g., fertilizer), a path for the machine dispensing the liquid is planned so that every portion of the field receives a desired amount of liquid. Typically, such a path is generated to cover every portion of the field so that the machine travels in one direction from one end of the field to the other and then turns (e.g., 180 degrees) and travels to the opposite end of the field where another turn (e.g., 180 degree) is performed and the process repeats. The portion of the path travelling from one end of a field to an opposite end is referred to as a swath. The radius of a turn a machine must make at the end of one swath in order to begin travelling along an adjacent swath depends on the turn radius the machine is capable of and the amount of overlap that is desired, if any, due to the width of the implement performing the operation (e.g., a liquid sprayer, harvester, etc.).

[0013] In one embodiment, the path is a spline trajectory comprising a plurality of control points (i.e., locations along the spline trajectory / path). In one embodiment, an initial splinetrajectory (i.e., an initial path) is determined without taking into account obstacles in the area. This initial spline trajectory allows for analysis of the entire area without omissions (e.g., omissions to account for obstacles). The initial spline trajectory is then deformed using methods and techniques described herein to account for obstacles to generate a modified spline trajectory (i.e., a modified path). In one embodiment, the initial spline trajectory is modified to account for obstacles by changing the control points of the initial spline trajectory. The new locations of control points are selected in order to avoid obstacles while maintaining a minimum area of gaps in area (e.g., field) coverage. In one embodiment, the new control point locations are determined by solving a convex optimization problem which allows for minimization of the sum of control point variations, which reduces the area of unprocessed samples to a minimum.

[0014] FIG. 1 shows agricultural machine 100 traversing field 102 in alternating directions (e.g. swaths) shown by arrows 108. Machine 100 is shown in FIG. 1 having traversed path portion 104 comprising approximately two and one-half swaths and having not yet travelled along path portion 106 comprising approximately five and one-half swaths. Machine 100 can be performing one or more processing functions such as spraying, harvesting, sowing, or mowing as it traverses field 102. In one embodiment, path portions 104 and 106 are part of a planned path for ensuring coverage of the field. In one embodiment, machine 100 has a front wheel steering mechanism with a constrained maximum steering angle (e.g., maximum rotation angle of steering wheels). Obstacle 110 is taken into account in part of the path planning stage by deforming swaths (i.e., deforming the planned path). A method for modifying a planned path of machine 100 is described herein to produce a realizable path that avoids obstacles.

[0015] There are many methods for generating a path that covers an area (e.g., a field) using a plurality of substantially parallel paths. For example, see “Coverage path planning algorithms for agricultural field machines,” by Oksanen, T., and Visala, A. in Journal of field robotics, 2009, no. 8, pp. 651-668; “Side-to-side 3d coverage path planning approach for agricultural robots to minimize skip / overlap areas between swaths” by Hameed, I. A., la Cour-Harbo, A., and Osen, O. L. in Robotics and Autonomous Systems, 2016, vol. 76, pp. 36-45; “Coverage path planning on three-dimensional terrain for arable farming” by Jin, J., and Tang, L. in Journal of field robotics, 2011, no. 3, pp. 424-440.

[0016] Generating a path covering a field with a plurality of substantially parallel swaths requires taking into account operational limitations of the machine that will traverse the path. For example, such paths are often traversed by agricultural vehicles that have steerable frontwheels. The steerable front wheels have a limited angle of rotation. This limited angle of rotation is referred to as restriction u . Restriction u at each control point of a trajectory can be written as , where is a minimum radius of curvature that can be realizedby the vehicle.

[0017] In one embodiment, an initial spline trajectory (i.e., an initial path) is generated using homogeneous cubic B-splines. The initial spline trajectory is represented by a set of elementary sections, each of which is constructed from four control points.

[0018] Each elementary B-spline is constructed using four control pointsin the following way:where t is a spline parameter, . Thus, the spline trajectory is given by a set ofcontrol points , where each four consecutive points defines an elementary segmentof the trajectory. The set of control points of the trajectory is supplemented with additional points , for example: . In one embodiment, the first andsecond derivatives of the path are continuous. To approximate the 3D path, uniform cubic B- splines are first constructed in the X-Y plane, and then projected onto the surface using a function. The projection can be carried out by elevation interpolation, for example, by bilinear interpolation, kriging, or spline interpolation.

[0019] A local Cartesian coordinate system ENU (East, North and Up) is used with reference to a certain point of the field. In one embodiment, the x -axes and the y -axes are directed East and North respectively and the z -axis coordinate shows the height relative to the reference point. FIG. 2 shows coordinate system 200 having a first axis 201 designated to represent north in meters (N, m) and a second axis 203 perpendicular to the first axis designated to represent East in meters (E, m). Field 205 is shown located in working area 202 and enclosed by field border 206. A plurality of swaths 204 are shown arranged to cover entire field 205 located within field border 206. Swaths 204 are shown prior to deformation for obstacle avoidance.

[0020] The swath of a field which intersects with an obstacle is denoted byare used to denote equidistant control points of the swath, • For each vector rtthe variables are used to represent thevector as shown by the equation .

[0021] dtis defined as the length of a shift vector for spline control point . Thedirection of the shift vector is defined as follows. The unit normal vector is defined as tospline curve in the (x,y) plane at the point rt. The component isdefined by the rulewhere z(x, y) is interpolation function for the elevation of point (x, y) .The shift direction vector (with unit length) is defined as

[0022] In one embodiment, dj, where, is set to be a variable. A control point , where of the neighboring swath on the vector Ntis selected such that. To minimize skipped areas, the Euclidean norm of shifts vector can be used. Restrictions on the choice of dtare determined by thegeometry of the field and the conditions for the feasibility of the trajectory.

[0023] For the feasibility of the trajectory normal curvature condition,must be met for an entire B-spline. First the two-dimensional field in the plane (x,y) is considered. In this case the length of a trajectory curvature vector matches a trajectory normal curvature. At the pointit can be calculated by the formulawhere is the acute (or right) angle between the vectors of the first spline derivativeand the second spline derivative . For an elementary cubic B-spline themaximum value of , is reached at t = 0 or at t = 1 . The value of isan almost constant function on an elementary spline.

[0024] Considering the case of t = 0 and equally spaced control points . For this point

[0025] Since the vectors and form a rhombus, it follows that

[0026] Defining as

[0027] The following is obtained

[0028] Then is lower bound for . The inequalitiesfor provide sufficient conditions for .

[0029] In the 3D case, the constraint on the curvature of the trajectory in the tangent plane needs to be taken into account. (0) is denoted by . As an approximation of thetangent plane at the point the plane with vectors is chosen. In the targetplane, the sufficient conditions can be formulated aswhere

[0030] For the solution of path deformation, the following optimization problem is solved.with restriction for allwhere are constants defined by the geometry of the area (e.g., field). In oneembodiment, the problem must be convex, so the direction (left or right) for traversing each obstacle must be chosen. In one embodiment, the direction is chosen based on the sum of the displacements in the absence of a curvature constraint and is the vectorof weights. This allows for deflection of the trajectory more strongly near obstacles and almost unchanged away from obstacles. For certain configurations of obstacles, the optimization problem can be infeasible due to the fact that the uniformity of the location of control points is violated. In this case, the problem is solved for a larger value , theresulting path redefined with an almost uniform set of control points, and then the problem for the original is solved.

[0031] Coordinate system 300 of FIG. 3 shows the result of the path deformation. To account for obstacles 302, swaths 304 are deformed like a string. However, near the obstacle, the deviations of the control points are minimized, which reduces the area of untreated areas. The conditions of the minimum radius of curvature for the case of incompatibility of theoptimization problem have been weakened with a step of 9 cm. Weights have been defined as 0.1 for points in the obstacles and 1 for points outside the obstacles. Coordinate system 400 of FIG. 4 shows one variant of the routes for two different wheeled machines that are processing an entire field. A first wheeled machine traverses route 402 (referred to as Route 1 in the label of FIG. 4) and a second wheeled machine traverses route 404 (referred to as Route 2 in the label of FIG. 4).

[0032] FIG. 5 shows method 500 for deforming an initial spline trajectory based on an obstacle located along the spline trajectory. At step 502, an obstacle is determined to be located along an initial spline trajectory (i.e., an initial path). At step 504 an elementary section of the initial spline trajectory on which the obstacle is located is determined. At step 506, one or more of a plurality of control points of the elementary section are modified based on the obstacle to generate a modified spline trajectory. At step 508, a machine is controlled based on the modified spline trajectory.

[0033] In one embodiment, the initial spline trajectory is generated to cover an area over which a machine will travel. In one embodiment, a Euclidian norm of shifts vector is used to minimize skipped areas of the area over which the machine will travel. The initial spline trajectory is generated using homogeneous cubic B-splines according to one embodiment. In one embodiment, each elementary section is generated using four control points using the equationswhere t is a spline parameter,.

[0034] In one embodiment, the modifying one or more of the plurality of control points is based on a maximum steering angle of the machine. In one embodiment, modifying one of the one or more of the plurality of control points is based on how close the one of the one or more of the plurality of control points is located to the obstacle.

[0035] In one embodiment, the method shown in FIG. 5 is performed on a computer that can be located on an agricultural machine or remote from one or more agricultural machines. A high-level block diagram of such a computer is illustrated in FIG. 6. Computer602 contains a processor 604 which controls the overall operation of the computer 602 by executing computer program instructions which define such operation. The computer program instructions may be stored in a storage device 612, or other computer readable medium (e.g., magnetic disk, CD ROM, etc.), and loaded into memory 610 when execution of the computer program instructions is desired. Thus, the method steps of FIG. 5, as well as other methods and algorithms described herein, can be defined by the computer program instructions stored in the memory 610 and / or storage 612 and controlled by the processor 604 executing the computer program instructions. For example, the computer program instructions can be implemented as computer executable code programmed by one skilled in the art to perform an algorithm defined by the method steps of FIG. 5, as well as other methods and algorithms described herein. Accordingly, by executing the computer program instructions, the processor 604 executes an algorithm defined by the method steps of FIG. 5 or other methods and algorithms described herein. The computer 602 also includes one or more network interfaces 606 for communicating with other devices via a network. The computer 602 also includes input / output devices 608 that enable user interaction with the computer 602 (e.g., display, keyboard, mouse, speakers, buttons, etc.) One skilled in the art will recognize that an implementation of an actual computer could contain other components as well, and that FIG. 6 is a high-level representation of some of the components of such a computer for illustrative purposes.

[0036] The foregoing Detailed Description is to be understood as being in every respect illustrative and exemplary, but not restrictive, and the scope of the inventive concept disclosed herein is not to be determined from the Detailed Description, but rather from the claims as interpreted according to the full breadth permitted by the patent laws. It is to be understood that the embodiments shown and described herein are only illustrative of the principles of the inventive concept and that various modifications may be implemented by those skilled in the art without departing from the scope and spirit of the inventive concept. Those skilled in the art could implement various other feature combinations without departing from the scope and spirit of the inventive concept.

Claims

CLAIMS:

1. A method for deforming an initial spline trajectory based on an obstacle located along the initial spline trajectory, the method comprising: determining an obstacle is located along an initial spline trajectory; determining an elementary section of the initial spline trajectory on which the obstacle is located; modifying one or more of a plurality of control points of the elementary section based on the obstacle to generate a modified spline trajectory; controlling a machine based on the modified spline trajectory.

2. The method of claim 1, wherein the initial spline trajectory is generated to cover an area over which the machine will travel.

3. The method of claim 2, wherein a Euclidian norm of shifts vector is used to minimize skipped areas of the area over which the machine will travel.

4. The method of claim 1, wherein the initial spline trajectory is generated using homogeneous cubic B-splines.

5. The method of claim 1, wherein the elementary section is generated using four control pointsusing the equationwhere, andt is a spline parameter,6. The method of claim 1, wherein the modifying one or more of the plurality of control points is based on a maximum steering angle of the machine.

7. The method of claim 1, wherein the modifying one of the one or more of the plurality of control points is based on how close the one of the one or more of the plurality of control points is located to the obstacle.

8. An apparatus comprising: a processor; and a memory to store computer program instructions, the computer program instructions for deforming an initial spline trajectory based on an obstacle located along the initial spline trajectory, which, when executed on the processor cause the processor to perform operations comprising: determining an obstacle is located along an initial spline trajectory; determining an elementary section of the initial spline trajectory on which the obstacle is located; modifying one or more of a plurality of control points of the elementary section based on the obstacle to generate a modified spline trajectory; controlling a machine based on the modified spline trajectory.

9. The apparatus of claim 8, wherein the initial spline trajectory is generated to cover an area over which the machine will travel.

10. The apparatus of claim 9, wherein a Euclidian norm of shifts vector is used to minimize skipped areas of the area over which the machine will travel.

11. The apparatus of claim 8, wherein the initial spline trajectory is generated using homogeneous cubic B-splines.

12. The apparatus of claim 8, wherein the elementary section is generated using four control points using the equationwhereand t is a spline parameter,13. The apparatus of claim 8, wherein the modifying one or more of the plurality of control points is based on a maximum steering angle of the machine.

14. The apparatus of claim 8, wherein the modifying one of the one or more of the plurality of control points is based on how close the one of the one or more of the plurality of control points is located to the obstacle.

15. A computer readable medium storing computer program instructions for deforming an initial spline trajectory based on an obstacle located along the initial spline trajectory, which, when executed on a processor, cause the processor to perform operations comprising: determining an obstacle is located along an initial spline trajectory; determining an elementary section of the initial spline trajectory on which the obstacle is located; modifying one or more of a plurality of control points of the elementary section based on the obstacle to generate a modified spline trajectory; controlling a machine based on the modified spline trajectory.

16. The computer readable medium of claim 15, wherein the initial spline trajectory is generated to cover an area over which the machine will travel.

17. The computer readable medium of claim 16, wherein a Euclidian norm of shifts vector is used to minimize skipped areas of the area over which the machine will travel.

18. The computer readable medium of claim 15, wherein the initial spline trajectory is generated using homogeneous cubic B-splines.

19. The computer readable medium of claim 15, wherein the elementary section is generated using four control points using the equationwhere, andt is a spline parameter,.

20. The computer readable medium of claim 15, wherein the modifying one or more of the plurality of control points is based on a maximum steering angle of the machine.

Images