A dynamic obstacle avoidance motion planning method and system

Abstract

The application relates to the technical field of robot control, in particular to a dynamic obstacle avoidance motion planning method and system. The method comprises the following steps: collecting initial environment data of a working area, constructing a three-dimensional obstacle model according to the initial environment data; setting an initial motion path of a robot according to the three-dimensional obstacle model and a preset path decision model, and setting a perception strategy according to the initial motion path; acquiring environment monitoring data according to the perception strategy, and judging whether to generate a correction instruction of the initial motion path according to the environment monitoring data. Through pre-dividing obstacle value intervals according to historical data and configuring iteration sub-strategies including iteration times, confidence domain adjustment strategies and other parameters for each interval, an execution library that can be quickly called is formed. In actual planning, the system can match the most suitable preset strategy according to the initial obstacle value calculated in real time, avoids repeated trial and error of a planner in a complex scene, and significantly improves the generation efficiency and success rate of an initial trajectory.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a dynamic obstacle avoidance motion planning method and system. Background Technology

[0002] In the field of robot motion planning, the Grasping Optimized Motion Planning (GOMP) algorithm achieves time-optimal trajectory planning under multiple grasping postures by combining grasping analysis and sequential quadratic programming (SQP) optimization. However, this technology faces two major bottlenecks in practical applications.

[0003] First, the computational real-time performance is insufficient. Its core SQP optimization process is computationally intensive, especially after introducing higher-order dynamic constraints, where the solution time can reach tens of seconds, making it difficult to meet the real-time requirements of high-frequency replanning. Although some studies have used deep learning for warm-start to shorten the time to milliseconds, this has not fundamentally changed the computational complexity of the optimization framework.

[0004] Secondly, it lacks the ability to handle dynamic environments. Traditional GOMP performs collision detection based on static environment assumptions, with obstacle models remaining fixed. In real-world scenarios such as warehousing and industry, it cannot perform real-time perception and trajectory prediction of dynamic obstacles such as moving robots and personnel, leading to collision risks in the planned path and insufficient safety. Summary of the Invention

[0005] The purpose of this application is to provide a dynamic obstacle avoidance motion planning method and system to solve the above-mentioned technical problems, aiming to improve the dynamic obstacle avoidance capability of robots.

[0006] In some embodiments of this application, an execution library that can be quickly invoked is formed by pre-dividing obstacle value intervals based on historical data and configuring iterative sub-strategies for each interval, including parameters such as the number of iterations and trust region adjustment strategies. During actual planning, the system can match the most suitable preset strategy based on the initial obstacle values ​​calculated in real time, avoiding repeated trial and error by the planner in complex scenarios and significantly improving the efficiency and success rate of initial trajectory generation.

[0007] In some embodiments of this application, multiple sensors are used to perceive and predict moving obstacles in real time, accurately locate risk areas, and determine the starting and ending points of corrections accordingly. Local path optimization is then performed, making the obstacle avoidance process more continuous and smooth. This effectively avoids sudden stops or replanning failures caused by the sudden appearance of obstacles, and improves the robustness of the overall system.

[0008] In some embodiments of this application, a dynamic obstacle avoidance motion planning method is provided, including:

[0009] Collect initial environmental data of the work area and construct a three-dimensional obstacle model based on the initial environmental data;

[0010] The robot's initial motion path is set based on the 3D obstacle model and the preset path decision model, and the perception strategy is set based on the initial motion path.

[0011] The system acquires environmental monitoring data based on the perception strategy and determines whether to generate a correction command for the initial motion path based on the environmental monitoring data.

[0012] In some embodiments of this application, the preset path decision model includes:

[0013] Construct a path planning model;

[0014] Multiple obstacle value ranges are set based on historical planning parameters;

[0015] Establish a sequence A of barrier value intervals;

[0016] A=(a1,a2…a i …a n ), where a i Let n be the i-th obstacle value interval; n is the number of obstacle value intervals.

[0017] Based on the obstacle value interval sequence A, a is set sequentially. i The target obstacle value range;

[0018] An iterative sub-strategy that sets the target obstacle value range;

[0019] The iterative sub-strategy includes: number of iterations, trust region adjustment strategy, and time node distribution;

[0020] Sequentially set the iterative sub-strategies for each obstacle value range, and build an execution library based on all the iterative sub-strategies;

[0021] Establish a path decision model based on the execution library and path planning model.

[0022] In some embodiments of this application, setting the initial motion path includes:

[0023] Construct an initial 3D model based on the work area;

[0024] Obtain initial environmental data for the work area;

[0025] A three-dimensional voxel map is constructed based on the processing results of the initial environmental data using a pre-defined obstacle recognition model.

[0026] The three-dimensional voxel map includes multiple obstacle sub-regions, and the initial obstacle value of the working area is set according to all obstacle sub-regions.

[0027] Generate a 3D obstacle model based on the 3D voxel map and the initial 3D model;

[0028] Set planning sub-strategies based on initial obstacle values ​​and path decision models;

[0029] Obtain task requirements and generate an initial motion path based on task requirements and planning sub-strategies;

[0030] The task requirements include a starting point and an ending point.

[0031] In some embodiments of this application, the setting of the perception strategy includes:

[0032] The initial running path is segmented, and multiple sub-paths are generated based on the segmentation results;

[0033] Establish a sub-path sequence B, B=(b1,b2…b i …b m ), where b i Let m be the i-th sub-path; m is the number of sub-paths.

[0034] Based on the sub-path sequence B, bi is sequentially set as the target sub-path;

[0035] The dynamic risk value of the target sub-path is set based on the three-dimensional obstacle model;

[0036] The perception parameters of the target sub-path are set according to the dynamic risk value, and the perception parameters include: perception range and data acquisition frequency;

[0037] Set the perception parameters for each sub-path in sequence, and set the perception strategy based on all the perception parameters.

[0038] In some embodiments of this application, the step of determining whether to generate a correction instruction for the initial motion path includes:

[0039] Select the sub-path to be monitored based on the robot's position parameters;

[0040] Obtain environmental monitoring data for the sub-path to be monitored, and establish a local three-dimensional model based on the environmental monitoring data;

[0041] Multiple dynamic obstacle points are generated based on the 3D obstacle model and the local 3D model;

[0042] Generate the expected motion trajectory of each dynamic obstacle point;

[0043] A dynamic obstacle risk value f is generated based on all expected motion trajectories;

[0044] Preset dynamic obstacle risk threshold F1;

[0045] If f > F1, generate correction instructions for the initial motion path.

[0046] In some embodiments of this application, generating the expected trajectory of each dynamic obstacle point includes:

[0047] A trajectory prediction model is established based on historical environmental monitoring data, and the trajectory prediction model includes multiple prediction sub-models.

[0048] Establish a dynamic obstacle point sequence H, H=(h1, h2…h i …h r ), where h i Let r be the i-th dynamic obstacle point; r is the number of dynamic obstacle points.

[0049] Based on the dynamic obstacle point sequence H, hi is sequentially set as the target obstacle point;

[0050] Set up a time-series monitoring package for the target obstacle point based on the environmental monitoring data of the sub-path to be monitored;

[0051] Historical trajectory parameters of the target obstacle point are generated based on the time-series monitoring package;

[0052] The matching values ​​between the target obstacle point and each prediction sub-model are generated based on historical trajectory parameters.

[0053] The prediction sub-model corresponding to the maximum value in the matching values ​​is set as the first-level prediction model;

[0054] Generate the expected trajectory of the target obstacle point based on historical trajectory parameters and a primary prediction model;

[0055] The expected trajectory of each dynamic obstacle point is generated sequentially.

[0056] In some embodiments of this application, the correction instructions include:

[0057] Establish a sequence B1 of associated sub-paths based on the sub-paths to be monitored;

[0058] B1=(b 11 ,b 12 …b 1i …b 1m1 ), where b 1i Let m1 be the i-th associated sub-path of the sub-path to be monitored; m1 is the number of associated sub-paths of the sub-path to be monitored, and m1 <m;

[0059] The interference risk value of each associated sub-path is generated based on the expected motion trajectory of all dynamic obstacle points;

[0060] Preset interference risk threshold K1;

[0061] If K1 <k i (i=1, 2…m1), the i-th associated sub-path is designated as a risky sub-path;

[0062] Where, k i Let m1 be the interference risk value of the i-th associated sub-path, and m1 be the number of associated sub-paths of the sub-path to be monitored.

[0063] Obtain all risk sub-paths and set the correction start and correction end points based on all risk sub-paths;

[0064] A dynamic obstacle model is constructed based on the correction start and correction end points;

[0065] The robot's corrected motion path is set based on the dynamic obstacle model and path decision model.

[0066] In some embodiments of this application, a dynamic obstacle avoidance motion planning system is provided, including:

[0067] The monitoring unit is used to collect initial environmental data of the work area;

[0068] The central control unit is used to construct a three-dimensional obstacle model based on the initial environmental data;

[0069] The central control unit includes:

[0070] The first processing module is used to set the robot's initial motion path based on the three-dimensional obstacle model and the preset path decision model.

[0071] The second processing module is used to set the perception strategy based on the initial motion path;

[0072] The third processing module is used to acquire environmental monitoring data according to the perception strategy, and to determine whether to generate a correction instruction for the initial motion path based on the environmental monitoring data.

[0073] The first processing module is also used for:

[0074] Construct a path planning model;

[0075] Multiple obstacle value ranges are set based on historical planning parameters;

[0076] Establish a sequence A of barrier value intervals;

[0077] A=(a1,a2…a i …a n ), where a i Let n be the i-th obstacle value interval; n is the number of obstacle value intervals.

[0078] Based on the obstacle value interval sequence A, a is set sequentially. i The target obstacle value range;

[0079] An iterative sub-strategy that sets the target obstacle value range;

[0080] The iterative sub-strategy includes: number of iterations, trust region adjustment strategy, and time node distribution;

[0081] Sequentially set the iterative sub-strategies for each obstacle value range, and build an execution library based on all the iterative sub-strategies;

[0082] Establish a path decision model based on the execution library and path planning model.

[0083] In some embodiments of this application, the first processing module is further configured to:

[0084] Construct an initial 3D model based on the work area;

[0085] Obtain initial environmental data for the work area;

[0086] A three-dimensional voxel map is constructed based on the processing results of the initial environmental data using a pre-defined obstacle recognition model.

[0087] The three-dimensional voxel map includes multiple obstacle sub-regions, and the initial obstacle value of the working area is set according to all obstacle sub-regions.

[0088] Generate a 3D obstacle model based on the 3D voxel map and the initial 3D model;

[0089] Set planning sub-strategies based on initial obstacle values ​​and path decision models;

[0090] Obtain task requirements and generate an initial motion path based on task requirements and planning sub-strategies;

[0091] The task requirements include a starting point and an ending point.

[0092] In some embodiments of this application, the second processing module is further configured to:

[0093] The initial running path is segmented, and multiple sub-paths are generated based on the segmentation results;

[0094] Establish a sub-path sequence B, B=(b1,b2…b i …b m ), where b i Let m be the i-th sub-path; m is the number of sub-paths.

[0095] Based on the sub-path sequence B, bi is sequentially set as the target sub-path;

[0096] The dynamic risk value of the target sub-path is set based on the three-dimensional obstacle model;

[0097] The perception parameters of the target sub-path are set according to the dynamic risk value, and the perception parameters include: perception range and data acquisition frequency;

[0098] Set the perception parameters for each sub-path in sequence, and set the perception strategy based on all the perception parameters.

[0099] Compared with the prior art, the dynamic obstacle avoidance motion planning method and system of this application have the following advantages:

[0100] By pre-dividing obstacle value intervals based on historical data and configuring iterative sub-strategies for each interval, including parameters such as the number of iterations and trust region adjustment strategies, a quickly invoked execution library is formed. During actual planning, the system can match the most suitable pre-set strategy based on the initial obstacle values ​​calculated in real time, avoiding repeated trial and error by the planner in complex scenarios and significantly improving the efficiency and success rate of initial trajectory generation.

[0101] By using multiple sensors to perceive and predict moving obstacles in real time, the system can accurately locate risk areas and determine the starting and ending points for corrections, thereby optimizing local paths. This makes the obstacle avoidance process more continuous and smooth, effectively avoiding sudden stops or replanning failures caused by the sudden appearance of obstacles, and improving the overall robustness of the system. Attached Figure Description

[0102] Figure 1 This is a flowchart illustrating a dynamic obstacle avoidance motion planning method in a preferred embodiment of this application. Detailed Implementation

[0103] The specific embodiments of this application will be described in further detail below with reference to the accompanying drawings and examples. The following examples are used to illustrate this application, but are not intended to limit the scope of this application.

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

[0105] The terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this application, unless otherwise stated, "a plurality of" means two or more.

[0106] In the description of this application, it should be noted that, unless otherwise expressly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection between two components. Those skilled in the art can understand the specific meaning of the above terms in this application based on the specific circumstances.

[0107] like Figure 1 As shown, a preferred embodiment of the present application provides a dynamic obstacle avoidance motion planning method, comprising:

[0108] S101: Collect initial environmental data of the work area and construct a three-dimensional obstacle model based on the initial environmental data;

[0109] S102: Set the robot's initial motion path based on the 3D obstacle model and the preset path decision model, and set the perception strategy based on the initial motion path;

[0110] S103: Obtain environmental monitoring data according to the perception strategy, and determine whether to generate a correction command for the initial motion path based on the environmental monitoring data.

[0111] Specifically, the working area is scanned by an additional monitoring unit (multi-source sensors such as lidar, stereo vision camera, and depth camera).

[0112] Specifically, the scope of the work area is determined according to the task requirements, and the work area is scanned and relevant environmental data is collected by the added monitoring units (multi-source sensors such as lidar, stereo vision camera and depth camera) (lidar (to acquire high-precision 3D point cloud at long distance), stereo vision camera (to provide texture and color information), and depth camera (to assist in fine detection at close range)), thereby generating the initial environmental data of the work area.

[0113] Specifically, the pre-defined path decision model includes:

[0114] Construct a path planning model;

[0115] Multiple obstacle value ranges are set based on historical planning parameters;

[0116] Establish a sequence A of barrier value intervals;

[0117] A=(a1,a2…a i …a n ), where a i Let n be the i-th obstacle value interval; n is the number of obstacle value intervals.

[0118] Based on the obstacle value interval sequence A, a is set sequentially. i The target obstacle value range;

[0119] An iterative sub-strategy that sets the target obstacle value range;

[0120] The iterative sub-strategy includes: number of iterations, trust region adjustment strategy, and time node distribution;

[0121] Sequentially set the iterative sub-strategies for each obstacle value range, and build an execution library based on all the iterative sub-strategies;

[0122] Establish a path decision model based on the execution library and path planning model.

[0123] Specifically, a path planning model is constructed based on offline training deep neural network technology. The path planning model can directly output an initial trajectory that meets the basic kinematic constraints according to the task requirements, making it close to the final solution.

[0124] Specifically, the historical planning parameters include recorded data of the robot's SQP iterative optimization in different obstacle scenarios, including the adjustment strategy of the time interval for iterative optimization, the number of iterations to obtain the optimal solution, and the adjustment process of the decision step size.

[0125] Specifically, by evaluating and aggregating different obstacle scenarios, multiple obstacle value ranges are constructed. Based on the relevant historical planning parameters of the obstacle scenarios corresponding to the target obstacle value range, associated record data of the target obstacle value range is generated. By analyzing the associated record data, an iterative sub-strategy for the target obstacle value range is set.

[0126] Specifically, the number of iterations refers to the optimal number of iterations required for the target obstacle value range, the trust region adjustment strategy refers to the adjustment strategy of the single decision step size (the mapping relationship between the degree of fit and the step size) based on the effect of the previous iteration when SQP is updated in each iteration, and the time node distribution refers to the time node distribution density parameter corresponding to the robot in each category of time period (the time period when it is close to static / dynamic obstacles, the time period when it interacts with the predicted trajectory of dynamic obstacles, and the time period when it moves in uniform linear motion) when the obstacle value in the working area is within the target obstacle value range.

[0127] Specifically, by setting an iterative sub-strategy for the target obstacle value range, the convergence speed of the decision model is optimized, and the solution process time is reduced.

[0128] It is understandable that in the above embodiments, by pre-dividing obstacle value intervals based on historical data and configuring iterative sub-strategies, including parameters such as the number of iterations and trust region adjustment strategies, an execution library that can be quickly invoked is formed. During actual planning, the system can match the most suitable preset strategy based on the initial obstacle values ​​calculated in real time, avoiding repeated trial and error by the planner in complex scenarios and significantly improving the efficiency and success rate of initial trajectory generation.

[0129] In a preferred embodiment of this application, setting an initial motion path includes:

[0130] Construct an initial 3D model based on the work area;

[0131] Obtain initial environmental data for the work area;

[0132] A three-dimensional voxel map is constructed based on the processing results of the initial environmental data using a pre-defined obstacle recognition model.

[0133] The 3D voxel map includes multiple obstacle sub-regions, and the initial obstacle value of the working area is set based on all obstacle sub-regions.

[0134] Generate a 3D obstacle model based on the 3D voxel map and the initial 3D model;

[0135] Set planning sub-strategies based on initial obstacle values ​​and path decision models;

[0136] Obtain task requirements and generate an initial motion path based on task requirements and planning sub-strategies;

[0137] The task requirements include the starting point and the ending point of the movement.

[0138] Specifically, an initial three-dimensional model is constructed based on the range parameters of the working area. This initial three-dimensional model is a three-dimensional spatial model that includes the entire range of the working area.

[0139] Specifically, multi-source fusion is performed on the initial environmental data collected based on hardware time synchronization and spatial calibration. An obstacle recognition model is then constructed based on a deep learning-based 3D instance segmentation network. This model processes the fused point cloud data to identify and classify different obstacles (such as shelves, equipment, and pedestrians), generating a 3D voxel map with semantic labels. This map not only contains geometric information but also labels the obstacle type, possible motion attributes (static / dynamic), and safety distance parameters. By combining the 3D voxel map with the initial 3D model, a 3D obstacle model of the work area is constructed.

[0140] Specifically, the initial obstacle value of the working area is generated based on the total number of obstacles, the number of dynamic obstacles, and the obstacle point distribution density (the ratio of the area of ​​the area with obstacles to the area of ​​the working area) in the three-dimensional obstacle model.

[0141] Specifically, the total number of obstacles, the number of dynamic obstacles, and the density of obstacle distribution are quantified to ensure that the reference values ​​of each parameter are within the same range. Furthermore, the larger the value of each parameter, the larger the corresponding reference value. The mapping relationship between the two can be set based on historical parameters. By weighting the parameter values ​​corresponding to each parameter in the 3D obstacle model, the initial obstacle values ​​for the working area are obtained. The weight coefficient of each parameter can be set according to its degree of interference with path optimization; the greater the interference, the larger the corresponding weight coefficient. The mapping relationship between the two can be set based on historical parameters, and the sum of the weight coefficients of each parameter is 1.

[0142] Specifically, the iterative sub-policy that selects the obstacle value interval corresponding to the initial obstacle value is the execution iterative policy.

[0143] Specifically, the planning sub-strategy includes an execution iteration strategy and a path planning model. The path planning model directly outputs an initial trajectory that satisfies basic kinematic constraints based on the three-dimensional obstacle model, the starting point of the movement, and the ending point of the movement. The initial trajectory is then optimized and iterated according to the execution iteration strategy, and the initial movement path is output based on the iteration results.

[0144] In a preferred embodiment of this application, a perception strategy is set, including:

[0145] The initial running path is segmented, and multiple sub-paths are generated based on the segmentation results;

[0146] Establish a sub-path sequence B, B=(b1,b2…b i …b m ), where b i Let m be the i-th sub-path; m is the number of sub-paths.

[0147] Based on the sub-path sequence B, bi is sequentially set as the target sub-path;

[0148] The dynamic risk value of the target sub-path is set based on the three-dimensional obstacle model;

[0149] The perception parameters of the target sub-path are set according to the dynamic risk value. The perception parameters include: perception range and data collection frequency.

[0150] Set the perception parameters for each sub-path in sequence, and set the perception strategy based on all the perception parameters.

[0151] Specifically, the length of a single sub-path is set based on the initial obstacle value. The larger the initial obstacle value, the shorter the length of the corresponding single sub-path. The mapping relationship between the two can be set based on historical parameters. The expected movement path is evenly divided according to the set single sub-path length to establish a sub-path sequence.

[0152] Specifically, a corresponding dynamic risk value is set based on the average spatiotemporal distance between the target sub-path and each dynamic obstacle (i.e., the average distance between the robot and each dynamic obstacle when the robot is moving on the target sub-path). The smaller the average spatiotemporal distance, the greater the possibility of encountering dynamic obstacle risks when the robot is on the target sub-path, and the greater the corresponding dynamic risk value. The mapping relationship between the two can be set based on historical parameters.

[0153] Specifically, the larger the dynamic risk value, the larger the corresponding sensing range (i.e., the coverage of the collected environmental data), and the mapping relationship between the two can be set according to historical parameters.

[0154] Specifically, the higher the dynamic risk value, the higher the corresponding data collection frequency. The mapping relationship between the two can be set according to historical parameters. By collecting environmental data at high frequency, the relative distance between each dynamic obstacle and the robot can be determined in a timely manner to ensure the safe operation of the robot.

[0155] Understandably, in the above embodiments, the sensing range and data acquisition frequency are intelligently adjusted based on the dynamic risk value of each sub-path in the initial motion trajectory. Monitoring is strengthened in high-risk areas, while the sensing load is appropriately reduced in low-risk areas, allowing computing power to be focused on key areas and improving the robot's dynamic obstacle avoidance capability during movement.

[0156] In a preferred embodiment of this application, determining whether to generate a correction instruction for the initial motion path includes:

[0157] Select the sub-path to be monitored based on the robot's position parameters;

[0158] Obtain environmental monitoring data for the sub-path to be monitored, and establish a local three-dimensional model based on the environmental monitoring data;

[0159] Multiple dynamic obstacle points are generated based on the 3D obstacle model and the local 3D model;

[0160] Generate the expected motion trajectory of each dynamic obstacle point;

[0161] A dynamic obstacle risk value f is generated based on all expected motion trajectories;

[0162] Preset dynamic obstacle risk threshold F1;

[0163] If f > F1, generate correction instructions for the initial motion path.

[0164] Specifically, based on the robot's real-time position parameters, the current sub-path of the robot is determined, and this sub-path is set as the sub-path to be monitored.

[0165] Specifically, the corresponding sensing area is set according to the sensing parameters of the sub-path to be monitored, and multiple sensing time nodes are set according to the corresponding data acquisition frequency. Environmental monitoring data within the corresponding sensing area is obtained according to the sensing time nodes.

[0166] Specifically, the local 3D model is the sub-3D obstacle model within the current perception area. Its construction principle is the same as the 3D obstacle model constructed at the beginning. By comparing the relevant obstacle parameters of the corresponding areas in the local 3D model and the 3D obstacle model, it is determined whether there is a new obstacle area in the local 3D model. If there is, the new obstacle area is set as a dynamic obstacle point, and the dynamic obstacle area corresponding to the current perception area in the 3D obstacle model is obtained and also set as a dynamic obstacle point.

[0167] Specifically, this is based on the spatiotemporal overlap between the expected trajectories of all dynamic obstacle points and the robot's initial path (i.e., the area within which a dynamic obstacle point and the robot appear in the same location at the same time). The larger the spatiotemporal overlap, the greater the corresponding dynamic obstacle risk value.

[0168] Specifically, the dynamic obstacle risk threshold can be set based on historical parameters. If the real-time dynamic obstacle risk value is greater than the preset dynamic obstacle risk threshold, it means that if the robot moves according to the initial movement path, there is a potential risk of collision with the dynamic obstacle. The robot's initial movement path needs to be adjusted in time to ensure that the robot always maintains a safe distance from the obstacle.

[0169] Specifically, the expected trajectory of each dynamic obstacle point is generated, including:

[0170] A trajectory prediction model is established based on historical environmental monitoring data. The trajectory prediction model includes multiple prediction sub-models.

[0171] Establish a dynamic obstacle point sequence H, H=(h1, h2…h i …h r ), where h i Let r be the i-th dynamic obstacle point; r is the number of dynamic obstacle points.

[0172] Based on the dynamic obstacle point sequence H, hi is sequentially set as the target obstacle point;

[0173] Set up a time-series monitoring package for the target obstacle point based on the environmental monitoring data of the sub-path to be monitored;

[0174] Historical trajectory parameters of the target obstacle point are generated based on the time-series monitoring package;

[0175] The matching values ​​between the target obstacle point and each prediction sub-model are generated based on historical trajectory parameters.

[0176] The prediction sub-model corresponding to the maximum value in the matching values ​​is set as the first-level prediction model;

[0177] Generate the expected trajectory of the target obstacle point based on historical trajectory parameters and a primary prediction model;

[0178] The expected trajectory of each dynamic obstacle point is generated sequentially.

[0179] Specifically, in this application embodiment, the trajectory prediction model is preferably either a physical prediction model (for obstacles that follow clear motion patterns (such as AGVs running along tracks), using Kalman filtering or extended Kalman filtering for state estimation and short-term trajectory prediction) or a behavioral prediction model (for agents (such as people), using long short-term memory neural networks to learn their historical motion patterns, and combining scene context (such as target points, passage areas) to predict their possible trajectory distribution in the next few seconds, and outputting the prediction results of the most probable predicted trajectories in the form of bounding boxes or envelope spaces on future time series, with timestamps and confidence levels). Based on these two models, further refinement can be performed, for example, further refinement can be performed for different people (predicting based on whether they have fixed tasks), etc.

[0180] Specifically, the time-series monitoring package contains location data of target obstacle points collected at various sensing time nodes based on time sequence filtering. By processing the time-series monitoring package, historical trajectory parameters of the target obstacle points (including historical movement trajectory and average movement speed) are generated. By analyzing the historical trajectory parameters, the similarity with the predicted trajectory type corresponding to each prediction sub-model is generated. The higher the similarity, the greater the corresponding matching value. The mapping relationship between the two can be set according to the historical parameters.

[0181] For example, if the historical trajectory parameters show that the movement is clearly along a fixed track, then the matching value between the historical trajectory parameters and the physical prediction model is higher, and the physical prediction model is set as a first-level prediction model. The first-level prediction model outputs the subsequent movement trajectory (i.e., the expected movement trajectory) of the target obstacle point by analyzing the historical trajectory parameters.

[0182] Specifically, the correction instructions include:

[0183] Establish a sequence B1 of associated sub-paths based on the sub-paths to be monitored;

[0184] B1=(b 11 ,b 12 …b 1i …b 1m1 ), where b 1i Let m1 be the i-th associated sub-path of the sub-path to be monitored; m1 is the number of associated sub-paths of the sub-path to be monitored, and m1 <m;

[0185] The interference risk value of each associated sub-path is generated based on the expected motion trajectory of all dynamic obstacle points;

[0186] Preset interference risk threshold K1;

[0187] If K1 <k i (i=1, 2…m1), the i-th associated sub-path is designated as a risky sub-path;

[0188] Where, k i Let m1 be the interference risk value of the i-th associated sub-path, and m1 be the number of associated sub-paths of the sub-path to be monitored.

[0189] Obtain all risk sub-paths and set the correction start and correction end points based on all risk sub-paths;

[0190] A dynamic obstacle model is constructed based on the correction start and correction end points;

[0191] The robot's corrected motion path is set based on the dynamic obstacle model and path decision model.

[0192] Specifically, all sub-paths following the sub-path to be monitored are defined as associated sub-paths of the sub-path to be monitored. That is, when the robot is on the sub-path to be monitored, all sub-paths that it has not yet passed through are associated sub-paths of the sub-path to be monitored.

[0193] Specifically, it determines whether the current associated sub-path overlaps with each dynamic obstacle point in time and space. If so, the interference risk value of the current associated sub-path is set to 1. If not, the interference risk value is set to 0.

[0194] Specifically, the threshold value for interference risk can be set based on historical parameters, and its value range is (0,1), with 0.5 being preferred in this application.

[0195] Specifically, by aggregating the regions corresponding to all risk sub-paths, the influence region of the current dynamic obstacle point is generated, and a correction start point and correction end point are set according to the influence region, with both the correction start point and correction end point located on the initial movement path.

[0196] Specifically, the dynamic obstacle model is the sub-3D obstacle model corresponding to the affected area. The path decision model generates an optimal motion path from the corrected starting point to the corrected ending point by analyzing the obstacle parameters in the dynamic obstacle model. This path is set as the corrected motion path, and the corrected motion path is used to replace the path from the corrected starting point to the corrected ending point in the initial motion path.

[0197] It is understood that in the above embodiments, by using multiple sensors to perceive and predict moving obstacles in real time, the risk area is accurately located, and the starting and ending points of the correction are determined accordingly. Local path optimization is then performed, making the obstacle avoidance process more continuous and smooth. This effectively avoids sudden stops or replanning failures caused by the sudden appearance of obstacles, and improves the robustness of the overall system.

[0198] In another preferred embodiment of the dynamic obstacle avoidance motion planning method based on any of the above preferred embodiments, a dynamic obstacle avoidance motion planning system is provided, comprising:

[0199] The monitoring unit is used to collect initial environmental data of the work area;

[0200] The central control unit is used to construct a three-dimensional obstacle model based on the initial environmental data;

[0201] The central control unit includes:

[0202] The first processing module is used to set the robot's initial motion path based on the three-dimensional obstacle model and the preset path decision model.

[0203] The second processing module is used to set the perception strategy based on the initial motion path;

[0204] The third processing module is used to acquire environmental monitoring data according to the perception strategy, and to determine whether to generate a correction instruction for the initial motion path based on the environmental monitoring data.

[0205] The first processing module is also used for:

[0206] Construct a path planning model;

[0207] Multiple obstacle value ranges are set based on historical planning parameters;

[0208] Establish a sequence A of barrier value intervals;

[0209] A=(a1,a2…a i …a n ), where a i Let n be the i-th obstacle value interval; n is the number of obstacle value intervals.

[0210] Based on the obstacle value interval sequence A, a is set sequentially. i The target obstacle value range;

[0211] An iterative sub-strategy that sets the target obstacle value range;

[0212] The iterative sub-strategies include: number of iterations, trust region adjustment strategy, and time node distribution;

[0213] Sequentially set the iterative sub-strategies for each obstacle value range, and build an execution library based on all the iterative sub-strategies;

[0214] Establish a path decision model based on the execution library and path planning model.

[0215] In a preferred embodiment of this application, the first processing module is further configured to:

[0216] Construct an initial 3D model based on the work area;

[0217] Obtain initial environmental data for the work area;

[0218] A three-dimensional voxel map is constructed based on the processing results of the initial environmental data using a pre-defined obstacle recognition model.

[0219] The 3D voxel map includes multiple obstacle sub-regions, and the initial obstacle value of the working area is set based on all obstacle sub-regions.

[0220] Generate a 3D obstacle model based on the 3D voxel map and the initial 3D model;

[0221] Set planning sub-strategies based on initial obstacle values ​​and path decision models;

[0222] Obtain task requirements and generate an initial motion path based on task requirements and planning sub-strategies;

[0223] The task requirements include the starting point and the ending point of the movement.

[0224] In a preferred embodiment of this application, the second processing module is further configured to:

[0225] The initial running path is segmented, and multiple sub-paths are generated based on the segmentation results;

[0226] Establish a sub-path sequence B, B=(b1,b2…b i …b m ), where b i Let m be the i-th sub-path; m is the number of sub-paths.

[0227] Based on the sub-path sequence B, bi is sequentially set as the target sub-path;

[0228] The dynamic risk value of the target sub-path is set based on the three-dimensional obstacle model;

[0229] The perception parameters of the target sub-path are set according to the dynamic risk value. The perception parameters include: perception range and data collection frequency.

[0230] Set the perception parameters for each sub-path in sequence, and set the perception strategy based on all the perception parameters.

[0231] Based on the first concept of this application, an execution library that can be quickly invoked is formed by pre-dividing obstacle value intervals according to historical data and configuring iterative sub-strategies for each interval, including parameters such as the number of iterations and trust region adjustment strategies. During actual planning, the system can match the most suitable pre-set strategy based on the initial obstacle values ​​calculated in real time, avoiding repeated trial and error by the planner in complex scenarios and significantly improving the efficiency and success rate of initial trajectory generation.

[0232] According to the second concept of this application, by using multiple sensors to perceive and predict moving obstacles in real time, the risk area is accurately located, and the starting and ending points of the correction are determined accordingly. Local path optimization is then performed, making the obstacle avoidance process more continuous and smooth. This effectively avoids sudden stops or replanning failures caused by the sudden appearance of obstacles, and improves the robustness of the overall system.

[0233] The above description is only a preferred embodiment of this application. It should be noted that for those skilled in the art, several improvements and substitutions can be made without departing from the technical principles of this application, and these improvements and substitutions should also be considered within the scope of protection of this application.

Claims

1. A dynamic obstacle avoidance motion planning method, characterized in that, include: Collect initial environmental data of the work area and construct a three-dimensional obstacle model based on the initial environmental data; The robot's initial motion path is set based on the 3D obstacle model and the preset path decision model, and the perception strategy is set based on the initial motion path. The system acquires environmental monitoring data based on the perception strategy and determines whether to generate a correction command for the initial motion path based on the environmental monitoring data. The preset path decision model includes: Construct a path planning model; Multiple obstacle value ranges are set based on historical planning parameters; Establish a sequence A of barrier value intervals; A=(a1,a2…a i …a n ), where a i Let n be the i-th obstacle value interval; n is the number of obstacle value intervals. Based on the obstacle value interval sequence A, a is set sequentially. i The target obstacle value range; An iterative sub-strategy that sets the target obstacle value range; The iterative sub-strategy includes: number of iterations, trust region adjustment strategy, and time node distribution; Sequentially set the iterative sub-strategies for each obstacle value range, and build an execution library based on all the iterative sub-strategies; Establish a path decision model based on the execution library and path planning model; The initial motion path of the robot is set, including: Construct an initial 3D model based on the work area; Obtain initial environmental data for the work area; A three-dimensional voxel map is constructed based on the processing results of the initial environmental data using a pre-defined obstacle recognition model. The three-dimensional voxel map includes multiple obstacle sub-regions, and the initial obstacle value of the working area is set according to all obstacle sub-regions. Generate a 3D obstacle model based on the 3D voxel map and the initial 3D model; Set planning sub-strategies based on initial obstacle values ​​and path decision models; Obtain task requirements and generate an initial motion path based on task requirements and planning sub-strategies; The task requirements include a starting point and an ending point. The defined perception strategy includes: The initial running path is segmented, and multiple sub-paths are generated based on the segmentation results; Establish a sub-path sequence B, B=(b1,b2…b i …b m ), where b i Let m be the i-th sub-path; m is the number of sub-paths. Based on the sub-path sequence B, bi is sequentially set as the target sub-path; The dynamic risk value of the target sub-path is set based on the three-dimensional obstacle model; The perception parameters of the target sub-path are set according to the dynamic risk value, and the perception parameters include: perception range and data acquisition frequency; Set the perception parameters for each sub-path in sequence, and set the perception strategy based on all perception parameters; The determination of whether to generate a correction instruction for the initial motion path includes: Select the sub-path to be monitored based on the robot's position parameters; Obtain environmental monitoring data for the sub-path to be monitored, and establish a local three-dimensional model based on the environmental monitoring data; Multiple dynamic obstacle points are generated based on the 3D obstacle model and the local 3D model; Generate the expected motion trajectory of each dynamic obstacle point; A dynamic obstacle risk value f is generated based on all expected motion trajectories; Preset dynamic obstacle risk threshold F1 If f > F1, generate correction instructions for the initial motion path.

2. The dynamic obstacle avoidance motion planning method as described in claim 1, characterized in that, The generation of the expected trajectory for each dynamic obstacle point includes: A trajectory prediction model is established based on historical environmental monitoring data, and the trajectory prediction model includes multiple prediction sub-models. Establish a dynamic obstacle point sequence H, H=(h1, h2…h i …h r ), where h i Let r be the i-th dynamic obstacle point; r is the number of dynamic obstacle points. Based on the dynamic obstacle point sequence H, hi is sequentially set as the target obstacle point; Set up a time-series monitoring package for the target obstacle point based on the environmental monitoring data of the sub-path to be monitored; Historical trajectory parameters of the target obstacle point are generated based on the time-series monitoring package; The matching values ​​between the target obstacle point and each prediction sub-model are generated based on historical trajectory parameters. The prediction sub-model corresponding to the maximum value in the matching values ​​is set as the first-level prediction model; Generate the expected trajectory of the target obstacle point based on historical trajectory parameters and a primary prediction model; The expected trajectory of each dynamic obstacle point is generated sequentially.

3. The dynamic obstacle avoidance motion planning method as described in claim 2, characterized in that, The correction instructions include: Establish a sequence B1 of associated sub-paths based on the sub-paths to be monitored; B1=(b 11 ,b 12 …b 1i …b 1m1 ), where b 1i Let m1 be the i-th associated sub-path of the sub-path to be monitored; m1 is the number of associated sub-paths of the sub-path to be monitored, and m1 <m; The interference risk value of each associated sub-path is generated based on the expected motion trajectory of all dynamic obstacle points; Preset interference risk threshold K1; If K1 <k i (i=1, 2…m1), the i-th associated sub-path is designated as a risky sub-path; Where, k i Let m1 be the interference risk value of the i-th associated sub-path, and m1 be the number of associated sub-paths of the sub-path to be monitored. Obtain all risk sub-paths and set the correction start and correction end points based on all risk sub-paths; A dynamic obstacle model is constructed based on the correction start and correction end points; The robot's corrected motion path is set based on the dynamic obstacle model and path decision model.

4. A dynamic obstacle avoidance motion planning system, employing the dynamic obstacle avoidance motion planning method according to any one of claims 1-3, characterized in that, include: The monitoring unit is used to collect initial environmental data of the work area; The central control unit is used to construct a three-dimensional obstacle model based on the initial environmental data; The central control unit includes: The first processing module is used to set the robot's initial motion path based on the three-dimensional obstacle model and the preset path decision model. The second processing module is used to set the perception strategy based on the initial motion path; The third processing module is used to acquire environmental monitoring data according to the perception strategy, and to determine whether to generate a correction instruction for the initial motion path based on the environmental monitoring data. The first processing module is also used for Construct a path planning model; Multiple obstacle value ranges are set based on historical planning parameters; Establish a sequence A of barrier value intervals; A=(a1,a2…a i …a n ), where a i Let n be the i-th obstacle value interval; n is the number of obstacle value intervals. Based on the obstacle value interval sequence A, a is set sequentially. i The target obstacle value range; An iterative sub-strategy that sets the target obstacle value range; The iterative sub-strategy includes: number of iterations, trust region adjustment strategy, and time node distribution; Sequentially set the iterative sub-strategies for each obstacle value range, and build an execution library based on all the iterative sub-strategies; Establish a path decision model based on the execution library and path planning model.

5. The dynamic obstacle avoidance motion planning system as described in claim 4, characterized in that, The first processing module is also used for: Construct an initial 3D model based on the work area; Obtain initial environmental data for the work area; A three-dimensional voxel map is constructed based on the processing results of the initial environmental data using a pre-defined obstacle recognition model. The three-dimensional voxel map includes multiple obstacle sub-regions, and the initial obstacle value of the working area is set according to all obstacle sub-regions. Generate a 3D obstacle model based on the 3D voxel map and the initial 3D model; Set planning sub-strategies based on initial obstacle values ​​and path decision models; Obtain task requirements and generate an initial motion path based on task requirements and planning sub-strategies; The task requirements include a starting point and an ending point.

6. The dynamic obstacle avoidance motion planning system as described in claim 5, characterized in that, The second processing module is also used for: The initial running path is segmented, and multiple sub-paths are generated based on the segmentation results; Establish a sub-path sequence B, B=(b1,b2…b i …b m ), where b i Let m be the i-th sub-path; m is the number of sub-paths. Based on the sub-path sequence B, bi is sequentially set as the target sub-path; The dynamic risk value of the target sub-path is set based on the three-dimensional obstacle model; The perception parameters of the target sub-path are set according to the dynamic risk value, and the perception parameters include: perception range and data acquisition frequency; Set the perception parameters for each sub-path in sequence, and set the perception strategy based on all the perception parameters.

Images