Obstacle avoidance car control method based on multi-sensor fusion and Kalman filtering

Abstract

The application discloses a barrier-avoiding vehicle control method based on multi-sensor fusion and Kalman filtering, and particularly relates to the field of barrier-avoiding vehicle environment adaptation and control. A data acquisition module collects multi-sensor original data of the barrier-avoiding vehicle in different scenes; a Kalman filtering fusion module performs data fusion; an environment feature recognition module marks the features of the fused data; a local map construction module constructs a local map in a two-dimensional coordinate system according to different scenes; a path feasibility evaluation module differentially calculates the passing safety indexes of each node; the deviation between the actual safety indexes of each node and the reference values is calculated; and a barrier-avoiding control module generates corresponding motor control instructions. After the passing safety indexes of the nodes are calculated, the application generates different motor instructions, avoids the operation delay caused by excessive braking in the traditional fixed control strategy, or the safety hazards caused by insufficient control, and reduces unnecessary energy consumption waste and fault shutdown cost.

Description

Technical Field

[0001] This invention relates to the field of obstacle avoidance vehicle environment adaptation and control, and more specifically, to an obstacle avoidance vehicle control method based on multi-sensor fusion and Kalman filtering. Background Technology

[0002] Against the backdrop of the deep integration of artificial intelligence and intelligent equipment, obstacle avoidance vehicles have become a key carrier for automated operations, widely used in scenarios such as warehousing and logistics, indoor inspection, and home services. They can operate autonomously for 8-16 hours a day. Their environmental perception and autonomous decision-making capabilities directly determine operational efficiency and safety. The vehicles collect environmental data and motion status data through multiple sensors, forming massive and multi-dimensional scene-related information, providing a data foundation for accurately adapting to complex environments and achieving intelligent obstacle avoidance.

[0003] Currently, in the field of obstacle avoidance vehicle environment adaptation and control, the main existing technologies used include perception methods based on single sensors and control technologies based on fixed strategies. Perception methods based on single sensors usually rely on a single device such as LiDAR or a vision camera to obtain environmental information. For example, the location of obstacles can be determined solely by the distance data from LiDAR. Control technologies based on fixed strategies execute actions according to preset obstacle avoidance logic. For example, when encountering an obstacle, a fixed mode of deceleration and steering is uniformly adopted. In addition, some solutions also adapt to different environments by manually preset scene parameters and manually switching working modes.

[0004] However, it still has some shortcomings in actual use. First, the perception method based on a single sensor relies too much on the performance of a single type of device and cannot fully and reliably perceive complex environments. It judges obstacles based solely on lidar data and ignores the impact of changes in lighting on ranging accuracy. The fixed strategy control technology only executes preset logic and is difficult to cope with sudden situations in dynamic scenes, resulting in obstacle avoidance decisions being out of touch with the actual environment and failing to achieve efficient and safe autonomous operation.

[0005] Secondly, in practical applications, the working environment is dynamic and varied, affected by various factors such as lighting and obstacle types. Under such circumstances, existing technologies cannot comprehensively consider these factors and cannot achieve accurate obstacle avoidance in different scenarios. The perception requirements faced by the car in an indoor environment with light and in an underground garage without light are different, but existing technologies cannot effectively adapt to this difference, resulting in obstacle avoidance performance lagging behind the requirements of actual scenarios. Summary of the Invention

[0006] In view of this, embodiments of the present invention provide a control method for an obstacle avoidance vehicle based on multi-sensor fusion and Kalman filtering. The present invention provides the following technical solution:

[0007] S1. The data acquisition module collects raw data from multiple sensors of the obstacle avoidance vehicle in two environmental scenarios: one with light source and one without, and stores it in the local database.

[0008] S2, the Kalman filter fusion module extracts the original data of the corresponding scene from the local database, fuses the data by constructing the corresponding state equation and observation equation, and outputs the result to the environmental feature recognition module;

[0009] S3, the environmental feature recognition module marks the features of the fused data and transmits the marked data to the local map construction module;

[0010] S4. The local map construction module constructs a local map in a two-dimensional coordinate system based on the feature recognition results, with the current coordinates of the vehicle as the origin, according to different scenarios.

[0011] S5. The path feasibility assessment module combines path node data in the local map with real-time information provided by Kalman filtering to calculate the passage safety index of each node in a differentiated manner.

[0012] S6. The path deviation analysis module calculates the deviation between the actual safety index of each node and the benchmark value based on the preset differentiated safety threshold.

[0013] S7 The obstacle avoidance control module generates corresponding motor control commands based on the deviation indicators under different scenarios to achieve differentiated obstacle avoidance control.

[0014] The technical effects and advantages of this invention are as follows:

[0015] 1. This invention dynamically adapts to light-source and non-light-source scenarios through a data acquisition module, and constructs state equations and observation equations for different scenarios by combining a Kalman filter fusion module. It is not limited to the traditional single sensor perception and general filtering algorithm that simply overlays data. On the one hand, it can achieve deep matching between environmental data and scene characteristics. On the other hand, it can avoid the data redundancy or missing problems caused by ignoring scene differences in traditional solutions, thereby improving the reliability and accuracy of environmental perception results.

[0016] 2. This invention generates the optimal path based on the high passability safety index (PSI) nodes output by the path feasibility assessment module. The matched path can be optimized by the local map construction module through scene-specific grid modeling, improving the map's adaptability to the actual environment and the efficiency of path planning. The scenario-based commands of the obstacle avoidance control module achieve a balance between risk and efficiency, prioritizing the execution of control strategies corresponding to high PSI paths to ensure that the vehicle follows the safest and most efficient motion logic in different scenarios, thereby reducing collision risks and improving work efficiency. At the same time, when the environmental scene changes abruptly, the safety threshold can be adjusted by the sensor switching and path deviation analysis module triggered by the photosensitive sensor, quickly adapting to the obstacle avoidance requirements of the new scene, saving the time and cost of manual intervention, and improving the autonomous adaptability in complex environments. On the other hand, since traditional fixed control strategies are prone to problems such as untimely braking or path redundancy when switching scenes, this invention allows the vehicle to respond more flexibly to changes in lighting and sudden obstacle situations, thereby improving the flexibility of obstacle avoidance decisions, increasing continuous operation time and task completion rate.

[0017] 3. After the environmental feature recognition module completes the obstacle semantic labeling and the path feasibility assessment module calculates the node PSI, the present invention performs an evaluation. As a result, the obstacle avoidance control module generates differentiated motor commands to achieve high-precision grasp of the strength of obstacle avoidance strategies in different scenarios. This avoids the operation delay caused by excessive braking or the safety hazards caused by insufficient control in traditional fixed control strategies, and reduces unnecessary energy waste and downtime costs. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the overall structure of the present invention.

[0019] Figure 2 This is a flowchart illustrating the overall structure of the present invention. Detailed Implementation

[0020] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0021] As attached Figure 1 The obstacle avoidance vehicle control method based on multi-sensor fusion and Kalman filtering shown includes a data acquisition module, a local database, a Kalman filter fusion module, an environmental feature recognition module, a local map construction module, a path feasibility evaluation module, a path deviation analysis module, and an obstacle avoidance control module.

[0022] The data acquisition module is used to collect raw data from multiple sensors of the obstacle avoidance vehicle in both light-emitting and light-free environments, and store it in a local database.

[0023] The local database is used to store the raw data from multiple sensors of the obstacle avoidance vehicle in both light-emitting and light-free environments.

[0024] The Kalman filter fusion module is used to construct the corresponding state equation and observation equation for data fusion.

[0025] The environmental feature recognition module is used to label the features of the fused data;

[0026] The local map construction module is used to construct local maps in a two-dimensional coordinate system according to different scenarios;

[0027] The path feasibility assessment module is used to calculate the passage safety index of each node in a differentiated manner.

[0028] The path deviation analysis module is used to calculate the deviation between the actual safety index of each node and the benchmark value;

[0029] The obstacle avoidance control module is used to generate corresponding motor control commands to achieve differentiated obstacle avoidance control.

[0030] As attached Figure 2 The obstacle avoidance vehicle control method based on multi-sensor fusion and Kalman filtering shown includes the following steps in its specific implementation:

[0031] S1. The data acquisition module collects raw data from multiple sensors of the obstacle avoidance vehicle in two environmental scenarios: one with light source and one without, and stores it in the local database.

[0032] As a preferred feasible embodiment, it should be explained that "having a light source" refers to a scene where there is a stable natural or artificial light source that can be identified by the vehicle's sensors, and the light intensity meets the normal working requirements of the visual and optical sensors carried by the obstacle avoidance vehicle, so that basic environmental perception can be achieved without relying on pure distance sensors; "having no light source" refers to a scene where there is no natural or artificial light source, or the light intensity is extremely low and cannot support the operation of visual / optical sensors, in which case the obstacle avoidance vehicle must rely entirely on pure distance sensors to achieve environmental perception.

[0033] It should be explained that the raw data from the multi-sensor system includes: obstacle distance data: the straight-line distance between obstacles and the vehicle from different directions; obstacle contour data: the geometric shape and edge features of the obstacles; vehicle motion state data: real-time speed, steering angle, and acceleration; and ambient light intensity data: the scene illumination intensity value.

[0034] The data acquisition method is as follows: In scenes with light sources, a visual camera is installed at the center of the front of the vehicle, with the lens facing horizontally forward and a 60° field of view. It is responsible for acquiring the color, texture, and outline details of obstacles within a range of 1-5m in front, and outputting RGB images with a resolution of 1280×720. A lidar is installed at the center of the top of the vehicle, scanning horizontally 360° with a detection radius of 0.1-10m, covering the distance and outline information of obstacles around the vehicle in all directions. Two speed encoders are integrated into the left and right drive wheel axles of the vehicle, respectively, to record the wheel speed in real time and calculate the vehicle's real-time speed and steering angle through differential speed. A photosensitive sensor is installed next to the visual camera, with a sampling frequency of 10Hz, and outputs the light intensity value in real time as the basis for scene switching.

[0035] In the absence of light, ultrasonic ranging data is used for close-range operations, while millimeter-wave radar data is used for medium- and long-range operations. Five HC-SR04 ultrasonic sensors are arranged in a fan shape at the front of the vehicle, with a detection range of 0.02-4m, a response time of ≤50ms, and covering a 120° fan-shaped area in front. Ranging is triggered synchronously every 50ms, focusing on monitoring obstacles at close range. The millimeter-wave radar is installed in the center of the vehicle's bumper, with a horizontal detection angle of 90° and a detection distance of 0.5-30m, supplementing the data for obstacles at medium and long ranges and compensating for blind spots of the ultrasonic sensors. The speed encoder and photosensitive sensor are used in scenarios with light sources.

[0036] It needs further explanation that the sensor is installed and fixed using a 3D-printed custom bracket, which is connected to the chassis and body of the car with M3 screws to ensure that the sensor detection direction is unobstructed; all sensors are connected to the STM32 main control board via DuPont wires, and the power supply is uniformly provided by the car's 12V lithium battery through a voltage regulator module to ensure the stability of power supply during synchronous data acquisition.

[0037] It needs further explanation that all the above sensors are installed on the same obstacle avoidance vehicle, which relies on photosensitive sensors to detect light intensity in real time. The trigger threshold is set to 200 lux. If the light intensity is lower than this value, it is determined to be a scene with no light source. The working state of the sensors is dynamically switched: when the light intensity is ≥200 lux, the vision camera and lidar are activated, and when the light intensity is <200 lux, the ultrasonic and millimeter-wave radar are switched. Instead of designing two separate vehicles for the two scenarios.

[0038] It should be noted that all raw data is structured and encapsulated in the format of scene type-timestamp-sensor ID, and transmitted to the database for storage via the local bus. Each data entry contains fields such as acquisition time, sensor type, raw measurement value, and data check code, which facilitates quick extraction and retrieval by subsequent modules according to scene type.

[0039] S2, the Kalman filter fusion module extracts the original data of the corresponding scene from the local database, fuses the data by constructing the corresponding state equation and observation equation, and outputs the result to the environmental feature recognition module;

[0040] As a preferred and feasible embodiment, it should be explained that the process of constructing the corresponding state equations and observation equations is as follows:

[0041] A1. Definition of state variables;

[0042] Based on the motion state and core environmental perception parameters of the obstacle avoidance vehicle, a unified state vector X(k) is defined: x and y are the position coordinates of the car in a two-dimensional coordinate system, in meters (m); θ is the car's heading angle, in rad; and v is the car's linear velocity, in meters per second (m / s). The angular velocity of the trolley is measured in rad / s. It represents the distances to obstacles in the three key directions ahead: directly in front, 30° to the left, and 30° to the right, in meters.

[0043] A2. Construct state equations for different scenarios;

[0044] The state equation describes how the state at time k evolves from the state at time k-1, and has the following form: X(k) and X(k-1) are the state vectors of the vehicle at different times, F(k) is the state transition matrix, B(k) is the control input matrix with the same dimension as the state vector X(k), used to map the control vector u(k) to the state space, u(k) is the control vector, such as the control command of the vehicle motor, W(k) is the process noise vector, and k is the discrete time step in seconds, synchronized with the sampling time interval Δt.

[0045] In scenarios with light sources, the state transition matrix Δt is the sampling time interval, typically taken as 0.1s. Since the lidar frame rate is synchronized with motion sampling, it is temporarily set to constant value transfer processing; in the absence of light source, based on the state transition matrix F(k) in the scene with light source, the following is applied: The attenuation coefficient is increased by 0.95. This coefficient is derived from fitting 100 sets of motion data of near-field obstacles in no-light scenarios, reflecting the rapid change characteristics of near-field obstacles. The state transition matrix F(k) is: In a scenario without a light source, Δt is the sampling time interval, typically set to 0.05s.

[0046] A3. Construct observation equations for different scenarios.

[0047] The observation equation describes how sensor measurements relate to state variables, and takes the form: Z(k) is the sensor measurement vector, where the velocity is in m / s, the angular velocity is in rad / s, and the distance is in m. V(k) is the observation noise vector, which follows Gaussian white noise with a mean of 0 and a variance equal to the observation noise covariance matrix R(k). H(k) is the observation matrix.

[0048] In scenarios with ambient light, the observed values ​​are derived from the omnidirectional distance measured by the lidar, the distance estimated from the contours of the visual camera, and the speed of the wheel speed encoder. , and The velocity and angular velocity are calculated for wheel speed. For distance measurement in the corresponding direction by lidar, The distance directly in front is calculated from the visual contour; the observation matrix H(k) is designed as follows: In scenarios without a light source, observations are obtained from ultrasonic waves, millimeter-wave radar, and wheel speed encoders. , It measures distance in three directions using ultrasound. For ranging directly in front of the millimeter-wave radar, the observation matrix H(k) is adjusted as follows: .

[0049] The specific steps for data fusion are as follows:

[0050] B1. Perform initialization;

[0051] Initial state X(0): initialized by zeroing wheel speed, setting initial position to (0, 0), and heading angle θ=0; initial covariance matrix P(0): set as a diagonal matrix, with motion state variance of 0.1 and distance variance of 0.05.

[0052] B2. Conduct the forecasting phase;

[0053] In the prediction phase, the predicted state at time k is calculated based on the state equation. Update the predicted covariance , P(k-1) is the state prediction covariance matrix at time k, reflecting the uncertainty of the predicted state. P(k-1) is the updated covariance matrix at time k-1, reflecting the accuracy of the state estimation at the previous time. Q(k) is the process noise covariance matrix. Motion noise is set to be small in scenes with light sources, and distance noise is set to be large in scenes without light sources.

[0054] B3. The update phase begins;

[0055] The update phase calculates the Kalman gain. R(k) is the observation noise covariance matrix: when there is a light source, the lidar noise is small, and a value of 0.01 is typically used; when there is no light source, the ultrasonic noise is slightly larger, and a value of 0.03 is typically used. The optimal state is obtained by fusing observations. Update the covariance matrix as follows: I is the identity matrix.

[0056] B4. Output results.

[0057] The fused optimal state X(k), including precise position, velocity, and obstacle distance, is transmitted to the environmental feature recognition module as the basic data for feature labeling. By designing matrix parameters for different scenarios, Kalman filtering can adapt to the sensor characteristics of two environments. For example, it enhances the dynamic correction of distance when there is no light source and focuses on motion accuracy when there is a light source, ultimately outputting more reliable fused data than a single sensor.

[0058] S3, the environmental feature recognition module marks the features of the fused data and transmits the marked data to the local map construction module;

[0059] As a preferred feasible embodiment, it should be explained that the environmental feature recognition and labeling process is based on the expansion of the state vector X(k) after Kalman filtering fusion. The core objective is to extract structured environmental features from multi-source data to provide semantically labeled basic information for local map construction. Because the characteristics of sensor data differ significantly between scenarios with and without light sources, the feature processing logic needs to be designed specifically: First, in scenarios with light sources, data from visual cameras and LiDAR are dominant. For the RGB images output by the visual cameras, edge detection and contour extraction algorithms are used to calculate the bounding rectangle parameters of obstacle contours. Simultaneously, K-Means color clustering is used to distinguish obstacles from the background, recording color features. After the LiDAR point cloud is timestamped and calibrated with the coordinate transformation matrix, it is projected onto a two-dimensional plane and fused with the visual contour. The average distance and distribution span angle of the obstacle point cloud are calculated, combined with the vehicle's position (x, y) and heading angle θ, using the formula... , d represents the lidar ranging, which converts obstacle features into global relative coordinates to achieve motion state correlation.

[0060] Secondly, in scenarios without light sources, data from ultrasonic and millimeter-wave radar is relied upon: Perform difference analysis, using the rate of change of distance between adjacent time points. It distinguishes between static and dynamic obstacles, then integrates ultrasonic short-range ranging and millimeter-wave radar mid-to-long-range data, supplements layered distance labels, and similarly combines the vehicle's position and heading angle, using the above coordinate transformation formula to generate global relative coordinates to complete feature localization.

[0061] Ultimately, all features are structurally encapsulated in a unified format, and confidence labels are added to the features. A photosensitive sensor determines whether to switch the feature extraction algorithm: visual processing is enabled when there is a light source, and color / contour analysis is skipped when there is no light source. The encapsulated data is shared in memory and transmitted to the local map building module, providing it with semantic and confidence-based structured input to support scene-adaptive map construction: texture is refined when there is a light source, and obstacle areas are quickly updated when there is no light source.

[0062] S4. The local map construction module constructs a local map in a two-dimensional coordinate system based on the feature recognition results, with the current coordinates of the vehicle as the origin, according to different scenarios.

[0063] As a preferred feasible embodiment, it should be explained that the local map construction is based on the semantically labeled data output by the environmental feature recognition module. The core logic is to dynamically generate an adapted two-dimensional grid map by combining the differences between scenes with and without light sources. The specific process is as follows: the real-time coordinates (x, y) of the car are used as the origin of the local map, and the map is dynamically translated as the car moves; a two-dimensional grid map with a resolution of 0.1m×0.1m is constructed, covering a range of 5m×5m or 4m×4m around the car, and the environment is marked by the grid status: idle / occupied / dynamic / unknown.

[0064] It should be explained that the map covers a 5m x 5m area of ​​the vehicle in a well-lit environment, and a 4m x 4m area in a dark environment.

[0065] In scenarios with available light, leveraging the high-precision features of the visual camera and LiDAR, a 50×50 grid is generated centered on the vehicle's coordinates, initially marked as idle with a grid value of 0; the global coordinates of obstacles are then... To convert coordinates to local raster coordinates, use the following formula: , Combine the contour size to mark continuous grids as occupied, with grid value = 1, and write semantic labels; mark dynamic obstacle grids by the difference between visual frames, with grid value = 2, and use laser point cloud density weighting to optimize the confidence of edge grids.

[0066] In scenarios without light sources, relying on the distance characteristics of ultrasonic and millimeter-wave radar, a 40×40 grid is generated due to sensor ranging limitations. Initially marked as unknown, the grid value is -1. Similarly, the global coordinates of obstacles are converted into local grids, and nearby obstacles are marked as high-confidence occupied, with a grid value of 1 and a weight of +0.3, while medium-distance obstacles are marked as low-confidence occupied, with a grid value of 1 and a weight of +0.1. Dynamic obstacles are marked based on the distance change rate, with a grid value of 2, and the position at the next moment is predicted. Grids with continuous no feedback are downgraded to idle, with a grid value of 0 and a weight of -0.1.

[0067] Finally, the map is packaged in a unified format, including timestamps, scene type, origin coordinates, grid resolution, grid state array, and semantic label list: associated grid coordinates, obstacle type, motion state, confidence level, and dynamic predicted position. The packaged data is used by the path planning module and decision control module. Specifically, the A* algorithm prioritizes avoiding static obstacles when there is light and avoids dynamic obstacles when there is no light. It also triggers priority responses based on semantic labels, such as slowing down and increasing the turning angle when encountering dynamic obstacles. At the same time, the impact of noise is reduced through multi-frame fusion voting weighting. When switching scenes, idle grids are retained and occupied grids are reset to unknown, achieving a smooth transition. This ensures that the map is suitable for the high-precision requirements of scenes with light and meets the dynamic safety requirements of scenes without light, providing a reliable environmental model for upper-layer applications.

[0068] S5. The path feasibility assessment module combines path node data in the local map with real-time information provided by Kalman filtering to calculate the passage safety index of each node in a differentiated manner.

[0069] As a preferred feasible embodiment, it should be explained that the traffic safety index is calculated by weighting three dimensions: static obstacle risk, dynamic obstacle risk, and sensor confidence. The weights are allocated differently based on whether the scene has a light source or not. The specific calculation process is as follows: for the path node to be evaluated... The system obtains three types of data from various modules: local map information: extracting the occupancy status and confidence weight of the raster map around the node for static obstacle risk assessment; dynamic obstacle status: calling the optimal state vector X(k) output by the Kalman filter fusion module to obtain the predicted position and velocity of the dynamic obstacle; and scene and sensor parameters: determining whether the current scene type has a light source or no light source through the photosensitive sensor, and combining the raster confidence and sensor noise characteristics in the local map to provide a basis for sensor confidence scoring.

[0070] The main purpose of static obstacle risk scoring is to assess the density and distance of static obstacles around path nodes. The specific steps are as follows:

[0071] C1. Establish an evaluation window;

[0072] Centered on the node to be evaluated, define an evaluation window of 4×4 or 5×5 grid to cover the key areas around the node.

[0073] C2. Calculate the grid occupancy cost;

[0074] Different costs are assigned based on the local map grid state: occupying a grid value of 1 incurs the highest cost. Dynamic grid value = 2, a threat exists. Unknown raster value = -1, proceed with caution in scenes without light source. Free grid value = 0, no threat. .

[0075] C3. Calculate obstacle density;

[0076] The initial barrier density (den) is obtained by averaging the cost of all grids within the evaluation window.

[0077] C4. Introducing distance attenuation;

[0078] Calculate the Euclidean distance *dist* from the node to the current position of the vehicle using the decay function. This reduces the urgency of decision-making at distant nodes.

[0079] C5. Output the final static obstacle risk score.

[0080] The final static obstacle risk score is: The physical meaning of this formula is that the more obstacles there are and the closer they are to the car, The lower the value, the higher the risk.

[0081] The main purpose of dynamic obstacle risk scoring is to assess the probability of collision between dynamic obstacles and path nodes. The specific steps are as follows:

[0082] D1. Predict the trajectory of dynamic obstacles;

[0083] Based on the dynamic obstacle state output by Kalman filter, predict the motion trajectory within the next 1-3 seconds.

[0084] D2. Calculate the shortest distance and collision time;

[0085] Shortest distance It is the minimum distance between the predicted trajectory of the dynamic obstacle and the target node. The collision time TTC is the time when the dynamic obstacle moves towards the node with a large relative speed. The smaller the TTC, the higher the risk.

[0086] D3. Construct the scoring function;

[0087] Combined with safety threshold and safe collision time ,calculate ,when Or when TTC is below the threshold A sharp drop could pose a high risk of collision.

[0088] D4. Handling multi-obstacle scenarios.

[0089] If multiple dynamic obstacles exist, then all of them are taken. The minimum value in the range is used as the final dynamic risk score for the node.

[0090] Sensor confidence score reflects the reliability of the environmental information generated by the node. In scenarios with light sources, where visual cameras and LiDAR dominate, the acquired data quality is high, and the sensor confidence score is [value missing]. 'a' is the visual outline sharpness factor, determined by the resolution of the visual camera. The sharper the outline captured by the camera, the better. The closer it is to 1.0;

[0091] In scenarios without light sources, relying primarily on ultrasonic and millimeter-wave radar, the data suffers from noise and blind spots. In such cases, the sensor confidence score is... b represents the confidence level of the millimeter-wave radar, which is determined by the accuracy of the millimeter-wave radar; c represents the proportion of unknown grid cells. The more unknown grid cells there are... The lower the value, the better.

[0092] Based on two scene types, with and without light source, the weights of the three dimensions are adjusted to meet the following requirements. To reflect safety strategies in different scenarios; in scenarios with light sources, visual cameras and LiDAR can collect high-precision static obstacle data, which is more suitable for the needs of fine obstacle avoidance. Therefore, the weight of static risk scoring is increased, and the standard value is set at [value missing]. , , The weighting was verified through testing in 50 scenarios with light sources. In scenarios without light sources, visual sensors fail, and static maps built solely by ultrasonic and millimeter-wave radar have poor integrity. Furthermore, sensor data is susceptible to environmental interference, necessitating priority to ensure safety redundancy. Therefore, the weights for dynamic risk warning and data reliability verification were strengthened, with conventional values ​​assigned. , , .

[0093] Finally, for path nodes Integrating the sub-item scores and scenario weights yields: , This represents the safety index of the i-th path node, with a value ranging from [0, 1]. The closer the value is to 1, the higher the safety of the node. These are the weighting coefficients for static obstacle risk scoring. These are the weighting coefficients for dynamic obstacle risk scoring. The weighting coefficients for sensor confidence scores; the path planning module will prioritize nodes with high PSI to generate paths, and in scenarios with light sources, it will use high-precision static information to plan accurate paths; in scenarios without light sources, it will strengthen dynamic risk and confidence considerations and plan conservative and safe paths.

[0094] S6. The path deviation analysis module calculates the deviation between the actual safety index of each node and the benchmark value based on the preset differentiated safety threshold.

[0095] As a preferred feasible embodiment, it should be explained that the path deviation analysis aims to monitor fluctuations in the safety status of path nodes and provide a basis for path adjustment or emergency response. The module input data includes the actual passage index of nodes output by the path feasibility assessment module. In addition, there are scenario-based safety benchmarks. Taking a scenario with light sources as an example, the same adaptation applies to scenarios without light sources, for each path node. The deviation is calculated in three parts: Static obstacle deviation ΔS: The classification rules are as follows: when ΔS < 0.1, the static environment is stable; when 0.1 ≤ ΔS < 0.3, the static environment changes; when ΔS ≥ 0.3, the static obstacle risk changes abruptly; dynamic obstacle deviation ΔD: The classification rules are as follows: ΔD < 0.1 indicates stable dynamic threat; 0.1 ≤ ΔD < 0.3 indicates abnormal dynamic obstacle movement; ΔD ≥ 0.3 indicates a high collision risk warning; sensor confidence deviation ΔC: The classification rules are as follows: when ΔC < 0.05, the sensor data is stable; when 0.05 ≤ ΔC < 0.2, the data quality deteriorates; when ΔC ≥ 0.2, the sensor malfunctions or the scene changes abruptly. The three deviations are then fused according to the scene weight to obtain the comprehensive deviation index ΔPSI. When ΔPSI < 0.1, it is a green deviation, indicating that the path is safe and the original plan continues; when 0.1 ≤ ΔPSI < 0.3, it is a yellow deviation, indicating that there is a potential risk and local path optimization will be triggered; when ΔPSI ≥ 0.3, it is a red deviation, indicating that the path risk is too high and an emergency response will be triggered.

[0096] It needs further explanation that the scenario-based benchmark values ​​are pre-calibrated and stored in a local database, combining scenario characteristics with historical safety data. For scenarios with light sources, risk-free paths are statistically analyzed based on extensive testing in clear environments. , , Distribution, with the mean as the benchmark. In low-light environments with no light source, considering sensor noise and dynamic threats, the benchmark value prioritizes dynamic safety. The benchmark for no light source is... .

[0097] In terms of scene adaptive adjustment, scenes with light sources are more sensitive to static deviation ΔS, and yellow deviation triggers local optimization; scenes without light sources are more stringent to dynamic deviation ΔD and confidence deviation ΔC, and yellow deviation activates the safety redundancy strategy.

[0098] S7 The obstacle avoidance control module generates corresponding motor control commands based on the deviation indicators under different scenarios to achieve differentiated obstacle avoidance control.

[0099] As a preferred feasible embodiment, it should be explained that the core objective of the obstacle avoidance control module is to enable the vehicle to dynamically adjust its motion state according to safety deviations in different scenarios, ensuring that the obstacle avoidance behavior is both adapted to environmental characteristics and meets safety requirements. Its core inputs include the green, yellow, and red deviation labels output by the path deviation analysis module, the scene type determined by the photosensitive sensor, the grid state of the local map, and motion parameters such as velocity v and angular velocity ω output by the Kalman filter module. These inputs together provide the environmental and motion basis for the control commands.

[0100] In scenarios with light sources, the control strategy focuses on precise path following. When the deviation is green and the path is safe and stable, the module maintains the current speed v and steering angle θ, and drives according to the original path node sequence. For example, when the current speed is 0.3 m / s and the steering angle is 5, it continuously outputs the same PWM duty cycle to control the drive wheels, and the servo maintains the original angle. If the deviation is yellow, it indicates that there are fluctuations in the static environment or sensor data. At this time, the path planning module is called to search for high PSI nodes within 1m around the current node to generate a locally adjusted path segment. At the same time, the control commands are adjusted according to the source of risk. If the density of static obstacles increases, the speed will be reduced to 0.2 m / s, and the servo angle will be linearly adjusted according to the direction difference of the new path nodes. When the deviation reaches red, it means that there is a sudden change in static obstacles or a high risk of collision. The module will immediately reset the PWM duty cycle of the drive wheels to zero and quickly return the servo to center, allowing the car to decelerate to 0 in a short time. At the same time, the global path planning module is called to replan the path based on the latest local map. After the new path is generated, it starts at a low initial speed of 0.1 m / s and gradually resumes normal driving.

[0101] In scenarios without light sources, the control strategy prioritizes rapid response. When a green deviation occurs, although the path is safe, the data reliability is limited. The module will maintain a conservative motion state, keeping a low speed of 0.2 m / s and a small steering angle of ≤10, while the servo motor will fine-tune by 1° every 0.5 seconds to scan the surroundings to cover dynamic threats. When a yellow deviation occurs, it indicates the presence of dynamic threats or a decline in sensor data quality. The module will immediately reduce the speed to 0.1 m / s to extend the collision response time, and increase the steering angle in advance to avoid potential collision areas based on the predicted trajectory of dynamic obstacles. If the proportion of unknown grids is high, the trigger frequency of the ultrasonic sensor will be shortened from 50ms to 30ms to supplement environmental data. When the deviation is red, it indicates a high risk of collision or sensor failure. The module will instantly reduce the PWM duty cycle of the drive wheels to zero and bring the car to a stop within 0.2 seconds. At the same time, it will control the servo motor to quickly turn away from dynamic obstacles to increase the safety distance. If a red deviation is triggered by sensor confidence deviation, the module will automatically switch to the backup sensor group and mark the faulty sensor for subsequent repair.

[0102] Secondly: The accompanying drawings of the embodiments disclosed in this invention only involve the structures involved in the embodiments disclosed in this invention. Other structures can refer to the general design. In the absence of conflict, the same embodiment and different embodiments of this invention can be combined with each other.

[0103] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A control method for an obstacle avoidance car based on multi-sensor fusion and Kalman filtering, characterized in that, The method comprises a data acquisition module, a local database, a Kalman filter fusion module, an environment feature recognition module, a local map construction module, a path feasibility evaluation module, a path deviation analysis module, and an obstacle avoidance control module. S1, the data acquisition module acquires the multi-sensor original data of the obstacle avoidance vehicle according to two environment scenes with and without light sources, and stores the data in the local database. S2, the Kalman filter fusion module extracts the original data of the corresponding scene from the local database, performs data fusion through the construction of corresponding state equations and observation equations, and outputs the results to the environment feature recognition module. The process of constructing corresponding state equations and observation equations is as follows: A1, state variable definition; With the motion state of the obstacle avoidance car and the core environmental perception quantity as the core, a unified state vector X(k) is defined: x, y are the position coordinates of the car in the two-dimensional coordinate system, with units of m, θ is the heading angle of the car, with units of rad, v is the linear speed of the car, with units of m / s, is the angular velocity of the car, with units of rad / s, is the distance of the obstacle in the front three key directions, i.e. straight ahead, left by 30°, and right by 30°, with units of m; A2, scene-based state equation construction; A3, scene-based observation equation construction; The state equation describes how the state at time k evolves from the state at time k-1, in the form of: , X(k), X(k-1) are state vectors of the trolley at different times, F(k) is a state transition matrix, B(k) is a control input matrix, the dimension of which is consistent with the state vector X(k), used to map the control vector u(k) to the state space, u(k) is a control vector, which is a control instruction for the motor of the trolley, and W(k) is a process noise vector; In scenarios with light sources, the state transition matrix Δt is the sampling time interval, which takes a value of 0.1s. Because the lidar frame rate is synchronized with motion sampling, it is set to constant value transfer processing; in the absence of light source, based on the state transition matrix F(k) of the scene with light source, the following is applied: The attenuation coefficient is increased by 0.

95. This coefficient is derived from fitting 100 sets of motion data of near-field obstacles in no-light scenarios, reflecting the rapid change characteristics of near-field obstacles. The state transition matrix F(k) is: In a scenario without a light source, Δt is the sampling time interval, with a value of 0.05s. The observation equation describes how the sensor measurements are related to the state quantities in the form Z(k) is the sensor measurement vector, V(k) is the observation noise vector, which is Gaussian white noise with mean 0 and variance the observation noise covariance matrix R(k), and H(k) is the observation matrix. In the light source scene, the observations are from the omnidirectional range measured by the laser radar, the contour extrapolation distance of the visual camera, and the speed of the wheel speed encoder, , and the speed and angular velocity calculated based on the wheel speed, the ranging of the corresponding direction of the laser radar, the front distance extrapolated by the visual contour; the observation matrix H(k) is designed as: ; in the light source-free scene, the observations are from the ultrasonic wave, the millimeter wave radar, and the wheel speed encoder, , the ranging of three directions of the ultrasonic wave, the front ranging of the millimeter wave radar, and at this time the observation matrix H(k) is adjusted as: ; S3, the environment feature recognition module labels the fused data with features, and transmits the labeled data to the local map construction module; S4, the local map construction module constructs a local map in a two-dimensional coordinate system with the current coordinates of the vehicle as the origin according to the feature recognition results; S5, the path feasibility evaluation module combines the path node data in the local map with the real-time information provided by the Kalman filter to differentially calculate the passing safety index of each node; The passage safety index is calculated by weighting three dimensions of static obstacle risk score , dynamic obstacle risk score , and sensor confidence score , and is assigned with different weights according to the difference between light source and non-light source scenes, and the formula is: , represents the passage safety index of the i-th path node, is the weight coefficient of the static obstacle risk score, is the weight coefficient of the dynamic obstacle risk score, is the weight coefficient of the sensor confidence score, and satisfies ; S6, the path deviation analysis module calculates the deviation between the actual safety index and the reference value of each node according to the preset differential safety threshold; S7, the obstacle avoidance control module generates corresponding motor control instructions according to the deviation index in different scenes to realize differential obstacle avoidance control.

2. The multi-sensor fusion and Kalman filter based obstacle avoidance vehicle control method according to claim 1, characterized in that: The light source refers to a stable natural light source or artificial light source in the scene that can be recognized by the vehicle sensor, and the light intensity meets the normal working requirements of the vision and optical sensors carried by the obstacle avoidance vehicle, and the basic environment perception can be realized without relying on pure distance sensors; there is no natural light source and artificial light source in the scene without light source, or the light source intensity is too low to support the work of vision and optical sensors, at this time the obstacle avoidance vehicle needs to completely rely on pure distance sensors to realize environment perception.

3. The multi-sensor fusion and Kalman filter based obstacle avoidance vehicle control method according to claim 1, characterized in that: The multi-sensor original data includes: obstacle distance data: the straight-line distance of obstacles in different directions from the vehicle; obstacle contour data: the geometric shape and edge features of the obstacle; vehicle motion state data: real-time speed, steering angle, acceleration; environmental light intensity data: scene light intensity value.

4. The multi-sensor fusion and Kalman filter based obstacle avoidance vehicle control method according to claim 1, characterized in that: The specific implementation steps of data fusion are as follows: B1, initialization; B2, prediction stage; B3, update stage; B4, output result.

5. The multi-sensor fusion and Kalman filter based obstacle avoidance vehicle control method according to claim 4, characterized in that: The prediction phase calculates the predicted state at time k according to the state equation , update the predicted covariance , is the state prediction covariance matrix at time k, reflecting the uncertainty of the predicted state, P(k-1) is the updated covariance matrix at time k-1, reflecting the accuracy of the state estimation at the last time, Q(k) is the process noise covariance matrix, which is small in the light source scene and large in the light source-free scene.

6. The multi-sensor fusion and Kalman filter based obstacle avoidance vehicle control method according to claim 4, characterized in that: The update stage calculates Kalman gain , R(k) is the observation noise covariance matrix: the laser radar noise is small when there is a light source, and the value is 0.01; the ultrasonic noise is slightly large when there is no light source, and the value is 0.03; the optimal state is obtained by fusing the observation values ; and the covariance matrix is updated as .

Images