Gas inspection robot cooperative control method and system based on UWB positioning

Abstract

The application discloses a gas inspection robot cooperative control method and system based on UWB positioning, comprising the following steps: S1, collecting multi-modal data of the inspection robot through a robot UWB tag signal and a robot sensor; S2, modeling the state of the behavior data in the multi-modal data, and obtaining accurate pose data through dynamic filtering iteration of an improved Kalman filtering algorithm; S3, constructing a task model of the inspection area based on the robot pose data, and outputting the best motion control instruction through the task model of the inspection area combined with an optimization algorithm; and S4, cooperatively controlling the gas inspection robot through the motion control instruction, and outputting the best motion instruction through model predictive control rolling optimization, so that the multi-robot can give priority to high-risk task disposal and group collision avoidance safety in task allocation and path planning, and a dynamic task redistribution mechanism can quickly reconstruct the inspection strategy when a single robot fails, thereby guaranteeing the global task coverage.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, and in particular to a gas inspection robot cooperative control method and system based on UWB positioning. BACKGROUND

[0002] Traditional gas inspection robots rely on a single sensor (such as an inertial measurement unit or an odometer) for positioning. In the complex environment of a coal mine, the positioning error accumulates rapidly over time due to factors such as roadway obstruction and electromagnetic interference, making it difficult to meet the precise inspection requirements of gas sensor deployment points. Although the ultra-wideband (UWB) positioning technology has the potential for centimeter-level positioning, the significant ranging noise caused by non-line-of-sight propagation makes it difficult for traditional filtering algorithms to effectively fuse multi-source heterogeneous data, resulting in insufficient reliability of pose estimation.

[0003] Currently, existing gas inspection robots lack a task allocation mechanism based on dynamic environmental changes when working in groups. High-priority tasks (such as gas over-limit point disposal) and low-priority tasks cannot be dynamically scheduled, often resulting in delayed responses. Additionally, multi-robot motion planning does not integrate environmental constraints such as roadway obstacle distribution and gas accumulation risk zones, leading to significant robot collision and congestion problems, making it difficult to achieve efficient cooperation in narrow roadways. Therefore, a gas inspection robot cooperative control method and system based on UWB positioning are proposed. SUMMARY

[0004] To overcome the above-mentioned defects of the prior art, in order to achieve the above-mentioned purposes, the present application proposes the following technical solutions:

[0005] The gas inspection robot cooperative control method based on UWB positioning comprises:

[0006] S1: Collecting multi-modal data of the inspection robot through robot UWB tag signals and robot self-sensor data;

[0007] S2: State modeling of behavior data in multi-modal data, and dynamic filtering iteration to obtain accurate pose data through improved Kalman filtering algorithm;

[0008] S3: Constructing a task model of the inspection area based on the robot pose data, and outputting the best motion control instruction through the task model of the inspection area combined with the optimization algorithm;

[0009] S4: Cooperative control of the gas inspection robot through the motion control instruction.

[0010] The multi-modal data includes behavior data and UWB tag data.

[0011] The implementation process of state modeling of behavior data in multi-modal data is as follows:

[0012] define the state dimension of the multi-modal behavior data, and derive the state transition relationship dimension by dimension, including the relationship between displacement increment data and linear velocity and the relationship between displacement increment data and angular velocity;

[0013] Based on the state transition relationship, a state transition equation is constructed for behavior state modeling, represented as:

[0014] ;

[0015] wherein X is a state transition vector after state modeling, is a control input, is noise, and f is a state transition function representing the state transition relationship.

[0016] The relationship between displacement increment data and linear velocity in the state transition relationship is specifically:

[0017] Let the linear velocity be v, the linear acceleration be a, and the linear velocity update formula be: ;

[0018] The displacement increment data update is represented as: ;

[0019] wherein is a time step, is an initial linear velocity, is a linear velocity at the next time, is an initial displacement increment data, is a displacement increment data at the next time;

[0020] The relationship between displacement increment data and angular velocity in the state transition relationship is specifically:

[0021] Let the angular velocity be , , wherein is a current attitude angle, is an angular velocity at the next time, is an initial angular velocity;

[0022] The vector of the linear velocity and direction of the robot, when the direction changes, the displacement increment path generated by the same linear velocity will be curved, and the new linear velocity is represented as Due to the change of direction, the actual displacement increment is no longer a straight line, which is a vector along the new direction: .

[0023] The process of obtaining accurate pose data through dynamic filtering iteration by improving the Kalman filtering algorithm is:

[0024] The discretization method is adopted to obtain the time t to Discrete form of state transition at time , the prior state prediction is performed, and the state estimation of the previous filtering iteration is obtained based on a Kalman filtering algorithm , and the prior state at time is predicted , and the prior covariance matrix is obtained at the same time

[0025] The observation data matrix at time is collected , the observation matrix is obtained by deriving the prior state through the observation function h , the Kalman gain is obtained based on the prior covariance matrix , the observation data matrix , and the noise covariance matrix obtained by observation

[0026] Finally, the Kalman gain is used to fuse the observation data matrix and the prior covariance matrix , to obtain the new posterior state , and the posterior state is regularized into a data matrix according to a preset dimension order to obtain accurate pose data .

[0027] The inspection area task model construction process is:

[0028] The underground gas inspection task is decomposed into point, line and surface elements;

[0029] Based on the accurate pose data , the task elements are mapped to the three-dimensional space to construct a task model framework M containing position constraints, task priority constraints and environment constraints, represented as:

[0030] ;

[0031] Wherein is the priority weight of the task point i, represents the predicted position of the robot at time t+i in the future, is the coordinate of the i-th task point, is the environmental constraint term, is the constraint penalty coefficient, is the objective function of the model;

[0032] The task model framework M is subjected to constraint embedding, considering the physical motion constraint and the dynamics constraint of the robot, and is added to the task model framework M to obtain a complete inspection area task model.

[0033] The optimal motion control instruction acquisition process is:

[0034] Define the prediction time domain And the control time domain ;

[0035] In each control cycle, based on the current pose , predict the state sequence of the future Step ;

[0036] Then perform target function iterative optimization, combine the target function J of the task model framework M, and iteratively solve the control instruction sequence that minimizes J output under motion constraints through gradient descent This control instruction sequence that minimizes J output is the optimal motion control instruction.

[0037] A gas inspection robot cooperative control system based on UWB positioning, comprising:

[0038] Data acquisition module: through the robot UWB tag signal and the robot's own sensor, collect multi-modal data of the inspection robot;

[0039] Positioning calculation module: state modeling is performed on the behavior data in the multi-modal data, and dynamic filtering iteration is performed through the improved Kalman filtering algorithm to obtain accurate pose data;

[0040] Task planning module: based on the robot pose data, a task model of the inspection area is constructed, and the optimal motion control instruction is output through the task model of the inspection area combined with the optimization algorithm;

[0041] Cooperative control module: the gas inspection robot is cooperatively controlled through the motion control instruction.

[0042] The present application has the following beneficial effects:

[0043] 1. Through the multi-modal data time synchronization framework and the improved Kalman filtering algorithm, the UWB positioning data and inertial measurement, odometer data are deeply fused, the influence of non-line-of-sight noise is effectively suppressed, cm-level pose estimation is realized underground, the robot can accurately reach the gas monitoring point, and the dynamic noise adjustment mechanism can adaptively optimize the filtering parameters according to the environmental changes, and the positioning reliability can still be maintained in the sensor partial failure scene.

[0044] 2. The task model of the inspection area constructed based on the point, line and surface task model, realizes dynamic adjustment of the priority of the gas overrun point and embedding of the roadway environment constraint, outputs the optimal motion instruction through model predictive control (MPC) rolling optimization, so that the multi-robot gives consideration to the high-risk task priority disposal and group collision avoidance safety in task allocation and path planning, the dynamic task re-allocation mechanism can quickly reconfigure the inspection strategy when a single robot fails, guarantees the global task coverage, and improves the intelligence and safety of the underground inspection. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 A method step diagram of the gas inspection robot cooperative control method and system based on UWB positioning proposed by the application.

[0046] Figure 2 A system block diagram of the gas inspection robot cooperative control method and system based on UWB positioning proposed by the application. DETAILED DESCRIPTION

[0047] The technical solutions in the embodiments of the application will be described clearly and completely below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the protection scope of the application.

[0048] Embodiment one: as shown in the figure, the gas inspection robot cooperative control method and system based on UWB positioning proposed by the application comprises: Figure 1

[0049] S1: Collect multi-modal data of the inspection robot through the robot UWB tag signal and the robot's own sensor;

[0050] The multi-modal data comprises behavior data and UWB tag data, and the collection process is as follows:

[0051] For the UWB tag data, a time synchronization framework is built to make the UWB tag signal and the robot's own sensor (IMU inertial measurement unit, odometer laser radar) data collected under the same time reference;

[0052] For the behavior data, the angular velocity and linear velocity data of the IMU, and the displacement increment data of the odometer are synchronously acquired at a fixed sampling period of 10 ms based on the timestamp of the UWB positioning;

[0053] And the collected original data (behavior data and UWB tag data) is preprocessed:

[0054] ​The UWB data is removed by the NLOS identification algorithm to eliminate abnormal ranging values caused by the shielding of the tunnel, and the effective data is retained and normalized;

[0055] The angular velocity and linear velocity data are preliminarily processed by complementary filtering or Kalman filtering to compensate the gravity acceleration component (such as decomposing the acceleration measured by the IMU into motion acceleration and gravity acceleration components, extracting pure motion acceleration data and simultaneously performing normalization processing;

[0056] The displacement increment data is combined with the global position calibration of the UWB to accumulate displacement error and simultaneously perform normalization processing;

[0057] The preprocessed original behavior data and UWB tag data are integrated to form time-aligned multi-modal data ;

[0058] Specifically, UWB (ultra-wideband positioning technology) uses extremely narrow pulse signals to achieve centimeter-level ranging and positioning in complex environments. The robot's own sensors can capture motion parameters such as angular velocity and linear acceleration in real time, reflecting the robot's instantaneous attitude changes. For example, an odometer based on a wheeled or tracked motion mechanism accumulates displacement information, and a laser radar can scan the surrounding environment profile to assist in identifying obstacles and passable areas.

[0059] S2: State modeling of behavior data in multi-modal data, and dynamic filtering iteration through improved Kalman filtering algorithm to obtain accurate pose data;

[0060] Because the behavior data in multi-modal data is nonlinear (in actual scenarios, the relationship between attitude angle and angular velocity is nonlinear due to rotational coupling, and the observation equation of UWB ranging is also nonlinear), traditional Kalman filtering cannot directly process it;

[0061] The implementation process of state modeling is as follows:

[0062] Define the state dimension of multi-modal behavior data, and derive the state transition relationship by dimension, including the relationship between displacement increment data and linear velocity and the relationship between displacement increment data and angular velocity:

[0063] The relationship between displacement increment data and linear velocity is:

[0064] Let the linear velocity be v and the linear acceleration be a, then the linear velocity update formula is: ;

[0065] The displacement increment data update is represented as: ;

[0066] where, is the time step, is the initial linear velocity, linear velocity for next time, initial displacement increment data, displacement increment data for next time;

[0067] Specifically, based on Newton's law of motion, the linear velocity is the first derivative of the position, and the linear acceleration is the first derivative of the linear velocity, so the displacement increment data (position update) is obtained by integrating the linear velocity;

[0068] The relationship between displacement increment data and angular velocity is:

[0069] Let the angular velocity be , where, is the current attitude angle, is the angular velocity for next time, is the initial angular velocity;

[0070] The vector of the linear velocity and direction of the robot, when the direction changes, the displacement increment path generated by the same linear velocity will be curved, and the new linear velocity is expressed as Due to the change of direction, the actual displacement increment is no longer a straight line, but a vector along the new direction: ;

[0071] Specifically, the angular velocity is the first derivative of the attitude angle (attitude angle change rate = angular velocity), and the change of the attitude angle will cause the direction or path of the displacement increment to change (such as when the robot turns, the angular velocity makes the attitude angle deflect, and then makes the displacement trajectory curve);

[0072] Based on the state transition relationship, the state transition equation is constructed to model the behavior state:

[0073] Quantify the state transition relationship (position-velocity, attitude-angular velocity, etc.), and integrate it into the state transition equation to realize behavior state modeling, which is expressed as:

[0074] ;

[0075] Where X is the state transition vector after state modeling, is the control input, is the noise, and f is the state transition function, representing the state transition relationship;

[0076] Specifically, the data collected by the coal mine gas inspection robot is from different sources, and the format and dimension are different. Through behavior state modeling, clear state dimensions (such as position, linear velocity, angular velocity, attitude angle, etc.) are defined, and scattered, heterogeneous multi-source data is integrated into a unified state vector framework;

[0077] Kalman filtering uses the state transition relationship to predict the robot's state at the next moment based on the current state input. If there is no motion quantization relationship constructed by behavioral state modeling, the filtering algorithm cannot obtain the next state and can only rely on the observation data, which will lead to the rapid accumulation of errors and make it impossible to achieve accurate pose estimation.

[0078] The improved Kalman filter algorithm obtains the following process:

[0079] Because the robot controller uses discrete time steps (e.g., 50ms) When executing an instruction, the continuous domain state transition equation (differential form) must be converted into a discrete form;

[0080] First, discretization is performed. Using the discretization method, the time from time t to... Discrete form of state transition at time step: ;

[0081] This is the discrete form of the state transition at the next time step. It is the control input (such as the angular velocity and linear acceleration measured by the IMU, and f is the state transition function);

[0082] Then, prior state prediction is performed, and the state estimate from the previous filtering iteration is obtained based on the Kalman filter algorithm. and predict Prior state at time ;

[0083] Simultaneously obtain the prior covariance matrix (reflecting prediction uncertainty): ;

[0084] Let be the covariance matrix of the posterior state at time t (the quantification of state uncertainty after fusion of observations). The state transition matrix (derived from the state transition function) For the state vector Differentiation reveals that the matrix is ​​constant in linear scenarios and variable in nonlinear scenarios, reflecting the sensitivity of model parameters to state changes. The process noise covariance matrix (description) Statistical characteristics, such as noise variance;

[0085] Constructing observation equations for data acquisition Observation data matrix at time Where h is the observation function, which maps the prior state to the observation space. For the observed noise vector;

[0086] The prior state is observed through the observation function h. Obtain the observation matrix by differentiation To reflect the impact of state changes on observations, Kalman gain is calculated to obtain the Kalman gain. The process is as follows:

[0087] Based on the prior covariance matrix Observation data matrix and the observed noise covariance matrix Obtain Kalman gain ; ,in, This represents the transpose of the observation matrix;

[0088] Specifically, through Describe the relationship between the uncertainty of the prior state and the observation process; the greater the uncertainty of the prior state ( Larger, more sensitive the observation to changes in state ( The larger the gain, the larger the numerator, and the more inclined to use observation correction for prediction. The denominator integrates the uncertainty of prior state propagation and the uncertainty of observation noise to form the covariance of the observation residual. If the observation noise is small ( Smaller denominators approach the numerator, increasing the Kalman gain and fostering greater confidence in observations, especially if the prior state is highly certain. (small), denominator approaches The gain decreases, leading to greater confidence in the prediction;

[0089] Finally, Kalman gain is utilized. fusion of observation data With prior state To obtain the new posterior state (The most reliable pose estimate at the current moment);

[0090] posterior state The position, velocity, attitude angle, and other data are organized into a data matrix according to a preset dimensional order. The matrix contains the most reliable pose data at the current moment, forming accurate pose data that can be directly read. ;

[0091] Specifically, by obtaining the new posterior state through Kalman gain Organize into a data matrix The key pose information, such as position, velocity, and attitude angle, is standardized into a format that can be directly read by the machine, making it easy for subsequent models to call it directly.

[0092] S3: Construct an inspection area task model based on robot pose data, and output the best motion control command by combining the inspection area task model with the optimization algorithm.

[0093] The process of constructing the inspection area task model is as follows:

[0094] First, task element mapping is performed, and the underground gas inspection task is decomposed into point, line, and surface elements:

[0095] Point elements are key locations such as gas sensor deployment points and danger warning points, which need to be accessed according to priority;

[0096] Specifically:

[0097] Definition: Key locations such as gas sensor deployment points, danger warning points, and ventilation monitoring points are accessed according to priority;

[0098] Constraint logic: High-priority points (such as gas over-limit warning points) need to be reached by the robot first, and low-priority points (such as regular ventilation openings) can be flexibly scheduled;

[0099] Line elements are pre-set inspection paths for robots and roadway connection routes, which constrain the robot's trajectory;

[0100] Further: Define the pre-set inspection path of the robot (such as the reference line along the center axis of the roadway) and the roadway connection route (such as passable junctions and air door passages);

[0101] Constraint logic: Constrain the robot's trajectory to avoid entering non-connected areas (such as closed air doors and collapsed sections);

[0102] Surface elements are gas accumulation risk areas and equipment-intensive areas;

[0103] Further: Define gas accumulation risk areas (such as low-lying areas in the roadway), equipment-intensive areas (such as along the belt conveyor), and obstacle distribution areas (such as ventilation pipes and temporarily stacked equipment);

[0104] Constraint logic: The robot should avoid risk surfaces and be forced to avoid obstacles when encountering them;

[0105] Based on accurate pose data Map these task elements to three-dimensional space and build a task model framework M containing position constraints, task priority constraints, and environmental constraints, represented as:

[0106] ;

[0107] Where is the priority weight of task point i, represents the robot's predicted position at future time t+i, is the coordinate of the i-th task point, is the environmental constraint term (when encountering obstacles increases), is the constraint penalty coefficient, is the objective function of the model;

[0108] Specifically, the weight is larger, the robot is closer to the corresponding task point, for example, if a gas sensor triggers an over-limit warning temporarily increases, the robot will adjust the path to shorten the distance to the point, and the environmental constraint penalty term acts as an obstacle, increases, forcing to rise, driving the robot to adjust the path to avoid obstacles, for example, there are ventilation pipes in the mine corresponding to the pipe area, the robot approaches quickly increases, rises, and the optimization algorithm will automatically adjust the trajectory to move away from the pipe;

[0109] The task model framework M is constrained and embedded, considering the physical motion constraints (maximum linear velocity, maximum angular velocity) and dynamics constraints of the robot, and added to the task model framework M to obtain a complete inspection area task model:

[0110] ;

[0111] ;

[0112] where, is the maximum current linear velocity, is the maximum current angular velocity, is the maximum linear acceleration, is the maximum angular acceleration;

[0113] Specifically, the physical motion constraint ensures that the robot motion conforms to the physical limit, avoiding inertia drift (affecting pose accuracy) or collision (such as sudden stop not in time) due to excessive speed, and the dynamics constraint uses the MPC (model predictive control) optimization method to solve the robot motion control under the inspection area task model. The implementation process is as follows:

[0114] First, a rolling optimization window is performed, defining a prediction time domain (future 5s motion state prediction) and a control time domain (solve the control instruction for the next 2s);

[0115] At each control cycle, based on the current pose , the state sequence of the next steps is predicted ;

[0116] Then, the objective function is iteratively optimized, combining the objective function J of the task model framework M, and the control instruction sequence that minimizes J is iteratively solved under the motion constraint , this control instruction sequence that minimizes J output is the optimal motion control instruction;

[0117] Specifically, the target function iterative optimization logic is:

[0118] First, the initial control quantity, assuming that the initial control quantity is the current speed, angular velocity;

[0119] Iterative adjustment: calculate the J corresponding to the control quantity, adjust the control quantity (such as increasing the linear speed to approach the high-priority task point, and reducing the angular velocity to avoid collision), until J converges to the minimum value;

[0120] Execution and feedback: only execute the first step control instruction , the next step is based on the new pose Re-optimization, iterative optimization, adapt to the dynamic environment (such as changes in gas concentration, temporary obstacles);

[0121] The core of the optimal motion control instruction is to take the current optimal first step instruction in the dynamic environment through rolling optimization (iterative optimization) each time, abandon the invalid pre-computed instructions, and adapt to the environment in real time. This logic balances the forward-looking of model prediction and environmental dynamics and computing power limitations, allowing the robot to efficiently complete the inspection task in the complex scene of the well and safely avoid obstacles, achieving dynamic optimal control.

[0122] S4: Cooperatively control the gas inspection robot through the motion control instruction;

[0123] The process of cooperatively controlling the gas inspection robot is:

[0124] The underground gas inspection task is decomposed into point, line, and surface sub-tasks, and the ground dispatch center generates a global task based on real-time monitoring of multi-modal data (behavior data and UWB tag data);

[0125] Point task: gas sensor deployment point, danger warning point, marked by priority, such as Gas over-limit point, priority , Remote regular point, , Nearby regular point, ;

[0126] Line task: preset inspection path (such as reference line along the axis of the roadway), constraint robot trajectory;

[0127] Surface task: gas accumulation risk area, equipment-intensive area, marked as obstacle avoidance constraint surface;

[0128] The ground dispatch center collects the real-time pose of multiple robots , running the inspection area task model and the MPC optimization algorithm outputs a single machine task list of robot control includes:

[0129] As (robots 1) allocation (high priority over-limit point) and (nearby conventional point), (robots 2) allocation (remote conventional point);

[0130] Allocation logic: prefer to let the robot with short distance and low load perform high priority tasks, such as (robots 1) distance (high priority over-limit point) is only 5m, and the power is sufficient, and is preferentially allocated;

[0131] Each robot generates motion control instructions (linear velocity, angular velocity, etc.) based on the assigned subtasks to control the robot to perform the best inspection action.

[0132] Embodiment two: as Figure 2 shown, the gas inspection robot cooperative control system based on UWB positioning proposed by the application includes:

[0133] Data acquisition module: through the robot UWB tag signal and the robot's own sensor, multi-modal data of the inspection robot is collected;

[0134] Positioning solution module: the behavior data in the multi-modal data is state modeled, and precise pose data is obtained through dynamic filtering iteration of the improved Kalman filtering algorithm;

[0135] Task planning module: based on the robot pose data, an inspection area task model is constructed, and the best motion control instruction is output through the inspection area task model combined with the optimization algorithm;

[0136] Cooperative control module: the gas inspection robot is cooperatively controlled through the motion control instruction.

[0137] In the application, several formulas involved are calculated by taking their values after dimensionless, and the establishment of the formula is obtained by software simulation of a formula closest to the real situation by collecting a large amount of data, and part of the coefficients or weights in the formula are set by the person skilled in the art according to the actual situation, so this will not be described in detail.

[0138] The above-described embodiments can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented by software, the above-described embodiments can be implemented in whole or in part in the form of a computer program product. A person of ordinary skill in the art can be aware that units and algorithm steps of the examples described in connection with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether the functions are performed by hardware or software depends on the specific application and design constraints of the technical solutions.

[0139] Although embodiments of the present application have been shown and described, it would be appreciated by those skilled in the art that changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the application, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for cooperative control of a gas inspection robot based on UWB positioning, characterized in that, The method comprises the following steps: S1: Collecting multi-modal data of the inspection robot through robot UWB tag signals and robot self-sensor; S2: State modeling on behavior data in the multi-modal data, and obtaining accurate pose data through dynamic filtering iteration of the improved Kalman filtering algorithm; The process of obtaining accurate pose data through dynamic filtering iteration of the improved Kalman filtering algorithm is as follows: Using a discretization method, the time from time t to... Discrete form of state transition at time t Prior state prediction is performed, and the state estimate of the previous filtering iteration is obtained based on the Kalman filter algorithm. and predict Prior state at time Simultaneously obtain the prior covariance matrix ; Collecting Observation data matrix of time , the prior state Derivation of observation data matrix , based on the prior covariance matrix , observation data matrix , and the observation noise covariance matrix , obtain Kalman gain ; Finally, the Kalman gain is utilized , the observation data matrix is fused with the prior covariance matrix , the new posterior state is obtained , the posterior state is used as a basis to regularize the data matrix in a preset dimension order to obtain accurate pose data ; S3: Constructing a task model of the inspection area based on the robot pose data, and outputting the best motion control instruction through the task model of the inspection area combined with an optimization algorithm; The construction process of the task model of the inspection area is as follows: Decomposing the underground gas inspection task into point, line and surface elements; Based on accurate pose data The task elements are mapped to a three-dimensional space, and a task model framework M containing position constraints, task priority constraints, and environment constraints is constructed, represented as: ； wherein is a priority weight of the task point i, represents a position predicted by the robot at a future time t+i, is the i-th task point coordinate, is an environmental constraint term, is a constraint penalty coefficient, is an objective function of the model; Constraining and embedding the task model framework M, considering the physical motion constraint and dynamics constraint of the robot, and adding them to the task model framework M to obtain a complete task model of the inspection area; S4: Cooperatively controlling the gas inspection robot through the motion control instruction.

2. The UWB positioning-based gas inspection robot cooperative control method according to claim 1, characterized in that, The multi-modal data comprises behavior data and UWB tag data.

3. The UWB positioning-based gas inspection robot cooperative control method according to claim 2, characterized in that, The implementation process of state modeling on the behavior data in the multi-modal data is as follows: Defining the state dimension of the multi-modal behavior data, and deriving the state transition relationship in different dimensions, including the relationship between displacement increment data and linear velocity and the relationship between displacement increment data and angular velocity; Based on the state transition relationship, a state transition equation is constructed to model the behavior state, which is expressed as: ； wherein X is a state transition vector after state modeling, is a control input, is a noise, and f is a state transition function representing a state transition relationship.

4. The UWB positioning-based gas inspection robot cooperative control method according to claim 3, characterized in that, The relationship between the displacement increment data and the linear velocity in the state transition relationship is specifically as follows: Let the linear velocity be v, the linear acceleration be a, and the linear velocity update formula be: ; The displacement increment data update is expressed as: ; wherein, is a time step, is an initial linear velocity, is a next time linear velocity, is an initial displacement delta data, a next time displacement delta data; The relationship between the displacement increment data and the angular velocity in the state transition relationship is specifically as follows: Let the angular velocity be , where, is the current attitude angle, is the attitude angle at the next time, is the initial angular velocity; The vector of the linear velocity and direction of the robot, when the direction changes, the displacement increment path produced by the same linear velocity will bend, and the new linear velocity is expressed as The actual displacement increment due to the change in direction Is no longer a straight line, and the vector along the new direction is: .

5. The UWB positioning-based gas inspection robot cooperative control method according to claim 1, characterized in that, The process of obtaining the best motion control instruction is as follows: defining a prediction time domain and a control time domain ; In each control cycle, based on the current pose , a sequence of states of future steps is predicted ; Then the objective function iteration optimization is performed, the objective function J of the task model framework M is combined, and the control instruction sequence that makes J output minimum is solved through gradient descent iteration under the motion constraint This control instruction sequence that makes J output minimum is the optimal motion control instruction.

6. A gas inspection robot cooperative control system based on UWB positioning, according to the method of any one of claims 1-5, characterized in that, The method comprises the following steps: A data collection module: collecting multi-modal data of the inspection robot through robot UWB tag signals and robot self-sensor; A positioning solution module: state modeling on behavior data in the multi-modal data, and obtaining accurate pose data through dynamic filtering iteration of the improved Kalman filtering algorithm; A task planning module: constructing a task model of the inspection area based on the robot pose data, and outputting the best motion control instruction through the task model of the inspection area combined with an optimization algorithm; A cooperative control module: cooperatively controlling the gas inspection robot through the motion control instruction.

Images