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

Abstract

To plan a route that is able to reduce risks.SOLUTION: A route planning device 100 includes: an acquisition unit 45 that acquires obstacle information including information on a position of an obstacle; and a generator 46 that, based on the obstacle information, generates a target route for a moving body 1 from a start point S to a target point G so as to avoid interference between the moving body 1 and the obstacle. The generator 46 searches for a plurality of candidates for the target route and, based on a route cost, determines the target route from among the plurality of candidates. The route cost includes a meandering cost associated with a degree of meanderings of each of the plurality of candidates.SELECTED DRAWING: Figure 6

Description

Technical Field

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

Background Art

[0002] Conventionally, a device for planning a route of a moving body has been known. For example, in the route planning disclosed in Patent Document 1, a route that avoids interference with other moving bodies is planned in consideration of the movement of other moving bodies.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0004] In the route planning according to Patent Document 1, safety is ensured by planning a route that avoids interference with other moving bodies. However, from the viewpoint of risk avoidance, there is room for further improvement.

[0005] The technology disclosed herein has been made in view of such points, and its object is to plan a route capable of reducing risks.

Means for Solving the Problems

[0006] The path planning device disclosed herein includes an acquirer that acquires obstacle information including information on the positions of obstacles, 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 obstacles. The generator searches for a plurality of candidates for the target path, 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.

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

[0008] The path planning method disclosed herein includes acquiring obstacle information including information on the positions of obstacles, and generating 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 obstacles. Generating the target path includes searching for a plurality of candidates for the target path, determining 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.

[0009] The path planning program disclosed herein causes a computer to realize a function of acquiring obstacle information including information on the positions of obstacles, and a function of generating 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 obstacles. The function of generating the target path includes searching for a plurality of candidates for the target path, determining 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.

Advantages of the Invention

[0010] According to the path planning device, a path with reduced risk can be planned.

[0011] According to the driving support system, a route that can reduce risks can be planned.

[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.

Brief Description of the Drawings

[0014]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10

Figure 11

Figure 12

Figure 13

Figure 14

Best Mode for Carrying Out the Invention

[0015] Hereinafter, exemplary embodiments will be described in detail with reference to the drawings. FIG. 1 is a schematic plan view showing a state where the moving body 1 is navigating the ocean. The moving body 1 performs automatic driving. The driving support system 1000 includes a route planning device 100 that generates a target route of the moving body 1, and a display 15 that is mounted on the moving body 1 that performs automatic driving and displays the target route from the route planning device 100. In this example, the moving body 1 is a ship. Inside and outside the harbor shown in FIG. 1, there are a plurality of moving bodies 1. The plurality of moving bodies 1 are distinguished from each other by the alphabetical suffix after the symbol "1". For example, the plurality of moving bodies 1 in the sea area of FIG. 1 include a moving body 1A, a moving body 1B, a moving body 1C, and a moving body 1D. In addition, when not distinguishing each of the plurality of moving bodies 1, they are simply referred to as "moving body 1". In the harbor shown in FIG. 1, there are a quay wall 91, a breakwater 92, etc. as obstacles. Also, for one moving body 1, other moving bodies 1 become obstacles.

[0016] The route planning device 100 generates a target route of the moving body 1 that performs automatic driving. When a plurality of moving bodies 1 perform automatic driving, the route planning device 100 generates an individual target route for each moving body 1. The route planning device 100 is arranged in the control tower 120. The route planning device 100 communicates with the target moving body 1 and transmits the target route to the moving body 1. The target moving body 1 performs automatic driving according to the target route. Hereinafter, the case of generating the target route of the moving body 1A to the target point G 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 in automatic driving to the user, that is, the crew. Further, the display 15 may display a plurality of target routes and allow the user to select the target route used for automatic driving.

[0018] -Moving Body- The mobile body 1 performs automatic driving, specifically, autonomous driving. The mobile body 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 body 1. The mobile body 1 includes a monitoring sensor 11, an actuator 12, a communicator 13, a position detector 14, a display 15, an input device 16, and a control device 2.

[0019] The monitoring sensor 11 acquires monitoring information within a predetermined range in the environment where the mobile body 1 moves. The monitoring information may include terrain, buildings, structures, facilities, or mobile bodies, etc. The monitoring sensor 11 includes at least one of a camera, LiDAR (Light Detection And Ranging), an infrared sensor, a laser rangefinder, and a Doppler LiDAR. For example, the camera captures still images or moving images. 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 driving source of the mobile body 1. The actuator 12 may include a first actuator 12A and a second actuator 12B. For example, when 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. The communicator 13 communicates with, for example, a route planning device 100. The position detector 14 detects the position of the mobile body 1. For example, the position detector 14 is a GNSS (Global Navigation Satellite System) receiver or an IMU (Inertial Measurement Unit).

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

[0023] The input device 16 is operated by the user, that is, it is a device for the user to input operations. The input device 16 is, for example, a keyboard, a mouse, or a button, etc. The display 15 and the input device 16 may be integrally configured. For example, the display 15 and the input device 16 may be a touch panel.

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

[0025] The processor 21 performs various arithmetic processes. For example, the processor 21 is formed of a processor such as a CPU (Central Processing Unit). The processor 21 may be formed of an MCU (Micro Controller Unit), an MPU (Micro Processor Unit), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), a system LSI, etc.

[0026] The storage device 22 stores programs executed by the processor 21 and various data. For example, the storage device 22 stores a control program. The storage device 22 is formed of a non-volatile memory, an HDD (Hard Disc Drive), an SSD (Solid State Drive), etc. The memory 23 temporarily stores data, etc. 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 perform autonomous driving. The processor 21 receives a target route from the route planning device 100. The processor 21 operates the actuator 12 based on the detection results of 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 execute a collision avoidance operation as necessary during the movement of the moving body 1 along the target route. The processor 21 may transmit the detection results of the monitoring sensor 11 and the position detector 14 to the route planning device 100 for generating the target route.

[0028] FIG. 4 is a functional block diagram of the processor 21. The processor 21 realizes various functions by reading and expanding a control program from the memory 22 into the memory 23. Specifically, the processor 21 functions as a state acquirer 24 that acquires the state of the self-moving body 1 and the state of other moving bodies 1, and an arithmetic unit 25 that calculates a command operation amount of the actuator 12. The arithmetic unit 25 calculates the command operation amount of the actuator 12 based on the state of the self-moving body 1 and the state of other moving bodies 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 self-moving body 1 and the state of other moving bodies 1. In addition, the processor 21 further functions as an image generator 211 that generates an image to be displayed on the display 15 and a receiver 212 that receives a user's 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 nautical chart showing the target route. When a plurality of target routes are sent from the route planning device 100, the image generator 211 generates images of the plurality of target routes. When priority orders are assigned to the plurality of target routes, the image generator 211 also displays the priority orders of the plurality of target routes. The modes of displaying the priority orders are various. For example, the image generator 211 may display the priority order, that is, a numerical value, corresponding to each target route. Alternatively, the image generator 211 may represent the priority order of each target route by the color of each target route. That is, the image generator 211 may color-code the plurality of target routes according to the priority order. Alternatively, the image generator 211 may represent the priority order of each target route by the line type of each target route.

[0030] The image generator 211 may display only part of the priority orders of the plurality of target routes, not all of them. For example, when three target routes are displayed, the image generator 211 may display that the target route with the first priority is the first priority, and it may not be necessary to assign priority orders to the target routes with the second and third priorities. That is, the image generator 211 may display the target route with the first priority separately from the target routes with priorities other than the first priority, for example, the second and third priorities. The second target route and the third target route may not be displayed separately from each other. The distinction between the first target route and the target routes other than the first priority may be based on the color or line type of the target route.

[0031] In addition, when the 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 display the route cost as the priority order. That is, the smaller the route cost, the lower the priority order.

[0032] The receiver 212 receives the selection of one target route from among a plurality of target routes by the user. The user can select one target route to be used for the automatic driving from among the plurality of target routes displayed on the display 15 by operating the input device 16. When the receiver 212 receives the input of the selection by the user, it sets the selected target route as the target route to be used for the automatic driving.

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

[0034] The state recognizer 26 realizes the function of obtaining the state of the self-mobile body 1 and the function of obtaining the state of other mobile bodies 1. The state recognizer 26 outputs the state of the self-mobile body 1 and the state of other mobile bodies 1 to the state acquirer 24. The state recognizer 26 may, in addition to the state of the self-mobile body 1 and the state of other mobile bodies 1, obtain the state of the environment and output it to the state acquirer 24.

[0035] Map information, information on other mobile bodies 1, the detection results of the monitoring sensor 11, and the detection results of the position detector 14 are input to the state recognizer 26. The map information and the information on other mobile bodies 1 are transmitted, for example, from the route planning device 100 or the control tower 120 and stored in the storage 22. The map information includes at least one of roads, passageways, air routes, and ground features (objects on the ground or at sea, whether natural or artificial, such as buildings, trees, and rocks). The map information can be information regarding general maps such as road maps, facility maps, and air route maps. The map information may include information regarding obstacles. The information regarding obstacles includes at least one of the position, speed, and type of the obstacles. The obstacles can include ground features, buildings, other mobile bodies other than the self-mobile body 1 (i.e., other mobile bodies 1), construction sites, uneven ground, shallows, fish farms, anchorages, etc. The information on other mobile bodies includes, for example, the type of other mobile bodies.

[0036] For example, the state recognizer 26 estimates the state of the autonomous mobile body 1 using a Kalman filter. The state recognizer 26 may perform self-position estimation using SLAM (Simultaneous Localization and Mapping) technology. The state recognizer 26 may estimate the size of the other mobile body 1 or the relative speed of the other mobile body 1 with respect to the autonomous mobile body 1 based on the detection result of the position detector 14. The state recognizer 26 may perform state estimation of obstacles in the environment. The state recognizer 26 outputs information regarding the autonomous mobile body 1 such as the current position and speed of the autonomous mobile body 1, information regarding the other mobile body such as the relative position and speed of the other mobile body 1 with respect to the autonomous mobile body 1, information regarding the obstacle such as the relative position and speed of the obstacle with respect to the autonomous mobile body 1, and information regarding disturbances.

[0037] That is, the state acquirer 24 acquires map information including the position of obstacles, etc., information regarding the autonomous mobile body 1 such as the current position and speed of the autonomous mobile body 1, information regarding the other mobile body 1 such as the relative position and speed of the other mobile body 1 with respect to the autonomous mobile body 1, information regarding the obstacle such as the relative position and speed of the obstacle with respect to the autonomous mobile body 1, and information regarding disturbances.

[0038] The arithmetic unit 25 realizes the function of generating the target trajectory of the autonomous mobile body 1. The target trajectory is a trajectory that is a relatively short-term target from the current position. The arithmetic unit 25 further realizes the function of calculating the control input of the actuator 12 based on the target trajectory. Specifically, the arithmetic unit 25 includes a path acquirer 27 that acquires the target path of the autonomous mobile body 1, a trajectory generator 28 that generates the target trajectory of the autonomous mobile 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 to follow the target trajectory.

[0039] The path acquirer 27 receives the target path. The path acquirer 27 receives the target path from the path planning device 100. In this example, since the user selects one target path from among the plurality of target paths output from the path planning device 100, the path acquirer 27 receives the selected target path from the receiver 212.

[0040] The trajectory generator 28 generates a target trajectory from the current position of the autonomous mobile body 1 to the waypoint. The waypoint is a target position relatively close to the current position. The target route is input to the trajectory generator 28 from the route acquirer 27, and the map information and information about the autonomous mobile body 1 are input 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 autonomous mobile body 1 by 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 autonomous mobile body 1 avoids obstacles. The map information, information about the autonomous mobile body 1, information about other mobile bodies 1, and information about disturbances are input to the trajectory corrector 29 from the state acquirer 24. For example, the trajectory corrector 29 corrects the target trajectory by non-linear model predictive control (NMPC). As a result, a target trajectory that avoids obstacles is generated while following the target trajectory generated by the trajectory generator 28 as much as possible. The trajectory corrector 29 outputs the target position and the target speed as the target trajectory.

[0042] The operation amount calculator 210 calculates the operation amount of the actuator 12 corresponding to the target trajectory, that is, the control input. The operation amount calculator 210 calculates a command force for causing the autonomous mobile body 1 to follow the target trajectory. The map information, information about the autonomous mobile body 1, information about other mobile bodies 1, and information about disturbances are input to the operation amount calculator 210 from the state acquirer 24. The operation amount calculator 210 performs, for example, PID control. Note that the control of the operation amount calculator 210 is not limited to PID control and may be robust control or the like. At this time, the operation amount calculator 210 may limit the command speed corresponding to the target trajectory using, for example, a control barrier function (CBF).

[0043] Furthermore, the operation amount calculator 210 distributes the command force to the plurality of actuators 12 and calculates the respective command operation amounts of the plurality of actuators 12. When the actuator 12 is an electric motor, for example, the operation amount is the rotational speed or torque of the electric motor. When the actuator 12 is an engine, for example, the operation amount is the fuel injection amount.

[0044] Each actuator 12 operates according to the command operation amount. The actuator 12 may be provided with a dedicated controller for operating the actuator 12. For example, when the actuator 12 is an electric motor, the actuator 12 further has a servo amplifier. In that case, the servo amplifier operates the electric motor according to the command operation amount. As a result, the self-mobile body 1 exerts thrust and moves toward the target position.

[0045] The path planning device 100 generates the target path of the mobile body 1. The path planning device 100 can generate the target paths of a plurality of mobile bodies 1. That is, the mobile body 1 having the function of performing automatic driving and capable of receiving the target path from the path planning device 100 can be the object of path planning. For example, the path planning device 100 receives a request from the mobile body 1, generates a target path, and transmits it.

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

[0047] The monitoring device 3 includes a fixed monitoring device 3 and a mobile monitoring device 3. For example, when distinguishing each of the plurality of monitoring devices 3, an alphabetic suffix is attached after the symbol "3" (see FIG. 1). In this example, the monitoring devices 3A, 3B, 3C, and 3D are fixed monitoring devices. The monitoring device 3E is a mobile monitoring device. When not distinguishing the monitoring devices 3A, 3B, 3C, 3D, and 3E from each other, they are simply referred to as "monitoring device 3". The monitoring device 3 has a monitoring sensor that acquires monitoring information within a predetermined range. The monitoring information may include terrain, buildings, features, facilities, moving objects, wind direction, or tidal current, etc. The monitoring sensor includes at least one of a camera, LiDAR (Light Detection And Ranging), an infrared sensor, a laser rangefinder, a Doppler LiDAR, and an anemometer. For example, the camera takes still images or moving images. The laser rangefinder may employ a green laser.

[0048] The fixed monitoring device 3 is fixedly installed in the environment. The environment means the surrounding environment in which the moving object 1 can move. The fixed monitoring device 3 is installed, for example, on a building, a street lamp, or a telegraph 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 invariant or changeable. In this example, the fixed monitoring device 3 is configured to be able to change its orientation by 360 degrees. That is, the fixed monitoring device 3 can change the monitoring range over time and acquire monitoring information for 360 degrees around it.

[0049] The mobile monitoring device 3 is a moving object such as an aircraft or a vehicle. For example, the mobile monitoring device 3 is a drone as an aircraft. The mobile monitoring device 3 is equipped with the aforementioned monitoring sensor and acquires monitoring information for any area in the environment by freely flying within 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 the detection result of the monitoring sensor 11 provided in the moving body 1 from the moving body 1. The moving body 1 provided with the monitoring sensor 11 monitors obstacles around the moving body 1 with the monitoring sensor 11. The path planning device 100 acquires obstacle information around the moving body 1 by receiving the detection result of the monitoring sensor 11 of the moving body 1.

[0052] The path planning device 100 may acquire information of the moving body 1 from the 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 path planning device 100 uses the information of the moving body 1 itself acquired from the moving body 1 as obstacle information for another 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 performs state estimation of the moving body 1, the obstacle information received from the moving body 1 may include the state estimation value estimated by the moving body 1. Specifically, the obstacle information may include information about the own moving body 1 such as the current position or speed of the own moving body 1, information about another moving body such as the relative position or speed of another moving body 1 with respect to the own moving body 1, information about an obstacle such as the relative position or speed of the obstacle with respect to the own moving body 1, or information about disturbance.

[0054] FIG. 5 is a diagram showing the hardware configuration of the path planning device 100. The path planning device 100 has a processor 41, a storage 42, a memory 43, and a communicator 44.

[0055] The processor 41 generates the target path of the mobile body 1 based on the obstacle information. The processor 41 performs various arithmetic processes. For example, the processor 41 is formed of a processor such as a CPU (Central Processing Unit). The processor 41 may 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 memory 42 stores the programs and various data executed by the processor 41. For example, the memory 42 stores the path planning program 42a. The memory 42 is formed of a non-volatile memory, an HDD (Hard Disc Drive), an SSD (Solid State Drive), 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 communicator 44 performs wireless communication with an external device. The communicator 44 communicates with, for example, the mobile body 1.

[0058] FIG. 6 is a functional block diagram of the processor 41. The processor 41 realizes various functions by reading out and expanding the path planning program 42a from the memory 42 to the memory 43. Specifically, the processor 41 functions as an acquirer 45 that acquires obstacle information including information regarding the positions of obstacles, and a generator 46 that generates the target path of the mobile body 1 from the starting 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 body 1 and the obstacles.

[0059] Obstacle information includes at least one of the position, velocity, and type of the obstacle. The obstacle may include ground features, buildings, moving objects, construction sites, uneven terrain, shallows, aquaculture farms, etc. Since the route planning device 100 generates the target route of each moving object 1 performing autonomous driving, one moving object 1 can be an obstacle to another moving object 1. The information of the moving object includes, for example, the type of the moving object. The type of the moving object is, for example, a ship, a vehicle, an aircraft, etc. Further, the type of the moving object may include detailed types such as large ships, medium-sized ships, and small ships.

[0060] Information such as ground features, buildings, construction sites, uneven terrain, shallows, and aquaculture 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 can acquire information about the moving object 1 from the monitoring sensor 11. The acquirer 45 may also acquire information about the moving object 1 from the moving object 1 itself. The acquirer 45 can acquire information about the moving object 1 itself as obstacle information from the moving object 1. When the moving object 1 has a function of detecting another moving object 1, the acquirer 45 may acquire information about another moving object 1 from the moving object 1.

[0062] The acquirer 45 sequentially acquires obstacle information. Specifically, the monitoring device 3 transmits monitoring information to the route planning device 100 at an appropriate timing. The moving object 1 transmits 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 accumulates in the memory 42. Thereby, the acquirer 45 acquires the change over time of the obstacle information.

[0063] The generator 46 generates a target path of the moving body from the starting point S to the target point G. The generator 46 searches for a plurality of candidates for the target path that avoids interference between the moving body 1 and the obstacles, and determines the target path based on the path cost from among the plurality of candidates. The generator 46 predicts the change over time in the position of the obstacles and searches for a plurality of candidates that avoid interference between the moving body 1 and the obstacles. The generator 46 sequentially updates the search for the plurality of candidates and the determination of the target path. For example, the starting 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] Specifically, the generator 46 includes a predictor 47 that predicts the change over time in the position of the obstacles, a space generator 48 that generates a search space which is the space for executing path search, and a searcher 49 that searches for a plurality of candidates for the target path in the search space and determines the target path from among the plurality of candidates.

[0065] The predictor 47 predicts the change over time in the position of the obstacles based on the obstacle information. Specifically, the predictor 47 predicts the change over time in the position of the obstacles based on the obstacle information acquired by the acquirer 45. The predictor 47 obtains a prediction area of the obstacles where the obstacles may exist in the future. Note that when the obstacle is a stationary object, the position does not change. When the obstacle is a stationary object, the position of the obstacle predicted by the predictor 47 is 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 moving body 1 moves. That is, the position of the obstacle is the two-dimensional position of the obstacle in the plane in which the moving body 1 moves and does not include the height position.

[0066] The prediction area is an area where the obstacles exist or are likely to exist in the future. The predictor 47 obtains continuous or discrete prediction areas according to time. The prediction area at each time is a two-dimensional area in the two-dimensional space in which the moving body 1 moves. The prediction area has a size that includes the planar shape, i.e., the two-dimensional shape, of the obstacle. The prediction area is represented by, for example, a circle, an ellipse, or a polygon.

[0067] Predictor 47 may predict the behavior of an obstacle over time based on the obstacle information and determine a predicted area of the obstacle where the obstacle may exist. The prediction of the obstacle behavior is executed by various known methods. For example, when the obstacle information includes the speed of the obstacle, Predictor 47 may predict the movement range of the obstacle assuming that the obstacle performs uniform motion. Alternatively, Predictor 47 may obtain the speed of the obstacle based on the change over time of the obstacle information and predict the movement range of the obstacle moving at the obtained speed with uniform motion. Alternatively, Predictor 47 may assume a predicted trajectory of the obstacle with a motion model and obtain a prediction distribution using a Kalman filter, a Bayesian filter, or the like. At this time, Predictor 47 estimates various parameters of the motion model based on the type and outer shape of the obstacle. Alternatively, Predictor 47 may estimate the motion model or the distribution of the input using data-driven control. Predictor 47 may use past obstacle information to estimate a probabilistic motion model and the distribution of the input by machine learning (such as a Gaussian process or Bayesian estimation). Predictor 47 may calculate the prediction distribution of the obstacle by a sampling method such as the MCMC (Markov Chain Monte Carlo) method. Alternatively, Predictor 47 may approximate the prediction distribution of the obstacle by moment matching or the like.

[0068] Figure 7 is an example of a search space. The search space is a three-dimensional space obtained by adding time as a dimension to the two-dimensional space in which the mobile body 1 moves. In Figure 7, the XY plane is the two-dimensional space in which the mobile body 1 moves. The Z axis is the time axis. That is, the larger the Z coordinate, the more time has elapsed. The starting point S is located in the layer with the earliest time in the time axis direction. The target point G is located in the layer with the most advanced time in the time axis direction. In this example, the target point G is represented by a region having a predetermined area.

[0069] The space generator 48 creates a three-dimensional obstacle region 61 that includes the future predicted positions of obstacles in the search space. The space generator 48 creates the obstacle region 61 based on the prediction region obtained by the predictor 47. The prediction region at each time is a two-dimensional region. The space generator 48 arranges the two-dimensional prediction regions at each time in the search space in the time axis direction and creates a three-dimensional region by continuously connecting these prediction regions. This three-dimensional region is the obstacle region 61. In FIG. 7, five obstacle regions 61A, obstacle region 61B, obstacle region 61C, obstacle region 61D, and obstacle region 61E are created. When not distinguishing each of them, it is simply referred to as "obstacle region 61".

[0070] The obstacle regions 61A and 61B correspond to stationary obstacles. For example, the obstacle region 61A corresponds to the quay wall 91. The obstacle region 61B corresponds to the breakwater 92. The obstacle regions 61A and 61B corresponding to stationary obstacles are regions where the planar shape of the obstacle simply extends in the time axis direction. That is, the obstacle regions 61A and 61B have a shape parallel to the time axis. The obstacle regions 61C, 61D, and 61E correspond to moving obstacles. For example, the obstacle regions 61C, 61D, and 61E each correspond to another moving object 1. The obstacle regions 61C, 61D, and 61E corresponding to moving obstacles are regions that are linearly or curvilinearly inclined with respect to the time axis. When the movement of the obstacle is at a constant speed, the obstacle regions 61C, 61D, and 61E are linearly inclined with respect to the time axis. When the movement of the obstacle is non-uniform, the obstacle regions 61C, 61D, and 61E are curvilinearly inclined with respect to the time axis.

[0071] The explorer 49 explores a plurality of candidates for the target path that do not interfere with the obstacle region 61 in the search space. That is, the explorer 49 explores the plurality of candidates while avoiding the obstacle region 61 in the search space. Since obstacles are regarded as stationary solids in the search space, the path is explored without considering the time-dependent behavior of the obstacles. The explorer 49 utilizes the RRT-star algorithm to explore a plurality of candidates. The configuration of each candidate depends on the search method. For example, each candidate includes a plurality of nodes.

[0072] Since the search space has a time axis, when the explorer 49 explores a path, it explores new nodes on the side where time elapses in the time axis direction. Furthermore, since the mobile body 1 has a maximum speed, the range (hereinafter referred to as the "movable range") A within which the mobile body 1 can move per unit time is limited. In a three-dimensional search space, the movable range A is a conical (inverted conical in FIG. 7) region that expands toward the side where time advances in the time axis direction with the starting point S as the apex. The inclination of the generatrix of the cone with respect to the time axis corresponds to the maximum speed of the mobile body 1. The explorer 49 explores a plurality of candidates within the movable range A. Furthermore, when exploring a new node connected to a certain node, the inclination of the edge connecting the two nodes with respect to the time axis is also limited by the maximum speed of the mobile body 1. That is, a new node connected to a certain node is explored under the condition that the inclination of the edge with respect to the time axis is equal to or less than a predetermined threshold value. The threshold value corresponds to the maximum speed of the mobile body 1.

[0073] FIG. 8 is a schematic diagram showing a plurality of candidates explored in the search space. The search space in FIG. 8 is the same as that in FIG. 7. The explorer 49 explores new nodes that do not interfere with the obstacle region 61 from the starting point S toward the side where time advances, and generates a path reaching the target point G as a candidate for the target path. In the example of FIG. 8, three candidates R1, R2, and R3 are generated by the explorer 49.

[0074] At this time, the explorer 49 searches for a plurality of candidates based on the nonholonomic characteristics of the mobile body 1. For example, the explorer 49 searches for a path on the condition that the mobile body 1 does not skid laterally. Specifically, each candidate includes a plurality of nodes, and the nodes are defined by the position and orientation (angle) of the mobile body 1. When setting a new node connected to one node, the explorer 49 assumes that the mobile body 1 moves in an arc from one node to the new node, and determines the orientation of the mobile body 1 at the new node. The position of the mobile body 1 that defines the node is searched in a three-dimensional search space including the time axis, while the orientation of the mobile body 1 that defines the node is searched in the two-dimensional space in which the mobile 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 body 1 moves. It is assumed that the turning radius during the movement between the nodes is constant. Let the coordinates of the already set node m in the global coordinate system of the two-dimensional space be (x m , y m ). The orientation of the mobile body 1 is represented by the angle with respect to the positive part of the x-axis of the global coordinate system. Let the orientation of the mobile body 1 at the node m be θm. Let the coordinates of the new node n in the global coordinate system of the two-dimensional space be (x n , y n ). The orientation matrix of the mobile body 1 at the node m is represented by the following equation (1).

[0076]

Equation

[0077] A local coordinate system of the mobile body 1 is set, with the traveling direction of the mobile body 1 as the x'-axis and the direction orthogonal to the traveling direction as the y'-axis. The front side in the traveling direction is defined as positive for the x'-axis, and the right side with respect to the traveling direction is defined as positive for the y'-axis. Let the coordinates of the node n in the local coordinate system be (x b , y b ), then the following relational expressions hold.

[0078]

Equation

[0079] When the moving body 1 moves from node m to node n in an arc shape, it rotates around a point C located on the y'-axis. The traveling direction of the moving body 1 at node n is along the tangent direction of a circle centered at point C with a radius r.

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

[0081]

Equation

[0082] Let the turning angle of the moving body 1 from node m to node n be Δθ. The change in the posture of the moving body 1 at node n with respect to the moving body 1 at node m, that is, the relative angle θ mn has the following relationship with the turning angle Δθ.

[0083]

Equation

[0084] Also, from the geometric relationship in FIG. 10, the following formula holds.

[0085]

Equation

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

[0087]

Mathematics

[0088] Therefore, by the Chain Rule of poses, the pose matrix Rn of node n is represented by the following equation.

[0089]

Mathematics

[0090] When new node position coordinates are set for a single node, the pose of the moving body 1 at the new node is determined by the pose matrix of Equation (7). Thus, the explorer 49 sequentially sets the position and pose of the new node and generates one candidate.

[0091] At this time, when the turning radius r represented by Equation (3) is less than a predetermined threshold value, the explorer 49 excludes the corresponding node from the candidates for the new node and re - selects another node as the new node. For example, the threshold value is the minimum turning radius specific to the moving body 1. According to the non - holonomic characteristics of the moving body 1, the moving body 1 cannot turn with a turning radius r that is too small. The explorer 49 can obtain 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, the minimum turning radius corresponding to the size or type of the moving body 1 is stored in advance in the memory 42. The explorer 49 reads the minimum turning radius corresponding to the obstacle information from the memory 42.

[0092] In this way, the explorer 49 explores a path based on the non - holonomic characteristics of the moving body 1 by determining the pose of the moving body 1 at the new node on the premise that the moving body 1 turns in an arc shape. Furthermore, the explorer 49 also explores a path based on the non - holonomic characteristics of the moving body 1 by excluding the corresponding node when the turning radius r is too small.

[0093] The explorer 49 determines a target route based on the route cost from among a plurality of candidates. That is, the explorer 49 obtains the route cost of each of the plurality of candidates. The explorer 49 determines, as the target route, the candidate with the minimum route cost from among the plurality of candidates. 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 plurality of candidates. The degree of meandering means the magnitude of curvature, the number of turns, or the number of changes in the turning direction, etc. A large degree of meandering means a large curvature, a large number of turns, or a large number of changes in the turning direction, etc. Normally, without any constraints, the mobile body 1 moves linearly from the starting point S toward the target point G. In this case, the degree of meandering of the route is small. However, when the mobile body 1 detours around an obstacle, the route will meander. The more obstacles are detoured, the more complex the route becomes and the greater the degree of meandering. That is, a large meandering cost means that the route becomes complex and the risk of interference between the mobile body 1 and the obstacle is high.

[0094] In this example, the meandering cost may include a curvature cost related to the curvature in each of the plurality of candidates. A curvature cost corresponding to the curvature is assigned to the portion where the mobile body 1 turns in each candidate. When a plurality of turning portions are included, a curvature cost is assigned to each of the turning portions. The greater the curvature of the 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 with respect to a plurality of nodes included in each candidate. As described above, each candidate includes a plurality of nodes connected in sequence. Although each candidate is strictly a polyline, the moving body 1 actually moves in a curved shape following a plurality of nodes. The searcher 49 searches for a plurality of nodes on the premise that the moving body 1 turns, that is, moves in an arc shape, 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. That is, the turning radius r of the moving body 1 is the radius of curvature between each two nodes. The searcher 49 assigns a larger curvature cost between the corresponding two nodes as the turning radius r is smaller. Among the plurality of nodes, there may be two nodes connected in a substantially straight line. A small curvature cost is assigned between such two nodes. The searcher 49 evaluates the curvature between all nodes in each candidate and assigns a curvature cost. As a result, the larger the number of turning portions included in the path, the larger the total curvature cost of the candidate, that is, the overall curvature cost. Alternatively, when the path includes a turning portion with a large curvature, the overall curvature cost of the candidate becomes large.

[0096] The path cost may include a cost related to distance, that is, a distance cost. For example, when each candidate includes a plurality of nodes, a distance cost corresponding to the straight-line distance between each two connected nodes may be assigned. The longer the straight-line distance, the larger the distance cost assigned. The overall distance cost of each candidate can be evaluated by the sum of the distance costs between all nodes included in each candidate.

[0097] The path cost may include a cost related to the degree of approach to an obstacle, that is, an approach cost. For example, when each candidate includes a plurality of nodes, a cost corresponding to the shortest distance between each node and surrounding obstacles may be assigned as the approach cost. The shorter the shortest distance to the obstacle, the larger the approach cost assigned. The degree of approach of each candidate to the obstacle can be evaluated by the total value of the approach costs of each candidate.

[0098] The explorer 49 determines one or more target paths from among a plurality of candidates. When one target path is determined, the explorer 49 determines, as the target path, the candidate with the minimum path cost from among the plurality of candidates. When a plurality of target paths are determined, the explorer 49 determines, as the plurality of target paths, the number of candidates corresponding to the number of target paths in ascending order of path cost from among the plurality of candidates. The path cost includes at least a meandering cost. Thereby, the explorer 49 determines, as the target path, a candidate with a smaller degree of meandering from among the plurality of candidates. Meandering occurs when the moving body 1 bypasses an obstacle. When the moving body 1 moves through a group of a plurality of obstacles, the number of turns may increase or the turns may become abrupt. That is, when the moving body 1 moves through the plurality of obstacles in a sewing-like manner, the degree of meandering of the path is likely to increase. When the moving body 1 moves through the plurality of obstacles in a sewing-like manner, the risk of the moving body 1 interfering with the obstacles also increases. On the other hand, when the moving body 1 moves by making a large detour around a group of a plurality of obstacles, the number of turns may decrease or the turns may become gentle. That is, the degree of meandering of the path is likely to decrease. In that case, the risk of the moving body 1 interfering with the obstacles also decreases. By the explorer 49 determining the target path in consideration of the meandering cost, a candidate that makes a large detour around a group of a plurality of obstacles tends to be determined as the target path.

[0099] FIG. 11 is a two-dimensional schematic diagram of a plurality of candidates that have been explored. In FIG. 11, a plurality of candidates R1, R2, and R3 are shown on the two-dimensional space in which the moving body 1 moves. The plurality of candidates R1, R2, and R3 in FIG. 11 correspond to the plurality of candidates R1, R2, and R3 in FIG. 8. Regarding the obstacle regions 61C, 61D, and 61E corresponding to the moving obstacles, the position when the moving body 1 is located at the starting point S is represented by a solid line, and the predicted movement range is represented by a two-dot chain line.

[0100] Candidate R1 passes between obstacle regions 61A and 61C from starting point S, bypasses obstacle region 61D to the left, and reaches target point G. By bypassing obstacle region 61D to the left, candidate R1 avoids passing between obstacle regions 61D and 61E. Therefore, although the overall distance of candidate R1 becomes slightly longer, the number of meanders of candidate R1 is relatively small, and the curvature of the turning portions included in candidate R1 is relatively small.

[0101] Candidate R2 passes between obstacle regions 61A and 61C from starting point S, passes between obstacle regions 61D and 61E, and reaches target point G. Candidate R2 passes between obstacle regions 61D and 61E. Therefore, although the overall distance of candidate R2 becomes relatively short, the number of meanders of candidate R2 is large, and the curvature of the turning portions included in candidate R2 is large.

[0102] Candidate R3 passes between obstacle regions 61B and 61C from starting point S, bypasses obstacle region 61E to the right, and reaches target point G. By bypassing obstacle region 61E to the right, candidate R3 avoids passing between obstacle regions 61D and 61E. Therefore, although the overall distance of candidate R3 becomes slightly longer, the number of meanders of candidate R3 is relatively small, and the curvature of the turning portions included in candidate R3 is relatively small.

[0103] In this example, the path cost includes a distance cost in addition to a meander cost. Comparing the three candidates R1, R2, and R3, with respect to the distance, the distance cost of candidate R2 is the smallest, and the distance cost of candidate R3 is the largest. With respect to meandering, the meander cost of candidate R1 is the smallest, and the meander cost of candidate R2 is the largest. With respect to the overall path cost including the distance cost and the meander cost, the path cost of candidate R1 is the smallest. That is, 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 mobile body 1 and the obstacles is small.

[0104] When the explorer 49 determines a single target path, it comprehensively judges the risk of interference and the overall distance, and determines candidate R1 as the target path. When the explorer 49 determines a plurality of, for example, two target paths, it comprehensively judges the risk of interference and the overall distance, and determines candidate R1 and candidate R3 as the target paths.

[0105] The explorer 49 outputs the determined target path. In this example, the target path is transmitted to the target moving body 1.

[0106] Note that the path cost may include an approach cost. However, in this example, the approach costs of candidates R1, R2, and R3 are all of the same degree.

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

[0108] In this example, obstacle information is appropriately transmitted from the monitoring sensor 11, and the acquirer 45 acquires the obstacle information at any time. In addition, the acquirer 45 also acquires the obstacle information transmitted from the moving body 1 at any time. That is, in parallel with the processing after step S101 described later, acquiring obstacle information including information on the position of the obstacle is being executed.

[0109] Under such circumstances, in step S101, the generator 46 determines whether the generation condition is satisfied. The generation condition is a condition for starting the route plan. For example, it is that the route planning device 100 receives a request from the moving body 1. That is, the route planning device 100 executes the route plan upon receiving a request from the moving body 1. The generation condition may be that a predetermined generation period arrives. The generation period arrives at a predetermined time interval. That is, the route planning device 100 periodically executes the route plan. The generation condition may be that a predetermined time has elapsed since the previous target route was generated. That is, when a certain amount of time has elapsed since the target route was generated, the route planning device 100 executes the route plan again. The generation condition may be that the surrounding environment has changed since the previous target route was generated. The change in the surrounding environment is that a new obstacle is detected, or a known obstacle behaves differently from the predicted behavior, etc. As will be described later, the behavior of the obstacle is predicted in order to generate the target route. The generator 46 determines that there has been a change in the surrounding environment when the current position of the obstacle, that is, the position based on the latest obstacle information, is outside the most recent prediction area. The generator 46 also determines that there has been a change in the surrounding environment when a new moving body 1 is detected based on the latest obstacle information. That is, when the predicted behavior of the obstacle that was the premise of the previous target route deviates from the actual behavior of the obstacle, the route planning device 100 executes the route plan again. The generation condition may be that any one of the above-described request reception, generation period arrival, predetermined time elapse, and change in the surrounding environment is satisfied.

[0110] If the generation condition is not satisfied, the generator 46 repeats step S101 and waits for the generation condition to be satisfied.

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

[0112] Subsequently, 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 body 1 moves.

[0113] Then, in step S104, the explorer 49 searches for a plurality of candidates for the target path within the search space. The explorer 49 searches for a plurality of candidates where the moving body 1 does not interfere with the obstacle region. At this time, the explorer 49 searches for a path based on the non-holonomic characteristics of the moving body 1. Specifically, when the explorer 49 searches for the next node from one node, it searches for the next node that can be moved according to the non-holonomic characteristics of the moving body 1.

[0114] Furthermore, the explorer 49 determines the target path based on the path cost among the plurality of candidates. The explorer 49 determines the candidate with the minimum path cost among the plurality of candidates as the target path. The path cost includes a slalom cost. As a result, a candidate with a gentle slalom or a small number of slaloms is determined as the target path. Note that the path cost may further include a distance cost or an approach cost. As a result, in addition to a small degree of slalom, a candidate with a short overall distance or not approaching the obstacle too much is determined as the target path. Step S104 corresponds to generating the target path of the moving body from the starting point to the target point based on the obstacle information so as to avoid interference between the moving body and the obstacle.

[0115] When the target path is determined, in step S105, the generator 46 transmits the target path to the target moving body 1.

[0116] After the target path is transmitted, the generator 46 determines the determination of the generation condition again in step S101. As a result, the generation of the aforementioned target path is repeatedly executed. The generation of the target path is repeated until the target moving body 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 the 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 a plurality of target routes are sent from the route planning device 100 will be described.

[0118] When the control device 2 receives a plurality of target routes from the route planning device 100, in step S201, the display 15 displays images of the plurality of target routes. Specifically, the image generator 211 generates images of the plurality of 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 nautical chart including the two target routes. At this time, the display 15 displays the respective priorities together with the plurality of target routes.

[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 priority of the target route P1 is the highest. In the example of FIG. 14, the display 15 displays the target route P1 with the highest priority as a solid line and the target route P3 as a dashed line. The display 15 may display the respective route costs of the target routes P1 and P3. The user can select one target route via the input device 16 with reference to the target routes displayed on the display 15.

[0120] Subsequently, when the user selects one target route from among the plurality of target routes via the input device 16, in step S202, the receiver 212 receives the selection of 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 target route P1 is selected by the user in the image shown in FIG. 14, the receiver 212 sets the target route P1 as the target route for automatic driving. The user can also select the target route P3.

[0122] When the target route is set, the control device 2 executes 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, and automatically drives the moving body 1.

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

[0124] When the control device 2 receives the target route, it returns to step S201. That is, the display 15 updates the display of the target route. In step S202 after the update of the target route, if there is no selection input from the user, the control device 2 sets a target route close to the target route set before the update as the target route to be used for automatic driving, and continues automatic driving in step S203. That is, after the automatic driving is started, unless the user changes the target route, the initially selected target route is adjusted by the update, and the automatic driving continues along the target route.

[0125] In addition, when there is only one target route output from the route planning device 100, step S202 is deleted from the flowchart of FIG. 13. The display 15 displays the received single target route (step S201), and the control device 2 executes automatic driving according to the single target route (step S203). When the single target route is updated, the display 15 displays the updated target route (step S201), and the control device 2 executes automatic driving according to the updated target route (step S203).

[0126] According to such a path plan, a target path with a small degree of meandering is generated. A small degree of meandering is correlated with less detouring of obstacles by the moving body 1. Less detouring of obstacles means that the risk of the moving body 1 interfering with the obstacles is small. For example, when there are a plurality of obstacles, a path that bypasses the plurality of obstacles as a whole is determined as the target path rather than a path that moves the shortest distance through the gaps between the plurality of obstacles. As a result, the risk of the moving body 1 interfering with the obstacles is significantly reduced.

[0127] Furthermore, the temporal change in the position of the obstacle is predicted, and based on the predicted area of the obstacle, a path is searched for that avoids interference between the moving body 1 and the obstacle. As a result, the accuracy of path generation that avoids interference with the obstacle is improved.

[0128] In addition, the path search is performed in a three-dimensional search space obtained by adding a time axis to the two-dimensional space in which the moving body 1 moves. As a result, the moving obstacle is regarded as a stationary solid, and the path is searched. As a result, the computational load of the path search can be reduced.

[0129] The path planning device 100 updates the target path by repeatedly executing such path planning. Obstacles can exhibit unexpected behavior. Even when path planning is performed based on the predicted behavior of a moving obstacle, the updated target path can handle unexpected behavior of the obstacle.

[0130] The path cost referred to when selecting the target path includes a meandering cost related to the degree of meandering. The meandering cost includes a curvature cost related to the curvature of each candidate path. A large curvature of the path means that the moving body 1 makes a sharp turn. A sharp turn may result in a sudden avoidance of an obstacle by the moving body 1. That is, a small curvature cost means less sudden detouring of the obstacle, and as a result, a small risk of interference between the moving body 1 and the obstacle. Thus, by considering the curvature of the path, the target path can be determined in consideration of the risk of interference between the moving body 1 and the obstacle.

[0131] Specifically, a curvature cost corresponding to the curvature is applied to the turning part of the moving body 1 included in each candidate path. More specifically, when searching for candidate target paths, the non-holonomic characteristics of the moving body 1 are considered, and nodes are searched on the premise that the moving body 1 turns and moves. When a node is searched, the turning radius r of the moving body 1, that is, the radius of curvature, is obtained. A curvature cost is applied based on the turning radius r between each pair of connected nodes. Thereby, the curvature of the path is evaluated in consideration of the actual behavior of the moving body 1.

[0132] As described above, by searching for a plurality of candidates based on the non-holonomic characteristics of the moving body 1, a path faithful to the actual behavior of the moving body 1 can be searched. Furthermore, in addition to searching for candidates, the curvature of the path can be derived.

[0133] By searching for the position of a new node in a three-dimensional search space including the time axis and obtaining 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 path that realizes both avoidance of interference with moving obstacles and movement of the moving body 1 in accordance with non-holonomic characteristics.

[0134] Since the target path is displayed on the display 15 of the moving body 1, the user, that is, the passenger, can confirm the target path in the automatic driving. For example, when the future path is unclear, the user may feel uneasy about the behavior of the moving body 1. However, by knowing the future path, the user can reduce uneasiness about the behavior of the moving body 1. Alternatively, when the target path of the automatic driving is a path that the user does not want, the user can also switch the automatic driving to manual driving.

[0135] Furthermore, the path planning device 100 outputs a plurality of target paths, and the display 15 displays the plurality of target paths. Thereby, the user can execute the determination of one target path from among the plurality of target paths. That is, present the user with target paths narrowed down to several based on the path cost, and the user can determine the final target path considering factors other than the path cost.

[0136] At this time, by also displaying the priority of the target route on the display 15, it is possible to assist the user in selecting the target route.

[0137] 《Other Embodiments》 As described above, the above embodiments have been described as examples of the technology disclosed in the present application. However, the technology in the present disclosure is not limited to this, and is also applicable to embodiments in which appropriate changes, replacements, additions, omissions, etc. are made. It is also possible to combine the components described in the above embodiments to form a new embodiment. In addition, among the components described in the accompanying drawings and the detailed description, there may be included not only the components essential for solving the problem, but also the components not essential for solving the problem for the purpose of exemplifying the technology. Therefore, just because those non-essential components are described in the accompanying drawings and the detailed description, it should not be immediately determined that those non-essential components are essential.

[0138] For example, the moving body 1 that receives the target route is not limited to the above example. For example, the moving body 1 may be a vehicle, an autonomous robot (e.g., a carrier pallet), an aircraft (e.g., a drone), etc. Any moving body can be adopted as long as the moving body 1 can execute autonomous driving. Another moving body 1 that may become an obstacle in the generation of the target route does not necessarily have to have an autonomous driving function. Another moving body 1 may be a living being such as a human. For example, another moving body 1 may be a pedestrian passing on the road or a worker working within the site. The number of moving bodies 1 included in the driving support system 1000 is not limited to the example in FIG. 1 and is arbitrary.

[0139] The actuator 12 is an actuator related to the movement of the moving body 1. The actuator 12 is not limited to an electric motor, an engine, and a rudder. When the moving body 1 is a vehicle, the actuator 12 can be a steering device. When the moving body 1 is a drone, the propeller is the actuator.

[0140] The configuration of the control device 2 of the moving body 1 is not limited to the foregoing configuration. Any configuration may be adopted as long as the automatic driving of the moving body 1 along the target path can be executed. For example, the control device 2 may not estimate the state of the self-moving body 1 or other moving bodies 1, but may acquire the estimated state of the self-moving body 1 or other moving bodies 1 from an external device such as the path planning device 100. Alternatively, the control device 2 may estimate the state of the self-moving body 1, and the state of the other moving bodies 1 may be estimated by the other moving bodies 1 or the path planning device 100 or the like. In that case, the control device 2 acquires the estimation 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, modifying the target trajectory, and calculating the operation amount. For example, a learned model by machine learning may be used, and the target trajectory may be output by inputting the state quantity of the self-moving body 1 and the state quantity of the other moving bodies 1.

[0141] The path planning device 100 is not limited to the control tower 120 and may be arranged at any location. For example, the path planning device 100 may be arranged in the moving body 1 that performs automatic driving. In that case, the moving body 1 generates a target path by the path planning device 100 and executes automatic driving along the target path by the control device 2. The path planning device 100 may not be a single device but may be formed by a plurality of devices.

[0142] The path planning device 100 may request obstacle information from the monitoring sensor 11 or the moving body 1 or the like. For example, the acquirer 45 may periodically request obstacle information from the monitoring sensor 11 and the moving body 1 or the like, and acquire the reply of the obstacle information from the monitoring sensor 11 and the moving body 1 or the like.

[0143] Any method may be adopted for generating the target path. For example, the predicted areas of the moving obstacles at each time may not have the same area. Since it becomes difficult to predict the position of the obstacle as time passes, the predicted area at each time may become larger as time progresses. That is, the obstacle area may be an area that expands toward the side where time progresses.

[0144] The search for a path is not limited to RRT-star. The search for a path may utilize search algorithms such as A-star, Hybrid A-star, kinodynamic RRT-star, and RRT. Further, the search for a path may not be executed based on the nonholonomic characteristics of the mobile body 1.

[0145] The curvature of the curved portion included in each path candidate is not limited to being based on the turning radius r of the mobile body 1 during the search for nodes. The curvature of the curved portion can be evaluated by any method. For example, the curvature of the curved portion may be obtained by curvilinearly interpolating the path formed by a plurality of nodes.

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

[0147] The display of the target path by the display 15 is not essential. The generation of the target path by the path planning device 100 and the automatic driving according to the target path by the mobile body 1 may be performed without presenting the target path to the user.

[0148] Even when the display 15 displays the target path, the display 15 may display only one target path used for automatic driving. For example, the path planning device 100 may output only one target path. Alternatively, the display 15 may display only the target path with the highest priority among the plurality of target paths output from the path planning device 100, for example, the target path with the smallest path cost.

[0149] The flowcharts of FIGS. 12 and 14 are merely examples. The steps in the flowcharts may be appropriately changed, replaced, added, omitted, etc. Also, the order of the steps in the flowchart may be changed, or serial processing may be performed in parallel. For example, the generation conditions of step S101 may be arbitrarily set. The generation conditions may be at least one of the reception of the aforementioned request, the arrival of the generation cycle, the elapse of a predetermined time, and the change in the surrounding environment, or any combination thereof.

[0150] The functions realized by the components described in this specification may be implemented in circuitry or processing circuitry including a general-purpose processor, a specific-purpose processor, an integrated circuit, ASICs (Application Specific Integrated Circuits), a CPU (a Central Processing Unit), a conventional circuit, and / or a combination thereof, which are programmed to realize the described functions. The processor includes transistors and other circuits and is regarded as a circuit or processing circuitry. The processor may be a programmed processor that executes a program stored in a memory.

[0151] In this specification, circuitry, unit, and means are hardware programmed to realize the described functions or hardware that executes them. The hardware may be any hardware disclosed in this specification or any hardware known to be programmed or execute to realize the described functions.

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

[0153] [Aspect] 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 positions of obstacles, and a generator 46 that generates a target path of the moving body 1 from the starting point S to the target point G based on the obstacle information so as to avoid interference between the moving body 1 and the obstacles. The generator 46 searches for a plurality of candidates for the target path, determines the target path from among the plurality of candidates based on the path cost, and the path cost includes a meandering cost related to the degree of meandering of each of the plurality of candidates.

[0155] According to this configuration, since the path cost includes the meandering cost, the target path is determined after considering the degree of meandering of each candidate. If the degree of meandering of the path is large, there is a possibility that a sudden detour of the obstacle by the moving body 1 or many detours of the obstacle by the moving body 1 are included in the path. In autonomous driving, it is natural for the moving body 1 to avoid interference with obstacles and move, but if the detour is sharp or the number of detours is large, the risk of interference between the moving body 1 and the obstacles increases. By determining the target path in consideration of the degree of meandering of the path, a target path with a low risk of interference between the moving body 1 and the obstacles is generated.

[0156] (Aspect 2) In the path planning device 100 described in Aspect 1, the generator 46 predicts the change over time in the positions of the obstacles and searches for the plurality of candidates that avoid interference between the moving body 1 and the obstacles.

[0157] According to this configuration, since a plurality of candidates that avoid interference between the moving body 1 and the obstacles are searched after predicting the change over time in the positions of the obstacles, a target path that anticipates the future behavior of the obstacles is generated. That is, even for moving obstacles, a target path that avoids interference between the moving body 1 and the obstacles is generated.

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

[0159] According to this configuration, the moving obstacles can be regarded as stationary solids, and the path can be searched. Since it is not necessary to search for the path while considering the dynamic behavior of the obstacles, the computational load of the path search can be reduced.

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

[0161] According to this configuration, the curvature of each candidate is evaluated as the meandering cost. The meandering cost of a candidate with a large curvature of the path becomes high, and it becomes difficult to be selected as the target path. The curvature of the path is correlated with the sharpness of the turning during meandering. That is, when the curvature is large, the turning becomes sharp. A sharp turn may involve a high-risk movement such as suddenly bypassing an obstacle. By including the curvature cost as the meandering cost, a candidate with a gentle turn or a small number of sharp turns is more likely to be selected as the target path.

[0162] (Aspect 5) In the path planning device 100 described in any one of Aspects 1 to 4, the generator 46 searches for the plurality of candidates based on the non-holonomic characteristics of the moving body 1.

[0163] According to this configuration, unrealistic behaviors of the moving body 1 are excluded, and the path search is executed. For example, a path based on a smooth behavior that does not include movements such as skidding of the moving body 1 is searched. Thereby, a target path that conforms to the actual behavior of the moving body 1 is generated.

[0164] For example, as a non-holonomic characteristic of the moving body 1, when path search is executed on the premise of turning instead of bending during path change, it becomes easier to evaluate the curvature cost of each candidate. Specifically, when searching for a new node that constitutes a path, by assuming the turning of the moving body 1, the curvature of the turning part of the path 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 path 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 path.

[0166] According to this configuration, it is possible to cope with temporal changes in the surrounding environment. Specifically, the target path is generated based on the positions of obstacles so that the moving body 1 and the obstacles do not interfere with each other. The obstacles may move. By sequentially updating the target path, a target path corresponding to the moving obstacles can be generated.

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

[0168] According to this configuration, the passenger of the moving body 1 can confirm the target path used for automatic driving via the display 15. Thereby, the passenger can predict the future behavior of the moving body 1 in automatic driving.

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

[0170] According to this configuration, the path planning device 100 determines not one target path but a plurality of target paths. Then, a plurality of target paths are displayed on the display 15.

[0171] (Aspect 9) In the driving support system 1000 described in Aspect 7 or Aspect 8, a receiver 212 is further provided to receive from the user a selection of one of the plurality of target routes displayed on the display 15 for use in the automatic driving of the moving body 1.

[0172] According to this configuration, the occupant can select one target route for use in automatic driving from among the plurality of target routes presented by the display 15. Thereby, the route planning device 100 narrows down several target routes based on the route cost, and the user can determine the final target route considering factors other than the route cost from among the plurality of target routes presented.

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

[0174] According to this configuration, the occupant can know the superiority and inferiority of the plurality of target routes displayed on the display 15, particularly the superiority and inferiority regarding the route cost. The priorities are helpful when the occupant selects one target route.

[0175] (Aspect 11) A route planning method includes obtaining obstacle information including information on the positions of obstacles, and generating a target route of the moving body 1 from a starting point S to a target point G based on the obstacle information so as to avoid interference between the moving body 1 and the obstacles. 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.

[0176] According to this configuration, since the route cost includes the meandering cost, the target route is determined after considering the degree of meandering of each candidate. If the degree of meandering of the route is large, there is a possibility that the moving body 1 makes a sharp detour around an obstacle or makes many detours around the obstacle. In autonomous driving, it is natural for the moving body 1 to avoid interference with obstacles and move. However, if the detour is sharp or the number of detours is large, the risk of interference between the moving body 1 and the obstacle increases. By determining the target route in consideration of the degree of meandering of the route, a target route with a low risk of interference between the moving body 1 and the obstacle is generated.

[0177] (Aspect 12) The route planning program 42a causes a computer to realize a function of acquiring obstacle information including information on the positions of obstacles and a function of generating a target route of the moving body 1 from the starting point S to the target point G based on the obstacle information so as to avoid interference between the moving body 1 and the obstacles. The function of generating the target route searches for a plurality of candidates for the target route and determines the target route based on the route cost from among the plurality of candidates. The route cost includes a meandering cost related to the degree of meandering of each of the plurality of candidates.

[0178] According to this configuration, since the route cost includes the meandering cost, the target route is determined after considering the degree of meandering of each candidate. If the degree of meandering of the route is large, there is a possibility that the moving body 1 makes a sharp detour around an obstacle or makes many detours around the obstacle. In autonomous driving, it is natural for the moving body 1 to avoid interference with obstacles and move. However, if the detour is sharp or the number of detours is large, the risk of interference between the moving body 1 and the obstacle increases. By determining the target route in consideration of the degree of meandering of the route, a target route with a low risk of interference between the moving body 1 and the obstacle is generated.

Explanation of Reference Numerals

[0179] 1000 Driving support system 100 Route planning device 1 Moving body 15 Display 42a Route Planning Program 45 Acquirer 46 Generator 61 Obstacle Area S Starting Point G Target Point

Claims

1. An acquirer that acquires obstacle information including information regarding the position of an obstacle, 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, The generator searches for a plurality of candidates for the target path and determines the target path based on a path cost from among the plurality of candidates, The path cost includes a meandering cost related to the degree of meandering of each of the plurality of candidates, and is a path planning device.

2. In the path planning device according to Claim 1, The generator predicts a change over time in the position of the obstacle and searches for the plurality of candidates that avoid interference between the moving body and the obstacle, and is a path planning device.

3. In the path planning device according to Claim 2, 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, The path planning device searches for the plurality of candidates while avoiding the obstacle region in the three-dimensional space.

4. In the path planning device according to Claim 1, The meandering cost includes a curvature cost related to the curvature in each of the plurality of candidates, and is a path planning device.

5. In the path planning device according to Claim 1, The generator searches for the plurality of candidates based on the non-holonomic characteristics of the moving body, and is a path planning device.

6. In the path planning device according to Claim 1, The generator sequentially updates the search for the plurality of candidates and the determination of the target path, and is a path planning device.

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

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

9. In the driving support system according to Claim 8, The driving support system further includes a receiver that receives a selection of one of the target paths 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 path planning device assigns priorities according to the path cost to the plurality of target paths, The display device is a driving support system that further displays the priorities of the plurality of target paths.

11. acquiring obstacle information including information on the position of an obstacle; generating 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, generating the target path includes searching for a plurality of candidates for the target path and determining the target path based on a path cost from among the plurality of candidates, The path cost is a path planning method including a meandering cost related to the degree of meandering of each of the plurality of candidates.

12. a function of acquiring obstacle information including information on the position of an obstacle; causing a computer to realize a function of generating 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, the function of generating the target path includes searching for a plurality of candidates for the target path and determining the target path based on a path cost from among the plurality of candidates, The path cost is a path planning program including a meandering cost related to the degree of meandering of each of the plurality of candidates.

Images