The invention relates to a sideslip angle estimator suitable for straight-line navigation of agricultural machinery. The automatic navigation system of agricultural machinery comprises a vehicle front wheel angle sensor (1) and a GNSS positioning and orientation device (2), and is characterized in that the sideslip angle estimator comprises a comprehensive error calculator (8), a first estimator (9), a second estimator (10) and a third estimator (11).
- the front wheel angle sensor (1) is used for collecting front wheel steering angle information, and the front wheel steering angle information is processed and transmitted to the input end of the second estimator (10). The GNSS positioning and orientation device (2) is used for collecting forward speed information, antenna positioning information and current attitude information of agricultural machinery, and the collected forward speed information is filtered and transmitted to the input end of the second estimator (10). After analyzing and calculating the collected antenna positioning information and current vehicle attitude information, the position deviation measurement value y(j) and heading deviation measurement value {tilde over (θ)}(j) between the navigation point coordinate information and the path planning line are obtained and transmitted to the input end of the comprehensive error calculator (8);
- the output end of the comprehensive error calculator (8) is respectively connected with the input ends of the first estimator (9), the second estimator (10) and the third estimator (11). The output end of the first estimator (9) is respectively connected with the input ends of the second estimator (10) and the third estimator (11). The output end of the second estimator (10) is respectively connected with the input ends of the comprehensive error calculator (8) and the third estimator (11). The output end of the third estimator (11) is connected with the input end of the comprehensive error calculator (8);
- the comprehensive error calculator (8) is used for analyzing and obtaining a comprehensive error signal ε(j) at time j, namely:
- ε(j)=ky(y(j)−ŷ(j))+kθ({tilde over (θ)}(j)−{circumflex over ({tilde over (θ)})}(j)) (3)
- among them, (j) indicates the heading deviation estimation value. ŷ(j) indicates position deviation estimation value, ky and kθ are coefficients, satisfying kθ+ky<1 and kθ<ky. The estimated value of position deviation is obtained according to the third estimator (11), and the estimated value of heading deviation is obtained according to the second estimator (2), and its initial values are all 0;
- the first estimator estimates (9) the sideslip angle estimated value {circumflex over (β)}(j) at time j, i.e.:
- {circumflex over (β)}(j)={circumflex over (β)}(j−1)+k1ε(j)Ts (4)
- among them, k1 is the coefficient, and Ts represents the system control period;
- the second estimator (10) is used for estimating the heading deviation at time j to obtain an the heading deviation estimated value, namely:
- θ
- ~
- ^
-
- (
- j
- )
- =
- θ
- ~
- ^
-
- (
- j
- -
- 1
- )
- +
- T
- s
-
- {
- v
-
- (
- j
- )
- L
-
-
- (
- j
- )
-
- [
- tan
-
- (
- δ
-
- (
- j
- )
- +
- β
- ^
-
- (
- j
- )
- )
- -
-
- (
- j
- )
- ]
- +
- k
- 2
-
- ɛ
-
- (
- j
- )
- }
- (
- 5
- )
- among them, ν(j) is the current speed of the vehicle, L is the length of the vehicle body, δ(j) is the current front wheel steering angle, and k2 is the coefficient;
- the third estimator (11) estimates the position deviation of the heading to obtain an estimated value of the position deviation, namely:
- ŷ(j)=ŷ(j−1)+Ts[ν(j)sin({circumflex over ({tilde over (θ)})}(j)+{circumflex over (β)}(j))+k3ε(j)] (6)
- among it, k3 is the coefficient.