Angular velocity derivation device and angular velocity derivation method

Abstract

To provide a technique for suppressing deterioration in derivation accuracy of an angular velocity when using a three axis gyro sensor.SOLUTION: The extended Kalman filter 314 derives the attitude angle of the moving object based on the output value of the three axis acceleration sensor and the output value of the three axis gyro sensor. The attitude angle correction unit 330 corrects the attitude angle derived by the extended Kalman filter 314. The triaxial B-mode angular velocity derivation unit 318 derives the angular velocity based on the temporal change of the attitude angle derived by the extended Kalman filter 314 or the temporal change of the attitude angle corrected by the attitude angle correction unit 330 according to the variance value of the output value of the triaxial acceleration sensor and the variance value of the output value of the triaxial gyro sensor.SELECTED DRAWING: Figure 11

Description

[Technical Field]

[0001] The present invention relates to an angular velocity derivation technique, and more particularly to an angular velocity derivation device and method for deriving an angular velocity based on output values ​​of a three-axis gyro sensor. [Background technology]

[0002] The angular velocity derivation device is mounted on a vehicle or the like and updates the attitude angle using, for example, a three-axis gyro sensor. The angular velocity derivation device derives the angular velocity from the attitude angle as long as the accuracy of the attitude angle is maintained. On the other hand, when the accuracy of the attitude angle decreases, the angular velocity derivation device derives the angular velocity using a one-axis gyro sensor (see, for example, Patent Documents 1 and 2). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Publication No. 2019-74377 [Patent Document 2] Japanese Patent Application Publication No. 2019-74378 Summary of the Invention [Problem to be solved by the invention]

[0004] When deriving angular velocity using a three-axis gyro sensor, it is necessary to suppress deterioration in derivation accuracy.

[0005] The present invention has been made in view of the above circumstances, and its purpose is to provide a technique for suppressing deterioration in the accuracy of deriving angular velocity when a three-axis gyro sensor is used. [Means for solving the problem]

[0006] In order to solve the above problem, an angular velocity derivation device according to one embodiment of the present invention is an angular velocity derivation device that can be mounted on a moving body, and includes: an extended Kalman filter that derives an attitude angle of the moving body based on output values ​​of a triaxial acceleration sensor and output values ​​of a triaxial gyro sensor; an attitude angle correction unit that corrects the attitude angle derived in the extended Kalman filter; and an angular velocity derivation unit that derives an angular velocity based on the time change in the attitude angle derived in the extended Kalman filter or the time change in the attitude angle corrected in the attitude angle correction unit, in accordance with the variance of the output values ​​of the triaxial acceleration sensor and the variance of the output values ​​of the triaxial gyro sensor.

[0007] Another aspect of the present invention is an angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, the method comprising the steps of: deriving an attitude angle of the moving body by an extended Kalman filter based on output values ​​of a triaxial acceleration sensor and output values ​​of a triaxial gyro sensor, correcting the attitude angle derived by the extended Kalman filter, and deriving an angular velocity based on a time change in the attitude angle derived by the extended Kalman filter or a time change in the corrected attitude angle in accordance with a variance of the output values ​​of the triaxial acceleration sensor and a variance of the output values ​​of the triaxial gyro sensor.

[0008] Any combination of the above components, and any transformation of the present invention into a method, device, system, recording medium, computer program, etc., are also valid aspects of the present invention. [Effects of the Invention]

[0009] According to the present invention, it is possible to suppress deterioration in accuracy of deriving angular velocity when a three-axis gyro sensor is used. [Brief explanation of the drawings]

[0010] [Figure 1] 1 is a diagram illustrating a configuration of an angular velocity deriving device according to a first embodiment. [Figure 2] FIG. 2 is a diagram showing a coordinate system according to the first embodiment. [Figure 3]FIG. 2 is a diagram illustrating the configuration of a 6-axis A-mode processing unit in FIG. [Figure 4] FIG. 2 is a diagram illustrating a configuration of a four-axis mode processing unit in FIG. [Figure 5] FIG. 10 is a diagram illustrating an outline of a tilt angle derivation process. [Figure 6] FIG. 2 is a diagram illustrating the configuration of a six-axis B-mode processing unit in FIG. [Figure 7] 2 is a diagram showing mode transitions in the angular velocity derivation device of FIG. 1. FIG. [Figure 8] 4 is a flowchart showing a selection procedure in the angular velocity derivation device of FIG. [Figure 9] 4 is a flowchart showing a switching procedure in the angular velocity derivation device of FIG. [Figure 10] 4 is a flowchart showing a switching procedure in the angular velocity derivation device of FIG. [Figure 11] FIG. 10 is a diagram illustrating a configuration of a six-axis B-mode processor according to a second embodiment. [Figure 12] 12 is a flowchart showing a derivation procedure in the angular velocity derivation device of FIG. DETAILED DESCRIPTION OF THE INVENTION

[0011] Example 1 Before describing the present invention in detail, an overview will be provided. This embodiment relates to an angular velocity derivation device that is mounted on a vehicle or the like and derives an angular velocity using a three-axis gyro sensor. As described above, it is required to suppress deterioration in the accuracy of derivation of the angular velocity in the angular velocity derivation device. The angular velocity derivation device according to this embodiment can execute multiple modes for deriving the angular velocity and uses the angular velocity derived in the mode appropriate for the situation.

[0012] In the first mode (hereinafter referred to as "6-axis A mode"), the initial attitude is derived based on the output values ​​from the 3-axis acceleration sensor, and the attitude angle is derived by updating the initial attitude using the angular velocity output from the 3-axis gyro sensor. Here, there are three ways to express the attitude angle: Euler angles, direction cosine matrix, and quaternion. Euler angles are used for the initial attitude and attitude angle, and quaternions are used for the update process. Furthermore, the angular velocity is derived from the change in the attitude angle over time.

[0013] In the second mode (hereinafter referred to as "4-axis mode"), angular velocity is derived from the output value of one of the 3-axis gyro sensors and the output value of the 3-axis acceleration sensor. When the initial attitude calculated from the output value of the 3-axis acceleration sensor is updated using the output value of the 3-axis gyro sensor, as in the 6-axis A mode, errors in the offset and sensitivity of the 3-axis gyro sensors used in the calculation accumulate in the attitude angle over time. This deteriorates the accuracy of the attitude angle, and therefore the accuracy of the angular velocity calculated from that difference also deteriorates. On the other hand, in the 4-axis mode, this deterioration in accuracy does not occur, but accuracy deteriorates due to the influence of the tilt of the detection axis caused by changes in the vehicle's attitude angle.

[0014] In the third mode (hereinafter referred to as "6-axis B mode"), the attitude angle is updated by an extended Kalman filter using output values ​​from a 3-axis gyro sensor and output values ​​from a 3-axis acceleration sensor. The angular velocity is derived from the change in the attitude angle over time. The angular velocity derivation device according to this embodiment derives the angular velocity in each of three independent modes and selects one of the three angular velocities depending on the situation.

[0015] (1) Composition 1 shows the configuration of angular velocity derivation device 1000. Angular velocity derivation device 1000 includes a triaxial acceleration sensor 10, a triaxial gyro sensor 12, a velocity sensor 14, a six-axis A-mode processing unit 100, a four-axis mode processing unit 200, a six-axis B-mode processing unit 300, a control unit 400, and an output unit 500. Control unit 400 also includes an angular velocity evaluation value calculation unit 410. Angular velocity derivation device 1000 can be mounted on a vehicle (not shown).

[0016] The three-axis acceleration sensor 10 measures acceleration corresponding to each of the three axes. Here, FIG. 2 is used to explain the three axes. FIG. 2 shows a coordinate system according to the first embodiment. An orthogonal coordinate system consisting of an x-axis, a y-axis, and a z-axis is defined as the three axes. The x-axis points in the traveling direction of the vehicle 2000, the z-axis points in a normal direction downward from the floor surface of the vehicle 2000, and the y-axis points in a direction perpendicular to the x-axis and z-axis. The three-axis acceleration sensor 10 measures an acceleration a in the x-axis direction. x , acceleration in the y-axis direction a y , acceleration in the z-axis direction a z Returning to FIG. 1, the triaxial acceleration sensor 10 outputs these as output values ​​of the triaxial acceleration sensor 10 to the 6-axis A-mode processing unit 100, the 4-axis mode processing unit 200, the 6-axis B-mode processing unit 300, and the control unit 400.

[0017] The three-axis gyro sensor 12 is preferably installed so that the x-axis, y-axis, and z-axis of the three-axis acceleration sensor 10 are oriented in the same direction. In this case, the angular velocity vector ω=[pqr] T The three-axis gyro sensor 12 outputs the output values ​​to the six-axis A-mode processing unit 100, the four-axis mode processing unit 200, and the six-axis B-mode processing unit 300 in sequence.

[0018] The speed sensor 14 is installed midway along a speedometer cable that rotates in response to the rotation of the drive shaft, and outputs a speed pulse signal in response to the rotation of the drive shaft. The speed sensor 14 periodically detects the number of pulses by counting the speed pulse signal output as the vehicle moves at predetermined intervals. Since this number of pulses is proportional to the speed of the vehicle 2000, the speed sensor 14 can be said to measure the speed of the vehicle 2000. The speed sensor 14 outputs the measured speed to the 4-axis mode processing unit 200. Note that the speed of the vehicle 2000 may be obtained by a Global Navigation Satellite System (GNSS) instead of the speed sensor 14.

[0019] The 6-axis A mode processing unit 100 derives angular velocity by executing the above-mentioned 6-axis A mode processing. The 4-axis mode processing unit 200 derives angular velocity by executing the above-mentioned 4-axis mode processing. The 6-axis B mode processing unit 300 derives angular velocity by executing the above-mentioned 6-axis B mode processing. The 6-axis A mode processing, 4-axis mode processing, and 6-axis B mode processing will be described later.

[0020] The control unit 400 selects any one of the angular velocity derived in the 6-axis A mode processing unit 100, the angular velocity derived in the 4-axis mode processing unit 200, and the angular velocity derived in the 6-axis B mode processing unit 300. The processing by the control unit 400 will be described later. The control unit 400 instructs the output unit 500 of the selection result. In response to the instruction from the control unit 400, the output unit 500 selects and outputs any one of the angular velocity derived in the 6-axis A mode processing unit 100, the angular velocity derived in the 4-axis mode processing unit 200, and the angular velocity derived in the 6-axis B mode processing unit 300.

[0021] (1.1) Configuration of 6-axis A-mode processing unit 100 3 shows the configuration of the 6-axis A-mode processing unit 100. The 6-axis A-mode processing unit 100 includes an initial attitude derivation unit 110, a first conversion unit 112, an update unit 114, a second conversion unit 116, and a 6-axis A-mode angular velocity derivation unit 118.

[0022] The initial attitude derivation unit 110 receives the output value of the triaxial acceleration sensor 10. The initial attitude derivation unit 110 derives an initial attitude expressed in Euler angles based on the output value of the triaxial acceleration sensor 10. In the Euler angle expression, the rotation angle around the X axis in a fixed coordinate system in which the vertical direction is the Z axis is called the roll angle φ, the rotation angle around the Y axis is called the pitch angle θ, and the rotation angle around the Z axis is called the yaw angle ψ. The φ and θ of the initial attitude are expressed as follows:

number

[0023] The first conversion unit 112 converts the initial orientation expressed in Euler angles derived by the initial orientation derivation unit 110 into an initial orientation using a direction cosine matrix, and then converts the initial orientation using the direction cosine matrix into an initial orientation using a quaternion. The initial orientation expressed in Euler angles is converted as follows using the direction cosine matrix E:

number

number

number

[0024] The update unit 114 solves the differential equation of the attitude angle using the following quaternion.

number

[0025] Subsequently, the update unit 114 substitutes the attitude angles q1, q2, q3, and q4 based on the derived quaternions into the right-hand side of the differential equation, and also substitutes the components p, q, and r of the new angular velocity vector into the right-hand side of the differential equation. By solving the differential equation, the update unit 114 re-derives the attitude angles q1, q2, q3, and q4 based on the quaternions. That is, the update unit 114 updates the attitude angles q1, q2, q3, and q4 based on the quaternions by repeatedly solving the differential equation of the attitude angles based on the quaternions while sequentially substituting the output values ​​of the three-axis gyro sensor 12. The update unit 114 outputs the updated attitude angles q1, q2, q3, and q4 based on the quaternions to the second conversion unit 116.

[0026] The second conversion unit 116 converts the attitude angles q1, q2, q3, and q4 based on the quaternions updated by the update unit 114 into attitude angles based on the direction cosine matrix E, and then converts the attitude angles based on the direction cosine matrix E into attitude angles expressed in Euler angles. The attitude angles q1, q2, q3, and q4 based on the updated quaternions are converted into attitude angles based on the direction cosine matrix E as follows:

number

number

[0027] The six-axis A-mode angular velocity derivation unit 118 inputs the yaw angle ψ from among the attitude angles expressed in Euler angles converted by the second conversion unit 116. The six-axis A-mode angular velocity derivation unit 118 derives an angular velocity based on the time change t [sec] of the yaw angle ψ. For example, if the yaw angle at a time n of interest is ψ n and the yaw angle at time n-1 is ψ n-1 If we denote it as follows, the angular velocity is (ψ n -ψ n-1) / t. The six-axis A-mode angular velocity derivation unit 118 outputs the derived angular velocity to the output unit 500.

[0028] (1.2) Configuration of the 4-axis mode processing unit 200 4 shows the configuration of the four-axis mode processing unit 200. The four-axis mode processing unit 200 includes a pitch angle derivation unit 210 and a four-axis mode angular velocity derivation unit 218.

[0029] The pitch angle derivation unit 210 receives the output value of the triaxial acceleration sensor 10, as well as the output value of the speed sensor 14 and the speed acquired by the GNSS. The pitch angle derivation unit 210 derives the pitch angle θ of the acceleration sensor 10 based on these. When the triaxial acceleration sensor 10 is mounted horizontally to the vehicle, the pitch angle θ is derived as the inclination angle of the vehicle 2000 with respect to the road surface based on the output value of the triaxial acceleration sensor 10 and the output value of the speed sensor 14. Figure 5 shows an overview of the inclination angle derivation process. For the vehicle 2000 traveling on a slope, the x-axis, y-axis, and z-axis are defined as before, so the acceleration a in the x-axis direction is calculated. x , acceleration in the y-axis direction a y , acceleration in the x-axis direction a z is acquired by the three-axis acceleration sensor 10. The velocity of the vehicle 2000, which is the output value of the velocity sensor 14, indicates the direction of v. The pitch angle derivation unit 210 acquires acceleration a by differentiating the velocity v. This acceleration a also has the same direction as the velocity v. When the gravitational acceleration is represented as g, the relationship in FIG. 5 is expressed as follows:

number

[0030] In addition, when the three-axis acceleration sensor 10 is not mounted horizontally to the vehicle, the mounting angle of the three-axis acceleration sensor 10 to the vehicle is calculated by the acceleration a in the x-axis direction among the output values ​​of the three-axis acceleration sensor 10. xand the value obtained by differentiating the velocity of the vehicle 2000 acquired by the GNSS. The pitch angle θ is the sum of the inclination angle with respect to the road surface and the mounting angle of the triaxial acceleration sensor 10. The pitch angle derivation unit 210 outputs the pitch angle θ to the four-axis mode angular velocity derivation unit 218.

[0031] The four-axis mode angular velocity derivation unit 218 acquires the output value of one axis from the output values ​​of the three-axis gyro sensor 12. The four-axis mode angular velocity derivation unit 218 also receives the pitch angle θ from the pitch angle derivation unit 210. The four-axis mode angular velocity derivation unit 218 derives the angular velocity ω as follows.

number

[0032] (1.3) Configuration of the 6-axis B-mode processing unit 300 6 shows the configuration of the 6-axis B-mode processing unit 300. The 6-axis B-mode processing unit 300 includes an initial attitude derivation unit 310, a first conversion unit 312, an extended Kalman filter 314, a second conversion unit 316, and a 6-axis B-mode angular velocity derivation unit 318. The extended Kalman filter 314 includes a system update unit 320 and an observation update unit 322. The initial attitude derivation unit 310, the first conversion unit 312, and the second conversion unit 316 perform the same processing as the initial attitude derivation unit 110, the first conversion unit 112, and the second conversion unit 116, and therefore their description will be omitted here. The initial attitude derivation unit 310 may be integrated with the initial attitude derivation unit 110, the first conversion unit 312 may be integrated with the first conversion unit 112, and the second conversion unit 316 may be integrated with the second conversion unit 116.

[0033] The extended Kalman filter 314 receives the initial attitudes q1, q2, q3, and q4 based on the quaternions converted by the first conversion unit 312, the components p, q, and r of the angular velocity vector from the three-axis gyro sensor 12, and the output value (acceleration) from the three-axis acceleration sensor 10. Based on these, the extended Kalman filter 314 derives the attitude angle of the vehicle 2000. The processing in the extended Kalman filter 314 is divided into processing by a system update unit 320 and processing by an observation update unit 322.

[0034] As a preparation step, the system update unit 320 sets the initial attitudes q1, q2, q3, and q4 based on the quaternions converted by the first conversion unit 312 as state variables x for system update as follows.

number

number

[0035] In the following description, the a priori estimated variables, the a posteriori estimated variables, the a priori error covariance matrix, and the a posteriori error covariance matrix are represented as follows:

number

[0036] In an update stage (for example, update at step k), the system update unit 320 updates the prior estimation variables from f(x) and the posterior estimation variables at step k-1 Δt seconds ago as follows:

number

number

[0037] As a preparation step, the observation update unit 322 sets the output value (acceleration) from the three-axis acceleration sensor 10 as the observation value z for observation update.

number

number

[0038] In an update step (for example, update at step k), the observation update unit 322 calculates a Kalman gain K from the prior error covariance matrix, the Jacobian H of h(x), and the observation error covariance. k Ask for.

number

number

[0039] The observation update unit 322 calculates the prior error covariance matrix, the Jacobian H, and the Kalman gain K k The posterior error covariance matrix is ​​derived from

number

[0040] Like the second conversion unit 116, the second conversion unit 316 converts the attitude angle in the quaternion representation into an attitude angle using the direction cosine matrix E, and then converts the attitude angle using the direction cosine matrix E into an attitude angle using the Euler angle representation. The second conversion unit 316 outputs the yaw angle ψ, of the attitude angles using the Euler angle representation, to the six-axis B-mode angular velocity derivation unit 318.

[0041] Like the 6-axis A-mode angular velocity derivation unit 118, the 6-axis B-mode angular velocity derivation unit 318 receives the yaw angle ψ, which is one of the attitude angles expressed in Euler angles and converted by the second conversion unit 316. The 6-axis B-mode angular velocity derivation unit 318 derives an angular velocity based on the time change t [sec] of the yaw angle ψ. The 6-axis B-mode angular velocity derivation unit 318 outputs the derived angular velocity to the output unit 500.

[0042] This configuration can be realized in hardware terms by the CPU, memory, and other LSIs of any computer, and in software terms by programs loaded into memory, but here we depict functional blocks realized by the cooperation of these. Therefore, it will be understood by those skilled in the art that these functional blocks can be realized in various forms using only hardware, only software, or a combination of both.

[0043] (2) Mode switching control As described above, the control unit 400 selects one of the angular velocities derived by the 6-axis A mode processing unit 100, the 4-axis mode processing unit 200, and the 6-axis B mode processing unit 300. That is, the control unit 400 controls switching between the 6-axis A mode, the 4-axis mode, and the 6-axis B mode. FIG. 7 shows mode transitions in the angular velocity derivation device 1000. The control unit 400 primarily uses the 6-axis A mode 600, but transitions the 6-axis A mode 600 to the 6-axis B mode 604 when the accuracy of the attitude angle in the 6-axis A mode 600 decreases. On the other hand, when the accuracy of the attitude angle in the 6-axis A mode 600 improves while using the 6-axis B mode 604, the control unit 400 transitions the 6-axis B mode 604 to the 6-axis A mode 600.

[0044] The control unit 400 switches between the 6-axis B mode 604 and the 4-axis mode 602. Switching control between the 6-axis B mode 604 and the 4-axis mode 602 is called "first switching control," and the first switching control will be described later. Switching control from the 4-axis mode 602 to the 6-axis B mode 604 is called "second switching control," and the second switching control will be described later. To switch from the 4-axis mode 602 to the 6-axis B mode 604, a combination of the first switching control and the second switching control, or either the first switching control or the second switching control, is used. The control unit 400 also switches from the 4-axis mode 602 to the 6-axis A mode 600. Switching control from the 4-axis mode 602 to the 6-axis A mode 600 is called "third switching control," and the third switching control will be described later.

[0045] (2.1) First switching control The angular velocity evaluation value calculation unit 410 receives a pitch angle (hereinafter referred to as a "first pitch angle") from among the attitude angles derived by the extended Kalman filter 314 of the 6-axis B-mode processing unit 300. The angular velocity evaluation value calculation unit 410 also receives a pitch angle (hereinafter referred to as a "second pitch angle") derived by the pitch angle derivation unit 210 of the 4-axis mode processing unit 200. The second pitch angle corresponds to a pitch angle derived based on the output value of the 3-axis acceleration sensor 10 and the output value of the velocity sensor 14. The angular velocity evaluation value calculation unit 410 calculates the absolute value of the difference between the first pitch angle and the second pitch angle as an angular velocity evaluation value. The angular velocity evaluation value is an evaluation value of the likelihood of the angular velocity derived by the 6-axis B-mode angular velocity derivation unit 318, and a smaller value indicates greater accuracy.

[0046] The control unit 400 compares the angular velocity evaluation value calculated by the angular velocity evaluation value calculation unit 410 with a threshold value. The threshold value is set in advance. If the angular velocity evaluation value is equal to or greater than the threshold value, the control unit 400 selects the 4-axis mode 602. On the other hand, if the angular velocity evaluation value is smaller than the threshold value, the control unit 400 selects the 6-axis B mode 604. If the 6-axis A mode 600 is selected, no selection is made between the 4-axis mode 602 and the 6-axis B mode 604. The control unit 400 outputs the selection result to the output unit 500.

[0047] When the control unit 400 selects the 4-axis mode 602, the output unit 500 selects and outputs the angular velocity from the 4-axis mode processing unit 200. On the other hand, when the control unit 400 selects the 6-axis B mode 604, the output unit 500 selects and outputs the angular velocity from the 6-axis B mode processing unit 300.

[0048] 8 is a flowchart showing the selection procedure in the angular velocity derivation device 1000. The extended Kalman filter 314 derives a first pitch angle (S10). The second pitch angle is derived based on the output value of the three-axis acceleration sensor 10 and the output value of the velocity sensor 14 (S12). The angular velocity evaluation value calculation unit 410 calculates an angular velocity evaluation value based on the first pitch angle and the second pitch angle, and if the angular velocity evaluation value is equal to or greater than a threshold value (Y in S14), the control unit 400 selects the four-axis mode 602 (S16). On the other hand, if the angular velocity evaluation value is not equal to or greater than the threshold value (N in S14), the control unit 400 selects the six-axis B mode 604 (S18).

[0049] (2.2) Second switching control Here, it is assumed that the 4-axis mode 602 is selected by the control unit 400 and the output unit 500 is outputting the angular velocity from the 4-axis mode processing unit 200. The extended Kalman filter 314 of the 6-axis B mode processing unit 300 calculates the Kalman gain even when the output unit 500 is outputting the angular velocity derived by the 4-axis mode processing unit 200. The control unit 400 receives the Kalman gain from the 6-axis B mode processing unit 300. When the magnitude of the Kalman gain becomes smaller than a threshold value, the control unit 400 selects the 6-axis B mode 604 instead of the 4-axis mode 602. The control unit 400 outputs the selection result to the output unit 500. When the selection result from the control unit 400 changes from the 4-axis mode 602 to the 6-axis B mode 604, the output unit 500 switches from the angular velocity from the 4-axis mode processing unit 200 to the angular velocity from the 6-axis B mode processing unit 300 and outputs it.

[0050] 9 is a flowchart showing a switching procedure in angular velocity derivation device 1000. Control unit 400 determines to use 4-axis mode 602 (S50). If the magnitude of the Kalman gain is smaller than the threshold value (Y in S52), control unit 400 determines to use 6-axis B mode 604 (S54). On the other hand, if the magnitude of the Kalman gain is larger than the threshold value (N in S52), step 54 is skipped, and use of 4-axis mode 602 continues.

[0051] (2.3) Third switching control Here, it is assumed that the 4-axis mode 602 is selected in the control unit 400 and the output unit 500 is outputting the angular velocity from the 4-axis mode processing unit 200. The control unit 400 calculates the variance of the output values ​​of the 3-axis acceleration sensor 10 and the 3-axis gyro sensor 12. Any known technique may be used to calculate the variance. For example, the output values ​​(a x , a y , a z ), (p, q, r), respectively. When the variance values ​​of the output values ​​of the three-axis acceleration sensor 10 and the three-axis gyro sensor 12 become less than a threshold value, the control unit 400 selects the six-axis A mode 600 instead of the four-axis mode 602. The control unit 400 outputs the selection result to the output unit 500. When the selection result from the control unit 400 changes from the four-axis mode 602 to the six-axis A mode 600, the output unit 500 switches the angular velocity from the four-axis mode processing unit 200 to the angular velocity from the six-axis A mode processing unit 100 and outputs it.

[0052] 10 is a flowchart showing a switching procedure in the angular velocity derivation device 1000. The control unit 400 determines to use the 4-axis mode 602 (S100). If the variances of the output values ​​of the triaxial acceleration sensor 10 and the triaxial gyro sensor 12 are each less than the threshold value (Y in S102), the control unit 400 determines to use the 6-axis A mode 600 (S104). On the other hand, if the variances of the output values ​​of the triaxial acceleration sensor 10 and the triaxial gyro sensor 12 are not each less than the threshold value (N in S102), that is, if at least one of the variances of the output values ​​of the triaxial acceleration sensor 10 and the triaxial gyro sensor 12 is equal to or greater than the threshold value, step 104 is skipped and the use of the 4-axis mode 602 continues.

[0053] The third switching control is also used when the 6-axis B mode 604 is selected by the control unit 400. When the 6-axis B mode 604 is selected and the variance values ​​of the output values ​​of the 3-axis acceleration sensor 10 and the 3-axis gyro sensor 12 each become less than a threshold value, the control unit 400 selects the 6-axis A mode 600 instead of the 6-axis B mode 604.

[0054] According to this embodiment, the attitude angle is updated by an extended Kalman filter using an observation equation based on the output values ​​of a triaxial acceleration sensor, so that the attitude angle can be updated while correcting errors due to the triaxial gyro sensor even during movement. Furthermore, since the attitude angle is updated while correcting errors due to the triaxial gyro sensor even during movement, deterioration of the accuracy of angular velocity derivation can be suppressed when a triaxial gyro sensor is used. Furthermore, since an evaluation value of the likelihood of the angular velocity is calculated as the angular velocity evaluation value, the accuracy of angular velocity derivation can be grasped. Furthermore, since the accuracy of angular velocity derivation is grasped, deterioration of the accuracy of angular velocity derivation can be suppressed when a triaxial gyro sensor is used.

[0055] Furthermore, the difference between the pitch angle derived based on the output values ​​of the three-axis acceleration sensor and the velocity sensor and the pitch angle of the attitude angle derived in the extended Kalman filter is calculated as an angular velocity evaluation value, so the accuracy of the attitude angle derived in the extended Kalman filter can be evaluated. Furthermore, the angular velocity is derived based on the output value of one axis of the three-axis gyro sensor and the output value of the three-axis acceleration sensor, and the angular velocity is switched according to the angular velocity evaluation value, so the influence of errors can be reduced. Furthermore, the extended Kalman filter repeats system updates and observation updates, so the attitude angle can be derived with high accuracy.

[0056] Furthermore, when the angular velocity is derived based on the output value of a one-axis gyro sensor and the magnitude of the Kalman gain becomes smaller than a threshold value, the system switches to deriving the angular velocity using an extended Kalman filter, thereby preventing deterioration in the accuracy of the angular velocity derivation when a three-axis gyro sensor is used. Furthermore, when the angular velocity is derived based on the output value of a one-axis gyro sensor and the variance of the output values ​​of the three-axis acceleration sensor 10 and the three-axis gyro sensor becomes smaller than a threshold value, the system switches to deriving the angular velocity using quaternion-based attitude angle updates, thereby preventing deterioration in the accuracy of the angular velocity derivation when a three-axis gyro sensor is used.

[0057] Example 2 Next, a second embodiment will be described. Similar to the first embodiment, the second embodiment relates to an angular velocity derivation device that derives an angular velocity using a three-axis gyro sensor, particularly an angular velocity derivation device that derives an angular velocity in a six-axis B mode. Since attitude angle updating based only on the output values ​​of the three-axis gyro sensor does not allow correction until the moving object comes to a standstill, errors continue to accumulate over time. In contrast, the angular velocity derivation device according to the second embodiment updates the attitude angle using an extended Kalman filter in the six-axis B mode, and also performs correction on the updated attitude angle using a complementary filter that inputs an attitude angle obtained from the output values ​​of an acceleration sensor. This allows the attitude angle to be updated while correcting errors due to the output values ​​of the three-axis gyro sensor or integral calculations, even during movement.

[0058] On the other hand, the attitude angle updated by the extended Kalman filter or the attitude angle output from the complementary filter is affected by noise from the acceleration sensor or external forces other than gravity. In contrast, the angular velocity derivation device according to this embodiment sets a threshold value for the variance of the output values ​​of the triaxial acceleration sensor and the triaxial gyro sensor, and applies the complementary filter only when the variance falls within the threshold value. This enables correction only when there is little external force or noise on the acceleration sensor. The angular velocity derivation device 1000 according to the second embodiment is of the same type as that shown in FIG. 1, but may include only the 6-axis B-mode processing unit 300, without the 6-axis A-mode processing unit 100 and the 4-axis mode processing unit 200. The following description will focus on the differences from the first embodiment.

[0059] 11 shows the configuration of the 6-axis B-mode processing unit 300. The 6-axis B-mode processing unit 300 includes an initial attitude derivation unit 310, a first conversion unit 312, an extended Kalman filter 314, a second conversion unit 316, a 6-axis B-mode angular velocity derivation unit 318, and an attitude angle correction unit 330. The initial attitude derivation unit 310, the first conversion unit 312, the extended Kalman filter 314, and the second conversion unit 316 are the same as before.

[0060] The attitude angle correction unit 330 receives the attitude angle in Euler angle representation from the second conversion unit 316. The attitude angle in Euler angle representation corresponds to the attitude angle in quaternion representation derived in the extended Kalman filter 314. The attitude angle in Euler angle representation is expressed as follows:

number

number

[0061] The attitude angle correction unit 330 corrects the attitude angle derived in the extended Kalman filter 314 by calculating a weighted average of the attitude angle derived in the extended Kalman filter 314 and the attitude angle derived based on the output value of the three-axis acceleration sensor 10.

number

[0062] The control unit 400 receives the output values ​​of the triaxial acceleration sensor 10 and the triaxial gyro sensor 12. The control unit 400 calculates the variance of the output values ​​of the triaxial acceleration sensor 10 and the variance of the output values ​​of the triaxial gyro sensor 12. Any known technique may be used to calculate the variances. For example, the output values ​​(a x , a y , a z ), and is calculated for each output value (p, q, r).

[0063] If the variances of the output values ​​of the triaxial acceleration sensor 10 and the triaxial gyro sensor 12 are each less than the threshold value, the control unit 400 determines to use the attitude angle corrected by the attitude angle correction unit 330. On the other hand, if at least one of the variances of the output values ​​of the triaxial acceleration sensor 10 and the triaxial gyro sensor 12 is greater than the threshold value, the control unit 400 determines to use the attitude angle derived by the extended Kalman filter 314, i.e., the attitude angle output from the second conversion unit 316. The control unit 400 outputs the determination result to the 6-axis B-mode angular velocity derivation unit 318.

[0064] The 6-axis B-mode angular velocity derivation unit 318 receives the determination from the control unit 400. If the determination is to use the attitude angle corrected by the attitude angle correction unit 330, the 6-axis B-mode angular velocity derivation unit 318 derives an angular velocity based on the time change in the attitude angle output from the attitude angle correction unit 330. Since the derivation of the angular velocity is the same as before, a description thereof will be omitted here. If the determination is to use the attitude angle output from the second conversion unit 316, the 6-axis B-mode angular velocity derivation unit 318 derives an angular velocity based on the time change in the attitude angle output from the second conversion unit 316. The 6-axis B-mode angular velocity derivation unit 318 outputs the derived angular velocity to the output unit 500.

[0065] 12 is a flowchart showing a derivation procedure in the angular velocity derivation device 1000. The control unit 400 calculates the variance of the output values ​​of the triaxial acceleration sensor 10 and the variance of the output values ​​of the triaxial gyro sensor 12 (S150). If the variance of the output values ​​of the triaxial acceleration sensor 10 and the variance of the output values ​​of the triaxial gyro sensor 12 are each less than a threshold value (Y in S152), the 6-axis B-mode angular velocity derivation unit 318 derives an angular velocity based on the corrected attitude angle (S154). If the variance of the output values ​​of the triaxial acceleration sensor 10 and the variance of the output values ​​of the triaxial gyro sensor 12 are each not less than a threshold value (N in S152), that is, if at least one of the variance of the output values ​​of the triaxial acceleration sensor 10 and the variance of the output values ​​of the triaxial gyro sensor 12 is greater than a threshold value, the 6-axis B-mode angular velocity derivation unit 318 derives an angular velocity based on the attitude angle derived in the extended Kalman filter 314 (S156).

[0066] According to this embodiment, the angular velocity is derived based on the time change of the attitude angle derived in the extended Kalman filter or the time change of the corrected attitude angle, so that it is possible to suppress deterioration in the accuracy of the derivation of the angular velocity when a triaxial gyro sensor is used. Moreover, it is possible to switch whether or not to correct the attitude angle derived in the extended Kalman filter depending on the state of the triaxial acceleration sensor and the triaxial gyro sensor, so that it is possible to improve the accuracy of the derivation of the attitude angle of the triaxial acceleration sensor and the triaxial gyro sensor.

[0067] When the variances of the output values ​​of the triaxial acceleration sensor and the triaxial gyro sensor are each less than a threshold, the attitude angle correction unit derives the angular velocity based on the time change in the corrected attitude angle, allowing the attitude angle to be corrected when the accuracy of the attitude angle derived from the acceleration sensor output values ​​is high. Furthermore, when at least one of the variances of the output values ​​of the triaxial acceleration sensor and the triaxial gyro sensor is greater than a threshold, the angular velocity is derived based on the time change in the attitude angle derived in the extended Kalman filter, allowing the attitude angle to be avoided when the accuracy of the attitude angle derived from the acceleration sensor output values ​​is not high. Furthermore, a weighted average is taken of the attitude angle derived in the extended Kalman filter and the attitude angle derived based on the output values ​​of the triaxial acceleration sensor, improving the accuracy of the attitude angle derivation.

[0068] The present invention has been described above based on the embodiments. These embodiments are merely examples, and it will be understood by those skilled in the art that various modifications are possible in the combination of the components and treatment processes, and that such modifications are also within the scope of the present invention. [Explanation of symbols]

[0069] 10 3-axis acceleration sensor, 12 3-axis gyro sensor, 14 velocity sensor, 100 6-axis A mode processing unit, 110 initial attitude derivation unit, 112 first conversion unit, 114 update unit, 116 second conversion unit, 118 6-axis A mode angular velocity derivation unit, 200 4-axis mode processing unit, 210 pitch angle derivation unit, 218 4-axis mode angular velocity derivation unit, 300 6-axis B mode processing unit, 310 initial attitude derivation unit, 312 first conversion unit, 314 extended Kalman filter, 316 second conversion unit, 318 6-axis B mode angular velocity derivation unit, 318 6-axis B mode angular velocity derivation unit, 320 system update unit, 322 observation update unit, 330 attitude angle correction unit, 400 control unit, 410 Angular velocity evaluation value calculation unit, 500 output unit, 600 6-axis A mode, 602 4-axis mode, 604 6-axis B mode, 1000 angular velocity derivation device, 2000 vehicle.

Claims

1. An angular velocity derivation device that can be mounted on a moving body, an extended Kalman filter that derives an attitude angle of the moving body based on output values ​​of a three-axis acceleration sensor and an output value of a three-axis gyro sensor; an attitude angle correction unit that corrects the attitude angle derived in the extended Kalman filter; an angular velocity derivation unit that derives an angular velocity based on the time change of the attitude angle derived by the extended Kalman filter or the time change of the attitude angle corrected by the attitude angle correction unit, in accordance with a variance of the output values ​​of the triaxial acceleration sensor and a variance of the output values ​​of the triaxial gyro sensor; An angular velocity deriving device comprising:

2. the angular velocity derivation unit derives an angular velocity based on a time change in the attitude angle corrected by the attitude angle correction unit when a variance value of the output values ​​of the triaxial acceleration sensor and a variance value of the output values ​​of the triaxial gyro sensor are each less than a threshold value; 2. The angular velocity derivation device according to claim 1, wherein the angular velocity derivation unit derives the angular velocity based on the time change of the attitude angle derived in the extended Kalman filter when at least one of the variance of the output values ​​of the triaxial acceleration sensor and the variance of the output values ​​of the triaxial gyro sensor is greater than a threshold value.

3. 3. The angular velocity derivation device according to claim 1, wherein the attitude angle correction unit corrects the attitude angle derived by the extended Kalman filter by calculating a weighted average of the attitude angle derived by the extended Kalman filter and an attitude angle derived based on output values ​​of the three-axis acceleration sensor.

4. The extended Kalman filter is (A) a system update in which a quaternion obtained by converting the output values ​​of the three-axis acceleration sensor is used as a state variable, and a differential equation including the state variable and the output values ​​of the three-axis gyro sensor is used as a state equation; 2. The angular velocity deriving device according to claim 1, further comprising: (B) an observation update in which output values ​​of the three-axis acceleration sensors are used as observation values ​​and a gravity direction component of the observation values ​​is used as an observation equation.

5. An angular velocity deriving method for an angular velocity deriving device that can be mounted on a moving body, comprising: deriving an attitude angle of the moving body by an extended Kalman filter based on output values ​​of a three-axis acceleration sensor and an output value of a three-axis gyro sensor; correcting the attitude angle derived in the extended Kalman filter; deriving an angular velocity based on the time change of the attitude angle derived in the extended Kalman filter or the time change of the corrected attitude angle according to a variance of the output values ​​of the triaxial acceleration sensor and a variance of the output values ​​of the triaxial gyro sensor; An angular velocity derivation method comprising:

Images