Positioning method of tractor navigation control system based on multi-sensor fusion

Abstract

The positioning method of the multi-sensor fusion tractor navigation control system provided by the application has the advantages that the tractor is positioned by the upper computer, the positioning adopts the fuzzy self-adaptive extended Kalman filtering algorithm, the ratio of the measured new information variance and the theoretical variance obtained in real time can be obtained, the weighting coefficient of the measurement noise covariance matrix is continuously adjusted by using the fuzzy self-adaptive controller, the correction position error is reduced, the accuracy and stability of the tractor positioning data are improved, and the application has the accurate vehicle positioning and path tracking performance.

Description

Technical Field

[0001] This invention relates to the field of tractor automatic navigation, specifically to a positioning method for a tractor navigation control system based on multi-sensor fusion. Background Technology

[0002] In current agricultural operations, drivers typically operate the machinery alone, resulting in high labor intensity, repetitive and monotonous work, easily leading to fatigue and operational errors. Automatic navigation technology for agricultural machinery is a crucial technology in precision agriculture. This technology can automatically obtain navigation paths and control agricultural machinery to travel along the target path, assisting drivers in operating the machinery. Automatic tractor navigation is a vital foundation for agricultural modernization. Implementing automatic tractor navigation can reduce the workload of agricultural workers, significantly improve the operational accuracy of agricultural machinery, increase land utilization, reduce production costs, increase crop yields and economic benefits, and liberate workers from arduous fieldwork.

[0003] Tractor navigation technology research began earlier in North America, Japan, and some European countries, yielding significant results. Domestic experts and scholars have also conducted extensive research and achieved some success. For example, invention patent application number 2014101751440 discloses a through-beam laser tractor auxiliary navigation control system; invention patent authorization number 101833334A discloses a tractor automatic navigation control system and method; and invention patent application number 104656647A discloses a navigation control system for a self-propelled tractor used in low-growing crop fields. It can be seen that these all employ single navigation and positioning technologies, such as GPS navigation and laser emission navigation. Each navigation method has its advantages, but due to limitations imposed by single navigation technologies, they all have certain usage conditions and are difficult to provide continuous, stable, and high-quality positioning information. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a positioning method for a tractor navigation control system with multi-sensor fusion, which has accurate vehicle positioning and path tracking performance.

[0005] The present invention achieves its objective by employing the following technical solution:

[0006] A multi-sensor fusion tractor navigation control system includes a host computer and a slave computer, which are interconnected via a serial communication module to achieve precise tractor positioning and accurate tracking of a set path. The host computer is connected to a touchscreen, a Beidou positioning receiver module, an image acquisition module, a pose detection module, and a serial communication module. The slave computer is connected to a data storage module, a pressure detection module, a CAN communication module, a directional valve control module, a voltage detection module, a vehicle speed detection module, a wheel angle detection module, a power supply module, an alarm and emergency handling module, and a serial communication module.

[0007] (1) In this technical solution, the navigation control system adopts a combination of Beidou positioning and machine vision positioning, which can achieve the complementary advantages of the two navigation technologies and avoid the vehicle position error caused by using a single positioning. The combined navigation function can be used in both crop-bearing and cropless farmland. The combined navigation positioning technology is better than the single navigation in terms of reliability and accuracy. (2) A dual-antenna Beidou signal receiving device is used, and the data from the pose detection module is used to correct the Beidou positioning information, resulting in more accurate tractor position data. (3) Not only are the current route offset and current heading angle offset of the tractor selected, but also the estimated path curvature and reference speed of the tractor are selected for processing, so that the navigation control system has a good path control effect.

[0008] The tractor's current course deviation, current heading angle deviation, and estimated path curvature value are calculated and output by the host computer using BeiDou navigation data or image data; the tractor's reference speed V U (s) Vehicle speed V sent by the host computer R (s) and vehicle speed collection V M (s) is determined according to the following formula:

[0009]

[0010] Because the accuracy of the tractor's speed obtained through Beidou navigation is lower than that obtained by the vehicle speed detection module when the tractor is traveling at low speeds, the accuracy of the vehicle speed V collected by the vehicle speed detection module should be considered when determining the vehicle speed. M (s) less than or equal to 2 m / s or the vehicle speed V received by the host computer R When (s) equals 0, the determined reference speed is V. M (s), this avoids speed errors caused by BeiDou navigation at low speeds or in case of malfunction; when the vehicle speed detection module collects the vehicle speed V M (s) If the speed is greater than 2 m / s or the vehicle speed detection module malfunctions, the reference vehicle speed is determined to be the vehicle speed V sent by the host computer. R (s).

[0011] As a further limitation of this technical solution:

[0012] The directional valve control module is electrically connected to both the voltage detection module and the hydraulic steering system.

[0013] The pressure detection module is also electrically connected to the hydraulic steering system.

[0014] The host computer uses the EPCS-8980 industrial control computer.

[0015] The touchscreen used is a four-wire resistive touchscreen SV0804S-03.

[0016] The BeiDou positioning receiver module uses the UM220 module.

[0017] The image acquisition module consists of an OKAC1310CCD camera and a signal conditioning circuit.

[0018] The attitude detection module uses the AHRS-3000 small attitude measurement module.

[0019] The lower-level machine uses a 16-bit Freescale microcontroller MC9S12XS128MAL.

[0020] The data storage module uses two 32MB SDRAM MT48LC4M32B2 data storage chips.

[0021] The pressure detection module consists of a pressure sensor MBS1250 and a signal conditioning circuit.

[0022] The CAN communication module consists of a TJA1043T chip and its peripheral circuitry. Through this module, the lower-level computer can send the tractor's desired speed, determined based on the estimated travel distance and path curvature, to the tractor controller. This ensures effective control when the tractor reaches its destination or turns, especially when the current path offset is zero. R (s) Exceeds the maximum permissible route offset O M At that time, a stop signal is sent to the tractor controller through this module.

[0023] The commutation valve control module consists of a D / A converter chip DAC0832, an operational amplifier LM358N, and peripheral circuitry.

[0024] The voltage detection module consists of an operational amplifier AD741 and peripheral circuitry.

[0025] By designing a voltage detection module, closed-loop control of the output voltage of the directional valve control module can be implemented, and output voltage runaway can be avoided by real-time detection of the output voltage.

[0026] The vehicle speed detection module consists of a VB-Z9400 speed sensor and a signal conditioning circuit.

[0027] The wheel angle detection module consists of a linear displacement sensor HPS-M1 and a signal conditioning circuit.

[0028] The power module consists of a 24V voltage generating chip LT4356IS, a 5V voltage generating chip LM7805, a ±15V voltage generating chip MD20-12D15, a 10V voltage generating chip AD581, and peripheral circuitry.

[0029] The serial communication module consists of the MAX232 serial communication chip and its peripheral circuits.

[0030] The alarm and emergency handling module consists of a horn, LEDs, optocouplers, and relays. The alarm and emergency handling module can operate in liquid...

[0031] When the pressure valve reaches its maximum operating value or when the curve path has excessive curvature, an alarm message is issued and the corresponding emergency response action is initiated.

[0032] The data sent from the host computer to the slave computer for path tracking via the serial communication module includes: current route offset, current heading angle offset, estimated travel distance, current travel speed, and estimated path curvature.

[0033] The positioning method of the tractor navigation control system based on multi-sensor fusion, which performs tractor positioning through a host computer, includes the following steps:

[0034] (1) Set the tractor's field working mode via the touch screen: no crop field, crop field;

[0035] (2) A dual-antenna Beidou receiver is used to obtain the absolute position, speed and heading angle information of the tractor using the Beidou positioning receiver module;

[0036] (3) The tractor’s heading angle, roll angle and pitch angle data are obtained by using the pose detection module, and the Beidou positioning information is corrected based on these data;

[0037] (4) If the field mode is set to a field without crops, the image acquisition module is turned off and the machine vision positioning method is not used; if the field mode is set to a field with crops, the image acquisition module is turned on, the acquired image data is processed, and a navigation baseline is generated and basic points representing the characteristics of crops are obtained.

[0038] (5) If the field mode is set to a field without crops, the BeiDou positioning and path data corrected by the pose information are used. If the field mode is set to a field with crops, the fuzzy adaptive extended Kalman filter is used to fuse the BeiDou positioning data and the machine vision data. The BeiDou positioning provides the absolute position coordinates, heading angle and driving speed of the tractor, and the machine vision provides the relative position coordinates of known points in the navigation path. After the coordinates of the BeiDou positioning system and the machine vision system are unified, the filter filters the position data provided by BeiDou and the position data provided by machine vision to obtain accurate position data and path data.

[0039] Compared with the extended Kalman filter and unscented Kalman filter commonly used in current multi-sensor fusion technology, the fuzzy adaptive extended Kalman filter algorithm can reduce the correction position error and improve the accuracy and stability of tractor positioning data by using the ratio of the measurement innovation variance to the theoretical variance obtained in real time and by continuously adjusting the weighting coefficients of the measurement noise covariance matrix using a fuzzy adaptive controller.

[0040] A control method for a tractor navigation control system based on multi-sensor fusion, wherein the control method is path tracking control, includes the following steps:

[0041] (1) The path tracking control system is designed using a cascade control structure, in which the main loop adopts a feedforward plus feedback control method, and the secondary loop adopts a unity negative feedback method. The entire system structure is based on the offset setpoint O. S (s), Current route offset O R (s), Offset controller C O (s), Angle Controller C S (s), Reversing valve controller C V (s), voltage acquisition V M (s), Steering system G(s), Estimated path curvature PC R (s), Current heading angle offset A R (s), heading angle controller C A (s), wheel angle feedback A(s), vehicle speed V sent by the host computer R (s), Vehicle speed collection V M (s), Reference vehicle speed V U (s) composition;

[0042] (2) The feedback controller in the main loop is the offset controller C. O (s), whose setpoint input is the offset setpoint O S (s) and current route offset O R The difference between (s) and the offset controller C O The output of (s) is the steering angle θ. FBIt is calculated using the following formula:

[0043]

[0044] Where ΔO=O S (s)-O R (s), K FB1 K is the proportional coefficient of the offset controller. FB2 K represents the differential coefficient of the offset controller. FB3 These are the second-order derivative coefficients of the offset controller. Since the direct control quantity for tractor path tracking is the route offset, this invention reduces overshoot and overcomes oscillation by implementing proportional, derivative, and second-order derivative control on the offset difference. Furthermore, by setting a maximum allowable route offset O... M This allows the tractor to stop steering when the deviation from the course exceeds the allowable range, and controls the offset controller output to 0, thus preventing damage to the hydraulic valve or safety accidents.

[0045] (3) The feedforward controller in the main loop is the heading angle controller C. A (s), whose reference input is the estimated path curvature PC R (s), Current heading angle offset A R (s) and current route offset O R (s), heading angle controller C A The output of (s) is the steering angle θ. FF It is calculated using the following formula:

[0046]

[0047] Where K FF1 K is the proportional coefficient for path curvature control of the heading angle controller. FF2 K is the proportional coefficient of the heading angle controller. FF3 This is the differential proportional coefficient for the heading angle controller. Within the allowable value of the course deviation O... P Within the specified range, by using the heading angle as a reference, pre-control can be implemented to reduce fluctuations in the path tracking process; by using the estimated path curvature as a reference, pre-control can be achieved when driving on curves or turning, improving the tracking effect on non-straight paths.

[0048] (4) The secondary loop controller is the angle controller C. S (s), whose set value is θ FB With θ FF The difference between the sum of the values ​​and the wheel angle feedback A(s), where the reference inputs are the wheel angle feedback A(s) and the reference vehicle speed V. U (s), where the reference vehicle speed V U (s) Vehicle speed V sent by the host computer R(s) and vehicle speed collection V M (s) determines the reference vehicle speed according to the following formula:

[0049]

[0050] Angle Controller C S The output of (s) is the wheel steering angle setpoint θ, which is calculated using the following formula: Where K θ For the angle controller C S The proportionality coefficient (s). Because the accuracy of the travel speed obtained by the Beidou navigation system when the tractor is traveling at low speed is lower than that obtained by the vehicle speed detection module, when determining the vehicle speed, if the vehicle speed V collected by the vehicle speed detection module is lower... M (s) less than or equal to 2 m / s or the vehicle speed V received by the host computer R When (s) equals 0, the determined reference speed is V. M (s), this avoids speed errors caused by BeiDou navigation at low speeds or in case of malfunction; when the vehicle speed detection module collects the vehicle speed V M (s) If the speed is greater than 2 m / s or the vehicle speed detection module malfunctions, the reference vehicle speed is determined to be the vehicle speed V sent by the host computer. R (s). Using the vehicle speed signal as a parameter to determine the wheel steering angle setpoint can reduce system adjustment time;

[0051] (5) Reversing valve controller C V The setting value of (s) is the wheel angle setting value θ, which controls the hydraulic steering system to implement steering by outputting a voltage in the range of negative 10V to positive 10V through the reversing valve control module.

[0052] Compared with the prior art, the present invention selects not only the current tractor route deviation and current heading angle deviation when setting the control and reference quantities, but also the estimated path curvature and current driving speed of the tractor. The proposed tractor path tracking cascade control structure adopts a feedforward plus feedback control method in the main loop and a unity negative feedback method in the secondary loop. This control method not only ensures the path tracking effect when driving in a straight line, but also has high control accuracy when turning. Attached Figure Description

[0053] Figure 1 This is a schematic diagram of the principle of the present invention.

[0054] Figure 2 This is a schematic diagram of the control method used in this invention.

[0055] Figure 3 This is a circuit diagram of the reversing valve control module of the present invention.

[0056] Figure 4This is a circuit diagram of the voltage detection module of the present invention.

[0057] Figure 5 This is a circuit diagram of the vehicle speed detection module of the present invention.

[0058] Figure 6 This is a circuit diagram of the corner detection module of the present invention.

[0059] Figure labeling: 1 - Touch screen; 2 - Data storage module; 3 - Pressure detection module; 4 - Beidou positioning receiver module; 5 - Host computer; 6 - Alarm and emergency handling module; 7 - Slave computer; 8 - CAN communication module; 9 - Image acquisition module; 10 - Serial communication module; 11 - Reversing valve control module; 12 - Hydraulic steering system; 13 - Tractor; 14 - Position and posture detection module; 15 - Power supply module; 16 - Vehicle speed detection module; 17 - Wheel angle detection module; 18 - Voltage detection module. Detailed Implementation

[0060] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. The specific embodiments described herein are merely illustrative and not intended to limit the invention. This invention covers any substitutions, modifications, or equivalent methods and solutions made within the spirit and scope of the invention as defined by the claims. To provide a better understanding of this invention, specific details are described in detail in the specific embodiments; parts not described are conventional techniques in the art.

[0061] Combination Figure 1 , Figure 2 As can be seen, this embodiment of the invention provides a multi-sensor fusion tractor navigation control system, consisting of a host computer 5 and a slave computer 7. The host computer 5 and the slave computer 7 are interconnected through a serial communication module 10 to achieve precise positioning of the tractor 13 and accurate tracking of the set path. The host computer 5 is connected to a touch screen 1, a Beidou positioning receiver module 4, an image acquisition module 9, a pose detection module 14, and a serial communication module 10. The slave computer 7 is connected to a data storage module 2, a pressure detection module 3, a CAN communication module 8, a directional valve control module 11, a voltage detection module 18, a vehicle speed detection module 16, a wheel angle detection module 17, a power supply module 15, an alarm and emergency handling module 6, and a serial communication module 10. The directional valve control module 11 is electrically connected to the voltage detection module 18 and the hydraulic steering system 12, respectively, and the pressure detection module 3 is also electrically connected to the hydraulic steering system 12.

[0062] In this invention, the host computer 5 is an EPCS-8980 industrial control computer, which is the core component for processing vehicle location information and the center for human-machine dialogue in the entire navigation system. It works in conjunction with the touch screen 1 to complete functions such as setting field mode, configuring navigation parameters, downloading routes, setting navigation tasks, planning routes, and displaying vehicle trajectory in real time.

[0063] The touch screen 1 is connected to the host computer 5 through function pins. It serves as the human-machine interface for the tractor 13 during operation and can realize functions such as navigation mode selection, navigation parameter input, and vehicle trajectory display. This makes it convenient for operators to judge the working status of the tractor 13 navigation system. The present invention uses a four-wire resistive touch screen 1SV0804S-03.

[0064] The Beidou positioning receiver module 4 and the host computer 5 are connected to each other through a serial communication pin. This invention uses the UM220 module to receive Beidou satellite signals to determine the vehicle's location. Since it uses conventional Beidou receiving technology, it will not be described in detail here.

[0065] The image acquisition module 9 is connected to the host computer 5 via a serial communication pin. The OKAC1310CCD camera acquires crop images, which are then sent to the host computer 5 via a serial port after passing through a signal conditioning circuit. After grayscale transformation, image segmentation, and image denoising, the navigation baseline is extracted to determine the navigation path feature values.

[0066] The attitude detection module 14 is connected to the host computer 5 via a serial communication pin. The AHRS-3000 small attitude measurement module is selected to send the roll angle, pitch angle and heading angle of the tractor 13 in dynamic and static environments to the host computer 5 via the serial port. The host computer 5 uses these parameters to compensate for the Beidou positioning data, which can reduce or avoid the positioning error caused by the tilt of the vehicle body due to the influence of ground undulation.

[0067] The lower-level machine 7 uses a 16-bit Freescale microcontroller MC9S12XS128MAL, which is the core of the path tracking control of the tractor 13. It receives the position information of the tractor 13 sent by the upper-level machine 5 through the serial communication module 10, and after control calculation, the reversing valve control module 11 outputs a voltage signal to control the hydraulic steering system 12.

[0068] Data storage module 2 uses two 32MB SDRAM MT48LC4M32B2 chips, which are connected to the Freescale microcontroller via function pins to store navigation status information and control output information, facilitating later evaluation and analysis of the navigation system's performance.

[0069] The pressure detection module 3 is electrically connected to the lower-level machine 7 and the hydraulic steering system 12. It is used to determine the driving mode of the tractor 13. The pressure sensor MBS1250 installed on the navigation valve group detects the valve group pressure and converts it into a voltage signal, which is sent to the digital input port of the lower-level machine 7. The sensor outputs a low-pressure signal to indicate that the tractor 13 is in automatic navigation mode, and the sensor outputs a high-pressure signal to indicate that the tractor 13 is in manual driving mode.

[0070] The CAN communication module 8 consists of a TJA1043T chip and its peripheral circuitry. Through this module, the lower-level computer 7 can send the desired speed of the tractor 13, determined based on the estimated travel distance and the estimated path curvature, to the tractor 13 controller. This ensures effective control when the tractor 13 reaches its destination or turns, especially when the current path offset is O. R (s) Exceeds the maximum permissible route offset O M At that time, a stop signal is sent to the tractor 13 controller through this module.

[0071] Combination Figure 3 It is known that the directional valve control module 11 is electrically connected to the voltage detection module 18 and the hydraulic steering system 12, respectively. A D / A converter chip DAC0832 is used to output a digital value of 0-255. This digital value is amplified by amplifiers U18-U21 to generate a voltage within the range of +10V to -10V to control the directional solenoid valve, thereby controlling the hydraulic steering system 12. This invention uses a proportional directional valve of model 4WRA6E20-10B / 24Z4.

[0072] Combination Figure 4 It can be seen that the voltage detection module 18 consists of an operational amplifier and peripheral circuits. By designing the voltage detection module, the output voltage of the commutation valve control module 11 can be controlled in a closed loop. By detecting the output voltage in real time, the phenomenon of output voltage runaway can be avoided.

[0073] Combination Figure 5 It can be seen that the vehicle speed detection module 16 is connected to the interrupt input port of the lower-level machine 7. This module consists of a speed sensor and a conditioning circuit. The speed sensor is a VB-Z9400 Hall speed sensor, which outputs a square wave signal by sensing the raised teeth or recessed grooves on the magnetic conductor. The square wave signal is filtered by a hysteresis comparator circuit composed of a quarter LM339 and resistors R38, R39, and R40 to remove noise interference and is sent to the external interrupt pin of the lower-level machine 7. The lower-level machine 7 captures the vehicle speed signal through an external interrupt triggered by a falling edge.

[0074] Combination Figure 6It can be seen that the wheel angle detection module 17 is electrically connected to the lower-level machine 7. This module is used to complete the real-time detection of the wheel angle. The HPS-M1 linear displacement sensor is selected to convert the wheel angle signal into a voltage signal. After being filtered by resistor R13 and capacitor C11, the signal enters the inverting input terminal of amplifier U12. The non-inverting input terminal of amplifier U12 is grounded through resistor R15. The output terminal of U12 is connected to its inverting input terminal through resistor R14. At the same time, the output terminal of U12 is connected to the inverting input terminal of amplifier U13 through resistor R16. The non-inverting input terminal of amplifier U13 is grounded through resistor R18. The output terminal of U13 is connected to its inverting input terminal through resistor R17. It is also connected to the analog input port PA5 of the lower-level machine 7.

[0075] The power module 15 is electrically connected to the lower-level machine 7 and is powered by the vehicle battery. It generates a 24V surge protection voltage and a 5V voltage through the LT4356IS circuit and the LM7805 circuit, respectively. It generates a ±15V voltage through the MD20-12D15 module and a 10V reference voltage through the AD581. To ensure that the power supply voltage of the lower-level machine 7 system is not too high or too low, a power monitoring and protection circuit is designed.

[0076] The serial communication module 10 is connected to the host computer 5 and the slave computer 7 respectively. The module consists of the MAX232 serial communication chip and its peripheral circuits. Data exchange between the slave computer 7 and the host computer 5 can be completed through the serial communication module 10.

[0077] The alarm and emergency handling module 6 is electrically connected to the lower-level machine 7. When the hydraulic valve runs to its maximum or when the curve path is too curved, the lower-level machine 7 can control the relay to open, so that the pin connected to the buzzer and the light is at a high level, issue an alarm message, and control the output voltage to zero so that the reversing valve returns to the center position, and at the same time set the manual mode.

[0078] The host computer 5 sends the following data to the slave computer 7 for path tracking via the serial communication module 10: current route offset, current heading angle offset, estimated travel distance, current travel speed, and estimated path curvature.

[0079] The positioning method of the tractor navigation control system based on multi-sensor fusion, which positions the tractor 13 via a host computer 5, includes the following steps:

[0080] (1) Set the working field mode of tractor 13 via touch screen 1: no crop field, crop field;

[0081] (2) A dual-antenna Beidou receiver is used to obtain the absolute position, speed and heading angle information of the tractor 13 using the Beidou positioning receiver module 4;

[0082] (3) The pose detection module 14 is used to obtain the heading angle, roll angle and pitch angle data of the tractor 13, and the Beidou positioning information is corrected based on these data;

[0083] (4) If the field mode is set to a field without crops, the image acquisition module 9 is turned off and the machine vision positioning method is not used; if the field mode is set to a field with crops, the image acquisition module 9 is turned on, the acquired image data is processed, and a navigation baseline is generated and basic points representing the characteristics of crops are obtained.

[0084] (5) If the field mode is set to a field without crops, the Beidou positioning and path data corrected by the pose information are used. If the field mode is set to a field with crops, the fuzzy adaptive extended Kalman filter is used to fuse the Beidou positioning data and the machine vision data. The Beidou positioning provides the absolute position coordinates, heading angle and driving speed of the tractor 13, and the machine vision provides the relative position coordinates of known points in the navigation path. After the coordinates of the Beidou positioning system and the machine vision system are unified, the filter filters the position data provided by Beidou and the position data provided by machine vision to obtain accurate position data and path data.

[0085] Compared with the extended Kalman filter and unscented Kalman filter commonly used in current multi-sensor fusion technology, the fuzzy adaptive extended Kalman filter algorithm can reduce the correction position error and improve the accuracy and stability of the tractor 13 positioning data by using the ratio of the measurement innovation variance to the theoretical variance obtained in real time and by continuously adjusting the weighting coefficients of the measurement noise covariance matrix using a fuzzy adaptive controller.

[0086] A control method for a tractor navigation control system based on multi-sensor fusion, wherein the control method is path tracking control, includes the following steps:

[0087] (1) The path tracking control system is designed using a cascade control structure, in which the main loop adopts a feedforward plus feedback control method, and the secondary loop adopts a unity negative feedback method. The entire system structure is based on the offset setpoint O. S (s), Current route offset O R (s), Offset controller C O (s), Angle Controller C S (s), Reversing valve controller C V (s), voltage acquisition V M (s), Steering system G(s), Estimated path curvature PC R (s), Current heading angle offset A R (s), heading angle controller C A (s), wheel angle feedback A(s), vehicle speed V sent by host computer 5 R(s), Vehicle speed collection V M (s), Reference vehicle speed V U (s) composition;

[0088] (2) The feedback controller in the main loop is the offset controller C. O (s), whose setpoint input is the offset setpoint O S (s) and current route offset O R The difference between (s) and the offset controller C O The output of (s) is the steering angle θ. FB It is calculated using the following formula:

[0089]

[0090] Where ΔO=O S (s)-O R (s), K FB1 K is the proportional coefficient of the offset controller. FB2 K represents the differential coefficient of the offset controller. FB3 The second-order derivative coefficients of the offset controller are used. Since the direct control quantity for tractor 13 path tracking is the route offset, this invention reduces overshoot and overcomes oscillation by implementing proportional, derivative, and second-order derivative control on the offset difference. Furthermore, by setting a maximum allowable route offset O... M This allows the tractor 13 to stop steering when the deviation from the course exceeds the allowable range, and controls the output of the offset controller to be 0, thus avoiding damage to the hydraulic valve or safety accidents.

[0091] (3) The feedforward controller in the main loop is the heading angle controller C. A (s), whose reference input is the estimated path curvature PC R (s), Current heading angle offset A R (s) and current route offset O R (s), heading angle controller C A The output of (s) is the steering angle θ. FF It is calculated using the following formula:

[0092]

[0093] Where K FF1 K is the proportional coefficient for path curvature control of the heading angle controller. FF2 K is the proportional coefficient of the heading angle controller. FF3 This is the differential proportional coefficient for the heading angle controller. Within the allowable value of the course deviation O... PWithin the specified range, by using the heading angle as a reference, pre-control can be implemented to reduce fluctuations in the path tracking process; by using the estimated path curvature as a reference, pre-control can be achieved when driving on curves or turning, improving the tracking effect on non-straight paths.

[0094] (4) The secondary loop controller is the angle controller C. S (s), whose set value is θ FB With θ FF The difference between the sum of the values ​​and the wheel angle feedback A(s), where the reference inputs are the wheel angle feedback A(s) and the reference vehicle speed V. U (s), where the reference vehicle speed V U (s) Vehicle speed V sent by host computer 5 R (s) and vehicle speed collection V M (s) determines the reference vehicle speed according to the following formula:

[0095]

[0096] Angle Controller C S The output of (s) is the wheel steering angle setpoint θ, which is calculated using the following formula: Where K θ For the angle controller C S The proportionality coefficient (s). Because the accuracy of the travel speed obtained by the Beidou navigation system when the tractor 13 is traveling at low speed is lower than that obtained by the vehicle speed detection module 16, when determining the vehicle speed, if the vehicle speed V collected by the vehicle speed detection module 16 is lower, the accuracy of the travel speed V is lower. M (s) less than or equal to 2 m / s or the vehicle speed V received by the host computer 5 R When (s) equals 0, the determined reference speed is V. M (s), this avoids speed errors caused by Beidou navigation at low speeds or in case of malfunction; when the vehicle speed detection module 16 collects the vehicle speed V M (s) If the speed is greater than 2 m / s or the vehicle speed detection module 16 malfunctions, the reference vehicle speed is determined to be the vehicle speed V sent by the host computer 5. R (s). Using the vehicle speed signal as a parameter to determine the wheel steering angle setpoint can reduce system adjustment time.

[0097] (5) Reversing valve controller C V The setting value of (s) is the wheel angle setting value θ, which controls the hydraulic steering system 12 to implement steering by outputting a voltage in the range of negative 10V to positive 10V through the reversing valve control module 11.

[0098] Compared with existing technologies, this invention selects not only the current path deviation and current heading angle deviation of tractor 13, but also the estimated path curvature and current travel speed of tractor 13 when setting control and reference quantities. The proposed cascade control structure for tractor 13 path tracking adopts a feedforward plus feedback control method in the main loop and a unity negative feedback method in the secondary loop. This control method not only ensures the path tracking effect when traveling in a straight line, but also has high control accuracy when turning.

[0099] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A positioning method of a tractor navigation control system based on multi-sensor fusion, the navigation control system comprising an upper computer and a lower computer, the upper computer and the lower computer being connected to each other through a serial communication module, and together realizing accurate positioning of the tractor and accurate tracking of the set path; the upper computer being connected to a touch screen, a Beidou positioning receiving module, an image acquisition module, a pose detection module and a serial communication module; the lower computer being connected to a data storage module, a pressure detection module, a CAN communication module, a reversing valve control module, a voltage detection module, a vehicle speed detection module, a wheel rotation angle detection module, a power module, an alarm and emergency processing module and a serial communication module; the reversing valve control module being electrically connected to the voltage detection module and a hydraulic steering system respectively; the pressure detection module also being electrically connected to the hydraulic steering system, characterized in that: The positioning method, by the host computer, sends data for path tracking to the lower computer through the serial communication module, and the data includes: current route offset, current heading angle offset, estimated travel distance, current travel speed and estimated path curvature. The positioning method comprises the following steps: (1) setting the tractor working field mode through the touch screen: no crop field and crop field; (2) using a Beidou receiving device with double antennas to obtain the absolute position, travel speed and heading angle information of the tractor through a Beidou positioning receiving module; (3) The pose detection module is used to acquire the heading angle, roll angle and pitch angle data of the tractor, and the Beidou positioning information is corrected according to the data, so that the position data of the tractor is more accurate; the current course deviation of the tractor, the current heading angle deviation and the curvature value of the estimated path of the tractor are calculated and output by the host computer through Beidou navigation data or image data; the reference speed V U (s) of the tractor is determined by the host computer R (s) and the speed collection V M (s) determines, and is determined according to the following formula: ； Because the accuracy of the running speed obtained by the Beidou navigation is lower than the vehicle speed obtained by the vehicle speed detection module when the tractor is running at low speed, when determining the vehicle speed, if the vehicle speed V M (s) collected by the vehicle speed detection module is less than or equal to 2 m / s or the vehicle speed V R (s) received by the upper computer is 0, the determined reference vehicle speed is V M (s), so as to avoid the speed error caused by the Beidou navigation at low speed or failure; when the vehicle speed V M (s) collected by the vehicle speed detection module is greater than 2 m / s or the vehicle speed detection module fails, the reference vehicle speed is the vehicle speed V R (s) sent by the upper computer. (4) if the field mode is set as the no crop field, the image acquisition module is turned off, and the machine vision positioning mode is not used; if the field mode is set as the crop field, the image acquisition module is turned on, the collected image data is processed, a navigation reference line is generated, and basic points representing crop features are obtained; (5) if the field mode is set as the no crop field, the Beidou positioning and path data corrected by the pose information are completely used; if the field mode is set as the crop field, a fuzzy self-adaptive extended Kalman filter is used to fuse the Beidou positioning data and the machine vision data, wherein the Beidou positioning provides the absolute position coordinates, the heading angle and the travel speed of the tractor, the machine vision provides the relative position coordinates of the known points in the navigation path, and after the coordinates of the Beidou positioning system and the machine vision system are unified, the filter is used to filter the position data provided by the Beidou and the position data provided by the machine vision, to generate accurate tractor position data and path planning data.

Images