JP2016017879A - Positioning device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a positioning device for highly accurately determining a position of a moving object in response to relative positions of the moving object and a target object relative to each other.SOLUTION: A positioning device detects relative positions of a moving object and a plurality of target objects on the basis of reflection waves reflected by the target objects after irradiating the target object with a survey wave, and acquires positional information on the respective target objects. The positioning device sets a dispersion of error in the relative position of each target object in response to measurement values corresponding to the relative positions of the moving object and the target object (S420); and sets a weight of a positioning parameter set on the basis of the measurement values corresponding to the relative positions among a plurality of positioning parameters, on the basis of the dispersion of error in the relative position if the positioning parameters set on the basis of the positional information and the relative positions of the target objects are weighted (S422). The positioning device determines a position of the moving object on the basis of the positioning parameter weighted by weighting means (S424).SELECTED DRAWING: Figure 5

Description

  The present invention relates to a technique that is mounted on a moving object and measures the position of the moving object based on the relative positions of the moving object and a plurality of target objects.

  As a positioning device that measures the position of a moving object, a technique for positioning the position of a moving object based on the position information of the positioning satellite and the distance between the moving object and the positioning satellite using GPS is known (for example, patents). Reference 1).

  In Patent Document 1, the value of a weighting matrix component used when obtaining the position of a moving object by the least square method is based on the standard deviation of the distance error calculated for each positioning satellite while the moving object is stopped. To set. When the moving object starts moving from the stopped state, the weighting matrix set in the stopped state is used for positioning the position of the moving object.

JP 2008-209227 A

  When the position of the moving object is measured based on the position information of the positioning satellite and the distance between the moving object and the positioning satellite as in Patent Document 1, the positioning satellite and the moving object are stopped when the moving object is stopped and moving. The amount of change with respect to distance is very small. Therefore, positioning accuracy is ensured even if the position of the moving object is measured using the weighting matrix set based on the standard deviation of the distance error calculated for each positioning satellite while the moving object is stopped. You might be able to do that.

  Since the technique of positioning the position of a moving object using a positioning satellite as in Patent Document 1 is not effective in an environment where a positioning satellite such as a tunnel, an underground parking lot, or an urban area cannot be used, the moving object and the ground It is conceivable that the relative position with the target object is measured by irradiating the target object with the exploration wave, and the position of the moving object is measured based on the position information of the target object and the relative position between the moving object and the target object.

Here, the relative position between the moving object and the target object represents the distance between the moving object and the target object, and the other direction relative to one of the moving object and the target object.
When the relative position between the moving object and the target object is measured by the exploration wave, the relative position between the moving object and the target object changes according to the movement of the moving object. The standard deviation of the relative position error varies depending on the measurement value for measuring the relative position, which corresponds to the relative position between the moving object and the target object.

  Therefore, as in Patent Document 1, a technique for positioning the position of a moving object using a weighting matrix set in a state where the moving object is stopped is used, and the relative position between the moving object and the target object is determined as a moving object. It is difficult to measure the position of a moving object with high accuracy even if it is applied to positioning performed in an environment that changes according to the movement of the moving object.

  The present invention has been made to solve the above-described problems, and an object of the present invention is to provide a positioning device that accurately measures the position of a moving object according to the relative position between the moving object and the target object.

The positioning device of the present invention is a positioning device that is mounted on a moving object and measures the position of the moving object, and includes a distance measuring means, an error setting means, a weighting means, and a positioning means.
The ranging means measures the relative position between the moving object and the plurality of target objects based on the reflected waves reflected from the plurality of target objects by irradiating the exploration wave, and the error setting means is set to the relative position measured by the ranging means. The variance of each relative position error is set according to the corresponding measurement value.

  The weighting means sets the weight of the positioning parameter set based on the measurement value corresponding to the relative position among the plurality of positioning parameters based on the variance set by the error setting means. The positioning means measures the position of the moving object based on the positioning parameter weighted by the weighting means.

  According to this configuration, the positioning parameter set based on the measurement value corresponding to the relative position among the weighted positioning parameters is set according to the measurement value corresponding to the relative position between the moving object and the target object. Weight based on variance. Therefore, the position of the moving object can be measured with high accuracy by using the positioning parameter weighted according to the relative position between the moving object and the target object.

  In addition, the code | symbol in the parenthesis described in the claim shows the correspondence with the specific means as described in embodiment mentioned later as one aspect, Comprising: The technical scope of this invention is limited is not.

The block diagram which shows the positioning system to which the positioning apparatus by 1st Embodiment is applied. The schematic diagram which shows the positional relationship of a moving object and a target object. The flowchart which shows the main routine of a positioning process. The flowchart which shows a ranging process. The flowchart which shows a positioning process. The schematic diagram explaining dispersion | distribution of the error of distance. The block diagram which shows the positioning apparatus by 2nd Embodiment. The schematic diagram explaining the positioning which considered the height of the target object. The block diagram which shows the positioning apparatus by 3rd Embodiment. The schematic diagram explaining the positioning which considered the attitude | position of the vehicle. The flowchart which shows the positioning process which considered the attitude | position of the vehicle. The block diagram which shows the positioning apparatus by 4th Embodiment. The schematic diagram which shows the positional relationship of the moving object and target object by movement of a vehicle. Explanatory drawing explaining the positioning which considered the moving distance of the vehicle. The schematic diagram explaining the positioning based on the reliability by 5th Embodiment. The block diagram which shows the positioning apparatus by 6th Embodiment. The schematic diagram which shows the positional relationship of a moving object and a target object.

Embodiments to which the present invention is applied will be described below with reference to the drawings.
[1. First Embodiment]
[1-1. Constitution]
A positioning system 2 shown in FIG. 1 includes a positioning device 10 and a communication device 30. As shown in FIG. 2, the positioning device 10 is mounted on a vehicle 100. The communication device 30 as the target object is installed on the road 110 side or on a building.

The positioning device 10 includes a light emitting unit 12, a light receiving unit 14, a position acquisition unit 16, a distance measuring unit 18, and a positioning processing unit 20.
The light emitting unit 12 periodically emits laser light as an exploration wave for each preset light emission period within a predetermined angle range centered on the front of the vehicle 100. The light receiving unit 14 receives reflected light that is a reflected wave with respect to the laser light emitted from the light emitting unit 12 and information light transmitted from the communication device 30 through optical wireless communication, and outputs a light reception signal indicating the intensity of the received light. To do. The information light transmitted from the communication device 30 includes a position signal that represents the position of the communication device 30 in latitude and longitude.

  The position acquisition unit 16 acquires the position of the communication device 30 from the position signal included in the light reception signal when the light reception signal output from the light reception unit 14 includes a position signal representing the position as the position information of the communication device 30. .

  When the light receiving signal output from the light receiving unit 14 does not include a position signal indicating the position of the communication device 30, the distance measuring unit 18 reflects the light received by the light receiving unit 14 from the laser light emitted from the light emitting unit 12. It is judged that.

Then, the distance measuring unit 18 calculates the time difference between the latest reflected light received before the light receiving unit 14 receives the information light including the position signal and the laser light emitted from the light emitting unit 12 corresponding to the reflected light. Based on this time difference, distances d ₁ and d ₂ between the vehicle 100 and the two communication devices 30 are measured.

The elapsed time from when the light emitting unit 12 emits laser light to when the light receiving unit 14 receives reflected light corresponds to the distances d ₁ and d ₂ between the vehicle 100 and the two communication devices 30. , Measured values for measuring the distances d ₁ and d ₂ .

In addition to the time, the phase difference between the laser light emitted by the light emitting unit 12 and the reflected light received by the light receiving unit 14 or the difference in intensity between the laser light emitted by the light emitting unit 12 and the reflected light received by the light receiving unit 14 The distances d ₁ and d ₂ may be measured using a certain light intensity difference as a measurement value.

Further, the distance measuring unit 18 measures the directions of the two communication devices 30 with respect to the vehicle 100 based on the direction of the reflected light of the laser light received by the light receiving unit 14.
For example, the distance measuring unit 18 calculates the elapsed time from when the light emitting unit 12 emits laser light and starts searching for the predetermined angular range described above at a predetermined rotational angular velocity until the light receiving unit 14 receives the reflected light. taking measurement. Since the direction of the reflected light received by the light receiving unit 14 is known based on the elapsed time, the distance measuring unit 18 measures the directions of the two communication devices 30 based on the vehicle 100 based on the direction of the reflected light. .

  The elapsed time from when the light emitting unit 12 emits laser light and the search is started at a predetermined rotational angular speed until the light receiving unit 14 receives the reflected light is the direction of the two communication devices 30 based on the vehicle 100. It is a measured value for measuring.

Here, the distance between the vehicle 100 and each communication device 30 and the direction of each communication device 30 with respect to the vehicle 100 represent the relative positions of the vehicle 100 and each communication device 30.
The positioning processing unit 20 calculates an angle θ formed by the two communication devices 30 with respect to the vehicle 100 based on the directions of the two communication devices 30 based on the vehicle 100 measured by the distance measuring unit 18. Furthermore, the distance d ₁₂ between the two communication devices 30 is calculated based on the positions of the two communication devices 30 acquired by the positioning processing unit 20 and the position acquisition unit 16.

The positioning processing unit 20 includes positions of the two communication devices 30, distances d ₁ and d ₂ between the vehicle 100 and the two communication devices 30, and an angle formed by the two communication devices 30 with respect to the vehicle 100. The position of the vehicle 100 is measured based on θ and the distance d ₁₂ between the two communication devices 30. Details of the positioning process will be described later.

The communication device 30 includes a light receiving unit 32, a distance measuring light detection unit 34, a transmission control unit 36, a communication information generation unit 38, and a light emitting unit 40.
The light receiving unit 32 receives the laser light emitted from the positioning device 10 and outputs a light reception signal corresponding to the intensity of the received laser light.

  When the level of the light reception signal output from the light receiving unit 32 becomes equal to or higher than the reference intensity, the distance measuring light detection unit 34 outputs a laser light detection signal indicating that the communication device 30 has received the laser light from the positioning device 10. Here, the reference intensity is an intensity set in advance as the lowest light receiving level of the laser light received by the communication device 30 from the positioning device 10.

  The transmission control unit 36 outputs a trigger signal to the communication information generation unit 38 when the ranging light detection unit 34 outputs a laser light detection signal. When the communication information generation unit 38 receives the trigger signal from the transmission control unit 36, the communication information generation unit 38 generates communication information including a position signal representing the latitude and longitude of the communication device 30 stored in advance, and the light emission unit 40 generates information light such as infrared rays. Send it.

[1-2. processing]
Next, processing executed by the positioning device 10 will be described based on the flowcharts of FIGS. 3 to 5, “S” represents a step.

(Main process)
The flowchart of FIG. 3 is executed at predetermined time intervals. In S400, the distance measuring unit 18 performs a distance measuring process based on the laser light emitted from the light emitting unit 12 and the reflected light reflected from the communication device 30 with respect to the laser light. In the distance measurement process, the distance between the vehicle 100 and the communication device 30 and the direction of the communication device 30 with respect to the vehicle 100 are measured. Details of the distance measurement processing in S400 will be described later based on the flowchart of FIG.

In S <b> 402, the position acquisition unit 16 acquires the position of the communication device 30 as an absolute position represented by the latitude and longitude from the position signal included in the information light transmitted from the communication device 30.
In S404, the positioning processing unit 20 measures the position of the vehicle 100 based on the execution result of the ranging process in S400 and the position acquired in S402. Details of the positioning process in S404 will be described later with reference to FIG.

(Ranging process)
When the light emitting unit 12 emits the laser light in S410 of FIG. 4, the distance measuring unit 18 starts measuring the elapsed time after the light emission (S412).

When the light receiving unit 14 receives the reflected light of the laser beam emitted from the light emitting unit 12 (S414: Yes), the distance measuring unit 18 finishes measuring the elapsed time (S416). The distance measuring unit 18 determines the distances d ₁ between the vehicle 100 and the two communication devices 30 based on the elapsed time from when the light emitting unit 12 emits laser light until the light receiving unit 14 receives reflected light. measures _{d 2} (S418).

Further, the distance measuring unit 18 measures the direction of the communication device 30 with respect to the vehicle 100 based on the reflected light (S418).
In the present embodiment, as described above, the distance measuring unit 18 determines the time difference between the latest reflected light received before receiving the information light including the position signal and the laser light emitted corresponding to the reflected light. Based on this, the distance between the vehicle 100 and the communication device 30 is measured.

In step S418, the distance measurement unit 18 measures the distances d ₁ and d ₂ and the direction of the communication device 30 a plurality of times with a laser beam, and stores the measurement result in a RAM or the like.
(Positioning process)
In S420 of FIG. 5, the positioning processing unit 20 acquires the directions of the two communication devices 30 measured a plurality of times with the vehicle 100 as a reference from the distance measuring unit 18, and 2 for the vehicle 100 from the acquired direction of the communication device 30. A plurality of angles θ formed by each communication device 30 are calculated.

Furthermore, the positioning processor 20 acquires the position of the two communication devices 30 from the position acquiring unit 16 calculates the distance d ₁₂ between the communication device 30 from the position of the two communication devices 30 acquired.
Further, the positioning processing unit 20 acquires the result of measuring the distance between the vehicle 100 and the two communication devices 30 a plurality of times from the distance measuring unit 18, and calculates the average value between the vehicle 100 and the two communication devices 30. Calculated as distances d ₁ and d ₂ .

The positioning processing unit 20 then calculates the error variance σ _di ² (i = 1, 2) of the distances d ₁ and d ₂ between the vehicle 100 and the two communication devices 30 and the distance between the two communication devices 30. The error variance σ ₁₂ ² of d _{12 and} the error variance σ _θ ² of the angle θ formed by the two communication devices 30 with respect to the vehicle 100 are calculated (S420).

Here, since the variance of the error of the angle θ varies depending on the relative position between the vehicle 100 and the two communication devices 30, it is possible to calculate the variance of the error of the angle θ including the variation of the variance of the error. desirable. However, the variation in the error variance of the angle θ is smaller than the variation in the error variance of the distances d ₁ and d ₂ described later. Therefore, in the present embodiment, the error variance σ _θ ² of the angle θ is calculated from the plurality of angles θ calculated in S420 without including the variation of the error variance of the angle θ.

Since the distance d ₁₂ between the two communication devices 30 is determined by the positions of the two communication devices 30, the error variance σ ₁₂ ² of the distance d ₁₂ is the relative position between the vehicle 100 and the two communication devices 30. Regardless.

On the other hand, the error variance σ _di ² of the distances d ₁ and d ₂ varies according to the distance that is the relative position between the vehicle 100 and the two communication devices 30. The reason will be described below.
As shown in FIG. 6A, the laser beam 300 spreads in the x-axis direction and the y-axis direction with the minus direction of the z-axis as the light emission direction. In FIG. 6A, the spread angle of the laser beam 300 with respect to the x-axis direction is represented by θ _x , and the spread angle with respect to the y-axis direction is represented by θ _y . In FIG. 6A, the spread of the laser beam 300 is represented by a rectangle on the xy plane. The range in which the laser beam 300 spreads increases as the distance between the vehicle 100 and the communication device 30 increases.

First, the error variance of the distance in the x-axis direction and the y-axis direction on the xy plane will be described. 6A and 6B, regarding the error variance in the y-axis direction, the intersection point between the x-axis and the y-axis is the origin, and the error in the y-axis direction between the distance measurement point 302 and the origin is the y-axis direction. to a lower limit -dsin and (θ _y / 2) to be uniformly distributed in the range between the upper limit dsin (θ _y / 2). Then, the probability density function f (y) is expressed by Equation (1), where d is the distance measured at the upper and lower limit values.

そして、一様分布の確率密度関数ｆ（ｙ）の誤差分散σ_ｙ１ ^２は、式（２）で表わされる。

Then, the error variance σ _y1 ² of the probability density function f (y) with uniform distribution is expressed by Expression (2).

また、距離の測定点３０２と原点とのｘ軸方向の誤差がｘ軸方向の下限値である−ｄｓｉｎ（θ_ｘ／２）と上限値であるｄｓｉｎ（θ_ｘ／２）との範囲で一様に分布しているとすると、ｘ軸方向の誤差分散σ_ｘ１ ^２もｙ軸方向の誤差分散と同様にして式（３）で表わされる。

Further, the error in the x-axis direction between the distance measurement point 302 and the origin is one in the range of −dsin (θ _x / 2) which is the lower limit value in the x-axis direction and dsin (θ _x / 2) which is the upper limit value. Assuming that they are distributed in the same manner, the error variance σ _x1 ^{2 in} the x-axis direction is also expressed by equation (3) in the same manner as the error variance in the y-axis direction.

次に、図６の（Ｃ）に示すように、レーザ光３００の球面に対し、ｘｙ平面までの距離に誤差が生じることを考慮するときに、ｚ軸方向についてｘ軸方向成分およびｙ軸方向成分の距離の誤差分散について説明する。

Next, as shown in FIG. 6C, when considering that an error occurs in the distance to the xy plane with respect to the spherical surface of the laser beam 300, the x-axis direction component and the y-axis direction in the z-axis direction. The error variance of the component distance will be described.

If the y-axis direction component is uniformly distributed in the range between the lower limit value 0 and the upper limit value {d-dcos (θ _y / 2)}, in the same manner as the error variance obtained by the equation (2), The error variance σ _y2 ² of the y-axis direction component is expressed by Expression (4).

ｘ軸方向成分が下限値０と上限値｛ｄ−ｄｃｏｓ（θ_ｘ／２）｝との範囲で一様に分布しているとすると、式（３）で求めた誤差分散と同様にして、ｘ軸方向成分の誤差分散σ_ｘ２ ^２は、式（５）で表わされる。

If the x-axis direction component is uniformly distributed in the range between the lower limit value 0 and the upper limit value {d−dcos (θ _x / 2)}, in the same manner as the error variance obtained by the equation (3), The error variance σ _x2 ² of the x-axis direction component is expressed by Expression (5).

したがって、式（２）〜（５）に基づいて、誤差分散σ^２は式（６）で表わされる。式（６）において、θ_ｘおよびθ_ｙはレーザ光の広がり角であるから０に近い値である。そして、θ_ｙに比較してθ_ｘは非常に小さく、ｓｉｎ（θ_ｙ／２）に比較してｓｉｎ（θ_ｘ／２）の値は無視できるものとしている。

Therefore, based on the equations (2) to (5), the error variance σ ² is expressed by the equation (6). In equation (6), θ _x and θ _y are values close to 0 since they are the spread angles of the laser light. Further, θ _x is very small compared to θ _y , and the value of sin (θ _x / 2) is negligible compared to sin (θ _y / 2).

そして、図５のＳ４２０で算出する距離の誤差分散のうち、予め測位装置１０を使用して測定した得られた距離に関わらず一定の誤差分散をσ_ｓ ^２とすると、σ_ｓ ^２と距離ｄに応じて変動する式（６）の誤差分散σ^２とを合わせた距離ｄの誤差分散σ_ｄ ^２は、式（７）で表わされる。式（７）が示すように、距離ｄ_１、ｄ_２の誤差分散は距離に応じて変動する。

Then, among the error variances of the distances calculated in S420 in FIG. 5, assuming that a constant error variance is σ _s ² regardless of the distance obtained by using the positioning device 10 in advance, σ _s ² and the distance d The error variance σ _d ² of the distance d combined with the error variance σ ² of the equation (6) that varies according to the equation (7) is expressed by the equation (7). As Equation (7) shows, the error variance of the distances d ₁ and d ₂ varies depending on the distance.

次に、図５のＳ４２２において、測位処理部２０は車両１００の位置を最小二乗法で測位するときの重みを設定する。

Next, in S422 of FIG. 5, the positioning processing unit 20 sets a weight when positioning the position of the vehicle 100 by the least square method.

First, the positions of the two communication devices 30 acquired in S402 of FIG. 3 are calculated (x _i , y _i : i = 1, 2), the position of the vehicle 100 is (x, y), and calculated in S400 of FIG. Assuming that the distance between the vehicle 100 and the communication device 30 is d _i (i = 1, 2), d _i is expressed by Expression (8).

式（８）のｄ_ｉをｘ、ｙについて偏微分した式を式（９）、（１０）に示す。

Equations (9) and (10) show partial differentiation of d _i in equation (8) with respect to x and y.

距離ｄ_ｉの誤差は誤差伝搬の関係から、式（９）、（１０）を用いて式（１１）で表わされる。

The error of distance d _i is expressed by equation (11) using equations (9) and (10) because of error propagation.

距離ｄ_１２の誤差、角度θの誤差も同様にそれぞれ式（１２）、（１３）で表わされる。式（１１）〜（１３）は式（１４）を用いて式（１５）と表わされるので、式（１５）の行列Ｘは式（１６）から求めることができる。

Error of the distance _{d 12,} the error similarly each type of angle theta (12), represented by (13). Since Expressions (11) to (13) are expressed as Expression (15) using Expression (14), the matrix X of Expression (15) can be obtained from Expression (16).

ここで式（１６）の行列Ｄの４個の成分は、車両１００の位置を測位するために、車両１００と通信装置３０との距離と、車両１００を基準とした通信装置３０の方向と、通信装置３０の位置とに基づいて設定された測位パラメータである。各測位パラメータを重み付けする重み行列Ｗは式（１７）で表わされる。

Here, the four components of the matrix D in the equation (16) include the distance between the vehicle 100 and the communication device 30 to determine the position of the vehicle 100, the direction of the communication device 30 with respect to the vehicle 100, The positioning parameters are set based on the position of the communication device 30. A weight matrix W for weighting each positioning parameter is expressed by Expression (17).

式（１７）において、車両１００と２個の通信装置３０との距離ｄ_１、ｄ_２に対応する重みには、車両１００と２個の通信装置３０との相対位置である距離ｄ_１、ｄ_２の誤差分散の変動が含まれている。

In Expression (17), the weights corresponding to the distances d ₁ and d ₂ between the vehicle 100 and the two communication devices 30 are distances d ₁ and d that are relative positions of the vehicle 100 and the two communication devices 30. ₂ error variance variations are included.

The weight corresponding to the angle θ formed by the two communication devices 30 with respect to the vehicle 100 includes a variation in the error variance of the angle θ calculated from the direction that is the relative position between the vehicle 100 and the two communication devices 30. It is desirable to include. However, as described above, the variation in the error variance at the angle θ is smaller than the variation in the error variance at the distances d ₁ and d ₂ . Therefore, the weight corresponding to the angle θ in Expression (17) does not include the variation in the error variance of the angle θ.

Since the two communication devices 30 a distance d ₁₂ between the absence variation of error variance, the weights corresponding to the two communication devices 30 a distance d ₁₂ between not change the error variance are considered.
The positioning processing unit 20 calculates the position (x, y) of the vehicle 100 from the equation (18) by the least square method using the weight matrix W including the weight calculated in S422 as a component (S424).

［１−３．効果］
以上説明した第１実施形態によれば、以下の効果が得られる。

[1-3. effect]
According to the first embodiment described above, the following effects can be obtained.

[1A] In the case where the position of the vehicle 100 is measured by the weighted least square method in consideration of the variation in the error of the distance depending on the distance between the vehicle 100 measured by the laser beam and the communication device 30 as the target object. , the weight of the positioning parameter set based on the distance d _1, d ₂ of the positioning parameters, distance d _1, was set on the basis of the error variance which varies as a function of d _2.

As a result, the position of the vehicle 100 can be measured with higher accuracy than in the case where the weights of the weight matrix are set without considering the variation in distance error variance.
[2. Second Embodiment]
[2-1. Difference from the first embodiment]
The description of the configuration common to the second embodiment and the first embodiment is omitted, and the configuration of the second embodiment will be described based on FIGS. 7 and 8 with a focus on the differences.

  As illustrated in FIG. 7, the positioning device 50 has a configuration in which a feature amount acquisition unit 52 is added to the positioning device 10 of the first embodiment. The feature amount acquisition unit 52 acquires the height at which the communication device 30 is installed as the feature amount of the communication device 30 from the light reception signal output from the light reception unit 14. The height of the communication device 30 is transmitted from the communication device 30 as information light.

In addition to the distances d ₁ and d ₂ between the vehicle 100 and the two communication devices 30, the positioning device 50 has a height H _i (i = 1, 2) at which the communication device 30 is installed as shown in FIG. ) And the height h of the positioning device 50 mounted on the vehicle 100, the position of the vehicle 100 is measured. The height h at which the positioning device 50 is mounted is stored in the positioning device 50.

Considering the height H _i at which the communication device 30 is installed and the height h at which the positioning device 50 is mounted on the vehicle 100, the error Δd _i ′ caused by the spread angle of the laser beam 300 is expressed by the equation (19). It is represented by

式（１９）には、高さ方向のレーザ光３００の広がり角θ_ｙから生じるｓｉｎ（θ_ｙ／２）による誤差が含まれている。したがって、広がり角θ_ｙから生じる誤差に比べて小さいので第１実施形態では省略した横方向のレーザ光３００の広がり角θ_ｘから生じるｓｉｎ（θ_ｘ／２）による誤差を考慮すると、第１実施形態の式（６）、（７）はそれぞれ式（２０）、（２１）で表わされる。

The equation (19) includes an error due to sin (θ _y / 2) generated from the spread angle θ _y of the laser beam 300 in the height direction. Therefore, since the error is smaller than the error caused by the spread angle θ _y , the first embodiment is considered in consideration of the error due to sin (θ _x / 2) caused by the spread angle θ _{x of} the lateral laser beam 300 omitted in the first embodiment. Expressions (6) and (7) of the form are expressed by expressions (20) and (21), respectively.

式（２１）に基づいて、式（１７）の重み行列の成分のうち、車両１００と通信装置３０との距離ｄ_１、ｄ_２に関する成分の値が設定される。

Based on Expression (21), among the components of the weight matrix of Expression (17), the values of the components related to the distances d ₁ and d ₂ between the vehicle 100 and the communication device 30 are set.

[2-2. effect]
According to the second embodiment described above, in addition to the effect [1A] of the first embodiment, the following effect can be obtained.

[2A] The height H _i is acquired as the feature quantity of the communication device 30 that is the target object, and in addition to the spread angle θ _y of the laser beam 300 in the height direction in the weight matrix W, Since the weight including the error variance resulting from the divergence angle θ _x is set, the positioning accuracy of the position of the vehicle 100 is improved.

[3. Third Embodiment]
[3-1. Difference from the first embodiment]
The description of the configuration common to the third embodiment and the first embodiment will be omitted, and the configuration of the third embodiment will be described based on FIGS.

  As shown in FIG. 9, the positioning device 60 is configured by adding a posture acquisition unit 62 to the positioning device 10 of the first embodiment. The attitude acquisition unit 62 acquires the inclination of the vehicle 100 based on the gradient α of the road on which the vehicle 100 travels as the attitude of the vehicle 100 based on detection signals of the gyro sensor and the acceleration sensor, as shown in FIG. The positioning device 60 measures the position of the vehicle 100 in consideration of the inclination of the vehicle 100.

A positioning process according to the third embodiment will be described with reference to FIG. In the flowchart of FIG. 11, “S” represents a step.
In S430 of FIG. 11, the posture acquisition section 62 acquires the road gradient alpha _k. The initial value of the subscript i of α _k is, for example, 1, and is incremented by 1 every time processing is returned from S436 to S430. The positioning processing unit 20 multiplies the distance d _i between the vehicle 100 measured by the laser beam 300 on the road having the road gradient α _k and the communication device 30 by cos α _k and corrects the distance d _i to a horizontal distance. Dk is obtained (S432).

Next, the posture acquisition unit 62 re-acquires the road gradient α _{k + 1} (S434), and determines whether the distance d _i and the correction distance D _k used in cos α _{k + 1} and S432 satisfy the inequality described in S436. To do. That is, because the difference between the road gradient α _k acquired in S430 and the road gradient α _{k + 1} acquired in S434 is small, the left side of the inequality shown in S436 is less than or equal to the set threshold value δ, or the road gradient α _k and the road gradient Since the difference from α _{k + 1} is large, it is determined whether the left side of the inequality shown in S436 exceeds the threshold δ.

When the distance d _i , the correction distance D _k, and cos α _{k + 1} do not satisfy the inequality described in S436 (S436: No), that is, the difference between the road gradient α _k and the road gradient α _{k + 1} is large and the road gradient changes greatly. If so, the positioning processing unit 20 returns the process to S430.

When the distance d _i , the correction distance D _k, and cos α _{k + 1} satisfy the inequality described in S436 (S436: Yes), that is, the difference between the road gradient α _k and the road gradient α _{k + 1} is small, and the road gradient is almost changed. If not, the positioning processing unit 20 calculates the error variance using the correction distance _Dk calculated in S432 as the distance between the vehicle 100 and the communication device 30 (S438).

The processing of S440 and S442 is substantially the same as the processing of S422 and S424 in FIG.
[3-2. effect]
According to the third embodiment described above, the following effect is obtained in addition to the effect [1A] of the first embodiment.

[3A] When the road gradient α _k and the road gradient α _{k + 1} become equal, the positioning device 60 measures the position of the vehicle. That is, since the positioning is performed when the vehicle 100 has the same inclination in the front-rear direction as the posture of the vehicle 100, it is possible to reduce measurement variations in the distance between the vehicle 100 and the communication device 30 that is the target object.

Furthermore, since the measured distance d _i between the vehicle 100 and the communication device 30 is multiplied by cos α _k and corrected using the corrected distance D _k corrected to the horizontal distance, positioning can be performed with high accuracy.
[4. Fourth Embodiment]
[4-1. Difference from the first embodiment]
The description of the configuration common to the fourth embodiment and the first embodiment will be omitted, and the configuration of the fourth embodiment will be described with reference to FIGS.

  As illustrated in FIG. 12, the positioning device 70 is configured by adding a movement information acquisition unit 72 to the positioning device 10 of the first embodiment. The movement information acquisition unit 72 acquires the vehicle speed as movement information of the vehicle 100. The positioning device 70 measures the position of the vehicle 100 in consideration of the vehicle speed of the vehicle 100.

  The positioning process according to the fourth embodiment will be described with reference to FIGS. As shown in FIG. 13, the position (x, y) at which the positioning device 70 calculates the distance between the vehicle 100 and the first communication device 30, and two devices farther than the vehicle 100 and the first communication device 30. It is different from the position (x ′, y ′) for calculating the distance to the eye communication device 30.

The distance L between the position (x, y) and the position (x ′, y ′) is v for the vehicle speed, ω for the rotational angular velocity searched by the laser beam 300, and positions (x, y), (x ′, y ′). If the angle formed by the two communication devices 30 is θ ₁ , the equation (22) is obtained.

式（１７）の重み行列Ｗにおいて車両１００が移動することにより変化する重みの成分は、車両１００と２個の通信装置３０との距離ｄ_１、ｄ_２、車両１００に対する２個の通信装置３０の角度θに対応する成分である。車両１００の移動に関係しない重みの成分は２個の通信装置３０同士の距離ｄ_１２に対応する成分である。

The weight components that change as the vehicle 100 moves in the weight matrix W of Expression (17) are the distances d ₁ and d ₂ between the vehicle 100 and the two communication devices 30, and the two communication devices 30 for the vehicle 100. Is a component corresponding to the angle θ. Component weight not related to movement of the vehicle 100 is a component corresponding to the two communication devices 30 to each other a distance d ₁₂ of.

Accordingly, the weighting matrix, the distance d ₁ and distance d ₂ and the angle θ and the corresponding weight to smaller than normal, the distance d ₁ and distance the distance d ₁₂ to corresponding weights and d ₂ and the angle θ The positioning accuracy can be improved by relatively increasing the corresponding weight. For example, the normal weight corresponding to each of the distance d ₁ , the distance d _2, and the angle θ is reduced by multiplying by a coefficient according to the vehicle speed, and the weight of the distance d ₁₂ is relatively increased.

Alternatively, as shown in FIG. 14, the distance d ₁ ′ between the vehicle 100 and the _first communication device 30 at the time t2 when the distance between the vehicle 100 and the second communication device 30 is calculated, and the vehicles 100 and 2. the distance d ₂ between th communication device _30, the angle theta ₁ two communication apparatus 30 to the vehicle 100 is formed ', setting the weights based on the distance d ₁₂ of the two communication devices 30 Good.

In FIG. 14, d ₁ ′ is obtained by Expression (23). In equation (23), d ₁ is the distance between the vehicle 100 calculated at time t ₁ and the first communication device 30, and the integrated value is the vehicle speed v (t) between times t ₁ and t _2. It is the movement distance of the vehicle 100 calculated by integration.

また、図１４の角度βは式（２４）で求められる。

Further, the angle β in FIG. 14 is obtained by Expression (24).

また、図１４の時刻ｔ２において車両１００に対して２個の通信装置３０が形成する角度θ_１’は式（２５）で求められる。

Further, the angle θ ₁ ′ formed by the two communication devices 30 with respect to the vehicle 100 at time t2 in FIG. 14 is obtained by Expression (25).

そこで、図１４において、ｄ_１’とｄ_２とｄ_１２とθ_１’とにより規定される三角形において、ｄ_１’、ｄ_２、ｄ_１２、θ_１’の誤差分散を設定し、誤差分散から重みを設定して車両１００の位置を測位する。

Therefore, in FIG. 14, the triangle defined by _{d 1} 'and _{d 2} and _{d 12 θ 1' and, d 1 ', d 2,} d 12, θ 1' to set the error variance, the error variance A weight is set and the position of the vehicle 100 is measured.

[4-2. effect]
According to the fourth embodiment described above, in addition to the effect [1A] of the first embodiment, the following effect can be obtained.

[4A] Since the weight is set in consideration of the movement distance calculated based on the vehicle speed that is the movement information of the vehicle 100, the positioning accuracy of the vehicle 100 is improved.
[5. Fifth Embodiment]
[5-1. Difference from the first embodiment]
Since the basic configuration of the fifth embodiment is the same as that of the first embodiment, the description of the common configuration is omitted, and the description will focus on the differences. In the fifth embodiment, the weight of the component of the weight matrix is reduced based on the reliability of the positioning parameter according to the distance between the vehicle 100 and the communication device 30.

For example, as shown in (A) of FIG. 15, if the distance d ₁ between the vehicle 100 and one second communication device 30 that the distance measuring unit 18 measures the positioning device 10 is a predetermined distance or more, the positioning device 10 Determines that the reliability of the distance d ₂ longer than the distance d _{1 is} lower than the distance d ₁ , the distance d _12, and the angle θ. Therefore, as shown in the equation (26), the weight of the distance d ₂ is set to 0 among the components of the weight matrix W.

また、図１５の（Ｂ）に示すように、車両１００と１個目の通信装置３０との距離ｄ_１が前述した所定距離よりも短い所定距離以下であれば、測位装置１０は、角度θの信頼度は、距離ｄ_１距離ｄ_２と距離ｄ_１２とよりも低くなると判断する。そこで、式（２７）に示すように、重み行列Ｗの成分のうち、角度θの重みを０に設定する。

Further, as shown in (B) of FIG. 15, if a short predetermined distance less than the predetermined distance the distance d ₁ is described above the vehicle 100 and one second communication device 30, the positioning device 10, the angle θ reliability, it is determined that is lower than the distance _{d 1} distance _{d 2} and a distance _{d 12} Prefecture. Therefore, as shown in Expression (27), the weight of the angle θ among the components of the weight matrix W is set to zero.

［５−２．効果］
以上説明した第５実施形態によれば、第１実施形態の効果［１Ａ］に加え、以下の効果が得られる。

[5-2. effect]
According to the fifth embodiment described above, in addition to the effect [1A] of the first embodiment, the following effect can be obtained.

[5A]
Since the weight of the component of the selected weight matrix is set to 0 based on the reliability of the positioning parameter corresponding to the distance between the vehicle 100 and the communication device 30, the time required for positioning the vehicle 100 can be shortened.

[6. Sixth Embodiment]
[6-1. Difference from the first embodiment]
The description of the configuration common to the sixth embodiment and the first embodiment will be omitted, and the configuration of the sixth embodiment will be described based on FIG. 16 and FIG.

As shown in FIG. 16, the positioning device 80 has a configuration in which a position database (DB) 82 and a target object recognition unit 84 are added to the positioning device 10 of the first embodiment.
In the position DB 82, a target object 200 such as a characteristic building (see FIG. 17) and a position of the target object 200 represented by latitude and longitude are registered in advance by, for example, SLAM (Simultaneously Localization and Mapping). .

  The target object recognition unit 84 recognizes a characteristic target object 200 that can be distinguished from surrounding objects from the distance to the target object 200 measured by the distance measurement unit 18 and the direction of the target object 200 with reference to the vehicle 100. . The position acquisition unit 16 searches the position DB 82 for the target object 200 recognized by the target object recognition unit 84 and acquires the position. The point which acquires the position of the target object 200 not from the target object 200 itself but from the position DB 82 of the positioning device 80 is different from the first embodiment.

  The positioning processing unit 20 is based on the position of the target object 200 acquired by the position acquisition unit 16, the distance between the vehicle 100 and the target object 200 calculated by the distance measuring unit 18, and the direction of the target object 200 with respect to the vehicle 100. Position 100.

An error variance including fluctuations in the error variance of the distance is set according to the distances d ₁ and d ₂ between the vehicle 100 and the target object 200, and positioning is set based on the distances d ₁ and d ₂ based on the error variance. The point that the parameter weight is set based on the error variance is the same as in the first embodiment.

  When the target object 200 is a building or the like as in the sixth embodiment, the height and the horizontal length of the target object 200 may be acquired from the position DB 82 as the size of the target object 200 as the feature amount of the target object 200. . And based on the acquired feature-value, you may measure the position of the vehicle 100 similarly to 2nd Embodiment.

Further, the distance between the vehicle 100 and the target object 200 may be measured a plurality of times within the range of the size of the target object 200, and an average value of the measured distances may be adopted as the distance.
[6-2. effect]
[6A] According to the sixth embodiment described above, the effect obtained by replacing the communication device 30 with the target object 200 in the effect [1A] of the embodiment can be obtained.

  [6B] Without newly installing the communication device 30, the position of the vehicle 100 can be measured based on the position information of the target object 200 and the distance between the vehicle 100 and the target object 200 with a building or the like as the target object 200.

[7. Other Embodiments]
[7A] In the above embodiment, an example in which the number of target objects is two has been described. On the other hand, the number of target objects may be three or more. Even when there are three or more target objects, in the combination of selecting two from the three target objects, the distance between the vehicle 100 and the two target objects, the distance between the two target objects, The position of the vehicle can be measured based on the angle formed by the two target objects with respect to the vehicle 100.

If the number of target objects is three or more, a positioning parameter may be set based on only the distance as the relative position between the vehicle and the target object, and the position of the vehicle 100 may be measured.
[7B] In the above embodiment, laser light is used as the exploration wave. On the other hand, as long as the distance between the vehicle and the target object can be measured, the exploration wave is not limited to laser light, and may be other light waves such as LED light, or radio waves such as millimeter waves. Ultrasound may be used.

[7C] In the above embodiment, the position of the vehicle 100 is measured as a moving object. On the other hand, as long as it moves, the moving object is not limited to a vehicle, and may be a person.
[7D] In the above embodiment, the position of the vehicle 100 as a moving object is measured in two dimensions (x, y). On the other hand, the position of the moving object may be measured in three dimensions (x, y, z) including the height.

  [7E] In the second embodiment, the height at which the communication device 30 is installed is acquired as the feature amount of the communication device 30, and the weight of the positioning parameter is set based on the height. In addition to this, the lateral spread of the communication device 30 may be acquired as the feature amount of the communication device 30, and the weight of the positioning parameter may be set based on the lateral spread.

  [7F] In the third embodiment, the posture of the vehicle 100 is detected as the inclination of the vehicle 100 due to the road gradient, and the position of the vehicle 100 is measured when the road gradient is equal. In addition to this, the lateral direction of the vehicle 100 may be detected as the posture of the vehicle 100, and the position of the vehicle 100 may be measured when the lateral direction is equal.

  [7G] In the fourth embodiment, the movement distance of the vehicle 100 is calculated based on the vehicle speed of the vehicle 100 as the movement information of the vehicle 100, and the position of the vehicle 100 is determined in consideration of the movement distance. On the other hand, the position of the vehicle 100 may be measured in consideration of the movement distance of the vehicle 100 calculated based on the acceleration of the vehicle 100 as the movement information of the vehicle 100.

  [7H] In the fifth embodiment, instead of setting the weight of the positioning parameter having low reliability to 0, the normal weight is multiplied by a coefficient smaller than 1 according to the distance between the vehicle 100 and the communication device 30. May be.

  [7I] The functions of one component in the above embodiment may be distributed as a plurality of components, or the functions of a plurality of components may be integrated into one component. Further, at least a part of the configuration of the above embodiment may be replaced with a known configuration having the same function. Moreover, you may abbreviate | omit a part of structure of the said embodiment as long as a subject can be solved. Further, at least a part of the configuration of the above embodiment may be added to or replaced with the configuration of the other embodiment. In addition, all the aspects included in the technical idea specified only by the wording described in the claims are embodiments of the present invention.

  [7J] In addition to the positioning devices 10, 50, 60, 70, 80 described above, a positioning system including the positioning devices 10, 50, 60, 70, 80 as constituent elements, the positioning devices 10, 50, 60, 70, The present invention can also be realized in various forms such as a positioning program for causing a computer to function as 80, a recording medium recording the positioning program, and a positioning method.

2: positioning system, 10, 50, 60, 70, 80: positioning device, 18: distance measuring unit (ranging unit), 30: communication device (target object), 52: feature amount acquiring unit (feature amount acquiring unit) 62: Attitude acquisition unit (attitude acquisition means), 72: Movement information acquisition unit (movement information acquisition means), 100: Vehicle, 200: Target object, 300: Laser light (probing wave)

Claims (7)

A positioning device (10, 50, 60, 70, 80) mounted on a moving object (100) for positioning the position of the moving object,
Ranging means (18, S400) that irradiates a search wave (300) and measures the relative positions of the moving object and the plurality of target objects based on reflected waves reflected from the plurality of target objects (30, 200) , S410 to S418),
Error setting means (20, S420) for setting a variance of each relative position error according to a measurement value corresponding to the relative position measured by the distance measuring means;
Weighting means (20, S422) for setting a weight of the positioning parameter set based on the measured value corresponding to the relative position among a plurality of positioning parameters based on the variance set by the error setting means;
Positioning means (20, S424) for positioning the position of the moving object based on the positioning parameters weighted by the weighting means;
A positioning device comprising:   The positioning device according to claim 1, wherein the positioning unit measures the position of the moving object by a least square method in which the positioning parameters are weighted. Comprising feature quantity acquisition means (52) for acquiring the feature quantity of the target object;
The weighting unit weights the positioning parameter based on the feature amount acquired by the feature amount acquisition unit.
The positioning device (50) according to claim 1 or 2, characterized in that Posture acquisition means (62, S430, S434) for acquiring the posture of the moving object with respect to the target object;
The positioning means measures the position of the moving object when the attitude of the moving object acquired by the attitude acquisition means does not change;
The positioning device (60) according to any one of claims 1 to 3, characterized in that: Movement information acquisition means (72) for acquiring movement information when the moving object moves;
The weighting means weights the positioning parameter based on the movement information acquired by the movement information acquisition means.
The positioning device (70) according to any one of claims 1 to 4, characterized in that.   6. The weighting unit according to any one of claims 1 to 5, wherein the weight of the positioning parameter having a lower reliability than the other positioning parameters among the plurality of positioning parameters is made smaller than normal. The described positioning device.   The positioning device according to claim 6, wherein the weighting unit sets the weight of the positioning parameter having a lower reliability than the other positioning parameters among the plurality of positioning parameters to zero.

Images