# US 2021/0240192 A1 · Estimation method and estimator for sideslip angle of straight-line navigation of agricultural machinery

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2021/0240192 A1
- Document ID: US20210240192A1
- Status: Published application
- Kind: A1
- Application number: 17/143,449
- Filed: 2021-01-07
- Published: 2021-08-05
- Earliest claimed: 2020-01-17
- Assignees: Qingdao Agriculture University
- Applicants: Qingdao Agriculture University
- Inventors: Jian Zhang; Ranbing Yang
- Web page: https://patentgrep.com/patent/US20210240192A1

## CPC

- A01B 69/008: {automatic}
- G05D 1/0223: {involving speed control of the vehicle (vehicle fittings for automatically controlling, i.e. preventing speed from exceeding an arbitrarily established velocity or maintaining speed at a particular velocity, as selected by the vehicle operator B60K31/00)}
- B62D 15/021: {Determination of steering angle}
- G01S 19/52: Determining velocity
- G05D 1/0088: {characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours (using knowledge based models G06N5/00)}
- G05D 1/0212: {with means for defining a desired trajectory (involving a plurality of land vehicles G05D1/0287)}
- G05D 1/0278: {using satellite positioning signals, e.g. GPS}
- G05D 1/0891: {specially adapted for land vehicles}
- G01S 19/14: specially adapted for specific applications
- G01S 19/48: by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- A01B: SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL (making or covering furrows or holes for sowing, planting or manuring A01C5/00; mowers convertible to soil working apparatus or capable of soil working A01D42/04; mowers combined with soil working implements A01D43/12; soil working for engineering purposes E01, E02, E21)
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- B62D: MOTOR VEHICLES; TRAILERS (steering, or guiding on a desired track, of agricultural machines or implements A01B69/00; wheels, castors, axles, increasing wheel adhesion B60B; vehicle tyres, tyre inflation or tyre changing B60C; connections between vehicles of a train or the like B60D; vehicles for use on rail and road, amphibious or convertible vehicles B60F; suspension arrangements B60G; heating, cooling, ventilating or other air treating devices B60H; windows, windscreens, non-fixed roofs, doors or similar devices, protective coverings for vehicles not in use B60J; propulsion plant arrangements, auxiliary drives, transmissions, controls, instrumentation or dashboards B60K; electric equipment or propulsion of electrically-propelled vehicles B60L; power supply for electrically-propelled vehicles B60M; passenger accommodation not otherwise provided for B60N; adaptations for load transportation or to carry special loads or objects B60P; arrangement of signalling or lighting devices, the mounting or supporting thereof or circuits therefor, for vehicles in general B60Q; vehicles, vehicle fittings or vehicle parts not otherwise provided for B60R; servicing, cleaning, repairing, supporting, lifting, or manoeuvring, not otherwise provided for, B60S; brake arrangements, brake control systems or parts thereof B60T; air-cushion vehicles B60V; motor-cycles, accessories therefor B62J, B62K; testing of vehicles G01M)
- G01S: RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES

## Abstract

The invention discloses an estimation method and estimator for sideslip angle of straight-line navigation of agricultural machinery, which collect and analyze front wheel steering angle information, forward speed information of agricultural machinery, antenna positioning information and current attitude information. The estimation of sideslip angle is realized based on state observation theory. The first estimator, the second estimator and the third estimator are used to estimate the heading deviation of the vehicle body, position deviation and sideslip angle information. In the analysis process, integration is used instead of differentiation, which avoids the error of amplification by differential operation.

## Claims

### Claim 1

A sideslip angle estimation method suitable for straight-line navigation of agricultural machinery is characterized in that it includes the following steps:
S1, collecting front wheel steering angle information, forward speed information, antenna positioning information and current attitude information of the agricultural machinery during the traveling process of the agricultural machinery, and performing corresponding analysis and processing on the information;
S2, constructing a dynamic equation of agricultural machinery and taking the dynamic equation as a system state equation, and estimating the sideslip angle in the straight-line navigation path tracking process based on the state observer theory, specifically:
(1) according to the antenna positioning information and current attitude information of agricultural machinery collected in S1, analyze and obtain a comprehensive error signal ε(j) at time j:
ε(j)=ky(y(j)−ŷ(j))+kθ({tilde over (θ)}(j)−{circumflex over ({tilde over (θ)})}(j)) (3)
among them, y(j) represents the measured value of position deviation at time j, which is recorded as the distance between navigation point coordinates and the nearest point on the route planning line, {tilde over (θ)}(j) indicates the measured value of heading deviation at time j, which is recorded as the difference between the heading of the vehicle and the heading of the route planning line, {circumflex over ({tilde over (θ)})}(j) indicates the estimated value of heading deviation at j time, ŷ(j) estimates value of position deviation at time j, ky and kθ are coefficient, which is satisfied kθ+ky<1 and kθ<ky. The initial values of position deviation estimation and heading deviation estimation are both 0;
(2) according to the obtained comprehensive error signal ε(j), analyze and obtain the estimated value {circumflex over (β)}(j) of sideslip angle at time j:
{circumflex over (β)}(j)={circumflex over (β)}(j−1)+k1ε(j)Ts (4)
among them, {circumflex over (β)}(j−1) represents the estimated value of sideslip angle at time j−1, k1 is the coefficient, and Ts represents the system control period;
(3) according to the collected front wheel rotation angle information, forward speed information, comprehensive error signal ε(j) and sideslip angle estimated value {circumflex over (β)}(j), the heading deviation at time j is estimated to obtain the estimated value of heading deviation at time j:
θ
~
^
⁡
(
j
)
=
θ
~
^
⁡
(
j
-
1
)
+
T
s
⁢
{
v
⁡
(
j
)
L
⁢
⁢
(
j
)
⁡
[
tan
⁡
(
δ
⁡
(
j
)
+
β
^
⁡
(
j
)
)
-
⁢
(
j
)
]
+
k
2
⁢
ɛ
⁡
(
j
)
}
(
5
)
among them, {circumflex over ({tilde over (θ)})}(j−1) represents the estimated value of heading deviation at time j−1, ν(j) is the current forward speed of the vehicle, L is the length of the vehicle body, δ(j) δ(j) is the current front wheel steering angle, and k2 is the coefficient.
(4) according to the collected forward speed information of agricultural machinery, the estimated value of heading deviation, the estimated value of sideslip angle and the comprehensive error signal obtained by analysis, the position deviation of heading is estimated to obtain the estimated value of position deviation:
ŷ(j)=ŷ(j−1)+Ts[ν(j)sin({circumflex over ({tilde over (θ)})}(j)+{circumflex over (β)}(j))+k3ε(j)] (6)
among them, ŷ(j−1) represents the estimated value of position deviation at time j−1, and k3 is the coefficient.

### Claim 2 (depends on 1)

The sideslip angle estimation method suitable for straight-line navigation of agricultural machinery according to claim 1, characterized in that in step S1, when analyzing and processing the collected data, the following methods are specifically adopted:
(1) The collected front wheel steering angle information is A/D converted and filtered to obtain the digital value δ(j) of the front wheel steering angle at time j;
(2) Filtering the collected forward speed information of agricultural machinery to obtain the current forward speed ν(j) at time j;
(3) Through coordinate transformation and analysis of 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. The position deviation measure value y(j) at time j is defined as that distance between the coordinate of the navigation point and the nearest point on the path planning line. The heading deviation measured value {tilde over (θ)}(j) is the difference between the heading of the vehicle at time j and the heading of the route planning line.

### Claim 3 (depends on 1)

The sideslip angle estimation method suitable for straight-line navigation of agricultural machinery according to claim 1, characterized in that the dynamic equation of agricultural machinery constructed in step S2 is:
{
y
.
=
v
⁢
sin
⁡
(
θ
˜
+
β
)
θ
~
.
=
v
L
⁢
[
cos
⁢
⁢
β
⁢
(
tan
⁡
(
δ
+
β
)
-
tan
⁢
⁢
β
)
]
(
2
)
among them, δ indicates the front wheel angle, L indicates the length of agricultural machinery body, ν indicates the forward speed of the vehicle, β indicates the sideslip angle and {tilde over (θ)} indicates the heading deviation, and y indicates the position deviation, {dot over (y)} and {dot over ({tilde over (θ)})} respectively represent the first order reciprocal of the position deviation and the heading deviation.

### Claim 4 (depends on 2)

The sideslip angle estimation method suitable for straight-line navigation of agricultural machinery according to claim 2 is characterized in that: when filtering the collected front wheel steering angle information, mean filtering is adopted, and the filter points of mean filtering are defined as N, the sampling interval of A/D conversion is Δt, and the system control period is Ts, then the mean filter points N satisfy the relational expression:
N<0.5Ts/Δto

### Claim 5

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.

### Claim 6 (depends on 5)

The sideslip angle estimator suitable for straight-line navigation of agricultural machinery according to claim 5, characterized in that the output end of the front wheel angle sensor (1) is connected with the input end of the second estimator (10) through the A/D converter (3) and the first digital filter (4) in turn, and the first digital filter (4) is used for realizing the estimation of the front wheel after being converted by the A/D converter (3).

### Claim 7 (depends on 6)

The sideslip angle estimator suitable for straight-line navigation of agricultural machinery according to claim 6, which is characterized in that if the filter points of the first digital filter (4) are N, the sampling interval of the A/D converter (3) is Δt, and the control period of the automatic navigation system is Ts, then the filter points N satisfy the relational expression:
N<0.5Ts/Δt

### Claim 8 (depends on 5)

The sideslip angle estimator suitable for straight-line navigation of agricultural machinery according to claim 5, characterized in that one end of the output end of the GNSS positioning and orientation device (2) is connected with the input end of the second estimator (10) through a second digital filter (5), and the second digital filter (5) realizes filtering processing of the collected forward speed information;
the other end of the output end of the GNSS positioning and orientation device (2) is connected with the input end of a comprehensive error calculator (8) through a coordinate transformation module (6) and a tracking error calculator (7) in turn, wherein the coordinate transformation module (6) carries out coordinate transformation on collected information to obtain navigation point coordinate information, and the tracking error calculator (7) is used for calculating position deviation measurement value y(j) and heading deviation measurement value {tilde over (θ)}(j) between the navigation point coordinate information and the route planning line.