WO2024171259A1 - Path planning device, path planning method, and path planning program - Google Patents

Abstract

A control device 3 that plans a path of a moving body 100 comprises: an approximator 42 that approximates an obstacle to an approximate obstacle having spatial-rotation or spatial-translation symmetry; an assigner 43 that assigns a Riemannian metric of the approximate obstacle; and a calculator 44 that calculates a geodesic from a geodesic equation based on the Riemannian metric and sets a path on the basis of the calculated geodesic.

Description

Path planning device, path planning method, and path planning program

The technology disclosed herein relates to a route planning device, a route planning method, and a route planning program.

Various methods have been proposed for planning paths for moving objects. For example, a method for planning paths has been proposed that uses geodesic equations on a Riemannian manifold. Patent Document 1 discloses a method for solving optimization problems that uses geodesic equations.

JP 2003-30172 A

By the way, geodesic equations are expressed as nonlinear ordinary differential equations. Therefore, in most cases, geodesic equations are solved numerically, which requires a lot of time for calculation.

The technology disclosed here has been developed in light of these issues, and its purpose is to reduce the calculation time in path planning using geodesic equations.

The route planning device disclosed herein is a route planning device that plans a route for a moving body, and includes an approximator that approximates an obstacle to an approximate obstacle having symmetry of spatial rotation or spatial translation, an assigner that assigns a Riemann metric to the approximate obstacle, and a calculator that calculates a geodesic line from a geodesic equation based on the Riemann metric and sets a route based on the calculated geodesic line.

The route planning method disclosed herein is a route planning method for planning a route for a moving body, and includes approximating an obstacle to an approximate obstacle having symmetry of spatial rotation or spatial translation, assigning a Riemannian metric to the approximate obstacle, calculating a geodesic line from a geodesic equation based on the Riemannian metric, and setting a route based on the calculated geodesic line.

The route planning program disclosed herein is a route planning program for causing a computer to realize the planning of a route for a moving body, and causes the computer to realize the following functions: approximating an obstacle to an approximate obstacle having symmetry in spatial rotation or spatial translation; assigning a Riemannian metric to the approximate obstacle; and calculating a geodesic line from a geodesic equation based on the Riemannian metric, and setting a route based on the calculated geodesic line.

The above-mentioned path planning device can reduce the calculation time in path planning using geodesic equations.

The above-mentioned route planning method can reduce the calculation time in route planning using geodesic equations.

The above-mentioned route planning program can reduce calculation time in route planning using geodesic equations.

FIG. 1 is an explanatory diagram of a moving body. FIG. 2 is a diagram showing a schematic hardware configuration of a moving object. FIG. 3 is a diagram showing a schematic hardware configuration of the control device. FIG. 4 is a functional block diagram of the controller. FIG. 5 is an image diagram of a two-dimensional route. FIG. 6 is an explanatory diagram of a path of a moving object moving three-dimensionally. FIG. 7 is an image diagram of a two-dimensional path obtained based on a geodesic equation corresponding to a real obstacle. FIG. 8 is a flow chart of route planning.

Below, an exemplary embodiment will be described in detail with reference to the drawings. FIG. 1 is an explanatory diagram of a moving body 100. The moving body 100 performs autonomous driving. For example, the moving body 100 is a vehicle (e.g., an AGV: Automatic Guided Vehicle) or a robot (e.g., an AMR: Autonomous Mobile Robot) that runs on the ground. Alternatively, the moving body 100 may be an air vehicle such as a drone or a UAV (Unmanned Aerial Vehicle). The moving body 100 moves autonomously along a route so as to avoid obstacles 9.

2 is a diagram showing a schematic hardware configuration of the moving body 100. The moving body 100 includes a main body 10, a driver 11, a communicator 12, a sensor 2, and a control device 3. The main body 10 is provided with the driver 11, the communicator 12, the sensor 2, and the control device 3. The driver 11 includes a plurality of wheels and an electric motor that drives the plurality of wheels. The communicator 12 performs wireless communication with external devices. The communicator 12 employs any wireless communication standard. The sensor 2 detects various values related to the movement of the moving body 100. For example, the sensor 2 includes at least one of a camera, a LiDAR (Light Detection And Ranging), an infrared sensor, a laser range finder, and a Doppler LiDAR. In addition, when the moving body 100 is an air vehicle, the driver 11 may include a plurality of propellers and an electric motor that drives the plurality of propellers.

The control device 3 plans a route for the moving body 100 and generates a route. The control device 3 is an example of a route planning device. When the moving body 100 moves in a two-dimensional space like a traveling vehicle, the control device 3 generates a two-dimensional route. Note that when the moving body 100 moves in a three-dimensional space like an aircraft, the control device 3 generates a three-dimensional route.

In this example, the control device 3 also controls the movement of the moving body 100 so that the moving body 100 moves along the route. The control device 3 controls the driver 11 to move the moving body 100 along the route. The detection results of the sensor 2 are input to the control device 3. The control device 3 performs route planning or movement control based on the detection results of the sensor 2. Furthermore, the moving body 100 communicates with a control tower or an external device via the communication device 12.

FIG. 3 is a diagram showing a schematic hardware configuration of the control device 3. The control device 3 has a controller 31, a storage device 32, and a memory 33. The control device 3 is an example of a computer.

The controller 31 controls the entire moving body 100. The controller 31 performs various types of arithmetic processing. For example, the controller 31 is formed of a processor such as a CPU (Central Processing Unit). The controller 31 may also be formed of an MCU (Micro Controller Unit), an MPU (Micro Processor Unit), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), a system LSI, etc.

The memory 32 stores the programs executed by the controller 31 and various data. For example, the memory 32 stores a route planning program 34. The memory 32 is formed of a non-volatile memory, a hard disc drive (HDD) or a solid state drive (SSD), etc. The memory 33 temporarily stores data, etc. For example, the memory 33 is formed of a volatile memory.

FIG. 4 is a functional block diagram of the controller 31. The controller 31 realizes various functions by reading out a program from the storage device 32 into the memory 33 and expanding it. Specifically, the controller 31 functions as an approximator 42 that approximates an obstacle to an approximate obstacle, an assigner 43 that assigns a Riemannian metric to the approximate obstacle, and a calculator 44 that determines a geodesic line and sets a route. Furthermore, the controller 31 may function as a setter 41 that sets the start point and end point of the route, and a movement controller 45 that controls the movement of the moving body 100.

Below, the functions of the controller 31 will be explained mainly using an example of planning a two-dimensional route. Figure 5 is an image diagram of a two-dimensional route.

The setting device 41 sets the start point 51 and end point 52 of the route. For example, the start point 51 and end point 52 are input from the outside and stored in the storage device 32 or memory 33.

The approximator 42 acquires obstacle information, i.e., the position and shape of the obstacle 9, based on the detection results of the sensor 2. The obstacle information may be input to the control device 3 from the outside and stored in the storage device 32 or memory 33. The approximator 42 approximates the obstacle 9 to an approximate obstacle having symmetry of spatial rotation or spatial translation. For example, the symmetry of spatial rotation is spherical symmetry or circular symmetry. The symmetry of spatial translation is cylindrical symmetry.

When planning a two-dimensional path, the approximator 42 approximates the obstacle 9 to a circular approximation obstacle 81 that contains the obstacle 9. More specifically, the approximator 42 approximates the obstacle 9 to a circular approximation obstacle 81 that circumscribes the obstacle 9. For example, if the obstacle 9 is a container, the approximator 42 sets the circle that circumscribes the container as the approximation obstacle 81.

On the other hand, when planning a three-dimensional path, the approximator 42 approximates the obstacle 9 to a spherical or cylindrical approximation obstacle that contains the obstacle 9. FIG. 6 is an explanatory diagram of a path of a mobile body 100 moving three-dimensionally. More specifically, the approximator 42 approximates the obstacle 9 to a spherical or cylindrical approximation obstacle that circumscribes the obstacle 9. For example, if the obstacle 9 is a bird, the approximator 42 sets a sphere that circumscribes the bird as the approximate obstacle 82. If the obstacle 9 is a utility pole, the approximator 42 sets a cylinder that circumscribes the utility pole as the approximate obstacle 83. The memory 32 stores the type of obstacle 9 and the shape of the approximate obstacle in association with each other as a database. In other words, whether the approximate obstacle is to be spherical or cylindrical is determined depending on the type of obstacle 9.

In addition, if it is necessary to take into account the width of the moving body 100, the approximator 42 expands the approximate obstacle outward by a margin corresponding to the width of the moving body 100. For example, if the path is two-dimensional, the approximator 42 approximates the obstacle 9 to an approximate obstacle 81 obtained by expanding a circle circumscribing the obstacle 9 in the radial direction (the width of the moving body 100/2). The same applies when the path is three-dimensional. However, if the width of the moving body 100 is sufficiently small, it is not necessary to consider the width of the moving body 100.

The assigner 43 finds the difference in size between the approximate obstacle and the obstacle 9. If the difference in size is equal to or smaller than a predetermined standard, the assigner 43 assigns a Riemannian metric based on the approximate obstacle. On the other hand, if the difference in size is greater than the predetermined standard, the assigner 43 assigns a Riemannian metric based on the actual obstacle 9. In more detail, in the case of a circular approximate obstacle 81, the assigner 43 finds the difference between the area of the circle of the approximate obstacle 81 and the area of the obstacle 9 as the difference in size. In the case of a spherical approximate obstacle 82, the assigner 43 finds the difference between the volume of the sphere of the approximate obstacle 82 and the volume of the obstacle 9 as the difference in size. In the case of a cylindrical approximate obstacle 83, the assigner 43 finds the difference between the volume of the cylinder of the approximate obstacle 83 and the volume of the obstacle 9 as the difference in size.

The assigner 43 assigns different Riemannian metrics depending on the difference in magnitude. Below, we will explain the case of planning a two-dimensional route.

If the difference in size is equal to or smaller than the criterion, the assigner 43 defines an obstacle coordinate system with the center of the approximate obstacle 81 as the origin, and assigns a Riemannian metric in the obstacle coordinate system. For example, in the case of an approximate obstacle 81 that has circular symmetry, the assigner 43 assigns a Riemannian metric as follows:

On the other hand, if the difference in size is greater than the reference value, the assigner 43 assigns a Riemannian metric based on the actual obstacle 9 as follows. Note that even in this case, the assigner 43 can set the size of the actual obstacle 9 taking into account a margin equivalent to the width of the moving body 100.

The calculator 44 solves the geodesic equation to obtain a geodesic line based on the start point 51 and end point 52 set by the setter 41 and the Riemannian metric given by the assigner 43, and sets a route based on the obtained geodesic line. The geodesic equation is expressed by the following equation (3).

Here, the velocity vector is defined as follows:

The calculator 44 solves the geodesic equation and obtains the coordinate x ⁱ as a function of the geodesic parameter s, thereby obtaining a path that avoids the obstacle 9. When the assigner 43 assigns the Riemann metric of equation (3), the calculator 44 transforms the geodesic equation as follows due to the circular symmetry of the Riemann space.

As shown in equation (5), the geodesic equation is reduced to a form in which the two variables (r, θ) are separated. This allows the calculator 44 to analytically solve the geodesic equation. The calculator 44 solves equation (5) to obtain the geodesic line.

On the other hand, when the Riemannian metric of equation (4) is assigned by the assigner 43, the calculator 44 solves the following geodesic equation to obtain a geodesic line.

The calculator 44 numerically solves equation (6).

The calculator 44 sets the calculated geodesic line as a route. The calculator 44 stores the route in the storage unit 32 or memory 33. When a route is calculated from a geodesic equation corresponding to an approximate obstacle, route 5 as shown in FIG. 5 is set. On the other hand, when a route is calculated from a geodesic circle equation corresponding to a real object, route 5 as shown in FIG. 7 is set. FIG. 7 is an image diagram of a two-dimensional route obtained based on a geodesic equation corresponding to a real obstacle.

The movement controller 45 reads out the path 5 from the storage device 32 or the memory 33. The movement controller 45 creates a command according to the path 5 and outputs the created command to the actuator 11. The actuator 11 operates according to the command, causing the moving body 100 to move along the path 5.

When planning a three-dimensional path, the assigner 43 assigns a three-dimensional Riemannian metric. For example, if the approximate obstacle 82 has spherical symmetry and the difference in size is equal to or smaller than a standard, the assigner 43 assigns a Riemannian metric as follows:

If the approximate obstacle 83 has cylindrical symmetry and the size difference is less than a criterion, the assigner 43 assigns the Riemannian metric as follows:

If the difference in magnitude is equal to or smaller than the reference value, the calculator 44 transforms the geodesic equation (3) of the Riemannian metric expressed by equation (7) or (8) into a form with separated variables, and analytically solves the geodesic equation to obtain the geodesic.

Next, the flow of route planning by the control device 3 will be explained with reference to FIG. 8. FIG. 8 is a flowchart of route planning.

First, in step S101, the setting device 41 sets the start point 51 and end point 52 of the route. The setting device 41 reads the start point 51 and end point 52 from the storage device 32 or memory 33 and sets them.

Next, in step S102, the approximator 42 obtains the position and shape of the obstacle 9.

In step S103, the approximator 42 approximates the obstacle 9 to an approximate obstacle. Step S103 corresponds to approximating the obstacle to an approximate obstacle that has symmetry of spatial rotation or spatial translation.

Next, in step S104, the assigner 43 determines whether the difference in size between the approximate obstacle and the actual obstacle 9 is equal to or smaller than a reference value.

If the difference in size is equal to or smaller than the reference value, the assigner 43 assigns a Riemann metric corresponding to the approximate obstacle in step S105. Then, the calculator 44 analytically solves the geodesic equation based on the Riemann metric to obtain a geodesic line in step S106. In step S107, the calculator 44 sets a route 5 based on the obtained geodesic line. Step S105 corresponds to assigning a Riemann metric to the approximate obstacle. Step S106 corresponds to calculating a geodesic line from the geodesic equation based on the Riemann metric, and setting a route based on the calculated geodesic line.

On the other hand, if the difference in size is greater than the reference value, the assigner 43 assigns a Riemannian metric corresponding to the actual obstacle 9 in step S108. Then, in step S109, the calculator 44 numerically solves the geodesic equation based on the Riemannian metric to obtain a geodesic line. In step S107, the calculator 44 sets the route 5 based on the obtained geodesic line.

In this way, the control device 3 approximates the obstacle 9 to an approximate obstacle having symmetry of spatial rotation or spatial translation, and by applying a Riemannian metric based on the approximate obstacle, it is possible to express the geodesic equation based on the Riemannian metric in a form with variables separated. This allows the control device 3 to analytically solve the geodesic equation, thereby reducing the computational load in route planning and shortening the computation time.

The control device 3 also determines the accuracy of the approximation of the obstacle 9, and if the accuracy of the approximation is high, it adopts a Riemannian metric based on the approximate obstacle. This allows the control device 3 to calculate a route that does not take unnecessary detours in a short time. On the other hand, if the accuracy of the approximation is low, it adopts a Riemannian metric based on the actual obstacle 9. In this case, the control device 3 takes time to calculate because it numerically solves the geodesic equation. However, the control device 3 can calculate a route that does not take too many detours around the obstacle 9.

Furthermore, in the case of a two-dimensional path, the approximate obstacle is circular, and in the case of a three-dimensional path, the approximate obstacle is spherical or cylindrical. In either case, the approximate obstacle has a simple shape, so the control device 3 can easily approximate the obstacle 9.

Other Embodiments
As described above, the above embodiment has been described as an example of the technology disclosed in this application. However, the technology in this disclosure is not limited to this, and can be applied to embodiments in which modifications, replacements, additions, omissions, etc. are appropriately performed. In addition, it is also possible to combine the components described in the above embodiment to form a new embodiment. In addition, among the components described in the attached drawings and detailed description, not only components essential for solving the problem but also components that are not essential for solving the problem in order to exemplify the technology may be included. Therefore, the fact that these non-essential components are described in the attached drawings and detailed description should not immediately be taken to mean that these non-essential components are essential.

For example, the path planning device may be a device separate from the control device 3. In other words, the path planning device does not have to be incorporated into the moving body 100. For example, the path planning device may be a device separate from the moving body 100. The path planning device may be located in a control tower, etc. The path planning device may generate a route and transmit the generated route or a command corresponding to the generated route to the moving body 100. Even if the path planning device is incorporated into the moving body 100, the path planning device may only plan the route of the moving body 100 and not control the movement of the moving body 100. Another device of the moving body 100 may control the movement of the moving body 100.

The moving body 100 may be any moving body that moves along a path. The moving body 100 may move in two dimensions or three dimensions.

The control device 3 may set the geodesic line calculated from the geodesic equation as the route as is, or may adjust the geodesic line to set it as the route.

The shape of the approximate obstacle can be any shape as long as it has symmetry in spatial rotation or translation. For example, in the case of a two-dimensional route, the approximate obstacle may be a circle that is larger by a specified margin than the circle circumscribing the obstacle 9. In the case of a three-dimensional route, the approximate obstacle may be a sphere or cylinder that is larger by a specified margin than the sphere or cylinder circumscribing the obstacle 9.

The Riemannian metric mentioned above is just one example. The Riemannian metric corresponding to approximate or real obstacles can be expressed in any form.

The control device 3 does not need to determine the difference in size between the approximated obstacle and the actual obstacle. The control device 3 may obtain a geodesic line using a geodesic line equation based on a Riemannian metric corresponding to the approximated obstacle, regardless of the difference in size, i.e., the accuracy of the approximation.

Furthermore, if the difference in size between the approximate obstacle and the actual obstacle is greater than a predetermined standard, the control device 3 does not need to calculate the path using a geodesic equation based on a Riemannian metric. In that case, the control device 3 can calculate the path using any known method.

The flowchart is merely an example. Steps in the flowchart may be modified, replaced, added, omitted, etc. as appropriate. The order of steps in the flowchart may also be changed, and serial processing may be performed in parallel.

The functions provided by the components described herein may be implemented in circuitry or processing circuitry, including general purpose processors, application specific processors, integrated circuits, ASICs (Application Specific Integrated Circuits), CPUs (Central Processing Units), conventional circuits, and/or combinations thereof, programmed to provide the functions described. Processors include transistors and other circuits and are considered to be circuits or processing circuits. Processors may be programmable processors that execute programs stored in memory.

In this specification, a circuit, unit, or means is hardware that is programmed to realize or executes the described functions. The hardware may be any hardware disclosed in this specification or any hardware known to be programmed to realize or execute the described functions.

If the hardware is a processor that is considered to be a type of circuitry, the circuit, means, or unit is a combination of the hardware and software used to configure the hardware and/or the processor.

The technology disclosed herein can be summarized as follows:

[1] The control device 3 (path planning device) that plans the path of the moving body 100 includes an approximator 42 that approximates an obstacle 9 to an approximate obstacle having symmetry of spatial rotation or spatial translation, an assigner 43 that assigns a Riemannian metric to the approximate obstacle, and a calculator 44 that calculates a geodesic line from a geodesic equation based on the Riemannian metric and sets a path based on the calculated geodesic line.

With this configuration, the obstacle 9 is approximated to an approximate obstacle having symmetry of spatial rotation or spatial translation. The geodesic equation based on the Riemannian metric corresponding to the approximate obstacle can be reduced to a form with separated variables. This allows the geodesic equation to be solved analytically. As a result, the calculation time can be reduced in path planning using the geodesic equation.

[2] In the control device 3 described in [1], the assigner 43 assigns the Riemannian metric of the approximate obstacle when the difference in size between the approximate obstacle and the obstacle 9 is equal to or smaller than a predetermined criterion.

With this configuration, if the accuracy of the approximation of the approximate obstacle is high, a Riemannian metric corresponding to the approximate obstacle is assigned. As a result, it is possible to reduce the calculation time while preventing the vehicle from going too far around the actual obstacle 9.

[3] In the control device 3 described in [1] or [2], the approximate obstacle is spherical, cylindrical, or circular.

With this configuration, the shape of the approximate obstacle is simple, so obstacle 9 can be easily approximated to the approximate obstacle.

[4] A route planning method for planning a route for a moving body 100 includes approximating an obstacle 9 to an approximate obstacle having symmetry of spatial rotation or spatial translation, assigning a Riemannian metric to the approximate obstacle, calculating a geodesic line from a geodesic equation based on the Riemannian metric, and setting a route based on the calculated geodesic line.

With this configuration, the obstacle 9 is approximated to an approximate obstacle having symmetry of spatial rotation or spatial translation. The geodesic equation based on the Riemannian metric corresponding to the approximate obstacle can be reduced to a form with separated variables. This allows the geodesic equation to be solved analytically. As a result, the calculation time can be reduced in path planning using the geodesic equation.

[5] A path planning program for causing a computer to plan a path for a moving body 100 causes the computer to realize the following functions: approximating an obstacle 9 to an approximate obstacle having symmetry in spatial rotation or spatial translation; assigning a Riemannian metric to the approximate obstacle; calculating a geodesic line from a geodesic equation based on the Riemannian metric; and setting a path based on the calculated geodesic line.

With this configuration, the obstacle 9 is approximated to an approximate obstacle having symmetry of spatial rotation or spatial translation. The geodesic equation based on the Riemannian metric corresponding to the approximate obstacle can be reduced to a form with separated variables. This allows the geodesic equation to be solved analytically. As a result, the calculation time can be reduced in path planning using the geodesic equation.

100 Mobile object 3 Control device (route planning device, computer)
42 Approximator 43 Adder 44 Calculator

Claims (5)

A route planning device that plans a route for a moving object,
An approximator that approximates an obstacle to an approximate obstacle having symmetry of spatial rotation or spatial translation;
an applicator for applicating a Riemannian metric of the approximate obstacle;
a calculator that calculates a geodesic line from a geodesic equation based on the Riemannian metric and sets a route based on the calculated geodesic line. The route planning device according to claim 1 ,
The assigner is a path planning device that assigns the Riemannian metric of the approximate obstacle when a difference in size between the approximate obstacle and the obstacle is equal to or smaller than a predetermined standard. 3. The route planning device according to claim 1,
The approximate obstacle is a sphere, a cylinder, or a circle. A route planning method for planning a route of a moving object, comprising:
Approximating the obstacle to an approximation obstacle having symmetry of spatial rotation or spatial translation;
providing a Riemannian metric for the approximate obstacle;
calculating a geodesic curve from a geodesic equation based on the Riemannian metric, and setting a route based on the calculated geodesic curve. A route planning program for causing a computer to realize a route plan for a moving object, comprising:
A function of approximating an obstacle to an approximate obstacle having symmetry of spatial rotation or spatial translation;
a function for providing a Riemannian metric of the approximate obstacle;
A route planning program for causing a computer to realize a function of calculating a geodesic line from the geodesic equation based on the Riemannian metric, and setting a route based on the calculated geodesic line.

Images