Pitch angular velocity correction value calculation device, attitude angle calculation device, and method for calculating pitch angular velocity correction value
A pitch angular velocity correction value calculation device includes a pitch angle calculation unit and a correction value calculation unit. The pitch angle calculation unit calculates a pitch angle among attitude angles of a vehicle on the basis of a pitch angular velocity that is an angular velocity of the vehicle detected by a triaxial gyro sensor, and a correction value of the pitch angular velocity.
1. A pitch angular velocity correction value calculation device comprising:
a speed sensor that detects a speed of an object;
a triaxial acceleration sensor that detects an acceleration of the object;
a triaxial gyro sensor that detects at least a pitch angular velocity of the object;
a storage device that stores computer executable instructions and an offset correction value of the pitch angular velocity; and
a processor that executes the computer executable instructions to perform operations, comprising:
calculating a tilt angle α of the object based on the detected speed and the detected acceleration with an equation
α
=
sin
-
1
GA
G
=
sin
-
1
A
2
-
A
1
G
(
1
)
wherein G is gravitational acceleration, A1 is a differential value of the speed in a direction of an inclined road surface R detected by the speed sensor and A2 is the acceleration in the direction of the inclined road surface R detected by the triaxial acceleration sensor;
storing the calculated tilt angle of the object in the storage device;
determining a pitch angle θ of the object based on the pitch angular velocity and the offset correction value of the pitch angular velocity with an equation
q 1 =n 1 sin θ/2
q 2 =n 2 sin θ/2
q 3 =n 3 sin θ/2
q 4 =cos θ/2
q 1 2 +q 2 2 +q 3 2 +q 4 2 =|q 2 |+q 4 2 =1 (3)
ω 1 =2( {dot over (q)} 1 q 4 +{dot over (q)} 2 q 3 −{dot over (q)} 3 q 2 −{dot over (q)} 4 q 1 )
ω 2 =2( {dot over (q)} 2 q 4 +{dot over (q)} 3 q 1 −{dot over (q)} 1 q 3 −{dot over (q)} 4 q 2 )
ω 3 =2( {dot over (q)} 3 q 4 +{dot over (q)} 1 q 3 −{dot over (q)} 2 q 1 −{dot over (q)} 4 q 3 ) (9)
ω
=
2
[
q
4
q
3
-
q
2
-
q
1
-
q
3
q
4
q
1
-
q
2
q
2
-
q
1
q
4
-
q
3
]
[
q
.
1
q
.
2
q
.
3
q
.
4
]
(
10
)
q
∘
=
1
2
(
q
4
ω
+
q
×
ω
)
q
.
4
=
-
1
2
ω
q
d
dt
[
q
1
q
2
q
3
q
4
]
=
1
2
[
0
ω
3
-
ω
2
ω
1
-
ω
3
0
ω
1
ω
2
ω
2
-
ω
1
0
ω
3
-
ω
1
-
ω
2
-
ω
3
0
]
[
q
1
q
2
q
3
q
4
]
(
12
)
wherein q1, q2, q3, and q4 are quaternions, n 1 , n 2 , and n 3 are unit vectors, ω is an pitch angular velocity, and {dot over (q)} is a time differential of q in an object coordinate system;
modifying the offset correction value of the pitch angular velocity ω based on the current pitch angle θ calculated using the current correction value of the pitch angular velocity, the current tilt angle α calculated, the previous pitch angle θ calculated in the past and stored in the storage device using the current correction value of the pitch angular velocity, and the previous tilt angle α calculated at a time of calculating the previous correction value of the pitch angular velocity ω such that with respect to a difference between the calculated pitch angle θ and the calculated corresponding tilt angle α, a correction value of the pitch angular velocity ω is modified so that an amount of a difference value between the current difference and the past difference becomes zero; and
correcting the pitch angle based on the detected pitch angular velocity and the modified offset correction value of the pitch angular velocity and to output the corrected pitch angle.