Self-adaptive obstacle avoidance control system and application method thereof

Abstract

The invention relates to the field of intelligent mobile equipment control, and discloses a self-adaptive obstacle avoidance control system and an application method thereof, and the system comprises a data collection module, an environment modeling module, an obstacle segmentation module, a path planning module and a motion control module. The data acquisition module is used for acquiring multi-modal data of the surrounding environment of the vehicle; the environment modeling module fuses the multi-modal data, and generates a dynamic environment map including the spatial position, the dynamic characteristic and the free passing area of the obstacle; the obstacle segmentation module is used for segmenting the dynamic environment map and identifying an obstacle area and a free passing area; the path planning module calculates an optimal path with the minimum cost; the motion control module adjusts the motion state of the vehicle in real time through a PID control algorithm or a model prediction control method, and path tracking and dynamic obstacle avoidance are achieved. The method has high-precision environment perception capability, flexible path planning capability and efficient dynamic obstacle avoidance control capability, and can be widely applied to the fields of industrial automation and intelligent logistics.

Description

Technical Field

[0001] The present invention relates to the field of intelligent mobile device control technology, and in particular 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 logistics warehousing, factory automation and other fields. These devices usually need to complete autonomous navigation and obstacle avoidance tasks in dynamic and complex environments. However, achieving accurate environmental perception, path planning and motion control remains a key challenge in existing technologies.

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

[0004] On the other hand, in a dynamic environment, autonomous devices need to accurately distinguish between obstacles and 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 of dynamic obstacles in complex scenarios. For example, they cannot dynamically adjust the segmentation strategy according to the speed or direction of movement of the obstacle, resulting in the navigation path possibly crossing dangerous areas.

[0005] In addition, the real-time performance and flexibility of path planning algorithms in complex scenarios are also challenged. Many existing planning algorithms can only generate the shortest path based on static scenarios, but lack the ability to respond quickly to dynamic environmental changes. Especially in narrow spaces with dense obstacles, path planning algorithms often find it difficult to balance the safety and driving efficiency of the path, resulting in reduced system operating efficiency or potential safety hazards.

[0006] In terms of motion control, traditional control methods lack the ability to adapt to vehicle dynamics and environmental dynamic changes in real time, making it difficult to accurately track the planned path and avoid dynamic obstacles at the same time. When faced with sudden obstacles or environmental changes, existing control algorithms may react slowly or adjust inaccurately, resulting in the vehicle being unable to avoid obstacles in time, increasing safety risks.

[0007] Based on the above problems, the autonomous obstacle avoidance and path planning capabilities of existing technologies in complex dynamic environments need to be further improved. Summary of the invention

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

[0009] To achieve the above objectives, the present invention is implemented through the following technical solutions: an adaptive obstacle avoidance control system, comprising: 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.

[0010] Preferably, the data acquisition module includes: 3D LiDAR, which is used to collect point cloud data and identify the spatial location of obstacles; Vision sensor, used to collect image information of the surrounding environment and identify the type of obstacles.

[0011] Preferably, 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; The 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.

[0012] Preferably, 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.

[0013] Preferably, 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.

[0014] Preferably, 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; 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. The path points contain position coordinate information and motion state information when passing through the path points.

[0015] Preferably, the motion control module includes: 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.

[0016] The present invention also provides an application method of an adaptive obstacle avoidance control system, comprising the following steps: 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.

[0017] The present invention provides an adaptive obstacle avoidance control system and an application method thereof, which has the following beneficial effects: 1. The present invention not only accurately obtains the spatial position of obstacles, but also identifies the category and dynamic characteristics of obstacles through the multimodal data fusion of 3D laser radar and visual sensors. This environmental perception method that combines geometric information and semantic information enables the system to accurately perceive the surrounding situation in complex dynamic environments, providing a reliable foundation for subsequent path planning and motion control.

[0018] 2. The present invention uses the maximum flow minimum cut theorem to segment the dynamic environment map, which can efficiently and clearly separate the obstacle area from the free passage area. Whether it is a static obstacle or a moving dynamic obstacle, this method can effectively deal with it, providing a clear feasible space for path planning and avoiding the risk of collision caused by inaccurate segmentation.

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

[0020] 4. The present invention combines the PID control algorithm to achieve precise adjustment of the vehicle's linear speed and angular speed, enabling the vehicle to track the optimal path in real time. At the same time, the system's dynamic avoidance of sudden obstacles also shows extremely high stability, which can ensure the safe operation of the vehicle in a complex environment and provide reliable protection for dynamic obstacle avoidance and target tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 It is a schematic diagram of the system architecture of the present invention; Figure 2 It is a schematic diagram of the method flow of the present invention. DETAILED DESCRIPTION

[0022] The following will be combined with the drawings in the specification of the present invention 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.

[0023] Please refer to the attached Figure 1The 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.

[0024] 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.

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

[0026] 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.

[0027] 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.

[0028] 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.

[0029] In one possible implementation, the visual sensor collects environmental image data , supplementing the texture, color and other information that LiDAR cannot provide. Exemplarily, the visual 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. Combined with deep learning target detection algorithms (such as YOLO and Mask R-CNN), the visual sensor can identify the category and bounding box of obstacles, such as distinguishing pedestrians, vehicles, and cargo.

[0030] It should be noted that in order to achieve effective fusion of LiDAR point cloud data and visual image data, the present invention uses a timestamp synchronization method to time align the two types of data. Specifically, the LiDAR and visual sensor respectively record the data collection time through a unified timestamp, and correspond the point cloud data and image data to the same time segment.

[0031] As an option, after completing time synchronization, use the external calibration matrix , project the 3D lidar point cloud data into the image coordinate system of the visual sensor. Specifically, assuming that the 3D points in the point cloud Represents the position in the laser radar coordinate system, then its position in the image coordinate system It can be calculated by the following formula: ; in, is the internal parameter matrix of the visual sensor, It is the external parameter transformation matrix from the laser radar coordinate system to the visual sensor coordinate system.

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

[0033] It is understandable that in order to improve the reliability of data collection, the present embodiment pre-processes the collected data of the laser radar and the visual sensor. For example, the abnormal points in the point cloud data are removed by a denoising filtering method, and the commonly used denoising methods include statistical filtering and radius filtering. In addition, for visual image data, the image quality under low light or strong light conditions can be improved by an image enhancement method.

[0034] Specifically, the denoising filtering of point cloud data can be achieved based on the following steps: 1. Count the distance distribution of neighboring points around each point and remove abnormal points that are beyond the normal distribution range; 2. In radius filtering, a fixed neighborhood radius is set to remove isolated points with too few points in the neighborhood.

[0035] In another possible implementation, the visual image data can be smoothed by using a histogram equalization method to improve contrast, thereby enhancing image details. For the depth image data, a bilateral filtering method can also be used to remove noise while retaining edge characteristics.

[0036] It should be noted that the multimodal data acquisition module of this embodiment has high flexibility, and different types of laser radars and visual sensors can be selected according to the application scenario. For example, in an indoor environment, a lightweight laser radar and a low-power camera can be selected; in a complex outdoor environment, a sensor with a large range and high dynamic range can be selected.

[0037] It can be understood that the data acquisition module is the basis of the system of the present invention, and the multimodal data acquired by the data acquisition module directly affects the performance of subsequent modules.

[0038] In this embodiment, the environment modeling module is used to receive the multimodal sensor data provided by the data acquisition module, generate a dynamic environment map through data fusion and obstacle detection, and provide accurate environment modeling results 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.

[0039] 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 to describe the geometric characteristics of obstacles, while the image data provides visual characteristics such as color and texture of obstacles to supplement the detailed information of the point cloud data. The combination of the two makes the environment information more comprehensive.

[0040] Specifically, the data fusion unit receives the time-synchronized point cloud data and image data , using the above coordinate transformation formula, the point cloud is projected into the image coordinate system to obtain the fused environment information. The fusion result is a set of point cloud data with semantic labels, in which each point contains not only the spatial coordinates , but also includes visual features such as color and category.

[0041] In a possible implementation, the obstacle detection unit first performs cluster analysis on the point cloud data to identify the position and shape of obstacles in the space. Exemplarily, the clustering method can use the DBSCAN algorithm to cluster dense point cloud areas by calculating the number of neighborhood points within a fixed radius for each point and remove isolated points. The identified point cloud clustering results represent obstacles in the environment.

[0042] At the same time, the obstacle detection unit performs target detection and classification based on the visual image data. As an option, a deep learning target detection algorithm, such as YOLO or Mask R-CNN, can be used to identify obstacles in the image as categorized targets, such as pedestrians, vehicles, or cargo. It should be noted that the classification results can be used to enhance the semantic information of obstacles in the point cloud data, thereby improving the accuracy of environmental modeling.

[0043] It should be noted that in order to model the dynamic behavior of the obstacle, the dynamic characteristic extraction unit predicts the movement trajectory of the obstacle through Kalman filtering. Kalman filtering uses the historical position of the obstacle and speed As input, predict its future position and direction of movement. The prediction formula can be expressed as: ; ; in, is the state vector, which represents the current position and velocity of the obstacle; is the control vector, which represents the possible external influence; is the observation vector, which represents the collected sensor data; and They are the state transfer matrix and the observation matrix respectively; and is the noise vector.

[0044] As a possible implementation, the Kalman filter outputs a prediction including the future position of the obstacle. and direction of movement ,This information is used for the generation of dynamic environment graph.

[0045] The dynamic environment graph generation unit constructs a dynamic environment graph based on the fused environment information and dynamic characteristic prediction results. It should be noted that the dynamic environment graph consists of a set of nodes and a set of edges, where nodes represent the locations of obstacles or free passage areas, and edges are used to describe the passability between nodes. The weight of each edge is Characterizing slave nodes To Node The cost of passage.

[0046] Specifically, the edge weight Consider the following factors: node and The Euclidean distance between ; The speed of the obstacle where the node is located ; The angle between the obstacle's moving direction and the current path direction .

[0047] Alternatively, edge weights can be calculated as follows: ; in, and To adjust the parameters, we weigh the impact of distance, speed and direction on the travel cost. and The value of can adapt to the environmental modeling requirements in different scenarios. For example, the speed weight can be increased in high dynamic scenes, and the direction weight can be increased in complex obstacle scenes.

[0048] In some embodiments, the dynamic environment graph can also be expanded into a three-dimensional graph model, where nodes not only contain plane positions , also contains height information This has important implications for path planning in multi-story environments such as high-bay warehouses.

[0049] 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. This embodiment generates a high-precision and scalable dynamic environment representation through multimodal data fusion, obstacle dynamic characteristics extraction and environmental information modeling, providing reliable data support for dynamic obstacle avoidance.

[0050] It is understandable that the implementation of the environmental modeling module can be expanded according to specific scenarios and needs, such as adding the fusion of GPS data in outdoor environments and introducing V2X communication information in dynamic traffic scenarios.

[0051] 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 map construction unit, a flow calculation unit and a minimum cut segmentation unit. The functions and specific implementation methods of each unit are as follows.

[0052] As an option, the dynamic graph construction unit receives a dynamic environment graph generated by the environment modeling module, wherein the dynamic environment graph is in the form of a graph structure. Indicates that is a set of nodes, representing the location of obstacles or free areas, is a set of edges, representing the connection relationship between nodes. Specifically, each edge With weight , the weight value represents the slave node To Node The cost of passage.

[0053] It should be noted that the calculation of the pass cost comprehensively considers the geometric distance between nodes, the speed of obstacles, and the angle between the obstacle's movement direction and the path direction. As an implementation method, the pass cost can be calculated by the following formula: ; in, For Node and The Euclidean distance between is the projection value of the obstacle speed in the edge direction, is the angle between the obstacle’s moving direction and the path’s direction, and It is a tuning parameter used to adjust the weight of speed and direction on the travel cost.

[0054] In a possible implementation, the traffic calculation unit constructs a network flow model based on the dynamic environment graph. Exemplarily, the network flow model defines the current position of the vehicle as the source point , defining the target location as the sink For each edge Set capacity value , capacity value and edge travel cost The capacity value is associated with the reciprocal of , which is used to reflect the traffic capacity of the edge. The calculation formula of the capacity value is: ; It should be noted that the capacity value is defined in such a way that the smaller the travel cost (i.e. the better the path), the larger the capacity value, thus being more inclined to select a low-cost path for network flow calculation.

[0055] As an option, the flow calculation unit uses a classical maximum flow algorithm (such as the Ford-Fulkerson algorithm) to calculate the maximum flow from the source point To the meeting point Specifically, the flow calculation includes the following steps: Initialize network stream values , indicating that the flow of each edge is zero initially.

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

[0057] Update the flow value of each edge , and adjust the remaining capacity .

[0058] The process of finding augmenting paths is repeated until no new augmenting paths can be found.

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

[0060] The minimum cut segmentation unit extracts the minimum cut set from the result of the flow calculation unit based on the maximum flow minimum cut theorem. Specifically, the minimum cut is the network that separates the source point Hehuidian The minimum capacity set separated by the maximum flow and minimum cut theorem, the capacity corresponding to the minimum cut set is equal to the maximum flow value.

[0061] In a possible implementation, the minimum cut segmentation unit defines the minimum cut set as the obstacle area, that is, the nodes related to the minimum cut set in the dynamic environment graph 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.

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

[0063] As an extension, 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 according to the dynamic characteristics of the obstacle (such as speed, direction of movement) to increase the flexibility of the free passage area. This extension does not change the core design of the present invention.

[0064] It is understandable 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 the weight calculation formula) can be adjusted. and ) to meet the needs of different scenarios.

[0065] This embodiment realizes efficient segmentation of obstacle areas and free passage areas in a dynamic environment through the collaborative work of the dynamic graph construction unit, the flow calculation unit and the minimum cut segmentation unit, and provides reliable data support for the path planning module.

[0066] In this embodiment, the path planning module is used to receive the free passage area output by the obstacle segmentation module, and generate the optimal path based on the Fermat least action principle 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 method is as follows.

[0067] 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 is composed 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 the path points, and 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 expressed by the following formula: ; in: represents the comprehensive cost of the path; is the weight of the path length cost, which is used to adjust the impact of the path length in the cost value; is the weight of the obstacle distance cost, which is used to adjust the priority of path safety; is the speed of the path, indicating the length of the path; Indicates waypoints The distance to the nearest obstacle.

[0068] It should be noted that reasonable adjustments and The value of can make the path cost function adapt to the needs of different scenarios. For example, in a complex obstacle environment, it can increase The weights are used to prioritize avoiding obstacles.

[0069] In one possible implementation, the optimal path solving unit uses the path cost function as the optimization target based on Fermat's least action principle to find the optimal solution for the path. Specifically, the path planning problem is modeled as a minimum action problem, and its goal is to minimize the action SSS of the path: ; in, is the Lagrangian function of the path, and the path cost value Definition consistent.

[0070] 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: ; By calculating the partial derivatives of the path action, the analytical solution or numerical solution of the optimal path can be obtained.

[0071] As an alternative, when the 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 using numerical optimization methods. Specifically, the discrete path is represented as a set of path points , where each path point Contains location coordinates and time step The discrete path cost can be expressed as: ; in, represents the Euclidean distance between adjacent path points, Indicates waypoints The distance to the nearest obstacle.

[0072] Exemplarily, the discrete optimization unit uses a gradient descent algorithm to update the position of the path point. The iterative update formula of the gradient descent is: ; in, is the learning rate, is the gradient of the path cost with respect to the path points.

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

[0074] In a possible implementation, 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 contains a position coordinate and the motion state (such as speed and direction) when passing through this point. The optimization results can be directly used in subsequent motion control modules.

[0075] As an extension, 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 added to the path points to support time-constrained path planning; in a three-dimensional environment, the three-dimensional coordinates of the path points can be output. .

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

[0077] 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, generates control instructions through real-time error calculation and control strategy, adjusts the motion state of the vehicle, and realizes the path tracking and dynamic obstacle avoidance functions. 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 methods of each unit are as follows.

[0078] 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, which is used to reflect whether the vehicle deviates from the path; the heading error represents the angle between the current direction of movement of the vehicle and the direction of the path, which is used to describe the direction deviation of the vehicle.

[0079] It should be noted that the lateral error It can be calculated by the following formula: ; in, is the current position of the vehicle, is the optimal path point set, represents the Euclidean distance.

[0080] Heading Error It can be calculated by the angle between the path direction and the vehicle direction: ; in, is the current heading angle of the vehicle, is the direction angle of the nearest path point.

[0081] In a possible implementation, the trajectory tracking unit determines the speed control amount and the steering control amount of the vehicle according to the lateral error and the heading 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 by a kinematic model of the vehicle, and the kinematic equation of the vehicle is described as: ; in, is the linear speed of the vehicle, is the angular velocity of the vehicle, is the current heading angle of the vehicle.

[0082] 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 achieving path tracking.

[0083] As an option, the control law generation unit generates control instructions based on a PID control algorithm. Specifically, the PID control algorithm adjusts the linear speed and angular speed of the vehicle through proportional, integral and differential control. The control instructions include: According to the lateral error Calculated line speed control instructions ; According to the heading error Calculated angular velocity control command .

[0084] Line speed control command The formula for generating is: ; in, , , are the proportional, integral and derivative gains respectively.

[0085] Angular velocity control command The formula for generating is: ; It should be noted that by adjusting , , The value of can be used to optimize the control performance, for example, to balance the response speed with the steady-state accuracy.

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

[0087] For example, in a dynamic obstacle scenario, the vehicle execution unit can adjust the motion trajectory according to the real-time control instructions to avoid the dynamic obstacle. For example, when an obstacle suddenly enters the vehicle's forward path, the vehicle execution unit can increase the steering angle to avoid the dynamic obstacle. Or reduce line speed Obstacle avoidance operation is achieved in this way.

[0088] It should be noted that the implementation of the motion control module can be expanded according to the application scenario. For example, in complex dynamic environments, the model predictive control (MPC) method can be combined to further optimize the control performance by predicting the future state of the vehicle; in multi-vehicle cooperative scenarios, communication information between vehicles can be introduced to achieve global path optimization and collision avoidance.

[0089] This embodiment realizes precise motion control and dynamic obstacle avoidance of the vehicle on the optimal path through the coordinated work of the error calculation unit, trajectory tracking unit, control law generation unit and vehicle execution unit. This module combines the PID control algorithm with the vehicle kinematic model to meet the path tracking requirements in a variety of dynamic environments, providing the system with efficient and stable motion control capabilities.

[0090] In general, the present invention is suitable for complex dynamic environments of autonomous mobile devices such as unmanned forklifts and mobile robots. The system fuses the data of 3D laser radar and visual sensors through the 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 obstacle areas and free passage areas; the path planning module generates a global optimal path through the Fermat minimum action principle and 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 used in industrial automation, intelligent logistics and other fields.

[0091] Please refer to the attached Figure 2 The present invention also provides an application method of an adaptive obstacle avoidance control system, which is based on the workflow of the aforementioned system and combines the specific steps of multimodal data collection and processing, environment modeling, obstacle segmentation, path planning, and motion control to achieve adaptive obstacle avoidance and target tracking of vehicles in dynamic environments. The specific implementation methods are as follows: S1, multimodal data acquisition; In this embodiment, multimodal data of the vehicle's surrounding environment is first collected through 3D laser radar and visual sensors. The laser radar is used to obtain high-precision point cloud data and provide information on the spatial position and geometric shape of obstacles; the visual sensor is used to collect environmental images and supplement the category and dynamic information of obstacles.

[0092] As an option, the lidar point cloud data is aligned with the visual image data through time synchronization and coordinate transformation methods to achieve multimodal data fusion preparation. It should be noted that the collected data will serve as the basis for subsequent environmental modeling.

[0093] S2, dynamic environment map generation; In this step, the collected multimodal data are fused to generate a dynamic environment map containing the spatial position, speed and free passage area of ​​obstacles. The fused data is processed by the environment modeling module, which includes obstacle detection and dynamic feature extraction.

[0094] 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 category, and the Kalman filter is combined to predict the speed and movement direction of the obstacle. The dynamic environment graph consists of nodes and edges. The nodes represent the spatial position of obstacles or free areas, and the edges represent the travel cost between areas. The dynamic environment graph provides an environment representation for subsequent obstacle segmentation and path planning.

[0095] S3, segmentation of obstacles and free areas; Based on the dynamic environment graph, the maximum flow minimum cut theorem is used to segment the obstacle area from the free passage area. 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. The maximum flow from the source point to the sink point is calculated, and the obstacle area and the free passage area are determined according to the maximum flow minimum cut theorem.

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

[0097] S4, optimal path planning; In 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 determined by the path length and the distance to the obstacle. The planning goal is to find a path with the minimum total cost.

[0098] As an option, when it is difficult to analytically solve the optimal path, a discretization method can be used to represent the path as a series of discrete path points, and the path point positions can be 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 in the motion control module.

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

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

[0101] As a possible implementation method, in the obstacle avoidance scenario of dynamic obstacles, the vehicle can be dynamically avoided by reducing the linear speed of the vehicle or adjusting the steering angle. Ultimately, the vehicle can accurately reach the target location in a dynamic environment.

[0102] The application method of the present invention connects multimodal data acquisition, dynamic environment modeling, obstacle area segmentation, path planning and motion control in series through five main steps. The whole system takes the dynamic environment map as the core, combines the optimal path calculation and motion parameter adjustment, and completes accurate obstacle avoidance and path tracking in a dynamic environment. Each step in the method can be implemented collaboratively through specific modules, and has high real-time and robustness, and can be widely used in dynamic navigation and obstacle avoidance scenarios of unmanned forklifts, mobile robots and other equipment.

[0103] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that 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.

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 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; The 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.

4. The adaptive obstacle avoidance control system according to claim 3, 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.

5. The adaptive obstacle avoidance control system according to claim 1, characterized in that: 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.

6. The adaptive obstacle avoidance control system according to claim 1, characterized in that: 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; 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. The path points contain position coordinate information and motion state information when passing through the path points.

7. The adaptive obstacle avoidance control system according to claim 1, characterized in that: 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.

8. 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 7, 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