ROS-based self-adaptive illumination fusion four-wheeler autonomous navigation system and method

Abstract

The invention discloses a self-adaptive illumination fusion four-wheeler autonomous navigation system and method based on ROS. The system comprises a hardware platform, a sensing layer, a decision-making layer and an execution layer, wherein the hardware platform comprises a four-wheel drive vehicle body and various sensors; the sensing layer executes time synchronization for data acquired by a sensor, and calculates a dynamic fusion weight of each kind of sensing data according to illumination and environment so as to obtain a fusion pose; the decision-making layer updates a semantic grid map by adding light adaptive loopback detection and satellite positioning constraints based on the fusion pose, and obtains an optimized prediction trajectory; and the execution layer realizes navigation of a four-wheel drive vehicle body according to the optimized prediction trajectory, monitors a trajectory tracking error in real time, and feeds back the trajectory tracking error to the decision-making layer for local re-planning if the trajectory tracking error deviates from a set threshold value. According to the method, the inspection path can be optimized according to the environment dynamic state and the task requirement, and the method has the advantages of high robustness, wide adaptability and high efficiency.

Description

ROS-based adaptive illumination fusion autonomous navigation system and method for four-wheeled vehicles Technical Field

[0001] This invention relates to the fields of intelligent transportation and robot navigation technology, and more specifically, to an adaptive illumination fusion-based autonomous navigation system and method for four-wheeled vehicles based on ROS (Robot Operating System). Background Technology

[0002] With the development of autonomous navigation technology, ground mobile platforms such as four-wheeled vehicles are increasingly used in outdoor environments, such as logistics delivery and inspection operations. However, existing outdoor navigation systems still face many challenges. For example, sensor data lacks robustness under different lighting conditions; in strong or low light environments, visual sensors are easily interfered with, leading to decreased positioning accuracy. Furthermore, path planning struggles to simultaneously handle multi-objective optimization of static and dynamic obstacles, especially in unstructured outdoor terrain (such as grass or gravel roads), where semantic recognition of drivable road areas is inaccurate. In addition, in multi-target point inspection tasks, balancing energy consumption and time efficiency is difficult, and there is a lack of dynamic adjustment mechanisms for sensor fusion parameters.

[0003] In the prior art, patent application CN112214019A discloses a multi-sensor fusion control system for unmanned inspection equipment, emphasizing seamless indoor-outdoor switching and FPGA synchronization. However, it mainly targets high-precision map dependence for fixed inspection paths and does not address adaptive lighting adjustment or sequence optimization for multi-target inspection. Patent application CN106774313A describes an AGV obstacle avoidance navigation method based on LiDAR and cameras, using Kalman filtering for direction correction, but path planning is limited to the Floyd algorithm and does not integrate millimeter-wave radar or semantic segmentation for road classification. Patent application CN109490825A focuses on the fusion localization of LiDAR and UWB, relying on pre-built tags, and is suitable for indoor logistics rather than open outdoor environments. Patent application CN110622213A proposes a deep learning localization and segmentation framework based on 3D semantic maps, but relies on a monocular camera and pre-built maps, lacking real-time SLAM customization and dynamic multi-sensor fusion.

[0004] In summary, existing solutions are usually limited to single environmental adaptation, static fusion strategies or specific sensor combinations, which cannot effectively address the sensor noise amplification problem caused by changes in outdoor lighting, and lack a multi-target inspection mechanism for energy optimization. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and provide a four-wheeled vehicle autonomous navigation system and method based on ROS adaptive illumination fusion.

[0006] According to a first aspect of the present invention, an adaptive illumination fusion-based autonomous navigation system for four-wheeled vehicles based on ROS is provided. The system includes a hardware platform, a perception layer, a decision layer, and an execution layer. The hardware platform comprises a four-wheel drive vehicle body and various sensors, including a lidar sensor, a vision sensor, a millimeter-wave radar sensor, a satellite positioning module, an inertial measurement unit, and an ultrasonic sensor. The perception layer performs time synchronization on the data collected by the various sensors and calculates a dynamic fusion weight for each type of sensor data based on the monitored illumination and environment, thereby obtaining a fused pose based on the dynamic fusion weight. The decision layer, based on the fused pose, constructs and updates a semantic grid map in real time by incorporating illumination-adaptive loop closure detection and satellite positioning constraints to achieve target recognition and trajectory prediction, obtaining an optimized predicted trajectory. The decision layer also monitors the positioning error and recognition confidence in real time, and if the error falls below a set threshold, it feeds back to the perception layer to adjust the dynamic fusion weight. The execution layer implements navigation of the four-wheel drive vehicle body based on the optimized predicted trajectory and monitors the trajectory tracking error in real time. If the error deviates from the set threshold, it feeds back to the decision layer for local replanning.

[0007] According to a second aspect of the present invention, a method for autonomous navigation of a four-wheeled vehicle based on ROS adaptive illumination fusion is provided. The method includes: constructing a four-wheel drive vehicle body and setting up multiple sensors, including a lidar sensor, a vision sensor, a millimeter-wave radar sensor, a satellite positioning module, an inertial measurement unit, and an ultrasonic sensor; performing time synchronization on the data collected by the multiple sensors, and calculating a dynamic fusion weight for each type of sensor data based on the monitored illumination and environment, thereby obtaining a fused pose based on the dynamic fusion weight; based on the fused pose, constructing and updating a semantic grid map in real time by incorporating illumination-adaptive loop closure detection and satellite positioning constraints, achieving target recognition and trajectory prediction, obtaining an optimized predicted trajectory, and monitoring the positioning error and recognition confidence in real time, adjusting the dynamic fusion weight if it falls below a set threshold; and realizing navigation of the four-wheel drive vehicle body according to the optimized predicted trajectory, monitoring the trajectory tracking error in real time, and performing local replanning if it deviates from the set threshold.

[0008] Compared with existing technologies, the advantages of this invention lie in its ROS-based adaptive illumination fusion-based autonomous navigation system for four-wheeled vehicles. This system employs an innovative adaptive illumination fusion mechanism, integrating a self-developed semantic and target recognition module on top of an open-source SLAM algorithm. This enables efficient multi-target inspection and dynamic obstacle avoidance, while avoiding the fixed path dependence and non-adaptive fusion limitations of existing solutions. By constructing a modular and scalable ROS-based hardware and software system framework, this invention significantly improves navigation stability and reliability in drastically changing lighting environments. Furthermore, it integrates deep semantic information (drivable areas, key object identification) in real-time and tightly into the core processes of SLAM localization, mapping, and odometry estimation, enhancing the accuracy of environmental understanding and the intelligence of navigation decisions. In summary, this invention achieves efficient and flexible multi-target inspection functionality, capable of optimizing inspection paths based on environmental dynamics and task requirements, and possesses advantages such as strong robustness, wide adaptability, and high efficiency.

[0009] Other features and advantages of the invention will become clear from the following detailed description of exemplary embodiments of the invention with reference to the accompanying drawings. Attached Figure Description

[0010] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments of the invention and, together with their description, serve to explain the principles of the invention.

[0011] Figure 1 is an overall architecture diagram of a four-wheeled vehicle autonomous navigation system based on ROS adaptive illumination fusion according to an embodiment of the present invention; Figure 2 is a sensor arrangement diagram according to an embodiment of the present invention; Figure 3 is an overall workflow diagram of a four-wheeled vehicle autonomous navigation system based on ROS adaptive illumination fusion according to an embodiment of the present invention; Figure 4 is a sub-flowchart of illumination adaptive fusion according to an embodiment of the present invention; Figure 5 is a sub-flowchart of exception handling according to an embodiment of the present invention; Figure 6 is a semantic segmentation diagram according to an embodiment of the present invention; Figure 7 is a target recognition diagram according to an embodiment of the present invention; Figure 8 is a path optimization diagram according to an embodiment of the present invention. Detailed Implementation

[0012] Various exemplary embodiments of the present invention will now be described in detail with reference to the accompanying drawings. It should be noted that, unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of the invention.

[0013] The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the invention or its application or use.

[0014] Techniques, methods, and equipment known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and equipment should be considered part of the specification.

[0015] In all the examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values.

[0016] It should be noted that similar labels and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be discussed further in subsequent figures.

[0017] Existing solutions often ignore the impact of lighting on visual sensors (such as RGBD cameras), leading to distortion of depth information and increased positioning errors (up to 1-2m) in dawn, dusk, or rainy weather. Semantic recognition modules typically rely on pre-trained models and lack customized optimization for unstructured outdoor surfaces (such as dirt roads and grass boundaries), resulting in recognition accuracy below 85%. Multi-target inspection path planning lacks energy consumption models, making efficient sequence optimization impossible on four-wheeled vehicle platforms with limited batteries. Dynamic obstacle avoidance only handles single-type obstacles (e.g., static or dynamic only), neglecting trajectory prediction and fusion in mixed scenarios. This invention addresses the problems of unstable sensor fusion, low road surface semantic recognition accuracy, and insufficient multi-target inspection path optimization in existing outdoor navigation systems under varying lighting conditions, providing a robust and adaptable four-wheeled vehicle navigation solution.

[0018] Referring to Figure 1, the ROS-based adaptive illumination fusion multi-sensor four-wheeled vehicle outdoor autonomous navigation system provided by this invention generally includes modules such as a hardware platform, a perception layer, a decision layer, and an execution layer. This modular design facilitates expansion and debugging. This system emphasizes dynamic fusion of multiple sensors and adaptive illumination, avoiding excessive dependence on specific hardware. The overall architecture is connected through the ROS topic / service mechanism, with each layer node operating independently and supporting distributed computing.

[0019] The hardware platform includes a self-made four-wheel drive vehicle body and various types of sensors (or sensor arrays). For example, the four-wheel drive vehicle body has a wheelbase of 0.6m, a load capacity of 40kg, and a chassis height of 0.1m-0.15m to adapt to uneven outdoor paved terrain. It is equipped with ROS-compatible computing units (such as NVIDIA Jetson series or similar, GPU acceleration to support real-time AI inference, memory >16GB, storage >512GB SSD).

[0020] As shown in Figure 2, the sensor array includes a multi-line lidar, a vision sensor (such as an RGBD camera), a millimeter-wave radar, a satellite positioning module, an IMU (inertial measurement unit) sensor, and an ultrasonic sensor.

[0021] Multi-line LiDAR (e.g., 128 lines, 360° scan, 12m range) is used to generate high-density 3D point clouds, supporting real-time mapping and long-range obstacle detection. Noise suppression is achieved through built-in filters, resulting in point cloud density >100k points / scan.

[0022] The visual sensor can employ an RGBD camera (binocular camera, depth resolution 1280x720@30fps) for visual semantic segmentation, illumination monitoring, and near-field depth perception. An IR projector is supported to enhance low-light depth accuracy.

[0023] A millimeter-wave radar (77GHz, FMCW modulation, 4D imaging capability, including range, velocity, angle, and elevation, detection range 150-200m, velocity resolution 0.1m / s) is used for dynamic target tracking and velocity estimation in rainy and foggy weather, avoiding the proprietary fusion algorithms of existing 4D radars. The array antenna supports multi-target resolution, and MTI filtering removes static clutter.

[0024] Satellite positioning modules (such as GPS / BeiDou dual-mode, RTK differential positioning accuracy <5cm, supporting anti-multipath interference) are used for global positioning initialization and outdoor open area correction, and can be integrated with differential base station interfaces.

[0025] IMU sensors (9-axis, sampling rate 200-500Hz, accelerometer range ±16g, gyroscope range ±2000° / s, magnetometer calibrated) are used for attitude estimation and short-term inertial navigation, compensating for weak satellite signal regions. Zero-bias stability <0.01° / s.

[0026] Ultrasonic sensors (8 channels, arrayed at the bottom of the vehicle body, front, rear, left and right, detection range 0.2-6m, angle coverage 120°-180°) are used for low-speed, close-range obstacle avoidance, such as parking spaces or narrow passages, and can support beamforming to reduce cross-interference.

[0027] The perception layer, based on a ROS node architecture, is used to realize multi-sensor data acquisition, preprocessing, and fusion. Unlike existing solutions that utilize FPGA synchronization, the perception layer in this invention can use software timestamp alignment to reduce hardware costs. The nodes within the perception layer include acquisition nodes, data synchronization and preprocessing nodes (or data synchronization and preprocessing modules), illumination monitoring nodes (or illumination monitoring modules), and sensor fusion nodes (or sensor fusion modules), primarily implementing functions such as data synchronization and preprocessing, illumination and environmental monitoring, and dynamic weighted EKF (Extended Kalman Filter) fusion.

[0028] Data synchronization and preprocessing nodes: Utilizing the ROS message_filters library and approximate timepolicy, time alignment of multi-sensor data is achieved (latency <10ms, synchronization error <5ms). Preprocessing includes: point cloud denoising (voxel grid filter, voxel size 0.05-0.1m, combined with outlier removal); image distortion correction (RGBD intrinsic parameter calibration, using OpenCV undistort); and noise estimation (real-time calculation of the data covariance matrix for each sensor). For example, LiDAR uses point cloud density variance, while RGBD uses a depth noise model; data compression (point cloud downsampling to 50kpoints, image resizing to 640x480).

[0029] Light monitoring module: The RGBD camera extracts image brightness statistics in real time (calculating the mean μ, variance σ, and peak distribution after histogram equalization; threshold classification: strong light μ>180 lux and σ<50, low light μ<60 lux and σ>100, normal, and others; simultaneously calculating HSV channel saturation to detect overexposure / underexposure). Environmental monitoring is extended to rain and fog (judged by millimeter-wave reflection intensity threshold >0.5dB) and dust (LiDAR point cloud sparsity >20%). The monitoring frequency is 30Hz, and the output is an environmental status topic.

[0030] Sensor fusion module: Employs an Extended Kalman Filter (EKF) framework for state estimation (15-dimensional state vector: position x / y / z, velocity vx / vy / vz, attitude roll / pitch / yaw, quaternion q, bias ba / bg). Dynamic adjustment of fusion weights is achieved through a rule-based basis and a learned model, where the fusion formula is extended as follows: (Prediction Step) (1) (Update step) (2) Wherein, This represents the posterior state estimate of the vector at time k; This represents the prior state prediction at time k, and the temporary state predicted based on time k-1. This represents the state transition matrix, describing how the state evolves from time k-1 to time k; Indicates process noise and measurement noise; The observation matrix represents the state mapping to the measurement space; The Kalman gain determines the weights of the predicted and measured values. For weighted observation, Weight From the illumination function Determine (I represents the illumination level normalized from 0 to 1, For noise covariance, For environmental factors. For example: rain / fog e_radar=1.5; using sigmoid activation: Parameters a / b / c are trained offline. Example: In low light (I=0.2), w_rgbd=0.15, w_lidar=0.3, w_mmwave=0.35, w_imu=0.2, w_gps=0.05, w_ultrasonic=0.05 (total=1); In high light (I=0.8), w_rgbd=0.4, w_lidar=0.35, w_mmwave=0.1, w_imu=0.1, w_gps=0.04, w_ultrasonic=0.01. The fusion period is 5-10Hz, supports online parameter tuning (ROS dynamic_reconfigure), and outputs the fusion covariance. Used for downstream uncertainty propagation.

[0031] Decision Layer: The decision layer is the core of the algorithm and can be customized based on open-source SLAM such as Cartographer or LIO-SAM. The decision layer integrates deep learning modules and optimization planning. Nodes within the decision layer include a customized SLAM module (SLAM node), a semantic and object recognition module (or recognition node), and a path planning and obstacle avoidance module (or path planning and obstacle avoidance node).

[0032] 1) Customized SLAM Module: Based on LIO-SAM, the front-end odometry uses tightly coupled LiDAR-IMU (feature extraction: curvature-based edge / planar points, matching using iterative nearest point ICP, adding RGBD depth constraints through projection matching of >100 feature pairs); the back-end optimization uses the Ceres solver (factor map includes IMU pre-integration factor, LiDAR matching factor, RGBD cyclic factor, GPS position factor), and adds illumination-related constraints (reducing the visual loop closure detection weight to 0.5 in low light, using SIFT instead of ORB features); map type: hybrid raster (resolution 0.05-0.2m) + point cloud (octree storage) + semantic layer, supporting online updates (loop closure threshold 0.5m, optimized window 50 frames, error control <0.3m / 100m path). Extended support for exploration mode without prior map (initial pose via GPS + IMU bootstrapping).

[0033] 2) Semantic and Object Recognition Module: A lightweight neural network can be developed independently, using ROS tf2 broadcast coordinate transformation. Semantic segmentation adopts MobileNetV3 backbone + DeepLabV3 decoder (parameters <5M, supports Jetson real-time inference @15-20fps, input RGBD fused image (RGB + depth channel), output multi-class labels: drivable road surface (asphalt / gravel / dirt, confidence >0.75), non-road surface (grass / water / mud, confidence >0.8), others (sky / vegetation / unknown); training uses AdamW optimizer, loss function CE+Dice, dataset Cityscapes + custom outdoor >20k annotations). Target recognition used a YOLOv8 nano variant (mAP > 0.85 @ IoU 0.5, training dataset COCO + custom outdoor scene > 15k samples, detection categories: pedestrians (bbox + velocity < 3m / s, fused with millimeter-wave radar), vehicles (bbox + velocity > 5m / s), buildings (static polygons, area > 10m²); post-processing included NMS threshold of 0.4, and trajectory association used the Hungarian algorithm). The output was fused to a semantic map to form a dynamic occupancy grid (update rate 10-15Hz, grid includes occupancy probability + semantic label + velocity vector).

[0034] 3) The path planning and obstacle avoidance module can generate obstacle avoidance strategies through global planning and local planning.

[0035] Global Planning: The A* algorithm generates paths to the destination on a semantic raster map, incorporating a TSP variant to optimize multi-target point inspection. For example, minimizing the total distance plus energy consumption, the energy model is represented as: Where E represents total energy consumption, i.e., the total energy required for the machine to move along a path; k represents the distance correlation coefficient, which represents the energy consumption that is proportional to the distance traveled, mainly from friction, resistance, rolling resistance, or basic power consumption; d represents the distance traveled; and m represents the mass correlation coefficient. This indicates the speed at which the machine moves along the path.

[0036] Local planning: The Dynamic Window Approach (DWA) combines semantic constraints to predict dynamic obstacle trajectories (e.g., using millimeter-wave velocity data) and generate smooth trajectories.

[0037] Obstacle avoidance strategy: Static obstacles (buildings) use vector field histogram (VFH); dynamic obstacles (pedestrians / vehicles) are given priority and are confirmed by close proximity using ultrasonic waves (emergency braking when the distance is less than 1m).

[0038] 4) Illumination Adaptation: The decision layer monitors SLAM error / recognition confidence. If the error is less than the threshold (e.g., error > 0.5m or confidence < 0.7), the perception layer weights are readjusted to form a closed-loop control (feedback topic, period 2-5Hz).

[0039] Execution layer: ROS nav_msgs and cmd_vel interfaces control the four-wheel differential drive (motor encoder feedback, resolution >1000 pulses / rev), with a PID controller adjusting speed (Kp=1.5-2.0, Ki=0.1-0.2, Kd=0.5-1.0, anti-integral saturation) and steering (maximum angular velocity 1-1.5 rad / s, feedforward compensated slope). It supports fault tolerance (e.g., degrading to IMU+GPS mode when sensors fail, switching time <0.5s) and logging (ROS bag records all events for post-event analysis).

[0040] To further understand this invention, the system's workflow will be further described below with reference to the accompanying drawings. In general, after the system starts, the perception layer collects data, the decision layer generates a map and path, and the execution layer follows. Manual / automatic mode switching is supported. The system workflow emphasizes conditional branches, exception handling, and closed-loop feedback to ensure system robustness. The workflow is designed for iterative loops and supports real-time interruptions (such as user intervention).

[0041] 1. Overall Workflow The overall workflow is shown in Figure 3. After the system starts, initialization is performed first, including starting the ROS core, loading the YAML parameter file, completing the intrinsic and extrinsic parameter calibration and zero bias compensation of all sensors, and initializing to an empty raster map or loading an existing map. The entire initialization process takes no more than 10 seconds.

[0042] After initialization, the system enters the mode selection phase: if the user selects manual mode, the vehicle will be taken over directly by the remote control joystick until the end; if the user selects automatic navigation or multi-target inspection mode, the main loop will be entered.

[0043] The main loop acquires all sensor data in parallel at a frequency of 20-50Hz, including multi-line LiDAR (>100k points / frame), RGBD camera images and depth maps, 77GHz millimeter-wave radar 4D point cloud, RTK-GPS position, 9-axis IMU raw data, and 8-channel ultrasonic range array. Immediately after acquisition, ROS message_filters are used for time synchronization; if a frame is delayed by more than 50ms, it is discarded and logged.

[0044] The synchronized data first enters the illumination and environment monitoring module: the RGBD camera calculates the brightness histogram (mean μ, variance σ, HSV distribution) in real time, the millimeter-wave radar calculates the reflection intensity to determine rain and fog, and the LiDAR calculates the point cloud density to determine dust. Finally, it outputs the normalized illumination intensity I(0-1) and environmental factors. .

[0045] Next, based on the light intensity I and the noise covariance... and environmental factors The dynamic weights of each sensor are calculated using the sigmoid function. The observation noise matrix R of the EKF is updated accordingly. Then, the EKF prediction step (IMU pre-integration) and weighted update step are performed to output the high-precision fused pose / odom_fused.

[0046] The fused pose is fed into a customized LIO-SAM+RGBD SLAM module. The front end performs LiDAR-IMU-RGBD tightly coupled odometry, and the back end optimizes the factor map and adds illumination-adaptive loop closure detection and GPS constraints to build and update a semantic raster map with a resolution of 0.1m in real time.

[0047] After the semantic map is updated, the MobileNetV3+DeepLabV3 semantic segmentation network and the YOLOv8-nano object detection network are run simultaneously to output drivable / non-road masks as well as bounding boxes and velocity estimates for pedestrians, vehicles, and buildings. The results are then projected onto the grid map to form a dynamically occupied grid.

[0048] The path planning branch is entered based on the task type: Single-target navigation: A* global planning with energy cost is executed directly, and then handed over to the DWA local trajectory sampler; Multi-target inspection: Genetic algorithm with energy constraints is run first to solve the optimal access sequence of TSP, and then A*+DWA is executed for each segment in sequence.

[0049] The candidate trajectories generated by DWA are sent to the obstacle avoidance decision module: for static obstacles, the improved VFH+ algorithm is used to generate candidate directions, and for dynamic obstacles, the constant velocity / acceleration model is used to predict the future position in 5-10 seconds and calculate the collision time TTI. If the TTI is less than 5 seconds, priority is given to yielding or detouring. When the distance is less than 1m, ultrasonic waves are fused to trigger emergency braking.

[0050] The optimal trajectory after obstacle avoidance is smoothed using a Bezier curve (control point fitting to ensure curvature < 0.4), and finally output to the PID controller of the execution layer to generate cmd_vel instructions to drive the four-wheel differential chassis, while adding slope feedforward compensation.

[0051] Throughout the process, the execution layer monitors the trajectory tracking error in real time. If the deviation is greater than 0.2m, it immediately triggers local replanning. The decision layer monitors the positioning error and recognition confidence in real time. If the error is lower than the threshold, it feeds back to the perception layer to adjust the fusion weights, forming a complete closed loop.

[0052] When the vehicle is less than 0.5m away from the current target point, it is determined that it has arrived and logs such as the time, energy consumption, and mileage of this route are recorded. If it is a multi-target inspection, it will automatically switch to the next target point in the sequence. After all targets are completed, the map is saved and the vehicle stops safely.

[0053] At any stage of the main loop, once an anomaly is detected (sensor failure, fusion divergence, map drift, persistent path blockage, etc.), the anomaly handling sub-process is immediately initiated (see the anomaly handling sub-process below for details), and the loop returns after the anomaly is handled.

[0054] 2. Illumination Adaptive Fusion Sub-Flowchart 4 is the illumination adaptive fusion sub-flowchart.

[0055] For each frame of RGBD image (30Hz), a luminance histogram is first calculated to obtain the luminance mean μ, variance σ, and HSV distribution. The normalized illumination intensity I (0-1) is then output. Simultaneously, environmental factors are obtained by combining millimeter-wave reflection intensity and LiDAR point cloud density. .

[0056] Based on the light intensity I, the rule branch is as follows: Low light (μ<60 or I<0.3): Significantly reduce the RGBD weight to 0.15, increase the millimeter wave weight to 0.35, LiDAR to 0.3, and IMU to 0.2, while enabling infrared auxiliary projection of the RGBD and increasing the LiDAR scan density by 20%; Strong light (μ>180 or I>0.7): Significantly increase the RGBD weight to 0.4 and LiDAR to 0.35, reduce the millimeter wave weight to 0.1, while enabling the HDR mode and automatic exposure compensation of the RGBD; Normal light: Use a balanced weight (approximately 0.143), and then adjust according to real-time noise. Fine-tuning is required; inclement weather conditions such as rain, fog, or dust: additional environmental factors are added. This further increases the weight of corresponding high-penetration sensors (e.g., millimeter wave weight +0.15 in rain and fog).

[0057] After all weights are calculated, they are normalized. ), and dynamically update the observation noise matrix R of EKF accordingly (R_ii is inversely proportional to the weight).

[0058] The standard EKF iteration then begins: Prediction step: Pre-integration is performed using raw IMU data to update the state transition matrix F and process noise Q; Update step: Observations from each sensor are updated. Based on the latest weighted fusion Calculate Kalman gain Then, state and covariance corrections are performed.

[0059] If the covariance matrix If divergence (trace exceeds the threshold of 2m²) or matrix anomalies occur, additional process noise will be automatically injected or part of the state will be reset to ensure fusion stability.

[0060] Finally, a high-precision fused pose / odom_fused model is released for use by the decision-making layer. If the decision-making layer reports a large positioning error, the a / b / c parameters of the sigmoid function are fine-tuned online (step size ±0.01) to achieve bidirectional closed-loop adaptive perception-decision.

[0061] 3. Exception handling sub-flowchart 5 is the exception handling sub-flowchart.

[0062] The system monitors abnormal indicators in real time at every stage of data acquisition, fusion, mapping, planning, and execution, including sensor data rate / numerical range and fusion covariance. SLAM drift, number of planning failures, trajectory tracking deviation, etc.

[0063] Once an anomaly is triggered, it should be immediately categorized and handled according to safety priorities: Sensor failure (data rate <5Hz or a large number of NaN): Force the weight of this sensor to 0, while increasing the weight of the backup sensor, log the information, and publish an alarm topic; Fusion divergence ( (Trace > Threshold): Immediately reset EKF state estimation and return to the most recent reliable GPS or known pose; (Severe map drift > 1m): Trigger global relocation (full map bundle adjustment, duration < 2 seconds); (Path continuously blocked or no solution planned): Backtrack to the previous safe point, expand the search space or temporarily switch to RRT* fast exploration planner; (Execution layer stuck or motor abnormal): Immediately send an emergency braking command (PWM=0), restart the PID controller, and enter charging standby mode if the battery voltage < 40%.

[0064] After anomaly handling is completed, the recovery verification phase begins: Simulate 1 second of future movement without actual movement and check if the positioning error is less than 0.3m. If verification passes, seamlessly return to the main loop; if verification fails, enter a progressive degradation mode (first disable vision → disable LiDAR → finally retain only IMU dead reconting) until the vehicle safely stops and issues an audible and visual alarm, awaiting manual intervention.

[0065] In summary, the ROS-based adaptive illumination fusion multi-sensor four-wheeled vehicle outdoor autonomous navigation system provided by this invention includes a hardware platform, a perception layer, a decision layer, and an execution layer. The perception layer monitors illumination levels using an RGBD camera and dynamically adjusts the multi-sensor fusion weights. The decision layer is based on a customized SLAM based on LIO-SAM, integrating a self-developed MobileNetV3+DeepLabV3 semantic segmentation module to achieve drivable and off-road surface recognition, as well as YOLOv8 target recognition for pedestrian, vehicle, and building detection and trajectory prediction. Path planning in this system includes multi-objective optimization combining A* global planning with the TSP genetic algorithm, and DWA local planning fused with VFH+ static obstacle avoidance and constant speed dynamic trajectory prediction. The outdoor autonomous navigation method of this system includes steps such as illumination monitoring, dynamic fusion, SLAM mapping, semantic perception, path planning, obstacle avoidance execution, and anomaly handling, supporting closed-loop feedback. The dynamic fusion in this system adopts the EKF framework, and weight adjustment considers illumination, noise, and environment, supporting rain and fog overlay adjustment.

[0066] Accordingly, this invention provides a method for autonomous navigation of a four-wheeled vehicle based on ROS-adaptive illumination fusion. The method includes: constructing a four-wheel drive vehicle body and setting up multiple sensors, including a lidar sensor, a vision sensor, a millimeter-wave radar sensor, a satellite positioning module, an inertial measurement unit, and an ultrasonic sensor; performing time synchronization on the data collected by the multiple sensors, and calculating dynamic fusion weights for each type of sensor data based on the monitored illumination and environment, thereby obtaining a fused pose based on the dynamic fusion weights; based on the fused pose, by incorporating illumination-adaptive loop closure detection and satellite positioning constraints, constructing and updating a semantic grid map in real time to achieve target recognition and trajectory prediction, obtaining an optimized predicted trajectory, and monitoring the positioning error and recognition confidence in real time, adjusting the dynamic fusion weights if they fall below a set threshold; and implementing navigation of the four-wheel drive vehicle body based on the optimized predicted trajectory, monitoring the trajectory tracking error in real time, and performing local replanning if it deviates from the set threshold.

[0067] To further verify the effectiveness of this invention, experimental verification was conducted. Figure 6 is a schematic diagram of semantic segmentation, where Figure 6(a) is an example of semantic segmentation output based on LiDAR, and Figure 6(b) is an example of semantic segmentation output based on an RGB camera. Figure 7 is a schematic diagram of target recognition. Figure 8 is a schematic diagram of path optimization. Verification shows that this invention mainly has the following advantages: adaptive illumination fusion, improving all-weather robustness (day / night switching time <0.5s, adaptability >98%); semantic-driven planning, achieving a passable area recognition accuracy >96%, reducing boundary crossing risks; multi-target inspection efficiency improved by 35% (TSP + energy optimization, total path shortened by 20%); based on ROS open source, facilitating secondary development and integration; low-cost self-made platform, suitable for small and medium-sized applications; anomaly handling mechanism, improving system reliability (fault recovery time <2s); excellent real-time performance (overall latency <100ms, suitable for speeds of 5m / s).

[0068] In summary, this invention avoids dependence on indoor / outdoor switching, instead emphasizing dynamic fusion of outdoor lighting, improving low-light accuracy by 25% (experimental error <0.25m). Unlike fixed Kalman scattering, this invention uses adaptive weights to reduce noise impact by 15%. It avoids using UWB tags, employing a pure sensor solution to reduce deployment costs by 30%. Compared to monocular frames, this invention integrates RGBD+LiDAR real-time SLAM, supporting open environments without pre-maps. Further comparing with pure LiDAR, this invention adds illumination adjustment to avoid the limitations of a single sensor; unlike fusion using 77GHz radar, this invention incorporates energy-optimized TSP to avoid path redundancy; compared to existing ROS2SLAM research, this invention uses a custom semantic module to improve road surface recognition accuracy to 96%.

[0069] This invention can be a system, method, and / or computer program product. A computer program product may include a computer-readable storage medium having computer-readable program instructions loaded thereon for causing a processor to implement various aspects of the invention.

[0070] Computer-readable storage media can be tangible devices capable of holding and storing instructions for use by an instruction execution device. Computer-readable storage media can be, for example, but not limited to, electrical storage devices, magnetic storage devices, optical storage devices, electromagnetic storage devices, semiconductor storage devices, or any suitable combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), static random access memory (SRAM), portable compact disc read-only memory (CD-ROM), digital multifunction disc (DVD), memory sticks, floppy disks, mechanical encoding devices, such as punch cards or recessed protrusions storing instructions thereon, and any suitable combination thereof. The computer-readable storage media used herein are not to be construed as transient signals themselves, such as radio waves or other freely propagating electromagnetic waves, electromagnetic waves propagating through waveguides or other transmission media (e.g., light pulses through fiber optic cables), or electrical signals transmitted through wires.

[0071] The computer-readable program instructions described herein can be downloaded from computer-readable storage media to various computing / processing devices, or downloaded via a network, such as the Internet, local area network, wide area network, and / or wireless network, to an external computer or external storage device. The network may include copper transmission cables, fiber optic transmission, wireless transmission, routers, firewalls, switches, gateway computers, and / or edge servers. A network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards them to the computer-readable storage media in the respective computing / processing device.

[0072] The computer program instructions used to perform the operations of this invention may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Smalltalk, C++, Python, etc., and conventional procedural programming languages ​​such as "C" or similar languages. The computer-readable program instructions may be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or may be connected to an external computer (e.g., via the Internet using an Internet service provider). In some embodiments, electronic circuitry, such as programmable logic circuitry, field-programmable gate arrays (FPGAs), or programmable logic arrays (PLAs), is personalized by utilizing state information from the computer-readable program instructions. This electronic circuitry can execute the computer-readable program instructions to implement various aspects of the invention.

[0073] Various aspects of the present invention are described herein with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer-readable program instructions.

[0074] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to produce a machine such that, when executed by the processor of the computer or other programmable data processing apparatus, they create means for implementing the functions / actions specified in one or more blocks of the flowchart and / or block diagram. These computer-readable program instructions can also be stored in a computer-readable storage medium that causes a computer, programmable data processing apparatus, and / or other device to operate in a particular manner; thus, the computer-readable medium storing the instructions comprises an article of manufacture that includes instructions for implementing aspects of the functions / actions specified in one or more blocks of the flowchart and / or block diagram.

[0075] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device to cause a series of operational steps to be performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to perform the functions / actions specified in one or more boxes of a flowchart and / or block diagram.

[0076] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of an instruction containing one or more executable instructions for implementing a specified logical function. In some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions. It will be known to those skilled in the art that implementation in hardware, implementation in software, and implementation using a combination of software and hardware are equivalent.

[0077] The various embodiments of the present invention have been described above. These descriptions are exemplary and not exhaustive, and are not limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments. The terminology used herein is chosen to best explain the principles, practical application, or technical improvements to the embodiments in the market, or to enable others skilled in the art to understand the embodiments disclosed herein. The scope of the invention is defined by the appended claims.

Claims

1. A ROS-based adaptive illumination fusion autonomous navigation system for four-wheeled vehicles, comprising: The system comprises a hardware platform, a perception layer, a decision-making layer, and an execution layer. The hardware platform includes a four-wheel drive vehicle and various sensors, including lidar sensors, vision sensors, millimeter-wave radar sensors, a satellite positioning module, an inertial measurement unit, and ultrasonic sensors. The perception layer performs time synchronization on data collected by these sensors and calculates dynamic fusion weights for each type of sensor data based on monitored illumination and environment, thereby obtaining a fused pose. The decision-making layer, based on the fused pose, incorporates illumination-adaptive loop closure detection and satellite positioning constraints to construct and update a semantic grid map in real time, achieving target recognition and trajectory prediction to obtain an optimized predicted trajectory. The decision-making layer also monitors positioning errors and recognition confidence in real time, and if these fall below a set threshold, it feeds back to the perception layer to adjust the dynamic fusion weights. The execution layer implements navigation for the four-wheel drive vehicle based on the optimized predicted trajectory and monitors trajectory tracking errors in real time. If the error deviates from a set threshold, it feeds back to the decision-making layer for local replanning.

2. The system according to claim 1, characterized in that, The perception layer includes a data synchronization and preprocessing module, an illumination monitoring module, and a sensor fusion module. Specifically: the data synchronization and preprocessing module performs time alignment and preprocessing of data collected by the various sensors, including point cloud denoising, image distortion correction, noise estimation, and data compression; the illumination monitoring module extracts real-time image brightness statistics to calculate the mean, variance, and peak distribution, and determines the environmental state, outputting an environmental state statement; the sensor fusion module uses an extended Kalman filter for state estimation and adjusts the dynamic fusion weights.

3. The system according to claim 2, characterized in that, The dynamic fusion weights are obtained according to the following formula: in, Let represent the posterior state estimate of the vector at time k. This represents the prior state prediction at time k. Represents the state transition matrix. Indicates measurement noise. Represents the observation matrix. For Kalman gain, For weighted observation, Weight From the illumination function Determined, I represents the illumination level normalized from 0 to 1. For noise covariance, Environmental factors.

4. The system according to claim 1, characterized in that, The decision layer includes a SLAM customization module, a semantic and target recognition module, a path planning and obstacle avoidance module, and an illumination adaptation module. Specifically: the SLAM customization module constructs and updates a semantic grid map in real time by incorporating illumination-adaptive loop closure detection and satellite positioning constraints; the semantic and target recognition module uses a semantic segmentation network and a target detection network to obtain masks for drivable or non-road surfaces, as well as bounding boxes and velocity estimates of detected targets, and projects the results onto the semantic grid map to form a dynamically occupied grid; the path planning and obstacle avoidance module enters a path planning branch based on the task type, generates candidate trajectories, and then obtains an optimized predicted trajectory after obstacle avoidance decisions, wherein the path planning branches include a single-target navigation branch and a multi-target inspection branch; the illumination adaptation module monitors SLAM and recognition confidence, and when it falls below a set threshold, feeds back to the perception layer to adjust the dynamic fusion weights.

5. The system according to claim 1, characterized in that, It also includes determining whether an anomaly is triggered by real-time monitoring of abnormal indicators, including sensor data rate, sensor data value range, fusion covariance, SLAM drift, number of planning failures, and trajectory tracking deviation.

6. The system according to claim 3, characterized in that, Weight From the illumination function Confirmed, expressed as: in, 、 c are the coefficients of the related terms.

7. The system according to claim 1, characterized in that, The execution layer utilizes a PID controller to adjust the speed and steering of the four-wheel drive vehicle and supports fault tolerance and logging.

8. A method for autonomous navigation of four-wheeled vehicles based on ROS adaptive illumination fusion, comprising: A four-wheel drive vehicle body is constructed and equipped with various sensors, including a lidar sensor, a vision sensor, a millimeter-wave radar sensor, a satellite positioning module, an inertial measurement unit, and an ultrasonic sensor. Time synchronization is performed on the data collected by these sensors, and dynamic fusion weights for each type of sensor data are calculated based on monitored illumination and environment. A fused pose is then obtained based on these dynamic fusion weights. Based on the fused pose, a semantic grid map is constructed and updated in real time by incorporating illumination-adaptive loop closure detection and satellite positioning constraints to achieve target recognition and trajectory prediction. An optimized predicted trajectory is obtained, and the positioning error and recognition confidence are monitored in real time. If the error falls below a set threshold, the dynamic fusion weights are adjusted. Navigation of the four-wheel drive vehicle is achieved based on the optimized predicted trajectory, and the trajectory tracking error is monitored in real time. If the error deviates from a set threshold, local replanning is performed.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 7.

10. A computer device comprising a memory and a processor, wherein a computer program capable of running on the processor is stored in the memory, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 7.

Images