Adaptive Obstacle Avoidance Control System and Its Application Method

Abstract

This application relates to the field of intelligent mobile device control, and discloses an adaptive obstacle avoidance control system and its application method. The system includes: a data acquisition module, an environment modeling module, an obstacle segmentation module, a path planning module, and a motion control module; the data acquisition module acquires multi-modal data of the vehicle's surrounding environment; the environment modeling module fuses the multi-modal data to generate a dynamic environment map including the spatial positions, dynamic characteristics of obstacles, and free passage areas; the obstacle segmentation module segments the dynamic environment map to identify the obstacle areas and free passage areas; the path planning module calculates the optimal path with the minimum cost; the motion control module uses the PID control algorithm or the model predictive control method to adjust the vehicle's motion state in real time to achieve path tracking and dynamic obstacle avoidance. The present invention has high-precision environment perception ability, flexible path planning ability, and efficient dynamic obstacle avoidance control ability, and can be widely applied to the fields of industrial automation and intelligent logistics.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent mobile device control, and particularly to an adaptive obstacle avoidance control system for autonomous mobile devices such as unmanned forklifts and mobile robots, and an application method thereof. Background Art

[0002] With the rapid development of industrial automation and intelligence, autonomous mobile devices such as unmanned forklifts and mobile robots have been widely used in fields such as logistics warehousing and factory automation. These devices usually need to complete autonomous navigation and obstacle avoidance tasks in a dynamic and complex environment. However, achieving accurate environmental perception, path planning, and motion control remains a key problem in the existing technology.

[0003] Existing autonomous navigation systems mostly rely on a single type of sensor for environmental perception. For example, lidar is used to obtain point cloud data or visual sensors are used to capture image information. However, the data of a single sensor often has limitations. For example, although lidar can provide high-precision spatial information, it lacks semantic recognition ability; visual sensors can identify target categories, but perform poorly under complex lighting conditions and cannot directly obtain the three-dimensional geometric information of obstacles. This lack of perception ability easily leads to low accuracy in obstacle recognition and affects the overall navigation effect of the system.

[0004] On the other hand, in a dynamic environment, autonomous devices need to accurately distinguish obstacles from free passage areas to ensure that the planned path can avoid obstacles and reach the target efficiently. However, existing path segmentation algorithms usually rely on simple geometric or static rules and cannot effectively handle the segmentation problem of dynamic obstacles in complex scenarios. For example, they cannot dynamically adjust the segmentation strategy according to the speed or movement direction of obstacles, resulting in the navigation path possibly passing through dangerous areas.

[0005] In addition, the real-time performance and flexibility of path planning algorithms also face challenges in complex scenarios. Many existing planning algorithms can only generate the shortest path based on a static scenario and lack the ability to quickly respond to changes in the dynamic environment. Especially in narrow spaces with dense obstacles, path planning algorithms often have difficulty balancing the safety and driving efficiency of the path, resulting in a decrease in the system operation efficiency or potential safety hazards.

[0006] In terms of motion control, traditional control methods lack the real-time adaptation ability to vehicle dynamics and environmental dynamic changes, and it is difficult to accurately track the planned path and simultaneously avoid dynamic obstacles. When faced with sudden obstacles or environmental changes, existing control algorithms may react slowly or the adjustment is not precise enough, resulting in the vehicle being unable to avoid obstacles in time and increasing the safety risk.

[0007] Based on the above problems, the existing technology urgently needs to further improve the autonomous obstacle avoidance and path planning capabilities in complex dynamic environments. Summary of the Invention

[0008] In view of the deficiencies of the existing technology, the present invention provides an adaptive obstacle avoidance control system and its application method, which solves the problems of the existing technology that autonomous mobile devices lack high-precision environment perception, dynamic obstacle segmentation, flexible path planning, and real-time motion control in dynamic complex environments.

[0009] To achieve the above objectives, the present invention is realized through the following technical solutions: An adaptive obstacle avoidance control system, including:

[0010] A data acquisition module, used to collect multi-modal sensor data of the vehicle's surrounding environment, including 3D lidar point cloud data and visual sensor image data;

[0011] An environment modeling module, used to fuse the collected multi-modal sensor data to generate a dynamic environment map, where the dynamic environment map includes the spatial position, dynamic characteristics of obstacles, and free passage areas;

[0012] An obstacle segmentation module, used to segment the dynamic environment map according to the maximum flow minimum cut theorem to identify obstacle areas and free passage areas;

[0013] A path planning module, used to calculate the optimal path based on the free passage area through the principle of least action of Fermat;

[0014] A motion control module, used to control the vehicle's movement and adjust its trajectory in real time to avoid obstacles according to the optimal path generated by the path planning module.

[0015] Preferably, the data acquisition module includes:

[0016] A 3D lidar, used to collect point cloud data and identify the spatial position of obstacles;

[0017] A visual sensor, used to collect image information of the surrounding environment and identify the types of obstacles.

[0018] Preferably, the environment modeling module includes:

[0019] A data fusion unit, used to receive the point cloud data collected by the 3D lidar and the image data collected by the visual sensor, and fuse them to generate preliminary environment information;

[0020] An obstacle detection unit, used to perform spatial segmentation on the point cloud data based on the point cloud clustering method to identify the position and shape of obstacles, and at the same time perform target classification on the image data based on the visual detection algorithm to supplement the dynamic attribute information of obstacles;

[0021] A dynamic feature extraction unit, which is used to predict the trajectory of an obstacle through Kalman filtering and extract the dynamic features of the obstacle, including the spatial position, speed, and movement direction of the obstacle;

[0022] A dynamic environment map generation unit, which is used to model the fused environmental information as a dynamic environment map, where the dynamic environment map includes a node set and an edge set. The node set represents obstacles and free passage areas, and the weights of the edge set are used to characterize the passage cost between regions.

[0023] Preferably, the edge weights of the dynamic environment map are comprehensively determined by the following factors:

[0024] The Euclidean distance between nodes;

[0025] The magnitude of the speed of the obstacle;

[0026] The angle between the movement direction of the obstacle and the current path direction.

[0027] Preferably, the obstacle segmentation module includes:

[0028] A dynamic graph construction unit, which is used to receive the dynamic environment map generated by the environmental modeling module, define the nodes of the dynamic environment map as obstacles or free passage areas, and set the weights of the edges as the passage cost between nodes, where the passage cost is calculated according to the distance between nodes, the speed of the obstacle, and the movement direction of the obstacle;

[0029] A flow calculation unit, which is used to construct a network flow model based on the dynamic environment map, set the current position of the vehicle as the source point of the network flow, set the target position as the sink point of the network flow, and set a capacity value for each edge according to the weights of the edges in the dynamic environment map. The capacity value is associated with the reciprocal of the passage cost;

[0030] A minimum cut segmentation unit, which is used to calculate the maximum flow value from the source point to the sink point in the network flow through the maximum flow algorithm and identify the obstacle area and the free passage area based on the maximum flow minimum cut theorem. The obstacle area is determined by the minimum cut set corresponding to the maximum flow, and the free passage area is the part of the dynamic environment map that does not belong to the obstacle area.

[0031] Preferably, the path planning module includes

[0032] A path cost calculation unit, which is used to receive the free passage area output by the obstacle segmentation module, define the cost of the path as the weighted sum of the path length and the obstacle distance cost, where the path length cost is calculated based on the distance between path points, and the obstacle distance cost is determined inversely according to the distance between the path point and the nearest obstacle, and assign a comprehensive cost value to each possible path;

[0033] The optimal path solving unit is used to optimize the path cost based on Fermat's principle of least action, construct a path action model, define the optimal path as the path that minimizes the action, and solve the optimal solution of the path action through numerical optimization methods;

[0034] The discrete optimization unit is used to discretize the continuous path into a sequence of path points, and iteratively update the positions of the path points based on the gradient descent algorithm or other numerical optimization algorithms to minimize the comprehensive cost of the sequence of path points;

[0035] The path output unit is used to output the optimized optimal path, and represent the path as a sequence of path points from the starting point to the target point, where the path points contain position coordinate information and motion state information when passing through the path points.

[0036] Preferably, the motion control module includes:

[0037] The error calculation unit is used to receive the optimal path output by the path planning module and the current position of the vehicle, and calculate the lateral error and heading error between the current position of the vehicle and the optimal path, where the lateral error is the shortest distance between the current position of the vehicle and the path point, and the heading error is the angle between the current motion direction of the vehicle and the path direction;

[0038] The trajectory tracking unit is used to determine the speed control amount and steering control amount of the vehicle according to the lateral error and heading error calculated by the error calculation unit, so as to reduce the error of the vehicle deviating from the path and keep the vehicle moving along the optimal path;

[0039] The control law generation unit is used to generate control instructions based on the PID control algorithm, where the PID control algorithm adjusts the linear velocity and angular velocity of the vehicle through proportional, integral, and differential controls, and the control instructions include the speed control instructions calculated according to the lateral error and the angular velocity control instructions calculated according to the heading error;

[0040] The vehicle execution unit is used to receive the control instructions output by the control law generation unit, and adjust the linear velocity and angular velocity of the vehicle in real time, so that the vehicle moves along the optimal path and realizes the avoidance operation of dynamic obstacles.

[0041] The present invention also provides an application method of the adaptive obstacle avoidance control system, including the following steps:

[0042] Collect multi-modal data of the vehicle surrounding environment through a 3D lidar and a vision sensor;

[0043] Fuse the multi-modal data to generate a dynamic environment map, where the dynamic environment map includes the spatial position, speed, and free passage area of the obstacles;

[0044] Based on the maximum flow minimum cut theorem, divide the obstacle area and the free passage area in the dynamic environment map;

[0045] Within the free passage area, by defining the action of the path and based on Fermat's principle of least action, calculate the optimal path with the minimum cost.

[0046] According to the optimal path, use the PID control algorithm or model predictive control method to adjust the vehicle's motion parameters to achieve dynamic obstacle avoidance and precise tracking of the target position.

[0047] The present invention provides an adaptive obstacle avoidance control system and its application method. It has the following beneficial effects:

[0048] 1. Through the multi-modal data fusion of 3D lidar and vision sensors, the present invention can not only accurately obtain the spatial position of obstacles, but also identify the categories and dynamic characteristics of obstacles. This environmental perception method that combines geometric information and semantic information enables the system to accurately perceive the surrounding situation in a complex dynamic environment, providing a reliable basis for subsequent path planning and motion control.

[0049] 2. By using the maximum flow minimum cut theorem to segment the dynamic environment map, the present invention can efficiently and clearly divide the obstacle area and the free passage area. Whether it is static obstacles or moving dynamic obstacles, this method can effectively cope with them, providing a clear feasible space for path planning and avoiding the collision risk caused by inaccurate segmentation.

[0050] 3. By introducing Fermat's principle of least action, the present invention can quickly generate the optimal path with the minimum cost according to different environmental and task requirements. Whether in a narrow scene with dense obstacles or in a large-scale dynamic environment, the system can adjust the weights of the path planning algorithm according to the situation and generate a safe and efficient motion path.

[0051] 4. By combining the PID control algorithm, the present invention realizes the precise adjustment of the vehicle's linear velocity and angular velocity, enabling the vehicle to track the optimal path in real time. At the same time, the system also shows extremely high stability in dynamically avoiding sudden obstacles, ensuring the safe operation of the vehicle in a complex environment and providing a reliable guarantee for dynamic obstacle avoidance and target tracking. Brief Description of the Drawings

[0052] Figure 1 It is a schematic diagram of the system architecture of the present invention;

[0053] Figure 2 It is a schematic diagram of the method flow of the present invention. Detailed Embodiments

[0054] The following will be combined with the drawings of the present invention specification to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0055] Please refer to the attached Figure 1 The present invention provides an adaptive obstacle avoidance control system, which is suitable for dynamic obstacle avoidance scenarios of unmanned forklifts, mobile robots and other autonomous mobile devices. The system generates a dynamic environment map by collecting multimodal sensor data around the vehicle, divides the free passage area and the obstacle area by combining the maximum flow minimum cut theorem, plans the optimal path by using the Fermat minimum action principle, and realizes path tracking and dynamic obstacle avoidance through the motion control module, ensuring the efficient operation of the system in a complex dynamic environment.

[0056] like Figure 1 As shown, the adaptive obstacle avoidance control system may include: a data acquisition module, an environment modeling module, an obstacle segmentation module, a path planning module and a motion control module.

[0057] The various modules of the system of the present invention are described in detail below.

[0058] In this embodiment, the data acquisition module is used to collect multimodal sensor data of the vehicle's surrounding environment to provide input for subsequent environmental modeling and path planning. The data acquisition module specifically includes a three-dimensional laser radar and a visual sensor, and combines time synchronization and coordinate transformation to achieve multimodal data fusion.

[0059] As an option, a 3D laser radar is used to collect point cloud data of the vehicle's surroundings. The point cloud data contains the 3D coordinate information of each spatial point, such as the point , and reflection intensity information. LiDAR has high spatial resolution and real-time performance, can accurately locate obstacles in the environment, and provide an accurate geometric basis for dynamic environment modeling.

[0060] Specifically, the 3D LiDAR obtains a collection of spatial point cloud data around the vehicle through each scan. , where each point Location coordinates Represents the three-dimensional spatial information of obstacles. In addition, the reflection intensity value returned by the lidar can be used as an important feature to identify the surface characteristics of objects.

[0061] In one possible implementation, the visual sensor collects environmental image data , supplement information such as texture and color that lidar is difficult to provide. Exemplarily, the vision sensor can be an RGB camera or a depth camera, where the RGB camera provides a two-dimensional color image and the depth camera provides the depth information of each pixel point. Combining with deep learning object detection algorithms (such as YOLO, Mask R-CNN), the vision sensor can identify the category and bounding box of obstacles, for example, distinguish pedestrians, vehicles, goods, etc.

[0062] It should be noted that in order to achieve the effective fusion of lidar point cloud data and vision image data, the present invention performs time alignment on the two types of data through a timestamp synchronization method. Specifically, the lidar and the vision sensor respectively record the data acquisition time through a unified timestamp, and map the point cloud data and the image data to the same time segment.

[0063] As an option, after completing the time synchronization, use the extrinsic calibration matrix , project the three-dimensional lidar point cloud data into the image coordinate system of the vision sensor. Specifically, assume that the three-dimensional point in the point cloud represents the position in the lidar coordinate system, then its position in the image coordinate system can be calculated by the following formula:

[0064] ;

[0065] Among them, is the intrinsic matrix of the vision sensor, is the extrinsic transformation matrix from the lidar coordinate system to the vision sensor coordinate system.

[0066] In some embodiments, through projection, the point cloud data can be mapped to the pixel coordinates of the image, thereby realizing the fusion of the two types of sensor data. This multi-modal fusion method can combine the high-precision spatial characteristics of the point cloud data and the rich visual characteristics of the image data, and provide multi-dimensional information for subsequent dynamic environment modeling.

[0067] It can be understood that in order to improve the reliability of data acquisition, the present embodiment preprocesses the acquisition data of the lidar and the vision sensor. For example, abnormal points in the point cloud data are removed through a denoising filtering method, and common denoising methods include statistical filtering and radius filtering. In addition, for vision image data, the image quality under low-light or strong-light conditions can be improved through image enhancement methods.

[0068] Specifically, the denoising filtering of the point cloud data can be realized based on the following steps:

[0069] 1. Statistically analyze the distance distribution of the neighboring points around each point, and remove the abnormal points that exceed the normal distribution range;

[0070] 2. In radius filtering, a fixed neighborhood radius is set to eliminate isolated points with too few points in the neighborhood.

[0071] In another possible implementation, the visual image data can enhance the contrast through histogram equalization method, thereby enhancing the image details. For depth image data, bilateral filtering method can also be used for smoothing to remove noise while retaining edge features.

[0072] It should be noted that the multi-modal data acquisition module of this embodiment has high flexibility and can select different types of lidars and visual sensors according to the application scenario. For example, in an indoor environment, a lightweight lidar and a low-power camera can be selected; in an outdoor complex environment, sensors with a larger measurement range and high dynamic range can be selected.

[0073] It can be understood that the data acquisition module is the basis of the system of the present invention, and the multi-modal data it acquires directly affects the performance of subsequent modules.

[0074] In this embodiment, the environment modeling module is used to receive the multi-modal sensor data provided by the data acquisition module, generate a dynamic environment map through data fusion and obstacle detection, and provide an accurate environment modeling result for subsequent obstacle segmentation and path planning. The environment modeling module includes a data fusion unit, an obstacle detection unit, a dynamic feature extraction unit, and a dynamic environment map generation unit.

[0075] As an option, the environment modeling module generates preliminary environment information by fusing the point cloud data of the lidar and the image data of the visual sensor. The point cloud data provides high-precision three-dimensional spatial coordinates for describing the geometric characteristics of obstacles, while the image data provides visual characteristics such as the color and texture of obstacles, supplementing the detailed information of the point cloud data. The combination of the two makes the environment information more comprehensive.

[0076] Specifically, the data fusion unit receives the time-synchronized point cloud data and image data , projects the point cloud into the image coordinate system using the aforementioned coordinate transformation formula, and obtains the fused environment information. The result of the fusion is a set of point cloud data with semantic labels, where each point contains not only spatial coordinates , but also visual characteristics such as color and category.

[0077] In a possible implementation, the obstacle detection unit first performs clustering analysis on the point cloud data to identify the positions and shapes of obstacles in the space. Exemplarily, the clustering method can adopt the DBSCAN algorithm. By calculating the number of neighboring points of each point within a fixed radius, dense point cloud regions are clustered, and isolated points are removed. The identified point cloud clustering results represent the obstacles in the environment.

[0078] Meanwhile, the obstacle detection unit performs object detection and classification based on the visual image data. As an option, deep learning object detection algorithms such as YOLO or Mask R-CNN can be adopted to identify the obstacles in the image as categorized objects, such as pedestrians, vehicles, or goods. It should be noted that the classification results can be used to enhance the semantic information of the obstacles in the point cloud data, thereby improving the accuracy of environmental modeling.

[0079] It should be noted that in order to model the dynamic behavior of obstacles, the dynamic characteristic extraction unit predicts the movement trajectory of obstacles through Kalman filtering. Kalman filtering uses the historical positions and speeds of the obstacles as inputs to predict their future positions and movement directions. The prediction formula can be expressed as:

[0080] ;

[0081] ;

[0082] where, is the state vector representing the current position and speed of the obstacle; is the control vector representing possible external influences; is the observation vector representing the collected sensor data; and are the state transition matrix and the observation matrix respectively; and are the noise vectors.

[0083] As a possible implementation, the prediction results output by Kalman filtering include the future position and movement direction of the obstacle, and this information is used for the generation of the dynamic environment map.

[0084] The dynamic environment map generation unit constructs a dynamic environment map based on the fused environmental information and the dynamic characteristic prediction results. It should be noted that the dynamic environment map consists of a set of nodes and a set of edges, where the nodes represent the positions of obstacles or free passage areas, and the edges are used to describe the passability between the nodes. The weight of each edge characterizes the passability from node The cost of passage.

[0085] Specifically, the edge weight Comprehensively consider the following factors:

[0086] Nodes and The Euclidean distance between ;

[0087] The speed magnitude of the obstacle where the node is located ;

[0088] The angle between the movement direction of the obstacle and the current path direction .

[0089] As an option, the edge weight can be calculated in the following form:

[0090] ;

[0091] Among them, and Are adjustment parameters to balance the influence of distance, speed and direction on the passage cost. It can be understood that by adjusting and Values, it can adapt to the environmental modeling requirements in different scenarios. For example, the speed weight can be increased in high-dynamic scenarios, and the direction weight can be increased in complex obstacle scenarios.

[0092] In some embodiments, the dynamic environment map can also be extended to a three-dimensional map model, and the nodes not only include the planar position , but also include height information . This is of great significance for path planning in multi-layer environments (such as stereoscopic warehouses).

[0093] It should be noted that the dynamic environment map is the basis for subsequent obstacle segmentation and path planning, and its accuracy directly affects the obstacle avoidance performance of the system. In this embodiment, through multi-modal data fusion, extraction of dynamic characteristics of obstacles and environmental information modeling, a high-precision and scalable dynamic environment representation is generated, providing reliable data support for dynamic obstacle avoidance.

[0094] It can be understood that the implementation method of the environmental modeling module can be extended according to specific scenarios and requirements. For example, GPS data fusion can be added in outdoor environments, and V2X communication information can be introduced in dynamic traffic scenarios.

[0095] In this embodiment, the obstacle segmentation module is used to receive the dynamic environment map generated by the environment modeling module, and segment the obstacle area and the free passage area in the dynamic environment based on the maximum flow minimum cut theorem, so as to provide a clear passable area for the path planning module. The obstacle segmentation module includes a dynamic graph construction unit, a flow calculation unit, and a minimum cut segmentation unit. The functions and specific implementation methods of each unit are as follows.

[0096] As an option, the dynamic graph construction unit receives the dynamic environment map generated by the environment modeling module, and the dynamic environment map is represented in a graph structure where is a set of nodes representing the positions of obstacles or free areas, is a set of edges representing the connection relationships between nodes. Specifically, each edge has a weight , and the weight value characterizes the passing cost from node to node .

[0097] It should be noted that the calculation of the passing cost comprehensively considers the geometric distance between nodes, the speed of obstacles, and the angle between the movement direction of obstacles and the path direction. As an implementation method, the passing cost can be calculated by the following formula:

[0098] ;

[0099] where is the Euclidean distance between nodes and , is the projection value of the obstacle speed in the edge direction, is the angle between the movement direction of the obstacle and the path direction, and are adjustment parameters used to adjust the weights of speed and direction on the passing cost.

[0100] In a possible implementation, the flow calculation unit constructs a network flow model based on the dynamic environment map. Exemplarily, the network flow model defines the current position of the vehicle as the source point , and defines the target position as the sink point . Set a capacity value for each edge , and the capacity value is associated with the reciprocal of the passing cost of the edge, and is used to reflect the passing ability of the edge. The calculation formula of the capacity value is:

[0101] ;

[0102] It should be noted that this capacity value definition method makes the smaller the traffic cost (i.e., the better the path), the larger the capacity value, so it is more inclined to select a low-cost path for network flow calculation.

[0103] As an option, the traffic calculation unit calculates the maximum flow value from the source point to the sink point using a classic maximum flow algorithm (such as the Ford-Fulkerson algorithm). Specifically, the traffic calculation includes the following steps:

[0104] Initialize the network flow value , indicating that the flow of each edge is zero initially.

[0105] Use depth-first search (DFS) or breadth-first search (BFS) to find an augmenting path and increase the flow in the augmenting path.

[0106] Update the flow values of each edge , and adjust the remaining capacity .

[0107] Repeat the process of finding an augmenting path until no new augmenting path can be found.

[0108] It can be understood that the calculation result of the maximum flow reflects the maximum passable capacity from the source point to the sink point in the current network flow model.

[0109] Based on the maximum flow minimum cut theorem, the minimum cut splitting unit extracts the minimum cut set from the result of the traffic calculation unit. Specifically, the minimum cut is the minimum capacity set in the network that separates the source point and the sink point . According to the maximum flow minimum cut theorem, the capacity corresponding to the minimum cut set is equal to the maximum flow value.

[0110] In a possible implementation, the minimum cut splitting unit defines the minimum cut set as the obstacle area, that is, the nodes related to the minimum cut set in the dynamic environment map belong to the obstacle area; the remaining nodes not included in the minimum cut set belong to the free passage area. In this way, the obstacles and free areas in the dynamic environment can be clearly separated.

[0111] It should be noted that the division result of the obstacle area and the free passage area will be used as the input of the path planning module to provide clear passage restriction information for path planning.

[0112] As an extension method, the minimum cut segmentation unit can adjust the definition of the obstacle area according to the requirements of the application scenario. For example, in a dynamic and complex environment, the minimum cut set can be further refined based on the dynamic characteristics of the obstacles (such as speed and movement direction), increasing the flexibility of the free passage area. This extension method does not change the core design of the present invention.

[0113] It can be understood that the functional implementation of the obstacle segmentation module depends on the accuracy of the dynamic environment map and the computational efficiency of the maximum flow algorithm. Therefore, in some embodiments, the construction parameters of the dynamic environment map (such as and ) can be optimized to meet the requirements of different scenarios.

[0114] In this embodiment, through the collaborative work of the dynamic graph construction unit, the flow calculation unit, and the minimum cut segmentation unit, the efficient segmentation of the obstacle area and the free passage area in the dynamic environment is achieved, providing reliable data support for the path planning module.

[0115] In this embodiment, the path planning module is used to receive the free passage area output by the obstacle segmentation module and generate an optimal path based on the principle of least action of Fermat and the optimization algorithm. The path planning module includes a path cost calculation unit, an optimal path solving unit, a discrete optimization unit, and a path output unit, and the specific implementation is as follows.

[0116] As an option, the path cost calculation unit first defines the cost value of the path according to the free passage area provided by the obstacle segmentation module. The path cost value consists of the weighted sum of the path length cost and the obstacle distance cost. Specifically, the path length cost is calculated based on the geometric distance between path points, while the obstacle distance cost is determined inversely according to the distance between the path point and the nearest obstacle. Exemplarily, the path cost value can be represented by the following formula:

[0117] ;

[0118] Where:

[0119] represents the comprehensive cost value of the path;

[0120] is the weight of the path length cost, used to adjust the influence of the path length on the cost value;

[0121] is the weight of the obstacle distance cost, used to adjust the priority of path safety;

[0122] is the speed magnitude of the path, representing the path length;

[0123] Indicates a path point The distance to the nearest obstacle

[0124] It should be noted that by reasonably adjusting and values, the path cost function can be adapted to the requirements of different scenarios. For example, in a complex obstacle environment, the weight of can be increased to avoid obstacles preferentially

[0125] In a possible implementation, the optimal path solving unit, based on Fermat's principle of least action, takes the path cost function as the optimization objective to find the optimal solution of the path. Specifically, the path planning problem is modeled as a least action problem, and its objective is to minimize the action S of the path:

[0126] ;

[0127] where is the Lagrangian function of the path, which is defined consistently with the path cost value

[0128] In a possible implementation, the optimal path solving unit determines the optimal path by solving the Euler - Lagrange equation. The Euler - Lagrange equation describes the optimality condition of the path:

[0129] ;

[0130] By calculating the partial derivative of the path action, an analytical solution or a numerical solution of the optimal path can be obtained

[0131] As an option, when an analytical solution is difficult to obtain, the discrete optimization unit discretizes the continuous path into a sequence of path points and iteratively optimizes the path points through numerical optimization methods. Specifically, the discrete path is represented as a set of path points , where each path point contains the position coordinates and the time step . The discrete path cost value can be expressed as:

[0132] ;

[0133] where represents the Euclidean distance between adjacent path points, and represents the distance from the path point to the nearest obstacle

[0134] Exemplarily, the discrete optimization unit uses the gradient descent algorithm to update the positions of the path points. The iterative update formula of gradient descent is:​

[0135] ;

[0136] wherein, is the learning rate, is the gradient of the path cost value with respect to the path point.

[0137] It should be noted that through iterative optimization, the comprehensive cost value of the path can be effectively reduced, thereby generating an optimal path point sequence.

[0138] In a possible implementation manner, the path output unit outputs the optimized optimal path point sequence as the planning result. Specifically, the path output unit represents the optimal path in the form of a path point sequence, where each path point includes the position coordinates and the motion state when passing through this point (such as speed and direction). The optimization result can be directly used for the subsequent motion control module.

[0139] As an extended manner, the path output unit can generate path representations in multiple formats according to different application requirements. For example, in a dynamic and complex environment, timestamp information can be attached to the path points to support path planning with time constraints; in a three-dimensional environment, the three-dimensional coordinates of the path points can be output .

[0140] It can be understood that the path planning module realizes the optimal path planning in the free area through the collaborative work of the path cost calculation unit, the optimal path solving unit, the discrete optimization unit, and the path output unit.

[0141] In this embodiment, the motion control module is used to receive the optimal path output by the path planning module, and based on the current state of the vehicle, generate control instructions through real-time error calculation and control strategies, and adjust the motion state of the vehicle to achieve the functions of path tracking and dynamic obstacle avoidance. The motion control module includes an error calculation unit, a trajectory tracking unit, a control law generation unit, and a vehicle execution unit. The functions and specific implementation manners of each unit are as follows.

[0142] As an option, the error calculation unit is used to calculate the error between the current position of the vehicle and the optimal path output by the path planning module. Specifically, the error includes two parts: lateral error and heading error. The lateral error represents the shortest distance between the current position of the vehicle and the optimal path, and is used to reflect whether the vehicle deviates from the path; the heading error represents the angle between the current motion direction of the vehicle and the path direction, and is used to describe the direction deviation of the vehicle.

[0143] It should be noted that the lateral error can be calculated by the following formula:

[0144] ;

[0145] Among them, is the current position of the vehicle, is the set of optimal path points, represents the Euclidean distance.

[0146] Course error can be calculated through the angle between the path direction and the vehicle direction:

[0147] ;

[0148] Among them, is the current heading angle of the vehicle, is the direction angle of the nearest path point.

[0149] In a possible implementation, the trajectory tracking unit determines the speed control amount and steering control amount of the vehicle according to the lateral error and course error output by the error calculation unit to reduce the error of the vehicle deviating from the path. Exemplarily, the trajectory tracking can be implemented through the kinematic model of the vehicle, and the kinematic equation of the vehicle is described as:

[0150] ;

[0151] Among them, is the linear velocity of the vehicle, is the angular velocity of the vehicle, is the current heading angle of the vehicle.

[0152] It can be understood that by controlling the linear velocity and angular velocity of the vehicle, the vehicle can adjust its motion state in real time, thereby realizing path tracking.

[0153] As an option, the control law generation unit generates control instructions based on the PID control algorithm. Specifically, the PID control algorithm adjusts the linear velocity and angular velocity of the vehicle through proportional, integral, and derivative controls. The control instructions include:

[0154] The linear velocity control instruction calculated according to the lateral error ;

[0155] The angular velocity control instruction calculated according to the course error .

[0156] The generation formula of the linear velocity control instruction is:

[0157] ;

[0158] Among them, , ,​​ They are the proportional, integral, and derivative gains respectively.

[0159] The angular velocity control command has the following generation formula:

[0160] ;

[0161] It should be noted that by adjusting , , 's values, the control performance can be optimized, for example, making a trade-off between response speed and steady-state accuracy.

[0162] In a possible implementation, the vehicle execution unit receives the linear velocity and angular velocity commands output by the control law generation unit and adjusts the vehicle's motion state in real time. Specifically, the vehicle execution unit achieves precise control of the vehicle's linear velocity and angular velocity by controlling the rotation speed and direction of the drive motor.

[0163] Exemplarily, in a dynamic obstacle scenario, the vehicle execution unit can adjust the motion trajectory according to real-time control commands to avoid dynamic obstacles. For example, when an obstacle suddenly enters the vehicle's forward path, the vehicle execution unit can increase the steering angle or decrease the linear velocity to achieve obstacle avoidance operations.

[0164] It should be noted that the implementation of the motion control module can be extended according to the application scenario. For example, in a complex dynamic environment, the model predictive control (MPC) method can be combined to further optimize the control performance by predicting the vehicle's future state; in a multi-vehicle cooperation scenario, vehicle-to-vehicle communication information can be introduced to achieve global path optimization and collision avoidance.

[0165] In this embodiment, through the collaborative work of the error calculation unit, trajectory tracking unit, control law generation unit, and vehicle execution unit, precise motion control of the vehicle on the optimal path and dynamic obstacle avoidance operations are achieved. This module combines the PID control algorithm with the vehicle kinematic model, which can meet the path tracking requirements in various dynamic environments and provides the system with efficient and stable motion control capabilities.

[0166] Generally speaking, the present invention is applicable to complex dynamic environments of autonomous mobile devices such as driverless forklifts and mobile robots. The system fuses the data of 3D lidar and vision sensors through a data acquisition module to generate environmental information containing the dynamic characteristics of obstacles; the environmental modeling module constructs a dynamic environmental map to model the spatial position, speed, and movement direction of obstacles; the obstacle segmentation module segments the dynamic environmental map based on the maximum flow minimum cut theorem to identify the obstacle area and the free passage area; the path planning module generates a globally optimal path through the principle of least action of Fermat and an optimization algorithm; the motion control module combines error calculation and PID control algorithm to adjust the vehicle motion state in real time to achieve path tracking and dynamic obstacle avoidance. The system of the present invention has high-precision environmental perception, real-time path planning, and efficient obstacle avoidance control capabilities, and can be widely applied to fields such as industrial automation and intelligent logistics.

[0167] Please refer to the attached Figure 2 , the present invention also provides an application method for the adaptive obstacle avoidance control system, which is carried out based on the working process of the foregoing system, and combines the specific steps of multi-modal data acquisition and processing, environmental modeling, obstacle segmentation, path planning, and motion control to achieve adaptive obstacle avoidance and target tracking of the vehicle in a dynamic environment. The specific implementation is as follows:

[0168] S1. Multi-modal data acquisition;

[0169] In this embodiment, first, multi-modal data of the vehicle surrounding environment is collected through a 3D lidar and a vision sensor. The lidar is used to obtain high-precision point cloud data to provide the spatial position and geometric shape information of obstacles; the vision sensor is used to collect environmental images to supplement the category and dynamic information of obstacles.

[0170] As an option, through time synchronization and coordinate transformation methods, the lidar point cloud data and the vision image data are aligned to achieve the fusion preparation of multi-modal data. It should be noted that the collected data will be used as the basis for subsequent environmental modeling.

[0171] S2. Generation of dynamic environmental map;

[0172] In this step, the collected multi-modal data is fused to generate a dynamic environmental map containing the spatial position, speed, and free passage area of obstacles. The fused data is processed by the environmental modeling module, which includes obstacle detection and dynamic characteristic extraction.

[0173] Specifically, the point cloud clustering method is used to identify the position and shape of obstacles, while the visual target detection algorithm is used to identify the obstacle categories, and the Kalman filter is combined to predict the speed and movement direction of obstacles. The dynamic environment map is composed of nodes and edges. The nodes represent the spatial positions of obstacles or free areas, and the edges represent the passage costs between regions. The dynamic environment map provides an environmental representation for subsequent obstacle segmentation and path planning.

[0174] S3. Segmentation of obstacles and free areas;

[0175] Based on the dynamic environment map, using the maximum flow minimum cut theorem, the obstacle area and the free passage area are segmented. In this embodiment, the current position of the vehicle is set as the source point of the network flow model, and the target position is set as the sink point. By calculating the maximum flow from the source point to the sink point and according to the maximum flow minimum cut theorem, the obstacle area and the free passage area are determined.

[0176] It should be noted that the division result of the free passage area will be used as the input for path planning to ensure that the generated path avoids the obstacle area.

[0177] S4. Optimal path planning;

[0178] Within the free passage area, by defining the action of the path, the optimal path with the minimum cost is calculated based on Fermat's principle of least action. The cost of the path is comprehensively determined by the path length and the distance from the obstacles. The goal of the planning is to find a path with the minimum total cost.

[0179] As an option, when it is difficult to solve the optimal path analytically, the discretization method can be used to represent the path as a series of discrete path points, and the position of the path points is iteratively updated through a numerical optimization algorithm to finally generate an optimal path point sequence. It should be noted that the generated optimal path points contain position and motion state information and can be directly used for the motion control module.

[0180] S5. Motion control and dynamic obstacle avoidance;

[0181] According to the optimal path, the motion parameters of the vehicle are adjusted through the PID control algorithm or the model predictive control method to achieve path tracking and dynamic obstacle avoidance.

[0182] Specifically, in this embodiment, first, the errors between the current position of the vehicle and the optimal path are calculated, including the lateral error and the heading error. The error calculation results are used to generate control commands, including the linear velocity and angular velocity commands. The PID control algorithm adjusts the speed and steering of the vehicle in real time according to the error magnitude to ensure that the vehicle moves along the optimal path.

[0183] As a possible implementation, in the scenario of obstacle avoidance for dynamic obstacles, dynamic avoidance of sudden obstacles can be achieved by reducing the vehicle's linear velocity or adjusting the steering angle. Eventually, the vehicle can accurately reach the target position in the dynamic environment.

[0184] The application method of the present invention connects multi-modal data acquisition, dynamic environment modeling, obstacle area segmentation, path planning, and motion control through five main steps. The entire system takes the dynamic environment map as the core, combines the calculation of the optimal path and the adjustment of motion parameters, and completes accurate obstacle avoidance and path tracking in the dynamic environment. Each step in the method can be jointly implemented through specific modules, and has high real-time performance and robustness, and can be widely applied to the dynamic navigation and obstacle avoidance scenarios of equipment such as automated forklifts and mobile robots.

[0185] Although the embodiments of the present invention have been shown and described, those of ordinary skill in the art can understand that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. Adaptive obstacle avoidance control system, characterized in that: include: A data acquisition module, which is used to collect multimodal sensor data of the vehicle's surrounding environment, including 3D lidar point cloud data and visual sensor image data; The environment modeling module is used to fuse the collected multi-modal sensor data to generate a dynamic environment map, where the dynamic environment map includes the spatial position, dynamic characteristics and free passage area of ​​obstacles; An obstacle segmentation module is used to segment the dynamic environment map according to the maximum flow minimum cut theorem to identify obstacle areas and free passage areas; Path planning module, used to calculate the optimal path based on the free passage area through Fermat's principle of least action; The motion control module is used to control the vehicle motion and adjust its trajectory in real time to avoid obstacles according to the optimal path generated by the path planning module; The environment modeling module includes: A data fusion unit is used to receive the point cloud data collected by the 3D laser radar and the image data collected by the visual sensor, and fuse them to generate preliminary environmental information; The obstacle detection unit is used to perform spatial segmentation of point cloud data based on the point cloud clustering method, identify the location and shape of obstacles, and classify the image data based on the visual detection algorithm to supplement the dynamic attribute information of obstacles; A dynamic characteristic extraction unit, used to predict the trajectory of the obstacle through Kalman filtering and extract the dynamic characteristics of the obstacle, including the spatial position, speed and movement direction of the obstacle; A dynamic environment graph generation unit is used to model the fused environment information into a dynamic environment graph, wherein the dynamic environment graph includes a node set and an edge set, the node set represents obstacles and free passage areas, and the weight of the edge set is used to represent the passage cost between areas; The obstacle segmentation module comprises: A dynamic graph construction unit is used to receive the dynamic environment graph generated by the environment modeling module, define the nodes of the dynamic environment graph as obstacles or free passage areas, and set the weights of the edges as the passage costs between the nodes, wherein the passage costs are calculated based on the distance between the nodes, the speed of the obstacles, and the direction of movement of the obstacles; A flow calculation unit is used to construct a network flow model based on a dynamic environment graph, set the current position of the vehicle as the source point of the network flow, set the target position as the sink point of the network flow, and set a capacity value for each edge according to the weight of the edge in the dynamic environment graph, wherein the capacity value is associated with the inverse of the travel cost; The minimum cut segmentation unit is used to calculate the maximum flow value from the source point to the sink point in the network flow through the maximum flow algorithm, and identify the obstacle area and the free passage area based on the maximum flow minimum cut theorem, where the obstacle area is determined by the minimum cut set corresponding to the maximum flow, and the free passage area is the part of the dynamic environment map that does not belong to the obstacle area; The path planning module includes: A path cost calculation unit is used to receive the free passage area output by the obstacle segmentation module, define the cost of the path as the weighted sum of the path length and the obstacle distance cost, wherein the path length cost is calculated based on the distance between the path points, and the obstacle distance cost is determined inversely proportional to the distance between the path point and the nearest obstacle, and assign a comprehensive cost value to each possible path; The optimal path solving unit is used to optimize the path cost based on Fermat's least action principle, build a path action model, define the optimal path as the path with the smallest action, and solve the optimal solution of the path action through a numerical optimization method; Discrete optimization unit, used to discretize the continuous path into a sequence of path points, iteratively update the position of the path points based on the gradient descent algorithm or other numerical optimization algorithms, so as to minimize the comprehensive cost of the path point sequence; A path output unit is used to output the optimized optimal path and represent the path as a sequence of path points from the starting point to the target point. The path points contain position coordinate information and motion state information when passing through the path points. The motion control module comprises: An error calculation unit is used to receive the optimal path output by the path planning module and the current position of the vehicle, and calculate the lateral error and heading error between the current position of the vehicle and the optimal path, wherein the lateral error is the shortest distance between the current position of the vehicle and the path point, and the heading error is the angle between the current moving direction of the vehicle and the path direction; A trajectory tracking unit, used for determining a speed control amount and a steering control amount of the vehicle according to the lateral error and the heading error calculated by the error calculation unit, so as to reduce the error of the vehicle deviating from the path and keep the vehicle moving along the optimal path; A control law generating unit, used for generating control instructions based on a PID control algorithm, wherein the PID control algorithm adjusts the linear speed and angular speed of the vehicle through proportional, integral and differential control, and the control instructions include a speed control instruction calculated according to a lateral error and an angular speed control instruction calculated according to a heading error; The vehicle execution unit is used to receive the control instructions output by the control law generation unit, adjust the linear speed and angular speed of the vehicle in real time, make the vehicle move along the optimal path, and achieve avoidance operations for dynamic obstacles.

2. The adaptive obstacle avoidance control system according to claim 1, characterized in that: The data acquisition module comprises: 3D LiDAR, used to collect point cloud data and identify the spatial location of obstacles; Visual sensors are used to collect image information of the surrounding environment and identify the types of obstacles.

3. The adaptive obstacle avoidance control system according to claim 1, characterized in that: The edge weights of the dynamic environment graph are determined by the following factors: Euclidean distance between nodes; The speed of the obstacle; The angle between the obstacle's moving direction and the current path direction.

4. An application method of an adaptive obstacle avoidance control system, based on the adaptive obstacle avoidance control system according to any one of claims 1 to 3, characterized in that: The following steps are involved: Collect multimodal data of the vehicle's surroundings through 3D lidar and visual sensors; Fuse multimodal data to generate a dynamic environment map, which includes the spatial position, speed and free passage area of ​​obstacles; Based on the maximum flow minimum cut theorem, the obstacle area and the free passage area in the dynamic environment map are segmented; In the free passage area, by defining the action of the path, based on Fermat's principle of least action, the optimal path with the lowest cost is calculated; According to the optimal path, the vehicle motion parameters are adjusted using PID control algorithm or model predictive control method to achieve dynamic obstacle avoidance and accurate tracking of the target position.

Images