A radar-based vehicle wading path planning system and method

Abstract

The present invention discloses a radar-based vehicle wading path planning system and method thereof, relating to the technical fields of intelligent driving and vehicle path planning. The system comprises: a multimodal perception module that collects millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data to construct a standardized environmental perception data set; a feature fusion module that performs weighted fusion and outputs a fused environmental feature map; a boundary enhancement module that extracts boundary gradients and generates a boundary-enhanced environmental feature map; a state estimation module that combines a multi-output Gaussian process regression model with an extended Kalman filter method to correct the vehicle state; a path evaluation and planning module that calculates wading stability indicators and generates a driving trajectory; an execution control module that outputs control instructions; and an emergency state detection module that monitors environmental risks in real time and triggers path replanning. The present invention can improve the risk perception accuracy and path control reliability of vehicles in wading environments.

Description

Technical Field

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

[0002] With the rapid development of smart cars, assisted driving, and autonomous driving technologies, a vehicle's perception, decision-making, and path planning capabilities in complex road environments have become crucial design criteria for intelligent systems. Among these special road scenarios, flooded sections, due to their unpredictability, high risk, and dynamic nature, have become a significant constraint on vehicle safety.

[0003] Existing vehicle water-crossing identification and path-determination systems often rely on a single type of sensor (such as visual cameras or millimeter-wave radar) for environmental perception, making it difficult to accurately identify key water-crossing risk factors such as water extent, depth, and flow velocity. Furthermore, traditional image recognition methods suffer from reduced robustness during rainy seasons or in low-visibility environments. Furthermore, while some systems have introduced multi-sensor data fusion mechanisms, they generally employ rule-driven or static weighting approaches, making it difficult to dynamically adapt to changes in multimodal data weights under varying environments, limiting the expressive power of fused features. Furthermore, existing path planning strategies often employ static pathpoint placement and lack a pathpoint selection mechanism based on water-crossing stability assessment metrics. While some systems employ model predictive control (MPC) for path generation, these systems fail to effectively integrate vehicle state estimation errors, boundary identification results, and real-time water level trends for closed-loop adjustments, making it impossible to achieve real-time path reconstruction and risk response in high-risk areas.

[0004] It can be seen that how to build a vehicle water wading risk assessment and path planning system with multimodal environmental perception capabilities, feature fusion adaptation, accurate state estimation, stable path planning and dynamic reconstruction capabilities has become a technical problem that needs to be solved urgently in this field. Summary of the Invention

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

[0006] The present invention achieves the above-mentioned purpose through the following technical solutions:

[0007] In one aspect, the present invention provides a radar-based vehicle wading path planning system and method thereof, comprising:

[0008] A multimodal perception module, which collects millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data to generate a standardized environmental perception data set;

[0009] A feature fusion module is used to perform weighted feature interactive fusion on the environmental perception data set and output a fused environmental feature map;

[0010] A boundary enhancement module is used to extract boundary gradient features from the fused environment feature map and enhance edge expression to generate a boundary enhanced environment feature map;

[0011] The state estimation module is used to 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;

[0012] Path evaluation and planning module, which is used to evaluate path stability and generate driving trajectory data based on system status information and boundary-enhanced environmental feature maps;

[0013] an execution control module, configured to output a vehicle driving control instruction according to the driving trajectory data;

[0014] Emergency detection module, used to detect environmental risk changes and trigger path replanning and control strategy switching.

[0015] A further improvement of the present invention is that the multimodal perception module includes:

[0016] The radar sensing subunit is used to collect millimeter-wave radar echo data and extract distance, reflectivity, and speed information related to the vehicle's travel path;

[0017] Sonar sensing subunit, used to collect underwater sonar depth data and riverbed contour information to generate a two-dimensional depth distribution map;

[0018] The visual perception subunit is used to collect infrared polarization image data and extract texture smoothing features, color features, and edge features of obstructions;

[0019] The registration subunit is used to perform unified time synchronization processing and spatial coordinate registration processing on the data collected by the radar perception subunit, sonar perception subunit and visual perception subunit, wherein the time synchronization error does not exceed 5 milliseconds, and three-dimensional spatial coordinate registration is performed based on the external parameter matrix, with the registration error not exceeding 0.2 meters, and output a standardized environmental perception data set.

[0020] A further improvement of the present invention is that the feature fusion module includes:

[0021] A feature extraction subunit is used to extract visual perception feature maps and millimeter-wave radar perception feature maps from a standardized environmental perception dataset. The visual perception feature maps are extracted based on infrared polarization image data through a convolutional neural network, and the millimeter-wave radar perception feature maps are extracted based on millimeter-wave radar echo map data through a feature encoding network.

[0022] The channel splicing subunit is used to spatially align the visual perception feature map and the radar perception feature map in the channel dimension and then splice them to form a fusion input tensor;

[0023] A weight generation subunit is used to input the fused input tensor into a convolutional network structure to extract interaction features, and normalize it through a Sigmoid activation function to generate a fusion weight matrix, where each channel of the fusion weight matrix corresponds to a weight factor of a different modality feature map;

[0024] The fusion subunit is used to perform element-by-element weighted fusion of the visual perception feature map and the radar perception feature map according to the fusion weight matrix, introduce a channel-by-channel product term between the two types of feature maps as a nonlinear interaction enhancement term, and output a fused environment feature map.

[0025] A further improvement of the present invention is that the boundary enhancement module includes:

[0026] The edge gradient extraction subunit is used to input the fused environment feature map into the multi-scale Sobel convolution kernel group to extract the multi-scale horizontal boundary gradient map, vertical boundary gradient map and diagonal boundary gradient map respectively. The extraction method is as follows:

[0027] ;

[0028] in: Represents the fusion environment feature map; 、 、 Indicates the Horizontal, vertical and diagonal Sobel convolution kernels at the layer scale; 、 、 Indicates the extracted Boundary gradient map at layer scale; Represents a multi-scale hierarchical index;

[0029] The edge feature fusion subunit is used to perform weighted fusion of boundary gradient maps of different scales and directions to generate a boundary response map. The fusion formula is:

[0030] ;

[0031] in: represents the boundary response plot; Indicates the The fusion weight corresponding to the layer, represents the diagonal boundary enhancement factor; Represents element-by-element absolute value operation; Indicates the number of multi-scale layers;

[0032] The boundary enhancement generation subunit is used to splice the boundary response map and the fusion environment feature map in the channel dimension, and perform channel compression through 1×1 convolution to generate the boundary enhancement environment feature map. The expression is as follows:

[0033] ;

[0034] in: Represents the boundary enhanced environment feature map; Represents the channel-wise feature concatenation operation; It is a one-dimensional convolution operation.

[0035] A further improvement of the present invention is that the state estimation module includes:

[0036] An error prediction subunit is used to perform error prediction on system state information based on a multi-output Gaussian process regression model (MOGPR). The MOGPR model uses historical system state information and state estimation error information to construct training samples, performs feature mapping on the current input data based on a radial basis function kernel (RBF kernel), and then outputs the prediction error mean and error covariance matrix of the corresponding system state through an inner product operation;

[0037] A covariance correction subunit is used to input the error prediction result into an extended Kalman filter (EKF) for state recursive correction, and perform state update in combination with the error covariance matrix to output corrected system state information;

[0038] The state update subunit is used to use the corrected system state information as input to subsequent modules to support path planning and dynamic decision calculation.

[0039] A further improvement of the present invention is that the path evaluation and planning module includes:

[0040] The stability index calculation subunit is used to calculate the vehicle wading stability index according to the following formula , the expression is:

[0041] ;

[0042] in: Indicates the vehicle wheel radius, which affects the vehicle's wading contact surface; Indicates the adhesion coefficient between tire and riverbed, used to calculate tire grip; Indicates water depth, affecting vehicle buoyancy and stability; Indicates vehicle speed, affecting the vehicle's kinetic energy and traction when wading; Indicates the water flow speed, which affects the stability of the vehicle in wading; is a small positive constant used to avoid numerical instability when the water velocity is close to zero;

[0043] The path screening subunit is used to evaluate and screen multiple path point candidate sets based on 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 path point set with satisfactory stability.

[0044] The trajectory generation subunit is used to perform a path optimization operation on the path point set based on the model predictive control method (MPC) to generate a sequence of future vehicle driving trajectory points, wherein the trajectory optimization considers the cost function between the control input and the state change, and outputs the trajectory point sequence for the execution control module to call.

[0045] A further improvement of the present invention is that the path evaluation and planning module further includes:

[0046] A path dynamic adjustment subunit, configured to periodically adjust the path point set based on updated system status information;

[0047] The risk level change detection subunit is used to perform time series change analysis on the risk level indicators generated during the path evaluation process, and send a path re-planning request signal to the path evaluation and planning module when it detects that the risk level of any path point has changed by more than 10% compared with the last evaluation value.

[0048] A further improvement of the present invention is that the execution control module includes:

[0049] The steering angle control subunit is used to generate a steering angle control instruction based on the vehicle driving trajectory data set, and control the actual steering angle of the vehicle within the set trajectory angle ±10° error range through the vehicle steering actuator;

[0050] The speed control subunit is used to generate speed control instructions based on the speed information in the driving trajectory and adjust the actual vehicle speed to within the error range of ±5km / h of the target speed;

[0051] The power mode control subunit is used to generate power system control instructions for controlling the vehicle engine air intake height adjustment, engine speed increase and differential lock state control when receiving a water risk warning signal;

[0052] The navigation projection control subunit is used to generate an augmented reality navigation path based on the vehicle driving trajectory dataset, and superimpose the navigation route on the image output in an augmented reality manner in the image projection area in front of the vehicle for the driver to navigate visually.

[0053] A further improvement of the present invention is that the emergency state detection module includes:

[0054] The water level change monitoring subunit is used to calculate the water level rise rate from upstream water level data periodically extracted from the standardized environmental sensing dataset;

[0055] The risk threshold judgment subunit is used to trigger an emergency path replanning request when the water level rise rate exceeds a preset threshold of 5 cm / min;

[0056] The power response control subunit is used to output a power boost control instruction to the execution control module to increase the engine speed to no less than 2500rpm after receiving the emergency trigger signal, and simultaneously activate the anti-stall protection strategy.

[0057] In another aspect, the present invention provides a vehicle wading risk assessment and path planning method based on a multi-band radar, which uses any of the above-described vehicle wading risk assessment and path planning systems based on a multi-band radar, comprising the following steps:

[0058] Step 1: Use the multimodal perception module to collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data. Perform time synchronization and spatial coordinate registration on various types of perception data to generate a standardized environmental perception dataset.

[0059] 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;

[0060] Step 3: Input the fused environment feature map into the boundary enhancement module, and generate a boundary enhanced environment feature map through boundary gradient feature extraction and edge enhancement;

[0061] Step 4: Use the multi-output Gaussian process regression model to predict the vehicle state error and error covariance matrix, and combine it with the extended Kalman filter algorithm to correct the vehicle state and output the system state information;

[0062] Step 5: Input the system status information and the boundary-enhanced environmental feature map into the path evaluation and planning module, calculate the path stability index, and use it to filter the path point set and generate vehicle driving trajectory data based on the model predictive control method;

[0063] Step 6: Generate vehicle steering control instructions, speed control instructions, and power system control instructions in the execution control module based on the driving trajectory data;

[0064] Step 7: Analyze the water level changes in the environmental perception data in the emergency detection module. When it is detected that the risk condition meets the preset threshold, the replanning process of the path evaluation and planning step is triggered, and the execution control module is triggered to switch the control strategy at the same time.

[0065] The present invention has the following beneficial effects: By integrating a multimodal perception module with millimeter-wave radar, sonar, and infrared polarization imagery, it enables comprehensive perception of water surface reflections, water depth, and flow velocity in a variety of complex water-related environments. This significantly improves the robustness of environmental modeling compared to traditional single-sensor approaches. The feature fusion module, through its weighted interactive feature fusion mechanism, dynamically integrates multimodal features at the channel level, enhancing the system's perception of key water-related areas. The boundary enhancement module utilizes multi-scale edge extraction and fusion compression to effectively highlight key spatial features such as water-land boundaries and obstacle outlines, providing accurate boundary information for subsequent path analysis. The state estimation module, through a combined multi-output Gaussian process regression model and extended Kalman filter, improves vehicle pose estimation accuracy and minimizes the impact of error accumulation on path prediction. The path evaluation and planning module utilizes a water-related stability indicator to guide path point selection and, in conjunction with a model predictive control algorithm, dynamically generates a controllable trajectory, improving the reliability of the water-related path. The execution control module outputs vehicle steering, speed, and power control commands in real time based on the generated trajectory, achieving closed-loop control during path tracking. The emergency detection module is capable of rapidly identifying and responding to water level trends. It automatically triggers path replanning and powertrain switching in the event of a sudden water rise, effectively enhancing the vehicle's emergency safety in extreme water-related situations. This invention achieves synergistic improvements in multimodal environmental perception, state estimation accuracy, path stability assessment, and risk response capabilities, demonstrating its high technological advancement and engineering application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be derived from these drawings without inventive effort. Among them:

[0067] Figure 1 It is a system modular diagram of the present invention; Figure 2 Flow chart of the method of the present invention. DETAILED DESCRIPTION

[0068] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the described embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field are within the scope of protection of the present invention.

[0069] like Figure 1 FIG. 1 is an embodiment of the present invention, which provides a radar-based vehicle wading path planning system and method thereof, including:

[0070] (1) Multimodal perception module

[0071] Used to collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data to generate standardized environmental perception data sets;

[0072] The radar sensing subunit is used to collect millimeter-wave radar echo data and extract distance, reflectivity, and speed information related to the vehicle's travel path;

[0073] Sonar sensing subunit, used to collect underwater sonar depth data and riverbed contour information to generate a two-dimensional depth distribution map;

[0074] The visual perception subunit is used to collect infrared polarization image data and extract texture smoothing features, color features, and edge features of obstructions;

[0075] The registration subunit is used to perform unified time synchronization processing and spatial coordinate registration processing on the data collected by the radar perception subunit, sonar perception subunit and visual perception subunit, wherein the time synchronization error does not exceed 5 milliseconds, and three-dimensional spatial coordinate registration is performed based on the external parameter matrix, with the registration error not exceeding 0.2 meters, and output a standardized environmental perception data set.

[0076] In this embodiment, to achieve high-precision detection of key physical information in a water environment, the multimodal perception module further adopts the following structural optimization and data coordination mechanism:

[0077] The millimeter-wave radar sensing unit utilizes an FMCW millimeter-wave radar array based on 24GHz and 77GHz dual-frequency synthesis. It is positioned symmetrically on the left and right sides of the vehicle's front bumper, enabling simultaneous acquisition of water surface reflection intensity, incident angle echo delay, and dynamic velocity vectors. To reduce multipath interference, the system incorporates an angular filtering enhancement algorithm. During the data preprocessing phase, vertical blind spot signals are dynamically blocked based on the field of view angle, improving the continuity and reliability of the target radar profile.

[0078] The sonar sensing unit utilizes a 300kHz high-frequency side-scan structure, mounted on both sides of the vehicle's bottom. Its scanning resolution is better than 1cm / pixel. To enhance underwater profile stability, the system identifies differences in bottom material by matching the attenuation characteristics of acoustic intensity echoes. It also calculates the rate of change of echo amplitude in real time to aid in identifying bottom attachments or potential obstacles.

[0079] The infrared polarization image perception unit is equipped with a dual-axis polarization camera, fusing polarization angles with infrared brightness channels to form a combined texture / occlusion image set. To improve image boundary segmentation capabilities in low-contrast water scenes, the system integrates a directional gradient enhancement module, using a Gabor filter bank to extract texture structures along the water surface, enabling robust recognition of distant water contours.

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

[0081] Compared with the single-modal solutions in the existing technology that only use radar or image data for water wading identification, this embodiment uses radar-sonar-vision three-source joint perception, which not only improves the ability to recognize multi-dimensional information such as water depth, boundaries and dynamic flow rate, but also enhances the system's robustness in extreme environments such as rain, fog, night and low light, significantly expanding the adaptability and data credibility of the perception system in actual water wading path planning.

[0082] (2) Feature fusion module

[0083] Used to perform weighted feature interactive fusion on the environmental perception data set and output a fused environmental feature map;

[0084] A feature extraction subunit is used to extract visual perception feature maps and millimeter-wave radar perception feature maps from a standardized environmental perception dataset. The visual perception feature maps are extracted based on infrared polarization image data through a convolutional neural network, and the millimeter-wave radar perception feature maps are extracted based on millimeter-wave radar echo map data through a feature encoding network.

[0085] The channel splicing subunit is used to spatially align the visual perception feature map and the radar perception feature map in the channel dimension and then splice them to form a fusion input tensor;

[0086] A weight generation subunit is used to input the fused input tensor into a convolutional network structure to extract interaction features, and normalize it through a Sigmoid activation function to generate a fusion weight matrix, where each channel of the fusion weight matrix corresponds to a weight factor of a different modality feature map;

[0087] The fusion subunit is used to perform element-by-element weighted fusion of the visual perception feature map and the radar perception feature map according to the fusion weight matrix, introduce a channel-by-channel product term between the two types of feature maps as a nonlinear interaction enhancement term, and output a fused environment feature map.

[0088] In this embodiment, the feature fusion module further employs a multi-channel attention weighting mechanism combined with a nonlinear interaction structure to perform deep fusion of key target areas within multimodal environmental perception data. Specifically, the system first normalizes the visual perception feature map and the millimeter-wave radar perception feature map, and calculates their spatial variance map and gradient direction map for each channel dimension, extracting the response salient regions within each modality.

[0089] Subsequently, the fusion module calls the Fusion Attention Net to associate the above-mentioned saliency map with the channel feature tensor at the element level, assign a dynamically adjustable fusion factor to each channel, generate a channel attention weight matrix, and introduce a Sigmoid-Gated gating mechanism to shrink the weight range to avoid overfitting or masking of the overall output result by a certain mode.

[0090] To further enhance the complementary expressiveness between modalities, the system introduces a cross-modal residual nonlinear interaction structure. This structure creates a dual-path convolution branch during the fusion process, performing intra-channel convolution and inter-modal convolution respectively, and superimposing them onto the main fusion feature path via a residual connection. This entire structure enables dynamic enhancement of feature maps at different scales and orientations.

[0091] The output of this fusion process is encoded into a unified fused environmental feature map, which simultaneously incorporates multiple dimensions of information, including visual texture, boundary contours, radar spatial response, and depth estimation, ensuring both spatial structural consistency and physical semantic alignment. Systematic experiments demonstrate that this fusion mechanism can reliably identify water boundaries and areas of water change even in low visibility conditions, achieving an average improvement of over 12% in mean Intersection Over Union (MIoU) and boundary accuracy compared to traditional weighted averaging or stacking methods.

[0092] The feature fusion module achieves highly robust integrated expression of multi-source perception data through attention guidance and residual nonlinear interactive fusion mechanism, providing high-resolution and high-reliability input feature support for subsequent boundary enhancement and path evaluation modules.

[0093] (3) Boundary enhancement module

[0094] Used to extract boundary gradient features from the fused environment feature map and enhance edge expression to generate a boundary enhanced environment feature map;

[0095] The edge gradient extraction subunit is used to input the fused environment feature map into the multi-scale Sobel convolution kernel group to extract the multi-scale horizontal boundary gradient map, vertical boundary gradient map and diagonal boundary gradient map respectively. The extraction method is as follows:

[0096] ;

[0097] in: Represents the fusion environment feature map; 、 、 Indicates the Horizontal, vertical and diagonal Sobel convolution kernels at the layer scale; 、 、 Indicates the extracted Boundary gradient map at layer scale; Represents a multi-scale hierarchical index;

[0098] The edge feature fusion subunit is used to perform weighted fusion of boundary gradient maps of different scales and directions to generate a boundary response map. The fusion formula is:

[0099] ;

[0100] in: represents the boundary response plot; Indicates the The fusion weight corresponding to the layer, represents the diagonal boundary enhancement factor; Represents element-by-element absolute value operation; Indicates the number of multi-scale layers;

[0101] The boundary enhancement generation subunit is used to splice the boundary response map and the fusion environment feature map in the channel dimension, and perform channel compression through 1×1 convolution to generate the boundary enhancement environment feature map. The expression is as follows:

[0102] ;

[0103] in: Represents the boundary enhanced environment feature map; Represents the channel-wise feature concatenation operation; It is a one-dimensional convolution operation.

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

[0105] 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 environment feature map, calculates it through the spatial position gradient angle consistency index (such as cosine similarity), and marks the areas in the boundary response map of each direction (horizontal, vertical, diagonal) where the gradient direction difference is greater than the set threshold (such as 30 degrees) as boundary discontinuity areas, and performs edge interpolation and reconstruction processing on the area.

[0106] During the edge feature fusion process, in order to avoid boundary blur or noise interference caused by large differences between directional response maps, the system introduces an anisotropic direction adjustment mechanism. Specifically, the direction field tensor is first constructed based on the gradient response map of the fused environment feature map, and the consistency of the boundaries in each direction is calculated using spatial position gradient angle consistency indicators (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 neighboring direction by more than a set threshold (such as 30 degrees), the system marks the area as a boundary discontinuity area and performs completion and reconstruction through edge interpolation.

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

[0108] In addition, after the channel splicing of the fused boundary response map and the original fused feature map in the boundary enhancement module, it is not directly input into the 1×1 convolution layer for compression. Instead, it is first input through the multi-channel feature selection network (Channel Selection Net) to complete redundant feature elimination and boundary feature enhancement, and then the convolution dimensionality reduction operation is performed, effectively avoiding the loss of boundary feature information due to compression during the fusion stage.

[0109] The resulting boundary-enhanced environmental feature map retains clear land-water boundary morphology and water body contours, and robustly represents multi-directional gradient structures, significantly improving the system's accuracy and robustness in identifying boundaries in water-bounded areas. Experimental tests show that under typical low-light conditions, this module achieves improvements of approximately 17.4% in Edge Precision (EP) and 13.1% in Hausdorff distance (HD) compared to traditional Sobel fusion solutions.

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

[0111] (4) State estimation module

[0112] Used to jointly correct vehicle status based on the multi-output Gaussian process regression model and the extended Kalman filter algorithm and output system status information;

[0113] An error prediction subunit is used to perform error prediction on system state information based on a multi-output Gaussian process regression model (MOGPR). The MOGPR model uses historical system state information and state estimation error information to construct training samples, performs feature mapping on the current input data based on a radial basis function kernel (RBF kernel), and then outputs the prediction error mean and error covariance matrix of the corresponding system state through an inner product operation;

[0114] A covariance correction subunit is used to input the error prediction result into an extended Kalman filter (EKF) for state recursive correction, and perform state update in combination with the error covariance matrix to output corrected system state information;

[0115] The state update subunit is used to use the corrected system state information as input to subsequent modules to support path planning and dynamic decision calculation.

[0116] In this embodiment, the state estimation module combines nonlinear modeling with a multidimensional covariance management mechanism to construct a highly robust vehicle state inference architecture for wading scenarios. First, the error prediction subunit uses a Multi-Output Gaussian Process Regression (MOGPR) model to model the system state error distribution by combining historical state variable sequences with the trajectory of sensory feature changes.

[0117] In this system, the MOGPR model uses a collaborative kernel group based on a composite radial basis function (RBF) kernel, and combines the inter-frame differences of perception data in the temporal dimension as nonlinear mapping input to achieve the joint output of the state error mean and covariance matrix. The state dimension covers six continuous variables: the vehicle's two-dimensional position (x, y), velocity vector (vx, vy), heading angle, and angular velocity.

[0118] After the prediction output, the covariance correction subunit feeds it into an Extended Kalman Filter (EKF) for error correction and state smoothing. The EKF module dynamically adjusts gain weights based on the residual between the fused feature map estimate and the prior state prediction, combined with the main diagonal elements in the state error covariance matrix, to improve the stability of the system's state estimation under strong noise input. In particular, when sonar data fluctuates significantly or when underwater obstructions are severe, the filter automatically reduces the weight of sonar observations in updating the fused state, forming an adaptive adjustment mechanism for perceptual robustness.

[0119] After completing the EKF correction, the state update subunit will synchronously push the corrected state variables to the path evaluation and planning module, dynamic control module, and emergency response module, and cache the historical state trajectory in this cycle for error prediction call in the next cycle, forming a timing closed loop.

[0120] Compared to existing state estimation systems that rely solely on single-modal observation data or traditional linear prediction methods, this proposed system improves its adaptability to non-Gaussian error distributions and time-varying error characteristics through a combined multi-output nonlinear regression and extended filtering mechanism, significantly reducing cumulative positioning offset in wading and fluctuating water environments. Simulation results show that in typical dynamic water scenarios, the module maintains an average state estimation error of less than 0.28m, surpassing the error level of over 0.4m achieved by existing similar systems. Furthermore, the system exhibits adaptive error constraint capabilities.

[0121] (5) Path evaluation and planning module

[0122] Used to evaluate path stability and generate driving trajectory data based on system status information and boundary-enhanced environmental feature maps;

[0123] The stability index calculation subunit is used to calculate the vehicle wading stability index according to the following formula , the expression is:

[0124] ;

[0125] in: Indicates the vehicle wheel radius, which affects the vehicle's wading contact surface; Indicates the adhesion coefficient between tire and riverbed, used to calculate tire grip; Indicates water depth, affecting vehicle buoyancy and stability; Indicates vehicle speed, affecting the vehicle's kinetic energy and traction when wading; Indicates the water flow speed, which affects the stability of the vehicle in wading; is a small positive constant used to avoid numerical instability when the water velocity is close to zero;

[0126] The path screening subunit is used to evaluate and screen multiple path point candidate sets based on 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 path point set with satisfactory stability.

[0127] The trajectory generation subunit is used to perform a path optimization operation on the path point set based on the model predictive control method (MPC) to generate a sequence of future vehicle driving trajectory points, wherein the trajectory optimization considers the cost function between the control input and the state change, and outputs the trajectory point sequence for the execution control module to call.

[0128] A path dynamic adjustment subunit, configured to periodically adjust the path point set based on updated system status information;

[0129] The risk level change detection subunit is used to perform time series change analysis on the risk level indicators generated during the path evaluation process, and send a path re-planning request signal to the path evaluation and planning module when it detects that the risk level of any path point has changed by more than 10% compared with the last evaluation value.

[0130] In this embodiment, the path assessment and planning module further employs a dynamic path reconstruction mechanism guided by stability perception. To quantify the vehicle's maneuverability at different path points under water-risk conditions, the system incorporates a multi-parameter water-stability index calculation formula, where the variables are derived from a fusion of vehicle chassis parameters, water flow perception results, and radar image feature extraction. To accommodate different vehicle models, the stability calculation model incorporates a wheel diameter calibration coefficient table and a dynamic adhesion adjustment model, and supports real-time adjustment of the ε suppression term to accommodate varying flow velocity fields.

[0131] During the path screening process, the system no longer uses a unified threshold rejection strategy. Instead, it introduces an adaptive stability confidence interval mechanism. When the stability index of a path point approaches the threshold boundary, the module automatically performs regional interpolation of the upper and lower neighboring points to calculate the local mean stability and maximum gradient of the path segment. If a gradient anomaly occurs (i.e., a stability change greater than 2σ), the path segment is marked as a "potential high-risk segment" and prioritized for adjustment by the path avoidance submodule.

[0132] For trajectory generation, the system constructs a five-step time-domain prediction window, predicting the vehicle state two seconds into the future at each step. Using a nonlinear model predictive control method with state constraints (NL-MPC), the system takes the current system state vector, the set of path points to be traversed, and control reference constraints as inputs. The system then outputs a steering angle-acceleration control sequence and an objective function that minimizes trajectory error. Specifically, to address the issue of tire slip affecting control input accuracy on flooded roads, the system incorporates estimated road adhesion trends to dynamically adjust the desired steering angle response speed.

[0133] In the dynamic path adjustment module, the module regularly evaluates the state consistency differences between the traveled path segments and the planned path segments ahead. A "risk level volatility" metric is introduced within the risk detection submodule to determine whether the path planning results require reconstruction. If the difference between the mean stability of the current and subsequent paths exceeds 10%, or the gradient of the maximum change point exceeds a threshold value θ (empirically set at 0.15), the system simultaneously issues a path replanning command and a controller pre-adjustment command to ensure dynamic stability during the path correction process.

[0134] Compared with the existing technology that generally adopts 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 water wading path planning system, significantly improving the path success rate and control accuracy.

[0135] (6) Execution control module

[0136] Used to output vehicle driving control instructions according to the driving trajectory data;

[0137] The steering angle control subunit is used to generate a steering angle control instruction based on the vehicle driving trajectory data set, and control the actual steering angle of the vehicle within the set trajectory angle ±10° error range through the vehicle steering actuator;

[0138] The speed control subunit is used to generate speed control instructions based on the speed information in the driving trajectory and adjust the actual vehicle speed to within the error range of ±5km / h of the target speed;

[0139] The power mode control subunit is used to generate power system control instructions for controlling the vehicle engine air intake height adjustment, engine speed increase and differential lock state control when receiving a water risk warning signal;

[0140] The navigation projection control subunit is used to generate an augmented reality navigation path based on the vehicle driving trajectory dataset, and superimpose the navigation route on the image output in an augmented reality manner in the image projection area in front of the vehicle for the driver to navigate visually.

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

[0142] (7) Emergency detection module

[0143] Used to detect environmental risk changes and trigger path replanning and control strategy switching.

[0144] The water level change monitoring subunit is used to calculate the water level rise rate from upstream water level data periodically extracted from the standardized environmental sensing dataset;

[0145] The risk threshold judgment subunit is used to trigger an emergency path replanning request when the water level rise rate exceeds a preset threshold of 5 cm / min;

[0146] The power response control subunit is used to output a power boost control instruction to the execution control module to increase the engine speed to no less than 2500rpm after receiving the emergency trigger signal, and simultaneously activate the anti-stall protection strategy.

[0147] In this embodiment, the emergency detection module integrates three core functions: multi-period upstream trend modeling, dynamic comparison of risk thresholds, and coordinated control strategy triggering. This ensures that the vehicle can enter a dual-channel response state of "active protection and path reconstruction" in real time in sudden flooding risk scenarios. Compared to existing vehicle flooding mechanisms that rely solely on driver subjective judgment or single sensor triggering, this invention integrates trend perception, bidirectional verification, dynamic calculation, and a multi-level response mechanism to achieve a full-process closed-loop response system from "risk assessment - path reconstruction - control switching - driving assistance", effectively enhancing the vehicle's active safety protection capabilities in sudden flooding situations.

[0148] like Figure 2 FIG. 1 is another embodiment of the present invention, which provides a vehicle wading risk assessment and path planning method based on a multi-band radar. The vehicle wading risk assessment and path planning system based on a multi-band radar as described above is applied, and includes the following steps:

[0149] Step 1: Use the multimodal perception module to collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data. Perform time synchronization and spatial coordinate registration on various types of perception data to generate a standardized environmental perception dataset.

[0150] 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;

[0151] Step 3: Input the fused environment feature map into the boundary enhancement module, and generate a boundary enhanced environment feature map through boundary gradient feature extraction and edge enhancement;

[0152] Step 4: Use the multi-output Gaussian process regression model to predict the vehicle state error and error covariance matrix, and combine it with the extended Kalman filter algorithm to correct the vehicle state and output the system state information;

[0153] Step 5: Input the system status information and the boundary-enhanced environmental feature map into the path evaluation and planning module, calculate the path stability index, and use it to filter the path point set and generate vehicle driving trajectory data based on the model predictive control method;

[0154] Step 6: Generate vehicle steering control instructions, speed control instructions, and power system control instructions in the execution control module based on the driving trajectory data;

[0155] Step 7: Analyze the water level changes in the environmental perception data in the emergency detection module. When it is detected that the risk condition meets the preset threshold, the replanning process of the path evaluation and planning step is triggered, and the execution control module is triggered to switch the control strategy at the same time.

[0156] In summary, the radar-based vehicle wading path planning system and method proposed in the present invention integrate seven modules: multimodal environmental perception, feature fusion, boundary enhancement, state estimation, path planning, execution control and emergency response, forming a closed-loop intelligent system integrating perception, decision-making and control. Through the coordinated cooperation between the modules, the present invention not only significantly improves the path recognition accuracy and state estimation reliability in wading environments, but also improves the safety and dynamic adaptability of path generation. Compared with traditional single perception or static path methods, this system has stronger environmental adaptability and risk avoidance capabilities, and is particularly suitable for typical complex working conditions such as urban flooded roads, mountain river crossings, rainstorms and sudden disasters, and has significant engineering practical value and industrial promotion prospects. The overall technical solution of the present invention has clear innovation points, feasibility and practical applicability, and meets the high-standard technical requirements of current intelligent driving systems in terms of safety, accuracy and dynamic decision-making.

[0157] In the description of this specification, the reference terms "one embodiment," "some embodiments," "example," "specific example," or "some examples" mean that the specific features, structures, materials, or characteristics described in conjunction 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 any appropriate manner in any one or more embodiments or examples. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification, as well as features of different embodiments or examples, unless they are mutually inconsistent.

[0158] Any process or method description in the flowchart or otherwise described herein can be understood to represent a module, segment or portion of code that includes one or more executable instructions for implementing the steps of a specific logical function or process. The scope of the preferred embodiments of the present application includes additional implementations in which the functions may be performed in a different order than shown or discussed, including in a substantially simultaneous manner or in a reverse order depending on the functions involved.

[0159] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any person skilled in the art can easily conceive of various modifications or substitutions within the technical scope disclosed in this application, and such modifications or substitutions should be included within the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A radar-based vehicle wading path planning system, characterized in that: include: A multimodal perception module, which collects millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data to generate a standardized environmental perception data set; Feature fusion module, used to perform weighted feature interaction fusion on the environmental perception dataset and output a fused environmental feature map; The boundary enhancement module is used to extract boundary gradient features from the fused environment feature map and enhance edge expression to generate a boundary enhanced environment feature map; The state estimation module is used to 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; Path evaluation and planning module, which is used to evaluate path stability and generate driving trajectory data based on system status information and boundary-enhanced environmental feature maps; The path evaluation and planning module includes: The stability index calculation subunit is used to calculate the vehicle wading stability index according to the following formula , the expression is: ； in: Indicates the vehicle wheel radius; Indicates the adhesion coefficient between tire and riverbed; Indicates water depth; Indicates vehicle speed; Indicates water flow velocity; is a small positive constant used to avoid numerical instability when the water velocity is close to zero; The path screening subunit is used to evaluate and screen multiple path point candidate sets based on 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 path point set with satisfactory stability. The trajectory generation subunit is used to perform path optimization operations on the path point set based on the model predictive control method MPC to generate a sequence of future vehicle trajectory points. The trajectory optimization considers the cost function between the control input and the state change, and outputs the trajectory point sequence for the execution control module to call; An execution control module, configured to output a vehicle driving control instruction according to the driving trajectory data; Emergency detection module, 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, characterized in that: The multimodal perception module includes: The radar sensing subunit is used to collect millimeter-wave radar echo data and extract distance, reflectivity, and speed information related to the vehicle's path; Sonar sensing subunit, used to collect underwater sonar depth data and riverbed contour information to generate a two-dimensional depth distribution map; The visual perception subunit is used to collect infrared polarization image data and extract texture smoothing features, color features, and edge features of obstructions; The registration subunit is used to perform unified time synchronization processing and spatial coordinate registration processing on the data collected by the radar perception subunit, sonar perception subunit and visual perception subunit, wherein the time synchronization error does not exceed 5 milliseconds, and three-dimensional spatial coordinate registration is performed based on the external parameter matrix, with the registration error not exceeding 0.2 meters, and output a standardized environmental perception data set.

3. The radar-based vehicle wading path planning system according to claim 2, characterized in that: The feature fusion module includes: A feature extraction subunit is used to extract visual perception feature maps and millimeter-wave radar perception feature maps from a standardized environmental perception dataset. The visual perception feature maps are extracted based on infrared polarization image data through a convolutional neural network, and the millimeter-wave radar perception feature maps are extracted based on millimeter-wave radar echo map data through a feature encoding network. The channel splicing subunit is used to spatially align the visual perception feature map and the radar perception feature map in the channel dimension and then splice them to form a fusion input tensor; The weight generation subunit is used to input the fused input tensor into the convolutional network structure to extract the interaction features, and normalize it through the Sigmoid activation function to generate a fusion weight matrix. Each channel of the fusion weight matrix corresponds to the weight factor of the feature map of different modalities; The fusion subunit is used to perform element-by-element weighted fusion of the visual perception feature map and the radar perception feature map according to the fusion weight matrix, introduce the channel-by-channel product term between the two types of feature maps as a nonlinear interaction enhancement term, and output the fused environment feature map.

4. The radar-based vehicle wading path planning system according to claim 3, characterized in that: The boundary enhancement module includes: The edge gradient extraction subunit is used to input the fused environment feature map into the multi-scale Sobel convolution kernel group to extract the multi-scale horizontal boundary gradient map, vertical boundary gradient map and diagonal boundary gradient map respectively. The extraction method is as follows: ； in: Represents the fusion environment feature map; 、 、 Indicates the Horizontal, vertical and diagonal Sobel convolution kernels at the layer scale; 、 、 Indicates the extracted Boundary gradient map at layer scale; Represents a multi-scale hierarchical index; The edge feature fusion subunit is used to perform weighted fusion of boundary gradient maps of different scales and directions to generate a boundary response map. The fusion formula is: ； in: represents the boundary response plot; Indicates the The fusion weight corresponding to the layer, represents the diagonal boundary enhancement factor; Represents element-by-element absolute value operation; Indicates the number of multi-scale layers; The boundary enhancement generation subunit is used to splice the boundary response map and the fusion environment feature map in the channel dimension, and perform channel compression through 1×1 convolution to generate the boundary enhancement environment feature map. The expression is as follows: ； in: Represents the boundary enhanced environment feature map; Represents the channel-wise feature concatenation operation; It is a one-dimensional convolution operation.

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

6. The radar-based vehicle wading path planning system according to claim 1, characterized in that: The path evaluation and planning module further includes: A path dynamic adjustment subunit, configured to periodically adjust the path point set based on updated system status information; The risk level change detection subunit is used to perform time series change analysis on the risk level indicators generated during the path evaluation process, and send a path re-planning request signal to the path evaluation and planning module when it detects that the risk level of any path point has changed by more than 10% compared with the last evaluation value.

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

8. 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 rise rate from upstream water level data periodically extracted from the standardized environmental sensing dataset; The risk threshold judgment subunit is used to trigger an emergency path replanning request when the water level rise rate exceeds a preset threshold of 5 cm / min; The power response control subunit is used to output a power boost control instruction to the execution control module to increase the engine speed to no less than 2500rpm after receiving the emergency trigger signal, and simultaneously activate the anti-stall protection strategy.

9. A radar-based vehicle wading path planning method, using the radar-based vehicle wading path planning system according to any one of claims 1 to 8, characterized in that: The steps include: Step 1: Use the multimodal perception module to collect millimeter-wave radar echo data, underwater sonar depth data, and infrared polarization image data. Perform time synchronization and spatial coordinate registration on various types of perception data to 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 environment feature map into the boundary enhancement module, and generate a boundary enhanced environment 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 error covariance matrix, and combine it with the extended Kalman filter algorithm to correct the vehicle state and output the system state information; Step 5: Input the system status information and the boundary-enhanced environmental feature map into the path evaluation and planning module, calculate the path stability index, and use it to filter the path point set and generate vehicle driving trajectory data based on the model predictive control method; Step 6: Generate vehicle steering control instructions, speed control instructions, and power system control instructions in the execution control module based on the driving trajectory data; Step 7: Analyze the water level changes in the environmental perception data in the emergency detection module. When it is detected that the risk condition meets the preset threshold, the replanning process of the path evaluation and planning step is triggered, and the execution control module is triggered to switch the control strategy at the same time.

Images