Autonomous navigation control method and system for unmanned vehicle
Through multi-sensor fusion and Markov decision-making model, combining road topology and vehicle dynamics, scenario evaluation and model prediction control are adopted to solve the navigation problem of unmanned vehicles in complex traffic scenarios, and the safety and accuracy of navigation are improved.
Patent Information
- Application Number
- CN202411546465.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-11-01
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2044-11-01
AI Technical Summary
The existing unmanned vehicle navigation control methods have poor decision-making adaptability, complex environmental modeling, and untimely response to dynamic changes in complex traffic scenarios, making it difficult to meet the requirements of safety, real-timeness and generalization.
Through multi-sensor fusion perception environment, a semantic map model is established, and the vehicle information is processed using principal component analysis and dimensionality reduction, a Markov decision model is constructed, and an initial behavioral strategy is generated by combining road topology and vehicle dynamics model. The candidate trajectory set is a scenario evaluation mechanism, selects the optimal driving strategy, adjusts the pre-purpose distance in real time, and generates optimal control instructions through the model prediction controller for autonomous navigation control.
It improves the autonomous navigation control capabilities of unmanned vehicles in complex environments, enhances safety, comfort and robustness, and achieves high-precision trajectory tracking and stability.
Smart Images

Figure CN119045342A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to navigation control technology, and in particular to an autonomous navigation control method and system for an unmanned vehicle. Background Art
[0002] With the rapid development of artificial intelligence and autonomous driving technologies, unmanned vehicles (UAVs) are garnering increasing attention. UAVs, capable of autonomous perception, decision-making, and control in complex traffic environments, are expected to significantly improve traffic efficiency and travel safety. Traditional UAV navigation and control methods typically predefine a series of rules to determine the current state and select the appropriate control strategy based on environmental information collected by vehicle sensors. While this approach achieves autonomous navigation and control for UAVs to a certain extent, it still suffers from poor decision-making adaptability, complex environmental modeling, and slow response to dynamic changes. This makes it difficult to meet safety, real-time, and generalization requirements in complex traffic scenarios. Therefore, a more intelligent, efficient, and robust UAV autonomous navigation and control method is urgently needed to adapt to the ever-changing traffic environment and enhance the autonomous decision-making and control capabilities of UAVs. Summary of the Invention
[0003] The embodiments of the present invention provide a method and system for autonomous navigation control of an unmanned vehicle, which can solve the problems in the prior art.
[0004] According to a first aspect of the embodiments of the present invention, Provided is an autonomous navigation control method for an unmanned vehicle, comprising: The vehicle's surrounding environment is perceived through multi-sensor fusion, and a semantic map model is established. The driving information of the unmanned vehicle is extracted using the semantic map model. The driving information of the unmanned vehicle is subjected to dimensionality reduction processing using the principal component analysis method to obtain low-dimensional state features. A Markov decision model is constructed based on the low-dimensional state features, and the initial behavior strategy is obtained by solving the Markov decision model; Based on the initial behavior strategy, combined with the road topology and vehicle dynamics model, a trajectory planning algorithm is used to generate local trajectory segments. Adaptive step sizes are used to discretize these local trajectory segments and generate a set of candidate trajectories. A scenario-based evaluation mechanism is introduced for the candidate trajectory set to calculate the expected cost of each candidate trajectory under various scenarios. The candidate trajectory with the lowest expected cost is selected as the optimal driving strategy. According to the optimal driving strategy, the current speed of the vehicle and the curvature of the road ahead are obtained. The preview distance is adjusted in real time according to the changes in the current speed of the vehicle and the curvature of the road ahead to obtain the predicted time domain length. According to the preset model prediction controller, the optimal control instructions within the predicted time domain length are generated, and the unmanned vehicle is autonomously navigated and controlled through the optimal control instructions.
[0005] In an optional embodiment, Based on the initial behavior strategy, combined with the road topology and vehicle dynamics model, a trajectory planning algorithm is used to generate local trajectory segments. At the same time, an adaptive step size is used to discretize the local trajectory segments. The generated candidate trajectory set includes: Decoding the initial behavior policy into a set of motion constraints, including lateral and longitudinal constraints, and optimizing the set of motion constraints in combination with a road topology, including road curvature, lane width, and intersection locations. Simultaneously, feasibility checks are performed on the set of motion constraints using a pre-built vehicle dynamics model to obtain feasible motion constraints. A trajectory planning algorithm using layered spatiotemporal sampling is used to generate motion primitives covering the achievable motion space of the unmanned vehicle. The layered spatiotemporal sampling divides the sampling space into a lateral subspace and a longitudinal subspace. Lateral motion primitives are generated by sampling in the lateral subspace, and longitudinal motion primitives are generated by sampling in the longitudinal subspace. The lateral motion primitive and the longitudinal motion primitive are combined to generate local trajectory segments that satisfy the feasible motion constraints, the trajectory discrete steps are adaptively adjusted according to the road curvature and obstacle density, the local trajectory segments are discretized according to the obtained discrete steps, and a candidate trajectory set is generated.
[0006] In an optional embodiment, Optimizing the motion constraint set in combination with the road topology includes: The cost terms of road curvature, lane width, and intersection position in the road topology are calculated separately. The comprehensive objective function of motion constraint is constructed by combining the road curvature cost term, lane width cost term, and intersection position cost term. The calculation formula is as follows: ; Where J represents the comprehensive objective function, T represents the time domain, ω1 represents the weight coefficient of the road curvature cost term, α1 represents the balance factor of the curvature size, η represents the road curvature, s represents the path arc length, t represents the time, and α2 represents the balance factor of the curvature change. represents the rate of change of road curvature, ω2 represents the weight coefficient of lane width cost term, β1 represents the amplification coefficient of exponential cost term from lane boundary, β2 represents the attenuation coefficient related to exponential cost term from lane boundary, d(t) represents the distance from vehicle to the nearest lane boundary at time t, ω3 represents the weight coefficient of intersection position cost term, C int Represents the number of intersections, i represents the i-th intersection, γ1 represents the amplification coefficient related to the exponential cost term of the distance intersection, γ2 represents the attenuation coefficient related to the exponential cost term of the distance intersection, represents the distance from the vehicle to the i-th intersection at time t.
[0007] In an optional embodiment, For the candidate trajectory set, a scenario-based evaluation mechanism is introduced to calculate the expected cost of each candidate trajectory in each scenario. The candidate trajectory with the minimum expected cost is selected as the optimal driving strategy, including: Determining multiple driving scenarios corresponding to each candidate trajectory in the candidate trajectory set and constructing a scenario set, and using the scenario set to segment the candidate trajectory, each candidate trajectory segment corresponding to a driving scenario; The trained risk assessment model is used to calculate the risk metric of each candidate trajectory in the corresponding driving scenario, obtaining a segmented trajectory risk sequence. The scenario transition probability is introduced to construct the transition relationship between different candidate trajectories. The scenario transition probability is used to map the segmented trajectory risk sequence into a scenario path risk distribution. Based on the scenario path risk distribution, trajectory evaluation indicators and cost functions are designed to obtain a comprehensive trajectory evaluation system for measuring candidate trajectories in different driving scenarios. Using the comprehensive trajectory evaluation system, the cost of each candidate trajectory is calculated in each driving scenario. The comprehensive trajectory cost is calculated by combining the costs under different driving scenarios. The candidate trajectory with the smallest comprehensive trajectory cost is selected as the optimal driving strategy.
[0008] In an optional embodiment, The calculation formula of trajectory evaluation index is as follows: ; Among them, G represents the trajectory evaluation index, M represents the number of scenarios, and δ b represents the weight coefficient of the b-th scenario, K represents the number of trajectory segments, a represents the a-th trajectory, j represents the j-th segment, r a,j represents the risk measure of the j-th segment of the a-th trajectory, and P represents the scenario transition probability.
[0009] In an optional embodiment, The optimal control instructions within the predicted time domain length are generated according to the preset model predictive controller. The autonomous navigation control of the unmanned vehicle through the optimal control instructions includes: Predicting the vehicle's trajectory tracking error within the prediction time domain length using a pre-built vehicle dynamics model and trajectory tracking error model. The trajectory tracking error includes lateral position error, heading angle error, and longitudinal velocity error. The vehicle's trajectory tracking error within the predicted time domain length is used as the input of the model predictive controller. The control variable change rate is introduced, and the controller multi-objective function is constructed in combination with the trajectory tracking error. The numerical optimization algorithm is used to solve the controller multi-objective function, and the optimal control instructions within the predicted time domain length are generated. The unmanned vehicle is autonomously navigated and controlled by the optimal control instructions.
[0010] In an optional embodiment, The calculation formula of the controller multi-objective function is as follows: ; Among them, M represents the controller multi-objective function, N p represents the length of the prediction time domain, represents the error prediction value, k represents the current moment, i represents the offset, Q represents the weight coefficient of the error prediction value, N c represents the length of the control time domain, u represents the control quantity sequence, △u(·) represents the control quantity change rate, R represents the weight coefficient of the control quantity change rate, and S represents the weight coefficient of the control quantity.
[0011] According to a second aspect of the embodiments of the present invention, Provided is an autonomous navigation control system for an unmanned vehicle, comprising: The first unit uses multi-sensor fusion to perceive the vehicle's surrounding environment, establishes a semantic map model, uses the semantic map model to extract the unmanned vehicle's driving information, uses principal component analysis to reduce the dimension of the unmanned vehicle's driving information to obtain low-dimensional state features, constructs a Markov decision model based on the low-dimensional state features, and obtains the initial behavior strategy by solving the Markov decision model; The second unit is used to generate local trajectory segments using a trajectory planning algorithm based on the initial behavior strategy, combined with the road topology and vehicle dynamics model. The local trajectory segments are discretized using an adaptive step size to generate a candidate trajectory set. A scenario-based evaluation mechanism is introduced for the candidate trajectory set to calculate the expected cost of each candidate trajectory in various scenarios. The candidate trajectory with the minimum expected cost is selected as the optimal driving strategy. The third unit is used to obtain the vehicle's current speed and the curvature of the road ahead according to the optimal driving strategy, adjust the preview distance in real time according to the changes in the vehicle's current speed and the curvature of the road ahead, obtain the predicted time domain length, generate the optimal control instructions within the predicted time domain length according to the preset model prediction controller, and perform autonomous navigation control of the unmanned vehicle through the optimal control instructions.
[0012] According to a third aspect of the embodiments of the present invention, An electronic device is provided, comprising: processor; a memory for storing processor-executable instructions; The processor is configured to call the instructions stored in the memory to execute the aforementioned method.
[0013] According to a fourth aspect of the embodiments of the present invention, A computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the method described above is implemented.
[0014] In this embodiment, a semantic map model is established through multi-sensor fusion technology, which can comprehensively perceive and describe the environmental information surrounding the unmanned vehicle, including lane markings, obstacles, and traffic regulations, providing high-quality environmental perception data for subsequent decision-making and planning. Principal component analysis is used to reduce the dimensionality of vehicle driving information and extract low-dimensional state features. This allows for a concise and efficient description of the vehicle's current state, laying the foundation for behavioral strategy planning based on a Markov decision process. By constructing a Markov decision model and solving the initial behavioral strategy, decisions can be made at an abstract level, generating high-level behavioral intentions such as lane keeping and lane changing, which guide downstream trajectory planning and control modules. Based on the initial behavioral strategy and road topology information, a trajectory planning algorithm using spatiotemporal sampling is used to generate a set of candidate trajectories that meet the constraints. A scenario evaluation mechanism is then introduced to select the optimal driving trajectory, resulting in a high degree of safety and comfort. By adjusting the preview distance in real time according to vehicle speed and road conditions and combining it with a model predictive controller to generate the optimal control sequence, the system can accurately control the vehicle to follow the planned trajectory, significantly improving trajectory tracking accuracy and stability. At the same time, it can achieve high-precision autonomous navigation control of unmanned vehicles in complex environments, and has good safety, comfort and robustness, providing strong support for the practical application of unmanned driving technology. BRIEF DESCRIPTION OF THE DRAWINGS
[0015] Figure 1 Schematic diagram of the flow of the autonomous navigation control method for an unmanned vehicle according to an embodiment of the present invention; Figure 2 The figure is a schematic structural diagram of an autonomous navigation control system for an unmanned vehicle according to an embodiment of the present invention. DETAILED DESCRIPTION
[0016] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.
[0017] The following specific embodiments are used to describe the technical solution of the present invention in detail. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.
[0018] Figure 1 FIG. 1 is a flow chart of an autonomous navigation control method for an unmanned vehicle according to an embodiment of the present invention. Figure 1 As shown, the method includes: S101. The vehicle's surrounding environment is perceived through multi-sensor fusion, and a semantic map model is established. The driving information of the unmanned vehicle is extracted using the semantic map model. The driving information of the unmanned vehicle is subjected to dimensionality reduction processing using the principal component analysis method to obtain low-dimensional state features. A Markov decision model is constructed based on the low-dimensional state features. The initial behavior strategy is obtained by solving the Markov decision model.
[0019] For example, a variety of sensors, including lidar, millimeter-wave radar, and visual cameras, are selected to achieve spatiotemporal registration and fusion of data from these sensors through steps such as data synchronization, coordinate system integration, and feature matching. Based on this multi-sensor fusion, a semantic map model is further constructed. This semantic map contains not only the geometric information of objects in the environment, but also their semantic attributes and scene semantic relationships.
[0020] The constructed semantic map model is used to extract key information that affects the driving decisions of unmanned vehicles, mainly including the vehicle's position, speed, acceleration, heading angle and other motion state information, as well as the vehicle's size, dynamic parameters and other attribute information, the road type, road width, road curvature and other geometric parameters around the vehicle, as well as road speed limits, traffic signs and other semantic information, as well as the position, speed, acceleration and other state information of obstacles around the vehicle, as well as the type, size and other attribute information of the obstacles.
[0021] The extracted autonomous vehicle driving information forms a high-dimensional state space. This information is then subjected to dimensionality reduction. Specifically, principal component analysis (PCA) is used to reduce the dimensionality of the autonomous vehicle driving information. PCA projects the original high-dimensional data into a set of orthogonal low-dimensional feature spaces through linear transformations, preserving the data's key features while removing redundant and noisy information.
[0022] After obtaining low-dimensional state features, this paper uses a Markov decision process (MDP) to model the decision-making optimization problem of autonomous vehicles. The MDP consists of a state space, an action space, state transition probabilities, and a reward function. Based on the MDP model, the decision-making problem of autonomous vehicles can be transformed into the problem of selecting the optimal action to maximize the long-term cumulative reward under a given state.
[0023] A decision optimization algorithm combining temporal difference (TD) learning and Monte Carlo tree search (MCTS) can be used to solve the MDP model and obtain the optimal behavior policy. Specifically, the Q function estimate obtained through TD learning is used as heuristic information in MCTS to guide the selection and expansion of the tree search. Simultaneously, the simulation results of MCTS can be used as training samples for TD learning to update the Q function estimate. By iteratively alternating TD learning and MCTS, the quality of the policy can be continuously improved to obtain the initial behavior policy.
[0024] In this embodiment, multi-sensor fusion technology is used to obtain comprehensive and accurate information about the vehicle's surroundings, including the geometry, semantic attributes, and relationships of objects, providing high-quality environmental perception data support for autonomous vehicle decision-making and planning. Principal component analysis algorithms are used to reduce the dimensionality of raw high-dimensional vehicle driving information and extract low-dimensional state features, enabling a concise and efficient description of the current state and reducing the computational complexity of subsequent decisions. Modeling autonomous vehicle decision-making as a Markov decision process allows for optimal decision-making within a theoretical framework, generating reasonable high-level behavioral strategies. Combining optimization algorithms using temporal difference learning and Monte Carlo tree search allows for efficient solution of complex Markov decision models, resulting in near-optimal initial behavioral strategies. This provides high-quality, robust decision-making support for autonomous driving in complex environments.
[0025] S102. Based on the initial behavior strategy, combined with the road topology and vehicle dynamics model, a trajectory planning algorithm is used to generate local trajectory segments. Adaptive step sizes are used to discretize these local trajectory segments and generate a set of candidate trajectories. A scenario-based evaluation mechanism is introduced for the candidate trajectory set to calculate the expected cost of each candidate trajectory under various scenarios. The candidate trajectory with the lowest expected cost is selected as the optimal driving strategy.
[0026] Among them, the trajectory planning algorithm takes the initial behavior decision sequence as input, and generates a motion primitive library covering the achievable motion range of the unmanned vehicle by performing hierarchical and adaptive step-size sampling of the unmanned vehicle's motion space.
[0027] In an optional embodiment, Based on the initial behavior strategy, combined with the road topology and vehicle dynamics model, a trajectory planning algorithm is used to generate local trajectory segments. At the same time, an adaptive step size is used to discretize the local trajectory segments. The generated candidate trajectory set includes: Decoding the initial behavior policy into a set of motion constraints, including lateral and longitudinal constraints, and optimizing the set of motion constraints in combination with a road topology, including road curvature, lane width, and intersection locations. Simultaneously, feasibility checks are performed on the set of motion constraints using a pre-built vehicle dynamics model to obtain feasible motion constraints. A trajectory planning algorithm using layered spatiotemporal sampling is used to generate motion primitives covering the achievable motion space of the unmanned vehicle. The layered spatiotemporal sampling divides the sampling space into a lateral subspace and a longitudinal subspace. Lateral motion primitives are generated by sampling in the lateral subspace, and longitudinal motion primitives are generated by sampling in the longitudinal subspace. The lateral motion primitive and the longitudinal motion primitive are combined to generate local trajectory segments that satisfy the feasible motion constraints, the trajectory discrete steps are adaptively adjusted according to the road curvature and obstacle density, the local trajectory segments are discretized according to the obtained discrete steps, and a candidate trajectory set is generated.
[0028] For example, the initial behavior policy, which includes lane keeping, lane changing, and deceleration, is decoded into motion constraints. Information such as target speed, acceleration, and steering angle from the policy is extracted to construct a set of motion constraints. The set of motion constraints includes lateral constraints that define the vehicle's lateral motion range, and longitudinal constraints that define the vehicle's speed and acceleration range.
[0029] Combined with the road topology, the path topology information between the vehicle's current position and the target position is obtained, including road curvature, lane width, intersection location, etc. The topology information is used to optimize the motion constraints and adjust the constraint boundaries to ensure that the generated trajectory meets the road geometry constraints.
[0030] A vehicle dynamics model is established, taking into account parameters such as vehicle mass, moment of inertia, and tire cornering stiffness, and the kinematic constraints are mapped to the vehicle control space. The vehicle dynamics model is used to check whether the optimized set of kinematic constraints is feasible under the current vehicle state. If not, the constraints are appropriately relaxed and the optimization is repeated until a feasible set of kinematic constraints is obtained.
[0031] The sampling space is divided into a transverse subspace and a longitudinal subspace. The transverse subspace includes transverse displacement, transverse velocity, etc., and the longitudinal subspace includes longitudinal displacement, longitudinal velocity, etc.
[0032] The motion space of the unmanned vehicle is sampled in layers of space and time, and the motion space is divided into three dimensions: longitudinal, lateral, and time. Sampling in the longitudinal dimension generates longitudinal motion primitives with different speeds and accelerations, sampling in the lateral dimension generates lateral motion primitives with different lateral offsets and curvatures, and sampling in the time dimension generates trajectory segments of different durations.
[0033] The sampling results in the longitudinal, lateral, and temporal dimensions are permuted and combined to generate local trajectory segments that satisfy the feasible motion constraints. Adaptive step sizes are used to discretize these local trajectory segments, adaptively adjusting the discrete step sizes based on road curvature and obstacle density to improve sampling efficiency while ensuring trajectory smoothness.
[0034] Combine local trajectory segments into a global trajectory, forming a complete trajectory from the current position to the target position. Smooth the global trajectory to eliminate sharp changes and improve trajectory comfort. Use a cost function to evaluate candidate trajectories, and select the trajectory with the lowest overall cost as the optimal trajectory.
[0035] In an optional embodiment, Optimizing the motion constraint set in combination with the road topology includes: The cost terms of road curvature, lane width, and intersection position in the road topology are calculated separately. The comprehensive objective function of motion constraint is constructed by combining the road curvature cost term, lane width cost term, and intersection position cost term. The calculation formula is as follows: ; Where J represents the comprehensive objective function, T represents the time domain, ω1 represents the weight coefficient of the road curvature cost term, α1 represents the balance factor of the curvature size, η represents the road curvature, s represents the path arc length, t represents the time, and α2 represents the balance factor of the curvature change. represents the rate of change of road curvature, ω2 represents the weight coefficient of lane width cost term, β1 represents the amplification coefficient of exponential cost term from lane boundary, β2 represents the attenuation coefficient related to exponential cost term from lane boundary, d(t) represents the distance from vehicle to the nearest lane boundary at time t, ω3 represents the weight coefficient of intersection position cost term, C int Represents the number of intersections, i represents the i-th intersection, γ1 represents the amplification coefficient related to the exponential cost term of the distance intersection, γ2 represents the attenuation coefficient related to the exponential cost term of the distance intersection, represents the distance from the vehicle to the i-th intersection at time t.
[0036] In this embodiment, by decoding the initial behavioral decision into lateral and longitudinal motion constraints and integrating the road topology and vehicle dynamics models, the generated trajectory is guaranteed to meet various constraints, thereby improving the feasibility and safety of the trajectory. By employing a layered spatiotemporal sampling approach, combined sampling of lateral and longitudinal subspaces effectively covers the entire reachable motion space of the unmanned vehicle, avoiding the omission of feasible solutions. By constructing a library of motion primitives and generating local trajectory segments in a combined manner, a diverse set of trajectory samples can be generated, providing sufficient alternatives for subsequent selection. Trajectory discrete step lengths are dynamically adjusted based on road curvature and obstacle density, with denser sampling in sections with high curvature or dense obstacles and sparser sampling in straight or open sections, improving sampling efficiency while ensuring the necessary accuracy. By decomposing the planning process into modules such as behavioral decision making, constraint fusion, trajectory sampling, and optimal selection, each module is relatively independent and computationally efficient. This module can meet the requirements of online, real-time trajectory planning for unmanned vehicles in complex urban road environments, providing high-quality trajectory input for the final decision-making of the unmanned driving system.
[0037] In an optional embodiment, For the candidate trajectory set, a scenario-based evaluation mechanism is introduced to calculate the expected cost of each candidate trajectory in each scenario. The candidate trajectory with the minimum expected cost is selected as the optimal driving strategy, including: Determining multiple driving scenarios corresponding to each candidate trajectory in the candidate trajectory set and constructing a scenario set, and using the scenario set to segment the candidate trajectory, each candidate trajectory segment corresponding to a driving scenario; The trained risk assessment model is used to calculate the risk metric of each candidate trajectory in the corresponding driving scenario, obtaining a segmented trajectory risk sequence. The scenario transition probability is introduced to construct the transition relationship between different candidate trajectories. The scenario transition probability is used to map the segmented trajectory risk sequence into a scenario path risk distribution. Based on the scenario path risk distribution, trajectory evaluation indicators and cost functions are designed to obtain a comprehensive trajectory evaluation system for measuring candidate trajectories in different driving scenarios. Using the comprehensive trajectory evaluation system, the cost of each candidate trajectory is calculated in each driving scenario. The comprehensive trajectory cost is calculated by combining the costs under different driving scenarios. The candidate trajectory with the smallest comprehensive trajectory cost is selected as the optimal driving strategy.
[0038] Exemplarily, for each trajectory in the candidate trajectory set, the driving scenarios that may be encountered during the execution of the trajectory are predicted, where the driving scenario is a set of parameters that describe the environment around the unmanned vehicle, including lane lines, obstacle positions and motion states, traffic rules, etc. For each candidate trajectory, it is divided into multiple driving scenarios according to its relative position relationship with the obstacles, and a scenario set is constructed. For each driving scenario, the feature vector related to risk assessment is extracted, and the risk assessment model is trained using a deep learning method. The driving scenarios and trajectory segments are input into the trained risk assessment model, and the risk measurement of each candidate trajectory segment in the corresponding driving scenario is output to construct a trajectory segment risk sequence.
[0039] The scenario transition probability is defined through statistical or modeling methods. This is the probability of transitioning from one driving scenario to another. Using the scenario transition probability, the trajectory segment risk sequence is mapped to a scenario path risk distribution. The scenario path risk distribution represents the probability distribution of corresponding risk values when driving along the candidate trajectory.
[0040] Multiple evaluation metrics, such as expected risk, risk variance, and probability of hazardous events, are designed to describe the different statistical characteristics of the scenario path risk distribution. A comprehensive cost function is constructed, whose value reflects the comprehensive risk level of the candidate trajectory under various scenarios.
[0041] Based on trajectory evaluation indicators and cost functions, a comprehensive trajectory evaluation system is derived to measure candidate trajectories in different driving scenarios. Using this comprehensive trajectory evaluation system, the cost of each candidate trajectory is calculated under various driving scenarios. The comprehensive trajectory cost is then combined to obtain the candidate trajectory with the smallest comprehensive cost. The optimal driving strategy is then selected.
[0042] In an optional embodiment, The calculation formula of trajectory evaluation index is as follows: ; Among them, G represents the trajectory evaluation index, M represents the number of scenarios, and δ b represents the weight coefficient of the b-th scenario, K represents the number of trajectory segments, a represents the a-th trajectory, j represents the j-th segment, r a,j represents the risk measure of the j-th segment of the a-th trajectory, and P represents the scenario transition probability.
[0043] In this embodiment, by introducing the concept of scenarios, candidate trajectories are segmented according to driving scenarios, and the risk and cost of each trajectory are evaluated separately for each scenario. This approach better reflects real-world scenarios and provides a more objective and accurate evaluation. By constructing a scenario transition probability model, the segmented trajectory risk is mapped to a scenario-based path risk distribution, reflecting the dynamic and time-varying nature of scenarios and avoiding the limitations of static evaluation. The segmented trajectory risk is calculated using a trained risk assessment model, trained using machine learning techniques and possessing strong generalization capabilities, making the risk assessment more accurate and reliable. By designing a cost function that comprehensively considers multiple evaluation metrics, such as risk, comfort, and efficiency, the evaluation is more comprehensive and capable of balancing multiple optimization objectives. The segmented evaluation and comprehensive ranking process is efficient and concise, significantly improving computational efficiency compared to simultaneously evaluating all trajectory scenario combinations, and resulting in more reliable and stable evaluation results. This scenario-based trajectory optimization solution overcomes the limitations of traditional single-static evaluation and proposes a novel scenario-based dynamic evaluation paradigm. By incorporating techniques such as scenario segmentation, scenario transition, and a comprehensive evaluation system, it enables more accurate, comprehensive, and efficient evaluation and optimization of candidate trajectories in complex situations, providing strong support for high-quality decision-making for autonomous vehicles.
[0044] S103. Obtain the vehicle's current speed and the curvature of the road ahead based on the optimal driving strategy. Adjust the preview distance in real time according to changes in the vehicle's current speed and the curvature of the road ahead to obtain the predicted time domain length. Generate the optimal control instructions within the predicted time domain length based on the preset model prediction controller, and perform autonomous navigation control on the unmanned vehicle through the optimal control instructions.
[0045] Among them, the preview distance is a key parameter affecting trajectory tracking performance. A reasonable preview distance can improve the accuracy and stability of trajectory tracking, while an inappropriate preview distance can cause trajectory deviation or system oscillation. The main factors affecting the selection of preview distance include: first, vehicle speed. The higher the speed, the longer the preview distance should be to provide sufficient advance prediction and response time; second, road curvature. The greater the road curvature, the shorter the preview distance should be to avoid trajectory deviation caused by excessive preview distance; third, vehicle dynamics. Different vehicles have different dynamic response characteristics, and the preview distance needs to be adjusted according to the vehicle characteristics; fourth, control system bandwidth. The response speed and stability of the control system also affect the selection of preview distance.
[0046] In response to the above-mentioned influencing factors, an adaptive preview distance adjustment algorithm is used to achieve real-time adaptive adjustment of the preview distance. Specifically, the benchmark preview distance is calculated based on the current vehicle speed and the speed adjustment coefficient, and the curvature adjustment factor is calculated based on the curvature of the road ahead. The speed adjustment coefficient and the curvature adjustment factor are multiplied to obtain a comprehensive adjustment factor. Finally, the benchmark preview distance is multiplied by the comprehensive adjustment factor to obtain the adaptive preview distance, and the adaptive preview distance is used as the prediction time domain length.
[0047] In an optional embodiment, The optimal control instructions within the predicted time domain length are generated according to the preset model predictive controller. The autonomous navigation control of the unmanned vehicle through the optimal control instructions includes: Predicting the vehicle's trajectory tracking error within the prediction time domain length using a pre-built vehicle dynamics model and trajectory tracking error model. The trajectory tracking error includes lateral position error, heading angle error, and longitudinal velocity error. The vehicle's trajectory tracking error within the predicted time domain length is used as the input of the model predictive controller. The control variable change rate is introduced, and the controller multi-objective function is constructed in combination with the trajectory tracking error. The numerical optimization algorithm is used to solve the controller multi-objective function, and the optimal control instructions within the predicted time domain length are generated. The unmanned vehicle is autonomously navigated and controlled by the optimal control instructions.
[0048] For example, to implement model predictive control, it is first necessary to establish a vehicle dynamics model and a trajectory tracking error model, where the vehicle dynamics model includes a lateral motion model and a longitudinal motion model. By establishing a lateral motion differential equation such as a two-wheel model or a unit mass model, the lateral motion dynamics characteristics of the vehicle are described, and a longitudinal motion differential equation is constructed to describe the longitudinal motion dynamics of the vehicle, including acceleration, velocity, etc.
[0049] Trajectory tracking error state quantities such as lateral position error, heading angle error and longitudinal velocity error are defined. An error model describing the trajectory tracking error dynamics is established using the vehicle dynamics model and the error state quantity definitions.
[0050] The constructed vehicle dynamics model and trajectory tracking error model are used to predict the lateral position error, heading angle error and longitudinal velocity error within the prediction time domain length.
[0051] The vehicle's trajectory tracking error within the predicted time domain is used as the input of the model predictive controller. The control variable change rate is introduced, and the multi-objective function of the controller is constructed in combination with the trajectory tracking error. Multiple objectives are combined in the form of weighted sum or product, and the importance of different objectives is balanced by adjusting the weight coefficient.
[0052] Optionally, robustness constraints are introduced in the design of the model predictive controller. The robustness constraints include using state feedback to compensate for modeling errors and external disturbances, describing the uncertainty range of vehicle model parameters and incorporating it into constraints, and introducing closed-loop control system robustness evaluation indicators as constraints.
[0053] The multi-objective function is solved using numerical optimization algorithms (such as quadratic programming, gradient descent, etc.) to obtain the optimal control sequence. At each sampling moment, the first optimal control instruction is applied to control the vehicle.
[0054] In an optional embodiment, The calculation formula of the controller multi-objective function is as follows: ; Among them, M represents the controller multi-objective function, N p represents the length of the prediction time domain, represents the error prediction value, k represents the current moment, i represents the offset, Q represents the weight coefficient of the error prediction value, N c represents the length of the control time domain, u represents the control quantity sequence, △u(·) represents the control quantity change rate, R represents the weight coefficient of the control quantity change rate, and S represents the weight coefficient of the control quantity.
[0055] In this embodiment, adaptive adjustment of the preview distance is achieved by comprehensively considering multiple influencing factors, such as vehicle speed and road curvature. Appropriately increasing the preview distance on straight, high-speed sections enhances planning foresight, while appropriately reducing it on curved, low-speed sections improves immediate responsiveness. This helps the unmanned vehicle maintain excellent trajectory tracking performance under various operating conditions. Using a vehicle dynamics model and a trajectory tracking error model, the trajectory tracking error in the future time domain, including lateral position, heading angle, and longitudinal velocity errors, can be accurately predicted, providing high-quality trajectory error feedback to the controller. The controller integrates multiple objectives, such as trajectory tracking error and control variable change rate, into a single optimization objective function. Solved using a numerical optimization algorithm, the generated control instructions balance tracking accuracy and control smoothness, achieving multi-objective optimization. By combining the feedforward function of the prediction model with the feedback function of the trajectory tracking error, the model predictive controller can promptly correct accumulated errors, improving control accuracy and stability. The controller takes the optimal driving strategy determined by the trajectory plan as input and transmits the calculated optimal control instructions to the execution module, effectively connecting with upstream and downstream modules to form a complete closed-loop control system.
[0056] Figure 2 FIG. 1 is a schematic diagram of the structure of an autonomous navigation control system for an unmanned vehicle according to an embodiment of the present invention. Figure 2 As shown, the system includes: The first unit uses multi-sensor fusion to perceive the vehicle's surrounding environment, establishes a semantic map model, uses the semantic map model to extract the unmanned vehicle's driving information, uses principal component analysis to reduce the dimension of the unmanned vehicle's driving information to obtain low-dimensional state features, constructs a Markov decision model based on the low-dimensional state features, and obtains the initial behavior strategy by solving the Markov decision model; The second unit is used to generate local trajectory segments using a trajectory planning algorithm based on the initial behavior strategy, combined with the road topology and vehicle dynamics model. The local trajectory segments are discretized using an adaptive step size to generate a candidate trajectory set. A scenario-based evaluation mechanism is introduced for the candidate trajectory set to calculate the expected cost of each candidate trajectory in various scenarios. The candidate trajectory with the minimum expected cost is selected as the optimal driving strategy. The third unit is used to obtain the vehicle's current speed and the curvature of the road ahead according to the optimal driving strategy, adjust the preview distance in real time according to the changes in the vehicle's current speed and the curvature of the road ahead, obtain the predicted time domain length, generate the optimal control instructions within the predicted time domain length according to the preset model prediction controller, and perform autonomous navigation control of the unmanned vehicle through the optimal control instructions.
[0057] According to a third aspect of the embodiments of the present invention, An electronic device is provided, comprising: processor; a memory for storing processor-executable instructions; The processor is configured to call the instructions stored in the memory to execute the aforementioned method.
[0058] According to a fourth aspect of the embodiments of the present invention, A computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the method described above is implemented.
[0059] The present invention may be a method, an apparatus, a system and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for executing various aspects of the present invention.
[0060] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.
Claims
1. An autonomous navigation control method for an unmanned vehicle, characterized in that: include: The vehicle surrounding environment is perceived through multi-sensor fusion, a semantic map model is established, the driving information of the unmanned vehicle is extracted using the semantic map model, the driving information of the unmanned vehicle is reduced in dimension using the principal component analysis method to obtain low-dimensional state features, a Markov decision model is constructed based on the low-dimensional state features, and an initial behavior strategy is obtained by solving the Markov decision model; Based on the initial behavior strategy, combined with the road topology and vehicle dynamics model, a trajectory planning algorithm is used to generate local trajectory segments. At the same time, an adaptive step size is used to discretize the local trajectory segments to generate a candidate trajectory set. For the candidate trajectory set, a scenario-based evaluation mechanism is introduced to calculate the expected cost of each candidate trajectory in each scenario, and the candidate trajectory with the smallest expected cost is selected as the optimal driving strategy. According to the optimal driving strategy, the current speed of the vehicle and the curvature of the road ahead are obtained. The preview distance is adjusted in real time according to the changes in the current speed of the vehicle and the curvature of the road ahead to obtain the predicted time domain length. According to the preset model prediction controller, the optimal control instructions within the predicted time domain length are generated, and the unmanned vehicle is autonomously navigated and controlled through the optimal control instructions.
2. The method according to claim 1, characterized in that Based on the initial behavior strategy, combined with the road topology and vehicle dynamics model, the trajectory planning algorithm is used to generate local trajectory segments. At the same time, the adaptive step size is used to discretize the local trajectory segments. The candidate trajectory set generated includes: Decoding the initial behavior strategy into a motion constraint set, the motion constraint set including lateral constraints and longitudinal constraints, optimizing the motion constraint set in combination with a road topology structure, the road topology structure including road curvature, lane width, and intersection location, and performing a feasibility check on the motion constraint set using a pre-built vehicle dynamics model to obtain feasible motion constraints; A trajectory planning algorithm using layered spatiotemporal sampling is used to generate motion primitives covering the achievable motion space of the unmanned vehicle. The layered spatiotemporal sampling divides the sampling space into a lateral subspace and a longitudinal subspace. The lateral motion primitives are generated by sampling in the lateral subspace, and the longitudinal motion primitives are generated by sampling in the longitudinal subspace. The lateral motion primitive and the longitudinal motion primitive are combined to generate a local trajectory segment that satisfies the feasible motion constraint, the trajectory discrete step length is adaptively adjusted according to the road curvature and the obstacle density, the local trajectory segment is discretized according to the obtained discrete step length, and a candidate trajectory set is generated.
3. The method according to claim 2, characterized in that Optimizing the motion constraint set in combination with the road topology structure includes: The cost terms of road curvature, lane width and intersection position in the road topology are calculated respectively, and the comprehensive objective function of motion constraint is constructed by combining the road curvature cost term, lane width cost term and intersection position cost term. The calculation formula is as follows: ; Among them, J represents the comprehensive objective function, T represents the time domain range, ω1 represents the weight coefficient of the road curvature cost term, α1 represents the balance factor of the curvature size, η represents the road curvature, s represents the path arc length, t represents the time, α2 represents the balance factor of the curvature change, represents the rate of change of road curvature, ω2 represents the weight coefficient of the lane width cost term, β1 represents the amplification coefficient of the exponential cost term from the lane boundary, β2 represents the attenuation coefficient related to the exponential cost term from the lane boundary, d(t) represents the distance from the vehicle to the nearest lane boundary at time t, ω3 represents the weight coefficient of the intersection position cost term, C int represents the number of intersections, i represents the i-th intersection, γ1 represents the amplification coefficient related to the exponential cost term of the distance intersection, γ2 represents the attenuation coefficient related to the exponential cost term of the distance intersection, Represents the distance from the vehicle to the i-th intersection at time t.
4. The method according to claim 1, characterized in that: For the candidate trajectory set, a scenario-based evaluation mechanism is introduced to calculate the expected cost of each candidate trajectory in each scenario, and the candidate trajectory with the smallest expected cost is selected as the optimal driving strategy, including: Determine multiple driving scenarios corresponding to each candidate trajectory in the candidate trajectory set and construct a scenario set, and use the scenario set to segment the candidate trajectory, each segment of the candidate trajectory corresponds to a driving scenario; The risk metric of each candidate trajectory in the corresponding driving scenario is calculated using the trained risk assessment model to obtain the trajectory segment risk sequence. The scenario transition probability is introduced to construct the transition relationship of driving scenarios between different candidate trajectories. The scenario transition probability is used to map the trajectory segment risk sequence into the scenario path risk distribution. Based on the scenario path risk distribution, trajectory evaluation indicators and cost functions are designed to obtain a comprehensive trajectory evaluation system for measuring candidate trajectories in different driving scenarios. The comprehensive trajectory evaluation system is used to calculate the cost of each candidate trajectory in each driving scenario. The comprehensive cost of the trajectory is obtained by combining the costs under different driving scenarios, and the candidate trajectory with the smallest comprehensive cost is selected as the optimal driving strategy.
5. The method according to claim 4, characterized in that The calculation formula of trajectory evaluation index is as follows: ; Among them, G represents the trajectory evaluation index, M represents the number of scenarios, and δ b represents the weight coefficient of the b-th scenario, K represents the number of trajectory segments, a represents the a-th trajectory, j represents the j-th segment, and r a,j represents the risk measure of the jth segment of the ath trajectory, and P represents the scenario transition probability.
6. The method according to claim 1, characterized in that The optimal control instructions within the predicted time domain length are generated according to the preset model predictive controller, and the autonomous navigation control of the unmanned vehicle is performed through the optimal control instructions, including: Predicting the vehicle's trajectory tracking error within the prediction time domain length by using a pre-built vehicle dynamics model and a trajectory tracking error model, wherein the trajectory tracking error includes a lateral position error, a heading angle error, and a longitudinal velocity error; The vehicle's trajectory tracking error within the prediction time domain length is used as the input of the model predictive controller. The control variable change rate is introduced, and the controller multi-objective function is constructed in combination with the trajectory tracking error. The numerical optimization algorithm is used to solve the controller multi-objective function, and the optimal control instructions within the prediction time domain length are generated. The unmanned vehicle is autonomously navigated and controlled through the optimal control instructions.
7. The method according to claim 6, characterized in that The calculation formula of the controller multi-objective function is as follows: ; Where M represents the controller multi-objective function, N p represents the prediction time domain length, represents the error prediction value, k represents the current time, i represents the offset, Q represents the weight coefficient of the error prediction value, N c represents the control time domain length, u represents the control quantity sequence, △u(·) represents the control quantity change rate, R represents the weight coefficient of the control quantity change rate, and S represents the weight coefficient of the control quantity.
8. An autonomous navigation control system for an unmanned vehicle, used to implement the method described in any one of claims 1 to 7, characterized in that: include: Unit 1: Use multi-sensor fusion to perceive the surrounding environment of the vehicle, establish a semantic map model, use the semantic map model to extract the driving information of the unmanned vehicle, use the principal component analysis method to reduce the dimension of the driving information of the unmanned vehicle to obtain low-dimensional state features, build a Markov decision model based on the low-dimensional state features, and obtain the initial behavior strategy by solving the Markov decision model; The second unit is used to generate local trajectory segments based on the initial behavior strategy, combined with the road topology and vehicle dynamics model, using the trajectory planning algorithm, and using the adaptive step size to discretize the local trajectory segments to generate a candidate trajectory set. For the candidate trajectory set, a scenario-based evaluation mechanism is introduced to calculate the expected cost of each candidate trajectory in each scenario, and the candidate trajectory with the smallest expected cost is selected as the optimal driving strategy; The third unit is used to obtain the current speed of the vehicle and the curvature information of the road ahead according to the optimal driving strategy, adjust the preview distance in real time according to the changes in the current speed of the vehicle and the curvature information of the road ahead, obtain the predicted time domain length, generate the optimal control instructions within the predicted time domain length according to the preset model prediction controller, and perform autonomous navigation control on the unmanned vehicle through the optimal control instructions.
9. An electronic device, characterized in that: include: processor; a memory for storing processor-executable instructions; The processor is configured to call the instructions stored in the memory to execute the method described in any one of claims 1 to 7.
10. A computer-readable storage medium having computer program instructions stored thereon, characterized in that: When the computer program instructions are executed by a processor, the method according to any one of claims 1 to 7 is implemented.
Citation Information
Patent Citations
Intelligent public transport vehicle path planning method based on multi-objective dynamic particle swarm optimization
CN110928297A
Trajectory tracking control method and system based on longitudinal and transverse coordination
CN111409641A
Real-time planning method for safe movement track of mobile robot
CN112432648A
Multi-scene-oriented automatic driving planning method and system
CN112964271A
Hierarchical decoupling unmanned anthropomorphic trajectory planning method
CN116147631A
Cited By
Unmanned vehicle speed centralized control method and system
CN119649629A
A method and system for centralized speed control of unmanned vehicles
CN119649629B
Automatic steering driving humanoid trajectory planning method and system
CN119886495A
Dynamic walking control method for indicating machine based on traffic lane management and control
CN119960465A
Vehicle, driving track generation method, electronic equipment, storage medium and product
CN120517442A
