阅读说明：本技术 方位推定装置 (Azimuth estimation device ) 是由 宫岛朗 松冈克宏 于 2018-06-22 设计创作，主要内容包括：本发明涉及方位推定装置。具备：卫星数据获取部(101),获取方位推定装置、导航信号的多普勒频移频率；传感器数据获取部(102),从偏航率传感器获取偏航率；多普勒方位推定部(103),推定移动体的方位；状态变量决定部(S14),决定表示方位角的可靠度的状态变量(S)；阈值选择部(S16),从用于决定是否采用上述方位作为基准方位的多个组的评价参数阈值群选择上述评价参数阈值群；评价参数计算部(S17),计算用于评价方位的可靠度的多个评价参数；以及采用与否判断部(S18、S19),对多个上述评价参数和上述阈值选择部选择的上述评价参数阈值群进行比较,并基于判断为满足采用条件而决定为采用上述方位作为上述基准方位。(The present invention relates to an azimuth estimating device. The disclosed device is provided with: a satellite data acquisition unit (101) for acquiring Doppler frequency shift frequencies of the navigation signal and the direction estimation device; a sensor data acquisition unit (102) that acquires a yaw rate from a yaw rate sensor; a Doppler direction estimation unit (103) that estimates the direction of the mobile body; a state variable determination unit (S14) for determining a state variable (S) indicating the reliability of the azimuth; a threshold value selection unit (S16) for selecting the evaluation parameter threshold value group from a plurality of evaluation parameter threshold value groups for determining whether or not to adopt the azimuth as a reference azimuth; an evaluation parameter calculation unit (S17) for calculating a plurality of evaluation parameters for evaluating the reliability of the orientation; and a use/non-use determination unit (S18, S19) that compares the plurality of evaluation parameters with the evaluation parameter threshold group selected by the threshold selection unit, and determines to use the azimuth as the reference azimuth based on a determination that a use condition is satisfied.)

1. An azimuth estimating device mounted on a mobile body and sequentially estimating the azimuth of movement of the mobile body, comprising:

a satellite data acquisition unit (101) that acquires, in time series, the Doppler shift frequency of a navigation signal received by a GNSS receiver;

a sensor data acquisition unit (102) that acquires the yaw rate of the mobile body in time series from a yaw rate sensor used in the mobile body;

a Doppler azimuth estimating unit (103) that estimates the azimuth of the mobile body at each time on the basis of an equation that estimates the azimuth of the mobile body at each time, the equation being created on the basis of a plurality of Doppler shift frequencies acquired in time series, and in which an unknown parameter that changes at each time is constrained by an azimuth angle change amount calculated on the basis of the yaw rate;

a state variable determination unit (S14) that determines a state variable (S) that increases according to the elapsed time after a reference azimuth added with the azimuth change amount in autonomous navigation is updated, and that indicates the reliability of the azimuth;

a threshold value selection unit (S16) configured to select, from among a plurality of sets of evaluation parameter threshold value groups for determining whether or not to use the azimuth estimated by the doppler azimuth estimation unit as the reference azimuth, the evaluation parameter threshold value group that easily satisfies a use condition even if the reliability of the azimuth is low, the state variable determined by the state variable determination unit having a value indicating that the reliability of the azimuth is low;

an evaluation parameter calculation unit (S17) which calculates a plurality of evaluation parameters for evaluating the reliability of the azimuth when the Doppler azimuth estimation unit estimates the azimuth; and

and a decision unit (S1, S19) for comparing the plurality of evaluation parameters with the evaluation parameter threshold group selected by the threshold selection unit, and deciding the azimuth estimated by the doppler azimuth estimation unit as the reference azimuth based on the decision that the adoption condition is satisfied.

2. An azimuth estimating device mounted on a mobile body and sequentially estimating the azimuth of movement of the mobile body, comprising:

a satellite data acquisition unit (101) that acquires, in time series, the Doppler shift frequency of a navigation signal received by a GNSS receiver;

a sensor data acquisition unit (102) that acquires the magnitude of the speed of the mobile body in time series from a speed sensor used in the mobile body, and that acquires the yaw rate of the mobile body in time series from a yaw rate sensor used in the mobile body;

a Doppler azimuth estimating unit (103) that estimates the azimuth of the mobile body at each time on the basis of an equation that estimates the azimuth of the mobile body at each time and is created on the basis of the Doppler shift frequencies acquired in time series, wherein the unknown parameter that changes at each time is constrained by the size of the speed of the mobile body and the linearization of the time variation of the clock drift;

a state variable determination unit (S14) that determines a state variable (S) that increases according to the elapsed time after a reference azimuth is updated, the reference azimuth being added with an azimuth change amount calculated based on the yaw rate during autonomous navigation, the state variable indicating the reliability of the azimuth;

a threshold value selection unit (S16) configured to select, from among a plurality of sets of evaluation parameter threshold value groups for determining whether or not to use the azimuth estimated by the doppler azimuth estimation unit as the reference azimuth, the evaluation parameter threshold value group that easily satisfies a use condition even if the reliability of the azimuth is low, the state variable determined by the state variable determination unit having a value indicating that the reliability of the azimuth is low;

an evaluation parameter calculation unit (S17) which calculates a plurality of evaluation parameters for evaluating the reliability of the azimuth when the Doppler azimuth estimation unit estimates the azimuth; and

and a decision unit (S18, S19) for comparing the plurality of evaluation parameters with the evaluation parameter threshold group selected by the threshold selection unit, and deciding the azimuth estimated by the doppler azimuth estimation unit as the reference azimuth based on the decision that the adoption condition is satisfied.

3. The orientation estimation device according to claim 1 or 2,

when the speed of the mobile body is relatively high, the state variable is less increased in accordance with an increase in the elapsed time than when the speed of the mobile body is relatively low.

4. The orientation estimation device according to claim 3,

further comprising a reliability level determination unit (S15) for determining an orientation reliability level based on the state variables determined by the state variable determination unit,

the threshold selection unit (S16) selects the evaluation parameter threshold group based on the azimuth reliability level and the velocity level determined by the reliability level determination unit, the more the azimuth reliability level is a level indicating that the reliability of the azimuth is low, and the more the velocity level is a level having a lower velocity, the less the reliability of the azimuth is, the evaluation parameter threshold group satisfying the adoption condition is selected.

5. The orientation estimation device according to claim 4,

the reliability level determination unit determines the bearing reliability level as a level having the highest reliability when the doppler direction estimation unit can estimate the direction for a predetermined time or more.

6. The orientation estimation device according to any one of claims 1 to 5,

the state variable determining unit sets an initial value of the state variable after the azimuth estimating device is powered on to a previous end value that is a value of the state variable when the azimuth estimating device was powered off last time, and sets the initial value to a preset initial value when the previous end value is not present.

Technical Field

The present disclosure relates to an azimuth estimating device that estimates an azimuth in which a mobile body moves.

Background

Patent document 1 discloses a method of accumulating doppler shift frequencies, which are frequency shift amounts of navigation signals transmitted from navigation satellites, in time series, acquiring one or both of a vehicle speed and a yaw rate in time series, and estimating a bearing with high accuracy from the acquired frequencies.

In addition, a current position estimation method called autonomous navigation is widely known. In autonomous navigation, the azimuth is updated by adding the amount of change in the azimuth determined from the value detected by the yaw rate sensor to the reference azimuth. The velocity is also estimated from the detection value of a velocity sensor or an acceleration sensor.

Patent document 1 Japanese patent No. 5879977

The use of the method disclosed in patent document 1 enables the azimuth to be estimated with high accuracy. Therefore, it is considered that the azimuth estimated by the method disclosed in patent document 1 is set as a reference azimuth used for autonomous navigation.

However, the estimated azimuth has an error, of course, and the error varies every time the azimuth is estimated. Therefore, when the azimuth can be estimated by the method disclosed in patent document 1, if the azimuth needs to be set as the reference azimuth, the error of the estimated azimuth may increase.

On the other hand, it is widely known that, if autonomous navigation continues, errors in yaw rate detected by the yaw rate sensor are accumulated, and thus errors in azimuth estimation increase. Therefore, even if the time for not updating the reference azimuth is long, the azimuth estimation error continues to be large.

Disclosure of Invention

An object of the present disclosure is to provide an azimuth estimation device capable of suppressing an increase in error of an azimuth estimated in autonomous navigation.

A bearing estimation device according to a first aspect of the present disclosure is a bearing estimation device mounted on a mobile body and sequentially estimating a bearing in which the mobile body moves, the bearing estimation device including: a satellite data acquisition unit that acquires the Doppler frequency shift frequency of a navigation signal received by a GNSS receiver in time series; a sensor data acquisition unit that acquires a yaw rate of the mobile body in time series from a yaw rate sensor used for the mobile body; a Doppler azimuth estimating section that estimates an azimuth of the mobile body based on an equation that estimates an azimuth of the mobile body at each time based on a plurality of Doppler shift frequencies acquired in time series, and an unknown parameter that changes at each time is constrained by an azimuth change amount calculated based on a yaw rate; a state variable determination unit that determines a state variable that increases according to an elapsed time after a reference azimuth, to which an azimuth angle change amount is added during autonomous navigation, is updated, and that indicates the reliability of the azimuth angle; a threshold value selection unit that selects, from among a plurality of sets of evaluation parameter threshold value groups for determining whether or not to use the azimuth estimated by the doppler azimuth estimation unit as a reference azimuth, an evaluation parameter threshold value group that easily satisfies a use condition even if the reliability of the azimuth is low, the state variable determined by the state variable determination unit having a value indicating that the reliability of the azimuth is low; an evaluation parameter calculation unit that calculates a plurality of evaluation parameters for evaluating the reliability of the azimuth when the doppler azimuth estimation unit estimates the azimuth; and a determination unit for comparing the plurality of evaluation parameters with the evaluation parameter threshold group selected by the threshold selection unit, and determining that the azimuth estimated by the doppler azimuth estimation unit is used as the reference azimuth based on the determination that the adoption condition is satisfied.

A bearing estimation device according to a second aspect of the present disclosure is a bearing estimation device mounted on a mobile body and sequentially estimating a bearing in which the mobile body moves, the bearing estimation device including: a satellite data acquisition unit that acquires the Doppler frequency shift frequency of a navigation signal received by a GNSS receiver in time series; a sensor data acquisition unit that acquires a magnitude of a velocity of the moving body in time series from a velocity sensor used in the moving body, and acquires a yaw rate of the moving body in time series from a yaw rate sensor used in the moving body; a doppler direction estimating unit that estimates a direction of the mobile body based on an equation that estimates the direction of the mobile body at each time based on a plurality of doppler shift frequencies acquired in time series, and an unknown parameter that changes at each time is constrained by a linear change in the magnitude of the velocity of the mobile body and a time change in clock drift; a state variable determination unit that determines a state variable that increases in accordance with an elapsed time after a reference azimuth is updated, the reference azimuth being added with an azimuth change amount calculated based on a yaw rate during autonomous navigation, and that indicates the reliability of the azimuth; a threshold value selection unit that selects, from among a plurality of sets of evaluation parameter threshold value groups for determining whether or not to use the azimuth estimated by the doppler azimuth estimation unit as a reference azimuth, an evaluation parameter threshold value group that easily satisfies a use condition even if the reliability of the azimuth is low, the state variable determined by the state variable determination unit having a value indicating that the reliability of the azimuth is low; an evaluation parameter calculation unit that calculates a plurality of evaluation parameters for evaluating the reliability of the azimuth when the doppler azimuth estimation unit estimates the azimuth; and a determination unit for comparing the plurality of evaluation parameters with the evaluation parameter threshold group selected by the threshold selection unit, and determining that the azimuth estimated by the doppler azimuth estimation unit is used as the reference azimuth based on the determination that the adoption condition is satisfied.

These azimuth estimating apparatuses determine a state variable indicating the magnitude of the reliability of the azimuth, and the more the state variable indicates that the reliability of the azimuth is low, the evaluation parameter threshold group to which a plurality of evaluation parameters are compared is set as an evaluation parameter threshold group that easily satisfies the adoption condition even if the reliability of the azimuth is low. Thus, the azimuth estimated by the doppler azimuth estimating unit is used as the reference azimuth even if the reliability is sufficiently high. Therefore, the reference azimuth can be easily updated at an early stage, and therefore, an increase in error of the azimuth estimated in autonomous navigation can be suppressed.

Drawings

The above objects, and other objects, features, and advantages of the present disclosure will become more apparent from the following detailed description with reference to the accompanying drawings. In the drawings, wherein:

fig. 1 is a block diagram showing the configuration of a direction estimation device.

Fig. 2 is a flowchart showing a process executed by the doppler direction estimating unit in fig. 1.

Fig. 3 is a flowchart showing a process executed by the INS bearing determination unit shown in fig. 1.

Fig. 4 is a diagram showing an orientation reliability level determination table used in a part of the processing of fig. 3.

Fig. 5 is a diagram showing a threshold group selection table used in a part of the processing of fig. 3.

Fig. 6 is a diagram showing an evaluation parameter threshold group list.

Detailed Description

Hereinafter, embodiments will be described with reference to the drawings. The direction estimation device 100 shown in fig. 1 is mounted on a vehicle 1 as a moving body together with a GNSS receiver 2, a vehicle speed sensor 3, a gyro sensor 4, and a shift position sensor 5. When the power of the vehicle 1 is turned on, the power is supplied to the direction estimation device 100, and the direction estimation device 100 sequentially estimates the azimuth angle θ while the power is supplied to the direction estimation device 100.

< summary of the Structure >

The GNSS receiver 2 receives a navigation signal transmitted from a navigation satellite Si provided in a GNSS (global navigation satellite system). Further, i is the number of the navigation satellite. The GNSS is, for example, GPS. The navigation signal is superimposed on a carrier wave and transmitted as an electric wave from the navigation satellite Si. Hereinafter, the radio wave transmitted from the navigation satellite Si is referred to as a GNSS radio wave. The GNSS receiver 2 demodulates the received GNSS radio waves to extract navigation signals.

Then, the pseudo range ρ is determined from the extracted navigation signal_iDoppler shift frequency D_iSatellite position (X)_si，Y_si，Z_si) State of satellites, navigation data, etc. The navigation data includes a satellite number of the navigation satellite Si, an ephemeris which is orbit information of the navigation satellite Si, a time when the navigation satellite Si transmits a radio wave, and the like.

Satellite position (X) for each navigation satellite Si_si，Y_si，Z_si) The calculation is performed based on the ephemeris of each navigation satellite Si and the time at which the GNSS radio wave is transmitted. For the pseudorange ρ_iThe time difference between the time when the navigation satellite Si transmits the GNSS radio wave and the time when the GNSS receiver 2 receives the GNSS radio wave is calculated by multiplying the radio wave propagation time by the speed of light C.

Doppler shift frequency D_iIs the frequency difference between the carrier frequency of the radio wave transmitted by the navigation satellite Si and the carrier frequency of the received GNSS radio wave. The carrier frequency of the radio wave transmitted by the navigation satellite Si is determined in advance, and the frequency is stored in advance in a predetermined storage unit provided in the GNSS receiver 2.

The GNSS receiver 2 outputs these signals to the direction estimation device 100 at a fixed cycle together with the S/N of the navigation signal as the reception signal and the time when the navigation signal is received. The information output from the GNSS receiver 2 to the direction estimation device 100 is information before calculating the position in the satellite navigation. Hereinafter, information before position calculation in satellite navigation is referred to as satellite data. The certain period during which the GNSS receiver 2 outputs the satellite information is, for example, between 200 milliseconds and 400 milliseconds. There are a plurality of navigation satellites Si. The GNSS receiver 2 determines satellite data from all navigation signals that can be demodulated from GNSS radio waves, and outputs all the determined satellite data to the direction estimation device 100.

The vehicle speed sensor 3 detects the rotational speed of the wheels of the vehicle 1. The vehicle speed sensor 3 outputs a signal indicating the rotation speed of the wheel to the direction estimating device 100.

The gyro sensor 4 detects rotational angular velocities around a yaw axis, a pitch axis, and a roll axis of the vehicle 1, and outputs a signal indicating the detected rotational angular velocities to the azimuth estimating device 100. The gyro sensor 4 detects a yaw rate, which is a rotational angular velocity about the yaw axis, and thus functions as a yaw rate sensor.

The shift position sensor 5 detects a shift position of the vehicle 1, and outputs a signal indicating the shift position to the direction estimating device 100. The signal output from the shift position sensor 5 determines which of the forward and backward directions the vehicle 1 is moving.

The vehicle speed sensor 3, the gyro sensor 4, and the shift position sensor 5 are behavior detection sensors 6 that output signals indicating behaviors that are motions of the vehicle 1.

The direction estimating device 100 is a computer including a CPU, a ROM, a RAM, and the like, which are not shown. The CPU utilizes the temporary storage function of the RAM and executes a program stored in a non-migration entity recording medium such as a ROM. In this way, the bearing estimation device 100 performs the functions of the satellite data acquisition unit 101, the sensor data acquisition unit 102, the doppler bearing estimation unit 103, and the INS bearing determination unit 104. When these functions are executed, a method corresponding to the program stored in the non-migration entity recording medium is executed. In addition, a part or all of the functions executed by the orientation estimation apparatus 100 may be configured by hardware using one or more ICs or the like.

< overview of processing executed by the direction estimation device 100 >

Next, an outline of processing performed by the orientation estimation device 100 will be described. A part of the processing performed by the direction estimation device 100 will be described in detail using flowcharts shown in fig. 2 and 3.

The satellite data acquisition unit 101 acquires satellite data from the GNSS receiver 2 in time series at a satellite data acquisition cycle, and stores the acquired satellite data in the satellite data accumulation unit 110. The satellite data acquisition period is the same as the period in which the GNSS receiver 2 outputs satellite data. The satellite data storage unit 110 is a writable storage medium, and may be volatile or nonvolatile. The satellite data storage unit 110 may use, for example, a RAM.

The sensor data acquisition unit 102 acquires the signals detected by the behavior detection sensor 6 in time series at a sensor value acquisition cycle. The sensor value acquisition period is shorter than the period in which the GNSS receiver 2 outputs the navigation signal, for example, several tens of milliseconds. The sensor data acquisition unit 102 stores the acquired signal in the sensor data storage unit 111. The sensor data storage unit 111 is a writable storage medium, and may be volatile or nonvolatile. The sensor data storage unit 111 may use, for example, a RAM. The sensor data storage unit 111 can use the same storage medium as the satellite data storage unit 110.

The doppler direction estimating unit 103 calculates an azimuth angle (i.e., an initial azimuth angle value) θ of the traveling direction of the vehicle 1 at an initial time using the direction estimating equation shown in equation (1)⁰. The initial time is the updated azimuth angle initial value θ⁰The time of day.

[ number 1]

Vs^t _i＝V^t _wheelGx^t _icos(θ⁰+θ^t _gyro)+V^t _wheelGy^t _isin(θ⁰+θ^t _gyro)-Cbv⁰-At (1)

(1) The formula (iv) is described as formula (6) in patent document 1. In the formula (1), Vs_iIs the satellite direction velocity, t is the time, V_wheelIs a detected value of the vehicle speed sensor 3, theta_gyroIs a relative azimuth, Cbv, which is a variation amount of an azimuth in a traveling direction of the vehicle 1⁰The clock drift at the initial time, a is the slope of the time change of the clock drift Cbv, and Gx and Gy are the x component and y component of the line-of-sight vector from the vehicle 1 to the navigation satellite Si.

Satellite directional velocity Vs on the left of equation (1)_iCalculated according to equation (2).

[ number 2]

(2) The first term on the right of the equation means the relative speed Vr of the vehicle 1 with respect to the navigation satellite Si_iThe second, third, and fourth terms are the speed of the navigation satellite Si in the direction of the vehicle 1. The sum of these means the speed of the vehicle 1 in the direction of the navigation satellite Si, so equation (2) holds. In the formula (2), D_iIs the doppler shift frequency. Thus, equation (1) uses the Doppler shift frequency D_iTo calculate an initial value theta of the azimuth angle⁰。

The processing of the doppler direction estimation unit 103 will be described with reference to fig. 2. At S1, the satellite data storage unit 110 stores satellite positions (X) of the navigation satellites Si_si，Y_si，Z_si) Calculates a velocity vector (Vxs) of each navigation satellite Si from the time-series data_i，Vys_i，Vzs_i)。

At S2, the doppler shift frequency D included in the satellite data stored in the satellite data storage unit 110 is used_iThe current relative speed Vr of the vehicle with respect to the navigation satellite Si is calculated by substituting the equation (3)_i. In the formula (3), C is the speed of light, and F is the frequency of the GNSS radio wave transmitted from the navigation satellite Si.

[ number 3]

Vr_i＝-D_i·C/F (3)

In S3, the current position P (X) of the vehicle 1 is calculated_v，Y_v，Z_v). The position P of the vehicle 1 is used to calculate a line-of-sight vector (Gx) from the vehicle 1 to the navigation satellite Si_i，Gy_i，Gz_i)。

As is widely known, if four or more good-quality navigation signals are received, the position P (X) as an unknown number can be calculated by establishing four or more simultaneous equations using the navigation signals_v，Y_v，Z_v) And clock drift Cbv. However, the position of the vehicle 1 here is used to calculate the line-of-sight vector (Gx) from the vehicle 1 to the navigation satellite Si in the next S4_i，Gy_i，Gz_i). Since the navigation satellite Si exists in a distant place, the accuracy of the current position used to determine the angle between the navigation satellite Si and the vehicle 1 may be low. Therefore, four or more signals with good quality may not be received.

Therefore, for example, in S3, the current coordinates (X) of the vehicle 1 can be calculated using four or more navigation signals including a signal that cannot be determined to be of good quality_v，Y_v，Z_v). The coordinates (X) of the current vehicle 1 are calculated from four or more navigation signals_v，Y_v，Z_v) In the case of (3), the pseudo range ρ is calculated_iAnd the satellite position (X) of the navigation satellite Si_si，Y_siZs) and determines the coordinates (X) of the vehicle 1_v，Y_v，Z_v) So that the pseudorange ρ_iThe residual error of (a) is minimal.

In addition, the pseudo range ρ may be used_iDetermine the position P (X) of the vehicle 1 by a method with low accuracy other than the position determination of (1)_v，Y_v，Z_v). Depending on the estimation accuracy allowed by the system or the like, if the position error of the vehicle 1 is in the range of several hundreds of meters, the velocity estimation error becomes 1 meter/sec or less, and there is no significant problem. Therefore, for example, the position may be determined from a map or the like, or the position P (X) of the vehicle 1 may be determined based on information such as a measurement history of past positions and beacons_v，Y_v，Z_v)。

In S4, a line-of-sight vector (Gx) from the vehicle 1 to the navigation satellite Si is calculated_i，Gy_i，Gz_i). The x component, y component, and z component of the line-of-sight vector are calculated by equation (4).

[ number 4]

In the formula (4), ρ^t _iIs the pseudo range of the navigation satellite Si at time t, (X)^t _si，Y^t _si，Z^t _si) Is the satellite position of the navigation satellite Si at time t. (X)^t _V，Y^t _V，Z^t _V) The current position of the vehicle 1 at the time t is calculated in S3.

In S5, the relative speed Vr calculated in S2 is substituted into the above equation (2)_iAnd the sight line vector (Gx) calculated in S4_i，Gy_i，Gz_i) And the velocity vector (Vxs) of the navigation satellite Si calculated in S1_i，Vys_i，Vzs_i) To calculate the satellite direction velocity Vs of the vehicle 1 in the direction of the navigation satellite Si_i。

In S6, the satellite direction velocity Vs calculated in S5 is substituted into equation (1)_iAnd a relative azimuth angle θ updated using the yaw rate acquired from the sensor data storage unit 111_gyroAnd the sight line vector (Gx) calculated in S4_i，Gy_iGzi). Thus, in the formula (1), the unknown parameter is θ⁰、Cbv⁰And a.

Therefore, three or more satellite direction velocities Vs substituted into equation (1) are established_iRelative azimuth angle theta_gyroLine of sight vector (Gx)_i，Gy_i，Gz_i) The latter equation. Then, the simultaneous equations composed of those three or more equations are solved. Thus, the azimuth angle initial value θ serving as an unknown parameter in equation (1) can be obtained⁰。

If the parameter theta is unknown⁰、Cbv⁰And a is after the initial time, the same even if the time is different. Therefore, it is not necessary to create three equations at the same time, and the unknown parameter can be obtained if the total number of equations created at a plurality of times is 3 or more. For example, even if 3 times (t) are assumed₀，t₁，t₂) Is 1, the initial azimuth value θ can be obtained using the observed satellite data⁰。

The INS azimuth determination unit 104 calculates the amount of change in azimuth angle (i.e., relative azimuth angle θ) by multiplying the yaw rate obtained from the detection value of the gyro sensor 4, which is an inertial sensor, by the sensor value acquisition period_gyro). Will be provided withThe azimuth angle variation is added to a reference azimuth angle to determine an azimuth angle θ. The reference azimuth is an azimuth initial value θ estimated by the doppler azimuth estimating unit 103⁰。

Since the yaw rate is obtained at the sensor value acquisition cycle, the INS bearing determination unit 104 updates the azimuth angle θ at the sensor value acquisition cycle. The azimuth angle theta is obtained by taking the initial value theta of the azimuth angle as a reference azimuth⁰Plus a relative azimuth angle theta_gyroTo obtain the final product.

However, the INS bearing determination unit 104 does not necessarily update the initial azimuth value θ every time the doppler bearing estimation unit 103 updates the initial azimuth value θ⁰Updating the reference azimuth to the latest initial value theta of the azimuth angle⁰. Because of the initial value of the azimuth angle theta⁰There is also an error, using the azimuth angle initial value θ used so far⁰It is also possible to be able to determine the azimuth angle θ with less error.

The processing executed by the INS direction determination unit 104 will be described with reference to fig. 3. The INS direction determination unit 104 executes the processing shown in fig. 3 at a sensor value acquisition cycle.

In S11, it is determined whether or not the doppler direction estimating unit 103 has calculated the initial azimuth angle value θ⁰. To calculate an initial value theta of the azimuth angle⁰Satellite data is required. Satellite data is acquired at a satellite data acquisition period that is longer than the sensor value acquisition period. In addition, if the radio wave environment is poor, the navigation signal may not be received. Therefore, the determination at S11 may be no. The doppler direction estimating unit 103 calculates an initial value θ of the direction angle after the previous execution of fig. 3⁰In the case of (3), the determination at S11 is yes. If the determination at S11 is yes, the process proceeds to S12.

In S12, it is determined whether or not the doppler direction estimation is continued. The doppler direction means a direction estimated by the doppler direction estimating unit 103, that is, an initial value θ of the azimuth direction⁰. The doppler direction estimation being continued means that the doppler direction estimation unit 103 is in a state in which the direction can be continuously estimated. When the doppler direction estimation unit 103 is capable of continuously estimating the direction for a predetermined time or more every satellite data acquisition cycle, the doppler direction estimation unit is assumed to be a doppler direction estimation unitAnd continues. The fixed time is set to a time equal to or longer than a plurality of times of the satellite data acquisition cycle, for example, 1 second.

If the doppler direction estimation is continued, the error of the estimated direction becomes small, that is, the reliability of the estimated direction becomes high. Therefore, if the determination at S12 is yes, the orientation reliability level is set to the highest level at S13. Further, the value of the state variable S is set to 0. The state variable S is explained in S14. S19 is proceeded to after S13 is executed.

If the determination at S12 is no, the routine proceeds to S14. In S14, a state variable S is calculated. In the present embodiment, the state variable S is calculated using the expression (5).

[ number 5]

In the formula (5), S⁰Is the initial value of the state variable S. The initial value S⁰This is a value used until S13 is executed after the power of the direction estimation device 100 is turned on. If S13 is executed, the state variable S becomes 0. Thus, if S13 is executed, the initial value S is set⁰Becomes 0.

When the previous end value, which is the value of the state variable S at the time of power-off of the previous direction estimation device 100, exists, the initial value S is set⁰Is the previous end value. If there is no previous end value, the value is set to a preset initial value.

In equation (5), tc is an azimuth initial value θ used by the INS azimuth determination unit 104 to calculate the azimuth θ⁰And a time point when the orientation reliability level at this time is the highest level is updated. However, tc is the time when the power is turned on until S13 is executed after the power of the direction estimation device 100 is turned on.

t is the current time, v is the vehicle speed, and A (v) is a value defined with the vehicle speed as a variable. Specifically, when the velocity v is relatively high, a (v) is relatively small, and when the velocity v is relatively low, a (v) is relatively large. For example, a (v) can be set as a function of: when the vehicle speed is 0, a (v) is 0, when the speed v is higher than a certain speed v1, a (v) becomes a1, and when the speed v is lower than the speed v1 and higher than 0, a (v) becomes a2 (> a 1).

The value of a (v) may be a function that continuously decreases as the speed v increases. Further, a (v) may be in the form of a table for determining a value corresponding to the velocity v.

The INS azimuth determination unit 104 updates the azimuth initial value θ used for the calculation of the azimuth θ⁰The longer the elapsed time thereafter, the larger the value of the state variable S. Updating the initial value theta of the azimuth angle⁰The longer the elapsed time thereafter, the greater the error in the azimuth angle θ, because the error in the yaw rate is accumulated. The state variable S is a variable for evaluating the error. Therefore, a (v) is determined in such a manner that the value of the state variable S is correlated with the error of the azimuth angle θ. As described above, the reason why a (v) is relatively reduced when the speed v is relatively large is because the error of the yaw rate is smaller as the speed v is higher.

In S15, the orientation reliability level is determined based on the state variable S and the orientation reliability level determination table shown in fig. 4. In FIG. 4, DIRLVn _ LMT (n is 1 to 5) is a threshold value for the state variable S. Therefore, the orientation reliability level determination table shown in fig. 4 is a table for determining the orientation reliability level based on the value of the state variable S.

In fig. 4, continuation of the azimuth estimation operation means that the doppler azimuth estimation described above is continued. The autonomous navigation means a positioning method for sequentially calculating the movement amount of the vehicle 1 based on the azimuth angle θ and the velocity v determined by the INS azimuth determination unit 104 and updating the current position of the vehicle 1.

In fig. 4, the greater the value of the azimuth reliability level, the higher the reliability of the azimuth angle θ, in other words, the smaller the error of the azimuth angle θ. In fig. 4, an azimuth reliability level of 7 means that the error is 0.5 degrees or less.

The degree of error when the bearing reliability level is the highest level is determined by measuring in advance how much the difference from the reference value converges. The reference value is a value measured by mounting a high-precision position measurement system including a high-precision gyro sensor, a high-resolution speedometer, and the like used in an aircraft on the vehicle 1. When the difference between the reference value and the direction estimated by the doppler direction estimating unit 103 converges to a certain range in a place where the radio wave environment is good, the range of the difference is set as the error at the highest level.

The threshold value for determining the state variable S of the bearing reliability level lower than the highest level is also determined by the difference between the actual measurement and the reference value measured by the high-precision position measurement system.

In the next S16, an evaluation parameter threshold group P is selected based on the bearing reliability level and the current velocity v determined in S15, and the threshold group selection table shown in fig. 5. The threshold group selection table is a table for selecting one evaluation parameter threshold group P from a plurality of groups of evaluation parameter threshold groups P.

Each evaluation parameter threshold value group P includes threshold values for a plurality of evaluation parameters (hereinafter, evaluation parameter threshold values). The evaluation parameter threshold is a threshold for evaluating an error of the azimuth estimated by the doppler azimuth estimating unit 103. The evaluation parameter is, for example, the Doppler shift frequency D of each navigation satellite Si_iMaximum, average, variance, covariance, time series variance, etc. of the residuals. As is known from the expressions (1) and (2), the frequency D is shifted by Doppler_iGiving an initial value of theta⁰Since the values of (a) and (b) have an influence, they become evaluation parameters for evaluating an error of the azimuth estimated by the doppler azimuth estimating unit 103.

Fig. 5 is a table for determining the evaluation parameter threshold group P based on the bearing reliability level and the velocity level. The evaluation parameter threshold groups P1 to P4 shown in fig. 5 are each composed of a plurality of evaluation parameter thresholds. Fig. 6 shows an example of the evaluation parameter threshold values included in the evaluation parameter threshold value groups P1 to P4. In fig. 6, THRE _ VAR1 _ A, THRE _ VAR1 _ B and the like are evaluation parameter thresholds.

In the threshold group selection table shown in fig. 5, the lower the azimuth reliability level, in other words, the lower the azimuth reliability level is a level indicating that the azimuth reliability is low, the lower the reliability is, the evaluation parameter threshold value becomes a value indicating that the azimuth reliability is low. The evaluation parameter threshold value has a value indicating a lower reliability as the speed level is lower.

Similarly to fig. 4, each of the evaluation parameter threshold values constituting the evaluation parameter threshold value group P shown in fig. 5 is obtained from the doppler shift frequency D actually obtained in a state where the difference between the measured reference value and the azimuth estimated by the doppler azimuth estimating unit 103 converges to the azimuth reliability level shown in fig. 4 by mounting the high-precision position measuring system on the vehicle 1_iEtc. are calculated and determined.

Therefore, when the doppler direction estimating unit 103 estimates the direction, the frequency D is shifted according to the doppler frequency at that time_iThe reliability of the azimuth estimated by the doppler azimuth estimating unit 103 can be evaluated by comparing the calculated evaluation parameter with an evaluation parameter threshold value.

Therefore, in S17, the doppler shift frequency D at the time of estimating the azimuth by the doppler azimuth estimating unit 103_iEtc. to calculate the evaluation parameters. In S18, each evaluation parameter threshold value constituting the evaluation parameter threshold group P selected in S16 is compared with the evaluation parameter calculated in S17, and it is determined whether or not the adoption condition is satisfied.

For example, the condition that all the evaluation parameters realize the evaluation parameter threshold value can be set. The evaluation parameter threshold value is realized by the evaluation parameter, which means that the evaluation parameter is a value on the side where the reliability of the orientation is higher than the evaluation parameter threshold value.

In addition to the adoption condition, the adoption condition may be satisfied when the evaluation parameter threshold is realized at a predetermined ratio (for example, 8 out) among the plurality of evaluation parameters. Further, a grade may be set for the achievement degree of each evaluation parameter and the evaluation may be made, and whether or not the adoption condition is satisfied may be determined using the total score of all the evaluation parameters.

If the determination at S18 is yes, it can be considered that the reliability of the azimuth estimated by the doppler azimuth estimating unit 103 is higher than the reliability of the azimuth θ at the current time. Therefore, if the determination at S18 is yes, the process proceeds to S19, and the initial azimuth value θ calculated by the doppler azimuth estimator 103 is used⁰The INS bearing determination unit 104 determines the reference bearing. That is, the azimuth initial value θ used for the INS azimuth determining unit 104⁰And (6) updating. In addition, inIn the case of executing S13, S19 is also executed. In S20, the slave update azimuth initial value θ is calculated⁰The variation of the azimuth angle up to the current time and the initial value theta of the azimuth angle⁰And added up to update the azimuth angle θ.

In S21, the azimuth angle θ updated in S20 and the latest azimuth reliability level are output to the application program using them. As an example of the application program, there is a position estimation application program for performing autonomous navigation using the azimuth angle θ and the velocity v of the vehicle 1.

In the processing of fig. 3, S14 corresponds to a state variable determination unit, S15 corresponds to a reliability level determination unit, S16 corresponds to a threshold selection unit, S17 corresponds to an evaluation parameter calculation unit, and S18 and S19 correspond to a non-use determination unit.

< summary of the embodiments >

The azimuth estimating device 100 of the present embodiment calculates a state variable S indicating the magnitude of the reliability of the azimuth angle θ (S14), and sets the evaluation parameter threshold group P to be compared with the plurality of evaluation parameters as the state variable S indicates that the reliability of the azimuth angle θ is lower, as the evaluation parameter threshold group P that easily satisfies the adoption condition even if the reliability of the azimuth angle θ is lower. Therefore, even if the reliability of the azimuth estimated by the doppler azimuth estimating unit 103 is not sufficiently high, it is used as the reference azimuth. This makes it easy to update the reference azimuth at an early stage, and therefore, an increase in error of the azimuth estimated by autonomous navigation can be suppressed.

The embodiments have been described above, but the disclosed technology is not limited to the above-described embodiments, and the following modifications are included in the disclosed scope, and various changes can be made without departing from the gist of the invention in addition to the following.

The flowchart or flowchart process described in this application is configured by a plurality of units (or steps), and each unit is represented as S2, for example. Further, each part may be divided into a plurality of sub-parts, or a plurality of parts may be combined into one part. Further, each unit configured as described above may be referred to as an apparatus, a module, or a method.

< modification 1 >

The formula (1) shown in the above embodiment is such that the following constraint conditions 1 to 3 are adopted for the satellite directional velocity Vs as described in patent document 1_iAnd the speed vector of the vehicle 1. Further, Vx, Vy are the x component and y component of the velocity vector of the vehicle 1, respectively.

[ number 6]

Constraint 1

Constraint 2Vz^t＝O

Constraint 3Cbv^t＝Cbv⁰+At

As disclosed in patent document 1, all of the three constraints need not be used as constraints. The constraint condition may be set to use only any one of the vehicle speed data, the azimuth angle change amount, and the time change of the clock drift Cbv. In addition, constraints may be set by appropriately combining the respective components.

As a constraint, in the case where only the azimuth angle variation is used, V is represented by formula (1)^t _wheel、θ⁰And Cbv^tBecomes an unknown parameter. Therefore, the number of unknown parameters in all time points of the time-series data is "the number of time points × 2+ 1".

In addition, as a constraint condition, when only the time change of the clock drift Cbv is used, V is represented by formula (1)^t _wheel、θ⁰、θ^t _gyro、Cbv⁰And a becomes an unknown parameter, the number of unknown parameters at all times in the time series data is "the number of times × 2+ 3".

In addition, as the constraint condition, when the time change of the clock drift Cbv and the vehicle speed data are used, in the formula (1), θ⁰、θ^t _gyro、Cbv⁰And a becomes an unknown parameter, the number of unknown parameters at all times in the time series data is "the number of times × 1+ 3".

< modification 2 >

In the above-described embodiment, the mobile body is a vehicle, but the mobile body may be other than a vehicle.