Trajectory tracking method, trajectory tracking device, and storage medium

Abstract

The application discloses a trajectory tracking method, a trajectory tracking device and a storage medium, and relates to the technical field of automatic driving. The trajectory tracking method comprises the following steps: acquiring pose information of a vehicle, wherein the pose information is collected by a positioning receiver arranged on the vehicle and at least comprises position and heading information of the positioning receiver; taking the position of the positioning receiver as a control reference point of the vehicle, calculating a steering control amount of the vehicle based on the pose information and a pre-acquired target trajectory; and controlling movement of the vehicle according to the steering control amount; wherein the target trajectory is composed of a plurality of trajectory points, each trajectory point contains position, heading and curvature information of the trajectory point, a trajectory segment between any two adjacent trajectory points is constrained to be a straight line or a circular arc with a fixed curvature, and adjacent trajectory segments are connected tangentially. The application can simultaneously consider work efficiency and work accuracy.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, specifically to a trajectory tracking method, a trajectory tracking device, and a storage medium. Background Technology

[0002] With the acceleration of agricultural modernization, automatic driving technology for agricultural machinery such as tractors has become a core support for improving operational efficiency and ensuring operational accuracy. Among these, the accurate representation and stable tracking of curved trajectories are key technical aspects for achieving automated agricultural machinery operations (such as sowing, fertilizing, and tilling). In existing agricultural machinery automatic driving technologies, the generation and tracking of curved trajectories mainly employ two schemes: one involves manually driving the agricultural machinery, recording the driving path with a positioning receiver, and then smoothing the path using tools such as quadratic optimization solvers to generate a trackable operational trajectory; the other involves directly deriving the target curved trajectory through theoretical calculations. In the tracking control stage, classic control algorithms such as LQR (linear quadratic regulator) are commonly used to achieve lateral control of the agricultural machinery along the target trajectory.

[0003] However, existing technologies still have significant shortcomings, making it difficult to meet the actual needs of efficient and high-precision agricultural machinery operations. On the one hand, existing trajectory generation methods, whether through post-recording smoothing optimization or theoretical calculation derivation, require processing massive amounts of path data. Moreover, the generation process relies on complex mathematical operations, resulting in a large computational load and significant consumption of the agricultural machinery's computing resources. This not only affects the trajectory generation speed but also limits the generation and application of ultra-long-distance operation trajectories. On the other hand, in the trajectory tracking control process, the yaw angle is a core control parameter, and its measurement accuracy directly affects the control precision. However, in practical applications, due to factors such as receiver installation location and vehicle structure, it is difficult to accurately obtain stable and reliable yaw angle data. This problem is particularly prominent in scenarios where agricultural machinery travels at high curvature, leading to additional correction and control errors, resulting in decreased trajectory tracking precision and affecting the quality of agricultural operations.

[0004] Therefore, existing technologies related to the trajectory of automatic driving of agricultural machinery cannot simultaneously achieve both operational efficiency and operational accuracy. Summary of the Invention

[0005] The purpose of this application is to provide a trajectory tracking method, trajectory tracking device, and storage medium to solve the problem that existing agricultural machinery automatic driving curve trajectory related technologies cannot simultaneously achieve both operational efficiency and operational accuracy.

[0006] To achieve the above objectives, the first aspect of this application provides a trajectory tracking method, the method comprising: The vehicle's pose information is acquired through a positioning receiver installed on the vehicle, and includes at least the position and heading information of the positioning receiver. Using the position of the positioning receiver as the control reference point of the vehicle, the steering control quantity of the vehicle is calculated based on the pose information and the pre-acquired target trajectory. The vehicle movement is controlled according to the steering control amount; The target trajectory consists of multiple trajectory points. Each trajectory point contains the position, heading, and curvature information of the trajectory point. The trajectory segment between any two adjacent trajectory points is constrained to be a straight line or a circular arc with a fixed curvature, and the adjacent trajectory segments are tangent to each other.

[0007] A second aspect of this application provides a trajectory tracking device, the device comprising: The first acquisition module is used to acquire the vehicle's pose information, which is collected by a positioning receiver installed on the vehicle and includes at least the position and heading information of the positioning receiver. The first calculation module is used to calculate the vehicle's steering control quantity based on the pose information and the pre-acquired target trajectory, using the position of the positioning receiver as the vehicle's control reference point. The control module is used to control the movement of the vehicle according to the steering control amount; The target trajectory consists of multiple trajectory points. Each trajectory point contains the position, heading, and curvature information of the trajectory point. The trajectory segment between any two adjacent trajectory points is constrained to be a straight line or a circular arc with a fixed curvature, and the adjacent trajectory segments are tangent to each other.

[0008] A third aspect of this application provides a machine-readable storage medium storing instructions that cause a machine to perform a trajectory tracking method according to the first aspect.

[0009] In this application, firstly, the target trajectory is composed of trajectory points with fully defined positions, headings, and curvatures. Trajectory segments between any adjacent points are strictly constrained to be straight lines or fixed-curvature circular arcs, ensuring tangential connections. This design gives the trajectory clear and concise geometric characteristics, eliminating the need for complex smoothing optimization or theoretical derivation of massive path data. This significantly reduces the storage and computational burden of trajectory data, saves onboard computing resources, and supports the rapid generation and efficient processing of ultra-long-distance operational trajectories. Secondly, in the tracking and control stage, the vehicle pose is directly constructed using the position and heading information collected by the positioning receiver, and the receiver position is used as the sole control reference point to calculate the steering control input. This method avoids indirect measurement or estimation of the vehicle's yaw angle, eliminating heading measurement errors caused by installation deviations, vehicle structure, and other factors. It also ensures the accuracy and stability of control parameters, especially during high-curvature turns. In summary, this method achieves lightweighting at the trajectory generation stage and improves robustness at the tracking and control stage, thus simultaneously achieving a significant improvement in operational efficiency and trajectory tracking accuracy under limited computing power.

[0010] Other features and advantages of the embodiments of this application will be described in detail in the following detailed description section. Attached Figure Description

[0011] The accompanying drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the following detailed description to explain the embodiments of this application, but do not constitute a limitation on the embodiments of this application. In the drawings: Figure 1 The illustration shows a schematic flowchart of a trajectory tracking method according to an embodiment of this application; Figure 2 The illustration shows a schematic diagram representing a target trajectory according to an embodiment of this application; Figure 3 A schematic diagram illustrating a target trajectory generated by recording according to an embodiment of this application is shown. Figure 4 A schematic diagram illustrating delay compensation according to an embodiment of this application is shown. Figure 5 This illustration schematically shows a calculation diagram of the reference front wheel steering angle according to an embodiment of this application; Figure 6 This illustration schematically shows an interpolation diagram of the original geometric trajectory according to an embodiment of this application; Figure 7 A schematic diagram of the hardware structure of the operating device according to an embodiment of this application is shown. Detailed Implementation

[0012] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only for illustration and explanation of the embodiments of this application and are not intended to limit the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0013] It should be noted that the acquisition, transmission, storage, use, and processing of data in the technical solution of this application all comply with relevant laws and regulations. In the embodiments of this application, certain existing industry solutions such as software, components, and models may be mentioned. These should be considered exemplary, intended only to illustrate the feasibility of implementing the technical solution of this application, and do not imply that the applicant has already used or necessarily used such solutions.

[0014] It should be noted that if the embodiments of this application involve directional indicators (such as up, down, left, right, front, back, etc.), the directional indicators are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicators will also change accordingly.

[0015] Furthermore, if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, features defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0016] It should be noted that the trajectory tracking method provided in the subsequent embodiments of this application can be applied to any working equipment. For the purpose of clearly illustrating the technical solution, the application of the trajectory tracking method to working equipment is used as an example to illustrate the embodiments.

[0017] The operating equipment of this application is described below with reference to the accompanying drawings.

[0018] Figure 1 A schematic flowchart of a trajectory tracking method according to an embodiment of this application is shown. Figure 1 As shown in the figure, this application provides a trajectory tracking method, which may include the following steps.

[0019] Step 101: Obtain the vehicle's pose information, which is collected by a positioning receiver installed on the vehicle and includes at least the position and heading information of the positioning receiver. In this embodiment, the vehicle's pose information is acquired through a positioning receiver installed near the center of the vehicle. This positioning receiver is a data acquisition device that integrates GNSS positioning technology and an IMU (Inertial Measurement Unit). Its core function is to obtain the vehicle's real-time position coordinates through GNSS positioning, and simultaneously perform fusion calculations using angle sensor data from the IMU to output the heading information (i.e., navigation angle) at the location of the positioning receiver. This navigation angle has been verified to be highly consistent with the horizontal velocity direction at the receiver's location and can be directly used as the heading parameter in the pose information. The acquired pose information includes at least the two-dimensional position coordinates (x, y) and heading angle (θ) of the positioning receiver. These data provide the basic input for subsequent trajectory tracking control references and steering control calculations. The acquisition frequency matches the fixed time difference between two fused positioning data sets from the receiver, ensuring the real-time nature and continuity of the data. Furthermore, the fusion of GNSS and IMU improves the stability and accuracy of the pose information in complex operating environments, laying a data foundation for subsequent precise control.

[0020] Step 102: Using the position of the positioning receiver as the control reference point of the vehicle, calculate the steering control amount of the vehicle based on the pose information and the pre-acquired target trajectory; In this embodiment, firstly, in the prior art, there is an installation distance between the receiver and the rear axle center, which makes it impossible to accurately obtain the true yaw angle of the rear axle center. However, in this embodiment, the receiver position can be used as a reference point, and the pose information collected by the receiver can be directly used for control calculation, avoiding additional correction and control errors caused by reference point offset, thereby improving the accuracy of control.

[0021] Secondly, the pre-acquired target trajectory has specific geometric characteristics: the trajectory is formed by combining multiple trajectory points, each containing three core pieces of information: position, heading, and curvature. Furthermore, the trajectory segment between any two adjacent trajectory points is strictly constrained to be a straight line or a circular arc with a fixed curvature, and adjacent trajectory segments must satisfy a tangent connection. For example, if the preceding trajectory segment is a straight line (corresponding to 0 curvature), then the subsequent connecting circular arc trajectory segment must be tangent to that straight line; if adjacent trajectory segments are both circular arcs, then the two circular arcs must remain tangent. This constraint design ensures good geometric continuity of the target trajectory, not only facilitating subsequent rapid interpolation to generate a trackable trajectory but also creating conditions for improving the smoothness and accuracy of trajectory tracking.

[0022] As an example, such as Figure 2 As shown, the target trajectory is represented as follows: Figure 2 The trajectory runs from point A to point H, represented by a total of 8 points AH. The starting point A is the first point, containing five parameters: coordinates (x0, y0), heading θ0, curvature k0, and cumulative distance dis0. Since the distance from A to the next point is a straight line, the curvature k0 = 0. The second point B, the yellow point on the diagram, has coordinates (x1, y1), and dis1 = distance from A to B + dis0. Since the distance from A to B is a straight line, the heading at B remains unchanged, i.e., θ1 = θ0. After point B, the trajectory begins to curve, with curvature k1 between B and C. Note that if an arc is drawn from B towards C with tangent at angle θ1 and curvature k1, then point C (x2, y2) must lie on this arc, and the arc distance between B and C is equal to dis2 - dis1, and the heading θ2 at C is equal to the tangent of the arc with curvature k1 at point C. Next, the arc between points C and D has a greater curvature, and the arc between C and D is tangent to the arc between C and D. All adjacent points must satisfy this geometric constraint. The final point H is similar to all the previous points, except that since point H is the endpoint, there will be no further trajectory segments, so kappa is invalid at this point. The other parameters have the same meaning as the previous points.

[0023] The target trajectory can be automatically generated by the planning program or generated by manually driving the vehicle and recording it. The recorded trajectory is as follows: Figure 3 As shown.

[0024] Finally, the calculation of the steering control quantity is based on the real-time pose information of the positioning receiver and the target trajectory with the aforementioned characteristics. Combined with the subsequently adapted trajectory tracking algorithm (such as pure tracking, LQR, and PID integrated control algorithm), the steering control quantity used to control the lateral movement of the vehicle is calculated by analyzing the deviation between the vehicle's current pose and the target trajectory. The steering control quantity can be the target steering angle of the front wheels.

[0025] Step 103: Control the vehicle movement according to the steering control amount; The target trajectory consists of multiple trajectory points. Each trajectory point contains the position, heading, and curvature information of the trajectory point. The trajectory segment between any two adjacent trajectory points is constrained to be a straight line or a circular arc with a fixed curvature, and the adjacent trajectory segments are tangent to each other.

[0026] In this embodiment, the obtained steering control quantity is sent to the vehicle's steering execution system to achieve precise control of the vehicle's movement. The vehicle is an agricultural machine (such as a tractor) using an Ackerman chassis. Its steering execution system includes core components such as a steering wheel motor and a hydraulic steering system. The steering control quantity drives the system to adjust the actual steering angle of the front wheels, thereby changing the lateral movement direction of the vehicle and causing the vehicle to travel along the pre-acquired target trajectory.

[0027] In this embodiment, firstly, the target trajectory is composed of trajectory points with fully defined positions, headings, and curvatures. Trajectory segments between any adjacent points are strictly constrained to be straight lines or fixed-curvature circular arcs, ensuring tangential connections. This design gives the trajectory clear and concise geometric characteristics, eliminating the need for complex smoothing optimization or theoretical derivation of massive path data. This significantly reduces the storage and computational burden of trajectory data, saves onboard computing resources, and supports the rapid generation and efficient processing of ultra-long-distance operational trajectories. Secondly, in the tracking control stage, the vehicle pose is directly constructed using the position and heading information collected by the positioning receiver, and the receiver position is used as the sole control reference point to calculate the steering control quantity. This method avoids indirect measurement or estimation of the vehicle's yaw angle, eliminating heading measurement errors caused by installation deviations, vehicle structure, and other factors. It also ensures the accuracy and stability of control parameters, especially during high-curvature turns. In summary, this method achieves lightweighting at the trajectory generation stage and improves robustness at the tracking control stage, thus simultaneously achieving a significant improvement in operational efficiency and trajectory tracking accuracy under limited computing power.

[0028] In one embodiment of this application, the step of using the position of the positioning receiver as the control reference point of the vehicle, and calculating the steering control amount of the vehicle based on the pose information and the pre-acquired target trajectory, includes: Obtain the lateral distance between the position of the positioning receiver and the target trajectory; When the lateral distance is greater than a first preset distance, the steering control quantity is calculated using a pure tracking algorithm based on the pose information and the pre-acquired target trajectory; When the lateral distance is less than or equal to a first preset distance, the steering control quantity is calculated based on the pose information and the pre-acquired target trajectory using a linear quadratic regulator model with the positioning receiver as the control reference point.

[0029] In this embodiment, the lateral distance refers to the vertical distance from the current position of the positioning receiver to the target trajectory. Its calculation is based on the geometric features of the target trajectory and the receiver's real-time position information: utilizing the characteristic that the target trajectory consists of continuous trajectory points with position, heading, and curvature, and that adjacent trajectory segments are tangent, the nearest projection point of the receiver's current position on the target trajectory is first found. Then, the vertical distance between the two points is obtained through geometric calculation, which is the lateral distance. This lateral distance directly reflects the degree of deviation between the vehicle and the target trajectory.

[0030] Secondly, based on the relationship between the lateral distance and the first preset distance, the corresponding algorithm is selected to calculate the steering control quantity: When the lateral distance exceeds a first preset distance, the vehicle is moving away from the target trajectory. The core requirement at this point is to quickly bring the vehicle closer to the target trajectory; therefore, a pure tracking algorithm is used to calculate the steering control input.

[0031] When the lateral distance is less than or equal to a first preset distance, the vehicle has approached the target trajectory. At this point, the core requirement shifts to high-precision and stable tracking. Therefore, a linear quadratic regulator (LQR) model based on the positioning receiver as the control reference point is used to calculate the steering control quantity. This LQR model breaks through the conventional design of using the rear axle center of the vehicle as the control reference point in existing technology. It directly uses the position of the positioning receiver as the control reference point, avoiding yaw angle errors and additional correction requirements caused by the installation distance between the receiver and the rear axle center, thus significantly improving control accuracy.

[0032] In this embodiment, by adaptively switching the algorithm based on lateral distance, the problem of rapid convergence when the vehicle is far from the trajectory is solved, and the high-precision tracking requirement is met when the vehicle is close to the trajectory. At the same time, relying on the design advantage of the positioning receiver as the control reference point, the control error in the prior art is effectively reduced, and the overall performance of trajectory tracking is further improved.

[0033] In one embodiment of this application, based on the pose information and the pre-acquired target trajectory, the steering control quantity is calculated using a linear quadratic regulator model with the positioning receiver as the control reference point, including: Based on the pose information, the projection point of the positioning receiver on the target trajectory is determined, and the projection point is the point where the positioning receiver is closest to the target trajectory; The steering control quantity is calculated using a linear quadratic regulator model based on the projection point as the control reference point.

[0034] In this embodiment, firstly, based on the real-time pose information of the positioning receiver, its projection point on the target trajectory is determined. This projection point is defined as the point closest to the target trajectory from the current position of the positioning receiver. Specifically, in the solution process, the real-time position coordinates (x, y) and heading angle (θ) of the positioning receiver are used, combined with the geometric characteristics of the target trajectory (a straight line or a continuous trajectory segment of a circular arc with a fixed curvature). The trajectory points and segments on the target trajectory are traversed, and the trajectory segment with the smallest distance to the receiver's current position is selected through geometric distance calculations (such as the perpendicular distance from a point to a line, or the shortest distance formula from a point to an arc). Then, based on the straight line or circular arc properties of this trajectory segment, the perpendicular projection point of the receiver's position on this trajectory segment is solved using analytical geometry methods; this is the required projection point.

[0035] Secondly, an LQR model based on the projection point as the control reference point is used to calculate the steering control quantity. The core design here is to use the projection point as the direct reference object for LQR control, combined with the characteristic of the positioning receiver as the vehicle control reference point, to form a precise alignment relationship between "receiver real-time position and trajectory projection point". Specifically, the input of the LQR model includes two core pieces of information: first, the real-time pose information (position, heading) of the positioning receiver, reflecting the current actual motion state of the vehicle; second, the state information (position, heading, curvature) of the projection point, reflecting the ideal motion state of the target trajectory at the current reference position. During the model construction process, the ideal state of the projection point is used as the control target. By calculating the deviation between the real-time state of the receiver and the ideal state of the projection point (such as lateral error, heading error, etc.), the LQR state-space equation is constructed. Then, based on the kinematic characteristics of the Ackerman chassis agricultural machinery, the optimal control quantity that minimizes the deviation, i.e., the steering control quantity, is solved.

[0036] In one embodiment of this application, determining the projection point of the positioning receiver on the target trajectory based on the pose information includes: Based on the pose information, the vehicle's motion state, and the pre-calibrated delay time, the predicted pose of the positioning receiver after the delay time is obtained. Based on the predicted pose, at least two trajectory points on the target trajectory are determined, and the distances between the at least two trajectory points and the positioning receiver satisfy a preset condition; The projection point is obtained by linear interpolation based on the at least two trajectory points.

[0037] In this embodiment, firstly, based on the real-time pose information of the positioning receiver, the vehicle's motion state, and the pre-calibrated delay time, the predicted pose of the positioning receiver after the delay time is predicted. In the scenario of automated driving of agricultural machinery, the steering control command is affected by the response delay of the actuators such as the steering wheel motor and hydraulic steering system from issuance to execution. If the projection point is directly calculated based on the current pose information, it will cause a deviation between the control reference and the actual motion state of the vehicle, affecting the tracking accuracy. Therefore, this step first obtains the delay time through pre-calibration. T (the time interval between the control command being issued and the actuator producing the actual steering action, which needs to be calibrated experimentally based on the specific steering system characteristics of the vehicle); then, real-time motion parameters of the vehicle are collected, including the linear velocity v of the rear axle center (calculated from receiver fusion data) and the yaw rate ω (acquired based on IMU sensor data); finally, based on the Ackerman chassis bicycle model and combined with real-time pose information, kinematic prediction is performed to calculate the delay time. Predicted pose after T.

[0038] As an example, such as Figure 4 As shown, the vehicle's current reference position is at point F0, with coordinates (x0, y0) and direction of travel θ0. Due to the lag between the issuance and execution of vehicle control commands, particularly in the steering wheel motor and the vehicle's steering system (especially the hydraulic steering system), it is necessary to calibrate the control delay. T, and propel the vehicle forward according to kinematic relationships. At time T, the predicted position point F of the vehicle is obtained, and subsequent LQR tracking control is performed based on point F.

[0039] The specific prediction formula for the pose is as follows:

[0040] Where dx, dy, and dθ are the changes in coordinates and direction, x0, y0, and θ0 are the vehicle's current coordinates and direction at point F0, x, y, and θ are the vehicle's coordinates and direction at point F, v is the linear velocity of the vehicle's rear axle center, and ω is the vehicle's yaw rate. T represents the control delay time.

[0041] Secondly, based on the predicted pose, at least two trajectory points on the target trajectory that meet preset conditions are determined. The preset condition here is that "the distance between the trajectory point and the predicted pose of the positioning receiver meets the nearest distance requirement". The specific selection process is as follows: First, using the position coordinates of the predicted pose as a reference, the trajectory points on the target trajectory are traversed, and the straight-line distance between each trajectory point and the predicted position is calculated; then, based on the distance sorting results, the two closest trajectory points (denoted as A1 and A2) are selected, ensuring that these two trajectory points are located on both sides of the projection point of the predicted position on the target trajectory.

[0042] Finally, linear interpolation is performed on at least two selected trajectory points to obtain the projection point. Since the adjacent trajectory segments of the target trajectory are straight lines or circular arcs with fixed curvature, and the adjacent trajectory segments are tangent, the trajectory segment between the two selected trajectory points A1 and A2 has good smoothness, meeting the accuracy requirements of linear interpolation. In the specific interpolation process, the core parameters of the two trajectory points are first obtained, including position coordinates, heading angle, curvature, and cumulative distance; then, the interpolation weight is calculated based on the distance ratio between the predicted position and the two trajectory points; finally, linear interpolation is performed on the position coordinates, heading angle, curvature, and other parameters based on this weight to obtain the complete state information of the projection point R, including: coordinates (xr, yr), direction θr, and curvature kr.

[0043] In this embodiment, the projection point is accurately obtained by predicting the pose through delay compensation, filtering the nearest neighbor points of the target trajectory and linear interpolation, which solves the reference deviation caused by the delay of the steering system, provides a reliable benchmark for subsequent LQR control, and effectively improves the real-time performance and accuracy of trajectory tracking.

[0044] In one embodiment of this application, calculating the steering control quantity using a linear quadratic regulator model based on the projection point as the control reference point includes: Based on the state information of the projection point and the vehicle parameters, the reference front wheel angle is calculated. The state information of the projection point includes at least heading and curvature, and the vehicle parameters include at least wheelbase and the distance from the positioning receiver to the center of the rear axle of the vehicle. A vehicle kinematic model is established with the positioning receiver as the control reference point. Based on the vehicle kinematic model, the state information of the projection point, and the pose information, a state space equation is constructed. The state variables of the state space equation include lateral error and heading error. Based on the state-space equation, the error front wheel angle is obtained by solving the linear quadratic regulator algorithm. The steering control value is obtained by superimposing the error front wheel angle with the reference front wheel angle.

[0045] In this embodiment, firstly, in the state information of the projection point, curvature directly reflects the degree of curvature of the target trajectory at that point (curvature of straight segments is 0, and curvature of circular segments is a fixed value), determining the basic turning radius required by the vehicle; heading is the tangential direction of the trajectory at that point, providing the reference direction for turning. Among the vehicle parameters, wheelbase L is a core dimension parameter of the Ackerman chassis (the distance between the center of the front axle and the center of the rear axle), directly affecting the mapping relationship between the steering angle and the driving trajectory; the distance d1 from the positioning receiver to the center of the vehicle's rear axle is a key adaptation parameter of this scheme. Since the control reference point is the receiver rather than the center of the rear axle, this distance must be used to correct the angle calculation error.

[0046] like Figure 5 As shown, Figure 5 This is a schematic diagram for calculating the steering angle of the front wheel. The trajectory reference point R has a direction of θr and a curvature of k. r As shown in the four figures below, green represents the target trajectory. If the positioning receiver reference point theoretically moves strictly according to the trajectory, then at point R on the trajectory, the vehicle's receiver position must also be at point R. At this time, the center of the front axle is point C, the center of the rear axle is point B, the navigation running angle must be θr, and the reference front wheel rotation angle is δr to be determined.

[0047] Since the reference curvature at point R is kr, therefore:

[0048] In the diagram, point A is the center of the circle in which point R moves in a circular motion with radius r. At the same time, points B and C also move in a circular motion around point A, with radius r at point B. b In triangle ABR, d1 is the known distance from the receiver to the center of the rear axis. r can be calculated using the Pythagorean theorem. b :

[0049] Then, in triangle ABC, L is the vehicle wheelbase, and δr equals ∠CAB, so we have:

[0050] In summary, we can finally calculate:

[0051] Secondly, in discrete space, the most important aspect of applying the LQR control method is to represent the system using the following state-space equations:

[0052] Where X is the system state variable and u is the output control variable. In the vehicle motion control of this paper, since we only care about the lateral motion control of the vehicle, we define the state variable and control variable matrices of the LQR state-space equation as follows:

[0053] Based on the vehicle kinematics model of the Ackermann chassis bicycle model (assuming that the rear axle center and navigation receiver positioning point both satisfy the following): (1) Where v is the linear velocity of the positioning receiver, θ is the navigation operating angle, δ is the front wheel steering angle of the vehicle, and L is the wheelbase of the vehicle.

[0054] The core of the state-space equation is to quantify the "deviation between the actual vehicle state and the ideal state of the projection point," and the selection of its state variables directly determines the control accuracy. In this embodiment, the state variables explicitly include lateral error and heading error: the lateral error is the deviation between the receiver's current position and the projection point position in the direction perpendicular to the trajectory (reflecting the distance the vehicle deviates from the trajectory), and the heading error is the difference between the receiver's current heading angle and the projection point's heading angle (reflecting the deviation of the vehicle's driving direction from the trajectory direction). During the construction process, using the ideal state of the projection point (ideal position, ideal heading) as a benchmark, the difference between the vehicle's actual pose information (receiver real-time position, heading) and the ideal state is calculated to obtain the error state vector; then, based on the above kinematic model, the error state is linearized (ignoring higher-order small terms), and partial derivatives are obtained by combining the Jacobian matrix, finally yielding the state-space equation in discrete space. Specifically, At the trajectory reference point R, we have:

[0055] Performing a Taylor expansion of the vehicle state X at the reference point, ignoring terms of order one and above, yields: (2) make:

[0056] That is, let ~X be the error between the current state of the vehicle and the state at the reference point. Since Xr and ur are known at the reference point, according to the previous formula (1), by taking the partial derivative through the Jacobian matrix, we can obtain:

[0057] Equation (2) can be rewritten in the following form: (3) In discrete space, the forward Euler method can be used, that is: (4) Where T is the time interval between two consecutive discrete states k+1 and k, which is the fixed time difference between the two fused positioning data of the receiver. In equation (3), time k is chosen, that is:

[0058] Combining equation (4) and the previous derivation, we can obtain:

[0059] At this point, the LQR state equations in the discrete space have been obtained.

[0060] Then, by solving the standard discrete-space Racatti equations in the LQR control method, the linear feedback coefficient matrix K can be obtained, and then:

[0061] Finally, the steering control value is obtained as follows:

[0062] That is, the sum of the error front wheel steering angle and the reference front wheel steering angle.

[0063] In this embodiment, the receiver is used as the control reference point for modeling. By combining the reference front wheel angle with the error angle superposition logic obtained by LQR, the trajectory deviation is accurately corrected, the tracking accuracy is improved, and the steering is smooth and stable. This can efficiently meet the stringent operational accuracy requirements of agricultural machinery automatic driving.

[0064] In one embodiment of this application, after calculating the vehicle's steering control amount based on the pose information and the pre-acquired target trajectory, using the position of the positioning receiver as the vehicle's control reference point, and before controlling the vehicle's movement according to the steering control amount, the method further includes: Calculate the real-time lateral deviation between the position of the positioning receiver and the target trajectory; Based on the real-time lateral deviation, the steering compensation amount is calculated by a proportional-integral-derivative controller. The steering compensation amount and the steering control amount are superimposed to obtain the target control amount; The step of controlling the vehicle movement according to the steering control amount includes: The vehicle movement is controlled according to the target control quantity.

[0065] In this embodiment, when calculating the real-time lateral deviation, the system first traverses the target trajectory based on the receiver's current position to find the nearest trajectory segment. Then, it calculates the vertical distance from the receiver's position to this trajectory segment using analytical geometry, which is the real-time lateral deviation. The sign of this deviation can be defined according to the trajectory direction (e.g., positive for the left side of the trajectory and negative for the right side), which not only reflects the degree of deviation but also clarifies the direction of deviation.

[0066] The PID controller takes real-time lateral deviation as input and outputs steering compensation. The proportional (P) terminator directly outputs the compensation based on the deviation magnitude, enabling rapid response. The integral (I) terminator accumulates historical deviations, eliminating static deviations (such as continuous small deviations caused by uneven ground or mechanical wear). The derivative (D) terminator has its coefficient set to 0 to prevent oscillations in the compensation due to sudden deviation changes (such as small fluctuations in receiver data), ensuring smooth steering.

[0067] To adapt to different stages of the curve-tracking state machine, the PID controller can dynamically adjust the proportional (P) coefficient without changing the cumulative result of the integral term: a larger P coefficient is used when approaching the state to accelerate the convergence of the deviation; a smaller P coefficient is used when in the online state to avoid trajectory fluctuations caused by over-response. Simultaneously, to prevent excessive compensation from causing sudden changes in the front wheel steering angle, a maximum limit is set on the steering compensation amount output by the PID controller to ensure the gentleness and safety of the compensation action.

[0068] By using the PID control formula and dynamically adjusted parameters, the real-time steering compensation amount is calculated. This compensation amount corresponds to the direction of the lateral deviation, achieving precise directional correction.

[0069] Finally, the superimposed target control quantity is sent to the vehicle's steering execution system to control the front wheels to perform the corresponding steering action and drive the vehicle to travel along the target trajectory.

[0070] In this embodiment, the real-time lateral deviation is finely compensated by the PID controller, and the target control quantity is formed by superimposing the LQR control quantity. This effectively eliminates residual deviation, dynamically adapts to the working conditions, enhances anti-interference ability, and significantly improves the accuracy and steady-state stability of agricultural machinery trajectory tracking.

[0071] In one embodiment of this application, the pre-acquired target trajectory is generated through the following steps: Obtain the original geometric trajectory defined by the plurality of trajectory points; Between any two adjacent trajectory points of the original geometric trajectory, interpolation is performed based on the constraints of the straight line or the circular arc with a fixed curvature to generate at least one interpolation point. The trajectory points of the original geometric trajectory and the generated interpolation points together constitute the target trajectory.

[0072] In this embodiment, as Figure 6 As shown, A, B, C, and D represent the original points of the geometric trajectory. L1 and L2 are the interpolation distances, typically around 0.1m and no greater than 0.15m. The blue dots are interpolation points; the positions of the interpolation points must lie on the original geometric trajectory and also satisfy the previous geometric relationships. Specifically: 1. On the line segment AB, the interpolation points lie on the ray originating from point A and with a direction of θ0, which is the line AB. The angle θ0 at each interpolation point is... x All are equal to θ0, and the heading θ0x is also equal to θ0. dis0 x - dis0 x-1 It equals L1.

[0073] 2. In the BC arc segment, the interpolation point lies on the arc with B as the starting point, θ1 as the tangent direction of the arc, and 1 / k1 as the radius. The angle θ1 of the interpolation point is...x Let k1 be the direction of the tangent at that point on the arc. x Equal to k1, dis1 x - dis1 y It equals L2.

[0074] When using this interpolation method, first find the point on the original geometric trajectory that is closest to the vehicle position. Then, based on this point, interpolate a distance between the points before and after the geometric trajectory to generate a target curve trajectory for the final curve tracking control.

[0075] The original geometric trajectory can be obtained in two ways. One is through automatic generation by the planning program. Based on parameters such as the boundary of the work area and the requirements of the work path, the algorithm automatically calculates the trajectory points that meet the geometric constraints, thus forming the original trajectory. The other is through recording by manually driving agricultural machinery. When manually driving agricultural machinery along the expected work path, the navigation receiver collects the trajectory point parameters of key nodes in real time, and the original geometric trajectory is formed after screening.

[0076] In this embodiment, by generating the target trajectory, the problems of large amount of trajectory data and high resource consumption in the prior art can be solved. The generated target trajectory is geometrically continuous and smooth, with fast generation speed, low computing power requirement, and reasonable data volume, which can provide a reliable foundation for subsequent high-precision trajectory tracking.

[0077] Figure 7 A schematic diagram of the trajectory tracking device provided in another embodiment of this application is shown. For ease of explanation, only the parts related to the embodiments of this application are shown.

[0078] Reference Figure 7 The trajectory tracking device 700 may include: The first acquisition module 701 is used to acquire the vehicle's pose information, which is collected by a positioning receiver installed on the vehicle and includes at least the position and heading information of the positioning receiver. The first calculation module 702 is used to calculate the vehicle's steering control quantity based on the pose information and the pre-acquired target trajectory, using the position of the positioning receiver as the control reference point of the vehicle. Control module 703 is used to control the movement of the vehicle according to the steering control amount; The target trajectory consists of multiple trajectory points. Each trajectory point contains the position, heading, and curvature information of the trajectory point. The trajectory segment between any two adjacent trajectory points is constrained to be a straight line or a circular arc with a fixed curvature, and the adjacent trajectory segments are tangent to each other.

[0079] Optionally, the first computing module 702 includes: The acquisition submodule is used to acquire the lateral distance between the position of the positioning receiver and the target trajectory; The first calculation submodule is used to calculate the steering control quantity based on the pose information and the pre-acquired target trajectory using a pure tracking algorithm when the lateral distance is greater than the first preset distance. The second calculation submodule is used to calculate the steering control quantity based on the pose information and the pre-acquired target trajectory, using a linear quadratic regulator model with the positioning receiver as the control reference point, when the lateral distance is less than or equal to the first preset distance.

[0080] Optionally, the second calculation submodule includes: The first determining unit is configured to determine, based on the pose information, the projection point of the positioning receiver on the target trajectory, wherein the projection point is the closest point of the positioning receiver to the target trajectory; The first calculation unit is used to calculate the steering control quantity using a linear quadratic regulator model based on the projection point as the control reference point.

[0081] Optionally, the first determining unit includes: The prediction subunit is used to make a prediction based on the pose information, the vehicle's motion state, and the pre-calibrated delay time to obtain the predicted pose of the positioning receiver after the delay time. A determining subunit is used to determine at least two trajectory points on the target trajectory based on the predicted pose, wherein the distance between the at least two trajectory points and the positioning receiver satisfies a preset condition; An interpolation subunit is used to perform linear interpolation based on the at least two trajectory points to obtain the projection point.

[0082] Optionally, the first computing unit includes: The calculation subunit is used to calculate the state information and vehicle parameters based on the projection point, and to calculate the reference front wheel angle. The state information of the projection point includes at least heading and curvature, and the vehicle parameters include at least wheelbase and the distance from the positioning receiver to the center of the rear axle of the vehicle. A subunit is established to build a vehicle kinematic model with the positioning receiver as the control reference point, and a state space equation is constructed based on the vehicle kinematic model, the state information of the projection point and the pose information. The state variables of the state space equation include lateral error and heading error. The solution subunit is used to obtain the error front wheel steering angle based on the state-space equation using a linear quadratic regulator algorithm. The superposition subunit is used to superimpose the error front wheel steering angle with the reference front wheel steering angle to obtain the steering control quantity.

[0083] Optionally, the sub-unit is calculated, specifically for: The reference front wheel steering angle is calculated using the first formula, where the first formula is:

[0084] Where L is the vehicle wheelbase. The curvature of the projection point. The distance from the positioning receiver to the center of the vehicle's rear axle is denoted as .

[0085] Optionally, the trajectory tracking device 700 further includes: The second calculation module is used to calculate the real-time lateral deviation between the position of the positioning receiver and the target trajectory; The third calculation module is used to calculate the steering compensation amount based on the real-time lateral deviation using a proportional-integral-derivative controller. The superposition module is used to superimpose the steering compensation amount and the steering control amount to obtain the target control amount; The control module is specifically used for: The vehicle movement is controlled according to the target control quantity.

[0086] Optionally, the trajectory tracking device 700 further includes: The second acquisition module is used to acquire the original geometric trajectory defined by the plurality of trajectory points; The generation module is used to interpolate between any two adjacent trajectory points of the original geometric trajectory according to the constraints of the straight line or the circular arc with a fixed curvature, and generate at least one interpolation point. A constitutive module is used to combine the trajectory points of the original geometric trajectory with the generated interpolation points to form the target trajectory.

[0087] Furthermore, in conjunction with the methods in the above embodiments, this application embodiment can provide a storage medium for implementation. This storage medium stores program instructions; when these program instructions are executed by the controller, they implement any of the methods in the above embodiments.

[0088] This application also provides a chip, which includes a controller and a communication interface. The communication interface and the controller are coupled. The controller is used to run programs or instructions to implement the various processes of the above method embodiments and achieve the same technical effect. To avoid repetition, it will not be described again here.

[0089] It should be understood that the chip mentioned in the embodiments of this application may also be referred to as a system-on-a-chip, system chip, chip system, or system-on-a-chip, etc.

[0090] This application provides a computer program product, which is stored in a storage medium and executed by at least one controller to implement the various processes of the above method embodiments and achieve the same technical effect. To avoid repetition, it will not be described again here.

[0091] It should be clarified that this application is not limited to the specific configurations and processes described above and shown in the figures. For the sake of brevity, detailed descriptions of known methods are omitted here. In the above embodiments, several specific steps are described and shown as examples. However, the method process of this application is not limited to the specific steps described and shown. Those skilled in the art can make various changes, modifications, and additions, or change the order of steps, after understanding the spirit of this application.

[0092] The functional modules shown in the above block diagram can be implemented as hardware, software, firmware, or a combination thereof. When implemented in hardware, they can be, for example, electronic circuits, application-specific integrated circuits (ASICs), appropriate firmware, plug-ins, function cards, etc. When implemented in software, the elements of this application are programs or code segments used to perform the required tasks. Programs or code segments can be stored on machine-readable media or transmitted over a transmission medium or communication link via data signals carried on a carrier wave. "Machine-readable media" can include any medium capable of storing or transmitting information. Examples of machine-readable media include electronic circuits, semiconductor memory devices, ROM, flash memory, erasable ROM (EROM), floppy disks, CD-ROMs, optical disks, hard disks, fiber optic media, radio frequency (RF) links, etc. Code segments can be downloaded via computer grids such as the Internet, intranets, etc.

[0093] It should also be noted that the exemplary embodiments mentioned in this application describe methods or systems based on a series of steps or apparatus. However, this application is not limited to the order of the above steps; that is, the steps can be performed in the order mentioned in the embodiments, or in a different order, or several steps can be performed simultaneously.

[0094] The aspects of this disclosure have been described above with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and program products according to embodiments of this disclosure. It should be understood that each block in the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These program instructions can be provided to a controller of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to create a machine such that these instructions, executable via the controller of the computer or other programmable data processing apparatus, enable the implementation of the functions / actions specified in one or more blocks of the flowchart illustrations and / or block diagrams. Such a controller can be, but is not limited to, a general-purpose controller, a special-purpose controller, an application-specific controller, or a field-programmable logic circuit. It is also understood that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can also be implemented by special-purpose hardware performing the specified functions or actions, or can be implemented by a combination of special-purpose hardware and computer instructions.

[0095] The above are merely specific embodiments of this application. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, modules, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here. It should be understood that the protection scope of this application is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in this application, and these modifications or substitutions should all be covered within the protection scope of this application.

Claims

1. A trajectory tracking method, characterized in that, The method includes: The vehicle's pose information is acquired through a positioning receiver installed on the vehicle, and includes at least the position and heading information of the positioning receiver. Using the position of the positioning receiver as the control reference point of the vehicle, the steering control quantity of the vehicle is calculated based on the pose information and the pre-acquired target trajectory. The vehicle movement is controlled according to the steering control amount; The target trajectory consists of multiple trajectory points. Each trajectory point contains the position, heading, and curvature information of the trajectory point. The trajectory segment between any two adjacent trajectory points is constrained to be a straight line or a circular arc with a fixed curvature, and the adjacent trajectory segments are tangent to each other.

2. The method as described in claim 1, characterized in that, The step of using the position of the positioning receiver as the control reference point for the vehicle, and calculating the vehicle's steering control quantity based on the pose information and the pre-acquired target trajectory, includes: Obtain the lateral distance between the position of the positioning receiver and the target trajectory; When the lateral distance is greater than a first preset distance, the steering control quantity is calculated using a pure tracking algorithm based on the pose information and the pre-acquired target trajectory; When the lateral distance is less than or equal to a first preset distance, the steering control quantity is calculated based on the pose information and the pre-acquired target trajectory using a linear quadratic regulator model with the positioning receiver as the control reference point.

3. The method as described in claim 2, characterized in that, Based on the pose information and the pre-acquired target trajectory, the steering control quantity is calculated using a linear quadratic regulator model with the positioning receiver as the control reference point, including: Based on the pose information, the projection point of the positioning receiver on the target trajectory is determined, and the projection point is the point where the positioning receiver is closest to the target trajectory; The steering control quantity is calculated using a linear quadratic regulator model based on the projection point as the control reference point.

4. The method as described in claim 3, characterized in that, Determining the projection point of the positioning receiver on the target trajectory based on the pose information includes: Based on the pose information, the vehicle's motion state, and the pre-calibrated delay time, the predicted pose of the positioning receiver after the delay time is obtained. Based on the predicted pose, at least two trajectory points on the target trajectory are determined, and the distances between the at least two trajectory points and the positioning receiver satisfy a preset condition; The projection point is obtained by linear interpolation based on the at least two trajectory points.

5. The method as described in claim 3, characterized in that, The calculation of the steering control quantity using a linear quadratic regulator model based on the projection point as the control reference point includes: Based on the state information of the projection point and the vehicle parameters, the reference front wheel angle is calculated. The state information of the projection point includes at least heading and curvature, and the vehicle parameters include at least wheelbase and the distance from the positioning receiver to the center of the rear axle of the vehicle. A vehicle kinematic model is established with the positioning receiver as the control reference point. Based on the vehicle kinematic model, the state information of the projection point, and the pose information, a state space equation is constructed. The state variables of the state space equation include lateral error and heading error. Based on the state-space equation, the error front wheel angle is obtained by solving the linear quadratic regulator algorithm. The steering control value is obtained by superimposing the error front wheel angle with the reference front wheel angle.

6. The method as described in claim 5, characterized in that, The calculation of the reference front wheel steering angle based on the state information of the projection point and vehicle parameters includes: The reference front wheel steering angle is calculated using the first formula, where the first formula is: Where L is the vehicle wheelbase. The curvature of the projection point. The distance from the positioning receiver to the center of the vehicle's rear axle is denoted as .

7. The method as described in claim 5, characterized in that, After calculating the vehicle's steering control amount based on the pose information and the pre-acquired target trajectory, using the position of the positioning receiver as the vehicle's control reference point, and before controlling the vehicle's movement according to the steering control amount, the method further includes: Calculate the real-time lateral deviation between the position of the positioning receiver and the target trajectory; Based on the real-time lateral deviation, the steering compensation amount is calculated by a proportional-integral-derivative controller. The steering compensation amount and the steering control amount are superimposed to obtain the target control amount; The step of controlling the vehicle movement according to the steering control amount includes: The vehicle movement is controlled according to the target control quantity.

8. The method according to any one of claims 1 to 7, characterized in that, The pre-acquired target trajectory is generated through the following steps: Obtain the original geometric trajectory defined by the plurality of trajectory points; Between any two adjacent trajectory points of the original geometric trajectory, interpolation is performed based on the constraints of the straight line or the circular arc with a fixed curvature to generate at least one interpolation point. The trajectory points of the original geometric trajectory and the generated interpolation points together constitute the target trajectory.

9. A trajectory tracking device, characterized in that, The device includes: The first acquisition module is used to acquire the vehicle's pose information, which is collected by a positioning receiver installed on the vehicle and includes at least the position and heading information of the positioning receiver. The first calculation module is used to calculate the vehicle's steering control quantity based on the pose information and the pre-acquired target trajectory, using the position of the positioning receiver as the vehicle's control reference point. The control module is used to control the movement of the vehicle according to the steering control amount; The target trajectory consists of multiple trajectory points. Each trajectory point contains the position, heading, and curvature information of the trajectory point. The trajectory segment between any two adjacent trajectory points is constrained to be a straight line or a circular arc with a fixed curvature, and the adjacent trajectory segments are tangent to each other.

10. A machine-readable storage medium, characterized in that, The machine-readable storage medium stores instructions for causing the machine to perform the trajectory tracking method according to any one of claims 1 to 8.

Images