Route prediction device, route prediction system, route prediction method, and program

Abstract

A route prediction device (2) comprises: a tracking processing unit (21) that, on the basis of time-series data of observation values for a target (TG), calculates tracking information including the position and speed of the target (TG); a route generation unit (22) that, on the basis of the tracking information for the target (TG) calculated by the tracking processing unit (21), generates a route of the target so as to satisfy a constraint condition relating to the movement of the target (TG); and a route smoothing unit (23) that samples the route generated by the route generation unit (22) and generates, as a predicted route of the target TG, a route that is smoothed by filtering individual sampled points.

Description

Route prediction device, route prediction system, route prediction method and program

[0001] The present disclosure relates to a route prediction device, a route prediction system, a route prediction method, and a program.

[0002] A technology for predicting a future path of a target, which is a moving object, from its current location to a destination based on observed values ​​of the target is becoming widespread. For example, Non-Patent Document 1 describes a technology for predicting a path of a robot. The technology described in Non-Patent Document 1 searches for a path of the target, which is a moving object, using Particle Swarm Optimization (PSO). The PSO generates a path that is a polygonal line.

[0003] Shuqing Wang, Ye Ma, “Path Planning of Mobile Robot Based on Neural Process-Particle Swarm Optimization”, 2019 International Conference on Artificial Intelligence and Advanced Manufacturing (AIAM).

[0004] The conventional technology described in Non-Patent Document 1 has a problem in that the target path is predicted as a polygonal line, which may result in an unrealistic path that does not reflect the dynamic characteristics of the target. For example, the boundary of the polygonal line is not differentiable, making it difficult to realize as a trajectory of a moving object.

[0005] The present disclosure is intended to solve the above-mentioned problem, and has an object to provide a path prediction device that can suppress prediction of a path that does not reflect the dynamic characteristics of a target.

[0006] The path prediction device according to the present disclosure includes a tracking processing unit that calculates tracking information including the position and speed of the target based on time series data of observation values ​​of the target; a path generation unit that generates a path for the target based on the tracking information of the target calculated by the tracking processing unit so as to satisfy constraints on the movement of the target; and a path smoothing unit that samples the path generated by the path generation unit and generates a path smoothed by filtering each sampled point as a predicted path for the target.

[0007] According to the present disclosure, a path of a target is generated so as to satisfy constraints on the movement of the target, the generated path is sampled, and the sampled points are filtered to generate a smoothed path as a predicted path of the target. In this way, the path prediction device according to the present disclosure can suppress prediction of a path that does not reflect the dynamic characteristics of the target.

[0008] FIG. 1 is a schematic diagram showing an overview of path prediction. FIG. 2 is a block diagram showing the configuration of a path prediction system according to embodiment 1. FIG. 3 is a flowchart showing a path prediction method according to embodiment 1. FIG. 4 is a schematic diagram showing an overview of confluence area estimation using prediction errors of paths of two targets. FIG. 5 is a schematic diagram showing an overview of initial paths of two targets. FIG. 6A, FIG. 6B, and FIG. 6C are schematic diagrams showing an overview of path prediction using disturbance addition. FIG. 7 is a schematic diagram showing an overview of smoothing a polygonal line path of PSO. FIG. 8 is a schematic diagram showing an overview of the measurement error between the polygonal line path of PSO and the smoothed path. FIG. 9A and FIG. 9B are schematic diagrams showing an overview of a tracking gate. FIG. 10A and FIG. 10B are block diagrams showing a hardware configuration for realizing the functions of a path prediction device according to embodiment 1.

[0009] First Embodiment First, an overview of route prediction by a route prediction device according to a first embodiment will be described. Fig. 1 is a schematic diagram showing an overview of route prediction. In Fig. 1, a target TG to be tracked is a ship navigating in a marine area 100. The route prediction device according to the first embodiment estimates a route from the target TG to a destination DP in a land area 101 facing the sea. Constraints related to the navigation of the target TG are set for the target TG.

[0010] For example, the constraint conditions are areas set based on a constraint map based on past cases, and include a passing area that the target TG must pass through, an avoidance area that the target TG must avoid, a merging area where two targets merge, etc. The route of the target TG is estimated from the current time before arriving at the destination, assuming that the constraint conditions are satisfied.

[0011] For example, in Fig. 1, a ship, which is a target TG, is observed by radar at a specified time. The radar outputs observation information of the target TG as a detection point. An avoidance area AA and a passing area WP are set in the constraint map based on past cases. The position and speed of the target TG are estimated and predicted by a tracking calculation of the target TG based on time-series data of the observation values ​​of the target TG, and the tracking information of the target TG is updated.

[0012] The correlation between the trajectory of the target TG and the observed value is determined, for example, by MHT (Multiple Hypothesis Tracking). In MHT, multiple hypotheses that represent how the target TG moved, which observed value corresponds to which target TG, etc. are processed in parallel. Then, when the correctness of one of the hypotheses is determined based on the reliability of each hypothesis, the final correlation is determined.

[0013] The tracking filter may use, for example, an Ornstein-Uhlenbeck (OU) model as a motion model. The OU model can calculate a tracking trajectory A that returns the target TG to the vicinity of the original trajectory over time, even if the target TG deviates from the pre-planned trajectory.

[0014] For example, PSO (Particle Swarm Optimization) is used to search for a predicted path of the target TG. In PSO, a path is generated using polygonal lines. The number of lines constituting the polygonal line path can be set in advance as a parameter. In PSO, passing points of the target TG are set so that low cost is achieved when constraint conditions based on a constraint map are satisfied. In addition, the cost of an area on the constraint map is set as a parameter. For example, the cost is set using a parameter so that high cost is achieved in the avoidance area AA and low cost is achieved in the passing area WP.

[0015] In PSO, route candidates are generated using particles that are combinations of random numbers, and a cost function is applied to the generated route candidates. The route is repeatedly modified so that the total cost calculated by the cost function becomes lower. The number of repetitions is set as a parameter, and if the restriction of passing through the passage area WP without passing through the avoidance area AA is not satisfied after the number of repetitions is reached, the route search is restarted.

[0016] A tracking trajectory A up to the current position is calculated by tracking the target TG. Searching begins with a straight-line route connecting the current position of the target TG and the destination point DP. By shaking the particles, a disturbance is applied to the straight-line route. The effect of the disturbance appears at the position of the intermediate node of the polygonal line. By repeating this process, the route candidates gradually approach the route that achieves the lowest cost. When a preset number of repetitions is completed or a route with a sufficiently low cost is obtained, the repetition ends, and the obtained route is designated as the predicted route B.

[0017] However, in PSO, the path of the target TG is predicted using a polygonal line as described above, which predicts an unrealistic path that does not reflect the dynamic characteristics of the target TG. For example, it is impossible to differentiate at the boundaries of the polygonal line, i.e., at the points where the direction of the line segments changes. For this reason, it is difficult to realize the trajectory of the target TG except for movements that involve complete stillness. Therefore, the path prediction device according to the first embodiment samples the polygonal line path and generates a path smoothed by filtering each sampled point as the predicted path B of the target TG. This makes it possible to suppress prediction of a path that does not reflect the dynamic characteristics of the target TG.

[0018] (Basic Configuration of Route Prediction System) Fig. 2 is a block diagram showing the configuration of a route prediction system 1 according to embodiment 1. In Fig. 2, the route prediction system 1 is a system that inputs time series data of observed values ​​of a target TG and predicts the route of the target TG using the input time series data. The route prediction system 1 includes a route prediction device 2, a radar 3, a constraint map generation unit 4, a storage unit 5, and a junction prediction unit 6.

[0019] (Route Prediction Device) The route prediction device 2 acquires time series data of the observation values ​​of the target TG from the radar 3, performs tracking processing of the target TG using the acquired time series data, readably stores the tracking trajectory in the memory unit 5, predicts a route from the current position to the destination based on a constraint map, and displays the predicted route on the display unit 7. For example, the route prediction device 2 calculates tracking information of the target TG based on the time series data of the observation values ​​of the target TG acquired from the radar 3, generates a route of the target TG based on the tracking information of the target TG so as to satisfy the constraint conditions indicated by the constraint map, and generates a route smoothed by filtering each point sampled from the generated route as the predicted route of the target TG.

[0020] (Radar) The radar 3 is, for example, a synthetic aperture radar (SAR) mounted on a satellite. The SAR scans the sea surface by transmitting microwaves toward the sea surface and receiving reflected waves while the satellite flies above the Earth. This allows observation to be performed without being affected by weather conditions such as clouds or rain.

[0021] (Constraint Map Generator) The constraint map generator 4 generates a constraint map in which a constraint region is set based on past case data, and sets the generated constraint map as a constraint condition in the path prediction device 2. For example, the past case data is data on cases that occurred in the past in an area including the observation region of the radar 3, and is stored in databases of public institutions or academic research institutions, open data platforms, social media, crowdsourcing, etc. The constraint map generator 4 accesses servers that manage these to acquire the past case data.

[0022] Past case data includes, for example, location information, past event information, infrastructure information, or environmental information. Location information includes, for example, GPS data, satellite image data, etc. Event information includes historical information such as disaster occurrence locations, traffic accident locations, and crime occurrence locations. Infrastructure information includes location information of roads, bridges, buildings, public facilities, etc. Environmental information includes topographical information, weather data, or nature conservation areas, etc.

[0023] The constraint map generator 4 converts the past case data into an analyzable format. For example, it performs data cleaning, data integration, coordinate transformation, etc. on the past case data. Data cleaning includes filling in missing data or correcting incorrect data. Data integration includes consolidating data from different sources and converting it into a consistent format. Coordinate transformation is the process of converting different coordinate systems into a common coordinate system.

[0024] The constraint map generator 4 extracts features for identifying constraint regions from the past case data converted into an analyzable format. For example, clustering or heat maps are used to extract the features. In clustering, a clustering method such as K-means or DBSCAN is used to group the case data for identifying constraint regions. A heat map visualizes the importance of specific regions based on the frequency of past event data.

[0025] The constraint map generation unit 4 sets specific constraint conditions based on the feature extraction results. Constraint conditions include constraints on the location of buildings or roads, seasonal road closures, temporary constraints when an event occurs, and zoning based on risk levels such as areas at risk of flooding or areas prone to earthquakes. The constraint map generation unit 4 generates a constraint map that reflects the constraint conditions by linking with, for example, map creation software. To generate the constraint map, different constraint conditions are integrated as multiple layers and overlaid.

[0026] For example, when the constraint map generation unit 4 acquires past case data relating to avoidance areas AA where grounding has occurred in the past and pass areas WP where the ship was able to navigate to the destination DP, the constraint map generation unit 4 integrates the position information of the avoidance areas AA and the pass areas WP based on the acquired past case data and clusters them by area. Based on the past case data clustered by area, the constraint map generation unit 4 generates a constraint map in which the avoidance areas AA are displayed in red and the pass areas WP are displayed in green. The constraint map generated by the constraint map generation unit 4 is output to the route prediction device 2.

[0027] The constraint map generation unit 4 may generate a constraint map in which a merging area where two targets are to merge is set. For example, the constraint map generation unit 4 collects past merging case data as past case data. The past merging case data includes position information, time, speed, etc. of multiple objects. The constraint map generation unit 4 identifies targets to be merging, sets constraint conditions including merging timing, merging position, speed range, etc., and calculates a possible merging area based on the current positions of the targets and the predicted routes. Then, the constraint map generation unit 4 sets the merging area in the constraint map based on the calculation result of the merging area.

[0028] In addition, if a confluence area is set in the constraint map but the constraint conditions do not specify a location where the two targets should confluence, and a confluence time is specified, the constraint map generation unit 4 outputs the confluence time to the confluence prediction unit 6.

[0029] 2 , the constraint map generation unit 4 may be a component included in an external device provided separately from the route prediction device 2. In this case, the constraint map generation unit 4 is connected to the route prediction device 2 for communication via a communication unit included in the external device, and transmits the constraint map to the route prediction device 2. Alternatively, the constraint map generation unit 4 may be a component included in the route prediction device 2.

[0030] (Storage Unit) The storage unit 5 stores information related to the tracking trajectory of the target TG. For example, the storage unit 5 is a storage device such as a hard disk drive (HDD) or a solid state drive (SSD). The storage unit 5 may be any storage device accessible by the route prediction device 2, and may be provided outside the route prediction device 2. For example, the tracking trajectory is a trajectory made up of position information of the target TG acquired at regular intervals in the tracking process, and is stored as time-series data.

[0031] (Meeting Prediction Unit) The meeting prediction unit 6 predicts a meeting point where multiple targets will meet based on the tracking trajectories of each target read from the storage unit 5 and the meeting time specified by the constraint map generation unit 4. For example, the meeting prediction unit 6 acquires tracking trajectory data, which is time-series data on the past position, speed, and direction of each target, and the meeting time at which the targets are to meet. Based on this information, the meeting prediction unit 6 calculates the current position of each target and predicts the future position of each target based on the target's motion characteristics, such as changes in speed, acceleration, and direction. For example, a physical model, a machine learning model, or a Kaliman filter may be used to predict the target positions. The constraint map generation unit 4 calculates a predicted route from each target's current position to the specified meeting time and identifies a meeting point where the predicted routes of each target may intersect. The meeting prediction unit 6 then predicts the meeting point based on the degree of positional agreement at the meeting time or the reachability of each target. The meeting point predicted by the meeting prediction unit 6 is output to the path prediction device 2.

[0032] (Display Unit) The display unit 7 is a display device that displays information related to the predicted route output from the route prediction device 2, and for example, displays the predicted route of the target TG calculated by the route prediction device 2. Furthermore, the display unit 7 may be a display unit included in the route prediction device 2, or may be a display unit included in an external device that can communicate with the route prediction device 2 via a wired or wireless signal line. For example, the display unit 7 may be an LCD (Liquid Crystal Display) or an organic EL (Electroluminescence).

[0033] (Basic Configuration of Route Prediction Device) As shown in Fig. 2, the route prediction device 2 includes a tracking processing unit 21, a route generation unit 22, and a route smoothing unit 23. For example, the route prediction device 2 is realized by a computer. A memory included in the computer stores a program that constitutes an information processing application for realizing the functions of the tracking processing unit 21, the route generation unit 22, and the route smoothing unit 23. A processor included in the computer reads the information processing application from the memory and executes the information processing application, thereby realizing the functions of the tracking processing unit 21, the route generation unit 22, and the route smoothing unit 23.

[0034] (Tracking Processing Unit) The tracking processing unit 21 calculates tracking information including the position and velocity of the target TG based on time-series data of the observation values ​​of the target TG. The tracking information is information about the tracking trajectory of the target TG, and is time-series data of the position, velocity, and direction of the target TG. For example, the tracking processing unit 21 acquires time-series data of the observation values ​​of the target TG from the radar 3, and sets hypotheses regarding the initial position and velocity of the target TG based on a dynamic model of the target TG, such as a linear motion model or an accelerated motion model. The tracking processing unit 21 uses a Kaliman filter or a particle filter to update hypotheses that take new observation data into account based on each current hypothesis. Next, the tracking processing unit 21 generates a new hypothesis based on the new observation data. At this time, it determines whether the new observation corresponds to the existing hypothesis, and if not, generates a hypothesis for a new target. The tracking processing unit 21 calculates the reliability of each hypothesis based on the degree of match with the observation data, the accuracy of the predicted position, the duration of the hypothesis, etc., deletes hypotheses with low reliability, and selects hypotheses with a reliability equal to or greater than a certain threshold, or selects hypotheses with a reliability up to the best n. Then, the tracking processing unit 21 generates a composite hypothesis that integrates multiple hypotheses based on the observation data for a certain period, and updates the tracking trajectory, which includes the position, speed, and acceleration of the target TG, based on the composite hypothesis.

[0035] (Path Generation Unit) The path generation unit 22 generates a path for the target TG based on the tracking information of the target TG calculated by the tracking processing unit 21, so as to satisfy constraints related to the movement of the target TG. For example, the path generation unit 22 acquires a tracking trajectory, which is time-series data on the position, velocity, acceleration, etc. of the target TG, and acquires a constraint map including constraints such as a passing area, a merging area, or an avoidance area. The path generation unit 22 may receive a merging point as a constraint from the merging prediction unit 6. Based on the acquired information, the path generation unit 22 randomly initializes multiple particles representing candidate paths and sets PSO parameters such as the number of particles and the maximum number of iterations. The path generation unit 22 calculates the cost of each particle using a cost function and updates the cost of each particle to satisfy the constraints. The path generation unit 22 then predicts a rough path for the target TG by repeatedly correcting the particles so as to reduce the total cost. The path generated by the path generation unit 22 is output to the path smoothing unit 23. The path of the target TG generated by the path generating unit 22 is a polygonal line path generated by PSO, and is a rough path that does not reflect the dynamic characteristics of the target TG.

[0036] (Route Smoothing Unit) The route smoothing unit 23 samples the route generated by the route generation unit 22 and generates a route formed by smoothing the sampled points through filtering as a predicted route of the target TG. For example, the route smoothing unit 23 samples the route obtained by PSO at regular intervals to generate a set of discrete points. The route smoothing unit 23 predicts the position and velocity of the target TG at the next time through filtering using a tracking filter such as a Kaliman filter. The route smoothing unit 23 then updates the predicted values ​​of the position and velocity of the target TG based on the sampled observation points, and generates a smoothed route by repeating the prediction and updating for all sampled points. The route smoothing unit 23 generates display information for displaying the predicted route of the target TG and outputs it to the display unit 7. The display unit 7 displays the predicted route of the target TG based on the display information input from the route smoothing unit 23.

[0037] Furthermore, in addition to avoidance and passing, the confluence of multiple ships is specified as a constraint condition, and the route prediction device 2 predicts a route in which multiple targets will not collide with each other at the confluence point. The route prediction device 2 according to the first embodiment sets the same number of spatial dimensions as the number of target ships, and calculates polygonal trajectories using PSO as rough routes. The rough routes are then corrected to predict the final routes of the multiple ships.

[0038] If the route generated by the route generation unit 22 does not satisfy the constraints, for example, if the route does not pass through the transit area, the cost of the transit area calculated using the cost function is finite, and the route candidate may fall into a local solution. In this case, the route generation unit 22 predicts the route again so that the constraints are satisfied.

[0039] The path generated by the path generation unit 22 is a polygonal trajectory of the PSO. The path smoothing unit 23 samples the polygonal trajectory of the PSO, considers each sampled point as an observation value of the target TG, and treats parts that do not reflect the dynamic characteristics of the target TG as errors, which are removed by filtering using a tracking filter. This enables the path prediction device 2 to correct the path of the target TG to a realistic path that reflects the dynamic characteristics of the ship, while satisfying constraints on the roughly generated path of the target TG.

[0040] The route prediction method according to the first embodiment will be described below. Fig. 3 is a flowchart showing the route prediction method according to the first embodiment, illustrating a case where a route prediction device 2 predicts routes of multiple ships tracked by a radar 3 mounted on an artificial satellite. The series of processes shown in Fig. 3 are performed at a regular observation time. In other words, the series of processes shown in Fig. 3 are performed once for each observation time.

[0041] 3 , the radar 3 observes ships in the observation area as targets TG at designated times and outputs the observation information as detection points to the path prediction device 2. Furthermore, the constraint map generation unit 4 generates a constraint map in which a passing area WP and an avoidance area AA are set based on past case data, and outputs the constraint map to the path prediction device 2.

[0042] The tracking processing unit 21 calculates tracking information for each target based on time-series data of the observation values ​​of each target (step ST1). For example, the tracking processing unit 21 estimates and predicts the position and speed of each target using the time-series data of the observation values ​​of each target, and updates the tracking trajectory including the position and speed of each target. The tracking processing unit 21 determines the correlation between the tracking trajectory and the observation values, for example, using MHT. The tracking trajectory of the target generated by the tracking processing unit 21 is stored in the storage unit 5. In addition, the latest (current time) tracking trajectory of the target generated by the tracking processing unit 21 is output to the path generation unit 22.

[0043] The tracking process utilizes, for example, an OU model, which is effective in predicting the navigation trajectory of a ship. In the OU model, the transition of the target state s(t) is expressed by the stochastic differential equation of the following formula (1). In the following formula (1), the state s(t) is the state vector of the target at time t, and includes, for example, the position and velocity of the target. ds(t) is a small change in the state vector s(t) and indicates the change from time t to t+dt. Matrix A is a constant matrix that determines the rate of change of the state s(t) and represents the dynamic characteristics of the system. Matrix G is a control matrix that indicates the influence of the target velocity vector v on the state s(t). Matrix B conveys the influence of dw(t), which indicates a disturbance, to the state s(t). dw(t) is the differential value of the disturbance and represents random fluctuations in the system.

[0044] The target motion is expressed by a quasi-constant velocity (CV) model as shown in the following equation (2). In equation (2), the first term Ads(t) represents the constant velocity rectilinear motion of the target. The second term Bdw(t) corresponds to randomly fluctuating disturbances.

[0045] The difference between the above formula (1) and formula (2) is the presence or absence of Gvdt, the second term of OU. Gvdt indicates the influence that the target's random velocity vector v has on the target's state s(t), and has the effect of pulling the velocity component of the target's state s(t) back to its average value. This has the effect of returning the target to near its pre-planned trajectory over time, even if it deviates from the pre-planned trajectory.

[0046] Next, the junction prediction unit 6 predicts a junction point where multiple targets will merge, based on the tracking trajectories of each target read from the storage unit 5 and the junction time specified by the constraint map generation unit 4 (step ST2). Note that if a junction point is specified in the constraint map, the processing of step ST2 is not necessary. In other words, the path prediction system 1 does not need to include the junction prediction unit 6. Here, an example is given in which a junction is specified as a constraint condition, but the location of the junction point is not specified and a junction time is specified.

[0047] For example, assume that the time when targets TG(1) and TG(2) will meet is two hours from the current time. The meeting prediction unit 6 predicts and extrapolates the tracking trajectories of the targets read from the storage unit 5, and estimates the meeting area of ​​targets TG(1) and TG(2) from the overlap of the prediction error ellipses. Here, the meeting target group G is expressed by the following equation (3). In the following equation (3), T ｉ is the track of each target included in the target group G. is a subscript indicating the number i assigned to each target to be merged, where i = 1, 2, ..., M. M is an integer equal to or greater than 2.

[0048] The track T of the target group consisting of the target TG(i) at time k ｉ Track reliability β ｋ，ｉ is the sum of the MHT hypothesis reliability that refers to the trajectories of the targets included in the target group. The extrapolated predicted time is the meeting time of the target group specified by the constraint map generator 4. The predicted vector of the trajectory of target TG(i) at time k is expressed as x ｋ，ｉ The underscore (-) is used to denote the prediction error covariance matrix P of the trajectory of the target TG(i) at time k. ｋ，ｉ(-). Note that, due to the nature of electronic filing, the underscore cannot be written, so the predicted vector of the trajectory of the target TG(i) is x ｋ，ｉ It is written as an underscore (-).

[0049] 4 is a schematic diagram showing an outline of estimating a confluence area using prediction errors of the paths of two targets TG(1) and TG(2), where i=1, 2. FIG. 4 shows the area in which targets TG(1) and TG(2) move, represented in an XY coordinate system. As indicated by the arrows in FIG. 4, the confluence area between targets TG(1) and TG(2), shown in the diagram on the right, is estimated from the predicted paths of targets TG(1) and TG(2), shown in the diagram on the left.

[0050] As shown in the left diagram of Figure 4, the confluence prediction unit 6 uses the predicted vector of the trajectory R1 of the target TG (1) and the predicted vector of the trajectory R2 of the target TG (2) to calculate a prediction error ellipse C, which is the prediction error covariance matrix of the trajectory R1 of the target TG (1), and calculates a prediction error ellipse D, which is the prediction error covariance matrix of the trajectory R2 of the target TG (2).

[0051] Next, the confluence prediction unit 6 samples a plurality of prediction points based on the prediction vectors of the trajectory R1 of the target TG(1) and the trajectory R2 of the target TG(2) and their prediction error covariance matrices, and calculates the center of gravity of the sampled prediction points as the center of gravity G of the extrapolated position. As shown in the right diagram of Figure 4, the confluence prediction unit 6 calculates the center x of the confluence area E using the following equation (4) based on the center of gravity G of the extrapolated position of the trajectory R1 of the target TG(1) and the trajectory R2 of the target TG(2): ｋ，Ｇ Calculate the underscore (-). In the following formula (4), i = 1, 2. β ｋ，ｉ is the track T of the target group at time k ｉ is the track reliability.

[0052] As shown in the right diagram of Fig. 4, the confluence prediction unit 6 calculates the extent of the confluence area E using the following formula (5) based on the center of gravity G of the extrapolated positions of the trajectory R1 of the target TG(1) and the trajectory R2 of the target TG(2). The confluence area E has an elliptical extent. The confluence prediction unit 6 calculates the track reliability β, which is the reliability of the tracking trajectories of the multiple targets TG. ｋ，ｉand the error estimate value, which is the extent of the confluence area E calculated using the following equation (5). ｋ，ｉ A constraint map is generated that sets the confluence areas of multiple target TGs predicted according to the estimated error value together with constraint areas based on past case data, and the generated constraint map is set in the route prediction device 2 as a constraint condition.

[0053] Next, the path generating unit 22 generates a path for the target based on the latest tracking trajectory of the target calculated by the tracking processing unit 21 so as to satisfy the constraint conditions indicated by the constraint map (step ST3). For example, the path generating unit 22 generates a rough path consisting of a combination of polygonal line elements of the number indicated by the parameters. The cost of the constraint region in the constraint map is set in the path generating unit 22.

[0054] For example, the costs shown in the following formula (6) are set for the avoidance area, the passing area, the merging area, and the collision area. In the following formula (6), C ａ is the cost of the avoidance area, and C ｔ is the cost of the passage area, and C ｇ is the cost of merging, and C ｃ is the cost of collision. In the avoidance region, the cost C ａ If C is infinity, the area must be avoided, and if C is 0, passage is permitted. ｔ If the parameter param_t is a negative value, non-passage of this area is set, and if the parameter param_t is 0, passage of this area is set. ｇ If the parameter param_t is a negative value, the merge is not set and the merge does not occur. If the parameter param_t is 0, the merge is set and the merge occurs. In a collision, the cost C ｃIf the parameter param_t is a negative value, a collision is not set and no collision occurs. If the parameter param_t is 0, a collision is set and a collision occurs. Regarding collision avoidance, a high cost is assigned when the distance between multiple targets at the same time falls below a certain value.

[0055] FIG. 5 is a schematic diagram illustrating the initial routes of target TG(1) and target TG(2). FIG. 5 illustrates a case in which target TG(1) and target TG(2) travel to a common destination DP. It is also assumed that target TG(1) and target TG(2) are set to merge before reaching the destination DP. The route generation unit 22 uses tracking trajectories, which are time-series data on the positions, velocities, accelerations, etc., of target TG(1) and target TG(2), to generate an initial route for target TG(1), an initial route for target TG(2), and a designated merge area AB so as to satisfy constraints such as merging. At the start of the route search, as shown in FIG. 5, a straight-line route is generated connecting the current position of target TG(1) and the destination DP, and a straight-line route is generated connecting the current position of target TG(2) and the destination DP. The initial routes for target TG(1) and target TG(2) are route candidates generated using particles, which are combinations of random numbers.

[0056] The route generation unit 22 calculates the cost of an initial route using a cost function, and iteratively searches for a route by modifying route candidates starting from the initial route so as to reduce the total cost. The number of iterations is set as a parameter in the route generation unit 22. In FIG. 3 , the route generation unit 22 determines whether the route candidate obtained when the number of iterations set as a parameter has been reached satisfies constraints such as avoidance, merging, and passing (step ST4). If the route candidate does not satisfy the constraints (step ST4; NO), the route generation unit 22 returns to the processing of step ST3 and restarts the route search from the generation of the initial route. If the route candidate satisfies the constraints (step ST4; YES), the route generation unit 22 outputs this route candidate to the route smoothing unit 23 as a rough route.

[0057] 6A, 6B, and 6C are schematic diagrams illustrating an overview of route prediction using disturbance addition, showing route candidates obtained by adding disturbance to the initial route shown in FIG. 5. FIG. 6A shows route candidates for target TG(1) and target TG(2) generated by adding disturbances that move them away from each other. FIG. 6B shows a route candidate generated by adding a disturbance that moves the route candidate for target TG(1) shown in FIG. 6A closer to the designated merging area AB. FIG. 6C shows a route candidate generated by adding a disturbance that moves the route candidate for target TG(2) shown in FIG. 6B closer to the designated merging area AB. As shown in FIGS. 6A, 6B, and 6C, the influence of the disturbance appears at the intermediate node of the polygonal line. As a result of cost calculation, the route candidate shown in FIG. 6C is selected as the likely route.

[0058] Next, the path generation unit 22 further shakes particles to apply disturbance based on the path candidate shown in FIG. 6C . In this case, the effect of the disturbance also appears at the intermediate node of the polygonal line in the path candidate. The path generation unit 22 repeatedly generates path candidates, selects path candidates based on cost, applies disturbance to the path candidates, and selects path candidates based on cost, thereby gradually approaching the path candidate with the lowest cost. The repetition is terminated either at a preset number of times or when a path with a sufficiently low cost is obtained. If the path candidate obtained at the time the repetition is terminated satisfies the constraints, the path generation unit 22 outputs this path to the path smoothing unit 23 as a rough path.

[0059] 3, the route smoothing unit 23 samples the route generated by the route generation unit 22, and generates a route formed by smoothing the sampled points by filtering as a predicted route of the target TG (step ST6). For example, the route smoothing unit 23 samples the general route generated by the PSO, and operates a tracking filter based on a motion model of the ship by using each point obtained by sampling as an observation value of the target ship.

[0060] 7 is a schematic diagram illustrating an outline of smoothing a polygonal path by PSO, showing smoothing by filtering the polygonal path. As shown in FIG. 7, the path smoother 23 samples points P from the polygonal path F and generates a smoothed path G by filtering each of the sampled points P. When the direction or speed of the trajectory changes significantly, a tracking filter such as an interactive multiple model (IMM) may be used. The IMM is a filter that uses multiple motion models and integrates estimation results based on the models by weighting them with likelihood.

[0061] Before filtering, the route smoothing unit 23 identifies points P outside the tracking gate from among points P sampled from the polygonal route, and excludes the identified points P from the filtering process. In this way, the route smoothing unit 23 outputs time-series data of smoothed values ​​obtained as a result of smoothing as a final smoothed route G. For example, the route smoothing unit 23 generates display information for displaying the smoothed route G, and outputs the generated display information to the display unit 7. The display unit 7 displays the smoothed route G based on the display information.

[0062] The route smoothing unit 23 may calculate an observation error covariance matrix used for filtering based on the magnitude of the allowable difference between the polygonal route generated by the route generating unit 22 and the smoothed route generated by filtering. For example, the polygonal route is calculated by an observation model in an orthogonal coordinate system expressed by the following equation (7). In the following equation (7), z Ｎ，ｋ The underscores represent the positions of the targets that make up the polygonal path in the x and y coordinates, and the z Ｎ，ｋ Underscore = (x Ｎ , y Ｎ ) H Ｎ is the observation matrix. ｋ The underbar is the observation noise vector at time k, which is composed of observation noise in rectangular coordinates.

[0063] Observation matrix H Ｎ is expressed by the following formula (8): In the following formula (8), I is a 2×2 unit matrix.

[0064] Target position z Ｎ，ｋ Each element in the underscore (x Ｎ , y Ｎ ) error, i.e., the standard deviation of the difference from the true position (σ ｘ , σ ｙ ), then the covariance matrix R Ａ，ｋ is expressed by the following formula (9).

[0065] 8 is a schematic diagram showing an outline of the measurement error between the polygonal line path F of the PSO and the smoothed path G. In FIG. 8, the allowable difference σ of the x component of the point P between the polygonal line path F and the smoothed path G is ｘ and the allowable difference σ of the y component of the point P between the polygonal line path F and the smoothed path G ｙ The standard deviation (σ ｘ , σ ｙ ) is set as a parameter in the route smoothing unit 23. The route smoothing unit 23 calculates the standard deviation (σ ｘ , σ ｙ ) to obtain the covariance matrix R according to the above equation (9). Ａ，ｋ Calculate.

[0066] A tracking gate is set in the path smoothing unit 23 to mitigate discontinuities in the trajectory at the boundary of a polygonal line with a sharp angle. For example, the path smoothing unit 23 uses the following equation (10) to determine whether or not there is a correlation between an observed value, which is a point sampled from the polygonal line path F, and a predicted value on the smoothed path. In the following equation (10), d is a parameter of the statistical distance allowed in the chi-square test. The path smoothing unit 23 determines that there is a correlation if the inequality in the following equation (10) holds, and that there is no correlation if it does not hold. If there is no correlation, the path smoothing unit 23 discards the corresponding sample point.

[0067] Furthermore, the path smoothing unit 23 calculates the observation error covariance matrix S of the target TG(N) at time k according to the following equation (11): Ｎ，ｋ and the predicted position z of the target TG(N) at time k. Ｎ，ｋ Calculate (-).

[0068] The path smoothing unit 23 uses gate processing to discard points on the broken-line path that deviate from the expected motion and correct the trajectory to one that is more ship-like. FIG. 9A is a diagram showing points P included in gate GA among points P on the broken-line path F. FIG. 9B is a diagram showing points P not included in gate GA among points P on the broken-line path F. As shown in FIG. 9A, points P included in gate GA are determined to be observed values ​​included in a path that reflects the expected motion of the target. As shown in FIG. 9B, points P not included in gate GA are determined to be observed values ​​not included in a path that reflects the expected motion of the target TG. The path smoothing unit 23 discards points P that are determined not to be included in a path that reflects the expected motion of the target TG. For example, when predicting the path of the ship shown in FIG. 1, the path smoothing unit 23 can correct the broken-line path of the ship to a path that reflects the dynamic characteristics of the ship.

[0069] If the smoothed route does not satisfy the constraint conditions, the route smoothing unit 23 may increase the number of samples of the route generated by the route generation unit 22 to smooth it. For example, the route smoothing unit 23 determines whether the smoothed route satisfies the constraint conditions, and if it determines that the smoothed route satisfies the constraint conditions, outputs this smoothed route as the final route. On the other hand, the route smoothing unit 23 rejects the smoothed route determined not to satisfy the constraint conditions, increases the number of samples set by the parameters, and repeats route smoothing by performing sampling again. In this way, the route prediction device 2 can obtain a smoothed route that satisfies the constraint conditions.

[0070] Furthermore, if the smoothed route generated by the route smoothing unit 23 does not satisfy the constraint conditions, the route generation unit 22 may increase the number of target routes to be generated, and may repeat the route generation by the route generation unit 22 and the route smoothing by the route smoothing unit 23. For example, the route smoothing unit 23 determines whether the smoothed route satisfies the constraint conditions, and if it determines that the smoothed route satisfies the constraint conditions, it outputs this smoothed route as the final route. On the other hand, the route smoothing unit 23 rejects the smoothed route determined not to satisfy the constraint conditions, and the route generation unit 22 increases the number of polygonal route generated that are set by the parameters in accordance with an instruction from the route smoothing unit 23. Then, the route generation unit 22 generates a route and the route smoothing unit 23 smooths the route again. This enables the route prediction device 2 to obtain a smoothed route that satisfies the constraint conditions.

[0071] Next, a description will be given of a hardware configuration that realizes the functions of the route prediction device 2. The functions of the tracking processing unit 21, the route generation unit 22, and the route smoothing unit 23 provided in the route prediction device 2 are realized by processing circuits. That is, the route prediction device 2 includes a processing circuit for executing the processes from step ST1 to step ST5 shown in Fig. 3. The processing circuit may be dedicated hardware, or may be a CPU (Central Processing Unit) that executes a program stored in memory.

[0072] Fig. 10A is a block diagram showing a hardware configuration for realizing the functions of the route prediction device 2. Fig. 10B is a block diagram showing a hardware configuration for executing software for realizing the functions of the route prediction device 2. In Figs. 10A and 10B , a tracking processing unit 21 acquires time-series data of target observation values ​​from the radar 3 via an input interface 200, and acquires a tracking trajectory from the storage unit 5. A route generation unit 22 acquires a constraint map from the constraint map generation unit 4 via the input interface 200, and acquires a junction point from the junction prediction unit 6. The tracking processing unit 21 outputs the tracking trajectory to the storage unit 5 via an output interface 201. A route smoothing unit 23 outputs a predicted route to the display unit 7 via the output interface 201.

[0073] 10A, the processing circuit 202 may be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a combination thereof. The functions of the tracking processing unit 21, the path generation unit 22, and the path smoothing unit 23 included in the path prediction device 2 may be realized by separate processing circuits, or these functions may be realized together by a single processing circuit.

[0074] 10B , the functions of the tracking processing unit 21, the path generating unit 22, and the path smoothing unit 23 included in the path prediction device 2 are realized by software, firmware, or a combination of software and firmware. The software or firmware is written as a program and stored in the memory 204.

[0075] The processor 203 reads and executes programs stored in the memory 204 to realize the functions of the tracking processing unit 21, the route generation unit 22, and the route smoothing unit 23 included in the route prediction device 2. For example, the route prediction device 2 includes a memory 204 for storing a program that, when executed by the processor 203, results in the processing of steps ST1 to ST5 shown in FIG. 3 being executed. These programs cause a computer to execute the procedures or methods of the processing performed by the tracking processing unit 21, the route generation unit 22, and the route smoothing unit 23. The memory 204 may be a computer-readable storage medium that stores programs for causing a computer to function as the tracking processing unit 21, the route generation unit 22, and the route smoothing unit 23.

[0076] The memory 204 may be, for example, a non-volatile or volatile semiconductor memory such as a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read Only Memory), or an EEPROM (Electrically-EPROM) (registered trademark), a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, or a DVD.

[0077] Some of the functions of the tracking processing unit 21, the route generation unit 22, and the route smoothing unit 23 included in the route prediction device 2 may be realized by dedicated hardware, and the other functions may be realized by software or firmware. For example, the function of the tracking processing unit 21 may be realized by the processing circuit 202, which is dedicated hardware, and the functions of the route generation unit 22 and the route smoothing unit 23 may be realized by the processor 203 reading and executing a program stored in the memory 204. In this way, the processing circuit can realize the above functions by hardware, software, firmware, or a combination of these.

[0078] As described above, the path prediction device 2 according to the first embodiment includes the tracking processing unit 21 that calculates tracking information including the position and velocity of the target TG based on time-series data of the observation values ​​of the target TG, the path generation unit 22 that generates a path of the target so as to satisfy constraints on the movement of the target TG based on the tracking information of the target TG calculated by the tracking processing unit 21, and the path smoothing unit 23 that samples the path generated by the path generation unit 22 and generates a path smoothed by filtering each sampled point as a predicted path of the target TG. This enables the path prediction device 2 to suppress prediction of a path that does not reflect the dynamic characteristics of the target TG.

[0079] In the route prediction device 2 according to the first embodiment, the route smoothing unit 23 calculates an observation error covariance matrix to be used for filtering, based on the magnitude of the allowable difference value between the polygonal route generated by the route generation unit 22 and the smoothed route generated by filtering. This enables the route prediction device 2 to suppress prediction of a route that does not reflect the dynamic characteristics of the target TG.

[0080] In the route prediction device 2 according to the first embodiment, when the smoothed route does not satisfy the constraint conditions, the route smoother 23 smoothes the route generated by the route generator 22 by increasing the number of samples. This enables the route prediction device 2 to obtain a smoothed route that satisfies the constraint conditions.

[0081] In the route prediction device 2 according to the first embodiment, if the smoothed route generated by the route smoothing unit 23 does not satisfy the constraint conditions, the route generation unit 22 increases the number of routes for the target TG to be generated, and performs route generation by the route generation unit 22 again and route smoothing by the route smoothing unit 23 again. This enables the route prediction device 2 to obtain a smoothed route that satisfies the constraint conditions.

[0082] The route prediction system 1 according to the first embodiment includes a route prediction device 2 and a constraint map generator 4 that generates a constraint map in which a constraint region is set based on past case data and sets the generated constraint map as a constraint condition in the route prediction device 2. This makes it possible to provide a route prediction system 1 that can suppress prediction of a route that does not reflect the dynamic characteristics of the target TG.

[0083] In the route prediction system 1 according to the first embodiment, the constraint map generation unit 4 generates a constraint map in which a confluence area of ​​multiple target TGs predicted according to the reliability of the tracking trajectories of the multiple target TGs and the error estimate value is set together with a constraint area based on past case data, and sets the generated constraint map as a constraint condition in the route prediction device 2. This enables the route prediction system 1 to predict a route including a confluence of multiple target TGs as a route that reflects the dynamic characteristics of the target TGs.

[0084] The path prediction method according to the first embodiment includes step ST1 in which a tracking processing unit 21 calculates tracking information including the position and velocity of the target TG based on time-series data of observation values ​​of the target TG, steps ST2 to ST4 in which a path generation unit 22 generates a path of the target TG so as to satisfy constraints on the movement of the target TG based on the tracking information of the target TG calculated by the tracking processing unit 21, and step ST5 in which a path smoothing unit 23 samples the path generated by the path generation unit 22 and generates a path smoothed by filtering each sampled point as a predicted path of the target TG. By having the path prediction device 2 execute the above method, it is possible to suppress prediction of a path that does not reflect the dynamic characteristics of the target TG.

[0085] A computer that executes a program according to the first embodiment functions as a tracking processing unit 21 that calculates tracking information including the position and velocity of the target TG based on time-series data of the observation values ​​of the target TG, a path generating unit 22 that generates a path of the target TG so as to satisfy constraints related to the movement of the target TG based on the tracking information of the target TG calculated by the tracking processing unit 21, and a path smoothing unit 23 that samples the path generated by the path generating unit 22 and generates a path smoothed by filtering each sampled point as a predicted path of the target TG. This makes it possible to provide a path prediction device 2 that can suppress prediction of a path that does not reflect the dynamic characteristics of the target TG.

[0086] Various aspects of the present disclosure are summarized below as appendices.

[0087] (Supplementary Note 1) A route prediction device comprising: a tracking processing unit that calculates tracking information including a position and a velocity of the target based on time-series data of observation values ​​of the target; a route generation unit that generates a route of the target based on the tracking information of the target calculated by the tracking processing unit so as to satisfy constraints on the movement of the target; and a route smoothing unit that samples the route generated by the route generation unit and generates a route smoothed by filtering each sampled point as a predicted route of the target. (Supplementary Note 2) The route prediction device according to Supplementary Note 1, wherein the route smoothing unit calculates an observation error covariance matrix to be used for filtering based on the magnitude of an allowable difference value between the polygonal line route generated by the route generation unit and a smoothed route generated by filtering. (Supplementary Note 3) The route prediction device according to Supplementary Note 1 or Supplementary Note 2, wherein the route smoothing unit increases the number of samples of the route generated by the route generation unit to smooth the route when the smoothed route does not satisfy the constraints. (Supplementary Note 4) The route prediction device according to any one of Supplements 1 to 3, wherein, when the smoothed route generated by the route smoothing unit does not satisfy the constraint conditions, the route generation unit increases the number of routes for the targets to be generated, and repeats the route generation by the route generation unit and the route smoothing by the route smoothing unit. (Supplementary Note 5) A route prediction system comprising: the route prediction device according to any one of Supplements 1 to 4; and a constraint map generation unit that generates a constraint map in which a constraint region is set based on past case data, and sets the generated constraint map as the constraint condition in the route prediction device. (Supplementary Note 6) The route prediction system according to Supplementary Note 5, wherein the constraint map generation unit generates the constraint map in which a confluence region of the multiple targets predicted according to reliability of the tracking trajectories of the multiple targets and an error estimate value is set together with the constraint region based on the past case data, and sets the generated constraint map as the constraint condition in the route prediction device.(Supplementary Note 7) A path prediction method using a path prediction device, comprising: a step by a tracking processing unit calculating tracking information including a position and a velocity of the target based on time series data of observation values ​​of the target, a step by a path generation unit generating a path of the target based on the tracking information of the target calculated by the tracking processing unit so as to satisfy constraints on the movement of the target, and a step by a path smoothing unit sampling the path generated by the path generation unit and generating a path smoothed by filtering each sampled point as a predicted path of the target. (Supplementary Note 8) A program causing a computer to function as: a tracking processing unit calculating tracking information including the position and velocity of the target based on time series data of observation values ​​of the target, a path generation unit generating a path of the target based on the tracking information of the target calculated by the tracking processing unit so as to satisfy constraints on the movement of the target, and a path smoothing unit sampling the path generated by the path generation unit and generating a path smoothed by filtering each sampled point as a predicted path of the target.

[0088] Any of the components of the embodiments may be modified or omitted.

[0089] A path prediction device according to the present disclosure can be used, for example, to control the movement of a robot.

[0090] 1 Route prediction system, 2 Route prediction device, 3 Radar, 4 Constraint map generation unit, 5 Memory unit, 6 Merging prediction unit, 7 Display unit, 21 Tracking processing unit, 22 Route generation unit, 23 Route smoothing unit, 100 Marine area, 101 Land area, 200 Input interface, 201 Output interface, 202 Processing circuit, 203 Processor, 204 Memory.

Claims

1. A path prediction device comprising: a tracking processing unit that calculates tracking information including the position and speed of a target based on time series data of observation values ​​of the target; a path generation unit that generates a path for the target based on the tracking information of the target calculated by the tracking processing unit so as to satisfy constraints on the movement of the target; and a path smoothing unit that samples the path generated by the path generation unit and generates a path smoothed by filtering each sampled point as a predicted path for the target.

2. The route prediction device according to claim 1, characterized in that the route smoothing unit calculates an observation error covariance matrix to be used for filtering based on the magnitude of the allowable difference value between the polygonal route generated by the route generation unit and the smoothed route generated by filtering.

3. A route prediction device according to claim 1 or claim 2, characterized in that, when the smoothed route does not satisfy the constraints, the route smoothing unit increases the number of samples of the route generated by the route generation unit to smooth the route.

4. A route prediction device according to any one of claims 1 to 3, characterized in that, if the smoothed route generated by the route smoothing unit does not satisfy the constraints, the route generation unit increases the number of routes to be generated for the target, and repeats the route generation by the route generation unit and the route smoothing by the route smoothing unit.

5. A route prediction system comprising: a route prediction device according to any one of claims 1 to 4; and a constraint map generation unit that generates a constraint map in which a constraint region is set based on past case data, and sets the generated constraint map as the constraint condition in the route prediction device.

6. The path prediction system according to claim 5, wherein the constraint map generation unit generates the constraint map by setting the confluence area of ​​the multiple targets predicted according to the reliability of the tracking trajectories of the multiple targets and error estimate values ​​together with the constraint area based on the past case data, and sets the generated constraint map as the constraint condition in the path prediction device.

7. A route prediction method using a route prediction device, comprising: a step in which a tracking processing unit calculates tracking information including the position and speed of the target based on time series data of observation values ​​of the target; a step in which a route generation unit generates a route for the target based on the tracking information of the target calculated by the tracking processing unit so as to satisfy constraints on the movement of the target; and a step in which a route smoothing unit samples the route generated by the route generation unit and generates a route smoothed by filtering each sampled point as a predicted route for the target.

8. A program for causing a computer to function as: a tracking processing unit that calculates tracking information including the position and speed of a target based on time series data of observation values ​​of the target; a path generation unit that generates a path for the target based on the tracking information of the target calculated by the tracking processing unit so as to satisfy constraints on the movement of the target; and a path smoothing unit that samples the path generated by the path generation unit and generates a path as a predicted path for the target by smoothing the path of each sampled point through filtering.

Images