Field automatic row system of cane grabber and control method thereof

Abstract

The application discloses a kind of cane grabber field automatic alignment systems and control method thereof, system includes path storage unit, vehicle real-time pose detection system, core controller, steering actuator and man-machine interface.The method comprises: obtaining the three-dimensional space path information of sugarcane ridge row;During operation, the data of vehicle GNSS double-antenna positioning and orientation module, cab inertial measurement unit and chassis inertial measurement unit are fused, first use cab inertial measurement unit data to convert the main antenna coordinates to the center of chassis rotation by Euler coordinate conversion, then the coordinates and chassis inertial measurement unit data are fused by Kalman filtering, and the accurate position of chassis is estimated;Controller calculates lateral deviation and heading deviation, generates steering command through control algorithm, and drives steering actuator to realize accurate alignment.The application can realize centimeter-level navigation, is not affected by weather and light, can work at all times, protects the ratoons, reduces labor intensity and improves work efficiency.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of sugarcane grabbers, and particularly relates to a field automatic alignment system of a sugarcane grabber and a control method thereof. BACKGROUND

[0002] As an important economic crop in southern China, sugarcane is widely planted in a new three or even four host planting system, and the yield stability of ratoon cane directly affects the economic benefits of the sugarcane industry. In the sugarcane harvesting process, the sugarcane grabber is the core equipment for realizing the picking, transporting and loading of sugarcane, which greatly reduces the labor intensity of sugarcane farmers and significantly improves the harvesting efficiency, and has been widely popularized in the main sugarcane producing areas in China.

[0003] However, the current field operation of the sugarcane grabber still faces key technical bottlenecks, which seriously restricts the high-quality development of the sugarcane industry. On the one hand, during the sugarcane harvesting stage, the stems are laid down in the field after being cut into the soil, and the sugarcane ridge is covered with scattered sugarcane leaves, making it difficult for the driver to accurately determine the actual position of the sugarcane ridge. When the sugarcane grabber operates back and forth between the sugarcane pile and the transport vehicle, it frequently rolls over the ratoon cane, causing a significant loss in ratoon cane yield the following year, which has long plagued the sugarcane industry in China. On the other hand, the traditional sugarcane grabber operation completely relies on manual control by the driver, which not only has high labor intensity and low efficiency, but also is prone to unstable operation and fatigue effects, resulting in poor straightness of the operation, further exacerbating the damage to the ratoon, and at the same time, puts high demands on the operation skills of the driver.

[0004] In terms of automatic alignment navigation technology application, the real-time sensing technologies commonly used in existing agricultural machinery, such as machine vision, laser radar or mechanical touch rod, are completely ineffective due to the problem of sugarcane ridge being covered and blocked by sugarcane leaves after harvesting, and cannot meet the alignment operation requirements of the sugarcane grabber. The positions of the crop rows and sugarcane ridges formed during sugarcane planting remain fixed throughout the growth cycle, which provides a feasible basis for automatic alignment solutions based on positioning technology. The Beidou Satellite Positioning System (BDS) has been applied in the field of agricultural machinery navigation due to its high-precision positioning capability, but the unique multi-body structure of the sugarcane grabber poses new challenges to the adaptation of this technology: the combination of the driver's cabin, mechanical arm and engine box can freely rotate around the center axis of the body, forming a separable multi-body structure with the running chassis, which is significantly different from the integrated structure of traditional agricultural machinery, making the installation position selection and pose detection method of GNSS antenna and inertial sensor a technical problem.

[0005] In addition, the existing agricultural machinery navigation technology is not adapted and optimized for the multi-body structure characteristics of the sugarcane grabbing machine, and it is difficult to realize accurate matching of the "perceived position" and the "actual motion center of the vehicle". When working on undulating terrain, the change of the vehicle attitude will introduce positioning errors, affecting the rowing accuracy. Therefore, developing a technical solution that adapts to the structural characteristics of the sugarcane grabbing machine, is not affected by environmental light, and can realize high-precision automatic rowing, to avoid stubble rolling and improve operation quality and efficiency, has become a key problem to be solved in the current sugarcane harvesting mechanization field. SUMMARY

[0006] The main purpose of the present application is to overcome the shortcomings and deficiencies of the prior art, and to provide a sugarcane grabbing machine automatic rowing system in the field and a control method thereof, which realizes automatic rowing of the sugarcane grabbing machine based on high-precision GNSS and dual-IMU inertial navigation fusion.

[0007] In order to achieve the above-mentioned purpose, the following technical solutions are adopted in the present application:

[0008] In a first aspect, the present application provides a sugarcane grabbing machine automatic rowing system in the field, comprising the following parts:

[0009] A path storage unit is used to pre-store a three-dimensional space path of a sugarcane ridge row;

[0010] A pose detection unit includes a GNSS dual-antenna positioning and orientation module, a cab inertial measurement unit and a chassis inertial measurement unit. The GNSS dual-antenna positioning and orientation module and the cab inertial measurement unit are fixedly installed on the top of the cab of the sugarcane grabbing machine, and the chassis inertial measurement unit is installed on the chassis of the sugarcane grabbing machine. The pose detection unit is used to obtain the original sensing data of the sugarcane grabbing machine. The GNSS dual-antenna positioning and orientation module includes a master GNSS antenna and a slave GNSS antenna.

[0011] A core controller is configured to perform the following operations:

[0012] (1) reading the pre-stored three-dimensional space path of the sugarcane ridge row in the path storage unit;

[0013] (2) receiving the data output by the pose detection unit;

[0014] (3) based on the cab attitude calculated from the data of the GNSS dual-antenna positioning and orientation module and the cab inertial measurement unit, the coordinates of the master GNSS antenna are converted to the center of the chassis rotation of the sugarcane grabbing machine in real time through the Euler coordinate conversion model, and the center of the chassis rotation is obtained. coordinates;

[0015] (4) fuse the converted chassis rotation center coordinates with the data of the chassis inertial measurement unit, and estimate the three-dimensional pose of the chassis through the Kalman filter;

[0016] (5) Project the estimated three-dimensional pose of the chassis to the horizontal plane with the pre-stored three-dimensional space path, and calculate the lateral deviation and the heading deviation, the lateral deviation is the vertical distance from the current control point of the sugarcane harvester to the connecting line of the pre-stored three-dimensional space path and the preview point, and the heading deviation is the angle between the current heading angle of the vehicle and the tangent direction of the three-dimensional space path at the preview point;

[0017] (6) Based on the lateral deviation and the heading deviation, generate a steering control instruction through a path tracking control algorithm;

[0018] A steering actuator for receiving the steering control instruction output by the core controller and driving the wheels to steer;

[0019] A human-computer interaction interface for communicating with the core controller through Ethernet or CAN bus, displaying the real-time pose of the vehicle, the pre-stored three-dimensional space path, the tracking deviation, and the system status, supporting the selection of working rows and the start and pause control of automatic row-to-row operation.

[0020] As a preferred technical solution, the three-dimensional space path comprises:

[0021] The sugarcane Beidou navigation planting record is used as the operation path information;

[0022] The operation path information is obtained by aerial photography and surveying with a UAV in the seedling stage of the sugarcane field;

[0023] The operation path information is obtained based on the method of artificial field calibration.

[0024] As a preferred technical solution, the GNSS double-antenna positioning and orientation module is fixedly installed on the longitudinal axis of the cab roof of the sugarcane harvester with a preset baseline distance, and the carrier phase difference of the satellite signals received by the master GNSS antenna and the slave GNSS antenna is calculated to obtain the three-dimensional position coordinates in centimeters, and the true heading angle of the vehicle is calculated.

[0025] As a preferred technical solution, the cab inertial measurement unit includes a first three-axis gyroscope and a first three-axis accelerometer for measuring the angular velocity and acceleration of the cab, respectively; and the chassis inertial measurement unit includes a second three-axis gyroscope and a second three-axis accelerometer for independently measuring the angular velocity and acceleration of the chassis body.

[0026] As a preferred technical solution, the Euler coordinate conversion is specifically:

[0027] A three-dimensional rotation matrix or quaternion transformation is constructed based on the heading angle, pitch angle and roll angle of the vehicle calculated from the dual-antenna GNSS positioning and orientation module and the chassis inertial measurement unit data, the fixed three-dimensional offset vector between the main GNSS antenna and the vehicle chassis rotation center is rotated and transformed, and the three-dimensional coordinates of the main GNSS antenna are converted to the vehicle chassis rotation center by subtracting the rotated three-dimensional offset vector from the three-dimensional coordinates of the main GNSS antenna.

[0028] As a preferred technical solution, the state vector of the Kalman filter is the planar coordinates of the chassis navigation control point position and the Z-axis gyroscope heading angle deviation of the chassis inertial measurement unit; the measurement vector is the planar coordinates of the chassis rotation center; the state transition equation is constructed based on the principle of dead reckoning of position and speed; the measurement equation is constructed based on the coordinate transformation formula between the vehicle coordinate system and the geodetic navigation coordinate system; the state transition matrix and the measurement matrix are updated in real time according to the original roll angle, pitch angle, forward speed of the chassis and the heading angle accumulated by the yaw rate.

[0029] As a preferred technical solution, the control rate of the path tracking control algorithm is:

[0030] ;

[0031] wherein, wherein K is the gain coefficient, v is the speed of the cane grabbing machine, e d is the lateral deviation, e θ is the heading deviation.

[0032] As a preferred technical solution, the steering actuator is a hydraulic steering system integrated with an electro-hydraulic servo valve or an independent electric power steering machine.

[0033] In a second aspect, the present application provides a cane grabbing machine automatic row control method in the field, which is applied to the automatic row control system in the field of the cane grabbing machine and includes the following steps:

[0034] S1: Before operation, load the pre-stored three-dimensional space path of the sugarcane ridge row through the core controller;

[0035] S2: Start the pose detection unit, obtain the three-dimensional coordinates of the main GNSS antenna and the vehicle heading angle in real time through the GNSS dual-antenna module, obtain the angular velocity and acceleration information of the cab in real time through the cab inertial measurement unit, and obtain the angular velocity and acceleration information of the chassis in real time through the chassis inertial measurement unit;

[0036] S3: The core controller calculates the cab attitude based on the cab inertial measurement unit data, and through an Euler coordinate conversion model, rotates and converts the fixed three-dimensional offset vector between the main GNSS antenna and the vehicle chassis rotation center, which is pre-measured, to convert the coordinates of the main GNSS antenna to the vehicle chassis rotation center to obtain the chassis rotation center coordinates;

[0037] S4: The core controller fuses the chassis rotation center coordinates obtained in step S3 with the data of the chassis inertial measurement unit, and estimates the three-dimensional pose of the chassis through a Kalman filter;

[0038] S5: Project the three-dimensional pose of the chassis and the pre-stored three-dimensional space path onto the horizontal plane to calculate the lateral deviation and the heading deviation;

[0039] S6: Based on the lateral deviation and the heading deviation, calculate the steering control amount through the path tracking control algorithm, and the calculation of the steering control amount satisfies the formula: Where K is the gain coefficient, e d is the lateral deviation, v is the vehicle speed, e θ is the heading deviation;

[0040] S7: The core controller converts the steering control amount into a control signal and sends it to the steering actuator to drive the wheels to deflect by a corresponding angle;

[0041] S8: Return to step S2, continuously collect pose data and repeat the above steps S2-S7 to form a closed loop control, and realize the automatic driving operation of the cane grabbing machine along the pre-stored ridge center line.

[0042] As a preferred technical solution, when the GNSS signal is short-time locked, the core controller calculates the inertial navigation pose through the data of the chassis inertial measurement unit to maintain the continuity and smoothness of the control until the GNSS signal is restored.

[0043] Compared with the prior art, the present application has the following advantages and beneficial effects:

[0044] (1) The present application adopts the innovative mode of "three-dimensional space path + GNSS / IMU fusion navigation", which skillfully avoids the industry problem of real-time perception failure caused by the shading of sugarcane ridge by sugarcane leaves during the harvesting period. The operation path information is obtained by using the Beidou navigation planting record of sugarcane or the aerial surveying and mapping of unmanned aerial vehicle at the seedling stage or the method based on artificial field calibration, which ensures the absolute authenticity of the navigation reference and guarantees the row accuracy from the source, overcoming the problem of possible misalignment of the pre-stored path. The problem of the application of traditional navigation technologies such as machine vision and laser radar in this scene is solved from the root, providing a stable and reliable navigation foundation for the automatic rowing of the cane grabbing machine.

[0045] (2) The application is directed to the multi-body structure characteristics of the cane grabbing machine cab and chassis that can be separated, and innovatively proposes a two-step pose estimation strategy of "coordinate conversion decoupling first, and then combination navigation fusion". In the first step, the cab antenna coordinates are rigidly converted to the chassis rotation center through Euler coordinate conversion, which fundamentally isolates the error introduced by the cab rotation; in the second step, the converted coordinates and chassis IMU data are fused through EKF to obtain the pure chassis motion state. The technical bottleneck of poor adaptability of traditional navigation technology on multi-body agricultural machinery is solved.

[0046] (3) The application adopts a technical scheme of GNSS double-antenna positioning and orientation module, deep fusion of cab inertial measurement unit (IMU1) and chassis inertial measurement unit (IMU2), and realizes data complementation through Kalman filtering algorithm. When the GNSS signal is short-time locked (such as passing under a tree or under a high-voltage line), IMU2 can continuously provide high-frequency pose data through inertial navigation algorithm, ensuring the continuity and smoothness of control. The system of the patent is completely not affected by environmental factors such as day and night, weather, and illumination, and can realize reliable operation for 24 hours in all time periods and all seasons, breaking through the operation restrictions of traditional manual operation and visual navigation.

[0047] (4) The application selects mature and efficient path tracking algorithms such as pure tracking algorithm or Stanley controller, and combines the cooperative control logic of lateral deviation and heading deviation to ensure that the cane grabbing machine travels smoothly and stably along the preset path. The core algorithm calculation formula is simple and fast in response (data update frequency is up to 50-100Hz), which can be directly adapted to the steering actuator of the existing cane grabbing machine, without the need for large-scale modification of the equipment, and is convenient for technical popularization and application. BRIEF DESCRIPTION OF DRAWINGS

[0048] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0049] Figure 1 is a structural schematic diagram of the field automatic alignment control system of the cane grabbing machine of the embodiment of the present application;

[0050] Figure 2 is a principle schematic diagram of coordinate conversion to the vehicle chassis rotation center in the embodiment of the present application:

[0051] Figure 3 is a flowchart of the field automatic alignment control method of the cane grabbing machine of the embodiment of the present application;

[0052] Figure 4The figure shows the installation position of the main equipment of the system in the cane grasping machine in the embodiment of the application.

[0053] Figure 5 The figure is a raw error sampling curve diagram of automatic alignment experiment, wherein the upper sub-diagram is a lateral deviation curve and the lower sub-diagram is a heading deviation curve.

[0054] Explanation of reference numerals:

[0055] 1-main GNSS antenna; 2-slave GNSS antenna; 3-cab inertial measurement unit; 4-chassis inertial measurement unit; 5-core controller; 6-electric steering wheel. DETAILED DESCRIPTION

[0056] In order for those skilled in the art to better understand the scheme of the present application, the technical scheme in the embodiments of the present application will be described clearly and completely below in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0057] In the present application, the phrase "embodiment" means that the specific features, structures or characteristics described in combination with the embodiment can be contained in at least one embodiment of the present application. The appearance of this phrase at various places in the specification does not necessarily mean the same embodiment, nor is it an independent or alternative embodiment to other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described in the present application can be combined with other embodiments.

[0058] Explanation of terms:

[0059] IMU (Inertial Measurement Unit) represents an inertial measurement unit;

[0060] IMU1 as a whole represents a cab inertial measurement unit;

[0061] IMU2 as a whole represents a chassis inertial measurement unit;

[0062] GNSS (Global Navigation Satellite System) represents a global navigation satellite system;

[0063] RTK (Real Time Kinematic) represents real-time dynamic differential positioning technology.

[0064] The embodiment provides a sugarcane grabbing machine field automatic alignment system. The system first adopts a sugarcane Beidou navigation planting record or a sugarcane field unmanned aerial vehicle aerial surveying and mapping in the seedling stage or a method based on artificial field calibration to obtain operation path information, and stores AB reference lines of all ridges and rows of an entire sugarcane field. When the sugarcane grabbing machine operates, the system performs two-step pose solving: first, coordinate decoupling: by using a GNSS double-antenna positioning and orientation module at the top of a cab and a cab inertial measurement unit (IMU1), three-dimensional coordinates of a phase center of a main GNSS antenna are converted to a center of a vehicle chassis in real time and accurately through an Euler coordinate conversion model. The core function of this step is to "anchor" a perception position to a geometric point fixed to the chassis from a rotatable cab, and eliminate all influences of the motion of the cab. Second, fusion estimation: the chassis center coordinates obtained in the first step, which have been stripped of the influence of the cab, are combined with data of a chassis inertial measurement unit (IMU2), data fusion and state estimation are performed through a Kalman filtering algorithm, and finally a high-precision three-dimensional pose of the vehicle chassis for control is obtained. The controller compares the pose with a pre-stored target ridge and row three-dimensional space path, calculates a deviation, and generates a control instruction, so that automatic alignment is realized.

[0065] As shown in Figure 1 The sugarcane grabbing machine field automatic alignment system of the embodiment comprises a path storage unit, a pose detection unit, a core controller, a steering execution mechanism and a man-machine interaction interface. The path storage unit is used for storing three-dimensional space path information of sugarcane ridges and rows. The pose detection unit comprises a GNSS double-antenna positioning and orientation module, a cab inertial measurement unit (IMU1) and a chassis inertial measurement unit (IMU2), and is used for acquiring multi-source sensing data of a vehicle in real time. The core controller adopts an industrial-grade embedded computer or a high-performance automatic driving domain controller. The steering execution mechanism is used for receiving a steering control instruction output by the core controller and driving a wheel to steer. The man-machine interaction interface is used for displaying real-time pose of the vehicle, a pre-stored path, a tracking deviation and a system state, and supporting selection of an operation ridge and row and starting and pausing control of automatic alignment operation. In the embodiment, the GNSS double-antenna positioning and orientation module, the IMU1 and the IMU2 send data to the core controller through a CAN bus or a serial port, the core controller controls the steering execution mechanism through a CAN bus or an analog output port, and the man-machine interaction interface communicates with the core controller through Ethernet or CAN.

[0066] Further, the data sources of the path storage unit mainly include three types: one is to extract the historical trajectory record of sugarcane Beidou navigation planting; the second is to use a UAV to take aerial photographs and survey the crop row position in the sugarcane field at the seedling stage; and the third is field calibration, that is, selecting a sugarcane field with Beidou navigation planting, straight planting end and uniform row, after the sugarcane is harvested and before the sugarcane grabbing machine is operated, an operator holds a high-precision RTK-GNSS device and walks along the residual sugarcane roots in the field, and at the starting point A and the ending point B of each sugarcane ridge, the operator performs dotting operation to record a plurality of latitude and longitude coordinate points with time stamps, thereby forming the AB reference line of each ridge row. The AB reference line of each ridge row (i.e., the "reference path library") is stored in the solid state disk or the SD card of the vehicle-mounted controller. In this embodiment, the data source of the path storage unit is further described by taking the AB reference line as an example.

[0067] Further, the pose detection unit includes a GNSS dual-antenna positioning and orientation module, a cab inertial measurement unit (IMU1), and a chassis inertial measurement unit (IMU2).

[0068] The GNSS dual-antenna positioning and orientation module, as the main pose sensor of the system, includes two high-precision GNSS receiving antennas (a master GNSS antenna and a slave GNSS antenna) fixedly installed on the longitudinal axis of the roof of the cab of the sugarcane grabbing machine at a certain baseline distance (usually set to 1-2 meters). By solving the phase difference of the satellite signal received by the two antennas, a centimeter-level plane position (X, Y coordinates) is provided, and the high-precision heading angle of the vehicle can also be solved in real time, which is the key to realizing precise path tracking.

[0069] Further, the cab inertial measurement unit (IMU1) includes a first three-axis gyroscope and a first three-axis accelerometer, which are used to measure the angular velocity and acceleration of the cab body and provide the necessary attitude information of the cab for the first coordinate transformation.

[0070] Further, the chassis inertial measurement unit (IMU2) includes a second three-axis gyroscope and a second three-axis accelerometer, which are fixedly installed on the chassis frame of the sugarcane grabbing machine and are used to independently measure the angular velocity and acceleration of the chassis body, thereby providing the process update data source for the second step of Kalman filter fusion.

[0071] Further, the core controller is configured to perform the following operations:

[0072] (1) reading the pre-stored three-dimensional space path of the sugarcane ridge row in the path storage unit;

[0073] (2) receiving the GNSS, IMU1 data and IMU2 data output by the pose detection unit;

[0074] (3) Perform coordinate decoupling operation: based on the GNSS dual-antenna positioning and orientation module and the IMU1 data, the heading angle, pitch angle and roll angle of the cab are calculated, and through the Euler coordinate conversion model, the coordinates of the main GNSS antenna are converted to the center of the sugarcane harvester chassis in real time, and the coordinates of the center of the chassis are obtained. The output of this operation is a coordinate representing a fixed geometric point on the chassis, which has eliminated the influence of the rotation of the cab;

[0075] (4) Perform fusion navigation operation: the coordinates of the center of the chassis output in (3) are fused with the data of IMU2, and through the Kalman filter, the accurate three-dimensional pose of the vehicle chassis is estimated.

[0076] (5) Based on the estimated three-dimensional pose of the chassis, the control reference point for path tracking is determined according to the vehicle kinematic model, the control reference point is projected to the horizontal plane according to the pre-stored three-dimensional space path, and the lateral deviation and heading deviation are calculated. The lateral deviation is the perpendicular distance from the control reference point to the line connecting the pre-stored path preview points, and the heading deviation is the angle between the current heading angle of the vehicle and the tangent direction of the pre-stored path at the preview point.

[0077] (6) Based on the lateral deviation and heading deviation, the steering control instruction is generated through the path tracking control algorithm (such as pure pursuit algorithm, Stanley controller, etc.).

[0078] Further, the steering actuator can be a hydraulic steering system integrated with an electro-hydraulic servo valve or an independent electric power steering machine, which is responsible for receiving the electrical signal instruction of the controller and accurately driving the wheel steering.

[0079] Further, the human-machine interaction interface is a touch screen display, which is used to display the real-time pose of the vehicle, the pre-stored path, the tracking deviation, the system state, etc. The driver can select the working ridge line and start / pause the automatic alignment operation on the working ridge line on the interface.

[0080] As shown in Figure 3 , the embodiment provides a sugarcane harvester automatic alignment control method in the field, which comprises the following steps:

[0081] S1, system initialization and path loading: before operation, the driver selects the number of the sugarcane ridge line to be operated from the human-machine interaction interface, and the system loads the pre-set path coordinate sequence of the ridge line from the "reference path library" into the controller memory.

[0082] S2, real-time vehicle pose calculation: after the system is started, the GNSS dual-antenna positioning and orientation module continuously outputs the three-dimensional coordinates (X, Y, Z) and the heading angle (θ) of the main GNSS antenna. The cab inertial measurement unit (IMU1) and the chassis inertial measurement unit (IMU2) work in parallel, and output the high-frequency angular velocity and acceleration information of the cab and the chassis, respectively.

[0083] S3, Coordinate conversion to vehicle chassis rotation center:

[0084] Objective: Eliminate the influence of cab rotation on positioning reference, unify the perception point to the geometric point fixed to the chassis.

[0085] Parameter measurement: When the system is installed, the three-dimensional offset vector (a1, b1, h1) of the main GNSS antenna phase center relative to the vehicle chassis rotation center needs to be accurately measured. This vector is a fixed value in the vehicle coordinate system (origin at the rotation center).

[0086] Conversion calculation: As shown in Figure 2 , a1 represents the offset of the antenna phase center in the vehicle longitudinal axis direction relative to the rotation center, b1 represents the offset in the transverse axis direction, and h1 represents the offset of the antenna phase center in the vehicle Z-axis direction relative to the rotation center. The core controller constructs a three-dimensional rotation matrix R according to the real-time heading angle (ψ1), roll angle (φ1), and pitch angle (θ1) of the cab calculated from the dual-antenna and IMU1 data. This is achieved through the following formula:

[0087] ;

[0088] The real-time coordinates of the chassis rotation center in the geographical coordinate system P g (x g , y g ,z g ) are calculated.

[0089] Output: After this step, a coordinate (x g , y g ,z g ) that has been stripped of the influence of cab motion is obtained, which is one of the inputs for subsequent fusion navigation, but it is not directly used as the final control pose.

[0090] S4, Multi-sensor fusion and chassis pose estimation;

[0091] Objective: Based on stable position observations (S3 output) and chassis inertial data (IMU2), estimate the complete motion state of the chassis.

[0092] Algorithm process: The core controller runs a Kalman filter. The state vector is the planar coordinates of the chassis navigation control point (chassis rear axle center point) position and the Z-axis gyroscopic heading angle deviation of IMU2 ; the measurement vector is the planar coordinates of the chassis rotation center .The state transition equation is constructed based on the principle of dead reckoning of position and speed; the measurement equation is constructed based on the coordinate conversion formula of the vehicle body coordinate system and the geodetic navigation coordinate system; the state transition matrix and the measurement matrix are updated in real time according to the original roll angle, pitch angle, forward speed of the chassis and the heading angle obtained by accumulating the yaw rate according to steps S2 and S3.

[0093] Output: The Kalman filter outputs the optimal estimation of the three-dimensional pose of the vehicle chassis control point, including the position (x c ,y c , z c ), the heading angle (ψ), the roll angle (φ) and the pitch angle (θ). This “chassis pose” is the only correct reference for path tracking control.

[0094] The state transition equation of the Kalman filter is expressed as:

[0095] ;

[0096] wherein, , represents a state space vector;

[0097] is a state transition matrix, which is updated in real time by the gyroscopic cumulative measurement value of the heading angle;

[0098] , , is the speed of the sugarcane grabbing machine chassis motion, which is measured by GNSS; is a white noise sequence of the state transition equation; the system process noise covariance matrix is , which is used to represent the error size of the state transition equation, and in the present application, is set as a constant matrix, and the matrix parameters are adjusted during simulation and experiment.

[0099] The measurement equation of the Kalman filter is as follows:

[0100] ;

[0101] wherein, , , , ;

[0102] ; ;

[0103] ;

[0104] ;

[0105] a represents the offset of the antenna phase center from the chassis rotation center in the longitudinal direction relative to the rear axle center point, b represents the offset in the lateral direction, and h represents the offset of the chassis rotation center in the Z-axis direction relative to the rear axle center point. , , These are the original roll angle, pitch angle, and heading angle obtained by accumulating the yaw rate of the chassis IMU2, respectively. , This represents the random positioning error of the chassis rotation center in the horizontal coordinate system.

[0106] The noise variance matrix of the measurement vector is:

[0107] ;

[0108] in, , They are respectively , The variance statistics.

[0109] S5. Path Tracking and Deviation Calculation: The path tracking controller searches for a "pre-aiming point" on the pre-stored path based on the chassis control point pose (X_base, Y_base, θ_base). Then, it calculates the lateral deviation e. d and heading deviation e θ :

[0110] Lateral deviation e d The vertical distance between the vehicle's current control point and the target aiming point. This reflects the vehicle's lateral offset relative to the target path.

[0111] Heading deviation e θ The angle between the vehicle's current heading angle and the tangent to the target path at the aiming point. It reflects the deviation between the vehicle's direction and the path direction.

[0112] S6. Steering control quantity calculation: The controller will calculate the lateral deviation e d and heading deviation e θ As input, a path tracking control algorithm (such as the Stanley controller) is used for calculation. The control law of this algorithm is typically:

[0113] (Where K is the gain coefficient and v is the vehicle speed).

[0114] This formula can simultaneously eliminate lateral position deviation and adjust heading, enabling the vehicle to converge smoothly and stably onto the target path.

[0115] S7, steering execution: the calculated theoretical steering angle is converted into a specific control signal (such as PWM duty cycle), sent to the steering execution mechanism, and the execution mechanism drives the steering wheel of the sugarcane harvester to deflect by a corresponding angle.

[0116] S8, closed-loop feedback and continuous tracking: after steering, the sugarcane harvester enters a new pose state. The sensor system immediately senses this change and feeds back the new pose data to the controller. The controller again performs steps S2 to S7, calculates the new deviation, and issues a new steering instruction. This cycle continues, forming a high-frequency closed-loop control system that dynamically guides the sugarcane harvester to "bite" the preset AB reference line tightly until the row operation is completed.

[0117] To verify the actual operation effect of the system and the robustness of the algorithm, a field test was conducted at a sugarcane planting base in Zhanjiang, Guangdong.

[0118] The test used a certain type of wheeled sugarcane harvester from Jihe Heavy Industry as the controlled object, which had a multi-body structure feature with a rotatable cab. The experimental platform and parameter calibration test used a wheeled sugarcane harvester with a rotatable cab structure. The system hardware installation is shown in Figure 4 : The main GNSS antenna 1, the slave GNSS antenna 2, and the cab inertial measurement unit 3 are fixed to the top of the rotatable cab, used to capture the absolute pose of the cab; the chassis inertial measurement unit 4 is fixedly installed on the center of the chassis, used to sense the motion state of the chassis; the core controller 5 and the electric steering wheel 6 are set in the cab. After starting, the system first performs static initialization, and the core controller reads the pre-stored three-dimensional offset vector (Δx, Δy, Δz) of the main GNSS antenna relative to the center of the chassis rotation. The three-dimensional offset vector is obtained by manually measuring multiple times and taking the average value, ensuring the uniqueness of the physical reference of the coordinate transformation model (as described in step S3).

[0119] Test environment: The test environment is scattered and broken sugarcane leaves with a thickness of about 5-10 cm on the ground surface, making it difficult for traditional vision algorithms to identify the ridge feature. Before the test, the AB reference line of the sugarcane ridge was calibrated by an RTK device to construct a digital path library in the farmland. During operation, the controller compares the decoupled chassis pose with the AB reference line in real time by performing steps S4 to S6.

[0120] This embodiment statistically analyzes the full data of 5 groups of operation sections, and the statistical data of each experimental group is shown in Table 1:

[0121] Table 1: Error statistics table of 5 groups of field tests of the automatic row alignment system

[0122] Experiment group number Lateral root mean square error (m) Lateral maximum deviation (m) Course root mean square error (°) Course mean absolute error (°) Experiment group 1 0.046 0.145 1.317 0.994 Experiment group 2 0.035 0.097 0.965 0.790 Experiment group 3 0.033 0.062 1.006 0.823 Experiment group 4 0.036 0.097 0.971 0.788 Experiment group 5 0.050 0.108 1.324 1.146 Average value 0.040 0.102 1.117 0.908

[0123] According to Table 1 and the real-time error curve shown in Figure 5 :

[0124] In the case of frequent rotation of the cab with the grab cane action, the lateral root mean square error (RMSE) of the 5 groups of experiments is only 0.040 m. The lateral root mean square error is 0.040 m, and the maximum deviation is ≤0.145 m, which verifies the effectiveness of the coordinate conversion decoupling strategy;

[0125] The average of the heading root mean square error is 1.117°, which shows that the system heading tracking is stable;

[0126] The error curve is smooth, and there is no obvious oscillation, indicating that the EKF filter has good noise suppression for the inertial sensor;

[0127] The results of multiple tests are consistent, and the system still maintains stable operation in the complex cane leaf coverage environment.

[0128] Aiming at the core problem that the "perceived position" and the "control position" are not unified due to the multi-body structure of the cane grabber, the solving idea of the present application is to perform kinematic decoupling. Specifically, by installing a GNSS antenna on the top of the cab and a cab inertial measurement unit (IMU1), the pose of the cab is perceived in real time; by using these information, through strict three-dimensional rigid body geometric transformation, the coordinates of the GNSS antenna are converted to the mechanical center of rotation fixed to the chassis. After this step, no matter how the cab rotates, the obtained center of rotation coordinates have been stripped of the influence of the relative motion of the cab. Subsequently, the center of rotation coordinates are regarded as a stable position observation source, together with the chassis inertial measurement unit (IMU2) data directly installed on the chassis, input into the standard combined navigation filtering algorithm, and finally the high-precision pose reflecting the motion state of the chassis itself is obtained, thereby providing the correct reference for precise path tracking control.

[0129] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program. The program can be stored in a non-volatile computer readable storage medium, and when the program is executed, the processes of the above-mentioned embodiment methods can be included. Any reference to memory, storage, database or other medium used in the embodiments provided in the present application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration but not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0130] Any combination of the technical features of the above embodiments can be made. In order to make the description simple, all possible combinations of the technical features in the above embodiments are not described, however, as long as the combination of the technical features does not exist, it should be considered as the scope of the present application.

[0131] The above embodiments are the preferred embodiments of the present application, but the embodiments of the present application are not limited to the above embodiments, and any changes, modifications, substitutions, combinations and simplifications of the embodiments of the present application without departing from the spirit and principles of the present application are equivalent replacement methods, and are included in the protection scope of the present application.

Claims

1. A field auto-rowing system for a cane harvester, characterised in that, The method comprises the following parts: A path storage unit is used to pre-store a sugarcane row three-dimensional space path; A pose detection unit comprises a GNSS double-antenna positioning and orientation module, a cab inertial measurement unit and a chassis inertial measurement unit, the GNSS double-antenna positioning and orientation module and the cab inertial measurement unit are fixedly installed on the top of the cab of the sugarcane harvester, and the chassis inertial measurement unit is installed on the chassis of the sugarcane harvester; the pose detection unit is used to obtain original sensing data of the sugarcane harvester; the GNSS double-antenna positioning and orientation module comprises a master GNSS antenna and a slave GNSS antenna; A core controller is configured to perform the following operations: (1) reading the pre-stored sugarcane row three-dimensional space path in the path storage unit; (2) receiving data output by the pose detection unit; (3) based on the attitude of the cab calculated from the data of the GNSS double-antenna positioning and orientation module and the cab inertial measurement unit, the coordinates of the master GNSS antenna are converted to the center of the chassis rotation in real time through an Euler coordinate conversion model, and the coordinates of the center of the chassis rotation are obtained; (4) the converted coordinates of the center of the chassis rotation are fused with the data of the chassis inertial measurement unit, and the three-dimensional pose of the chassis is estimated through a Kalman filter; (5) the estimated three-dimensional pose of the chassis is projected to a horizontal plane together with the pre-stored three-dimensional space path, and a lateral deviation and a heading deviation are calculated, the lateral deviation is the perpendicular distance from the current control point of the sugarcane harvester to the connecting line of the pre-stored three-dimensional space path preview point, and the heading deviation is the included angle between the current heading angle of the vehicle and the tangent direction of the three-dimensional space path at the preview point; (6) based on the lateral deviation and the heading deviation, a steering control instruction is generated through a path tracking control algorithm; A steering execution mechanism is used to receive the steering control instruction output by the core controller and drive the wheels to steer; A human-computer interaction interface communicates with the core controller through Ethernet or CAN bus, and is used to display the real-time pose of the vehicle, the pre-stored three-dimensional space path, the tracking deviation and the system state, and support the selection of the working row, the start and pause control of the automatic row operation.

2. The harvester field auto-rowing system according to claim 1, characterized in that, The three-dimensional space path comprises: The sugarcane Beidou navigation planting record is used as the working path information; The working path information is obtained by aerial photography and surveying of a UAV in the seedling stage of the sugarcane field; The working path information is obtained based on an artificial field calibration method.

3. The harvester self-rowing system in the field according to claim 1, characterized in that, The GNSS double-antenna positioning and orientation module is fixedly installed on the longitudinal axis of the roof of the cab of the sugarcane harvester at a preset baseline distance, the phase difference of the satellite signal carrier received by the master GNSS antenna and the slave GNSS antenna is calculated to obtain a three-dimensional position coordinate of centimeter level, and the real heading angle of the vehicle is calculated.

4. The harvester field auto-rowing system according to claim 1, wherein, The cab inertial measurement unit comprises a first three-axis gyroscope and a first three-axis accelerometer, which are used to measure the angular velocity and acceleration of the cab respectively; the chassis inertial measurement unit comprises a second three-axis gyroscope and a second three-axis accelerometer, which are used to independently measure the angular velocity and acceleration of the chassis body respectively.

5. The harvester field auto-rowing system according to claim 1, wherein, The Euler coordinate conversion is specifically: A three-dimensional rotation matrix or quaternion transformation is constructed based on the heading angle, pitch angle and roll angle of the vehicle calculated from the dual-antenna GNSS positioning and orientation module and the cab inertial measurement unit data, the fixed three-dimensional offset vector between the main GNSS antenna and the vehicle chassis center of rotation is rotated and transformed, and the three-dimensional coordinates of the main GNSS antenna are converted to the vehicle chassis center of rotation by subtracting the rotated three-dimensional offset vector from the three-dimensional coordinates of the main GNSS antenna.

6. The harvester field auto-rowing system according to claim 1, wherein, The state vector of the Kalman filter is the planar coordinates of the chassis navigation control point position and the Z-axis gyro heading angle deviation of the chassis inertial measurement unit; the measurement vector is the planar coordinates of the chassis center of rotation; the state transition equation is constructed based on the principle of position and velocity dead reckoning; the measurement equation is constructed based on the coordinate transformation formula between the vehicle body coordinate system and the geodetic navigation coordinate system; the state transition matrix and the measurement matrix are updated in real time according to the original roll angle, pitch angle, forward speed of the chassis and the heading angle accumulated by the yaw rate.

7. The harvester self-rowing system in the field according to claim 1, characterized in that, The control rate of the path tracking control algorithm is: ； where K is a gain coefficient, v is the speed of the cane harvester, e d is the lateral deviation, e θ is the heading deviation.

8. The harvester field auto-rowing system according to claim 1, wherein, The steering actuator is a hydraulic steering system integrated with an electro-hydraulic servo valve or an independent electric power steering machine.

9. A method for automatically aligning a sugarcane harvester in a field, applied to the system for automatically aligning a sugarcane harvester in a field according to any one of claims 1 to 8, characterized by, The method comprises the following steps: S1: Before operation, load the pre-stored sugarcane ridge three-dimensional space path through the core controller; S2: Start the pose detection unit, obtain the three-dimensional coordinates of the main GNSS antenna and the vehicle heading angle in real time through the GNSS dual-antenna module, obtain the angular velocity and acceleration information of the cab in real time through the cab inertial measurement unit, and obtain the angular velocity and acceleration information of the chassis in real time through the chassis inertial measurement unit; S3: The core controller calculates the cab attitude based on the data of the cab inertial measurement unit, and rotates and transforms the fixed three-dimensional offset vector between the main GNSS antenna and the vehicle chassis center of rotation through the Euler coordinate conversion model, converts the coordinates of the main GNSS antenna to the vehicle chassis center of rotation, and obtains the chassis center of rotation coordinates; S4: The core controller fuses the chassis center of rotation coordinates obtained in step S3 with the data of the chassis inertial measurement unit, estimates the three-dimensional pose of the chassis through the Kalman filter; S5: Project the three-dimensional pose of the chassis and the pre-stored three-dimensional space path onto the horizontal plane, and calculate the lateral deviation and heading deviation; S6: calculating a steering control variable by a path tracking control algorithm based on the lateral deviation and the heading deviation, the calculation of the steering control variable satisfying the formula: where K is a gain coefficient, e d is the lateral deviation, and v is the vehicle speed, e θ is the heading deviation; S7: The core controller converts the steering control amount into a control signal and sends it to the steering actuator to drive the wheels to deflect by a corresponding angle; S8: Return to step S2, continuously collect pose data and repeat the above steps S2-S7 to form a closed-loop control and realize automatic driving operation of the sugarcane grabbing machine along the pre-stored ridge center line.

10. The field automatic row alignment control method of claim 9, wherein, When the GNSS signal is temporarily lost, the core controller calculates the pose through inertial navigation based on the data of the chassis inertial measurement unit, maintains the continuity and smoothness of the control, and waits until the GNSS signal is restored.

Images