Automatic calibration method of an angle sensor for an automatic drive control system of a farm machine

Abstract

An automatic calibration method of an angle sensor for an automatic drive control system of a farm machine includes the following steps. S1: fixing a steering wheel of the farm machine to make front wheels of a vehicle kept at a fixed angle. S2: collecting a plurality of pieces of current position information of the farm machine, and processing the plurality of pieces of current position information to obtain an average value. S3: establishing a two-wheel farm machine kinematics model based on a center of a rear axle. S4: performing a radius calculation to obtain a set of angle correspondences. S5: rotating the farm machine by a preset angle at a constant speed with the rear axle of the farm machine as a center, and performing S1 through S4. S6: after performing S5 for several times, performing an angle value fitting calculation to obtain a calibration coefficient.

Claims

1. An automatic calibration method of an angle sensor for an automatic drive control system of a farm machine, wherein the method comprises the following steps: S1: fixing a steering wheel of the farm machine to make front wheels of a vehicle kept at a fixed angle; S2: collecting a plurality of pieces of current position information of the farm machine, and processing the plurality of pieces of current position information to obtain an average value; S3: establishing a two-wheel farm machine kinematics model based on a center of a rear axle; S4: performing a radius calculation to obtain a set of angle correspondences; S5: rotating the farm machine by a preset angle at a constant speed with the rear axle of the farm machine as a center, and performing S1 through S4; S6: after performing S5 for several times, performing an angle value fitting calculation to obtain a calibration coefficient.

2. The automatic calibration method of the angle sensor for the automatic drive control system of the farm machine of claim 1, wherein in S6, data of left turns and right turns of the vehicle are respectively collected for not less than 5 times.

3. The automatic calibration method of the angle sensor for the automatic drive control system of the farm machine of claim 1, wherein S3 comprises: assuming that a radius of a circle of a front wheel and a rear wheel is R, and obtaining equation set 1 below: $?=2*\mathrm{asin}\left(\frac{L}{2*R}\right)$ $?=?/2$ wherein ? is a front wheel steering angle of the tractor, and L is a wheelbase of the tractor; in equation set 1, ? is a corresponding central angle of the wheelbase L, and ? is a corresponding circumferential angle of the wheelbase L; an equation 2 below is derived from the equation set 1: $?=\mathrm{asin}\left(\frac{L}{2*R}\right).$

4. The automatic calibration method of the angle sensor for the automatic drive control system of the farm machine of claim 3, wherein S4 comprise an equation set 3 below:
a=2*(x.sub.2?x.sub.1)
b=2*(y.sub.2?y.sub.1)
c=x.sub.2.sup.2+y.sub.2.sup.2?x.sub.1.sup.2?y.sub.1.sup.2
d=2*(x.sub.3?x.sub.2)
e=2*(y.sub.3?y.sub.2)
f=x.sub.3.sup.2+y.sub.3.sup.2?x.sub.2.sup.2?y.sub.2.sup.2
x=(b*f?e*c)/(b*d?e*a)
y=(d*c?a*f)/(b*d?e*a)
R=((x?x.sub.1).sup.2?(y?y.sub.1).sup.2).sup.0.5 x and y are coordinates of a circle center; x.sub.1, y.sub.1, x.sub.2, y.sub.2, x.sub.3, y.sub.3 are coordinates of three points recorded during a driving process of the farm machine.

5. The automatic calibration method of the angle sensor for the automatic drive control system of the farm machine of claim 4, wherein the steps of the angle value fitting calculation comprise: assuming that a value of the angle sensor is x.sub.n, a corresponding angle sensor value is y.sub.n, wherein y.sub.n is equal to y calculated according to the position sampling points in equation set 3, n is a n-.sub.th correspondence, and calibration quadratic coefficients of the angle sensor are a.sub.0, a.sub.1, and a.sub.2; $\left[\begin{array}{ccc}1& x_1& x_1^2\\ 1& x_2& x_2^2\\ \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}\\ 1& x_n& x_n^2\end{array}\right]\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ \mathrm{.Math.}\\ y_n\end{array}\right];$ and a.sub.0, a.sub.1 and a.sub.2 are calibration results.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) The present invention and characteristics, appearance, and advantages thereof will become clearer by reading the detailed description of the non-limiting embodiment with reference to the drawings below. In the drawings, identical reference numerals represent the same part. The drawings are drawn according to specific proportions, the emphasis of the drawings is to illustrate the substance of the present invention.

(2) FIG. 1 is a flow chart showing an automatic calibration method for an angle sensor of an automatic drive control system of a farm machine according to the present invention;

(3) FIG. 2 is a schematic diagram showing a kinematic model of a two-wheel tractor based on a rear axle center;

(4) FIG. 3 is a flow chart of the present invention according to an embodiment.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(5) In the following descriptions, numerous specific details are given for a complete understanding of the present invention. However, it is apparent to those skilled in the art that the present invention may be implemented without one or more of these details. In other examples, in order to avoid confusion with the present invention, some technical features known in the art are not described.

(6) For a complete understanding of the present invention, detailed steps and detailed structures will be provided in the following descriptions in order to clearly illustrate the technical solution of the present invention. A preferred embodiment of the present invention is described in detail hereinafter. However, besides these detailed descriptions, the present invention may have other implementations.

(7) The present invention provides an automatic calibration method of an angle sensor for an automatic drive control system of a farm machine, which includes the following steps.

(8) Step S1: fixing a steering wheel of the farm machine to make the front wheels of the vehicle at a fixed angle;

(9) Step S2: collecting a plurality of pieces of current position information of the farm machine, and processing the plurality of pieces of current position information to obtain an average value;

(10) Step S3: establishing a two-wheel farm machine kinematics model based on the center of a rear axle;

(11) Step S4: performing a radius calculation to obtain a set of angle correspondences;

(12) Step S5: rotating the farm machine by a preset angle at a constant speed with the rear axle of the farm machine as a center, and performing Step S1 through Step S4;

(13) Step S6: after performing Step S5 several times, performing an angle value fitting calculation to obtain a calibration coefficient.

(14) In an optional embodiment of the present invention, the data of left turns and right turns of the vehicle is respectively collected for not less than 5 times.

(15) FIG. 2 is a schematic diagram showing a kinematic model of a two-wheel tractor based on a rear axle center. In the figure, ? is the front wheel steering angle of the tractor, V is the speed of the rear wheel of the tractor, V.sub.f is the speed of the front wheel of the tractor, L is the wheelbase (or chord length) of the tractor, point C is perpendicular to L and is located at the circumference of the circles of the front and rear wheels. Namely, ? is the circumferential angle corresponding to the chord length L.

(16) Assuming that the radius of the circles of the front and rear wheels is R, then

(17) $\begin{array}{cc}?=2*\mathrm{asin}\left(\frac{L}{2*R}\right)?=?/2& \mathrm{Equation}1\end{array}$

(18) In Equation 1, ? is the central angle corresponding to the chord length L, and ? is the circumferential angle corresponding to the chord length L. Also, ? is equal to the value of the angle sensor of the front wheel. Equation 2 is derived from Equation 1, that is to say, the rotation angle of the front wheel and the radius have a non-linear correspondence. If the radius value can be known, the angle value of the front wheel can be acquired through reverse derivation:

(19) $\begin{array}{cc}?=\mathrm{asin}\left(\frac{L}{2*R}\right).& \mathrm{Equation}2\end{array}$

(20) For high-precision GNSS, by using Real Time Kinematic (RTK) technology, the positioning accuracy can be improved to be within the range of 1 cm. In the automatic drive control system of a farm machine, the angle sensor is one of the components of the entire system, and the high-precision GNSS board is another component. Since the present patent focus on the research of angle sensor calibration, and does not involve RTK technology. The present application directly uses the position information output by the high-precision board in the implementations.

(21) Equation 3 includes calculation formulas of an algebraic equation of a circle that is determined by three points. The coordinates of three points recorded during the driving process of the tractor are denoted by P1 (x.sub.1, y.sub.1), P2 (x.sub.2, y.sub.2) and P3 (x.sub.3, y.sub.3). The steps of calculating the radius R include:
a=2*(x.sub.2?x.sub.1)
b=2*(y.sub.2?y.sub.1)
c=x.sub.2.sup.2+y.sub.2.sup.2?x.sub.1.sup.2?y.sub.1.sup.2
d=2*(x.sub.3?x.sub.2)
e=2*(y.sub.3?y.sub.2)
f=x.sub.3.sup.2+y.sub.3.sup.2?x.sub.2.sup.2?y.sub.2.sup.2
x=(b*f?e*c)/(b*d?e*a)
y=(d*c?a*f)/(b*d?e*a)
R=((x?x.sub.1).sup.2?(y?y.sub.1).sup.2).sup.0.5Equation 3

(22) wherein a, b, c, d, e and f are intermediate calculation variables; x, y are coordinates of circle center; x.sub.1, y.sub.1, x.sub.2, y.sub.2, x.sub.3 and y.sub.3 are the coordinates of three points recorded during the driving process of the farm machine.

(23) According to Equation 2 and Equation 3, a radius value corresponding to the fixed front wheel rotation angle can be acquired. Theoretically, a corresponding line can be obtained according to two angle values and a radius value. However, due to the impacts of various environmental factors, the change of the angle value may not be linear. In order to improve the calibration accuracy, the present patent uses the least squares method of the quadratic curve to quadratically fit a plurality of angle values and radius values.

(24) Assuming that the value of the angle sensor is X.sub.n, the corresponding value of the angle sensor that is calculated by the position sampling points in the Equation 3 is y.sub.n, n is the n.sub.th correspondence (i.e. rotating the farm machine the n.sub.th time), and the calibration quadratic coefficients of the angle sensor are a.sub.0, a.sub.1, and a.sub.2.

(25) $\begin{array}{cc}\left[\begin{array}{ccc}1& x_1& x_1^2\\ 1& x_2& x_2^2\\ \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}\\ 1& x_n& x_n^2\end{array}\right]\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ \mathrm{.Math.}\\ y_n\end{array}\right]& \mathrm{Equation}4\end{array}$

(26) a.sub.0, a.sub.1 and a.sub.2 are the calibration results.

(27) The present invention has the following advantages.

(28) 1. By applying the technique combining vehicle kinematics and high-precision GNSS in the present application, a high-precision angle sensor calibration can be realized.

(29) 2. The present application can realize the automatic calibration without the external assisting apparatus and related experiential parameter input.

(30) 3. The present application is especially suitable for the precise agricultural automatic drive control systems.

(31) The preferred embodiment of the present invention has been described above. It should be understood that the present invention is not limited to the specific embodiments described above. Devices and structures that are not described in detail herein should be understood as being implemented in a common manner known in the art. Various possible changes and modifications or equivalent embodiments obtained by equivalent substitutions may be derived from the technical solution of the present invention according to the method and technical features recited above without departing from the technical solution of the present invention by any skilled person in the art, which do not have any impact on the essence of the present invention. Therefore, any simple modification, equivalent substitution and modification made based upon the above embodiment according to the technical essence of the present invention without departing from the content of the technical solution of the present invention should still be considered as falling within the scope of the technical solution of the present invention.