Route planning device, driving assistance system, route planning method, and route planning program

Abstract

A route planning device 100 comprises: an acquisition unit 45 for acquiring obstacle information including information relating to the position of an obstacle; and a generation unit 46 that generates a target route for a moving body 1 from a start point S to a target point G on the basis of the obstacle information so as to avoid interference between the moving body 1 and the obstacle. The generation unit 46 searches for a plurality of candidates for the target route and determines the target route from among the plurality of candidates on the basis of route costs. The route costs include a meandering cost relating to the degree of meandering of each of the plurality of candidates.

Description

Route planning device, driving assistance system, route planning method, and route planning program

[0001] The technology disclosed herein relates to a route planning device, a driving assistance system, a route planning method, and a route planning program.

[0002] For example, in the route planning disclosed in Patent Literature 1, a route is planned that takes into account the movements of other moving objects and avoids interference with the other moving objects.

[0003] Japanese Patent Application Laid-Open No. 2020-4095

[0004] The route planning disclosed in Patent Document 1 ensures safety by planning a route that avoids interference with other moving objects. However, there is room for further improvement in terms of risk avoidance.

[0005] The technology disclosed herein has been made in consideration of the above points, and its purpose is to plan a route that can reduce risks.

[0006] The route planning device disclosed herein includes an acquirer that acquires obstacle information including information about the position of an obstacle, and a generator that generates a target route for a moving body from a start point to a target point based on the obstacle information so as to avoid interference between the moving body and the obstacle, the generator searches for multiple candidates for the target route and determines the target route from among the multiple candidates based on route costs, the route costs including a meandering cost related to the degree of meandering of each of the multiple candidates.

[0007] The driving assistance system disclosed herein includes the route planning device and a display mounted on a mobile body that performs automatic driving and displays the target route from the route planning device.

[0008] The route planning method disclosed herein includes acquiring obstacle information including information regarding the position of an obstacle, and generating a target route for a moving body from a start point to a target point based on the obstacle information so as to avoid interference between the moving body and the obstacle, wherein generating the target route includes searching for multiple candidates for the target route and determining the target route from among the multiple candidates based on route costs, and the route costs include a meandering cost related to the degree of meandering of each of the multiple candidates.

[0009] The route planning program disclosed herein enables a computer to perform the following functions: acquire obstacle information including information about the location of obstacles; and generate a target route for a moving body from a starting point to a target point based on the obstacle information so as to avoid interference between the moving body and the obstacles. The target route generating function searches for multiple candidates for the target route and determines the target route from among the multiple candidates based on route costs, and the route costs include a meandering cost related to the degree of meandering of each of the multiple candidates.

[0010] The route planning device makes it possible to plan a route that reduces risk.

[0011] The driving assistance system makes it possible to plan a route that reduces risk.

[0012] According to the route planning method, a route that can reduce risks can be planned.

[0013] According to the route planning program, a route that can reduce risks can be planned.

[0014] FIG. 1 is a schematic plan view showing a mobile body navigating the ocean. FIG. 2 is a diagram showing a schematic hardware configuration of a mobile body. FIG. 3 is a diagram showing the hardware configuration of a control device. FIG. 4 is a functional block diagram of a processor. FIG. 5 is a diagram showing the hardware configuration of a path planning device. FIG. 6 is a functional block diagram of a processor. FIG. 7 is an example of a search space. FIG. 8 is a schematic diagram showing multiple candidates searched for in the search space. FIG. 9 is an explanatory diagram for explaining the search for a new node. FIG. 10 is an explanatory diagram showing the positional relationship between one node and a new node in the global coordinate system of the mobile body. FIG. 11 is a two-dimensional schematic diagram of multiple candidates searched for. FIG. 12 is a flowchart of path planning. FIG. 13 is a flowchart of autonomous driving. FIG. 14 is an example of an image displayed by a display.

[0015] An exemplary embodiment will be described in detail below with reference to the drawings. FIG. 1 is a schematic plan view showing a mobile body 1 navigating the ocean. The mobile body 1 is autonomously driven. A driving assistance system 1000 includes a path planning device 100 that generates a target route for the mobile body 1 and a display 15 that is mounted on the autonomously driven mobile body 1 and displays the target route from the path planning device 100. In this example, the mobile body 1 is a ship. Multiple mobile bodies 1 exist inside and outside the port shown in FIG. 1. The multiple mobile bodies 1 are distinguished from one another by alphabetical subscripts following the reference number "1." For example, the multiple mobile bodies 1 in the sea area shown in FIG. 1 include mobile body 1A, mobile body 1B, mobile body 1C, and mobile body 1D. Note that when the multiple mobile bodies 1 are not distinguished from one another, they will simply be referred to as "mobile body 1." The port shown in FIG. 1 includes obstacles such as a quay 91 and a breakwater 92. Furthermore, from one mobile body 1, other mobile bodies 1 are obstacles.

[0016] The route planning device 100 generates a target route for a mobile body 1 that performs autonomous driving. When multiple mobile bodies 1 perform autonomous driving, the route planning device 100 generates an individual target route for each mobile body 1. The route planning device 100 is located in a control tower 120. The route planning device 100 communicates with a target mobile body 1 and transmits a target route to the mobile body 1. The target mobile body 1 performs autonomous driving according to the target route. Below, a case where a target route for a mobile body 1A to a target point G is generated will be described.

[0017] The display 15 displays the target route sent from the route planning device 100. That is, the display 15 presents the target route for autonomous driving to the user, i.e., the occupant. Furthermore, the display 15 may display multiple target routes and allow the user to select a target route to be used for autonomous driving.

[0018] -Mobile object- The mobile object 1 performs automatic driving, more specifically, autonomous driving. The mobile object 1 may be capable of switching between manual driving and automatic driving. FIG. 2 is a diagram showing a schematic hardware configuration of the mobile object 1. The mobile object 1 has a monitoring sensor 11, an actuator 12, a communication device 13, a position detector 14, a display device 15, an input device 16, and a control device 2.

[0019] The monitoring sensor 11 acquires monitoring information of a predetermined range within the environment in which the mobile object 1 moves. The monitoring information may include topography, buildings, features, facilities, or mobile objects. The monitoring sensor 11 includes at least one of a camera, a LiDAR (Light Detection and Ranging), an infrared sensor, a laser rangefinder, and a Doppler LiDAR. For example, the camera captures still images or videos. The laser rangefinder may employ a green laser. In this example, the monitoring sensor 11 is a LiDAR. The monitoring sensor 11 acquires point cloud data of objects within a predetermined monitoring area as monitoring information.

[0020] The actuator 12 is a drive source for the mobile body 1. The actuator 12 may include a first actuator 12A and a second actuator 12B. For example, if the mobile body 1 is a ship, the first actuator 12A is an engine or an electric motor that drives a propeller, and the second actuator 12B is an engine or an electric motor that drives a rudder.

[0021] The communicator 13 performs wireless communication with an external device. For example, the communicator 13 communicates with the route planning device 100. The position detector 14 detects the position of the mobile object 1. For example, the position detector 14 is a Global Navigation Satellite System (GNSS) receiver or an Inertial Measurement Unit (IMU).

[0022] The display 15 is, for example, a liquid crystal display or an organic EL display. The display 15 can display a plurality of target routes. That is, the display 15 presents the user with options for the target routes.

[0023] The input device 16 is operated by a user, i.e., a device through which the user inputs operations. The input device 16 is, for example, a keyboard, a mouse, or a button. The display device 15 and the input device 16 may be integrally configured. For example, the display device 15 and the input device 16 may be a touch panel.

[0024] 3 is a diagram showing the hardware configuration of the control device 2. The control device 2 controls the entire mobile object 1. The control device 2 controls the automatic driving of the mobile object 1. In other words, the control device 2 operates the actuator 12 to move the mobile object 1. The control device 2 has a processor 21, a storage device 22, and a memory 23.

[0025] The processor 21 performs various types of arithmetic processing. For example, the processor 21 is formed of a processor such as a CPU (Central Processing Unit). The processor 21 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, or the like.

[0026] The storage unit 22 stores programs and various data to be executed by the processor 21. For example, the storage unit 22 stores a control program. The storage unit 22 is formed of a non-volatile memory, a hard disk drive (HDD), a solid state drive (SSD), or the like. The memory 23 temporarily stores data and the like. For example, the memory 23 is formed of a volatile memory.

[0027] The processor 21 operates the actuator 12, causing the moving body 1 to drive autonomously. The processor 21 receives a target route from the path planning device 100. The processor 21 operates the actuator 12 based on the detection results from the monitoring sensor 11 and the position detector 14 so that the moving body 1 moves along the target route. The processor 21 may cause the moving body 1 to perform a collision avoidance operation as necessary while the moving body 1 is moving along the target route. The processor 21 may transmit the detection results from the monitoring sensor 11 and the position detector 14 to the path planning device 100 to generate the target route.

[0028] FIG. 4 is a functional block diagram of the processor 21. The processor 21 realizes various functions by reading a control program from the storage device 22 into the memory 23 and expanding the program. Specifically, the processor 21 functions as a state acquirer 24 that acquires the state of the host moving body 1 and the state of the other moving body 1, and a calculator 25 that calculates a command operation amount for the actuator 12. The calculator 25 calculates a command operation amount for the actuator 12 based on the state of the host moving body 1 and the state of the other moving body 1 acquired by the state acquirer 24. In this example, the processor 21 further functions as a state recognizer 26 that recognizes the state of the host moving body 1 and the state of the other moving body 1. In addition, the processor 21 further functions as an image generator 211 that generates an image to be displayed on the display device 15, and a receiver 212 that receives a user operation input from the input device 16.

[0029] The image generator 211 generates an image and outputs the generated image to the display 15. The image generator 211 generates an image of the target route from the route planning device 100. For example, the image generator 211 generates a route chart showing the target route. When multiple target routes are sent from the route planning device 100, the image generator 211 generates images of the multiple target routes. When priorities are assigned to the multiple target routes, the image generator 211 also displays the priorities of the multiple target routes. There are various modes for displaying the priorities. For example, the image generator 211 may display the priority of each target route, i.e., a numerical value, associated with each target route. Alternatively, the image generator 211 may represent the priority of each target route by using a color for each target route. In other words, the image generator 211 may color-code multiple target routes according to their priority. Alternatively, the image generator 211 may represent the priority of each target route by using a line type for each target route.

[0030] The image generator 211 may display the priorities of only some of the multiple target routes, rather than all of them. For example, when three target routes are displayed, the image generator 211 may display the target route with the highest priority as marked with "1st place," and may not mark the target routes with the second and third priorities. In other words, the image generator 211 may display the target route with the highest priority as distinguished from the target routes with priorities other than the first, for example, the second and third priorities. The second and third target routes do not need to be displayed as distinguished from each other. The first target route may be distinguished from the target routes other than the first by the color or line type of the target route, etc.

[0031] In addition, when a route cost (described later) is sent from the route planning device 100 together with the target route, the image generator 211 may display the corresponding route cost together with the target route. The image generator 211 may also display the route cost as a priority. In other words, the smaller the route cost, the lower the priority.

[0032] The acceptor 212 accepts a user's selection of one target route from among multiple target routes. The user can select one target route to be used for automated driving from the multiple target routes displayed on the display 15 by operating the input device 16. When the acceptor 212 accepts the user's selection input, it sets the selected target route as the target route to be used for automated driving.

[0033] The state acquirer 24 realizes a function of acquiring the state of the own moving body 1 and a function of acquiring the state of the other moving body 1. The state acquirer 24 may further acquire the state of the environment in addition to the state of the own moving body 1 and the state of the other moving body 1. The state acquirer 24 acquires the state of the own moving body 1, the state of the other moving body 1, and the state of the environment from the state recognizer 26.

[0034] The state recognizer 26 realizes a function of determining the state of the own moving body 1 and a function of determining the state of the other moving body 1. The state recognizer 26 outputs the state of the own moving body 1 and the state of the other moving body 1 to the state acquirer 24. In addition to the state of the own moving body 1 and the state of the other moving body 1, the state recognizer 26 may also determine the state of the environment and output it to the state acquirer 24.

[0035] The state recognizer 26 receives map information, information about the other moving body 1, the detection results of the monitoring sensor 11, and the detection results of the position detector 14. The map information and information about the other moving body 1 are transmitted, for example, from the route planning device 100 or the control tower 120 and stored in the memory 22. The map information includes at least one of roads, passageways, waterways, and features (buildings, trees, rocks, and other natural and man-made objects on land or sea). The map information may be information about general maps such as road maps, facility maps, and route maps. The map information may also include information about obstacles. The information about obstacles includes at least one of the position, speed, and type of the obstacle. Obstacles may include features, buildings, moving bodies other than the own moving body 1 (i.e., other moving bodies 1), construction sites, rough terrain, shallow waters, aquaculture facilities, anchorages, etc. The information about the other moving bodies includes, for example, the type of the other moving body.

[0036] For example, the state recognizer 26 estimates the state of the host moving body 1 using a Kalman filter. The state recognizer 26 may also estimate its own position using SLAM (Simultaneous Localization and Mapping) technology. The state recognizer 26 may estimate the size of the other moving body 1 or the relative speed of the other moving body 1 with respect to the host moving body 1 based on the detection result of the position detector 14. The state recognizer 26 may also estimate the state of an obstacle in the environment. The state recognizer 26 outputs information about the host moving body 1, such as the current position and speed of the host moving body 1, information about the other moving body, such as the relative position and speed of the other moving body 1 with respect to the host moving body 1, information about the obstacle, such as the relative position and speed of the obstacle with respect to the host moving body 1, and information about disturbances.

[0037] In other words, the state acquirer 24 acquires map information including the location of the obstacle, information about the own moving body 1 such as the current position and speed of the own moving body 1, information about other moving bodies 1 such as the relative position and speed of other moving bodies 1 with respect to the own moving body 1, information about the obstacle such as the relative position and speed of the obstacle with respect to the own moving body 1, and information about disturbances.

[0038] The calculator 25 realizes the function of generating a target trajectory for the moving body 1. The target trajectory is a relatively short-term target trajectory from the current position. The calculator 25 further realizes the function of calculating a control input for the actuator 12 based on the target trajectory. In detail, the calculator 25 has a path acquirer 27 that acquires a target path for the moving body 1, a trajectory generator 28 that generates a target trajectory for the moving body 1, a trajectory corrector 29 that corrects the target trajectory, and an operation amount calculator 210 that calculates the operation amount of the actuator 12 that follows the target trajectory.

[0039] The route acquirer 27 receives the target route. The route acquirer 27 receives the target route from the route planning device 100. In this example, the user selects one target route from the multiple target routes output from the route planning device 100, and the route acquirer 27 receives the selected target route from the acceptor 212.

[0040] The trajectory generator 28 generates a target trajectory from the current position of the mobile body 1 to a waypoint. A waypoint is a target position that is relatively close to the current position. The trajectory generator 28 receives the target route from the route acquirer 27, and receives map information and information related to the mobile body 1 from the state acquirer 24. The trajectory generator 28 sets waypoints based on the target route. The trajectory generator 28 generates the target trajectory of the mobile body 1 using a predetermined method (e.g., line-of-sight guidance law).

[0041] The trajectory corrector 29 corrects the target trajectory generated by the trajectory generator 28 as necessary. For example, the trajectory corrector 29 corrects the target trajectory so that the host moving body 1 avoids obstacles. The trajectory corrector 29 receives map information, information about the host moving body 1, information about the other moving body 1, and disturbance information from the state acquirer 24. For example, the trajectory corrector 29 corrects the target trajectory by nonlinear model predictive control (NMPC). As a result, a target trajectory that avoids obstacles while following the target trajectory generated by the trajectory generator 28 as closely as possible is generated. The trajectory corrector 29 outputs a target position and a target speed as the target trajectory.

[0042] The manipulated variable calculator 210 calculates the manipulated variable of the actuator 12, i.e., the control input, corresponding to the target trajectory. The manipulated variable calculator 210 calculates a command force for causing the host moving body 1 to follow the target trajectory. Map information, information about the host moving body 1, information about the other moving body 1, and disturbance information are input to the manipulated variable calculator 210 from the state acquirer 24. The manipulated variable calculator 210 performs, for example, PID control. Note that the control performed by the manipulated variable calculator 210 is not limited to PID control, and may be robust control or the like. In this case, the manipulated variable calculator 210 may limit the command speed corresponding to the target trajectory using, for example, a control barrier function (CBF).

[0043] Furthermore, the manipulated variable calculator 210 distributes the command force to the multiple actuators 12 and calculates the command manipulated variable for each of the multiple actuators 12. If the actuator 12 is an electric motor, the manipulated variable is, for example, the rotation speed or torque of the electric motor. If the actuator 12 is an engine, the manipulated variable is, for example, the fuel injection amount.

[0044] Each actuator 12 operates in accordance with a command operation amount. The actuator 12 may be provided with its own controller for operating the actuator 12. For example, if the actuator 12 is an electric motor, the actuator 12 further includes a servo amplifier. In this case, the servo amplifier operates the electric motor in accordance with the command operation amount. As a result, the moving body 1 exerts thrust and moves toward the target position.

[0045] The route planning device 100 generates a target route for a moving body 1. The route planning device 100 can generate target routes for multiple moving bodies 1. In other words, any moving body 1 that has an autonomous driving function and can receive a target route from the route planning device 100 can be a target of route planning. For example, the route planning device 100 receives a request from the moving body 1, generates a target route, and transmits it.

[0046] The path planning device 100 generates a target path that avoids interference with obstacles. The path planning device 100 can acquire obstacle information from a monitoring device 3 that detects obstacles. In particular, the path planning device 100 can acquire information about moving obstacles from the monitoring device 3. Note that the path planning device 100 may store in advance information about fixed obstacles such as features, buildings, construction sites, uneven ground, shallow water, and fish farms.

[0047] The monitoring devices 3 include fixed monitoring devices 3 and mobile monitoring devices 3. For example, when multiple monitoring devices 3 are distinguished from one another, an alphabetic suffix is ​​added after the reference number "3" (see FIG. 1). In this example, monitoring devices 3A, 3B, 3C, and 3D are fixed monitoring devices. Monitoring device 3E is a mobile monitoring device. When monitoring devices 3A, 3B, 3C, 3D, and 3E are not distinguished from one another, they are simply referred to as "monitoring devices 3." The monitoring devices 3 include monitoring sensors that acquire monitoring information within a predetermined range. The monitoring information may include topography, buildings, features, facilities, moving objects, wind direction, tidal currents, and the like. The monitoring sensors include at least one of a camera, a light detection and ranging (LiDAR), an infrared sensor, a laser rangefinder, a Doppler LiDAR, and an anemometer. For example, the camera captures still images or video. The laser rangefinder may employ a green laser.

[0048] The fixed monitoring device 3 is fixedly installed in the environment. The environment refers to the surrounding environment in which the mobile body 1 can move. The fixed monitoring device 3 is installed, for example, on a building, a street light, or a telephone pole. Alternatively, the fixed monitoring device 3 is installed on a dedicated structure such as a tower. The monitoring range of the fixed monitoring device 3 may be fixed or may be changeable. In this example, the fixed monitoring device 3 is configured so that its orientation can be changed 360 degrees. In other words, the fixed monitoring device 3 can change its monitoring range over time and acquire monitoring information for a 360-degree area around it.

[0049] The mobile monitoring device 3 is a moving object such as a flying device or a vehicle. For example, the mobile monitoring device 3 is a drone as a flying device. The mobile monitoring device 3 is equipped with the above-mentioned monitoring sensor and can fly freely within the environment to obtain monitoring information of any area in the environment.

[0050] The monitoring device 3 communicates with the route planning device 100 via a communication network or the like. The monitoring device 3 transmits the monitoring information detection result to the route planning device 100.

[0051] The path planning device 100 may acquire obstacle information by receiving, from the mobile object 1, a detection result of a monitoring sensor 11 provided in the mobile object 1. The mobile object 1 equipped with the monitoring sensor 11 monitors obstacles around the mobile object 1 using the monitoring sensor 11. The path planning device 100 acquires obstacle information around the mobile object 1 by receiving the detection result of the monitoring sensor 11 of the mobile object 1.

[0052] The route planning device 100 may acquire information about a moving body 1 from a moving body 1 as obstacle information for another moving body 1. For one moving body 1, another moving body 1 can be an obstacle. Therefore, the route planning device 100 uses information about the moving body 1 itself acquired from the moving body 1 as obstacle information for the other moving body 1.

[0053] The obstacle information received from the moving body 1 includes at least one of the position, speed, and type of the moving body 1. When the moving body 1 estimates the state of the moving body 1, the obstacle information received from the moving body 1 may include a state estimation value estimated by the moving body 1. In detail, the obstacle information may include information about the moving body 1, such as the current position or speed of the moving body 1, information about other moving bodies, such as the relative position or speed of the other moving bodies 1 with respect to the moving body 1, information about obstacles, such as the relative position or speed of the obstacle with respect to the moving body 1, or information about disturbances.

[0054] 5 is a diagram showing the hardware configuration of the route planning device 100. The route planning device 100 includes a processor 41, a storage device 42, a memory 43, and a communication device 44.

[0055] The processor 41 generates a target route for the moving object 1 based on the obstacle information. The processor 41 performs various types of arithmetic processing. For example, the processor 41 is formed of a processor such as a CPU (Central Processing Unit). The processor 41 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, or the like.

[0056] The storage unit 42 stores various data and programs executed by the processor 41. For example, the storage unit 42 stores a route planning program 42a. The storage unit 42 is formed of a non-volatile memory, a hard disk drive (HDD), a solid state drive (SSD), or the like. The memory 43 temporarily stores data and the like. For example, the memory 43 is formed of a volatile memory.

[0057] The communication device 44 performs wireless communication with an external device, for example, the mobile object 1.

[0058] 6 is a functional block diagram of the processor 41. The processor 41 realizes various functions by reading out the path planning program 42a from the storage unit 42 into the memory 43 and expanding it. Specifically, the processor 41 functions as an acquirer 45 that acquires obstacle information including information about the position of an obstacle, and a generator 46 that generates a target path for the mobile object 1 from the start point S to the target point G. The generator 46 generates the target path based on the obstacle information so as to avoid interference between the mobile object 1 and the obstacle.

[0059] The obstacle information includes at least one of the position, speed, and type of the obstacle. Obstacles may include features, buildings, moving bodies, construction sites, rough terrain, shallow water, fish farms, etc. Since the path planning device 100 generates a target path for each moving body 1 performing autonomous driving, one moving body 1 may become an obstacle for other moving bodies 1. The information about the moving body includes, for example, the type of the moving body. Examples of the type of the moving body include ships, vehicles, and aircraft. Furthermore, the type of the moving body may include detailed types such as large ships, medium-sized ships, and small ships.

[0060] Information on features, buildings, construction sites, uneven ground, shallow water, and fish farms is stored in the memory 42 or an external database. The acquirer 45 acquires this information from the memory 42 or the database.

[0061] The acquirer 45 may acquire information about the moving body 1 from the monitoring sensor 11. The acquirer 45 may also acquire information about the moving body 1 from the moving body 1. The acquirer 45 may acquire information about the moving body 1 itself from the moving body 1 as obstacle information. If the moving body 1 has a function to detect other moving bodies 1, the acquirer 45 may acquire information about the other moving bodies 1 from the moving body 1.

[0062] The acquirer 45 sequentially acquires obstacle information. Specifically, the monitoring device 3 transmits the monitoring information to the route planning device 100 at an appropriate timing. The moving body 1 transmits the monitoring information to the route planning device 100 at an appropriate timing. The acquirer 45 receives the transmitted monitoring information and stores it in the memory 42. The acquired obstacle information is accumulated in the memory 42. In this way, the acquirer 45 acquires changes in the obstacle information over time.

[0063] The generator 46 generates a target route for the moving body 1 from a start point S to a target point G. The generator 46 searches for multiple candidate target routes that avoid collisions between the moving body 1 and obstacles, and determines a target route from among the multiple candidates based on the route cost. The generator 46 predicts changes in the position of the obstacle over time, and searches for multiple candidates that avoid collisions between the moving body 1 and the obstacles. The generator 46 sequentially updates the search for the multiple candidates and the determination of the target route. For example, the start point S is the current position of the moving body 1. The target point G is received by the path planning device 100 from the moving body 1.

[0064] In detail, the generator 46 includes a predictor 47 that predicts changes in the position of an obstacle over time, a space generator 48 that generates a search space, which is the space when performing a route search, and a searcher 49 that searches for multiple candidates for the target route in the search space and determines the target route from among the multiple candidates.

[0065] The predictor 47 predicts the change in the position of the obstacle over time based on the obstacle information. More specifically, the predictor 47 predicts the change in the position of the obstacle over time based on the obstacle information acquired by the acquirer 45. The predictor 47 determines a predicted obstacle area in which the obstacle may exist in the future. If the obstacle is a stationary object, its position does not change. If the obstacle is a stationary object, the position of the obstacle predicted by the predictor 47 remains constant over time. In this example, the position of the obstacle is the two-dimensional position of the obstacle in the two-dimensional space in which the mobile object 1 moves. In other words, the position of the obstacle is the two-dimensional position of the obstacle in the plane in which the mobile object 1 moves, and does not include the height position.

[0066] The prediction area is an area where an obstacle exists or is likely to exist in the future. The predictor 47 calculates a continuous or discrete prediction area according to time. The prediction area at each time is a two-dimensional area in the two-dimensional space in which the moving object 1 moves. The prediction area has a size that encompasses the planar shape of the obstacle, i.e., the two-dimensional shape. The prediction area is expressed, for example, as a circle, an ellipse, or a polygon.

[0067] The predictor 47 may predict the behavior of an obstacle over time based on the obstacle information and determine a predicted obstacle area in which the obstacle may exist. Obstacle behavior prediction is performed using various known techniques. For example, if the obstacle information includes the obstacle's speed, the predictor 47 may predict the obstacle's movement range assuming that the obstacle moves at a constant speed. Alternatively, the predictor 47 may calculate the obstacle's speed based on changes in the obstacle information over time and predict the movement range of the obstacle moving at the calculated constant speed. Alternatively, the predictor 47 may assume the predicted trajectory of the obstacle using a motion model and calculate a predicted distribution using a Kalman filter, a Bayesian filter, or the like. In this case, the predictor 47 estimates various parameters of the motion model based on the type and shape of the obstacle. Alternatively, the predictor 47 may estimate the motion model or input distribution using data-driven control. The predictor 47 may estimate a probabilistic motion model and input distribution using machine learning (e.g., Gaussian process or Bayesian estimation) using past obstacle information. The predictor 47 may calculate the predicted distribution of obstacles by a sampling method such as the Markov Chain Monte Carlo (MCMC) method. Alternatively, the predictor 47 may approximate the predicted distribution of obstacles by moment matching or the like.

[0068] FIG. 7 is an example of a search space. The search space is a three-dimensional space in which the dimension of time is added to the two-dimensional space in which the moving object 1 moves. In FIG. 7, the XY plane is the two-dimensional space in which the moving object 1 moves. The Z axis is the time axis. In other words, the larger the Z coordinate, the more time has passed. The start point S is located in the layer with the earliest time along the time axis. The target point G is located in the layer with the most advanced time along the time axis. In this example, the target point G is represented by an area with a predetermined area.

[0069] The space generator 48 creates a three-dimensional obstacle region 61 in the search space, which includes a predicted future position of the obstacle. The space generator 48 creates the obstacle region 61 based on the prediction region obtained by the predictor 47. The prediction region for each time is a two-dimensional region. The space generator 48 arranges the two-dimensional prediction regions for each time in the search space along the time axis and continuously connects these prediction regions to create a three-dimensional region. This three-dimensional region is the obstacle region 61. In FIG. 7, five obstacle regions, namely, an obstacle region 61A, an obstacle region 61B, an obstacle region 61C, an obstacle region 61D, and an obstacle region 61E, have been created. Note that when there is no need to distinguish between them, they will simply be referred to as "obstacle regions 61."

[0070] Obstacle region 61A and obstacle region 61B correspond to stationary obstacles. For example, obstacle region 61A corresponds to quay wall 91. Obstacle region 61B corresponds to breakwater 92. Obstacle region 61A and obstacle region 61B corresponding to stationary obstacles are regions in which the planar shapes of the obstacles simply extend in the time axis direction. In other words, obstacle region 61A and obstacle region 61B have shapes parallel to the time axis. Obstacle region 61C, obstacle region 61D, and obstacle region 61E correspond to moving obstacles. For example, obstacle region 61C, obstacle region 61D, and obstacle region 61E correspond to other moving bodies 1, respectively. Obstacle region 61C, obstacle region 61D, and obstacle region 61E corresponding to moving obstacles are regions inclined linearly or curvedly with respect to the time axis. When the obstacle moves at a constant speed, the obstacle regions 61C, 61D, and 61E are inclined linearly with respect to the time axis. When the obstacle moves at a non-constant speed, the obstacle regions 61C, 61D, and 61E are inclined curvedly with respect to the time axis.

[0071] The searcher 49 searches for multiple candidates for the target path that do not interfere with the obstacle region 61 in the search space. That is, the searcher 49 searches for multiple candidates in the search space while avoiding the obstacle region 61. Because obstacles are regarded as stationary solids in the search space, a path is searched for without considering the behavior of the obstacles over time. The searcher 49 uses the RRT-star algorithm to search for multiple candidates. The configuration of each candidate depends on the search method. For example, each candidate includes multiple nodes.

[0072] Because the search space has a time axis, the searcher 49 searches for a new node on the side where time elapses along the time axis when searching for a route. Furthermore, because the moving object 1 has a maximum speed, the range A within which the moving object 1 can move per unit time (hereinafter referred to as the "movable range") is limited. In a three-dimensional search space, the movable range A is a cone-shaped area (inverted cone-shaped in FIG. 7 ) with the starting point S as its apex and expanding toward the side where time advances along the time axis. The inclination of the generatrix of the cone with respect to the time axis corresponds to the maximum speed of the moving object 1. The searcher 49 searches for multiple candidates within the movable range A. Furthermore, when searching for a new node connected to a given node, the inclination of the edge connecting two nodes with respect to the time axis is limited by the maximum speed of the moving object 1. In other words, a new node connected to a given node is searched for under the condition that the inclination of the edge with respect to the time axis is equal to or less than a predetermined threshold. The threshold corresponds to the maximum speed of the moving object 1.

[0073] Fig. 8 is a schematic diagram showing multiple candidates searched for in a search space. The search space in Fig. 8 is the same as that in Fig. 7. The searcher 49 searches for a new node that does not interfere with the obstacle area 61 in the direction in which time advances from the start point S, and generates a path that reaches the target point G as a candidate target path. In the example in Fig. 8, the searcher 49 generates three candidates R1, R2, and R3.

[0074] At this time, the searcher 49 searches for multiple candidates based on the nonholonomic characteristics of the moving body 1. For example, the searcher 49 searches for a route under the condition that the moving body 1 does not skid. Specifically, each candidate includes multiple nodes, and the node is defined by the position and attitude (angle) of the moving body 1. When setting a new node to be connected to a node, the searcher 49 determines the attitude of the moving body 1 at the new node by assuming that the moving body 1 moves in an arc from the first node to the new node. The position of the moving body 1 that defines the node is searched in a three-dimensional search space including a time axis, while the attitude of the moving body 1 that defines the node is searched for in a two-dimensional space in which the moving body 1 moves.

[0075] FIG. 9 is an explanatory diagram for explaining the search for a new node. FIG. 9 shows the two-dimensional space in which the mobile object 1 moves. It is assumed that the turning radius during movement between nodes is constant. The coordinates of the already set node m in the global coordinate system of the two-dimensional space are (x ｍ , y ｍ The orientation of the moving body 1 is expressed as an angle relative to the positive part of the x-axis of the global coordinate system. The orientation of the moving body 1 at node m is expressed as θm. The coordinates of the new node n in the global coordinate system of the two-dimensional space are expressed as (x ｎ , y ｎ ) The posture matrix of the moving object 1 at the node m is expressed by the following equation (1).

[0076]

[0077] A local coordinate system of the moving body 1 is set, with the x' axis being the direction of travel of the moving body 1 and the y' axis being the direction perpendicular to the direction of travel. The front side in the direction of travel is set as the positive x' axis, and the right side in the direction of travel is set as the positive y' axis. The coordinates of node n in the local coordinate system are (x ｂ , y ｂ ), the following relation holds:

[0078]

[0079] When moving from node m to node n in an arc, the moving body 1 turns around point C located on the y' axis. The moving direction of the moving body 1 at node n is tangent to a circle with point C as its center and radius r.

[0080] 10 is an explanatory diagram showing the positional relationship between one node and a new node in the global coordinate system of moving body 1. In the global coordinate system, the position of moving body 1 at node m is defined as point O, and the position of moving body 1 at node n is defined as point Q. The intersection of the perpendicular line from point Q to side OC and side OC is defined as A, and the intersection of the perpendicular line from point C to side OQ and side OQ is defined as B. Since triangles OAQ and OBC are similar, the radius r is expressed by the following equation.

[0081]

[0082] The rotation angle of the moving body 1 from node m to node n is Δθ. The change in the attitude of the moving body 1 at node n relative to the moving body 1 at node m, that is, the relative angle θ ｍｎ has the following relationship with the turning angle Δθ:

[0083]

[0084] Furthermore, the following equations hold from the geometric relationships in FIG.

[0085]

[0086] Therefore, the relative orientation Rnm of node n with respect to node m is expressed by the following equation.

[0087]

[0088] Therefore, according to the chain rule of the posture, the posture matrix Rn of the node n is expressed by the following equation.

[0089]

[0090] When the position coordinates of a new node are set for one node, the attitude of the moving body 1 at the new node is determined by the attitude matrix of equation (7). In this way, the searcher 49 sequentially sets the position and attitude of the new node and generates one candidate.

[0091] In this case, if the turning radius r expressed by equation (3) is less than a predetermined threshold, the searcher 49 excludes the corresponding node from candidates for the new node and reselects another node as the new node. For example, the threshold is the minimum turning radius specific to the moving body 1. According to the nonholonomic characteristics of the moving body 1, the moving body 1 cannot turn with a turning radius r that is too small. The searcher 49 can determine the minimum turning radius specific to the moving body 1 as an obstacle based on the obstacle information acquired by the acquirer 45. For example, a minimum turning radius corresponding to the size, model, etc. of the moving body 1 is stored in advance in the memory 42. The searcher 49 reads out the minimum turning radius corresponding to the obstacle information from the memory 42.

[0092] In this way, the searcher 49 determines the attitude of the mobile body 1 at a new node on the assumption that the mobile body 1 turns in an arc, thereby searching for a path based on the nonholonomic characteristics of the mobile body 1. Furthermore, if the turning radius r is too small, the searcher 49 also searches for a path based on the nonholonomic characteristics of the mobile body 1 by excluding the corresponding node.

[0093] The searcher 49 determines a target route from among multiple candidates based on the route cost. That is, the searcher 49 calculates the route cost for each of the multiple candidates. The searcher 49 determines the candidate with the smallest route cost from among the multiple candidates as the target route. The route cost may include various costs. For example, the route cost includes a meandering cost related to the degree of meandering of each of the multiple candidates. The degree of meandering refers to the magnitude of curvature, the number of turns, or the number of changes in turning direction, etc. A large degree of meandering refers to a large curvature, a large number of turns, or a large number of changes in turning direction, etc. Normally, without any constraints, the moving body 1 moves in a straight line from the start point S to the target point G. In this case, the degree of meandering of the route is small. However, if the moving body 1 bypasses an obstacle, the route will become meandering. The more obstacles that are bypassed, the more complex the route becomes and the greater the degree of meandering. In other words, a large meandering cost means a complex route and a high risk of interference between the moving body 1 and the obstacle.

[0094] In this example, the meandering cost may include a curvature cost associated with the curvature of each of the multiple candidates. A curvature cost according to the curvature is assigned to the portion of each candidate along which the moving body 1 turns. If multiple turning portions are included, a curvature cost is assigned to each turning portion. The greater the curvature of a turning portion, the greater the curvature cost. The greater the number of turning portions, the greater the overall curvature cost.

[0095] More specifically, the searcher 49 assigns a curvature cost between each two nodes connected to each other for the multiple nodes included in each candidate. As described above, each candidate includes multiple nodes connected in sequence. Although each candidate is strictly speaking a polygonal line, the moving body 1 actually moves in a curved line by tracing multiple nodes. The searcher 49 searches for multiple nodes on the assumption that the moving body 1 turns, i.e., moves in an arc, between each two nodes connected to each other. The searcher 49 evaluates the curvature between each two nodes connected to each other based on the turning radius r of the moving body 1. In other words, the turning radius r of the moving body 1 is the curvature radius between each two nodes. The searcher 49 assigns a larger curvature cost between the corresponding two nodes as the turning radius r decreases. The multiple nodes may include two nodes connected in an approximately linear manner. A small curvature cost is assigned between such two nodes. The searcher 49 evaluates the curvature between all nodes in each candidate and assigns curvature costs. As a result, the more turns a path contains, the higher the total or overall curvature cost of the candidate. Alternatively, if a path contains turns with large curvatures, the overall curvature cost of the candidate will be higher.

[0096] The path cost may include a cost related to distance, i.e., a distance cost. For example, if each candidate includes multiple nodes, a distance cost may be assigned according to the straight-line distance between each two connected nodes. The longer the straight-line distance, the larger the distance cost assigned. The overall distance cost of each candidate may be evaluated by summing up the distance costs between all nodes included in each candidate.

[0097] The path cost may include a cost related to the degree of proximity to an obstacle, i.e., an approach cost. For example, if each candidate includes multiple nodes, a cost according to the shortest distance between each node and a surrounding obstacle may be assigned as the approach cost. The shorter the shortest distance to the obstacle, the larger the approach cost assigned. The degree of proximity of each candidate to the obstacle can be evaluated based on the total value of the approach costs of each candidate.

[0098] The searcher 49 determines one or more target routes from among multiple candidates. When one target route is determined, the searcher 49 determines the candidate with the smallest route cost from among the multiple candidates as the target route. When multiple target routes are determined, the searcher 49 determines multiple target routes from among the multiple candidates in ascending order of route cost, a number of candidates equivalent to the number of target routes. The route cost includes at least a meandering cost. As a result, the searcher 49 determines the candidate with the smallest degree of meandering from among the multiple candidates as the target route. Meandering occurs when the moving body 1 detours around an obstacle. When the moving body 1 moves through a group of multiple obstacles, the number of turns may increase or the turns may become sharper. In other words, when the moving body 1 moves while weaving through multiple obstacles, the degree of meandering of the route is likely to increase. When the moving body 1 moves while weaving through multiple obstacles, the risk of the moving body 1 interfering with the obstacles also increases. On the other hand, when the moving body 1 moves while making a large detour around a group of multiple obstacles, the number of turns may decrease or the turns may become gentler. In other words, the degree of meandering of the route is likely to be small. In this case, the risk of the moving body 1 interfering with an obstacle is also reduced. When the searcher 49 determines the target route taking the meandering cost into consideration, a candidate that makes a large detour around a group of multiple obstacles tends to be determined as the target route.

[0099] Fig. 11 is a two-dimensional schematic diagram of the searched candidates. In Fig. 11, the candidates R1, R2, and R3 are displayed in the two-dimensional space in which the moving object 1 moves. The candidates R1, R2, and R3 in Fig. 11 correspond to the candidates R1, R2, and R3 in Fig. 8. For obstacle regions 61C, 61D, and 61E corresponding to moving obstacles, the positions of the moving object 1 when it is located at the start point S are indicated by solid lines, and the predicted movement ranges are indicated by two-dot chain lines.

[0100] Candidate R1 passes between obstacle area 61A and obstacle area 61C from the start point S, makes a detour to the left around obstacle area 61D, and reaches target point G. By making a detour to the left around obstacle area 61D, candidate R1 avoids passing between obstacle area 61D and obstacle area 61E. Therefore, although the overall distance of candidate R1 is slightly longer, the number of meanders in candidate R1 is relatively small, and the curvature of the turning portion included in candidate R1 is relatively small.

[0101] Candidate R2 passes from the start point S between obstacle area 61A and obstacle area 61C, passes between obstacle area 61D and obstacle area 61E, and reaches the target point G. Candidate R2 passes between obstacle area 61D and obstacle area 61E. Therefore, although the overall distance of candidate R2 is relatively short, candidate R2 meanders many times and the curvature of the turning portion included in candidate R2 is large.

[0102] Candidate R3 passes between obstacle areas 61B and 61C from the start point S, makes a detour to the right around obstacle area 61E, and reaches the target point G. By making a detour to the right around obstacle area 61E, candidate R3 avoids passing between obstacle areas 61D and 61E. Therefore, although the overall distance of candidate R3 is slightly longer, the number of meanders in candidate R3 is relatively small, and the curvature of the turning portion included in candidate R3 is relatively small.

[0103] In this example, the path cost includes a distance cost in addition to a meandering cost. Comparing the three candidates R1, R2, and R3, in terms of distance, the distance cost of candidate R2 is the smallest, and the distance cost of candidate R3 is the largest. In terms of meandering, the meandering cost of candidate R1 is the smallest, and the meandering cost of candidate R2 is the largest. In terms of the overall path cost including the distance cost and meandering cost, the path cost of candidate R1 is the smallest. In other words, although the overall distance of candidate R1 is somewhat longer, the degree of meandering of candidate R1 is small, and the risk of interference between the mobile unit 1 and an obstacle is small.

[0104] When determining one target route, the searcher 49 comprehensively evaluates the risk of interference and the overall distance and determines candidate R1 as the target route. When determining multiple target routes, for example, two target routes, the searcher 49 comprehensively evaluates the risk of interference and the overall distance and determines candidate R1 and candidate R3 as the target routes.

[0105] The searcher 49 outputs the determined target route, which in this example is transmitted to the target mobile unit 1.

[0106] The route cost may include the proximity cost, but in this example, the proximity costs of the candidates R1, R2, and R3 are all approximately the same.

[0107] Next, the operation of the route planning device 100 will be described with reference to Fig. 12. Fig. 12 is a flowchart of route planning. The route planning device 100 generates a target route for a target moving body 1. When there are multiple target moving bodies 1, the route planning device 100 generates a target route for each of the target moving bodies 1.

[0108] In this example, obstacle information is transmitted from the monitoring sensor 11 as needed, and the acquirer 45 acquires the obstacle information as needed. In addition, the acquirer 45 also acquires obstacle information transmitted from the mobile object 1 as needed. That is, in parallel with the processing from step S101 onward, which will be described later, the acquirer 45 acquires obstacle information including information on the position of the obstacle.

[0109] Under these circumstances, the generator 46 determines in step S101 whether a generation condition is satisfied. The generation condition is a condition for starting route planning. For example, the path planning device 100 receives a request from the mobile body 1. That is, the path planning device 100 executes route planning upon receiving a request from the mobile body 1. The generation condition may be the arrival of a predetermined generation period. The generation period occurs at predetermined time intervals. That is, the path planning device 100 executes route planning periodically. The generation condition may be the passage of a predetermined time since the previous target route was generated. That is, the path planning device 100 executes route planning again after a certain amount of time has passed since the target route was generated. The generation condition may be a change in the surrounding environment since the previous target route was generated. The change in the surrounding environment may be the detection of a new obstacle, or a known obstacle behaving differently from its predicted behavior. As will be described later, the behavior of the obstacle is predicted to generate the target route. The generator 46 determines that a change in the surrounding environment has occurred when the current position of the obstacle, i.e., the position based on the latest obstacle information, is outside the most recent predicted area. The generator 46 also determines that a change in the surrounding environment has occurred when a new moving object 1 is detected based on the latest obstacle information. In other words, if the predicted behavior of the obstacle that was the premise of the previous target route differs from the actual behavior of the obstacle, the path planning device 100 executes path planning again. The generation condition may be the satisfaction of any one of the aforementioned reception of a request, the arrival of a generation period, the passage of a predetermined time, and a change in the surrounding environment.

[0110] If the generation condition is not met, the generator 46 repeats step S101 and waits until the generation condition is met.

[0111] If the generation condition is met, the predictor 47 predicts the behavior of the obstacle based on the obstacle information in step S102. Specifically, the predictor 47 determines whether the obstacle is stationary or moving. If the obstacle is moving, the predictor 47 predicts the change in the position of the obstacle over time, i.e., the movement.

[0112] Next, in step S103, the space generator 48 generates a search space. Specifically, the space generator 48 creates a three-dimensional obstacle region 61 in a three-dimensional space obtained by adding a time axis to the two-dimensional space in which the moving object 1 moves.

[0113] Then, in step S104, the searcher 49 searches for multiple candidates for the target route within the search space. The searcher 49 searches for multiple candidates that will not cause the moving body 1 to interfere with the obstacle area. At this time, the searcher 49 searches for a route based on the nonholonomic characteristics of the moving body 1. Specifically, when searching from one node to the next node, the searcher 49 searches for the next node to which the moving body 1 can move in accordance with the nonholonomic characteristics of the moving body 1.

[0114] Furthermore, the searcher 49 determines a target route from among multiple candidates based on the route cost. The searcher 49 determines the candidate with the smallest route cost from among the multiple candidates as the target route. The route cost includes a meandering cost. As a result, a candidate with a gentle meandering or a small number of meanders is determined as the target route. Note that the route cost may further include a distance cost or an approach cost. As a result, a candidate with a small degree of meandering, a short overall distance, or a candidate that does not approach obstacles very closely is determined as the target route. Step S104 corresponds to generating a target route for the moving body from the start point to the target point based on the obstacle information so as to avoid interference between the moving body and the obstacle.

[0115] Once the target route is determined, in step S105, the generator 46 transmits the target route to the target mobile object 1.

[0116] After the target route is transmitted, the generator 46 again determines whether the generation condition is satisfied in step S101. This causes the generation of the target route to be repeatedly executed. The generation of the target route is repeated until the target moving object 1 reaches the target point.

[0117] Next, the operation of the moving body 1 will be described with reference to Fig. 13. Fig. 13 is a flowchart of automatic driving. The moving body 1 is configured to be able to switch between manual driving and automatic driving. When switching to automatic driving, the control device 2 transmits a request for a target route to the route planning device 100. Here, a case where multiple target routes are sent from the route planning device 100 will be described.

[0118] When the control device 2 receives multiple target routes from the route planning device 100, in step S201, the display 15 displays images of the multiple target routes. Specifically, the image generator 211 generates images of the multiple target routes and outputs the generated images to the display 15. For example, when two target routes are received from the route planning device 100, the display 15 displays a route chart including the two target routes. At this time, the display 15 displays the multiple target routes together with their respective priorities.

[0119] FIG. 14 is an example of an image displayed by the display 15. The target route P1 corresponds to the candidate R1 in FIG. 10, and the target route P3 corresponds to the candidate R3 in FIG. 10. The route cost of the target route P1 is smaller than the route cost of the target route P3. The target route P1 has the highest priority. In the example of FIG. 14, the display 15 displays the target route P1, which has the highest priority, with a solid line, and displays the target route P3 with a dashed line. The display 15 may also display the route costs of each of the target routes P1 and P3. The user can refer to the target routes displayed on the display 15 and select one target route via the input device 16.

[0120] Next, when the user selects one target route from multiple target routes via the input device 16, in step S202, the acceptor 212 accepts the selection of the one target route from the user and sets the selected target route as the target route to be used for automatic driving.

[0121] For example, when the user selects the target route P1 in the image shown in Fig. 14, the acceptor 212 sets the target route P1 as the target route for automated driving. The user can also select the target route P3.

[0122] Once the target route is set, the control device 2 performs automatic driving in step S203. The control device 2 outputs a command operation amount to the actuator 12 so that the moving body 1 moves along the set target route, thereby automatically driving the moving body 1.

[0123] Thereafter, in step S204, the control device 2 determines whether or not the target route has been received from the route planning device 100. The route planning device 100 updates the target route from time to time. That is, the control device 2 waits for reception of an updated target route by repeating step S204.

[0124] When the control device 2 receives the target route, the process returns to step S201. That is, the display device 15 updates the display of the target route. In step S202 after the target route has been updated, if there is no selection input from the user, the control device 2 sets a target route that is closest to the target route set before the update as the target route to be used for automated driving, and continues automated driving in step S203. That is, after automated driving has started, automated driving continues along the initially selected target route while adjusting it through updating, unless there is an operation by the user to change the target route.

[0125] 13. If only one target route is output from the route planning device 100, step S202 is deleted from the flowchart in Fig. 13. The display 15 displays the received target route (step S201), and the control device 2 performs automatic driving according to the target route (step S203). If the target route is updated, the display 15 displays the updated target route (step S201), and the control device 2 performs automatic driving according to the updated target route (step S203).

[0126] According to such a route plan, a target route with a small degree of meandering is generated. A small degree of meandering correlates with the mobile object 1 making fewer detours around obstacles. Fewer detours around obstacles reduces the risk of the mobile object 1 interfering with the obstacles. For example, when multiple obstacles exist, a route that completely detours around the multiple obstacles is determined as the target route, rather than a route that weaves between the multiple obstacles and travels the shortest distance. This significantly reduces the risk of the mobile object 1 interfering with the obstacles.

[0127] Furthermore, the change in the position of the obstacle over time is predicted, and a route that avoids interference between the moving body 1 and the obstacle is searched for based on the predicted area of ​​the obstacle, thereby improving the accuracy of generating a route that avoids interference with the obstacle.

[0128] In addition, the route search is performed in a three-dimensional search space, which adds a time axis to the two-dimensional space in which the mobile object 1 moves. This allows the route search to be performed by treating moving obstacles as stationary solid objects. As a result, the calculation load of the route search can be reduced.

[0129] The path planning device 100 updates the target path by repeatedly executing such path planning. Obstacles may behave in unexpected ways. Even when a path planning is performed based on a predicted behavior of a moving obstacle, updating the target path can accommodate the unexpected behavior of the obstacle.

[0130] The route cost referenced when selecting a target route includes a meandering cost related to the degree of meandering. The meandering cost includes a curvature cost related to the curvature of each candidate route. A large curvature of the route means that the moving body 1 will make a sharp turn. A sharp turn may result in the moving body 1 making a sudden avoidance of an obstacle. In other words, a small curvature cost means that there will be fewer sudden detours around obstacles, and as a result, there will be a small risk of interference between the moving body 1 and the obstacle. In this way, by taking the curvature of the route into consideration, a target route can be determined taking into account the risk of interference between the moving body 1 and the obstacle.

[0131] Specifically, a curvature cost according to the curvature is assigned to a portion of each candidate route where the moving object 1 turns. More specifically, when searching for candidates for the target route, the nonholonomic characteristics of the moving object 1 are taken into consideration, and a search for nodes is performed on the assumption that the moving object 1 will turn as it moves. When searching for nodes, the turning radius r of the moving object 1, i.e., the radius of curvature, is calculated. A curvature cost is assigned between each of the interconnected nodes based on the turning radius r. In this way, the curvature of the route is evaluated taking into account the actual behavior of the moving object 1.

[0132] As described above, by searching multiple candidates based on the nonholonomic characteristics of the moving object 1, it is possible to search for a path that is faithful to the actual behavior of the moving object 1. Furthermore, in addition to searching for candidates, it is possible to derive the curvature of the path.

[0133] By searching for the position of a new node in a three-dimensional search space including a time axis, and determining the posture of the moving body 1 at the new node in the two-dimensional space in which the moving body 1 moves, it is possible to easily search for a route that achieves both avoidance of interference with moving obstacles and movement of the moving body 1 in accordance with nonholonomic characteristics.

[0134] The target route is displayed on the display 15 of the moving body 1, so the user, i.e., the occupant, can confirm the target route for autonomous driving. For example, if the future route is unknown, the user may feel anxious about the behavior of the moving body 1. However, by knowing the future route, the user can reduce anxiety about the behavior of the moving body 1. Alternatively, if the target route for autonomous driving is not the route the user desires, the user can switch from autonomous driving to manual driving.

[0135] Furthermore, the route planning device 100 outputs a plurality of target routes, and the display 15 displays the plurality of target routes. This allows the user to determine one target route from among the plurality of target routes. In other words, the user is presented with a narrowed-down list of target routes based on route cost, and the user can determine the final target route taking into consideration factors other than route cost.

[0136] At this time, the display 15 also displays the priority of the target routes, thereby helping the user to select a target route.

[0137] Other Embodiments As described above, the above-described embodiments have been described as examples of the technology disclosed in the present application. However, the technology of the present disclosure is not limited to these embodiments and can be applied to embodiments in which modifications, substitutions, additions, omissions, etc. are made as appropriate. Furthermore, the components described in the above-described embodiments can be combined to create new embodiments. Furthermore, the components described in the accompanying drawings and detailed description may include 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. Therefore, the fact that these non-essential components are described in the accompanying drawings or detailed description should not be interpreted as immediately determining that these non-essential components are essential.

[0138] For example, the moving body 1 that receives the target route is not limited to the above-mentioned examples. For example, the moving body 1 may be a vehicle, an autonomous robot (e.g., a carrier pallet), or a flying device (e.g., a drone), etc. Any moving body can be used as the moving body 1 as long as it is capable of performing autonomous driving. Other moving bodies 1 that may become obstacles in generating the target route do not need to have autonomous driving functions. The other moving bodies 1 may be living beings such as humans. For example, the other moving bodies 1 may be pedestrians traveling on a road, or workers working on a site, etc. The number of moving bodies 1 included in the driving assistance system 1000 is not limited to the example of FIG. 1 and is arbitrary.

[0139] The actuator 12 is an actuator related to the movement of the mobile object 1. The actuator 12 is not limited to an electric motor, an engine, or a rudder. If the mobile object 1 is a vehicle, the actuator 12 may be a steering device. If the mobile object 1 is a drone, a propeller is the actuator.

[0140] The configuration of the control device 2 for the moving body 1 is not limited to the above-described configuration. Any configuration may be adopted as long as it is capable of executing automatic driving of the moving body 1 along the target route. For example, the control device 2 may not estimate the state of the moving body 1 or the other moving body 1, but may instead acquire the estimated state of the moving body 1 or the other moving body 1 from an external device such as the path planning device 100. Alternatively, the control device 2 may estimate the state of the moving body 1, and the state of the other moving body 1 may be estimated by the other moving body 1 or the path planning device 100, etc. In this case, the control device 2 acquires the estimated result of the state of the other moving body 1 from an external device. Furthermore, any method may be adopted for generating the target trajectory, correcting the target trajectory, and calculating the operation amount. For example, a model trained by machine learning may be used to input the state quantities of the moving body 1 and the other moving body 1, and output the target trajectory.

[0141] The route planning device 100 is not limited to being placed in the control tower 120, and may be placed in any location. For example, the route planning device 100 may be placed in a mobile object 1 that performs autonomous driving. In this case, the mobile object 1 generates a target route using the route planning device 100, and performs autonomous driving along the target route using the control device 2. The route planning device 100 may be formed by multiple devices rather than a single device.

[0142] The path planning device 100 may request obstacle information from the monitoring sensor 11, the mobile body 1, etc. For example, the acquirer 45 may periodically request obstacle information from the monitoring sensor 11, the mobile body 1, etc., and acquire the obstacle information returned from the monitoring sensor 11, the mobile body 1, etc.

[0143] Any method may be used to generate the target path. For example, the predicted area of ​​a moving obstacle at each time point does not have to be the same area. Since predicting the position of an obstacle becomes more difficult as time passes, the predicted area at each time point may become larger as time progresses. In other words, the obstacle area may be an area that expands as time progresses.

[0144] The route search is not limited to RRT-star. The route search may utilize search algorithms such as A-star, Hybrid A-star, kinodynamic RRT-star, and RRT. Furthermore, the route search does not have to be performed based on the nonholonomic characteristics of the moving object 1.

[0145] The curvature of the curved portion included in each candidate route is not limited to that based on the turning radius r of the moving object 1 when searching for the node. The curvature of the curved portion may be evaluated by any method. For example, the curvature of the curved portion may be calculated by curvilinearly interpolating the route formed by multiple nodes.

[0146] The route cost includes at least a meandering cost. In addition to the meandering cost, the route cost may further include at least one of a distance cost, an approach cost, and other costs.

[0147] It is not essential to display the target route on the display 15. The route planning device 100 may generate a target route and the mobile object 1 may automatically drive according to the target route without presenting the target route to the user.

[0148] Even when a target route is displayed on the display 15, the display 15 may display only one target route used for autonomous driving. For example, the route planning device 100 may output only one target route. Alternatively, the display 15 may display only the target route with the highest priority, for example, the smallest route cost, among the multiple target routes output from the route planning device 100.

[0149] The flowcharts in Figures 12 and 14 are merely examples. Steps in the flowcharts may be changed, replaced, added, omitted, etc. as appropriate. The order of steps in the flowcharts may also be changed, and serial processing may be performed in parallel. For example, the generation condition of step S101 may be set arbitrarily. The generation condition may be at least one of the above-mentioned reception of a request, arrival of a generation period, passage of a predetermined time, and a change in the surrounding environment, or any combination thereof.

[0150] The functions performed 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), a Central Processing Unit (CPU), conventional circuits, and / or combinations thereof, programmed to perform the described functions. A processor includes transistors and other circuits and is considered a circuit or processing circuit. A processor may also be a programmable processor that executes a program stored in a memory.

[0151] In this specification, a circuit, unit, or means is hardware that is programmed to realize or performs the described functions, which may be any hardware disclosed herein or any hardware known to be programmed to realize or perform the described functions.

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

[0153] [Aspects] The above-described embodiments are specific examples of the following aspects.

[0154] (Aspect 1) The path planning device 100 includes an acquirer 45 that acquires obstacle information including information on the position of an obstacle, and a generator 46 that generates a target route for a moving body 1 from a start point S to a target point G based on the obstacle information so as to avoid interference between the moving body 1 and the obstacle, the generator 46 searches for multiple candidates for the target route and determines the target route from the multiple candidates based on route costs, and the route costs include a meandering cost related to the degree of meandering of each of the multiple candidates.

[0155] According to this configuration, since the meandering cost is included in the route cost, the degree of meandering of each candidate is taken into consideration when determining the target route. If the degree of meandering of the route is large, the route may include a sudden detour around an obstacle by the mobile body 1, or multiple detours around obstacles by the mobile body 1. In autonomous driving, it is natural that the mobile body 1 moves while avoiding interference with obstacles, but if the detours are sudden or the number of detours is large, the risk of interference between the mobile body 1 and obstacles increases. By determining the target route taking into consideration the degree of meandering of the route, a target route with a low risk of interference between the mobile body 1 and obstacles is generated.

[0156] (Aspect 2) In the path planning device 100 according to aspect 1, the generator 46 predicts a change in the position of the obstacle over time and searches for the plurality of candidates that avoid interference between the moving object 1 and the obstacle.

[0157] According to this configuration, a plurality of candidates that avoid interference between the mobile body 1 and the obstacle are searched for after predicting the change in the position of the obstacle over time, and therefore a target route that anticipates the future behavior of the obstacle is generated. In other words, a target route that avoids interference between the mobile body 1 and the obstacle is generated even for a moving obstacle.

[0158] (Aspect 3) In the path planning device 100 described in Aspect 1 and Aspect 2, the generator 46 creates a three-dimensional obstacle area 61 including a predicted future position of the obstacle in a three-dimensional space obtained by adding time as a dimension to the two-dimensional space in which the moving object 1 moves, and searches for the multiple candidates in the three-dimensional space while avoiding the obstacle area.

[0159] This configuration allows a route to be searched for by regarding moving obstacles as stationary solid objects. Since there is no need to consider the dynamic behavior of obstacles when searching for a route, the computational load of the route search can be reduced.

[0160] (Aspect 4) In the route planning device 100 according to any one of Aspects 1 to 3, the meandering cost includes a curvature cost associated with a curvature in each of the plurality of candidates.

[0161] According to this configuration, the curvature of each candidate is evaluated as the meandering cost. A candidate with a large route curvature has a high meandering cost and is less likely to be selected as a target route. The curvature of a route is correlated with the sharpness of turns when meandering. In other words, the larger the curvature, the sharper the turns. A sharp turn can be a high-risk movement, such as a sudden detour around an obstacle. By including the curvature cost as the meandering cost, candidates with gentler turns or fewer sharp turns are more likely to be selected as a target route.

[0162] (Aspect 5) In the path planning device 100 according to any one of Aspects 1 to 4, the generator 46 searches for the plurality of candidates based on nonholonomic characteristics of the moving object 1.

[0163] According to this configuration, a route search is performed while eliminating unrealistic behavior of the moving object 1. For example, a route is searched for based on smooth behavior of the moving object 1 that does not include movements such as skidding. In this way, a target route that conforms to the actual behavior of the moving object 1 is generated.

[0164] For example, it becomes easier to evaluate the curvature cost of each candidate route if a route search is performed assuming a turn rather than a bend when changing course, as a nonholonomic characteristic of the moving body 1. Specifically, by assuming a turn of the moving body 1 when searching for a new node that constitutes the route, the curvature of the turning portion of the route can be evaluated based on the turning radius r of the moving body 1 used when searching for the new node.

[0165] (Aspect 6) In the route planning device 100 according to any one of Aspects 1 to 5, the generator 46 sequentially updates the search for the plurality of candidates and the determination of the target route.

[0166] This configuration makes it possible to respond to changes in the surrounding environment over time. Specifically, the target route is generated based on the position of an obstacle so that the moving body 1 does not interfere with the obstacle. The obstacle may move. By successively updating the target route, a target route that corresponds to a moving obstacle can be generated.

[0167] (Aspect 7) A driving assistance system 1000 includes the route planning device 100 according to any one of Aspects 1 to 6, and a display 15 mounted on a moving body 1 that performs autonomous driving and that displays the target route from the route planning device 100.

[0168] According to this configuration, the occupant of the moving body 1 can check the target route used for autonomous driving via the display 15. This allows the occupant to predict the future behavior of the moving body 1 during autonomous driving.

[0169] (Aspect 8) In the driving assistance system 1000 according to aspect 7, the route planning device 100 determines a plurality of the target routes from among the plurality of candidates, and the display 15 displays the plurality of the target routes.

[0170] According to this configuration, the route planning device 100 determines not one target route but multiple target routes, and the display 15 displays the multiple target routes.

[0171] (Aspect 9) The driving assistance system 1000 according to aspect 7 or aspect 8 further includes a receiver 212 that receives, from a user, a selection of one of the target routes displayed on the display 15 to be used for automatic driving of the moving body 1.

[0172] According to this configuration, the occupant can select one target route to be used for autonomous driving from among the multiple target routes presented by the display 15. This allows the route planning device 100 to narrow down the target routes to several based on the route cost, and allows the user to determine a final target route from among the multiple target routes presented to the user, taking into consideration factors other than the route cost.

[0173] (Aspect 10) In the driving assistance system 1000 according to any one of Aspects 7 to 9, the route planning device 100 assigns priorities to the plurality of target routes according to the route costs, and the display 15 further displays the priorities of the plurality of target routes.

[0174] This configuration allows the occupant to know the relative merits of the multiple target routes displayed on the display 15, particularly in terms of route cost. The priority helps the occupant in selecting one target route.

[0175] (Aspect 11) A route planning method includes acquiring obstacle information including information on the position of an obstacle, and generating a target route for a moving body 1 from a start point S to a target point G based on the obstacle information so as to avoid interference between the moving body 1 and the obstacle, wherein generating the target route includes searching for multiple candidates for the target route and determining the target route from the multiple candidates based on route costs, and the route costs include a meandering cost related to the degree of meandering of each of the multiple candidates.

[0176] According to this configuration, since the meandering cost is included in the route cost, the degree of meandering of each candidate is taken into consideration when determining the target route. If the degree of meandering of the route is large, the route may include a sudden detour around an obstacle by the mobile body 1, or multiple detours around obstacles by the mobile body 1. In autonomous driving, it is natural that the mobile body 1 moves while avoiding interference with obstacles, but if the detours are sudden or the number of detours is large, the risk of interference between the mobile body 1 and obstacles increases. By determining the target route taking into consideration the degree of meandering of the route, a target route with a low risk of interference between the mobile body 1 and obstacles is generated.

[0177] (Mode 12) The route planning program 42a causes a computer to realize a function of acquiring obstacle information including information on the position of an obstacle, and a function of generating a target route for a moving body 1 from a start point S to a target point G based on the obstacle information so as to avoid interference between the moving body 1 and the obstacle, wherein the function of generating the target route searches for multiple candidates for the target route and determines the target route from among the multiple candidates based on route costs, and the route costs include a meandering cost related to the degree of meandering of each of the multiple candidates.

[0178] According to this configuration, since the meandering cost is included in the route cost, the degree of meandering of each candidate is taken into consideration when determining the target route. If the degree of meandering of the route is large, the route may include a sudden detour around an obstacle by the mobile body 1, or multiple detours around obstacles by the mobile body 1. In autonomous driving, it is natural that the mobile body 1 moves while avoiding interference with obstacles, but if the detours are sudden or the number of detours is large, the risk of interference between the mobile body 1 and obstacles increases. By determining the target route taking into consideration the degree of meandering of the route, a target route with a low risk of interference between the mobile body 1 and obstacles is generated.

Claims

1. A path planning device comprising: an acquirer that acquires obstacle information including information on the position of an obstacle; and a generator that generates a target path of a moving body from a starting point to a target point based on the obstacle information so as to avoid interference between the moving body and the obstacle, wherein the generator searches for a plurality of candidates for the target path and determines the target path from among the plurality of candidates based on a path cost, and the path cost includes a meandering cost related to the degree of meandering of each of the plurality of candidates.

2. The path planning device according to claim 1, wherein the generator predicts a change over time in the position of the obstacle and searches for the plurality of candidates for avoiding interference between the moving body and the obstacle.

3. The path planning device according to claim 2, wherein the generator creates a three-dimensional obstacle region including a future predicted position of the obstacle in a three-dimensional space obtained by adding time as a dimension to the two-dimensional space in which the moving body moves, and searches for the plurality of candidates while avoiding the obstacle region in the three-dimensional space.

4. The path planning device according to claim 1, wherein the meandering cost includes a curvature cost related to the curvature in each of the plurality of candidates.

5. The path planning device according to claim 1, wherein the generator searches for the plurality of candidates based on non-holonomic characteristics of the moving body.

6. The path planning device according to claim 1, wherein the generator sequentially updates the search for the plurality of candidates and the determination of the target path.

7. A driving support system comprising: the path planning device according to any one of claims 1 to 6; and a display mounted on a moving body that performs automatic driving and that displays the target path from the path planning device.

8. The driving support system according to claim 7, wherein the path planning device determines a plurality of the target paths from among the plurality of candidates, and the display displays the plurality of the target paths.

9. The driving support system according to claim 8, further comprising a receiver that receives from a user a selection of one of the target paths to be used for automatic driving of the moving body from among the plurality of the target paths displayed on the display.

10. In the driving support system according to claim 9, the route planning device assigns priorities according to the route costs to the plurality of target routes, and the display further displays the priorities of the plurality of target routes. A driving support system.

11. Obtaining obstacle information including information on the position of an obstacle, and generating a target route of a moving body from a starting point to a target point based on the obstacle information so as to avoid interference between the moving body and the obstacle, wherein generating the target route includes searching for a plurality of candidates for the target route and determining the target route based on the route cost from among the plurality of candidates, and the route cost includes a meandering cost related to the degree of meandering of each of the plurality of candidates. A route planning method.

12. A computer is caused to realize a function of obtaining obstacle information including information on the position of an obstacle, and a function of generating a target route of a moving body from a starting point to a target point based on the obstacle information so as to avoid interference between the moving body and the obstacle, wherein the function of generating the target route includes searching for a plurality of candidates for the target route and determining the target route based on the route cost from among the plurality of candidates, and the route cost includes a meandering cost related to the degree of meandering of each of the plurality of candidates. A route planning program.

Images