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

Abstract

The invention discloses a gas inspection robot cooperative control method and system based on UWB positioning. The method comprises the steps that S1, multi-modal data of an inspection robot are collected through a robot UWB label signal and a robot sensor; s2, performing state modeling on behavior data in the multi-modal data, and performing dynamic filtering iteration through an improved Kalman filtering algorithm to obtain accurate pose data; s3, constructing an inspection area task model based on the robot pose data, and outputting an optimal motion control instruction through the inspection area task model in combination with an optimization algorithm; and S4, performing cooperative control on the gas inspection robot through the motion control instruction, and outputting an optimal motion instruction through model prediction control rolling optimization, so that the multiple robots give consideration to high-risk task priority disposal and group collision avoidance safety in task allocation and path planning. A dynamic task redistribution mechanism can quickly reconstruct an inspection strategy when a single robot breaks down, and the global task coverage rate is guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a collaborative control method and system for a gas inspection robot based on UWB positioning. Background Art

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

[0003] At present, the existing gas inspection robots lack a task allocation mechanism based on dynamic environmental changes when working in groups. High-priority tasks (such as gas limit point disposal) and low-priority tasks cannot be dynamically scheduled, and response delays often occur. At the same time, multi-robot motion planning does not integrate environmental constraints such as the distribution of obstacles in the tunnel and the risk area of ​​gas accumulation. The problem of robot collision and congestion is prominent, making it difficult to achieve efficient collaboration in narrow tunnels. Therefore, a collaborative control method and system for gas inspection robots based on UWB positioning are proposed here. Summary of the Invention

[0004] In order to overcome the above-mentioned defects of the prior art and to achieve the above-mentioned objectives, the present invention proposes the following technical solutions: The collaborative control method of the gas inspection robot based on UWB positioning includes: S1: Collect multimodal data of the inspection robot through the robot's UWB tag signal and the robot's own sensors; S2: Model the behavior data in the multimodal data and obtain accurate posture data through dynamic filtering iteration using the improved Kalman filter algorithm; S3: Build an inspection area task model based on the robot's posture data, and output the optimal motion control instructions by combining the inspection area task model with the optimization algorithm; S4: Collaboratively control the gas inspection robot through motion control instructions.

[0005] The multimodal data includes behavior data and UWB tag data.

[0006] The implementation process of state modeling for behavioral data in multimodal data is as follows: Define the state dimensions of multimodal behavior data and derive the state transition relationships by dimension, 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: ; Among them, X is the state transition vector after state modeling, is the control input, is the noise, f is the state transfer function, which represents the state transfer relationship.

[0007] The relationship between the displacement increment data and the linear velocity in the state transition relationship is specifically as follows: Assume that the linear velocity is v and the linear acceleration is a. The linear velocity update formula is: ; The displacement increment data update is expressed as: ; in, is the time step, is the initial linear velocity, is the linear velocity at the next moment, is the initial displacement increment data, Displacement increment data at the next moment; The relationship between the displacement increment data and the angular velocity in the state transition relationship is specifically: Assume the angular velocity is , ,in, is the current attitude angle, is the angular velocity at the next moment, is the initial angular velocity; The vector of the robot's linear velocity and direction. When the direction changes, the displacement increment path generated by the same linear velocity will bend. The new linear velocity is expressed as Due to the change in direction, the actual displacement increment It is no longer a straight line, but a vector in the new direction: .

[0008] The process of obtaining accurate pose data by performing dynamic filtering iteration through the improved Kalman filter algorithm is as follows: Using the discretization method, we can obtain the The discrete form of state transition at time , perform a priori state prediction, and obtain the state estimate of the previous round of filtering iteration based on the Kalman filter algorithm , and predict Prior state at the moment , and obtain the prior covariance matrix ; collection The observation data matrix at time , by observing the function h on the prior state Derivative to obtain the observation matrix , based on the prior covariance matrix , observation data matrix , and the observed noise covariance matrix , get the Kalman gain ; Finally, the Kalman gain , fused observation data matrix and the prior covariance matrix , and obtain the new posterior state , based on the posterior state Regularize the data matrix according to the preset dimension order to obtain accurate pose data .

[0009] The process of constructing the inspection area task model is as follows: Break down underground gas inspection tasks into point, line, and surface elements; Based on accurate posture data , map the task elements into three-dimensional space, and construct a task model framework M that includes position constraints, task priority constraints, and environmental constraints, which can be expressed as: ; in is the priority weight of task point i, represents the robot's predicted position at time t+i in the future, is the coordinate of the i-th task point, is the environmental constraint, is the constraint penalty coefficient, is the objective function of the model; Constraints are embedded into the task model framework M, and the robot's physical motion constraints and dynamic constraints are considered and added to the task model framework M to obtain a complete inspection area task model.

[0010] The optimal motion control instruction acquisition process is as follows: Define the prediction horizon and control time domain ; In each control cycle, based on the current pose , predicting the future State sequence of the step ; Then, the objective function is iteratively optimized, and the objective function J of the task model framework M is combined with the control instruction sequence that minimizes the output of J through gradient descent iteration under motion constraints. , the control instruction sequence that minimizes J output is the optimal motion control instruction.

[0011] A collaborative control system for a gas inspection robot based on UWB positioning, comprising: Data acquisition module: collects multimodal data of the inspection robot through the robot's UWB tag signal and the robot's own sensors; Positioning solution module: It performs state modeling on the behavioral data in the multimodal data and obtains accurate posture data through dynamic filtering iteration through the improved Kalman filter algorithm; Task planning module: Builds an inspection area task model based on the robot's posture data, and outputs the optimal motion control instructions by combining the inspection area task model with the optimization algorithm; Collaborative control module: Collaboratively control the gas inspection robot through motion control instructions.

[0012] The present invention has the following beneficial effects: 1. Through a multimodal data time synchronization framework and an improved Kalman filter algorithm, UWB positioning data is deeply integrated with inertial measurement and odometer data, effectively suppressing the impact of non-line-of-sight noise and achieving centimeter-level pose estimation underground, ensuring that the robot accurately reaches the gas monitoring point. In addition, the dynamic noise adjustment mechanism can adaptively optimize the filtering parameters according to environmental changes, maintaining positioning reliability in scenarios where some sensors fail.

[0013] 2. The inspection area task model constructed based on point, line, and surface task models enables dynamic adjustment of the priority of gas limit-exceeding points and embedding of tunnel environmental constraints. Through model predictive control (MPC) rolling optimization, the optimal motion instructions are output, allowing multiple robots to take into account both high-risk task priority handling and group collision avoidance safety in task allocation and path planning. The dynamic task reallocation mechanism can quickly reconstruct the inspection strategy when a single robot fails, ensuring global task coverage and improving the intelligence and safety of underground inspections. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 This is a method step diagram of the collaborative control method and system of the gas inspection robot based on UWB positioning proposed by the present invention.

[0015] Figure 2 This is a system block diagram of the collaborative control method and system for the gas inspection robot based on UWB positioning proposed by the present invention. DETAILED DESCRIPTION

[0016] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0017] Example 1: Figure 1 As shown, the collaborative control method and system of the gas inspection robot based on UWB positioning proposed by the present invention include: S1: Collect multimodal data of the inspection robot through the robot's UWB tag signal and the robot's own sensors; Multimodal data includes behavioral data and UWB tag data. The collection process is as follows: For UWB tag data, a time synchronization framework is established so that the UWB tag signal and the robot's own sensor (IMU inertial measurement unit, odometry lidar) data are collected on the same time basis; For behavioral data, based on the UWB positioning timestamp, 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 10ms. And pre-process the collected raw data (behavior data and UWB tag data): The UWB data is treated with NLOS recognition algorithm to remove abnormal ranging values ​​caused by roadway occlusion, retaining valid data and performing normalization processing; The angular velocity and linear velocity data are preliminarily processed using complementary filtering or Kalman filtering to compensate for the gravity acceleration component (for example, the acceleration measured by the IMU is decomposed into motion acceleration and gravity acceleration components, and the pure motion acceleration data is extracted and normalized at the same time); The displacement increment data is combined with the accumulated displacement error of the UWB global position calibration and normalized simultaneously; Integrate the preprocessed raw behavior data and UWB tag data to form time-aligned multimodal data ; 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 posture changes. For example, the odometer is based on a wheeled or tracked motion mechanism to accumulate displacement information, and the lidar can scan the contours of the surrounding environment to assist in identifying obstacles and traversable areas.

[0018] S2: Model the behavior data in the multimodal data and obtain accurate posture data through dynamic filtering iteration using the improved Kalman filter algorithm; Because the behavioral data in multimodal data is nonlinear (in actual scenarios, the relationship between attitude angle and angular velocity is nonlinear due to rotation coupling, and the observation equation of UWB ranging is also nonlinear), traditional Kalman filtering cannot directly process it; The implementation process of state modeling is: Define the state dimension of multimodal 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: The relationship between displacement increment data and linear velocity is: Assume that the linear velocity is v and the linear acceleration is a, then the linear velocity update formula is: ; The displacement increment data update is expressed as: ; in, is the time step, is the initial linear velocity, is the linear velocity at the next moment, is the initial displacement increment data, Displacement increment data at the next moment; Specifically, based on Newton's laws of motion, linear velocity is the first derivative of position, and linear acceleration is the first derivative of linear velocity. Therefore, the displacement increment data (position update) is obtained by integrating the linear velocity; The relationship between displacement increment data and angular velocity is: Assume the angular velocity is , ,in, is the current attitude angle, is the angular velocity at the next moment, is the initial angular velocity; The vector of the robot's linear velocity and direction. When the direction changes, the displacement increment path generated by the same linear velocity will bend. The new linear velocity is expressed as Due to the change in direction, the actual displacement increment It is no longer a line, but a vector in the new direction: ; Specifically, angular velocity is the first-order derivative of the attitude angle (attitude angle change rate = angular velocity). Changes in attitude angle will cause the direction or path of the displacement increment to change (for example, when the robot turns, the angular velocity causes the attitude angle to deflect, thereby causing the displacement trajectory to bend). Based on the state transition relationship, a state transition equation is constructed to model the behavior state: The state transition relationship (position-velocity, attitude-angular velocity, etc.) is quantified and integrated into the state transition equation to realize the behavior state modeling, which is expressed as: ; Among them, X is the state transition vector after state modeling, is the control input, is the noise, f is the state transfer function, which represents the state transfer relationship; Specifically, the data collected by underground coal mine gas inspection robots come from different sources, formats, and dimensions. Through behavioral state modeling, by defining clear state dimensions (such as position, linear velocity, angular velocity, attitude angle, etc.), the scattered and heterogeneous multi-source data can be integrated into a unified state vector framework; The Kalman filter uses state transition relationships to predict the robot's next state based on the current state input. Without the motion quantization relationship constructed by behavioral state modeling, the filtering algorithm cannot obtain the next state and can only rely solely on observation data, which will lead to rapid error accumulation and inability to achieve accurate pose estimation. The improved Kalman filter algorithm acquisition process is: Since the robot controller operates in discrete time steps (e.g. 50ms) To execute the instruction, the continuous domain state transfer equation (differential form) needs to be converted into discrete form; First, discretization is performed, and the discretization method is used to obtain the time from time t to The discrete form of state transition at time: ; is the discrete form of the state transition at the next moment, is the control input (such as angular velocity and linear acceleration measured by IMU, f is the state transfer function; Then perform a priori state prediction and obtain the state estimate of the previous filtering iteration based on the Kalman filter algorithm , and predict Prior state at the moment ; At the same time, obtain the prior covariance matrix (reflecting the prediction uncertainty): ; is the covariance matrix of the posterior state at time t (quantification of state uncertainty after fusion observations), is the state transfer matrix (given by the state transfer function For the state vector The derivative is a constant matrix in the linear scenario and a variable matrix in the nonlinear scenario, reflecting the sensitivity of state changes to model parameters). is the process noise covariance matrix (describing Statistical properties of , such as noise variance; Construct observation equation collection The observation data matrix at time , where h is the observation function, which maps the prior state to the observation space, is the observation noise vector; By observing the function h, the prior state Derivative to obtain the observation matrix , reflecting the impact of state changes on observations, and performing Kalman gain calculation to obtain Kalman gain. The process is: Based on the prior covariance matrix , observation data matrix , and the observed noise covariance matrix , get the Kalman gain ; ,in, represents the transpose of the observation matrix; Specifically, through Describes the relationship between the uncertainty of the prior state and the observation process. The more uncertain the prior state is ( The larger the observation is, the more sensitive it is to state changes ( The larger the gain is, the more likely it is that the observation will be used to correct the prediction. The denominator integrates the uncertainty of the prior state transmission and the uncertainty of the observation noise to form the covariance of the observation residual. If the observation noise is small ( Small), the denominator approaches the numerator, the Kalman gain increases, and the observation is more trusted. If the prior state is very certain ( small), the denominator approaches , the gain decreases, and the prediction is trusted more; Finally, the Kalman gain , fusion observation data With the prior state , and obtain the new posterior state (the most reliable pose estimate at the current moment); The posterior state The position, speed, attitude angle and other data in the data are organized into a data matrix according to the preset dimension order. The matrix contains the most reliable posture data at the current moment, forming accurate posture data that can be directly read. ; Specifically, the new posterior state obtained by the Kalman gain Organize into data matrix , the key posture information such as position, speed, and attitude angle are organized into a format that can be directly read by the machine, so that subsequent models can directly call it.

[0019] S3: Build an inspection area task model based on the robot's posture data, and output the optimal motion control instructions by combining the inspection area task model with the optimization algorithm; The process of building the inspection area task model is as follows: First, we map the task elements and decompose the underground gas inspection task into point, line, and surface elements: Point features are key locations such as gas sensor deployment points and danger warning points, which need to be visited according to priority; Specifically: Definition: Key locations such as gas sensor deployment points, danger warning points, and ventilation monitoring points are accessed by priority; Constraint logic: Robots must arrive at high-priority points (such as gas over-limit warning points) first, while low-priority points (such as regular ventilation openings) can be flexibly dispatched. Line elements are preset inspection paths and lane connection routes for robots, constraining the robot's trajectory; Further: define the robot's preset inspection path (such as a reference line along the central axis of the lane), lane connection routes (such as accessible forks and air gate channels); Constraint logic: constrains the robot's trajectory to avoid entering non-connected areas (such as unopened dampers and collapsed sections); Area elements include gas accumulation risk areas, equipment-intensive areas, etc. Further: define gas accumulation risk areas (such as low-lying areas at the top of the tunnel), equipment-intensive areas (such as along belt conveyors), and obstacle distribution areas (such as ventilation ducts and temporarily stacked equipment); Constraint logic: The robot stays away from risky areas and is forced to avoid obstacles when encountering them; Based on accurate posture data , map these task elements into three-dimensional space, and construct a task model framework M that includes location constraints, task priority constraints, and environmental constraints, which can be expressed as: ; in is the priority weight of task point i, represents the robot's predicted position at time t+i in the future, is the coordinate of the i-th task point, is the environmental constraint (when encountering obstacles Increase), is the constraint penalty coefficient, is the objective function of the model; Specifically, when the task is executed, the weight The larger the value, the more likely the robot will approach the corresponding task point. For example, if a gas sensor triggers an over-limit warning ( Temporary increase), the robot will adjust the path, give priority to shortening the distance to the point, and the environmental constraint penalty The function is to encounter obstacles. Increase, force Ascend, drive the robot to adjust the path to avoid obstacles, for example, there is a ventilation duct underground ( corresponding to the pipeline area), when the robot approaches Rapid growth, Ascending, the optimization algorithm automatically adjusts the trajectory to stay away from the pipe; Constraints are embedded into the task model framework M, taking into account the robot's physical motion constraints (maximum linear velocity, maximum angular velocity) and dynamic constraints, and added to the task model framework M to obtain a complete inspection area task model: ; ; in, is the maximum current linear speed, is the maximum current angular velocity, is the maximum linear acceleration, is the maximum angular acceleration; Specifically, physical motion constraints ensure that the robot's motion complies with physical limits, avoiding inertial drift (affecting posture accuracy) or collisions (such as untimely emergency stops) caused by excessive speed. Dynamic constraints use the MPC (model predictive control) optimization method to solve the robot's motion control variables under the inspection area task model. The implementation process is as follows: First, perform rolling optimization window and define the prediction time domain (Motion state prediction for the next 5 seconds) and control time domain (solve the control instructions for the next 2 seconds); In each control cycle, based on the current pose , predicting the future State sequence of the step ; Then, the objective function is iteratively optimized, and the objective function J of the task model framework M is combined with the control instruction sequence that minimizes the output of J through gradient descent iteration under motion constraints. , the control instruction sequence that minimizes J output is the optimal motion control instruction; Specifically, the iterative optimization logic of the objective function is: First, the initial control quantity is assumed to be the current speed and angular velocity; Iterative adjustment: Calculate the J corresponding to the control variable and adjust the control variable (such as increasing the linear velocity to approach the high-priority task point, reducing the angular velocity to avoid collision) until J converges to the minimum value; Execution and feedback: only execute the first step of the control instruction , the next step is based on the new pose Re-optimize and achieve iterative optimization to adapt to the dynamic environment downhole (such as changes in gas concentration and temporary obstacles); The core of optimal motion control instructions is to use rolling optimization (iterative optimization) in a dynamic environment, taking only the currently optimal first-step instruction each time, abandoning invalid pre-calculated instructions, and adapting to the environment with real-time iteration. This logic balances the foresight of model predictions with environmental dynamics and computing power limitations, allowing the robot to efficiently complete inspection tasks in complex underground scenarios, safely avoid obstacles, and achieve dynamic optimal control.

[0020] S4: Coordinated control of the gas inspection robot through motion control instructions; The process of collaborative control of the gas inspection robot is as follows: The underground gas inspection task is decomposed into point, line, and surface subtasks. The ground dispatch center generates global tasks based on real-time monitoring multimodal data (behavior data and UWB tag data). Point tasks: gas sensor deployment points, danger warning points, marked by priority, such as Gas exceeding limit point, priority , For remote conventional points, , For nearby regular points, ; Line task: preset inspection path (such as a reference line along the central axis of the lane) to constrain the robot's trajectory; Surface tasks: Gas accumulation risk areas and equipment-intensive areas are marked as obstacle avoidance constraint surfaces; The ground dispatch center collects the real-time poses of multiple robots , running the inspection area task model and the MPC optimization algorithm output robot control single machine task list includes: like (Robot 1) Assignment (high priority limit point) and (Nearby regular points), (Robot 2) Assignment (remote conventional point); Allocation logic: Prioritize robots with short distances and low loads to perform high-priority tasks, such as (Robot 1) Distance (High priority over-limit point) is only 5m and has sufficient power, so it is allocated first; Based on the assigned subtask, each robot generates motion control instructions (linear velocity, angular velocity, etc.) to control the robot to perform the optimal inspection action.

[0021] Example 2: Figure 2 As shown, the UWB positioning-based gas inspection robot collaborative control system proposed in the present invention includes: Data acquisition module: collects multimodal data of the inspection robot through the robot's UWB tag signal and the robot's own sensors; Positioning solution module: It performs state modeling on the behavioral data in the multimodal data and obtains accurate posture data through dynamic filtering iteration through the improved Kalman filter algorithm; Task planning module: Builds an inspection area task model based on the robot's posture data, and outputs the optimal motion control instructions by combining the inspection area task model with the optimization algorithm; Collaborative control module: Collaboratively control the gas inspection robot through motion control instructions.

[0022] In the application, several formulas involved are calculated by taking their numerical values ​​after removing the dimensions, and the formulas are established by collecting a large amount of data and performing software simulation to obtain a formula for the most recent real situation. Some coefficients or weights in the formulas are set by technical personnel in this field according to actual conditions, so they will not be elaborated here.

[0023] The above embodiments can be implemented in whole or in part by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. Those skilled in the art will appreciate that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution.

[0024] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A collaborative control method for a gas inspection robot based on UWB positioning, characterized in that: include: S1: Collect multimodal data of the inspection robot through the robot's UWB tag signal and the robot's own sensors; S2: Model the behavior data in the multimodal data and obtain accurate posture data through dynamic filtering iteration using the improved Kalman filter algorithm; S3: Build an inspection area task model based on the robot's posture data, and output the optimal motion control instructions by combining the inspection area task model with the optimization algorithm; S4: Collaboratively control the gas inspection robot through motion control instructions.

2. The collaborative control method of a gas inspection robot based on UWB positioning according to claim 1 is characterized in that: The multimodal data includes behavior data and UWB tag data.

3. The collaborative control method of a gas inspection robot based on UWB positioning according to claim 2 is characterized in that: The implementation process of state modeling for behavioral data in multimodal data is as follows: Define the state dimensions of multimodal behavior data and derive the state transition relationships by dimension, 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: ； Among them, X is the state transition vector after state modeling, is the control input, is the noise, f is the state transfer function, which represents the state transfer relationship.

4. The collaborative control method of a gas inspection robot based on UWB positioning according to claim 3 is characterized in that: The relationship between the displacement increment data and the linear velocity in the state transition relationship is specifically as follows: Assume that the linear velocity is v and the linear acceleration is a. The linear velocity update formula is: ; The displacement increment data update is expressed as: ; in, is the time step, is the initial linear velocity, is the linear velocity at the next moment, is the initial displacement increment data, Displacement increment data at the next moment; The relationship between the displacement increment data and the angular velocity in the state transition relationship is specifically: Assume the angular velocity is , ,in, is the current attitude angle, is the angular velocity at the next moment, is the initial angular velocity; The vector of the robot's linear velocity and direction. When the direction changes, the displacement increment path generated by the same linear velocity will bend. The new linear velocity is expressed as Due to the change in direction, the actual displacement increment It is no longer a straight line, but a vector in the new direction: .

5. The collaborative control method of a gas inspection robot based on UWB positioning according to claim 4 is characterized in that: The process of obtaining accurate pose data by performing dynamic filtering iteration through the improved Kalman filter algorithm is as follows: Using the discretization method, we can obtain the The discrete form of state transition at time , perform a priori state prediction, and obtain the state estimate of the previous round of filtering iteration based on the Kalman filter algorithm , and predict Prior state at the moment , and obtain the prior covariance matrix ; collection The observation data matrix at time , by observing the function h on the prior state Derivative to obtain the observation matrix , based on the prior covariance matrix , observation data matrix , and the observed noise covariance matrix , get the Kalman gain ; Finally, the Kalman gain , fused observation data matrix and the prior covariance matrix , and obtain the new posterior state , based on the posterior state Regularize the data matrix according to the preset dimension order to obtain accurate pose data .

6. The collaborative control method of a gas inspection robot based on UWB positioning according to claim 5 is characterized in that: The process of constructing the inspection area task model is as follows: Break down underground gas inspection tasks into point, line, and surface elements; Based on accurate posture data , map the task elements into three-dimensional space, and construct a task model framework M that includes position constraints, task priority constraints, and environmental constraints, which can be expressed as: ； in is the priority weight of task point i, represents the robot's predicted position at time t+i in the future, is the coordinate of the i-th task point, is the environmental constraint, is the constraint penalty coefficient, is the objective function of the model; Constraints are embedded into the task model framework M, and the robot's physical motion constraints and dynamic constraints are considered and added to the task model framework M to obtain a complete inspection area task model.

7. The collaborative control method of a gas inspection robot based on UWB positioning according to claim 6 is characterized in that: The optimal motion control instruction acquisition process is as follows: Define the prediction horizon and control time domain ; In each control cycle, based on the current pose , predicting the future State sequence of the step ; Then, the objective function is iteratively optimized, and the objective function J of the task model framework M is combined with the control instruction sequence that minimizes the output of J through gradient descent iteration under motion constraints. , the control instruction sequence that minimizes J output is the optimal motion control instruction.

8. A collaborative control system for a gas inspection robot based on UWB positioning, according to the method according to any one of claims 1 to 7, characterized in that: include: Data acquisition module: collects multimodal data of the inspection robot through the robot's UWB tag signal and the robot's own sensors; Positioning solution module: It performs state modeling on the behavioral data in the multimodal data and obtains accurate posture data through dynamic filtering iteration through the improved Kalman filter algorithm; Task planning module: Builds an inspection area task model based on the robot's posture data, and outputs the optimal motion control instructions by combining the inspection area task model with the optimization algorithm; Collaborative control module: Collaboratively control the gas inspection robot through motion control instructions.

Images