WO2019123558A1

WO2019123558A1 - Self-position estimation system

Info

Publication number: WO2019123558A1
Application number: PCT/JP2017/045664
Authority: WO
Inventors: 尚子高田
Original assignee: 株式会社日立製作所
Priority date: 2017-12-20
Filing date: 2017-12-20
Publication date: 2019-06-27

Abstract

The objective of the present invention is to provide a self-position estimation system for carrying out stable self-position estimation even under conditions in which the environment around external-environment sensors changes rapidly. To achieve this objective, this self-position estimation system for estimating its own position by referring to the output of a plurality of external-environment sensors is provided with: an error index value calculation unit for estimating error index values serving as indexes for the measurement errors of the external-environment sensors, a weight setting unit for setting weights for the external-environment sensors on the basis of the error index values, and a self-position estimation integration unit for estimating a final self-position by using the weights to obtain a weighted average of self-positions estimated on the basis of the output of the external-environment sensors.

Description

Self-position estimation system

The present invention relates to a self-position estimation system that performs self-position estimation with higher accuracy by using a plurality of self-position estimation results based on the output of a self-position measurement sensor in combination.

With the development of machine learning, improvement of processing capability of control devices, and expansion of usage range of big data, expectations for autonomous vehicles and autonomous mobile robots to which these are applied are increasing. Stable self-positioning is essential for autonomous vehicles and autonomous mobile robots to move safely away from their hands. In particular, since high safety is required in an autonomous vehicle, a plurality of self-position measuring sensors (hereinafter simply referred to as "external sensors") are mounted to enhance external recognition performance and improve self-position estimation accuracy. However, since each external sensor has an environment in which the accuracy is lowered, it is necessary to use a plurality of external sensors appropriately combined according to the environment in order to stabilize the self-position estimation accuracy.

For example, in the abstract of Patent Document 1 and FIG. 4, “the first positioning means for positioning the position of the mobile by the instantaneous positioning method based on the observation data of the signal from the satellite according to the present invention And second positioning means for positioning the position of the moving body using the positioning result of the position of the moving body in the past and the velocity information of the moving body, and the positioning result of the first or second positioning means in the past It comprises: third positioning means for positioning the position of a mobile using the obtained variable solution; and control means 60 for selecting positioning means for executing positioning processing among the positioning means, the control means selecting If the reliability of the positioning result of the determined positioning means is lower than a predetermined reference value, another positioning means is selected to execute the positioning process. " Reliability of the external sensor and the variance of the sensor output Calculated from, in the case the reliability of the positioning results of a sensor is lower than a predetermined reference value, techniques for switching to a different sensor is disclosed.

JP 2008-128793 A

However, in the prior art, since the sensor used is switched based on the dispersion of the sensor, if the surrounding environment changes in a short time, etc., there is a possibility that the change in sensor accuracy in real time may not be followed. It may be stable.

Therefore, it is an object of the present invention to provide a self-position estimation system that performs stable self-position estimation even in a situation where the surrounding environment of an external sensor suddenly changes.

In order to solve this problem, the self-position estimation system of the present invention estimates self-position with reference to the outputs of a plurality of external sensors, and an error index value serving as an index of measurement error of each external sensor Using the above weights for the error index value calculation unit to be estimated, the weight setting unit for setting the weight of each external sensor based on each error index value, and the self position estimated based on the output of each external sensor And a self-position estimation control unit for taking a weighted average and estimating a final self-position.

Further, the present invention is intended to estimate a self position by referring to radio waves of a plurality of GPS satellites, and select a GPS receiver which receives the radio waves of the GPS satellites and a GPS satellite whose CN ratio of radio waves is a predetermined value or more. The first satellite selection means, the second satellite selection means for selecting satellites whose PN code correlation of radio waves is unimodal, and the third satellite for selecting satellites having a shielding rate by a surrounding obstacle of a predetermined value or more A GPS satellite comprising: selection means, and a self position estimation unit for estimating a self position based on radio waves of a plurality of selected GPS satellites, and used for self position estimation according to a combination of outputs of the satellite selection means The combination of

According to the present invention, it is possible to provide a self-position estimation system that performs stable self-position estimation even in a situation where the surrounding environment changes in a short time.

FIG. 1 is a functional block diagram of a self-position estimation system according to a first embodiment. 7 shows an example of an external sensor arrangement of an autonomous vehicle equipped with the self-position estimation apparatus of the first embodiment. The schematic diagram of the electromagnetic wave path from a GPS Satellite to a GPS receiver. FIG. 7 is a diagram for explaining the correlation of PN codes observed from radio waves of a satellite A. The figure explaining the correlation of the PN code observed from the electric wave of satellite B. FIG. FIG. 7 is a functional block diagram of a self-position estimation system according to a second embodiment. List of satellite selection logic corresponding to the reception status of the GPS receiver. The flowchart of the satellite selection method of mode 1. The flowchart of the satellite selection method of mode 2. The flowchart of the satellite selection method of mode 3. The flowchart of the satellite selection method of mode 4. Flow chart of satellite selection method of mode 5. Flow chart of the satellite selection method of mode 6. The σR setting logic list corresponding to the reception status of the GPS receiver. The figure which illustrates the possible range of σR ₁ ′ to σR ₅ ′. diagram for explaining a method of setting the value of sigma] R 1 _'. diagram for explaining a method of setting the value of sigma] R 1 _'. diagram for explaining a method of setting the value of sigma] R 2 _'. diagram for explaining a method of setting the value of sigma] R 2 _'. diagram for explaining a method of setting the value of sigma] R 3 _'. diagram for explaining a method of setting the value of sigma] R 3 _'. diagram for explaining a method of setting the value of sigma] R 3 _'. diagram for explaining a method of setting the value of sigma] R 4 _'. A figure explaining how to set a value of σR ₅ '.

Hereinafter, embodiments of the present invention will be described with reference to the drawings. The present invention is not limited to the following embodiments, and various modifications and applications may be included within the scope of the technical concept of the present invention.

The details of the self-position estimation system 100 according to the first embodiment, which estimates the self-position of a mobile unit 200 such as an autonomous vehicle, will be described with reference to FIGS. 1 and 2.

FIG. 1 is a functional block diagram of the self-position estimation system 100. As shown in FIG. As shown here, the self-position estimation system 100 includes a self-position estimation unit 12 and an error index value in addition to an external sensor 11 (for example, a GPS sensor, a camera, a rider, a laser, a wheel speed sensor) that measures the self position. The calculation unit 13, the weight setting unit 14, the self-position estimation generalization unit 15, and the control device 16 are included. Here, the configuration using n external sensors 11 (11a to 11n) and the corresponding self position estimation units 12 (12a to 12n) is exemplified, but the type and number of external sensors may be changed as necessary. And you should choose appropriately. In the same figure, each function expressed by the self position estimation unit 12, the error index value calculation unit 13, the weight setting unit 14, and the self position estimation generalization unit 15 is an auxiliary storage such as a hard disk provided in the self position estimation system 100. This is realized by loading a program recorded in the device into a main storage device such as a semiconductor memory and executing it by an arithmetic device such as a CPU. In the following, such a known operation is appropriately omitted. While explaining.

A method of processing the output of the external sensor 11 in the self-position estimation system 100 will be described using an example in which the first external sensor 11a is a GPS sensor. When GPS information which is the output of the first external sensor 11a is input to the first self-position estimating unit 12a, the first self-position estimating unit 12a estimates the self-position estimation value r ₁ based on the GPS information. . Although this estimated self-position value r ₁ contains an error σ ₁ and deviates from the true self-position r _0, since the error σ ₁ is unknown, the first self-position estimation unit 12 a is true The self position r ₀ can not be determined.

On the other hand, when the GPS information which is the output of the first external sensor 11a is input to the first error index value calculator 13a, the first error index value calculator 13a generates the first external sensor based on the GPS information. An error index value that is an index of the magnitude of the error σ ₁ of 11 a is calculated. When calculating the error index value of the first external sensor 11a, in addition to the GPS information from the first external sensor 11a, another external sensor (the second external sensor 11b in the example of FIG. 1) You may refer to the output of

Then, the first weight setting unit 14a calculates the weight w ₁ (reference strength) of the reliability of the first external sensor 11a based on the error index value calculated by the first error index value calculation unit 13a. Output.

Similarly, self position estimate values r ₂ to r _n and weights w ₂ to w _n are obtained also for the second external sensor 11 b to the n-th external sensor 11 n. Then, the self position estimation generalization unit 15 estimates a final self position estimated value r ^ more emphasizing the large self position estimated value r with a large weight w, using a plurality of self position estimated values r and weights w. , To the control device 16 of the mobile unit 200. Here the estimated r ^, in order to approximate the true self position r _0, by using this r ^, the control device 16, it is possible to more stably control the moving object 200.

Next, details of the external sensor 11 provided in the mobile unit 200 will be described using FIG. As shown here, the mobile unit 200 is provided with a GPS receiver 21, a camera 22, a rider 23 and a wheel speed / steering angle sensor 24 as an external sensor 11 which measures its own position. Moreover, in addition to the external sensor of these self-position measurements, the rainfall sensor 25 which acquires rainfall as external information is also provided.

Below, the calculation method of the self-position estimate and error index value based on the output of each external sensor is outlined.

First, the method of calculating the self-position estimation value r _i in the self-position estimating unit 12 of Kakugaikai sensor. The self-position estimation unit 12 of the GPS sensor 21 receives radio waves from a plurality of GPS satellites (hereinafter referred to as "satellites") flying overhead, calculates the distance to each satellite, and estimates the self-position. The self-position estimation unit 12 of the camera 22 captures an image of the surroundings, compares features (such as a signboard) in the image with a map, and estimates the self-position. The self-position estimation unit 12 of the lidar 23 obtains the three-dimensional object shape of the periphery, compares the feature of the shape with the map as in the camera, and estimates the self-position. The self-position estimation unit 12 of the wheel speed and steering angle sensor 24 obtains the rotational speed and direction of each wheel, integrates the distance and direction of travel of the moving body, and estimates the self position.

Next, a method of calculating the error index value in the error index value calculation unit 13 of each external sensor will be described. The error index value calculation unit 13 of the GPS sensor 21 calculates an error index value from the height, distance, and density of the surrounding obstacle. For example, it is estimated that the error in the self-position estimation by the GPS receiver 21 is larger as the number of obstacles at the position where the elevation angle is high as viewed from the GPS receiver 21 is larger. The error index value calculation unit 13 of the camera 22 calculates an error index value from the degree of coincidence with the map. For example, it is estimated that the error in self-position estimation by the camera 22 is larger as the degree of coincidence between the landmark registered in the map, the type of the observed landmark, and the position is lower. Similarly to the camera, the error index value calculation unit 13 of the lidar 23 estimates that the lower the degree of coincidence of the observation result with the landmark on the map, the larger the error in self-position estimation by the lidar. The error index value calculation unit 13 of the wheel speed and steering angle sensor 24 estimates that the error in self position estimation by the wheel speed and steering angle sensor is larger as the deviation of the observation result of each wheel is larger.

Subsequently, a method of calculating the final self position estimated value r ^ in the self position estimation generalization unit 15 will be described. Prior to the processing by the self position estimation generalization unit 15, the weight setting unit corresponding to each external sensor assigns a weight w proportional to the reciprocal of each error index value to the self position estimation value r. Then, the self-position estimation generalization unit 15 calculates a weighted average using a plurality of self-position estimate values r and weights w to estimate a final self-position estimate value r ^. This makes it possible to minimize the error of the self-position estimation result for the following reasons.

For example, an error σ _i is generally included in the self-position estimation value r _i based on the output of the i-th external sensor 11 _i , and the following equation is established with the true self-position r ₀ .

Now, n external sensors are mounted, and the weight w _i indicating the likelihood is added to the self position estimate r _i based on each output, and the final self position estimate is obtained by taking a weighted average Consider calculating r ^ by the following equation.

Here, if w _i is taken so that _Σ w _i = n, it is the final error

The minimum value of can be expressed by the following formula from the formula of additive geometric mean.

Since this condition holds true in the case of σ ₁ · w ₁ = σ ₂ · w ₂ = ... = σ _n · w _n

By setting the weight w _i to be, it is possible to minimize the error of the final self-position estimate r ^.

Even if the reliability of each external sensor suddenly changes due to a rapid change of the surrounding environment, the final self estimated position value r ^ The true self position r ₀ can be quickly approximated. Therefore, the stability of autonomous control can be improved by controlling the mobile unit 200 using the final self-position estimation value r ^ obtained by the self-position estimation generalization unit 15 of this embodiment.

Next, the self-position estimation system according to the second embodiment of the present invention, which estimates self-position mainly from GPS information, will be described using FIG. 3 to FIG. The same points as in the first embodiment will not be repeatedly described.

In the first embodiment, as a method of calculating the error index value of the GPS receiver 21, a simple method has been exemplified in which the error is estimated to increase as the number of obstacles increases at a position where the elevation angle is high as viewed from the GPS receiver 21. In the embodiment, a method of using the CN ratio indicating the magnitude of a signal (carrier) with respect to noise and the PN code information to be collated to estimate the distance to the satellite is used to calculate the error index value of the GPS receiver 21. explain.

When self-position estimation is performed using a GPS system, it is necessary to receive radio waves from four or more satellites in principle. Since the self-position estimation accuracy is affected by the geometric arrangement of each satellite and the distance estimation accuracy with each satellite, the self-position estimation accuracy is improved by excluding the bad satellites. it can.

Here, the geometrical arrangement of each satellite can be evaluated by calculating and referring to the positional accuracy deterioration degree DOP (Dilution of Precision). The DOP is an index calculated from the arrangement of each satellite, and indicates that the smaller the DOP is, the higher the self-position estimation accuracy is.

Moreover, the distance estimation accuracy with each satellite can be evaluated based on the influence of the surrounding environment on radio waves. The influence of the surrounding environment on the quality of the radio wave received by the GPS receiver 21 will be described with reference to FIGS.

FIG. 3 is a schematic view in which radio wave paths from satellites to the GPS receiver 21 when the GPS receiver 21 is located in a valley of a building are classified into four modes. As shown here, the radio wave path from satellite A is composed of only direct waves (solid line), and the radio wave path from satellite B is composed of multipaths of direct waves (solid line) and reflected waves (broken line) The radio wave path from is constituted only by the reflected wave (broken line), and the radio wave path from the satellite D is constituted only by the diffracted wave (broken line). These four satellites are representative examples of each aspect of the radio wave path, and in fact, the GPS receiver 21 also receives radio waves from many satellites other than the four satellites.

Among them, the radio wave from the satellite C which is a reflected wave and the radio wave from the satellite D which is a diffracted wave have a reduced CN ratio as a result of reflection and diffraction. In order to exclude the use of such satellites, a lower limit value of CN ratio is provided, and by selecting only radio waves of quality exceeding this lower limit, satellites of low quality radio waves such as reflected waves and diffracted waves Can be excluded.

On the other hand, the satellite B to which both the direct wave and the reflected wave reach does not necessarily have a low CN ratio, and the radio wave quality can not be judged only by the CN ratio. Therefore, the quality of the radio wave is judged based on the correlation profile of the PN code.

FIG. 4A is a diagram for explaining the PN code correlation Φ (t) of the radio wave from the satellite A. The vertical axis is the PN code correlation Φ (t), and the horizontal axis is the time t. As in the case of the satellite A, when only direct waves are received, the correlation profile of the PN code has only one correlation peak (single peak). As in the satellites C and D, single peaks are also observed in the case where only reflected waves or diffracted waves reach. On the other hand, in the satellite B in which a multipath occurs in which a reflected wave is also received in addition to the direct wave, as shown in FIG. 4B, a correlation peak of the reflected wave is observed after a predetermined time has elapsed from the correlation peak of the direct wave Peak). Therefore, by checking whether the correlation profile of the PN code is unimodal or multimodal, it is possible to exclude satellites in which multipath is generated.

As described above, the GPS receiver 21 can extract satellites (satellite A) with high radio wave quality by selecting a signal with a sufficiently high CN ratio and a single peak of the delay profile of the PN signal. Then, by calculating DOPs of a plurality of satellites selected by this method, it is possible to estimate the self-position estimation accuracy by the GPS receiver 21.

Next, the satellite selection method of this embodiment will be described in more detail. FIG. 5 shows a process of selecting a satellite to be used by the GPS receiver 21 based on the CN ratio of radio waves of each satellite and information of PN code, etc., and calculating the self position estimated value r and the error index value from the radio waves of selected satellites. Is shown. Note that the processing of S51 to S57 is actually processing performed by any of the GPS receiver 21, the self position estimation unit 12, and the error index value calculation unit 13. However, for convenience of explanation, the GPS receiver 21 is used. It is displayed out of etc. Further, since illustration of self position estimation by an external sensor other than the GPS receiver 21 is omitted, illustration of the weight setting unit 14 is also omitted, but self position estimation is performed by an external sensor other than the GPS receiver 21. It goes without saying that the weight setting unit 14 is essential in this case.

First, when the GPS receiver 21 receives a radio wave of a satellite, the PN code correlation ((t) is acquired from the radio wave (S51). If the acquired PN code correlation ((t) is a single peak as shown in FIG. 4A, the satellite is selected as an available satellite (S52). On the other hand, if there are multiple peaks as shown in FIG. 4B, the satellite is not selected.

Further, the CN ratio is acquired from the radio wave received by the GPS receiver 21 (S53). If the acquired CN ratio is equal to or greater than a predetermined value, the satellite is selected as an available satellite (S54). On the other hand, if it is less than the predetermined value, the satellite is not selected.

Further, the position information of the satellite is acquired from the radio wave received by the GPS receiver 21 (S55). Also, based on the azimuth and elevation angle of the satellite relative to the vehicle identified from the position information of the satellite, the position of the surrounding obstacle is specified from the output of the camera 22, radar 23a, laser 23b, sonar 23c etc. Estimate the presence or absence of an obstacle or the blocking rate between Then, satellites not shielded by surrounding obstacles (hereinafter referred to as "visible satellites") or satellites having a shielding rate equal to or less than a predetermined value are selected as available satellites (S56). On the other hand, satellites shielded by surrounding obstacles are not selected.

By the processes of S52, S54, and S56 described above, it is possible to specify a satellite whose CN ratio is equal to or more than a predetermined value, the correlation of the PN code exhibits unimodality, and satellites not shielded by surrounding obstacles. The self-position estimation unit 12 can realize highly accurate self-position estimation by using only the information of the satellite selected in this way. Furthermore, after DOP is calculated based on the positional relationship of the plurality of selected satellites (S57), the error index value calculation unit 13 obtains the product of the DOP and the error index .sigma.R, and calculates the product by the GPS receiver 21 It is an error index value for position estimation. Note that σR is an error index that can be calculated from the PN code correlation ((t), and indicates an error in the distance to each satellite, which will be described later in detail.

Here, it is desirable to use only the satellites whose radio waves are unimodal, the CN ratio is a predetermined value or more, and are not shielded by an obstacle for self-position estimation by the GPS system. In some cases, the number of satellites satisfying all these conditions is less than 4, and there is a period in which satellites with inferior radio wave quality can not but be used. Therefore, in the following, a self-position estimation method in the case of using a satellite with inferior radio wave quality will be described.

FIG. 6 is a list of the radio wave reception status of the GPS receiver and the correspondence of the satellite selection logic. For example, in the mode 1 row, the satellite selection logic in the case where both PN code correlation and CN ratio can be referred to It indicates that "satellites are added until DOP falls below a predetermined value in descending order of CN ratio". Hereinafter, the details of the satellite selection logic of each mode will be described using FIGS. 7 to 12. Note that which mode to use is successively changed according to the reception status of radio waves, and mode 1 is used at normal times, but any of mode 2 to mode 6 is appropriate according to radio wave conditions. Things are temporarily available.
<Mode 1>
FIG. 7 is a flowchart of a mode 1 satellite selection method that can be employed when PN code correlation and CN ratio can be referenced. The details of the process of this flowchart will be described below.

First, satellites transmitting high-quality radio waves are extracted from satellites in which the GPS receiver 21 receives radio waves (S1). Specifically, all satellites having a CN ratio equal to or greater than a predetermined value and having a PN code correlation Φ (t) of unimodality are extracted.

Thereafter, it is confirmed whether the number of extracted satellites is equal to or more than a predetermined value (an arbitrary number of 4 or more) (S2). If it is equal to or more than the predetermined value, four satellites having the best CN ratio are selected. Based on the GPS signal, σR and DOP are calculated (S3). The method of calculating σ R will be described later.

Next, using the calculated .sigma.R and DOP, f which is the reciprocal of the error index value is determined, and the satellite combination when this f is calculated is stored (S4). The error index value is the product of σR and DOP, and is expressed by f = 1 / (σR · DOP). Therefore, if .sigma.R or DOP is small, f will be large, and if self-position information detected by another external sensor is used together, the weight of the self-position information detected by the GPS receiver 21 will be large.

Then, it is confirmed whether the DOP of the satellite combination corresponding to this f is less than or equal to a predetermined value (S5). If it is less than or equal to the predetermined value, the self-position estimation accuracy only with the GPS signal from the currently selected satellite Is predicted to be sufficient, so that satellite combination and f are output (S6). The predetermined value used in S5 (and S9 described later) is set by the user according to the accuracy required for self-position estimation, and a smaller value may be set if high accuracy is required.

If the DOP is greater than the predetermined value in S5, it is predicted that the self-position estimation accuracy is insufficient only with the GPS signal from the currently selected satellite, so from the unselected satellites, the CN ratio is the next highest. One satellite is selected, and .sigma.R and DOP in the satellite combination to which this satellite is added are calculated (S7). Then, f ′ is calculated using the new σR and DOP (S8), and it is checked whether the new DOP is less than or equal to a predetermined value (S9). If the new DOP is less than the specified value, it can be determined that DOP has improved, but f 'may be worse due to the addition of satellites, so check whether f' is greater than f ( S10). When f 'is large, it can be determined that the accuracy of self-position estimation has been improved by the addition of satellites, so the stored satellite combination is updated to one corresponding to f' and the stored f is a larger value. After updating to f ′ having (S11), the process of S6 described above is executed.

When f 'is less than f in S10, DOP is improved by adding satellites, but the self-position estimation accuracy is deteriorated, so the satellite combination corresponding to f before adding satellites is the best. Output as a satellite combination (S6).

On the other hand, when DOP is larger than the predetermined value in S9, it can be determined that DOP is insufficient even after satellite addition, but there is also a possibility that the error index value is improved. Therefore, it is determined whether f 'after satellite addition is larger than f before satellite addition (S13). If f 'is larger than f, it can be determined that the error index value has been improved by satellite addition, so that the stored satellite combination is updated to one corresponding to f' after satellite addition, and f 'is newly updated. After storing as f (S14), the process returns to S7. On the other hand, if f 'is equal to or less than f in S13, it can be determined that there is no improvement in the error index value due to satellite addition, so the process returns to S7 without updating the stored satellite combination f.

If the number of satellites extracted in S2 does not reach a predetermined value (for example, 4), it is considered that the GPS receiver 21 can not estimate its own position in principle, so “impossible to measure” is output ( S15) The process in mode 1 is ended.

Although “impossible to measure” is output in S15 in FIG. 7, self-position estimation may be realized using a satellite that does not meet the reference of S1. Specifically, among the satellites that did not meet the S1 criteria, any satellite with the highest CN ratio, the satellite with the lowest shielding ratio, the satellite with the smallest DOP, or the satellite with the highest elevation angle from other satellites. By sequentially adding できる, a satellite combination that can secure a desired DOP or f may be obtained, and self-position estimation may be realized using the satellite combination.
<Mode 2>
FIG. 8 is a flowchart of a mode 2 satellite selection method that can be employed when PN code correlation can be referred to and the blocking rate of the satellite due to an obstacle can be estimated. In the following, the processing unique to mode 2 will be described, omitting the points common to mode 1.

In mode 2, the processes of S1a, S3a, and S7a shown in FIG. That is, in S1a, all satellites in which the PN code correlation Φ (t) is unimodal and the blocking ratio is equal to or less than a predetermined value are extracted. In S3a, four satellites with the lowest shielding rate are selected, and σR and DOP are calculated based on these GPS signals. In S7a, when DOP is larger than a predetermined value, the satellite with the next lowest shielding ratio is added to calculate σR and DOP.

According to the satellite selection method of mode 2, even if the CN ratio can not be referred to, it is possible to determine the best combination of satellites that can be selected under the conditions.
<Mode 3>
FIG. 9 is a flow chart of a mode 3 satellite selection method that can be employed when only PN code correlation can be referenced. In the following, details of processing specific to mode 3 will be described, omitting points common to mode 1.

In FIG. 9, the processes of S1 b, S3 b, and S7 b are different from those in mode 1. That is, in S1b, all satellites whose PN code correlation Φ (t) is unimodal and whose elevation angle is a predetermined value or more are extracted. In S3b, four satellites with the highest elevation angle are selected, and σR and DOP are calculated based on these GPS signals. In S7b, when DOP is larger than a predetermined value, the satellite with the next highest elevation angle is added to calculate σR and DOP.

According to the satellite selection method of mode 3, even if the CN ratio can not be referred to and the shielding ratio of the satellite can not be estimated either, it is possible to determine the best combination of satellites that can be selected under the conditions.
<Mode 4>
FIG. 10 is a flowchart of a mode 4 satellite selection method that can be employed when only CN ratio can be referenced. In the following, details of the processing specific to mode 4 will be described, omitting points common to mode 1.

In FIG. 10, the process of S1c is different from that of mode 1. That is, in S1c, all satellites whose CN ratio is equal to or more than a predetermined value are extracted (S1c).

According to the satellite selection method of mode 4, even if the PN code correlation can not be referred to and the shielding factor of the satellite can not be estimated, the best combination of satellites that can be selected under the conditions can be determined.
<Mode 5>
FIG. 11 is a flowchart of a satellite selection method of mode 5 that can be adopted when only the blocking rate of the satellite due to an obstacle can be estimated. In the following, details of processing specific to mode 5 will be described, omitting points common to mode 1.

In FIG. 11, the processes of S1 d, S3 a, and S7 a are different from those of mode 1. That is, in S1d, all satellites whose shielding ratio is less than or equal to a predetermined value are extracted. In S3a, four satellites with the lowest shielding rate are selected, and σR and DOP are calculated based on these GPS signals. In S7a, when DOP is larger than a predetermined value, the satellite with the next lowest shielding ratio is added to calculate σR and DOP.

According to the satellite selection method of mode 5, even if PN code correlation and CN ratio can not be referred to together, it is possible to find the best combination of satellites that can be selected under the conditions.
<Mode 6>
FIG. 12 is a flowchart of a satellite selection method of mode 6 that can be adopted even when the PN code correlation and the CN ratio can not be referred to and the shielding factor of the satellite can not be estimated either. In the following, details of processing specific to mode 6 will be described, omitting points common to mode 1.

In FIG. 12, the processes of S1 e, S3 b, and S7 b are different from those of mode 1. That is, in S1e, all satellites whose elevation angle is equal to or more than a predetermined value are extracted. In S3b, four satellites with the highest elevation angle are selected, and σR and DOP are calculated based on these GPS signals. In S7b, when DOP is larger than a predetermined value, the satellite with the next highest elevation angle is added to calculate σR and DOP.

According to the satellite selection method of mode 6, even if the PN code correlation and the CN ratio can not both be referred to and the shielding factor of the satellite can not be estimated either, finding the best combination of satellites that can be selected under that condition Can.
<Method of calculating σ R>
Next, the method of calculating σ R in the above-described mode 1 to mode 6 will be described with reference to FIGS.

FIG. 13 is a list of the reception status of the GPS receiver and the correspondence of the σR setting logic. For example, the row of mode A can refer to the PN code correlation and the CN ratio, and the number of selected satellites 7 shows an outline of the policy of calculating σ R when 4 is 4 (Yes in S5 of FIG. 7 for explaining mode 1).

Further, FIG. 14 is a diagram outlining the magnitude relationship of σR ₁ to σR ₆ preset for mode A to mode K, in which the error is larger as it goes upward, and the error is smaller as it goes downward. It shows. That is, in the same figure, it is shown that σ R ₁ corresponding to mode A and mode B is the best, and σ R ₆ ()) corresponding to mode K is the worst. Note that σR _n ′ is a value after the initial σR _n is degraded due to the influence of the surrounding environment etc., and σR ₁ ′ to σR ₅ ′ in FIG. 14 indicate that ∞ can be achieved due to the degradation of the environment. Hereinafter, the details of σ R in each mode of FIG. 13 will be described with reference to FIGS.
<Mode A>
If PN code correlation and CN ratio can be referred to and DOP less than the specified value can be calculated only with the four satellites with the best CN ratio (Yes in S5 in FIG. 7), only satellites with good radio quality can be used. , Mode A is adopted in which the predetermined σ R ₁ is made σ R as it is.
<Mode B>
If PN code correlation and CN ratio can be referred to but DOP less than the specified value can not be calculated with only 4 satellites with the best CN ratio (if it is necessary to select 5 or more satellites) (No at S5 in Figure 7) ), to indicate that it is also used bad satellite relatively radio wave quality, employing a mode B to sigma] R of sigma] R 1 _{'subordinated} to the default sigma] R _1. In this mode B, σR ₁ ′ is calculated using FIG. 15A or FIG. 15B.

The vertical axis in FIG. 15A is σR, and the horizontal axis is the CN ratio average value. In the figure, if the CN ratio average value is sufficiently large, σR ₁ ′ approximates to the predetermined σR ₁ , and the smaller the CN ratio average value, the larger the σR ₁ ′, and the CN ratio average value becomes a predetermined shielding occurrence. Below the index value (a numerical value used to determine whether the satellite is behind an obstacle, for example, 30 dB), σR ₁ ′ indicates ∞.

Further, FIG. 15B also corresponds to mode B, and the vertical axis represents σR and the horizontal axis represents ξ. Here, ξ is expressed by equation 6.

As is clear from this equation, when the number of satellites hidden by the obstacle increases or when the average CN ratio of radio waves from the satellites behind the obstacle deteriorates, the ξ becomes large, and σ R It turns out that ₁ 'also becomes large.

In mode B described above, σR ₁ ′ can be calculated by using the relationship of FIG. 15A or FIG. 15B.
<Mode C>
If PN code correlation can be referred to and DOP less than the predetermined value can be calculated only with the four satellites with the best shielding ratio (Yes in S5 in FIG. 8), only satellites with good radio quality can be used. A mode C is adopted in which a predetermined σ R ₂ proportional to the density of objects is taken as σ R as it is.
<Mode D>
If PN code correlation can be referred to but DOP less than the predetermined value can not be calculated with only the four satellites with the best shielding ratio (if it is necessary to select five or more satellites) (No at S5 in FIG. 8), relative manner to indicate that it is also used bad satellite radio wave quality, employing the mode D to sigma] R of sigma] R 2 _{'subordinated} to the default sigma] R _2. In this mode D, σR ₂ ′ is calculated using FIG. 16A or 16B. 16A is used when all the selected satellites are unimodal, and FIG. 16B is used when the selected satellites include multi-modal ones.

In both FIGS. 16A and 16B, the vertical axis is σR, and the horizontal axis is (the number of correlation peaks / the number of satellites included in all the satellites). FIG. 16A shows that the number of correlation peaks and σR ₂ ′ are proportional when using only unimodal satellites. On the other hand, FIG. 16B shows that when multi-peak satellites are used, σ R ₂ ′ rises sharply as the number of correlation peaks increases.

In mode D described above, σR ₂ ′ can be calculated by using the relationship of FIG. 16A or 16B.
<Mode E>
If the CN ratio can be referred to and the DOP less than the predetermined value can be calculated only with the four satellites with the best CN ratio (Yes in S5 of FIG. 10), only the satellites with good radio quality can be used. ₂ to a sigma] R ₃ multiplied by (the height of the obstacle) / (distance to the obstacle) as it adopts the mode E to sigma] R. In the environment where reflection easily occurs, this σ R ₃ is estimated to have a larger error risk than σ R ₂ .
<Mode F>
If the CN ratio can be referred to but the DOP less than the specified value can not be calculated with only the four satellites with the best CN ratio (if it is necessary to select more than five satellites) (No at S5 in FIG. 10), relative to indicate that it is also used bad satellite radio wave quality, adopts a mode F to sigma] R of sigma] R 3 _{'subordinated} to the default sigma] R _3. In this mode F, σ R ₃ ′ is calculated using any of FIGS. 17A to 17C.

In both FIGS. 17A and 17B, the vertical axis is σR, and the horizontal axis is the CN ratio average value. Figure 17A, if the CN ratio average value is large enough sigma] R 3 _'is similar to sigma] R _3, sigma] R 3 if falls below a predetermined CN ratio good _index' indicates a situation where a ∞. On the other hand, in FIG. 17B, if the CN ratio average value is sufficiently large, σ R ₃ ′ approximates to σ R ₃ and σ R ₃ ′ gradually decreases between the predetermined CN ratio good index value and the shielding occurrence index value, and the predetermined shielding occurrence occurs. Below the index value, it is shown that σ R ₃ ′ becomes ∞.

Further, in FIG. 17C, the vertical axis is σR, and the horizontal axis is ξ. Since ξ is the one described in equation 6, even in mode F, when the number of satellites hidden by an obstacle increases or when the average CN ratio of radio waves from satellites behind an obstacle deteriorates In this case, it is understood that ξ increases and σ R ₃ ′ also increases.

In mode F described above, σR ₃ ′ can be calculated by using any one of the relationships in FIGS. 17A to 17C.
<Mode G>
When the PN code correlation and the CN ratio are not referable, and the number of selected satellites is four, and the shielding ratio of those satellites can be estimated, mode G is adopted in which a predetermined σR ₄ is used as it is as σR. Note that σ R ₄ is set to a value larger than the above-mentioned σ R ₃ .
<Mode H>
If the PN code correlation and CN ratio can not be referred to and the number of selected satellites is 5 or more, and the shielding ratio of those satellites can be estimated, it indicates that satellites with relatively poor radio quality are also used. In this case, mode H is adopted in which σ R ₄ ′, which is subordinate to the predetermined σ R ₄ , is σ R. In this mode H, σ R ₄ ′ proportional to the number of potentially occluded satellites is calculated using FIG.
<Mode I>
When the PN code correlation and the CN ratio can not be referred to, the number of selected satellites is 4, and the shielding ratio of the satellites can not be estimated, mode I is adopted in which a predetermined σR ₅ is used as it is as σR. Note that σ R ₅ is set to a value larger than the above-mentioned σ R ₄ .
<Mode J>
If the PN code correlation and CN ratio can not be referred to, and the number of selected satellites is 5 or more, and the shielding ratio of those satellites can not be estimated, it indicates that satellites with relatively poor radio quality are also used, A mode J is adopted in which σ R is a σ R ₅ ′ that is subordinate to the default σ R ₅ . In this mode J, σ R ₅ ′ proportional to the number of potentially occluded satellites is calculated using FIG.
<Mode K>
Since the satellite radio wave can not be received during the cold start or warm start preparation time, mode K is adopted in which σR ₆ substantially meaning ∞ is used as it is as the error index σ R.
<Self-position estimation method of this embodiment>
The error index value calculation unit 13 multiplies σR calculated in any of the above-mentioned modes A to K by the value of DOP calculated from the positional relationship of a plurality of satellites selected at that time, and the reciprocal thereof is calculated By calculating, the error index value f of self-position estimation by the GPS receiver 21 is calculated. Then, by using this error index value in the flowcharts of mode 1 to mode 6, the best satellite combination under each environment can be determined.

The estimation result of the error index value may be stored in the map together with the date and time information and may be used as reference information at the time of route selection in the next and subsequent traveling. By providing date and time information, the arrangement of all satellites at that time can be estimated from the satellite operation information.

According to the self-position estimation system of the present embodiment described above, even if the surrounding environment of the GPS receiver 21 or the radio wave condition reached from each satellite suddenly changes, the error index value σ R · · · according to the situation. The DOP can be calculated sequentially, and the optimum combination of satellites can be updated sequentially with reference to it. Then, the self position estimation unit 12 calculates the self position estimation value r based on the radio waves from the satellites of the combination, and the self position estimation generalization unit 15 calculates the self position estimation value r and the error index value σR · DOP. Use to estimate the final self position estimate r ^. That is, according to this embodiment, stable self-position estimation corresponding to environmental change of the GPS system can be easily continued.

Next, a self-position estimation system according to a third embodiment of the present invention will be described. The same points as the above-described embodiment will not be repeatedly described.

The self-position estimation system of this embodiment is intended to be interlocked with a car navigation system provided in the mobile unit 200.

That is, when the car navigation system creates a plurality of routes to the destination position and selects one of them to move, the target point arrival time is predicted from the current time and the average moving speed of the moving object 200, The satellite arrangement from the point of time to the predicted time is acquired, and based on the map held by the car navigation system, the number of satellites with high C / N ratio and PN signal delay profile with unimodality in each route. It estimates, C / N ratio is high, and the delay profile of PN signal starts autonomous movement so as to select a route with many satellites having unimodality, and provides guidance to the passenger.

According to such a present Example, the movement path | route which can use an electromagnetic wave with a more favorable quality can be selected easily.

Reference Signs List 100 self

position estimation system

11, 11a

external sensor

12, 12a self

position estimation unit

13, 13a error index value calculation unit 14, 14a weight setting unit 15, self position estimation generalization unit 16, control device 200 mobile unit , 21 GPS receiver, 22 camera, 23 lidar, 23a radar, 23b laser, 23c sonar, 24 wheel speed and steering angle sensor, 25 rainfall sensor, r ₀ true self position, r, r ₁ self position estimate, r ^ final self position estimate w, w ₁ weight, DOP position accuracy degradation

Claims

A self-position estimation system for estimating self-position with reference to outputs of a plurality of external sensors, comprising:
An error index value calculation unit that estimates an error index value that is an index of the measurement error of each external sensor;
A weight setting unit that sets the weight of each external sensor based on each error index value;
A self-position estimation generalization unit that takes a weighted average using the weights on self-positions estimated based on the output of each external sensor, and estimates a final self-position;
A self-position estimation system comprising:
In the self-position estimation system according to claim 1,
The weight of each external sensor set by the weight setting unit is a value proportional to the reciprocal of each error index value.
A self position estimation system for estimating self position by referring to radio waves of a plurality of GPS satellites,
A GPS receiver for receiving radio waves of the GPS satellites;
First satellite selection means for selecting a GPS satellite whose CN ratio of radio waves is equal to or greater than a predetermined value;
Second satellite selection means for selecting a satellite whose PN code correlation of radio waves is unimodal;
Third satellite selection means for selecting a satellite whose shielding rate due to surrounding obstacles is lower than a predetermined value;
A self position estimation unit that estimates self position based on radio waves of a plurality of selected GPS satellites;
Equipped with
A self-position estimation system characterized in that a combination of GPS satellites used for self-position estimation is different according to a combination of outputs of the satellite selection means.
In the self-position estimation system according to claim 3,
If the CN ratio of radio waves is equal to or greater than a predetermined value and it is possible to select four or more satellites with single-peak PN code correlation of radio waves, select four GPS satellites with the best CN ratio of radio waves and calculate DOP ,
If the calculated DOP exceeds the predetermined value, then the GPS satellites with the next highest radio CN ratio are sequentially added until the DOP falls below the predetermined value to calculate DOP,
A self-position estimation system characterized by estimating a self-position using a combination of GPS satellites when the DOP is less than the predetermined value.
In the self-position estimation system according to claim 3,
When the CN ratio of radio waves is equal to or more than a predetermined value and four or more satellites having the shielding ratio equal to or less than the predetermined value can be selected, the four GPS satellites having the best shielding ratio are selected to calculate DOP.
If the calculated DOP exceeds a predetermined value, GPS satellites with the next lowest shielding ratio are sequentially added to calculate the DOP until the DOP falls below the predetermined value.
A self-position estimation system characterized by estimating a self-position using a combination of GPS satellites when the DOP is less than the predetermined value.
In the self-position estimation system according to claim 3,
If four or more satellites with single-peak PN code correlation of radio waves can be selected, DOP is calculated by selecting the four GPS satellites with the best elevation angle,
If the calculated DOP exceeds a predetermined value, GPS satellites having a higher elevation angle are sequentially added to calculate the DOP until the DOP falls below the predetermined value.
A self-position estimation system characterized by estimating a self-position using a combination of GPS satellites when the DOP is less than the predetermined value.
In the self-position estimation system according to claim 3,
When it is possible to select four or more satellites whose CN ratio of radio waves is a predetermined value or more, select four GPS satellites whose CN ratio of radio waves is the best and calculate DOP,
If the calculated DOP exceeds the predetermined value, then the GPS satellites with the next highest radio CN ratio are sequentially added until the DOP falls below the predetermined value to calculate DOP,
A self-position estimation system characterized by estimating a self-position using a combination of GPS satellites when the DOP is less than the predetermined value.
In the self-position estimation system according to claim 3,
When it is possible to select four or more satellites whose shielding ratio is equal to or less than a predetermined value, the four GPS satellites having the best shielding ratio are selected to calculate DOP,
If the calculated DOP exceeds a predetermined value, GPS satellites with the next lowest shielding ratio are sequentially added to calculate the DOP until the DOP falls below the predetermined value.
A self-position estimation system characterized by estimating a self-position using a combination of GPS satellites when the DOP is less than the predetermined value.
In the self-position estimation system according to claim 3,
Calculate the DOP by selecting the four GPS satellites with the best elevation angle,
If the calculated DOP exceeds a predetermined value, GPS satellites having a higher elevation angle are sequentially added to calculate the DOP until the DOP falls below the predetermined value.
A self-position estimation system characterized by estimating a self-position using a combination of GPS satellites when the DOP is less than the predetermined value.
The self-position estimation system according to any one of claims 4 to 9,
The weight of the output of the GPS receiver is a value proportional to the reciprocal of the DOP.