Sensor module

Abstract

To provide a sensor module capable of accurately correcting angular velocity errors caused by misalignment of the angular velocity sensor. [Solution] A sensor module comprising: a first angular velocity sensor that detects the angular velocity around the first axis and outputs a first angular velocity signal, detects the angular velocity around the second axis and outputs a second angular velocity signal, and detects the angular velocity around the third axis and outputs a third angular velocity signal, when three mutually orthogonal axes are designated as the first axis, second axis, and third axis; a second angular velocity sensor that detects the angular velocity around the fourth axis corresponding to the third axis and outputs a fourth angular velocity signal; and a correction circuit that corrects alignment errors, which are errors in the angular velocity around the fourth axis due to the misalignment of the fourth axis relative to the third axis, based on the sensitivity ratio of the angular velocity around the third axis and the first, second, third, and fourth angular velocity signals.

Description

[Technical Field]

[0001] This invention relates to a sensor module. [Background technology]

[0002] Patent Document 1 describes an inertial measuring device comprising an inertial sensor including a 3-axis angular velocity sensor and a high-precision Z-axis angular velocity sensor. [Prior art documents] [Patent Documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2019-158425 [Overview of the project] [Problems that the invention aims to solve]

[0004] In cases where a high-precision Z-axis angular velocity sensor is provided separately from the inertial sensor, which includes a 3-axis angular velocity sensor, as in the inertial measurement device described in Patent Document 1, misalignment may occur between the 3-axis angular velocity sensor and the Z-axis angular velocity sensor. This misalignment may cause errors in the angular velocity signal output from the Z-axis angular velocity sensor. [Means for solving the problem]

[0005] One aspect of the sensor module according to the present invention is: When three mutually orthogonal axes are designated as the first axis, second axis, and third axis, A first angular velocity sensor that detects the angular velocity around the first axis and outputs a first angular velocity signal, detects the angular velocity around the second axis and outputs a second angular velocity signal, and detects the angular velocity around the third axis and outputs a third angular velocity signal, A second angular velocity sensor detects the angular velocity around the fourth axis corresponding to the third axis and outputs a fourth angular velocity signal, The system includes a correction circuit that corrects alignment errors, which are errors in angular velocity around the fourth axis due to the misalignment of the fourth axis with respect to the third axis, based on the sensitivity ratio of angular velocity around the third axis and the first, second, third, and fourth angular velocity signals. [Brief explanation of the drawing]

[0006] [Figure 1] This figure shows an example configuration of a self-position estimation system incorporating the sensor module of this embodiment. [Figure 2] Diagram illustrating the X, Y, and Z axes. [Figure 3] This figure shows an example configuration of the sensor module of this embodiment. [Figure 4] Diagram illustrating misalignment. [Figure 5] Diagram illustrating misalignment. [Figure 6] Block diagram of the Kalman filter operation. [Modes for carrying out the invention]

[0007] Preferred embodiments of the present invention will be described in detail below with reference to the drawings. The embodiments described below are not intended to unduly limit the scope of the present invention as described in the claims. Furthermore, not all of the configurations described below are necessarily essential components of the present invention.

[0008] 1. First Embodiment 1-1. Configuration of the Integrated Navigation System Figure 1 shows an example configuration of a self-position estimation system 100 incorporating the sensor module 1 of this embodiment. As shown in Figure 1, the self-position estimation system 100 includes the sensor module 1, a GNSS receiver 110, a direction / velocity / position estimation unit 120, a wheel speed sensor 130, a coordinate transformation unit 140, a velocity / position estimation unit 150, and a composite navigation calculation unit 160, and is mounted on an automobile 400, for example, as shown in Figure 2. Note that in Figure 2, parts of the self-position estimation system 100 other than the sensor module 1 are omitted from the illustration.

[0009] The GNSS receiver 110 receives satellite signals transmitted from multiple satellites that constitute part of the GNSS (Global Navigation Satellite System) via an antenna (not shown), performs positioning based on the received satellite signals, and outputs positioning information. Examples of GNSS systems include GPS (Global Positioning System), QZSS (Quasi Zenith Satellite System), EGNOS (European Geostationary Navigation Overlay Service), GLONASS (Global Navigation Satellite System), GALILEO, and BeiDou.

[0010] The direction, speed, and position estimation unit 120 estimates the direction, speed, and position of the vehicle 400 based on the positioning information output from the GNSS receiver 110, and determines the direction Ψ in the NED coordinate system. gnss , velocity V gnss and position P gnss Outputs.

[0011] Sensor module 1 is an inertial sensor module that detects acceleration in three mutually orthogonal axial directions and angular velocity around these three axes. As shown in Figure 2, for example, sensor module 1 is mounted on the automobile 400 so that its three axes are aligned with mutually orthogonal X, Y, and Z axes. The X axis is the axis aligned with the direction of travel of the automobile 400, the Y axis is the axis perpendicular to the direction of travel of the automobile 400 and aligned to the right, and the Z axis is the axis aligned downwards and perpendicular to the plane on which the automobile 400 travels.

[0012] Sensor module 1 detects acceleration and angular velocity in the three axes and the azimuth Ψ output from the azimuth, velocity, and position estimation unit 120. gnssBased on this, the roll angle Φ, pitch angle Θ, and yaw angle Ψ of the vehicle 400 are calculated, and the calculated roll angle Φ, pitch angle Θ, and yaw angle Ψ are output externally. The roll angle Φ is the rotation angle with the X-axis as the rotation axis, the pitch angle Θ is the rotation angle with the Y-axis as the rotation axis, and the yaw angle Ψ is the rotation angle with the Z-axis as the rotation axis. The roll angle Φ and pitch angle Θ represent the attitude of the vehicle 400, and the yaw angle Ψ represents the relative orientation of the vehicle 400.

[0013] The wheel speed sensor 130 detects the rotational speed of the wheels of the vehicle 400 and outputs a wheel speed signal.

[0014] The coordinate conversion unit 140 converts the wheel speed signal output from the wheel speed sensor 130 into a signal of the three-axis speed in the NED coordinate system based on the roll angle Φ, pitch angle Θ, and yaw angle Ψ output from the sensor module 1.

[0015] The speed and position estimation unit 150 estimates the speed and position of the vehicle 400 are estimated based on the signal of the three-axis speed output from the coordinate conversion unit 140, and the speed V ins and position P ins are output.

[0016] The integrated navigation calculation unit 160 performs an integrated navigation calculation process using the speed V gnss and position P gnss output from the orientation, speed, and position estimation unit 120, and the speed V ins and position P ins output from the speed and position estimation unit 150, and calculates the speed V and position P of the vehicle 400.

[0017] The calculated speed V and position P of the vehicle 400 are used, for example, in autonomous driving (AD) or advanced driver assistance systems (ADAS), etc.

[0018] In the system shown in Figure 1, the combined navigation is loose coupling, which integrates the velocity and position estimation results from GNSS with those from INS. Other combined navigation methods include tight coupling, which integrates raw data from GNSS with INS estimation results, and deep coupling, which further feeds the INS estimation results back into GNSS tracking.

[0019] 1-2. Sensor Module Configuration Figure 3 shows an example configuration of sensor module 1. As shown in Figure 3, sensor module 1 includes a 6DoF sensor 10. DoF is an abbreviation for Degrees of Freedom. The 6DoF sensor 10 includes inertial sensors, a 3-axis accelerometer 11 and a 3-axis angular velocity sensor 12.

[0020] The 3-axis accelerometer 11 includes an X-axis accelerometer 20X, a Y-axis accelerometer 20Y, and a Z-axis accelerometer 20Z.

[0021] The X-axis accelerometer 20X detects acceleration in the X-axis direction, using the X-axis as the detection axis, and outputs an X-axis acceleration signal corresponding to the detected acceleration. The Y-axis accelerometer 20Y detects acceleration in the Y-axis direction, using the Y-axis as the detection axis, and outputs a Y-axis acceleration signal corresponding to the detected acceleration. The Z-axis accelerometer 20Z detects acceleration in the Z-axis direction, using the Z-axis as the detection axis, and outputs a Z-axis acceleration signal corresponding to the detected acceleration. For example, the X-axis accelerometer 20X, Y-axis accelerometer 20Y, and Z-axis accelerometer 20Z may each be capacitive type MEMS accelerometers having sensor elements processed from a silicon substrate using MEMS technology. MEMS is an abbreviation for Micro Electro Mechanical Systems.

[0022] For example, the X-axis accelerometer 20X, the Y-axis accelerometer 20Y, and the Z-axis accelerometer 20Z each output a digital signal corresponding to the acceleration detected at a constant sampling period Δt.

[0023] Thus, the 3-axis acceleration sensor 11 detects acceleration in the X-axis direction and outputs an X-axis acceleration signal, detects acceleration in the Y-axis direction and outputs a Y-axis acceleration signal, and detects acceleration in the Z-axis direction and outputs a Z-axis acceleration signal.

[0024] The 3-axis angular velocity sensor 12 includes an X-axis angular velocity sensor 21X, a Y-axis angular velocity sensor 21Y, and a Z-axis angular velocity sensor 21Z.

[0025] The X-axis angular velocity sensor 21X detects the angular velocity around the X-axis, using the X-axis as the detection axis, and outputs an X-axis angular velocity signal corresponding to the detected angular velocity. The Y-axis angular velocity sensor 21Y detects the angular velocity around the Y-axis, using the Y-axis as the detection axis, and outputs a Y-axis angular velocity signal corresponding to the detected angular velocity. The Z-axis angular velocity sensor 21Z detects the angular velocity around the Z-axis, using the Z-axis as the detection axis, and outputs a Z-axis angular velocity signal corresponding to the detected angular velocity. In this embodiment, the X-axis angular velocity sensor 21X, the Y-axis angular velocity sensor 21Y, and the Z-axis angular velocity sensor 21Z are capacitive type MEMS gyro sensors having sensor elements made from a silicon substrate processed using MEMS technology.

[0026] For example, the X-axis angular velocity sensor 21X, the Y-axis angular velocity sensor 21Y, and the Z-axis angular velocity sensor 21Z each output a digital signal corresponding to the angular velocity detected at a constant sampling period Δt.

[0027] Thus, the 3-axis angular velocity sensor 12 detects the angular velocity around the X axis and outputs an X-axis angular velocity signal, detects the angular velocity around the Y axis and outputs a Y-axis angular velocity signal, and detects the angular velocity around the Z axis and outputs a Z-axis angular velocity signal.

[0028] Sensor module 1 includes digital filters 30X, 30Y, 30Z, 31X, 31Y, 31Z and bias correction units 40X, 40Y, 40Z, 41X, 41Y, 41Z.

[0029] Digital filter 30X performs low-pass filtering on the X-axis acceleration signal output from the X-axis acceleration sensor 20X to reduce unwanted signal components outside the frequency band of the vehicle 400's motion. Digital filter 30Y performs low-pass filtering on the Y-axis acceleration signal output from the Y-axis acceleration sensor 20Y to reduce unwanted signal components outside the frequency band of the vehicle 400's motion. Digital filter 30Z performs low-pass filtering on the Z-axis acceleration signal output from the Z-axis acceleration sensor 20Z to reduce unwanted signal components outside the frequency band of the vehicle 400's motion.

[0030] Digital filter 31X performs low-pass filtering on the X-axis angular velocity signal output from the X-axis angular velocity sensor 21X to reduce unwanted signal components outside the frequency band of the vehicle 400's motion. Digital filter 31Y performs low-pass filtering on the Y-axis angular velocity signal output from the Y-axis angular velocity sensor 21Y to reduce unwanted signal components outside the frequency band of the vehicle 400's motion. Digital filter 31Z performs low-pass filtering on the Z-axis angular velocity signal output from the Z-axis angular velocity sensor 21Z to reduce unwanted signal components outside the frequency band of the vehicle 400's motion.

[0031] The bias correction unit 40X removes the bias, which is the error of the automobile 400 when it is stationary, from the output signal of the digital filter 30X, thereby correcting the acceleration a in the X-axis direction. x The bias correction unit 40Y removes the bias from the output signal of the digital filter 30Y, thereby calculating the acceleration a in the Y-axis direction. y The bias correction unit 40Z removes the bias from the output signal of the digital filter 30Z, thereby calculating the acceleration a in the Z-axis direction. z Calculate.

[0032] The bias correction unit 41X removes the bias, which is the error of the stationary automobile 400, from the output signal of the digital filter 31X, thereby correcting the angular velocity ω around the X axis. xThe bias correction unit 41Y removes the bias from the output signal of the digital filter 31Y, thereby calculating the angular velocity ω around the Y axis. y The bias correction unit 41Z removes the bias from the output signal of the digital filter 31Z, thereby calculating the angular velocity ω around the Z axis. z Calculate.

[0033] The bias correction units 40X, 40Y, 40Z, 41X, 41Y, and 41Z can remove bias in several ways, including, for example, correcting the bias using the average output value when stationary, correcting it through integration with other measuring instruments such as GNSS or LiDAR, and removing low-frequency noise and bias components outside the frequency band of the vehicle's motion using a high-pass filter.

[0034] Sensor module 1 includes a Z-axis angular velocity sensor 22Z, a digital filter 32Z, and a bias correction unit 42Z.

[0035] The Z-axis angular velocity sensor 22Z detects the angular velocity around the Z' axis, which corresponds to the Z axis, and outputs a Z' axis detection signal corresponding to the detected angular velocity. In this embodiment, the Z-axis angular velocity sensor 22Z is ​​a crystal gyro sensor that has a sensor element made of quartz and detects angular velocity with high precision.

[0036] The digital filter 32Z performs a low-pass filter on the Z'-axis angular velocity signal output from the Z-axis angular velocity sensor 22Z, reducing unwanted signal components that are outside the bandwidth of the vehicle 400's motion.

[0037] The bias correction unit 42Z removes the bias, which is the error of the stationary automobile 400, from the output signal of the digital filter 32Z, thereby correcting the angular velocity Ω around the Z' axis. z Calculate.

[0038] The sensor module 1 includes a misalignment correction unit 50 and an attitude / direction estimation unit 60.

[0039] Ideally, the Z-axis, which is the detection axis of the Z-axis angular velocity sensor 21Z, and the Z'-axis, which is the detection axis of the Z-axis angular velocity sensor 22Z, should perfectly coincide. However, in reality, the 6DoF sensor 10, which includes the Z-axis angular velocity sensor 21Z, and the Z-axis angular velocity sensor 22Z are separate components, resulting in alignment errors due to the misalignment of the Z'-axis relative to the Z-axis.

[0040] The misalignment correction unit 50 controls the angular velocity ω around the Z axis. z Based on the sensitivity ratio and the X-axis angular velocity signal, Y-axis angular velocity signal, Z-axis angular velocity signal, and Z'-axis angular velocity signal, the angular velocity Ω around the Z' axis due to the displacement of the Z' axis relative to the Z axis is calculated. z The alignment error, which is an error of ω, is corrected. As will be described later, in this embodiment, the misalignment correction unit 50 corrects the angular velocity ω x ,ω y ,ω z ,Ω z Based on this, the angular velocity ω is determined by the Kalman filter. z The coefficient for the sensitivity ratio is estimated, and the sensitivity ratio is calculated from the estimated coefficient. Then, the misalignment correction unit 50 calculates the angular velocity ω z Based on the sensitivity ratio, angular velocity Ω z The angular velocity Ω corrected for alignment error z C Calculate.

[0041] The attitude / direction estimation unit 60 receives the direction Ψ output from the direction / velocity / position estimation unit 120. gnss And acceleration a x ,a y ,a z and angular velocity ω x ,ω y ,Ω z C Based on this, the roll angle Φ and pitch angle Θ are estimated as the relative attitude of the sensor module 1, and the yaw angle Ψ is estimated as the relative orientation of the sensor module 1, using various known methods. Since the sensor module 1 is fixed to the automobile 400, the relative attitude and orientation of the sensor module 1 correspond to the relative attitude and orientation of the automobile 400.

[0042] 1-3. Processing of the misalignment correction unit In this embodiment, the misalignment correction unit 50 assumes that the following conditions [1] to [3] are met, and uses a Kalman filter to determine the angular velocity ω around the Z axis. z The coefficient relating to the sensitivity ratio is estimated, and the angular velocity ω calculated from that coefficient is z Based on the sensitivity ratio, angular velocity Ω z The alignment error is corrected. [1] The reference axes for alignment are the X, Y, and Z axes, which are the detection axes of the 6DoF sensor 10. Since the 3-axis angular velocity sensor 12, which is a MEMS gyro sensor, is generally manufactured by photolithography, the X, Y, and Z axes intersect each other at approximately 90°. [2] The Z-axis angular velocity sensor 22Z, which is a quartz gyro sensor, has a smaller sensitivity error compared to the 3-axis angular velocity sensor 12, which is a MEMS gyro sensor. [3] The automobile 400 on which the sensor module 1 is mounted is in motion.

[0043] The true value of the angular velocity around the X axis is ω x,in The true value of the angular velocity around the Y axis is ω y,in The true value of the angular velocity around the Z axis is ω z,in Let the angular velocity ω x The sensitivity ratio is S x -1 , angular velocity ω y The sensitivity ratio is S y -1 , angular velocity ω z The sensitivity ratio is S z -1 Then, angular velocity ω x ,ω y ,ω z This is expressed by equation (1). Sensitivity ratio S x -1 ,S y -1 ,S z -1 Ideally, it is 1, but in reality, it can be greater than 1 or less than 1.

[0044]

number

[0045] Similar to the Z' axis corresponding to the Z axis, we assume an X' axis corresponding to the X axis and a Y' axis corresponding to the Y axis, and as shown in Figure 4, the angle between the X axis and the X' axis is γ x The angle between the Y-axis and the Y'-axis is γ y The angle between the Z axis and the Z' axis is γ z Let's assume that angle γ corresponds to the misalignment of the Z' axis relative to the Z axis. z Assuming that is sufficiently small, the angle γ is as shown in Figure 5. z This corresponds to the angle γ of misalignment of the Z' axis in the X-axis direction. zx And the angle γ corresponds to the misalignment of the Z' axis in the Y-axis direction. zy It is decomposed into two parts. Therefore, angle γ z and angle γ zx γ zy The relationship (2) is obtained.

[0046]

number

[0047] Similarly, angle γ corresponds to the misalignment of the X' axis relative to the X axis. x This corresponds to the angle γ of misalignment in the Y-axis direction of the X' axis. xy The angle γ corresponds to the misalignment of the X' axis in the Z-axis direction. xz It is decomposed into and . Similarly, angle γ corresponds to the misalignment of the Y' axis with respect to the Y axis. y This corresponds to the angle γ of the Y' axis in the X-axis direction. yx And the angle γ corresponds to the misalignment of the Y' axis in the Z-axis direction. yz It is broken down into two parts.

[0048] Therefore, angular velocity Ω x ,Ω y ,Ω z and the true value of angular velocity ω x,in ,ω y,in ,ω z,in The relationship is expressed by equation (3). In equation (3), from the previously mentioned condition [2], the angular velocity Ω zThe sensitivity ratio is set to 1. Also, in reality, there are no angular velocity sensors for detecting the X'-axis and the Y'-axis, so the angular velocity Ω x , Ω y has a sensitivity ratio of 0, and the angular velocity Ω y , Ω z is also 0.

[0049]

Number

[0050] From equations (1) and (3), equation (4) is derived.

[0051]

Number

[0052] Expanding equation (4), equation (5) is derived. In equation (5), the coefficient a1 = S x ·sinγ zx , the coefficient a2 = S y ·sinγ zy , a3 = S z ·cosγ z .

[0053]

Number

[0054] From equation (1), equation (6) is derived.

[0055]

Number

[0056] From equations (5) and (6), equation (7) is derived.

[0057]

Number

[0058] In this embodiment, the misalignment correction unit 50 estimates the coefficients a 1,k , a 2,k , a 3,k using a Kalman filter. Therefore, as shown in Equation (8), a state variable x k having the coefficients a1, a2, a3 as elements is defined.

[0059]

Equation

[0060] The state equation is defined by Equation (9). In Equation (9), F k is the state transition matrix, and as shown in Equation (10), it is assumed to be a 3×3 identity matrix. Also, in Equation (9), v k is the system noise, and as shown in Equation (11), it has elements of σ v1 , σ v2 , σ v3 set to appropriate values in advance. σ v1 , σ v2 , σ v3 are set to finite values of 0 or more based on the sensitivity characteristics of the Z-axis angular velocity sensor 21Z, the alignment accuracy during mounting, the temperature characteristics, etc.

[0061]

Equation

[0062]

Equation

[0063]

Equation

[0064] In this embodiment, as shown in Equation (12), the angular velocity Ω k about the Z' axis is set as the observation variable y z,k .

[0065]

number

[0066] The observation equation is defined by equation (13). In equation (13), H k is the observation matrix, and is expressed by equation (14). Also, in equation (13), w k σ is the observed noise, and as shown in equation (15), it is a value that is appropriate in advance. w It will be set to σ. w This value is set to a finite value greater than or equal to 0, based on the noise characteristics of the Z-axis angular velocity sensor 22Z.

[0067]

number

[0068]

number

[0069]

number

[0070] The misalignment correction unit 50 performs the prediction step, observation step and update step of the Kalman filter, and coefficient a 1,k ,a 2,k ,a 3,k We estimate this.

[0071] First, in the prediction step, the misalignment correction unit 50 uses equation (16) based on the state equation (9) to correct the state variable x^ updated in the update step at time k-1. k-1|k-1 From, the state variable x^ at time k k|k-1 Predict the state variable x^ k-1|k-1 x^ is the posterior estimate at time k-1, and the state variable x^ k|k-1 This is a prior estimate of time k.

[0072]

number

[0073] Furthermore, in the prediction step, the misalignment correction unit 50 uses equation (17) to correct the state variable x^ updated in the update step at time k-1. k-1|k-1 The covariance matrix P k-1|k-1 From, the state variable x^ at time k k|k-1 The covariance matrix P k|k-1 Predicts that in equation (17), V k is system noise v k This is the covariance matrix, which is calculated by equation (18).

[0074]

number

[0075]

number

[0076] Next, in the observation step, the misalignment correction unit 50 uses equation (19) based on the observation equation (13) to correct the state variable x^ predicted in the prediction step at time k. k|k-1 Calculate the values ​​observed from the data.

[0077]

number

[0078] Then, the misalignment correction unit 50 calculates the observed variable y at the time k that was actually observed according to equation (20). k The observed residual e is the difference between the value of and the value calculated by equation (19). k Calculate.

[0079]

number

[0080] Furthermore, in the observation step, the misalignment correction unit 50 uses equation (21) to correct the state variable x^ predicted in the prediction step at time k. k|k-1 The covariance matrix P k|k-1 From, the observed residual e k The covariance matrix S k Calculate the following. In equation (21), W k The observed noise w k This is the covariance matrix, which is calculated by equation (22).

[0081]

number

[0082]

number

[0083] Finally, in the update step, the misalignment correction unit 50 adjusts the state variable x^ predicted in the prediction step at time k. k|k-1 The covariance matrix P k|k-1 And the observational residual e calculated at the observation step at time k. k The covariance matrix S k Therefore, according to equation (23), the Kalman gain K k Calculate.

[0084]

number

[0085] Then, in the update step, the misalignment correction unit 50 calculates the Kalman gain K using equation (24). k And the observational residual e calculated at the observation step at time k. k Based on this, the state variable x^ predicted at the prediction step at time k k|k-1 The state variable x^ k|k Update to the state variable x^ k|k-1 x^ is a prior estimate of time k. Also, the state variable x^ k|kThis is the posterior estimate of time k.

[0086]

number

[0087] Thus, the misalignment correction unit 50 uses a Kalman filter to correct the state variable x k The coefficient a included in 1,k ,a 2,k ,a 3,k We estimate this. Figure 6 shows the operational block diagram of the Kalman filter according to equations (8) to (24).

[0088] Furthermore, in the update step, the misalignment correction unit 50 calculates the Kalman gain K using equation (25). k Based on this, the state variable x^ predicted at the prediction step at time k k|k-1 The covariance matrix P k|k-1 The state variable x^ k|k The covariance matrix P k|k Update to [date / time].

[0089]

number

[0090] covariance matrix P k|k In p 11 is the coefficient a 1,k The variance of p 22 is the coefficient a 2,k The variance of p 33 is the coefficient a 3,k This is the variance.

[0091] Then, the misalignment correction unit 50 calculates the angle γ corresponding to the misalignment of the Z' axis in the X-axis direction from the coefficients a1 and a2 estimated by the Kalman filter using equations (26) and (27). zx,k And angle γ corresponding to the misalignment of the Z' axis in the Y-axis direction. zy,k Calculate.

[0092]

number

[0093]

number

[0094] Furthermore, the misalignment correction unit 50 calculates the angle γ corresponding to the misalignment of the Z' axis with respect to the Z axis according to equation (28). z Calculate the absolute value of [the value].

[0095]

number

[0096] Furthermore, the misalignment correction unit 50 calculates the sensitivity ratio S according to equation (29). x -1 Calculate the reciprocal of |γ. z,k |<90°, therefore cosγ z,k >0

[0097]

number

[0098] Then, the misalignment correction unit 50 calculates the angular velocity Ω according to equations (7) and (29) mentioned above. z The angular velocity Ω corrected for alignment error z C Calculate.

[0099] Note that the 3-axis angular velocity sensor 12 is an example of a "first angular velocity sensor," and the Z-axis angular velocity sensor 22Z is ​​an example of a "second angular velocity sensor." Also, the X-axis is an example of a "first axis," the Y-axis is an example of a "second axis," the Z-axis is an example of a "third axis," and the Z'-axis is an example of a "fourth axis." Furthermore, the X-axis angular velocity signal is an example of a "first angular velocity signal," the Y-axis angular velocity signal is an example of a "second angular velocity signal," the Z-axis angular velocity signal is an example of a "third angular velocity signal," and the Z'-axis angular velocity signal is an example of a "fourth angular velocity signal." In addition, the misalignment correction unit 50 is an example of a "correction circuit."

[0100] 1-4. Effects As described above, in the sensor module 1 of the first embodiment, the Kalman filter measures the angular velocity ω around the Z axis. z Sensitivity ratio S z -1 Since the coefficient a3 related to can be estimated with high accuracy, the sensitivity ratio S can be derived from the coefficient a3. z -1 This can be calculated with high accuracy. Therefore, according to the sensor module 1 of the first embodiment, the angular velocity ω around the X axis x , angular velocity ω around the Y axis y , angular velocity ω around the Z axis z and angular velocity Ω around the Z' axis z Along with the angular velocity ω around the Z axis z Sensitivity ratio S z -1 Taking this into consideration, the angular velocity Ω around the Z' axis caused by the misalignment of the Z-axis angular velocity sensor 22Z relative to the 3-axis angular velocity sensor 12 is z This allows for accurate correction of errors.

[0101] 2. Second Embodiment In the following description of the second embodiment, the same reference numerals are used for components similar to those in the first embodiment, and explanations that overlap with those in the first embodiment will be omitted or simplified. The main points to be described will be those that differ from the first embodiment.

[0102] For example, the sensor module 1 of the second embodiment is incorporated into the self-position estimation system 100, similar to the first embodiment. The function and configuration of the sensor module 1 of the second embodiment are the same as in Figure 3, so its illustration and description are omitted. In the sensor module 1 of the second embodiment, at least a part of the processing of the misalignment correction unit 50 differs from that of the first embodiment.

[0103] In the first embodiment, the misalignment correction unit 50 adjusts the angle γ corresponding to the misalignment of the Z' axis in the X-axis direction. zx , angle γ which corresponds to the misalignment of the Z' axis in the Y-axis direction. zy and angular velocity ω z Sensitivity ratio S z -1 This is calculated by equations (26) to (29) based on coefficients a1 and a2 estimated by the Kalman filter. In contrast, in the second embodiment, the misalignment correction unit 50 uses the Kalman filter to calculate the angle γ zx γ zy and sensitivity ratio S z -1 We directly estimate it.

[0104] Substituting equations (26) to (28) into equation (5) gives equation (30), and from equation (30) and equation (6) gives equation (31).

[0105]

number

[0106]

number

[0107] In the second embodiment, as shown in formula (32), angle γ zx γ zy and sensitivity ratio S z -1 A state variable x whose elements are the reciprocals of k This is defined.

[0108] [Number]

[0109] In the second embodiment as well, the equation of state is the same as the above-described equation (9), and as in the above-described equation (12), the observation variable y k is the angular velocity Ω around the Z'-axis z,k is set.

[0110] In the second embodiment, the observation equation is defined by Equation (33). In Equation (33), w k is the observation noise and is shown by the above-described equation (15).

[0111] [Number]

[0112] As shown in Equation (33), since the observation equation is non-linear with respect to the state variable x k , it is necessary to introduce a non-linear Kalman filter. The misalignment correction unit 50, as a non-linear Kalman filter, for example, by an extended Kalman filter that locally linearizes a non-linear function to calculate the covariance, estimates the angles γ zx , γ zy and the sensitivity ratio S z -1 . In Equation (33), H k is the observation Jacobian and is represented by Equation (34).

[0113] [Number]

[0114] The misalignment correction unit 50 executes the prediction step, the observation step, and the update step of the extended Kalman filter, and estimates the angles γ zx,k , γ zy,k and the sensitivity ratio S z,k -1 .

[0115] First, in the prediction step, the misalignment correction unit 50 predicts the state variable x^ at time k from the state variable x^ updated in the update step at time k - 1 according to the previous equation (16) based on the previous state equation (9). k-1|k-1 from the state variable x^ k|k-1 at time k.

[0116] Also, in the prediction step, the misalignment correction unit 50 predicts the covariance matrix P of the state variable x^ at time k from the covariance matrix P of the state variable x^ updated in the update step at time k - 1 according to the previous equation (17). k-1|k-1 of k-1|k-1 the state variable x^ k|k-1 at time k - 1 to the covariance matrix P k|k-1 of the state variable x^ at time k.

[0117] Next, in the observation step, the misalignment correction unit 50 calculates the observed value from the state variable x^ predicted in the prediction step at time k according to the equation (35) based on the observation equation (33). k|k-1

[0118]

Number

[0119] Then, the misalignment correction unit 50 calculates the observation residual e, which is the difference between the value of the observed variable y at time k actually observed and the value calculated by the equation (35), according to the equation (36). k k

[0120]

Number

[0121] Also, in the observation step, the misalignment correction unit 50 calculates the covariance matrix S of the observation residual e from the covariance matrix P of the state variable x^ predicted in the prediction step at time k according to the previous equation (21). k|k-1 of k|k-1 the state variable x^ k to the covariance matrix S k of the observation residual e.

[0122] ​​​ Finally, in the update step, the misalignment correction unit 50 adjusts the state variable x^ predicted in the prediction step at time k. k|k-1 The covariance matrix P k|k-1 And the observational residual e calculated at the observation step at time k. k The covariance matrix S k Therefore, according to equation (23) above, the Kalman gain K k Calculate.

[0123] Then, in the update step, the misalignment correction unit 50 calculates the Kalman gain K using the aforementioned equation (24). k And the observational residual e calculated at the observation step at time k. k Based on this, the state variable x^ predicted at the prediction step at time k k|k-1 The state variable x^ k|k Update to [date / time].

[0124] Thus, the misalignment correction unit 50 uses an extended Kalman filter to correct the state variable x k The angle γ included zx,k γ zy,k and sensitivity ratio S z,k -1 We estimate this.

[0125] Furthermore, in the update step, the misalignment correction unit 50 calculates the Kalman gain K using the aforementioned equation (25). k Based on this, the state variable x^ predicted at the prediction step at time k k|k-1 The covariance matrix P k|k-1 The state variable x^ k|k The covariance matrix P k|k Update to [date / time].

[0126] Then, the misalignment correction unit 50 calculates the angular velocity Ω according to the aforementioned equation (31). z The angular velocity Ω corrected for alignment error z C Calculate.

[0127] The sensor module 1 of the second embodiment described above provides the same effects as the sensor module 1 of the first embodiment. Furthermore, the sensor module 1 of the second embodiment uses a Kalman filter to measure the angle γ zx γ zy and sensitivity ratio S z -1 Directly estimating this can sometimes improve estimation accuracy.

[0128] 3. Third Embodiment In the following description of the third embodiment, the same reference numerals are used for components similar to those in the first or second embodiment. Descriptions that overlap with those of the first or second embodiment will be omitted or simplified, and the differences from the first and second embodiments will be described primarily.

[0129] For example, the sensor module 1 of the third embodiment is incorporated into the self-position estimation system 100, similar to the first and second embodiments. The function and configuration of the sensor module 1 of the third embodiment are the same as in Figure 3, so its illustration and description are omitted. In the sensor module 1 of the third embodiment, at least a part of the processing of the misalignment correction unit 50 differs from that of the first and second embodiments.

[0130] In the third embodiment, the misalignment correction unit 50 reduces the computational load of the Kalman filter by adjusting the angular velocity Ω z and angular velocity ω x ,ω y ,ω z The approximate formula obtained by Taylor expanding the trigonometric functions of the aforementioned equation (30), which is a relationship between the angle γ, is used as the observation equation for the Kalman filter. zx γ zy and sensitivity ratio S z -1 We estimate this.

[0131] For example, if we approximate the sine term in equation (30) above by performing a Taylor expansion and using a linear expansion, we obtain equation (37), and from equation (37) and equation (6) above, we can derive equation (38).

[0132] [Number]

[0133] [Number]

[0134] Furthermore, when the cosine term in Equation (37) is expanded by Taylor series and approximated to the zero order, Equation (39) is obtained. From Equation (39) and the previous Equation (6), Equation (40) is derived.

[0135] [Number]

[0136] [Number]

[0137] In the third embodiment, similar to the second embodiment, the angle γ is as in the previous Equation (32). zx , γ zy and the reciprocal of the sensitivity ratio S z -1 are used as elements to define the state variable x. k Also in the third embodiment, the state equation is the same as the previous Equation (9), and the angular velocity Ω around the Z'-axis is set as the observation variable y k as in the previous Equation (12). z,k

[0138] For example, the misalignment correction unit 50 sets the observation equation as in Equation (41) based on Equation (37), and estimates the angle γ zx,k , γ zy,k and the sensitivity ratio S z,k -1 using a non-linear Kalman filter such as an extended Kalman filter. In this case, the misalignment correction unit 50 can calculate the angular velocity Ω z with the alignment error of the angular velocity Ω corrected by Equation (38). z C ​

[0139]

number

[0140] Alternatively, the misalignment correction unit 50 sets the observation equation as shown in equation (42) based on equation (39), and uses a linear Kalman filter to determine the angle γ zx,k γ zy,k and sensitivity ratio S z,k -1 It is also possible to estimate the angular velocity Ω. In this case, the misalignment correction unit 50 calculates the angular velocity Ω according to equation (40). z The angular velocity Ω corrected for alignment error z C It is possible to calculate this.

[0141]

number

[0142] The sensor module 1 of the third embodiment described above provides the same effects as the sensor module 1 of the second embodiment. Furthermore, the sensor module 1 of the third embodiment reduces the computational load by using an approximation formula obtained by Taylor expanding trigonometric functions as the observation equation for the Kalman filter. This can increase the computation rate and may improve the estimation accuracy.

[0143] 4. Fourth Embodiment In the following description of the fourth embodiment, the same reference numerals are used for components that are the same as those in the first to third embodiments. Descriptions that overlap with those in the first to third embodiments will be omitted or simplified, and the description will mainly focus on aspects that differ from any of the first to third embodiments.

[0144] For example, the sensor module 1 of the fourth embodiment is incorporated into the self-position estimation system 100, similar to the first to third embodiments. The function and configuration of the sensor module 1 of the fourth embodiment are the same as in Figure 3, so its illustration and description are omitted. In the sensor module 1 of the fourth embodiment, at least a part of the processing of the misalignment correction unit 50 differs from that of the first to third embodiments.

[0145] While the digital filters 31X, 31Y, 31Z, and 32Z can remove unwanted high-frequency noise, it is difficult to appropriately set parameters such as the cutoff frequency and order of the digital filters 31X, 31Y, 31Z, and 32Z in advance when the bandwidth of the vehicle 400's motion cannot be predicted. Therefore, in the fourth embodiment, the misalignment correction unit 50 measures the angular velocity Ω around the Z' axis. z The angle Ψ obtained by integrating z And the angular velocity ω around the X-axis x The angle θ obtained by integrating x , angular velocity ω around the Y axis y The angle θ obtained by integrating y and angular velocity ω around the Z axis z The angle θ obtained by integrating z The relationship between this and the angle γ is used as the observation equation for the Kalman filter. zx γ zy and sensitivity ratio S z -1 We estimate the angular velocity Ω. z ,ω x ,ω y ,ω z By integrating, unwanted high-frequency noise components are reduced.

[0146] For example, integrating equation (5) with respect to time yields equation (43).

[0147]

number

[0148] After the implementation of the 6DoF sensor 10 and the Z-axis angular velocity sensor 22Z, the time-series fluctuations of misalignment and sensitivity ratio may be small. Assuming that these are time-invariant, equation (44) can be obtained from equation (43).

[0149]

number

[0150] In equation (44), discretizing each integral as in equation (45) and substituting it back into equation (44) yields equation (46).

[0151]

number

[0152]

number

[0153] In the fourth embodiment, for example, as shown in equation (8) above, a state variable x has coefficients a1, a2, and a3 as its elements. k The equation is defined, and the state equation is the same as equation (9) above. Also, as shown in equation (47), the observed variable y k Ω represents the angular velocity around the Z' axis. z,k The angle Ψ obtained by integrating z,k This will be set.

[0154]

number

[0155] The observation equation is defined by equation (13) above, and the observation matrix H k This is expressed by equation (48).

[0156]

number

[0157] The misalignment correction unit 50 performs the prediction step, observation step, and update step of the Kalman filter in the same manner as in equations (16) to (25) above, and calculates the coefficient a 1,k ,a 2,k ,a 3,k The misalignment correction unit 50 then estimates the angular velocity Ω from the coefficients a1, a2, a3 estimated by equations (7), (26) to (29) above. z The angular velocity Ω corrected for alignment error z C Calculate.

[0158] Furthermore, the misalignment correction unit 50 uses angular velocity Ω z ,ω x ,ω y ,ω z Since integration errors accumulate when the integral is integrated over a long period of time, the integral value may be reset to zero as needed. In other words, the misalignment correction unit 50 may reset the previous integral value to zero at any given time, as shown in equation (49).

[0159]

number

[0160] The sensor module 1 of the fourth embodiment described above provides the same effects as the sensor module 1 of the first embodiment. Furthermore, the sensor module 1 of the fourth embodiment provides the angular velocity Ω z ,ω x ,ω y ,ω z Integrating the signal can reduce high-frequency noise, potentially improving estimation accuracy.

[0161] 5. Fifth Embodiment In the following description of the fifth embodiment, the same reference numerals are used for components that are the same as those in the first to fourth embodiments. Descriptions that overlap with those in the first to fourth embodiments will be omitted or simplified, and the description will mainly focus on aspects that differ from the first to fourth embodiments.

[0162] For example, the sensor module 1 of the fifth embodiment is incorporated into the self-position estimation system 100, similar to the first to fourth embodiments. The function and configuration of the sensor module 1 of the fifth embodiment are the same as in Figure 3, so its illustration and description are omitted. In the sensor module 1 of the fifth embodiment, at least a part of the processing of the misalignment correction unit 50 differs from that of the first to fourth embodiments.

[0163] After the 6DoF sensor 10 and the Z-axis angular velocity sensor 22Z are implemented, the time-series fluctuations of misalignment and sensitivity ratio may be small. Therefore, in the fifth embodiment, the misalignment correction unit 50 controls the angular velocity Ω z Alignment error and angular velocity ω z Sensitivity ratio S z -1 We apply the Kalman filter assuming that the angular velocity Ω is invariant with respect to time. For example, angular velocity Ω z Alignment error and angular velocity ω z Sensitivity ratio S z -1 Assuming that is time-invariant, the misalignment correction unit 50 calculates the system noise v of the Kalman filter shown in equation (11) above, as shown in equation (50). k You can also set it to zero.

[0164]

number

[0165] Then, the misalignment correction unit 50 applies a Kalman filter similar to that of any of the first to fourth embodiments, and the angular velocity Ω z The angular velocity Ω corrected for alignment error z C Calculate.

[0166] According to the sensor module 1 of the fifth embodiment described above, the same effects as the sensor module 1 of any of the first to fourth embodiments can be obtained. Furthermore, according to the sensor module 1 of the fifth embodiment, angular velocity Ωz Alignment error and angular velocity ω z Sensitivity ratio S z -1 By applying a Kalman filter while assuming that the parameter is invariant with respect to time, it is possible to obtain computational conditions that are closer to reality, which can improve estimation accuracy.

[0167] 6. Sixth Embodiment In the following description of the sixth embodiment, the same reference numerals are used for components that are the same as those in the first to fifth embodiments. Descriptions that overlap with those in the first to fifth embodiments will be omitted or simplified, and the description will mainly focus on aspects that differ from any of the first to fifth embodiments.

[0168] For example, the sensor module 1 of the sixth embodiment is incorporated into the self-position estimation system 100, similar to the first to fifth embodiments. The function and configuration of the sensor module 1 of the sixth embodiment are the same as in Figure 3, so its illustration and description are omitted. In the sensor module 1 of the sixth embodiment, at least a part of the processing of the misalignment correction unit 50 differs from that of the first to fifth embodiments.

[0169] In the first to fifth embodiments, the detected angular velocity ω x ,ω y ,ω z If all of them are small, the angular velocity Ω due to the misalignment correction unit 50 z The accuracy of the alignment error correction may be reduced. Therefore, in the sixth embodiment, the misalignment correction unit 50 uses the angular velocity ω x ,ω y ,ω z If at least one of the following satisfies a predetermined condition, the angular velocity Ω z The alignment error is corrected. For example, the misalignment correction unit 50 corrects the angular velocity Ω for the union A∪B∪C of sets A, B, and C shown in equation (51). z The alignment error may be corrected. That is, the misalignment correction unit 50 corrects the angular velocity ω x The absolute value of is a predetermined threshold ω th,xLarger than or equal to angular velocity ω y The absolute value of is a predetermined threshold ω th,y Larger than or equal to angular velocity ω z The absolute value of is a predetermined threshold ω th,z If it is greater than, the angular velocity ω x ,ω y ,ω z Applying this to a Kalman filter, the angular velocity Ω z The alignment error may be corrected.

[0170]

number

[0171] According to the sensor module 1 of the sixth embodiment described above, the same effects as the sensor module 1 of any of the first to fifth embodiments can be obtained. Furthermore, according to the sensor module 1 of the sixth embodiment, the angular velocity ω x ,ω y ,ω z Angular velocity Ω when at least one of the following satisfies a predetermined condition z Correcting alignment errors can sometimes improve correction accuracy.

[0172] 7. Seventh Embodiment In the following description of the seventh embodiment, the same reference numerals are used for components that are the same as those in the first to sixth embodiments. Descriptions that overlap with those in the first to sixth embodiments will be omitted or simplified, and the description will mainly focus on aspects that differ from the first to sixth embodiments.

[0173] For example, the sensor module 1 of the seventh embodiment is incorporated into the self-position estimation system 100, similar to the first to sixth embodiments. The function and configuration of the sensor module 1 of the seventh embodiment are the same as in Figure 3, so its illustration and description are omitted. In the sensor module 1 of the seventh embodiment, at least a part of the processing of the misalignment correction unit 50 differs from that of the first to sixth embodiments.

[0174] Depending on the motion state of the automobile 400, the estimation accuracy of the Kalman filter may decrease or the estimation may become unstable. To avoid this, in the seventh embodiment, the misalignment correction unit 50 uses the state variable x of the Kalman filter from any of the first to sixth embodiments. k If the variance of each element falls below the minimum value of that variance within a predetermined period, each element is updated, and the state variable x k Based on angular velocity Ω z The alignment error is corrected. For example, when applying the Kalman filter of the first embodiment, the misalignment correction unit 50 corrects the covariance matrix P of equation (25) mentioned above. k|k The coefficient a included in 1,k Variance p 11 The variance p over a predetermined period 11 If it falls below the minimum value, the state variable x k The coefficient a included in 1,k Update coefficient a 2,k Variance p 22 The variance p over a predetermined period 22 If it falls below the minimum value, the state variable x k The coefficient a included in 2,k Update coefficient a 3,k Variance p 33 The variance p over a predetermined period 33 If it falls below the minimum value, the state variable x k The coefficient a included in 3,k Update the distribution p. 11 ,p 22 ,p 33 These are coefficients a, respectively. 1,k ,a 2,k ,a 3,k This serves as an indicator of reliability, and the smaller the value, the higher the reliability. Therefore, the misalignment correction unit 50 uses the coefficient a that has the highest reliability over a predetermined period. 1,k ,a 2,k ,a 3,k Using angular velocity Ω z This can correct alignment errors.

[0175] Furthermore, if the specified period is too long, even if the state of the sensor module 1 changes due to deterioration over time, the state variable x kThere is a risk that the current situation may not be reflected because the data is not updated. Therefore, the specified period is set to a period during which there is little change in the state of sensor module 1, for example, one day.

[0176] According to the sensor module 1 of the seventh embodiment described above, the same effects as the sensor module 1 of any of the first to sixth embodiments can be obtained. Furthermore, according to the sensor module 1 of the seventh embodiment, a highly reliable state variable x k Based on angular velocity Ω z Correcting alignment errors can sometimes improve correction accuracy.

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

[0178] In each of the embodiments described above, the misalignment correction unit 50 corrects alignment errors related to the Z-axis angular velocity, but it may also correct alignment errors related to at least one of the X-axis angular velocity, Y-axis angular velocity, Z-axis angular velocity, X-axis acceleration, Y-axis acceleration, and Z-axis acceleration.

[0179] Furthermore, in the above embodiments, the self-position estimation system 100 for an automobile 400 was given as an example of a system using the sensor module 1, but the sensor module 1 may be used in other systems. Also, the sensor module 1 may be mounted on any moving object other than an automobile.

[0180] Furthermore, in each of the above embodiments, an example was given in which the Z-axis angular velocity sensor 22Z, which has a smaller sensitivity error compared to the 3-axis angular velocity sensor 12, is a crystal gyro sensor. However, the Z-axis angular velocity sensor 22Z may also be a FOG sensor. FOG is an abbreviation for Fiber Optic Gyroscope.

[0181] The embodiments and variations described above are examples only and are not limited thereto. For example, each embodiment and each variation can be combined as appropriate.

[0182] The present invention includes configurations substantially identical to those described in the embodiments, for example, configurations with the same function, method, and results, or configurations with the same purpose and effect. Furthermore, the present invention includes configurations in which non-essential parts of the configurations described in the embodiments are replaced. Furthermore, the present invention includes configurations that produce the same effects or achieve the same purpose as those described in the embodiments. Finally, the present invention includes configurations that add known technology to the configurations described in the embodiments.

[0183] The following can be derived from the embodiments and modifications described above.

[0184] One embodiment of a sensor module is: When three mutually orthogonal axes are designated as the first axis, second axis, and third axis, A first angular velocity sensor that detects the angular velocity around the first axis and outputs a first angular velocity signal, detects the angular velocity around the second axis and outputs a second angular velocity signal, and detects the angular velocity around the third axis and outputs a third angular velocity signal, A second angular velocity sensor detects the angular velocity around the fourth axis corresponding to the third axis and outputs a fourth angular velocity signal, The system includes a correction circuit that corrects alignment errors, which are errors in angular velocity around the fourth axis due to the misalignment of the fourth axis with respect to the third axis, based on the sensitivity ratio of angular velocity around the third axis and the first, second, third, and fourth angular velocity signals.

[0185] This sensor module allows for accurate correction of errors in angular velocity around the fourth axis caused by misalignment of the second angular velocity sensor relative to the first angular velocity sensor, by taking into account the sensitivity ratio of the angular velocity around the third axis, along with the first, second, third, and fourth angular velocity signals.

[0186] In one embodiment of the sensor module, The correction circuit described above is The coefficients related to the sensitivity ratio may be estimated using a Kalman filter, and the sensitivity ratio may be calculated from the estimated coefficients.

[0187] According to this sensor module, the Kalman filter can accurately estimate the coefficient related to the sensitivity ratio of the angular velocity around the third axis, and from this coefficient, the sensitivity ratio of the angular velocity around the third axis can be calculated with high accuracy.

[0188] In one embodiment of the sensor module, The correction circuit described above is The sensitivity ratio may be estimated using a Kalman filter.

[0189] According to this sensor module, the estimation accuracy can be improved by directly estimating the sensitivity ratio using a Kalman filter.

[0190] In one embodiment of the sensor module, The correction circuit described above is The sensitivity ratio may be estimated using an approximate formula obtained by Taylor expanding the trigonometric functions of the relationship between the angular velocity around the fourth axis and the angular velocity around the first axis, the angular velocity around the second axis, and the angular velocity around the third axis, as the observation equation for the Kalman filter.

[0191] This sensor module reduces the computational load by using an approximation formula derived from a Taylor expansion of trigonometric functions as the observation equation for the Kalman filter. This can increase the computation rate and potentially improve estimation accuracy.

[0192] In one embodiment of the sensor module, The correction circuit described above is The sensitivity ratio may be estimated using the observation equation of the Kalman filter as the relationship between the angle obtained by integrating the angular velocity around the fourth axis, the angle obtained by integrating the angular velocity around the first axis, the angle obtained by integrating the angular velocity around the second axis, and the angle obtained by integrating the angular velocity around the third axis.

[0193] According to this sensor module, integrating the angular velocity can reduce high-frequency noise components, potentially improving estimation accuracy.

[0194] In one embodiment of the sensor module, The correction circuit described above is The Kalman filter may be applied assuming that the alignment error and the sensitivity ratio are invariant with respect to time.

[0195] This sensor module allows for calculation conditions that are closer to reality, which can sometimes improve estimation accuracy.

[0196] In one embodiment of the sensor module, The correction circuit described above is The system noise of the Kalman filter may be set to zero.

[0197] This sensor module allows for calculation conditions that are closer to reality, which can sometimes improve estimation accuracy.

[0198] In one embodiment of the sensor module, The correction circuit described above is The alignment error may be corrected if at least one of the angular velocity around the first axis, the angular velocity around the second axis, and the angular velocity around the third axis satisfies a predetermined condition.

[0199] According to this sensor module, correction accuracy may be improved by correcting alignment errors when the angular velocity meets certain conditions.

[0200] In one embodiment of the sensor module, The correction circuit described above is The elements of the state variables of the Kalman filter may be updated if the variance of each element falls below the minimum value of the variance within a predetermined period.

[0201] According to this sensor module, correction accuracy may be improved by correcting alignment errors based on highly reliable state variables. [Explanation of Symbols]

[0202] 1…Sensor module, 10…6DoF sensor, 11…3-axis accelerometer, 12…3-axis angular velocity sensor, 20X…X-axis accelerometer, 20Y…Y-axis accelerometer, 20Z…Z-axis accelerometer, 21X…X-axis angular velocity sensor, 21Y…Y-axis angular velocity sensor, 21Z,22Z…Z-axis angular velocity sensor, 30X,30Y,30Z,31X,31Y,31Z,32Z…Digital filter, 40X,40Y,40Z,41X,41Y,41Z,42Z…Bias correction unit, 50…Misalignment correction unit, 60…Attitude / direction estimation unit, 110…GNSS receiver, 120…Direction / speed / position estimation unit, 130…Wheel speed sensor, 140…Coordinate transformation unit, 150…Speed / position estimation unit, 160…Complex navigation calculation unit, 400…Automobile

Claims

1. When the three mutually orthogonal axes are designated as the first axis, the second axis, and the third axis, A first angular velocity sensor that detects the angular velocity around the first axis and outputs a first angular velocity signal, detects the angular velocity around the second axis and outputs a second angular velocity signal, and detects the angular velocity around the third axis and outputs a third angular velocity signal, A second angular velocity sensor detects the angular velocity around the fourth axis corresponding to the third axis and outputs a fourth angular velocity signal, A sensor module comprising: a sensitivity ratio of angular velocity around the third axis; and a correction circuit that corrects alignment errors, which are errors in angular velocity around the fourth axis due to the misalignment of the fourth axis with respect to the third axis, based on the first angular velocity signal, the second angular velocity signal, the third angular velocity signal, and the fourth angular velocity signal.

2. In claim 1, The correction circuit described above is A sensor module that estimates a coefficient related to the sensitivity ratio using a Kalman filter and calculates the sensitivity ratio from the estimated coefficient.

3. In claim 1, The correction circuit described above is A sensor module that estimates the sensitivity ratio using a Kalman filter.

4. In claim 3, The correction circuit described above is A sensor module that estimates the sensitivity ratio by using an approximate formula obtained by Taylor expanding the trigonometric functions of the relationship between the angular velocity around the fourth axis and the angular velocity around the first axis, the angular velocity around the second axis, and the angular velocity around the third axis, as the observation equation for the Kalman filter.

5. In claim 1, The correction circuit described above is A sensor module that estimates the sensitivity ratio by using the relationship between the angle obtained by integrating the angular velocity around the fourth axis, the angle obtained by integrating the angular velocity around the first axis, the angle obtained by integrating the angular velocity around the second axis, and the angle obtained by integrating the angular velocity around the third axis as the observation equation for a Kalman filter.

6. In any one of claims 2 to 5, The correction circuit described above is A sensor module that applies the Kalman filter assuming that the alignment error and the sensitivity ratio are invariant with respect to time.

7. In claim 6, The correction circuit described above is A sensor module that sets the system noise of the aforementioned Kalman filter to zero.

8. In any one of claims 1 to 5, The correction circuit described above is A sensor module that corrects the alignment error when at least one of the angular velocity around the first axis, the angular velocity around the second axis, and the angular velocity around the third axis satisfies a predetermined condition.

9. In any one of claims 2 to 5, The correction circuit described above is A sensor module that updates each element of the state variables of the Kalman filter when the variance of each element falls below the minimum value of the variance within a predetermined period.

Images