Multi-mode sensing trajectory planning method and system for new energy vehicle

Abstract

The invention relates to the technical field of new energy automobile intelligent driving, and discloses a new energy automobile multi-mode sensing trajectory planning method and system, and the method comprises the steps: obtaining multi-mode original sensor data, and carrying out the preprocessing; feature extraction and cross attention fusion are carried out; performing dynamic obstacle prediction, and setting a dynamic safety buffer area; constructing a behavior decision input state vector, and reasoning and adjusting a behavior decision; generating a candidate trajectory curve and performing feasibility check on the candidate trajectory curve; comprehensively evaluating the candidate track set; predicting a nonlinear optimization problem by taking the initial optimal trajectory curve as a reference trajectory, and solving and verifying the nonlinear optimization problem; according to the method, robust fusion and adaptive weight adjustment of multi-modal sensor data are realized by establishing a sensor health monitoring mechanism, a dynamic weight distribution strategy and a cross attention fusion network.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent driving of new energy vehicles, more specifically, it relates to a new energy vehicle multi-modal perception trajectory planning method and system. BACKGROUND

[0002] With the increasing urgency of global energy structure transformation and environmental protection demand, new energy vehicles have become the main direction of the development of the automobile industry. In the process of intelligentization of new energy vehicles, automatic driving technology, as a key technology to improve traffic safety, improve travel efficiency and improve user experience, has received widespread attention from the industry and academia. Trajectory planning, as the core module of the automatic driving system, its performance directly determines whether the vehicle can safely, efficiently and comfortably travel in complex dynamic environments. Especially in high-density urban mixed traffic environments, new energy operating vehicles such as taxis and online car-hailing vehicles need to frequently interact with other traffic participants, facing high dynamicity, high uncertainty and strict energy management requirements, which puts higher technical challenges on the trajectory planning system.

[0003] Current automatic driving trajectory planning technology mainly faces the following problems. The robustness of multi-modal sensor data fusion is insufficient, existing methods mostly use fixed weight fusion strategy, which fails to dynamically adjust the fusion weight according to the real-time health status of the sensor and environmental conditions, resulting in reduced system reliability when the sensor is partially failed or performance is degraded; the coupling degree of trajectory planning and energy management is not enough, traditional methods treat energy management as an independent module, without fully considering the energy flow characteristics, regenerative braking energy recovery opportunities and battery state of charge constraints of new energy vehicles in the trajectory planning stage, resulting in low energy utilization efficiency; obstacle trajectory prediction does not consider uncertainty quantification, and the trajectory planned based on deterministic prediction results lacks safety margin when the actual behavior of the obstacle deviates from the prediction; optimization algorithms are difficult to balance between real-time performance and optimization quality, sampling-based methods have good real-time performance but limited optimization quality, optimization-based methods have high quality but high computational complexity.

[0004] These technical problems seriously restrict the automatic driving capability of new energy vehicles in high-density urban mixed traffic environments, especially for operating vehicles sensitive to range, lack of energy-aware trajectory planning will increase operating costs and reduce economic efficiency.

[0005] Therefore, there is an urgent need for a trajectory planning method that can fuse multi-modal perception information, consider the energy characteristics of new energy vehicles, and have robustness and real-time performance, to meet the intelligent driving needs of new energy operating vehicles in complex urban environments. SUMMARY

[0006] The application provides a new energy vehicle multi-modal perception trajectory planning method and system, and solves the technical problems of poor perception robustness, low energy efficiency and insufficient safety in related technologies.

[0007] The application provides a new energy vehicle multi-modal perception trajectory planning method, which comprises the following steps: Obtain multi-modal original sensor data, perform preprocessing, and obtain a time-space synchronous multi-modal sensor data set; Perform feature extraction and cross-attention fusion on the time-space synchronous multi-modal data to obtain a fusion feature representation; Perform dynamic obstacle prediction based on the fusion feature representation, set a dynamic safety buffer zone, and obtain obstacle information with the dynamic safety buffer zone; Construct an action decision input state vector based on the obstacle information with the dynamic safety buffer zone and the fusion feature representation, infer and adjust the action decision, and obtain action reference parameters; Based on the action reference parameters and the obstacle information with the dynamic safety buffer zone, generate a candidate trajectory curve and perform feasibility checking on the candidate trajectory curve to obtain a candidate trajectory set; Comprehensively evaluate the candidate trajectory set to obtain an initial optimal trajectory curve; Take the initial optimal trajectory curve as a reference trajectory to predict a nonlinear optimization problem, solve the nonlinear optimization problem, and verify the nonlinear optimization problem to obtain an optimized trajectory curve and a control instruction.

[0008] In a preferred embodiment, the preprocessing comprises abnormal data detection, and the abnormal data detection comprises the following steps: Use a multi-dimensional quality index to evaluate the health status of each sensor, count the number of effective points and the uniformity of density distribution of point clouds, and set the health status identifier to abnormal when the number of effective points is lower than a first point cloud threshold value; Calculate the average brightness value and the definition index of the image of the camera, and set the health status identifier to abnormal when the average brightness exceeds the normal brightness range and the definition index is lower than a first definition threshold value; Statistically calculate the average signal-to-noise ratio of the radar echo of the millimeter wave radar, and set the health status identifier to abnormal when the signal-to-noise ratio is lower than a first signal-to-noise ratio threshold value; Determine the system operation mode based on the health status identifier of the sensor, and determine the full-function mode when all the sensors are available, and determine the degraded mode when there is an unavailable sensor.

[0009] In a preferred embodiment, the feature extraction and cross-attention fusion on the time-space synchronous multi-modal data comprises the following steps: Calculate the dynamic weight coefficient and the confidence score of each mode; Use a Softmax function to normalize the confidence score of each sensor into a dynamic weight coefficient; Multi-modal feature fusion is performed based on cross-attention mechanism, a laser radar three-dimensional space feature tensor is taken as a query vector, an image semantic feature tensor and a radar motion feature vector are taken as key and value vectors, attention weight is calculated and weighted fusion is performed.

[0010] In a preferred embodiment, the dynamic obstacle prediction based on the fusion feature representation, the setting of the dynamic safety buffer comprises: A traffic scene graph representation is constructed, the ego vehicle and each dynamic obstacle are represented as nodes of the graph, and an edge connection is established based on the distance between nodes being less than a first distance threshold; The node neighborhood information is aggregated through the message passing mechanism of the multi-layer graph convolution layer; A conditional variational autoencoder architecture is used to generate multiple hypothesis future trajectories, an encoder encodes the node features into a latent space probability distribution, and a decoder samples from the latent space to generate multiple candidate trajectories; The dynamic safety buffer is set based on the trajectory prediction uncertainty measure, and the buffer radius is equal to the basic safety distance plus the uncertainty measure multiplied by a safety coefficient.

[0011] In a preferred embodiment, the behavior decision input state vector is constructed based on the obstacle information with a dynamic safety buffer and the fusion feature representation, and the inference and adjustment of the behavior decision comprises: The decision input state vector is constructed, including an environmental state feature vector, an obstacle interaction feature vector, an ego state vector, a task target vector and an energy state vector; The decision input state vector is input into a deep reinforcement learning policy network, and a probability distribution of discrete behavior actions is output; The behavior decision is adjusted in combination with the battery state of charge.

[0012] In a preferred embodiment, the generation step of the candidate trajectory set comprises: A Frenet coordinate system with the lane centerline as a reference is established as a trajectory sampling coordinate system; In the longitudinal direction, multiple target longitudinal velocities and target arrival times are sampled based on a desired velocity range, and in the lateral direction, multiple target lateral positions and target lateral velocities are sampled based on a target lane desired lateral position; A candidate trajectory curve is generated, and six coefficients of the longitudinal and lateral polynomials are determined by the boundary conditions of the starting point and the end point; The candidate trajectory is subjected to a feasibility check, and a candidate trajectory set is obtained.

[0013] In a preferred embodiment, the comprehensive evaluation of the candidate trajectory set comprises: The safety of each candidate trajectory is evaluated, and the minimum safety margin and the minimum collision time of the ego vehicle and the obstacle are calculated; evaluate the comfort of each candidate trajectory, calculate the maximum lateral acceleration, maximum longitudinal acceleration and jerk indicators of the trajectory; predict the energy consumption of each candidate trajectory, calculate the force to overcome air resistance, rolling resistance, slope resistance and provide acceleration based on the driving force demand, combine the motor efficiency characteristics and regenerative braking efficiency to calculate the net energy consumption; evaluate the consistency with the task target, calculate the lateral position deviation, longitudinal speed deviation and time deviation of the trajectory endpoint; Based on the current battery state of charge, dynamically adjust the weight coefficients of each evaluation dimension, calculate the comprehensive score by weighted summation and select the optimal trajectory.

[0014] In a preferred embodiment, the nonlinear optimization problem is predicted with the initial optimal trajectory curve as the reference trajectory, the nonlinear optimization problem is solved and verified, which includes: Construct a multi-objective optimization function containing trajectory tracking error terms, control input smoothness terms, energy consumption terms and safety margin terms; Establish the vehicle dynamics model equation constraint and the inequality constraint condition containing control input constraint, road boundary constraint, obstacle avoidance constraint, motor torque constraint; Convert the nonlinear problem into a convex quadratic programming problem by using convex optimization relaxation method and quickly solve to get a feasible initial solution; Take the convex optimization solution as the initial working point, use the sequential quadratic programming algorithm to finely optimize the original nonlinear optimization problem, and update the working point until convergence by iteratively solving the quadratic programming subproblem.

[0015] In a preferred embodiment, the feasibility check of the candidate trajectory includes: Calculate the maximum curvature and curvature rate of the trajectory, mark as meeting the kinematic constraint when the maximum curvature is within the physical range of the vehicle steering system and the curvature rate is within the preset rate range, calculate the maximum acceleration and acceleration rate of the trajectory, mark as meeting the dynamic constraint when the maximum acceleration is within the range allowed by the motor torque and tire friction and the acceleration rate is within the comfort constraint range; Obtain the left and right boundaries of the road corresponding to the trajectory point, mark as meeting the boundary constraint when the trajectory point plus the vehicle width and safety margin is still within the road boundary; Obtain the predicted position of the obstacle at each time of the trajectory and the dynamic safety buffer zone, calculate the minimum distance between the vehicle and the obstacle safety buffer zone, mark as meeting the avoidance constraint when the minimum distance is not less than the safety distance threshold; Screen out the trajectories that meet the kinematic constraint, dynamic constraint, boundary constraint and avoidance constraint at the same time as the candidate trajectory set.

[0016] In a preferred embodiment, a new energy vehicle multi-modal perception trajectory planning system is used to execute the new energy vehicle multi-modal perception trajectory planning method described above, comprising: A multi-modal data preprocessing module is used to obtain multi-modal original sensor data, perform preprocessing, and obtain a spatiotemporal synchronous multi-modal sensor dataset; A feature extraction and fusion module is used to perform feature extraction and cross-attention fusion on the spatiotemporal synchronous multi-modal data to obtain a fusion feature representation; A dynamic obstacle prediction module is used to perform dynamic obstacle prediction based on the fusion feature representation, set a dynamic safety buffer, and obtain obstacle information with a dynamic safety buffer; A behavior decision module is used to construct a behavior decision input state vector based on the obstacle information with a dynamic safety buffer and the fusion feature representation, reason and adjust a behavior decision, and obtain behavior reference parameters; A trajectory generation module is used to generate a candidate trajectory curve based on the behavior reference parameters and the obstacle information with a dynamic safety buffer, and perform feasibility checking on the candidate trajectory curve to obtain a candidate trajectory set; A trajectory evaluation module is used to perform comprehensive evaluation on the candidate trajectory set to obtain an initial optimal trajectory curve; A trajectory optimization module is used to take the initial optimal trajectory curve as a reference trajectory to predict a nonlinear optimization problem, solve and verify the nonlinear optimization problem, and obtain an optimized trajectory curve and a control command.

[0017] The present application has the following advantages: by establishing a sensor health monitoring mechanism, a dynamic weight distribution strategy, and a cross-attention fusion network, robust fusion and adaptive weight adjustment of multi-modal sensor data are realized, the working state and data quality of each sensor are evaluated in real time, the weight of the sensor is automatically reduced or abnormal data is removed when the sensor is partially failed or the performance is degraded, the contribution of different modalities in fusion is dynamically adjusted according to environmental conditions, and the complementary advantages of each sensor are fully utilized; the cross-attention fusion network learns the relevance between different modalities to realize deep fusion at the semantic level, which improves the accuracy and integrity of environmental perception compared with traditional feature concatenation or simple weighting methods; the system operation mode determination and degradation strategy ensures that the system can continue to operate in a degraded mode when a key sensor fails, avoiding the risk of complete system failure; this multi-level robustness guarantee mechanism enables the trajectory planning system of the present application to operate stably and reliably in a complex and variable urban environment, improving the safety and usability of the system and providing an important guarantee for the commercial operation of new energy vehicles.

[0018] The application deeply couples the energy flow characteristics of new energy vehicles with trajectory planning, realizes the system-level collaborative optimization of safety, comfort and energy efficiency through the establishment of an accurate energy consumption prediction model, an energy-aware behavior decision mechanism and a multi-objective optimization framework. The energy consumption prediction model takes into account factors such as motor efficiency characteristics, regenerative braking energy recovery, road slope influence and auxiliary system energy consumption, and can accurately predict the energy consumption along different trajectories. The behavior decision module dynamically adjusts the driving strategy according to the battery state of charge, prioritizing task efficiency when energy is sufficient, and prioritizing energy saving when energy is insufficient. The trajectory optimization objective function takes energy efficiency as an important dimension, minimizes energy consumption and maximizes regenerative braking energy recovery under the premise of ensuring safety and comfort; compared with traditional methods, the application can reduce energy consumption, prolong the cruising range, reduce the charging frequency, improve the operation efficiency and economic benefit under the same driving task. Especially in the frequent start-stop and deceleration scenarios of urban working conditions, the energy recovery effect is good by planning the speed curve in advance to fully utilize regenerative braking. The energy-aware trajectory planning method provides a system-level solution for energy management of new energy operation vehicles, and has important application value and economic benefit. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 is a flow chart of a new energy vehicle multi-modal perception trajectory planning method in the application; Figure 2 is a module diagram of a new energy vehicle multi-modal perception trajectory planning system in the application. DETAILED DESCRIPTION

[0020] The subject matter described herein will now be discussed with reference to example implementations. It should be understood that the discussion of these implementations is merely meant to provide a better understanding of the subject matter described herein and can include changes, modifications, additions or omissions without departing from the scope of the content of this specification. Various processes or components can be omitted, substituted, or added according to desired implementations. In addition, features described in some examples can be combined in other examples.

[0021] A new energy vehicle multi-modal perception trajectory planning method is disclosed in at least one embodiment of the application, as shown in Figure 1 includes: Step 1, acquire multi-modal original sensor data, pre-process to obtain a spatio-temporally synchronized multi-modal sensor dataset; Step 1.1, collect multi-modal original sensor data and evaluate using multi-dimensional quality indicators; Based on the real-time acquisition of the original data stream by the vehicle-mounted multi-sensor system, including the point cloud data stream output by the laser radar, the image data stream output by the camera, the target list data stream output by the millimeter wave radar, and the angular velocity and acceleration data stream output by the inertial navigation unit, a real-time health monitoring algorithm is used to evaluate the working state of each sensor.

[0022] For the laser radar, the number of valid points of the current frame point cloud is counted, the uniformity index of the point cloud density distribution is calculated, whether there is an invalid area exceeding the threshold area is detected, and whether the point cloud number is lower than the normal working threshold is judged. When the number of valid points is lower than the first point cloud threshold or there is a large area of invalid area, the laser radar health status identifier is set to abnormal, otherwise it is set to normal. For the camera, the average brightness value and brightness variance of the image are calculated, the distribution characteristics of the image histogram are counted, the sharpness index of the image is calculated using the Laplace operator, and whether the image is overexposed, too dark or blurred is judged. When the average brightness exceeds the normal brightness range or the sharpness index is lower than the first sharpness threshold, the camera health status identifier is set to abnormal, otherwise it is set to normal. For the millimeter wave radar, the number of targets detected in the current frame is calculated, the average signal-to-noise ratio of the radar echo is counted, and whether the number of target detection is significantly deviated from the historical statistical distribution is analyzed. Whether the signal-to-noise ratio is lower than the first signal-to-noise ratio threshold is judged. When the signal-to-noise ratio is too low or the number of targets is abnormal, the millimeter wave radar health status identifier is set to abnormal, otherwise it is set to normal. For the inertial navigation unit, whether the measured values of angular velocity and acceleration exceed the reasonable range of vehicle physical motion is checked, whether the measured values have sudden changes or continuous abnormalities is judged, and when the angular velocity exceeds the first angular velocity threshold or the acceleration exceeds the first acceleration threshold, the inertial navigation unit health status identifier is set to abnormal, otherwise it is set to normal.

[0023] Get the health status identifier of each sensor, including the laser radar health status identifier, the camera health status identifier, the millimeter wave radar health status identifier and the inertial navigation unit health status identifier. Each identifier takes the value of normal or abnormal.

[0024] Step 1.2, abnormal data detection and filtering processing of each sensor health status identifier; Based on the health status identifier of each sensor and the corresponding original data stream, special processing is performed on the sensor data with abnormal identifier.

[0025] When the health status of the laser radar is identified as abnormal, the time domain filtering method is used to process the point cloud data, the consistency of the time sequence of the front and rear multiple frames of point cloud is detected and the outliers are removed, the statistical filtering method is used to calculate the distance statistical characteristics of each point and the neighborhood points, the abnormal points with distance exceeding the threshold are removed, and the filtered laser radar point cloud is obtained; when the health status of the laser radar is normal, the original point cloud data is retained. When the health status of the camera is identified as abnormal, if the image is too dark, the histogram equalization method is used to enhance the contrast of the image; if the image is overexposed, the gamma correction method is used to reduce the brightness; if the image is blurred but the blurring degree is lower than the second clarity threshold, the image sharpening algorithm is used to enhance the edge information; if the blurring degree exceeds the second clarity threshold, the frame image is marked as unusable, and the corrected camera image or unusable mark is obtained. When the health status of the millimeter wave radar is identified as abnormal, based on the historical information of target tracking, the consistency of the current frame detection result is checked, the false targets inconsistent with the historical trajectory are removed, and the credible targets are retained, and the filtered millimeter wave radar target list is obtained. When the health status of the inertial navigation unit is identified as abnormal, the median filtering method is used to smooth the angular velocity and acceleration data, or the historical data interpolation method is used to estimate the reasonable value at the current time, and the corrected inertial navigation data is obtained.

[0026] The quality-verified multi-modal data stream is obtained, including the quality-verified laser radar point cloud data stream, the camera image data stream, the millimeter wave radar target list data stream and the inertial navigation data stream.

[0027] Step 1.3, based on the quality-verified available sensor types and quantities, the system running mode determination strategy is used to obtain the system running mode identification; Based on the quality-verified multi-modal data stream and its availability state, the types and quantities of available sensors at the current time are counted.

[0028] Determine whether the lidar, camera and millimeter wave radar are all available, when all three main sensors are available and the data quality is good, determine the system running mode as full function mode, in this mode the system runs at the highest performance, can provide the most accurate environment perception and optimal trajectory planning. When one of the lidar or camera is unavailable but the other and the millimeter wave radar are available, determine the system running mode as the first degraded mode, in this mode the system relies on the remaining available sensors for perception, appropriately reduces the vehicle speed and increases the safety distance, maintains the basic automatic driving function. When the lidar and camera are both unavailable but the millimeter wave radar and inertial navigation unit are available, determine the system running mode as the second degraded mode, in this mode the system can only perform basic front obstacle detection and lane keeping, significantly reduce the vehicle speed, prepare to hand over control or execute the minimum risk strategy. When all key sensors are unavailable or the data quality cannot guarantee safety, determine the system running mode as the safe parking mode, in this mode the system sends a takeover request, if the driver does not respond, automatically execute the deceleration parking operation.

[0029] Obtain a system running mode identifier, taking values of full function mode, first degraded mode, second degraded mode or safe parking mode.

[0030] Step 1.4, establish a vehicle motion state vector, time align the multi-modal data stream after quality verification; Based on the inertial navigation data after quality verification obtained in step 1.2, including three-axis angular velocity, three-axis acceleration and time stamp information, an extended Kalman filter algorithm is used to estimate the motion state of the vehicle at each data collection time. A vehicle motion state vector is established, including the position coordinates, velocity vector, acceleration vector and attitude angle of the vehicle in the geodetic coordinate system, a state transition equation is established based on the vehicle kinematics model, an observation equation is established based on the measurement values of the inertial navigation unit, and an extended Kalman filter is used to recursively calculate the state estimation value and covariance matrix, to obtain the position, velocity, acceleration and attitude estimation results of the vehicle at each time.

[0031] The reference time is determined as the starting time of the current control period. For the laser radar point cloud data, based on the time difference between the point cloud collection time and the reference time, the pose change amount of the vehicle within the time difference is calculated. The point cloud is transformed from the vehicle coordinate system at the collection time to the vehicle coordinate system at the reference time by using the rigid body transformation method, the point cloud distortion caused by the vehicle motion is compensated, and the time-aligned laser radar point cloud is obtained. For camera image data, based on the time difference between the image collection time and the reference time and the vehicle pose change amount, the camera extrinsic parameter matrix is updated, the mapping relationship between the image pixels and the three-dimensional space points at the reference time is established, and the time-aligned camera data is obtained. For millimeter wave radar target data, based on the time difference between the target detection time and the reference time, the position and speed of the target at the reference time are calculated combined with the radial speed of the target and the vehicle pose change, and the time-aligned millimeter wave radar target list is obtained.

[0032] Based on the extrinsic calibration matrix of each sensor, the time-aligned sensor data is converted from the respective sensor coordinate system to the unified vehicle coordinate system, spatial alignment is achieved, and the spatiotemporally synchronized laser radar point cloud data, camera image data and millimeter wave radar target list data are obtained.

[0033] The spatiotemporally synchronized multi-modal sensor data set is obtained, including the laser radar point cloud, camera image and its three-dimensional mapping relationship under the vehicle coordinate system, and the millimeter wave radar target list, as well as the system running mode identifier.

[0034] Step 2, feature extraction and cross-attention fusion are performed on the spatiotemporally synchronized multi-modal data to obtain the fused feature representation. Step 2.1, laser radar three-dimensional spatial feature tensor is extracted based on the spatiotemporally synchronized laser radar point cloud. Based on the spatiotemporally synchronized laser radar point cloud data obtained in step 1, the point cloud is represented as a three-dimensional point set under the vehicle coordinate system, each point containing spatial coordinates and reflection intensity information. A voxelization method is used to divide the three-dimensional space into a regular grid, and the number of point clouds and features in each voxel are counted. The irregular point cloud data is converted into regular voxel representation to obtain voxelized point cloud. Based on the voxelized point cloud, a three-dimensional convolutional neural network is used for feature extraction. The network includes multiple three-dimensional convolutional layers, batch normalization layers and activation layers. Local geometric features of the point cloud are extracted through convolution operation, and multi-scale features are aggregated through pooling operation to obtain deep feature representation of the point cloud.

[0035] The point cloud segmentation network is used for semantic segmentation of deep feature representation of the point cloud, and the point cloud is classified into categories such as ground, road, building, vehicle, pedestrian, and vegetation, and each point or voxel is assigned a semantic label and a segmentation confidence. Based on the deep feature representation and the semantic label, a three-dimensional spatial feature tensor is constructed, which contains the geometric features, semantic categories and confidence information of each spatial position, and a three-dimensional spatial feature tensor of the lidar is obtained.

[0036] Step 2.2, image semantic feature tensor extraction based on camera image with space-time synchronization; A pre-trained convolutional neural network is used as a feature extraction backbone network, which includes multiple convolutional layers, pooling layers and residual connection modules. The space-time synchronized camera image data obtained in step 1 and the mapping relationship between the image and the three-dimensional space are input into the network, and multiple layers of feature maps are calculated through forward propagation. The shallow feature map contains rich texture and edge information, and the deep feature map contains high-level semantic information, and a multi-scale image feature map is obtained. A target detection network is used to detect the multi-scale image feature map, and the vehicle, pedestrian, traffic sign, traffic signal, etc. in the image are detected, and the bounding box coordinates, class label and detection confidence of each detection target are output, and the target detection result is obtained. A semantic segmentation network is used to perform pixel-level semantic segmentation on the multi-scale image feature map, and each pixel is assigned a semantic class label, including road, lane line, sidewalk, building, sky, vegetation, etc. The semantic segmentation result is obtained.

[0037] Based on the multi-scale feature map, the target detection result and the semantic segmentation result, the image semantic feature tensor is fused and generated, which contains the visual appearance feature, semantic category and confidence information of each pixel or feature position. The mapping relationship between the image and the three-dimensional space is combined, and the image semantic feature tensor is projected into the three-dimensional space to obtain the image semantic feature tensor associated with the three-dimensional coordinates.

[0038] Step 2.3, radar motion feature vector extraction based on millimeter wave radar target list with space-time synchronization; Based on the spatio-temporal synchronization millimeter wave radar target list data obtained in step 1, each target contains distance, azimuth angle, radial velocity and radar scattering cross section information. For each detected radar target, based on multi-frame historical data, the time sequence motion trajectory of the target is constructed, the trajectory contains the position, velocity and acceleration information of the target at the historical time, and the radar target time sequence trajectory set is obtained. The time sequence trajectory of each target is represented as time series data, and a long short-term memory network is used to encode the time sequence data. The network learns the time sequence pattern and trend of target motion through the gating mechanism, extracts the motion characteristics of the target, and obtains the motion feature vector of each radar target. The motion feature vector contains the average speed, acceleration, motion direction, speed change rate and other motion characteristic information of the target, and also contains the stability and confidence information of target detection, and obtains the radar motion feature vector set, each element in the set corresponds to a radar detection target.

[0039] Step 2.4, based on the point cloud data quality, image data quality and radar data quality, calculate the dynamic weight coefficient of each modality; Based on the sensor health status identifier obtained in step 1 and the modality features and their inherent quality indicators obtained in steps 2.1 to 2.3, the credibility of each sensor data is quantitatively evaluated.

[0040] For lidar, based on the point cloud feature tensor obtained in step 2.1, the spatial density of the point cloud is counted, the effective point cloud number in unit volume is calculated, the integrity of the point cloud coverage is evaluated, and the average segmentation confidence of different semantic categories is counted. When the point cloud density is high, the coverage is complete, and the segmentation confidence is high, the lidar confidence score is high. The density, coverage and confidence indicators are mapped to the interval of zero to one by normalization method, and the lidar confidence score is obtained by weighted summation. For camera, based on the image data obtained in step 1 and the image feature tensor obtained in step 2.2, it is analyzed whether the brightness distribution of the image is within the normal range, the image definition is calculated, and the average confidence of target detection and semantic segmentation is counted. When the image quality is good, the definition is high, and the detection confidence is high, the camera confidence score is high. The brightness, definition and detection confidence indicators are mapped to the interval of zero to one by normalization method, and the camera confidence score is obtained by weighted summation. For millimeter wave radar, based on the radar motion feature vector obtained in step 2.3, the radar echo signal-to-noise ratio is analyzed, the stability of target detection is evaluated, and the continuity index of target in multi-frame detection is calculated. When the signal-to-noise ratio is high, the detection is stable and the continuity is good, the millimeter wave radar confidence score is high. The signal-to-noise ratio, stability and continuity indicators are mapped to the interval of zero to one by normalization method, and the millimeter wave radar confidence score is obtained by weighted summation.

[0041] Based on the confidence score of each sensor and the sensor health status identification obtained in step 1, the dynamic weight coefficient is calculated, when the sensor health status identification is abnormal, the dynamic weight coefficient is set to zero or a small value close to zero, when the sensor health status identification is normal, the confidence score is taken as input, and the dynamic weight coefficient is calculated directly by using the Softmax function normalization, the Softmax function ensures that the sum of the weight coefficients of all normal sensors is one, and the laser radar dynamic weight coefficient, the camera dynamic weight coefficient and the millimeter wave radar dynamic weight coefficient are obtained.

[0042] Step 2.5, based on the modal feature tensor and the dynamic weight coefficient, the multi-modal feature fusion is carried out, and the fusion feature representation is obtained; Based on the laser radar three-dimensional space feature tensor obtained in step 2.1, the image semantic feature tensor obtained in step 2.2, the radar motion feature vector set obtained in step 2.3 and the dynamic weight coefficient of each mode obtained in step 2.4, the cross attention mechanism is used for multi-modal feature fusion.

[0043] The laser radar three-dimensional space feature tensor is taken as the main feature representation, a regular grid is established in the three-dimensional space, each grid cell corresponds to a space position, and the geometric and semantic features of the position are extracted from the laser radar feature tensor as the query vector. After projecting the image semantic feature tensor to the three-dimensional space, the image features spatially adjacent to the query position are extracted as the key vector and the value vector, the similarity between the query vector and the key vector is calculated, the similarity is obtained by scaling and Softmax normalization after vector dot product, the attention weight represents the importance of the image feature to the space position, and the attention weight is used for weighted summation of the value vector to obtain the contribution feature of the image to the position. The target features in the radar motion feature vector set that are spatially adjacent to the query position are taken as the key vector and the value vector, the similarity between the query vector and the radar key vector is calculated, the radar attention weight is obtained, and the radar value vector is weighted and summed by using the attention weight to obtain the contribution feature of the radar to the position.

[0044] Based on the laser radar dynamic weight coefficient, the camera dynamic weight coefficient and the millimeter wave radar dynamic weight coefficient, the contribution features of each mode are weighted and fused, specifically, the laser radar original feature is multiplied by the laser radar weight coefficient, the image contribution feature is multiplied by the camera weight coefficient, and the radar contribution feature is multiplied by the millimeter wave radar weight coefficient, then the weighted features are spliced or added to obtain the fusion feature vector.

[0045] The above operation is repeated for all grid cells in the space to obtain the fusion feature representation of the entire perception space, which contains three-dimensional geometric information, visual semantic information and motion state information, and the adaptive weight distribution is made according to the reliability of each sensor.

[0046] Step 3, dynamic obstacle prediction based on the fusion feature representation, setting a dynamic safety buffer, obtaining obstacle information with a dynamic safety buffer; Step 3.1, dynamic obstacle detection and tracking based on the fusion feature representation, obtaining a dynamic obstacle state list; Based on the fusion feature representation obtained in step 2, a three-dimensional target detection algorithm is used to identify dynamic obstacles in the environment. A three-dimensional target detection network is applied on the fusion feature representation, and the network outputs a three-dimensional bounding box, which contains the center position coordinates, length, width, height, orientation angle, target class and detection confidence. The target class includes car, truck, bus, motorcycle, bicycle, pedestrian, etc. The three-dimensional target detection result of the current frame is obtained.

[0047] Based on the target tracking results of the historical frames and the detection results of the current frame, a multi-target tracking algorithm is used to establish the temporal association of the targets. For each historical tracking target, its position in the current frame is predicted based on its motion model, and the predicted position is associated with the detection results of the current frame. The Hungarian algorithm is used to solve the optimal matching, and the tracking state of the matched target is updated. The un-matched detection results are initialized as new tracking targets, and the un-matched historical targets are marked as temporarily lost. The updated target tracking list is obtained.

[0048] For each target in the updated target tracking list, based on its historical trajectory data, Kalman filter or extended Kalman filter is used to estimate the current state of the target, including position, velocity, acceleration and orientation angle. The covariance matrix of the state is also estimated, representing the uncertainty of the estimation. The state estimation and uncertainty of each dynamic obstacle are obtained.

[0049] Stable targets with continuous tracking frame number greater than a threshold are selected from the tracking list to eliminate possible false detections, and a dynamic obstacle state list is obtained. Each element in the list contains the unique identifier, class, position, velocity, acceleration, orientation angle, size and historical trajectory data of the target.

[0050] Step 3.2, constructing a traffic scene graph based on the dynamic obstacle state list; Based on the dynamic obstacle state list obtained in step 3.1 and high-precision map data, a graph representation of the traffic scene is constructed, which is used to model the interaction between traffic participants.

[0051] The ego vehicle and each dynamic obstacle are represented as nodes of a graph, and for each node, a feature vector is extracted, which includes the position, velocity, acceleration, orientation angle, target category, size, and other attributes of the node, as well as historical trajectory features of the node. The trajectory features are obtained by encoding the historical trajectory data, and a set of node features is obtained. Based on the spatial relationship between the nodes, the edges of the graph are connected, and when the distance between two nodes is less than a first distance threshold, an edge is established between the two nodes, indicating that there is a potential interaction relationship between the two traffic participants. For each edge, the edge features are calculated, including the relative distance, relative velocity, and relative orientation angle between the two nodes, and a set of edges and edge features is obtained.

[0052] Based on the road topology information of the high-precision map, road context features are extracted for each node, including the type of the lane where the node is located, the lane width, the lane centerline curvature, whether it is at an intersection, the topology of the road ahead, and other information. The road context features are integrated into the node features to obtain enhanced node features. A traffic scene graph is constructed, which consists of a set of nodes, a set of edges, node features, and edge features. This graph represents the individual attributes, mutual relationships, and road environment constraints of traffic participants, and a traffic scene graph representation is obtained.

[0053] Step 3.3, predicting the obstacle multi-hypothesis future trajectory and the occurrence probability; Based on the traffic scene graph representation obtained in step 3.2, a graph neural network is used to model the interaction between traffic participants and predict future trajectories. The traffic scene graph is input into the graph neural network, which contains multiple layers of graph convolution. Each layer of graph convolution aggregates the information of the node's neighborhood through a message passing mechanism. Specifically, for each node, the features of all its neighbor nodes and the features of the connecting edges are collected, and then a learnable transformation matrix is used to transform the features. Then, an aggregation operation is performed, which can be summation, averaging, or attention weighted summation, to obtain the updated features of the node. Through the stacking of multiple layers of graph convolution, each node can perceive the influence of other traffic participants in a larger range, and the encoded node feature representation of the graph neural network is obtained.

[0054] Based on the encoded node features, a multi-hypothesis trajectory generator is used to generate multiple possible future trajectories for each dynamic obstacle. The trajectory generator uses a conditional variational autoencoder architecture, which includes an encoder and a decoder. The encoder encodes the node features and historical trajectories into a probability distribution in the latent space, and the decoder samples multiple latent vectors from the latent space. For each sampled latent vector, the decoder generates a future trajectory, which is represented as a sequence of position coordinates at future time points. Through multiple samplings, multiple different candidate trajectories are generated for each obstacle, and a set of multi-hypothesis future trajectories is obtained for each obstacle.

[0055] For each obstacle, a probability classifier is adopted to evaluate the occurrence probability of each trajectory, which is based on node features, road topology constraints and the plausibility of the trajectory, and outputs the probability value of each trajectory. The probabilities of all candidate trajectories are normalized so that their sum is one, obtaining the multi-hypothesis future trajectories of each obstacle and their corresponding probability distribution.

[0056] Step 3.4, quantifying the uncertainty of trajectory prediction; Based on the multi-hypothesis future trajectories of each obstacle and their probability distribution obtained in step 3.3, the uncertainty of trajectory prediction is quantified. For each obstacle, at each future time, the expected value of the position at that time is calculated based on the multiple hypothesis trajectories and their probabilities, which is the probability-weighted average of the positions of each hypothesis trajectory at that time. The variance of the position at that time is calculated, which is the probability-weighted average of the square of the deviation of the position of each hypothesis trajectory from the expected value. The variance represents the degree of dispersion of the predicted position, and the larger the variance, the higher the uncertainty. The position expectation and position variance at each future time are obtained.

[0057] The Monte Carlo method is used to further estimate the uncertainty. A large number of trajectory samples are randomly sampled from the probability distribution. For each future time, the position distribution of the sampled trajectories at that time is counted, and the confidence interval of the position distribution is calculated, which reflects the uncertainty of the prediction. The position confidence interval at each future time is obtained. Based on the size of the position variance or the confidence interval, the uncertainty measure of trajectory prediction is quantified, which increases with the increase of the prediction time, because the long-term prediction has more diverse possibilities. The trajectory prediction uncertainty measure of each obstacle at each future time is obtained.

[0058] Step 3.5, setting dynamic safety buffer; Based on the trajectory prediction uncertainty measure obtained in step 3.4 and the multi-hypothesis future trajectories obtained in step 3.3, a dynamic safety buffer is set for each obstacle. For each obstacle, at each future time, the radius of the safety buffer is calculated based on the trajectory prediction uncertainty measure at that time. Specifically, the buffer radius is equal to the basic safety distance plus the uncertainty measure multiplied by the safety coefficient. The basic safety distance is determined according to the vehicle speed and braking performance, and the safety coefficient is used to adjust the degree of conservatism of the uncertainty. The larger the uncertainty, the larger the buffer radius. The dynamic safety buffer radius of each obstacle at each future time is obtained. Based on the size information of the obstacle and the dynamic safety buffer radius, the safety buffer is expanded around the three-dimensional bounding box of the obstacle. The buffer represents a prohibited area around the obstacle, and the planned trajectory of the ego vehicle should avoid entering this area. The dynamic safety buffer geometry of each obstacle is obtained.

[0059] The multi-hypothesis future trajectory, probability distribution, uncertainty metric and dynamic safety buffer geometry of the dynamic obstacle are integrated together to obtain the obstacle information with dynamic safety buffer, which provides a safety constraint considering prediction uncertainty for subsequent trajectory planning.

[0060] Step 4, constructing the behavior decision input state vector based on the obstacle information with dynamic safety buffer and the fusion feature representation, reasoning and adjusting the behavior decision to obtain the behavior reference parameter; Step 4.1, constructing the input state vector of behavior decision; Obtain the task target information of the current driving, including the global path planning result, the target lane of the current road section, the recommended driving speed, the turning instruction of the front intersection, the remaining distance to the final destination and other navigation related information. Based on the fusion feature representation obtained in step 2, the obstacle information with dynamic safety buffer obtained in step 3, the current state sensor data of the vehicle and the task target information, the input state vector of behavior decision is constructed.

[0061] Extract key environmental information from the fusion feature representation, including the road passable area, lane line position, static obstacle distribution and other spatial structure information in front, left, right and rear of the vehicle, and obtain the environmental state feature vector by dimension reduction and coding of the information. Extract key interaction information from the obstacle information, select a number of obstacles closest to the vehicle, extract the relative position, relative speed, motion trend, main mode of the predicted trajectory and uncertainty degree of each obstacle, and obtain the obstacle interaction feature vector by coding the information. Obtain the current state information of the vehicle, including the longitudinal position, lateral position, longitudinal speed, lateral speed, heading angle, heading angle speed, longitudinal acceleration and lateral acceleration of the vehicle in the road coordinate system, and obtain the vehicle state vector. Extract key navigation information from the task target information, including the target lane, target speed, turning instruction of the front intersection, remaining distance to the target point and other task related information, and obtain the task target vector by coding the information. Obtain the state of charge of the vehicle from the battery management system, including the current battery power percentage, remaining range, battery temperature, charging and discharging power and other energy state information, and obtain the energy state vector.

[0062] The environmental state feature vector, obstacle interaction feature vector, vehicle state vector, task target vector and energy state vector are spliced to obtain the decision input state vector, which comprehensively describes the current driving scene and vehicle state.

[0063] Step 4.2, behavior decision reasoning; The decision input state vector is input into a deep reinforcement learning policy network, the policy network adopts a multi-layer fully connected neural network or an attention network architecture, the network calculates the probability distribution of discrete behavior actions through forward propagation, the discrete behavior actions include lane keeping, left lane changing, right lane changing, accelerating overtaking, decelerating following, left turning at an intersection, right turning at an intersection, straight driving at an intersection, and parking on the side, etc., and the output probabilities of each behavior category are obtained.

[0064] The policy network is optimized by combining offline training and online fine-tuning, imitative learning is performed on large-scale human driving data in the offline training stage to learn the decision-making mode of human drivers, and a reinforcement learning method is used to optimize the strategy in a simulation environment, the optimization objectives include safety reward, task completion reward, comfort reward and energy efficiency reward, and the strategy is updated according to actual driving feedback and safety supervision signals in the online fine-tuning stage.

[0065] Based on the output probabilities of each behavior category, the behavior category with the highest probability is selected as the initial behavior decision, or a random sampling method controlled by a temperature coefficient is used to sample behaviors from the probability distribution, which introduces moderate exploration while ensuring the rationality of the decision, and the initial behavior decision is obtained, which is represented by a behavior category identifier.

[0066] Step 4.3, adjust the behavior decision by combining the initial behavior decision and the energy state vector; Based on the initial behavior decision obtained in step 4.2 and the energy state vector obtained in step 4.1, the behavior decision is adjusted according to the energy state of the vehicle. The battery state of charge percentage is extracted from the energy state vector to determine the battery power level. When the battery state of charge is higher than the first power threshold, the vehicle has sufficient energy and does not need to adjust the initial behavior decision; when the battery state of charge is lower than the first power threshold but higher than the second power threshold, the vehicle energy is at an intermediate level and energy efficiency needs to be considered appropriately; when the battery state of charge is lower than the second power threshold, the vehicle energy is insufficient and energy saving needs to be considered first, and the energy level identifier is obtained.

[0067] When the energy level is identified as medium, the initial behavior decision is moderately adjusted, if the initial behavior decision is to accelerate to overtake, the necessity of overtaking and energy consumption are evaluated, if it is not necessary or the energy consumption is too large, the behavior decision is adjusted to decelerate to follow or keep the lane; if the initial behavior decision is to drive at high speed, the target speed is appropriately reduced, and a more energy-efficient speed is selected within the scope of safety and task permission, to obtain the adjusted behavior decision. When the energy level is identified as insufficient, the initial behavior decision is significantly adjusted, and behaviors with low energy consumption are preferentially selected to avoid frequent acceleration and high-speed driving, if the task permits, a path passing through a charging station is planned, and the behavior intention is adjusted to go to the charging station, if it is not possible to go to the charging station, the most energy-efficient driving mode is adopted, including reducing the cruising speed, increasing the coasting distance, and fully utilizing regenerative braking strategies, to obtain the energy-saving priority behavior decision. When the energy level is identified as sufficient, the initial behavior decision is kept unchanged, or a more time-efficient behavior is selected under the premise of meeting safety and comfort, to obtain the final behavior intention, which is represented as a behavior category identifier.

[0068] Step 4.4, generating behavior reference parameters for trajectory planning; Based on the final behavior intention obtained in step 4.3 and the road information of the high-definition map, the reference parameters for trajectory planning are generated.

[0069] When the behavior intention is lane keeping, the target lane is determined to be the current lane, the expected lateral position is set to the current lane center line, the expected speed is set to the road speed limit or traffic flow speed, and the acceleration range is set to the comfortable acceleration range, to obtain the reference parameters for lane keeping. When the behavior intention is to change lanes to the left or to the right, the target lane is determined to be the left lane or the right lane, the expected lateral position is set to the target lane center line, the longitudinal distance and time of lane changing are set, and the safe lane changing speed range is set based on the traffic flow state of the adjacent lane, to obtain the reference parameters for lane changing. When the behavior intention is to accelerate to overtake, the target lane and the overtaking object for overtaking are determined, the expected speed is set to be higher than the current speed, the acceleration is set to a larger acceleration but not exceeding the comfort threshold and vehicle performance limit, the longitudinal distance and time window of overtaking are planned, to obtain the reference parameters for overtaking. When the behavior intention is to decelerate to follow, the front vehicle is determined as the following target, the expected following distance is determined according to the vehicle speed and safety time interval, the expected speed is set to follow the speed of the front vehicle, and the acceleration is set to follow the acceleration of the front vehicle and maintain the safety time interval, to obtain the reference parameters for following. When the behavior intention is to turn at the intersection, the turning direction and target road at the intersection are determined, the expected speed for passing through the intersection is set, and the passing strategy is adjusted according to the intersection traffic signal state and traffic flow state, to obtain the reference parameters for turning at the intersection.

[0070] The reference parameters of each behavior type are mapped to a unified parameter space, including standardized parameters such as target lane identification, expected lateral position, expected longitudinal speed, expected speed range, acceleration range, and behavior duration. For conflicting parameters, adjust according to the priority of the behavior intention and the safety requirement, for example, when the speed range conflicts with the safety requirement, prioritize safety; when the lateral position conflicts with the lane constraint, prioritize meeting the lane boundary constraint. For missing parameters, use default values or reasonable estimates based on the current vehicle state to supplement. Finally, the behavior reference parameters are obtained, including target lane identification, expected lateral position, expected longitudinal speed, expected speed range, acceleration range, and behavior duration, which provide clear constraint conditions and optimization targets for subsequent trajectory sampling.

[0071] Step 5, based on the behavior reference parameters and obstacle information with dynamic safety buffer, generate candidate trajectory curves and perform feasibility check on the candidate trajectory curves to obtain a candidate trajectory set; Step 5.1, construct a trajectory sampling coordinate system; Based on the center line of the lane where the vehicle is currently located and high-precision map data, a Frenet coordinate system is established as the reference coordinate system for trajectory sampling. Lane recognition and positioning are performed, based on the position coordinates in the vehicle motion state vector obtained in step 1.4 and the lane line semantic information contained in the fusion feature representation obtained in step 2, combined with the lane topology information of the high-precision map, the lane where the vehicle is currently located is determined. Specifically, the vehicle position coordinates estimated by the extended Kalman filter in step 1.4 are matched with the lane boundaries in the high-precision map, the lateral distance from the vehicle position to each lane center line is calculated, and the lane with the smallest distance is selected as the candidate lane, and the lane line position information obtained by semantic segmentation in step 2.2 is used for verification and correction, ensuring the accuracy of lane recognition through multi-source information fusion.

[0072] The center line of the determined current lane is extracted from the high-precision map, and the center line is represented as a discrete point sequence in the geodetic coordinate system. The center line is spline interpolated to obtain a smooth and continuous center line curve. The tangent direction of the center line curve is defined as the longitudinal direction, and the direction perpendicular to the tangent is defined as the lateral direction, obtaining the Frenet coordinate system along the center line.

[0073] In the Frenet coordinate system, the position of the vehicle is represented by longitudinal coordinates and lateral coordinates. The longitudinal coordinates represent the cumulative arc length of the vehicle along the center line, and the lateral coordinates represent the lateral distance of the vehicle from the center line. Positive values indicate deviation to the left, and negative values indicate deviation to the right. The speed and acceleration of the vehicle are also decomposed into longitudinal and lateral components.

[0074] Convert the current state of the vehicle from the geodetic coordinate system to the Frenet coordinate system, calculate the current longitudinal position, lateral position, longitudinal velocity, lateral velocity, longitudinal acceleration and lateral acceleration of the vehicle, and obtain the current state of the vehicle in the Frenet coordinate system.

[0075] Obtain a trajectory sampling coordinate system, including a centerline reference curve and a Frenet coordinate system definition, and the current state of the vehicle in the coordinate system.

[0076] Step 5.2, generate trajectory sampling parameters; In the Frenet coordinate system, the trajectory sampling parameters are generated in space-time coupling. In the longitudinal direction, based on the expected speed range, a plurality of target longitudinal velocities are sampled within the range, and the number of samples is adaptively adjusted according to the behavior complexity, fewer samples are used for simple behaviors such as lane keeping, and more samples are used for complex behaviors such as lane changing and overtaking, based on the target longitudinal velocity and the current longitudinal velocity, a plurality of target arrival times are sampled, the range of the arrival time is determined according to the acceleration limit and the comfort requirement, a longitudinal sampling parameter set is obtained, including a plurality of combinations of target longitudinal velocities and target arrival times. In the lateral direction, based on the expected lateral position of the target lane, a plurality of target lateral positions are sampled near the expected position, the sampling range covers a reasonable interval of the lane width, based on the current lateral velocity and the target lateral position, a target lateral velocity is sampled, which is usually set to zero to ensure that the trajectory is stable at the target position, a lateral sampling parameter set is obtained, including a plurality of combinations of target lateral positions and target lateral velocities.

[0077] The longitudinal sampling parameter set and the lateral sampling parameter set are combined by Cartesian product to form a space-time coupled sampling parameter set, each set of sampling parameters includes a target longitudinal velocity, a target arrival time, a target lateral position and a target lateral velocity, the number of sampling parameters is balanced by controlling the sampling density in the longitudinal and lateral directions to balance the trajectory diversity and the calculation efficiency, and a trajectory sampling parameter set is obtained.

[0078] Step 5.3, generate candidate trajectory curves; Based on the trajectory sampling parameter set obtained in step 5.2 and the current state of the vehicle obtained in step 5.1, a candidate trajectory curve is generated using a quintic polynomial method. For each set of sampling parameters, a quintic polynomial trajectory is constructed in the longitudinal direction, with time as the independent variable and longitudinal position as the dependent variable. The six coefficients of the polynomial are determined by boundary conditions, with the starting point boundary conditions being the current longitudinal position, current longitudinal speed, and current longitudinal acceleration, and the end point boundary conditions being the target longitudinal position, target longitudinal speed, and target longitudinal acceleration. The target longitudinal position is calculated from the current longitudinal position, current longitudinal speed, target longitudinal speed, and target arrival time. The target longitudinal acceleration is usually set to zero. The longitudinal polynomial coefficients are obtained by solving a linear equation set, and the longitudinal trajectory curve is obtained. In the lateral direction, a quintic polynomial trajectory is constructed, with time as the independent variable and lateral position as the dependent variable. The six coefficients of the polynomial are determined by boundary conditions, with the starting point boundary conditions being the current lateral position, current lateral speed, and current lateral acceleration, and the end point boundary conditions being the target lateral position, target lateral speed, and target lateral acceleration. The target lateral speed is obtained from the sampling parameters, and the target lateral acceleration is set to zero. The lateral polynomial coefficients are obtained by solving a linear equation set, and the lateral trajectory curve is obtained. The longitudinal trajectory curve and the lateral trajectory curve are combined to form a two-dimensional trajectory in the Frenet coordinate system. For each time point on the trajectory, the corresponding longitudinal position and lateral position are calculated, and then the Frenet coordinates are converted back to the geodetic coordinate system to obtain the position sequence of the trajectory in the geodetic coordinate system. The kinematics parameters such as velocity, acceleration, and curvature of the trajectory are also calculated to obtain a complete candidate trajectory curve.

[0079] The above process is repeated for all sampling parameters to obtain a set of candidate trajectory curves, each trajectory in the set corresponding to a set of sampling parameters.

[0080] Step 5.4, feasibility check is performed on the set of candidate trajectory curves. Based on the candidate trajectory curve set obtained in step 5.3, the obstacle information with dynamic safety buffer obtained in step 3, and the road boundary information, the feasibility of the candidate trajectory is checked. For each trajectory in the candidate trajectory set, it is checked whether the trajectory violates the vehicle kinematics constraints, the maximum curvature and curvature rate of the trajectory are calculated, it is judged whether it exceeds the vehicle steering ability limit, the maximum acceleration and acceleration rate of the trajectory are calculated, it is judged whether it exceeds the vehicle dynamic performance and comfort limit, and if the constraints are violated, the trajectory is marked as infeasible. It is checked whether the trajectory exceeds the road boundary. For each point on the trajectory, the left and right boundaries of the road corresponding to the point are obtained based on the high-precision map, it is judged whether the trajectory point is within the road boundary, the vehicle width and safety margin are considered, and if any point on the trajectory exceeds the road boundary, the trajectory is marked as infeasible. It is checked whether the trajectory collides with the obstacle. For each time on the trajectory and each obstacle, the predicted position and dynamic safety buffer of the obstacle at the time are obtained, the position of the ego vehicle traveling along the trajectory at the time is calculated, the bounding box collision detection method or the minimum distance calculation method is used to judge whether the safety buffer of the ego vehicle and the obstacle overlaps or the distance is less than the safety distance threshold, and if there is a collision risk, the trajectory is marked as infeasible.

[0081] All trajectories marked as feasible are screened out, and the infeasible trajectories are removed to obtain a candidate trajectory set, and the trajectories in the set meet the basic kinematics constraints, road boundary constraints and obstacle avoidance constraints.

[0082] Step 6, comprehensive evaluation is performed on the candidate trajectory set to obtain an initial optimal trajectory curve; Step 6.1, the safety of each candidate trajectory is evaluated; Based on the candidate trajectory set obtained in step 5 and the obstacle multi-hypothesis future trajectory and dynamic safety buffer obtained in step 3, the safety of each candidate trajectory is evaluated.

[0083] For each trajectory in the candidate trajectory set, each time on the trajectory is traversed, for each time and each obstacle, the spatial distance between the ego vehicle and the obstacle is calculated, considering the multiple predicted trajectory hypotheses of the obstacle, the distance between the ego vehicle and each hypothesis trajectory is calculated, the hypothesis trajectory with the smallest distance is selected as the most dangerous situation, and the minimum distance between the time and the obstacle is obtained.

[0084] Based on the minimum distance and the dynamic safety buffer radius, the safety margin is calculated, the safety margin is equal to the actual distance minus the safety buffer radius, the safety margin is positive indicating safety, the larger the safety margin the safer, the safety margin is negative indicating that there is a risk of invading the safety buffer, the collision time is calculated, the collision time represents how long the current relative motion state will be maintained before collision occurs, the longer the collision time the safer, and the safety index of the time is obtained.

[0085] The minimum safety margin and the minimum collision time are counted for all time instants and all obstacles of the trajectory, and a safety score is calculated based on the minimum safety margin and the minimum collision time by using a nonlinear mapping function, the safety score is higher when the minimum safety margin is larger and the minimum collision time is longer, and the safety score is significantly reduced when the minimum safety margin is close to zero or negative, or the minimum collision time is too short, and the safety score of the trajectory is obtained, and the score range is from zero to one.

[0086] The above process is repeated for all feasible candidate trajectories to obtain a set of safety scores of the trajectories.

[0087] Step 6.2, evaluate the comfort of each candidate trajectory; Based on the set of candidate trajectories and their kinematic parameters obtained in step 5, the comfort of each candidate trajectory is evaluated.

[0088] For each candidate trajectory, the curvature sequence, the curvature rate sequence, the longitudinal acceleration sequence and the longitudinal acceleration rate sequence of the trajectory are extracted, which reflect the smoothness of the trajectory and the degree of vehicle motion.

[0089] The maximum lateral acceleration of the trajectory is calculated, the lateral acceleration is equal to the square of the vehicle speed multiplied by the curvature, excessive lateral acceleration will cause the passengers to deviate laterally and discomfort, the time proportion or amplitude of the lateral acceleration exceeding the comfort threshold is counted, and the lateral acceleration comfort index is obtained.

[0090] The maximum longitudinal acceleration and maximum longitudinal deceleration of the trajectory are calculated, excessive acceleration or deceleration will cause the passengers to sway back and forth and discomfort, the time proportion or amplitude of the longitudinal acceleration exceeding the comfort threshold is counted, and the longitudinal acceleration comfort index is obtained.

[0091] The jerk of the trajectory is calculated, the jerk is the derivative of acceleration with respect to time, which reflects the rate of change of acceleration, excessive jerk will cause impact, the maximum value and the average value of the jerk are counted, and the jerk comfort index is obtained.

[0092] Based on the lateral acceleration comfort index, the longitudinal acceleration comfort index and the jerk comfort index, a weighted sum method is used to calculate the comfort score, when each index is within the comfort range, the comfort score is higher, when any index exceeds the threshold value by a large margin, the comfort score is reduced, the comfort score of the trajectory is obtained, and the score range is from zero to one.

[0093] The above process is repeated for all feasible candidate trajectories to obtain a set of comfort scores of the trajectories.

[0094] Step 6.3, predict the energy consumption along each trajectory; Based on the candidate trajectory set obtained in step 5, vehicle dynamics parameters, motor efficiency characteristics, battery parameters and road slope information, the energy consumption along each trajectory is predicted.

[0095] For each candidate trajectory, the speed sequence and acceleration sequence of the trajectory are extracted, and the trajectory is discretized into a number of time steps. For each time step, the driving force required by the vehicle is calculated.

[0096] The driving force includes the force to overcome air resistance, the force to overcome rolling resistance, the force to overcome slope resistance, and the force to provide acceleration. The air resistance is equal to the air resistance coefficient multiplied by the windward area multiplied by half of the speed square multiplied by the air density. The rolling resistance is equal to the rolling resistance coefficient multiplied by the vehicle mass multiplied by the gravitational acceleration multiplied by the slope correction coefficient. The slope resistance is equal to the vehicle mass multiplied by the gravitational acceleration multiplied by the slope sine value. The acceleration force is equal to the vehicle mass multiplied by the acceleration. The total driving force requirement is obtained by summing the four resistances. Based on the total driving force requirement and the vehicle speed, the mechanical power required by the motor output is calculated, which is equal to the driving force multiplied by the speed.

[0097] When the mechanical power is positive, the vehicle is in driving mode. Based on the motor efficiency characteristic curve, the motor efficiency is queried according to the motor speed and torque. The motor speed is calculated from the vehicle speed and the transmission ratio, and the motor torque is calculated from the driving force and the tire radius. The battery output power is equal to the mechanical power divided by the motor efficiency, plus the vehicle auxiliary system power consumption, to obtain the battery discharge power of this time step. When the mechanical power is negative, the vehicle is in braking mode. It is judged whether the regenerative braking condition is met. If the condition is met, regenerative braking is enabled. Based on the regenerative braking efficiency characteristic, the electrical power recovered by the motor is calculated. The battery recovery power is equal to the absolute value of the mechanical power multiplied by the regenerative braking efficiency, to obtain the battery charging power of this time step. If the regenerative braking condition is not met, mechanical braking is adopted, and the energy is dissipated in the form of heat energy, and the battery recovery power is zero.

[0098] The above calculation is repeated for all time steps of the trajectory, and the battery discharge energy minus the battery charging energy of each time step is accumulated to obtain the net energy consumption of the trajectory.

[0099] Based on the net energy consumption, the energy efficiency score is calculated. The smaller the net energy consumption, the higher the energy efficiency score. A normalization method is used to map the energy consumption to a score range of zero to one to obtain the energy efficiency score of the trajectory. The above process is repeated for all feasible candidate trajectories to obtain a set of energy efficiency scores for each trajectory.

[0100] Step 6.4, evaluate the consistency of each candidate trajectory with the task target; For each candidate trajectory, calculate the deviation of the lateral position of the trajectory endpoint from the target lane centerline, the smaller the deviation, the better the trajectory reaches the target lane, calculate the deviation of the longitudinal speed of the trajectory endpoint from the expected speed, the smaller the deviation, the better the trajectory meets the speed requirement, obtain the position deviation and the speed deviation. Calculate the average speed of the trajectory, compare it with the expected average speed, evaluate whether the trajectory efficiently completes the driving task, calculate the expected time for the trajectory to reach the target point, compare it with the expected arrival time, and obtain the time deviation. Based on the position deviation, the speed deviation and the time deviation, the task consistency score is calculated by using the weighted summation method, the smaller the deviation, the higher the score, and the task consistency score of the trajectory is obtained, the score range is zero to one.

[0101] Repeat the above process for all feasible candidate trajectories to obtain a set of task consistency scores for each trajectory.

[0102] Step 6.5, dynamically weighted comprehensive evaluation and selection of initial optimal trajectory curve; Based on the safety score, comfort score, energy efficiency score and task consistency score of each trajectory obtained in steps 6.1 to 6.4, and the current battery state of charge, calculate the comprehensive score and select the optimal trajectory.

[0103] Obtain the current battery state of charge percentage from the battery management system, determine the weight coefficients of each evaluation dimension according to the battery state of charge and the driving mode setting, the weight coefficients satisfy the non-negative and normalization conditions. At normal power level, set the safety weight coefficient to a large value to ensure safety first, the comfort weight coefficient to a medium value to ensure the ride experience, the energy efficiency weight coefficient to a medium or small value, and the task consistency weight coefficient to a medium value to ensure task completion. When the power is low, appropriately increase the energy efficiency weight coefficient and correspondingly reduce the task consistency weight coefficient, prioritize energy saving under the premise of ensuring safety and basic comfort, and obtain the dynamically adjusted weight coefficients.

[0104] For each trajectory in the candidate trajectory set, multiply its safety score, comfort score, energy efficiency score and task consistency score by the corresponding weight coefficient respectively, and then sum to obtain the comprehensive score, to obtain the comprehensive score of the trajectory. Compare the comprehensive scores of all candidate trajectories, select the trajectory with the highest comprehensive score as the initial optimal trajectory curve, extract the position sequence, speed sequence and acceleration sequence corresponding to the trajectory, and obtain the initial optimal trajectory curve and its control parameter sequence.

[0105] Step 7, using the initial optimal trajectory curve as the reference trajectory to predict the nonlinear optimization problem, solving the nonlinear optimization problem and verifying, obtaining the optimized trajectory curve and control command; Step 7.1, predict the control optimization problem; Based on the initial optimal trajectory curve obtained in step 6 as the reference trajectory, combined with the vehicle dynamics model, the obstacle information obtained in step 3 and multiple constraint conditions, a vehicle dynamics model predictive control optimization problem is constructed. Specifically: Define the optimization variables as the vehicle state and control input at each time in the prediction horizon, the vehicle state includes position coordinates, speed, acceleration and heading angle, and the control input includes steering angle and longitudinal acceleration command.

[0106] Construct the optimization objective function, which contains multiple weighted terms. The first term is the trajectory tracking error term, which calculates the weighted sum of squares of the position and speed deviations between the predicted state and the reference trajectory. Minimizing this term makes the vehicle track the reference trajectory as much as possible. The second term is the control input smoothness term, which calculates the square sum of the control input changes between adjacent time points. Minimizing this term makes the control command change smoothly to avoid sudden changes. The third term is the energy consumption term, which calculates the total energy consumption in the prediction horizon according to the energy flow model in step 6.3. Minimizing this term improves energy efficiency and maximizes regenerative braking energy recovery. The fourth term is the safety margin term, which calculates the distance between the vehicle and the obstacle safety buffer. Maximizing this term increases the safety margin. After multiplying each term by the corresponding weight coefficient and summing them up, the optimization objective function is obtained.

[0107] Establish the equality constraint condition, which is the vehicle dynamics model. The next state is calculated by the current state and control input through the dynamics differential equation. The differential equation is discretized using the Euler method or Runge-Kutta method to obtain the discrete-time dynamics equality constraint.

[0108] Establish the inequality constraint condition, including control input constraints, steering angle and steering angle rate limited within the physical range of the vehicle steering system, longitudinal acceleration limited within the range allowed by motor torque and tire friction; state constraints, vehicle speed non-negative, heading angle rate limited within a reasonable range, vehicle lateral and longitudinal acceleration satisfying comfort and dynamics constraints; road boundary constraints, vehicle position within the road boundary; obstacle avoidance constraints, the distance between the vehicle and each obstacle dynamic safety buffer is not less than the safety distance threshold; motor torque constraints, motor output torque does not exceed the rated torque and the maximum torque allowed by the current battery power; battery power constraints, battery output power and recovered power do not exceed the maximum charge and discharge power allowed by the battery, to obtain the inequality constraint condition set.

[0109] The optimization variables, optimization objective function, equality constraints and inequality constraints are combined to obtain a nonlinear optimization problem.

[0110] Step 7.2, convert the nonlinear optimization problem into a convex optimization problem and solve it quickly; Based on the nonlinear optimization problem obtained in step 7.1, a convex optimization relaxation method is used to convert the problem into a convex optimization problem and solve it quickly. Linearization is performed on the vehicle dynamics model, Taylor expansion is used near the reference trajectory, the first-order term is retained, and a linear dynamics model is obtained, which is expressed in the form of a state transition matrix and a control input matrix. The nonlinear equality constraints are converted into linear equality constraints. The obstacle avoidance constraint is convexized, the dynamic safety buffer zone of the obstacle is represented as a polygon or an ellipse, and the separation hyperplane method is used to represent the avoidance constraint as a linear inequality. Specifically, a separation hyperplane is constructed between the vehicle and the obstacle, and the vehicle is constrained to be on the safe side of the hyperplane, thereby converting the non-convex collision avoidance constraint into a convex constraint. The energy consumption term is convexly approximated, and piecewise linear functions are used to approximate the motor efficiency curve and the regenerative braking efficiency curve, so that the energy consumption objective function is approximated as a linear or quadratic function. The quadratic term in the optimization objective function is retained, and the linearized and convexized constraint conditions are integrated to obtain a convex quadratic programming problem, which has a quadratic objective function and linear constraints. The interior point method or the active set method is used to solve the convex quadratic programming problem. These methods can efficiently solve convex optimization problems, have fast calculation speed, and obtain the optimal solution of the convex optimization problem, which satisfies the relaxed constraint conditions and serves as a feasible initial solution for the original nonlinear problem.

[0111] Step 7.3, fine optimization of the nonlinear optimization problem using the convex optimization feasible solution as the initial solution; Based on the convex optimization feasible solution obtained in step 7.2 as the initial working point, a sequential quadratic programming algorithm is used to fine-tune the original nonlinear optimization problem in step 7.1, and a fine-tuned solution is obtained. At the current working point, a second-order Taylor expansion is performed on the objective function of the nonlinear optimization problem to obtain a quadratic objective function, and a first-order Taylor expansion is performed on the nonlinear equality and inequality constraints to obtain linear constraints, and a quadratic programming subproblem is constructed. Solve the quadratic programming subproblem to obtain the search direction and step size of the control input, and use the line search method to determine the actual step size to ensure sufficient decrease in the objective function and reduction in constraint violation. Based on the search direction and step size, the control input and state trajectory are updated to obtain a new working point, and a new quadratic programming subproblem is constructed at the new working point.

[0112] The above process is iteratively executed, and the working point is updated each time the iteration is performed to solve a new quadratic programming subproblem. When the convergence condition is met, the iteration is stopped, and the convergence condition includes that the change in the objective function is less than a threshold value, the constraint violation is less than a threshold value, or the maximum number of iterations is reached, and the optimization solution of the sequential quadratic programming algorithm is obtained. The solution is a local optimal solution of the original nonlinear optimization problem.

[0113] Step 7.4, verification of the fine-tuned solution; Based on the fine optimization solution obtained in step 7.3, it is verified whether it satisfies all original constraint conditions. Check whether all states and control inputs of the optimization solution satisfy the inequality constraints, check the violation degree of the dynamic model equation constraints, calculate the constraint violation quantity, and when all constraint violation quantities are less than the allowable error threshold, confirm that the optimization solution is feasible, and take the optimization solution as the final optimization trajectory and control command.

[0114] When there is a constraint violation quantity exceeding the allowable error threshold, or the sequential quadratic programming algorithm fails to converge, it is judged that the optimization fails, and an emergency strategy is started. The emergency strategy includes the following measures: first, try to relax part of the non-critical constraints, such as appropriately increasing the allowable error of the safety distance threshold, and re-solve the optimization problem; if a feasible solution still cannot be obtained, reduce the expected speed of the vehicle, reduce the acceleration demand, generate a more conservative reference trajectory, and re-optimize; if there is still no feasible solution, perform an emergency avoidance maneuver or brake deceleration, and at the same time feedback to the upper-level decision module that the behavior intention needs to be re-planned, and obtain the control command of the emergency strategy.

[0115] Based on the final optimization trajectory, a rolling horizon control strategy is adopted to extract the control command of the first control period in the prediction horizon, including the steering angle command and the longitudinal acceleration command, as the control output sent to the vehicle's lower-level control system at the current time, to obtain the optimal control command at the current time.

[0116] A new energy vehicle multi-modal perception trajectory planning system, as shown in Figure 2 shown, for executing the above-mentioned new energy vehicle multi-modal perception trajectory planning method, comprising: A multi-modal data preprocessing module is configured to obtain multi-modal raw sensor data, perform preprocessing, and obtain a spatiotemporally synchronized multi-modal sensor dataset. A feature extraction and fusion module is configured to perform feature extraction and cross-attention fusion on the spatiotemporally synchronized multi-modal data to obtain a fused feature representation. A dynamic obstacle prediction module is configured to perform dynamic obstacle prediction based on the fused feature representation, set a dynamic safety buffer, and obtain obstacle information with a dynamic safety buffer. A behavior decision module is configured to construct a behavior decision input state vector based on the obstacle information with a dynamic safety buffer and the fused feature representation, reason and adjust a behavior decision, and obtain behavior reference parameters. A trajectory generation module is configured to generate candidate trajectory curves based on the behavior reference parameters and the obstacle information with a dynamic safety buffer, and perform feasibility checking on the candidate trajectory curves to obtain a candidate trajectory set. A trajectory evaluation module is configured to comprehensively evaluate the candidate trajectory set to obtain an initial optimal trajectory curve. The trajectory optimization module takes the initial optimal trajectory curve as a reference trajectory to predict a nonlinear optimization problem, solves and verifies the nonlinear optimization problem, and obtains an optimized trajectory curve and a control instruction.

[0117] In an embodiment of the present application, a specific example is provided: Actual application tests were carried out in a high-density urban mixed traffic environment. The test vehicle was a certain type of pure electric sedan with a battery capacity of 60 kWh and a full charge range of 420 km. The test scene was the morning peak period in the central area of a certain city, with high traffic density on the road, and frequent complex interaction scenes such as lane changing, overtaking and intersection passing.

[0118] During the test, the system collected multi-modal sensor data as shown in Table 1: Table 1: Multi-modal sensor data example; In a certain intersection left turn scene, the system successfully detected the opposite straight vehicle and the transverse street pedestrian, predicted the future trajectory of the opposite vehicle and pedestrian through the graph neural network, and planned a safe left turn trajectory based on the prediction result. After waiting for the opposite straight vehicle to pass, the vehicle selected the appropriate opportunity to complete the left turn, and the safe distance from the surrounding traffic participants was greater than 2 meters throughout the journey. The passengers did not feel obvious discomfort, and the energy consumption was within a reasonable range.

[0119] In a certain lane changing and overtaking scene, the system identified a slow-moving vehicle in front, determined that the lane changing and overtaking conditions were met through the behavior decision module, and generated a trajectory for left lane changing and overtaking. During the lane changing process, the system continuously monitored the rear vehicle on the left lane, dynamically adjusted the lane changing time and speed, successfully completed the overtaking and returned to the original lane. The entire overtaking process was smooth and smooth, the peak lateral acceleration did not exceed the comfort threshold, and the regenerative braking recovered part of the energy when decelerating back to the lane.

[0120] After three months of continuous road testing, a total of 5000 kilometers, the system performed stably and reliably in various typical urban traffic scenes, successfully handled thousands of complex interaction scenes, and no safety accidents occurred. Compared with the traditional single sensor trajectory planning method, the obstacle detection recall rate and accuracy of the method of the present application are improved, the success rate of trajectory planning is higher, the ride comfort is better, and the energy consumption is lower, which verifies the practicability and effectiveness of the method of the present application.

[0121] The above describes embodiments of the present application, but the embodiments are not limited to the specific implementation described above, which is only illustrative and not limiting. Those skilled in the art can make more forms of equivalent embodiments under the inspiration of the embodiments, which are all within the protection scope of the embodiments.

Claims

1. A new energy vehicle multi-modal perception trajectory planning method, characterized in that, The method comprises the following steps: acquiring multi-modal original sensor data, preprocessing to obtain a spatio-temporally synchronized multi-modal sensor dataset; performing feature extraction and cross-attention fusion on the spatio-temporally synchronized multi-modal sensor dataset to obtain a fused feature representation; based on the fused feature representation, predicting dynamic obstacles, setting a dynamic safety buffer, and obtaining obstacle information with a dynamic safety buffer; based on the obstacle information with a dynamic safety buffer and the fused feature representation, constructing a behavior decision input state vector, reasoning and adjusting the behavior decision, and obtaining behavior reference parameters; based on the behavior reference parameters and the obstacle information with a dynamic safety buffer, generating a candidate trajectory curve and performing feasibility checking on the candidate trajectory curve to obtain a candidate trajectory set; performing comprehensive evaluation on the candidate trajectory set to obtain an initial optimal trajectory curve; using the initial optimal trajectory curve as a reference trajectory to predict a nonlinear optimization problem, solving and verifying the nonlinear optimization problem to obtain an optimized trajectory curve and a control command. 2.The new energy vehicle multi-modal perception trajectory planning method according to claim 1, characterized in that, The preprocessing includes abnormal data detection, which comprises the following steps: using multi-dimensional quality indicators to evaluate the health status of each sensor, counting the number of effective points and the uniformity of density distribution of the point cloud, and setting the health status identifier to abnormal when the number of effective points is lower than a first point cloud threshold; calculating the average brightness value and the definition index of the image of the camera, and setting the health status identifier to abnormal when the average brightness exceeds the normal brightness range and the definition index is lower than a first definition threshold; statistically counting the average signal-to-noise ratio of the radar echo of the millimeter wave radar, and setting the health status identifier to abnormal when the signal-to-noise ratio is lower than a first signal-to-noise ratio threshold; based on the health status identifier of the sensor, determining the system operation mode, and determining the full-function mode when all sensors are available and determining the degraded mode when there is an unavailable sensor. 3.The new energy vehicle multi-modal perception trajectory planning method of claim 1, wherein, The feature extraction and cross-attention fusion on the spatio-temporally synchronized multi-modal data comprises the following steps: calculating dynamic weight coefficients and confidence scores for each modality; using a Softmax function to normalize the confidence scores of each sensor into dynamic weight coefficients; based on the cross-attention mechanism, performing multi-modal feature fusion, taking the three-dimensional spatial feature tensor of the laser radar as a query vector, taking the image semantic feature tensor and the radar motion feature vector as key and value vectors, calculating attention weights, and performing weighted fusion. 4.The new energy vehicle multi-modal perception trajectory planning method according to claim 1, characterized in that, The dynamic obstacle prediction based on the fused feature representation and the setting of the dynamic safety buffer comprise the following steps: constructing a traffic scene graph representation, representing the ego vehicle and each dynamic obstacle as a node of the graph, and establishing an edge connection based on the distance between nodes being less than a first distance threshold; aggregating the neighborhood information of the nodes through the message passing mechanism of the multi-layer graph convolution layer; using a conditional variational autoencoder architecture to generate multiple hypothetical future trajectories, an encoder encoding node features into a latent space probability distribution, and a decoder sampling multiple candidate trajectories from the latent space; based on the uncertainty measure of the trajectory prediction, setting a dynamic safety buffer, and the buffer radius being equal to the basic safety distance plus the uncertainty measure multiplied by a safety coefficient.

5. The multi-modal perception trajectory planning method for new energy vehicles according to claim 1, characterized in that, The construction of the behavior decision input state vector based on the obstacle information with a dynamic safety buffer and the fused feature representation, the reasoning and adjustment of the behavior decision comprise the following steps: A decision input state vector is constructed, including an environmental state feature vector, an obstacle interaction feature vector, a host vehicle state vector, a task target vector, and an energy state vector; The decision input state vector is input into a deep reinforcement learning policy network, and a probability distribution of discrete behavior actions is output; Behavior decision adjustment is combined with the state of charge of the battery. 6.The new energy vehicle multi-modal perception trajectory planning method according to claim 1, characterized in that, The generation of the candidate trajectory set includes: A Frenet coordinate system with the lane centerline as the reference is established as the trajectory sampling coordinate system; A plurality of target longitudinal velocities and target arrival times are sampled in the longitudinal direction based on the expected velocity range, and a plurality of target lateral positions and target lateral velocities are sampled in the lateral direction based on the target lane expected lateral position; A candidate trajectory curve is generated, and six coefficients of the longitudinal and lateral polynomials are determined by the start point and end point boundary conditions; The candidate trajectory is checked for feasibility, and a candidate trajectory set is obtained.

7. The multi-modal perception trajectory planning method for new energy vehicles according to claim 1, characterized in that, The comprehensive evaluation of the candidate trajectory set includes: The safety of each candidate trajectory is evaluated, and the minimum safety margin and minimum collision time of the host vehicle and the obstacle are calculated; The comfort of each candidate trajectory is evaluated, and the maximum lateral acceleration, maximum longitudinal acceleration, and jerk indicators of the trajectory are calculated; The energy consumption of each candidate trajectory is predicted, the force required to overcome air resistance, rolling resistance, slope resistance, and provide acceleration is calculated based on the driving force demand, and the net energy consumption is calculated based on the motor efficiency characteristic and regenerative braking efficiency; The consistency with the task target is evaluated, and the trajectory endpoint lateral position deviation, longitudinal velocity deviation, and time deviation are calculated; Based on the current state of charge of the battery, the weight coefficients of each evaluation dimension are dynamically adjusted, the comprehensive score is calculated by weighted summation, and the optimal trajectory is selected. 8.The new energy vehicle multi-modal perception trajectory planning method of claim 1, wherein, The nonlinear optimization problem is solved and verified with the initial optimal trajectory curve as the reference trajectory, including: A multi-objective optimization function is constructed, including a trajectory tracking error term, a control input smoothness term, an energy consumption term, and a safety margin term; A vehicle dynamics model equation constraint and an inequality constraint condition including control input constraints, road boundary constraints, obstacle avoidance constraints, and motor torque constraints are established; A convex optimization relaxation method is used to convert the nonlinear problem into a convex quadratic programming problem and quickly solve to obtain a feasible initial solution; The convex optimization solution is used as the initial working point, and a sequential quadratic programming algorithm is used to finely optimize the original nonlinear optimization problem, and the working point is updated until convergence is achieved by iteratively solving the quadratic programming subproblem. 9.The new energy vehicle multi-modal perception trajectory planning method of claim 6, wherein, The feasibility check of the candidate trajectory includes: The maximum curvature and curvature rate of the trajectory are calculated, and when the maximum curvature is within the physical range of the vehicle steering system and the curvature rate is within the preset rate range, it is marked as meeting the kinematic constraint. The maximum acceleration and acceleration rate of the trajectory are calculated, and when the maximum acceleration is within the allowable range of motor torque and tire friction and the acceleration rate is within the comfort constraint range, it is marked as meeting the dynamic constraint; The left and right boundaries of the road corresponding to the trajectory point are obtained, and when the trajectory point plus the vehicle width and safety margin is still within the road boundary, it is marked as meeting the boundary constraint. Obtain the predicted position and dynamic safety buffer zone of the obstacle at each time of the trajectory, calculate the minimum distance between the ego vehicle and the safety buffer zone of the obstacle, and mark as meeting the avoidance constraint when the minimum distance is not less than the safety distance threshold; Screen out the trajectories that simultaneously meet the kinematic constraint, the dynamic constraint, the boundary constraint and the avoidance constraint as the candidate trajectory set. 10.A new energy vehicle multi-modal perception trajectory planning system, characterized in that, A new energy vehicle multi-modal perception trajectory planning method for executing any one of claims 1-9, comprising: A multi-modal data preprocessing module for obtaining multi-modal original sensor data, preprocessing to obtain a spatiotemporally synchronized multi-modal sensor dataset; A feature extraction and fusion module for extracting and cross-attention fusion of spatiotemporally synchronized multi-modal data to obtain a fusion feature representation; A dynamic obstacle prediction module for predicting dynamic obstacles based on the fusion feature representation, setting a dynamic safety buffer zone, and obtaining obstacle information with the dynamic safety buffer zone; A behavior decision module for constructing a behavior decision input state vector based on the obstacle information with the dynamic safety buffer zone and the fusion feature representation, reasoning and adjusting the behavior decision, and obtaining behavior reference parameters; A trajectory generation module for generating candidate trajectory curves based on the behavior reference parameters and the obstacle information with the dynamic safety buffer zone, and performing feasibility checking on the candidate trajectory curves to obtain a candidate trajectory set; A trajectory evaluation module for comprehensively evaluating the candidate trajectory set to obtain an initial optimal trajectory curve; A trajectory optimization module for taking the initial optimal trajectory curve as a reference trajectory to predict a nonlinear optimization problem, solving and verifying the nonlinear optimization problem to obtain an optimized trajectory curve and a control command.

Images