Vehicle wading path planning system and method based on radar

Abstract

The invention discloses a radar-based vehicle wading path planning system and method, and relates to the technical field of intelligent driving and vehicle path planning. The system comprises a multi-mode sensing module which collects millimeter wave radar echo data, underwater sonar depth data and infrared polarization image data and constructs a standardized environment sensing data set; the feature fusion module executes weighted fusion and outputs a fusion environment feature map; the boundary enhancement module extracts a boundary gradient and generates a boundary enhancement environment feature map; the state estimation module is combined with a multi-output Gaussian process regression model and an extended Kalman filtering method to correct the state of the vehicle; the path evaluation and planning module is used for calculating a wading stability index and generating a driving track; the execution control module outputs a control instruction; the emergency state detection module monitors environmental risks in real time and triggers path re-planning. According to the invention, the risk sensing precision and the path control reliability of the vehicle in the wading environment can be improved.

Description

Technical Field

[0001] The present invention relates to the technical fields of intelligent driving and vehicle path planning, and particularly to a radar-based vehicle wading path planning system and method thereof. Background Art

[0002] With the rapid development of intelligent vehicles, assisted driving, and autonomous driving technologies, the perception, decision-making, and path planning capabilities of vehicles in various complex road environments have become important indicators for intelligent system design. In special road scenarios, wading sections, due to their unpredictability, high risk level, and strong dynamic changes, have become one of the important factors restricting the safety of intelligent vehicle driving.

[0003] Existing vehicle wading recognition and path decision-making systems mostly rely on a single type of sensor (such as a vision camera or millimeter-wave radar) for environmental perception, making it difficult to accurately identify key wading risk factors such as the water body range, water depth, and water flow velocity. Moreover, in rainy seasons or low visibility environments, traditional image recognition methods face the problem of reduced robustness. At the same time, although some systems introduce a multi-sensor data fusion mechanism, they generally adopt rule-driven or static weight methods, making it difficult to dynamically adapt to the weight changes of multi-modal data in different environments, resulting in limited expression ability of fusion features. In addition, existing path planning strategies mostly adopt static path point layout, lacking a path point screening mechanism based on wading stability evaluation indicators; although some systems use model predictive control (MPC) for path generation, they fail to effectively combine vehicle state estimation errors, boundary recognition results, and real-time water level change trends for closed-loop adjustment, and cannot achieve real-time path reconstruction and risk response in high-risk areas.

[0004] Therefore, how to construct a vehicle wading risk assessment and path planning system with multi-modal environmental perception ability, feature fusion adaptability, accurate state estimation, stable path planning, and dynamic reconstruction ability has become a technical problem urgently to be solved in this field. Summary of the Invention

[0005] The present invention aims to overcome the problems of low multi-modal environmental perception accuracy, poor feature fusion robustness, large state estimation errors, insufficient path planning stability, and lack of dynamic re-planning mechanism in emergency situations in the prior art, and provides a radar-based vehicle wading path planning system and method, so as to achieve high-precision environmental perception, risk identification, and path control for complex wading scenarios.

[0006] The present invention achieves the above object through the following technical solutions: On the one hand, the present invention provides a radar-based vehicle wading path planning system and method, including: The multi-modal perception module is used to collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data, and generate a standardized environmental perception data set; The feature fusion module is used to perform weighted feature interaction fusion on the environmental perception data set and output a fused environmental feature map; The boundary enhancement module is used to extract boundary gradient features from the fused environmental feature map and enhance edge expression to generate a boundary-enhanced environmental feature map; The state estimation module is used to jointly correct the vehicle state based on a multi-output Gaussian process regression model and an extended Kalman filter algorithm and output system state information; The path evaluation and planning module is used to evaluate path stability and generate driving trajectory data based on the system state information and the boundary-enhanced environmental feature map; The execution control module is used to output a vehicle driving control instruction according to the driving trajectory data; The emergency state detection module is used to detect environmental risk changes and trigger path replanning and control strategy switching.

[0007] A further improvement of the present invention lies in that the multi-modal perception module includes: The radar perception sub-unit is used to collect millimeter-wave radar echo data and extract distance, reflectivity, and speed information related to the vehicle's traveling path; The sonar perception sub-unit is used to collect underwater sonar depth data and riverbed contour information and generate a two-dimensional depth distribution map; The visual perception sub-unit is used to collect infrared polarization image data and extract texture smoothness features, color features, and occluder edge features; The registration sub-unit is used to perform unified time synchronization processing and spatial coordinate registration processing on the data collected by the radar perception sub-unit, sonar perception sub-unit, and visual perception sub-unit, where the time synchronization error does not exceed 5 milliseconds, and three-dimensional spatial coordinate registration is performed based on the external parameter matrix, and the registration error does not exceed 0.2 meters, and a standardized environmental perception data set is output.

[0008] A further improvement of the present invention lies in that the feature fusion module includes: The feature extraction sub-unit is used to separately extract a visual perception feature map and a millimeter-wave radar perception feature map from the standardized environmental perception data set, where the visual perception feature map is extracted through a convolutional neural network based on the infrared polarization image data, and the millimeter-wave radar perception feature map is extracted through a feature encoding network based on the millimeter-wave radar echo map data; The channel splicing sub-unit is used to splice the visual perception feature map and the radar perception feature map after spatially aligning them in the channel dimension to form a fused input tensor; A weight generation subunit, configured to input the fused input tensor into a convolutional network structure to extract interaction features, and normalize them through a Sigmoid activation function to generate a fused weight matrix, where each channel of the fused weight matrix corresponds to a weight factor of different modality feature maps; A fusion subunit, configured to perform element-wise weighted fusion on the visual perception feature map and the radar perception feature map according to the fused weight matrix, and introduce an element-wise product term between the two types of feature maps as a non-linear interaction enhancement term, and output a fused environment feature map.

[0009] A further improvement of the present invention lies in that the boundary enhancement module includes: An edge gradient extraction subunit, configured to input the fused environment feature map into a multi-scale Sobel convolutional kernel group to extract horizontal boundary gradient maps, vertical boundary gradient maps, and diagonal boundary gradient maps at multiple scales respectively. The extraction method is as follows: ; Where: represents the fused environment feature map; , , represents the layer-scale horizontal, vertical, and diagonal direction Sobel convolutional kernels; , , represents the extracted boundary gradient maps at the layer scale; represents the multi-scale level index; An edge feature fusion subunit, configured to perform weighted fusion on boundary gradient maps of different scales and directions to generate a boundary response map. The fusion formula is: ; Where: represents the boundary response map; represents the fusion weight corresponding to the layer degree, represents the diagonal direction boundary enhancement factor; represents the element-wise absolute value operation; represents the number of multi-scales; A boundary enhancement generation subunit, configured to splice the boundary response map and the fused environment feature map in the channel dimension, and perform channel compression through 1×1 convolution to generate a boundary enhanced environment feature map. The expression is as follows: ; Where: represents the boundary enhanced environment feature map; represents the channel direction feature splicing operation; is a one-dimensional convolution operation.

[0010] A further improvement of the present invention lies in that the state estimation module includes: An error prediction sub-unit, which is used to predict the error of the system state information based on the multi-output Gaussian process regression model (MOGPR). The MOGPR model constructs training samples using historical system state information and state estimation error information, and after performing feature mapping on the current input data based on the radial basis function kernel (RBF kernel), outputs the predicted error mean and error covariance matrix corresponding to the system state through inner product operations; A covariance correction sub-unit, which is used to input the error prediction result into the extended Kalman filter (EKF) for state recursion correction, and perform state update in combination with the error covariance matrix, and output the corrected system state information; A state update sub-unit, which is used to use the corrected system state information as the input for the subsequent module to support path planning and dynamic decision-making calculations.

[0011] A further improvement of the present invention lies in that the path evaluation and planning module includes: A stability index calculation sub-unit, which is used to calculate the vehicle wading stability index according to the following formula , and the expression is: ; Where: represents the vehicle wheel radius, which affects the vehicle wading contact surface; represents the adhesion coefficient between the tire and the riverbed, which is used to calculate the tire grip; represents the water depth, which affects the vehicle buoyancy and stability; represents the vehicle speed, which affects the kinetic energy and traction force of the vehicle when wading; represents the water flow speed, which affects the stability of the vehicle during wading; is a small positive constant, which is used to avoid numerical instability when the water flow speed is close to zero; A path screening sub-unit, which is used to evaluate and screen a set of multiple path point candidates according to the calculated stability index. When the stability index of any path point is lower than the preset threshold of 1.0, the corresponding path point is removed from the candidate set to form a set of path points that meet the stability requirements; A trajectory generation sub-unit, which is used to perform path optimization operations on the set of path points based on the model predictive control method (MPC) to generate a sequence of vehicle future driving trajectory points. The trajectory optimization considers the cost function between the control input quantity and the state change, and outputs a sequence of trajectory points for the execution control module to call.

[0012] A further improvement of the present invention lies in that the path evaluation and planning module further includes: A path dynamic adjustment subunit, configured to periodically adjust a set of path points according to updated system status information; A risk level change detection subunit, configured to perform time series change analysis on risk level indicators generated during path evaluation, and when it is detected that the change amplitude of the risk level of any path point exceeds 10% compared to the previous evaluation value, send a path replanning request signal to the path evaluation and planning module.

[0013] A further improvement of the present invention lies in that the execution control module includes: A steering angle control subunit, configured to generate a steering angle control instruction according to a vehicle driving trajectory data set, and control the actual steering angle of the vehicle within an error range of ±10° of the set trajectory angle through a vehicle steering actuator; A speed control subunit, configured to generate a speed control instruction according to speed information in the driving trajectory, and adjust the actual vehicle speed within an error range of ±5 km / h of the target speed; A power mode control subunit, configured to generate a power system control instruction for controlling the adjustment of the engine air intake height, the increase of the engine speed, and the differential lock state when receiving a wading risk warning signal; A navigation projection control subunit, configured to generate an augmented reality navigation path based on a vehicle driving trajectory data set, and superimpose the navigation route in an augmented reality manner within the image projection area in front of the vehicle for visual navigation by the driver.

[0014] A further improvement of the present invention lies in that the emergency state detection module includes: A water level change monitoring subunit, configured to calculate the water level rising rate from upstream water level data periodically extracted from a standardized environment perception data set; A risk threshold judgment subunit, configured to trigger an emergency path replanning request when the water level rising rate exceeds a preset threshold of 5 cm / min; A power response control subunit, configured to output a power increase control instruction for increasing the engine speed to not less than 2500 rpm to the execution control module after receiving an emergency trigger signal, and synchronously activate an anti-stall protection strategy.

[0015] On the other hand, the present invention provides a vehicle wading risk assessment and path planning method based on a multi-band radar, which applies the vehicle wading risk assessment and path planning system according to any one of the above, and includes the following steps: Step 1: Collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data through a multi-modal perception module, perform time synchronization and spatial coordinate registration on various types of perception data, and generate a standardized environment perception data set; Step 2: Input the standardized environmental perception dataset into the feature fusion module, and perform modal feature fusion based on the weighted feature interaction fusion mechanism to generate a fused environmental feature map; Step 3: Input the fused environmental feature map into the boundary enhancement module, and generate a boundary-enhanced environmental feature map through boundary gradient feature extraction and edge enhancement; Step 4: Use the multi-output Gaussian process regression model to predict the vehicle state error and the error covariance matrix, and combine the extended Kalman filter algorithm to correct the vehicle state and output the system state information; Step 5: Input the system state information and the boundary-enhanced environmental feature map into the path evaluation and planning module, calculate the path stability index, and accordingly screen the path point set, and generate vehicle driving trajectory data based on the model predictive control method; Step 6: Generate the steering control instruction, speed control instruction, and power system control instruction of the vehicle in the execution control module according to the driving trajectory data; Step 7: Analyze the water level change in the environmental perception data in the emergency state detection module. When it is detected that the risk condition meets the preset threshold, trigger the replanning process of the path evaluation and planning step, and at the same time trigger the execution control module to switch the control strategy.

[0016] The beneficial effects of the present invention are as follows: By integrating the multi-modal perception module of millimeter-wave radar, sonar, and infrared polarization images, the present invention can comprehensively perceive information such as water surface reflection, water depth, and flow velocity under various complex water-related environments, and significantly improves the robustness of environmental modeling compared with traditional single perception methods. Through the weighted feature interaction fusion mechanism introduced by the feature fusion module, the dynamic fusion of multi-modal features in the channel dimension is realized, enhancing the system's perception ability of key water-related areas. The boundary enhancement module adopts a multi-scale edge extraction and fusion compression method to effectively highlight key spatial features such as the water-land boundary and obstacle contours, providing accurate boundary information for subsequent path analysis. The state estimation module jointly models through the multi-output Gaussian process regression model and the extended Kalman filter, which can improve the vehicle pose estimation accuracy and reduce the impact of error accumulation on path prediction. The path evaluation and planning module uses the water-crossing stability index to guide the screening of path points, and dynamically generates a controllable trajectory in combination with the model predictive control algorithm, improving the passing reliability of the water-crossing path. The execution control module outputs the vehicle steering, speed, and power control instructions in real time according to the generated trajectory, realizing the closed-loop control during the path tracking process. The emergency state detection module has the ability to quickly identify and respond to the water level change trend, and can automatically trigger path replanning and power system switching under sudden water level rise conditions, effectively improving the emergency safety level of the whole vehicle under extreme water-crossing risks. The present invention has achieved collaborative improvement in aspects such as multi-modal environmental perception, state estimation accuracy, path stability evaluation, and risk response ability, and has high technological advancement and engineering application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings. Among them: Figure 1 is the system modular diagram of the present invention; Figure 2 is the method flow chart of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0018] In order to make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the following will clearly and completely describe the technical solutions of the embodiments of the present invention in conjunction with the drawings of the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Based on the described embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art belong to the scope of protection of the present invention.

[0019] As Figure 1 shown, it is an embodiment of the present invention. This embodiment provides a radar-based vehicle wading path planning system and its method, including: (1) Multimodal perception module It is used to collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data, and generate a standardized environmental perception data set; The radar perception sub-unit is used to collect millimeter-wave radar echo data and extract distance, reflectivity, and speed information related to the vehicle's travel path; The sonar perception sub-unit is used to collect underwater sonar depth data and riverbed contour information, and generate a two-dimensional depth distribution map; The visual perception sub-unit is used to collect infrared polarization image data and extract texture smoothness features, color features, and occluder edge features; The registration sub-unit is used to perform unified time synchronization processing and spatial coordinate registration processing on the data collected by the radar perception sub-unit, sonar perception sub-unit, and visual perception sub-unit. Among them, the time synchronization error does not exceed 5 milliseconds, and three-dimensional spatial coordinate registration is performed based on the external parameter matrix, and the registration error does not exceed 0.2 meters, and a standardized environmental perception data set is output.

[0020] In this embodiment, in order to achieve high-precision detection of key physical information in the wading environment, the multimodal perception module further adopts the following structural optimization and data collaboration mechanism: The millimeter-wave radar sensing unit selects an FMCW millimeter-wave radar array based on the synthesis of 24GHz and 77GHz dual frequencies, which is set at the symmetric positions on the left and right of the vehicle's front bumper and is used to synchronously collect the water surface reflection intensity, incident angle echo delay, and dynamic velocity vector. To reduce multipath interference, the system introduces an angular domain filtering enhancement algorithm to dynamically shield the vertical blind area signals according to the field of view angle during the data preprocessing stage, improving the continuity and credibility of the target radar profile.

[0021] The sonar sensing unit adopts a 300kHz high-frequency side-scan structure, which is installed on both sides of the vehicle bottom, and the scanning resolution is better than 1cm / pixel. To enhance the underwater profile stability, the system judges the difference in bottom bed materials through the matching method of sound intensity echo attenuation characteristics, and calculates the change rate of echo amplitude in real time to assist in judging bottom attachments or potential obstacles.

[0022] The infrared polarization image sensing unit is configured with a dual-optical-axis polarization camera, which fuses the polarization angle and the infrared brightness channel to form a texture / occlusion joint image group. To improve the boundary segmentation ability of the image in low-contrast wading scenarios, the system integrates a directional gradient enhancement module, extracts the texture structure along the water surface direction with a Gabor filter bank, and realizes the robust recognition of the far-distance water body profile.

[0023] In terms of multi-modal data alignment, the system adopts a synchronization mechanism based on a shared clock source (GPS-PPS + CAN internal network protocol) to ensure that the acquisition error of each mode is controlled within 5 milliseconds; in terms of spatial registration, a linear calibration matrix based on the vehicle body coordinate system is introduced, and the external parameter correction + bilinear interpolation method is used to realize the high-precision projection of sonar point clouds and millimeter-wave radar echoes in a unified three-dimensional space, and the registration error is controlled within 0.2 meters.

[0024] Compared with the single-modal scheme that only uses radar or image data for wading recognition in the prior art, this embodiment uses the three-source joint perception of radar-sonar-vision, which not only improves the recognition ability of multi-dimensional information such as water depth, boundary, and dynamic flow velocity, but also enhances the robustness of the system in extreme environments such as rain, fog, night, and low light, significantly expanding the adaptation range and data credibility of the sensing system in actual wading path planning.

[0025] (2) Feature fusion module It is used to perform weighted feature interaction fusion on the environmental perception data set and output a fused environmental feature map; The feature extraction sub-unit is used to separately extract a visual perception feature map and a millimeter-wave radar perception feature map from the standardized environmental perception data set. The visual perception feature map is extracted through a convolutional neural network based on infrared polarization image data, and the millimeter-wave radar perception feature map is extracted through a feature encoding network based on millimeter-wave radar echo map data; A channel splicing subunit, which is used to splice the visually perceived feature map and the radar-perceived feature map after spatially aligning them in the channel dimension to form a fused input tensor; A weight generation subunit, which is used to input the fused input tensor into a convolutional network structure to extract interaction features, and normalize them through a Sigmoid activation function to generate a fused weight matrix, where each channel of the fused weight matrix corresponds to a weight factor of different modality feature maps; A fusion subunit, which is used to perform element-wise weighted fusion on the visually perceived feature map and the radar-perceived feature map according to the fused weight matrix, and introduce the channel-wise product term between the two types of feature maps as a non-linear interaction enhancement term, and output a fused environmental feature map.

[0026] In this embodiment, the feature fusion module further adopts a method combining a multi-channel attention weighting mechanism and a non-linear interaction structure to deeply fuse the key target regions in the multi-modal environmental perception data. Specifically, the system first normalizes the visually perceived feature map and the millimeter-wave radar-perceived feature map respectively, and calculates their spatial variance maps and gradient direction maps respectively in each channel dimension, and extracts the response significant regions within each modality.

[0027] Subsequently, the fusion module calls the Fusion Attention Net to perform element-level association on the above-mentioned significant maps and the channel feature tensors, assigns a dynamically adjustable fusion factor to each channel, generates a channel attention weight matrix, and introduces a Sigmoid-Gated gating mechanism to perform a contraction mapping on the weight range to avoid overfitting or masking of the overall output result by a certain modality.

[0028] To further improve the complementary expression ability between different modalities, the system introduces a cross-modal residual non-linear interaction structure, which newly builds dual-path convolutional branches during the fusion process, respectively performs in-channel convolution and inter-modal convolution, and superimposes them on the main path of the fused features through residual connections. The entire structure can realize the dynamic enhancement of feature maps at different scales and directions.

[0029] The output of the above fusion process is encoded as a unified fused environmental feature map, which simultaneously contains multiple-dimensional information such as visual texture, boundary contour, radar spatial response, and depth estimation, and has spatial structure consistency and physical semantic alignment. System experiments show that this fusion mechanism can still stably identify the wading boundary and water body change areas under low visibility conditions, and the average improvement in the mIoU and boundary accuracy metrics is more than 12% compared with traditional weighted average or stacking methods.

[0030] The feature fusion module realizes the highly robust integrated expression of multi-source perception data through the attention-guided and residual non-linear interaction fusion mechanism, providing high-resolution and high-reliability input feature support for the subsequent boundary enhancement and path evaluation modules.

[0031] (3) Boundary Enhancement Module It is used to extract boundary gradient features from the fused environmental feature map and enhance the edge expression to generate a boundary-enhanced environmental feature map; Edge gradient extraction sub-unit, which is used to input the fused environmental feature map into a multi-scale Sobel convolution kernel group to extract horizontal boundary gradient maps, vertical boundary gradient maps, and diagonal boundary gradient maps at multiple scales respectively. The extraction method is as follows: ; Where: represents the fused environmental feature map; , , represents the layer-scale horizontal, vertical, and diagonal direction Sobel convolution kernels; , , represents the extracted boundary gradient maps at the layer scale; represents the multi-scale level index; Edge feature fusion sub-unit, which is used to weight and fuse boundary gradient maps in different scales and directions to generate a boundary response map. The fusion formula is: ; Where: represents the boundary response map; represents the fusion weight corresponding to the layer degree, represents the diagonal direction boundary enhancement factor; represents the element-wise absolute value operation; represents the number of multi-scale layers; Boundary enhancement generation sub-unit, which is used to splice the boundary response map and the fused environmental feature map in the channel dimension and perform channel compression through 1×1 convolution to generate a boundary-enhanced environmental feature map. The expression is as follows: ; Where: represents the boundary-enhanced environmental feature map; represents the channel direction feature splicing operation; is a one-dimensional convolution operation.

[0032] In this embodiment, on the basis of multi-scale edge extraction and direction fusion, the boundary enhancement module further introduces a gradient coherence enhancement mechanism (GCE) to improve the separability and structural consistency of boundary features in low-contrast wading scenes.

[0033] Specifically, first in the multi-scale Sobel edge extraction stage, the system constructs a direction field tensor based on the gradient response map of the fused environmental feature map, calculates it through a spatial position gradient angle consistency index (such as cosine similarity), and marks the regions where the gradient direction differences in the boundary response maps of each direction (horizontal, vertical, diagonal) are greater than a set threshold (such as 30 degrees) as boundary discontinuous regions, and performs edge interpolation and reconstruction processing on these regions.

[0034] During the edge feature fusion process, to avoid boundary blurring or noise interference caused by large differences between direction response maps, the system introduces an anisotropic direction adjustment mechanism. Specifically, first, a direction field tensor is constructed based on the gradient response map of the fused environmental feature map, and the consistency of boundaries in each direction is calculated using a spatial position gradient angle consistency index (such as cosine similarity). When the gradient direction in the boundary response map of a certain direction (such as horizontal, vertical, or diagonal) deviates from the neighborhood direction by more than a set threshold (such as 30 degrees), the system marks this region as a boundary discontinuous region and performs complementation and reconstruction processing through edge interpolation.

[0035] In addition, the system also constructs a boundary saliency mask map (Saliency Mask) for each direction respectively. This mask is dynamically generated based on indicators such as boundary connectivity, gradient magnitude, and regional sparsity, and is multiplied with the boundary response map of the corresponding direction through a product gating method to achieve the suppression of noise response and regional-level fusion adjustment, thereby improving the robustness and discriminability of the boundary enhancement module in complex scenes.

[0036] In addition, after the boundary response map after fusion in the boundary enhancement module is concatenated with the original fused feature map in channels, it is not directly input into the 1×1 convolutional layer for compression. Instead, through a multi-channel feature selection network (ChannelSelection Net), redundant feature elimination and boundary feature enhancement are first completed, and then convolutional dimensionality reduction operations are performed, effectively avoiding the loss of boundary feature information during the fusion stage.

[0037] The finally output boundary-enhanced environmental feature map retains the clear water-land boundary form and water body contour information, and has the ability to stably express the multi-directional gradient change structure, significantly improving the accuracy and robustness of the system for identifying the boundaries of wading areas. Experimental tests show that under typical low-light wading conditions, this module improves by approximately 17.4% and 13.1% respectively compared to the traditional Sobel fusion scheme in terms of the edge precision index (EdgePrecision, EP) and the Hausdorff distance (HD).

[0038] The boundary enhancement module described in this embodiment realizes a high-quality boundary expression process from "edge extraction - heterogeneous fusion - spatial encoding" through a gradient direction consistency judgment, anisotropic response adjustment, and feature channel selection and compression mechanism, effectively supporting the high-demand input of spatial semantics for the subsequent state estimation and path planning modules.

[0039] (4) State Estimation Module It is used to jointly correct the vehicle state based on the multi-output Gaussian process regression model and the extended Kalman filter algorithm, and output the system state information; The error prediction subunit is used to predict the error of the system state information based on the multi-output Gaussian process regression model (MOGPR). The MOGPR model constructs training samples using historical system state information and state estimation error information, and after performing feature mapping on the current input data based on the radial basis function kernel (RBF kernel), outputs the predicted error mean and error covariance matrix of the corresponding system state through inner product operations; The covariance correction subunit is used to input the error prediction result into the extended Kalman filter (EKF) for state recursive correction, and perform state update in combination with the error covariance matrix, and output the corrected system state information; The state update subunit is used to use the corrected system state information as the input for the subsequent module to support path planning and dynamic decision-making calculations.

[0040] In this embodiment, the state estimation module combines a non-linear modeling and multi-dimensional covariance management mechanism to construct a highly robust vehicle state inference structure for wading scenarios. First, the error prediction subunit calls the multi-output Gaussian process regression model (Multi-Output Gaussian Process Regression, MOGPR) to model the system state error distribution by jointly considering the historical state variable sequence and the perception feature change trajectory.

[0041] The MOGPR model in this system adopts a collaborative kernel group based on the Composite RBF Kernel, and combines the inter-frame difference of the perception data in the time series dimension as the input of the non-linear mapping to achieve the joint output of the mean of the state error and the covariance matrix. Among them, the state dimension covers six continuous variables such as the two-dimensional position (x, y), velocity vector (vx, vy), heading angle and angular velocity of the vehicle.

[0042] After the prediction output, the covariance correction sub-unit inputs it into the Extended Kalman Filter (EKF) to perform error correction and state smoothing. The EKF module dynamically adjusts the gain weight according to the residual between the estimated value of the fusion feature map and the prior state prediction value, combined with the main diagonal elements in the state error covariance matrix, to improve the state estimation stability of the system under strong noise input. Especially when the sonar data fluctuates greatly and the underwater occlusion is serious, the filter can automatically reduce the update ratio of the sonar observation to the fusion state, forming a perception robustness adaptive adjustment mechanism.

[0043] After the state update sub-unit completes the EKF correction, it synchronously pushes the corrected state variables to the path evaluation and planning module, the dynamic control module and the emergency response module, and caches the historical state trajectory in this cycle for the next cycle error prediction call, constituting a time series closed-loop.

[0044] Compared with the existing systems that only use single-modal observation data or traditional linear prediction methods for state estimation, the present invention improves the adaptability of the system to non-Gaussian error distribution and time-varying error characteristics through a multi-output non-linear regression and extended filtering joint mechanism, and significantly reduces the cumulative positioning offset in the wading fluctuation environment. The simulation results show that in a typical dynamic water area scenario, the average state estimation error of this module is controlled within 0.28m, which is better than the error level of more than 0.4m of the existing similar systems, and has the ability of adaptive error constraint.

[0045] (5) Path Evaluation and Planning Module Used to evaluate the path stability and generate driving trajectory data based on the system state information and the boundary enhanced environmental feature map; The stability index calculation sub-unit is used to calculate the vehicle wading stability index according to the following formula , and the expression is: ; Where: represents the vehicle wheel radius, which affects the vehicle wading contact surface; represents the adhesion coefficient between the tire and the river bed, which is used to calculate the tire grip; represents the water depth, which affects the buoyancy and stability of the vehicle; represents the vehicle speed, which affects the kinetic energy and traction force of the vehicle when wading; represents the water flow speed, which affects the stability of the vehicle when wading; is a small positive constant used to avoid numerical instability when the water flow speed approaches zero; The path screening subunit is used to evaluate and screen multiple sets of candidate path points according to the calculated stability index. When the stability index of any path point is lower than the preset threshold of 1.0, the corresponding path point is removed from the candidate set to form a set of path points that meet the stability requirements; The trajectory generation subunit is used to perform path optimization operations on the set of path points based on the model predictive control method (MPC) to generate a sequence of future driving trajectory points of the vehicle. The trajectory optimization considers the cost function between the control input quantity and the state change, and outputs the sequence of trajectory points for the execution control module to call.

[0046] The path dynamic adjustment subunit is used to periodically adjust the set of path points according to the updated system state information; The risk level change detection subunit is used to perform time series change analysis on the risk level index generated during the path evaluation process, and when it is detected that the change amplitude of the risk level of any path point exceeds 10% compared with the previous evaluation value, a path replanning request signal is sent to the path evaluation and planning module.

[0047] In this embodiment, the path evaluation and planning module further adopts a dynamic path reconstruction mechanism based on stability perception guidance. To quantify the traffic controllability of the vehicle at different path points under wading risk conditions, the system introduces a multi-parameter wading stability index calculation formula, where each variable comes from the fusion of vehicle chassis parameters, water flow perception results, and radar image feature extraction results. To adapt to different vehicle models, the stability calculation model incorporates a wheel diameter calibration coefficient table and an adhesion dynamic adjustment model, and supports real-time adjustment of the ε suppression term to adapt to different flow velocity fields.

[0048] During the path screening process, the system no longer adopts a unified threshold elimination strategy, but introduces a stability confidence interval adaptive mechanism: when the stability index of a path point approaches the threshold boundary, the module automatically performs regional interpolation on the upper and lower neighborhood points to calculate the local mean stability and the maximum gradient of this path segment. If a gradient anomaly occurs (i.e., the stability change is greater than 2σ), then this path segment is marked as a "potential high-risk segment" for the path avoidance sub-module to adjust preferentially.

[0049] In terms of trajectory generation, the system constructs a five-step time-domain prediction window, predicts the vehicle state for the next 2 seconds in each step, and adopts a nonlinear model predictive control method with state constraints (NL-MPC). The current system state vector, the set of path points to be traversed, and the control reference constraints are jointly input, and a steering angle-acceleration control sequence and the objective function value that minimizes the trajectory error are output. In particular, to solve the problem that tire slippage on wading roads affects the accuracy of control inputs, the system fuses the estimated changing trend of road adhesion and dynamically adjusts the expected corner response speed of the trajectory.

[0050] In the part of path dynamic adjustment, the module regularly evaluates the state consistency difference between the traversed path segment and the planned path segment ahead, and introduces the "risk level volatility" index in the risk detection sub-module to determine whether the path planning result needs to be reconstructed. When the mean difference in path stability between two consecutive times exceeds 10% or the gradient of the maximum change point exceeds the threshold θ (empirically set to 0.15), the system will simultaneously issue a path replanning instruction and a controller pre-adjustment instruction to ensure dynamic stability during the path correction process.

[0051] Compared with the existing technologies that generally adopt the methods of static path point selection and non-feedback trajectory tracking control, this embodiment combines stability perception guidance + nonlinear MPC feedback control + dynamic risk response to achieve a highly controllable and adaptable wading path planning system, significantly improving the path passing success rate and control accuracy.

[0052] (6) Execution control module Used to output vehicle driving control instructions according to the driving trajectory data; The steering angle control sub-unit is used to generate a steering angle control instruction according to the vehicle driving trajectory data set, and control the actual steering angle of the vehicle within the error range of ±10° of the set trajectory angle through the vehicle steering actuator; The speed control sub-unit is used to generate a speed control instruction according to the speed information in the driving trajectory, and adjust the actual vehicle speed within the error range of ±5 km / h of the target speed; The power mode control sub-unit is used to generate a power system control instruction for controlling the height adjustment of the vehicle engine intake port, the engine speed increase, and the differential lock state when receiving a wading risk warning signal; The navigation projection control sub-unit is used to generate an augmented reality navigation path based on the vehicle driving trajectory data set, and superimpose the navigation line in the image projection area in front of the vehicle in an augmented reality manner for output for the driver's visual navigation.

[0053] In this embodiment, the execution control module integrates a multi-path data verification mechanism and a multi-loop redundant control signal verification mechanism to ensure the stable execution of driving control instructions in the vehicle electronic control system (VECU). After receiving the vehicle trajectory data output by the path evaluation and planning module, the module converts it into high-frequency steering / speed / drive control cycle instructions through a trajectory parsing engine, and dynamically adjusts the output frequency based on the current vehicle state residuals (such as heading angle difference, speed offset, yaw rate) to optimize the control response speed and real-time performance.

[0054] (7) Emergency state detection module It is used to detect changes in environmental risks and trigger path replanning and control strategy switching.

[0055] The water level change monitoring subunit is used to calculate the water level rising rate from the upstream water level data periodically extracted from the standardized environmental perception dataset; The risk threshold judgment subunit is used to trigger an emergency path replanning request when the water level rising rate exceeds the preset threshold of 5 cm / min; The power response control subunit is used to output a power boost control instruction to increase the engine speed to no less than 2500 rpm to the execution control module after receiving an emergency trigger signal, and synchronously activate the anti-stall protection strategy.

[0056] In this embodiment, the emergency state detection module integrates three core functions: multi-period upstream trend modeling, dynamic comparison of risk thresholds, and triggering of linkage control strategies, ensuring that the vehicle can enter the "active protection and path reconstruction" dual-channel response state in real time in the event of a sudden wading risk situation. Compared with the existing vehicle wading that only relies on the driver's subjective judgment or a single sensor trigger mechanism, the present invention realizes a full-process closed-loop response system from "risk judgment - path reconstruction - control switching - driving assistance" by integrating trend perception, two-way verification, dynamic calculation, and multi-level response mechanisms, effectively improving the active safety guarantee ability of the whole vehicle in a sudden wading environment.

[0057] As Figure 2 shown, another embodiment of the present invention provides a method for vehicle wading risk assessment and path planning based on multi-band radar, which applies the vehicle wading risk assessment and path planning system based on multi-band radar described in any one of the above, including the following steps: Step 1: Collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data through a multi-modal perception module, perform time synchronization and spatial coordinate registration on various perception data, and generate a standardized environmental perception dataset; Step 2: Input the standardized environmental perception dataset into the feature fusion module, perform modal feature fusion based on the weighted feature interaction fusion mechanism, and generate a fused environmental feature map; Step 3: Input the fused environmental feature map into the boundary enhancement module, and generate a boundary-enhanced environmental feature map through boundary gradient feature extraction and edge enhancement; Step 4: Use the multi-output Gaussian process regression model to predict the vehicle state error and the error covariance matrix, and combine the extended Kalman filter algorithm to correct the vehicle state and output the system state information; Step 5: Input the system state information and the boundary-enhanced environmental feature map into the path evaluation and planning module, calculate the path stability index, and accordingly screen the path point set, and generate vehicle driving trajectory data based on the model predictive control method; Step 6: Generate the steering control instruction, speed control instruction and power system control instruction of the vehicle in the execution control module according to the driving trajectory data; Step 7: Analyze the water level change in the environmental perception data in the emergency state detection module. When it is detected that the risk condition meets the preset threshold, trigger the replanning process of the path evaluation and planning step, and at the same time trigger the execution control module to switch the control strategy.

[0058] In summary, the radar-based vehicle wading path planning system and method proposed by the present invention integrates seven modules: multi-modal environmental perception, feature fusion, boundary enhancement, state estimation, path planning, execution control and emergency response, forming a perception-decision-control integrated closed-loop intelligent system. Through the coordinated cooperation among the modules, the present invention not only significantly improves the path recognition accuracy and state estimation reliability in the wading environment, but also enhances the safety and dynamic response ability of path generation. Compared with the traditional single perception or static path method, this system has stronger environmental adaptability and risk avoidance ability, and is especially suitable for typical complex working condition scenarios such as urban waterlogging sections, mountain river crossings, and sudden rainstorm disasters, with significant engineering practical value and industrial promotion prospects. The overall technical solution of the present invention has clear innovation points, feasibility and practical applicability, meeting the high-standard technical requirements of current intelligent driving systems in terms of safety, accuracy and dynamic decision-making.

[0059] In the description of this specification, the descriptions referring to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples", etc. mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. Moreover, the specific features, structures, materials, or characteristics described may be combined in a suitable manner in any one or more embodiments or examples. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples.

[0060] Any process or method description, whether in a flowchart or otherwise described herein, can be understood to represent a module, segment, or portion of code including one or more executable instructions for implementing a specific logical function or process. And the scope of the preferred embodiments of the present application includes additional implementations, where the functions can be performed in a substantially simultaneous manner or in the reverse order according to the functions involved, rather than in the order shown or discussed.

[0061] As described above, the above are only specific embodiments of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art within the technical scope disclosed in the present application can easily think of various changes or substitutions, and these should all be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A radar-based vehicle wading path planning system, characterized in that, Including: A multi-modal perception module, which is used to collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data, and generate a standardized environmental perception data set; A feature fusion module, which is used to perform weighted feature interaction fusion on the environmental perception data set and output a fused environmental feature map; A boundary enhancement module, which is used to extract boundary gradient features from the fused environmental feature map and enhance edge expression to generate a boundary-enhanced environmental feature map; A state estimation module, which is used to jointly correct the vehicle state based on a multi-output Gaussian process regression model and an extended Kalman filter algorithm and output system state information; A path evaluation and planning module, which is used to evaluate path stability and generate driving trajectory data based on the system state information and the boundary-enhanced environmental feature map; An execution control module, which is used to output vehicle driving control instructions according to the driving trajectory data; An emergency state detection module, which is used to detect environmental risk changes and trigger path replanning and control strategy switching.

2. The radar-based vehicle wading path planning system according to claim 1, wherein The multi-modal perception module includes: A radar perception sub-unit, which is used to collect millimeter-wave radar echo data and extract distance, reflectivity, and speed information related to the vehicle's traveling path; A sonar perception sub-unit, which is used to collect underwater sonar depth data and riverbed contour information and generate a two-dimensional depth distribution map; A visual perception sub-unit, which is used to collect infrared polarization image data and extract texture smoothness features, color features, and occluder edge features; A registration sub-unit, which is used to perform unified time synchronization processing and spatial coordinate registration processing on the data collected by the radar perception sub-unit, sonar perception sub-unit, and visual perception sub-unit. The time synchronization error does not exceed 5 milliseconds, and three-dimensional spatial coordinate registration is performed based on the external parameter matrix, and the registration error does not exceed 0.2 meters, and a standardized environmental perception data set is output.

3. The radar-based vehicle wading path planning system according to claim 2, wherein The feature fusion module includes: A feature extraction sub-unit, which is used to separately extract a visual perception feature map and a millimeter-wave radar perception feature map from the standardized environmental perception data set. The visual perception feature map is extracted through a convolutional neural network based on the infrared polarization image data, and the millimeter-wave radar perception feature map is extracted through a feature encoding network based on the millimeter-wave radar echo map data; A channel splicing sub-unit, which is used to splice the visual perception feature map and the radar perception feature map after spatial alignment in the channel dimension to form a fused input tensor; A weight generation sub-unit, which is used to input the fused input tensor into a convolutional network structure to extract interaction features and normalize them through a Sigmoid activation function to generate a fused weight matrix. Each channel of the fused weight matrix corresponds to a weight factor of different modal feature maps; A fusion sub-unit, which is used to perform element-wise weighted fusion on the visual perception feature map and the radar perception feature map according to the fused weight matrix, and introduce the channel-wise product term between the two types of feature maps as a non-linear interaction enhancement term, and output a fused environmental feature map.

4. The radar-based vehicle wading path planning system according to claim 3, characterized in that, The boundary enhancement module includes: An edge gradient extraction sub-unit, which is used to input the fused environmental feature map into a multi-scale Sobel convolution kernel group to separately extract multi-scale horizontal boundary gradient maps, vertical boundary gradient maps, and diagonal boundary gradient maps. The extraction method is: ； Wherein: represents the fused environmental feature map; , , represents the horizontal, vertical, and diagonal Sobel convolution kernels at the , , layer scale; represents the boundary gradient map extracted at the represents the multi-scale level index; An edge feature fusion subunit, which is used to perform weighted fusion on boundary gradient maps of different scales and directions to generate a boundary response map. The fusion formula is: ； Wherein: represents the boundary response map; represents the fusion weight corresponding to the layer degree; represents the diagonal direction boundary enhancement factor; represents the element-by-element absolute value operation; represents the multi-scale number of layers; A boundary enhancement generation subunit, which is used to splice the boundary response map and the fused environmental feature map in the channel dimension, and perform channel compression through 1×1 convolution to generate a boundary-enhanced environmental feature map. The expression is as follows: ； Wherein: represents a boundary enhanced environmental feature map; represents a channel direction feature splicing operation; is a one-dimensional convolution operation.

5. The radar-based vehicle wading path planning system according to claim 4, wherein The state estimation module includes: An error prediction subunit, which is used to perform error prediction on the system state information based on the multi-output Gaussian process regression model MOGPR. The MOGPR model constructs training samples using historical system state information and state estimation error information. After performing feature mapping on the current input data based on the radial basis function kernel, it outputs the predicted error mean and error covariance matrix corresponding to the system state through inner product operations; A covariance correction subunit, which is used to input the error prediction result into the extended Kalman filter for state recursion correction, and perform state update in combination with the error covariance matrix, and output the corrected system state information; A state update subunit, which is used to use the corrected system state information as the input of the subsequent module.

6. The radar-based vehicle wading path planning system according to claim 5, wherein The path evaluation and planning module includes: A stability index calculation subunit, configured to calculate a vehicle wading stability index according to the following formula , and the expression is: ； Wherein: represents the wheel radius of the vehicle; represents the adhesion coefficient between the tire and the riverbed; represents the water depth; represents the vehicle speed; represents the water flow speed; is a small positive constant used to avoid numerical instability when the water flow speed approaches zero; A path screening subunit, which is used to evaluate and screen multiple sets of path point candidates according to the calculated stability index. When the stability index of any path point is lower than the preset threshold of 1.0, the corresponding path point is removed from the candidate set to form a set of path points that meet the stability requirements; A trajectory generation subunit, which is used to perform path optimization operations on the set of path points based on the model predictive control method MPC to generate a sequence of future vehicle driving trajectory points. The trajectory optimization considers the cost function between the control input quantity and the state change, and outputs the sequence of trajectory points for the execution control module to call.

7. The radar-based vehicle wading path planning system according to claim 6, wherein The path evaluation and planning module further includes: A path dynamic adjustment subunit, which is used to periodically adjust the set of path points according to the updated system state information; A risk level change detection subunit, which is used to perform time series change analysis on the risk level index generated during the path evaluation process, and when it detects that the change amplitude of the risk level of any path point exceeds 10% compared with the previous evaluation value, it sends a path replanning request signal to the path evaluation and planning module.

8. The radar-based vehicle wading path planning system according to claim 6, characterized in that, The execution control module includes: A steering angle control subunit, which is used to generate a steering angle control instruction according to the vehicle driving trajectory data set, and control the actual steering angle of the vehicle within the error range of ±10° of the set trajectory angle through the vehicle steering actuator; A speed control subunit, which is used to generate a speed control instruction according to the speed information in the driving trajectory, and adjust the actual vehicle speed within the error range of ±5 km / h of the target speed; A power mode control subunit, which is used to generate a power system control instruction for controlling the adjustment of the engine intake port height, the increase of the engine speed, and the differential lock state control of the vehicle when receiving a wading risk warning signal; A navigation projection control subunit, which is used to generate an augmented reality navigation path based on the vehicle driving trajectory data set, and superimpose the navigation line in the augmented reality manner within the image projection area in front of the vehicle for visual navigation by the driver.

9. The radar-based vehicle wading path planning system according to claim 1, characterized in that, The emergency state detection module includes: The water level change monitoring subunit is used to calculate the water level rising rate from the upstream water level data periodically extracted from the standardized environmental perception dataset; The risk threshold judgment subunit is used to trigger an emergency path replanning request when the water level rising rate exceeds the preset threshold of 5 cm / min; The power response control subunit is used to output a power boost control instruction to increase the engine speed to no less than 2500 rpm to the execution control module and synchronously activate the anti-stall protection strategy after receiving the emergency trigger signal.

10. A radar-based vehicle wading path planning method, which applies the radar-based vehicle wading path planning system according to any one of claims 1-9, characterized in that, It includes the following steps: Step 1: Collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data through the multimodal perception module, perform time synchronization and spatial coordinate registration on various perception data, and generate a standardized environmental perception dataset; Step 2: Input the standardized environmental perception dataset into the feature fusion module, perform modal feature fusion based on the weighted feature interaction fusion mechanism, and generate a fused environmental feature map; Step 3: Input the fused environmental feature map into the boundary enhancement module, and generate a boundary-enhanced environmental feature map through boundary gradient feature extraction and edge enhancement; Step 4: Use the multi-output Gaussian process regression model to predict the vehicle state error and the error covariance matrix, and combine the extended Kalman filter algorithm to correct the vehicle state and output the system state information; Step 5: Input the system state information and the boundary-enhanced environmental feature map into the path evaluation and planning module, calculate the path stability index, and accordingly screen the path point set, and generate vehicle driving trajectory data based on the model predictive control method; Step 6: Generate the steering control instruction, speed control instruction, and power system control instruction of the vehicle in the execution control module according to the driving trajectory data; Step 7: Analyze the water level change in the environmental perception data in the emergency state detection module. When it is detected that the risk condition meets the preset threshold, trigger the replanning process of the path evaluation and planning step, and at the same time trigger the execution control module to switch the control strategy.

Images