JP7101840B1 - Position estimation device, position estimation method and automatic driving system - Google Patents

Abstract

A position estimation device capable of estimating the self-position of a mobile object with high accuracy is obtained. A position estimation device (100) of the present disclosure includes a distance measurement unit (101) that measures a distance by wireless communication between an anchor (2) and a wireless communication device (111) mounted on a mobile object (1), and a distance measurement unit that manages the timing of distance measurement. a timing management unit 102; a self-position prediction unit 103 that predicts the self-position and moving direction of the mobile unit 1 at each ranging timing; A line-of-sight state determination unit 104 that determines whether or not there is a line-of-sight state between the anchor 2 to be distance-measured and the wireless communication device 111 at each range-finding timing based on the position prediction result of the mobile body 1 from the position prediction unit 103; and a self-position estimation unit 105 for estimating the self-position of the mobile object 1 based on the distance measurement between the anchor 2 and the wireless communication device 111. [Selection drawing] Fig. 1

Description

The present application relates to a position estimation device, a position estimation method and an automated driving system.

In order to realize automatic driving of a moving body represented by a vehicle, it is necessary to always accurately grasp the self-position of the moving body even while moving. As one of the methods for measuring the self-position of a moving body moving on a road, wireless communication between a wireless transmitting device called an anchor installed on the roadside of the road and a wireless communication device mounted on the moving body is performed. There is a method of estimating the position of the moving body by using it.

Patent Document 1 describes a means for estimating the self-position of a moving body by configuring an anchor node and a target node, and a method for dynamically changing the weighting of data used for position estimation according to the received signal quality of wireless communication. ing. Here, as an example, the anchor node may be a roadside machine installed on the roadside of a road. Further, as an example, the target node may be a wireless communication device mounted on a mobile body.

Further, in Patent Document 1, a plurality of anchor nodes and one target node are used, and the distance between the anchor node and the target node is measured by using wireless communication between the anchor node and three or more anchor nodes. A system for estimating the position of a target node from the distance between the target node and the target node is disclosed.

Japanese Patent No. 6375400

As for the configuration of the anchor node and the target node, if the anchor node is installed at a position higher than the moving body and the target node is placed on the ceiling of the moving body, the line-of-sight state (LOS: Line Of Site), that is, It becomes easy to establish a state in which there is no object that blocks wireless communication between the anchor node and the target node.

However, the anchor node is not always installed at a position higher than the ceiling of the moving body. Further, even if the target node is installed on the ceiling of the moving body, wireless communication with the anchor node may be interrupted depending on the shape of the ceiling.

On the other hand, if the target node is placed on a part other than the ceiling of the moving body, for example, at the corner of the moving body, the space between the anchor node and the target node is compared with the case where the target node is placed on the ceiling of the moving body. There is a high probability that a part of the moving body is present in, in which case the moving body itself interferes with the shielded object located between the anchor node and the target node, that is, the radio communication between the anchor node and the target node. do. That is, due to the shielding of wireless communication by a part of the mobile body, there is no line-of-sight between the anchor node and the target node, that is, a non-line-of-sight state (NLOS: Non Line Of Sign).

When there is a non-line-of-sight state (NLOS) between the anchor node and the target node, the amount of energy reached by wireless communication is significantly reduced, that is, the communication quality is deteriorated. In particular, when a high frequency higher than a microwave is used for wireless communication, the amount of energy reached tends to be significantly reduced. In addition, the communication quality is deteriorated due to the influence of shadowing or fading caused by reflected waves and diffracted waves. Furthermore, it may not be possible to measure the correct distance due to the influence of multipath.

In the position estimation method disclosed in Patent Document 1, a weighting coefficient is generated using the received signal quality and used in the calculation of the self-position estimation of the moving body to stabilize the self-position estimation accuracy. There is. However, the position estimation method disclosed in Document 1 requires a high cost for calculation based on the distance measurement data measured between a large number of anchor nodes and the target node, and also the anchor node and the target node. If the communication quality is not good between them, the weighting coefficient for the obtained distance measurement data can be lowered, but the estimation accuracy of the self-position by further using the distance measurement data with low communication quality in the calculation can be achieved. The decline became a problem.

In addition, even when the line-of-sight state (LOS) is set between the anchor node and the target node, if the distance between the two is large or the strength of the transmitted radio wave from the anchor node is weak due to some factor, reception is performed. Signal quality may deteriorate. On the other hand, even if there is a non-line-of-sight state (NLOS) between the two, the received signal quality may be good if the distance between the two is short or if the strength of the transmitted radio wave from the anchor node is strong for some reason. be. Therefore, in the method of estimating the self-position only by the received signal quality, the line-of-sight state (LOS) or the non-line-of-sight state (NLOS) cannot be accurately determined, and there is a problem that the self-position estimation accuracy is lowered.

The present application discloses a technique for solving the above-mentioned problems, and improves the estimation accuracy of the self-position by accurately determining the line-of-sight state (LOS) or the non-line-of-sight state (NLOS). It is an object of the present invention to provide a position estimation device, an automatic driving system, and a position estimation method which can be used.

The position estimation device disclosed in the present application is
A ranging unit that measures the distance between the plurality of anchors and the wireless communication device by wireless communication between the plurality of anchors installed on the roadside of the road and the wireless communication device mounted on the moving body. ,
The distance measurement timing management unit that manages the timing of distance measurement,
A self-position prediction unit that predicts the self-position and movement direction of the moving body at each distance measurement timing, and
Each of the plurality of anchors is based on the shape of the moving body, the position where the wireless communication device is placed on the moving body, the position information of the anchor, and the result of the position prediction of the moving body by the self-position prediction unit. In either the line-of-sight state in which a part of the moving body does not exist or the non-line-of-sight state in which a part of the moving body exists between the anchor to be distanced and the wireless communication device at each distance measuring timing. The outlook status judgment unit that determines whether or not there is,
It is provided with a self-position estimation unit that estimates the self-position of the moving body based on the distance measurement result between the anchor determined to be in the line-of-sight state and the radio communication device.

The automated driving system disclosed in this application is
With the above position estimation device,
The wireless communication device mounted on the mobile body and
A mobile body control device that controls the moving body based on the self-position information of the moving body estimated by the position estimation device is provided.

The position estimation method disclosed in the present application is as follows.
A step of measuring the shape of the moving body and the position of the wireless communication device mounted on the moving body, and
Steps to acquire the position information of multiple anchors installed on the roadside of the road,
A step of predicting the self-position and the moving direction of the moving body at each distance measuring timing,
Based on the shape of the moving body, the position where the wireless communication device is mounted on the moving body, the position information of the anchor, and the result of the position prediction of the moving body, the distance measurement timing of each of the plurality of anchors is In the step of determining whether there is a line-of-sight state in which a part of the moving body does not exist or a non-line-of-sight state in which a part of the moving body exists between the anchor to be distanced and the wireless communication device. When,
A step of measuring the distance between the anchor and the wireless communication device based on the preset distance measurement timing, and
It includes a step of estimating the self-position of the moving body based on the distance measurement result between the anchor determined to be in the line-of-sight state and the wireless communication device.

According to the position estimation device, the automatic operation system and the position estimation method disclosed in the present application, the shape of the moving body, the position where the wireless communication device is mounted on the moving body, and the result of the position prediction of the moving body by the self-position predicting unit. , And, based on the position information of each anchor, it is determined based on the geometrical relationship whether each anchor and the wireless communication device are in the line-of-sight state (LOS) or the non-line-of-sight state (NLOS). Therefore, in each anchor, even when a part of the moving body exists between the anchor and the wireless communication device, the estimation accuracy of the self-position of the moving body can be improved.

FIG. 3 is a schematic block diagram of a position estimation device and an automated driving system according to the first embodiment. It is a functional block diagram of the position estimation device and the automatic operation system by Embodiment 1. FIG. It is a schematic diagram which shows the anchor installed on the road side of a road, and the moving body moving on a road. It is a schematic diagram which shows an example of the position estimation method using an anchor and a wireless communication device. It is a schematic diagram which shows an example of the position estimation method when there is a shielded object. It is a schematic diagram which shows an example of the position estimation method by Embodiment 1. FIG. It is a schematic diagram which shows an example of the position estimation method by Embodiment 1. FIG. It is a schematic diagram which shows an example of the position estimation method by Embodiment 1. FIG. It is a schematic diagram which shows an example of the position estimation method by Embodiment 1. FIG. It is a flowchart of the position estimation method by Embodiment 2. FIG. It is a schematic diagram which shows an example of the position estimation method by Embodiment 3. FIG. It is a schematic diagram which shows an example of the position estimation method by Embodiment 3. FIG. It is a flowchart of the position estimation method according to Embodiment 4. FIG. It is a figure which shows an example of the hardware of the position estimation apparatus and the automatic driving system by Embodiments 1-3.

Embodiment 1.
FIG. 1 shows a schematic block diagram of the position estimation device and the automated driving system according to the first embodiment. The position estimation device 100 is mounted on the moving body 1. The position estimation device 100 includes a control unit 150 and a communication function unit 107. The control unit 150 and the communication function unit 107 are connected so as to enable bidirectional communication.

The control unit 150 includes a distance measurement unit 101, a distance measurement timing management unit 102, a self-position prediction unit 103, a line-of-sight state determination unit 104, a self-position estimation unit 105, and a moving body information management unit 106. Each function of the position estimation device 100 is realized by a processing circuit included in the position estimation device 100. Specifically, as shown in FIG. 14, the position estimation device 100 includes a processor 801 such as a CPU (Central Processing Unit), a storage device 802, and the like.

Then, in each function such as the functional units 101 to 107 included in the position estimation device 100, the processor 801 executes the software (program) stored in the storage device 802, and the storage device 802 and the input / output device (not shown). ) Etc., which is realized by cooperating with other hardware of the position estimation device 100. It should be noted that the setting data such as the distance measured distance and the preset conditions used by the functional units 101 to 107 and the like are stored in a storage device such as a ROM (Read Only Memory) as a part of the software (program). It is stored in 802. FIG. 14 will be described in detail later.
Hereinafter, each function of the position estimation device 100 will be described in detail.

The position estimation device 100 exchanges electric signals with the wireless communication device 111 mounted on the mobile body 1 via the communication function unit 107. Further, the position estimation device 100 transmits an electric signal to the mobile body control device 112 mounted on the mobile body 1 via the communication function unit 107, or electricity is supplied from the inertial measurement unit 113 via the communication function unit 107. Receive a signal.

An example of the moving body 1 is a vehicle. Further, as an example of the wireless communication device 111, a V2X (Vehicle-to-Everything) wireless transmitter can be mentioned. Further, the wireless communication device 111 may be in the form of an electronic tag.

The ranging unit 101 issues a command to measure the distance between the wireless communication device 111 and the anchor 2 by wireless communication between the wireless communication device 111 and the anchor 2 installed on the road side of the road. The anchor 2 will be described later.

The distance measurement timing management unit 102 determines or manages a time interval for measuring the distance between the wireless communication device 111 and the anchor 2, that is, the distance measurement timing. This is because the distance measurement between the wireless communication device 111 and the anchor 2 is basically measured at regular time intervals, so it is necessary to manage the distance measurement timing. Further, as will be described later, the distance measurement timing may be dynamically changed, such as distance measurement when a preset condition is satisfied.

The self-position prediction unit 103, for example, based on the self-position information such as the self-position and the moving direction of the moving body 1 measured at a certain point in the past, the self-position and the moving direction of the moving body 1 at the current distance measurement timing. To estimate. Further, based on the output signal from the inertial measurement unit 113 described later, for example, the self-position and the moving direction of the moving body 1 may be predicted when the moving body 1 makes an inertial motion. The details of the method of predicting the self-position and the moving direction of the moving body 1 will be described later.

The line-of-sight state determination unit 104 determines the shape of the mobile body 1, the position where the wireless communication device 111 is placed on the mobile body 1, the position information of the anchor 2, and the self-position and the movement direction of the mobile body 1 by the self-position prediction unit 103. Based on the prediction result, when wireless communication is performed between the wireless communication device 111 and the anchor 2, the moving body 1 is located between the anchor 2 to be distance-measured and the wireless communication device 111 at each distance measurement timing for each anchor 2. Whether a state in which a part of the mobile body 1 does not exist is established, that is, a line-of-sight state (LOS) is established, or a state in which a part of the mobile body 1 exists, that is, a non-line-of-sight state ((NLOS)). Judge if it is.

The self-position estimation unit 105 estimates the self-position of the moving body 1. The details of the self-position estimation method will be described later.

The moving body information management unit 106 manages the information necessary for estimating the self-position. As information on the mobile body 1, the shape of the mobile body 1 and the position of the wireless communication device 111 mounted on the mobile body 1 are indispensable. It also manages the position information of the anchor 2.

As described above, the communication function unit 107 includes a wireless communication device 111 mounted on the mobile body 1 outside the position estimation device 100, a mobile body control device 112 mounted on the mobile body 1, and an inertial measurement device 113. And exchange electrical signals. In addition, various electric signals are exchanged with the control unit 150 inside the position estimation device 100.

Using the self-position information estimated by the position estimation device 100 described above, the moving body control device 112 automatically operates the moving body 1. That is, the automatic driving system 200 according to the first embodiment is composed of a position estimation device 100, a wireless communication device 111, and a mobile body control device 112, and also includes an inertial measurement device 113, if necessary.

The wireless communication device 111 acquires and outputs the position information of the anchor 2, the distance to the anchor 2, the received signal quality, and the like by wirelessly communicating with the anchor 2. The electric signal output from the wireless communication device 111 is sent to the position estimation device 100 via the communication function unit 107 and used as self-position estimation data. On the other hand, the wireless communication device 111 receives an electric signal or the like instructing the timing of distance measurement from the position estimation device 100. The wireless communication device 111 is mounted on the mobile body 1. The number of wireless communication devices 111 mounted on one mobile body 1 may be one or a plurality.

The mobile body control device 112 controls so that the mobile body 1 can be automatically operated based on the self-position information estimated by the self-position estimation unit 105 of the position estimation device 100.

The inertial measurement unit 113 measures the inertial state of the moving body 1. Specifically, the data obtained by measuring the moving speed, moving direction, etc. of the moving body 1 is output to the position estimation device 100. The output data is processed by the self-position prediction unit 103, and is used for predicting the self-position of the moving body 1 in the inertial state.

FIG. 2 is a functional block diagram of the position estimation device 100 and the automatic driving system 200 according to the first embodiment. An electric signal related to distance measurement is output to the self-position estimation unit 105 from the wireless communication device 111 that exchanges electric signals with the anchor 2 via wireless communication. On the other hand, the wireless communication device 111 receives an electric signal such as a distance measurement timing from the line-of-sight state determination unit 104 via the self-position prediction unit 103.

The distance measurement timing management unit 102 outputs an electric signal instructing the distance measurement at each distance measurement timing to the self-position prediction unit 103.

For example, data on the moving speed and moving direction of the moving body 1 output from the inertial measurement unit 113 outside the position estimation device 100 is input to the self-position predicting unit 103, and the moving body 1 is based on the data. The prediction result of the self-position and the moving direction is output to the line-of-sight state determination unit 104.

The mobile body information management unit 106 outputs the shape of the mobile body 1 measured in advance, the position information of the wireless communication device 111 mounted on the mobile body 1, the position information of the anchor 2, and the like to the line-of-sight state determination unit 104. The position information of the anchor 2 may be acquired at any time in the wireless communication with the wireless communication device 111, and the position information of the anchor 2 on the path on which the moving body 1 moves may be previously transmitted to the moving body information management unit 106. You may remember it.

The self-position estimation unit 105 receives from the line-of-sight state determination unit 104 a determination result of whether or not a line-of-sight state (LOS) has been established between the anchor 2 and the wireless communication device 111 at each distance measurement timing, and wirelessly. The self-position of the moving body 1 at each distance measurement timing is estimated including the distance measurement result from the communication device 111. The estimated self-position information of the moving body 1 is output to the moving body control device 112, and is applied to the control of the moving body 1, for example, the control of automatic driving.

In the functional block diagram shown in FIG. 2, the self-position information of the mobile body 1 is output from the self-position estimation unit 105 to the mobile body control device 112. However, since the self-position information of the moving body 1 after a certain time from the distance measuring timing is often required, the self-position prediction unit 103 predicts the self-position of the moving body 1 after a certain time from the distance measuring timing. It may be output.

The operation of the position estimation device 100 will be described below.
First, the anchor 2 will be described. FIG. 3 is a schematic diagram showing an anchor 2 installed on the road side of a road and a moving body 1 moving on the road. As shown in FIG. 3, a plurality of anchors 2 are installed on the road sides on both sides along the road. Generally, in order to estimate the self-position of the moving body 1, it is necessary to measure the distance to three or more anchors 2. Further, in order to estimate the self-position at each point where the moving body 1 moving on the road is moving, it is necessary to sequentially measure the distance between the optimum combinations of anchors 2 while the moving body 1 is moving. Each time of distance measurement executed sequentially is called distance measurement timing.

The anchor 2 is typically a wireless transmitter. Each anchor 2 emits a unique radio wave. The radio wave emitted from the anchor 2 is received by the wireless communication device 111 mounted on the mobile body 1. The wireless communication device 111 acquires data for calculating the self-position of the moving body 1 from the positions and distances of the plurality of anchors 2.

Examples of the method for measuring the distance between the anchor 2 and the wireless communication device 111 include propagation delay measurement (ToF: Time of Flight) or RSSI (Received Signal Strength Indicator). However, in the position estimation device 100 and the automatic driving system 200 according to the first embodiment, the distance measuring method is not particularly limited. That is, various ranging methods can be applied.

As an example of the wireless communication method between the anchor 2 and the wireless communication device 111, an impulse type ultra-wideband wireless communication (IR-UWB: Impulse-Radio Ultra Wide Band) can be mentioned. However, the distance is not limited to IR-UWB, and distance measurement may be performed based on various wireless communication methods such as BLE (Bluetooth Low Energy) and WiFi (Wireless Fidelity). Among the wireless communication methods, IR-UWB is particularly susceptible to the line-of-sight state (LOS) described later.

The wireless communication device 111 may receive the position information of the anchor 2 by wireless communication before and after distance measurement. Alternatively, the positions of all the anchors 2 on the path on which the moving body 1 moves may be stored in advance, that is, before the distance measurement is started, for example, inside the position estimation device 100. Further, when the position of the mobile body 1 is calculated by the anchor 2 and the server connected to the wireless communication device 111 via the Internet or the like, the positions of all the anchors 2 on the path to which the mobile body 1 moves are set in advance. You may store it in the server.

FIG. 4 shows a position estimation method by the ToA (Time-of-Arrival) method, which is an example of a three-sided survey position estimation method using the anchor 2 and the wireless communication device 111. The distance between two objects is measured based on the arrival time of a signal with a known velocity. In the TOA method or the TDoA (Time Difference Of Arrival) method, the wireless communication between the anchor 2 and the wireless communication device 111 is executed in a time division manner.

In the three-sided survey position estimation method, the position of the radio communication device 111 is estimated based on the estimated distances to the three anchors 2a, 2b, and 2c, respectively. For the distances to each anchor 2a, 2b and 2c, for example, a circle centered on the anchors 2a, 2b and 2c is defined by a known friss formula. The intersection of each of the three circles is used to estimate the position of the radio communicator 111.

In FIG. 4, each circle centered on each of the three anchors 2a, 2b, and 2c intersects at a single point presumed to be the position of the radio communicator 111. The self-position T of the radio communication device 111 is estimated from the distances d ₁ , d ₂ and d ₃ between the anchors 2a, 2b and 2c and the radio communication device 111, respectively.

However, the estimated position accuracy of the self-position T of the wireless communication device 111 strongly depends on the received signal quality in the wireless communication between the anchors 2a to 2c and the wireless communication device 111. This is because the received signal quality of the radio communication device 111 for each of the anchors 2a, 2b, and 2c is not always good while the moving body 1 is moving.

In the above, the received signal quality is expressed by the degree of deterioration or interference of radio waves generated during wireless communication. The degree of radio wave deterioration or interference is determined by the transmission time, loss, signal-to-noise ratio, crosstalk, echo, interruption, frequency response, etc. of wireless communication. The received signal quality is not uniquely expressed depending on the received signal strength. This is because the received signal strength of the radio wave reflected by some reflecting object may be higher than that of the radio wave from the linear direction.

FIG. 5 is a schematic diagram showing the case where the shielded object 51 is present in the self-position estimation method by the three-sided survey position estimation method. If there is a shielding object 51 between the anchor 2a and the wireless communication device 111, the wireless communication device 111 is reflected by the reflecting object 52 instead of the radio waves that would have arrived from the anchor 2a without the shielding object 51. Only the radio waves received are received. When the distance between the anchor 2a and the wireless communication device 111 is measured based on the reflected radio wave, the anchor 2a apparently exists at the position of the virtual anchor 2s shown by the dotted line in FIG. Will be regarded as. As a result, the position of the radio communication device 111 estimated by the distance from the anchors 2b, 2c and 2s is estimated to be the position S having an error with respect to the self-position T which should have originally existed. A problem occurs.

In the three-sided survey position estimation method, it is basically possible to calculate the self-position of the object to be measured by three-sided survey from the positions and distances of the three anchors. Further, it is also possible to calculate from the positions and distances of three or more anchors by the weighted least squares method so as to minimize the error. In this case, the wireless communication method between the anchor 2 and the wireless communication device 111 does not matter.

Next, the operation of the position estimation device 100 and the position estimation method according to the first embodiment will be described. FIG. 6 is a schematic diagram showing a position estimation method when one wireless communication device 111 is attached to a corner portion of the mobile body 1. Along the road (not shown) on which the moving body 1 moves, four anchors 2a to 2d capable of wireless communication are installed on the road side of the road. The position information of each anchor 2a to 2d is acquired in advance, or is transmitted from each anchor 2a to 2d to the wireless communication device 111 side by wireless communication.

Of the four anchors 2a to 2d, a line-of-sight state (LOS) is established between the three anchors 2a, 2c, and 2d and the radio communication device 111. This is because there is no object that blocks wireless communication, such as a shielding object 51, that is, a part of the moving body 1 between the anchors 2a, 2c, and 2d and the wireless communication device 111.

Whether or not a line-of-sight state (LOS) is established between the anchor and the wireless communication device 111 is determined by the three-dimensional position information of the anchor 2 and the three-dimensional position information of the wireless communication device 111 on the moving body 1. , Three-dimensional shape data of the moving body 1, for example, when the moving body 1 is a vehicle, the three-dimensional shape data of the vehicle body is used, and further, the current position predicted from the past position information of the moving body 1 and the like. By using the prediction results of the self-position and the self-position in the moving direction, it can be determined from the geometrical relationship as to whether or not a part of the moving body 1 exists on the straight line connecting the anchor 2 and the wireless communication device 111.

On the other hand, the line-of-sight state (LOS) has not been established between the anchor 2b and the wireless communication device 111. That is, it is in a non-line-of-sight state (NLOS). The reason for this is that a part of the moving body 1 exists between the anchor 2b and the wireless communication device 111 as an object that shields the wireless communication. In FIG. 6, when the line-of-sight state (LOS) is established, it is shown by a solid line, and when it is a non-line-of-sight state (NLOS), it is shown by a dotted line.

As a specific example in which a part of the moving body 1 exists as an object that shields wireless communication, when the moving body 1 is a car, for example, when the wireless communication device 111 is placed on the bonnet, it is placed on the rear part of the car. For the located anchor 2, the seat portion of the automobile is a shielding object. That is, a part of the automobile becomes a shielding object when wirelessly communicating with the wireless communication device 111 on the bonnet and the anchor 2 located behind the automobile.

In the line-of-sight state determination unit 104 of the position estimation device 100 according to the first embodiment, the shape of the moving body 1, the position where the wireless communication device 111 is mounted on the moving body 1, the position information of the anchor 2, and the self-position prediction unit 103 are used. Based on the prediction result of the self-position and the moving direction, it is determined whether the anchor 2b and the wireless communication device 111 are in the line-of-sight state (LOS) or the non-line-of-sight state (NLOS).

That is, whether or not the line-of-sight state (LOS) is established is determined by predicting the self-position and the moving direction of the moving body 1 at each distance measuring timing by the self-position predicting unit 103 of the position estimation device 100, and wirelessly in the moving body 1. A part of the moving body 1 exists between the wireless communication device 111 and each of the anchors 2a to 2d at each distance measurement timing based on the position where the communication device 111 is placed, the shape of the moving body 1, and the position information of the anchor 2. Judgment is made by calculating whether or not. Since wireless communication with the anchor 2 determined to be in the non-line-of-sight state (NLOS) causes a loss of time resources, wireless communication may not be performed. In some cases, wireless communication itself is possible even for the anchor 2 in the non-line-of-sight state (NLOS).

Here, the self-position prediction method of the moving body 1 in the self-position prediction unit 103 of the position estimation device 100 will be described below.
As one of the methods for predicting the self-position of the moving body 1, there is an estimation method for determining the time point of the next ranging timing using the past position information of the moving body 1. That is, when the moving body 1 is measured at a certain distance measurement timing (one distance measurement timing), the self-position of the moving body 1 already measured at the past distance measurement timing before the one distance measurement timing and This is a method of predicting the self-position and the moving direction of the moving body 1 at one distance measuring timing based on the self-position information such as the moving direction.

Specifically, among the past distance measurement timings, linear prediction is performed for the current self-position and movement direction of the moving body 1 from the information on the self-position and the moving direction of the moving body 1 at a plurality of distance-finding timings. It is possible to predict the self-position and the moving direction of the moving body 1 at the current ranging timing after a certain period of time has passed from the past ranging timing.

By predicting the self-position and the moving direction of the moving body 1 at the next distance measuring timing, the anchor 2 in the non-line-of-sight state (NLOS) is predicted among the anchors 2, and the distance measuring result by the anchor 2 is obtained. It is also possible to process that it is not used for self-position estimation.

As another method of the position prediction method, the current self-position and movement of the moving body 1 include the information output from the inertial measurement unit 113 in addition to the above-mentioned information on the self-position and the moving direction of the moving body 1 in the past. There is a method of predicting the direction. In such a self-position prediction method, based on the motion model of the inertial state of the moving body 1, the self-position and the moving direction of the moving body 1 at the current distance measuring timing are predicted by the output from the inertial measurement unit 113.

The distance measurement timing managed by the distance measurement timing management unit 102 will be described below. The distance measurement timing may be managed based on the number of anchors for which the line-of-sight state (LOS) is established. When the self-position and the moving direction of the moving body 1 are calculated based on the principle of error minimization, it can be seen that the greater the number of anchors for which the line-of-sight state (LOS) is established, the better the accuracy of the distance in distance measurement. Therefore, the distance measurement timing management unit 102 manages the distance measurement timing of the wireless communication device 111 so that the number of anchors for which the line-of-sight state (LOS) is established at the distance measurement timing is as large as possible.

In order to measure the distance between the anchors 2a to 2d and the wireless communication device 111, the wireless communication between the anchors 2a to 2d and the wireless communication device 111 has a reception signal strength of a certain level or higher and reception. Signal quality is required. When the line-of-sight state (LOS) is established, basically, the communication distance between each anchor 2a to 2d and the wireless communication device 111 often becomes a problem. Therefore, the distance measurement timing management unit 102 manages the distance measurement timing so that the average of the distances from the wireless communication device 111 to all the anchors 2a to 2d that can be communicated becomes small.

In order to judge whether the received signal strength and the received signal quality are above a certain level, it is judged that the received signal strength and the received signal quality are above a certain level when compared with the preset threshold value. You may.

As for the distance measurement timing, a plurality of distance measurement timing candidates may be calculated by each of the above methods, and the optimum distance measurement timing may be selected and applied from each candidate. Further, when the line-of-sight state determination unit 104 determines that the line-of-sight state is generally poor with each anchor 2, that is, it is in the non-line-of-sight state (NLOS), the distance measurement timing management unit 102 determines the distance measurement timing. By changing it repeatedly, the optimum ranging timing may be found.

The determination result in the line-of-sight state determination unit 104 is output to the self-position estimation unit 105. The self-position estimation unit 105 estimates the self-position of the mobile body 1 based on the distance measured between the three anchors 2a, 2c, and 2d for which the line-of-sight state (LOS) has been established and the radio communication device 111. That is, the distance is not measured between the anchor 2b in the non-line-of-sight state (NLOS) and the wireless communication device 111, or even if the distance is measured, the distance measurement result is not used for self-position estimation of the moving body 1. To.

In the state schematically shown in FIG. 6, when the self-position estimation is performed including the distance measurement result in the state where the received signal quality is deteriorated such as the anchor 2b which is the non-line-of-sight state (NLOS), FIG. Due to the influence of multipath caused by the shielded object 51 as shown, the calculation may be performed with incorrect distance data, and the accuracy of self-position estimation may decrease. Therefore, in the position estimation device 100 according to the first embodiment, the accuracy of the position estimation of the moving body 1 is prevented from being lowered by not using the distance measurement result using the anchor 2 in the non-line-of-sight state (NLOS). is doing.

FIG. 7 shows a position estimation method in which one wireless communication device 111 is attached to a corner portion of the mobile body 1, and the radios mounted on the anchors 2a to 2d installed on the roadside of the road and the mobile body 1 are mounted on the mobile body 1. It is a schematic diagram which shows the situation that the wireless communication state with a communication device 111 changes with the movement of a moving body 1.

Along the road (not shown) on which the moving body 1 moves, four anchors 2a to 2d capable of wireless communication are installed on the road side of the road. The position information of each anchor 2a to 2d is known.

FIG. 7 shows points P and Q among the positions of the moving body 1 that change along with the movement of the moving body 1. That is, the moving body 1 first passes through the P point and then moves to the Q point as it moves.

When the moving body 1 is located at the point P, the line-of-sight state (LOS) is set between the three anchors 2a, 2c and 2d of the four anchors 2a to 2d and the wireless communication device 111 at the point P. Establish. This is because there is no object that blocks wireless communication, such as a shielding object 51, between the anchors 2a, 2c, and 2d and the wireless communication device 111.

On the other hand, the line-of-sight state (LOS) has not been established between the anchor 2b and the wireless communication device 111. That is, it is in a non-line-of-sight state (NLOS). The reason for this is that a part of the moving body 1 exists between the anchor 2b and the wireless communication device 111 as an object that shields the wireless communication.

When the moving body 1 moves from the P point to the Q point, at the Q point, there is a line-of-sight between the three anchors 2a, 2b, and 2c of the four anchors 2a to 2d and the wireless communication device 111. While the state (LOS) is established, the line-of-sight state (LOS) is not established between the anchor 2d and the wireless communication device 111, that is, the non-line-of-sight state (NLOS).

That is, the anchor 2d whose line-of-sight state (LOS) has been established by the movement of the moving body 1 from the point P to the point Q has changed to the non-line-of-sight state (NLOS) and the anchor 2b which has been in the non-line-of-sight state (NLOS). Changes so that the line-of-sight state (LOS) is established. The reason for this is that as the moving body 1 moves, that is, as the self-position changes, the anchors shielded by the moving body 1 also change between the anchors 2a to 2d and the wireless communication device 111. Therefore, it is important at which distance measurement timing the distance is measured while the moving body 1 is moving. The distance measurement timing is managed by the distance measurement timing management unit 102 so as to be an appropriate timing.

Further, if the self-position and the moving direction of the moving body 1 at the next ranging timing can be predicted at the time when the moving body 1 is located at the P point, the self-position of the moving body 1 can be estimated with higher accuracy. Therefore, the self-position prediction unit 103 predicts the self-position and the moving direction of the moving body 1 at the next distance measuring timing when the moving body 1 is located at the P point.

FIG. 8 shows a position estimation method in which one wireless communication device 111 is attached to a corner portion of the mobile body 1, and the radios mounted on the anchors 2a to 2e installed on the roadside of the road and the mobile body 1 are mounted on the mobile body 1. It is a schematic diagram which shows the situation that the wireless communication state with a communication device 111 changes with the movement of a moving body 1. The difference from FIG. 7 is that the number of anchors in FIG. 7 was 4, but in FIG. 8, the anchor 2e installed farther away is also added, and the total number of anchors is increased by 1 to 5. It is a point.

Five anchors 2a to 2e capable of wireless communication are installed on the road side of the road along the road (not shown) on which the mobile body 1 moves. The position information of each anchor is known.

FIG. 8 shows points P and Q among the positions of the moving body 1 that change along with the movement of the moving body 1. That is, the moving body 1 first passes through the P point and then moves to the Q point as it moves.

When the moving body 1 is located at the point P, the line-of-sight state (LOS) is obtained between the four anchors 2a, 2c, 2d and 2e of the five anchors 2a to 2e at the point P and the wireless communication device 111. Is established. This is because there is no object that blocks wireless communication, such as a shielding object 51, between the anchors 2a, 2c, 2d, and 2e and the wireless communication device 111.

On the other hand, since a part of the mobile body 1 exists between the anchor 2b and the wireless communication device 111, the line-of-sight state (LOS) has not been established, that is, it is in the non-line-of-sight state (NLOS).

When the moving body 1 moves from the P point to the Q point, the line-of-sight state between the four anchors 2a, 2b, 2c and 2e of the five anchors 2a to 2e at the Q point and the wireless communication device 111. While (LOS) is established, there is a non-line-of-sight state (NLOS) between the anchor 2d and the wireless communication device 111.

In the installed state of the anchors 2a to 2e as shown in FIG. 8, since the number of anchors that the mobile body 1 can wirelessly communicate with increases, the self-position of the mobile body 1 can be estimated with higher accuracy. That is, if the line-of-sight state (LOS) is established even for the anchor 2e that is far away in terms of distance, the anchors 2b and 2d that are in the non-line-of-sight state (NLOS) are measured at points P and Q, respectively. It is not necessary to use the distance result to estimate the self-position of the moving body 1. This is because the self-position of the moving body 1 can be estimated with high accuracy by using only the distance measurement results of the remaining four anchors for which the line-of-sight state (LOS) has been established.

FIG. 9 shows a position estimation method in which one wireless communication device 111 is attached to a corner portion of the mobile body 1, and the radios mounted on the anchors 2a to 2e installed on the roadside of the road and the mobile body 1 are mounted on the mobile body 1. It is a schematic diagram which shows the situation that the wireless communication state with a communication device 111 changes with the movement of a moving body 1. The installation state of each of the anchors 2a to 2e is the same as in the case of FIG. 8, but since the wireless communication device 111 is mounted in the corner portion behind the moving body 1 in the traveling direction, the anchors 2a to 2a are installed. The wireless communication state between the 2e and the wireless communication device 111 is different from that in FIG.

The points where the five anchors 2a to 2e installed on the roadside of the road and the moving body 1 first pass the point P and then move to the point Q as the moving body 1 moves are the same as in the case of FIG. 8 above. The same is true.

When the mobile body 1 is located at the P point, the line-of-sight state (LOS) is set between the three anchors 2a, 2b and 2d of the five anchors 2a to 2e and the wireless communication device 111 at the P point. On the other hand, the line-of-sight state (LOS) is not established because a part of the mobile body 1 exists between the two anchors of the anchors 2c and 2e and the wireless communication device 111, that is, the non-line-of-sight state (that is, the non-line-of-sight state (LOS)). NLOS).

When the moving body 1 moves from the P point to the Q point, the line-of-sight state (LOS) is obtained between the three anchors 2b, 2d and 2e of the five anchors 2a to 2e at the Q point and the wireless communication device 111. ) Is established, while the two anchors of the anchors 2a and 2c and the wireless communication device 111 are in a non-line-of-sight state (NLOS).

Even if the positions of the anchors arranged on the road side of the road are the same, the line-of-sight state (LOS) may be different depending on the position of the wireless communication device 111 mounted on the moving body 1 and the moving direction of the moving body 1. The number of anchors to be established may be reduced.

Of the plurality of anchors 2, when the total number of anchors 2 having a line-of-sight state (LOS) established for the wireless communication device 111 is equal to or more than a preset number, the anchors 2 having a line-of-sight state (LOS) established. The self-position of the moving body 1 may be estimated based only on the distance measurement result for the moving body 1. In this case, the preset number or more typically means three or more, which is the minimum required in the three-sided survey position estimation method.

As can be seen from the above cases, it is important at which distance measurement timing the distance is measured when the moving body 1 is moved. The distance measurement timing is managed by the distance measurement timing management unit 102 so as to be an appropriate timing. Further, if the line-of-sight state (LOS) is established, the distance measurement result for the anchor far away from the distance such as the anchor 2e is also to secure the total number of anchors required for self-position estimation of the moving body 1. Available.

As an example of the above-mentioned appropriate timing, when the preset conditions are satisfied at each distance measurement timing, it is used for distance measurement from each anchor 2 as the self-position of the moving body 1 changes. It is possible to determine the combination of anchors 2 to be used.

As an example of the above-mentioned preset conditions, the total number of anchors 2 for which the line-of-sight state (LOS) has been established is equal to or greater than the preset number within a predetermined range centered on the self-position of the moving body 1. For example, there are conditions such as three or more.

The method of estimating the self-position of the moving body 1 based on the distance measurement result between the anchors 2a to 2e installed on the roadside of the road and the wireless communication device 111 mounted on the moving body 1 has been described above. It is also possible to estimate the self-position of the mobile body 1 by using a server (not shown) that organizes each anchor placed on the roadside of the road via wireless communication.

As described above, in the position estimation device, the automatic driving system, and the position estimation method according to the first embodiment, the self-position of the moving body is estimated using only the distance measurement result by the anchor whose line-of-sight state (LOS) is established. Even when there is an anchor in the non-line-of-sight state (NLOS) due to a part of the moving body, the self-position estimation of the moving body can be realized with high accuracy.

Embodiment 2.
In the position estimation method according to the first embodiment, the self-position of the moving body 1 is estimated using only the distance measurement result by the anchor whose line-of-sight state (LOS) is established. On the other hand, in the position estimation method according to the second embodiment, in addition to the anchor in which the line-of-sight state (LOS) is established, the distance measurement result from the anchor in the non-line-of-sight state (NLOS) is also used to use the self of the moving body 1. Estimate the position.

In order to use the distance measurement result from the anchor 2 in the non-line-of-sight state (NLOS), there is a method of weighting each distance data measured between each anchor 2 and the wireless communication device 111. Suitable. Here, the weighting means that when the self-position of the moving body 1 is estimated using the range-finding data measured for each anchor 2, each range-finding data is reflected in the calculation of the self-position estimation. Represents the degree.

That is, the weighting coefficient for the distance measurement result from the anchor 2 in which the line-of-sight state (LOS) is established is increased, while the weighting coefficient for the distance measurement result from the anchor 2 in the non-line-of-sight state (NLOS) is adjusted to be small. do.

An example of self-position estimation using a weighting coefficient is given.
When the estimated value of the self-position of the moving body 1 is expressed in three-dimensional coordinates, it is assumed to be (x _s , y _s , z _s ). On the other hand, the anchor A ₁ is (X ₁ , Y ₁ , Z ₁ ), and the anchor A ₂ is the installation position of each of the _N anchors A ₁ , A ₂ , ..., AN represented by the three-dimensional coordinates. Is (X ₂ , Y ₂ , Z ₂ ), ..., The nth anchor _An is (X _n , Y _n , Z _n ). Further, the distance measured between each of the _N anchors A ₁ , A ₂ , ..., AN and the wireless communication device 111 is the distance d ₁ for the anchor A ₁ and the distance d for the anchor A ₂ . ₂ , ..., The nth anchor _An is a distance d _n .

In order to estimate the self-position of the moving body 1 when the line-of-sight state (LOS) is established for all N anchors, the least squares method is applied and the square expressed by the following equation (1) is applied. The error function F (x _s , y _s , z _s ) may be minimized. The calculated self-position of the moving body 1 is estimated as the final self-position of the moving body 1.

When the line-of-sight state (LOS) is established for some of the _N anchors An, while the line-of-sight state (LOS) is established for the remaining anchors, the following equation (2) is used. As shown above, a weighting coefficient _Wn is used for each of the _N anchors An. For anchors with an established line-of-sight state (LOS), the weighting factor _Wn is set large, while for anchors with a non-line-of-sight state (NLOS), the weighting factor _Wn is set small. ..

If it is desired to estimate the self-position of the moving body 1, the least squares method may be applied to minimize the function F (x _s , y _s , z _s ) of the square error expressed by the equation (2). The calculated self-position of the moving body 1 is estimated as the final self-position of the moving body 1.

Regarding the set value of each weighting coefficient _Wn depending on the line-of-sight state (LOS) and the non-line-of-sight state (NLOS), basically it should be set to 1.0 for the anchor for which the line-of-sight state (LOS) has been established. It's fine. On the other hand, for the anchor in the non-line-of-sight state (NLOS), the weighting coefficient _Wn may be set to a fixed value, for example, 0.2. However, the above is an example of the setting value of the weighting coefficient _Wn , and the optimum weighting coefficient _Wn may be appropriately set according to the situation.

Alternatively, using the information of the error variance σ ² when the part of the moving body 1 is in the non-line-of-sight state (NLOS) in advance, the weighting coefficient W _n is set to 1 / (σ) so as to be inversely proportional to the error variance. ² ) may be set.

In addition to the above method for setting the weighting coefficient W _n , there are cases where the moving body 1 is completely shielded and cases where only a small part is shielded. Therefore, the weighting coefficient W _n is determined by the shielding rate by the moving body 1. May be changed dynamically. Further, if there are other factors such as low reception signal strength of wireless communication, N anchors A are each regardless of whether the anchor 2 is in the line-of-sight state (LOS) or the non-line-of-sight state (NLOS). The weighting coefficient W _n of _n may be added separately.

As an example of setting the weighting coefficient _Wn , a case where four anchors exist within a communicable range will be described.
Of the _four anchors, the anchors A1 and A3 are in the line _- of-sight state (LOS), while the anchors _A2 and A4 are in the non-line _- of-sight state (NLOS). The weighting coefficients of each anchor are W ₁ for anchor A ₁ , W ₂ for anchor A ₂ , W ₃ for anchor A ₃ , and W ₄ for anchor A ₄ .

The weighting coefficients W2 and W4 of the anchors _A2 and A4 in the non _- line _- of _- sight state (NLOS) _are the weighting coefficients W1 and W3 of the anchors A1 and A3 in which the line _- of _- sight state ₍ LOS) is established. It will be a smaller value than. That is, the relationship of the following equation (3) is obtained.
W ₂ , W ₄ <W ₁ , W ₃ (3)

For example, the weighting coefficients W1 and W3 for the distance measurement results from the anchors A1 and A3 for which the line _- of _- sight state (LOS) has been established are set to _{1.0 ,} and the anchors _A2 and A4 that are in the non-line-of _- sight state (NLOS) are set to 1.0. The weighting coefficients W ₂ and W ₄ for the distance measurement result from are set to be as small as 0.2. In short, each weighting coefficient may be appropriately set according to the degree of reflection on the estimation of the self-position of the moving body 1.
The above is the description of the self-position estimation method using the weighting coefficient.

When the received signal quality on the wireless communication device 111 side in wireless communication between the anchor 2 in the non-line-of-sight state (NLOS) and the wireless communication device 111 is equal to or higher than a preset threshold value, it is estimated by the self-position estimation unit 105. The reliability of the self-position information of the moving body 1 may be lowered, or it may be determined that the self-position information of the moving body 1 is incorrect.

If good reception signal quality is obtained even though an attempt is made to wirelessly communicate with the anchor 2 that should be in the non-line-of-sight state (NLOS), the wrong self is used when determining the non-line-of-sight state (NLOS). This is because there is a high possibility that information such as the position and the moving direction was used. In such a case, information such as an erroneous self-position and a moving direction may be removed, but information such as a past moving direction may also be removed.

The position estimation method according to the second embodiment will be described with reference to the flowchart of FIG. The feature of the position estimation method according to the second embodiment is that the above-mentioned weighting coefficient _Wn is applied, but the flowchart also includes the position estimation method described in the first embodiment.

In step ST201, the shape of the mobile body 1 and the installation position of the wireless communication device 111 mounted on the mobile body 1 are measured in advance. The measured information is managed by the mobile information management unit 106.

Step ST202 represents a self-position estimation loop. In step ST203, wireless communication is performed between each anchor 2 installed on the road side of the road along the road as shown in FIG. 3 and the wireless communication device 111, and the position information of each anchor 2 is acquired.

In step ST204, when measuring the distance, it is determined whether or not there is self-position information of the moving body 1 based on the data measured at the past distance measurement timing.

If there is no self-position information of the moving body 1 measured at the past distance measurement timing in step ST204, distance measurement is performed between all communicable anchors 2 and the wireless communication device 111 in step ST205. When wireless communication is performed between each anchor 2 and the wireless communication device 111, whether or not the line-of-sight state (LOS) is established between each anchor 2 and the wireless communication device 111 in the line-of-sight state determination unit 104. To judge.

In step ST206, the self-position estimation unit 105 estimates the self-position of the moving body 1 based on the above-mentioned distance measurement result. In the self-position estimation, the weight is reduced, that is, the weighting coefficient is reduced for the distance measurement result with the anchor 2 determined to be in the non-line-of-sight state (NLOS).

In step ST207, it is determined whether or not the communication quality of the anchor 2 determined to be in the non-line-of-sight state (LOS) is not established, that is, the non-line-of-sight state (NLOS) is good. The communication quality of the anchor 2 is determined exclusively by the quality of the received signal received by the wireless communication device 111. If the communication quality of the anchor 2 in the non-line-of-sight state (NLOS) is not good, the self-position estimation is repeated by the self-position estimation loop in step ST208.

On the other hand, if the communication quality of the anchor 2 determined to be in the non-line-of-sight state (NLOS) in step ST207 is good, the reliability of the self-position information of the moving body 1 calculated based on the distance measurement is lowered in step ST214. Let me. In addition to lowering the reliability of the self-position information, the self-position information of the moving body 1 may be removed as an error.

If the self-position information of the moving body 1 measured at the past distance measurement timing exists in step ST204, the self-position is estimated from the distance measurement results at the plurality of distance measurement timings made in the past in step ST209. ..

In step ST210, the number of anchors 2 for which the line-of-sight state (LOS) has been established and the distance to the wireless communication device 111 are calculated at a plurality of distance measurement timings.

In step ST211th, the distance measurement timing with the best conditions for the line-of-sight state (LOS) is selected from the plurality of distance measurement timings described above.

In step ST212, it is determined whether or not the total number of anchors 2 for which the line-of-sight state (LOS) has been established is equal to or greater than the number of distance-measurable numbers required to estimate the self-position of the moving body 1. For example, when the total number of anchors 2 is 3 or more, the number may be equal to or more than the number that enables distance measurement. If the total number of anchors 2 for which the line-of-sight state (LOS) has been established in step ST212 is less than the required number of distance-measurable anchors, the above-mentioned step ST205 or lower is executed.

On the other hand, if the total number of anchors 2 for which the line-of-sight state (LOS) has been established in step ST212 satisfies the required number of distance-measurable anchors, only for the anchors 2 for which the line-of-sight state (LOS) has been established in step ST213. Distance measurement is performed with the wireless communication device 111, and the above-mentioned step ST206 or lower is executed.
The above is the position estimation method according to the second embodiment.

According to the position estimation method according to the second embodiment, by applying the weighting coefficient _Wn to the distance measurement data by the anchor determined to be in the non-line-of-sight state (NLOS), it is utilized as a part of the distance measurement data. Therefore, it has the effect that the self-position estimation of the moving body can be realized with higher accuracy.

Embodiment 3.
The position estimation device and the position estimation method according to the third embodiment estimate the self-position based on the distance measurement results from a plurality of wireless communication devices mounted on one moving body. 11 and 12 show the case where one wireless communication device is attached to each corner on the front left side and the rear right side of the mobile body 1, and a total of two wireless communication devices 111a and 111b are mounted. It is a schematic diagram which shows the position estimation method.

When a plurality of wireless communication devices are mounted on one mobile body, the state of wireless communication between each anchor and each wireless communication device is wireless communication even if measured at the same distance measurement timing. It may be different for each machine.

Therefore, in the position estimation device 100 according to the third embodiment, it is decided to measure the distance at the optimum distance measurement timing for each of the plurality of wireless communication devices. The range-finding timing management unit 102 manages each of the plurality of wireless communication devices so that the range-finding timing is optimal.

FIG. 11 shows distance measurement by the wireless communication device 111a mounted in front of the mobile body 1, and FIG. 12 shows distance measurement by the wireless communication device 111b mounted in the rear of the mobile body 1. As the moving body 1 moves, it first passes through the point P and then moves to the point Q.

When the moving body 1 is located at the point P, as shown in FIG. 11, for the wireless communication device 111a mounted in front of the moving body 1, at the point P, of the five anchors 2a to 2e, the anchor 2a, A line-of-sight state (LOS) is established between the four anchors 2c, 2d and 2e and the wireless communication operator 111a. On the other hand, since a part of the mobile body 1 exists between the anchor 2b and the wireless communication device 111a, it is in a non-line-of-sight state (NLOS).

Similarly, when the moving body 1 is located at the P point, the wireless communication device 111b mounted behind the moving body 1 is an anchor among the five anchors 2a to 2e at the P point, as shown in FIG. A line-of-sight state (LOS) is established between the three anchors 2a, 2b and 2d and the radio operator 111b. On the other hand, since a part of the mobile body 1 exists between the two anchors 2c and 2e and the wireless communication device 111b, the line-of-sight state (LOS) has not been established. That is, it is in a non-line-of-sight state (NLOS).

As described above, even when the mobile body 1 is located at the point P, the two wireless communication devices 111a and 111b mounted on the mobile body 1 have anchors 2 for which a line-of-sight state (LOS) is established, respectively. The number is 4 in the wireless communication device 111a, whereas it is 3 in the wireless communication device 111b.

When the moving body 1 moves from the point P and is located at the point Q, the wireless communication device 111a mounted in front of the moving body 1 has five anchors 2a to 2e at the point Q as shown in FIG. Of these, a line-of-sight state (LOS) is established between the four anchors 2a, 2b, 2c and 2e and the wireless communication device 111a. On the other hand, since a part of the mobile body 1 exists between the anchor 2d and the wireless communication device 111a, it is in a non-line-of-sight state (NLOS).

Similarly, when the moving body 1 is located at the Q point, the wireless communication device 111b mounted behind the moving body 1 is an anchor among the five anchors 2a to 2e at the Q point, as shown in FIG. A line-of-sight state (LOS) is established between the three anchors 2b, 2d and 2e and the wireless communication operator 111b. On the other hand, since a part of the mobile body 1 exists between the two anchors 2a and 2c and the wireless communication device 111b, the line-of-sight state (LOS) has not been established. That is, it is in a non-line-of-sight state (NLOS).

In this way, even when the mobile body 1 is located at the Q point, in the two wireless communication devices 111a and 111b mounted on the mobile body 1, the line-of-sight state (LOS) of the anchor 2 is established, respectively. The number is 4 in the wireless communication device 111a, whereas it is 3 in the wireless communication device 111b.

To summarize the above, when the mobile body 1 is located at the point P, the number of anchors for which the line-of-sight state (LOS) is established is 4 for the wireless communication device 111a and 3 for the wireless communication device 111b, while the number of anchors is 3. When the mobile body 1 is located at the Q point, the number is 4 in the wireless communication device 111a and 3 in the wireless communication device 111b.

As the moving body 1 moves, the anchor for which the line-of-sight state (LOS) is established changes, that is, dynamically changes. Further, when a plurality of wireless communication devices are mounted on the mobile body 1, as the mobile body 1 moves, the anchor for which the line-of-sight state (LOS) is established is dynamically changed for each wireless communication device.

Therefore, in the position estimation device 100 according to the third embodiment, the distance is measured at the optimum distance measurement timing for each of the plurality of wireless communication devices. In the example shown in FIGS. 11 and 12, the wireless communication device 111a and the wireless communication device 111b are managed by the distance measurement timing management unit 102 so as to measure the distance at different distance measurement timings.

Explaining with reference to FIGS. 11 and 12, when the mobile body 1 is located at the point P, the wireless communication device 111a mounted in front of the mobile body 1 and the anchors 2a and 2c for which the line-of-sight state (LOS) is established are established. When distance measurement is performed between the four anchors of 2, 2d and 2e (distance measurement timing A) and the moving body 1 moves from the P point and is located at the Q point, the wireless communication device 111a and the line-of-sight state (LOS). ) Is established between the four anchors 2a, 2b, 2c and 2e (distance measurement timing B), the self-position of the moving body 1 can be estimated in the optimum combination at each point. ..

When a plurality of wireless communication devices are configured for one mobile body 1 as described above, distance measurement by the plurality of wireless communication devices is performed when the preset conditions are satisfied at each distance measurement timing. It may be possible to determine the order of. By determining the order of the anchors for distance measurement so that the distance from the anchor for which the line-of-sight state (LOS) is established is short, it is possible to stably estimate the self-position.

The preset conditions described above are, for example, between the anchor 2 having an established line-of-sight state (LOS) and the wireless communication device 111 within a predetermined range centered on the self-position of the moving body 1. There are cases where the distance is less than or equal to the preset distance.

As shown in FIGS. 11 and 12, two diagonally located corners of the mobile body 1 are preferable as positions for placing the two wireless communication devices 111a and 111b on the mobile body 1. Is. This is because wireless communication with each anchor installed on both sides of the road can be efficiently performed.

As described above, in the position estimation device and the position estimation method according to the third embodiment, the accuracy of self-position estimation is maintained high by dynamically switching the distance measurement timings of a plurality of wireless communication devices so as to be advantageous for distance measurement. It becomes possible.

Embodiment 4.
In the position estimation method according to the second embodiment, in step ST201 of the flowchart shown in FIG. 10, the shape of the moving body 1 and the wireless communication device 111 mounted on the moving body 1 are performed before the start of distance measurement by the position estimation device 100. The installation position of the anchor 2 was measured (step ST201), and at the same time as the distance measurement was started, wireless communication was performed with the anchor 2 to acquire the installation position of the anchor 2 (step ST203). On the other hand, in the position estimation method according to the fourth embodiment, the information necessary for self-position estimation is started by a flow different from the flow shown in FIG. 10 of the second embodiment and by the flow shown in FIG. It is characterized by the fact that it is acquired in advance.

The above-mentioned information necessary for self-position estimation is the shape of the mobile body 1, the installation position of the wireless communication device 111, and the installation position of the anchor 2. Since the shape of the mobile body 1 and the installation position of the wireless communication device 111 were acquired before the start of distance measurement in the flowchart shown in FIG. 10, the distance measurement is newly performed by the position estimation method according to the fourth embodiment. The information acquired before the start is the installation position of the anchor 2.

In the position estimation method according to the fourth embodiment, the installation positions of all the anchors 2 on the path on which the moving body 1 moves are stored in, for example, inside the position estimation device 100 before the distance measurement is started. As described in the first embodiment, when the self-position of the mobile body 1 is calculated by the server connected to the anchor 2 and the wireless communication device 111 via the Internet or the like, the mobile body 1 moves on the path. The installation positions of all the anchors 2 may be stored in the server in advance.

FIG. 13 shows a flowchart of acquisition of information necessary for the above-mentioned self-position estimation among the position estimation methods according to the fourth embodiment. Hereinafter, the flowchart of FIG. 13 will be described. In step ST301, the shape of the moving body 1 is measured in advance. In step ST302, the installation position of the wireless communication device 111 mounted on the mobile body 1 is measured in advance. In step ST303, as described above, the installation positions of all anchors 2 on the path for distance measurement are acquired.

When the flowchart shown in FIG. 13 is executed, steps ST201 and ST203 of the flowchart of FIG. 10 are omitted in the subsequent distance measurement.

As described above, in the position estimation method according to the fourth embodiment, since the information necessary for self-position estimation is acquired in advance before the start of distance measurement, the self-position estimation of the moving body can be realized at higher speed while maintaining high accuracy. It has the effect of being able to do it.

In the configuration of the position estimation device 100 according to the above-described first to third embodiments, the position estimation device 100 and the automatic operation system 200 are described as functional blocks, but the position estimation device 100 and the automatic operation system 200 are stored. FIG. 14 shows an example of the configuration as hardware. The hardware 800 includes a processor 801 and a storage device 802. Although not shown, the storage device 802 includes a volatile storage device such as a random access memory and a non-volatile auxiliary storage device such as a flash memory.

Further, the auxiliary storage device of the hard disk may be provided instead of the flash memory. The processor 801 executes the program input from the storage device 802. In this case, the program is input from the auxiliary storage device to the processor 801 via the volatile storage device. Further, the processor 801 may output data such as a calculation result to the volatile storage device of the storage device 802, or may store the data in the auxiliary storage device via the volatile storage device.

The present disclosure describes various exemplary embodiments and examples, although the various features, embodiments, and functions described in one or more embodiments are those of a particular embodiment. It is not limited to application, but can be applied to embodiments alone or in various combinations.

Therefore, innumerable variations not exemplified are envisioned within the scope of the techniques disclosed herein. For example, it is assumed that at least one component is modified, added or omitted, and further, at least one component is extracted and combined with the components of other embodiments.

1 Moving object, 2, 2a, 2b, 2c, 2d, 2e, 2s anchor, 51 Shielding object, 52 Reflecting object, 100 Position estimation device, 101 Distance measurement unit, 102 Distance measurement timing management unit, 103 Self-position prediction unit, 104 Line-of-sight status determination unit, 105 Self-position estimation unit, 106 Mobile information management unit, 107 Communication function unit, 111, 111a, 111b wireless communication device, 150 control unit, 200 automatic operation system, 800 hardware, 801 processor, 802 Storage device

Claims (18)

A ranging unit that measures the distance between the plurality of anchors and the wireless communication device by wireless communication between the plurality of anchors installed on the roadside of the road and the wireless communication device mounted on the moving body. ,
The distance measurement timing management unit that manages the timing of distance measurement,
A self-position prediction unit that predicts the self-position and movement direction of the moving body at each distance measurement timing, and
Each of the plurality of anchors is based on the shape of the moving body, the position where the wireless communication device is mounted on the moving body, the position information of the anchor, and the result of the position prediction of the moving body by the self-position prediction unit. In either the line-of-sight state in which a part of the moving body does not exist or the non-line-of-sight state in which a part of the moving body exists between the anchor to be distanced and the wireless communication device at each distance measuring timing. The outlook status judgment unit that determines whether or not there is,
A self-position estimation unit that estimates the self-position of the moving body based on the distance measurement result between the anchor determined to be in the line-of-sight state and the wireless communication device.
A position estimator equipped with. In addition to the distance measurement result between the anchor determined to be in the line-of-sight state and the wireless communication device, the self-position estimation unit has the anchor and the wireless communication device determined to be in the non-line-of-sight state. The feature is that the self-position of the moving body is estimated by using the distance-finding result adjusted by the weighting coefficient indicating the degree of reflection on the estimation of the self-position of the moving body. The position estimation device according to claim 1. The weighting coefficient for the distance measurement result from the anchor determined to be in the non-line-of-sight state is set smaller than the weighting coefficient for the distance measurement result from the anchor determined to be in the line-of-sight state. 2. The position estimation device according to claim 2. In the self-position estimation unit, the self-position represented by the three-dimensional coordinates of the moving body is (x _s , y _s , z _s ), and the number of the anchors for which the distance measurement distance is obtained is N, the N pieces. The _nth anchor of the anchor is An, the installation position represented by the three-dimensional coordinates of the anchor _{An is (X n ,} Y _n , _Zn ), and the anchor _An is between the wireless communication device. The self-position of the moving body that minimizes the square error calculated by the following formula is calculated and calculated by the least squares method, where the distance measurement distance is d _n and the weighting coefficient for the anchor _{An is W n .} The position estimation device according to claim 2 or 3, wherein the result is estimated as the final self-position of the moving body.

At the time of one of the distance measurement timings, the self-position prediction unit obtains the self-position information of the moving body measured at a distance measurement timing earlier than the one distance measurement timing. The position estimation device according to any one of claims 1 to 4, further comprising predicting the self-position of the moving body. Claims 1 to 5 further include a mobile body information management unit that manages information on the self-position and movement direction of the mobile body, and position information of the anchor acquired by wireless communication with the wireless communication device. The position estimation device according to any one of the above items. When the total number of the anchors determined to be in the line-of-sight state with respect to the wireless communication device is equal to or more than a preset number, the self-position estimation unit determines that the anchors are in the line-of-sight state. The position estimation device according to claim 1, wherein the self-position of the moving body is estimated based only on the distance measurement result of the distance between the mobile device and the wireless communication device. When the preset conditions are satisfied at each distance measurement timing, the combination of the anchors used for distance measurement is determined from the plurality of anchors according to the change in the self-position of the moving body. The position estimation device according to any one of claims 1 to 7, wherein the position estimation device is characterized. The condition according to claim 8, wherein the total number of the anchors in the line-of-sight state is equal to or more than a preset number within a predetermined range centered on the self-position of the moving body. Position estimation device. 1. The first aspect of the present invention is that when a plurality of the wireless communication devices are configured and preset conditions are satisfied at each distance measurement timing, the order of distance measurement in the plurality of wireless communication devices is determined. The position estimation device according to any one of 7 to 7. The condition is that the distance between the anchor determined to be in the line-of-sight state and the wireless communication device within a predetermined range centered on the self-position of the moving body is equal to or less than a preset distance. The position estimation device according to claim 10, wherein the position estimation device is characterized by the above. When the received signal quality of the wireless communication device in wireless communication is equal to or higher than the threshold value between the anchor determined to be in a non-line-of-sight state and the wireless communication device, the self-position estimation unit estimates the above. The position estimation device according to claim 1, wherein the reliability of the self-position information of the moving body is lowered, or the self-position information of the moving body is removed as an error. The position estimation device according to any one of claims 1 to 12, and the position estimation device.
The wireless communication device mounted on the mobile body and
A mobile body control device that controls the moving body based on the self-position information of the moving body estimated by the position estimation device, and
An automated driving system equipped with. The automatic driving system according to claim 13, wherein the mobile communication device has a plurality of the wireless communication devices mounted on the mobile body. A step of measuring the shape of the moving body and the position of the wireless communication device mounted on the moving body, and
Steps to acquire the position information of multiple anchors installed on the roadside of the road,
A step of predicting the self-position and the moving direction of the moving body at each distance measuring timing,
Based on the shape of the moving body, the position where the wireless communication device is mounted on the moving body, the position information of the anchor, and the result of the position prediction of the moving body, the distance measurement timing of each of the plurality of anchors is In the step of determining whether there is a line-of-sight state in which a part of the moving body does not exist or a non-line-of-sight state in which a part of the moving body exists between the anchor to be distanced and the wireless communication device. When,
A step of measuring the distance between the anchor and the wireless communication device based on the preset distance measurement timing, and
A step of estimating the self-position of the moving body based on the distance measurement result between the anchor determined to be in the line-of-sight state and the wireless communication device, and
Position estimation method including. In the step of estimating the position of the moving body, in addition to the distance measurement result between the anchor determined to be in the line-of-sight state and the wireless communication device, the anchor determined to be in the non-line-of-sight state With respect to the distance measurement result with the wireless communication device, the self-position of the moving body is estimated by using the distance measurement result adjusted by the weighting coefficient indicating the degree of reflection on the estimation of the self-position of the moving body. The position estimation method according to claim 15, wherein the position is estimated. The position estimation method according to claim 15, wherein the position information of the anchor is acquired by wireless communication between the anchor and the wireless communication device. The position estimation method according to claim 15, wherein the position information of the anchor is acquired before the distance measurement is started.

Images