Row path planning and tracking method based on multi-source perception fusion

Abstract

The present application relates to the field of control adjustment technology, in particular to a method for inter-ridge path planning and tracking based on multi-source perception fusion. The method synchronously collects image, point cloud and inertial measurement unit data, projects image ridge line coordinates and point cloud to vehicle body coordinate system for space-time synchronization based on external parameter calibration matrix and time stamp; calculates real-time confidence of vision and radar, constructs a dynamic topological graph with current vehicle position as root node and kinematic forward simulation trajectory as edge, and uses confidence weighted result to evaluate cost of topological graph edge node to output optimal planning path; establishes a model predictive control model containing steering angular velocity and speed constraints, and inputs optimal path to solve optimal front wheel steering angle. The present application eliminates spatial dislocation of perception data caused by sampling frequency difference of multi-source sensors, avoids infeasible trajectories of planning path beyond minimum turning radius, and reduces lateral deviation and heading angle oscillation in inter-ridge tracking process.

Description

Technical Field

[0001] This invention relates to the field of control and regulation technology, and specifically to a method for inter-row path planning and tracking based on multi-source sensing fusion. Background Technology

[0002] Current automatic navigation between rows in agricultural machinery typically employs a combination of visual cameras and lidar to acquire environmental information. In practice, conventional solutions utilize extended Kalman filtering to fuse row line features extracted from visual images with row surface point cloud features extracted from lidar using fixed weights, generating a two-dimensional occupancy grid map. In the path planning stage, existing technologies generally employ a gridded search algorithm to find node sequences that avoid crop obstacles within this occupancy grid map. In the path tracking stage, conventional solutions use this node sequence as a reference trajectory, inputting it into a conventional model predictive controller or a pure proportional-integral-derivative controller. The front wheel steering angle is then obtained by solving the deviation equation and transmitted to the agricultural machinery's steering actuator.

[0003] Based on the specific implementation methods of the aforementioned existing technologies, a core technical problem exists: the distortion of asynchronous multi-source sensing data fusion and the deviation of the gridded search path from the kinematic constraints of agricultural machinery in the existing technologies result in infeasible planned paths and large tracking deviations when the agricultural machinery is traveling between rows. Due to the differences in physical sampling frequencies and communication delays between cameras and radars, conventional fixed-weight extended Kalman filters do not compensate for the spatial position of each sensor's temporal asynchrony, leading to spatial misalignment of the fused sensing results in the vehicle coordinate system. When this spatially misaligned sensing data is used for gridded search, the positions of obstacles on the map deviate from their actual physical positions; simultaneously, the discrete node sequence generated by the gridded search is not deeply integrated with the nonholonomic constraints of the agricultural machinery, resulting in paths containing broken lines that do not conform to the minimum turning radius of the agricultural machinery. Inputting such paths that deviate from the actual boundaries and violate kinematic laws into the controller will cause the agricultural machinery to generate unexecutable steering commands when traveling between rows, thereby triggering continuous lateral position and heading deviations. Summary of the Invention

[0004] The purpose of this invention is to provide a method for inter-row path planning and tracking based on multi-source sensing fusion, which can effectively solve the problems in the background art.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A multi-source sensing fusion-based inter-row path planning and tracking method includes: simultaneously acquiring camera images, lidar point clouds, and inertial measurement unit data; Based on the extrinsic calibration matrix of the camera and lidar and the timestamps of each sensor, the ridge pixel coordinates extracted from the image and the ridge point cloud extracted by the radar are projected onto a unified vehicle coordinate system for spatiotemporal synchronization. The visual feature matching score and radar point cloud flatness score are used as the real-time confidence scores of each sensor. A dynamic topology graph is constructed with the current position of the vehicle body as the root node and the trajectory generated by the forward kinematic simulation as the edge. The collision detection and cost evaluation of the edge nodes of the dynamic topology graph are performed using the real-time confidence-weighted fusion perception results, and the optimal planning path is output. A model predictive control model is established that includes constraints on the steering angle and speed of the agricultural machinery. The optimal planned path is input into the model predictive control model as a reference trajectory to solve for the optimal front wheel steering angle at the current moment and send it down to the underlying actuator.

[0006] Preferably, the step of projecting the ridge pixel coordinates extracted from the image and the ridge point cloud extracted by the radar onto a unified vehicle coordinate system for spatiotemporal synchronization based on the extrinsic parameter calibration matrix of the camera and the lidar and the timestamps of each sensor includes: obtaining the nearest neighbor historical synchronization time of the camera and the lidar; using the nearest neighbor historical synchronization time as a reference, reading the angular velocity and acceleration integral of the inertial measurement unit between the current time and the nearest neighbor historical synchronization time; and using the angular velocity and acceleration integral to construct the rotation and translation matrix of the vehicle coordinate system at the current time relative to the vehicle coordinate system at the nearest neighbor historical synchronization time. The radar point cloud collected by the lidar at the current moment is transformed into the vehicle coordinate system of the nearest neighbor historical synchronization moment using the rotation and translation matrix; The ridge pixel coordinates extracted from the current moment image are back-projected into three-dimensional space using the camera intrinsic parameter matrix and extrinsic parameter calibration matrix. Then, the three-dimensional space coordinates are transformed into the vehicle coordinate system of the nearest neighbor historical synchronization moment.

[0007] Preferably, the calculation of visual feature matching score and radar point cloud flatness score as the real-time confidence of each sensor includes: dividing the image ridge point set in the vehicle coordinate system into multiple equal-width strips along the vehicle's horizontal axis, calculating the quantity variance and slope variance of the ridge points in each strip, and substituting the quantity variance and slope variance into a preset mapping relationship to calculate the visual feature matching score. In the radar point cloud set in the vehicle coordinate system, the point cloud is divided into multiple grids. Plane fitting is performed on the point cloud in each grid, the fitting residual is calculated, and the mean of the fitting residual is used as the radar point cloud flatness score. The visual feature matching score and the radar point cloud flatness score are normalized respectively. The normalized visual feature matching score and the normalized radar point cloud flatness score are then weighted and summed to obtain the fusion confidence weight matrix.

[0008] Preferably, the construction of a dynamic topology graph with the current position of the vehicle body as the root node and the trajectory generated by the forward kinematic simulation as the edge includes: taking the current position of the vehicle body as the root node, uniformly sampling within the executable range of the front wheel steering angle of the agricultural machinery according to a preset control input sampling period to obtain multiple discrete steering angle inputs; With the current longitudinal vehicle speed as the initial state, the multiple discrete steering angle inputs are substituted into the kinematic differential equation of the agricultural machine for forward integration. The integration step size is the sampling period of the control input, and the total integration time is the prediction time domain length, thus obtaining multiple forward simulation trajectories. The endpoint of each forward simulation trajectory is taken as a candidate child node, and the forward simulation trajectory connecting the root node and the candidate child node is taken as an edge of the dynamic topology graph. Candidate child nodes and their corresponding edges whose endpoints exceed the physical boundary of the furrow are eliminated.

[0009] Preferably, the step of using the real-time confidence-weighted fusion perception result to perform collision detection and cost evaluation on the edge nodes of the dynamic topology graph includes: discretizing the edges of the dynamic topology graph into multiple trajectory sampling points, and mapping the trajectory sampling points to the spatial positions corresponding to the fusion confidence weight matrix to obtain the weight cost of each trajectory sampling point; For each trajectory sampling point, a collision detection window of a preset size is established with the trajectory sampling point as the center. The number of point clouds marked as obstacles in the fused perception results within the collision detection window is counted. When the number of point clouds exceeds a preset threshold, the trajectory sampling point is marked as a collision point. Edges containing collision points are removed. For edges that do not contain collision points, the sum of the weighted costs of all trajectory sampling points on that edge is calculated as the total cost of the edge.

[0010] Preferably, the step of establishing a model predictive control model that includes agricultural machinery steering angular velocity constraints and speed constraints, and inputting the optimal planned path as a reference trajectory into the model predictive control model to solve for the optimal front wheel steering angle at the current moment, includes: establishing a state-space equation with the agricultural machinery lateral deviation, heading deviation and front wheel steering angle as state variables; Within the prediction time domain of the state-space equation, an objective function is constructed with the goal of minimizing the sum of squares of the deviations between the state variables and the corresponding points of the reference trajectory. The objective function is constrained by adding upper and lower limits of front wheel steering angle based on the physical limits of agricultural machinery steering tie rod, upper and lower limits of steering angle change at adjacent sampling times based on the response characteristics of steering hydraulic cylinder, and upper and lower limits of longitudinal vehicle speed based on the limits of tire-soil adhesion. The objective function with added constraints is then solved using a quadratic programming solver.

[0011] Preferably, obtaining the nearest neighbor historical synchronization time of the camera and the lidar includes: maintaining a timestamp circular buffer in the memory of the agricultural machinery vehicle computing platform, wherein the timestamp circular buffer records the arrival timestamp of the camera image and the arrival timestamp of the lidar point cloud; When a camera image or LiDAR point cloud is received at the current moment, the historical timestamp of the other sensor with the smallest time difference from the current moment is queried in the timestamp circular buffer, and the historical timestamp is used as the nearest neighbor historical synchronization moment. If the time difference is greater than the preset maximum allowable time difference threshold, the current camera image or LiDAR point cloud is discarded, and a hardware trigger signal is sent to the corresponding sensor to resynchronize the exposure time and scan start and end time.

[0012] Preferably, after weighted summation of the normalized visual feature matching score and the normalized radar point cloud flatness score to obtain the fusion confidence weight matrix, the method further includes: establishing a sliding window queue with a preset number of frames, storing the fusion confidence weight matrix calculated for the current frame into the sliding window queue, and removing the fusion confidence weight matrix of the earliest frame from the sliding window queue. For the historical fusion confidence weight matrix sequence at the same spatial grid position in the sliding window queue, calculate the median of the historical fusion confidence weight matrix sequence; Replace the value at the corresponding spatial grid position in the current frame's fusion confidence weight matrix with the median, and output the smoothed fusion confidence weight matrix.

[0013] Preferably, after using the forward simulation trajectory connecting the root node and the candidate child node as the edge of the dynamic topology graph and removing the candidate child node and the corresponding edge whose endpoint exceeds the physical boundary of the furrow, the method further includes: extracting the position coordinates of the trajectory points according to the time step order of the forward integration for the retained forward simulation trajectory. Using the position coordinates of three adjacent trajectory points, the approximate curvature at each trajectory point is calculated based on the vector cross product; Determine whether the approximate curvature is greater than the maximum permissible curvature calculated based on the current longitudinal speed of the agricultural machinery and the preset maximum lateral acceleration threshold; If there is a trajectory point in the forward simulation trajectory whose approximate curvature is greater than the maximum allowable curvature, then the forward simulation trajectory is truncated at the trajectory point where the approximate curvature is greater than the maximum allowable curvature, and the truncated forward simulation trajectory is used as a new edge to update the candidate child node position.

[0014] Preferably, adding upper and lower limit constraints on the front wheel steering angle based on the physical limits of the agricultural machinery steering tie rod to the objective function includes: introducing a relaxation variable with the same number as the number of front wheel steering angles, and transforming the upper and lower limit constraints on the front wheel steering angle into a mixed integer complementary constraint condition combining equality constraints and inequality constraints; By adding the penalty term coefficient of the slack variable to the objective function, the objective function containing the mixed integer complementary constraints and the added penalty term coefficient is transformed into the standard form of a linear complementarity problem. The coefficient matrix in the standard form of the linear complement problem is decomposed into blocks. The matrix after block decomposition is iteratively solved using the predictive correction interior point method until the dual interval is less than the preset convergence threshold. The optimal front wheel steering angle at the current moment is then output.

[0015] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. By constructing a rotation and translation matrix based on the extrinsic parameter calibration matrix and timestamps, and utilizing the integrals of angular velocity and acceleration, the pixel coordinates of the image ridge line and the radar point cloud are projected onto the same vehicle coordinate system for spatiotemporal synchronization, eliminating spatial misalignment of sensing data caused by differences in sampling frequencies and communication delays among multiple sensors. To replace gridded search, a dynamic topology graph is constructed with the current vehicle position as the root node and the trajectory generated by forward kinematic simulation as the edges. Discrete steering angle inputs are substituted into the kinematic differential equations for forward integration to generate edge nodes. This underlying mechanism ensures that the planned path strictly conforms to the kinematic constraints of agricultural machinery, avoiding infeasible broken lines exceeding the minimum turning radius of the agricultural machinery in the planned path.

[0016] 2. By calculating the visual feature matching score and radar point cloud flatness score as real-time confidence scores, and combining this with median replacement smoothing using a sliding window queue, a fusion confidence weight matrix is ​​generated. This matrix is ​​then used to calculate the weight cost in collision detection of the dynamic topology map, avoiding path deviation caused by single-type sensor failure in fixed-weight fusion. In the model predictive control, steering angle constraints based on the physical limits of the steering tie rod, steering angle change constraints based on the hydraulic cylinder response characteristics, and vehicle speed constraints based on the tire-soil adhesion limits are introduced, transforming them into mixed integer complementary constraints and solving them using the predictive correction interior point method. This suppresses overshoot and oscillation of the steering actuator during the control process. Furthermore, by introducing an approximate curvature calculation and truncation mechanism based on the maximum lateral acceleration threshold in the forward simulated trajectory, trajectory nodes that do not meet the dynamic rollover risk criteria are further eliminated. Attached Figure Description

[0017] Figure 1 This is a flowchart of the spatiotemporal synchronization process for multi-source sensor data according to the present invention; Figure 2 This is a flowchart illustrating the generation process of the sensor real-time confidence score and fusion weight matrix in this invention. Figure 3 This is a flowchart of the dynamic topology graph construction based on kinematic forward simulation of the present invention; Figure 4 This is a flowchart of the trajectory collision detection and cost evaluation method based on the fusion confidence weighting of the present invention; Figure 5 This is a flowchart of the curvature constraint and truncation processing of the forward simulation trajectory of the present invention. Figure 6 This is a flowchart of the model predictive control solution with multi-physics constraints according to the present invention. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] Please refer to Figure 1 This embodiment provides a method for inter-row path planning and tracking based on multi-source sensing fusion. The vehicle-mounted computing platform simultaneously acquires camera images, LiDAR point clouds, and inertial measurement unit data. During acquisition, the platform adds a timestamp corresponding to the acquisition time to each set of received sensor data. The clock source for the timestamps is uniformly the local system clock of the vehicle-mounted computing platform, eliminating timing deviations caused by differences in clock sources between different sensors. The camera images are scene images of the crop rows between the rows. After acquisition, the images are preprocessed, including Gaussian filtering to remove image noise, histogram equalization to enhance the contrast between crop rows and furrow areas, extraction of crop row edge features from the image using edge detection operators, and then obtaining the straight line equation of the ridge line through Hough transform fitting. Based on the ridge line equation, the set of pixel coordinates corresponding to the ridge line is extracted, with each pixel coordinate containing horizontal and vertical coordinate values ​​in the image coordinate system. The LiDAR point cloud data consists of 3D point cloud data of the ridge-to-ridge scene surrounding the vehicle. After acquisition, the point cloud undergoes preprocessing, including direct-pass filtering to remove point cloud data exceeding the preset detection range, statistical filtering to remove discrete outliers, separation of ground points and non-ground points in the preprocessed point cloud, and extraction of the point cloud set corresponding to the ridge surface based on elevation and normal vector angle thresholds to obtain the 3D coordinate data of the ridge surface point cloud. The inertial measurement unit data includes triaxial angular velocity and triaxial acceleration data. After acquisition, the data undergoes zero-bias compensation and low-pass filtering to remove high-frequency noise and measurement errors caused by sensor zero bias.

[0020] Furthermore, based on the extrinsic calibration matrices of the camera and LiDAR and the timestamps of each sensor, the pixel coordinates of the ridge lines extracted from the image and the ridge surface point cloud extracted by the radar are projected onto a unified vehicle coordinate system for spatiotemporal synchronization. The vehicle coordinate system has its origin at the center of the rear axle of the agricultural machinery, with the forward driving direction of the vehicle as the positive X-axis, the left lateral direction of the vehicle as the positive Y-axis, and the vertical upward direction of the ground as the positive Z-axis. The extrinsic calibration matrices of the camera and LiDAR are pre-calibrated rotation and translation matrices between the camera coordinate system and the LiDAR coordinate system, used to achieve pose transformation between the two sensor coordinate systems. Based on the timestamps corresponding to the data from each sensor, the acquisition times of the camera image and the LiDAR point cloud are matched, and the pixel coordinates of the ridge lines in the image and the ridge surface point cloud in the radar are transformed to the same vehicle coordinate system at the same time through pose transformation, achieving spatial alignment and temporal synchronization of multi-source sensing data.

[0021] Specifically, the visual feature matching score and radar point cloud flatness score are calculated as the real-time confidence scores of each sensor. In the vehicle coordinate system with spatiotemporal synchronization, the image ridge point set and radar ridge surface point set are extracted. For the image ridge point set, multiple equal-width strips are divided along the vehicle's horizontal axis. The number of ridge points in each strip is counted, and the slope of the straight line containing the ridge points in each strip is fitted. Based on the number of ridge points and the slope of each strip, the corresponding variance is calculated. The variance of the number of points and the variance of the slope are substituted into a preset mapping relationship to calculate the visual feature matching score. The visual feature matching score is used to characterize the consistency and reliability of the visually extracted ridge features; a higher score indicates a higher confidence level for the visual ridge features. For the radar ridge point set, the point cloud is divided into multiple equal-sized grids in the XY plane of the vehicle coordinate system. The least squares method is used to fit the plane of the point cloud within each grid. The vertical distance from each point cloud to the fitting plane is calculated as the fitting residual. The mean of the fitting residuals across all grids is calculated, and the radar point cloud flatness score is obtained based on the mean residual. The radar point cloud flatness score characterizes the flatness and reliability of the ridge point cloud extracted by the radar; a higher score indicates higher confidence in the radar ridge point cloud. The visual feature matching score and the radar point cloud flatness score are both subjected to min-max normalization, mapping their values ​​to the [0,1] interval. A weighted sum is then performed on the normalized scores to obtain the fusion confidence weight matrix. Each element in the fusion confidence weight matrix corresponds to the perceptual confidence weight value of the next spatial grid in the vehicle coordinate system.

[0022] Furthermore, a dynamic topology graph is constructed with the current position of the vehicle body as the root node and the trajectory generated by forward kinematic simulation as the edges. Collision detection and cost evaluation of the edge nodes of the dynamic topology graph are performed using real-time confidence-weighted fusion perception results, and the optimal planned path is output. Taking the current position of the rear axle center of the agricultural machinery in the vehicle coordinate system as the root node of the dynamic topology graph, uniform sampling is performed within the executable range of the front wheel steering angle of the agricultural machinery according to the preset control input sampling period to obtain multiple discrete steering angle input values. With the current longitudinal speed of the agricultural machinery as the initial state, each discrete steering angle input value is substituted into the kinematic differential equation of the agricultural machinery for forward integration. The integration step size is consistent with the control input sampling period, and the total integration time is the preset prediction time domain length. Multiple forward simulation trajectories are obtained through forward integration. Each forward simulation trajectory contains multiple trajectory points arranged in chronological order. Each trajectory point contains the position coordinates and heading angle in the vehicle coordinate system. The endpoint of each forward simulation trajectory is used as a candidate child node of the dynamic topology graph. The forward simulation trajectory connecting the root node and the candidate child node is used as the edge of the dynamic topology graph. It is determined whether the endpoint of each forward simulation trajectory exceeds the physical boundary of the furrow. Candidate child nodes and their corresponding edges whose endpoints exceed the physical boundary of the furrow are removed, thus completing the initial construction of the dynamic topology graph.

[0023] Specifically, for each edge retained in the dynamic topology graph, the corresponding forward simulation trajectory is discretized into multiple equally spaced trajectory sampling points. Each trajectory sampling point is mapped to the spatial grid position corresponding to the fusion confidence weight matrix, and the weight cost value corresponding to each trajectory sampling point is obtained. The weight cost value is negatively correlated with the fusion confidence weight value of the corresponding grid; the higher the confidence value of the grid, the lower the weight cost value. For each trajectory sampling point, a collision detection window of a preset size is established with the trajectory sampling point as the center. The size of the collision detection window matches the outer dimensions of the agricultural machinery vehicle. The number of point clouds marked as obstacles in the fusion perception results within the collision detection window is counted. When the counted number of obstacle point clouds exceeds a preset threshold, the trajectory sampling point is marked as a collision point. Edges containing at least one collision point are removed from the dynamic topology graph. For the remaining edges that do not contain collision points, the sum of the weight costs of all trajectory sampling points on the edge is calculated as the total cost value of the edge. The forward simulation trajectory corresponding to the edge with the smallest total cost value is selected as the optimal planning path output.

[0024] Furthermore, a model predictive control (MMCC) model incorporating constraints on the agricultural machinery's steering angular velocity and speed is established. The optimal planned path is used as the reference trajectory input to the MMCC model to solve for the optimal front wheel steering angle at the current moment and distribute the result to the underlying actuators. A state-space equation is established using the agricultural machinery's lateral position deviation, heading angle deviation, and front wheel steering angle as state variables. Based on the agricultural machinery's kinematic model, the state-space equation is discretized to obtain a discrete-time state-space equation, adapting to the discrete solution logic of model predictive control. Within the prediction time domain of the state-space equation, an objective function is constructed with the core objective of minimizing the sum of squared deviations between the state variables and the corresponding points on the reference trajectory. A penalty term for changes in control input is added to the objective function to suppress drastic fluctuations in the control input. Multi-dimensional constraints are added to the objective function, including upper and lower limits of the front wheel steering angle based on the physical limits of the agricultural machinery steering tie rod, upper and lower limits of the steering angle change at adjacent sampling times based on the response characteristics of the steering hydraulic cylinder, upper and lower limits of the longitudinal vehicle speed based on the limits of tire-soil adhesion, and upper and lower limits of the steering angular velocity based on the rated speed of the steering motor. The objective function with added constraints is solved using a quadratic programming solver to obtain the front wheel steering angle sequence in the control time domain. The first element in the sequence is taken as the optimal front wheel steering angle at the current time and sent to the underlying steering actuator of the agricultural machinery through the vehicle bus to complete the path tracking control.

[0025] In this embodiment, Table 1 is inserted to record the weighted cost and collision detection results of discrete sampling points of a forward simulated trajectory in the dynamic topology graph, providing data support for the calculation of the total cost of the edge and collision elimination.

[0026] Table 1. Correspondence between the weighted cost of trajectory sampling points and collision detection results. ; In this table, the X-axis of the vehicle coordinate system represents the forward driving direction of the vehicle, and the Y-axis represents the lateral direction of the vehicle. The fusion confidence weight value is negatively correlated with the fusion confidence weight value of the corresponding position. A collision marker of 1 indicates that the sampling point is marked as a collision point, and a collision marker of 0 indicates that the sampling point has no collision risk.

[0027] In this embodiment, by synchronously acquiring and synchronizing multi-source sensor data, spatial alignment of ridge line features and ridge surface point clouds in a unified vehicle coordinate system is achieved. By calculating visual feature matching scores and radar point cloud flatness scores, the real-time confidence of each sensor and adaptive weighted fusion of multi-source sensing data are realized. By constructing a dynamic topology graph with kinematic forward simulation trajectories as edges, the generated planned path is adapted to the kinematic characteristics of the agricultural machinery from the bottom layer. Through a model prediction and control model with multi-dimensional physical constraints, stable tracking of the planned path is achieved, eliminating the tracking deviation problem caused by spatial misalignment of sensing data and infeasibility of the planned path.

[0028] In a preferred embodiment, the spatiotemporal synchronization process of multi-source sensor data is refined. A timestamp circular buffer is maintained in the memory of the agricultural machinery onboard computing platform. The storage capacity of the timestamp circular buffer is a preset fixed value, corresponding to the sensor timestamp data within a preset duration during the agricultural machinery's operation. The buffer adopts a first-in, first-out (FIFO) storage mechanism. When a new timestamp corresponding to sensor data is received, the timestamp is written to the corresponding storage location in the buffer. If the buffer is full, the earliest written timestamp data is overwritten. The timestamp circular buffer records the arrival timestamps of camera images and LiDAR point clouds, respectively. These two types of timestamps are stored in separate channels for easy quick querying and matching.

[0029] Specifically, when the onboard computing platform receives the camera image or LiDAR point cloud at the current moment, it extracts the timestamp corresponding to the current data and queries the historical timestamp of the other sensor with the smallest time difference from the current moment in the timestamp circular buffer. The moment corresponding to this historical timestamp is taken as the nearest neighbor historical synchronization moment. The time difference between the current moment and the nearest neighbor historical synchronization moment is calculated. If the calculated time difference is greater than the preset maximum allowable time difference threshold, the camera image or LiDAR point cloud received at the current moment is discarded. At the same time, a hardware trigger signal is sent to the corresponding sensor through the hardware trigger interface to adjust the sensor's exposure time and scan start and end time, thereby resynchronizing the sensor acquisition timing. If the calculated time difference is less than or equal to the preset maximum allowable time difference threshold, the subsequent pose transformation and spatiotemporal synchronization operations are performed based on the nearest neighbor historical synchronization moment.

[0030] Furthermore, the inertial measurement unit (IMU) reads the three-axis angular velocity and three-axis acceleration data between the current moment and the nearest neighbor historical synchronization moment. The three-axis angular velocity data is integrated within this time interval to obtain the attitude angle changes of the vehicle body relative to the nearest neighbor historical synchronization moment, including roll, pitch, and yaw angle changes. The three-axis acceleration data is integrated within this time interval, and gravity compensation is applied in conjunction with the attitude angle changes to eliminate the influence of gravitational acceleration on displacement calculation, thus obtaining the three-dimensional displacement changes of the vehicle body relative to the nearest neighbor historical synchronization moment. Using the obtained attitude angle changes, a rotation matrix is ​​constructed for the vehicle body coordinate system at the current moment relative to the vehicle body coordinate system at the nearest neighbor historical synchronization moment. The corresponding translation vector is constructed using the obtained displacement changes. Based on the rotation matrix and translation vector, a homogeneous rotation-translation matrix is ​​constructed to describe the pose transformation between the vehicle body coordinate systems at different times.

[0031] Specifically, the rotation matrix is ​​constructed using the ZYX Euler angle rotation order, corresponding to the yaw, pitch, and roll attitude changes of the vehicle body. The expression for the rotation matrix is:

[0032] in, , , These are the rotation matrices around the X, Y, and Z axes of the vehicle coordinate system, respectively. , , These are the changes in roll angle, pitch angle, and yaw angle at the current time relative to the nearest neighboring historical synchronization time, respectively, obtained by integrating the three-axis angular velocity data of the inertial measurement unit within the corresponding time interval.

[0033] The expression for the homogeneous rotation-translation matrix constructed based on the rotation matrix and the translation vector is:

[0034] in, This is the translation vector of the vehicle coordinate system at the current moment relative to the vehicle coordinate system at the nearest neighbor historical synchronization moment. It is obtained by integrating the three-axis acceleration data of the inertial measurement unit over the corresponding time interval and then applying gravity compensation. It is a three-dimensional zero vector.

[0035] Furthermore, using the constructed rotation and translation matrix, the point cloud of the ridge surface acquired by the lidar at the current moment is transformed into the vehicle coordinate system of the nearest neighbor historical synchronization moment. The extrinsic parameter calibration matrix between the lidar coordinate system and the vehicle coordinate system is a pre-calibrated fixed matrix. First, the lidar ridge surface point cloud at the current moment is transformed from the lidar coordinate system to the vehicle coordinate system at the current moment through this extrinsic parameter calibration matrix. Then, the point cloud is transformed from the vehicle coordinate system at the current moment to the vehicle coordinate system of the nearest neighbor historical synchronization moment through the rotation and translation matrix, thus completing the temporal synchronization and spatial alignment of the lidar point cloud.

[0036] Specifically, using a pre-calibrated camera intrinsic parameter matrix and camera-LiDAR extrinsic parameter calibration matrix, the ridge pixel coordinates extracted from the current image are back-projected into three-dimensional space. Then, the three-dimensional coordinates are transformed into the vehicle coordinate system of the nearest neighbor historical synchronization time. The camera intrinsic parameter matrix is ​​a pre-calibrated matrix containing the camera's focal length and principal point coordinate parameters. Based on the camera intrinsic parameter matrix, the ridge pixel coordinates in the image coordinate system are back-projected into three-dimensional coordinates in the camera coordinate system. The expression for the back-projection is:

[0037] in, These are the pixel coordinates of the points on the ridge line in the image coordinate system. For the camera intrinsic parameter matrix, These are the three-dimensional coordinates of the ridge line points in the camera coordinate system. The depth value of the ridge point in the camera coordinate system is obtained by matching the depth information of the radar ridge point cloud after spatiotemporal synchronization.

[0038] The 3D coordinates of the ridge line in the camera coordinate system are transformed to the LiDAR coordinate system through the camera-LiDAR extrinsic calibration matrix, then transformed to the vehicle coordinate system at the current moment through the LiDAR-vehicle extrinsic calibration matrix, and finally transformed to the vehicle coordinate system at the nearest neighbor historical synchronization moment through the rotation and translation matrix, thus completing the temporal synchronization and spatial alignment of the ridge line features in the image.

[0039] In this embodiment, Table 2 is inserted to record the pose transformation parameters of the vehicle coordinate system at the current time relative to the vehicle coordinate system at the nearest neighbor historical synchronization time under different sampling times, so as to provide a basis for pose compensation for the spatiotemporal synchronization of multi-source sensing data.

[0040] Table 2. Correspondence of vehicle coordinate system pose transformation parameters at different sampling times. ; In this table, the time difference is the time interval between the current sampling time and the nearest neighbor historical synchronization time; the yaw angle component of the rotation matrix is ​​the change in the vehicle's angle around the Z-axis; and the X-axis and Y-axis components of the translation vector are the changes in the vehicle's displacement in the forward and lateral directions, respectively.

[0041] In this embodiment, a timestamp-based circular buffer enables rapid querying and matching of the nearest neighbor synchronization time between the camera and the lidar. The angular velocity and acceleration integrals of the inertial measurement unit are used to construct the pose transformation matrix of the vehicle coordinate system at different times, realizing the spatiotemporal synchronization of the asynchronously acquired image ridge features and the radar point cloud in a unified coordinate system. This eliminates the spatial misalignment of perception data caused by differences in sampling frequency and communication delays among multiple source sensors. At the same time, dynamic calibration of the sensor acquisition timing is achieved through time difference threshold judgment and feedback of hardware trigger signals, avoiding fusion distortion caused by timing mismatch.

[0042] In a preferred embodiment, reference Figure 2The process of real-time confidence calculation and fusion confidence weight matrix generation for sensors is refined. In the vehicle coordinate system with spatiotemporal synchronization, image ridge point sets are extracted and divided into multiple equal-width strips along the vehicle's horizontal axis. The width and number of strips are preset fixed values, and the length of all strips along the forward direction of the vehicle covers a preset sensing range. For each strip, the number of ridge points within that strip is counted, and a straight line is fitted to these ridge points to obtain the fitted slope of the ridge line within that strip. The number of ridge points in all strips is counted, the mean of the number of ridge points in all strips is calculated, and the variance of the number of ridge points in each strip is calculated based on the mean. The fitted slope of the ridge line in all strips is counted, the mean of the fitted slope of the ridge line in all strips is calculated, and the variance of the fitted slope of the ridge line in each strip is calculated based on the mean slope.

[0043] Specifically, the calculated quantity variance and slope variance are substituted into a preset mapping relationship to calculate the visual feature matching score. The expression for the visual feature matching score is as follows:

[0044] in, Let V be the variance of the number of ridge points within each equal-width strip along the transverse axis of the vehicle body. Let V be the variance of the fitted slope of the ridge line within each equal-width strip along the transverse axis of the vehicle body. The score is the visual feature matching score. The score is positively correlated with the consistency of the ridge features. The higher the score, the higher the confidence of the visually extracted ridge features.

[0045] Furthermore, in the vehicle coordinate system with spatiotemporal synchronization, the radar surface point set is extracted. The point cloud is then divided into multiple equally sized grids in the XY plane of the vehicle coordinate system. The grid size is a preset fixed value, and all grids cover a preset sensing range around the vehicle. For each grid, if the number of points within that grid is greater than a preset minimum point cloud count threshold, the least squares method is used to fit the plane to obtain the corresponding plane equation. The perpendicular distance from each point within that grid to the fitting plane is calculated as the fitting residual for that point. If the number of points within that grid is less than or equal to the preset minimum point cloud count threshold, that grid is marked as invalid and will not participate in subsequent flatness calculations.

[0046] Specifically, the fitting residuals of the point cloud within all valid grids are statistically analyzed, the mean of all fitting residuals is calculated, and the radar point cloud flatness score is calculated based on the mean residual. The expression for the radar point cloud flatness score is as follows:

[0047] in, This represents the mean of the plane fitting residuals of the point cloud within each effective grid in the vehicle coordinate system. The radar point cloud flatness score is positively correlated with the flatness of the ridge point cloud; a higher score indicates a higher confidence level in the ridge point cloud extracted by the radar.

[0048] Furthermore, the visual feature matching score and radar point cloud flatness score are respectively subjected to min-max normalization, mapping the values ​​of the two scores to the [0,1] interval. For each spatial grid in the vehicle coordinate system, the normalized visual feature matching score and normalized radar point cloud flatness score corresponding to that grid are calculated. Based on preset weight coefficients, the two normalized scores are weighted and summed to obtain the fusion confidence weight value corresponding to that grid. The fusion confidence weight values ​​of all grids constitute the fusion confidence weight matrix. The expression of the fusion confidence weight matrix is ​​as follows:

[0049] in, This is the index of the spatial mesh in the vehicle coordinate system. For the first Normalized visual feature matching score within each grid For the first The flatness score of the radar point cloud after normalization within each grid. The preset visual weight coefficients, For the first The fusion confidence weight value corresponding to each grid.

[0050] Specifically, after obtaining the fusion confidence weight matrix of the current frame, a sliding window queue with a preset number of frames is established. The length of the sliding window queue is fixed, and the queue uses a first-in, first-out (FIFO) storage mechanism. The fusion confidence weight matrix calculated for the current frame is stored in the sliding window queue, while the fusion confidence weight matrix of the oldest frame in the sliding window queue is removed, keeping the queue length constant. For the historical fusion confidence weight matrix sequence at the same spatial grid position in the sliding window queue, the fusion confidence weight values ​​corresponding to that grid for all frames in the sequence are extracted, the median of the sequence is calculated, and the calculated median is used to replace the value at the corresponding spatial grid position in the fusion confidence weight matrix of the current frame, completing the smoothing process of the fusion confidence weight matrix, and outputting the smoothed fusion confidence weight matrix.

[0051] The expression for smoothing is:

[0052] in, For the first in the sliding window queue Frame number The fusion confidence weight value of each grid, The preset length of the sliding window queue. This is the median calculation function. For the smoothed first The fusion confidence weight value of each grid.

[0053] In this embodiment, Table 3 is inserted to record the perception confidence calculation results and smoothing processing results of different spatial grids in the vehicle coordinate system, providing a weight basis for the trajectory cost evaluation of the dynamic topology map.

[0054] Table 3 Correspondence between Spatial Mesh Fusion Confidence Weight Calculation and Smoothing Results ; In this table, the vehicle coordinate system grid range is the coordinate interval of the corresponding grid in the X and Y axis directions; the initial fusion confidence weight is obtained by weighted sum of the normalized visual score and radar score; the smoothed fusion confidence weight is obtained by replacing the median of the historical weight sequence in the sliding window queue.

[0055] In this embodiment, the matching degree of visual ridge features is quantified by calculating the variance of the strips, and the flatness of the radar point cloud is quantified by calculating the residual of the grid plane fitting. Based on the two scores, the real-time confidence and fusion confidence weight matrices of each sensor are generated. By using a fixed-length sliding window queue and median replacement operation, the temporal smoothing of the fusion confidence weight matrix is ​​achieved, avoiding the weight abrupt change caused by single-frame data fluctuations. At the same time, it solves the path offset problem caused by fixed weight fusion when a single type of sensor fails, and improves the stability of the fusion perception results.

[0056] In a preferred embodiment, reference Figure 3 This paper refines the processes of dynamic topology graph construction, trajectory cost evaluation, trajectory curvature constraint, and model predictive control solution. Using the current position of the rear axle center in the vehicle coordinate system as the root node of the dynamic topology graph, uniform sampling is performed within the executable range of the front wheel steering angle according to a preset control input sampling period. This yields multiple discrete steering angle input values, with the number and interval of samples set as preset fixed values, covering the entire executable range of the front wheel steering angle. Using the current longitudinal speed of the agricultural machinery as the initial state, each discrete steering angle input value is substituted into the kinematic differential equation of the front wheel steering bicycle for forward integration. The integration step size is consistent with the control input sampling period, and the total integration time is the preset prediction time domain length. Multiple forward simulated trajectories are obtained through forward integration. Each forward simulated trajectory contains multiple trajectory points arranged in chronological order, and each trajectory point contains its position coordinates and heading angle in the vehicle coordinate system.

[0057] The expression for the kinematic differential equation of a bicycle with front wheel steering in agricultural machinery is:

[0058] in, Let these be the coordinates of the center of the rear axle of the agricultural machinery in the vehicle coordinate system. The heading angle of the agricultural machinery body. The longitudinal speed of the agricultural machinery. This refers to the steering angle of the front wheels of the agricultural machinery. The wheelbase between the front and rear axles of the agricultural machinery. , , These are the time derivatives of the corresponding state variables.

[0059] Specifically, refer to Figure 5 The forward integral is calculated using the fourth-order Runge-Kutta method to improve integration accuracy and reduce accumulated errors during the integration process. The endpoint of each forward simulation trajectory is used as a candidate child node of the dynamic topology graph, and the forward simulation trajectory connecting the root node and the candidate child node is used as an edge of the dynamic topology graph. It is determined whether the endpoint of each forward simulation trajectory exceeds the physical boundary of the furrow. The physical boundary of the furrow is obtained by fitting the furrow line features in the fusion sensing results. Candidate child nodes and their corresponding edges whose endpoints exceed the physical boundary of the furrow are removed, thus completing the initial construction of the dynamic topology graph.

[0060] Furthermore, for the forward simulation trajectories retained in the dynamic topology graph, the position coordinates of the trajectory points are extracted according to the time step order of the forward integration. Using the position coordinates of three adjacent trajectory points, the approximate curvature at each trajectory point is calculated based on the vector cross product. For three adjacent trajectory points arranged in time order... , , First, calculate the area of ​​the triangle formed by the three trajectory points. Then, calculate the length of the line segment between adjacent trajectory points. Based on the area of ​​the triangle and the length of the line segment, calculate the trajectory points. The approximate curvature at point A is expressed as follows:

[0061] in, Let be the area of ​​the triangle formed by the three points on the trajectory. , , These are the line segment lengths between adjacent trajectory points. For trajectory points The approximate curvature at that point.

[0062] Specifically, based on the current longitudinal speed of the agricultural machinery and the preset maximum lateral acceleration threshold, the maximum permissible curvature is calculated. The expression for the maximum permissible curvature is as follows: ,in The preset maximum lateral acceleration threshold, The current longitudinal speed of the agricultural machinery is given. The algorithm determines whether the approximate curvature of each trajectory point in the forward simulated trajectory is greater than the calculated maximum permissible curvature. If there is a trajectory point in the forward simulated trajectory with an approximate curvature greater than the maximum permissible curvature, the forward simulated trajectory is truncated at that point, retaining the portion of the trajectory before that point. The truncated forward simulated trajectory is used as a new edge in the dynamic topology graph, and the corresponding candidate child node positions are updated to the positions of the truncated trajectory points. If there is no trajectory point in the forward simulated trajectory with an approximate curvature greater than the maximum permissible curvature, the complete forward simulated trajectory and its corresponding candidate child nodes are retained.

[0063] Further, refer to Figure 4 For each edge ultimately retained in the dynamic topology graph, the corresponding forward simulation trajectory is discretized into multiple equally spaced trajectory sampling points. Each trajectory sampling point is mapped to the spatial grid position corresponding to the fusion confidence weight matrix, and the weight cost value corresponding to each trajectory sampling point is obtained. The weight cost value is negatively correlated with the fusion confidence weight value of the corresponding grid; the higher the confidence value of the grid, the lower the weight cost value. For each trajectory sampling point, a collision detection window of a preset size is established with the trajectory sampling point as the center. The size of the collision detection window matches the outer dimensions of the agricultural machinery vehicle. The number of point clouds marked as obstacles in the fusion perception results within the collision detection window is counted. When the number of obstacle point clouds exceeds a preset threshold, the trajectory sampling point is marked as a collision point. Edges containing at least one collision point are removed from the dynamic topology graph. For the remaining edges that do not contain collision points, the sum of the weight costs of all trajectory sampling points on the edge is calculated as the total cost value of the edge. The forward simulation trajectory corresponding to the edge with the smallest total cost value is selected as the optimal planning path output.

[0064] Specifically, refer to Figure 6 A model predictive control (MDC) model incorporating steering angular velocity and velocity constraints for agricultural machinery is established. The lateral position deviation, heading angle deviation, and front wheel steering angle of the agricultural machinery are used as state variables to establish the state-space equations. The lateral position deviation is the lateral distance between the current position of the rear axle center and the corresponding point on the reference trajectory, and the heading angle deviation is the angle difference between the current heading angle of the agricultural machinery and the tangent direction angle of the corresponding point on the reference trajectory. Based on the kinematic model of the agricultural machinery, the continuous state-space equations are discretized to obtain the discrete-time state-space equations. Adapting to the discrete solution logic of model predictive control, the expression for the discrete state-space equations is:

[0065] in, For the first The state vector at each sampling time. This is a lateral positional deviation. For heading angle deviation, This refers to the front wheel steering angle; For the first The control input at each sampling moment is the front wheel steering angular velocity; The discrete state matrix, The input matrix is ​​a discrete matrix obtained by discretizing the kinematic model of agricultural machinery.

[0066] Furthermore, within the prediction time domain of the state-space equations, an objective function is constructed with the core objective of minimizing the sum of squared deviations between the state variables and the corresponding points of the reference trajectory. A penalty term for the control input is added to the objective function to suppress drastic fluctuations in the control input. The expression of the objective function is:

[0067] in, To predict the length of the time domain, To control the length of the time domain, For the first The reference state vector at each sampling time is obtained from the corresponding point of the optimal planned path; The state weight matrix is... To control the input weight matrix; The objective function value is to minimize the optimization objective. .

[0068] Specifically, multi-dimensional constraints are added to the objective function, including upper and lower limits for the front wheel steering angle based on the physical limits of the agricultural machinery steering tie rod; upper and lower limits for the change in steering angle between adjacent sampling times based on the response characteristics of the steering hydraulic cylinder; upper and lower limits for the longitudinal vehicle speed based on the limits of tire-soil adhesion; and upper and lower limits for the steering angular velocity based on the rated speed of the steering motor. For the upper and lower limits of the front wheel steering angle, relaxation variables with the same number as the front wheel steering angle are introduced, transforming the inequality constraints into a mixed integer complementary constraint condition combining equality and inequality constraints. The upper limit constraint for the front wheel steering angle... Introducing slack variables , transformed and Complementary constraints; lower limit constraint for front wheel steering angle. Introducing slack variables , transformed and The complementary constraints are then applied. Similarly, for the remaining inequality constraints, corresponding slack variables are introduced to transform them into mixed integer complementary constraints.

[0069] Furthermore, a penalty term for slack variables is added to the objective function. This penalty term is the sum of the products of the slack variables and their corresponding penalty coefficients. This transforms the objective function, which includes mixed-integer complementarity constraints and the added penalty term, into the canonical form of a linear complementarity problem. The expression for the canonical form of the linear complementarity problem is as follows:

[0070] in, The coefficient matrix for the linear complementarity problem is constructed from the coefficients of the objective function and the constraints. To optimize the variable vector, it includes state variables, control input variables, and slack variables; It is a constant vector, constructed from the reference state and constraint boundary values.

[0071] Specifically, the coefficient matrix in the standard form of the linear complement problem The system is decomposed into submatrices corresponding to state variables, control input variables, and relaxation variables. The predicted and corrected interior point method is used to iteratively solve the decomposed matrix. In each iteration, a prediction step and a correction step are executed to update the optimization variable vector and the dual variable vector. The iteration process continues until the dual interval is less than the preset convergence threshold, at which point the iteration process is terminated. The solved front wheel steering angle sequence in the control time domain is output. The first element in the sequence is taken as the optimal front wheel steering angle at the current moment and sent to the underlying steering actuator of the agricultural machinery through the vehicle bus to complete the path tracking control.

[0072] In this embodiment, Table 4 is inserted to record the parameter settings and solver configuration parameters of various constraints in the model predictive control model, providing constraint basis and solution configuration for solving the optimal front wheel steering angle.

[0073] Table 4. Correspondence between model predictive control constraints and solution parameters ; In this table, the constraint parameter expressions are the control variables of the corresponding constraints, and the upper and lower limits of the parameters are determined by the physical limits of the agricultural machinery actuator and the adhesion conditions of the driving environment; the slack variable penalty coefficient is used for the optimization of the objective function after constraint transformation; the iterative convergence threshold is the iterative termination condition of the prediction correction interior point method.

[0074] In this embodiment, a dynamic topology graph is constructed based on forward integration of kinematic differential equations, ensuring that the generated trajectory strictly conforms to the nonholonomic kinematic constraints of the agricultural machinery from the bottom layer, thus avoiding infeasible broken lines in the planned path that exceed the minimum turning radius of the agricultural machinery. Adaptive optimization of the planned path is achieved through trajectory cost evaluation and collision detection based on a fused confidence weight matrix. Trajectory nodes with dynamic rollover risk are eliminated through approximate curvature calculation and truncation mechanism of trajectory points. By introducing multi-dimensional physical constraints into the model predictive control model and converting the constraints into mixed integer complementary constraints and solving them using the predictive correction interior point method, overshoot and oscillation of the steering actuator during the control process are suppressed, improving the stability and control accuracy of path tracking.

Claims

1. A method for inter-row path planning and tracking based on multi-source sensing fusion, characterized in that, include: Simultaneously acquire camera images, lidar point clouds, and inertial measurement unit data; Based on the extrinsic calibration matrix of the camera and lidar and the timestamps of each sensor, the ridge pixel coordinates extracted from the image and the ridge point cloud extracted by the radar are projected onto a unified vehicle coordinate system for spatiotemporal synchronization. The visual feature matching score and radar point cloud flatness score are used as the real-time confidence scores of each sensor. A dynamic topology graph is constructed with the current position of the vehicle body as the root node and the trajectory generated by the forward kinematic simulation as the edge. The collision detection and cost evaluation of the edge nodes of the dynamic topology graph are performed using the real-time confidence-weighted fusion perception results, and the optimal planning path is output. A model predictive control model is established that includes constraints on the steering angle and speed of the agricultural machinery. The optimal planned path is input into the model predictive control model as a reference trajectory to solve for the optimal front wheel steering angle at the current moment and send it down to the underlying actuator.

2. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 1, characterized in that, The method of projecting the ridge pixel coordinates extracted from the image and the ridge point cloud extracted by the radar onto a unified vehicle coordinate system for spatiotemporal synchronization based on the extrinsic parameter calibration matrix of the camera and the lidar and the timestamps of each sensor includes: obtaining the nearest neighbor historical synchronization time of the camera and lidar; using the nearest neighbor historical synchronization time as a reference, reading the angular velocity and acceleration integral of the inertial measurement unit between the current time and the nearest neighbor historical synchronization time; and using the angular velocity and acceleration integral to construct the rotation and translation matrix of the vehicle coordinate system at the current time relative to the vehicle coordinate system at the nearest neighbor historical synchronization time. The radar point cloud collected by the lidar at the current moment is transformed into the vehicle coordinate system of the nearest neighbor historical synchronization moment using the rotation and translation matrix; The ridge pixel coordinates extracted from the current moment image are back-projected into three-dimensional space using the camera intrinsic parameter matrix and extrinsic parameter calibration matrix. Then, the three-dimensional space coordinates are transformed into the vehicle coordinate system of the nearest neighbor historical synchronization moment.

3. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 2, characterized in that, The calculation of visual feature matching score and radar point cloud flatness score as the real-time confidence of each sensor includes: dividing the image ridge point set in the vehicle coordinate system into multiple equal-width strips along the vehicle's horizontal axis, calculating the quantity variance and slope variance of the ridge points in each strip, and substituting the quantity variance and slope variance into a preset mapping relationship to calculate the visual feature matching score. In the radar point cloud set in the vehicle coordinate system, the point cloud is divided into multiple grids. Plane fitting is performed on the point cloud in each grid, the fitting residual is calculated, and the mean of the fitting residual is used as the radar point cloud flatness score. The visual feature matching score and the radar point cloud flatness score are normalized respectively. The normalized visual feature matching score and the normalized radar point cloud flatness score are then weighted and summed to obtain the fusion confidence weight matrix.

4. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 3, characterized in that, The construction of a dynamic topology graph with the current position of the vehicle body as the root node and the trajectory generated by the forward kinematic simulation as the edge includes: taking the current position of the vehicle body as the root node, uniformly sampling within the executable range of the front wheel steering angle of the agricultural machinery according to a preset control input sampling period to obtain multiple discrete steering angle inputs; With the current longitudinal vehicle speed as the initial state, the multiple discrete steering angle inputs are substituted into the kinematic differential equation of the agricultural machine for forward integration. The integration step size is the sampling period of the control input, and the total integration time is the prediction time domain length, thus obtaining multiple forward simulation trajectories. The endpoint of each forward simulation trajectory is taken as a candidate child node, and the forward simulation trajectory connecting the root node and the candidate child node is taken as an edge of the dynamic topology graph. Candidate child nodes and their corresponding edges whose endpoints exceed the physical boundary of the furrow are eliminated.

5. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 4, characterized in that, The step of using the real-time confidence-weighted fusion perception result to perform collision detection and cost evaluation on the edge nodes of the dynamic topology graph includes: discretizing the edges of the dynamic topology graph into multiple trajectory sampling points, and mapping the trajectory sampling points to the spatial positions corresponding to the fusion confidence weight matrix to obtain the weight cost of each trajectory sampling point. For each trajectory sampling point, a collision detection window of a preset size is established with the trajectory sampling point as the center. The number of point clouds marked as obstacles in the fused perception results within the collision detection window is counted. When the number of point clouds exceeds a preset threshold, the trajectory sampling point is marked as a collision point. Edges containing collision points are removed. For edges that do not contain collision points, the sum of the weighted costs of all trajectory sampling points on that edge is calculated as the total cost of the edge.

6. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 5, characterized in that, The establishment of a model predictive control model that includes agricultural machinery steering angle velocity constraints and speed constraints, and the input of the optimal planned path as a reference trajectory into the model predictive control model to solve for the optimal front wheel steering angle at the current moment, includes: establishing a state space equation with the agricultural machinery lateral deviation, heading deviation and front wheel steering angle as state variables; Within the prediction time domain of the state-space equation, an objective function is constructed with the goal of minimizing the sum of squares of the deviations between the state variables and the corresponding points of the reference trajectory. The objective function is constrained by adding upper and lower limits of front wheel steering angle based on the physical limits of agricultural machinery steering tie rod, upper and lower limits of steering angle change at adjacent sampling times based on the response characteristics of steering hydraulic cylinder, and upper and lower limits of longitudinal vehicle speed based on the limits of tire-soil adhesion. The objective function with added constraints is then solved using a quadratic programming solver.

7. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 6, characterized in that, The process of obtaining the nearest neighbor historical synchronization time of the camera and the lidar includes: maintaining a timestamp circular buffer in the memory of the agricultural machinery vehicle computing platform, wherein the timestamp circular buffer records the arrival timestamp of the camera image and the arrival timestamp of the lidar point cloud. When a camera image or LiDAR point cloud is received at the current moment, the historical timestamp of the other sensor with the smallest time difference from the current moment is queried in the timestamp circular buffer, and the historical timestamp is used as the nearest neighbor historical synchronization moment. If the time difference is greater than the preset maximum allowable time difference threshold, the current camera image or LiDAR point cloud is discarded, and a hardware trigger signal is sent to the corresponding sensor to resynchronize the exposure time and scan start and end time.

8. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 7, characterized in that, After the normalized visual feature matching score and the normalized radar point cloud flatness score are weighted and summed to obtain the fusion confidence weight matrix, the method further includes: establishing a sliding window queue with a preset number of frames, storing the fusion confidence weight matrix calculated for the current frame into the sliding window queue, and removing the fusion confidence weight matrix of the earliest frame in the sliding window queue. For the historical fusion confidence weight matrix sequence at the same spatial grid position in the sliding window queue, calculate the median of the historical fusion confidence weight matrix sequence; Replace the value at the corresponding spatial grid position in the current frame's fusion confidence weight matrix with the median, and output the smoothed fusion confidence weight matrix.

9. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 8, characterized in that, The method of using the forward simulation trajectory connecting the root node and the candidate child node as the edge of the dynamic topology graph, and removing the candidate child node and the corresponding edge whose endpoint exceeds the physical boundary of the furrow, further includes: extracting the position coordinates of the trajectory points according to the time step order of the forward integration for the retained forward simulation trajectory. Using the position coordinates of three adjacent trajectory points, the approximate curvature at each trajectory point is calculated based on the vector cross product; Determine whether the approximate curvature is greater than the maximum permissible curvature calculated based on the current longitudinal speed of the agricultural machinery and the preset maximum lateral acceleration threshold; If there is a trajectory point in the forward simulation trajectory whose approximate curvature is greater than the maximum allowable curvature, then the forward simulation trajectory is truncated at the trajectory point where the approximate curvature is greater than the maximum allowable curvature, and the truncated forward simulation trajectory is used as a new edge to update the candidate child node position.

10. The inter-row path planning and tracking method based on multi-source sensing fusion according to claim 9, characterized in that, The addition of upper and lower limits of the front wheel steering angle based on the physical limits of the agricultural machinery steering tie rod to the objective function includes: introducing a relaxation variable with the same number of front wheel steering angles, and transforming the upper and lower limits of the front wheel steering angle into a mixed integer complementary constraint condition combining equality constraints and inequality constraints; By adding the penalty term coefficient of the slack variable to the objective function, the objective function containing the mixed integer complementary constraints and the added penalty term coefficient is transformed into the standard form of a linear complementarity problem. The coefficient matrix in the standard form of the linear complement problem is decomposed into blocks. The matrix after block decomposition is iteratively solved using the predictive correction interior point method until the dual interval is less than the preset convergence threshold. The optimal front wheel steering angle at the current moment is then output.

Images