An intelligent unmanned vehicle environment perception system based on multi-sensor information fusion

Abstract

The application discloses a kind of intelligent unmanned vehicle environment perception systems based on multi-sensor information fusion, it is related to unmanned vehicle technical field.The system includes: sensor module, for collecting multi-source heterogeneous data, including GPS / IMU, laser radar, camera and millimeter wave radar;Data acquisition and fusion module, through the Kalman filter to multi-source data space-time synchronous fusion;Environment perception module, based on deep learning identification road boundary, obstacle and traffic sign, constructs dynamic perception model;High-precision map construction module, using SLAM technology and semantic information constructs and updates semantic three-dimensional map;Positioning algorithm module, fusion visual odometry, fusion data and high-precision map, calculates vehicle real-time pose;And control module, according to positioning and perception result carries out path planning and navigation control, the application provides a kind of intelligent unmanned vehicle environment perception systems based on multi-sensor information fusion, can be autonomously navigated and stably driven under complex and severe environment.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle technology, and in particular to an intelligent unmanned vehicle environmental perception system based on multi-sensor information fusion. Background Technology

[0002] The positioning technology of intelligent unmanned vehicles has gradually developed. Relying on the improvement of sensor performance and algorithm optimization, a technical system based on multi-sensor data and combining positioning and environmental perception has been formed. Specifically, this is manifested in the following aspects: Widespread application of sensors: With the improvement of the performance of sensors such as LiDAR, millimeter-wave radar, high-definition cameras, and GPS / IMU (such as improved ranging accuracy of LiDAR, improved resolution of cameras, and faster response speed of IMU), unmanned vehicles can now collect surrounding environmental data, including location, distance, image, and motion status information, through these sensors, providing basic data support for positioning. Moreover, the stability and adaptability of sensors have gradually increased, and they can meet the initial positioning and environmental perception needs in normal open environments. Diversified Exploration of Positioning Technologies: Multiple positioning technology paths have emerged within the industry, with GPS positioning being one of the mainstream technologies. It provides high-precision location information in open, unobstructed areas (such as suburbs and highways), meeting the basic navigation needs of autonomous vehicles. Simultaneously, SLAM (Simultaneous Localization and Mapping) technology is gradually being applied, using sensor data to build environmental maps in real time and achieve self-positioning, becoming an important direction for exploring alternatives to GPS in indoor or complex environments. Furthermore, visual odometry technology has also been developed, calculating the relative displacement of vehicles based on continuous images captured by cameras through feature point matching, assisting in positioning. Expanding Application Scenarios of Positioning Technology: Autonomous vehicle positioning technology has gradually extended from single open environments to multiple scenarios, with practical applications in autonomous driving, intelligent transportation systems, and logistics. For example, in closed logistics parks, autonomous vehicles can use preliminary positioning technology to transfer goods; in pilot areas on urban roads, simple driving tasks can also be completed based on existing positioning solutions. The integration of positioning technology with autonomous vehicle functions such as autonomous navigation, path planning, and obstacle avoidance is becoming increasingly close, driving autonomous vehicle technology from the laboratory to real-world scenarios.

[0003] Despite some progress in existing technologies, there are still significant shortcomings in practical and complex scenarios, making it difficult to meet the high-precision and high-reliability positioning requirements of intelligent unmanned vehicles. Specific technical problems are as follows: Significant positioning limitations: Traditional GPS positioning systems rely on satellite signals. While they can provide good positioning results in open areas, in scenarios such as urban areas with tall buildings (where signals are easily blocked by buildings and reflections create multipath interference), tunnels (where signals are completely blocked), and underground parking garages (where there is no satellite signal coverage), the signal will be severely blocked or interfered with, resulting in a significant decrease in positioning accuracy (such as the error increasing from the meter level to several meters or even tens of meters), or even a complete loss of positioning capability, which cannot meet the positioning needs of unmanned vehicles in complex urban roads or enclosed indoor scenarios.

[0004] The adaptability of SLAM technology is insufficient: Early applications of 2D SLAM technology have obvious scene limitations, only adapting to two-dimensional environments and struggling to handle three-dimensional scenes (such as environments with slopes, overpasses, and multi-level parking garages). It performs poorly in complex and ever-changing indoor and outdoor three-dimensional environments (such as shopping mall floor transitions and mountainous road undulations), failing to accurately construct three-dimensional environmental maps, thus leading to positioning errors. At the same time, even with the exploration of some 3D SLAM technologies, there are still problems such as missing semantic information, low map construction accuracy, and poor adaptability to dynamic environments, making it difficult to meet the requirements of autonomous vehicles for high-precision maps and positioning.

[0005] Weak Multi-Source Data Fusion Capability: Although existing technologies have attempted to combine multi-source data such as GPS, IMU, and sensors for positioning, their data fusion capabilities have significant shortcomings. On the one hand, there is a lack of efficient fusion algorithms, which cannot effectively handle the differences and errors between heterogeneous multi-source data (such as GPS location data, IMU motion data, LiDAR point cloud data, and camera image data), easily leading to data conflicts or redundancy, resulting in insufficient accuracy and reliability of the fused data. On the other hand, the data preprocessing stage is imperfect, and the filtering of noise and outliers in the raw sensor data is incomplete, further affecting the fusion effect, ultimately resulting in poor stability of the positioning results and an inability to cope with dynamic environmental changes during autonomous vehicle operation.

[0006] Insufficient environmental adaptability and robustness: Existing positioning technologies have weak adaptability to complex environments. On the one hand, in severe weather (such as heavy rain, fog, and sandstorms), the penetration capability of millimeter-wave radar is limited, the clarity of camera images decreases, and the noise of lidar point cloud data increases, resulting in reduced accuracy of sensor data acquisition and thus affecting positioning accuracy. On the other hand, visual odometry technology is sensitive to changes in lighting and scenes with missing textures. When there are sudden changes in ambient lighting (such as the moment of entering a tunnel) or when the road surface texture is monotonous (such as icy roads or blank wall areas), the difficulty of extracting and matching image feature points increases significantly, and the error of the calculated relative vehicle displacement is significantly amplified, which cannot provide effective assistance for positioning, resulting in insufficient robustness of the overall positioning system.

[0007] In summary, the limitations of existing technologies are mainly reflected in four aspects: failure of multi-source signals, single spatial perception dimension, insufficient depth of data fusion, and fragile environmental robustness. The lack of collaborative optimization among various technical modules leads to a sharp decline in the performance of the overall positioning system in complex environments. Summary of the Invention

[0008] To address the aforementioned technical problems, this invention provides an intelligent unmanned vehicle environmental perception system based on multi-sensor information fusion, which enables autonomous navigation and stable driving in complex and harsh environments.

[0009] This invention provides an intelligent unmanned vehicle environmental perception system based on multi-sensor information fusion, including: a sensor module configured to collect multi-source heterogeneous data of the environment surrounding the unmanned vehicle, the sensor module including a GPS / IMU integrated navigation unit, a lidar, a high-definition camera and a millimeter-wave radar; The data acquisition and fusion module is configured to preprocess multi-source heterogeneous data, and use the extended Kalman filter algorithm to perform spatiotemporal synchronization and fusion of the preprocessed multi-source data, and output fused environmental data. The environment perception module is configured to process image data and point cloud data in the fused environmental data based on deep learning algorithms, identify road boundaries, obstacles and traffic signs in the environment, and build a dynamic perception model of the environment around the vehicle, and output semantic information through the model. The high-precision map building module is configured to build and update a 3D map containing semantic labels in real time based on SLAM technology and semantic information. The positioning algorithm module is configured to calculate visual odometry data based on image data collected by a high-definition camera, and to fuse the visual odometry data, the fused environmental data, and the 3D map. The algorithm is optimized to calculate the real-time position and attitude of the unmanned vehicle in the global coordinate system. The control module is configured to perform path planning and generate control commands based on real-time position and attitude, dynamic perception model and 3D map, to drive the unmanned vehicle to perform autonomous navigation and obstacle avoidance actions.

[0010] Furthermore, the data acquisition and fusion module includes: The data preprocessing unit is configured to clean, denoise, and convert the format of multi-source heterogeneous data, remove outliers, and unify the data format. The data preprocessing unit employs a combination of statistical filtering and radius filtering to remove outliers from the lidar point cloud data: statistical filtering is based on the statistical distribution of the average distance between a point and its k nearest neighbors, where the average neighborhood distance of a point is... satisfy ,in, The global mean. Standard deviation If the value is a multiple of the standard deviation, the point is identified as an outlier and removed; radius filtering removes isolated points whose number of neighboring points within a preset search radius r is less than a preset threshold Nmin. Gross errors are removed from GPS data using the 3σ criterion based on a sliding window. When the deviation of newly acquired GPS data from the mean within the window exceeds three times the standard deviation, it is judged as a gross error and discarded. IMU data is then used for interpolation to fill the gaps. An adaptive denoising method based on wavelet transform is used for high-definition camera image data, and Bayesian threshold shrinkage is employed. Distinguish between image edges and noise, perform thresholding on high-frequency component coefficients, and reconstruct the image; whereby... It is the noise standard deviation. It is the signal standard deviation; The data fusion unit, connected to the data preprocessing unit, is configured to use an extended Kalman filter algorithm to fuse preprocessed GPS data, IMU data, LiDAR point cloud data, and camera image data. Specifically, it includes: Initialize the sub-unit and configure it to set the initial state vector and initial covariance matrix. The state vector includes three-dimensional position, three-dimensional velocity, quaternion attitude, accelerometer zero bias and gyroscope zero bias. The prediction subunit is configured to predict the current state estimate and covariance matrix based on the acceleration and angular velocity measured by the IMU using a kinematic model. The update subunit is configured to calculate the Kalman gain based on the sensor measurement data at the current moment, and to correct the predicted state estimate and covariance matrix. The adaptive adjustment subunit is configured to dynamically adjust the measurement noise covariance matrix according to GPS signal quality indicators, including the horizontal accuracy factor, the number of satellites participating in positioning, and the carrier-to-noise ratio. When the GPS signal is good, a smaller measurement noise covariance is set; when the GPS signal is weak, the measurement noise covariance is adaptively increased; and when the GPS signal fails, the GPS update channel is disabled and the process noise covariance is increased. The iterative sub-unit is configured to take the corrected state estimate and covariance matrix as input for the next time step and perform prediction and update operations in a loop.

[0011] Furthermore, the positioning algorithm module includes: The visual odometry unit is configured to receive continuous images captured by a high-definition camera, extract image feature points through the ORB algorithm, match feature points of adjacent frames, and calculate the relative displacement and attitude changes of the unmanned vehicle. The fusion positioning unit is connected to the visual odometry calculation unit, the data acquisition and fusion module, and the high-precision map construction module, respectively. It is configured to use a covariance adaptive weighted fusion method, integrating the global position reference provided by the fused environmental data, the local displacement reference output by the visual odometry calculation unit, and the positioning benchmark provided by the 3D map. Based on the current uncertainty of each information source, it calculates the weight matrix Wi=Pi. - ¹, of which The corresponding covariance matrix is... The weight matrix is ​​used to calculate the global position and attitude of the autonomous vehicle through weighted fusion.

[0012] Furthermore, the control module is also configured to feed back the actual execution status of the vehicle's actuator to the positioning algorithm module. The positioning algorithm module dynamically adjusts the filtering parameters or optimizes the constraints in the algorithm based on the actual execution status. The actual execution status fed back by the control module includes: The vehicle's actual steering angle, actual acceleration, actual deceleration, wheel speed, and yaw rate; The localization algorithm module makes at least one of the following dynamic adjustments based on the actual execution status: A vehicle kinematics model is constructed based on the actual steering angle and vehicle speed, serving as an independent motion prediction source, and then weighted and fused with the IMU prediction. Based on the residual between the actual execution status and the control command, determine whether the execution system is faulty, and switch the positioning algorithm to conservative mode when a fault is detected; The process noise covariance matrix of the extended Kalman filter is dynamically adjusted based on the deviation statistics between the actual execution status and the model prediction. Based on the actual vehicle speed and yaw rate, the feature point extraction threshold and matching search range of the visual odometer are dynamically adjusted.

[0013] Furthermore, the positioning algorithm module adopts a graph optimization-based positioning framework, incorporating the desired control quantity output by the control module as a control factor into the optimization problem. Control nodes are added to the factor graph to constrain the transformation relationship between states at adjacent time points. The error term is represented as: , in To control the desired pose change corresponding to the control command, To control uncertainty.

[0014] Furthermore, the environmental perception module includes: The data input unit is configured to receive fused environmental data from the data acquisition and fusion module, and filter out camera image data and lidar point cloud data. The data processing unit is configured to process camera image data and LiDAR point cloud data based on deep learning. It uses YOLOv8 to achieve 2D target detection, PointPillars to achieve 3D target detection, BEVFormer to achieve drivable area and lane line detection, SORT+DeepSORT to achieve multi-target tracking, Social-LSTM to achieve target motion prediction, and integrates the position, attributes and motion state of various environmental elements to build a dynamic perception model of the vehicle's surrounding environment. The system automatically classifies dynamic and static elements based on semantic categories. Lane lines and traffic signs are classified as static elements, while pedestrians and vehicles are classified as dynamic elements. Kalman filtering is used to continuously track the speed changes of dynamic targets.

[0015] Furthermore, the high-precision map building module includes: The point cloud registration subunit is configured to use the iterative nearest point algorithm to register point cloud data of consecutive frames, estimate the pose of the unmanned vehicle, and adjust the point cloud alignment. The semantic information fusion subunit is configured to back-project the image semantic segmentation results output by the environment perception module to the 3D point cloud space through the joint calibration parameters of the camera and LiDAR, attach semantic labels to each point cloud, and use the semantic information to assist SLAM front-end feature matching and semantic map construction. The global optimization subunit is configured to use a bundle adjustment algorithm to perform global optimization of the map. In feature matching, features with the same semantic label are prioritized, and the element status is determined based on the semantic label and the tracking algorithm, thus providing environmental constraints for localization.

[0016] Furthermore, the data acquisition and fusion module is also configured to fuse data from multiple sensors of the same type using an adaptive weighted average algorithm. The adaptive weighted average algorithm includes: Calculate the optimal weighting factor for each sensor based on the measurement variance of each sensor; Determine the weights of each sensor while minimizing the total variance; The fused sensor data value is calculated based on the weighted average of the measurements from each sensor. The weighted average algorithm should be extended to a multi-dimensional adaptive rule, including four dimensions: sensor intrinsic characteristics, environmental context, data consistency, and historical statistics. Specifically, an adaptive rule based on sensor intrinsic characteristics is adopted to dynamically adjust the variance according to the GPS horizontal accuracy factor and the number of satellites; an adaptive rule based on environmental context is adopted to dynamically adjust the sensor weights according to the current driving environment of the vehicle; an adaptive rule based on data consistency is adopted to dynamically adjust the variance according to the residual between the sensor measurement value and the current fused value; and an adaptive rule based on historical statistics is adopted to estimate the real-time statistical characteristics of the sensor using historical data within a sliding window.

[0017] A method for environmental perception and localization of intelligent unmanned vehicles based on multi-sensor information fusion includes the following steps: The sensor module collects multi-source heterogeneous data about the environment surrounding the autonomous vehicle. Multi-source heterogeneous data is preprocessed, and the extended Kalman filter algorithm is used to perform spatiotemporal synchronization and fusion of the preprocessed multi-source data, outputting fused environmental data; Based on deep learning algorithms, image data and point cloud data in the fused environmental data are processed to identify road boundaries, obstacles and traffic signs in the environment, and to build a dynamic perception model of the environment around the vehicle. Based on SLAM technology, and combined with the output semantic information, a 3D map containing semantic labels is constructed and updated in real time. By integrating visual odometry data, fused environmental data, and 3D maps, the real-time position and attitude of the unmanned vehicle in the global coordinate system are calculated through optimized algorithms; Based on real-time position and attitude, dynamic perception model and 3D map, path planning is performed and control commands are generated to drive the unmanned vehicle to perform autonomous navigation and obstacle avoidance actions.

[0018] The technical solution provided by this invention has the following advantages compared with the prior art: The intelligent unmanned vehicle environment perception system based on multi-sensor information fusion provided by this invention systematically improves upon the shortcomings of existing technologies, such as insufficient adaptability of SLAM technology, weak multi-source data fusion capability, and insufficient environmental adaptability and robustness, through multi-module collaboration and key technology integration. The system uses the environment perception module to identify semantic information such as road boundaries and obstacles based on deep learning, and the high-precision map building module combines this semantic information with SLAM technology to build a 3D map containing semantic labels. This not only breaks through the limitation of traditional 2DSLAM being only adaptable to planar environments, but also accurately restores 3D scenes with changes such as slopes, overpasses, etc., and solves the problems of missing semantic information and poor dynamic adaptability of existing 3DSLAM, significantly improving the mapping accuracy and positioning reliability in complex environments. Its data acquisition and fusion module preprocesses multi-source heterogeneous data from GPS / IMU, LiDAR, camera, and millimeter-wave radar, and uses the extended Kalman filter algorithm for spatiotemporal synchronization and optimal fusion. This approach effectively resolves the differences and errors between multi-source data, avoids data conflicts and redundancy, and outputs high-precision, highly consistent fused environmental data. It overcomes the poor positioning stability issues caused by the weak data fusion capabilities of traditional methods. Building upon this, the positioning algorithm module further integrates visual odometry data, environmental data fused via EKF, and semantically rich 3D maps. Through optimized algorithms, it comprehensively calculates the vehicle's real-time position and attitude. Even in adverse weather conditions such as heavy rain and fog that degrade sensor performance, or when sudden changes in lighting or texture loss cause visual odometry failure, the system can still rely on multi-source information complementarity and map matching for correction and compensation, effectively suppressing error expansion and greatly enhancing the system's adaptability and robustness in all-weather, complex, and dynamic environments. Furthermore, the control module feeds back the actual execution status of the vehicle's execution unit to the positioning algorithm module, enabling it to dynamically adjust filtering parameters or optimize constraints, forming an integrated closed-loop optimization of perception, decision-making, and control. This further improves positioning accuracy and the overall vehicle intelligence level, ultimately achieving high-precision autonomous navigation and stable driving of the unmanned vehicle in complex 3D spaces and harsh environments. Attached Figure Description

[0019] Figure 1 This is a structural diagram of an intelligent unmanned vehicle environmental perception system based on multi-sensor information fusion, provided in an embodiment of the present invention. Figure 2 A flowchart of SLAM mapping provided in an embodiment of the present invention; Figure 3 A laser sensor data acquisition process is provided as an embodiment of the present invention; Figure 4 A multi-sensor adaptive weighted model diagram provided in an embodiment of the present invention; Figure 5 This is a flowchart illustrating an intelligent unmanned vehicle environmental perception method based on multi-sensor information fusion, provided as an embodiment of the present invention. Detailed Implementation

[0020] The following detailed description of a specific embodiment of the present invention is provided in conjunction with the accompanying drawings. However, it should be understood that the scope of protection of the present invention is not limited to the specific embodiment.

[0021] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "axial," "radial," and "circumferential" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing the technical solution of this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0022] The present invention will be described below through several specific embodiments. To keep the following description of the embodiments clear and concise, detailed descriptions of known functions and components may be omitted. When any component of an embodiment of the present invention appears in more than one drawing, the component may be represented by the same reference numerals in each drawing.

[0023] Figure 1 This is a structural diagram of an intelligent unmanned vehicle environmental perception system based on multi-sensor information fusion, provided in an embodiment of the present invention. Figure 2 This is a flowchart of a SLAM mapping process provided in an embodiment of the present invention. Figure 3 This invention provides a laser sensor data acquisition process according to an embodiment of the invention. Figure 4 This is a diagram of a multi-sensor adaptive weighted model provided in an embodiment of the present invention. Figure 5 This is a flowchart illustrating an intelligent unmanned vehicle environmental perception method based on multi-sensor information fusion, provided as an embodiment of the present invention.

[0024] like Figure 1 and Figure 2As shown, this embodiment of the invention provides an intelligent unmanned vehicle environmental perception system based on multi-sensor information fusion, including: a sensor module configured to collect multi-source heterogeneous data of the environment surrounding the unmanned vehicle, the sensor module including a GPS / IMU integrated navigation unit, a lidar, a high-definition camera, and a millimeter-wave radar; a data acquisition and fusion module configured to preprocess the multi-source heterogeneous data, and use an extended Kalman filter algorithm to perform spatiotemporal synchronization and fusion of the preprocessed multi-source data, outputting fused environmental data; and an environmental perception module configured to process the image data and point cloud data in the fused environmental data based on a deep learning algorithm, identifying road boundaries, obstacles, and other features in the environment. The system includes a traffic sign module, which constructs a dynamic perception model of the vehicle's surrounding environment and outputs semantic information through the model; a high-precision map construction module, configured to build and update a 3D map containing semantic labels in real time based on SLAM technology and semantic information; a localization algorithm module, configured to calculate visual odometry data based on image data collected by high-definition cameras, and fuse the visual odometry data, fused environmental data, and 3D map, and calculate the real-time position and attitude of the unmanned vehicle in the global coordinate system through an optimized algorithm; and a control module, configured to perform path planning and generate control commands based on the real-time position and attitude, dynamic perception model, and 3D map, driving the unmanned vehicle to perform autonomous navigation and obstacle avoidance actions.

[0025] Furthermore, the data acquisition and fusion module includes: a data preprocessing unit configured to clean, denoise, and convert the format of multi-source heterogeneous data, remove outliers, and unify the data format; specifically, the data preprocessing unit employs a combination of statistical filtering and radius filtering strategies to remove outliers from LiDAR point cloud data: statistical filtering is based on the statistical distribution of the average distance between a point and its k nearest neighbors, removing points whose average distance exceeds the global mean μ±β·σ; radius filtering removes isolated points whose number of neighboring points within a preset search radius r is less than a preset threshold Nmin; for GPS data, a 3σ criterion based on a sliding window is used to remove gross errors, and when the deviation of newly acquired GPS data from the mean within the window exceeds 3 times the standard deviation, it is judged as a gross error and discarded, and IMU data is used for interpolation to fill in the gaps; for high-definition camera image data, an adaptive denoising method based on wavelet transform is used, and the threshold is shrunk using Bayesian methods. Distinguish between image edges and noise, perform thresholding on high-frequency component coefficients, and reconstruct the image; whereby... It is the noise standard deviation. It is the standard deviation of the signal.

[0026] Furthermore, the data acquisition and fusion module also includes: a data fusion unit, connected to the data preprocessing unit, configured to use an extended Kalman filter algorithm to fuse preprocessed GPS data, IMU data, LiDAR point cloud data, and camera image data. Specifically, this includes: an initialization subunit, configured to set an initial state vector and an initial covariance matrix, the state vector containing 3D position, 3D velocity, quaternion attitude, accelerometer bias, and gyroscope bias; a prediction subunit, configured to predict the current state estimate and covariance matrix based on the acceleration and angular velocity measured by the IMU using a kinematic model; and an update subunit, configured to update the current state estimate and covariance matrix based on the current time... The system uses sensor measurement data to calculate the Kalman gain and corrects the predicted state estimate and covariance matrix. An adaptive adjustment subunit is configured to dynamically adjust the measurement noise covariance matrix based on GPS signal quality indicators, including the horizontal accuracy factor, the number of satellites involved in positioning, and the carrier-to-noise ratio. When the GPS signal is good, a smaller measurement noise covariance is set; when the GPS signal is weak, the measurement noise covariance is adaptively increased; and when the GPS signal fails, the GPS update channel is disabled and the process noise covariance is increased. An iterative subunit is configured to use the corrected state estimate and covariance matrix as input for the next time step, cyclically performing prediction and update operations.

[0027] Furthermore, the localization algorithm module includes: a visual odometry unit, configured to receive continuous images captured by a high-definition camera, extract image feature points using the ORB algorithm, match feature points in adjacent frames, and calculate the relative displacement and attitude changes of the autonomous vehicle; and a fusion localization unit, connected to the visual odometry unit, the data acquisition and fusion module, and the high-precision map construction module, configured to use a covariance adaptive weighted fusion method to integrate the global position reference provided by the fused environmental data, the local displacement reference output by the visual odometry unit, and the positioning benchmark provided by the 3D map, and calculate the weight matrix Wi=Pi based on the current uncertainty of each information source. - ¹, The global position and attitude of the autonomous vehicle are calculated by weighted fusion.

[0028] Furthermore, the control module is also configured to feed back the actual execution state of the vehicle execution unit to the positioning algorithm module. The positioning algorithm module dynamically adjusts the filtering parameters or optimizes the constraints in the algorithm based on the actual execution state. The actual execution state fed back by the control module includes: the vehicle's actual steering angle, actual acceleration, actual deceleration, wheel speed, and yaw rate. The positioning algorithm module performs at least one of the following dynamic adjustments based on the actual execution state: constructing a vehicle kinematic model based on the actual steering angle and vehicle speed as an independent motion prediction source, and performing weighted fusion with IMU prediction; determining whether the execution system is faulty based on the residual between the actual execution state and the control command, and switching the positioning algorithm to conservative mode when a fault is detected; dynamically adjusting the process noise covariance matrix of the extended Kalman filter based on the deviation statistics between the actual execution state and the model prediction; and dynamically adjusting the feature point extraction threshold and matching search range of the visual odometer unit based on the actual vehicle speed and yaw rate.

[0029] Furthermore, the positioning algorithm module adopts a graph optimization-based positioning framework, incorporating the desired control quantity output by the control module as a control factor into the optimization problem. Control nodes are added to the factor graph to constrain the transformation relationship between states at adjacent time points. The error term is represented as: in To control the desired pose change corresponding to the control command, To control uncertainty.

[0030] Furthermore, the environmental perception module includes: a data input unit configured to receive fused environmental data from the data acquisition and fusion module, and filter out camera image data and LiDAR point cloud data; and a data processing unit configured to process the camera image data and LiDAR point cloud data based on deep learning, using YOLOv8 for 2D target detection, PointPillars for 3D target detection, BEVFormer for drivable area and lane line detection, SORT+DeepSORT for multi-target tracking, and Social-LSTM for target motion prediction, and integrate the position, attributes, and motion state of each environmental element to construct a dynamic perception model of the vehicle's surrounding environment; wherein, the system automatically classifies dynamic and static elements according to semantic categories, classifying lane lines and traffic signs as static elements, and pedestrians and vehicles as dynamic elements, and continuously tracks the speed changes of dynamic targets through Kalman filtering.

[0031] Furthermore, the high-precision map construction module includes: a point cloud registration subunit, configured to use an iterative nearest-point algorithm to register point cloud data of consecutive frames, estimate the pose of the unmanned vehicle, and adjust the point cloud alignment; a semantic information fusion subunit, configured to back-project the image semantic segmentation results output by the environment perception module to the 3D point cloud space through the joint calibration parameters of the camera and LiDAR, attach semantic labels to each point cloud, and use the semantic information to assist SLAM front-end feature matching and semantic map construction; and a global optimization subunit, configured to use a bundle adjustment algorithm to globally optimize the map; wherein, during feature matching, features with the same semantic labels are prioritized for matching, and the element status is determined based on the semantic labels combined with the tracking algorithm to provide environmental constraints for localization.

[0032] Further reference Figure 3 and Figure 4 The data acquisition and fusion module is also configured to fuse data from multiple sensors of the same type using an adaptive weighted average algorithm. This algorithm includes: calculating the optimal weighting factor for each sensor based on its measurement variance; determining the weight of each sensor while minimizing the total variance; calculating the fused sensor data value based on the weighted average of the sensor measurements; extending the weighted average algorithm to multi-dimensional adaptive rules, including four dimensions: sensor intrinsic characteristics, environmental context, data consistency, and historical statistics. Specifically, adaptive rules based on sensor intrinsic characteristics dynamically adjust the variance according to the GPS horizontal accuracy factor and the number of satellites; adaptive rules based on environmental context dynamically adjust the sensor weights according to the vehicle's current driving environment; adaptive rules based on data consistency dynamically adjust the variance based on the residual between the sensor measurements and the current fused value; and adaptive rules based on historical statistics estimate the real-time statistical characteristics of the sensors using historical data within a sliding window.

[0033] refer to Figure 5A method for environmental perception and localization of intelligent unmanned vehicles based on multi-sensor information fusion includes the following steps: collecting multi-source heterogeneous data of the environment surrounding the unmanned vehicle through sensor modules; preprocessing the multi-source heterogeneous data and using an extended Kalman filter algorithm to perform spatiotemporal synchronization and fusion of the preprocessed multi-source data, outputting fused environmental data; processing image data and point cloud data in the fused environmental data based on deep learning algorithms to identify road boundaries, obstacles, and traffic signs in the environment, and constructing a dynamic perception model of the environment surrounding the vehicle; constructing and updating a 3D map containing semantic labels in real time based on SLAM technology and combining the output semantic information; fusing visual odometry data, fused environmental data, and the 3D map, and calculating the real-time position and attitude of the unmanned vehicle in the global coordinate system through optimization algorithms; performing path planning and generating control commands based on the real-time position and attitude, dynamic perception model, and 3D map to drive the unmanned vehicle to perform autonomous navigation and obstacle avoidance actions; feeding back the actual execution state of the vehicle's execution unit, and dynamically adjusting the filtering parameters or constraints in the optimization algorithm according to the actual execution state.

[0034] This technical solution addresses the environmental perception and autonomous driving needs of intelligent unmanned vehicles by constructing a multi-sensor deep fusion and full-process closed-loop control intelligent unmanned vehicle environmental perception system. Through the full-link technology design of sensor data acquisition, multi-source data fusion, environmental perception modeling, high-precision map construction, multi-source fusion positioning, and autonomous control execution, it achieves high-precision positioning, reliable environmental perception, and stable autonomous driving of unmanned vehicles in complex scenarios. At the same time, through bidirectional data feedback and collaborative optimization of each module, it ensures the robustness and real-time performance of the system in dynamic driving environments.

[0035] The system achieves tight integration of hardware and software using embedded technology. Its core includes a sensor module, a data acquisition and fusion module, an environmental perception module, a high-precision map building module, a positioning algorithm module, and a control module. These modules work together to form a data closed loop, completing the entire process of environmental perception and autonomous driving for unmanned vehicles from the acquisition of raw environmental data to the execution of final control commands. The LiDAR uses the SLAMTEC RPLIDAR A3 single-line LiDAR sensor, and the core computing platform is compatible with embedded platforms such as NVIDIA Orin and TDA4. It is equipped with real-time operating systems such as QNX and Preempt-RT Linux to ensure operating efficiency.

[0036] The sensor module adopts a full-scene data acquisition solution, integrating four core sensors: GPS / IMU, LiDAR, HD camera, and millimeter-wave radar, to achieve comprehensive acquisition of multi-dimensional data including location, space, vision, and harsh environments. The GPS receiver obtains satellite positioning information to provide basic global position. The IMU uses accelerometers, gyroscopes, and magnetometers to measure vehicle speed, acceleration, heading angle, and other motion states in real time, compensating for positioning gaps when GPS signals are interrupted. The LiDAR emits laser beams to measure object distance, reflectivity, and velocity, generating 3D point cloud data to construct a high-precision spatial model of the environment, providing core data for map building and obstacle detection. The HD camera acquires high-resolution environmental images to identify visual information such as road signs, traffic lights, pedestrians, and vehicles, supporting visual mileage calculation and environmental perception. The millimeter-wave radar operates in the millimeter-wave band, can penetrate harsh environments such as fog, smoke, and dust, and stably outputs obstacle distance and velocity data under complex weather conditions, complementing the functions of the LiDAR and camera. The raw data from various sensors are standardized through a process of “sensor sensing unit (physical quantity to electrical signal) → signal processing unit (amplification, filtering, digital-to-analog conversion) → data transmission unit (RS485 / Modbus / Wi-Fi, etc.)” to ensure that the data is readable and transmitted to the data acquisition and fusion module with low noise.

[0037] The data acquisition and fusion module, as the core of the system's data processing, achieves optimized fusion of multi-source heterogeneous data through a three-level processing mechanism, providing high-quality data input for subsequent modules. The core module employs Extended Kalman Filter (EKF) as the fusion technique, combined with an adaptive weighted average algorithm to further optimize the fusion results. The data preprocessing unit addresses the "noisy and disordered" problem of multi-source data by using a combination of statistical and signal processing methods to formulate differentiated cleaning, denoising, format conversion, and spatiotemporal alignment strategies. For data cleaning, a combination of statistical filtering and radius filtering is used to remove outliers from both LiDAR point cloud and millimeter-wave radar data. In the statistical filtering, the average distance from each point in the point cloud to its k nearest neighbors is calculated. Then calculate the global mean. and standard deviation ,like ( If the value is between 1.0 and 3.0, it is considered an outlier and needs to be removed. For GPS / IMU data, an improved 3σ criterion combined with median filtering is used to remove outliers, and online IMU calibration is performed. Regarding data denoising, high-definition camera image data uses wavelet transform-based adaptive denoising to preserve edge features. The Bayesian shrinkage threshold is calculated using the following formula: For high-frequency component coefficients ,like Then set it to zero, if The distance and velocity data from both LiDAR and millimeter-wave radar are retained, and noise is suppressed by adjusting the parameters of the adaptive Kalman filter. Format conversion and spatiotemporal alignment are achieved using the GPS PPS second pulse as the global clock source for time unification, and all sensor data are converted to a vehicle coordinate system with the rear axle center as the origin for spatial unification. The point cloud data conversion formula is as follows: Meanwhile, the processed data is encapsulated into a Protobuf or ROS2 standardized message format containing timestamps, coordinate system identifiers, data volumes, and confidence levels, thus completing the data structure standardization.

[0038] The data fusion unit performs multi-source data fusion based on extended Kalman filtering. Through a cyclical process of initialization, prediction, updating, and iteration, it reduces single-sensor errors and improves data accuracy and reliability. The initialization phase sets a 21-dimensional initial state vector containing the vehicle's 3D position, 3D velocity, quaternion attitude, accelerometer bias, and gyroscope bias. With the initial covariance matrix The prediction phase relies on IMU kinematic recursion, combined with the vehicle system motion model, to predict the current state and covariance matrix. The formula is as follows:

[0039] , , in The process noise covariance matrix is ​​used; during the update phase, the Kalman gain is calculated using sensor measurements, the predicted state is corrected, and the covariance matrix is ​​updated. The Kalman gain formula is... , The state correction formula is , The covariance update formula is , in To measure the noise covariance matrix, the iterative phase uses the corrected state and covariance as inputs for the next time step, repeatedly performing optimization. Simultaneously, sensor measurement anomalies are detected using the chi-square test of residuals. The formula for the normalized sum of squares of the information is...

[0040] , in ,like If the value exceeds the preset threshold, the measurement is considered abnormal.

[0041] To address complex scenarios, the fusion process incorporates multi-model adaptive estimation and covariance matching techniques. The process noise and measurement noise covariance matrices are dynamically adjusted based on GPS signal quality indicators, sensor operating status, and vehicle dynamics. When the GPS signal is good, the following settings are applied: , When the signal is weak, adjust as follows: , When the signal fails The system is set to infinity; a federated Kalman filter architecture is employed to improve fault tolerance, ensuring that the position jump of the fused result does not exceed 0.5m when a single sensor temporarily fails. In areas with good GPS signal, the fused positioning error is ≤0.3m; in areas with weak GPS signal, the error is ≤1.5m; when the GPS signal is completely lost, the positioning error drift rate of the pure inertial / laser combination is ≤2%, and it can converge quickly after GPS reacquisition. The output unit distributes the fused data in a standardized format to subsequent modules such as environmental perception, high-precision map construction, and positioning algorithms. Simultaneously, an adaptive weighted average algorithm is used to post-process the Kalman-filtered data. The fusion formula is...

[0042] , in , The location results are from various information sources. This is the corresponding covariance matrix. Simultaneously, adaptive rules are formulated based on the intrinsic characteristics of the sensor; the GPS variance adjustment formula is: By combining environmental context, data consistency, and historical statistics to dynamically adjust sensor weights, the historical statistical variance estimation formula is as follows:

[0043] , Furthermore, weight iteration is completed by combining event triggering and periodic updates, which further improves the accuracy and stability of the fusion results.

[0044] The environmental perception module, driven by deep learning and combined with computer vision technology, achieves environmental element recognition and dynamic environment modeling, providing reliable environmental constraints for localization, map building, and control. The module receives fused data, filters key data such as camera images and LiDAR point clouds, and completes core perception tasks through a multi-task deep learning architecture: 2D target detection uses YOLOv8 for real-time detection of pedestrians, vehicles, and other targets; 3D target detection uses PointPillars to output the 3D position, size, and orientation of targets from the point cloud; drivable area and lane line detection uses BEVFormer to fuse images and point clouds to generate a bird's-eye view semantic raster; target tracking uses SORT+DeepSORT to achieve stable multi-target tracking through Kalman filtering and appearance feature matching; and motion prediction uses Social-LSTM to predict future target motion based on historical trajectories. The system automatically distinguishes between static and dynamic elements based on semantic categories, continuously tracks the speed changes of dynamic targets through Kalman filtering, and uses the surrounding environmental context to assist in the judgment of targets with low classification confidence. For dynamic obstacles, a three-stage pipeline of "detection-tracking-prediction" is constructed to complete motion state updates and trajectory predictions, which are then output to the control module. The perception results must meet strict quantitative accuracy standards: the average accuracy of target detection (mAP@0.5) must be no less than 90%, the 3D detection position error must be less than 0.3 meters, the multi-target tracking accuracy (MOTA) must be higher than 85%, the lane line detection pixel error must be less than 3 pixels, the positioning error of static elements in the global coordinate system must be less than 0.2 meters, and the perception system latency must not exceed 100 milliseconds, with an update frequency of no less than 20 Hz, to ensure accurate environmental constraints for subsequent modules.

[0045] The high-precision map building module uses SLAM technology as its core and integrates deep learning semantic extraction technology to build and update a high-precision dynamic map containing "spatial coordinates + semantic attributes" in real time, providing rich reference benchmarks for the positioning module. The module receives fused data and environmental perception results, first performing deduplication, filtering, and discrimination operations on the LiDAR point cloud data to improve data quality, and then completing the high-precision map construction in six steps: After preprocessing the point cloud with noise reduction filtering, the ICP algorithm is used to match the current point cloud with the existing map to estimate vehicle pose and adjust alignment to generate a point cloud map; based on deep learning, semantic information such as the category and location of objects like lane lines and traffic lights is extracted from visual data to add semantic attributes to the point cloud map; vMAP technology is used to vectorize map elements to achieve compact storage of map information; the current point cloud is stitched with the existing map, loop closures are detected and accumulated errors are corrected; and the BundleAdjustment algorithm is used to complete global optimization, achieving centimeter-level or even millimeter-level map accuracy. Meanwhile, the module is equipped with a real-time update unit to continuously receive new environmental data such as road construction and temporary obstacles, dynamically update map information and synchronize it to the positioning algorithm module and control module, respectively update the positioning reference benchmark and path planning basis, and ensure the consistency between the map and the actual driving environment.

[0046] In a specific embodiment, ICP is one of the most classic algorithms in point cloud registration. Its core objective is to solve the rigid transformation (rotation matrix R and translation vector t) between two point clouds through iterative optimization, so that the source point cloud can be aligned with the target point cloud as much as possible after transformation.

[0047] The following are the detailed mathematical principles and matching steps of the ICP algorithm: Source Cloud The point cloud collected at the current moment, including One point, denoted as Target point cloud Existing map point clouds or reference frame point clouds, denoted as Objective: To find an optimal rigid body transformation matrix. This makes the source cloud After transformation, it is consistent with the target point cloud. The alignment error is minimized.

[0048] The core logic of the ICP algorithm is a loop of "coordinate transformation - finding the nearest point - calculating the error - transformation again", which is specifically divided into the following four main steps: Before performing the first iteration, an initial transformation estimate needs to be provided. This initial value is typically provided by odometry, IMU, or the matching result from the previous frame. Note: If the initial value is too biased, ICP can easily get trapped in a local optimum (i.e., mismatch).

[0049] This is the most critical and time-consuming step in ICP. (For source cloud...) Each point in It is necessary to target point cloud Find the point closest to it As the corresponding point.

[0050] Distance metric: Euclidean distance is typically used. .

[0051] Acceleration strategy: Due to the huge number of point clouds (e.g. and For datasets in the hundreds of thousands, brute-force nearest neighbor search would be extremely slow. In practical engineering, the KD-Tree data structure is typically used to accelerate nearest neighbor search, reducing the time complexity from... Reduce to .

[0052] The previous step yielded a set of "hypothetical" corresponding point pairs. , Afterwards, ICP needs to calculate a transformation. , To minimize the following error function (usually using the least squares method): , Apply transformation: transform the calculation from the previous step Application to Source Cloud Above, a new point cloud is obtained. Calculation error: Calculation error of the transformed point cloud. 'With target point cloud The average distance between them (i.e., the distance error in step 2). Convergence is determined if the error change is less than a set threshold (e.g., ...). If the maximum number of iterations is reached (in meters), then stop iterating and output the final result. Otherwise, As a new source point cloud, return to step 2, find the nearest point again, and continue iterating.

[0053] The ICP algorithm, through the steps described above, calculates the relative motion of the autonomous vehicle from its previous position to its current position (i.e., , , This corrects the odometer drift. Using the calculated precise pose, the point cloud of the current frame is transformed to the global coordinate system and registered into the existing point cloud map. If the vehicle returns to a previously visited location, the ICP will match the current point cloud with the historical keyframe point clouds. If the match is successful (i.e., the error is minimal), it indicates a loop closure, and the system will trigger a backend global optimization (such as BundleAdjustment mentioned in your document) to eliminate accumulated errors.

[0054] The positioning algorithm module employs a multi-source fusion positioning scheme, combining multi-source fusion data, visual odometry calculation results, and high-precision maps to achieve high-precision positioning under dual constraints of "global + local". The module collects multi-source fusion data output from the data fusion module as the basis for positioning. Visual odometry calculation is performed using computer vision technology. The ORB algorithm extracts feature points from continuous camera images. Feature point extraction is completed through FAST corner detection, pyramid multi-scale processing, gray-scale centroid method orientation calculation, and rBRIEF feature descriptor generation. Reliable feature point matching is achieved by combining Hamming distance metric, FLANN matching, and cross-validation. Finally, epipolar geometric constraints or the PnP algorithm are used to solve for vehicle relative displacement and attitude changes, compensating for positioning errors when GPS signals are interrupted. The fusion positioning unit integrates global position reference from multi-source fusion data, local displacement reference from visual mileage, and positioning benchmark from high-precision maps. It uses optimization algorithms such as nonlinear least squares to calculate the vehicle's global position and attitude. The fusion process employs a covariance adaptive weighted fusion strategy, where the weights are inversely proportional to the current uncertainty of each information source, rather than a fixed weight master-slave relationship. Finally, the positioning results are output to the control module, providing accurate position and attitude information for autonomous driving.

[0055] Nonlinear least squares is an optimization technique that estimates model parameters by minimizing the sum of squares of the residuals of a nonlinear function. Its standard form is:

[0056] , in: It is the state vector to be estimated (e.g., the vehicle's position, attitude, velocity, etc.); It is the first The residuals (error terms) between individual observations and model predictions are typically non-linear. This represents the square of the Euclidean norm.

[0057] When observations contain uncertainty, weighting is typically introduced to extend the objective function into a weighted form (Mahavir distance): , here It is the information matrix, which is equal to the observation covariance matrix. The inverse of the information matrix. The size of the information matrix reflects the confidence level of the observation: the smaller the covariance (i.e., the lower the uncertainty), the larger the information matrix, and the higher the weight of the residual in the optimization.

[0058] Due to the residual function about It is nonlinear and cannot be solved directly with closed-form solutions. Iterative numerical optimization methods are usually used, for example: Gauss-Newton method: The residuals are expanded by first-order Taylor expansion, and the state is updated by solving the linear least squares problem; Levenberg-Marquardt method: Based on Gauss-Newton method, a damping factor is introduced to balance convergence speed and stability.

[0059] In the field of robot localization and mapping, this type of optimization is usually constructed as a graph-based optimization problem: the nodes in the graph represent variables to be estimated (such as vehicle pose), the edges represent observation constraints (such as GPS measurement, visual odometry, map matching, etc.), the weights of the edges are determined by the information matrix, and the optimization objective is to adjust the nodes to minimize the sum of errors of all edges.

[0060] The fusion unit, derived from the data acquisition and fusion module (such as the output of GPS / IMU extended Kalman filtering), provides the vehicle's absolute position in the global coordinate system. However, it is susceptible to occlusion, multipath effects, and other factors, with uncertainties varying over time. Relative motion calculated through continuous image feature point matching offers high short-term accuracy but suffers from cumulative drift and is sensitive to lighting and texture. The precise vehicle position on the map is obtained by matching the current LiDAR point cloud with a pre-built semantic map (e.g., using ICP or feature matching), but its accuracy depends on map quality and environmental dynamics.

[0061] Fusing this information into a precise global pose is a typical application scenario for the nonlinear least squares method. The specific steps are as follows:

[0062] Let the vehicle state to be estimated be... It typically includes three-dimensional location. And orientation (e.g., quaternions or rotation matrices) In sliding window or full trajectory optimization, the state variable may be the pose at a series of time steps. .

[0063] Each information source corresponds to an error term, and its weight is determined by the covariance matrix.

[0064] Assume at time... The location observations provided by the fusion module are The corresponding covariance matrix is (Dynamically adjusted according to signal quality). The error term is then:

[0065] , Its contribution to the total cost is: , in, An information matrix representing GPS measurement information, represent The transpose of .

[0066] Visual odometry provides information from time to time. arrive relative transformation (including rotation) Peaceful relocation Its covariance is The error term is then:

[0067] , here This represents the relative transformation between poses. The weighted contribution is:

[0068] , in, An information matrix representing visual odometry measurement information. represent transpose By registering the current LiDAR point cloud with a high-precision map (e.g., using ICP or feature matching), the vehicle's pose in the map can be observed. and its covariance The error term is:

[0069] Weighted contribution ,in An information matrix representing LiDAR-high-precision map registration measurement information. represent The transpose of .

[0070] By summing all error terms with weights, we obtain the overall cost function: , A nonlinear least squares optimizer (such as CeresSolver, g2o, or GTSAM) is used iteratively to minimize the error. In each iteration, the optimizer calculates the residuals and Jacobian matrix based on the current state, constructs the linear system, and solves for the state update until convergence.

[0071] The emphasis on "weights being inversely proportional to the current uncertainty of each information source" is precisely achieved through dynamically adjusting the covariance matrix. For example, when the horizontal precision factor (HDOP) increases or the number of satellites decreases, it increases accordingly, while its inverse matrix decreases, thus reducing the weight of the GPS factor in optimization and avoiding the introduction of erroneous information. When vehicles move at high speeds or there are sudden changes in lighting, the quality of feature matching deteriorates; this can be dynamically adjusted by estimating online the number of feature points and reprojection errors. If there are too many dynamic objects in the current environment or the point cloud is sparse, the covariance of map matching increases, and the weights automatically decrease.

[0072] This adaptive mechanism ensures that the fusion result is always dominated by the most reliable information source, rather than using fixed weighting coefficients, thereby significantly improving the robustness and accuracy of the positioning system in complex scenarios.

[0073] The control module, as the core of the system's execution, performs path planning and autonomous control based on positioning results, environmental perception information, and high-precision maps. Simultaneously, it achieves bidirectional feedback of data from each module through a closed-loop control process, dynamically adjusting positioning and planning strategies. The module's information receiving unit receives position and attitude information from the positioning module, the environmental perception model from the environmental perception module, and the high-precision map from the high-precision map construction module. The path planning unit, combining vehicle dynamics characteristics and traffic rules, plans the optimal driving path based on positioning information and environmental perception results. The control command generation unit converts the path planning results into specific control commands such as steering angle, acceleration, and deceleration. The vehicle execution unit drives the steering, power, and braking systems to execute the control commands, achieving autonomous driving for the unmanned vehicle.

[0074] The system's core technological innovations are reflected in three aspects: deep fusion of multiple sensors, combination of SLAM and semantic modeling, and full-process closed-loop control. The full-process closed-loop control is achieved through bidirectional feedback at three levels: data, parameters, and algorithms, allowing the control execution status to feed back into the adjustment and optimization of the positioning, perception, and planning algorithms. At the data level, the vehicle's actual execution status is transmitted back to the positioning algorithm module via CAN bus or in-vehicle Ethernet, based on the actual steering angle. and vehicle speed Construct the vehicle kinematics model, the formula is:

[0075] , in For the wheelbase, the model's predicted pose changes are weighted and fused with IMU predictions to improve accuracy. This also serves as a fault detection basis; when control commands and actual feedback are significantly inconsistent, a fault flag is triggered, causing the positioning algorithm to switch to a conservative mode. At the parameter level, key parameters of the positioning and perception algorithms are adjusted online using control execution state feedback. The process noise covariance matrix of the Kalman filter is dynamically adjusted based on model error. The feature point selection threshold of the visual odometer is adjusted based on vehicle speed and yaw rate. The replanning trigger threshold for path planning is adjusted based on the deviation between the actual trajectory and the planned trajectory. At the algorithm level, control commands are incorporated as prior information into the positioning optimization problem. Control nodes are added to the factor graph to form a joint state estimate. The error term formula is...

[0076] , Simultaneously, by analyzing the differences between control feedback and sensor observations, road slip parameters are estimated online. On the one hand, this information is fed back to the control module to adjust the PID or MPC model, and on the other hand, the Kalman filter observation equation is modified to improve positioning accuracy.

[0077] At the embedded level, the system deploys each module on different cores or processes of a high-performance computing platform, exchanging data via shared memory. Control execution status is received via the CANFD interface and directly written to shared memory, achieving microsecond-level data feedback. The highest priority is assigned to control feedback processing tasks to ensure that feedback data is processed within a defined time window. At the same time, the update phase of Kalman filtering is bound to control feedback reception, allowing positioning results to quickly respond to changes in execution status. PTP clock synchronization ensures that the time base of each module is consistent. When fusing feedback data, interpolation or extrapolation is performed based on timestamps to avoid state estimation errors caused by time misalignment. Ultimately, this achieves tight integration of hardware and software, ensuring the real-time performance and consistency of positioning, planning, and control, allowing the system to better adapt to dynamically changing driving environments.

[0078] Suppose that n identical sensors are used in a certain environment, and the measured values ​​of the sensors are as follows: , … And they are independent of each other, the true value to be estimated is The variances of each sensor are as follows: , ,… The weight is , , ..., ,and =1.

[0079] The total variance is shown below: , Since the sensors are located in different positions within the detection environment, their data can be approximated as independent. Therefore, the following formula applies: , , As shown in the above equation, the total variance is a multivariate quadratic function of the weighting coefficients, and therefore has a minimum value. According to the extremum theory of multivariate functions, the weighting coefficients corresponding to the minimum total variance are: , At this point, the minimum total variance is , Result in fusion value: , In the formula The average of k historical data measurements for each sensor.

[0080] The above inventions are merely a few specific embodiments of the present invention. However, the embodiments of the present invention are not limited thereto, and any variations that can be conceived by those skilled in the art should fall within the protection scope of the present invention.

Claims

1. An intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion, characterized in that, include: The sensor module is configured to collect multi-source heterogeneous data of the environment surrounding the unmanned vehicle. The sensor module includes a GPS / IMU integrated navigation unit, a lidar, a high-definition camera, and a millimeter-wave radar. The data acquisition and fusion module is configured to preprocess the multi-source heterogeneous data, and use the extended Kalman filter algorithm to perform spatiotemporal synchronization and fusion of the preprocessed multi-source data, and output the fused environmental data. The environment perception module is configured to process the image data and point cloud data in the fused environment data based on deep learning algorithms, identify road boundaries, obstacles and traffic signs in the environment, and construct a dynamic perception model of the environment around the vehicle, and output semantic information through the model. The high-precision map building module is configured to build and update a 3D map containing semantic labels in real time based on SLAM technology and the aforementioned semantic information. The positioning algorithm module is configured to calculate visual odometry data based on the image data collected by the high-definition camera, and fuse the visual odometry data, the fused environmental data and the three-dimensional map, and calculate the real-time position and attitude of the unmanned vehicle in the global coordinate system through an optimization algorithm; The control module is configured to perform path planning and generate control commands based on the real-time position and attitude, the dynamic perception model, and the 3D map, driving the unmanned vehicle to perform autonomous navigation and obstacle avoidance actions.

2. The intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion as described in claim 1, characterized in that, The data acquisition and fusion module includes: The data preprocessing unit is configured to clean, denoise, and convert the format of the multi-source heterogeneous data, remove outliers, and unify the data format. The data preprocessing unit employs a combination of statistical filtering and radius filtering to remove outliers from the lidar point cloud data: statistical filtering is based on the statistical distribution of the average distance between a point and its k nearest neighbors, where the average neighborhood distance of a point is... satisfy ,in, The global mean. Standard deviation If the value is a multiple of the standard deviation, the point is identified as an outlier and removed; radius filtering removes isolated points whose number of neighboring points within a preset search radius r is less than a preset threshold Nmin. Gross errors are removed from GPS data using the 3σ criterion based on a sliding window. When the deviation of newly acquired GPS data from the mean within the window exceeds three times the standard deviation, it is judged as a gross error and discarded. IMU data is then used for interpolation to fill the gaps. An adaptive denoising method based on wavelet transform is used for high-definition camera image data, and Bayesian threshold shrinkage is employed. Distinguish between image edges and noise, perform thresholding on high-frequency component coefficients, and reconstruct the image; whereby... It is the noise standard deviation. It is the signal standard deviation; The data fusion unit, connected to the data preprocessing unit, is configured to fuse preprocessed GPS data, IMU data, LiDAR point cloud data, and camera image data using an extended Kalman filter algorithm, specifically including: The initialization subunit is configured to set an initial state vector and an initial covariance matrix. The state vector includes three-dimensional position, three-dimensional velocity, quaternion attitude, accelerometer zero bias, and gyroscope zero bias. The prediction subunit is configured to predict the current state estimate and covariance matrix based on the acceleration and angular velocity measured by the IMU using a kinematic model. The update subunit is configured to calculate the Kalman gain based on the sensor measurement data at the current moment, and to correct the predicted state estimate and covariance matrix. An adaptive adjustment subunit is configured to dynamically adjust the measurement noise covariance matrix according to GPS signal quality indicators, including the horizontal accuracy factor, the number of satellites participating in positioning, and the carrier-to-noise ratio. When the GPS signal is good, a smaller measurement noise covariance is set; when the GPS signal is weak, the measurement noise covariance is adaptively increased; and when the GPS signal fails, the GPS update channel is disabled and the process noise covariance is increased. The iterative sub-unit is configured to take the corrected state estimate and covariance matrix as input for the next time step and perform prediction and update operations in a loop.

3. The intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion as described in claim 1, characterized in that, The positioning algorithm module includes: The visual odometry unit is configured to receive continuous images captured by the high-definition camera, extract image feature points through the ORB algorithm, match feature points of adjacent frames, and calculate the relative displacement and attitude change of the unmanned vehicle. The fusion positioning unit is connected to the visual odometry calculation unit, the data acquisition and fusion module, and the high-precision map construction module, respectively. It is configured to employ a covariance adaptive weighted fusion method, integrating the global position reference provided by the fused environmental data, the local displacement reference output by the visual odometry calculation unit, and the positioning benchmark provided by the 3D map. Based on the current uncertainty of each information source, it calculates a weight matrix Wi=Pi. - ¹, of which The corresponding covariance matrix is... The weight matrix is ​​used to calculate the global position and attitude of the autonomous vehicle through weighted fusion.

4. The intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion as described in claim 1, characterized in that, The control module is further configured to feed back the actual execution state of the vehicle execution unit to the positioning algorithm module. The positioning algorithm module dynamically adjusts the filtering parameters or optimizes the constraints in the algorithm based on the actual execution state. The actual execution state fed back by the control module includes: The vehicle's actual steering angle, actual acceleration, actual deceleration, wheel speed, and yaw rate; The positioning algorithm module makes at least one of the following dynamic adjustments based on the actual execution state: A vehicle kinematics model is constructed based on the actual steering angle and vehicle speed, serving as an independent motion prediction source, and then weighted and fused with the IMU prediction. Based on the residual between the actual execution status and the control command, determine whether the execution system is faulty, and switch the positioning algorithm to conservative mode when a fault is detected; The process noise covariance matrix of the extended Kalman filter is dynamically adjusted based on the deviation statistics between the actual execution status and the model prediction. Based on the actual vehicle speed and yaw rate, the feature point extraction threshold and matching search range of the visual odometer are dynamically adjusted.

5. The intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion as described in claim 4, characterized in that, The positioning algorithm module adopts a graph optimization-based positioning framework. The desired control quantity output by the control module is added as a control factor to the optimization problem. Control nodes are added to the factor graph to constrain the transformation relationship between states at adjacent time points. The error term is represented as: ， in To control the desired pose change corresponding to the control command, The vehicle condition to be estimated. To control uncertainty.

6. The intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion as described in claim 1, characterized in that, The environment sensing module includes: The data input unit is configured to receive fused environmental data from the data acquisition and fusion module, and filter out camera image data and lidar point cloud data. The data processing unit is configured to process the camera image data and lidar point cloud data based on deep learning, use YOLOv8 to achieve 2D target detection, use PointPillars to achieve 3D target detection, use BEVFormer to achieve drivable area and lane line detection, use SORT+DeepSORT to achieve multi-target tracking, use Social-LSTM to achieve target motion prediction, and integrate the position, attributes and motion state of various environmental elements to construct a dynamic perception model of the vehicle's surrounding environment. The system automatically classifies dynamic and static elements based on semantic categories. Lane lines and traffic signs are classified as static elements, while pedestrians and vehicles are classified as dynamic elements. Kalman filtering is used to continuously track the speed changes of dynamic targets.

7. The intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion as described in claim 1, characterized in that, The high-precision map building module includes: The point cloud registration subunit is configured to use the iterative nearest point algorithm to register point cloud data of consecutive frames, estimate the pose of the unmanned vehicle, and adjust the point cloud alignment. The semantic information fusion subunit is configured to back-project the image semantic segmentation results output by the environment perception module to the 3D point cloud space through the joint calibration parameters of the camera and the lidar, attach semantic labels to each point cloud, and use the semantic information to assist SLAM front-end feature matching and semantic map construction. The global optimization subunit is configured to use a bundle adjustment algorithm to perform global optimization of the map. In feature matching, features with the same semantic label are prioritized, and the element status is determined based on the semantic label and the tracking algorithm, thus providing environmental constraints for localization.

8. The intelligent unmanned vehicle environmental perception and positioning system based on multi-sensor information fusion as described in claim 1, characterized in that, The data acquisition and fusion module is further configured to fuse multiple sensor data of the same type using an adaptive weighted average algorithm. The adaptive weighted average algorithm includes: Calculate the optimal weighting factor for each sensor based on the measurement variance of each sensor; Determine the weights of each sensor while minimizing the total variance; The fused sensor data value is calculated based on the weighted average of the measurements from each sensor. The weighted average algorithm is extended to a multi-dimensional adaptive rule, including four dimensions: sensor intrinsic characteristics, environmental context, data consistency, and historical statistics. Specifically, an adaptive rule based on sensor intrinsic characteristics dynamically adjusts the variance according to the GPS horizontal accuracy factor and the number of satellites; an adaptive rule based on environmental context dynamically adjusts the sensor weights according to the vehicle's current driving environment; an adaptive rule based on data consistency dynamically adjusts the variance according to the residual between the sensor measurements and the current fused values; and an adaptive rule based on historical statistics estimates the real-time statistical characteristics of the sensors using historical data within a sliding window.

9. A method for environmental perception and localization of an intelligent unmanned vehicle based on multi-sensor information fusion, applied to the system described in any one of claims 1-8, characterized in that, Includes the following steps: The sensor module collects multi-source heterogeneous data about the environment surrounding the autonomous vehicle. The multi-source heterogeneous data is preprocessed, and the extended Kalman filter algorithm is used to perform spatiotemporal synchronization and fusion of the preprocessed multi-source data to output fused environmental data. Based on deep learning algorithms, the image data and point cloud data in the fused environmental data are processed to identify road boundaries, obstacles and traffic signs in the environment, and to construct a dynamic perception model of the vehicle's surrounding environment. Based on SLAM technology, and combined with the output semantic information, a 3D map containing semantic labels is constructed and updated in real time. By fusing visual odometry data, the fused environmental data, and the 3D map, the real-time position and attitude of the unmanned vehicle in the global coordinate system are calculated using an optimized algorithm. Based on the real-time position and attitude, the dynamic perception model, and the 3D map, path planning is performed and control commands are generated to drive the unmanned vehicle to perform autonomous navigation and obstacle avoidance actions.

Images