Angular speed derivation device and angular speed derivation method

Abstract

An angular speed derivation device 1000 can be installed in a mobile object. The angular speed derivation device 1000 includes an extended Kalman filter, a first angular speed derivation unit, and an angular speed evaluation value calculation unit 410. The extended Kalman filter derives an attitude angle of the mobile object on the basis of an output value of a triaxial acceleration sensor 10 and an output value of a triaxial gyro sensor 12. The first angular speed derivation unit derives an angular speed on the basis of a time-dependent change in the attitude angle derived in the extended Kalman filter. The angular speed evaluation value calculation unit 410 calculates, as an angular speed evaluation value, an evaluation value of certainty of the angular speed derived by the first angular speed derivation unit.

Description

Angular velocity derivation device and angular velocity derivation method

[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.

[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 while 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).

[0003] JP 2019-74377 A JP 2019-74378 A

[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.

[0006] In order to solve the above problem, an angular velocity derivation device according to one aspect of the present embodiment 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 three-axis acceleration sensor and an output value of a three-axis gyro sensor; a first angular velocity derivation unit that derives an angular velocity based on a change over time in the attitude angle derived in the extended Kalman filter; and an angular velocity evaluation value calculation unit that calculates an evaluation value of the likelihood of the angular velocity derived in the first angular velocity derivation unit as an angular velocity evaluation value.

[0007] Another aspect of the present embodiment is an angular velocity derivation device that can be mounted on a moving object and includes: an extended Kalman filter that derives an attitude angle of the moving object based on output values ​​of a triaxial acceleration sensor and output values ​​of a triaxial gyro sensor; a first angular velocity derivation unit that derives an angular velocity based on a time change in the attitude angle derived by the extended Kalman filter; a second angular velocity derivation unit that derives an angular velocity based on an output value of one of the output values ​​of the triaxial gyro sensor and an output value of the triaxial acceleration sensor; and an output unit that outputs one of the angular velocities derived by the first angular velocity derivation unit and the second angular velocity derivation unit. The extended Kalman filter calculates a Kalman gain even when the output unit is outputting the angular velocity derived by the second angular velocity derivation unit, and when the magnitude of the Kalman gain becomes smaller than a threshold, the output unit stops outputting the angular velocity derived by the second angular velocity derivation unit and switches to outputting the angular velocity derived by the first angular velocity derivation unit.

[0008] Yet another aspect of this embodiment is an angular velocity derivation device. This device is an angular velocity derivation device that can be mounted on a moving body, and includes: a first conversion unit that converts an initial attitude expressed in Euler angles, derived based on output values ​​of a three-axis acceleration sensor, into an initial attitude using quaternions; an update unit that updates the attitude using quaternions by repeatedly solving a differential equation for the attitude using quaternions while sequentially substituting output values ​​of the three-axis gyro sensor, using the initial attitude converted using quaternions in the first conversion unit as an initial value; a second conversion unit that converts the attitude using quaternions updated by the update unit into an attitude expressed in Euler angles; a first angular velocity derivation unit that derives an angular velocity based on a change over time in the attitude expressed in Euler angles converted by the second conversion unit; a second angular velocity derivation unit that derives an angular velocity based on an output value of one axis of the output values ​​of the three-axis gyro sensor and an output value of the three-axis acceleration sensor; and an output unit that outputs the angular velocity derived in the first angular velocity derivation unit and one of the angular velocities derived in the second angular velocity derivation unit. When the output unit is outputting the angular velocity derived in the second angular velocity derivation unit, if the variance value of the output values ​​of the three-axis gyro sensor becomes less than a threshold value, the output unit stops outputting the angular velocity derived in the second angular velocity derivation unit and switches to outputting the angular velocity derived in the first angular velocity derivation unit.

[0009] Yet another aspect of this embodiment is also 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 by the extended Kalman filter; and an angular velocity derivation unit that derives an angular velocity based on a time change in the attitude angle derived by the extended Kalman filter or a time change in 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.

[0010] Yet another aspect of the present embodiment is an angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, the method including 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 an output value of a triaxial gyro sensor; deriving an angular velocity based on a time change in the attitude angle derived by the extended Kalman filter; and calculating an evaluation value of the likelihood of the derived angular velocity as an angular velocity evaluation value.

[0011] Another aspect of the present embodiment is also an angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, the method including 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, deriving a first angular velocity based on a time change in the attitude angle derived by the extended Kalman filter, deriving a second angular velocity based on an output value of one of the output values ​​of the triaxial gyro sensor and an output value of the triaxial acceleration sensor, outputting one of the first angular velocity and the second angular velocity, calculating a Kalman gain by the extended Kalman filter even when the second angular velocity is being output, and stopping output of the second angular velocity and switching to outputting the first angular velocity when the magnitude of the Kalman gain becomes smaller than a threshold value.

[0012] Yet another aspect of this embodiment is also a method for deriving an angular velocity. This method is an angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, and includes the steps of converting an initial attitude expressed in Euler angles, derived based on output values ​​of a three-axis acceleration sensor, into an initial attitude using quaternions; updating the attitude using quaternions by repeatedly solving a differential equation for the attitude using quaternions while sequentially substituting output values ​​of the three-axis gyro sensor, using the initial attitude using the converted quaternions as an initial value; converting the updated attitude using quaternions into an attitude using Euler angles; deriving a first angular velocity based on a change over time in the attitude using Euler angles; deriving a second angular velocity based on an output value of one of the output values ​​of the three-axis gyro sensor and an output value of the three-axis acceleration sensor; outputting one of the first angular velocity and the second angular velocity; and, when the second angular velocity is being output, stopping output of the second angular velocity and switching to outputting the first angular velocity when the variance of the output values ​​of the three-axis gyro sensor becomes less than a threshold value.

[0013] Yet another aspect of this embodiment is also an angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, the method including 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.

[0014] Any combination of the above components, and conversion of the expression of this embodiment into a method, device, system, recording medium, computer program, etc. are also valid aspects of this embodiment.

[0015] According to this embodiment, when a three-axis gyro sensor is used, it is possible to suppress deterioration in the accuracy of deriving the angular velocity.

[0016] 11 is a diagram showing the configuration of an angular velocity derivation device according to Example 1. FIG. 12 is a diagram showing a coordinate system according to Example 1. FIG. 13 is a diagram showing the configuration of a 6-axis A-mode processing unit of FIG. 1. FIG. 14 is a diagram showing the configuration of a 4-axis mode processing unit of FIG. 1. FIG. 15 is a diagram showing an overview of tilt angle derivation processing. FIG. 16 is a diagram showing the configuration of a 6-axis B-mode processing unit of FIG. 1. FIG. 17 is a diagram showing mode transitions in the angular velocity derivation device of FIG. 1. A flowchart showing a selection procedure in the angular velocity derivation device of FIG. 1. A flowchart showing a switching procedure in the angular velocity derivation device of FIG. 1. A flowchart showing a switching procedure in the angular velocity derivation device of FIG. 1. FIG. 18 is a diagram showing the configuration of a 6-axis B-mode processing unit according to Example 2. FIG. 19 is a flowchart showing a derivation procedure in the angular velocity derivation device of FIG.

[0017] (First Embodiment) Before describing this embodiment 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.

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

[0019] In the second mode (hereinafter referred to as "four-axis mode"), the angular velocity is derived from the output value of one of the three-axis gyro sensors and the output value of the three-axis acceleration sensor. When the initial attitude calculated from the output value of the three-axis acceleration sensor is updated using the output value of the three-axis gyro sensor, as in the six-axis A mode, errors in the offset and sensitivity of the three-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 the difference also deteriorates. On the other hand, in the four-axis mode, such 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.

[0020] 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 three-axis gyro sensor and output values ​​from a three-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.

[0021] (1) Configuration Fig. 1 shows the configuration of angular velocity derivation device 1000. Angular velocity derivation device 1000 includes a three-axis acceleration sensor 10, a three-axis 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).

[0022] 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. ｘ , acceleration in the y-axis direction a ｙ , acceleration in the z-axis direction a ｚ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.

[0023] 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 ω=[p q r] Ｔ The three-axis gyro sensor 12 sequentially 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. The components p, q, and r are shown in FIG.

[0024] 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 also 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 four-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.

[0025] The 6-axis A-mode processing unit 100 derives angular velocity by performing the above-described 6-axis A-mode processing. The 4-axis mode processing unit 200 derives angular velocity by performing the above-described 4-axis mode processing. The 6-axis B-mode processing unit 300 derives angular velocity by performing the above-described 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.

[0026] 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.

[0027] (1.1) Configuration of the 6-axis A-mode processing unit 100 Fig. 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.

[0028] 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: The initial value of ψ may be any value, and is set to "0", for example.

[0029] The first conversion unit 112 converts the initial attitude expressed in Euler angles derived by the initial attitude derivation unit 110 into an initial attitude using a direction cosine matrix, and then converts the initial attitude using the direction cosine matrix into an initial attitude using a quaternion. The initial attitude expressed in Euler angles is converted as follows using the direction cosine matrix E: Furthermore, each element of the direction cosine matrix E is expressed as follows: A quaternion is defined by four components: a unit vector in the rotation direction and a rotation angle. The initial orientation by the direction cosine matrix E is the initial orientation q by the quaternion. １ , q ２ , q ３ , q ４ is converted to the following: The first conversion unit 112 converts the initial attitude q １ , q ２ , q ３ , q ４ to the update unit 114.

[0030] The update unit 114 solves the differential equation of the attitude angle using the following quaternion. In the initial stage, the update unit 114 updates the initial posture q １ , q ２ , q ３ , q ４ is substituted into the right side of the differential equation as an initial value, and the components p, q, and r of the angular velocity vector at that timing are also substituted into the right side of the differential equation. By solving the differential equation, the update unit 114 calculates the attitude angle q １ , q ２ , q ３ , q ４ is derived.

[0031] Following this, the update unit 114 updates the attitude angle q １ , q ２ , q ３ , q ４ is substituted into the right side of the differential equation, and the components p, q, and r of the new angular velocity vector are also substituted into the right side of the differential equation. By solving the differential equation, the update unit 114 obtains the attitude angle q １ , q ２ , q ３ , q ４ That is, the update unit 114 repeatedly solves the differential equation of the attitude angle based on the quaternion while sequentially substituting the output values ​​of the three-axis gyro sensor 12, thereby deriving the attitude angle q １ , q ２ , q ３ , q ４ The update unit 114 updates the attitude angle q １ , q ２ , q ３ , q ４ to the second conversion unit 116.

[0032] The second conversion unit 116 converts the attitude angle q １ , q ２ , q ３ , q ４ is converted into an attitude angle using the direction cosine matrix E, and then the attitude angle using the direction cosine matrix E is converted into an attitude angle expressed in Euler angles. １ , q ２ , q ３ , q ４ is converted to an attitude angle using a direction cosine matrix E as follows: The direction cosine matrix E is converted to the attitude angles in Euler angle representation as follows: The second conversion unit 116 outputs the yaw angle ψ, which is one of the attitude angles expressed in Euler angles, to the six-axis A-mode angular velocity derivation unit 118 .

[0033] 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 the 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 ψ ｎ and the yaw angle at time n-1 is ψ ｎ－１ Then, the angular velocity is (ψ ｎ -ψ ｎ－１ ) / t. The six-axis A-mode angular velocity derivation unit 118 outputs the derived angular velocity to the output unit 500.

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

[0035] The pitch angle derivation unit 210 receives the output value of the three-axis acceleration sensor 10, as well as the output value of the velocity sensor 14 and the velocity acquired by the GNSS. The pitch angle derivation unit 210 derives the pitch angle θ of the acceleration sensor 10 based on these. When the three-axis acceleration sensor 10 is mounted horizontally to the vehicle, the inclination angle of the vehicle 2000 with respect to the road surface is derived as the pitch angle θ based on the output value of the three-axis acceleration sensor 10 and the output value of the velocity sensor 14. Figure 5 shows an overview of the inclination angle derivation process. When the vehicle 2000 is 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. ｘ , acceleration in the y-axis direction a ｙ , acceleration in the x-axis direction a ｚ 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 denoted as g, the relationship in FIG. 5 is expressed as follows: By solving this simultaneous equation, the inclination θ of the slope in FIG. 5 is derived, and the inclination θ of the slope corresponds to the pitch angle θ.

[0036] 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 subtracting the acceleration a in the x-axis direction from the output values ​​of the three-axis acceleration sensor 10. ｘ and the value obtained by differentiating the velocity of the vehicle 2000 acquired by the GNSS. The pitch angle θ corresponds to the sum of the inclination angle with respect to the road surface and the mounting angle of the three-axis acceleration sensor 10. The pitch angle derivation unit 210 outputs the pitch angle θ to the four-axis mode angular velocity derivation unit 218.

[0037] 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. Here, Vout is the output value of one axis, Voffset is the offset value of the three-axis gyro sensor 12, and S (mV / (deg / sec)) is the sensitivity coefficient of the three-axis gyro sensor 12. The four-axis mode angular velocity derivation unit 218 outputs the angular velocity ω to the output unit 500.

[0038] (1.3) Configuration of the 6-axis B-mode Processing Unit 300 FIG. 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.

[0039] The extended Kalman filter 314 converts the initial attitude q １ , q ２ , q ３ , q ４ , each component p, q, 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. The extended Kalman filter 314 derives the attitude angle of the vehicle 2000 based on these. 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.

[0040] As a preparation step, the system update unit 320 updates the initial attitude q １ , q ２ , q ３ , q ４ Let x be the state variable of the system update as follows: The system update unit 320 defines a differential equation including the state variable x, the angular velocity vector (p, q, r) from the three-axis gyro sensor 12, and the white noise w (wp, wq, wr) as the state equation. Furthermore, the system update unit 320 replaces the right-hand term of the state equation with f(x) + Gw.

[0041] 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:

[0042] In the 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: The system update unit 320 calculates the Jacobian F of f(x), the posterior error covariance matrix of step k-1 Δt seconds ago, and the system input G ｋ and the system error covariance Q, the a priori error covariance matrix is ​​calculated.

[0043] 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. The observation update unit 322 calculates the measurement error δa (δa ｘ , δa ｙ , δa ｚ ) and the gravity component act on the observed value z, and an observation equation is prepared. The observation update unit 322 also replaces the right term of the observation equation with h(x)+v.

[0044] In the update stage (for example, update at step k), the observation update unit 322 calculates the Kalman gain K from the prior error covariance matrix, the Jacobian H of h(x), and the observation error covariance. ｋ Ask for. The observation update unit 322 calculates the prior estimated variables and the Kalman gain K ｋ and update the posterior estimated variables from the observed values ​​z and h.

[0045] The observation update unit 322 calculates the prior error covariance matrix, the Jacobian H, and the Kalman gain K ｋThe posterior error covariance matrix is ​​derived from The posterior covariance matrix in the observation update unit 322 is the attitude angle in quaternion representation. The above system update and observation update are repeated for each step.

[0046] 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.

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

[0048] 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.

[0049] (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 basically uses the 6-axis A mode 600, but when the accuracy of the attitude angle in the 6-axis A mode 600 decreases, the control unit 400 transitions from the 6-axis A mode 600 to the 6-axis B mode 604. 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 from the 6-axis B mode 604 to the 6-axis A mode 600.

[0050] 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.

[0051] (2.1) The first switching control angular velocity evaluation value calculation unit 410 receives a pitch angle (hereinafter referred to as the "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 the "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.

[0052] 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.

[0053] 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.

[0054] 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. 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).

[0055] (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 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.

[0056] 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.

[0057] (2.3) Third Switching Control Here, it is assumed that the four-axis mode 602 is selected in the control unit 400 and the output unit 500 is outputting the angular velocity from the four-axis mode processing unit 200. The control unit 400 calculates the variance value of the output values ​​of the three-axis acceleration sensor 10 and the three-axis gyro sensor 12. A known technique may be used to calculate the variance value. For example, ｘ , a ｙ , a ｚ ), (p, q, r). When the variance values ​​of the output values ​​of the three-axis acceleration sensor 10 and the three-axis gyro sensor 12 each 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.

[0058] 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 a 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 less than a 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 a threshold value, step 104 is skipped, and the use of the 4-axis mode 602 continues.

[0059] 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.

[0060] According to this embodiment, the attitude angle is updated by an extended Kalman filter using an observation equation based on the output values ​​of the 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 using a triaxial gyro sensor. 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 using a triaxial gyro sensor.

[0061] 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 by the extended Kalman filter is calculated as an angular velocity evaluation value, so the accuracy of the attitude angle derived by 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 repeatedly performs system updates and observation updates, so the attitude angle can be derived with high accuracy.

[0062] Furthermore, when the angular velocity is derived based on the output value of a one-axis gyro sensor, if 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 deriving the angular velocity 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, if 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 deriving the angular velocity when a three-axis gyro sensor is used.

[0063] Second Embodiment 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 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 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.

[0064] 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 three-axis acceleration sensor and the three-axis 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 six-axis B-mode processing unit 300, without including the six-axis A-mode processing unit 100 and the four-axis mode processing unit 200. The following description will focus on the differences from the first embodiment.

[0065] 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 those described above.

[0066] 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: Furthermore, the attitude angle correction unit 330 receives the output value of the triaxial acceleration sensor 10. The attitude angle correction unit 330 derives an attitude angle expressed in Euler angles based on the output value of the triaxial acceleration sensor 10. The attitude angle expressed in Euler angles is derived as in the initial attitude derivation unit 110, using equation (1). The attitude angle expressed in Euler angles is expressed as follows:

[0067] The attitude angle correction unit 330 corrects the attitude angle derived in the extended Kalman filter 314 by taking 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. The attitude angle correction unit 330 outputs the corrected attitude angle to the six-axis B-mode angular velocity derivation unit 318 .

[0068] The control unit 400 receives the output values ​​of the three-axis acceleration sensor 10 and the three-axis gyro sensor 12. The control unit 400 calculates the variance of the output values ​​of the three-axis acceleration sensor 10 and the three-axis gyro sensor 12. A known technique may be used to calculate the variances. For example, the output values ​​(a ｘ , a ｙ , a ｚ ), is calculated for each of the output values ​​(p, q, r).

[0069] When 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, when 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 six-axis B-mode angular velocity derivation unit 318.

[0070] The six-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 six-axis B-mode angular velocity derivation unit 318 derives an angular velocity based on the change over time 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 six-axis B-mode angular velocity derivation unit 318 derives an angular velocity based on the change over time in the attitude angle output from the second conversion unit 316. The six-axis B-mode angular velocity derivation unit 318 outputs the derived angular velocity to the output unit 500.

[0071] 12 is a flowchart showing the 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 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 not less than a threshold value (N in S152), i.e., 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 by the extended Kalman filter 314 (S156).

[0072] 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.

[0073] 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 by 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 by 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.

[0074] 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.

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

[0076] 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, comprising: 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; a first angular velocity derivation unit that derives an angular velocity based on the change over time in the attitude angle derived in the extended Kalman filter; and an angular velocity evaluation value calculation unit that calculates an evaluation value of the likelihood of the angular velocity derived in the first angular velocity derivation unit as an angular velocity evaluation value.

2. The angular velocity derivation device according to claim 1, further comprising a pitch angle derivation unit that derives a pitch angle of the moving body based at least on the output values ​​of the three-axis acceleration sensor and the velocity sensor, wherein the angular velocity evaluation value calculation unit calculates the difference between the pitch angle derived in the pitch angle derivation unit and the pitch angle of the attitude angle derived in the extended Kalman filter as the angular velocity evaluation value.

3. The angular velocity derivation device according to claim 1 or 2, further comprising: a second angular velocity derivation unit that derives an angular velocity based on an output value of one axis of the output values ​​of the three-axis gyro sensor and an output value of the three-axis acceleration sensor; and an output unit that outputs one of the angular velocity derived in the first angular velocity derivation unit and the angular velocity derived in the second angular velocity derivation unit based on the angular velocity evaluation value calculated in the angular velocity evaluation value calculation unit.

4. An angular velocity derivation device that can be mounted on a moving body, comprising: an extended Kalman filter that derives an attitude angle of the moving body based on output values ​​of a three-axis acceleration sensor and output values ​​of a three-axis gyro sensor; a first angular velocity derivation unit that derives an angular velocity based on time changes in the attitude angle derived in the extended Kalman filter; a second angular velocity derivation unit that derives an angular velocity based on an output value of one of the output values ​​of the three-axis gyro sensor and the output value of the three-axis acceleration sensor; and an output unit that outputs one of the angular velocity derived in the first angular velocity derivation unit and the angular velocity derived in the second angular velocity derivation unit, wherein the extended Kalman filter calculates a Kalman gain even when the output unit is outputting the angular velocity derived in the second angular velocity derivation unit, and when the magnitude of the Kalman gain becomes smaller than a threshold value, the output unit stops outputting the angular velocity derived in the second angular velocity derivation unit and switches to outputting the angular velocity derived in the first angular velocity derivation unit.

5. An angular velocity derivation device that can be mounted on a moving body, comprising: a first conversion unit that converts an initial attitude expressed in Euler angles derived based on output values ​​of a three-axis acceleration sensor into an initial attitude using quaternions; an update unit that updates the attitude using quaternions by repeatedly solving a differential equation for the attitude using quaternions while sequentially substituting output values ​​of a three-axis gyro sensor, with the initial attitude using quaternions converted by the first conversion unit as an initial value; a second conversion unit that converts the attitude using quaternions updated by the update unit into an attitude using Euler angles; a first angular velocity derivation unit that derives an angular velocity based on a time change in the attitude using Euler angles converted by the second conversion unit; a second angular velocity derivation unit that derives an angular velocity based on an output value of one axis of the output values ​​of the three-axis gyro sensor and an output value of the three-axis acceleration sensor; and an output unit that outputs the angular velocity derived by the first angular velocity derivation unit and one of the angular velocities derived by the second angular velocity derivation unit, When the output unit is outputting the angular velocity derived in the second angular velocity derivation unit, if a variance value of the output values ​​of the three-axis gyro sensor becomes less than a threshold value, the output unit stops outputting the angular velocity derived in the second angular velocity derivation unit and switches to outputting the angular velocity derived in the first angular velocity derivation unit.

6. An angular velocity derivation device that can be mounted on a moving body, comprising: 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.

7. The angular velocity derivation device according to claim 6, wherein the angular velocity derivation unit derives the angular velocity based on the time change in the attitude angle corrected in the attitude angle correction unit when the variance of the output values ​​of the triaxial acceleration sensor and the variance of the output values ​​of the triaxial gyro sensor are each less than a threshold value, and the angular velocity derivation unit derives the angular velocity based on the time change in 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.

8. An angular velocity derivation device as described in claim 6 or 7, wherein the attitude angle correction unit corrects the attitude angle derived in the extended Kalman filter by taking a weighted average of the attitude angle derived in the extended Kalman filter and an attitude angle derived based on the output values ​​of the three-axis acceleration sensor.

9. An angular velocity derivation device according to any one of claims 1 to 4 and 6 to 8, wherein the extended Kalman filter includes: (A) a system update in which quaternions converted from the output values ​​of the three-axis acceleration sensors are used as state variables, and a differential equation including the state variables and the output values ​​of the three-axis gyro sensors is used as a state equation; and (B) an observation update in which the output values ​​of the three-axis acceleration sensors are used as observation values, and the gravity direction component of the observation values ​​is used as an observation equation.

10. An angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, comprising: a step of deriving an attitude angle of the moving body using an extended Kalman filter based on output values ​​of a three-axis acceleration sensor and an output value of a three-axis gyro sensor; a step of deriving an angular velocity based on the time change in the attitude angle derived by the extended Kalman filter; and a step of calculating an evaluation value of the likelihood of the derived angular velocity as an angular velocity evaluation value.

11. An angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, comprising: a step of deriving an attitude angle of the moving body by an extended Kalman filter based on output values ​​of a three-axis acceleration sensor and output values ​​of a three-axis gyro sensor; a step of deriving a first angular velocity based on time changes in the attitude angle derived by the extended Kalman filter; a step of deriving a second angular velocity based on an output value of one of the output values ​​of the three-axis gyro sensor and the output value of the three-axis acceleration sensor; a step of outputting one of the first angular velocity and the second angular velocity; a step of the extended Kalman filter calculating a Kalman gain even when the second angular velocity is being output; and a step of stopping output of the second angular velocity and switching to outputting the first angular velocity when the magnitude of the Kalman gain becomes smaller than a threshold value.

12. An angular velocity derivation method for an angular velocity derivation device mountable on a moving body, comprising: a step of converting an initial attitude expressed in Euler angles derived based on output values ​​of a three-axis acceleration sensor into an initial attitude using quaternions; a step of updating the quaternion attitude by repeatedly solving a differential equation for the quaternion attitude while sequentially substituting output values ​​of a three-axis gyro sensor with the converted quaternion initial attitude as an initial value; a step of converting the updated quaternion attitude into an attitude expressed in Euler angles; a step of deriving a first angular velocity based on a time change in the converted attitude expressed in Euler angles; a step of deriving a second angular velocity based on an output value of one of the output values ​​of the three-axis gyro sensor and an output value of the three-axis acceleration sensor; a step of outputting one of the first angular velocity and the second angular velocity; and a step of stopping output of the second angular velocity and switching to outputting the first angular velocity when the variance of the output values ​​of the three-axis gyro sensor becomes less than a threshold value while the second angular velocity is being output. An angular velocity derivation method comprising:

13. An angular velocity derivation method for an angular velocity derivation device that can be mounted on a moving body, comprising: a step of deriving an attitude angle of the moving body using an extended Kalman filter based on output values ​​of a triaxial acceleration sensor and output values ​​of a triaxial gyro sensor; a step of correcting the attitude angle derived by the extended Kalman filter; and a step of deriving an angular velocity based on the time change of the attitude angle derived by the extended Kalman filter or the time change of the corrected attitude angle, depending on the variance of the output values ​​of the triaxial acceleration sensor and the variance of the output values ​​of the triaxial gyro sensor.

Images