JP7034353B2 - Positioning system and positioning method - Google Patents

Description

The present invention relates to a position determination system that estimates the position of an object using an angle measuring sensor, and a position determination method.

Generally, when estimating the position of an object (also called a target) such as a search target or a defect, multiple angle measuring sensors that measure the azimuth angle with respect to the object are used, and each angle measuring sensor is used based on the principle of triangulation. It can be estimated from the intersection of the azimuth lines extending to the object. On the other hand, when there are a plurality of objects, the number of directional lines extending from the angle measurement sensor to each object is obtained, so the intersection of the directional lines is other than the true position of the object. It will also appear in places (hereinafter referred to as virtual images). Therefore, due to the existence of this virtual image, there is a problem that it is erroneously recognized that there are more objects than the actual number, and the estimation accuracy of the position of the objects is significantly deteriorated.

In order to solve such a problem, conventionally, the actual measurement data of each target, the sensor position, and the classification data in which the possibility coefficient from which the actual measurement data is obtained are set for each target are input, and the actual measurement value is used. In order to reduce the difference, the cluster characteristic data optimization means that outputs the cluster characteristic data represented by a set of temporary positions of the target, and the difference between the actual measurement value and the cluster characteristic data, the actual measurement data, and the sensor position are input. With the classification data optimization means that resets the new possibility coefficient obtained from each target for each target and outputs it to the cluster characteristic data optimization means and the classification evaluation value calculation means as new classification data. , A technique provided with a classification evaluation value calculation means for calculating a classification evaluation value by using these as inputs has been proposed (see, for example, Patent Document 1). A PHD filter based on a Kalman filter is known as a technique for estimating a state for a plurality of targets (see, for example, Non-Patent Document 1).

Japanese Unexamined Patent Publication No. 09-304520

B-N. Vo and W-K. Ma, “The Gaussian Mixture Probability Hypothesis Density Filter,” IEEE Trans. Signal Processing, vol.54, no.11, pp.4091-4104, Nov., 2006.

The technique described in Patent Document 1 aims to output a combination of directional lines presumed to point to the same object and eliminate a virtual image. However, in reality, there is an observation error in the measured value of the azimuth measured by the angle measuring sensor, so even if all the virtual images can be eliminated, the position estimation of the object contains an error and the estimation accuracy. There is a concern that the price will decline. Further, in the technique described in Patent Document 1, it is necessary to input the true value of the number of objects in advance in order to estimate the position of the object, and when the number of objects is unknown or changes, the position of the object is estimated. Was difficult.

The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a position determination system and a position orientation method for improving the position estimation accuracy of a plurality of objects.

In order to solve the above-mentioned problems and achieve the object, the position positioning system according to the present invention is arranged independently of each other, and has at least three angle measuring sensors for measuring azimuths with respect to a plurality of objects, and three. An intersection determination unit that determines whether or not three azimuth lines extending from one angle sensor to each object intersect, and an intersection determination unit that determines that the three azimuth lines intersect, from the three azimuth lines. It is characterized by being provided with a position estimation unit that calculates a deemed intersection and estimates this deemed intersection as the position of an object.

According to this configuration, the deemed intersections are calculated from the three azimuth lines, and the deemed intersections are estimated as the positions of the objects, so that the positions of the objects can be estimated accurately. Further, since the position of the object is estimated by determining whether or not the three azimuth lines intersect, it is not necessary to input the true value of the number of objects in advance as in the conventional case, and the number of objects is unknown. The position of the object can be easily estimated even if it changes.

In this configuration, the intersection determination unit calculates the intersection of two azimuth lines extending from two arbitrarily selected angle measuring sensors to each object, and inside a circle having a predetermined error radius centered on this intersection. May determine that the three azimuth lines intersect when the azimuth lines extending from the remaining angle measuring sensors to the object pass.

Further, the intersection determination unit calculates the distance between the intersection of two azimuth lines extending from two arbitrarily selected angle measuring sensors to each object and the azimuth line extending from the remaining angle measuring sensors to the object. However, if this distance is smaller than a predetermined error radius of the circle centered on the intersection, it may be determined that the three azimuth lines intersect.

Further, the error radius may be set based on the value of the standard deviation of the angle measurement error of the angle measurement sensor. Further, the error radius may be set based on the value of the standard deviation of the angle measurement error in the angle measurement sensor having a large angle measurement error among the two arbitrarily selected angle measurement sensors.

Further, the position estimation unit may calculate the deemed intersection from the three azimuth lines by using the least squares method. Further, a correction unit may be provided for correcting the value of the deemed intersection by estimating the state of the object using the state estimation filter based on a predetermined motion model.

Further, the position positioning system according to the present invention is arranged independently of each other, and has at least three angle measuring sensors that measure the azimuth angle and the zenith angle that define the direction vector of the azimuth line with respect to a plurality of objects, and presently or A storage unit that stores all line intersections where all azimuth lines extending from all angle measuring sensors to one object intersect within a predetermined period in the past, and the object from a plurality of arbitrarily selected angle measuring sensors. An intersection calculation unit that calculates multiple intersections where azimuth lines extending to each intersect, a likelihood determination unit that determines the likelihood of multiple intersections based on all line intersections, and one based on the magnitude of the likelihood. It is characterized by including a position estimation unit for extracting an intersection and estimating the intersection as the position of one object.

According to this configuration, the likelihood of the intersection is determined based on the current or past all-line intersection, and the position of the intersection is estimated as the position of the object from the magnitude of the likelihood, so that the angle measuring sensor determines the object. Even if it cannot be detected at all times, the position of the object can be estimated accurately.

Further, the likelihood determination unit may be configured to determine the highest likelihood of the intersection having the shortest distance from the whole line intersection.

Further, when it is known in advance that a plurality of objects exist in close proximity to each other, the likelihood determination unit sets the likelihood of one intersection to the number of other intersections existing within a predetermined range of the intersection. Corrections may be made to increase accordingly.

Further, the intersection calculation unit may increase the number of angle measuring sensors selected as the detection probability that the angle measuring sensor detects an object increases.

Further, the storage unit may shorten the predetermined period as the product of the relative speed between the object and the angle measurement sensor and the measurement cycle of the angle measurement sensor increases.

In addition, the closest points of all the directional lines extending from all the angle measuring sensors to one object are calculated, and all the directional lines pass within a predetermined error region centered on this closest point. In some cases, an intersection determination unit that determines the closest approach point as an all-line intersection may be provided.

The position determination method according to the present invention uses at least three angle measuring sensors to measure an azimuth angle with respect to a plurality of objects, and three orientations extending from the three angle measuring sensors to each object. An intersection determination step for determining whether or not the lines intersect, and when it is determined that the three azimuth lines intersect, a deemed intersection is calculated from the three azimuth lines, and this deemed intersection is estimated as the position of the object. It is characterized by having a position estimation step.

In this configuration, the intersection determination step calculates the intersection of two azimuth lines extending from two arbitrarily selected angle measuring sensors to each object, and inside a circle having a predetermined error radius centered on this intersection. May determine that the three azimuth lines intersect when the azimuth lines extending from the remaining angle measuring sensors to the object pass.

Further, in the intersection determination step, the distance between the intersection of two azimuth lines extending from two arbitrarily selected angle measuring sensors to each object and the azimuth line extending from the remaining angle measuring sensors to the object is calculated. However, if this distance is smaller than a predetermined error radius of the circle centered on the intersection, it may be determined that the three azimuth lines intersect.

Further, in the intersection determination step, the error radius may be set based on the value of the standard deviation of the angle measurement error of the angle measurement sensor. Further, in the intersection determination step, the error radius may be set based on the value of the standard deviation of the angle measurement error in the angle measurement sensor having the larger angle measurement error among the two arbitrarily selected angle measurement sensors.

Further, in the position estimation step, the deemed intersection may be calculated from the three azimuth lines using the least squares method. Further, a correction step may be provided in which the value of the deemed intersection is corrected by estimating the state of the object using the state estimation filter based on a predetermined motion model.

The position determination method according to the present invention includes an angle measurement step of measuring an azimuth angle and a zenith angle that define direction vectors of azimuth lines with respect to a plurality of objects by using at least three angle measurement sensors, and present or past angle measurement steps. From a full-line intersection storage step that calculates and stores all-line intersections where all azimuth lines extending from all angle-finding sensors to one object intersect within a predetermined period, and from a plurality of arbitrarily selected angle-measurement sensors. An intersection calculation step for calculating a plurality of intersections where azimuth lines extending to the object intersect, a likelihood determination step for determining the likelihood of each of the plurality of intersections based on all line intersections, and a magnitude of the likelihood. It is characterized by comprising a position estimation step of extracting one intersection and estimating the intersection as the position of one object.

Further, in the likelihood determination step, the likelihood of the intersection having the shortest distance from the whole line intersection may be determined to be the highest.

Further, when it is known in advance that a plurality of objects exist in close proximity to each other, in the likelihood determination step, the likelihood of one intersection is set to the number of other intersections existing within a predetermined range of the intersection. Corrections may be made to increase accordingly.

Further, in the intersection calculation step, the number of angle measuring sensors selected may be increased as the detection probability that the angle measuring sensor detects an object increases.

Further, the full-line intersection storage step may be set to a shorter predetermined period as the product of the relative speed between the object and the angle measuring sensor and the measurement cycle of the angle measuring sensor increases.

When all the directional lines pass within a predetermined error region centered on this closest point while calculating the closest points of all the directional lines extending from all the angle measurement sensors to one object. , The intersection determination step for determining the closest approach point as an all-line intersection may be provided.

According to the present invention, since the deemed intersection is calculated from the three azimuth lines and the deemed intersection is estimated as the position of the object, the position of the object can be estimated accurately. Further, since the position of the object is estimated by determining whether or not the three azimuth lines intersect, it is not necessary to input the true value of the number of objects in advance as in the conventional case, and the number of objects is unknown. The position of the object can be easily estimated even if it changes.

Further, according to the present invention, since the likelihood of the intersection is determined based on the current or past all-line intersection and the position of the intersection is estimated as the position of the object from the magnitude of the likelihood, the angle measuring sensor is the target. Even if the object cannot be detected at all times, the position of the object can be estimated accurately.

FIG. 1 is a block diagram showing a schematic configuration of a position orientation system according to a first embodiment. FIG. 2 is a flowchart showing a procedure in which the position determination system estimates the position of an object. FIG. 3 is a diagram illustrating a determination procedure of the intersection determination step. FIG. 4 is a block diagram showing a schematic configuration of the position orientation system according to the second embodiment. FIG. 5 is a diagram for explaining an azimuth angle and a zenith angle that define a direction vector. FIG. 6 is a flowchart showing a procedure in which the position determination system estimates the position of an object. FIG. 7 is a flowchart showing a procedure of arithmetic processing using a PHD filter. FIG. 8 is a diagram illustrating a determination procedure of the intersection determination step. FIG. 9 is a diagram showing an example of the arrangement relationship between the intersection of all lines and the intersection of each azimuth line. FIG. 10 is a graph showing an example of the relationship between the detection probability and the number of angle measuring sensors selected. FIG. 11 is a graph showing the relationship between the time window width, the relative speed of the angle measurement sensor with respect to the angle measurement sensor, and the product of the measurement cycle of the angle measurement sensor.

Hereinafter, embodiments according to the present invention will be described in detail with reference to the drawings. The present invention is not limited to this embodiment. In addition, the components in the embodiment include those that can be easily replaced by those skilled in the art, or those that are substantially the same. Furthermore, the components described below can be combined as appropriate.

[First Embodiment]
FIG. 1 is a block diagram showing a schematic configuration of a position orientation system according to a first embodiment. The position determination system 1 estimates the position of an object such as a search target or a defect by using an angle measuring sensor. FIG. 1 describes a case where the positions of two objects M ₁ and M ₂ are estimated by using three angle measuring sensors S ₁ , S ₂ and S ₃ as an example. In the present embodiment, a predetermined direction from the angle measuring sensor toward the object is the X direction, and the direction orthogonal to the horizontal direction in the X direction is the Y direction. The number of angle measuring sensors is not limited if at least three are provided, and the number of objects is not limited if there are a plurality of objects. Here, the two objects M ₁ and M ₂ are objects existing on the same plane (XY plane) such as the ground surface, and even if they are stationary, they can move freely. You may.

As shown in FIG. 1, the position setting system 1 performs arithmetic processing based on three angle measuring sensors S ₁ , S ₂ , S ₃ and the measured values of these angle measuring sensors S ₁ , S ₂ , and S ₃ . It is provided with a processing unit 10. The angle measuring sensors S ₁ to S ₃ are arranged independently of each other at predetermined intervals in the Y direction, and are connected to the arithmetic processing unit 10 so as to be able to communicate with each other by wire or wirelessly. The angle measuring sensors S ₁ , S ₂ , and S ₃ receive signals (sound waves, light waves, radio waves, etc.) from the objects M ₁ and M ₂ and measure the transmission direction of these signals as the azimuth angle. For example, as the angle measuring sensor, a microphone that receives audio signals emitted by the objects M ₁ and M ₂ can be used. Further, as the angle measuring sensor, a camera that captures the surroundings may be used, and whether or not a predetermined shape indicating an object exists in the captured image information may be identified by image processing or the like.

The azimuth angle is defined as an angle formed by the reference line 5 extending in the X direction shown in FIG. 1, for example, the azimuth lines l ₁₁ and l ₁₂ (l _1p ) extending from the angle measuring sensor S ₁ to the objects M ₁ and M ₂ , respectively. , P = 1, 2) azimuths are represented as azimuths θ ₁₁ and θ ₁₂ (θ _1p , p = 1, 2). Similarly, the azimuths of the azimuth lines l ₂₁ , l ₂₂ (l _2q , q = 1, 2) extending from the angle measuring sensor S ₂ to the objects M ₁ and M ₂ , respectively, are the azimuth angles θ ₂₁ and θ ₂₂ (θ). _2q , q = 1, 2), and the azimuths of the azimuth lines l ₃₁ , l ₃₂ (l _3r , r = 1, 2) extending from the angle measuring sensor S ₃ to the objects M ₁ and M ₂ , respectively, are It is expressed as azimuth angles θ ₃₁ and θ ₃₂ (θ _3r , r = 1, 2). The angle measuring sensors S ₁ to S ₃ each have their own position information as known information, and arithmetically process the azimuth angles (measured values) θ _1p , θ _2q , and θ _3r measured at the same time as this position information. It is sequentially transmitted to the unit 10. In the present embodiment, the angle measuring sensors S1 to _{S3 are} fixedly installed at predetermined positions, but for example, they can acquire their own position information based on a GPS (Global Positioning System) signal. If so, the angle measuring sensors S1 to S3 may be _{mounted on} the moving body.

As shown in FIG. 1, the arithmetic processing unit 10 includes a data receiving unit 11, an intersection determination unit 12, a position estimation unit 13, a correction unit 14, a display unit 15, and a control unit 16. The data receiving unit 11 receives the azimuth angles θ _1p , θ _2q , and θ _3r sequentially transmitted from the angle measuring sensors S ₁ , S ₂ , and S ₃ . The intersection determination unit 12 extends from the angle measuring sensors S ₁ , S ₂ , S ₃ toward the objects M ₁ and M ₂ , respectively, based on the received information of the azimuth angles θ _1p , θ _2q , and θ _3r . It is determined whether or not l _1p , l _2q , and l _3r intersect. In the present embodiment, crossing includes not only the case where the directional lines l _1p , l _2q , l _3r intersect at one point, but also the case where each directional line exists in a predetermined region (passes in the predetermined region). .. When the position estimation unit 13 determines that the intersection determination unit 12 intersects the three azimuth lines l _1p , l _2q , l _3r , the position estimation unit 13 estimates the intersecting positions as the objects M ₁ and M ₂ . Further, when the intersection determination unit 12 determines that only two of the three direction lines l _1p , l _2q , and l _3r intersect, the position estimation unit 13 determines that the intersections 20, 21, and are crossed. 22 is presumed to be a virtual image.

The correction unit 14 corrects the positions of the objects M ₁ and M ₂ estimated by the position estimation unit 13 by state estimation using a state estimation filter based on a predetermined motion model. As the state estimation filter, for example, a Kalman filter or a particle filter can be used. Since the azimuths measured by the angle measurement sensors S ₁ , S ₂ , and S ₃ are instantaneous values, this state estimation can suppress the variation in the instantaneous values and suppress the position estimation error caused by the angle measurement error. can.

The display unit 15 is, for example, a display device, and displays the estimated position information of the objects M ₁ and M ₂ . The control unit 16 controls the operation of the position determination system ₁ , and controls the angle measurement sensors S1, S2, _{S3 ,} the data reception unit 11, the intersection determination unit 12, the position estimation unit 13, and the correction unit 14. Then, the operation of estimating the positions of the objects M ₁ and M ₂ is executed. Next, the operation procedure of the position positioning method will be described.

FIG. 2 is a flowchart showing a procedure in which the position determination system estimates the position of an object. Under the control of the control unit 16, the three angle measuring sensors S ₁ , S ₂ , and S ₃ measure the azimuth angles θ _1p , θ _2q , and θ _3r with respect to all the objects M ₁ and M ₂ (step S1). In this embodiment, for convenience of explanation, the number of objects M ₁ and M ₂ is two, but the number of objects is unknown in the actual measurement operation. Therefore, based on the received signals received by the angle measuring sensors S ₁ , S ₂ , and S ₃ , an object located in the direction in which the received signal is emitted is assumed, and the azimuth angles θ _1p and θ _2q with respect to this object. , Θ _3r is measured.

Next, the measurement information of each angle measuring sensor S ₁ , S ₂ , S ₃ is aggregated in the arithmetic processing unit 10 (step S2). This measurement information includes not only the azimuth angles θ _1p , θ _2q , and θ _3r (measured values) measured by the angle measuring sensors S ₁ , S ₂ , and S ₃ , but also the angle measuring sensors S ₁ , S ₂ , and S ₃ . Contains location information. Based on these measurement information, directional lines l _1p , l _2q , l _3r extending from the angle measurement sensors S ₁ , S ₂ , S ₃ to the objects M ₁ and M ₂ are assumed.

Next, the intersection determination unit 12 selects one azimuth angle θ ₁₁ from the azimuth angle θ _1p measured by the angle measurement sensor S ₁ (step S3). In this case, with the selection of the azimuth angle θ ₁₁ , the azimuth line l ₁₁ corresponding to the azimuth angle θ ₁₁ is selected. Next, the intersection determination unit 12 selects one azimuth angle θ ₂₁ from the azimuth angle θ _2q measured by the angle measurement sensor S ₂ (step S4). In this case, with the selection of the azimuth angle θ ₂₁ , the azimuth line l ₂₁ corresponding to the azimuth angle θ ₂₁ is selected. The azimuth angle θ ₂₁ (direction line l ₂₁ ) selected in step S4 is combined with the above-mentioned azimuth angle θ ₁₁ (direction line l ₁₁ ) to perform a determination process described later.

The intersection determination unit 12 determines whether the selected azimuth angle θ ₂₁ (θ _2q ) has already been used (step S5). Specifically, it is determined whether the selected azimuth θ ₂₁ is already combined with another azimuth. If it has already been used (step S5; Yes), a procedure of adding 1 to q is performed (step S6), and then the process is returned to step S4 to select the next azimuth angle θ ₂₂ from the azimuth angle θ _2q . If it is not used (step S5; No), the coordinates of the intersection P _pq (intersection point P ₁₂ in this example) of the two selected azimuth lines are calculated (step S7).

Here, in the present embodiment, the position information of the angle measuring sensors S ₁ and S ₂ and the two azimuth lines l ₁₁ and l ₂₁ extending from the angle measuring sensors S ₁ and S ₂ to the objects M ₁ and M ₂ . Since the azimuth angles θ ₁₁ and θ ₂₁ of are known, the coordinates (X ₁₂ , of the intersection P _pq (in this example, the intersection P ₁₂ ) are based on the linear function equation showing the two azimuth lines l ₁₁ and l ₂₁ . Y ₁₂ ) is calculated.

Next, the distance dr between all the azimuth lines l _3r of the angle measuring sensor S ₃ and the intersection point P _pq is calculated (step S8). In this case, the azimuth line l _3r can be expressed by the equation (1), where the coordinates of the angle measuring sensor S ₃ are (X ₃ , Y ₃ ) ^T.

Therefore, the distance dr between the azimuth line l _3r and the intersection P _pq can be expressed by the equation (2).

Based on this equation (2), the distances d ₁ and d ₂ between the azimuth lines l ₃₁ and l ₃₂ and the intersection P ₁₂ are calculated, and the shortest distance d _min is extracted from these distances d 1 and d 2 (step S9).

Next, the intersection determination unit 12 determines whether or not the three azimuth lines l ₁₁ , l ₂₁ , and l ₃₁ intersect. Since the azimuths measured by the angle measuring sensors S1, S2 _, and _S3 have measurement errors, it is difficult to intersect at _one point. Therefore, in the present embodiment, it is simply determined whether or not the three directional lines l ₁₁ , l ₂₁ , and l ₃₁ intersect. Specifically, as shown in FIG. 3, the error radius R _err of the circle 50 centered on the intersection P _pq (intersection P ₁₂ ) is calculated at the time of determination (step S10). _The error radius R _err is set based on the standard deviation σ of the angle measurement errors of the angle measurement sensors S1 and S2. For example, in the present embodiment, up to 3σ, which is _three times the standard deviation σ, is considered. It is the value that was set. This error radius R _err can be changed as appropriate. For example, when the angle measurement errors of the angle measurement sensors _S1 and S2 _are different, the standard of the angle measurement error of the angle measurement sensor having the larger angle measurement error. It is preferable to use the deviation σ. In this configuration, the error radius _Rerr can be set in consideration of the angle measurement error.

Next, the intersection determination unit 12 compares the magnitudes of the shortest distance d _min and the error radius R _err based on the equation (3).

That is, it is determined whether or not the azimuth line l ₃₁ passes through the circle 50 having the error radius R _err . In this determination, unless the distance d _min ≤ error radius R _err (step S11; No), the azimuth line l ₃₁ does not pass through the circle 50 of the error radius R _erra , so that the three azimuth lines l ₁₁ and l ₂₁ , L ₃₁ are determined not to intersect, and the process proceeds to step S6.

On the other hand, if the distance d _min ≤ error radius R _err (step S11; Yes), the deemed intersection P _pqr (in this example, the intersection P ₁₂₃ ) at which the three azimuth lines l ₁₁ , l ₂₁ , and l ₃₁ intersect is calculated. (Step S12). In this embodiment, the deemed intersection point _pqr is calculated by the method of least squares. Specifically, when three directional lines l ₁₁ , l ₂₁ , l ₃₁ (referred to as directional lines l ₁ , l ₂ , l ₃ for convenience) determined to intersect are obtained, each directional line is It can be expressed by the formula (4).

As shown in FIG. 3, although the azimuth lines l ₁₁ and l ₂₁ intersect, the nearest points of the three straight lines including the azimuth line l ₃₁ do not coincide with the intersection point P _pq (intersection point P ₁₂ ) and are calculated again. There is a need. Assuming that these nearest neighbor points are the intersections P _pqr (x _pqr , y _pqr ) ^T , the equation (5) holds.

Since this equation (5) is a so-called dominant decision system, the coordinates of the deemed intersection P _pqr (intersection P ₁₂₃ ) can be calculated by using the least squares method, and the object is at this deemed intersection P ₁₂₃ . It can be estimated that it exists. As a result, it can be regarded as a virtual intersection of three azimuth lines that do not surely intersect due to the influence of an error, and the position where the object exists can be estimated from the position of the deemed intersection P _pqr . Further, in this configuration, it is estimated that the object exists at the deemed intersection P _pqr where the three azimuth lines l ₁₁ , l ₂₁ , and l ₃₁ intersect, so even if the number of objects is unknown, the object is targeted. The position of the object M ₁ can be estimated accurately. In the equation (3), when the distance d _min is 0, the three azimuth lines l ₁₁ , l ₂₁ , l ₃₁ intersect at the intersection P _pq (intersection P ₁₂ ), and thus this intersection. P _pq may be regarded as the intersection P _pqr .

By the above calculation, the position of the object can be measured by using the angle measuring sensors S ₁ , S ₂ , and S ₃ . In particular, by calculating the coordinates of the deemed intersection P _pqr (intersection P ₁₂₃ ) by using the least squares method, it is possible to accurately estimate that an object exists at this deemed intersection P ₁₂₃ . On the other hand, the value of the above-mentioned deemed intersection P _pqr (intersection P ₁₂₃ ) is an instantaneous value affected by the angle measurement error of the angle measurement sensors S ₁ , S ₂ , S ₃ , and therefore, when evaluated in time series, There is a possibility that the behavior of the objects M _{1 and M 2 is significantly different from the actual behavior of the objects M 1 and M 2 , such} as a steep change in the position of the objects M 1 and M 2. Further, when the objects M ₁ and M ₂ are moving objects and it is necessary to estimate not only the position but also the motion parameters such as the velocity and acceleration thereof, the variation becomes larger due to the influence of the difference processing.

Therefore, in the present embodiment, the deemed intersection P _pqr is subjected to a smoothing process by state estimation (step S13). Specifically, the motion models of the objects M ₁ and M ₂ are introduced to estimate the state, and the influence of the error is suppressed. The motion model is shown by the equation of state of equation (6).

This equation means that the current positions of the objects M ₁ and M ₂ have a strong correlation with the immediately preceding positions, and the movements of the objects M ₁ and M ₂ are constrained by this equation. As a result, the influence of the angle measurement error can be suppressed, and the position can be estimated according to the actual movement (including stillness) of the object. Further, Ts and k represent the processing time and the sampling index when the flowchart of FIG. 2 is executed once, respectively.

The observation equation is expressed by Eq. (7).

The observed value in this case is the instantaneous value (x _pqr (k), y _pqr (k)) ^T of the position obtained in the above-mentioned step S12, and the Kalman filter and the particles are based on this observed value and the above-mentioned motion model. By applying a state estimation method such as a filter, it is possible to suppress the observation error of the measured azimuth and estimate the motion parameters including the positions of the objects M ₁ and M ₂ . Therefore, the positions of the objects M ₁ and M ₂ can be estimated more accurately. This state estimation can be applied not only when the objects M ₁ and M ₂ move but also when they are stationary.

Next, the control unit 16 determines whether or not the positions of all the objects have been estimated (step S14). Specifically, it is determined whether or not the positions of the objects M ₁ and M ₂ having the same number as the number of azimuth lines (2 lines) extending from the angle measuring sensors S ₁ , S ₂ and S ₃ are estimated. In this determination, when the positions of all the objects are not estimated (step S14; No), a procedure of adding 1 to p is performed (step S15), and then the process is returned to step S3 and the azimuth angle θ _1p . The next azimuth angle θ ₁₂ is selected from and the process is executed. On the other hand, when the positions of all the objects are estimated (step S14; Yes), the process ends.

As described above, according to the present embodiment, the deemed intersection P ₁₂₃ is calculated from the three azimuth lines l ₁₁ , l ₂₁ , l ₃₁ , and the deemed intersection P ₁₂₃ is estimated to be the position of the object M ₁ . Therefore, the position of the object M ₁ can be estimated accurately. Further, since the position of the object M1 is estimated by determining whether or not the three _{l 11} , l ₂₁ , and l ₃₁ intersect, it is necessary to input the true value of the number of objects in advance as in the conventional case. Even if the number of objects is unknown or changes, the position of the objects can be estimated.

[Second Embodiment]
In the first embodiment, as described above, the deemed intersection P 123 is calculated from the directional lines l ₁₁ , l ₂₁ , l ₃₁ extending from at least three angle measuring sensors S ₁ , S ₂ , and S ₃ , and the deemed intersection P ₁₂₃ is calculated. ₁₂₃ is estimated to be the position of the object _M1 . In this configuration, it is assumed that all (for example, three) angle measuring sensors S ₁ , S ₂ , and S ₃ always detect (observe) the object M ₁ , that is, the detection probability is 100%. However, in reality, not all angle measuring sensors S ₁ , S ₂ , and S ₃ can always detect all (for example, two) objects M ₁ and M ₂ , and the performance of the sensor processing system is affected. There is a possibility of losing sight of the objects M ₁ and M ₂ depending on the detection probability caused. In particular, when the objects M ₁ and M ₂ are located far from the angle measuring sensors S ₁ , S ₂ and S ₃ , the detection probability is relatively low, so that the angle measuring sensors S ₁ and S ₂ are relatively low. It is assumed that the objects M ₁ and M ₂ that can be detected by S ₃ are different from each other. Therefore, if the angle measuring sensor cannot always detect the object, the deemed intersection cannot be calculated accurately, and the accuracy of estimating the position of the object may be poor. Therefore, in the second embodiment, a configuration will be described in which the position of the object can be estimated accurately even when the angle measuring sensor cannot always detect the object.

FIG. 4 is a block diagram showing a schematic configuration of the position orientation system according to the second embodiment. FIG. 5 is a diagram for explaining an azimuth angle and a zenith angle that define a direction vector. The position determination system 100 estimates the position of an object such as a search target or a defect by using an angle measuring sensor. In FIG. 4, the positions of N objects (2 as an example) M ₁ and M ₂ are shown by using U (4 as an example) angle measuring sensors _{SO 1} , SO ₂ , SO ₃ and _{SO 4} as an example. The case of estimating is described. The number of angle measuring sensors is not limited as long as it is provided with at least three, and may be 3 <U. Further, the number of objects is not limited as long as there are a plurality of objects, and 2 <N objects may be used. In the second embodiment, as shown in FIG. 4, a predetermined one direction from the angle measuring sensor toward the object is defined as the Y direction, and a direction perpendicular to the Y direction in the horizontal direction is defined as the X direction. The direction orthogonal to the height direction in the Y direction is defined as the Z direction. The two objects M ₁ and M ₂ are objects existing in a three-dimensional space (XYZ space), and may be stationary or freely moving.

As shown in FIG. 4, the position setting system 100 includes four angle measuring sensors _SO1 to _SO4 and an arithmetic processing unit 30 that performs arithmetic processing based on the measured values of these angular measuring sensors _SO1 to _SO4 . .. The angle measuring sensors _SO1 to _SO4 are arranged independently of each other at predetermined intervals in a three-dimensional space, and are connected to the arithmetic processing unit 30 so as to be communicable by wire or wirelessly. _The angle measuring sensors _SO1 to _SO4 receive signals (sound waves, light waves, radio waves, etc.) from the objects M1 and _M2 , and determine the azimuth angle and the zenith angle that define the azimuth vector in the transmission direction of this signal. measure.

In FIG. 5, the direction vector v _mn extending from the m-th angle measuring sensor S _Om (m = 1, 2, ... U) toward the _n -th object Mn and the direction vector v _mn overlap with each other. The azimuth line l _mn is shown. In FIG. 5, for convenience of explanation, the angle measuring sensor _SOm is arranged at the origin O. In this case, the azimuth angle θ _mn is defined as the angle formed by the line _projected on the XY plane and the X-axis, as shown in FIG. Further, the zenith angle φ _mn is defined as the angle formed by the direction vector v _mn and the Z axis.

The angle measuring sensors _SO1 to _SO4 each have their own position information as known information, and the azimuth angle θ _mn and the zenith angle φ _mn (measured value) measured at the same time as this position information are calculated by the arithmetic processing unit 30. Sequentially sent to. In the present embodiment, the angle measuring sensors _SO1 to _SO4 are fixedly installed at predetermined positions. However, for example, if the configuration is such that the self-positioning information can be acquired based on the GPS signal, the measurement can be performed. The angle sensors _SO1 to _SO4 may be mounted on a moving body.

As shown in FIG. 4, the arithmetic processing unit 30 includes a data receiving unit 31, an intersection determination unit 32, a storage unit 33, an intersection calculation unit 34, a likelihood determination unit 35, a position estimation unit 36, a detection probability calculation unit 37, and a display. A unit 38 and a control unit 39 are provided.

The data receiving unit 31 receives the azimuth angle θ _mn and the zenith angle φ _mn sequentially transmitted from the angle measuring sensors _SO1 to _SO4 . The intersection determination unit 32 extends from all the angle measuring sensors _SO1 to _SO4 toward the object M1 ( _one object) based on the received information of the azimuth angle θ _mn and the zenith angle φ _mn . It is determined whether or not the azimuth lines l _mn (l ₁₁ , l ₂₁ , l ₃₁ , l ₄₁ ) of the above intersect. In the present embodiment, crossing means that not only all the directional lines _l11 , _l21 , _l31 , and _l41 intersect at one point, but also each directional line exists in a predetermined area (passes in the predetermined area). ) Including the case. Further, when all the azimuth lines l ₁₁ , l ₂₁ , l ₃₁ , and l ₄₁ do not exist in the predetermined area (do not pass through the predetermined area), the intersection determination unit 32 determines that they do not intersect. The intersection determination unit 32 also determines whether or not all the directional lines extending from all the angle measurement sensors _SO1 to _SO4 toward the object _M2 (one object) intersect. In FIG. 4, the description of the directional line extending from the angle measuring sensors _SO1 to _SO4 toward the object _M2 is omitted.

The storage unit 33 stores the crossed all-line intersection when all the azimuth lines l _mn (l ₁₁ , l ₂₁ , l ₃₁ , l ₄₁ ) intersect. Here, in the present embodiment, in view of the situation that all the angle measuring sensors _SO1 to _SO4 cannot always detect (observe) the objects M1 and _M2 , not _only the present but also the past retroactive from the present. Memorize multiple all-line intersections within a predetermined period.

The intersection calculation unit 34 is an azimuth line _l extending from a plurality of (for example, two) angle measuring sensors _SO1 and _SO2 arbitrarily selected from all the angle measuring sensors _SO1 to _SO4 to the object M1. Calculate the intersections where _mn (l ₁₁ , l ₂₁ ) intersect. The number of angle measuring sensors U'selected may be 2 or more and U or less. In this case, the intersection calculation unit 34 repeatedly calculates a plurality of intersections by the number of selected combinations (patterns) of the angle measuring sensors. For example, since there are 6 patterns in which two angle measuring sensors are arbitrarily selected from the four angle measuring sensors _SO1 to _SO4 , the intersection of the directional lines is calculated for these 6 patterns.

The likelihood determination unit 35 determines the likelihood of a plurality of calculated intersections based on the information of all-line intersections stored in the storage unit 33 by using a PHD (Probabilistic Hypothesis Density) filter. The PHD filter is a methodology proposed so that a state estimation technique such as a Kalman filter can be performed on a plurality of objects at the same time. It is adopted. In general, the larger the number of azimuth lines l _mn , the higher the probability (probability) that an object exists at the intersection (for example, all-line intersection) where these azimuth lines l _mn intersect, and the smaller the number of azimuth lines l _mn . , The imaginary image probability that these azimuth lines l _mn indicate the position of the imaginary image is increased. In the present embodiment, the likelihood determination unit 35 determines the likelihood of these intersections as a result by obtaining the virtual image probability of each intersection (observed value) calculated based on the information of the intersections of all lines. ..

The position estimation unit 36 extracts intersections based on the magnitude of the likelihood, and estimates these intersections as the positions of the objects M ₁ and M ₂ , respectively. Further, the position estimation unit 36 considers an intersection having a low likelihood, that is, a high probability of a virtual image, as a virtual image and excludes it from the candidates of the object.

The detection probability calculation unit 37 calculates the probability (detection probability) that the angle measuring sensors _SO1 to _SO4 detect either of the objects M1 and _M2 , _respectively . Specifically, each of the angle measuring sensors _SO1 to _SO4 attempts to measure an object (whether M1 or _M2 ) at _a predetermined measurement cycle. In this case, the detection probability is the number of times the object is detected for a predetermined number of measurements (for example, 10 times). The calculated detection probability is periodically output to the control unit 39.

The display unit 38 is, for example, a display device, and displays the estimated position information of the objects M ₁ and M ₂ . The control unit 39 controls the operation of the position determination system 100, and is the angle measurement sensors _SO1 to _SO4 , the data reception unit 31, the intersection determination unit 32, the storage unit 33, the intersection calculation unit 34, and the probability determination unit 35. , The position estimation unit 36 and the detection probability calculation unit 37 are controlled to execute an operation of estimating the positions of the objects M ₁ and M ₂ . Next, the operation procedure of the position positioning method will be described.

FIG. 6 is a flowchart showing a procedure in which the position determination system estimates the position of an object. Under the control of the control unit 39, the four angle measuring sensors _SO1 to _SO4 measure the azimuth angle θ _mn and the zenith angle φ _mn with respect to all the objects M ₁ and M ₂ (step S21). Then, using each of the measured azimuth angles θ _mn and each zenith angle φ _mn , the control unit 39 calculates the direction vector v _mn and the azimuth line l _mn (step S22).

In the present embodiment, for convenience of explanation, the objects M ₁ and M ₂ are set to two, but in the actual measurement operation, the true value N of the total number of objects is unknown. Here, the number of objects detected by the m-th angle measuring sensor _SOm is set to N _m (m = 1, 2, · · U). When the detection probability of each angle measuring sensor is 100%, the number of objects detected by each angle measuring sensor is the same, and N ₁ = ... = N _m = ... N _U = N is established. However, in the present embodiment, since it is assumed that the detection probability is less than 100%, this equation generally does not hold, and N _m ≤ N. Assuming that the azimuth angle and the zenith angle measured by the m-th angle measuring sensor _SOm are θ _m1 , ···, θ _mNm and φ _m1 , ···, φ _mN m, respectively, the direction vector v _mn is given by the equation (8). ).

Here, since the position of the m-th angle measuring sensor S _Om can be expressed as S _Om = [x _{Omy Om z Om ] T} ^, the m-th angle measuring sensor S _Om detects the nth object M _n . The corresponding azimuth line l _mn is represented by Eq. (9) using the parameter t.

Next, the intersection determination unit 32 has all azimuth lines l _mn (l) toward one object M _n (for example, object M ₁ ) from all U (for example, four) angle measuring sensors _SO1 to _SO4 . The closest point q of ( ₁₁ , l ₂₁ , l ₃₁ , l ₄₁ ) is calculated (step S23). Theoretically, in the real image (the position where the object actually exists), the azimuth lines l _mn intersect, so if there is a point where all the azimuth lines l _mn intersect (all-line intersection), the object exists at that point. Can be regarded. On the other hand, since measurement errors such as white noise actually occur, all the azimuth lines l _mn (l ₁₁ , l ₂₁ , l ₃₁ , l ₄₁ ) calculated from the measured values do not usually intersect. Therefore, the intersection determination unit 32 calculates the closest approach point q of the plurality of directional lines l _mn , and then determines whether or not each azimuth line l _mn intersects at the closest approach point q.

Assuming that the closest point q of a plurality of l _mns is q = [q _x q _y q _z ] ^T , this closest point q can be obtained by using a known technique such as the least squares method. For example, when the least squares method is used, the simultaneous equations shown in Eq. (10) are solved.

Here, v _m (m = 1, 2, · · U) is one of the direction vectors of the m-th angle measuring sensor _SOm . When unknown variables are put together in one vector, equation (10) can be written as equation (11).

In this simultaneous equation, the number of equations is 3U, the number of variables is 3 + U, and the number of equations is larger than the number of variables under the condition of U ≧ 2. By applying the least squares method to equation (11), the closest point q can be obtained. The closest approach obtained for all combinations of azimuth lines l _mn from all angle sensors S _Om (m = 1, 2, ... U) to all objects M _n (n = 1, 2, ... N _m ). Let the set of points q be q ₁ , q ₂ ... q _L.

Next, the intersection determination unit 32 determines whether or not the azimuth line l _mn intersects the closest approach point q (step S24). Specifically, as shown in FIG. 8, the center is q _l (l = 1, 2 ... L), and an error sphere (error region) 40 having a predetermined radius is assumed, and the inside of the error sphere 40 is assumed. It is determined whether or not all the azimuth lines l _mn (l ₁₁ , l ₂₁ , l ₃₁ , l ₄₁ ) extending toward one object M _n (M ₁ ) pass through. The radius of the error sphere 40 is set due to the measurement error of the angle measuring sensors _SO1 to _SO4 . In this determination, if all the directional lines l _mn (l ₁₁ , l ₂₁ , l ₃₁ , l ₄₁ ) pass through the inside of the error sphere 40, it is determined that the center thereof is the whole line intersection q _l . On the other hand, when all the azimuth lines l _mn (l ₁₁ , l ₂₁ , l ₃₁ , l ₄₁ ) do not pass through the inside of the error sphere 40, it is determined that this closest approach point q is a virtual image. The error sphere (error region) 40 does not necessarily have to have a spherical shape. For example, an error ellipsoid having a spheroid shape (error region) by utilizing the fact that the error of triangulation differs between the depth direction and the lateral direction. ) Can also be adopted. Let q ₁ , q ₂ ... q _L' be the set of all line intersections q _l selected by such a method (L'<L).

The intersection determination unit 32 calculates the whole line intersection q _l at predetermined time intervals based on the calculated information of the directional line l _mn . The storage unit 33 stores not only the current full-line intersection q _l but also a plurality of full-line intersection q _l within a predetermined period in the past.

Next, the intersection calculation unit 34 uses a plurality of angle measurement sensors S _Om (m = 2, ... U', U'<U) selected from all the angle measurement sensors S _Om . Intersection (observed value) z where the azimuth lines l _mn (for example, l ₁₁ , l ₂₁ ) extending from the angle measuring sensor _SO m (for example, _{SO 1} , SO ₂ ) to one object M _n (for example, M ₁ ) intersect. Is calculated (step S25). As described above, if there is an all-line intersection q _l where all the azimuth lines l _mn intersect, it can be considered that the object M _n exists at this all-line intersection q _l . Therefore, the all-line intersection q _l can be used as it is as an observation value. If possible, the position of the object _Mn can be easily estimated.

However, when all the angle measuring sensors _SOm cannot always detect all the objects _Mn , that is, when the detection probability of the angle measuring sensor _SOm is low (less than 100%), the whole line intersection q _l can be obtained in the first place. There is a problem that the probability is low. This problem becomes more remarkable as the number of angle measuring sensors _SOm increases. Therefore, it is not realistic to use only the full-line intersection q _l as the observed value, and although there is a possibility that many virtual images are included, the intersection (observed value) z is determined by the selected quantity of angle measuring sensors _SOm . Need to calculate. In this case, the intersection calculation unit 34 repeatedly calculates a plurality of intersections z by the number of selected combinations (patterns) of the angle measuring sensors.

Next, the arithmetic processing using the PHD filter is executed (step S26). In this embodiment, a PHD filter is used to estimate motion parameters such as acceleration, velocity, and position. Therefore, we introduce a state-space representation that is a set of equations of state and observations. This state-space representation is represented by Eqs. (12) and (13).

In the equations (12) and (13), _TS represents a measurement cycle (sampling cycle). In this example, _TS is set as a fixed value for simplification, but the measurement cycle may be varied. Further, u (k) and w (k) represent drive noise and observation noise, respectively.

Finally, the time is updated from k to k + 1 (step S27), the process is returned to step S21 again, and steps S21 to S27 are repeatedly executed.

Next, the arithmetic processing using the PHD filter (step S26) will be described. FIG. 7 is a flowchart showing a procedure of arithmetic processing using a PHD filter. In the present embodiment, the PHD filter calculates the virtual image probabilities of a plurality of intersections z as state candidates based on the information of the full-line intersection q _l stored in the storage unit 33, and the likelihood is based on the virtual image probabilities. Seeking. Here, the likelihood is an extensive representation of the likelihood that the calculated intersection Z is the object _Mn , and varies depending on the virtual image probability of the intersection z.

The likelihood determination unit 35 generates a state candidate newly generated by time update (step S31). In this case, the object _Mn entering and exiting the measurable area of the angle measuring sensor _SOm is taken into consideration. Next, a new state candidate that correlates with the state one time before (one measurement cycle before) is generated (step S32). In this case, for example, a state in which the object _Mn is split is considered. If no new state candidate is generated in these steps S31 and S32, the process proceeds to step S33 as it is.

Next, the likelihood determination unit 35 transitions the state candidate one time before (one measurement cycle before) (step S33). In this case, the state is changed using the equation of state of the above equation (12). Here, since there are a plurality of state candidates in the PHD filter, state transitions are performed for all of these state candidates.

Next, the likelihood determination unit 35 attenuates the weight of the state candidate according to the detection probability of the angle measuring sensor _SOm (step S34). Specifically, the weighting is increased for the state candidate of the object _Mn having a high detection probability, and conversely, the weighting is decreased for the state candidate of the object _Mn having a low detection probability. In this case, for example, the state candidate is kept alive for a predetermined time even for the object _Mn that cannot be detected by the angle measuring sensor _SOm .

Next, the likelihood determination unit 35 calculates the estimated value y (k) of the intersection (observed value) z and the Kalman gain using the observation equation of the equation (13) (step S35). Then, the likelihood determination unit 35 uses the Kalman gain K (k) ∈ R ^{9 × 3} and the intersection z (k) existing for all the state candidates, so that the state is shown in the equation (14). (Step S36). In this case, it is possible to rediscover the object _Mn that cannot be detected once.

With respect to the jth candidate in a state associated with such an intersection (observed value) z (k), the PHD filter calculates the likelihood ^wj as in the equation (15) and updates it each time (step S37).

In this equation (15), κ (z (k), Q (k)) is the imaginary probability of the intersection (observed value) z (k), and Q (k) is a predetermined value in the past that goes back from the present time k. It is a set of all line intersections q _l of time (time window width from a few steps before the past to the present time k; a predetermined period). As shown in this equation (15), the likelihood w ^j is set so that the higher the virtual image probability is, the smaller the likelihood is. Therefore, the likelihood w ^j can be obtained based on the calculated magnitude of the virtual image probability.

Next, the likelihood determination unit 35 rejects the state candidates having a low likelihood ^wj or aggregates similar state candidates (step S38). This makes it possible to estimate the number and status of targets to some extent.

Finally, the position estimation unit 36 extracts one from a plurality of intersections (observed values) z based on the magnitude of the likelihood w ^j , and uses the extracted intersection z as the position of the object _Mn . Estimate (step S39). The position estimation unit 36 extracts the state candidate having the highest likelihood w ^j and estimates the state (motion parameter such as position) of a plurality of objects M _n . According to this configuration, the likelihood w ^j of the intersection z is determined based on the current or past all-line intersection q _l , and the position of the intersection z is estimated to be the position of the object M _n from the magnitude of this likelihood w ^j . Therefore, even if the angle measuring sensor _SOm cannot always detect the object, the position of the object _Mn can be estimated accurately.

According to the present embodiment, the likelihood w ^j of the intersection (observed value) z is determined based on the information of the current or past all-line intersection q _l , and the intersection (observed value) z is determined from the magnitude of the likelihood w ^j . Since the position of the object _Mn is estimated as the position of the object Mn, the position of the object _Mn can be estimated accurately even if the angle measuring sensor _SOm cannot always detect the object _Mn .

[Third Embodiment]
In this third embodiment, the virtual image probability κ (z (k), Q (k)) in the equation (15) of the second embodiment described above is set to the intersection (observed value) z and the full-line intersection Q (k). Calculated based on the distance. FIG. 9 is a diagram showing an example of the arrangement relationship between the intersection of all lines and the intersection of each azimuth line. Since the position of the all-line intersection q _l is considered to be the position of the object, it is considered that the closer the intersection z is to the all-line intersection q _l , the lower the virtual image probability and the higher the likelihood. Therefore, in the present embodiment, the distance d between the intersection z obtained by a small number of selected angle measuring sensors and the full-line intersection q _l is measured. Since the set Q (k) of all line intersections q _l consists of L'elements, the virtual image probability κ (z (k), Q (k)) can be defined by the equation (16).

That is, in the present embodiment, the virtual image probability is defined by the distance from the intersection z closest to the full-line intersection q _l . However, since the value of the virtual image probability changes greatly depending on the absolute value as it is in the equation (16), it is preferable to normalize it so that the total sum of the virtual image probabilities is 1 by the equation (17).

In the present embodiment, since the virtual image probability is defined by the distance d from the intersection z closest to the all-line intersection q _l , the likelihood determination unit 35 determines the likelihood w of the intersection z having the shortest distance d from the all-line intersection q _l . ^{Since j} is determined to be the highest, even if the detection probability of the angle measuring sensor is low, the virtual image probability can be appropriately calculated, and the virtual image can be excluded and the position of the object M _n can be estimated accurately.

[Fourth Embodiment]
The fourth embodiment is an effective method when it is known that a plurality of objects _Mn are close to each other within a predetermined range. Normally, when a plurality of objects _Mn are close to each other within a predetermined range, it is difficult to separate and detect each object _Mn because it is affected by an observation error. On the other hand, when it is known in advance by another sensor or external information that a plurality of objects _Mn are close to each other within a predetermined range, that information can be used for estimating the position state. can. Specifically, as shown in the equation (18), the likelihood w ^j of one state candidate (intersection point) is set to the number n (x) of other state candidates (intersection points) existing within a predetermined range of the state candidate. Make corrections that increase according to ^j _T ). In this equation (18), G is a correction value, and G> 1.

According to this configuration, the larger the number n (x ^j _T ) of other state candidates existing in the vicinity of one state candidate (intersection), the higher the likelihood w ^j of one state candidate is corrected upward. Therefore, even if a plurality of objects M _n are present in close proximity to each other, the position of each object M _n can be estimated with high accuracy.

[Fifth Embodiment]
In the fifth embodiment, in step S25 of the second embodiment, the number of angle measurement sensors _SOm selected for calculating the intersection (observed value) z is changed according to the detection probability. FIG. 10 is a graph showing an example of the relationship between the detection probability and the number of angle measuring sensors selected.

In the second embodiment described above, assuming that the detection probability of the angle measuring sensor S _Om is not always 100%, the intersection z of the azimuth line l _mn is obtained using a small number of selected angle measuring sensors S _Om . ing. However, the detection probability varies depending on, for example, the distance from the object _Mn . In general, when the detection probability is high, increasing the number of angle measuring sensors _SOm used increases the number of directional lines and eliminates a virtual image, so that the position of the object _Mn can be estimated accurately. Further, when a small number of angle measuring sensors _SOm are used even though the detection probability is high, the intersection (observed value) z may become enormous and the calculation load may increase.

Therefore, in the present embodiment, the intersection calculation unit 34 increases the number of the angle measuring sensors _SOm selected as the detection probability that the angle measuring sensor _SOm detects the object _Mn increases. In the example of FIG. 10, when the detection probability is 50%, half the number (U / 2) of the angle measuring sensors S _Om is selected, and when the detection probability is 100%, all the angle measuring sensors S _Om are selected. You have selected. Here, as the detection probability for determining the number of the angle measuring sensor S _Om , for example, the detection probability of the lowest angle measuring sensor S _Om among all the detection probabilities of the angle measuring sensor S _Om is adopted. Further, the median value or the average value of the detection probabilities of all the angle measuring sensors _SO may be used.

According to the present embodiment, the number of angle measuring sensors S _Om used changes according to the detection probability, and as the detection probability increases, the number of angle measuring sensors S _Om increases, so that the detection probability is low. In this case, the intersection (number of observations) z can be increased, while when the detection probability is high, the intersection (number of observations) z with high likelihood can be obtained while avoiding the increase in the intersection (number of observations) z. As a result, the position of the object _Mn can be estimated accurately regardless of the detection probability.

[Sixth Embodiment]
In the sixth embodiment, the time window width TM (predetermined period) for holding the information of the whole line intersection q _l in the second embodiment retroactively is changed according to the state of the object _Mn . FIG. 11 is a graph showing the relationship between the time window width, the relative speed of the angle measurement sensor with respect to the angle measurement sensor, and the product of the measurement cycle of the angle measurement sensor. As described above, the set Q (k) of all line intersections q _l includes not only the current time k but also all line intersections q _l at past times k-1, k-2, .... This is a measure for securing a certain number of all-line intersections q _l even when the detection probability is low (less than 100%). Here, when the object M _n is moving relative to the angle measuring sensor _SOm , the past all-line intersection q _l does not correspond to the position of the object M _n .

In the present embodiment, as the product of the relative velocity between the object _Mn and the angle measuring sensor _SOm and the measurement cycle (sampling cycle) of the angle measuring sensor _SOm increases, the information of the whole line intersection q _l is obtained in the past. The time window width TM to be held retroactively is shortened. In this case, if the maximum speed of the object _Mn can be estimated in advance by another sensor or external information, the time window width TM is determined using that information. According to this configuration, the time window width TM for holding the information of the full-line intersection q _l can be appropriately adjusted according to the relative moving speed of the object M _n , so that the object M _n moves even if the object M n moves. The position of _Mn can be estimated accurately.

Although one embodiment of the present invention has been described, the present embodiment is presented as an example and is not intended to limit the scope of the invention. This embodiment can be implemented in various other forms, and various omissions, replacements, and changes can be made without departing from the gist of the invention. The present embodiment and its modifications are included in the scope and gist of the invention as well as the invention described in the claims and the equivalent scope thereof. In the first embodiment described above, whether or not the distance between the intersection P ₁₂ of the two arbitrarily selected directional lines l ₁₁ and l ₂₁ and the remaining directional lines l ₃₁ is smaller than the predetermined error radius R _err . It is determined whether or not the three azimuth lines l ₁₁ , l _{21, and} l ₃₁ intersect each other. Therefore, it is preferable to calculate the distances for the other two sets of directional lines and intersections, determine the intersection, and estimate the position of the combination of the directional lines and the intersections showing the shortest distance. In this configuration, since the intersections of the three azimuth lines l ₁₁ , l _{21, and} l ₃₁ can be calculated more accurately, the positions of the objects M ₁ and M ₂ can be estimated more accurately.

1,100 Positioning system 10,30 Arithmetic processing unit 12,32 Crossing determination unit 13,36 Position estimation unit 14 Correction unit 16,39 Control unit 33 Storage unit 34 Intersection point calculation unit 35 Probability determination unit P ₁₂₃ , P _pqr deemed Intersection points P ₁₂ , P _pq Intersection points R _erra Error radius d _min Distance M ₁ , M ₂ , M _n Objects S ₁ , S ₂ , S ₃ , _{SO 1} , SO ₂ , SO ₃ , _{SO 4} , _SO m Angle sensor

Claims (12)

At least three angle measuring sensors, which are arranged independently of each other and measure the azimuth and zenith angle that define the direction vector of the azimuth line for a plurality of objects, respectively.
A storage unit that stores all line intersections where all the azimuth lines extending from all the angle measuring sensors to one object, respectively, within a predetermined period of the present or the past.
An intersection calculation unit that calculates a plurality of intersections at which the azimuth lines extending from a plurality of arbitrarily selected angle measuring sensors to the object intersect.
A likelihood determination unit that determines the likelihood of each of a plurality of the intersections based on the whole line intersection,
A position estimation unit that extracts one intersection based on the magnitude of the likelihood and estimates the intersection as the position of one object.
A positioning system characterized by being equipped with. The position determination system according to claim 1, wherein the likelihood determination unit determines the highest likelihood of the intersection having the shortest distance to the all-line intersection. If it is known in advance that multiple objects are in close proximity,
The position according to claim 1 or 2, wherein the likelihood determination unit makes a correction for increasing the likelihood of one intersection according to the number of other intersections existing within a predetermined range of the intersection. Positioning system. One of claims 1 to 3, wherein the intersection calculation unit increases the number of the angle measuring sensors selected as the detection probability that the angle measuring sensor detects the object increases. Positioning system described in. Claims 1 to 4 are characterized in that the storage unit shortens the predetermined period as the product of the relative speed between the object and the angle measuring sensor and the measuring cycle of the angle measuring sensor increases. The position determination system according to any one of the above. The closest points of all the directional lines extending from all the angle measuring sensors to one of the objects are calculated, and all the directional lines pass within a predetermined error region centered on the closest points. The position determination system according to any one of claims 1 to 5, further comprising an intersection determination unit for determining the closest approach point as the all-line intersection. It is a position positioning method that estimates the position of an object using a position setting system equipped with at least three angle measuring sensors and an arithmetic processing unit.
An angle measurement step in which the arithmetic processing unit measures the azimuth angle and the zenith angle that define the direction vector of the azimuth line with respect to a plurality of objects by using the angle measurement sensor.
An all-line intersection storage step that calculates and stores all-line intersections where all the azimuth lines extending from all the angle-finding sensors to one object, respectively, within a predetermined period of the present or the past.
An intersection calculation step for calculating a plurality of intersections at which the azimuth lines extending from a plurality of arbitrarily selected angle measuring sensors to the object intersect.
A likelihood determination step for determining the likelihood of each of the plurality of intersections based on the whole line intersection, and
A position estimation step of extracting one intersection based on the magnitude of the likelihood and estimating the intersection as the position of one of the objects.
A positioning method characterized by performing . The position determination method according to claim 7, wherein the arithmetic processing unit determines in the likelihood determination step the likelihood of the intersection having the shortest distance to the all-line intersection. If it is known in advance that multiple objects are in close proximity,
7. The arithmetic processing unit is characterized in that, in the likelihood determination step, the likelihood of one intersection is corrected according to the number of other intersections existing in a predetermined range of the intersection. Or the position determination method according to 8. The calculation processing unit is characterized in that, in the intersection calculation step, the number of the angle measurement sensors selected is increased as the detection probability that the angle measurement sensor detects the object increases . The position determination method according to any one of 9. The arithmetic processing unit sets the predetermined period shorter as the product of the relative speed between the object and the angle measuring sensor and the measurement cycle of the angle measuring sensor increases in the all-line intersection storage step. The position determination method according to any one of claims 7 to 10, characterized in that. The arithmetic processing unit calculates the closest points of all the azimuth lines extending from all the angle measuring sensors to one of the objects, and within a predetermined error region centered on the closest points. The position determination method according to any one of claims 7 to 11, further comprising an intersection determination step of determining the closest approach point as the all-line intersection when all the directional lines pass.

Images