Low-cost vehicle automatic driving design implementation methodThe technical field is as follows: the invention relates to a vehicle automatic driving design implementation method, in particular to a low-cost high-precision automatic driving implementation method for agricultural machinery.
Background art: with the development of MEMS (Micro-Electro-Mechanical-System) sensors, navigation and control technologies and further increase of agricultural supporting strength in China, precision agriculture is rapidly becoming a trend. China is a traditional big agricultural country, but for various reasons, the operation is still carried out manually or in a semi-mechanized mode at present, the operation has the disadvantages of large manual investment, low working efficiency and poor working quality, resources such as diesel oil, chemical fertilizer, pesticide, seeds and the like are greatly wasted, the aging degree of China is gradually increased, and the embarrassing situation that young people cannot land and experienced old people cannot land finally occurs if the current agricultural planting mode is changed. Precision agriculture has already begun to be popularized in developed countries such as the united states, japan and australia over 10 years ago, and the products are also the most advanced and stable at present. As the domestic precision agriculture belongs to the initial stage, no product which wants to be stable exists, and the domestic market is monopolized by foreign products.
The invention content is as follows: in the automatic driving control process of the agricultural machinery, the attitude, the speed and the position information of the vehicle body can reflect the motion and the position information of the vehicle body in real time, and the information can provide important data input for high-precision combined navigation and control algorithms. Strapdown Inertial Navigation (SINS) has the characteristics of autonomous Navigation, good confidentiality, strong anti-interference capability, rich Navigation parameters, high precision in a short time and the like and is widely applied, but due to the inherent error of an Inertial sensor, the Navigation error accumulates with time, the Navigation precision is poor for a long time, and other Navigation System assistance with stable errors, such as high-precision GPS-RTK, is needed. The inertial navigation system obtains the speed and the position through integral operation according to the measured acceleration of the vehicle body. For this purpose, the initial velocity and position must be known. In addition, in inertial systems with geographic coordinate systems as navigation coordinate systems, both the physical and mathematical platforms are the reference for measuring acceleration, and the platforms must be accurately aligned and tracked to the geographic coordinate system to avoid acceleration measurement errors caused by platform errors. The accuracy of the initial alignment is directly related to the working accuracy of the navigation system, and is also one of important key technologies. If the combined navigation algorithm implemented according to the above scheme requires 3 gyroscopes, 3 accelerometers and 3 geomagnetic sensors, that is, a 9-axis IMU performs the full attitude algorithm settlement and the position and velocity information combined navigation settlement, the advantage of the scheme is that the attitude angle, position and velocity information can be calculated in real time at a higher frequency, a higher control frequency can be ensured, and a higher control precision is achieved. However, this solution has the drawback of being too costly, and the key to this method is known to be the accuracy of the gyroscope, but the price of the gyroscope is also proportional to its accuracy, and the geomagnetic sensor is very susceptible to interference due to the requirements of the use environment and cannot be corrected without passing through the 3D calibration, so that the heading angle accuracy cannot be guaranteed. Therefore, the invention designs a low-cost high-precision automatic driving implementation method of the agricultural implement in the field of precision agriculture so as to solve the problems. The invention consists of core technologies such as satellite positioning, antenna position attitude correction, installation error calibration, path planning, automatic control and the like. As shown in a drawing machine and a system model diagram, a satellite antenna receives satellite signals (GPS, Beidou and the like), centimeter-level position precision is obtained through a base station radio station RTK technology, a gyroscope 1 and acceleration in a controller are subjected to Kalman filtering fusion to calculate the inclination angle of a tractor in real time, the position signals are subjected to attitude correction by using the angle and a path planning strategy, transverse deviation is calculated, a gyroscope 2 in the controller and a heading angle output by the satellite are subjected to Kalman filtering fusion to calculate the heading angle of the tractor in real time, the transverse deviation and the heading angle are input as control system signals, a closed-loop control system is formed by a front wheel angle sensor, and an automatic control algorithm module and an electromagnetic valve are adopted to drive the tractor to automatically run. The invention discloses a satellite positioning and RTK technology, which belongs to a mature technology and is out of the discussion range of the invention.
The specific implementation mode is as follows:
1. attitude angle calculation
The method for calculating the full attitude angle is realized by nine-axis IMUs (a three-axis gyroscope, a three-axis accelerometer and a three-axis geomagnetic sensor) through Kalman filtering fusion at present, the key of the method is the accuracy of the gyroscope, however, the price of the gyroscope is also in direct proportion to the accuracy of the gyroscope, and the geomagnetic sensor is extremely easy to interfere and cannot be corrected through 3D calibration due to the requirement of a use environment, so the heading angle accuracy cannot be guaranteed. The pitch angle in the plane does not influence the transverse position of the tractor, and the invention abandons the full-attitude calculation method, and respectively calculates the roll angle and the course angle through the course angles output by the gyroscope plus the accelerometer and the gyroscope plus the satellite, thereby not only saving the hardware cost of one gyroscope, two accelerometers and three geomagnetic sensors, but also ensuring the precision.
Roll angle calculation model and Kalman filtering
Equation 1 is a roll angle Kalman filtering realization state and observation equation, wherein
Is a system variable, θaThe system roll angle is set, and b is the gyroscope null shift;
transferring the matrix for the system;
inputting a transfer matrix for the system;
u＝ωgyroas system input, ωgyroAn angular acceleration value output for the gyroscope 1;
as systematic error, ωgWhite noise for gyroscope 1;
h ═ 10 ] is an observation matrix, and upsilon is observation white noise;
θ＝arc cos(a1) The tilt angle calculated for the accelerometer 1, a1The acceleration value output for acceleration 1.
Course angle calculation model and Kalman filtering
Establishing a gyroscope model:
equation 2 is the gyroscope model build, where ω isTFor the assumed true output of the gyroscope, ωgyroFor gyroscope current output, α is the gyroscope scale factor, β is the gyroscope zero bias, θTAnd calculating a real angle of the gyroscope, wherein T is an angle calculation period, T is a time axis, gamma is the updating frequency of the gyroscope in the T time, and delta T is the updating period of the gyroscope.
Equation 3 is a heading angle Kalman filtering realization state and observation equation, wherein
Is a system variable, psi is a system course angle variable, α is a gyroscope scale factor, β is a gyroscope zero offset;
for the system transfer matrix, T is the angle calculation period, θgyroIntegrating angle of the gyroscope, wherein delta t is the updating period of the gyroscope;
omega is system white noise;
upsilon observation white noise;
h ═ 100 ] is the observation matrix;
ψ is the VTG heading angle of the GPS output.
2. Position and attitude correction
For better signal reception, the satellite antenna should be installed at a distance above the tractor. The tractor may tilt or pitch during driving over rough terrain, which may affect the satellite positioning accuracy and should be compensated or corrected.
The full attitude position correction equation is formula 4, where
The real position coordinates of the satellite after the attitude correction are obtained;
position coordinates output for the satellite board card;
is the projection of the satellite antenna on the X/Y/Z coordinate axis of the tractor coordinate system;
an attitude correction matrix is formed, wherein psi, theta and phi are respectively a course angle, a roll angle and a pitch angle;
assuming that the coordinate systems X, Y and Z of the tractor respectively correspond to the right upper front, since only the lateral deviation is concerned in the implementation of the tractor automatic control algorithm, the lateral deviation of the X direction is not greatly influenced by the course angle and the pitch angle, but the cost is greatly increased if the full-attitude correction is considered, and the complexity of system calculation is increased, therefore, the invention only considers the factor influencing the maximum lateral deviation- -the roll angle position correction, namely the correction of the roll angle position, namely the invention
3. Path planning
If the tractor works linearly and the tillage width is set, the tractor can complete linear operation according to the parallel lines of the determined straight lines, and if the curve operation is only to change the determined straight lines into a curve consisting of a plurality of straight lines, the tractor can complete curve operation according to the parallel curves. According to the mathematical principle, a straight line is determined by two points, namely a straight line path can be planned by the positions of the two points after posture correction, namely three parameters of a general equation formula five of the straight line, and then the transverse deviation of the current position of the tractor is calculated in real time through the distance from the six points to the straight line.
Ax + By + C ═ 0 (formula 5)
Wherein A, B, C is a linear coefficient;
(x0，y0) Is the current coordinate point;
d is the distance from the current coordinate point to the navigation line, i.e. the lateral deviation.
4. Linear tracking control algorithm based on pure tracking model
The pure tracking algorithm is a calculation method, and aims to calculate the arc length which the moving tractor needs to travel to reach a specified position. The method has the characteristics of simplicity, intuition and easy implementation, and the core of the method is to determine a proper look-ahead distance, and the description of the algorithm is shown in figure 2.
In the vehicle body coordinate system O ' x ' y ' in fig. 2, the point P (x ', y ') is a target point on the path, L is a chord length of an arc segment connecting the origin of the vehicle body coordinate system and the point P, i.e., a forward-looking distance, and R is a radius of the arc segment. The relation of x', L and R is
Obtained by the formula 7
In the formula 8, x' can be regarded as an abscissa of the target point on the tracking path under the vehicle body coordinate system.
Wherein,
pethe transverse tracking error of the mass center of the vehicle body relative to the tracking path is positive when the mass center of the vehicle body deviates to the right in the advancing direction of the vehicle body, and negative when the mass center of the vehicle body deviates to the left;
ψeand the difference value between the current course angle of the vehicle body and the course angle of the tracking linear target is obtained.
Obtaining the relation between the turning angle and the turning radius of the front wheel of the vehicle body according to the simplified two-wheel vehicle model:
arctan (E/R) (equation 10)
Wherein, the angle is the corner of the front wheel, and E is the wheelbase of the vehicle body.
Obtaining a steering wheel deflection angle control quantity calculated by a pure tracking model under the condition of linear tracking according to a formula 8-10:
in which E is known and peAnd psieCan be obtained by state estimation settlement of a Kalman filter.
The forward looking distance L determination method comprises the following steps:
in the performance index of the integrated optimal time response, two important factors of the error and the time required for the error process are generally considered. Typical overall performance metrics include: the integral of the square of the error ISE, the integral of the absolute value of the error ie, the integral of the time times the absolute value of the error ITAE, the integral of the time times the square of the error ITSE, etc. The system designed according to the ITAE criterion has small overshoot, moderate damping and good selectivity, so the invention adopts the ITAE criterion.
By using the EITAE obtained by the formula 12, the transverse tracking error data obtained under different forward looking distance conditions can be compared and analyzed, and the forward looking distance with the minimum value is the optimal value.
Drawings
FIG. 1 is a schematic diagram of a low cost vehicle autopilot system
Fig. 2 is a pure tracking model geometry resolution.