JP7052404B2 - State estimator and program - Google Patents

Description

The present invention relates to a state estimation device and a program.

Conventionally, there has been known a technique of calculating the future movement range of each moving object from a map and a moving state of a moving object by a risk potential, and notifying the danger by the overlap thereof (Patent Documents 1 to 4).

Further, a device for dynamically switching a calculation means when calculating a risk potential from a map and a moving state of a moving object is known (Patent Document 5).

Further, there is known a device that predicts movement by using a map showing the ease of existence (difficulty) of each attribute of a traffic participant (Patent Document 6).

In addition, using multiple camera images, the template position representing the vehicle area is extracted, and the template position information is subjected to multi-viewpoint processing and time-series processing to associate the vehicles detected by multiple cameras. A traveling vehicle detection device capable of calculating the motion trajectory of a traveling vehicle with high accuracy is known (Patent Document 7).

Special Table 2009-505260 Gazette Japanese Unexamined Patent Publication No. 2012-164159 International Publication No. 16/092591 Japanese Patent No. 5673127 Japanese Patent No. 5620147 International Publication No. 2012/033173 Japanese Unexamined Patent Publication No. 2001-357489

The techniques of Patent Documents 1 to 4 are based on the premise that the local and relative positional relationship between a moving object and a traffic environment can be accurately estimated.

However, it is difficult to realize with a simple configuration, and the calculation of risk potential by particles using random numbers has different results for each repeated trial, and different movement predictions and danger judgments are made in the same scene, so quality. Guarantee is also difficult.

Further, the technique of Patent Document 5 reduces the risk potential calculation cost by switching the prediction means depending on the factor promoting the change in behavior such as the ratio of the pedestrian road to the future prediction range of the locally moving object. However, in a complicated environment such as a parking lot, the calculation cost of risk potential cannot be reduced because complicated prediction means are always selected.

Further, the technique of Patent Document 6 needs to generate a local map in the form of an existence possibility map showing the ease of existence (difficulty) of each traffic participant, and the attribute is attributed due to misrecognition of sensing or the like. It is difficult to prepare a rational local map to handle unknown traffic participants.

Further, the technique of Patent Document 7 is based on the premise that video frames can be frame-synchronized between a plurality of cameras (sensors) at regular intervals. If frame synchronization (synchronization of detection result transmission timing) cannot be achieved between multiple sensors due to communication delay or sensor specifications, frames are stored for a certain period of time on the device that integrates the results, and synchronization processing such as data interpolation is performed. There is a need to do. Further, when the number of sensors is indefinite, it means that the sensors to be synchronized are indefinite, so that the synchronization process is difficult.

In order to solve the problem of Patent Document 7, an approach that does not synchronize frames between a plurality of sensors can be considered (FIG. 13). However, since it is premised that the sensing result arrives at the device reliably and instantly, there is a problem that it is not possible to deal with temporary communication blackout and false detection / non-detection. Further, even if the observed amount obtained from the sensing result is clearly wrong as the inside of an obstacle, there is a problem of generating a new state quantity when it does not correspond to the existing state quantity.

In order to solve the above problem, the state quantity is expressed by multiple particles, and the state quantity is corrected independently of the arrival of the sensing result by using the movable range map based on the marking information such as the pedestrian road as the observation quantity of the total state quantity. However, an approach that reflects it when a new state quantity is generated can be considered (Fig. 14).

However, in this configuration, when evaluating the degree of suitability (probability) of the state quantity on the movable range map, it is necessary to express the state quantity with multiple particles in principle or to perform some approximate calculation. There was a problem that the calculation efficiency deteriorated when handling the state quantity of. Also, the likelihood value must be defined for all combinations of markings and traffic participant attributes.

The present invention has been made to solve the above-mentioned problems, and even if a temporary communication blackout or false detection / non-detection occurs in the sensor, the state estimation of a moving object can be accurately obtained. The purpose is to provide equipment and programs.

In order to achieve the above object, the state estimation device of the present invention uses a sensor to receive the result of detecting the tracked object, the sensing result acquisition unit, and the tracked object to be tracked. For each of the state quantity storage unit that stores the hypothesis indicating the state of the object and the tracked object to be tracked, the hypothesis stored in the state quantity storage unit and the movable range of the tracked object are recorded. The boundary detection unit that detects the boundary of the movable range around the tracked object and each of the tracked objects that are being tracked are detected by using the sensor based on the moving object movable range information. A hypothesis prediction unit that predicts the state indicated by the hypothesis of the tracking object at the timing and corrects the prediction result of the hypothesis according to the boundary detected by the boundary detection unit, and the tracking target being tracked. For each of the objects, the hypothesis of the tracked object is configured to include a hypothesis updating unit that updates the hypothesis based on the prediction result of the hypothesis and the detection result of the tracked object.

Further, the program of the present invention is a hypothesis that indicates the state of the tracked object for each of the sensing result acquisition unit that receives the result of detecting the tracked object using a sensor and the tracked object that is being tracked. For each of the state quantity storage unit and the tracked object being tracked, the hypothesis stored in the state quantity storage unit and the moving object movable range information in which the movable range of the tracked object is recorded. Based on the above, for each of the boundary detection unit that detects the boundary of the movable range around the tracking object and the tracking object that is being tracked, the tracking object is detected at the detection timing using the sensor. The tracking target for each of the hypothesis prediction unit that predicts the state indicated by the hypothesis and corrects the prediction result of the hypothesis according to the boundary detected by the boundary detection unit, and the tracking object that is being tracked. It is a program for functioning as a hypothesis updating unit that updates the hypothesis of an object based on the prediction result of the hypothesis and the detection result of the tracked object.

According to the state estimation device and the program of the present invention, the sensing result acquisition unit receives the result of detecting the tracked object by using the sensor, and the state quantity storage unit receives each of the tracked objects to be tracked. A hypothesis indicating the state of the tracked object is stored, and the hypothesis stored in the state quantity storage unit and the movable range of the tracked object are recorded for each of the tracked objects tracked by the boundary detection unit. Based on the moving object movable range information, the boundary of the movable range around the tracked object is detected.

Then, the hypothesis prediction unit predicts the state indicated by the hypothesis of the tracking object at the detection timing for each of the tracking objects being tracked, and responds to the boundary detected by the boundary detection unit. The hypothesis updater corrects the prediction result of the hypothesis, and updates the hypothesis of the tracked object for each tracked object based on the hypothesis prediction result and the detection result of the tracked object. do.

In this way, for each of the tracked objects to be tracked, the tracking is based on the hypothesis stored in the state quantity storage unit and the moving object movable range information in which the movable range of the tracked object is recorded. Detects the boundary of the movable range around the object, predicts the state indicated by the hypothesis of the tracked object at the detection timing using the sensor, and corrects the prediction result of the hypothesis according to the detected boundary. By updating the hypothesis of the tracked object based on the prediction result of the hypothesis and the detection result of the tracked object, even if the sensor has a temporary communication interruption or false detection / undetection, the moving object The amount of state can be obtained with high accuracy.

Further, in the state estimation device of the present invention, the boundary detecting unit detects the boundary of the movable range around the tracking object for each of the tracking objects, and the detected said object. The range in which an obstacle exists is detected from the boundary of the movable range, and the hypothesis prediction unit uses the sensor to detect the hypothesis of the tracked object for each of the tracked objects. When the indicated state is predicted and the position included in the predicted state of the tracked object hypothesis is within the range in which the obstacle exists, the range in which the obstacle exists and the movable range The prediction result of the hypothesis can be modified according to the boundary with.

Further, the hypothesis updating unit of the state estimation device of the present invention provides a hypothesis indicating the state of the tracking object when there is a detection result of the tracking object that does not correspond to any of the prediction results of the hypothesis. The newly generated tracking object is included in the state indicated by the hypothesis of the newly generated tracking object when the position is within the range where the obstacle exists. The hypothesis can be eliminated or the probability of existence included in the state indicated by the newly generated hypothesis of the tracked object can be reduced.

Further, the sensing result acquisition unit of the state estimation device of the present invention can receive the result of detecting the tracking object by using a plurality of sensors.

As described above, according to the state estimation device and the program of the present invention, the hypothesis stored in the state quantity storage unit and the movable range of the tracked object are recorded for each of the tracked objects to be tracked. Based on the moving object movable range information, the boundary of the movable range around the tracked object is detected, and the sensor is used to predict and detect the state indicated by the hypothesis of the tracked object at the detection timing. Temporary interruption of communication to the sensor by modifying the hypothesis prediction result according to the boundary and updating the hypothesis of the tracked object based on the hypothesis prediction result and the tracked object detection result. Even if false detection or non-detection occurs, the effect that the state amount of the moving object can be accurately obtained can be obtained.

It is a block diagram which shows the pedestrian state quantity estimation system which concerns on embodiment of this invention. It is a block diagram which shows the local server which concerns on embodiment of this invention. It is an image diagram which shows the example which corrects the state quantity by the collision determination which concerns on embodiment of this invention. It is an image diagram which shows the example which corrects the state quantity by the intrusion determination which concerns on embodiment of this invention. It is an image diagram which shows the example which corrects the state quantity by the intrusion determination in the case of a plurality of marking boundary lines which concerns on embodiment of this invention. It is an image diagram which shows the example which corrects the state quantity by the intrusion determination in the case of a plurality of pedestrians which concerns on embodiment of this invention. It is an image diagram which shows the example which corrects the state quantity by the intrusion determination in the case of a plurality of pedestrians which concerns on embodiment of this invention. It is an image diagram which shows the example which corrects the state quantity by the intrusion determination in the case of a plurality of pedestrians which concerns on embodiment of this invention. It is an image diagram which shows the example which corrects the state quantity by the intrusion determination in the case of a plurality of pedestrians which concerns on embodiment of this invention. It is an image diagram at the time of deleting the state quantity which concerns on embodiment of this invention. It is a flowchart which shows the content of the pedestrian state quantity estimation processing routine in the local server which concerns on embodiment of this invention. It is an image diagram which shows the example when the present invention is applied to a more complicated environment. It is an image diagram of an approach that does not synchronize frames between multiple sensors. It is an image diagram of an approach that corrects the state quantity independently of the arrival of the sensing result and reflects it when a new state quantity is generated.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

<Outline of Embodiment of the present invention>
In the embodiment of the present invention, matching processing (correspondence and state quantity) is performed according to the closeness between the observation amount generated from the information reached asynchronously from a plurality of sensors and the movement prediction of the state quantity already held. If there is no appropriate correspondence, a state quantity is generated around the observed quantity in consideration of the movable range.

At this time, the movement prediction of the common sense state quantity of the moving object is calculated only by a simple geometric calculation by the approximation that the movement is suppressed near the obstacle or the marking boundary (white line of pedestrian road separation, etc.).

All state quantities have an existence probability and decrease when there is no observable allocation, and the amount of decrease fluctuates according to the positional relationship with each marking boundary, and when the existence probability becomes sufficiently small, the state The quantity is erased.

As a result, robust integrated processing can be realized even for temporary communication blackouts and false detection / non-detection.

In the embodiment of the present invention, the present invention is applied to a local server that estimates the amount of pedestrian status by integrating the detection results of a pedestrian detector mounted on a vehicle and a pedestrian detector as an infrastructure sensor. The case of application will be described as an example.

<System configuration of pedestrian state quantity estimation system>
As shown in FIG. 1, the pedestrian state amount estimation system 1 according to the first embodiment includes a local server 10, a base station 50, a plurality of detectors 60 mounted on a plurality of vehicles, and an infrastructure. A detector 62 as a sensor is provided, and the base station 50 and the local server 10 are connected by a network 70 such as the Internet, and the base station 50 and the detectors 60 and 62 are connected by wireless communication. There is. The local server 10 is an example of a state estimation device.

The detectors 60 and 62 detect pedestrians at any time using a camera or radar, and each time they detect a pedestrian, the detectors 60 and 62 transmit the detection result to the local server 10 via the base station 50.

For example, the detector 60 extracts an image feature amount (SIFT, FIND, HOG, etc.) for each sliding window from a road image in front captured by a camera that images the front of the own vehicle, and an image for each sliding window. A pedestrian is detected using a feature amount and a pedestrian detection model (SVM, AdaBoost), and image coordinates representing the detected pedestrian position are obtained. Further, the detector 60 converts the image coordinates representing the pedestrian position into a three-dimensional position. At this time, the error variance matrix is obtained according to the detected height of the pedestrian. Further, the detector 60 obtains the absolute three-dimensional position of the pedestrian based on the absolute coordinates of the own vehicle measured by the GPS mounted on the own vehicle and the obtained three-dimensional position.

The above-mentioned three-dimensional position of the pedestrian and the error dispersion matrix are obtained for each detected pedestrian, and the set of the three-dimensional position of the pedestrian detected by the detector 60 of the vehicle Ci ₌ {y _{i , 1} , ..., y _{i, m} } and the observation information O _i = {Y _i , R _i } consisting of the set R _i = {ri _{i, 1} , ..., r _{i, m} } of the observation error distribution matrix, for each observation. To the local server 10.

The plurality of detectors 60 and 62 asynchronously detect pedestrians. Further, since the plurality of detectors 60 are mounted on different vehicles, the number of detectors 60 and 62 for transmitting the detection result to the local server 10 is indefinite.

The local server 10 includes a CPU, a RAM, and a ROM that stores a program for executing a pedestrian state amount estimation processing routine described later, and is functionally configured as shown below.

As shown in FIG. 2, the local server 10 includes a communication unit 100, a sensing result acquisition unit 110, a state quantity storage unit 120, a boundary detection unit 130, a moving object movable range map 140, and a hypothesis prediction unit 150. A collision determination state quantity correction unit 160, a boundary intrusion determination state quantity correction unit 170, a hypothesis matching unit 180, a hypothesis elimination unit 190, a hypothesis update unit 200, and an obstacle boundary inside determination unit 210 are provided. There is.

The sensing result acquisition unit 110 receives the result of detecting a pedestrian by using a plurality of sensors having an indefinite number.

Specifically, the sensing result acquisition unit 110 receives an observation consisting of a set of pedestrian coordinates and a set of observation error distribution matrix transmitted from any of the detectors 60 and 62 by the communication unit 100 each time. , Get an observation consisting of a set of pedestrian coordinates and a set of observation error distribution matrices.

The state quantity storage unit 120 stores, for each of the tracked pedestrians, a hypothesis indicating the state of the pedestrian.

Specifically, the state quantity storage unit 120 stores the state quantity (including the position and speed of the pedestrian) updated by the hypothesis updating unit 200, and the last observed time. That is, the set X = {x ₁ , ..., X _n } of the state quantity of the pedestrian, the variance-covariance matrix V = {v ₁ , ..., v _n } of the state quantity, and the set T of the last observed time. = {T ₁ , ..., t _n } is memorized.

を、信念空間上の仮説と呼び、その構成要素

を追跡器と呼ぶこととする。 In addition, in this embodiment,

Is called a hypothesis on the belief space, and its components

Will be called a tracker.

で構成される境界地図Ｕを格納している。ここで、ｕは障害物を示す線分の集合であり、

は障害物でない線分の集合である。 The moving object movable range map 140 records the movable range of a pedestrian. Specifically, it is a map showing the range of movement of pedestrians by displaying obstacles and signs, and is a set of line segments labeled as to whether or not they are obstacles.

The boundary map U composed of is stored. Here, u is a set of line segments indicating obstacles.

Is a set of line segments that are not obstacles.

The boundary map U is acquired from, for example, the remote server 80, and the boundary map U of the moving object movable range map 140 is updated at a predetermined cycle. For example, the remote server 80 transmits the boundary map U to the local server 10 at a cycle of one hour, and the local server 10 updates the boundary map U of the moving object movable range map 140 every time the boundary map U is received. ..

The boundary detection unit 130 detects the boundary of the movable range around the pedestrian based on the hypothesis stored in the state quantity storage unit 120 and the boundary map U for each of the pedestrians being tracked. , The range where an obstacle exists from the boundary of the detected movable range and the marked boundary on the road surface as the boundary of the movable range around the pedestrian are detected.

Specifically, the boundary detection unit 130 describes the pedestrian's hypothesis for each of the tracked pedestrians based on the pedestrian's hypothesis stored in the state quantity storage unit 120 and the boundary map U. It is detected from the boundary map U with a line segment indicating an obstacle existing around the position of and a line segment which is a marking boundary (white line for pedestrian road separation, etc.) as a boundary.

に含まれる線分を、路面上の標示境界とする。 Then, among the line segments included in the detected boundary, the range in which the obstacle exists is detected from the line segments included in the set u of the line segments indicating the obstacle. Also, among the line segments included in the detected boundary, a set of line segments that are not obstacles.

The line segment included in is used as the marking boundary on the road surface.

The hypothesis prediction unit 150 predicts the state indicated by the pedestrian's hypothesis at the detection timing using a sensor for each of the pedestrians being tracked.

に対して、現時刻の状態に関する予測

を生成する。すなわち、全ての追跡器

について、時間幅

だけ予測を行う。 Specifically, the hypothesis prediction unit 150 repeatedly predicts the state amount of the pedestrian at the next time by the prediction step of the Kalman filter for each of the plurality of pedestrians according to the current time, and walks at the current time. The set X = {x ₁ , ..., x _n } of the state quantity of the person and the variance-covariance matrix V = {v ₁ , ..., v _n } of the state quantity are obtained. For example, the state quantity of a pedestrian at the next time is predicted by constant velocity prediction or the like. That is, the hypothesis prediction unit 150 is a hypothesis.

On the other hand, the prediction about the state at the current time

To generate. That is, all trackers

About the time width

Only make predictions.

For each of the pedestrians being tracked, the collision determination state quantity correction unit 160 determines that the position included in the predicted state of the pedestrian's hypothesis is within the range in which the obstacle exists. The prediction result of the hypothesis is modified according to the boundary between the range in which the pedestrian exists and the range of motion.

と、追跡器の予測前の状態量ｘ_ｎと、予測後の状態量

とをそれぞれ境界地図Ｕ上に投影して得た点

及び

で定義されるベクトル

が、障害物を示す線分の集合ｕ内のいずれかの線分ｕ_ｋと交差するか否かを計算する。 Specifically, the collision determination state quantity correction unit 160 is a tracker that made a prediction.

And the state quantity before prediction x _n of the tracker and the state quantity after prediction

Points obtained by projecting and on the boundary map U

as well as

Vector defined by

Calculates whether or not crosses any of the line segments _uk in the set u of the line segments indicating the obstacle.

と修正して、予測後の状態量

を修正する。この場合、修正した予測後の歩行者の位置は、図３（ｃ）のようになる。 When they intersect (FIG. 3 (a)), the intersections are set to x _{u, n, and k} . At this time, as shown in FIG. 3 (b),

And correct it, the state quantity after prediction

To fix. In this case, the corrected predicted position of the pedestrian is as shown in FIG. 3 (c).

とｕ_ｋとのなす角ψの倍角だけ

を回転させたベクトルと、

との合成ベクトルの差分

をｘ_ｎの新たな速度成分として、カルマンフィルタの予測ステップを再度行い、予測後の状態量

を修正してもよい。この場合、修正した予測後の歩行者の位置は、図３（ｅ）のようになる。 Further, the collision determination state quantity correction unit 160 is centered on the points x _{u, n, and k} as shown in FIG. 3 (d).

Only the double angle of the angle _ψ between and uk

With the rotated vector,

Difference of composite vector from

As a new velocity component of x _n , the prediction step of the Kalman filter is performed again, and the state quantity after prediction is performed.

May be modified. In this case, the corrected predicted position of the pedestrian is as shown in FIG. 3 (e).

The boundary intrusion determination state quantity correction unit 170 has, for each of the tracked pedestrians, a predetermined range centered on the position included in the state indicated by the predicted hypothesis of the pedestrian, and the detected marking boundary. When the pedestrians intersect, the prediction result of the hypothesis is modified according to the intersection state between the predetermined range and the marking boundary.

と、障害物でない線分の集合

内のいずれかの線分

が交差するか否かを計算する。なお、状態量に含まれる位置を中心とする一定確率の信頼楕円とは、歩行者の存在する信頼度が一定確率以上となる範囲を表す楕円である。 Specifically, the boundary intrusion determination state quantity correction unit 170 is a vector obtained by the collision determination state quantity correction unit 160.

And a set of line segments that are not obstacles

One of the line segments in

Calculate whether or not they intersect. The confidence ellipse having a certain probability centered on the position included in the state quantity is an ellipse representing a range in which the reliability in which a pedestrian exists has a certain probability or more.

と共分散

との固有値解析で得られる、状態量に含まれる位置を中心とする一定確率の信頼楕円の範囲に対して、検出された標示境界の線分

との交差判定を行う。 In the case of intersection (FIG. 4A), the boundary intrusion determination state quantity correction unit 170 is in the state quantity.

And covariance

The line segment of the detected marking boundary for the range of the confidence ellipse with a certain probability centered on the position included in the state quantity obtained by the eigenvalue analysis of

And the intersection is judged.

への垂線のベクトルαの方向に、予測前の歩行者の速度ベクトルを、その交点と楕円中心のなす角θに比例した修正量αＫだけ修正する。ここで、αは予め定めた量を持つベクトルであり、Ｋは角θに応じて定まる値である。Ｋは、例えば下記式（１）のように求めることができる。 The intersection can be determined by the discriminant of a quadratic equation, and the line segment of the marking boundary intersecting from the center of the ellipse.

In the direction of the perpendicular vector α to, the velocity vector of the pedestrian before prediction is corrected by the correction amount αK proportional to the angle θ formed by the intersection and the center of the ellipse. Here, α is a vector having a predetermined amount, and K is a value determined according to the angle θ. K can be obtained, for example, by the following equation (1).

When the position of the pedestrian after prediction is close to the marking boundary line, the pedestrian basically moves without crossing the marking boundary line, so the speed vector of the pedestrian before prediction so that the movement distance becomes small. Is corrected, the prediction step of the Kalman filter is performed again, and the position of the pedestrian after the prediction is corrected.

In this case, since the angle θ is close to π, the value in the equation (1) is close to 1. That is, since the correction amount αK is a correction amount close to α, the speed vector of the pedestrian before prediction can be corrected so as to reduce the movement distance of the pedestrian.

For example, when the position of the pedestrian after the prediction exceeds the marking boundary line (FIG. 4 (b)), the speed vector of the pedestrian before the prediction is corrected as shown in FIG. 4 (c). Further, when the position of the pedestrian after the prediction does not exceed the marking boundary line (FIG. 4 (d)), the speed vector of the pedestrian before the prediction is corrected as shown in FIG. 4 (e).

On the other hand, when the position of the pedestrian after the prediction is far from the marking boundary line, it is considered that the pedestrian has moved beyond the marking boundary line, so that it is not necessary to correct the position of the pedestrian after the prediction.

In this case, the angle θ is sufficiently smaller than π, so that the value in the equation (1) is close to 0. That is, since the correction amount αK is a correction amount close to 0, even if the speed vector of the pedestrian before the prediction is corrected, the position of the pedestrian after the prediction can be hardly corrected.

Further, the position of the pedestrian after the prediction may be corrected instead of the speed vector of the pedestrian before the prediction. Further, when the value of K is equal to or less than a predetermined value, the speed vector of the pedestrian before prediction may not be modified.

In this way, by correcting the position of the pedestrian after prediction by the angle formed by the intersection and the center of the ellipse, a non-linear prediction that does not cross the boundary except for a tracker that tries to cross the boundary at an extremely high speed. It can be performed.

This method can also be applied to the case where there are a plurality of target boundaries as shown in FIG.

For example, as shown in FIG. 5, when there are intersections for two marking boundaries, for each of the intersections, a line segment of each marking boundary intersecting from the center of the ellipse based on the angle θ formed by the intersection and the center of the ellipse. The correction amount can be obtained in the direction of the vector of the perpendicular line to, and the correction amount can be corrected using the combined vector as the final correction amount. Note that FIG. 5A is an image diagram showing an application example of the state amount and the marking boundary line before prediction, FIG. 5B is a correction amount, and FIG. 5C is a correction amount.

In addition, by applying it to trackers, it is possible to make predictions so that pedestrians avoid each other (see FIG. 6). In this case, the boundary is calculated based on the confidence ellipses centered on the position of the pedestrian.

For example, when two different pedestrians are facing each other, the magnitude of the correction amount is set as the center of each hypothesis with respect to the boundary line generated by the line segment between the intersections of the confidence ellipses. It is proportional to the angle formed by the intersection of the confidence ellipses, and the direction of the correction amount is the opposite direction of the perpendicular vector from the center of each hypothesis to the boundary line. Here, FIG. 6A is an image diagram showing an arrangement of state quantities of a plurality of pedestrians before prediction, FIG. 6B is an image diagram showing an application example of a correction amount, and FIG. 6C is an image diagram showing an application example of the correction amount.

Here, when two pedestrians face each other in a high-speed situation, if the degree is slightly faster than in the case of FIG. 6 (FIG. 7), the correction amount is calculated in the same manner as in FIG. .. Here, FIG. 7A is an image diagram showing an arrangement of state quantities of a plurality of pedestrians before prediction, FIG. 7B is an image diagram showing an application example of a correction amount, and FIG. 7C is an image diagram showing an application example of the correction amount.

On the other hand, when it is considerably faster than the case of FIG. 6 (hypotheses are just close to each other) (FIG. 8), a large correction amount is calculated in the direction away from each other. Since the purpose is not to predict a collision but to make a prediction for tracking, it can be assumed that all pedestrians are moving so as not to collide, so it is possible to assume that the prediction is made at the timing of passing each other. As a prediction, it can be predicted that pedestrians will avoid each other greatly.

Note that FIG. 8A is an image diagram showing an arrangement of state quantities of a plurality of pedestrians before prediction, FIG. 8B is an image diagram showing an application example of a correction amount, and FIG. 8C is an image diagram showing an application example of the correction amount.

Further, when the hypotheses pass each other (FIG. 9), which is extremely faster than in the case of FIG. 6, the correction amount is calculated in the direction of slightly accelerating. That is, as a prediction at a time after the passing timing, it is possible to correspond to a movement in which the pedestrian maintains / corrects the original movement direction after the avoidance is completed. Here, FIG. 9A is an image diagram showing an arrangement of state quantities of a plurality of pedestrians before prediction, FIG. 9B is an image diagram showing an application example of a correction amount, and FIG. 9C is an image diagram showing an application example of the correction amount.

When the hypothesis matching unit ₁₈₀ receives the latest observation information Oi from the sensing result acquisition unit 110, the hypothesis matching unit 180 tracks it by the hypothesis prediction unit 150 in response to the current time using the boundary map U of the moving object movable range map 140. The hypothesis predicted for each of the pedestrians is associated with the detection result for each of the _pedestrians represented by the latest observation information Oi.

の当てはまりのよさを意味する観測尤度

を計算する。 Specifically, the hypothesis matching unit 180 is a _tracker for the observation information Oi based on the predicted hypothesis and the boundary.

Observing likelihood, which means good fit

To calculate.

Then, the hypothesis matching unit 180 sets X = {x ₁ , ..., X _n } of the state quantity of the pedestrian at the current time based on the observation likelihood θ _{i, m} , and in the latest observation information O _i . Correspondence is made with the set of the detected three-dimensional positions of the pedestrian Y _i = {y _{i, 1} , ..., y _{i, m} }.

Further, the hypothesis matching unit 180 newly generates a hypothesis indicating the state of the pedestrian when there is a detection result of the pedestrian that does not correspond to any of the prediction results of the hypothesis.

Specifically, the hypothesis matching unit 180 detects pedestrians who did not correspond to the _tracker among the latest observation information _Oi based on the result of association with the detection result of the latest observation information Oi. From the result, a tracker containing a new state quantity x is generated.

It is assumed that the tracker including the new state quantity x has the same coordinates and error dispersion matrix as the observed three-dimensional position y of the pedestrian.

The hypothesis updating unit 200 updates the hypothesis of the pedestrian for each of the tracked pedestrians based on the prediction result of the hypothesis and the detection result of the pedestrian.

Specifically, when the sensing result acquisition unit 110 latest observation information O _i is received, the corresponding three-dimensional position y _i is obtained for each of the pedestrians associated with the hypothesis matching unit 180 latest observation information O _i . Using the error variance matrix r _{i, j} and the pedestrian state quantity and the variance-covariance matrix obtained in the prediction step with, _{and j} as observed values, the state quantity is updated by the filtering step of the Kalman filter. , Update the last observed time.

の観測尤度θ_i，ｍから、

となる追跡器を特定し、

が予め設定した閾値よりも大きい場合、状態量

と分散共分散行列

とを更新する。 The hypothesis update unit 200 is a _tracker for the observation information Oi.

From the observation likelihood θ _{i, m} of

Identify the tracker that will be

If is greater than a preset threshold, the state quantity

And the variance-covariance matrix

And update.

が予め設定した閾値以下である場合、

として観測情報Ｏ_ｉ，ｍを３次元空間に投影した位置に、一定の存在確率が設定された新規の追跡器を生成して、

に加える。 also,

Is less than or equal to a preset threshold

As a new tracker with a certain existence probability set at the position where the observation information _{Oi, m} is projected onto the three-dimensional space,

Add to.

を、現在時刻に設定し、存在確率を

等として、一定値だけ増加させる。 In addition, the hypothesis update unit 200 has the last update time of the updated or newly created tracker.

Is set to the current time, and the existence probability is set.

And so on, increase by a certain value.

等として、一定値だけ減少させる。 Also, the probability of existence of an unassigned tracker

And so on, reduce by a certain value.

Then, the hypothesis update unit 200 stores the updated or newly created tracker in the state quantity storage unit 120. At this time, the current time is stored as the last observed time.

The obstacle boundary inside determination unit 210 determines the newly generated pedestrian hypothesis when the position included in the state indicated by the newly generated pedestrian hypothesis is within the range in which the obstacle exists. Eliminate or reduce the probability of existence in the state indicated by the newly generated pedestrian hypothesis.

等として、当該追跡器が削除されるか、又はすぐには存在確率が上昇しないように設定する。 Specifically, when the position of the tracker newly generated by the hypothesis matching unit 180 is within the range where the obstacle exists, the obstacle boundary inner determination unit 210 determines the existence probability of the tracker.

For example, the tracker is deleted or set so that the existence probability does not increase immediately.

を境界地図Ｕに直情から投影して得た点

と、境界地図Ｕ上の障害物である線分の集合ｕのうち閉路を形成するものであるｕ_ｃについて、閉路内に

が存在するか否かに基づいて判定する。 Here, whether or not the obstacle is within the existing range is determined by the state quantity of the newly generated tracker.

Point obtained by projecting directly onto the boundary map U

And, of the set u of the line segments that are obstacles on the boundary map U, _uc , which forms a cycle, is within the cycle.

Judgment is based on whether or not is present.

に関する下記式（２）の不等式が成立している場合に、

は閉路ｕ_ｃ，ｊの内側に存在すると判定することができる。 For example, if the boundary map U is created so that the cycle is composed of only triangles consisting of all three lines, a cycle uc _{, j} = {uc _{, j, 1} , uc _{, j, 2} , u. For the three points that make up _{c, j, 3} }, the three outer products

When the inequality of the following equation (2) is satisfied,

Can be determined to exist inside the closed uc _{, j} .

The hypothesis erasing unit 190 erases a tracker including a pedestrian state quantity that has passed a certain time or more from the last observed time from the state quantity storage unit 120 (FIG. 10).

が十分に小さい追跡器を除去する。 Specifically, the hypothesis elimination unit 190 has an existence probability.

Removes a tracker that is small enough.

The local server 10 outputs the set X of the state quantity updated by the above series of processes as the integrated result of the pedestrian detection result.

<Operation of pedestrian state quantity estimation system 1>
Next, the operation of the pedestrian state quantity estimation system 1 according to the first embodiment will be described. First, pedestrians are sequentially detected by each of the plurality of detectors 60 mounted on the plurality of vehicles and the detector 62 as an infrastructure sensor, and each time the pedestrian is detected, the detection result is transmitted via the base station 50. , The pedestrian state amount estimation processing routine shown in FIG. 11 is executed in the local server 10 while being transmitted to the local server 10.

In step S100, the hypothesis update unit 200 initializes the hypothesis to an empty set as shown in the following equation (3).

In step S110, the sensing result acquisition unit 110 receives the latest observation information _Oi , which is the result of detecting a pedestrian, using the sensor.

In step S120, the hypothesis matching unit 180 converts the three-dimensional position of the pedestrian included in the latest observation information Oi received in step _S150 from the sensor coordinate system to the world coordinate system.

In step S130, the boundary detection unit 130 determines the boundary of the movable range around the pedestrian based on the hypothesis stored in the state quantity storage unit 120 and the boundary map U for each of the pedestrians being tracked. Is detected, and the range where an obstacle exists from the boundary of the detected movable range and the marked boundary on the road surface as the boundary of the movable range around the pedestrian are detected.

In step S140, the hypothesis prediction unit 150 predicts the state indicated by the pedestrian's hypothesis at the detection timing using the sensor for each of the pedestrians being tracked.

In step S150, when the position included in the predicted state of the pedestrian's hypothesis is within the range where an obstacle exists for each of the pedestrians being tracked by the collision determination state amount correction unit 160. In addition, the prediction result of the hypothesis is modified according to the boundary between the range where the obstacle exists and the range of movement.

In step S160, the boundary intrusion determination state quantity correction unit 170 is detected for each of the tracked pedestrians as a predetermined range centered on the position included in the predicted state of the pedestrian's hypothesis. When the marking boundary intersects with the marking boundary, the prediction result of the hypothesis is modified according to the intersection state between the predetermined range and the marking boundary.

In step _S170 , when the hypothesis matching unit 180 receives the latest observation information Oi from the sensing result acquisition unit 110, the hypothesis prediction unit 180 corresponds to the current time using the boundary map U of the moving object movable range map 140. The hypothesis predicted for each of the pedestrians tracked by 150 is associated with the detection result for each of the _pedestrians represented by the latest observation information Oi.

In step S180, the hypothesis updating unit 200 updates the hypothesis of the pedestrian for each of the pedestrians being tracked based on the prediction result of the hypothesis and the detection result of the pedestrian. In addition, when there is a pedestrian detection result that does not correspond to any of the hypothesis prediction results, a new hypothesis indicating the pedestrian state is generated.

In step S190, when the position included in the state indicated by the newly generated pedestrian hypothesis is within the range in which the obstacle exists, the obstacle boundary inner determination unit 210 is newly generated walking. Eliminate one's hypothesis or reduce the probability of being included in the state indicated by the newly generated pedestrian hypothesis.

In step S200, the hypothesis erasing unit 190 erases the tracker including the pedestrian state quantity that has passed a certain time or more from the last observed time from the state quantity storage unit 120.

Then, in step S210, the state indicated by the updated hypothesis is output as the integrated result of the pedestrian detection result, and the process returns to step S110. For example, the integrated result of the pedestrian detection result is transmitted to the remote server 80.

As described above, according to the state estimation device according to the embodiment of the present invention, for each of the tracked objects to be tracked, the hypothesis stored in the state quantity storage unit and the movable range of the tracked object. Detects the boundary of the movable range around the tracked object based on the recorded moving object movable range information, and predicts the state indicated by the hypothesis of the tracked object at the detection timing using a sensor. By modifying the hypothesis prediction result according to the detected boundary and updating the hypothesis of the tracked object based on the hypothesis prediction result and the tracked object detection result, the sensor is temporarily used. Even when communication is interrupted or false detection / non-detection occurs, the state amount of the moving object can be accurately obtained.

The present invention is not limited to the above-described embodiment, and various modifications and applications can be made without departing from the gist of the present invention.

For example, in the above embodiment, Kalman prediction is premised, but particle prediction may be performed, or sampling-based prediction using unscented conversion or random sampling may be performed. In that case, it is possible to make predictions in a more complicated environment as shown in FIG. Here, FIG. 12A is an example of prediction in a complicated environment, FIG. 12B is a simple Kalman prediction, and FIG. 12C is an image diagram when a particle filter or the like is used.

Further, in the present embodiment, the case of tracking a pedestrian has been described, but other moving objects such as a bicycle and an animal can be adopted as the tracking object.

Further, although described as an embodiment in which the program is pre-installed in the specification of the present application, it is also possible to store the program in a computer-readable recording medium and provide the program.

1 Pedestrian state quantity estimation system 10 Local server 50 Base station 60 Detector 62 Detector 70 Network 80 Remote server 100 Communication unit 110 Sensing result acquisition unit 120 State quantity storage unit 130 Boundary detection unit 140 Moving object movable range map 150 Hypothesis Prediction unit 160 Collision judgment state quantity correction unit 170 Boundary intrusion judgment state quantity correction unit 180 Hypothesis matching unit 190 Hypothesis elimination unit 200 Hypothesis update unit 210 Obstacle boundary inside determination unit

Claims (6)

A sensing result acquisition unit that receives the result of detecting a tracked object using a sensor,
For each of the tracked objects being tracked, a state quantity storage unit that stores a hypothesis indicating a state including the velocity of the tracked object, and
For each of the tracked objects to be tracked, the tracked object is based on the hypothesis stored in the state quantity storage unit and the moving object movable range information in which the movable range of the tracked object is recorded. A boundary detection unit that detects the boundary of the movable range around the
For each of the tracked objects being tracked, the sensor is used to predict the state indicated by the hypothesis of the tracked object at the detection timing, and the hypothesis is determined according to the boundary detected by the boundary detection unit. Hypothesis prediction unit that corrects the prediction result of
For each of the tracked objects being tracked, a hypothesis updating unit that updates the hypothesis of the tracked object based on the prediction result of the hypothesis and the detection result of the tracked object.
Including
The boundary detection unit detects the boundary of the movable range around the tracked object for each of the tracked objects, and an obstacle is present from the boundary of the detected movable range. Detect the range and
The hypothesis prediction unit predicts the state indicated by the hypothesis of the tracking object at the detection timing for each of the tracking objects being tracked, and the predicted tracking object. When the predetermined range centered on the position included in the state indicated by the hypothesis and the range in which the obstacle exists intersects, the intersection of the predetermined range and the range in which the obstacle exists and the predetermined range. The prediction result of the hypothesis is corrected by decelerating the speed as the angle formed by the center of the range becomes larger.
State estimator. The boundary detection unit detects the boundary of the movable range around the tracked object for each of the tracked objects, and an obstacle is present from the boundary of the detected movable range. Detect the range and
The hypothesis prediction unit predicts the state indicated by the hypothesis of the tracking object at the detection timing for each of the tracking objects being tracked, and the predicted tracking object. When the position included in the state indicated by the hypothesis is within the range in which the obstacle exists, the prediction result of the hypothesis is modified according to the boundary between the range in which the obstacle exists and the movable range. Item 1. The state estimation device according to item 1. The hypothesis update unit
When there is a detection result of the tracking object that does not correspond to any of the prediction results of the hypothesis, a hypothesis indicating the state of the tracking object is newly generated.
Whether to eliminate the hypothesis of the newly generated tracking object when the position included in the state indicated by the hypothesis of the newly generated tracking object is within the range in which the obstacle exists. , Or the state estimation device according to claim 2, which reduces the existence probability included in the state indicated by the hypothesis of the newly generated tracking object. The boundary detection unit detects, for each of the tracked objects, the marked boundary on the road surface as the boundary of the movable range around the tracked object.
The hypothesis prediction unit predicts the state indicated by the hypothesis of the tracking object at the detection timing for each of the tracking objects being tracked, and the predicted tracking object. When the predetermined range centered on the position included in the state indicated by the hypothesis intersects the detected marking boundary, the prediction result of the hypothesis is determined according to the intersection state between the predetermined range and the marking boundary. The state estimation device according to any one of claims 1 to 3. The state estimation device according to any one of claims 1 to 4, wherein the sensing result acquisition unit receives a result of detecting a tracking object by using a plurality of sensors. Computer,
Sensing result acquisition unit that receives the result of detecting the tracked object using the sensor,
A state quantity storage unit that stores a hypothesis indicating a state including the velocity of the tracked object for each of the tracked objects being tracked.
For each of the tracked objects to be tracked, the tracked object is based on the hypothesis stored in the state quantity storage unit and the moving object movable range information in which the movable range of the tracked object is recorded. Boundary detector that detects the boundary of the movable range around the
For each of the tracked objects being tracked, the sensor is used to predict the state indicated by the hypothesis of the tracked object at the detection timing, and the hypothesis is determined according to the boundary detected by the boundary detection unit. Hypothesis prediction unit that corrects the prediction result of
A program for functioning as a hypothesis update unit that updates the hypothesis of the tracked object for each of the tracked objects based on the prediction result of the hypothesis and the detection result of the tracked object. And
The boundary detection unit detects the boundary of the movable range around the tracked object for each of the tracked objects, and an obstacle is present from the boundary of the detected movable range. Detect the range and
The hypothesis prediction unit predicts the state indicated by the hypothesis of the tracking object at the detection timing for each of the tracking objects being tracked, and the predicted tracking object. When the predetermined range centered on the position included in the state indicated by the hypothesis and the range in which the obstacle exists intersects, the intersection of the predetermined range and the range in which the obstacle exists and the predetermined range. A program that corrects the prediction result of the hypothesis by decelerating the speed as the angle formed by the center of the range becomes larger.

Images