Unmanned vehicle autonomous navigation and precise trajectory tracking control method for complex environment

Abstract

This invention relates to the field of trajectory tracking control technology, providing a method for autonomous navigation and precise trajectory tracking control of unmanned vehicles in complex environments. The method includes the following steps: system state acquisition and environmental feature perception; adaptive preview point calculation based on exponential smoothing; multi-dimensional tracking error extraction; error dimensionality reduction and fusion based on curve sensitivity factors; PSO-scheduling gain-PID controller calculation; physical limiting and control output; and closed-loop feedback update. This invention combines an exponential smoothing adaptive preview method with coupled longitudinal velocity and path curvature feedforward, and establishes an error dimensionality reduction and fusion mechanism based on curve sensitivity factors to resolve multi-objective collaborative conflicts during trajectory tracking. It employs PSO combined with scheduling gain PID for underlying nonlinear compensation and integrates the physical limiting constraints of the underlying steering mechanism, enabling unmanned delivery vehicles to achieve high-precision, robust, and energy-efficient autonomous navigation operations in complex dynamic environments.

Description

Technical Field

[0001] This invention belongs to the field of trajectory tracking and control technology, and particularly relates to an autonomous navigation and precise trajectory tracking and control method for unmanned vehicles in complex environments. Background Technology

[0002] With the booming development of e-commerce and local retail, "last-mile" delivery is facing severe challenges such as rising labor costs and efficiency bottlenecks. Against this backdrop, unmanned delivery vehicles, with their advantages of all-weather operation, high efficiency, and low energy consumption, have become a key solution to the pain points of last-mile logistics. As a typical application of autonomous driving technology in low-speed scenarios, the large-scale deployment of unmanned delivery vehicles in closed parks, semi-open communities, and unstructured urban roads highly depends on the reliability of their underlying autonomous navigation system. Among these systems, precise trajectory tracking and control, as the core hub connecting upper-level path planning and lower-level chassis execution, directly determines the safety, smoothness, and mission fidelity of the vehicle's operation.

[0003] The fundamental task of trajectory tracking control is to drive unmanned vehicles to overcome their own dynamic nonlinearity and external environmental disturbances, and to travel steadily and accurately along the desired spatiotemporal reference path. However, unlike high-grade structured roads such as highways, the last-mile delivery environment of unmanned delivery vehicles is extremely complex and variable. Their driving conditions are usually intertwined with continuous sharp bends, narrow alleys, uneven road surfaces (such as speed bumps and potholes), and frequently changing ground adhesion coefficients. These complex conditions can cause strong external physical disturbances, causing the lateral position deviation and heading angle deviation of the vehicle to accumulate rapidly in a short period of time, which can easily lead to vehicle instability or deviation from the predetermined route.

[0004] While existing trajectory tracking control technologies have achieved certain results under specific conditions, they still exhibit significant drawbacks when faced with complex, dynamic, and unstructured road conditions with varying curvature for unmanned delivery vehicles. Traditional pure-pursuit algorithms or conventional PID control typically employ fixed aiming distances or fixed linear error weights. This approach fails to perceive the geometric characteristics of local paths, easily leading to severe "bending" at high speeds or on high-curvature curves, while also causing control oscillations on straight sections. Although existing technologies incorporate nonlinear lateral error feedback (such as the Stanley algorithm), achieving an adaptive optimal balance between the conflicting physical requirements of "straight-line anti-deviation" and "smooth curves" remains challenging. Advanced algorithms such as Model Predictive Control (MPC) and Linear Quadratic Regulators (LQR) can achieve high-precision trajectory fidelity through rolling optimization and rigorous steady-state error analysis. However, these algorithms heavily rely on extremely accurate vehicle dynamics models and require online matrix solving and handling of multiple constraints. The computational load is extremely large. For the onboard edge computing platform of unmanned delivery vehicles, which has extremely stringent requirements for cost, power consumption and computing power, such algorithms are prone to causing control command delays and poor real-time performance. To solve the parameter adaptation problem, existing technologies have introduced methods such as nonlinear sliding mode control (SMC), fuzzy logic adaptive PID and deep reinforcement learning (DDPG). However, sliding mode control is prone to output chattering, which can lead to serious wear on the steering actuator of the unmanned vehicle. Although fuzzy logic control can dynamically adjust PID parameters, the formulation of its fuzzy rules is highly dependent on human expert experience, which is highly subjective and prone to getting trapped in local optima. Although end-to-end reinforcement learning or neural network models have strong generalization ability, they often lack strict underlying physical boundary constraints (such as maximum steering angle limits), which pose serious safety hazards in actual engineering deployments.

[0005] Therefore, addressing the technical challenges of unmanned delivery vehicles, such as large trajectory tracking errors, poor environmental adaptability, and inability to accurately and stably track in complex road conditions, exploring novel control strategies that combine high precision, strong robustness, and low computational load has become a pressing technological focus for the industry. Deeply integrating vehicle kinematic characteristics and introducing adaptive mechanisms or intelligent optimization algorithms to dynamically compensate for nonlinearities in the underlying execution, thereby resolving the conflict between smoothness and tracking accuracy, is key to achieving stable and efficient operation of unmanned delivery platforms in dynamic environments. Summary of the Invention

[0006] The purpose of this invention is to provide an autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments, aiming to solve the problems mentioned in the background art.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] An autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments includes the following steps:

[0009] S1. System Status Acquisition and Environmental Feature Perception: The vehicle's current pose status in the global coordinate system is acquired in real time through the onboard high-precision positioning module RTK. With longitudinal driving speed It reads the expected reference path point set issued by the upper-level global path planner and uses the adjacent path point difference method to calculate the discrete curvature of the path ahead of the vehicle in real time. ;

[0010] S2. Adaptive anticipation point calculation based on exponential smoothing: based on the current vehicle speed. Calculate the adaptive aiming distance;

[0011] S3. Multidimensional tracking error extraction: Based on the adaptive pre-aiming distance, a pre-aiming reference point is searched on the desired path. Combined with the two-wheeled single-track kinematic model, the lateral position error and heading angle error between the current actual pose of the unmanned vehicle and the pre-aiming reference point are calculated.

[0012] S4. Error dimensionality reduction and fusion based on curve sensitivity factors: Extract the curve sensitivity factors of the current path, calculate the adaptive lateral weight and adaptive heading weight, reduce the dimensionality of the two errors and weight them, and output a single comprehensive control error.

[0013] S5: PSO-Scheduling Gain-PID Controller Solution: Using the particle swarm optimization algorithm, with the absolute error integral as the fitness function, the optimal reference gain parameter of the PID scheduling function is found in the multi-dimensional parameter space. The reconstructed comprehensive control error is input to the controller, and the proportional, integral and derivative parameters are dynamically called in real time according to the error magnitude to provide nonlinear compensation torque.

[0014] S6. Physical Limiting and Control Output: Based on the adjusted PID parameters, the theoretical front wheel steering angle control quantity is calculated. Combined with the mechanical limit of the unmanned vehicle's underlying steering actuator, the theoretical steering angle is subjected to absolute value saturation limiting. The safety control quantity that has been physically limited is sent to the unmanned vehicle's underlying drive-by-wire chassis for execution.

[0015] S7. Closed-loop feedback update: After the unmanned vehicle chassis responds to the steering control input, the physical spatial position changes, the sensor timestamp is updated, and the process returns to step S1 to enter the closed-loop iteration of the next control cycle until the unmanned vehicle successfully arrives at the delivery destination.

[0016] Compared with the prior art, the specific beneficial effects of the present invention are as follows:

[0017] (1) A nonlinear spatial kinematic model of unmanned vehicle based on the two-wheeled single-track kinematic model (Bicycle Model) was constructed, and the nonlinear coupling relationship between the bottom steering angle input and the global pose output of the vehicle was clarified, providing a precise theoretical boundary and mathematical basis for trajectory tracking algorithm under complex road conditions;

[0018] (2) An adaptive pre-aiming algorithm coupled with longitudinal velocity and path curvature feedforward is proposed. By introducing an exponential smooth decay function to dynamically correct the pre-aiming distance, the inherent defects of traditional algorithms in that they are prone to "cutting corners" on curves and oscillating on straight roads are effectively overcome, and a high degree of unity between driving smoothness and trajectory tracking accuracy is achieved.

[0019] (3) An adaptive error dimensionality reduction fusion mechanism based on curve sensitivity factor was designed. According to the path geometric characteristics, the dynamic weight allocation of "strong lateral and loose heading" was implemented, which completely resolved the internal state confrontation caused by the traditional multi-error mixed control and significantly improved the system robustness and cornering stability of the vehicle under continuous nonlinear path.

[0020] (4) The innovative PSO-scheduling gain-PID composite control strategy with absolute physical limit constraint is adopted. It not only uses the PSO algorithm to realize the global automatic optimization of the reference gain parameter, eliminating the blindness of manual trial and error, but also realizes nonlinear compensation with extremely low computational load, which perfectly meets the lightweight requirements of the vehicle edge computing of unmanned vehicles, while preventing overload damage to the steer-by-wire mechanism.

[0021] (5) A complete closed-loop control framework from multi-source perception, error calculation to drive-by-wire was constructed, enabling the system to adapt to sudden changes in road conditions and external physical disturbances in real time, and output pure and smooth comprehensive control commands to the bottom layer, which significantly improves the autonomous navigation accuracy, operation safety and delivery efficiency of unmanned delivery vehicles in dynamic traffic and unstructured roads. Attached Figure Description

[0022] Figure 1 A flowchart of an autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments, provided in an embodiment of the present invention;

[0023] Figure 2 A planned path diagram provided for embodiments of the present invention;

[0024] Figure 3 Position error diagram of Strategy A provided in this embodiment of the invention;

[0025] Figure 4 Position error diagram for Strategy B provided in this embodiment of the invention;

[0026] Figure 5 The position error diagram of strategy C provided in the embodiment of the present invention. Detailed Implementation

[0027] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0028] The specific implementation of the present invention will be described in detail below with reference to specific embodiments.

[0029] Example 1: A method for autonomous navigation and precise trajectory tracking control of unmanned vehicles in complex environments, such as... Figure 1 As shown, it includes the following steps:

[0030] S1. System Status Acquisition and Environmental Feature Perception: The vehicle's current pose status in the global coordinate system is acquired in real time through the onboard high-precision positioning module RTK. With longitudinal driving speed It reads the expected reference path point set issued by the upper-level global path planner and uses the adjacent path point difference method to calculate the discrete curvature of the path ahead of the vehicle in real time. ;

[0031] S2. Adaptive anticipation point calculation based on exponential smoothing: based on the current vehicle speed. Calculate the adaptive aiming distance;

[0032] S3. Multidimensional tracking error extraction: Based on the adaptive pre-aiming distance, a unique pre-aiming reference point is searched and determined on the desired path. Combined with the two-wheeled single-track kinematic model, the lateral position error and heading angle error between the current actual pose of the unmanned vehicle and the pre-aiming reference point are calculated.

[0033] S4. Error dimensionality reduction and fusion based on curve sensitivity factors: Extract the curve sensitivity factors of the current path, calculate the adaptive lateral weight and adaptive heading weight, reduce the dimensionality of the two errors and weight them, and output a pure and single comprehensive control error that conforms to the physical kinematic logic.

[0034] S5: PSO-Scheduling Gain-PID Controller Solution: Using the Particle Swarm Optimization (PSO) algorithm with the integral of absolute error (ITAE) as the fitness function, the optimal reference gain parameter of the PID scheduling function is found in the multi-dimensional parameter space. The reconstructed integrated control error is input to the controller, and the proportional, integral and derivative parameters are dynamically called in real time according to the error magnitude to provide nonlinear compensation torque.

[0035] S6. Physical Limiting and Control Output: Based on the adjusted PID parameters, the theoretical front wheel steering angle control quantity is calculated. Combined with the mechanical limit of the unmanned vehicle's underlying steering actuator, the theoretical steering angle is subjected to absolute value saturation limiting. The safety control quantity that has been physically limited is sent to the unmanned vehicle's underlying drive-by-wire chassis for execution via the CAN bus.

[0036] S7. Closed-loop feedback update: After the unmanned vehicle chassis responds to the steering control quantity, the physical spatial position changes, the sensor timestamp is updated, and the process returns to step S1 to enter the closed-loop iteration of the next control cycle until the unmanned vehicle successfully arrives at the delivery destination.

[0037] A precise trajectory tracking closed-loop control system that deeply integrates vehicle spatial kinematics, adaptive environmental perception mechanisms, and intelligent optimization algorithms is used to implement the above methods. It mainly comprises five modules: hardware communication architecture, kinematic modeling, adaptive anti-aiming calculation, dimensionality reduction error fusion, and underlying closed-loop control. The specific details are as follows:

[0038] 1. System hardware awareness and heterogeneous underlying communication architecture:

[0039] The hardware control network of the autonomous vehicle adopts a closed-loop architecture of "multi-source perception - edge computing - drive-by-wire" to ensure low latency and high fidelity of control commands. First, the system's bottom layer is equipped with a high-precision positioning module (such as RTK-GPS) and an inertial measurement unit (IMU). RTK-GPS provides centimeter-level global coordinate system position data. The IMU collects the vehicle's yaw rate and heading angle in real time. Simultaneously, the vehicle's current longitudinal speed is obtained through chassis wheel speed sensors or drive motor encoders. To address the issues of inconsistent sampling frequencies and data synchronization among multiple sensor sources, data from each hardware node is read via a high-speed CAN (Controller Area Network) bus through the Kvaser hardware communication device. After being appended with a unified timestamp, the data is centrally transmitted to the onboard industrial computer (edge ​​computing unit). The computer's internal control algorithm calculates trajectory errors and optimizes PID parameters, generating low-level control commands (front wheel steering angle, drive motor torque). These commands are then sent in real-time via the CAN bus to the drive-by-wire nodes on the vehicle chassis through Kvaser, forming a complete closed-loop execution network.

[0040] 2. Nonlinear spatial kinematics modeling of unmanned vehicles:

[0041] To accurately describe the motion state of the unmanned vehicle in a two-dimensional plane and provide a theoretical boundary for subsequent control algorithms, this embodiment of the invention adopts a classic two-wheeled monorail kinematic model (Bicycle Model). It assumes the vehicle travels on a flat road surface and satisfies the no-slip condition (i.e., wheel slip ratio and sideslip angle are negligible), and establishes a global Cartesian inertial coordinate system. ,set up This represents the absolute position of the vehicle's rear axle center in the global coordinate system. Let be the angle between the vehicle's longitudinal axis and the X-axis (heading angle). Based on the physical configuration of the lightweight unmanned platform in this embodiment, the vehicle wheelbase is set to 0.6m, and the effective steering angle of the front wheels is . According to the kinematic velocity vector relationship, the front wheel speed longitudinal velocity component in the vehicle local coordinate system Lateral velocity component They are represented as follows:

[0042] ;

[0043] Based on non-cylindrical geometric constraints, the pose change rate (first derivative) of the autonomous vehicle in the global coordinate system can be rigorously described by the following set of nonlinear differential equations:

[0044] ;

[0045] In the formula, Indicates the wheelbase of the car;

[0046] The above formula clarifies the steering angle input. Vehicle pose output The nonlinear coupling relationship between them constitutes the underlying controlled object model of the trajectory tracking algorithm.

[0047] 3. Adaptive pre-aiming model based on longitudinal velocity and curvature feedforward:

[0048] Traditional pure tracking algorithms typically rely on a fixed pre-aiming distance, which can easily lead to "curve cutting" distortion at bends and system oscillations on straight sections when faced with complex paths. To address this deficiency, this invention proposes a two-dimensional adaptive pre-aiming algorithm that simultaneously couples longitudinal velocity and path local curvature feedforward. Considering that autonomous vehicles often operate at low speeds of approximately 1.5 m / s in actual operation, their dynamic response is extremely sensitive to fluctuations in the pre-aiming distance. First, the basic dynamic pre-aiming distance is calculated based on the actual vehicle speed. Furthermore, an absolute upper limit saturation function is introduced to prevent the steering system from becoming "lazy" or experiencing sluggish response due to excessively long aiming distances.

[0049] ;

[0050] In the formula, The basic aiming distance to ensure the lower limit safety of the system; This is the velocity gain coefficient; The maximum aiming threshold;

[0051] Furthermore, the coordinates of discrete points on the reference path are extracted, and the curvature of the path ahead of the vehicle is calculated in real time using the three-point difference method. ;

[0052] An exponential smoothing decay function based on path curvature is introduced to nonlinearly correct the base aiming distance, and the final adaptive aiming distance is calculated. :

[0053] ;

[0054] In the formula, The pre-aiming curvature sensitivity coefficient (taken as follows) This mechanism endows the system with environmental awareness: when the vehicle is traveling in a straight line ( When the exponent term approaches 1, the system utilizes a longer pre-aiming distance. To ensure smooth overall driving; when entering a high-curvature curve, the exponential term decays rapidly, and the anticipation distance converges smoothly to... Drive closely following the reference trajectory.

[0055] Obtaining adaptive pre-aiming distance Next, the system needs to further convert the global reference trajectory into an error vector that can be recognized by the underlying controller. This process mainly involves the dynamic search of the aiming reference point, spatial normalization of the heading angle, and extraction of lateral error based on coordinate transformation. The specific calculation process is as follows:

[0056] (1) Pre-aiming reference point Dynamic locking:

[0057] Suppose the desired path is composed of a discrete set of points. The structure begins with the system traversing the point set. The current centroid position of the vehicle is determined according to the Euclidean distance minimization criterion. Index of projection points on the path :

[0058] ;

[0059] Subsequently, from the index Starting from the path, perform a forward integral search to find a location where the accumulated arc length equals the adaptive pre-aiming distance. The waypoints are locked as aiming reference points. The expected heading angle at that point It is obtained by approximating the difference between adjacent points:

[0060] ;

[0061] (2) Heading angle error Solution and normalization:

[0062] The heading angle error characterizes the vehicle's current actual heading angle. With the expected heading angle of the aiming point Deviation between:

[0063] ;

[0064] Considering the periodicity of angles in physical space, in order to avoid... Sudden changes can cause overshoot or instability in the control system, which must be addressed. Normalization is performed, and the error is constrained to within the range specified by the following mapping rule. Within the interval:

[0065] ;

[0066] (3) Lateral position error based on coordinate system rotation transformation extract:

[0067] Lateral position error is the lateral offset of the pre-aiming reference point relative to the longitudinal centerline of the vehicle. To simplify the calculation complexity and directly obtain the directional error value, a local coordinate system is established with the vehicle's center of mass as the origin and the longitudinal axis of the vehicle body as the positive X-axis. The global deviation is projected into the local coordinate system through a rotation matrix.

[0068] First, calculate the relative position vector in global coordinates. Then, the inverse rotation transformation is applied:

[0069] ;

[0070] Expand the second row of the matrix to obtain the lateral position error. Calculation formula:

[0071] .

[0072] This algorithm is given through coordinate transformation. Clear physical meaning: If the target point is located on the left side of the vehicle, a positive steering command should be output; otherwise, a rightward correction command should be output. The result obtained through the above steps... and This will be used as an input quantity in the subsequent "adaptive dual-error fusion mechanism" to achieve multi-objective coordinated control.

[0073] 4. Adaptive error reduction and fusion based on curve sensitivity factors:

[0074] The impact of lateral position error and heading error on vehicle stability exhibits significant spatial time-varying characteristics. Under complex paths, a fixed linear weighting strategy cannot simultaneously meet the dual requirements of "straight-line anti-deviation" and "smooth cornering".

[0075] To achieve optimal control with multi-objective cooperation, a dynamic weight allocation mechanism based on the Curve Sensitivity Factor is proposed, which reduces the two-dimensional error and reconstructs it into a comprehensive control error. :

[0076] ;

[0077] To address the physical differences between lateral and heading errors, controllable attenuation factors were designed separately. Weight adjustment strategy, in which For cornering sensitivity, :

[0078] 1) Adaptive lateral weighting (cornering emphasis):

[0079] ;

[0080] Under straight-line conditions, the lateral weight remains at the baseline value. When entering a curve, the weighting amplitude adaptively increases, increasing... This mechanism ensures that the vehicle prioritizes overcoming the lateral drift caused by centrifugal force when cornering;

[0081] 2) Adaptive heading weights (curve relaxation):

[0082] ;

[0083] In the formula, The basic heading weight (the order of magnitude is set to 0.5° equivalent). , The proportionality coefficient (satisfying) The design maintains full weight to keep the heading in straight driving; while in corners, the heading constraint is greatly relaxed (e.g., reduced to 20% of the baseline), allowing the front of the car to enter the corner freely;

[0084] This nonlinear decoupling strategy of "strong lateral movement in curves and relaxed heading" effectively resolves the internal state conflict that is easily caused when multiple errors are mixed in the traditional way, and outputs a pure comprehensive error signal to the PID controller that conforms to the physical kinematic logic.

[0085] 5. Design of an adaptive PID controller based on PSO and scheduling gain:

[0086] Due to the highly nonlinear characteristics of vehicle kinematics models, especially under the platform constraint of a 0.6 m wheelbase, traditional fixed-parameter PID controllers struggle to maintain optimal performance across all operating conditions. Although GainScheduling PID controllers can dynamically adjust proportional, integral, and derivative parameters, their scheduling rules are highly dependent on human experience, exhibiting strong subjectivity and a tendency to get trapped in local optima.

[0087] To enhance the automation and global optimization capabilities of controller parameter tuning, the particle swarm optimization (PSO) algorithm is deeply integrated with the gain scheduling mechanism:

[0088] ;

[0089] In the formula, for Control output at any given time; for The overall control error input; The dynamic proportional gain parameter is controlled by the current error state; The dynamic integral gain parameter is controlled by the current error state; The dynamic differential gain parameter is controlled by the current error state; This represents the current running time of the system.

[0090] The controller uses the integral of absolute error as the fitness function and leverages the PSO algorithm to offline optimize the baseline gain of the scheduling function in a multi-dimensional parameter space, completely eliminating the blindness of manual trial and error. During operation, the scheduling gain PID is adjusted based on the comprehensive error. Provides nonlinear compensation: outputs a strong restoring torque when the error is large, and automatically reduces the gain to suppress system overshoot when the error approaches zero;

[0091] Ultimately, the controller outputs the front wheel steering angle control value of the vehicle. To ensure that the theoretical control quantity matches the physical limits of the underlying actuator and to prevent mechanical overload, a 34° absolute steering angle saturation limit constraint is embedded at the end of the algorithm.

[0092] .

[0093] Performance testing:

[0094] To verify the effectiveness and superiority of the method proposed in the embodiments of the present invention, the method of the present invention and two traditional control strategies were compared in simulation and real vehicle tests under the condition of continuous nonlinear path (including straight lines, large curvature curves and continuous S-shaped curves). During the test, the vehicle cruising speed was set to 1.5 m / s.

[0095] The three control strategies used in the comparison are as follows:

[0096] Strategy A (fixed-look pure PID control): Directly utilize the absolute lateral error and heading error between the current vehicle's center of gravity position and the reference path to perform fixed-gain PID feedback control;

[0097] Strategy B (Adaptive Pre-aiming Pure PID Control): Adaptive pre-aiming based on longitudinal velocity and curvature feedforward is adopted, and fixed-gain PID feedback control is performed using error information at the forward pre-aiming point.

[0098] Strategy C (Multi-strategy adaptive PSO-PID control in this embodiment of the invention): Adaptive anticipation based on longitudinal velocity and curvature feedforward, dimensionality reduction fusion of curve sensitivity factor error, and PSO-scheduling gain-PID composite control with absolute physical limit.

[0099] The planned route map is as follows Figure 2 As shown, the position error diagram for strategy A is as follows: Figure 3 As shown, the position error diagram for strategy B is as follows: Figure 4 As shown, the position error diagram for strategy C is as follows: Figure 5 As shown;

[0100] When using Strategy A (fixed-look pure PID control): Since this strategy directly uses the current vehicle center of gravity position for error feedback, the system essentially lacks effective look-ahead capability for the geometry of the road ahead. When the car enters a curve with high curvature, the controller response exhibits severe lag, causing the lateral position error to diverge rapidly. When exiting the curve and recovering its posture, the fixed PID gain easily leads to significant overshoot in the system, resulting in a noticeable "dragon-like" trajectory and high-frequency oscillations. The maximum position error is close to 30 centimeters, and the error is often very large.

[0101] When using Strategy B (adaptive preview pure PID control): thanks to the introduction of an adaptive preview mechanism based on speed and curvature feedforward, the system's control lag is significantly improved. The vehicle can perceive curves in advance and make steering responses, eliminating the severe lag and "curve cutting" phenomenon in Strategy A. However, since the controller core still uses a fixed-gain PID and does not decouple multidimensional errors by reducing dimensions, when facing continuous curves with large curvature, the system experiences internal state conflict between "eliminating lateral errors" and "eliminating heading errors". This causes the vehicle to experience local steering vibrations near the apex of the curve, making it impossible to maintain perfect smoothness. As a result, the error is stabilized within 20 cm, which is close to perfect.

[0102] When using Strategy C (Multi-Strategy Adaptive PSO-PID Control): Based on adaptive anti-aiming, this invention dynamically adjusts the weight distribution of lateral and heading errors through cornering sensitivity factors (strong lateral movement in corners, slack heading), completely resolving multi-objective cooperative conflicts; combined with the nonlinear torque compensation provided by PSO-PID based on real-time errors, the vehicle not only smoothly and accurately conforms to all consecutive curves, but also does not exhibit overshoot or instability throughout the entire process. The chassis steering mechanism operates continuously and smoothly, demonstrating extremely strong nonlinear adaptability and robustness. Experiments show that the error is within 10 cm, achieving better results;

[0103] The experimental comparison results fully demonstrate that the improvement of Strategy B compared to Strategy A verifies the effectiveness of the "adaptive preview mechanism" in improving control lag; while the further significant leap of Strategy C compared to Strategy B strongly proves the core value of "curving sensitive factor error dimensionality reduction fusion" and "PSO-scheduling gain-PID" in overcoming multi-objective conflicts and nonlinear system control; the method of this invention successfully breaks the contradiction between "driving smoothness" and "trajectory tracking accuracy", while meeting the requirements of lightweight edge computing power, it provides unmanned vehicles with ultimate underlying execution accuracy and operational stability.

[0104] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for autonomous navigation and precise trajectory tracking control of unmanned vehicles in complex environments, characterized by, Includes the following steps: S1, system state acquisition and environment feature perception: through the vehicle-mounted high-precision positioning module RTK, the current pose state of the unmanned vehicle in the global coordinate system is acquired in real time With the longitudinal driving speed The expected reference path point set issued by the upper global path planner is read, and the discrete curvature of the path in front of the trolley is calculated in real time by using the adjacent path point difference method ; S2. Adaptive anticipation point calculation based on exponential smoothing: based on the current vehicle speed. Calculate the adaptive aiming distance; S3. Multidimensional tracking error extraction: Based on the adaptive pre-aiming distance, a pre-aiming reference point is searched on the desired path. Combined with the two-wheeled single-track kinematic model, the lateral position error and heading angle error between the current actual pose of the unmanned vehicle and the pre-aiming reference point are calculated. S4. Error dimensionality reduction and fusion based on curve sensitivity factors: Extract the curve sensitivity factors of the current path, calculate the adaptive lateral weight and adaptive heading weight, reduce the dimensionality of the two errors and weight them, and output a single comprehensive control error. S5: PSO-Scheduling Gain-PID Controller Solution: Using the particle swarm optimization algorithm, with the absolute error integral as the fitness function, the optimal reference gain parameter of the PID scheduling function is found in the multi-dimensional parameter space. The reconstructed comprehensive control error is input to the controller, and the proportional, integral and derivative parameters are dynamically called in real time according to the error magnitude to provide nonlinear compensation torque. S6. Physical Limiting and Control Output: Based on the adjusted PID parameters, the theoretical front wheel steering angle control quantity is calculated. Combined with the mechanical limit of the unmanned vehicle's underlying steering actuator, the theoretical steering angle is subjected to absolute value saturation limiting. The safety control quantity that has been physically limited is sent to the unmanned vehicle's underlying drive-by-wire chassis for execution. S7. Closed-loop feedback update: After the unmanned vehicle chassis responds to the steering control input, the physical spatial position changes, the sensor timestamp is updated, and the process returns to step S1 to enter the closed-loop iteration of the next control cycle until the unmanned vehicle successfully arrives at the delivery destination.

2. The autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments according to claim 1, characterized in that, In step S2, the adaptive pre-aiming point calculation process is specifically as follows: Based on actual vehicle speed, the dynamic aiming distance is calculated. And introduce an absolute upper limit saturation function: ； In the formula, The basic aiming distance to ensure the lower limit safety of the system; This is the velocity gain coefficient; The maximum aiming threshold; Extract the coordinates of discrete points on the reference path and use the three-point difference method to calculate the curvature of the path ahead of the autonomous vehicle in real time. ; An exponential smoothing decay function based on path curvature is introduced to nonlinearly correct the base aiming distance, and the final adaptive aiming distance is calculated. : ； In the formula, This is the pre-aiming curvature sensitivity coefficient.

3. The autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments according to claim 2, characterized in that, In S3, the specific kinematic model of the two-wheeled single-track system is as follows: Assuming the vehicle travels on a flat road and meets the condition of no sideslip, establish a global Cartesian inertial coordinate system. ,set up This represents the absolute position of the vehicle's rear axle center in the global coordinate system. The angle between the vehicle's longitudinal axis and the X-axis is given by the effective steering angle of the front wheels. According to the kinematic velocity vector relationship, the front wheel speed longitudinal velocity component in the vehicle local coordinate system Lateral velocity component They are represented as follows: ； Based on non-cylindrical geometric constraints, the pose change rate of the autonomous vehicle in the global coordinate system is described by the following set of nonlinear differential equations: ； In the formula, This indicates the wheelbase of the car.

4. The autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments according to claim 3, characterized in that, The specific process of step S3 is as follows: Pre-aiming reference point Dynamic locking: Suppose the desired path is composed of a discrete set of points. Composition, traversal of point sets The current centroid position of the vehicle is determined according to the Euclidean distance minimization criterion. Index of projection points on the path : ； Subsequently, from the index Starting from the path, perform a forward integral search to find a location where the accumulated arc length equals the adaptive pre-aiming distance. The waypoints are locked as aiming reference points. The expected heading angle at that point It is obtained by approximating the difference between adjacent points: ； Heading angle error Solution and normalization: The heading angle error characterizes the vehicle's current actual heading angle. With the expected heading angle of the aiming point Deviation between: ； right Normalization is performed, and the error is constrained to within the range specified by the following mapping rule. Within the interval: ； Lateral position error based on coordinate system rotation transformation extract: Lateral position error is the lateral offset of the pre-aiming reference point relative to the longitudinal center axis of the vehicle. A local coordinate system is established with the vehicle's center of gravity as the origin and the longitudinal axis of the vehicle body as the positive X-axis. The global deviation is projected into the local coordinate system through a rotation matrix. First, calculate the relative position vector in global coordinates. Then, the inverse rotation transformation is applied: ； Expand the second row of the matrix to obtain the lateral position error. Calculation formula: 。 5. The autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments according to claim 4, characterized in that, The specific process of step S4 is as follows: By employing a dynamic weight allocation mechanism based on curve sensitivity factors, the two-dimensional error is reduced in dimension and reconstructed into a comprehensive control error. : ； To address the physical differences between lateral and heading errors, controllable attenuation factors were designed separately. Weight adjustment strategy, in which For cornering sensitivity: Adaptive lateral weights: ； Under straight-line conditions, the lateral weight remains at the baseline value. When entering a curve, the weighting amplitude adaptively increases, increasing... ; Adaptive heading weights: ； In the formula, Based on the course weight, , This is the proportionality coefficient.

6. The autonomous navigation and precise trajectory tracking control method for unmanned vehicles in complex environments according to claim 5, characterized in that, The specific process of step S5 is as follows: Deeply integrate particle swarm optimization algorithm with gain scheduling mechanism: ； In the formula, for Control output at any given time; for The overall control error input; The dynamic proportional gain parameter is controlled by the current error state; The dynamic integral gain parameter is controlled by the current error state; The dynamic differential gain parameter is controlled by the current error state; This represents the current running time of the system. The controller uses the integral of absolute error as the fitness function and employs the PSO algorithm to offline optimize the baseline gain of the scheduling function in a multi-dimensional parameter space. During operation, the scheduling gain PID is adjusted based on the comprehensive error. Provides nonlinear compensation; Ultimately, the controller outputs the front wheel steering angle control value of the vehicle. To prevent mechanical overload, a 34° absolute steering angle saturation limiting constraint was implemented. 。

Images