Motion planning device

Abstract

A motion planning device (1) comprises: a point cloud input unit (2) that inputs a point cloud (PG), which is information about obstacles (10) around a moving body (100); a movable region generation unit (3) that specifies a plurality of positions (P_n) in a region where there is no obstacle (10), and generates movable regions that at least partially do not overlap each other, as movable regions (S_n) around the respective positions (P_n); and a travel region extraction unit (4) that sets at least one position set (T_m) including a specified position (P_n), selects one from the at least one position set (T_m) on the basis of the size of the movable region (S_n) corresponding to each position set (T_m) or the distance between the position (P_n) and the obstacle (10) closest to the movable region (S_n) around the position (P_n), and extracts at least a portion of the movable region (S_n) corresponding to the selected position set (T_m) as a travel region of the moving body (100).

Description

Motion planning device

[0001] The present disclosure relates to a motion planning device for a moving body.

[0002] A technique for generating a traveling route of a moving body based on distance measurement data around the moving body is known (for example, Patent Document 1). In the autonomous vehicle described in Patent Document 1, a local map is created based on the distance measurement data, and an autonomous vehicle in which a passing trajectory is searched on the obtained local map is disclosed.

[0003] Japanese Unexamined Patent Application Publication No. 2012-145998

[0004] However, in a conventional autonomous vehicle, it is determined whether or not to contact an obstacle after a passing trajectory is created. Also, depending on the situation, the creation of the passing trajectory may be repeated multiple times. Therefore, there has been a problem that the processing load increases in the creation of the passing trajectory of the autonomous vehicle. The present disclosure aims to provide a motion planning device capable of setting a traveling area of a moving body with a margin in the space from obstacles within the moving area with a low processing load.

[0005] The motion planning device according to the present disclosure includes a point cloud input unit that inputs a point cloud, which is information about obstacles around the moving body, a movable area generation unit that designates a plurality of positions in an area without obstacles and generates a movable area in which at least a part of the periphery of each position does not overlap with each other as a movable area, and at least one set of positions including the designated position is set, and one is selected from at least one set of positions based on the size of the movable area corresponding to each set of positions or the distance between the movable area and the obstacle, and at least a part of the movable area corresponding to the selected set of positions is extracted as the traveling area of the moving body.

[0006] According to the motion planning device of the present disclosure, it is possible to set a traveling area with a space secured from obstacles within the traveling range with a low processing load.

[0007] Figure 1 is a diagram showing the configuration of the motion planning device for a moving object according to Embodiment 1. Figure 2 is a block diagram showing the configuration of the travel area extraction unit according to Embodiment 1. Figure 3 is a diagram illustrating the function of the motion planning device according to Embodiment 1. Figure 4 is a diagram showing an example of a position set according to Embodiment 1. Figure 5 is a diagram showing another example of a position set according to Embodiment 1. Figure 6 is a diagram showing the movable area when the point cloud is a virtual point cloud. Figure 7 is a diagram showing a schematic of an obstacle when the point cloud is set as a fixed point cloud. Figure 8 is a diagram showing the operation flow of the motion planning device according to Embodiment 1. Figure 9 is a diagram showing the configuration of the motion planning device according to Embodiment 2. Figure 10 is a diagram showing the positions of the moving object and obstacles according to Embodiment 2. Figure 11 is a diagram illustrating the function of the motion planning device according to Embodiment 2. Figure 12 is a diagram showing an example of the position set T_m at time t_u according to Embodiment 2. Figure 13 is a diagram showing an example of the position set T_(m+1) at time t_u according to Embodiment 2. Figure 14 is a diagram showing an example of the position set T_(m+2) at time t_u according to Embodiment 2. Figure 15 shows an example of the position set T_m at time t_v according to Embodiment 2. Figure 16 shows an example of the position set T_(m+1) at time t_v according to Embodiment 2. Figure 17 shows an example of the position set T_(m+2) at time t_v according to Embodiment 2. Figure 18 shows the operation flow of the motion planning device according to Embodiment 2. Figure 19 shows the hardware configuration of the motion planning device according to Embodiments 1 and 2.

[0008] Embodiment 1. An action planning device according to Embodiment 1 of this disclosure will be described with reference to the drawings.

[0009] Figure 1 shows the configuration of the motion planning device 1 for an autonomously moving mobile body 100 according to this embodiment. The mobile body 100 comprises the motion planning device 1, a mobile body control unit 6, and a point cloud sensor 7. The motion planning device 1 comprises a point cloud input unit 2, a movable area generation unit 3, and a travel area extraction unit 4. The mobile body 100 is, for example, a robot that autonomously travels to supply parts, etc., to a production line in a factory, and is controlled to avoid obstacles such as pillars. The mobile body 100 may be a robot that travels within a limited travel area such as in a factory, or it may be like an automobile, where the travel path is determined but the travel range is not restricted, or it may be an aircraft that flies in three-dimensional space. The case in which the motion planning device 1 is provided on the mobile body 100 will be described as an example, but the motion planning device 1 may be provided on the outside of the mobile body 100.

[0010] A point cloud sensor 7 installed on the moving body 100 acquires a point cloud PG, which is information about obstacles 10 around the moving body 100. The point cloud sensor 7 measures the distance between the moving body 100 and the obstacles 10 and acquires the position information of the obstacles 10 as a point cloud PG. It is desirable that the point cloud sensor 7 can acquire a point cloud PG about obstacles 10 that are in front of the moving body 100 in the direction of travel, and may acquire a point cloud PG over a wider range than 180°, for example, 270° or 360°. This allows for the acquisition of position information of obstacles 10 over a wide range. The point cloud sensor 7 can be a (3D-)LiDAR sensor, and any distance measuring sensor capable of measuring the distance between the moving body 100 and the obstacles 10 is acceptable.

[0011] The point cloud PG acquired by the point cloud sensor 7 is input to the point cloud input unit 2.

[0012] Although the point cloud PG is said to be acquired by the point cloud sensor 7, the point cloud PG may also be generated based on information detected by a sensor 8, such as a camera, located outside the moving body 100. In this case, a means (not shown) generates the point cloud PG based on the information detected by the sensor 8 and inputs the set point cloud PG to the point cloud input unit 2.

[0013] The movable region generation unit 3, based on the point cloud PG input by the point cloud input unit 2, specifies multiple positions P_n in the area without obstacles 10, and generates movable regions S_n around each position, where at least a portion of each position does not overlap with the others. Details of the method for generating the movable regions S_n will be described later.

[0014] Figure 2 is a block diagram showing the configuration of the travel area extraction unit 4 according to this embodiment. The travel area extraction unit 4 includes a position set setting unit 41, a position set selection unit 42, and an extraction unit 43. The position set setting unit 41 sets at least one position set T_m that includes the position P_n specified by the movable area generation unit 3. The position set selection unit 42 selects one from the at least one position set T_m set by the position set setting unit 41 based on the size of the movable area S_n corresponding to each position set T_m or the distance between the movable area S_n and the obstacle 10. The extraction unit 43 then extracts at least a portion of the movable area S_n corresponding to the position set T_m selected by the position set selection unit 42 as the travel area R of the mobile body 100. Details of the method for extracting the travel area R will be described later. Even when not referring to a specific position P_n, movable area S_n, or position set T_m, the terms position P_n, movable area S_n, and position set T_m will be used.

[0015] The motion planning device 1 may include a route setting unit 5. The route setting unit 5 sets the route that the mobile body 100 will travel based on the travel area R extracted by the travel area extraction unit 4. The set route is input to the mobile body control unit 6 of the mobile body 100. The route setting unit 5 may be provided in the mobile body control unit 6 of the mobile body 100 instead of the motion planning device 1. In that case, the motion planning device 1 outputs the extracted travel area R to the mobile body control unit 6.

[0016] When the motion planning device 1 outputs a path for the mobile body 100 to travel, the mobile body control unit 6 controls the mobile body 100 to follow that path. When the motion planning device 1 outputs a travel area R, the mobile body control unit 6 performs the same functions as the path setting unit 5, and also controls the mobile body 100 to follow the path based on the path the mobile body 100 will travel.

[0017] Referring to Figure 3, the details of the motion planning device 1's functions will be explained. Figure 3 is a diagram illustrating the functions of the motion planning device 1 when the moving body 100 travels within a travel range that includes an obstacle 10. In this embodiment, the functions of the motion planning device 1 will be explained in the case where the obstacle 10 detected by the point cloud sensor 7 or sensor 8 is determined to be stationary. That is, the obstacle 10 is assumed to be fixed, and the travel range is assumed to be demarcated by the obstacle 10, such as a wall. In Figure 3, the range in which the point cloud PG is acquired is shown by a dotted line.

[0018] The movable region generation unit 3 specifies a position P_n (where n is a natural number) at a location free of obstacles 10, based on the point cloud PG. In Figure 3, the position P_n is indicated by a star (★). The movable region generation unit 3 specifies the position P_n by any method. In the example in Figure 3, seven positions P_n are specified. The position P_n may be specified randomly. When the position P_n is specified randomly, the processing load on the motion planning device 1 is reduced because it is not necessary to consider the positional relationship between the moving body 100 and the obstacles 10. The position P_n may be specified near the obstacles 10, or if there are multiple obstacles 10, the position P_n may be specified between the obstacles 10, or the position P_n may be specified between the moving body 100 and the obstacles 10. The size of the movable region S_n is affected by the position of the obstacles 10. The position P_n is effectively designated around the obstacle 10, and the number of positions P_n designated in areas without obstacles 10 is reduced, thereby reducing the processing load on the motion planning device 1.

[0019] The movable region generation unit 3 generates a movable region S_n corresponding to position P_n around position P_n, such that it does not overlap with the obstacle 10. In Figure 3, the movable region S_n is shown by a dashed line. The movable region S_n corresponding to position P_n and the movable region S_(n+1) corresponding to a different position P_(n+1) do not overlap in any part. The movable region S_n thus generated indicates the area in which the mobile body 100 can move from position P_n. In other words, if the size of the movable region S_n is large, the area in which the mobile body 100 can move is wide, and if the size of the movable region S_n is small, the area in which the mobile body 100 can move is narrow. The movable region generation unit 3 may also generate a movable region S_n based on the shape of the mobile body 100 so that the mobile body 100 fits inside. This reduces the processing load on the travel area extraction unit 4 because no movable region S_n that the mobile body 100 cannot travel in is generated. The size and shape of the movable region S_n may be any size and shape.

[0020] It is desirable that the movable region S_n is generated such that a line segment connecting any two points within the movable region S_n does not pass outside the movable region S_n. By generating the movable region S_n in this way, the obstacle 10 is not surrounded by any part of the movable region S_n. As a result, a limit is placed on the size of the movable region S_n within the travel range where the obstacle 10 is located.

[0021] Furthermore, the movable region generation unit 3 may generate a movable region S_n around position P_n so as to be tangent to the obstacle 10. Since the movable region S_n has a simple shape, the processing load on the motion planning device 1 is reduced. To further reduce the processing load on the motion planning device 1, it is desirable that the shape of the movable region S_n be a simple shape such as a circle. It may also be a convex polygon including a regular polygon, or an ellipse, or any shape is acceptable as long as the line segment connecting any two points within the movable region S_n does not pass outside the movable region S_n.

[0022] Figures 4 and 5 show an example of a position set T_m according to this embodiment. The travel area extraction unit 4 sets at least one position set T_m (where m is a natural number) so as to include the position P_n specified by the movable area generation unit 3, as shown in Figures 4 and 5. Figure 4 shows that the position set T_m is set for the position P_n specified in Figure 3 to be below the obstacle 10, and Figure 5 shows that the position set T_(m+1) is set for the position P_n specified in Figure 3 to be above the obstacle 10. The same position P_n may be included in multiple position sets T_m and T_(m+1). The travel area extraction unit 4 evaluates the distance between the movable area Sn corresponding to each position set T_m and T_(m+1) or the position P_n corresponding to each position set T_m and T_(m+1) and the obstacle 10, for the two position sets T_m and T_(m+1) shown in Figures 4 and 5 in the example of Figure 3, and selects one of the position sets T_m and T_(m+1) based on the evaluation result.

[0023] The travel area extraction unit 4 selects a position set T_m based, for example, on the area of ​​the movable area S_n. The travel area extraction unit 4 selects the smallest minimum movable area Smin_m among the movable areas S_n corresponding to each position set T_m for each position set T_m. The travel area extraction unit 4 compares the minimum movable areas Smin_m and selects the position set T_m that contains the smallest minimum movable area Smin_m. As a result, a travel area R is extracted in which space can be secured around the mobile body 100 even at the point where the space around the mobile body 100 is narrowest.

[0024] Furthermore, the travel area extraction unit 4 may select one location set T_m based on a statistical value other than the minimum value shown above as the statistical value of the area size of the movable region S_n corresponding to each location set T_m. This ensures that the size of all movable regions S_n included in the location set T_m is considered. The statistical value is, for example, the average value or variance of the area size of the movable region S_n corresponding to each location set T_m. When the average value is used as the statistical value of the area size of the movable region S_n, the size of all movable regions S_n included in the location set T_m can be considered while suppressing an increase in the processing load of the motion planning device 1. When the variance value is used as the statistical value of the area size of the movable region S_n, the spatial characteristics of the movable region S_n can be easily obtained. It is not limited to the average value as long as the characteristic quantities of the area size of the movable region S_n corresponding to each location set T_m can be extracted.

[0025] Alternatively, the travel area extraction unit 4 may select one location set T_m based on the distance between each location P_n constituting the location set T_m and the obstacle 10 surrounding the location P_n. The travel area extraction unit 4 may also select the smallest minimum distance Lmin_m for each location set T_m from the distance between each location P_n constituting the location set T_m and the obstacle 10 closest to the movable area S_n surrounding the location P_n, and then select the location set T_m that contains the largest minimum distance Lmin_m.

[0026] Furthermore, the travel area extraction unit 4 may select one location set T_m based on the shape characteristics of the location set T_m. The shape characteristics of the location set T_m are, for example, the distance from the current position of the autonomous mobile body 100 to the destination along the movable area S_n corresponding to the location set T_m. This makes it possible to set a route that has the shortest path length to the destination, that is, a route that takes the shortest time to reach the destination. Alternatively, it is the curvature of the path when the autonomous mobile body 100 moves from its current position to the destination along the movable area S_n corresponding to the location set T_m. The curvature may be the number of times the curvature changes, the minimum value of the curvature, etc. This makes it possible to set a route that allows the mobile body 100 to travel smoothly, avoiding routes that require abrupt changes in direction or routes that require many changes in direction.

[0027] The travel area extraction unit 4 selects one position set T_m using at least one of the methods described above. In the example in Figure 3, for example, the travel area extraction unit 4 compares the area of ​​the minimum movable area Smin_m and selects the position set T_m shown in Figure 4 with the larger area. The travel area extraction unit 4 also compares the minimum distance Lmin_m and selects the position set T_m shown in Figure 4 with the larger distance. The travel area extraction unit 4 extracts at least a portion of the movable areas S_n (four in the example of Figure 4) corresponding to the selected position set T_m as the travel area R of the mobile body 100.

[0028] The route setting unit 5 in the motion planning device 1 sets the travel path of the mobile body 100 based on the travel area R extracted by the travel area extraction unit 4. The motion planning device 1 may also output information of the travel area R to the mobile body control unit 6 of the mobile body 100. In this case, the route setting unit 5 in the mobile body control unit 6 sets the travel path of the mobile body 100 based on the travel area R. The mobile body 100 is controlled to travel along the set travel path.

[0029] The direction of travel of the moving body 100 is defined as forward, and the opposite side of the direction of travel is defined as rear. The movable area generation unit 3 may specify a position Pn on the rear side of the moving body 100. In this case, even if there is an obstacle 10 that cannot be detected by the point cloud sensor 7 at the start of movement and a movable area Sn cannot be generated in front of the moving body 100, the moving area R that passes through the rear of the moving body 100 will be extracted without the moving body 100 becoming stuck.

[0030] Figure 6 shows the movable area when the point cloud is a virtual point cloud, and Figure 7 shows a schematic diagram of an obstacle when the point cloud is set as a fixed point cloud. The point cloud PG is said to be acquired by the point cloud sensor 7, but the point cloud PG may also be generated based on information detected by a sensor 8 such as a camera installed outside the mobile body 100. The motion planning device 1 may also be equipped with a storage device (not shown), which stores map information of the travel range of the mobile body 100 in advance. In this case, the point cloud PG may also be a virtual point cloud VP (Figure 6) generated based on the map information stored in the storage device. If the obstacle 10 is a fixed structure, it may also be a fixed point cloud FP set from the position information of the obstacle 10 as shown in Figure 7. The storage device may be provided on the mobile body 100. Alternatively, the storage device may be provided on a network such as a cloud, and the mobile body 100, which has communication means, may access the storage device to acquire map information.

[0031] The point cloud sensor 7 may be, for example, a 3D-LiDAR, and the point cloud PG may include three-dimensional coordinate information. In this case, the movable region generation unit 3 generates a movable region V_n in three dimensions around a specified position Pn. The movable region V_n may be generated such that the cross-sectional shape in a plane parallel to the direction of movement of the moving body 100 is the same, or it may be generated such that the cross-sectional shape in a plane parallel to the direction of movement of the moving body 100 changes based on the shape of the moving body 100. The movable region V_n is generated such that, in a cross-section selected under predetermined conditions, a line segment connecting any two points does not pass outside the cross-sectional shape. The cross-section may be a cross-section cut by a plane parallel to the direction of movement of the moving body 100, a cross-section cut by a plane perpendicular to the direction of movement of the moving body 100, or a cross-section cut by a plane that approximately includes a point in the height direction where the distance to the obstacle 10 changes abruptly.

[0032] If a part of the mobile body 100 has a shape-changing portion that can change shape, such as the arm of a robot, and the size of the movable area Sn decreases or the movable area Sn cannot be generated due to the shape-changing portion coming into contact with an obstacle 10, the shape of the shape-changing portion may be changed so that the movable area Sn becomes larger.

[0033] If the point cloud PG includes three-dimensional coordinate information, the point cloud input unit 2 may receive input of the point cloud PG processed by a separate point cloud processing unit (not shown) from the motion planning device 1. The point cloud processing unit divides the moving body 100 into regions parallel to the direction of movement of the moving body 100, for example, based on the shape of the moving body 100. In each divided region, the shape of the moving body 100 is set such that the cross-sectional shape in the plane parallel to the direction of movement of the moving body 100 is equal. At a specified position Pn, the point cloud processing unit sets a point cloud PG corresponding to each divided region and extracts one of the divided regions based on the point cloud PG and the cross-sectional shape of the moving body 100 corresponding to each divided region. The point cloud processing unit outputs the point cloud PG and the shape of the moving body 100 corresponding to the extracted divided region to the motion planning device 1. The motion planning device 1 sets the movable region S_n based on the point cloud PG corresponding to position P_n input from the point cloud processing unit. With this configuration, even if the point cloud PG includes three-dimensional coordinate information, the motion planning device 1 can use only the minimum necessary point cloud PG, thereby reducing the processing load on the motion planning device 1. The motion planning device 1 may generate a movable region V_n of the three-dimensional shape and use the cross-sectional shape of the extracted divided region as the movable region S_n. In addition, since the shape of the moving body 100 in the divided region selected by the point cloud processing device is also output, if the selected divided region includes a shape-changeable area, the shape of the shape-changeable area can be changed.

[0034] <Operation Flow> The operation flow of Embodiment 1 will be described with reference to Figure 8. Figure 8 is a diagram showing the operation flow of the operation planning device 1 according to this embodiment.

[0035] In step ST1, the point cloud input unit 2 acquires a point cloud PG. The point cloud PG includes coordinate information of the obstacle 10. The point cloud input unit 2 outputs the point cloud PG to the movable region generation unit 3.

[0036] In step ST2, the movable region generation unit 3 specifies a location P_n based on the point cloud PG where there are no obstacles 10. The movable region generation unit 3 generates a movable region S_n corresponding to location P_n, such that it does not overlap with the obstacles 10 around location P_n.

[0037] In step ST3, the position set setting unit 41 within the travel area extraction unit 4 sets at least one position set T_m (where m is a natural number) that includes the position P_n specified by the movable area generation unit 3.

[0038] In step ST4, the position set selection unit 42 within the travel area extraction unit 4 selects one position set T_m based on the distance between the movable area S_n corresponding to each position set T_m or the position P_n corresponding to each position set T_m and the obstacle 10.

[0039] In step ST5, the extraction unit 43 within the travel area extraction unit 4 extracts at least a portion of the movable area S_n corresponding to the selected position set T_m as the travel area R of the mobile body 100, and then terminates its operation.

[0040] <Effects of Embodiment 1> The motion planning device 1 according to Embodiment 1 includes: a point cloud input unit 2 that inputs a point cloud PG which is information relating to obstacles 10 around a moving body 100; a movable region generation unit 3 that specifies a plurality of positions P_n in an area without obstacles 10 and generates movable regions S_n around each position P_n in which at least a portion does not overlap; and a travel region extraction unit that sets at least one position set T_m including the specified positions P_n, selects one from at least one position set T_m based on the size of the movable region S_n corresponding to each position set T_m or the distance between the movable region S_n and the obstacle 10, and extracts at least a portion of the movable region S_n corresponding to the selected position set T_m as the travel region R of the moving body 100.

[0041] That is, the operation planning device 1 according to Embodiment 1 sets the travel region R based on the movable region S_n generated around the designated position P_n. As a result, a travel route is generated so as to include the travel region R in which the mobile body 100 can easily travel. In addition, since the travel route is not generated more than necessary for all the regions where the mobile body 100 can travel, the processing load of the operation planning device 1 can be reduced.

[0042] Further, the movable region generation unit 3 of the operation planning device 1 according to Embodiment 1 generates a movable region S_n that is a plane in which a line segment connecting any two points does not pass through the outside. As a result, the movable region S_n has a simple shape, and the movable region S_n has no concave portion and does not surround the obstacle 10. Therefore, since the size of the movable region S_n is limited, the processing load of the operation planning device 1 is reduced. In addition, it is possible to prevent a region that is originally not drivable from being set as the travel region due to the generation of a large movable region.

[0043] In the operation planning device 1 according to Embodiment 1, the movable region generation unit 3 generates the movable region S_n so as to circumscribe the obstacle 10 around the designated position P_n. With such a configuration, the movable region S_n is generated so as to have the maximum area, the range in which the mobile body 100 can move increases, and the degree of freedom in setting the path of the mobile body 100 is improved. As a result, a path with a small turning radius is avoided and the movement of the mobile body 100 becomes smooth.

[0044] Further, the movable region generation unit 3 of the operation planning device 1 according to Embodiment 1 generates the movable region S_n such that the boundary of the movable region S_n is equidistant from the designated position P_n. As a result, the movable region S_n has a simple shape, and the processing load of the operation planning device 1 is reduced.

[0045] In the operation planning device 1 according to Embodiment 1, the movable region generation unit 3 randomly designates the position P_n. As a result, the position P_n is designated without considering the positional relationship between the mobile body 100 and the obstacle 10, and the processing load is reduced.

[0046] Also, in the operation planning device 1 according to Embodiment 1, the size of the movable area S_n is an area, and the travel area extraction unit 4 selects the smallest minimum movable area Smin_m among the movable areas S_n corresponding to the position set T_m for each of the position sets, and selects the position set T_m that includes the largest minimum movable area Smin_m among the minimum movable areas Smin_m. Thereby, a travel area R that can secure a space around the moving body 100 is extracted even at a location where the space around the moving body 100 is the narrowest.

[0047] Also, in the operation planning device 1 according to Embodiment 1, the travel area extraction unit 4 selects one from the position sets T_m based on the statistical value of the sizes of the movable areas S_n included in each of the plurality of position sets T_m. Thereby, the position set T_m is selected in consideration of the sizes of all the movable areas S_n included in the position set T_m.

[0048] Also, in the operation planning device 1 according to Embodiment 1, the statistical value is the average value of the areas of the movable areas S_n included in the plurality of position sets T_m. Thereby, the processing load can be reduced even when considering the areas of all the movable areas S_n included in the position set T_m.

[0049] Also, the travel area extraction unit 4 of the operation planning device 1 according to Embodiment 1 selects the smallest minimum distance Lmin_m among the distances between each position P_n constituting the position set T_m and the obstacle 10 closest to the movable area S_n around the position P_n for each of the position sets T_m, and selects the position set T_m that includes the largest minimum distance Lmin_m among the minimum distances Lmin_m. Thereby, a travel area R in which the distance between the moving body 100 and the obstacle 10 is secured is extracted.

[0050] Also, in the operation planning device 1 according to Embodiment 1, the travel area extraction unit 4 selects one from at least one position set T_m based on the shape characteristics of the position set T_m. Thereby, in addition to the size of the movable area S_n around the moving body 100, the shape of the path along the position set T_m is also considered.

[0051] Furthermore, in the motion planning device 1 according to Embodiment 1, the shape feature is the distance to the destination along the position set T_m. This allows for the extraction of a travel region R such that the travel distance of the moving body 100 is shortened.

[0052] Furthermore, in the motion planning device 1 according to Embodiment 1, the shape characteristic is the curvature of the path when moving along the position set T_m to the destination. This allows for the extraction of a travel region R that minimizes the number of direction changes required to reach the destination.

[0053] Furthermore, in the motion planning device 1 according to Embodiment 1, the point cloud PG is three-dimensional information, and the movable region generation unit 3 generates a movable region V_n with a three-dimensional shape. As a result, even when there is an obstacle 10 in a direction perpendicular to the direction of travel of the moving body 100, the travel region R can be extracted with low processing load.

[0054] Furthermore, the travel area extraction unit 4 of the motion planning device 1 according to Embodiment 1 calculates the width of the path in the area containing the position set T_m based on the size or distance of the movable area S_n corresponding to the position set T_m, and selects the position set T_m based on the width of the path. As a result, a travel area R with sufficient distance between the moving body 100 and the obstacle 10 is extracted.

[0055] Furthermore, in the motion planning device 1 according to Embodiment 1, the point cloud PG input by the point cloud input unit 2 includes a virtual point cloud VP set from map information of the area in which the moving object 100 moves. As a result, the travel area R is extracted even when the travel range is not partitioned by obstacles 10 such as walls.

[0056] Furthermore, in the motion planning device 1 according to Embodiment 1, the point cloud PG input by the point cloud input unit 2 includes a fixed point cloud FP set from the position information of fixed structures among the obstacles 10. As a result, even if there are obstacles 10 in a blind spot from the moving body 100, the travel area R is extracted taking the obstacles 10 into consideration.

[0057] Furthermore, the motion planning device 1 according to Embodiment 1 further includes a path setting unit 5 that determines the path the mobile body 100 will travel based on the extracted travel area R. This allows the path the mobile body 100 will travel to be set with a low processing load.

[0058] Embodiment 2. The following describes the motion planning device 1a according to Embodiment 2, focusing on the differences from Embodiment 1. In Embodiment 1, the point cloud PG, which is the position information of the obstacle 10, has a fixed absolute position, although its relative position to the moving body 100 changes. Embodiment 2 differs in that it also includes obstacles whose absolute position changes, such as a person or another moving body. In this embodiment, the function of the motion planning device 1a will be described in the case where it is determined that the obstacle 10a detected by the point cloud sensor 7 or sensor 8 includes a moving obstacle.

[0059] Figure 9 is a diagram showing the configuration of the motion planning device 1a according to this embodiment. The same parts as the motion planning device 1 shown in Figure 1 will not be explained. The motion planning device 1a further comprises a point cloud storage unit 11 and a point cloud prediction unit 12. The point cloud storage unit 11 stores the point cloud PG input by the point cloud input unit 2 as time data. The point cloud prediction unit 12 acquires the point cloud PG from the point cloud storage unit 11. Based on the point cloud PG, the point cloud prediction unit 12 calculates the direction and speed of movement of the obstacles 10a, predicts the future point cloud PG corresponding to each of the obstacles 10a, and outputs it to the movable area generation unit 3. The point cloud prediction unit 12 acquires the direction and speed of movement of the moving body 100a from the path setting unit 5. Based on the direction and speed of movement of the moving body 100a, the point cloud prediction unit 12 predicts the future position of the moving body 100a.

[0060] The movable region generation unit 3a assigns a position P_n at each time interval to a location free of obstacles 10a, corresponding to the point cloud PG obtained from the point cloud input unit 2 and the future point cloud PG of the obstacle 10a predicted by the point cloud prediction unit 12. The movable region generation unit 3a generates a movable region S_n corresponding to position P_n around position P_n, so as not to overlap with obstacles 10a, corresponding to each point cloud PG.

[0061] The function of the motion planning device 1a according to this embodiment will be described with reference to Figures 10 and 11. Figure 10 is a diagram showing the positions of the moving body 100a and the obstacle 10a, and Figure 11 is a diagram illustrating the function of the motion planning device 1a when the moving body 100a travels within a travel range that includes the moving obstacle 10a. Figure 10(a) shows the positions of the moving body 100 and the obstacle 10a at time t_u, and Figure 10(b) shows the predicted positions of the moving body 100a and the obstacle 10a at time t_v. Time t_v is a time later than time t_u. The position of the moving body 100a at time t_v is predicted, for example, from the direction and velocity of the moving body 100a at time t_u, assuming that the moving body 100a moves at a constant speed. The position of the obstacle 10a at time t_v is predicted by the point cloud prediction unit 12 based on the point cloud PG of the obstacle 10a at time t_u, for example, assuming that the obstacle 10a is moving at a constant velocity. The direction and velocity of the obstacle 10a at time t_u are shown by arrows in Figure 10(a). The point cloud prediction unit 12 may predict that the obstacle 10a will move at a constant velocity in the direction of movement, based on the direction and velocity of the obstacle 10a's movement. Alternatively, the point cloud prediction unit 12 may use the detected direction of movement as an initial value and predict the movement trajectory of the obstacle 10a if it moves at a constant velocity to avoid collisions with other obstacles 10a. Note that the prediction method is not limited to the case of movement at a constant velocity.

[0062] As shown in Figure 10(a), assume that at time t_u, the moving object 100a is at position M(t_u) and the obstacle 10a is at position O(t_u). At time t_v, which is later than time t_u, the moving object 100a will approach the obstacle 10a, and as shown in Figure 10(b), it is predicted that the moving object 100a will be at position M(t_v) and the obstacle 10a will be at position O(t_v).

[0063] At time t_u, as shown in Figure 11(a), the movable region generation unit 3 specifies a position P_n(t_u) and generates a movable region S_n(t_u) around position P_n(t_u). In the example in Figure 11(a), seven positions P_n(t_u) are specified. The method for specifying positions P_n(t_u) and generating the movable region S_n(t_u) is the same as in Embodiment 1. Based on the predicted positions of the moving body 100a and the obstacle 10a, the movable region generation unit 3 specifies a position P_n(t_v) at time t_v, as shown in Figure 11(b), and generates a movable region S_n(t_v) around position P_n(t_v). In the example in Figure 11(b), eight positions P_n(t_v) are specified.

[0064] Figures 12, 13, and 14 show examples of position sets T_m(t_u) at time t_u according to this embodiment. As shown in Figures 12, 13, and 14, the travel area extraction unit 4 sets at least one position set T_m(t_u) to include the position P_n(t_u) specified by the movable area generation unit 3. Figure 12 shows that position set T_m(t_u) is set for a position P_n(t_u) specified below the moving obstacle 10a in Figure 11(a), Figure 13 shows that position set T_(m+1)(t_u) is set for a position P_n(t_u) between the moving obstacle 10a and the central obstacle 10a in Figure 11(a), and Figure 14 shows that position set T_(m+2)(t_u) is set for a position P_n(t_u) above the central obstacle 10a in Figure 11(a).

[0065] Figures 15, 16, and 17 show examples of position sets T_m(t_v) at time t_v according to this embodiment. As shown in Figures 15, 16, and 17, the travel area extraction unit 4 sets at least one position set T_m so as to include the position P_n(t_v) specified by the movable area generation unit 3. Figure 15 shows that position set T_m(t_v) is set for position P_n(t_v) specified to be below the moving obstacle 10a in Figure 11(b), Figure 16 shows that position set T_(m+1)(t_v) is set for position P_n(t_v) between the moving obstacle 10a and the central obstacle 10a in Figure 11(b), and Figure 17 shows that position set T_(m+2)(t_v) is set for position P_n(t_v) above the central obstacle 10a in Figure 11(b).

[0066] The travel area extraction unit 4a selects one position set T_m(t_u) at time t_u based on the minimum movable area Smin_m(t_u) corresponding to the position set T_m(t_u) at time t_u and the minimum movable area Smin_m(t_v) corresponding to the position set T_m(t_v) predicted at time t_v. The method for selecting one position set T_m(t_u) is not limited to comparing the minimum movable areas Smin_m, but can also be done by comparing the minimum distance Lmin_m, as in Embodiment 1.

[0067] The method of comparing the minimum movable regions Smin_m will be explained. At time t_u shown in Figure 11(a), when comparing the minimum movable regions Smin_m(t_u), Smin_(m+1)(t_u), and Smin_(m+2)(t_u) corresponding to the position set T_m(t_u) shown in Figure 12, the minimum movable region Smin_m(t_u) corresponding to the position set T_m(t_u) shown in Figure 13, and the position set T_(m+2)(t_u) shown in Figure 14, the minimum movable region Smin_m(t_u) corresponding to the position set T_m(t_u) shown in Figure 12 is the largest. At time t_v shown in Figure 11(b), comparing the minimum movable regions Smin_m(t_v), Smin_(m+1)(t_v), and Smin_(m+2)(t_v) corresponding to the position set T_m(t_v) shown in Figure 15, the minimum movable region Smin_m(t_v) corresponding to the position set T_m(t_v) shown in Figure 16, and the position set T_(m+2)(t_v) shown in Figure 17, the minimum movable region Smin_m(t_v) corresponding to the position set T_m(t_v) shown in Figure 15 is the largest. In both cases of time t_u and time t_v, the minimum movable region Smin_m(t_u, t_v) corresponding to the position set T_m(t_u, t_v) is the largest. Therefore, the travel region extraction unit 4a selects the position set T_m(t_u) shown in Figure 12 at time t_u.

[0068] The method of comparing minimum distances Lmin_m will be explained. At time t_u shown in Figure 11(a), when comparing the minimum distances Lmin_m(t_u), Lmin_(m+1)(t_u), and Lmin_(m+2)(t_u) corresponding to the position set T_m(t_u) shown in Figure 12, the minimum distance Lmin_m(t_u) corresponding to the position set T_m(t_u) shown in Figure 13, and the position set T_(m+2)(t_u) shown in Figure 14, the minimum distance Lmin_m(t_u) corresponding to the position set T_m(t_u) shown in Figure 12 is the largest. At time t_v shown in Figure 11(b), comparing the minimum distances Lmin_m(t_v), Lmin_(m+1)(t_v), and Lmin_(m+2)(t_v) corresponding to the position set T_m(t_v) shown in Figure 15, the minimum distance Lmin_m(t_v) corresponding to the position set T_m(t_v) shown in Figure 16, and the position set T_(m+2)(t_v) shown in Figure 17, the minimum distance Lmin_m(t_v) corresponding to the position set T_m(t_v) shown in Figure 15 is the largest. In both cases of time t_u and time t_v, the minimum distance Lmin_m(t_u, t_v) corresponding to the position set T_m(t_u, t_v) is the largest. Therefore, the travel area extraction unit 4a selects the position set T_m(t_u) shown in Figure 12 at time t_u.

[0069] In the examples shown in Figures 11(a) and 11(b), the minimum movable area Smin_m and minimum distance Lmin_m corresponding to the position set T_m are the largest in both time t_u and time t_v. However, the method for selecting the position set T_m when the position set T_m with the largest minimum movable area Smin_m or minimum distance Lmin_m is different at time t_u and time t_v will be explained. For example, even if the minimum movable area Smin_m and minimum distance Lmin_m at time t_u are small, the position set T_m with the largest minimum movable area Smin_m and minimum distance Lmin_m at time t_v is expected to be easier for the mobile body 100 to travel in the future, so that position set T_m may be selected.

[0070] The movable region generation unit 3a may generate the movable region S_n(t_v) at time t_v around position P_n(t_v) by recalculating the movable region S_n(t_u) at time t_u and the predicted position of the obstacle 10a at time t_v. This reduces the processing load on the movable region generation unit 3a compared to when the movable region S_n(t_u) at time t_u is not used.

[0071] The point cloud prediction unit 12 predicts the future point cloud PG corresponding to each point cloud PG of the obstacle 10a based on the time-series data of the point cloud PG stored in the point cloud storage unit 11. However, it is also possible to predict the future position of each obstacle 10a based on information acquired by the sensor 8 provided outside the moving body 100a, and then predict the point cloud PG corresponding to each obstacle 10a based on the predicted position.

[0072] In this embodiment as well, the point cloud PG may include three-dimensional coordinate information. In that case, the movable region generation unit 3a generates a movable region Vn in three dimensions. Even if there is an obstacle 10a in a direction perpendicular to the direction of movement of the moving body 100a, the travel region R is extracted with low processing load.

[0073] <Operation Flow> The operation flow of the operation planning device 1a of Embodiment 2 will be explained with reference to Figure 18. Figure 18 is a diagram showing the operation flow of the operation planning device 1a of Embodiment 2. Only the differences from the operation flow of the operation planning device 1 of Embodiment 1 will be explained.

[0074] In step ST6, the point cloud prediction unit 12 calculates the direction and velocity of movement of the obstacle 10a based on the time-series data of the point cloud PG stored in the point cloud storage unit 11. In step ST7, the point cloud prediction unit 12 predicts the future point cloud PG corresponding to the obstacle 10a from the calculated direction and velocity of movement of the obstacle 10a.

[0075] In step ST8, the movable region generation unit 3a specifies a position P_n corresponding to the point cloud PG obtained from the point cloud input unit 2 and the point cloud PG predicted by the point cloud prediction unit 12, and generates a movable region S_n around the specified position P_n. In step ST9, the travel region extraction unit 4a sets at least one position set T_m so as to include the specified position P_n corresponding to the point cloud PG and the predicted point cloud PG. In step ST10, the travel region extraction unit 4a selects one position set T_m from the position sets T_m corresponding to the point cloud PG based on the position sets T_m corresponding to the point cloud PG and the predicted point cloud PG, respectively.

[0076] <Effects of Embodiment 2> The motion planning device 1a according to this embodiment further includes a point cloud prediction unit 12 that predicts future point cloud PGs corresponding to each of the obstacles 10a based on the direction and speed of movement of the obstacles 10a calculated from the time-series data of the point cloud PG. The movable region generation unit 3 generates a movable region Sn based on the future point cloud PGs corresponding to the obstacles 10a predicted by the point cloud prediction unit 12. The travel region extraction unit 4 sets at least one position set T_m corresponding to the point cloud PG and the predicted point cloud PG, and selects one position set T_m corresponding to the point cloud PG based on the position set T_m. With this configuration, even if there are obstacles 10a moving within the travel range, the travel region R of the moving body 100 is extracted so as not to collide with the obstacles 10a.

[0077] Here, the hardware configuration of the motion planning devices 1 and 1a in Embodiments 1 and 2 will be described. Each function of the motion planning devices 1 and 1a can be realized by a processing circuit 93. The processing circuit 93 comprises at least one processor 91 and at least one memory 92.

[0078] Figure 19 shows the hardware configuration of the motion planning devices 1 and 1a in Embodiments 1 and 2. The motion planning devices 1 and 1a can be realized by the processor 91 and memory 92 shown in Figure 19(a). The processor 91 is, for example, a CPU (Central Processing Unit, central processing unit, processing unit, arithmetic unit, microprocessor, microcomputer, processor, also called a DSP (Digital Signal Processor)) or a system LSI (Large Scale Integration).

[0079] Memory 92 includes, for example, non-volatile or volatile semiconductor memory such as RAM (Random Access Memory), ROM (Read Only Memory), flash memory, EPROM (Erasable Programmable Read Only Memory), EEPROM® (Electrically Erasable Programmable Read-Only Memory), HDD (Hard Disk Drive), magnetic disk, flexible disk, optical disk, compact disk, minidisc, or DVD (Digital Versatile Disk).

[0080] The functions of each part of the motion planning devices 1 and 1a are realized by software, firmware, or a combination of software and firmware. The software is written as a program and stored in memory 92. The processor 91 realizes the functions of each part by reading and executing the program stored in memory 92. In other words, this program can be said to cause the computer to execute the procedures or methods of the motion planning devices 1 and 1a.

[0081] The program executed by the processor 91 may be provided as a computer program product, stored on a computer-readable storage medium, in an installable or executable file format. Alternatively, the program executed by the processor 91 may be provided to the operation planning devices 1 and 1a via a network such as the Internet.

[0082] Furthermore, the motion planning devices 1 and 1a may be implemented by a dedicated processing circuit 93 shown in Figure 19(b). If the processing circuit 93 is dedicated hardware, it may be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a combination thereof.

[0083] The above describes a configuration in which the functions of each component of the motion planning devices 1 and 1a are realized by either software or hardware. However, this is not the only configuration; some components of the motion planning devices 1 and 1a may be realized by software, while others may be realized by dedicated hardware.

[0084] 1, 1a Motion planning device, 2 Point cloud input unit, 3, 3a Movable area generation unit, 4, 4a Travel area extraction unit, 41 Position set setting unit, 42 Position set selection unit, 43 Extraction unit, 5 Path setting unit, 6 Mobile body control unit, 7 Point cloud sensor, 8 Sensor, 9 Point cloud prediction unit, 10, 10a Obstacle, 11 Point cloud storage unit, 12 Point cloud prediction unit, 100, 100a Mobile body, 91 Processor, 92 Memory, 93 Processing circuit PG Point cloud, VP Virtual point cloud, FP Fixed point cloud, P_n Position, S_n Movable area, T_m Position set, Smin_m Minimum movable area, Lmin_m Minimum distance

Claims

1. A motion planning device comprising: a point cloud input unit that inputs a point cloud containing information about obstacles around a moving body; a movable region generation unit that specifies a plurality of positions in an area without obstacles and generates movable regions around each of the positions, where at least a portion of each position does not overlap with the others; and a travel region extraction unit that sets at least one set of positions including the specified positions, selects one from the at least one set of positions based on the size of the movable region corresponding to each set of positions or the distance between the position and the obstacle closest to the movable region around the position, and extracts at least a portion of the movable region corresponding to the selected set of positions as the travel region of the moving body.

2. The motion planning device according to claim 1, wherein the movable region generation unit generates a movable region which is a plane such that a line segment connecting any two points does not pass through the outside, or a solid having the cross section such that a line segment connecting any two points does not pass through the outside in a cross section selected under predetermined conditions.

3. The motion planning device according to claim 2, wherein the movable region generation unit generates the movable region so as to be tangent to the obstacle with respect to the specified position.

4. The motion planning device according to claim 3, wherein the movable region generation unit generates the boundary of the movable region such that it is equidistant from the designated position.

5. The motion planning device according to any one of claims 1 to 4, wherein the movable region generation unit randomly specifies the position.

6. The motion planning device according to any one of claims 1 to 5, wherein the size of the movable region is area or volume, and the travel region extraction unit selects the smallest minimum movable region among the movable regions corresponding to the position set for each of the position sets, and selects the position set that includes the largest minimum movable region among the minimum movable regions.

7. The motion planning device according to any one of claims 1 to 5, wherein the travel area extraction unit selects one of the position sets based on the statistical value of the size of the movable area included in each of the plurality of position sets.

8. The motion planning device according to claim 7, wherein the statistical value is the average value of the area or volume of the movable region included in a plurality of position sets.

9. The motion planning device according to any one of claims 1 to 5, wherein the travel area extraction unit selects the smallest minimum distance among the distances between the movable area corresponding to the position set and the obstacle for each of the position sets, and selects the position set that contains the largest of the minimum distances.

10. The motion planning device according to any one of claims 1 to 5, wherein the travel area extraction unit selects one from at least one of the position sets based on the shape characteristics of the position set.

11. The motion planning device according to claim 10, wherein the shape feature is the path length to the destination along the set of positions.

12. The motion planning device according to claim 10, wherein the shape feature is the curvature of the path when moving along the set of positions to the destination.

13. The motion planning device according to any one of claims 1 to 12, wherein the point cloud is three-dimensional information, and the movable region generation unit generates the movable region as a three-dimensional shape.

14. The motion planning device according to any one of claims 1 to 13, further comprising a point cloud prediction unit that predicts the future point cloud of the obstacle from the direction and speed of movement of the obstacle, wherein the movable region generation unit generates the movable region based on the future point cloud of the obstacle predicted by the point cloud prediction unit.

15. The motion planning device according to any one of claims 1 to 14, wherein the travel area extraction unit calculates the width of a path in the area containing the position set based on the size or distance of the movable area corresponding to the position set, and selects the position set based on the width of the path.

16. The motion planning device according to any one of claims 1 to 15, wherein the point cloud input by the point cloud input unit includes a virtual point cloud set from map information of the area in which the moving object moves.

17. The motion planning device according to any one of claims 1 to 16, wherein the point cloud input by the point cloud input unit includes a fixed point cloud set from the position information of fixed structures among the obstacles.

18. The motion planning device according to any one of claims 1 to 17, further comprising a path setting unit that determines a path for the moving body based on the extracted travel area.

Images