A multi-mobile robot obstacle avoidance control method based on dynamic control barrier function

Abstract

The application relates to a kind of multi-mobile robot obstacle avoidance control methods based on dynamic control barrier function, comprising the following steps: S1, obtaining multi-mobile robot state information and dynamic obstacle point cloud data;S2, pre-processing is obtained with several obstacle point cloud clusters;S3, ellipse parameterization fitting is obtained with ellipse obstacle parameters;S4, based on ellipse obstacle parameters, the ellipse obstacle sequence in prediction time domain is obtained;S5, extended ellipse obstacle sequence is obtained;S6, establishment formation reference model generates reference trajectory;S7, construct discrete-time dynamic control barrier function constraint as safety constraint;S8, calculate obstacle avoidance risk index and obstacle avoidance responsibility coefficient and adjust formation keeping weight and obstacle avoidance constraint weight in model predictive control;S9, based on the weight after adjustment, establish model, obtain prediction control input sequence, and carry out rolling optimization.The beneficial effects of the application are: improve modeling accuracy, motion trend utilization rate and prediction error safety margin accuracy.

Description

Technical Field

[0001] This invention relates to the fields of mobile robot cooperative control and dynamic obstacle perception technology, specifically to a multi-mobile robot obstacle avoidance control method based on dynamic control obstacle functions. Background Technology

[0002] Multi-mobile robot systems, due to their advantages such as high collaborative operation efficiency, wide task execution range, and strong system robustness, have been widely used in scenarios such as intelligent warehousing, industrial inspection, logistics handling, and disaster search and rescue. In practical applications, multiple mobile robots usually need to move collaboratively in a preset formation. The robot's operating environment often contains dynamic obstacles such as pedestrians, vehicles, and mobile devices. The position, speed, and external boundaries of these obstacles change over time, which can easily affect the safety and stability of the multi-robot formation movement.

[0003] In existing technologies, obstacle avoidance methods for multi-robot systems typically simplify obstacles into circular regions or regions with known boundaries, and construct obstacle avoidance constraints based on the distance between the robot and the obstacle. However, for non-circular obstacles such as pedestrians, vehicles, or irregular objects, circular approximations are difficult to accurately describe their actual boundaries. Furthermore, raw point cloud data is characterized by large data volume, numerous noise points, and discrete boundaries, making it difficult to directly use for obstacle avoidance constraints in model predictive control. Ignoring the shape and motion trend of obstacle boundaries can easily lead to problems such as delayed robot obstacle avoidance response, abrupt changes in control input, and unsmooth motion trajectories. Moreover, existing methods often assume that the obstacle state is known or only perform obstacle avoidance based on the current obstacle position, failing to fully utilize obstacle boundary parameters and future motion prediction information. If obstacle avoidance constraints are not handled properly, it can easily cause formation disruption or insufficient safety margin. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this application proposes a multi-mobile robot obstacle avoidance control method based on dynamic control obstacle functions. This method can solve the problems of inaccurate obstacle boundary modeling, insufficient utilization of dynamic obstacle movement trends, insufficient safety margin of prediction error, and unreasonable allocation of obstacle avoidance tasks for different robots in existing multi-mobile robot formation obstacle avoidance methods, thereby achieving stable formation and safe obstacle avoidance of multiple mobile robots in dynamic obstacle environments.

[0005] The following is the technical solution of the present invention: a multi-mobile robot obstacle avoidance control method based on dynamic control obstacle function, comprising the following steps:

[0006] S1. Acquire status information of multiple mobile robots and dynamic obstacle point cloud data;

[0007] S2. Preprocess the dynamic obstacle point cloud data, and use a point cloud clustering algorithm to segment the preprocessed point cloud to obtain several obstacle point cloud clusters.

[0008] S3. Perform elliptic parameter fitting on each obstacle point cloud cluster to obtain the elliptical obstacle parameters at the current time.

[0009] S4. Based on the elliptical obstacle parameters, combined with continuous frame data, perform dynamic obstacle data association, state estimation and future prediction to obtain the elliptical obstacle sequence in the prediction time domain.

[0010] S5. Calculate the shape fluctuation index based on the elliptical parameter changes in continuous frames, and perform uncertainty dilation on the elliptical obstacles in the prediction time domain to obtain the extended elliptical obstacle sequence.

[0011] S6. Establish a leader-follower formation reference model for multiple mobile robots and generate reference trajectories for each follower robot;

[0012] S7. Based on the extended elliptical obstacle sequence, construct discrete-time dynamic control obstacle function constraints as robot-obstacle dynamic safety constraints.

[0013] S8. Based on the safety function values ​​between each robot and the extended elliptical obstacle, calculate the obstacle avoidance risk index and obstacle avoidance responsibility coefficient of each robot, and dynamically adjust the formation holding weight and obstacle avoidance constraint weight in the model predictive control according to the obstacle avoidance responsibility coefficient.

[0014] S9. Based on the adjusted weights, establish a model that includes formation error, control input, control input increment and dynamic safety constraints to obtain the control input sequence in the future prediction time domain and perform rolling optimization.

[0015] In a preferred embodiment of the present invention, S2 involves preprocessing the dynamic obstacle point cloud data, including:

[0016] Spatial cropping is performed on the original point cloud data to retain the valid point cloud within the preset three-dimensional spatial range;

[0017] The effective point cloud is downsampled and outlier filtering is performed to remove redundant and noise points.

[0018] The preprocessed point cloud is mapped to a local raster map, and obstacle regions are extracted by point cloud density or elevation abrupt changes.

[0019] In a preferred embodiment of the present invention, in S3, the elliptical obstacle parameters include the position of the ellipse center. semi-major shaft semi-short shaft and direction angle ;

[0020] The data association process is as follows: calculate the Euclidean distance between the center of the ellipse in the previous frame and the center of the ellipse in the current frame, construct a matching cost matrix based on the distance, and use the minimum cost matching method to determine the correspondence between obstacles in adjacent frames; ellipses that do not match historical obstacles are judged as newly appearing obstacles and assigned a new number.

[0021] As a preferred embodiment of the present invention, in S5, uncertainty dilation is performed on the elliptical obstacle in the prediction time domain, including:

[0022] Extract the ellipse shape parameters of the j-th dynamic obstacle ;

[0023] The shape fluctuation index is calculated based on the changes in the elliptical shape parameters of the most recent m frames. ;

[0024] Combined with shape fluctuation expansion coefficient Predicting step-size expansion system and basic safety expansion amount Calculate the safe expansion amount under the prediction step s. ;

[0025] Calculate the extended semi-major axis and semi-short axis The expression is as follows:

[0026]

[0027] .

[0028] As a preferred embodiment of the present invention, in S7, constructing discrete-time dynamic control obstacle function constraints includes:

[0029] Calculate the angle between the direction from the i-th robot position to the center of the j-th extended elliptical obstacle and the major axis of the ellipse. ;

[0030] Calculate the distance from the center of the extended ellipse to the boundary along this direction. ;

[0031] Define robot-obstacle safety function ,in, To establish a preset safe distance;

[0032] The discrete-time dynamic control obstacle function constraint is constructed as follows:

[0033] .

[0034] As a preferred embodiment of the present invention, in S8, the obstacle avoidance responsibility coefficient of each robot is calculated, including:

[0035] Based on the safety function value corresponding to each robot Determine the obstacle avoidance risk indicators for each robot. ;

[0036] The obstacle avoidance responsibility coefficient of the i-th robot is calculated by normalization. ,in, The total number of robots;

[0037] In the process of model predictive control optimization, the obstacle avoidance responsibility coefficient Larger robots are assigned higher obstacle avoidance constraint weights or lower formation maintenance weights.

[0038] As a preferred embodiment of the present invention, in S9, the objective function expression of the model is as follows:

[0039]

[0040] In the above formula, In robot state, For reference only. To control the input, To control the input increment, Q, R, S, and P are weight matrices, and N is the prediction time domain length.

[0041] As a preferred embodiment of the present invention, in S1, the dynamic obstacle point cloud data is obtained by at least one of the following methods: LiDAR, depth camera, fixed environmental perception device, mobile robot on-board sensor or simulation platform.

[0042] A multi-mobile robot obstacle avoidance control system based on dynamic control obstacle function includes:

[0043] The perception module is used to acquire the status information of multiple mobile robots and the point cloud data of dynamic obstacles in the environment;

[0044] The data processing module is used to filter and cluster point cloud data, and parameterize it into an extended elliptical obstacle model with predictive information.

[0045] The formation control module is used to generate reference trajectories for each robot in the leader-follower formation;

[0046] The responsibility allocation module is used to calculate the obstacle avoidance responsibility coefficient based on the relative safety margin between the robot and the obstacle.

[0047] The model predictive control module is used to dynamically adjust the optimization weights based on the obstacle avoidance responsibility coefficient, and combined with the safety constraints of the dynamic control obstacle function, to solve the optimal control sequence in a rolling manner and send it to each mobile robot for execution.

[0048] The beneficial effects of this invention are as follows: By clustering and elliptic parameterizing the dynamic obstacle point cloud, the discrete point cloud boundary is transformed into a low-dimensional elliptical model suitable for predictive control, reducing the computational complexity of the controller in processing obstacle boundaries; by using continuous frame elliptical parameters for obstacle state prediction, the robot can obtain the motion trend of dynamic obstacles in the prediction time domain in advance, reducing obstacle avoidance response lag; by introducing an elliptical uncertainty expansion mechanism, the safety constraints can include point cloud noise, ellipse fitting error, and prediction error, improving the obstacle avoidance safety margin in dynamic obstacle environments; by constructing safety constraints in the prediction time domain through dynamic control obstacle functions, the robot is limited from approaching dynamic obstacles too quickly; through a dynamic obstacle avoidance responsibility allocation mechanism, robots closer to obstacles take priority in obstacle avoidance tasks, while robots farther from obstacles prioritize maintaining formation structure; and by using model predictive control to uniformly handle formation errors, control inputs, and dynamic obstacle avoidance constraints, multiple mobile robots can maintain good formation stability and obstacle avoidance safety in dynamic obstacle environments. Attached Figure Description

[0049] Figure 1 The control method flow of the present invention Figure 1 ;

[0050] Figure 2 The control method flow of the present invention Figure 2 ;

[0051] Figure 3 This is a schematic diagram illustrating the dynamic obstacle ellipse data association in the control method of the present invention;

[0052] Figure 4 This is a schematic diagram of the dynamic obstacle avoidance trajectory of multiple mobile robots based on model predictive control, which is the control method of the present invention. Detailed Implementation

[0053] To make the technical problems solved by the present invention, the technical solutions adopted, and the technical effects achieved clearer, the technical solutions of the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0054] Example 1

[0055] like Figures 1 to 4 As shown, a multi-mobile robot obstacle avoidance control method based on dynamic control obstacle function includes the following steps:

[0056] S1. Acquire status information of multiple mobile robots and dynamic obstacle point cloud data;

[0057] S2. Preprocess the dynamic obstacle point cloud data, and use a point cloud clustering algorithm to segment the preprocessed point cloud to obtain several obstacle point cloud clusters.

[0058] S3. Perform elliptic parameter fitting on each obstacle point cloud cluster to obtain the elliptical obstacle parameters at the current time.

[0059] S4. Based on the elliptical obstacle parameters, combined with continuous frame data, perform dynamic obstacle data association, state estimation and future prediction to obtain the elliptical obstacle sequence in the prediction time domain.

[0060] S5. Calculate the shape fluctuation index based on the elliptical parameter changes in continuous frames, and perform uncertainty dilation on the elliptical obstacles in the prediction time domain to obtain the extended elliptical obstacle sequence.

[0061] S6. Establish a leader-follower formation reference model for multiple mobile robots and generate reference trajectories for each follower robot;

[0062] S7. Based on the extended elliptical obstacle sequence, construct the discrete-time dynamic control obstacle function (DCBF) constraint as the robot-obstacle dynamic safety constraint.

[0063] S8. Based on the safety function values ​​between each robot and the extended elliptical obstacle, calculate the obstacle avoidance risk index and obstacle avoidance responsibility coefficient of each robot, and dynamically adjust the formation holding weight and obstacle avoidance constraint weight in the model predictive control according to the obstacle avoidance responsibility coefficient.

[0064] S9. Based on the adjusted weights, establish a model that includes formation error, control input, control input increment and dynamic safety constraints to obtain the control input sequence in the future prediction time domain and perform rolling optimization.

[0065] In step S1, the status information of multiple mobile robots and the point cloud data of dynamic obstacles are acquired, including:

[0066] The system collects the position, heading angle, linear velocity, and angular velocity of each robot in the global coordinate system; and acquires raw point cloud data of dynamic obstacles in the environment using LiDAR, depth cameras, or simulation platforms. The dynamic obstacle point cloud data is obtained through at least one of the following methods: LiDAR, depth cameras, fixed environmental sensing devices, mobile robot onboard sensors, or simulation platforms.

[0067] The multi-mobile robot system consists of one leader robot and multiple follower robots. Each robot employs a differential drive model. To avoid confusion between state symbols and position coordinates, the state of the i-th robot is denoted as... The expression is as follows:

[0068]

[0069] In the above formula, Here are the robot's position coordinates in the global coordinate system. This is the robot's heading angle.

[0070] The control input expression for the i-th robot is as follows:

[0071]

[0072] In the above formula, Linear velocity, ω is the angular velocity.

[0073] The discrete kinematics model of the robot is expressed as follows:

[0074]

[0075] In the above formula, The sampling period is specified. Dynamic obstacle point cloud data can be acquired by LiDAR, depth cameras, fixed environmental sensing devices, mobile robot onboard sensors, or generated by a simulation platform based on obstacle geometric boundaries. This point cloud data is used to characterize the spatial distribution of dynamic obstacles at the current moment.

[0076] In step S2, the dynamic obstacle point cloud data is preprocessed, and a point cloud clustering algorithm is used to segment the preprocessed point cloud to obtain several obstacle point cloud clusters, including:

[0077] The dynamic obstacle point cloud data is preprocessed; the obstacle point cloud is segmented using a point cloud clustering algorithm.

[0078] Spatial cropping of the original point cloud is performed to delineate the region of interest. Invalid points are removed, outliers are filtered, and downsampling is applied to construct a local grid map to assist in the identification of obstacle regions.

[0079] Let the point cloud set acquired at time k be:

[0080]

[0081] In the above formula, For the l-th point cloud point, This represents the number of point clouds.

[0082] To reduce computational complexity, the original point cloud is spatially clipped, with the clipping region defined as follows:

[0083]

[0084] The effective point cloud set after cropping is:

[0085]

[0086] Further downsampling and outlier removal are performed on the effective point cloud to reduce redundant and noisy points. If necessary, the effective point cloud can be mapped onto a local raster map, and obstacle areas can be identified based on raster gradient, elevation abrupt changes, or point cloud density.

[0087] A density-based clustering algorithm is used to segment the point cloud into one or more point cloud clusters, and noisy point cloud clusters with fewer than a preset threshold are removed.

[0088] Clustering and segmenting the preprocessed obstacle point cloud regions yields a set of obstacle point cloud clusters:

[0089]

[0090] In the above formula, Let j represent the j-th obstacle point cloud cluster, and n represent the number of obstacles detected at the current time.

[0091] In this embodiment, the DBSCAN algorithm is used for clustering. This algorithm distinguishes different obstacle point cloud clusters based on point cloud density, does not require a pre-set number of obstacles, and is suitable for environments with dynamically changing obstacle numbers. For points with fewer than a preset threshold... The point cloud clusters were treated as noisy point cloud clusters and removed.

[0092] In step S3, elliptic parameter fitting is performed on each obstacle point cloud cluster to obtain the elliptical obstacle parameters at the current time, including:

[0093] Perform minimum bounding ellipse fitting on each point cloud cluster and output the center position, semi-major axis, semi-minor axis and orientation angle parameters of the ellipse.

[0094] The obstacle point cloud cluster is parameterized using the minimum bounding ellipse. The process is as follows: For the j-th obstacle point cloud cluster... The ellipse parameters are obtained by fitting the boundary of the minimum bounding ellipse, as shown in the following expression:

[0095]

[0096] In the above formula, The location is the center of the ellipse. For the semi-major axis, It is a semi-minor axis. The ellipse is the direction angle.

[0097] In the local coordinate system of the ellipse, the boundary of the ellipse satisfies the expression:

[0098]

[0099] in:

[0100]

[0101]

[0102] Through the above processing, the dynamic obstacle is transformed from a discrete point cloud boundary into a low-dimensional elliptical model composed of the center position, semi-major axis, semi-minor axis and orientation angle.

[0103] In step S4, based on the elliptical obstacle parameters and combined with continuous frame data, data association, state estimation, and future prediction of dynamic obstacles are performed to obtain the elliptical obstacle sequence in the prediction time domain, including:

[0104] Calculate the Euclidean distance between the centers of the ellipses in adjacent frames, construct a matching cost matrix for data association; use Kalman filter to estimate the position, velocity, acceleration and shape of obstacles; predict the elliptical state sequence of obstacles in the next N sampling steps.

[0105] Let the set of ellipses in the previous frame be... The current frame ellipse set is The distance between the r-th ellipse center of the previous frame and the s-th ellipse center of the current frame is calculated using the following expression:

[0106]

[0107] In the above formula, The center of the r-th ellipse in the previous frame. This is the center of the s-th ellipse in the current frame. According to... A matching cost matrix is ​​constructed, and a minimum cost matching method is used to determine the correspondence between obstacles in adjacent frames. When the matching distance is greater than a preset threshold... If the match is not found, the match is rejected; if an ellipse in the current frame does not match a historical obstacle, it is considered a newly appearing obstacle and a new number is assigned to it.

[0108] Using the elliptic parameters of consecutive frames as observations, the state of dynamic obstacles is estimated and predicted. The j-th dynamic obstacle state vector is defined as:

[0109]

[0110] The state transition model expression is as follows:

[0111]

[0112] In the above formula, A is the state transition matrix. This represents process noise. The observation vector expression is as follows:

[0113]

[0114] The observation model expression is as follows:

[0115]

[0116] In the above formula, H is the observation matrix. To mitigate observation noise, a Kalman filter is used to recursively estimate the obstacle state and predict its state over the next N sampling steps. The expression is as follows:

[0117]

[0118] The sequence of elliptical obstacles in the prediction time domain is obtained as follows:

[0119] .

[0120] In step S5, a shape fluctuation index is calculated based on the elliptical parameter changes in consecutive frames, and uncertainty dilation is performed on the elliptical obstacles in the prediction time domain to obtain an extended elliptical obstacle sequence, including:

[0121] Calculate the fluctuation index of ellipse shape parameters in historical continuous frames; combine shape fluctuation, prediction step size and basic expansion amount to calculate dynamic safety expansion amount; expand the semi-major axis and semi-minor axis of the ellipse according to the expansion amount to generate an expanded ellipse obstacle sequence.

[0122] During dynamic obstacle point cloud detection and ellipse fitting, point cloud noise, occlusion, changes in observation angle, and changes in obstacle motion state can cause fluctuations in the center position, semi-major axis, semi-minor axis, and orientation angle of the minimum bounding ellipse. If the unprocessed ellipse parameters are directly used for obstacle avoidance constraints, the safe distance between the robot and the obstacle may be underestimated.

[0123] Let the ellipse shape parameters of the j-th dynamic obstacle be:

[0124]

[0125] The ellipse shape fluctuation index is calculated based on the changes in ellipse shape parameters in continuous historical frames, and the safe expansion amount is determined in combination with the prediction step size. The expression is as follows:

[0126]

[0127] In the above formula, Let be the elliptical shape fluctuation index of the j-th obstacle. The coefficient of shape fluctuation expansion. To predict the step-size expansion coefficient, This is the basic safety expansion amount.

[0128] The semi-major axis and semi-minor axis of the expanded ellipse are as follows:

[0129]

[0130]

[0131] This yields the extended predicted elliptical obstacle sequence. As the prediction step size increases, the obstacle state prediction error increases, and the corresponding safety inflation increases, enabling multi-mobile robots to retain a more sufficient obstacle avoidance safety margin during predictive control.

[0132] In step S6, a leader-follower formation reference model for multiple mobile robots is established, and reference trajectories for each follower robot are generated, including:

[0133] Set the reference trajectory or position of the leader robot; calculate the reference position and reference heading angle of each follower robot based on the preset formation offset.

[0134] Let the leader's reference position be The offset of the i-th follower robot relative to the leader's preset formation is Then the reference position of the i-th following robot is:

[0135]

[0136] According to the reference position The reference state of the i-th following robot can be constructed using the reference heading angle. .

[0137] Formation error is defined as:

[0138]

[0139] By constraining or minimizing formation errors in the optimization problem, each following robot can maintain the desired formation structure.

[0140] In step S7, based on the extended elliptical obstacle sequence, a discrete-time dynamic control obstacle function constraint is constructed as the robot-obstacle dynamic safety constraint, including:

[0141] Calculate the angle between the robot and the obstacle and the elliptical boundary distance in that direction; define a robot-obstacle safety function that includes an extended boundary and a safety distance; and construct discrete-time dynamic control obstacle function constraints based on the safety function.

[0142] For the i-th robot and the j-th predicted elliptical obstacle, let the robot's position be... The center of the obstacle ellipse is located at... The angle between the direction the robot points towards the center of the obstacle and the major axis of the ellipse is defined as:

[0143]

[0144] The distance from the center of the extended ellipse to the boundary in this direction is:

[0145]

[0146] The robot obstacle safety function is defined as follows:

[0147]

[0148] In the above formula, To establish a preset safe distance. When When the distance is between the robot and the obstacle, it indicates that the safe distance requirement is met.

[0149] Further construct discrete-time dynamic control barrier function constraints:

[0150]

[0151] This constraint is used to limit the robot from approaching dynamic obstacles too quickly within the prediction time domain.

[0152] In step S8, based on the safety function values ​​between each robot and the extended elliptical obstacle, the obstacle avoidance risk index and obstacle avoidance responsibility coefficient of each robot are calculated. The formation-keeping weight and obstacle avoidance constraint weight in the model predictive control are then dynamically adjusted according to the obstacle avoidance responsibility coefficient, including:

[0153] Based on the safety function values ​​of each robot and the extended elliptical obstacle, the obstacle avoidance risk index is calculated; the obstacle avoidance responsibility coefficient of each robot is normalized; and the formation holding weight and obstacle avoidance constraint weight in the model predictive control are dynamically adjusted based on the obstacle avoidance responsibility coefficient.

[0154] In obstacle avoidance within a multi-robot formation, dynamic obstacles typically pose a higher collision risk only to some robots in the formation. If all robots adopt the same obstacle avoidance weights, it may lead to unnecessary trajectory deviations in the overall formation. Therefore, this embodiment dynamically allocates obstacle avoidance responsibility to each robot based on the safety margin between the robot and the extended elliptical obstacle.

[0155] Let the obstacle avoidance risk index of the i-th robot be... This index is determined by the safety function value of the i-th robot relative to each extended elliptical obstacle. The obstacle avoidance responsibility coefficient is defined as:

[0156]

[0157] in, For the number of robots, This represents the proportion of obstacle avoidance responsibility undertaken by the i-th robot at the current moment. When the i-th robot is closer to the predicted elliptical obstacle, its safety function value decreases, its obstacle avoidance risk index increases, and its corresponding obstacle avoidance responsibility coefficient increases; when the i-th robot is farther from the obstacle, its obstacle avoidance responsibility coefficient decreases.

[0158] Based on the obstacle avoidance responsibility coefficient, the obstacle avoidance weight, formation maintenance weight, or safety constraint strength in the model predictive control are dynamically adjusted. Robots with a higher obstacle avoidance responsibility coefficient are given priority in meeting obstacle avoidance requirements, while robots with a lower coefficient are given priority in maintaining formation, thereby reducing the trajectory deviation of the overall formation during obstacle avoidance.

[0159] In step S9, based on the adjusted weights, a model is established that includes formation error, control input, control input increment, and dynamic safety constraints to obtain the control input sequence in the future prediction time domain, and rolling optimization is performed, including:

[0160] Construct an objective function that includes formation error, control input, control increment, and terminal error; incorporate DCBF safety constraints and system kinematic constraints into the optimization; solve the optimization problem in a rolling manner at each sampling time, output and execute the optimal control input sequence at the current time; solve the model at the current sampling time to obtain the control input sequence in the future prediction time domain, and apply the first control input in the sequence to the corresponding mobile robot, and proceed to the next sampling time for rolling optimization.

[0161] For the i-th mobile robot, the objective function is constructed in the prediction time domain N, and its expression is as follows:

[0162]

[0163] In the above formula, In robot state, For reference only. To control the input, To control the input increment, Q, R, S, and P are weight matrices, and N is the prediction time domain length.

[0164] In one implementation, the formation error weight, obstacle avoidance constraint weight, or dynamic safety constraint strength in the objective function are adjusted according to the obstacle avoidance responsibility coefficient, so that multiple robots can adaptively coordinate formation maintenance and safe obstacle avoidance during the obstacle avoidance process.

[0165] The overall objective function of the multi-robot system is:

[0166]

[0167] The model predictive control optimization problem is:

[0168]

[0169] The constraints include:

[0170]

[0171]

[0172]

[0173] In the above formula, , , .

[0174] At each sampling time, the controller solves the model predictive control optimization problem based on the current robot state, reference trajectory, and predicted elliptical obstacle sequence to obtain the control input sequence for the future prediction time domain. Then, only the first control input from the optimal control sequence is executed:

[0175]

[0176] Upon entering the next sampling moment, the robot state and obstacle point cloud information are reacquired, the dynamic obstacle ellipse parameters and predicted trajectory are updated, and the optimization problem is solved again to achieve rolling optimization control.

[0177] In this embodiment, to verify the feasibility of the method, a dynamic obstacle avoidance scenario involving multiple mobile robots can be constructed in MATLAB or other simulation platforms. A leader robot and multiple follower robots are set up in the simulation environment, and a dynamic obstacle-crossing robot formation motion path is defined. First, point cloud data is generated based on the geometric boundaries of the dynamic obstacles. Then, clustering and minimum bounding ellipse fitting are performed on the point cloud to obtain the ellipse parameters of the dynamic obstacles. Subsequently, Kalman filtering is used to predict the motion state of the obstacles in the prediction time domain. Further uncertainty dilation is applied to the predicted ellipse obstacles, and the obstacle avoidance responsibility of each robot is dynamically allocated according to the safety margin. Finally, the extended ellipse obstacle sequence and obstacle avoidance responsibility coefficients are introduced into the model predictive control optimization problem, enabling the multiple mobile robots to adjust their motion trajectories in advance based on the dynamic obstacle movement trend.

[0178] Example 2

[0179] A multi-mobile robot obstacle avoidance control system based on dynamic control obstacle function includes:

[0180] The perception module is used to acquire the status information of multiple mobile robots and the point cloud data of dynamic obstacles in the environment;

[0181] The data processing module is used to filter and cluster point cloud data, and parameterize it into an extended elliptical obstacle model with predictive information.

[0182] The formation control module is used to generate reference trajectories for each robot in the leader-follower formation;

[0183] The responsibility allocation module is used to calculate the obstacle avoidance responsibility coefficient based on the relative safety margin between the robot and the obstacle.

[0184] The model predictive control module is used to dynamically adjust the optimization weights based on the obstacle avoidance responsibility coefficient, and combined with the safety constraints of the dynamic control obstacle function, to solve the optimal control sequence in a rolling manner and send it to each mobile robot for execution.

[0185] This invention reduces the computational complexity of the controller in handling obstacle boundaries by clustering and elliptic parameterizing the dynamic obstacle point cloud. This is achieved by transforming the discrete point cloud boundary into a low-dimensional elliptical model suitable for predictive control. Obstacle state prediction using continuous frame elliptical parameters allows the robot to anticipate the motion trend of dynamic obstacles in the prediction time domain, reducing obstacle avoidance lag. The introduction of an elliptical uncertainty expansion mechanism ensures that safety constraints encompass point cloud noise, ellipse fitting error, and prediction error, improving obstacle avoidance safety margins in dynamic obstacle environments. Dynamic control of obstacle functions constructs safety constraints in the prediction time domain, limiting the robot's approach to dynamic obstacles too quickly. A dynamic obstacle avoidance responsibility allocation mechanism prioritizes robots closer to obstacles for obstacle avoidance tasks, while robots farther from obstacles prioritize maintaining formation. Model predictive control unifies the handling of formation errors, control inputs, and dynamic obstacle avoidance constraints, enabling multiple mobile robots to maintain good formation stability and obstacle avoidance safety in dynamic obstacle environments.

[0186] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Clearly, those skilled in the art can make various alterations and variations to the invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of equivalents of the invention, the invention is also intended to include these modifications and variations.

Claims

1. A multi-robot obstacle avoidance control method based on dynamic control obstacle function, characterized in that, Includes the following steps: S1. Acquire status information of multiple mobile robots and dynamic obstacle point cloud data; S2. Preprocess the dynamic obstacle point cloud data, and use a point cloud clustering algorithm to segment the preprocessed point cloud to obtain several obstacle point cloud clusters. S3. Perform elliptic parameter fitting on each obstacle point cloud cluster to obtain the elliptical obstacle parameters at the current time. S4. Based on the elliptical obstacle parameters, combined with continuous frame data, perform dynamic obstacle data association, state estimation and future prediction to obtain the elliptical obstacle sequence in the prediction time domain. S5. Calculate the shape fluctuation index based on the elliptical parameter changes in continuous frames, and perform uncertainty dilation on the elliptical obstacles in the prediction time domain to obtain the extended elliptical obstacle sequence. S6. Establish a leader-follower formation reference model for multiple mobile robots and generate reference trajectories for each follower robot; S7. Based on the extended elliptical obstacle sequence, construct discrete-time dynamic control obstacle function constraints as robot-obstacle dynamic safety constraints. S8. Based on the safety function values ​​between each robot and the extended elliptical obstacle, calculate the obstacle avoidance risk index and obstacle avoidance responsibility coefficient of each robot, and dynamically adjust the formation holding weight and obstacle avoidance constraint weight in the model predictive control according to the obstacle avoidance responsibility coefficient. S9. Based on the adjusted weights, establish a model that includes formation error, control input, control input increment and dynamic safety constraints to obtain the control input sequence in the future prediction time domain and perform rolling optimization.

2. The obstacle avoidance control method for multi-mobile robots based on dynamic control obstacle function according to claim 1, characterized in that, In S2, the dynamic obstacle point cloud data is preprocessed, including: Spatial cropping is performed on the original point cloud data to retain the valid point cloud within the preset three-dimensional spatial range; The effective point cloud is downsampled and outlier filtering is performed to remove redundant and noise points. The preprocessed point cloud is mapped to a local raster map, and obstacle regions are extracted by point cloud density or elevation abrupt changes.

3. The obstacle avoidance control method for multi-mobile robots based on dynamic control obstacle function according to claim 1, characterized in that, In S3, the elliptical obstacle parameters include the position of the ellipse center. semi-major shaft semi-short shaft and direction angle ; The data association process involves: calculating the Euclidean distance between the center of the ellipse in the previous frame and the center of the ellipse in the current frame; constructing a matching cost matrix based on the distance; and using the minimum cost matching method to determine the correspondence between obstacles in adjacent frames. Ellipses that did not match any historical obstacles were identified as newly appearing obstacles and assigned a new number.

4. The obstacle avoidance control method for multi-mobile robots based on dynamic control obstacle function according to claim 3, characterized in that, In S5, uncertainty dilation is performed on elliptical obstacles in the prediction time domain, including: Extract the ellipse shape parameters of the j-th dynamic obstacle ; The shape fluctuation index is calculated based on the changes in the elliptical shape parameters of the most recent m frames. ; Combined with shape fluctuation expansion coefficient Predicting step-size expansion system and basic safety expansion amount Calculate the safe expansion amount under the prediction step s. ; Calculate the extended semi-major axis and semi-short axis The expression is as follows: 。 5. The obstacle avoidance control method for multi-mobile robots based on dynamic control obstacle function according to claim 4, characterized in that, In S7, discrete-time dynamic control obstacle function constraints are constructed, including: Calculate the angle between the direction from the i-th robot position to the center of the j-th extended elliptical obstacle and the major axis of the ellipse. ; Calculate the distance from the center of the extended ellipse to the boundary along this direction. ; Define robot-obstacle safety function ,in, To establish a preset safe distance; The discrete-time dynamic control obstacle function constraint is constructed as follows: 。 6. The obstacle avoidance control method for multiple mobile robots based on a dynamic control obstacle function according to claim 5, characterized in that, In S8, the obstacle avoidance responsibility coefficient of each robot is calculated, including: Based on the safety function value corresponding to each robot Determine the obstacle avoidance risk indicators for each robot. ; The obstacle avoidance responsibility coefficient of the i-th robot is calculated by normalization. ,in, The total number of robots; In the process of model predictive control optimization, the obstacle avoidance responsibility coefficient Larger robots are assigned higher obstacle avoidance constraint weights or lower formation maintenance weights.

7. The obstacle avoidance control method for multi-mobile robots based on dynamic control obstacle function according to claim 6, characterized in that, In S9, the objective function expression of the model is as follows: In the above formula, In robot state, For reference only. To control the input, To control the input increment, Q, R, S, and P are weight matrices, and N is the prediction time domain length.

8. The obstacle avoidance control method for multiple mobile robots based on a dynamic control obstacle function according to claim 1, characterized in that, In S1, the dynamic obstacle point cloud data is obtained by at least one of the following methods: LiDAR, depth camera, fixed environmental perception device, mobile robot on-board sensor, or simulation platform.

9. A multi-mobile robot obstacle avoidance control system based on a dynamic control obstacle function, applicable to the multi-mobile robot obstacle avoidance control method based on a dynamic control obstacle function as described in any one of claims 1-8, characterized in that, include: The perception module is used to acquire the status information of multiple mobile robots and the point cloud data of dynamic obstacles in the environment; The data processing module is used to filter and cluster point cloud data, and parameterize it into an extended elliptical obstacle model with predictive information. The formation control module is used to generate reference trajectories for each robot in the leader-follower formation; The responsibility allocation module is used to calculate the obstacle avoidance responsibility coefficient based on the relative safety margin between the robot and the obstacle. The model predictive control module is used to dynamically adjust the optimization weights based on the obstacle avoidance responsibility coefficient, and combined with the safety constraints of the dynamic control obstacle function, to solve the optimal control sequence in a rolling manner and send it to each mobile robot for execution.

Images