JP7012001B2 - Vehicle posture estimation device - Google Patents

Description

The present invention relates to a vehicle posture estimation device that estimates the posture of a vehicle.

As the above-mentioned posture estimation device, one that estimates the posture of a two-wheeled vehicle by using a Kalman filter is known (see, for example, Patent Document 1). In the device described in Patent Document 1, a roll angular velocity sensor that detects a roll angular velocity in a vehicle, a yaw angular velocity sensor that detects a yaw angular velocity in a vehicle, a vertical acceleration sensor that detects a vertical acceleration in a vehicle, and a longitudinal acceleration sensor that detects a longitudinal acceleration in a vehicle are detected. Two-wheeled vehicles are equipped with a group of sensors such as a front-rear acceleration sensor, a left-right acceleration sensor that detects left-right acceleration in the vehicle, and a rear wheel speed sensor that detects rear wheel speed.

In the apparatus described in Patent Document 1, detection values relating to roll angular velocity, roll angular acceleration, yaw angular velocity, yaw angular acceleration, vertical acceleration, longitudinal acceleration, lateral acceleration, and rear wheel velocity are input to a calculation unit having a Kalman filter. ing. The calculation unit estimates and outputs the roll angle, vehicle speed in the front-rear direction, roll angle acceleration sensor offset, yaw angular speed sensor offset, left-right acceleration sensor offset, and vertical acceleration sensor offset using the Kalman filter, and estimates the posture of the two-wheeled vehicle. is doing.

Republished patent WO2017 / 175844

In the apparatus described in Patent Document 1, the roll angle, the vehicle speed in the front-rear direction, the roll angle acceleration sensor offset, the yaw angle speed sensor offset, the left-right acceleration sensor offset, the vertical acceleration sensor offset, and the like are estimated by using the Kalman filter. There is a possibility that an error may occur in the estimated value, and it is required to estimate the posture of the vehicle more accurately.

In view of this situation, a main object of the present invention is to provide a vehicle posture estimation device capable of accurately estimating the posture of a vehicle.

The first characteristic configuration of the present invention is a vehicle posture estimation device that estimates the posture of a four-wheeled vehicle in which the front wheels can be steered.
A positioning information detection unit that detects the positioning information of the vehicle using a satellite positioning system,
An angular velocity information detection unit that detects angular velocity information of the vehicle including roll angular velocity, pitch angular velocity, and yaw angular velocity in the vehicle.
An acceleration information detection unit that detects acceleration information of the vehicle including acceleration in the roll axis direction, acceleration in the pitch axis direction, and acceleration in the yaw axis direction in the vehicle.
Upon receiving the input of the positioning information detected by the positioning information detection unit, the angular velocity information detected by the angular velocity information detection unit, and the acceleration information detected by the acceleration information detection unit, the roll angle is used by using the Kalman filter. , A Kalman filter calculation unit for obtaining a state estimation value indicating the posture state of the vehicle including the pitch angle and the yaw angle, and
A steering angle detection unit that detects the steering angle in the vehicle, and
A determination unit that determines whether the vehicle is going straight or turning based on the steering angle detected by the steering angle detection unit.
When the determination unit determines that the vehicle is going straight, the straight-ahead calculation process obtains the current yaw angle of the vehicle, and when the determination unit determines that the vehicle is turning, the turning calculation process determines the current yaw of the vehicle. The current yaw angle calculation unit for finding the angle,
The yaw angle correction unit for correcting the yaw angle in the state estimation value obtained by the Kalman filter calculation unit by using the current yaw angle obtained by the current yaw angle calculation unit is provided.

According to this configuration, in addition to the Kalman filter calculation unit obtaining a state estimation value indicating the posture state of the vehicle including the roll angle, the pitch angle and the yaw angle, the yaw angle correction unit obtains the yaw angle in the Kalman filter calculation unit. Since the yaw angle in the state estimation value is corrected using the current yaw angle obtained by the current yaw angle calculation unit, the state estimation value can be obtained accurately while appropriately correcting the yaw angle. Moreover, when the current yaw angle calculation unit obtains the current yaw angle, the determination unit determines whether the vehicle is going straight or turning based on the steering angle of the vehicle, and even when the vehicle is going straight, it turns. Even when it is, the yaw angle is currently obtained by the arithmetic processing corresponding to the situation. Therefore, since the current yaw angle can be obtained with high accuracy, the accuracy of the state estimated value can be further improved by correcting the state estimated value using the accurate current yaw angle.

The second feature configuration of the present invention is that in the straight-ahead arithmetic processing, the current yaw angle arithmetic unit obtains the first yaw angle obtained from the positioning information detected by the positioning information detection unit, and the angular velocity information detection unit. The current yaw angle is obtained by using the second calculated yaw angle obtained from the angular velocity information detected in.
In the turning calculation process, the current yaw angle calculation unit obtains the current yaw angle from the angular velocity information detected by the angular velocity information detection unit.

According to this configuration, when the current yaw angle is obtained, if the vehicle is traveling straight, the current yaw angle calculation unit obtains the first calculated yaw angle obtained from the positioning information detected by the positioning information detection unit, and the angular velocity information. Since the current yaw angle is obtained by using the second calculated yaw angle obtained from the angular velocity information detected by the detection unit, the current yaw angle when traveling straight can be accurately obtained. Further, when the vehicle is turning, the current yaw angle calculation unit obtains the current yaw angle from the angular velocity information detected by the angular velocity information detection unit, so that the current yaw angle when turning can be accurately obtained. can. As a result, the yaw angle can be accurately obtained at present regardless of whether the vehicle is traveling straight or turning, and the accuracy of the state estimation value can be improved.

The third characteristic configuration of the present invention is the initial yaw angle from the positioning information detected by the positioning information detection unit when it is determined that the vehicle is going straight based on the steering angle detected by the steering angle detection unit. Is equipped with an initial yaw angle calculation unit that obtains
The Kalman filter calculation unit performs an initial operation for obtaining a state estimated value using the initial yaw angle obtained by the initial yaw angle calculation unit, and then performs a normal operation for obtaining a state estimated value using the previously obtained state estimated value. The point is to repeat.

According to this configuration, the initial yaw angle calculation unit obtains the initial yaw angle when the vehicle is traveling straight, and the Kalman filter calculation unit obtains the state estimation value using the initial yaw angle obtained by the initial yaw angle calculation unit. Since the initial calculation is performed, the state estimation value can be accurately obtained from the beginning when the calculation is performed by the Kalman filter calculation unit. Further, since the Kalman filter calculation unit obtains the state estimation value by using the state estimation value obtained last time in the normal operation, accurate state estimation is performed while reflecting the state estimation value obtained last time in the normal calculation as well. You can find the value.

Side view of rice transplanter Block diagram showing the schematic configuration of the posture estimation device Flow chart showing the operation in the calculation unit of the posture estimation device Flow chart showing the current operation in the yaw angle calculation unit

An embodiment of the posture estimation device according to the present invention will be described with reference to the drawings.
As shown in FIG. 1, this posture estimation device is provided in the rice transplanter 1 as a vehicle, but the vehicle provided with the posture estimation device may be a four-wheeled vehicle in which the front wheels can be steered, for example. Not only work vehicles such as tractors and combines, but also various other vehicles can be applied.

The rice transplanter 1 is provided with a planting portion 4 at the rear portion of the traveling machine body 2 via an elevating link mechanism 3. The traveling machine body 2 is provided with a vehicle body frame 5 extending in the front-rear direction, and various devices are supported by the vehicle body frame 5. The vehicle body frame 5 is provided with a bonnet 7 that supports an engine 6 as a drive source on the front side thereof and covers the engine 6.

A front axle case 8 is supported on the front portion of the vehicle body frame 5, and front wheels 9 are attached to both left and right sides of the front axle case 8. A rear axle case 10 is supported at the rear portion of the vehicle body frame 5, and rear wheels 11 are attached to both left and right sides of the rear axle case 10.

An arch-shaped mounting frame 12 is provided above the bonnet 7. Spare seedling stands 13 are provided on both the left and right sides of the mounting frame 12, and a sensor unit 14 is provided in the center of the upper part of the mounting frame 12. The sensor unit 14 is positioned to measure position information (positioning information) such as the current position of the rice planting machine 1 by using a GNSS (Global Navigation Satellite System) or the like, which is an example of a satellite positioning system (NSS: Navigation Satellite System). Sensor 15 (corresponding to the positioning information detection unit), angular velocity sensor 16 (corresponding to the angular velocity information detection unit) for measuring angular velocity information regarding roll angular velocity, pitch angular velocity and yaw angular velocity in the rice planting machine 1, roll axis direction in the rice planting machine 1. An acceleration sensor 17 (corresponding to an acceleration information detection unit) for measuring acceleration information regarding acceleration in the pitch axis direction, acceleration in the yaw axis direction, and acceleration in the yaw axis direction is provided.

The angular velocity sensor 16 and the acceleration sensor 17 are provided as an inertial measurement unit (IMU) in which the angular velocity sensor 16 and the acceleration sensor 17 are unitized. By the way, the front-rear direction of the rice planting machine 1 (vehicle) is the roll axis direction, the front side of the rice planting machine 1 is the plus side of the roll axis direction, and the left-right direction of the rice planting machine 1 (vehicle) is the pitch axis direction. The left side is the plus side in the pitch axis direction, the vertical direction of the rice planting machine 1 (vehicle) is the yaw axis direction, and the upper side of the rice planting machine 1 is the plus side in the yaw axis direction.

A steering wheel 18 capable of steering the front wheels 9 is provided at the rear of the bonnet 7, and a driver's seat 19 is provided at the rear of the steering wheel 18. The traveling machine 2 is provided with various sensors for detecting the state of the rice transplanter 1. As sensors, for example, as shown in FIG. 2, a steering angle sensor 20 (corresponding to a steering angle detection unit) for detecting the steering angle of the rice planting machine 1 with respect to the sensor steering angle or the turning angle of the front wheel 9 and a main shift operation lever are used. A lever position sensor 21 or the like for detecting which position of forward, neutral, and reverse is being operated is provided.

As shown in FIG. 2, the posture estimation device receives inputs such as detection values of various sensors, and receives a posture state estimation value indicating a posture state of the rice transplanter 1 including a roll angle, a pitch angle, and a yaw angle, and a rice transplanter. A calculation unit 100 for obtaining a state estimated value including a position state estimated value indicating the position of 1 is provided. The calculation unit 100 uses, for example, the detection value of the positioning sensor 15, the detection value of the angular velocity sensor 16, the detection value of the acceleration sensor 17, the detection value of the steering angle sensor 20, and the detection value of the lever position sensor 21 as the detection values of the sensor. It is an input.

The calculation unit 100 includes a coordinate conversion unit 101, a state transition determination unit 103, an initial yaw angle calculation unit 104, a current yaw angle calculation unit 106, a determination unit 107, a Kalman filter calculation unit 109, a yaw angle correction unit 110, and the like. ..

The coordinate conversion unit 101 converts the coordinate system in the positioning information of the rice transplanter 1 detected by the positioning sensor 15, the detection value of the acceleration sensor 17, and the detection value of the angular velocity sensor 16. Information obtained by converting the latitude and longitude of the rice transplanter 1 detected by the positioning sensor 15 into the NED coordinate system is input to the coordinate conversion unit 101. The coordinate conversion unit 101 converts the position of the rice transplanter 1 in the NED coordinate system based on the detection value of the positioning sensor 15 to the position in the ISO coordinate system. In this conversion, for example, the north is the X-axis, the west is the Y-axis, the sky is the Z-axis, and the signs of the Y-axis and the Z-axis are inverted. Further, since the detection value of the angular velocity sensor 16 and the detection value of the acceleration sensor 17 are the values of the NED coordinate system, the coordinate conversion unit 101 converts the value of the NED coordinate system into the ground coordinate system and outputs it. ..

The state transition determination unit 103 determines whether or not the rice transplanter 1 is running based on the positioning information of the rice transplanter 1 detected by the positioning sensor 15. Information obtained by converting the latitude and longitude of the rice transplanter 1 detected by the positioning sensor 15 into the NED coordinate system is input to the state transition determination unit 103. The state transition determination unit 103 obtains the velocity vectors Vx and Vy by differentiating the N-axis value and the E-axis value obtained by converting the latitude and longitude of the rice transplanter 1 detected by the positioning sensor 15 into the NED coordinate system. There is. The state transition determination unit 103 uses the obtained velocity vectors Vx and Vy to obtain the absolute value Vnorm of the velocity vector on the XY plane in the NED coordinate system by the following (Equation 1).

(Equation 1)
Vnorm = √ (Vx ² + Vy ² )

The state transition determination unit 103 compares the absolute value of the obtained speed vector with the threshold value, and when the absolute value Vnorm of the obtained speed vector is smaller than the threshold value, determines that the rice planting machine 1 is stopped and obtains it. When the absolute value Vnorm of the speed vector is equal to or greater than the threshold value, it is determined that the rice planting machine 1 is traveling.

The initial yaw angle calculation unit 104 obtains the initial yaw angle of the rice transplanter 1. In the initial yaw angle calculation unit 104, in addition to the information obtained by converting the latitude and longitude of the rice planting machine 1 detected by the positioning sensor 15 into the NED coordinate system, the detection value of the steering angle sensor 20 and the lever position sensor 21 The detection value has been entered. The initial yaw angle calculation unit 104 determines whether or not the rice transplanter 1 is facing straight in the traveling direction based on the detection value of the steering angle sensor 20 and the detection value of the lever position sensor 21, and the rice transplanter 1 determines. When facing straight in the direction of travel, the yaw angle at that time is calculated as the initial yaw angle.

When the initial yaw angle calculation unit 104 determines that the rice transplanter 1 is facing straight in the traveling direction, the value of the N axis obtained by converting the latitude and longitude of the rice transplanter 1 detected by the positioning sensor 15 into the NED coordinate system. And the value of the E axis are differentiated to obtain the velocity vectors Vx and Vy. The initial yaw angle calculation unit 104 obtains the initial yaw angle θ by the following (Equation 2) using the obtained velocity vectors Vx and Vy.

(Equation 2)
θ = tan ^-1 (Vy / Vx)

The initial yaw angle calculation unit 104 uses the obtained initial yaw angle θ to obtain the initial posture state value of the posture indicating the state facing the initial yaw angle θ. The initial yaw angle calculation unit 104 sets an arbitrary initial position state value indicating the initial position of the rice transplanter 1 in addition to the initial posture state value, and sets an initial state value including the initial posture state value and the initial position state value. Is being output.

Currently, the yaw angle calculation unit 106 is seeking the current yaw angle of the rice transplanter 1. Currently, the yaw angle calculation unit 106 has position information by the positioning sensor 15 whose coordinates have been converted by the coordinate conversion unit 101, initial state values from the initial yaw angle calculation unit 104, and an angular velocity sensor whose coordinates have been converted by the coordinate conversion unit 101. The angular velocity information according to 16, the determination result of the state transition determination unit 103, the detection value of the steering angle sensor 20, the detection value of the lever position sensor 21, and the like are input.

The current yaw angle calculation unit 106 first obtains the initial yaw angle in the initial state value from the initial yaw angle calculation unit 104 as the current yaw angle. After that, the current yaw angle calculation unit 106 obtains the current yaw angle obtained last time as the current yaw angle as it is, unless the Kalman filter calculation unit 109 is executing the calculation process.

When the Kalman filter calculation unit 109 is executing the calculation process and the rice transplanter 1 is stopped, the current yaw angle calculation unit 106 obtains the current yaw angle obtained last time as the current yaw angle this time. There is. When the rice transplanter 1 is running when the Kalman filter calculation unit 109 is executing the calculation process, the determination unit 107 is based on the detection value of the steering angle sensor 20 and the detection value of the lever position sensor 21. , It is determined whether the rice transplanter 1 is going straight or turning. The current yaw angle calculation unit 106 obtains the current yaw angle of the rice transplanter 1 by the straight-ahead calculation process when the determination unit 107 determines that the rice transplanter is going straight, and when the determination unit 107 determines that the rice transplanter is turning, the yaw angle calculation unit 106 is for turning. The current yaw angle of the rice transplanter 1 is obtained by arithmetic processing.

In the straight-ahead calculation process, the yaw angle calculation unit 106 currently obtains the first calculation yaw angle θ1 obtained from the positioning information detected by the positioning sensor 15 and the second calculation yaw obtained from the angular velocity information detected by the angular velocity sensor 16. The yaw angle is currently obtained using the angle θ2. Further, in the turning calculation process, the current yaw angle calculation unit 106 obtains the current yaw angle from the angular velocity information detected by the angular velocity sensor 16.

The Kalman filter calculation unit 109 is based on various input information, and the position state estimated value indicating the position of the rice transplanter 1 (for example, the position of the rice transplanter 1 (positioning sensor 15), the speed of the rice transplanter 1 (positioning sensor 15), Obtain a state estimation value including the acceleration of the rice transplanter 1 (acceleration sensor 17), the angular velocity of the three axes of the rice transplanter 1, the angular velocity of the three axes of the rice transplanter 1) and the attitude state estimation value indicating the posture of the rice transplanter 1. ing. The Kalman filter calculation unit 109 is provided with position information by the positioning sensor 15 coordinate-converted by the coordinate conversion unit 101, initial state values from the initial yaw angle calculation unit 104, and an angular velocity sensor 16 coordinate-converted by the coordinate conversion unit 101. The angular velocity information, the acceleration information from the acceleration sensor 17, the state estimation value previously obtained by the Kalman filter calculation unit 109, and the like are input.

The Kalman filter calculation unit 109 obtains a state estimation value using the following (1) to (6) as state variables. Further, the Kalman filter calculation unit 109 contains a three-axis position vector (specified value) from the center of the rear wheel axle to the positioning sensor 15, and a three-axis position vector (specified) from the center of the rear wheel axle to the angular velocity sensor 16 and the acceleration sensor 17. Value) has been entered.

(1) Position of the positioning sensor 15 as seen from the ground coordinate system (2) Speed of the positioning sensor 15 as seen from the ground coordinate system (3) Acceleration of the angular velocity sensor 16 and the acceleration sensor 17 as seen from the ground coordinate system (4) Ground Attitude of rice planting machine 1 seen from the coordinate system (5) Angular velocity of rice planting machine 1 seen from the aircraft coordinate system (6) Angle acceleration of rice planting machine 1 seen from the aircraft coordinate system

By the way, the horizontal coordinate system is a coordinate system in which a certain point on the ground is set as the origin, and the north at that time is the X axis, the west is the Y axis, and the sky is the Z axis. The aircraft coordinate system is a coordinate system with the center of the rear wheel axle of the rice transplanter 1 as the origin, the front side of the rice transplanter 1 as the X axis, the left side of the rice transplanter 1 as the Y axis, and the upper side of the rice transplanter 1 as the Z axis. be.

The yaw angle correction unit 110 corrects the yaw angle included in the state estimation value obtained by the Kalman filter calculation unit 109 by using the current yaw angle obtained by the current yaw angle calculation unit 106. The current attitude information from the current yaw angle calculation unit 106, the state estimation value obtained by the Kalman filter calculation unit 109, and the like are input to the yaw angle correction unit 110. The yaw angle correction unit 110 sets the difference between the current yaw angle obtained by the current yaw angle calculation unit 106 and the yaw angle included in the state estimation value obtained by the Kalman filter calculation unit 109 as the true yaw angle (after correction). Yaw angle). The yaw angle correction unit 110 outputs a yaw angle correction state estimation value obtained by correcting the yaw angle included in the state estimation value obtained by the Kalman filter calculation unit 109 using the obtained true yaw angle.

Hereinafter, the operation of the calculation unit 100 of the posture estimation device will be described with reference to the block diagram of FIG. 2 and the flowchart of FIG.
First, the calculation unit 100 determines whether or not it is necessary to perform the initial yaw angle calculation, and when it is necessary to perform the initial yaw angle calculation, the calculation unit 100 calculates the initial yaw angle (step # 1). In the case of Yes, step # 2). When the initial yaw angle calculation unit 104 calculates the initial yaw angle, the current yaw angle calculation unit 106 is currently calculating the yaw angle (step # 3). The operation when the current yaw angle calculation unit 106 calculates the current yaw angle will be described later.

Further, for example, when the initial yaw angle calculation is not necessary, for example, when the initial yaw angle calculation unit 104 has already calculated the initial yaw angle, the current yaw angle is not calculated without performing the initial yaw angle calculation. The calculation unit 106 is currently calculating the yaw angle (in the case of No in step # 1, step # 3).

Upon receiving input of various information, the Kalman filter calculation unit 109 calculates a state estimated value including a position state estimated value and a posture state estimated value by using the Kalman filter (step # 4). By the way, the Kalman filter calculation unit 109 performs calculation processing when various information is input, and the Kalman filter calculation unit 109 currently performs calculation processing regardless of whether or not the yaw angle calculation unit 106 is currently calculating the yaw angle. Operation processing is performed.

When the Kalman filter calculation unit 109 obtains the state estimation value, the yaw angle correction unit 110 includes the current yaw angle obtained by the current yaw angle calculation unit 106 in the state estimation value obtained by the Kalman filter calculation unit 109. The yaw angle is corrected (step # 5).

As described above, the operation of the calculation unit 100 of the posture estimation device has been described. However, the corrected state estimation value output by the calculation unit 100 of the posture estimation device can be used, for example, to grasp the current posture of the rice transplanter 1. It is also used to control the posture of the rice transplanter 1 to maintain it in a desired posture.

The calculation unit 100 repeats the operation shown in FIG. 3 in a predetermined cycle (for example, a cycle in which the position information of the rice transplanter 1 is detected by the positioning sensor 15). As a result, the posture of the rice transplanter 1 can be repeatedly estimated at a predetermined cycle. When the calculation unit 100 repeatedly performs the calculation process, the Kalman filter calculation unit 109 first receives the input of the initial yaw angle obtained by the initial yaw angle calculation unit 104, so that the initial yaw angle calculation unit 104 obtains the initial yaw angle. The initial operation to obtain the state estimate is performed using the yaw angle. After that, since the Kalman filter calculation unit 109 receives the input of the state estimation value obtained last time, the normal calculation for obtaining the state estimation value is repeated using the state estimation value obtained last time.

Based on the flowchart shown in FIG. 4, the operation when the current yaw angle calculation unit 106 calculates the current yaw angle will be described.
First, the yaw angle calculation unit 106 currently determines whether or not the Kalman filter calculation unit 109 is executing the calculation process based on the input of the determination result of the state transition determination unit 103 (step # 11). .. When the calculation process is not being executed by the Kalman filter calculation unit 109, the current yaw angle calculation unit 106 obtains the current yaw angle obtained last time as the current yaw angle of this time and holds the previous value (step # 11). In the case of No, step # 12).

When the calculation process is being executed by the Kalman filter calculation unit 109, whether or not the rice transplanter 1 is currently running based on the input of the determination result of the state transition determination unit 103 by the yaw angle calculation unit 106. Is determined (in the case of Yes in step # 11, step # 13). When the rice transplanter 1 is not running, the current yaw angle calculation unit 106 obtains the current yaw angle obtained last time as the current yaw angle of this time and holds the previous value (in the case of No in step # 13, the previous value is held). Step # 12).

When the rice transplanter 1 is running, the determination unit 107 determines whether the rice transplanter 1 is going straight or turning based on the detection value of the steering angle sensor 20 and the detection value of the lever position sensor 21. Yes (in the case of Yes in step # 11, step # 14). When the rice transplanter 1 is turning, the current yaw angle calculation unit 106 obtains the current yaw angle by performing a turning calculation process for obtaining the current yaw angle from the angular velocity information detected by the angular velocity sensor 16 (step). In the case of a turn of # 14, step # 15).

When the rice planting machine 1 is traveling straight, the yaw angle calculation unit 106 currently obtains the first yaw angle θ1 obtained from the positioning information detected by the positioning sensor 15 and the angular velocity information obtained from the angular velocity sensor 16. The current yaw angle is obtained by performing a straight-ahead arithmetic process for obtaining the current yaw angle using the two-calculation yaw angle θ2 (in the case of straight-ahead step # 14, step # 16).

In this way, if the yaw angle calculation unit 106 is currently executing the calculation process by the Kalman filter calculation unit 109, whether the rice transplanter 1 is running, or if the rice transplanter 1 is running. The current yaw angle is being calculated while changing the calculation method according to each situation of going straight or turning, and it is possible to obtain an accurate current yaw angle suitable for each situation. Therefore, the yaw angle correction unit 110 corrects the yaw angle in the state estimation value obtained by the Kalman filter calculation unit 109 by using the current yaw angle obtained by the current yaw angle calculation unit 106, so that an accurate state estimation value is obtained. Can be output.

[Another Embodiment]
Other embodiments of the present invention will be described.
It should be noted that the configurations of the respective embodiments described below are not limited to being applied independently, but can also be applied in combination with the configurations of other embodiments.

(1) In the above embodiment, further correction can be added to the state estimation value obtained by the Kalman filter calculation unit 109. For example, corrections can be made to improve the transient response associated with changes in attitude angle.

(2) In the above embodiment, the yaw angle calculation unit 106 currently has the first calculated yaw angle obtained from the positioning information detected by the positioning sensor 15 and the angular velocity information detected by the angular velocity sensor 16 in the straight-ahead calculation process. Although the yaw angle is currently obtained by using the second arithmetic yaw angle obtained from
Further, the yaw angle calculation unit currently obtains the current yaw angle from the angular velocity information detected by the angular velocity sensor 16 in the turning calculation process, but the turning calculation process is not limited to this calculation process and other calculations. Processing can also be used.

1 Rice transplanter (vehicle)
15 Positioning sensor (positioning information detection unit)
16 Angular velocity sensor (angular velocity information detection unit)
17 Accelerometer (accelerometer information detector)
20 Steering angle sensor (steering angle detection unit)
104 Initial yaw angle calculation unit 106 Current yaw angle calculation unit 107 Judgment unit 109 Kalman filter calculation unit 110 Yaw angle correction unit

Claims (3)

In a vehicle attitude estimation device that estimates the attitude of a four-wheeled vehicle whose front wheels can be steered freely
A positioning information detection unit that detects the positioning information of the vehicle using a satellite positioning system,
An angular velocity information detection unit that detects angular velocity information of the vehicle including roll angular velocity, pitch angular velocity, and yaw angular velocity in the vehicle.
An acceleration information detection unit that detects acceleration information of the vehicle including acceleration in the roll axis direction, acceleration in the pitch axis direction, and acceleration in the yaw axis direction in the vehicle.
Upon receiving the input of the positioning information detected by the positioning information detection unit, the angular velocity information detected by the angular velocity information detection unit, and the acceleration information detected by the acceleration information detection unit, the roll angle is used by using the Kalman filter. , A Kalman filter calculation unit for obtaining a state estimation value indicating the posture state of the vehicle including the pitch angle and the yaw angle, and
A steering angle detection unit that detects the steering angle in the vehicle, and
A determination unit that determines whether the vehicle is going straight or turning based on the steering angle detected by the steering angle detection unit.
When the determination unit determines that the vehicle is going straight, the straight-ahead calculation process obtains the current yaw angle of the vehicle, and when the determination unit determines that the vehicle is turning, the turning calculation process determines the current yaw of the vehicle. The current yaw angle calculation unit for finding the angle,
A vehicle attitude estimation device including a yaw angle correction unit that corrects the yaw angle in the state estimation value obtained by the Kalman filter calculation unit using the current yaw angle obtained by the current yaw angle calculation unit. In the straight-ahead calculation process, the current yaw angle calculation unit obtains the first yaw angle obtained from the positioning information detected by the positioning information detection unit and the angular velocity information detected by the angular velocity information detection unit. Find the current yaw angle using the second calculated yaw angle.
The vehicle attitude estimation device according to claim 1, wherein in the turning calculation process, the current yaw angle calculation unit obtains the current yaw angle from the angular velocity information detected by the angular velocity information detection unit. The initial yaw angle calculation unit that obtains the initial yaw angle from the positioning information detected by the positioning information detection unit when it is determined that the vehicle is going straight based on the steering angle detected by the steering angle detection unit is provided. Be,
The Kalman filter calculation unit performs an initial operation for obtaining a state estimated value using the initial yaw angle obtained by the initial yaw angle calculation unit, and then performs a normal operation for obtaining a state estimated value using the previously obtained state estimated value. The vehicle attitude estimation device according to claim 1 or 2, wherein the above is repeatedly performed.

Images