Operation planning device, operation planning method, and program

Abstract

Provided is an operation planning method including acquiring terrain information, creating a work machine operation plan for creating a route between a low land and a high land in a terrain indicated by the terrain information, and outputting data indicating the created operation plan, wherein: a target plane targeted to be created after performing terrain modification in the terrain indicated by the terrain information is determined; a position at which excavation of a deposit is started, a depth to be excavated, and a position at which the excavated deposit is unloaded are determined on the basis of the target plane; and the operation plan is created by compiling the position at which the deposit is excavated, the depth to be excavated, and the position at which the excavated deposit is unloaded that have been determined.

Description

Motion planning device, motion planning method, and program

[0001] This application claims priority to U.S. Provisional Patent Application No. 63 / 689,770, filed on September 2, 2024, the contents of which are incorporated herein by reference.

[0002] When carrying out recovery work after a landslide disaster, first create a temporary road using a backhoe or other terrain-altering machine to allow passage for the worker and following vehicles, and then move to the work site and transport supplies. Therefore, it is necessary to plan the route and terrain alteration in advance.

[0003] Several studies have been conducted on the issue of traversing uneven terrain. For example, Non-Patent Document 1 discloses a technology for traversing uneven terrain by controlling a mechanism according to the terrain, such as using a sub-crawler to climb steps. Non-Patent Document 2 discloses a technology for modifying uneven terrain by filling holes with urethane to create slopes. Non-Patent Document 3 discloses a technology for planning a path taking into account distance, gradient, and required energy.

[0004] Kazunori Ohno, Shouich Morimura, Satoshi Tadokoro, Eiji Koyanagi, and Tomoaki Yoshida. Semi-autonomous control system of rescue crawler robot having flippers for getting over unknown-steps. In 2007 IEEE / RSJ International Conference on Intelligent Robots and Systems, pp. 3012-3018, 2007.Ryusuke Fujisawa, Naohisa Nagaya, Shinya Okazaki, Ryota Sato, Yusuke Ikemoto, and Shigeto Dobata. Active modification of the environment by a robot with construction abilities. ROBOMECH Jornal, 2015.Alexandre S. Santos, H´ector Ignacio Perez Azp´urua, Gustavo Pessin, and Gustavo M. Freitas. Path planning for mobile robots on rough terrain. In 2018 Latin American Robotic Symposium, 2018 Brazilian Symposium on Robotics (SBR) and 2018 Workshop on Robotics in Education (WRE), pp. 265-270, 2018.Dmitri A. Dolgov, Sebastian Thrun, Michael Montemerlo, and James Diebel. Path planning for autonomous vehicles in unknown semi-structured environments. The International Journal of Robotics Research, Vol. 29, pp.485-501, 2010.

[0005] However, the terrain that has been altered and created may not be suitable for vehicles to travel on.

[0006] In view of the above circumstances, an object of the present invention is to provide a technique that can create a motion plan for creating smooth terrain.

[0007] One aspect of the present invention is a motion planning method that acquires terrain information, creates a motion plan for a work machine to create a route between lowlands and highlands on the terrain indicated by the terrain information, and outputs data indicating the created motion plan.The motion planning method determines a target plane to be created after terrain modification is performed on the terrain indicated by the terrain information, determines a position to start excavating the deposit, the excavation depth, and a position to unload the excavated deposit based on the target plane, and creates the motion plan by compiling the determined position to excavate the deposit, the excavation depth, and the position to unload the excavated deposit.

[0008] One aspect of the present invention is a motion planning device comprising: an information acquisition unit that acquires terrain information; a motion plan creation unit that creates a motion plan for a work machine to create a path between lowlands and highlands on the terrain indicated by the terrain information; and a motion plan output unit that outputs data indicating the created motion plan, wherein the motion plan creation unit comprises: a target plane determination unit that determines a target plane to be created after terrain modification is performed on the terrain indicated by the terrain information; an excavation position determination unit that determines a position to start excavating the deposit and the excavation depth based on the target plane; and an unloading position determination unit that determines a position at which to unload the excavated deposit, and the motion plan is created by compiling the determined position at which to excavate the deposit, the excavation depth, and the position at which to unload the excavated deposit.

[0009] The present invention allows for the creation of a motion plan for creating smooth terrain.

[0010] FIG. 1 is a diagram illustrating a motion planning device 1 according to an embodiment of the present invention; FIG. 2 is an example of a terrain formed by two rectangular parallelepiped deposits; FIG. 3 is a diagram illustrating a configuration of the motion planning device 1 according to an embodiment of the present invention; and FIG. 4 is a flowchart illustrating the operation of the motion planning device 1 according to an embodiment of the present invention. above and the volume of the space between the plane and the deposit, V below 1 is a diagram showing an example of a method for determining a target plane; dig , y-coordinate y dig The lowest point z bottom The distance between the excavation point Q and the target plane P is the length l of the bucket. bucket 1 is a diagram showing the deposits excavated when the distance between the excavation point Q and the target plane P is equal to or greater than the bucket length l. bucket Fig. 10 is a diagram showing the piles excavated when the length of the excavation path is shorter than 100 m / s, Fig. 11 is a flowchart showing a method for determining the excavation position, excavation volume, and unloading position, and Fig. 12 is a diagram showing the experimental results.

[0011] Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings. FIG. 1 is a diagram showing a motion planning apparatus 1 according to this embodiment. Topography information is input to the motion planning apparatus 1. The topography information is data indicating the locations of deposits such as clay, sand, soil, and gravel in three dimensions. The locations of the deposits are represented, for example, by two-dimensional coordinates and the height of the deposits corresponding to the two-dimensional coordinates. The topography information can be obtained, for example, by measuring the topography to be modified using LiDAR (Light Detection and Ranging). In this case, the topography information is point cloud data.

[0012] For simplicity of explanation, the terrain indicated by the terrain information is assumed to be a terrain formed by two rectangular parallelepiped deposits. The two rectangular parallelepiped deposits have portions of their sides in contact with each other, their top surfaces are parallel, and their heights are different. Figure 2 shows an example of a terrain formed by two rectangular parallelepiped deposits. Hereinafter, the top surface of the lower deposit will be referred to as the "lowland," and the top surface of the higher deposit will be referred to as the "highland." Points and planes on the terrain will be described using a three-dimensional coordinate system in which the x-y plane is parallel to the lowland and the highland. For example, a plane is expressed by Equation (1).

[0013] By arbitrarily setting a, b, c, and d, it is possible to set an arbitrary plane in the three-dimensional coordinate system. Note that the angle θ between the plane expressed by equation (1) and the xy plane can be expressed by equation (2).

[0014] The motion planning device 1 creates a motion plan for the backhoe V1 to create a route between low ground and high ground in the terrain indicated by the terrain information. For example, initial position information and destination information for the backhoe V1 are input to the motion planning device 1. The initial position of the backhoe V1 is an arbitrary position in low ground in the terrain indicated by the terrain information, and the destination is an arbitrary position in high ground in the terrain indicated by the terrain information. The motion planning device 1 is equipped with, for example, a display device, and displays the created motion plan. The backhoe V1 is an example of a work machine, and may be a work machine that is capable of excavating and loading / unloading, which will be described later.

[0015] 3 is a diagram showing the configuration of the motion planning apparatus 1 according to this embodiment. The motion planning apparatus 1 includes an information acquisition unit 11, a motion plan creation unit 12, a motion plan output unit 13, and a storage unit 19.

[0016] The information acquisition unit 11 acquires topographical information, initial position information, and destination information. Note that these pieces of information may not be information input from an external device, but may be generated by internal processing. For example, a terrain simulator may be installed in the motion planning device 1, and the topographical information, initial position information, and destination information may be generated by using the terrain simulator. The information acquisition unit 11 records the topographical information, initial position information, and destination information in the storage unit 19.

[0017] The motion plan creation unit 12 creates a motion plan for the backhoe V1 to create a route between the initial position and the destination on the terrain indicated by the terrain information.

[0018] The motion plan output unit 13 outputs data indicating the motion plan of the backhoe V1 created by the motion plan creation unit 12.

[0019] The following describes the operation plan of the backhoe. The operation of the backhoe includes excavating and unloading the deposits. The terrain changes as the backhoe excavates and unloads the deposits. The backhoe V1 moves at an angle θ traverse It can move freely on a plane with the following inclination angle: Angle θ traverse can be set arbitrarily depending on the type of backhoe V1. The traveling direction of the backhoe V1 can be set freely within the range of the plane on which the backhoe V1 can move. It is desirable that the traveling direction of the backhoe V1 be set based on the movement cost in each direction from the position of the backhoe V1 and a heuristic cost according to the distance from the position of the backhoe V1 to the destination. In the following explanation, the vector indicating the traveling direction of the backhoe V1 is defined as (s, t, u).

[0020] The motion plan creation unit 12 determines the position where the backhoe V1 will excavate the deposits and the position where the excavated deposits will be unloaded. Data indicating the motion plan output by the motion plan output unit 13 includes data on the position where the backhoe V1 will excavate the deposits, the position where the excavated deposits will be unloaded, and the position of the backhoe V1. A person modifying the terrain can modify the actual terrain and create a path between lowlands and highlands by moving the backhoe V1 on the actual terrain, excavating, and unloading based on the data output by the motion plan output unit 13.

[0021] 4 is a flowchart showing the operation of the motion planning device 1 according to this embodiment. The information acquisition unit 11 acquires terrain information (step S11). The motion plan creation unit 12 creates a motion plan for the backhoe V1 to create a route between lowlands and highlands in the terrain indicated by the terrain information (step S12). The motion plan output unit 13 outputs data indicating the motion plan for the backhoe V1 created by the motion plan creation unit 12 (step S13).

[0022] A method for determining the position where the backhoe V1 excavates the deposits will be described in detail below. The motion plan creation unit 12 includes a target plane determination unit 121, an excavation position determination unit 122, a loading / unloading position determination unit 123, a terrain modification unit 124, a recording unit 125, and a determination unit 126. The target plane determination unit 121 first determines a target plane P. The target plane P is a plane that is targeted to be created after the terrain modification is performed. The target plane P indicates the range that the bucket can reach and excavate from the set position of the backhoe V1. Note that since the backhoe V1 can move within a predetermined range, the target plane P is determined within the range that the backhoe V1 can move and excavate. The target plane P is determined to pass through a point on the ground surface located a predetermined distance ahead of the position of the backhoe V1 so that the backhoe V1 can excavate and unload the deposits from the set position and create the target plane P.

[0023] The target plane P is determined so as to satisfy a predetermined condition. In the following description, the target plane P is expressed by equation (3).

[0024] The first condition is that the angle between the target plane P and the lowland is θ traverse The first condition can be expressed by the following equation (4).

[0025] The second condition is that the target plane P is horizontal to the traveling direction of the backhoe V1. The second condition is that the cross product of the vector (s, t, u) indicating the traveling direction of the backhoe V1 and the vector (0, 0, 1) parallel to the z-axis is on the target plane P. The second condition can be expressed by equation (5).

[0026] The third condition is the volume of the deposit above the plane, V above and the volume of the space between the plane and the underlying deposit, V below 5 shows the volume V of the deposit above the plane. above and the volume of the space between the plane and the deposit, V below FIG.

[0027] A plane that meets the first condition is one that a backhoe can cross. A plane that meets the second condition is one that is horizontal to the direction of travel of the vehicle. A plane that meets the third condition is one that can be formed by excavating the deposits above the plane and moving them into the space between the plane and the deposits, without digging or unloading from other locations.

[0028] For example, a target plane that satisfies the first, second, and third conditions can be determined by the following method. Equation (3) is an equation that expresses the target plane P using a coordinate system fixed to the terrain (hereinafter referred to as the world coordinate system and also written as Σworld), but the target plane P that satisfies the above conditions may also be determined by using a coordinate system obtained by translating Σworld. Consider a coordinate system obtained by translating the origin of Σworld to the position of the backhoe V1 (hereinafter referred to as the robot coordinate system and also written as Σrobot), and a coordinate system obtained by translating the origin of Σrobot forward a predetermined distance (hereinafter referred to as Σo).

[0029] The target plane determination unit 121 first determines a point on the ground surface that is located a predetermined distance ahead of the position of the backhoe V1 in Σo and has an angle of θ traverse and determines a plane that is horizontal to the traveling direction of the backhoe V1. Since the point on the ground surface located a predetermined distance forward from the position of the backhoe V1 in Σo is the origin, the plane that passes through the point on the ground surface located a predetermined distance forward from the position of the backhoe V1 can be expressed as equation (6).

[0030]

[0031] The angle between the plane expressed by equation (6) and the xy plane is θ traverseThe fact that the plane expressed by equation (6) is horizontal to the traveling direction of the backhoe V1 is expressed by equation (8).

[0032] a, b, and c that satisfy the formulas (6), (7), and (8) are defined as a １ , b １ , c １ Let's say.

[0033] Thereafter, the target plane determination unit 121 calculates the volume V of the deposit above the set plane. above and the volume of the space between the set plane and the deposit V below After that, the target plane determination unit 121 calculates the angle between the set plane and the xy plane as θ traverse Set a, b, and c to a so that they are smaller than １ , b １ , c １ The angle between the set plane and the xy plane is θ plane (≦θ traverse ), then a, b, and c are the θ traverse θ plane The value satisfies the three equations substituted into the equation.

[0034] The target plane determination unit 121 determines the V above and V below Change the angle between the set plane and the xy plane until it is equal to V above and V below The plane when these are equal is determined. The plane determined here is the target plane P. Since Σo is a coordinate obtained by translating Σworld, a, b, and c remain unchanged when a plane in Σworld is transformed into a plane in Σo. In other words, the target plane P determined here is expressed by equation (9).

[0035] Here, the target plane determination unit 121 determines V above and V below In other words, the third condition is to determine the plane when the difference between the volume V of the deposit above the plane is the smallest. aboveand the volume of the space between the plane and the deposit, V below The difference between the angle θ plane In deciding on above and CV below The set plane may be changed until the difference between the two is 0 or a minimum value. Here, C is the rate of change in the volume of the deposit due to excavation or compaction of the deposit. C is the value obtained by dividing the volume of the deposit when compacted by the volume of the deposit to be excavated. The volume of the deposit when compacted can also be referred to as the volume of soil when compacted, and the volume of the deposit to be excavated can also be referred to as the volume of natural soil. This C may be calculated from the rate of change in soil volume, such as the loosening rate or compaction rate, which is determined by the soil quality.

[0036] Then, the equation that represents the target plane P in Σo is transformed into an equation that represents the target plane P in Σworld. The transformation from Σo to Σrobot is a translation. The transformation from Σrobot to Σworld is a translation. This transforms d in the equation that represents the target plane P in Σworld. ０ can be determined.

[0037] In this way, the target plane determination unit 121 can determine the target plane P.

[0038] After the target plane P is determined, the excavation position determination unit 122 determines the position where the deposit will be excavated and determines the excavation depth. The unloading position determination unit 123 determines the position where the excavated deposit will be unloaded. The terrain modification unit 124 reflects the excavation and unloading of the deposit in the terrain indicated by the terrain information, and creates information indicating the modified terrain. The recording unit 125 records the determined position where the deposit will be excavated, the excavation depth, the position where the deposit will be unloaded, the position of the backhoe V1, and information indicating the modified terrain in the memory unit 19. Methods for determining the excavation position, excavation depth, and unloading position will be described later.

[0039] Thereafter, the determination unit 126 determines whether a path that the backhoe V1 can travel on is created based on the target plane P in the changed terrain. For example, the determination unit 126 determines the volume V of the pile above the target plane. above and the volume V of the space between the target plane and the deposit.below is equal to or smaller than a predetermined value, the determination unit 126 determines that a path passable by the backhoe V1 has been created based on the target plane. For example, if an index indicating the degree of unevenness of the created path is equal to or smaller than a predetermined value, the determination unit 126 determines that a path passable by the backhoe V1 has been created based on the target plane. The index indicating the degree of unevenness can be calculated, for example, by applying a method for calculating the surface roughness of industrial products.

[0040] If the determination unit 126 determines that a passable path for the backhoe V1 has not been created based on the target plane P in the changed terrain, the excavation position determination unit 122 determines a new excavation position and excavation depth based on the target plane P, the unloading position determination unit 123 determines a new unloading position for the deposit to be excavated, and the terrain modification unit 124 creates information indicating the changed terrain. In other words, until a passable path is created based on the determined target plane P, the excavation position determination unit 122 determines the excavation position and excavation depth, and the unloading position determination unit 123 determines a new unloading position for the deposit to be excavated.

[0041] Note that the target plane determination unit 121 may create a new target plane P even if a path passable by the backhoe V1 has not been created in the changed terrain based on the target plane P. For example, even if the excavation position determination unit 122 determines the excavation position and excavation depth a predetermined number of times, the unloading position determination unit 123 determines the unloading position of the excavated deposits, and the terrain modification unit 124 creates information indicating the changed terrain, if a path passable by the backhoe V1 has not been created based on the target plane P, the target plane determination unit 121 may determine a new target plane P, the excavation position determination unit 122 may determine the excavation position and excavation depth based on the newly determined target plane P, the unloading position determination unit 123 may determine the unloading position of the excavated deposits, and the terrain modification unit 124 may create information indicating the changed terrain.

[0042] When a passable route for the backhoe V1 is created based on the target plane P in the changed terrain, the target plane determination unit 121 moves the position of the backhoe V1 and determines a new target plane. At this time, it is desirable to move the position of the backhoe V1 to the newly passable route and determine a new target plane P. Furthermore, it is desirable that the newly determined target plane P share an edge with the target plane determined immediately before, in addition to the conditions described above. In this case, the newly determined target plane P makes it possible to create a route closer to higher ground.

[0043] The target plane determination unit 121 determines new target planes P until a route from a low ground to a high ground is created. In this way, a route from a low ground to a high ground is created.

[0044] 6 is a flowchart showing the operation of the motion plan creation unit 12 according to this embodiment. The target plane determination unit 121 first determines a target plane P based on the initial terrain indicated by the terrain information and the initial position of the backhoe V1 (step S21). After determining the target plane, the excavation position determination unit 122 determines the position at which to excavate the deposits (step S22). The excavation position determination unit 122 determines the depth at which to excavate the deposits (step S23). The unloading position determination unit 123 determines the unloading position (step S24). The terrain modification unit 124 creates terrain information indicating the modified terrain based on the determined excavation position, excavation depth, and unloading position, and records this in the memory unit 19 (step S25).

[0045] The recording unit 125 records the determined excavation position, excavation depth, and unloading position in the memory unit 19 (step S26). Then, the determination unit 126 determines whether a path passable by the backhoe V1 has been created based on the target plane P in the changed terrain (step S27). If a path passable by the backhoe V1 has not been created based on the target plane P in the changed terrain (step S27: NO), the operation plan creation unit 12 again executes the operations of steps S22 to S26. The excavation position determination unit 122 determines a new excavation position and excavation depth based on the determined target plane P, the unloading position determination unit 123 determines the unloading position, and the terrain modification unit 124 creates terrain information indicating the changed terrain. If a path passable by the backhoe V1 has been created based on the target plane in the changed terrain (step S27: YES), the determination unit 126 determines whether a path from low ground to high ground has been created in the changed terrain (step S28). If a route from low ground to high ground has not been created in the changed terrain (step S28: NO), the target plane determination unit 121 moves the position of the backhoe V1 and determines a new target plane (step S29). After that, the motion plan creation unit 12 executes the operations of steps S22 to S26, and the excavation position determination unit 122 determines a new excavation position and excavation depth based on the newly determined target plane P, the unloading position determination unit 123 determines an unloading position, and the terrain modification unit 124 creates terrain information indicating the changed terrain.

[0046] If a route from low ground to high ground is created in the changed terrain (step S28: YES), the recording unit 125 compiles the information on the excavation position, excavation depth, loading / unloading position, position of the backhoe V1, and changed terrain recorded in the memory unit 19, and creates a motion plan for the backhoe V1 (step S30). Data indicating the created motion plan is output by the motion plan output unit 13.

[0047] 7 is a flowchart showing an example of a method for determining a target plane. The target plane determination unit 121 determines an angle θ plane But the angle θ traverse(Step S211). The target plane determination unit 121 determines the volume V of the deposit above the set plane. above and the volume of the space between the set plane and the deposit V below (Step S212). The target plane determination unit 121 calculates the angle θ plane (step S213), and the angle θ plane The volume of the deposit above the reduced plane V above and angle θ plane The volume of the space between the plane and the deposit is V below (step S214). The target plane determination unit 121 calculates the angle θ plane By reducing the volume V above and volume V below It is determined whether the difference between the volume V and the volume V has become small (step S215). above and volume V below When the difference between the horizontal plane and the lowland becomes small (step S215: YES), the target plane determination unit 121 performs the operation shown in step S213 again to determine the angle θ plane Further reduce the volume V above and volume V below When the difference between the angle θ and the target plane becomes large (step S215: NO), the target plane determination unit 121 determines whether the angle θ plane The plane before reducing is determined as the target plane P (step S216).

[0048] The initial position of the backhoe V1 may be any position on high ground in the terrain indicated by the terrain information, and the destination may be any position on low ground in the terrain indicated by the terrain information. In other words, the motion planning device 1 may create a motion plan in which the backhoe V1 excavates the terrain from high ground to low ground, and creates a path between the low ground and the high ground. In this case, the first condition is the angle θ between the target plane P and the high ground. plane is the angle θ traverse The following is true:

[0049] The method for determining the excavation position and excavation depth will be described below. The excavation position determination unit 122 determines the excavation position and excavation depth so that the backhoe V1 can excavate as much deposits as possible.

[0050] The excavation position determination unit 122 first determines the coordinates of the excavation point Q. The excavation point Q is the point on the surface of the deposit above the target plane P that is the farthest from the target plane P. When the surface of the deposit is represented by a three-dimensional point cloud, the point included in the point cloud that is above the target plane P and the farthest from the target plane P is determined to be the excavation point Q.

[0051] Then, the coordinates of the excavation start point (x dig , y dig The excavation start point indicates the position where the backhoe V1 inserts the bucket and starts excavation. The excavation start point is determined so that a large amount of the deposits at the excavation point Q can be excavated. dig is determined by, for example, equation (10), and y dig is determined by equation (11).

[0052] In formulas (10) and (11), x Ｑ is the x-coordinate of the drilling point Q, and y Ｑ is the y coordinate of the drilling point Q. dig is the length of the excavation area, which is a value specific to the backhoe V1. θ is the angle between the vector connecting the excavation point Q and the point indicating the backhoe V1 and the vector perpendicular to the direction of travel of the backhoe V1.

[0053] FIG. 8 shows the x-coordinate of the excavation start point x dig , y-coordinate y dig The backhoe V1 moves the bucket to the excavation start point (x dig , y dig ) and start drilling, the vertical l in the xy coordinate dig , horizontal w dig A rectangular area of ​​w can be excavated. dig is the width of the area that can be excavated, and is a value specific to the backhoe V1. The excavation position is determined by the above.

[0054] The coordinates of the excavation point Q may be determined based on the volume of the deposit. For example, a plurality of points included in the surface of the deposit above the target plane may be set as excavation point candidates, and the volumes of the deposits above the target plane that are included in the vertical direction of a two-dimensional region parallel to the xy plane around each of the excavation point candidates may be compared, and the point with the largest volume of deposit may be determined as the excavation point Q. The excavation point candidate is determined, for example, by sorting a plurality of points in descending order of distance from the target plane and selecting a predetermined number of points in descending order. When the surface of the deposit is represented by a three-dimensional point cloud, a predetermined number of points that are above the target plane and that are selected in descending order of distance from the target plane are the excavation point candidates.

[0055] The two-dimensional area around each of the excavation point candidates is, for example, a vertical line 1 as shown in FIG. dig , horizontal w dig That is, the x and y coordinates of the excavation start point located in the vertical direction of the rectangular area parallel to the xy plane with the excavation point candidate at the center are calculated by using equations (10) and (11). Ｑ is the x-coordinate of the drilling point candidate, y Ｑ This can be calculated by replacing θ with the y-coordinate of the excavation point candidate and θ with the angle between the vector connecting the excavation point candidate and the point indicating the backhoe V1 and the vector perpendicular to the direction of travel of the backhoe V1.

[0056] The process of comparing the volume of deposits above the target plane in the two-dimensional area surrounding each candidate excavation point and determining the point with the largest volume of deposits as excavation point Q can be expressed by equations (12) and (13).

[0057] A dig (p ’ ｉ ) is the point p ’ ｉ is the two-dimensional region around A, and d(q, P) is the dig (p ’ ｉ) and the target plane P. What is shown by equation (12) is the sum of a rectangular parallelepiped with a base of a rectangle of widths w and l and a height of d(q, P) when the surface of the deposit is represented by a three-dimensional point cloud, which is a set of points arranged every w in the x direction and every l in the y direction. That is, the area A dig (p ’ ｉ ) is the volume of sediment at

[0058] The point p where the volume calculated by equation (12) is the largest is calculated by equation (13). ’ ｉ is determined as the drilling point Q.

[0059] In excavation by the backhoe V1, the length of the bucket from the excavation point Q bucket The deposit is excavated up to the higher of the lower height and the height of the target plane in the vertical direction of the excavation start point. bottom is expressed by equation (14).

[0060] Bucket length l from digging point Q bucket The small height and the vertex R of the rectangle as shown in FIG. １ , R ２ , R ３ , R ４ The deposits may be excavated up to the highest height of the target plane in each of the vertical directions.

[0061] FIG. 9 shows the lowest point z bottom 10 is a diagram showing the amount of sediment to be excavated.

[0062] Excavation start point and lowest point z bottom The distance between the excavation point Q and the target plane P is calculated by the bucket length l. bucket If the distance between the excavation point Q and the target plane P is shorter than the bucket length l, the height of the deposit at the excavation point Q will be lower than the target plane, so the bucket cannot be inserted deepest at the excavation point Q. bucketIf the distance is shorter than the x-coordinate of the excavation start point, the excavation start point may be set at a position away from the excavation point Q by the distance between the excavation point Q and the target plane P, so that the deposits around the excavation start point are excavated. dig is calculated by equation (15), and the y-coordinate y dig is calculated by equation (16), and the lowest point z bottom is calculated by equation (17).

[0063] In formulas (15) and (16), d Ｑ is the distance between the digging point Q and the target plane P. FIG. 10 shows that the distance between the digging point Q and the target plane P is the bucket length l bucket 11 is a diagram showing the deposits excavated when the distance between the excavation point Q and the target plane P is equal to or greater than the bucket length l. bucket FIG. 10 illustrates the deposits excavated when the distance is shorter than the distance .

[0064] The distance between the excavation point Q and the target plane P is the length l of the bucket. bucket If the distance is shorter than the distance between the excavation point Q and the target plane P, the excavation start point may be set at a position away from the excavation point Q at a distance shorter than the distance between the excavation point Q and the target plane P. In this case, the volume of the excavated deposit will be smaller, but it is possible to prevent the height of the deposit at the excavation point Q from becoming lower than the target plane.

[0065] The method for determining the unloading position will be described below. The unloading position determination unit 123 determines the point indicating the position where the excavated deposit is to be the point of the deposit below the target plane that is the farthest from the target plane. If the deposit is represented by a three-dimensional point cloud, the point included in the point cloud that is below the target plane and the farthest from the target plane is determined to be the point indicating the position where the excavated deposit is to be unloaded. To prevent the bucket of the backhoe V1 from getting stuck in the unloading position and being unable to unload the deposit, the z-coordinate of the point indicating the position where the excavated deposit is to be unloaded may include an offset value.

[0066] The unloading position determination unit 123 may determine the position where the volume of the space between the target plane P and the pile in the area where the pile excavated by the backhoe V1 is to be unloaded is large as the unloading position.

[0067] A specific calculation method will be described below. First, the unloading position determination unit 123 calculates the volume of the deposit to be unloaded. The volume of the deposit to be unloaded can be calculated by calculating the volume of the excavated deposit based on the excavation position and excavation depth. Here, the volume of the deposit to be unloaded may be calculated by multiplying the volume of the excavated deposit by C (the rate of change in the volume of the deposit due to compaction of the deposit).

[0068] Thereafter, the unloading position determination unit 123 calculates the width of the area in which the pile spreads when the pile of the calculated volume is unloaded onto the xy plane. ｘ and the width l in the y-axis direction ｙ is calculated by simulation based on the volume and properties of the deposit.

[0069] Then, width l ｘ and width l ｙ The two-dimensional area of ​​the width l is moved on the surface of the pile, and the place where the volume of the space between the two-dimensional area and the target plane P is the largest is determined as the unloading position. ｘ and width l ｙ The two-dimensional area of ​​volume V below The load-unloading position is determined by moving the load on the surface of the pile, where the load-unloading position is calculated. For example, the volume V of the space between the load-unloading position and the target plane P in the two-dimensional area is calculated. Ｇｊｋ is calculated by equation (18).

[0070] d ｉ is the distance of the space without deposits between the points included in the two-dimensional region and the target plane P. Equation (18) shows that when the surface of the deposits is represented by a three-dimensional point cloud, which is a set of points arranged every w in the x direction and every l in the y direction, the base is a rectangle with widths of w and l and the height is d ｉ , that is, the volume of the space between the target plane P in a predetermined two-dimensional area.

[0071] The two-dimensional area with the largest volume calculated by equation (18) is determined as the unloading position.

[0072] 12 is a flowchart showing a method for determining the excavation position, excavation volume, and unloading position. The excavation position determination unit 122 determines the coordinates of the excavation point Q (step S41). Then, the excavation position determination unit 122 determines the coordinates of the point where excavation starts (step S42). The excavation position determination unit 122 determines the lowest point z bottom (Step S43). The unloading position determining unit 123 determines the coordinates of the point where unloading is to be performed (Step S44).

[0073] In the determination method described above, the target plane P is determined, and V above and V below This modifies the terrain so that the value of is smaller, allowing for smoother terrain.

[0074] Furthermore, when the coordinates of excavation point Q are determined based on the volume of the deposit, more deposits can be excavated than when the excavation point Q is determined as the point on the surface of the deposit above the target plane P that is the farthest from the target plane P, because the coordinates of excavation point Q are determined by calculating the volume. Furthermore, when the unloading position is determined as the location in the unloading area where the volume of the space between the target plane P and the deposit is large, the unloading position R indicating the location where the excavated deposits are to be unloaded is determined by calculating the volume, so the excavated deposits can be unloaded at a location where more deposits can be unloaded, compared to when the unloading point R indicating the location where the excavated deposits are unloaded is determined as the point on the deposit below the target plane that is the farthest from the target plane.

[0075] The distance between the digging point Q and the target plane P is the length l of the bucket. bucket If the distance is shorter than the distance between the excavation point Q and the target plane P, the excavation start point is set at a position away from the excavation point Q by the distance between the excavation point Q and the target plane P, thereby making it possible to appropriately adjust the amount of sediment excavated when a small amount of sediment is above the target plane P.

[0076] The experiment conducted will be explained below. The terrain indicated by the topographical information was assumed to be formed by two rectangular parallelepiped deposits as shown in Figure 2, and the difference between the lowland and the highland was set to 3 meters. traverse is set to 30 degrees, and in determining the target plane P, the equation (3) θ traverse Determine the values ​​of a, b, and c by V above and V below The target plane P was determined by changing the value of d until it was equal to . In this experiment, only one target plane P was determined, and a motion plan was created to create a path based on that target plane P. The number of cuts and fills, that is, the number of times excavation and loading and unloading were performed, was set to 15.

[0077] Two methods were used to determine the excavation start point and the lowest point to be excavated. In the first method, the excavation start point was determined using equations (10) and (11), and the lowest point to be excavated was determined using equation (14). In the second method, the distance between the excavation point Q and the target plane P was determined by the bucket length l. bucket If the distance is shorter than 100 m, the excavation start point is determined by equations (15) and (16), and the lowest point to be excavated is determined by equation (17).

[0078] V is used as a value to evaluate whether a planar path can be created. above , V below , V above Left-right difference V diff_above and V below Left-right difference V diff_below The motion plan was created five times for each method, and V above , V below , V diff_above and V diff_below The average value was calculated.

[0079] FIG. 13 shows the experimental results. above , V below , V diff_above and V diff_belowIt can be seen that the distance becomes smaller after 15 cuts and fills, and that it is possible to create a route that can be traversed by modifying the terrain through motion planning. Furthermore, the second method was able to create a route that can be traversed with fewer cuts and fills than the first method.

[0080] Other Embodiments One embodiment of the present invention has been described in detail above with reference to the drawings, but the specific configuration is not limited to that described above, and various design changes and the like can be made within the scope that does not deviate from the gist of the present invention.

[0081] In the above explanation, for the sake of simplicity, a terrain formed by two rectangular parallelepiped deposits has been used, but any terrain that can be set to lowlands and highlands may be used. For example, the terrain indicated by the terrain information may be a terrain with multiple steps or a terrain with slopes at an angle that makes it impossible to traverse.

[0082] Part or all of the motion planning device 1 in the above-described embodiment may be implemented by a computer. In this case, a program for implementing this function may be recorded on a computer-readable recording medium, and the program may be loaded into a computer system and executed. Note that the term "computer system" as used herein includes an operating system (OS) and peripheral hardware. Furthermore, the term "computer-readable recording medium" refers to portable media such as flexible disks, optical magnetic disks, ROMs, and CD-ROMs, as well as storage devices such as hard disks built into computer systems. Furthermore, the term "computer-readable recording medium" may also include media that dynamically store programs for a short period of time, such as communication lines used when transmitting programs via networks such as the Internet or communication lines such as telephone lines, or media that store programs for a fixed period of time, such as volatile memory within a computer system serving as a server or client. The program may be designed to implement part of the above-described functions, or may be capable of implementing the above-described functions in combination with a program already stored in the computer system. Furthermore, part or all of the motion planning device 1 may be implemented using a programmable logic device such as an FPGA (Field Programmable Gate Array).

[0083] REFERENCE SIGNS LIST 1 Motion planning device, 11 Information acquisition unit, 12 Motion plan creation unit, 121 Target plane determination unit, 122 Excavation position determination unit, 123 Unloading position determination unit, 124 Terrain modification unit, 125 Recording unit, 126 Determination unit, 13 Motion plan output unit

Claims

1. A motion planning method comprising: acquiring topographical information; creating a motion plan for a work machine to create a route between lowlands and highlands on the topography indicated by the topographical information; and outputting data indicating the created motion plan; wherein a target plane to be created after modifying the topography indicated by the topographical information is determined; a position to start excavating the deposit, the excavation depth, and a position to unload the excavated deposit are determined based on the target plane; and the determined position to excavate the deposit, the excavation depth, and the position to unload the excavated deposit are compiled to create the motion plan.

2. A motion planning method as described in claim 1, wherein, after the location for excavating the deposit, the excavation depth, and the location for unloading the excavated deposit have been determined, terrain information is created that shows the modified terrain after the excavation and unloading of the deposit; if a new passable route is created in the modified terrain, a new target plane is determined, and the location for excavating the deposit, the excavation depth, and the location for unloading the excavated deposit are determined based on the newly determined target plane; if a new passable route is not created in the modified terrain, a new location for excavating the deposit, the excavation depth, and the location for unloading the excavated deposit are determined based on the already determined target plane.

3. A motion planning method as described in claim 1, wherein the target plane is set so that its angle with the low ground or the high ground is equal to or less than a predetermined value, is horizontal to the direction of travel of the work machine, and minimizes the difference in volume of the space between the volume of the pile on the upper side and the volume of the pile on the lower side.

4. The motion planning method described in claim 1, wherein the position at which excavation of the pile begins is determined as an excavation point from a point on the surface of the pile above the target plane so that the work machine can excavate as much of the pile as possible, and is determined based on the position of the excavation point, the length of the area that can be excavated by the work machine, the position of the excavation point and the work machine, and the direction of travel.

5. The motion planning method according to claim 4, wherein the excavation point is the point on the surface of the deposit above the target plane that is the farthest from the target plane.

6. The motion planning method of claim 4, wherein the excavation point is a point with the largest volume of deposit when comparing the volumes of deposits above the target plane in the areas surrounding each of the excavation point candidates, and the excavation point is a point with the largest volume of deposit when comparing the volumes of deposits above the target plane in the areas surrounding each of the excavation point candidates.

7. A motion planning method according to any one of claims 4 to 6, wherein the lowest point of the excavation depth is the higher of the height from the excavation point minus the length of the bucket of the work machine and the height of the target plane in the vertical direction of the position where the excavation starts.

8. The distance between the digging point Q and the target plane P is the length l of the bucket. bucket 8. The motion planning method according to claim 7, wherein if the distance between the excavation point and the target plane is shorter than the distance between the excavation point and the target plane, the position at which excavation of the deposit starts is set at a position away from the excavation point by a distance shorter than the distance between the excavation point and the target plane.

9. The motion planning method according to claim 1, wherein the position at which the excavated deposit is unloaded is the point on the surface of the deposit below the target plane that is the farthest from the target plane.

10. The motion planning method according to claim 1, wherein the position at which the excavated deposits are unloaded is a position where the volume of the space between the target plane P and the deposits is large in the area where the deposits spread when the excavated deposits are unloaded.

11. A motion planning device comprising: an information acquisition unit that acquires terrain information; a motion plan creation unit that creates a motion plan for a work machine to create a path between lowlands and highlands on the terrain indicated by the terrain information; and a motion plan output unit that outputs data indicating the created motion plan, wherein the motion plan creation unit comprises: a target plane determination unit that determines a target plane to be created after terrain modification is performed on the terrain indicated by the terrain information; an excavation position determination unit that determines a position to start excavating the deposit and an excavation depth based on the target plane; and an unloading position determination unit that determines a position at which to unload the excavated deposit, wherein the motion plan is created by compiling the determined position at which to excavate the deposit, the excavation depth, and the position at which to unload the excavated deposit.

12. A program for causing a computer to execute the motion planning method according to claim 1.

Images