Azimuth calculation device and azimuth calculation method

By combining satellite signals and angular velocity sensor data in the navigation device, the temperature characteristic information of angular velocity deviation is updated in real time, and the problem of reducing the accuracy of angular velocity deviation in the prior art is solved, and high-precision azimuth calculation under different temperature conditions is realized.

CN119063689APending Publication Date: 2024-12-03SEIKO EPSON CORP
View PDF 1 Cites 0 Cited by

Patent Information

Application Number
CN202410689860.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Priority Date
2023-06-01
Filing Date
2024-05-30
Publication Date
2024-12-03

AI Technical Summary

Technical Problem

The existing navigation device may reduce the accuracy of estimating angular velocity deviation and cannot effectively update the temperature characteristic data when the moving body is stopped few times or the temperature when it is driven.

Method used

Using an azimuth calculation device, combined with satellite signals and angular velocity sensor data, the temperature characteristic information of angular velocity deviation is updated in real time through the temperature characteristic estimation and angular velocity deviation prediction components, and composite navigation is performed based on satellite signals and angular velocity signals to improve the correction accuracy of angular velocity deviation.

Benefits of technology

Whether the moving body is stopped or driven, it can calculate the angular velocity deviation with high accuracy to ensure the accuracy and stability of the azimuth calculation, and adapt to changes in angular velocity deviation under different temperature conditions.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119063689A_ABST
    Figure CN119063689A_ABST
Patent Text Reader

Abstract

The invention discloses an orientation calculation device and an orientation calculation method. An orientation calculation device is provided with: a first orientation calculation unit that calculates a first orientation on the basis of a satellite signal or a detection result from an external sensor; a second orientation calculation unit that calculates a second orientation on the basis of the angular velocity signal; a third orientation calculation unit that calculates a third orientation on the basis of the first orientation and the second orientation; a storage unit that stores temperature characteristic information of the angular velocity deviation; a temperature characteristic estimation unit that estimates the temperature characteristic of the angular velocity deviation on the basis of the second orientation and the third orientation, and updates the temperature characteristic information; and an angular velocity deviation prediction unit that predicts the angular velocity deviation on the basis of the temperature and the temperature characteristic information, and the second orientation calculation unit calculates the second orientation on the basis of the predicted angular velocity deviation.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to a position calculation device and a position calculation method. Background Art

[0002] Patent document 1 describes a navigation device that calculates temperature characteristic data of angular velocity deviation based on temperature detection values ​​and angular velocity detection values ​​when a moving body stops. When the temperature characteristic data of angular velocity deviation is valid, the angular velocity deviation calculated based on the temperature characteristics of the angular velocity sensor and the temperature is used as an initial value to perform GSP / INS comprehensive calculations, estimate the angular velocity deviation and angular velocity in real time, and calculate the orientation of the moving body.

[0003] Patent Document 1: Japanese Patent Application Publication No. 2009-192495

[0004] However, in the control device described in Patent Document 1, it is necessary to accumulate the relationship between the temperature detection value and the angular velocity detection value when the moving body stops to obtain the temperature characteristic data of the angular velocity deviation. Therefore, when the number of times the moving body stops is small and the amount of accumulated data is insufficient, or when the temperature during driving is different from the temperature when stopped, the estimated accuracy of the angular velocity deviation may be reduced. Summary of the invention

[0005] One embodiment of the azimuth calculation device involved in the present invention is provided with: a first azimuth calculation unit, which calculates a first azimuth based on a satellite signal received by a satellite signal receiver or a detection result of an external sensor; a second azimuth calculation unit, which calculates a second azimuth based on an angular velocity signal output by an angular velocity sensor when the angular velocity is detected; a third azimuth calculation unit, which calculates a third azimuth based on the first azimuth and the second azimuth; a storage unit, which stores temperature characteristic information of an angular velocity deviation; a temperature characteristic estimating unit, which estimates the temperature characteristic of the angular velocity deviation relative to a temperature detected by a temperature sensor based on the second azimuth and the third azimuth, and updates the temperature characteristic information stored in the storage unit based on the estimated temperature characteristic; and an angular velocity deviation predicting unit, which predicts the angular velocity deviation based on the temperature detected by the temperature sensor and the temperature characteristic information stored in the storage unit, wherein the second azimuth calculation unit calculates the second azimuth after the angular velocity deviation is corrected based on the angular velocity deviation predicted by the angular velocity deviation predicting unit last time.

[0006] One aspect of the azimuth calculation method involved in the present invention is to include: a first azimuth calculation step, calculating a first azimuth based on a satellite signal received by a satellite signal receiver or a detection result of an external sensor; a second azimuth calculation step, calculating a second azimuth based on an angular velocity signal output by an angular velocity sensor when detecting an angular velocity; a third azimuth calculation step, calculating a third azimuth based on the first azimuth and the second azimuth; a temperature characteristic estimation step, estimating a temperature characteristic of an angular velocity deviation relative to a temperature detected by a temperature sensor based on the second azimuth and the third azimuth, and updating temperature characteristic information of the angular velocity deviation stored in a storage unit based on the estimated temperature characteristic; and an angular velocity deviation prediction step, predicting the angular velocity deviation based on the temperature detected by the temperature sensor and the temperature characteristic information stored in the storage unit, wherein in the second azimuth calculation step, the second azimuth after the angular velocity deviation is corrected is calculated based on the last angular velocity deviation predicted in the angular velocity deviation prediction step. BRIEF DESCRIPTION OF THE DRAWINGS

[0007] Figure 1 It is a diagram showing a configuration example of the direction calculation device according to the first embodiment.

[0008] Figure 2 This is a diagram for explaining the X-axis, the Y-axis, and the Z-axis.

[0009] Figure 3 This is a diagram showing an example of temperature characteristic information.

[0010] Figure 4 This is a diagram showing an example of the temperature characteristics of the angular velocity deviation.

[0011] Figure 5 This is a flowchart showing an example of the procedure of the direction calculation method according to the present embodiment.

[0012] Figure 6 This is a flowchart showing an example of specific steps of the direction calculation method according to the first embodiment.

[0013] Figure 7 This is a flowchart showing an example of specific steps of the direction calculation method according to the second embodiment.

[0014] Figure 8 It is a diagram showing a configuration example of a direction calculation device according to a third embodiment.

[0015] Fig. 9 This is a flowchart showing an example of specific steps of the direction calculation method according to the third embodiment.

[0016] Fig.10 This is a flowchart showing another example of specific steps of the direction calculation method according to the third embodiment.

[0017] Description of Reference Numerals

[0018] 1…position calculation device, 5…vehicle, 10…sensor module, 11…satellite signal receiver, 12…wheel speed sensor, 13…angular velocity sensor, 14…6DoF sensor, 15…temperature sensor, 16…external sensor, 20…first position calculation unit, 30…second position calculation unit, 40…third position calculation unit, 50…angular velocity deviation prediction unit, 60…temperature characteristic estimation unit, 70…storage unit, 71…temperature characteristic information, 72…map information DETAILED DESCRIPTION

[0019] Hereinafter, preferred embodiments of the present invention will be described in detail using the accompanying drawings. In addition, the embodiments described below do not unduly limit the content of the present invention described in the scope of the claims. In addition, the components described below are not limited to the essential components of the present invention.

[0020] 1. First Implementation

[0021] 1-1. Direction calculation device

[0022] Figure 1 FIG. 2 is a diagram showing a configuration example of a direction calculation device according to the first embodiment. Figure 1 As shown, the first embodiment of the orientation calculation device 1 includes a first orientation calculation unit 20, a second orientation calculation unit 30, a third orientation calculation unit 40, an angular velocity deviation prediction unit 50, a temperature characteristic estimation unit 60 and a storage unit 70. In addition, the orientation calculation device 1 may also include a satellite signal receiver 11, a wheel speed sensor 12, an angular velocity sensor 13, a 6DoF sensor 14 and a temperature sensor 15. Dof is the abbreviation of Degrees Of Freedom. In addition, the orientation calculation device 1 may also be configured to omit or change Figure 1 part of the constituent elements or adding other constituent elements.

[0023] The position calculation device 1 is mounted on a mobile body. In the mobile body equipped with the position calculation device 1, a three-axis coordinate system based on the X-axis, Y-axis and Z-axis, i.e., a local coordinate system, is defined. The X-axis is an axis with the moving direction of the mobile body as positive, the Y-axis is an axis with the right direction perpendicular to the moving direction of the mobile body as positive, and the Z-axis is an axis with the downward direction perpendicular to the plane on which the mobile body is traveling as positive. In addition, a global coordinate system is defined in the space in which the mobile body moves. The global coordinate system is a coordinate system fixed to the earth. In the following description, it is assumed that the position calculation device 1 is mounted on a car as a mobile body.

[0024] The satellite signal receiver 11 receives satellite signals transmitted from satellites via an antenna not shown. Satellites constitute a part of a satellite positioning system. The satellite positioning system may be a GNSS (Global Navigation Satellite System). Examples of GNSS include GPS (Global Positioning System), EGNOS (European Geostationary Navigation Overlay Service), QZSS (Quasi Zenith Satellite System), GLONASS (Global Navigation Satellite System), GALILEO, BeiDou (Bei Dou Navigation Satellite System), and the like.

[0025] The wheel speed sensor 12 detects the rotation speed of the wheels of a vehicle as a moving object, and outputs a wheel speed signal.

[0026] The angular velocity sensor 13 detects the angular velocity and outputs an angular velocity signal. Specifically, the angular velocity sensor 13 has a detection axis and is mounted on the moving body in a manner such that the detection axis is along the Z axis, detects the angular velocity around the Z axis, and outputs a Z-axis angular velocity signal. For example, the angular velocity sensor 13 may also be a crystal gyro sensor having a sensor element (not shown) made of crystal and detecting the angular velocity with high accuracy.

[0027] The 6DoF sensor 14 detects the angular velocity around three mutually orthogonal axes and the acceleration in the three-axis directions, and outputs a three-axis angular velocity signal and a three-axis acceleration signal. Specifically, the 6DoF sensor 14 has three mutually orthogonal detection axes, and is mounted on the moving body in a manner that the three detection axes are along the X-axis, the Y-axis, and the Z-axis, respectively. In addition, the 6DoF sensor 14 detects the angular velocity around the X-axis, the angular velocity around the Y-axis, and the angular velocity around the Z-axis and outputs a three-axis angular velocity signal, and detects the acceleration in the X-axis direction, the acceleration in the Y-axis direction, and the acceleration in the Z-axis direction and outputs a three-axis acceleration signal. For example, the 6DoF sensor 14 may also be an electrostatic capacitance type sensor made by processing a silicon substrate using MEMS technology.

[0028] For example, the angular velocity sensor 13 and the 6DoF sensor 14 may constitute a sensor module 10, which is housed in a housing in such a way that the detection axis of the angular velocity sensor 13 and any one of the three detection axes of the 6DoF sensor 14 are in the same direction, and detects angular velocities around the three axes and accelerations in the directions of the three axes. In this case, for example, Figure 2 As shown in FIG. 1 , the sensor module 10 is mounted on a car 5 as a moving body in a manner that three axes are along the X axis, the Y axis, and the Z axis. In addition, the angular velocity sensor 13 detects the angular velocity around the Z axis, and the 6DoF sensor 14 detects the angular velocity around the X axis, the angular velocity around the Y axis, the angular velocity around the Z axis, the acceleration in the X axis direction, the acceleration in the Y axis direction, and the acceleration in the Z axis direction. The sensor module 10 may also output data including the angular velocity around the Z axis detected by the angular velocity sensor 13, and the angular velocities around the three axes and the accelerations in the three axis directions detected by the 6DoF sensor 14. In addition, for example, when the accuracy of the angular velocity around the Z axis detected by the angular velocity sensor 13 is higher than the angular velocity around the Z axis detected by the 6DoF sensor 14, the sensor module 10 may also output data including the angular velocity around the Z axis detected by the angular velocity sensor 13, and the angular velocity around the X axis, the angular velocity around the Y axis, and the accelerations in the three axis directions detected by the 6DoF sensor 14.

[0029] However, the angular velocity sensor 13 and the 6DoF sensor 14 may be separate bodies from each other.

[0030] The temperature sensor 15 detects temperature and outputs a temperature signal. For example, the temperature sensor 15 is disposed near the angular velocity sensor 13 and detects the temperature of the angular velocity sensor 13. Alternatively, the angular velocity sensor 13 and the temperature sensor 15 may be housed in one housing.

[0031] The first orientation calculation unit 20 calculates the orientation of the mobile object in the global coordinate system at time k, that is, the first orientation Ψ based on the satellite signal received by the satellite signal receiver 11. gnss,k The first orientation calculation unit 20 calculates the first orientation Ψ at a predetermined period such as 1 second. gnss,k For example, the first azimuth calculation unit 20 may calculate the velocity vector of the moving body by utilizing the phenomenon that the frequency of the satellite signal is shifted according to the relationship between the position of the satellite and the velocity of the moving body due to the Doppler effect, and calculate the first azimuth Ψ according to the direction of the velocity vector. gnss,k In addition, for example, the first orientation calculation unit 20 may calculate the position of the mobile body in the global coordinate system based on the satellite signal, calculate the velocity vector by differentiating the position, and calculate the first orientation Ψ according to the direction of the velocity vector. gnss,kIn addition, for example, antennas for receiving satellite signals may be respectively provided at two different positions of the mobile body, and the satellite signal receiver 11 calculates the positions of the two antennas based on the satellite signals received by the two antennas, and calculates the first azimuth Ψ based on the calculated two positions. gnss,k Alternatively, two satellite signal receivers 11 may be mounted on the mobile body, two positions may be calculated based on satellite signals received by the two satellite signal receivers 11, and the first orientation Ψ may be calculated based on the calculated two positions. gnss,k In addition, the first orientation calculation unit 20 may further calculate the position and speed of the moving object in the global coordinate system through well-known satellite navigation.

[0032] The second orientation calculation unit 30 calculates the second orientation Ψ, which is the orientation of the moving object in the global coordinate system at time k, based on the angular velocity signal output from the angular velocity sensor 13. ins,k The second azimuth calculation unit 30 calculates the angular velocity deviation δΩ at the previous time k-1 predicted by the angular velocity deviation prediction unit 50. k-1 , calculate the second orientation Ψi at time k after the angular velocity deviation is corrected ins,k The second orientation calculation unit 30 calculates the second orientation Ψ at a predetermined period such as 0.01 seconds. ins,k Specifically, the second orientation calculation unit 30 calculates the angular velocity ω about the Z axis at the time k detected by the angular velocity sensor 13. z and the angular velocity ω around the X axis at time k detected by the 6DoF sensor 14 x and the angular velocity ω around the Y axis y , the angular velocities of the roll angle φ, pitch angle θ and yaw angle ψ are calculated by equation (1). Figure 2 As shown in FIG. 1 , the roll angle φ is a rotation angle with the X-axis along the traveling direction of the automobile 5 as the rotation axis. In addition, the pitch angle θ is a rotation angle with the Y-axis along the right direction perpendicular to the traveling direction of the automobile 5 as the rotation axis. In addition, the yaw angle ψ is a rotation angle with the Z-axis along the downward direction perpendicular to the plane on which the automobile 5 is traveling as the rotation axis. The yaw angle ψ indicates the relative orientation of the moving body, and the roll angle φ and the pitch angle θ indicate the posture of the moving body.

[0033] [Formula 1]

[0034]

[0035] Then, the second orientation calculation unit 30 calculates the third orientation Ψ at the previous time k-1 calculated by the third orientation calculation unit 40 est,k-1 As the initial orientation, the third orientation Ψ est,k-1 Add the angular velocity from the yaw angle ψ minus the angular velocity deviation δΩ k-1 And the integrated value is used to calculate the second position Ψ at time kins,k In the driving control of the automobile 5, the yaw angle ψ is the most important among the roll angle φ, the pitch angle θ, and the yaw angle ψ. Therefore, in order to calculate the yaw angle ψ with high accuracy, the orientation calculation device 1 includes an angular velocity sensor 13 that detects the angular velocity around the Z axis with high accuracy in addition to the 6DoF sensor 14.

[0036] The third orientation calculation unit 40 calculates the first orientation Ψ at time k based on the first orientation calculation unit 20. gnss,k and the second orientation Ψ at time k calculated by the second orientation calculation unit 30 ins,k , calculate the orientation of the moving body in the global coordinate system at time k, that is, the third orientation Ψ est,k The third position calculation unit 40 calculates the third position Ψ at a predetermined period such as 1 second by using a known composite navigation method combining satellite navigation and inertial navigation. est,k For example, the third orientation calculation unit 40 may also calculate the first orientation Ψ gnss,k The timing of the first position Ψ gnss,k As a third party est,k , without calculating the first position Ψ gnss,k The timing of the second position Ψ ins,k As a third party est,k In addition, the third position calculation unit 40 may also use a Kalman filter to calculate the third position Ψ est,k In addition, the third position calculation unit 40 may further calculate the position of the moving body based on the speed, posture and orientation of the moving body. The speed of the moving body may be the speed calculated by the first orientation calculation unit 20, or the speed calculated based on the wheel speed signal output from the wheel speed sensor 12. In addition, the posture of the moving body is determined by the roll angle Φ est,k and pitch angle Θ est,k It may be the roll angle φ and the pitch angle θ calculated by the second orientation calculation unit 30 .

[0037] The storage unit 70 stores the angular velocity deviation δΩ k In this embodiment, the angular velocity deviation δΩ k is the second orientation Ψ calculated by the second orientation calculation unit 30 ins,k The deviation of the angular velocity is expressed by equation (2). ins,k The second position ins,k The error of the second position Ψ is shown in formula (3). ins,k The third position Ψ at time k calculated by the third position calculation unit 40 est,k The Δt in equation (2) is the difference between the third position calculation unit 40 and the third position Ψ. est,k The period is, for example, 1 second.

[0038] [Formula 2]

[0039]

[0040] [Formula 3]

[0041] δΨ ins,k =Ψ ins,k -Ψ est,k …(3)

[0042] Angular velocity deviation δΩ k As shown in equation (4), the temperature change ΔT at time k is k The temperature change ΔT is approximated by an n-degree polynomial of the variable. k is the temperature T detected by the temperature sensor 15 at time k k and the temperature T at time k-1 k The difference.

[0043] [Formula 4]

[0044]

[0045] In this embodiment, the temperature characteristic information 71 includes the coefficient c of the polynomial (4) for each of a plurality of temperatures. 0,k ~c n,k The value of . Figure 3 71 is a diagram showing an example of the temperature characteristic information 71. Figure 3 In the example of FIG. 1 , the temperature characteristic information 71 includes a coefficient c for each 1°C temperature from -40°C to +85°C. 0,k ~c n,k In addition, the temperature characteristic information 71 may not include the coefficient c for at least one temperature. 0,k ~c n,k At least one of the values ​​​​of .

[0046] The temperature characteristic estimation unit 60 determines the second orientation Ψ based on the second orientation calculation unit 30. ins,k and the third position Ψ calculated by the third position calculation unit 40 est,k , it is estimated that the temperature T detected by the temperature sensor 15 k Angular velocity deviation δΩ k The temperature characteristic estimating unit 60 may estimate the coefficient c of the polynomial (4) and update the temperature characteristic information 71 stored in the storage unit 70 based on the estimated temperature characteristic. 0,k ~c n,k The value is relative to the temperature T k Angular velocity deviation δΩ k The estimated coefficient c is calculated based on the temperature characteristics of 0,k ~cn,k The temperature characteristic estimation unit 60 includes the temperature characteristic information 71 for the temperature T k The coefficient c of the polynomial (4) i,k In the case of the value of coefficient c i,k When the variance of is less than a predetermined threshold, the coefficient c included in the temperature characteristic information 71 is i,k The value of is updated to the estimated coefficient c i,k The value of coefficient c i,k When the variance of is greater than a predetermined threshold, the coefficient c included in the temperature characteristic information 71 is not updated. i,k That is, the temperature characteristic estimation unit 60 may also be configured to estimate the value of k The coefficient c i,k When the estimation accuracy of the value of is greater than or equal to a predetermined value, the temperature T included in the temperature characteristic information 71 is updated. k The coefficient c i,k The storage unit 70 stores Figure 3 In the case of the temperature characteristic information 71 shown in FIG. k is 25℃, coefficient c 0,k ~c n,k If all the variances of are less than a predetermined threshold, the temperature characteristic estimation unit 60 sets the coefficient c for 25°C included in the temperature characteristic information 71 to 0,k ~c n,k The value of c 0(+25℃) ~c n(+25℃) Update to the estimated coefficient c 0,k ~c n,k The value of .

[0047] The temperature characteristic estimation unit 60 does not include the temperature characteristic information 71 for the temperature T k The coefficient c of the polynomial (4) 0,k ~c n,k In the case of a value of , the coefficient c included in the temperature characteristic information 71 is set regardless of the value of the variance. 0,k ~c n,k The value of is updated to the estimated coefficient c 0,k ~c n,k The value of .

[0048] The temperature characteristic estimation unit 60 updates the third position Ψ est,k The angular velocity deviation δΩ is estimated based on a predetermined period, such as a short period such as 1 second. k Therefore, in each cycle of the temperature characteristic estimation unit 60 estimating the temperature characteristic, the temperature change ΔT k Tiny. Figure 4 As shown, if the temperature changes ΔT kis small, it can be regarded as an angular velocity deviation δΩ k Relative to temperature change ΔT k changes linearly. That is, the angular velocity deviation δΩ k As shown in equation (5), the temperature change ΔT k Formula (5) is equivalent to the polynomial (4) when n is 1.

[0049] [Formula 5]

[0050]

[0051] Therefore, the temperature characteristic estimation unit 60 may estimate the temperature T corresponding to the temperature signal output from the temperature sensor 15. k The first-order coefficient c of the polynomial of formula (5) is 1,k and the 0th-order coefficient c 0,k In the present embodiment, the temperature characteristic estimation unit 60 estimates the first-order coefficient c for the temperature Tk using an extended Kalman filter. 1,k and the 0th-order coefficient c 0,k .

[0052] In this embodiment, as shown in formula (6), the linear coefficient c is defined as 1,k , 0th-order coefficient c 0,k and temperature T k is the state variable x of the element k .

[0053] [Formula 6]

[0054]

[0055] The state equation is defined by equation (7). In equation (7), F k is the state transfer matrix, as shown in equation (8), which is a 3×3 unit matrix. In addition, in equation (7), v k is the driving noise, as shown in equation (9), σ is pre-set to an appropriate value v1 , σ v2 , σ v3 As an element.

[0056] [Formula 7]

[0057] x k =F k x k-1 +v k …(7)

[0058] [Formula 8]

[0059]

[0060] [Formula 9]

[0061]

[0062] In this embodiment, as shown in equation (10), the angular velocity deviation δΩ is defined as k 、Angular velocity deviation change δΩ k -δΩ k-1 , Temperature T k is the observed variable y of the factor k .

[0063] [Formula 10]

[0064]

[0065] The observation equation is defined by equation (11). In equation (11), H k is the observation Jacobian, expressed by equation (12). In addition, in equation (11), w k is the observation noise, as shown in equation (13), and σ is pre-set to an appropriate value w1 , σ w2 , σ w3 As an element.

[0066] [Formula 11]

[0067]

[0068] [Formula 12]

[0069]

[0070] [Formula 13]

[0071]

[0072] The temperature characteristic estimation unit 60 performs a prediction step, an observation step, and an update step of the extended Kalman filter to estimate the angular velocity deviation δΩ k Temperature characteristics.

[0073] First, in the prediction step, the temperature characteristic estimation unit 60 calculates the temperature characteristic according to the state variable x^ updated in the update step at time k-1 by using equation (14) based on the state equation (7). k-1|k-1 , predict the state variable x^ at time k k|k-1 That is, the state variable x^ k-1|k-1 is the post-estimation value at time k-1, the state variable x^ k|k-1 is the a priori estimated value at time k.

[0074] [Formula 14]

[0075]

[0076] In addition, the temperature characteristic estimation unit 60 calculates the temperature characteristic estimation unit 60 in the prediction step according to the state variable x^ updated in the update step at time k-1 by using equation (15). k-1|k-1 The covariance matrix P k-1|k-1 , predict the state variable x^ at time k k|k-1 The covariance matrix P k|k-1 In formula (15), V k is the driving noise v k The covariance matrix of is calculated by formula (16).

[0077] [Formula 15]

[0078] P k|k-1 =F k P k-1|k-1 F k T +V k =P k-1|k-1 +V k …(15)

[0079] [Formula 16]

[0080]

[0081] Next, in the observation step, the temperature characteristic estimation unit 60 calculates the state variable x^ predicted in the prediction step at time k by using equation (17) based on the observation equation (11). k|k-1 Observed value. In formula (17), the temperature change ΔT k As shown in formula (18), the state variable x^ k|k-1 The temperature T contained in k|k-1 With the state variable x^ k-1|k-1 The temperature T contained in k-1|k-1 In addition, the temperature change ΔT k-1 As shown in formula (19), the state variable x^ k-1|k-1 The temperature T contained in k-1|k-1 With the state variable x^ k-2|k-2 The temperature T contained in k-2|k-2 The difference.

[0082] [Formula 17]

[0083]

[0084] [Formula 18]

[0085] ΔT k =T k|k-1 -T k-1|k-1 …(18)

[0086] [Formula 19]

[0087] ΔT k-1 =T k-1|k-1 -T k-2|k-2 …(19)

[0088] Then, the temperature characteristic estimation unit 60 calculates the observation residual e by equation (20): k , the observed residual e k is the observed variable y at time k k The difference between the value of and the value calculated by equation (17).

[0089] [Formula 20]

[0090]

[0091] In addition, the temperature characteristic estimation unit 60 calculates the state variable x^ according to the state variable x^ predicted in the prediction step at time k by using equation (21) in the observation step. k|k-1 The covariance matrix P k|k-1 , calculate the observation residual e k The covariance matrix S k In formula (21), W k is the observation noise w k The covariance matrix of is calculated by equation (22). In addition, H k is the observation Jacobian matrix, calculated by equation (23).

[0092] [Formula 21]

[0093] S k =W k +H k P k|k-1 H k T …(twenty one)

[0094] [Formula 22]

[0095]

[0096] [Formula 23]

[0097]

[0098] Finally, in the updating step, the temperature characteristic estimating unit 60 calculates the state variable x^ according to the state variable x^ predicted in the prediction step at the time k. k|k-1 The covariance matrix P k|k-1 , and the observation residual e calculated in the observation step at time k k The covariance matrix S k, calculate the Kalman gain K through formula (24) k .

[0099] [Formula 24]

[0100] K k =P k|k-1 H k T S k -1 …(twenty four)

[0101] Then, in the updating step, the temperature characteristic estimating unit 60 calculates the temperature characteristic based on the Kalman gain K according to equation (25). k and the observation residual e calculated in the observation step at time k k , the state variable x^ predicted in the prediction step at time k k|k-1 Update to the state variable x^ k|k As mentioned above, the state variable x^ k|k-1 is the pre-estimated value at time k. k|k is the ex post facto estimated value at time k.

[0102] [Formula 25]

[0103]

[0104] In this way, the temperature characteristic estimation unit 60 estimates the state variable x using the extended Kalman filter. k The first-order coefficient c contained in 1,k and the 0th-order coefficient c 0,k .

[0105] In the updating step, the temperature characteristic estimating unit 60 calculates the temperature characteristic based on the Kalman gain K according to equation (26). k , the state variable x^ predicted in the prediction step at time k k|k-1 The covariance matrix P k|k-1 Update to the state variable x^ k|k The covariance matrix P k|k .

[0106] [Formula 26]

[0107]

[0108] In the covariance matrix P k|k In, p 11 is the first-order coefficient c 1,k The variance, p 22 is the 0th degree coefficient c 0,k The temperature characteristic estimation unit 60 includes the temperature characteristic information 71 for the temperature T kThe first-order coefficient c 1,k In the case of the value of 11 When the first-order coefficient c contained in the temperature characteristic information 71 is less than a predetermined threshold value, 1,k The value of is updated to the estimated first-order coefficient c 1,k The value of variance p 11 If the value is greater than a predetermined threshold, the first-order coefficient c contained in the temperature characteristic information 71 is not updated. 1,k The temperature characteristic estimation unit 60 does not include the value of the temperature T in the temperature characteristic information 71. k The first-order coefficient c 1,k , regardless of the variance p 11 Regardless of the value of 1,k The value of is updated to the estimated first-order coefficient c 1,k The value of .

[0109] Similarly, the temperature characteristic estimation unit 60 includes the temperature characteristic information 71 for the temperature T k The 0th-order coefficient c 0,k In the case of the value of 22 When the zero-order coefficient c contained in the temperature characteristic information 71 is less than a predetermined threshold value, 0,k The value of is updated to the estimated 0th-order coefficient c 0,k The value of variance p 22 If the zero-order coefficient c contained in the temperature characteristic information 71 is greater than a predetermined threshold, the zero-order coefficient c contained in the temperature characteristic information 71 is not updated. 0,k The temperature characteristic estimation unit 60 does not include the value of the temperature T in the temperature characteristic information 71. k The 0th-order coefficient c 0,k , regardless of the variance p 22 Regardless of the value of 0,k The value of is updated to the estimated 0th-order coefficient c 0,k The value of .

[0110] The angular velocity deviation prediction unit 50 determines the angular velocity deviation based on the temperature T detected by the temperature sensor 15. k and the temperature characteristic information 71 stored in the storage unit 70, the angular velocity deviation δΩ is predicted k For example, the angular velocity deviation prediction unit 50 may also use the temperature T k-1 , Temperature T k and the temperature characteristic information 71 for the temperature T k The coefficient c 0,k ~c n,k Substitute into polynomial (4) to predict the angular velocity deviation δΩ k .

[0111] The direction calculation device 1 configured in this way can estimate the angular velocity deviation δΩ at a predetermined period such as 1 second, regardless of whether the automobile 5 as a moving object is stopped or moving. k , calculate the high-precision third position Ψ after the angular velocity deviation is corrected est,k .

[0112] Furthermore, the person or place that manufactures the angular velocity sensor 13 and the person or place that modularizes part or all of the orientation calculation device 1 including the angular velocity sensor 13 may be the same as or different from each other.

[0113] 1-2. Direction calculation method

[0114] Figure 5 FIG. 1 is a flowchart showing an example of the steps of the method for calculating the direction of the present embodiment. Figure 5 As shown, the orientation calculation method of this embodiment includes a first orientation calculation step S10, a second orientation calculation step S20, a third orientation calculation step S30, a temperature characteristic estimation step S40, and an angular velocity deviation prediction step S50. Figure 5 The direction calculation device 1 may also perform the processing of each step by executing a program not shown in the figure.

[0115] First, in the first orientation calculation step S10, the first orientation calculation unit 20 of the orientation calculation device 1 calculates the first orientation Ψ based on the satellite signal received by the satellite signal receiver 11. gnss,k .

[0116] Next, in the second orientation calculation step S20, the second orientation calculation unit 30 of the orientation calculation device 1 calculates the second orientation Ψ based on the angular velocity signal detected by the angular velocity sensor 13 and outputted. ins,k The second orientation calculation unit 30 calculates the angular velocity deviation δΩ based on the previous angular velocity deviation δΩ predicted in the angular velocity deviation prediction step S50 described later. k-1 , calculate the second orientation Ψ after the angular velocity deviation is corrected ins,k .

[0117] Next, in the third orientation calculation step S30, the third orientation calculation unit 40 of the orientation calculation device 1 calculates the first orientation Ψ based on the first orientation calculation step S10. gnss,k and the second orientation Ψ calculated in the second orientation calculation step S20 ins,k , calculate the third position Ψ est,k .

[0118] Next, in the temperature characteristic estimating step S40, the temperature characteristic estimating unit 60 of the orientation calculating device 1 calculates the second orientation Ψ based on the second orientation calculating step S20. ins,k and the third position Ψ calculated in the third position calculation step S30 est,k , it is estimated that the temperature T detected by the temperature sensor 15 k Angular velocity deviation δΩ k The angular velocity deviation δΩ stored in the storage unit 70 is updated based on the estimated temperature characteristic. k The temperature characteristic information 71.

[0119] The temperature characteristic estimation unit 60 may also estimate the coefficient c of the polynomial (4): 0,k ~c n,k The value of angular velocity deviation δΩ k The estimated coefficient c is calculated based on the temperature characteristics of 0,k ~c n,k The temperature characteristic information 71 includes the coefficient c for the temperature Tk. i,k In the case of the value of coefficient c i,k When the variance of is less than a predetermined threshold, the coefficient c included in the temperature characteristic information 71 is i,k The value of is updated to the estimated coefficient c i,k The value of coefficient c i,k When the variance of is greater than a predetermined threshold, the coefficient c included in the temperature characteristic information 71 is not updated. i,k The value of .

[0120] Next, in the angular velocity deviation prediction step S50, the angular velocity deviation prediction unit 50 of the direction calculation device 1 calculates the angular velocity deviation based on the temperature T detected by the temperature sensor 15. k and the temperature characteristic information 71 stored in the storage unit 70, the angular velocity deviation δΩ is predicted k .

[0121] The direction calculation device 1 repeats steps S10 to S50 until the direction calculation process is completed in step S60 .

[0122] Figure 6 This is a flowchart showing an example of specific steps of the direction calculation method according to the first embodiment.

[0123] like Figure 6 As shown, first, in step S1, the direction calculation device 1 initializes the time k to 0.

[0124] Next, when the satellite signal receiver 11 receives a satellite signal in step S2, the first direction calculation unit 20 of the direction calculation device 1 calculates the first direction Ψ based on the satellite signal received by the satellite signal receiver 11 in step S11. gnss,k If the satellite signal receiver 11 fails to receive the satellite signal in step S2, the first azimuth calculation unit 20 does not perform the processing of step S11. Step S11 is equivalent to Figure 5 The first orientation calculation step S10.

[0125] Next, in step S21, the second orientation calculation unit 30 of the orientation calculation device 1 sets the second orientation Ψ ins,k Specifically, the third position Ψ calculated in the step S31 described later is est,k-1 Set to the second position ins,k In addition, the second orientation calculation unit 30 does not calculate the last third orientation Ψ in step S31. est,k-1 In the case of the previous second orientation Ψ calculated in step S23 described later, ins,k-1 Set to the second position ins,k In addition, when the time k is 0, the second orientation calculation unit 30 converts the second orientation Ψ ins,k Set to an appropriate initial value.

[0126] Next, in step S22, the second orientation calculation unit 30 calculates the angular velocity ω around the Z axis detected by the angular velocity sensor 13 based on the above-mentioned equation (1). z,k The angular velocity of the yaw angle ψ is calculated by subtracting the previous angular velocity deviation δΩ predicted in step S51 described later from the angular velocity of the yaw angle ψ. k-1 To correct the deviation of the angular velocity of the yaw angle ψ.

[0127] Next, in step S23, the second orientation Ψ set in step S21 is ins,k The initial value of the deflection angle ψ after the deviation is corrected in step S22 is added to the value obtained by integrating the angular velocity of the deflection angle ψ, and the second azimuth ψ after the angular velocity deviation is corrected is calculated. ins,k .

[0128] Steps S21, S22, and S23 are equivalent to Figure 5 The second orientation calculation step S20.

[0129] Next, in step S28, the first orientation Ψ calculated in step S11 is gnss,k In the case of step S31, the third orientation calculation unit 40 of the orientation calculation device 1 calculates the first orientation Ψ based on the first orientation Ψ calculated in step S11. gnss,k and the second orientation Ψ calculated in step S23ins,k , calculate the third position Ψ est,k In step S30, the first orientation Ψ is not calculated in step S11. gnss,k In the case of, the third position calculation unit 40 does not perform the processing of step S31. Step S31 is equivalent to Figure 5 The third position calculation step S30.

[0130] Next, in step S41, the temperature characteristic estimation unit 60 of the orientation calculation device 1 calculates the second orientation Ψ by using the above equation (3). ins,k The error δΨ ins,k .

[0131] Next, in step S42, the temperature characteristic estimation unit 60 calculates the second orientation Ψ by using the above equation (2). ins,k The angular velocity deviation is the angular velocity deviation δΩ k .

[0132] Next, in step S43, the temperature characteristic estimation unit 60 calculates the angular velocity deviation δΩ k Specifically, the temperature characteristic estimation unit 60 estimates the temperature characteristic of the angular velocity based on the angular velocity deviation δΩ calculated in step S42. k and the temperature T detected by the temperature sensor 15 k , calculate the observed variable y shown in the above formula (10) k , using the extended Kalman filter, calculate the first-order coefficient c of the polynomial (5) shown in the above equation (6) 1,k and the 0th-order coefficient c 0,k and temperature T k The state variable x as an element k .

[0133] Next, in step S44, the temperature characteristic estimation unit 60 updates the temperature characteristic information 71 stored in the storage unit 70. Specifically, the temperature characteristic estimation unit 60 calculates the covariance matrix P represented by the above equation (26): k|k .

[0134] The temperature characteristic estimation unit 60 includes the temperature characteristic information 71 for the temperature T k The first-order coefficient c 1,k In the case of the value of 11 When the coefficient c contained in the temperature characteristic information 71 is less than a predetermined threshold value, 1,k The value of is updated to the estimated coefficient c 1,k The value of variance p 11 If the value is greater than a predetermined threshold, the coefficient c contained in the temperature characteristic information 71 is not updated. 1,kThe temperature characteristic estimation unit 60 does not include the value of the temperature T in the temperature characteristic information 71. k The first-order coefficient c 1,k , regardless of the variance p 11 Regardless of the value of 1,k The value of is updated to the estimated first-order coefficient c 1,k The value of .

[0135] Similarly, the temperature characteristic estimation unit 60 includes the temperature characteristic information 71 for the temperature T k The 0th-order coefficient c 0,k In the case of the value of 22 When the zero-order coefficient c contained in the temperature characteristic information 71 is less than a predetermined threshold value, 0,k The value is updated to the estimated 0th-order coefficient c 0,k The value of variance p 22 If the zero-order coefficient c contained in the temperature characteristic information 71 is greater than a predetermined threshold, the zero-order coefficient c contained in the temperature characteristic information 71 is not updated. 0,k The temperature characteristic estimation unit 60 does not include the value of the temperature T in the temperature characteristic information 71. k The 0th-order coefficient c 0,k , regardless of the variance p 22 Regardless of the value of 0,k The value of is updated to the estimated 0th-order coefficient c 0,k The value of .

[0136] Steps S41, S42, S43, and S44 are equivalent to Figure 5 The temperature characteristic estimating step S40 is performed.

[0137] Next, in step S51, the angular velocity deviation prediction unit 50 of the orientation calculation device 1 predicts the second orientation Ψi based on the temperature characteristic information 71 stored in the storage unit 70. ns,k Angular velocity deviation δΩ k For example, the angular velocity deviation prediction unit 50 may also use the temperature T k-1 , Temperature T k and the temperature characteristic information 71 for the temperature T k The first-order coefficient c 1,k and the 0th-order coefficient c 0,k Substitute the first-order polynomial (5) to predict the angular velocity deviation δΩ k Step S51 is equivalent to Figure 5 Angular velocity deviation prediction step S50.

[0138] Then, the direction calculation device 1 increments the time k by 1 in step S58 , and repeats steps S2 to S58 until the direction calculation process is completed in step S60 .

[0139] In this way, the position calculation device 1 can estimate the angular velocity deviation δΩ at a predetermined period such as 1 second, regardless of whether the automobile 5 as a moving object is stopped or moving. k , calculate the high-precision third position Ψ after the angular velocity deviation is corrected est,k .

[0140] 1-3. Effects

[0141] As described above, in the first embodiment, the first direction calculation unit 20 can calculate the first direction Ψ based on the satellite signal regardless of whether the moving body equipped with the direction calculation device 1 is stopped or moving. gnss,k Even when the satellite signal cannot be received, the second azimuth calculation unit 30 can calculate the second azimuth Ψ based on the angular velocity signal output from the angular velocity sensor 13. ins,k Therefore, the third position calculation unit 40 can calculate the first position based on the gnss,k and the second position ins,k Calculate the third position Ψ periodically without interruption est,k As a result, even when the moving object is moving, the temperature characteristic estimating unit 60 can estimate the temperature characteristic based on the second orientation Ψ ins,k and third party position est,k Periodically estimate the second position Ψ ins,k Angular velocity deviation δΩ k The temperature characteristic of the second azimuth calculation unit 30 is updated based on the temperature characteristic information 71 stored in the storage unit 70, so that the second azimuth calculation unit 30 can calculate the last angular velocity deviation δΩ based on the angular velocity deviation prediction unit 50 predicted based on the temperature characteristic information 71. k-1 , calculate the second orientation Ψ after the angular velocity deviation is corrected ins,k In addition, the temperature characteristic estimation unit 60 estimates the temperature T detected by the temperature sensor 15. k Angular velocity deviation δΩ k The temperature characteristics of the angular velocity can be used to estimate the angular velocity deviation δΩ with high accuracy for each narrow temperature range. k Therefore, according to the first embodiment, the orientation calculation device 1 can calculate the orientation after the angular velocity deviation is corrected with high accuracy, regardless of whether the moving body is stopped or moving.

[0142] In the first embodiment, the angular velocity deviation δΩ k Not by temperature T k is approximated by a polynomial with temperature change ΔT kThe temperature characteristic estimation unit 60 can estimate the angular velocity deviation δΩ with high accuracy for each narrow temperature range by using the polynomial (4) with ω as a variable. k Temperature characteristics. Moreover, within a narrow temperature range, the angular velocity deviation δΩ k By changing the temperature ΔT k The linear polynomial (5) with the variable is approximated with high accuracy, so the angular velocity deviation δΩ of the temperature characteristic estimation unit 60 can be maintained. k The estimation accuracy of the temperature characteristics of the device can be improved, and the calculation load for the estimation can be reduced.

[0143] In the first embodiment, the temperature characteristic estimation unit 60 only determines the approximate angular velocity deviation δΩ k The coefficient c of the polynomial (4) 0,k ~c n,k When the estimation accuracy of is greater than or equal to a predetermined value, the coefficient c included in the temperature characteristic information 71 is updated. 0,k ~c n,k Therefore, the angular velocity deviation prediction unit 50 can predict the angular velocity deviation δΩ with high accuracy based on the temperature characteristic information 71. k Therefore, according to the first embodiment, the orientation calculation device 1 can calculate the orientation after the angular velocity deviation has been corrected with high accuracy.

[0144] 2. Second Implementation

[0145] Hereinafter, regarding the second embodiment, the same components as those of the first embodiment are denoted by the same reference numerals, and the description overlapping with that of the first embodiment is omitted or simplified, and the description will mainly focus on the differences from the first embodiment.

[0146] In the second embodiment, the angular velocity deviation δΩ k This is different from the first embodiment. That is, in the first embodiment, the angular velocity deviation δΩ k is the second orientation Ψ calculated by the second orientation calculation unit 30 ins,k However, in the second embodiment, the angular velocity deviation δΩ k is the angular velocity ω around the Z axis detected by the angular velocity sensor 13 z,k The Z axis is equivalent to the yaw axis. In other words, the angular velocity deviation δΩ k is the angular velocity ω about the yaw axis z,k deviation.

[0147] The configuration example of the direction calculation device 1 of the second embodiment is Figure 1 However, in the direction calculation device 1 of the second embodiment, the processing performed by the second direction calculation unit 30, the angular velocity deviation prediction unit 50, and the temperature characteristic estimation unit 60 is different from that of the first embodiment.

[0148] The second orientation calculation unit 30 calculates the second orientation Ψ, which is the orientation of the moving object in the global coordinate system at time k, based on the angular velocity signal output from the angular velocity sensor 13. ins,k The second azimuth calculation unit 30 calculates the angular velocity deviation δΩ at the previous time k-1 predicted by the angular velocity deviation prediction unit 50. k-1 , calculate the second position Ψ at time k after the angular velocity deviation is corrected ins,k Specifically, the second orientation calculation unit 30 calculates the angular velocity ω around the Z axis detected by the angular velocity sensor 13. z,k Subtract the angular velocity deviation δΩ k-1 , calculate the angular velocity around the Z axis after the angular velocity deviation is corrected. Then, the second orientation calculation unit 30 calculates the third orientation Ψ at the last time k-1 calculated by the third orientation calculation unit 40 est,k-1 As the initial orientation, the third orientation Ψ est,k-1 The second orientation Ψ at time k is calculated by adding the value obtained by integrating the angular velocity of the yaw angle ψ obtained by substituting the angular velocity about the Z axis into the above equation (1). ins,k .

[0149] The temperature characteristic estimation unit 60 determines the second orientation Ψ based on the second orientation calculation unit 30. ins,k and the third position Ψ calculated by the third position calculation unit 40 est,k , it is estimated that the temperature T detected by the temperature sensor 15 k Angular velocity deviation δΩ k The temperature characteristic information 71 stored in the storage unit 70 is updated based on the estimated temperature characteristic.

[0150] In this embodiment, the temperature characteristic estimation unit 60 calculates the observation variable y represented by the above-mentioned equation (10) by equation (27): k The angular velocity deviation of the elements δΩ k .

[0151] [Formula 27]

[0152]

[0153] The temperature characteristic estimation unit 60 uses the third position Ψ calculated by the third position calculation unit 40 to estimate the temperature characteristic of the object. est,k , roll angle Φ est,k and the pitch angle θ est,k , through formula (28), calculate the angular velocity ω contained in formula (27) est z,k In formula (29), ΔΘ est,k and ΔΨ est,k They are expressed by equation (29) and equation (30) respectively.

[0154] [Formula 28]

[0155]

[0156] [Formula 29]

[0157] ΔΘ est,k =Θ est,k -Θ est,k-1 …(29)

[0158] [Formula 30]

[0159] ΔΨ est,k =Ψ est,k -Ψ est,k-1 …(30) Then, the temperature characteristic estimation unit 60 uses the same extended Kalman filter as in the first embodiment to estimate the temperature T k The linear coefficient c of the above linear polynomial (5) is 1,k and the 0th-order coefficient c 0,k .

[0160] The angular velocity deviation prediction unit 50 determines the angular velocity deviation based on the temperature T detected by the temperature sensor 15. k and the temperature characteristic information 71 stored in the storage unit 70, the angular velocity deviation δΩ is predicted k For example, the angular velocity deviation prediction unit 50 may also use the temperature T k-1 , Temperature T k and the temperature characteristic information 71 for the temperature T k The coefficient c 0,k ~c n,k Substitute the above polynomial (4) to predict the angular velocity deviation δΩ k .

[0161] The other structures and functions of the direction calculation device 1 of the second embodiment are the same as those of the first embodiment, and thus the description thereof will be omitted.

[0162] In addition, a flowchart showing an example of the steps of the direction calculation method of the second embodiment is shown in FIG. Figure 5 The same, therefore, the illustration and description thereof are omitted. However, in the second embodiment, Figure 5 The specific steps of the second orientation calculation step S20, the temperature characteristic estimation step S40, and the angular velocity deviation prediction step S50 are different from those of the first embodiment. Figure 7 FIG. 1 is a flowchart showing an example of specific steps of the direction calculation method according to the second embodiment. Figure 7 In, with Figure 6 The same steps are denoted by the same reference numerals.

[0163] like Figure 7As shown, first, in step S1, the direction calculation device 1 initializes the time k to 0.

[0164] Next, when the satellite signal receiver 11 receives a satellite signal in step S2, the first direction calculation unit 20 of the direction calculation device 1 calculates the first direction Ψ based on the satellite signal received by the satellite signal receiver 11 in step S11. gnss,k If the satellite signal receiver 11 fails to receive the satellite signal in step S2, the first azimuth calculation unit 20 does not perform the processing of step S11. Step S11 is equivalent to Figure 5 The first orientation calculation step S10.

[0165] Next, in step S21, the second orientation calculation unit 30 of the orientation calculation device 1 sets the second orientation Ψ ins,k Specifically, the third position Ψ calculated in the step S31 described later is est,k-1 Set to the second position ins,k In addition, the second orientation calculation unit 30 does not calculate the last third orientation Ψ in step S31. est,k-1 In the case of the previous second orientation Ψ calculated in step S23a described later, ins,k-1 Set to the second position ins,k In addition, when the time k is 0, the second orientation calculation unit 30 converts the second orientation Ψ ins,k Set to an appropriate initial value.

[0166] Next, in step S22a, the second orientation calculation unit 30 calculates the angular velocity ω around the Z axis detected by the angular velocity sensor 13. z,k The previous angular velocity deviation δΩ predicted in step S51a described later is subtracted from k-1 To correct the angular velocity deviation around the yaw axis.

[0167] Next, in step S23a, the second orientation Ψ set in step S21 is ins,k The second orientation Ψ is calculated by adding the value obtained by integrating the angular velocity of the yaw angle ψ obtained by substituting the angular velocity around the yaw axis corrected for deviation in step S22a into the above formula (1). ins,k .

[0168] Steps S21, S22a, and S23a are equivalent to Figure 5 The second orientation calculation step S20.

[0169] Next, in step S28, the first orientation Ψ calculated in step S11 is gnss,kIn the case of step S31, the third orientation calculation unit 40 of the orientation calculation device 1 calculates the first orientation Ψ based on the first orientation Ψ calculated in step S11. gnss,k and the second orientation Ψ calculated in step S23a ins,k , calculate the third position Ψ est,k In step S30, the first orientation Ψ is not calculated in step S11. gnss,k In the case of , the third position calculation unit 40 does not perform the process of step S31. In step S31, the third position calculation unit 40 also calculates the roll angle Φ est,k and pitch angle θ est,k Step S31 is equivalent to Figure 5 The third position calculation step S30.

[0170] Next, in step S41, the temperature characteristic estimation unit 60 of the orientation calculation device 1 calculates the second orientation Ψ by using the above equation (3). ins,k The error δΨ ins,k .

[0171] Next, in step S42a, the temperature characteristic estimation unit 60 calculates the angular velocity ω around the yaw axis using the above equation (27): z,k The deviation of angular velocity is δΩ k .

[0172] Next, in step S43a, the temperature characteristic estimation unit 60 calculates the angular velocity deviation δΩ k Specifically, the temperature characteristic estimation unit 60 estimates the temperature characteristic of the angular velocity based on the angular velocity deviation δΩ calculated in step S42a. k and the temperature T detected by the temperature sensor 15 k , calculate the observed variable y shown in the above formula (10) k , using the extended Kalman filter, calculate the first-order coefficient c of the polynomial (5) shown in the above equation (6) 1,k and the 0th-order coefficient c 0,k and temperature T k The state variable x as an element k .

[0173] Next, in step S44a, the temperature characteristic estimation unit 60 updates the temperature characteristic information 71 stored in the storage unit 70. Specifically, the temperature characteristic estimation unit 60 calculates the covariance matrix P represented by the above equation (26): k|k .

[0174] The temperature characteristic estimation unit 60 includes the temperature characteristic information 71 for the temperature T k The first-order coefficient c 1,k In the case of the value of 11When the coefficient c contained in the temperature characteristic information 71 is less than a predetermined threshold value, 1,k The value of is updated to the estimated coefficient c 1,k The value of variance p 11 If the value is greater than a predetermined threshold, the coefficient c contained in the temperature characteristic information 71 is not updated. 1,k The temperature characteristic estimation unit 60 does not include the value of the temperature T in the temperature characteristic information 71. k The first-order coefficient c 1,k , regardless of the variance p 11 Regardless of the value of 1,k The value of is updated to the estimated first-order coefficient c 1,k The value of .

[0175] Similarly, the temperature characteristic estimation unit 60 includes the temperature characteristic information 71 for the temperature T k The 0th-order coefficient c 0,k In the case of the value of 22 When the zero-order coefficient c contained in the temperature characteristic information 71 is less than a predetermined threshold value, 0,k The value of is updated to the estimated 0th-order coefficient c 0,k The value of variance p 22 If the zero-order coefficient c contained in the temperature characteristic information 71 is greater than a predetermined threshold, the zero-order coefficient c contained in the temperature characteristic information 71 is not updated. 0,k The temperature characteristic estimation unit 60 does not include the value of the temperature T in the temperature characteristic information 71. k The 0th-order coefficient c 0,k , regardless of the variance p 22 Regardless of the value of 0,k The value of is updated to the estimated 0th-order coefficient c 0,k The value of .

[0176] Steps S41, S42a, S43a, and S44a are equivalent to Figure 5 The temperature characteristic estimating step S40 is performed.

[0177] Next, in step S51a, the angular velocity deviation prediction unit 50 of the orientation calculation device 1 predicts the angular velocity deviation δΩ about the yaw axis based on the temperature characteristic information 71 stored in the storage unit 70. k For example, the angular velocity deviation prediction unit 50 may also use the temperature T k-1 , Temperature T k and the temperature characteristic information 71 for the temperature T k The first-order coefficient c 1,k and the 0th-order coefficient c 0,k Substitute the first-order polynomial (5) to predict the angular velocity deviation δΩk Step S51a is equivalent to Figure 5 Angular velocity deviation prediction step S50.

[0178] Then, the direction calculation device 1 increments the time k by 1 in step S58 , and repeats steps S2 to S58 until the direction calculation process is completed in step S60 .

[0179] As described above, in the second embodiment, even when the moving object is moving, the temperature characteristic estimating unit 60 can estimate the temperature characteristic based on the second orientation Ψ ins,k and third party position est,k , the angular velocity ω around the yaw axis detected by the angular velocity sensor 13 is periodically estimated z,k Angular velocity deviation δΩ k The temperature characteristic of the second azimuth operation unit 30 is updated based on the temperature characteristic information 71 stored in the storage unit 70. Therefore, the second azimuth operation unit 30 can calculate the angular velocity deviation δΩ based on the last angular velocity deviation δΩ predicted by the angular velocity deviation prediction unit 50 based on the temperature characteristic information 71. k-1 , calculate the second orientation Ψ after the angular velocity deviation is corrected ins,k Therefore, according to the second embodiment, regardless of whether the moving body is stopped or moving, the orientation calculation device 1 can calculate the orientation after the angular velocity deviation is corrected with high accuracy. In addition, according to the second embodiment, the same effects as those of the first embodiment can be obtained.

[0180] 3. Third Implementation

[0181] Hereinafter, regarding the third embodiment, the same components as those of the first embodiment or the second embodiment are denoted by the same reference numerals, the description repeated with the first embodiment or the second embodiment is omitted or simplified, and the contents different from the first embodiment and the second embodiment are mainly described.

[0182] Figure 8 FIG. 1 is a diagram showing a configuration example of a direction calculation device 1 according to a third embodiment. Figure 8 In, with Figure 1 The same components are marked with the same reference numerals. Figure 8 As shown, the orientation calculation device 1 of the third embodiment is similar to the first embodiment, and includes a first orientation calculation unit 20, a second orientation calculation unit 30, a third orientation calculation unit 40, an angular velocity deviation prediction unit 50, a temperature characteristic estimation unit 60, and a storage unit 70. In addition, the orientation calculation device 1 may also include a wheel speed sensor 12, an angular velocity sensor 13, a 6DoF sensor 14, a temperature sensor 15, and an external sensor 16. In addition, the orientation calculation device 1 may also be configured to omit or change Figure 8 part of the constituent elements or adding other constituent elements.

[0183] The third embodiment is different from the first and second embodiments in that the first azimuth calculation unit 20 performs processing using the external sensor 16 instead of the satellite signal receiver 11 .

[0184] The external sensor 16 is a sensor for detecting information about the surroundings of the moving body, and may be, for example, a LiDAR, a camera, a millimeter-wave radar, etc. used in AD, ADAS, etc. AD is the abbreviation of Autonomous Driving, and ADAS is the abbreviation of Advanced Driver Assistance System. LiDAR is the abbreviation of Light Detection and Ranging or Laser Imaging Detection and Ranging.

[0185] The first orientation calculation unit 20 calculates the orientation of the moving object in the global coordinate system at time k, that is, the first orientation Ψ based on the detection result of the external sensor 16. EXT,k The first orientation calculation unit 20 calculates the first orientation Ψ at a predetermined period such as 1 second. EXT,k For example, the first orientation calculation unit 20 compares the detection result of the external sensor 16, that is, the information about the surroundings of the moving body, with the map information 72 stored in the storage unit 70 to calculate the orientation of the moving body in the global coordinate system, that is, the first orientation Ψ EXT,k In addition, the first orientation calculation unit 20 may further calculate the position and speed of the moving object in the global coordinate system.

[0186] Similar to the first embodiment or the second embodiment, the second orientation calculation unit 30 calculates the orientation of the moving object in the global coordinate system at time k, that is, the second orientation Ψ based on the angular velocity signal output from the angular velocity sensor 13. ins,k The second azimuth calculation unit 30 calculates the angular velocity deviation δΩ at the previous time k-1 predicted by the angular velocity deviation prediction unit 50. k-1 , calculate the second position Ψ at time k after the angular velocity deviation is corrected ins,k .

[0187] The third orientation calculation unit 40 calculates the first orientation Ψ at time k based on the first orientation calculation unit 20. EXT,k and the second orientation Ψ at time k calculated by the second orientation calculation unit 30 ins,k , calculate the orientation of the moving body in the global coordinate system at time k, that is, the third orientation Ψ est,k The third position calculation unit 40 calculates the third position Ψ at a predetermined period such as 1 second. est,kFor example, the third orientation calculation unit 40 may also calculate the first orientation Ψ EXT,k The timing of the first position Ψ EXT,k As a third party est,k , without calculating the first position Ψ EXT,k The timing of the second position Ψ ins,k As a third party est,k In addition, the third position calculation unit 40 may also use a Kalman filter to calculate the third position Ψ est,k .

[0188] As in the first embodiment or the second embodiment, the storage unit 70 stores the angular velocity deviation δΩ k Temperature characteristic information 71. Angular velocity deviation δΩ k As shown in the above formula (2), the second orientation Ψ calculated by the second orientation calculation unit 30 may also be ins,k The angular velocity deviation is δΩ. k As shown in the above equation (27), the angular velocity ω around the yaw axis detected by the angular velocity sensor 13 may also be z,k In addition, in the third embodiment, the storage unit 70 stores the first orientation Ψ used by the first orientation calculation unit 20. EXT,k The calculated map information 72.

[0189] As in the first embodiment or the second embodiment, the temperature characteristic estimation unit 60 calculates the second orientation Ψ based on the second orientation calculation unit 30. ins,k and the third position Ψ calculated by the third position calculation unit 40 est,k , it is estimated that the temperature T detected by the temperature sensor 15 k Angular velocity deviation δΩ k The temperature characteristic estimation unit 60 updates the temperature characteristic information 71 stored in the storage unit 70 based on the estimated temperature characteristic. As described above, the temperature characteristic estimation unit 60 may use an extended Kalman filter to estimate the temperature T k The first-order coefficient c 1,k and the 0th-order coefficient c 0,k .

[0190] As in the first embodiment or the second embodiment, the angular velocity deviation prediction unit 50 generates an angular velocity deviation prediction signal based on the temperature T detected by the temperature sensor 15. k and the temperature characteristic information 71 stored in the storage unit 70, the angular velocity deviation δΩ is predicted k As described above, the angular velocity deviation prediction unit 50 may also use the temperature T k-1 , Temperature T k and the temperature characteristic information 71 for the temperature T k The coefficient c0,k ~c n,k Substitute the above polynomial (4) to predict the angular velocity deviation δΩ k .

[0191] The other structures and functions of the direction calculation device 1 of the third embodiment are the same as those of the direction calculation device 1 of the first embodiment or the second embodiment, and thus the description thereof will be omitted.

[0192] A flowchart showing an example of the steps of the direction calculation method of the third embodiment Figure 5 In the third embodiment, Figure 5 In the first orientation calculation step S10, the first orientation calculation unit 20 of the orientation calculation device 1 calculates the first orientation Ψ based on the detection result of the external sensor 16. EXT,k .exist Figure 5 After the second orientation calculation step S20, the orientation calculation device 1 performs the processing except replacing the first orientation Ψ gnss,k Using the first position Ψ EXT,k Other than this point, it is the same as the first embodiment or the second embodiment, and therefore the description thereof is omitted.

[0193] Fig. 9 FIG. 1 is a flowchart showing an example of specific steps of the direction calculation method according to the third embodiment. Fig. 9 In the Figure 6 The same processing steps are marked with the same reference numerals. Fig. 9 , the angular velocity deviation δΩ k The second position ins,k The deviation of the angular velocity.

[0194] like Fig. 9 As shown, first, in step S1, the direction calculation device 1 initializes the time k to 0.

[0195] Next, in step S12, the first orientation calculation unit 20 of the orientation calculation device 1 calculates the first orientation Ψ based on the detection result of the external sensor 16. EXT,k Step S12 is equivalent to Figure 5 The first orientation calculation step S10.

[0196] Next, in steps S21, S22, and S23, the second orientation calculation unit 30 of the orientation calculation device 1 performs the Figure 6 The same process is performed as in steps S21, S22, and S23 to calculate the second azimuth Ψ after the angular velocity deviation is corrected. ins,k Steps S21, S22, and S23 are equivalent to Figure 5 The second orientation calculation step S20.

[0197] Next, in step S31, the third orientation calculation unit 40 of the orientation calculation device 1 calculates the first orientation Ψ based on the first orientation Ψ calculated in step S12. EXT,k and the second orientation Ψ calculated in step S23 ins,k , calculate the third position Ψ est,k Step S31 is equivalent to Figure 5 The third position calculation step S30.

[0198] Next, in steps S41, S42, S43, and S44, the temperature characteristic estimation unit 60 of the direction calculation device 1 performs Figure 6 The temperature characteristic information 71 stored in the storage unit 70 is updated by performing the same processing as steps S41, S42, S43, and S44. Steps S41, S42, S43, and S44 are equivalent to Figure 5 The temperature characteristic estimating step S40 is performed.

[0199] Next, in step S51, the angular velocity deviation prediction unit 50 of the orientation calculation device 1 performs Figure 6 The same process as step S51 is performed to predict the second position Ψ ins,k Angular velocity deviation δΩ k .

[0200] Then, the direction calculation device 1 increments the time k by 1 in step S58 , and repeats steps S2 to S58 until the direction calculation process is completed in step S60 .

[0201] Fig.10 FIG. 1 is a flowchart showing another example of specific steps of the direction calculation method according to the third embodiment. Fig.10 In the Figure 7 The same processing steps are marked with the same reference numerals. Fig.10 , the angular velocity deviation δΩ k is the angular velocity ω around the yaw axis detected by the angular velocity sensor 13 z,k deviation.

[0202] like Fig.10 As shown, first, in step S1, the direction calculation device 1 initializes the time k to 0.

[0203] Next, in step S12, the first orientation calculation unit 20 of the orientation calculation device 1 calculates the first orientation Ψ based on the detection result of the external sensor 16. EXT,k Step S12 is equivalent to Figure 5 The first orientation calculation step S10.

[0204] Next, in steps S21, S22a, and S23a, the second orientation calculation unit 30 of the orientation calculation device 1 performs the Figure 7The same process is performed as in steps S21, S22a, and S23a to calculate the second azimuth Ψ after the angular velocity deviation is corrected. ins,k Steps S21, S22a, and S23a are equivalent to Figure 5 The second orientation calculation step S20.

[0205] Next, in step S31, the third orientation calculation unit 40 of the orientation calculation device 1 calculates the first orientation Ψ based on the first orientation Ψ calculated in step S12. EXT,k and the second orientation Ψ calculated in step S23a ins,k , calculate the third position Ψ est,k Step S31 is equivalent to Figure 5 The third position calculation step S30.

[0206] Next, in steps S41, S42a, S43a, and S44a, the temperature characteristic estimation unit 60 of the direction calculation device 1 performs the following steps: Figure 7 The temperature characteristic information 71 stored in the storage unit 70 is updated by performing the same processing as in steps S41, S42a, S43a, and S44a. Steps S41, S42a, S43a, and S44a are equivalent to Figure 5 The temperature characteristic estimating step S40 is performed.

[0207] Next, in step S51a, the angular velocity deviation prediction unit 50 of the orientation calculation device 1 performs Figure 7 The same process as step S51a is performed to predict the angular velocity deviation δΩ about the yaw axis. k .

[0208] Then, the direction calculation device 1 increments the time k by 1 in step S58 , and repeats steps S2 to S58 until the direction calculation process is completed in step S60 .

[0209] As described above, in the third embodiment, regardless of whether the moving body equipped with the orientation calculation device 1 is stopped or moving, the first orientation calculation unit 20 can calculate the first orientation Ψ based on the detection result of the external sensor 16. EXT,k The second orientation calculation unit 30 can calculate the second orientation Ψ based on the angular velocity signal output from the angular velocity sensor 13. ins,k Therefore, the third position calculation unit 40 can calculate the first position based on the EXT,k and the second position ins,k Calculate the third position Ψ periodically without interruption est,k As a result, even when the moving object is moving, the temperature characteristic estimating unit 60 can estimate the temperature characteristic based on the second orientation Ψ ins,k and third party position est,k Periodically estimate the angular velocity deviation δΩ kThe temperature characteristic of the second azimuth operation unit 30 is updated based on the temperature characteristic information 71 stored in the storage unit 70, so that the second azimuth operation unit 30 can calculate the angular velocity deviation δΩ based on the last angular velocity deviation δΩ predicted by the angular velocity deviation prediction unit 50 based on the temperature characteristic information 71. k-1 , calculate the second orientation Ψ after the angular velocity deviation is corrected ins,k In addition, the temperature characteristic estimation unit 60 estimates the temperature T detected by the temperature sensor 15. k Angular velocity deviation δΩ k The temperature characteristics of the angular velocity can be used to estimate the angular velocity deviation δΩ with high accuracy for each narrow temperature range. k Therefore, according to the third embodiment, regardless of whether the moving body is stopped or moving, the orientation calculation device 1 can calculate the orientation after the angular velocity deviation is corrected with high accuracy. In addition, according to the third embodiment, the same effect as the first embodiment or the second embodiment can be obtained.

[0210] 4. Modifications

[0211] The present invention is not limited to the present embodiment, and various modifications can be implemented within the scope of the gist of the present invention.

[0212] For example, in the above-mentioned embodiments, the position calculation device 1 is mounted on the automobile 5, but the position calculation device 1 may be mounted on a mobile object other than the automobile. As the mobile object, for example, in addition to the automobile, there may be agricultural machinery such as tractors, construction machinery such as forklifts, unmanned transport vehicles, robot mowers, robot vacuum cleaners, aircraft such as jet aircraft and helicopters, ships, rockets, artificial satellites, railway vehicles, etc.

[0213] In addition, for example, in each of the above-mentioned embodiments, the orientation calculation device 1 includes the angular velocity sensor 13 and the 6DoF sensor 14, but other sensors may be included instead of these. For example, the orientation calculation device 1 may include two 6DoF sensors.

[0214] The above-mentioned embodiment and modification examples are merely examples, and the present invention is not limited to these. For example, each embodiment and each modification example may be appropriately combined.

[0215] The present invention includes a configuration substantially the same as the configuration described in the embodiment, such as a configuration having the same function, method and result, or a configuration having the same purpose and effect. In addition, the present invention includes a configuration that replaces a non-essential part of the configuration described in the embodiment. In addition, the present invention includes a configuration that can achieve the same effect as the configuration described in the embodiment or a configuration that can achieve the same purpose. In addition, the present invention includes a configuration in which a known technology is added to the configuration described in the embodiment.

[0216] The following contents are derived from the above-mentioned embodiment and modification examples.

[0217] One embodiment of the azimuth calculation device comprises: a first azimuth calculation unit for calculating a first azimuth based on a satellite signal received by a satellite signal receiver or a detection result of an external sensor; a second azimuth calculation unit for calculating a second azimuth based on an angular velocity signal output by an angular velocity sensor when the angular velocity is detected; a third azimuth calculation unit for calculating a third azimuth based on the first azimuth and the second azimuth; a storage unit for storing temperature characteristic information of an angular velocity deviation; a temperature characteristic estimating unit for estimating the temperature characteristic of the angular velocity deviation relative to a temperature detected by a temperature sensor based on the second azimuth and the third azimuth, and updating the temperature characteristic information stored in the storage unit based on the estimated temperature characteristic; and an angular velocity deviation prediction unit for predicting the angular velocity deviation based on the temperature detected by the temperature sensor and the temperature characteristic information stored in the storage unit, wherein the second azimuth calculation unit calculates the second azimuth after the angular velocity deviation is corrected based on the angular velocity deviation predicted by the angular velocity deviation prediction unit last time.

[0218] In the orientation calculation device, whether the mobile body equipped with the orientation calculation device is stopped or moving, the first orientation calculation unit can calculate the first orientation based on the satellite signal or the detection result of the external sensor, and even when the satellite signal cannot be received, the second orientation calculation unit can calculate the second orientation based on the angular velocity signal, so the third orientation calculation unit can periodically calculate the third orientation without interruption based on the first orientation and the second orientation. As a result, even if the mobile body is moving, the temperature characteristic estimation unit can periodically estimate the temperature characteristic of the angular velocity deviation based on the second orientation and the third orientation, and update the temperature characteristic information, so the second orientation calculation unit can calculate the second orientation after the angular velocity deviation is corrected based on the last angular velocity deviation predicted by the angular velocity deviation prediction unit based on the temperature characteristic information. In addition, since the temperature characteristic estimation unit estimates the temperature characteristic of the angular velocity deviation relative to the temperature detected by the temperature sensor, the temperature characteristic of the angular velocity deviation can be estimated with high accuracy for each narrow temperature range. Therefore, according to this orientation calculation device, regardless of whether the moving body is stopped or moving, it is possible to calculate the orientation after the angular velocity deviation has been corrected with high accuracy.

[0219] In one embodiment of the orientation calculation device, the angular velocity deviation may be approximated by a polynomial having a temperature change as a variable.

[0220] According to this orientation calculation device, the angular velocity deviation is approximated by a polynomial having temperature change as a variable instead of a polynomial having temperature as a variable. Therefore, the temperature characteristic estimating unit can estimate the temperature characteristic of the angular velocity deviation with high accuracy for each narrow temperature range.

[0221] In one aspect of the direction calculation device, the temperature characteristic information may include a value of a coefficient of the polynomial equation for each of a plurality of temperatures.

[0222] In one embodiment of the direction calculation device, the polynomial may be a linear polynomial.

[0223] According to this orientation calculation device, within a narrow temperature range, the angular velocity deviation is approximated with high accuracy by a first-order polynomial with temperature change as a variable, thereby maintaining the estimation accuracy of the temperature characteristic of the angular velocity deviation by the temperature characteristic estimating unit and reducing the calculation load for estimation.

[0224] In one embodiment of the azimuth calculation device, the angular velocity deviation may be a deviation in angular velocity of the second azimuth.

[0225] In one embodiment of the azimuth calculation device, the angular velocity deviation may be a deviation in angular velocity around a yaw axis detected by the angular velocity sensor.

[0226] In one embodiment of the orientation calculation device, the temperature characteristic estimating unit estimates the value of the coefficient as the temperature characteristic and calculates the variance of the estimated coefficient. When the temperature characteristic information includes the value of the coefficient for the temperature detected by the temperature sensor, when the variance is below a prescribed threshold, the value of the coefficient included in the temperature characteristic information is updated to the estimated value of the coefficient. When the variance is greater than the prescribed threshold, the value of the coefficient included in the temperature characteristic information is not updated.

[0227] According to the azimuth calculation device, the temperature characteristic estimation unit updates the value of the coefficient included in the temperature characteristic information only when the estimation accuracy of the coefficient of the polynomial approximating the angular velocity deviation is greater than a predetermined value, so that the angular velocity deviation prediction unit can predict the angular velocity deviation with high accuracy based on the temperature characteristic information. Therefore, according to the azimuth calculation device, the azimuth after the angular velocity deviation is corrected with high accuracy can be calculated.

[0228] One aspect of the azimuth calculation method involved in the present invention is to include: a first azimuth calculation step, calculating a first azimuth based on a satellite signal received by a satellite signal receiver or a detection result of an external sensor; a second azimuth calculation step, calculating a second azimuth based on an angular velocity signal output by an angular velocity sensor when detecting an angular velocity; a third azimuth calculation step, calculating a third azimuth based on the first azimuth and the second azimuth; a temperature characteristic estimation step, estimating a temperature characteristic of an angular velocity deviation relative to a temperature detected by a temperature sensor based on the second azimuth and the third azimuth, and updating temperature characteristic information of the angular velocity deviation stored in a storage unit based on the estimated temperature characteristic; and an angular velocity deviation prediction step, predicting the angular velocity deviation based on the temperature detected by the temperature sensor and the temperature characteristic information stored in the storage unit, wherein in the second azimuth calculation step, the second azimuth after the angular velocity deviation is corrected is calculated based on the last angular velocity deviation predicted in the angular velocity deviation prediction step.

[0229] In this orientation calculation method, whether the mobile body is stopped or moving, the first orientation can be calculated based on the satellite signal or the detection result of the external sensor in the first orientation calculation step, and even if the satellite signal cannot be received, the second orientation can be calculated based on the angular velocity signal in the second orientation calculation step, so that in the third orientation calculation step, the third orientation can be calculated periodically without interruption based on the first orientation and the second orientation. As a result, even if the mobile body is moving, in the temperature characteristic estimation step, the temperature characteristic of the angular velocity deviation can be periodically estimated based on the second orientation and the third orientation, and the temperature characteristic information can be updated, so that in the second orientation calculation step, based on the last angular velocity deviation predicted based on the temperature characteristic information in the angular velocity deviation prediction step, the second orientation after the angular velocity deviation is corrected can be calculated. In addition, in the temperature characteristic estimation step, the temperature characteristic of the angular velocity deviation relative to the temperature detected by the temperature sensor is estimated, so the temperature characteristic of the angular velocity deviation can be estimated with high accuracy for each narrow temperature range. Therefore, according to this orientation calculation method, regardless of whether the moving object is stopped or moving, it is possible to calculate the orientation after the angular velocity deviation has been corrected with high accuracy.

Claims

1. A device for calculating a position, characterized in that: have: A first azimuth calculation unit, which calculates a first azimuth based on a satellite signal received by a satellite signal receiver or a detection result of an external sensor; A second azimuth calculation unit calculates a second azimuth based on an angular velocity signal output by the angular velocity sensor when detecting an angular velocity; a third orientation calculation unit, which calculates a third orientation based on the first orientation and the second orientation; a storage unit storing temperature characteristic information of an angular velocity deviation; a temperature characteristic estimating unit that estimates a temperature characteristic of the angular velocity deviation with respect to the temperature detected by the temperature sensor based on the second orientation and the third orientation, and updates the temperature characteristic information stored in the storage unit based on the estimated temperature characteristic; as well as an angular velocity deviation prediction unit that predicts the angular velocity deviation based on the temperature detected by the temperature sensor and the temperature characteristic information stored in the storage unit, The second orientation calculation unit calculates the second orientation after the angular velocity deviation is corrected, based on the previous angular velocity deviation predicted by the angular velocity deviation prediction unit.

2. The position calculation device according to claim 1, characterized in that: The angular velocity deviation is approximated by a polynomial that takes the temperature change as a variable.

3. The position calculation device according to claim 2, characterized in that: The temperature characteristic information includes values ​​of coefficients of the polynomial for each of a plurality of temperatures.

4. The position calculation device according to claim 2, characterized in that: The polynomial is a first-degree polynomial.

5. The position calculation device according to claim 1, characterized in that: The angular velocity deviation is a deviation of the angular velocity of the second orientation.

6. The position calculation device according to claim 1, characterized in that: The angular velocity deviation is a deviation of the angular velocity about the yaw axis detected by the angular velocity sensor.

7. The position calculation device according to claim 3, characterized in that: The temperature characteristic estimating unit estimates the value of the coefficient as the temperature characteristic and calculates the variance of the estimated coefficient. When the temperature characteristic information includes the value of the coefficient for the temperature detected by the temperature sensor, when the variance is below a prescribed threshold, the value of the coefficient included in the temperature characteristic information is updated to the estimated value of the coefficient. When the variance is greater than the prescribed threshold, the value of the coefficient included in the temperature characteristic information is not updated.

8. A method for calculating a position, characterized in that: include: A first position calculation step, calculating a first position based on a satellite signal received by a satellite signal receiver or a detection result of an external sensor; A second azimuth calculation step of calculating a second azimuth based on an angular velocity signal output by the angular velocity sensor when detecting an angular velocity; a third position calculation step of calculating a third position based on the first position and the second position; a temperature characteristic estimating step of estimating a temperature characteristic of an angular velocity deviation relative to a temperature detected by a temperature sensor based on the second orientation and the third orientation, and updating temperature characteristic information of the angular velocity deviation stored in a storage unit based on the estimated temperature characteristic; as well as an angular velocity deviation prediction step of predicting the angular velocity deviation based on the temperature detected by the temperature sensor and the temperature characteristic information stored in the storage unit, In the second orientation calculation step, the second orientation after the angular velocity deviation is corrected is calculated based on the previous angular velocity deviation predicted in the angular velocity deviation prediction step.

Citation Information

Patent Citations

  • Navigation system

    JP2009192495A