GIS in-place robot coordination control method with posture correction function

Abstract

The invention discloses a GIS in-place robot coordination control method with a posture correction function, and belongs to the technical field of robot coordination control. The method comprises the following steps: establishing a 2D-3D coordinate mapping corresponding relation; fusing the corresponding relation with an operation state predicted by an IMU-based inertial measurement and system dynamics model; performing error calculation on the obtained 6D pose information and target pose information; constructing a nonlinear kinetic equation of the robot, and converting a control target into an optimization problem of various constraints; and converting the obtained control instruction into a torque signal for actually driving the robot to move. According to the method, the influence of mismatching on pose estimation is effectively inhibited by establishing the 2D-3D coordinate mapping relation; through a joint torque PID control algorithm, the torque required by each joint is calculated in real time, and high-precision tracking of an expected trajectory is realized; an MPC optimization control framework with multiple constraints is constructed, and precise, smooth and safe coordination control over the GIS in-place process is achieved.

Description

Technical Field

[0001] This invention belongs to the field of robot coordination and control technology, specifically relating to a coordination and control method for a GIS positioning robot with posture correction. Background Technology

[0002] With economic and social development, land resources in urban centers are becoming increasingly scarce, making integrated and miniaturized equipment the preferred choice for substation construction. GIS (Gas Insulation System) has advantages such as small footprint, high reliability, and low maintenance workload, and is widely used in indoor power distribution systems. As the power grid continues to expand and urban power grids develop towards higher voltage and larger capacity, the demand for large GIS equipment in confined spaces such as urban center substations, underground substations, and offshore platforms is increasing. GIS units are large, structurally complex, and require high installation precision. Their placement process typically requires precise handling and attitude adjustment using hoisting and guide rail trolleys in narrow, high-risk environments. This not only results in long construction periods and high labor intensity but also poses significant safety and quality risks.

[0003] Currently, GIS placement operations at engineering sites still rely heavily on manual experience. Operators need to perform coordinated adjustments across multiple degrees of freedom within limited field of vision to ensure geometric constraints such as flange parallelism, bolt hole alignment, and busbar axis concentricity. To reduce reliance on manual experience and improve operational safety, academia and industry have begun to explore the use of industrial robots or mobile robots to assist GIS installation. However, existing robotic placement systems still face the following prominent problems in practical engineering applications.

[0004] On the one hand, in terms of pose perception and target recognition, existing methods mostly rely on monocular or binocular vision or a single inertial measurement unit (IMU). Visual positioning based on traditional feature points and geometric constraints is prone to problems such as unstable feature extraction, low matching rate, and high mismatch rate under complex working conditions such as changes in lighting, metallic reflections, equipment occlusion, and cluttered backgrounds in narrow spaces, resulting in insufficient accuracy in target component pose estimation. On the other hand, relying solely on IMUs for inertial navigation can lead to a rapid accumulation of pose errors over time due to factors such as integral drift and sensor zero bias, making it difficult to meet the millimeter-level position and high angle accuracy requirements in GIS positioning processes.

[0005] On the other hand, in terms of control and coordination mechanisms, many robot positioning systems still employ traditional joint space controllers based on PID controllers or simple trajectory tracking algorithms. These methods typically fail to systematically model the nonlinear dynamics of the robot, nor do they integrate pose errors and various physical constraints into the optimized control framework. This leads to: 1. Difficulty in simultaneously achieving end-effector positioning accuracy, motion smoothness, and actuator torque constraints; 2. Insufficient robustness to model uncertainties and external disturbances, making timely attitude correction difficult once pose estimation deviates. These problems significantly limit the widespread application of robots in the high-precision installation of large-scale power equipment such as GIS systems.

[0006] Therefore, the technical problem this invention aims to solve is how to address the issues of inaccurate posture perception, severe errors, and inaccurate control strategies in GIS placement robots, ultimately improving the automation level, installation accuracy, and operational safety of GIS placement operations in complex dynamic environments. Summary of the Invention

[0007] The purpose of this invention is to provide a coordinated control method for a GIS-based robot with posture correction, so as to solve the problems mentioned in the background art.

[0008] The objective of this invention is achieved as follows: a coordinated control method for a GIS-based in-situ robot with posture correction, characterized by the following steps:

[0009] Step S1: Use the visual sensors on the robot to acquire real-time visual images;

[0010] Step S2: Process the visual image to establish the correspondence between the two-dimensional pixel coordinates in the visual image and the three-dimensional spatial points of the object; the correspondence is a 2D-3D coordinate mapping correspondence.

[0011] Step S3: Fuse the 2D-3D coordinate mapping correspondence with the running state predicted by the IMU-based inertial measurement and system dynamics model to obtain the 6D pose information of the target object in the robot end effector;

[0012] Step S4: Calculate the error between the acquired 6D pose information and the target pose information to obtain the error vector;

[0013] Step S5: Construct the nonlinear dynamic equations of the robot, transform the control objective into an optimization problem with multiple constraints, and obtain the optimal control command at the current moment;

[0014] Step S6: Convert the optimized control commands into torque signals that actually drive the robot's motion, ensuring that the robot moves as expected.

[0015] Preferably, in step S2, the visual image is processed to establish the correspondence between the two-dimensional pixel coordinates in the visual image and the three-dimensional spatial points of the object, specifically as follows:

[0016] Step S2-1: Use the YOLOv8 object detection network to obtain the bounding box information of the target object in the visual image, realize the localization of the target object in the visual image, and preprocess the visual image:

[0017] Step S2-1-1: Visual image Process into standardized images ;in, The height of the visual image. The width of the visual image;

[0018] Step S2-1-2: Extract multi-level, multi-scale feature representations using the YOLOv8 object detection network's feature extraction backbone network:

[0019] Initial convolutional layer: performs preliminary feature extraction and 2x downsampling, represented as:

[0020] ;

[0021] in, This represents a 3×3 convolution operation. This indicates a batch normalization operation, which normalizes each channel. This represents the Sigmoid linear unit activation function; Indicates the step size;

[0022] Deep features are extracted step by step using four CSP modules:

[0023] Feature extraction and downsampling: ;

[0024] Extracting mid-level features to balance semantic information and spatial details:

[0025] ;

[0026] Extracting high-level semantic features: ;

[0027] Extracting the deepest features: ;

[0028] in, This indicates modules that span multiple stages. This indicates a downsampling operation; the original multi-scale feature map extracted by the feature extraction backbone network. ;

[0029] Step S2-1-3: Feature fusion using a neck network:

[0030] A top-down approach is adopted for feature fusion, ensuring that features at each scale contain semantic information from higher layers, thus addressing the problem of insufficient semantic information in lower-level features.

[0031] Top-level feature projection: Channel compression of the largest-scale features output by the backbone network.

[0032] ;

[0033] Mid-level feature fusion: The upsampled high-level semantic features are added to the mid-level features to achieve the sinking of semantic information.

[0034] ;

[0035] Low-level feature fusion: The features fused from the middle layer are further upsampled and combined with the low-level features to complete the transmission of semantic information throughout the process. ;

[0036] in, This represents a 1×1 convolution operation; Indicates an upsampling operation;

[0037] Feature enhancement is performed using a bottom-up approach to obtain enhanced multi-scale feature maps:

[0038] Low-level feature enhancement involves refining low-level features through convolution to extract rich spatial detail information.

[0039] ;

[0040] Mid-level feature enhancement adds the downsampled low-level detail features to the mid-level features, improving the localization accuracy of the mid-level features:

[0041] ;

[0042] Top-level feature enhancement combines downsampled mid-level enhanced features with the top-level features:

[0043] ;

[0044] in, This indicates a downsampling operation, where the neck network generates a fused and enhanced multi-scale feature map. ;

[0045] Step S2-1-4: Based on the multi-scale feature map fused by the neck network, use the detection and prediction head module to predict the bounding box coordinates, confidence score and class probability of the target;

[0046] Step S2-1-5: The post-processing optimization module performs quality filtering on the multi-scale prediction results output by the detection and prediction head module, selects the optimal detection box, and obtains the final bounding box. ;in, They are respectively coordinates and coordinate; This is the width of the bounding box; This represents the height of the bounding box;

[0047] Step S2-2: Use a deep feature extraction network based on the Transformer architecture to extract the features of the target object image and obtain the deep feature descriptors of the key points of the current image;

[0048] Step S2-3: Establish the 2D-3D coordinate mapping correspondence.

[0049] Preferably, in step S2-2, a deep feature extraction network with a Transformer architecture is used to extract features from the target region image, specifically as follows:

[0050] Step S2-2-1: Segment the preprocessed target region image into fixed-size image patches, and map each image patch to a high-dimensional embedding space through a linear projection layer to obtain an image patch embedding sequence. ;

[0051] Step S2-2-2: Embed the obtained image patch sequence Perform position encoding and summation:

[0052] The spatial location information of each image patch in the original two-dimensional image is encoded to generate a location encoding matrix. Each row of the position coding matrix corresponds to a unique two-dimensional position code for an image patch;

[0053] The location encoding matrix is ​​added element-wise to the image patch embedding sequence to inject spatial location information into each feature vector in the sequence: ;in, A feature sequence of spatial information;

[0054] Step S2-2-3: The feature sequence of spatial information is input into the encoder of the deep feature extraction network for deep feature transformation, at each layer. , , The following two sub-operations are executed in sequence:

[0055] Calculate multi-head self-attention, and then normalize the result through residual connections to obtain the intermediate feature sequence. ;

[0056] intermediate feature sequence By using a feedforward neural network, residual connections and layer normalization are performed to output the feature sequence. ;

[0057] go through The final high-level feature sequence after layer encoder processing ;

[0058] Step S2-2-4: Generate deep feature descriptors for image key points:

[0059] Select from 196 feature vectors One of the most representative features, serving as a deep feature descriptor for image keypoints. ,in, The number of feature points, For feature dimensions.

[0060] Preferably, the establishment of the 2D-3D coordinate mapping correspondence in steps S2-3 specifically involves:

[0061] S2-3-1: Calculate the similarity between the current image features and all features in the template feature library:

[0062] A 3D object model is obtained using a 3D scanner, and multiple feature points are defined using the 3D object model. Each 3D feature point generates a depth feature descriptor with viewpoint invariance, which are then aggregated into a template feature library.

[0063] Construct a similarity matrix Calculate the similarity between the current image features and all features in the template feature library, and use the similarity matrix. Matrix elements Indicates the first The image features and the first The similarity of template features is calculated using the following expression:

[0064] ;

[0065] in, This represents the natural exponential function. Indicates temperature parameter, This represents the summation index, traversing the template feature library from 1 to... All feature points, The first deep feature descriptor Feature vectors of key points in an image The first in the template feature library Feature vectors of template feature points; For feature dimensions;

[0066] S2-3-2: Finding the optimal correspondence:

[0067] Preliminary determination of matching relationship: a matching pair Acceptance is conditional upon meeting the following conditions simultaneously:

[0068] For image points Template points It is its most similar match; for template points Image points It is also its most similar match; all matching pairs that meet the above conditions are included in the initial matching set. ;

[0069] For the initial matching set Each matching pair in Calculate its ratio: ;

[0070] like If the match is , then the matching pair is retained, where The preset ratio threshold; the final set of matched pairs after filtering. ; Indicates intermediate variables;

[0071] S2-3-3: Generation of 2D-3D coordinate mapping:

[0072] Based on the descriptors in the template feature library index Find and obtain the corresponding three-dimensional spatial coordinates from the preset three-dimensional object model. Clarify the 2D-3D coordinate mapping pair ,in, This represents the final number of matching pairs.

[0073] Preferably, in step S3, the 2D-3D coordinate mapping correspondence is fused with the operating state predicted by IMU-based inertial measurement and system dynamics model to obtain the 6D pose information of the target object in the robot end effector, specifically:

[0074] Step S3-1: Based on the 2D-3D coordinate mapping correspondence, the optimal pose transformation matrix is ​​obtained by solving the optimization problem, and then converted into a Lie algebra format suitable for subsequent filtering and fusion.

[0075] The optimal pose transformation matrix is: ;

[0076] Among them, optimization problems This indicates minimizing the reprojection error. Let represent a 4×4 homogeneous transformation matrix. Let denote the special Euclidean group, which is the set of all 4×4 rigid body transformation matrices; This represents the Huber loss function, used to reduce the impact of mismatches; This represents the perspective projection function, which projects 3D points onto a 2D image plane. This indicates that 3D points in the world coordinate system Through pose transformation and camera internal reference Projected onto the image plane; Indicates the first The 2D pixel coordinates of a feature point in the image. ; Indicates the first The 3D coordinates of each feature point in the world coordinate system ; This represents the camera intrinsic parameter matrix. ; Indicates the number of final matching feature pairs; Let represent the pose transformation matrix of the camera relative to the target object; Denotes the Euclidean norm and calculates the length of a vector;

[0077] The optimal pose transformation matrix is ​​converted into a Lie algebra form suitable for subsequent filtering and fusion: ;

[0078] ;

[0079] in, Indicates will Transformation matrices in a group mapped to Lie algebras In space, a 6-dimensional vector representation is obtained; Represents the visual observation vector; Represents the Jacobian matrix of visual observation; This represents the noise covariance matrix of feature point detection, reflecting the uncertainty of feature extraction; Represents the visual observation noise covariance matrix;

[0080] Step S3-2: Based on the IMU's inertial measurement and system dynamics model, predict the robot's current motion state and uncertainties, specifically:

[0081] Define the state vector of the dynamic model IMU inertial measurement: ;

[0082] in, This is a position vector, representing the position of the machine in the world coordinate system; For attitude quaternions, representing the rotation of the machine relative to the world coordinate system; Let be the velocity vector, representing the linear velocity of the machine in the world coordinate system; Zero bias of the gyroscope indicates the systematic error in angular velocity measurement; Zero bias of the accelerometer indicates the systematic error in acceleration measurement; Indicates the transpose operation;

[0083] Based on the IMU-based dynamic model, the robot's current motion state is predicted: ;

[0084] in, Indicates time Measurement of angular velocity and acceleration; For IMU dynamics model; This represents the state vector from the previous time step. Indicates time Angular velocity and acceleration values ​​measured by the IMU;

[0085] IMU dynamic model Discretization is performed using the median integral method, and attitude, velocity, and position are updated sequentially through step-by-step integration, as follows:

[0086] Zero bias correction: Eliminating systematic errors in IMU sensors. , ;

[0087] in, This represents the corrected angular velocity. This indicates the corrected acceleration. This indicates the zero-bias estimate of the gyroscope at the previous moment. This indicates the zero bias estimate of the accelerometer at the previous moment; express The vector of raw acceleration values ​​obtained from IMU measurements at time 1;

[0088] Median calculation: , ;

[0089] in, This represents the median angular velocity. This represents the median acceleration. This indicates the angular velocity after correction at the previous moment. This represents the acceleration after correction at the previous moment;

[0090] Attitude update: Update the body rotation attitude based on angular velocity integral. ;

[0091] ; ;

[0092] in, Represents the rotation vector. This represents the quaternion corresponding to the rotation vector. This represents the quaternion multiplication operator. Represents the transformation function from a rotated vector to a quaternion;

[0093] Coordinate system transformation: Transform the acceleration in the body coordinate system to the world coordinate system:

[0094] ; ;

[0095] This represents the rotation matrix corresponding to the median pose; Represents the attitude quaternion at the median time. Represents acceleration in the world coordinate system; Represented as the gravitational acceleration vector in the world coordinate system;

[0096] Speed ​​update: Update the linear velocity of the machine body based on the integral of acceleration. ;

[0097] in, Represents the velocity vector at the current moment; Indicates the IMU integration time step;

[0098] Position Update: Update the unit's position based on velocity integral. ;

[0099] in, Represents the current position vector;

[0100] Zero bias prediction: estimating slow changes in sensor zero bias. , ;

[0101] in, This represents the gyroscope's zero-bias estimate at the current moment. This represents the current accelerometer bias estimate; [the result is missing from the original text] ;

[0102] Covariance prediction for the robot at the current moment: ;in, Represents the state transition Jacobian matrix. Represents the noise Jacobian matrix. Represents the IMU noise covariance matrix; Indicates based on Time information Prediction of the time-varying covariance matrix;

[0103] The IMU's predicted state is converted into a predicted pose that is directly compared with the visual observation pose. The predicted pose is:

[0104] ;

[0105] in, The observation model function directly extracts the position component from the state vector. quaternion pose Convert to angular axis representation The position and angular axis orientation are concatenated into a 6-dimensional vector to extract pose information from the state vector. This represents a function that converts a quaternion to an angular axis representation;

[0106] Step S3-3: Use the Kalman filter algorithm to perform optimal weighted fusion of the 2D-3D coordinate mapping on the system dynamics model to obtain the optimal state estimate;

[0107] Differences between computational vision pose observations and IMU dynamics predictions: ;

[0108] in, for The residual vector is observed at all times; for Time-based observation matrix; Represents the visual observation vector;

[0109] Calculate the Kalman gain matrix to determine the weights of the observation residuals on the state correction:

[0110] ;in, ,express The Kalman gain matrix at each time step; express Time covariance; express Time-based visual observation noise covariance matrix; express Time observation matrix Transpose of; Indicates based on Time information: Time prediction covariance matrix;

[0111] By correcting the predicted state using Kalman gain and observation residuals, the optimal state estimate is obtained: ;

[0112] The uncertainty in updating the state estimate reflects the confidence level of the fused state estimate:

[0113] ;

[0114] in, For based on Time information Prediction of the time-varying covariance matrix; It is the identity matrix;

[0115] Step S3-4: Convert the fused state estimate into the end-effector pose directly used by the robot control system, and perform coordinate transformation;

[0116] Extract position and orientation information from the state vector to construct a homogeneous transformation matrix. : ;

[0117] in, For quaternions The resulting rotation matrix; for The optimal position vector at time t; , representing the pose transformation matrix of the camera relative to the target;

[0118] The pose in the camera coordinate system is transformed to the robot end effector coordinate system through matrix multiplication, resulting in the standard coordinate system representation required for robot control. ;

[0119] in, The hand-eye calibration matrix is ​​obtained through the calibration process; This is the end-effector pose transformation matrix.

[0120] Preferably, in step S4, the acquired pose information and the target pose information are compared to calculate the error vector to obtain an error vector, specifically as follows:

[0121] The error transformation matrix is ​​calculated using matrix inversion and multiplication operations.

[0122] ;in, Here is the error transformation matrix. Through the The inverse operation is used to obtain it;

[0123] By applying the logarithmic mapping operation in Lie group theory, the elements of the SE(3) group are mapped to the Lie algebra space, transforming the complex matrix differential geometry operations into a linear representation in a 6-dimensional vector space, as expressed by:

[0124] ;in, Indicates translation error; This indicates rotational error.

[0125] Preferably, in step S5, constructing the nonlinear dynamic equations of the robot transforms the control objective into an optimization problem with multiple constraints, thereby obtaining the optimal control command at the current moment, specifically:

[0126] Step S5-1: Construct the nonlinear dynamic equations of the robot to predict future states and discretize them;

[0127] Define the state vector of the MPC controller model , ;

[0128] in, express Pose error vector at any given time. express The joint angle vector at any moment, express Joint velocity vector at any given moment; Indicates the transpose operation;

[0129] The continuous-time nonlinear dynamic equations are established as follows: ;

[0130] in, This represents the derivative of the state vector with respect to time. It is the control input vector, representing the joint acceleration command; It is a nonlinear function describing the dynamic characteristics of the system. Expanded according to the three components of the state vector:

[0131] ;in, These are the dynamic component functions of pose error, joint position, and joint velocity, respectively. It is the geometric Jacobian matrix of the robot; This represents the joint velocity vector. Represents the joint angle vector. Represents the control input vector;

[0132] Discrete state-space equations are obtained using a discretization method:

[0133] Discretization using the first-order forward Euler method ;

[0134] ;

[0135] The linearized discrete state-space equations are as follows: ;

[0136] in, It is a discrete-time index. This represents the state vector at the k-th sampling time. It is a control cycle. express Discrete state transition matrix at any given time; express Discrete control input matrix at different times; express Time-time discretization error and unmodeled dynamic terms; express The joint velocity vector at any moment, express Timing control vector;

[0137] Step S5-2: Based on optimal control theory, construct an optimization problem for the Model Predictive Controller (MPC) that includes an objective function and constraints, transforming the control objective into a solvable mathematical optimization problem:

[0138] The optimization problem is: ;

[0139] : ; ;

[0140] ; ;

[0141] in, This represents the objective function that needs to be minimized. This represents the state error penalty term, which penalizes the cumulative deviation of the system state from the expected trajectory in the prediction time domain; This represents the control input penalty term, which minimizes the energy consumed during the control process. This indicates a control increment penalty term, ensuring the timing smoothness of control instructions and suppressing abrupt changes; This represents the terminal state penalty term, which ensures the stability and convergence of the predicted time-domain terminal system; This indicates that the state error weight matrix is ​​positive definite and symmetric. This indicates that the control energy weight matrix is ​​positive definite and symmetric. This indicates that the control increment weight matrix is ​​positive definite and symmetric. This indicates that the terminal state weight matrix is ​​positive definite and symmetric. Indicates control increment; express Time-state vector; express Timing control vector; Indicates the length of the prediction time domain; Indicates dynamic constraints. Represents state constraints. Indicates control constraints. This indicates control increment constraints; This is the lower limit vector of the control quantity. This is the upper limit vector of the control quantity; This represents the lower bound of the state vector. Represents the upper bound of the state vector; This indicates the lower limit of the control increment. This indicates the upper limit of the control increment.

[0142] Preferably, in step S5-3, the optimization problem is transformed into a standard quadratic programming form, and the effective set method is used for the solution, specifically:

[0143] Step S5-3-1: Transform the optimization problem into a standard quadratic programming form: ;

[0144] in, Optimize variables, , dimension ; It is a Hessian matrix; The gradient vector; This is the constraint matrix; For constraint vectors;

[0145] Step S5-3-2: Solve the optimization problem using the effective set method:

[0146] Select an initial feasible control sequence Initial active constraint index set Set gradient convergence tolerance Constraints to violate tolerance Solve the equality constraint subproblem: ;

[0147] The optimal solution is obtained by solving using the Lagrange method. and Lagrange multiplier vectors ;

[0148] in, This represents the optimal solution in the current iteration; Represents the Lagrange multiplier vector; Represents the row vector of the i-th constraint; This represents the boundary value of the i-th constraint; This represents the working set at the k-th iteration;

[0149] Optimality test: If For all If true, check the gradient of the objective function; otherwise, remove the constraint corresponding to the least negative multiplier. Indicates the relationship with the first The Lagrange multipliers associated with each activity constraint;

[0150] The gradient of the objective function is determined as follows: And all inactive constraints are satisfied. Then return the optimal solution. ;

[0151] in, Represents the gradient of the objective function. ; The Jacobian matrix representing the working set constraints. Indicates not in the current working set The boundary value of the j-th constraint in the equation.

[0152] Preferably, the rolling optimization process based on model prediction in step S5-4 generates the optimal control quantity at the current moment, specifically as follows:

[0153] The optimal control sequence obtained by optimization is transformed into control instructions that can be actually executed at the current moment, and it is ensured that they meet the physical constraints of the system.

[0154] The first-term control strategy is adopted, which extracts only the first control variable from the sequence and applies it to the current time step: ;

[0155] in, This is the control vector to be applied; This represents the first control variable extracted from the optimal control sequence;

[0156] Limiting ensures that control commands remain within the actuator's tolerance range:

[0157] ;in, This is the lower limit vector of the control quantity. This is the upper limit vector of the control quantity; This represents the operation of finding the maximum value element by element in a vector. This represents the operation of finding the minimum value of each element of a vector;

[0158] Based on the system model and the control variables used in practical applications, predict the state at the next moment: ;

[0159] in, To predict the state vector, Here is the state transition matrix. Let this be the current state vector. To control the input matrix, This is the model error estimation vector;

[0160] Obtain actual state measurements and calculate prediction error: ;

[0161] in, For vector norm operations, For the prediction error scalar, This is the actual measured state vector;

[0162] The current solution results are used to accelerate the optimization calculation for the next time step, and the controller parameters are dynamically adjusted based on the prediction error to balance performance and robustness. Specifically:

[0163] Warm start initialization uses the current solution, shifted and used as the initial value for optimization in the next time step: ;

[0164] in, This is the initial vector for a hot start;

[0165] Parameters are adaptively adjusted, and the weights of the optimization problem are dynamically adjusted based on the prediction error. ;in, This is the adjusted state error weight matrix. This is the adjusted control weight matrix. This is the original state error weight matrix. This is the original control weight matrix. For state weight adaptive gain coefficients, To control the adaptive gain coefficient of the weights.

[0166] Preferably, in step S6, the optimized control command is converted into a torque signal that actually drives the robot's motion, specifically as follows:

[0167] Step S6-1: Using a numerical integration method, the acceleration command output by the MPC controller is converted into a joint position and velocity reference trajectory that can be directly tracked by the underlying driver;

[0168] The numerical integration method includes velocity integration and position integration. Velocity integration obtains the change in velocity by integrating the acceleration over a time interval, and the expression is: ;in, for Joint velocity vector at any given moment; For the present Joint velocity vector at any given moment; Indicates the current Optimal joint acceleration command at all times To control the cycle;

[0169] The position integral is obtained by integrating the velocity twice to obtain the position change, and the expression is: ;

[0170] in, for The joint position vector at any time, Indicates the current Joint position vector at any given time; This represents the displacement increment caused by the current velocity. This represents the displacement increment caused by acceleration;

[0171] Step S6-2: Compare the desired trajectory with the actual trajectory in real time, calculate the joint torque using the PID control algorithm, and send it to the driver;

[0172] The joint torque vector obtained by the PID control algorithm is: ;

[0173] in, express The joint torque vector calculated at each moment. Represents the proportional gain matrix. express Position tracking error vector at any given time. ; Represents the differential gain matrix. express The velocity tracking error vector at any given time, ; Represents the integral gain matrix. express The vector of cumulative error values ​​at each time step. , This represents the cumulative error at time k. This represents the position tracking error at time k; express The actual joint position vector at any given moment; Indicates the discrete time step index; express The actual joint velocity vector at any given time will Send to the joint driver.

[0174] Compared with the prior art, the present invention has the following improvements and advantages:

[0175] 1. By using the YOLOv8 target detection network, the target object can be quickly and accurately located and a similarity matrix can be constructed to establish a precise 2D-3D coordinate mapping relationship, effectively suppressing the impact of mismatches on pose estimation; an MPC optimization control framework considering multiple constraints is constructed to achieve precise, smooth and safe coordinated control of the GIS placement process.

[0176] 2. By using the joint torque PID control algorithm, the required torque of each joint is calculated in real time to achieve high-precision tracking of the desired trajectory. While ensuring the real-time performance of the control system, the trajectory tracking error, attitude oscillation and system divergence are effectively suppressed, and the smooth connection between control commands and actual torque drive is achieved, thereby improving trajectory tracking accuracy and system robustness.

[0177] 3. Introducing a hot start and weight adaptive mechanism, the state weights and control weights are automatically adjusted based on the optimization results of the previous moment and the current prediction error, achieving a dynamic balance between performance and robustness, shortening the optimization solution time, and significantly improving the automation level, safety and installation quality of GIS placement operations. Attached Figure Description

[0178] Figure 1 This is a flowchart illustrating the method of the present invention.

[0179] Figure 2 This is a schematic diagram of the YOLOv8 object detection network.

[0180] Figure 3 This is a schematic diagram of the YOLOv8 object detection network for environment perception and feature matching.

[0181] Figure 4 This diagram illustrates the comparison of pose estimation errors for different schemes in various scenarios.

[0182] Figure 5 This is a diagram showing the performance comparison results of the control schemes.

[0183] Figure 6 A diagram showing the real-time performance comparison results of the control schemes. Detailed Implementation

[0184] The invention will be further summarized below with reference to the accompanying drawings.

[0185] like Figure 1 As shown, a coordinated control method for a GIS-based in-situ robot with attitude correction is disclosed, comprising the following steps:

[0186] Step S1: Use the visual sensors on the robot to acquire real-time visual images;

[0187] Step S2: Process the visual image to establish the correspondence between the two-dimensional pixel coordinates in the visual image and the three-dimensional spatial points of the object; the correspondence is a 2D-3D coordinate mapping correspondence.

[0188] Step S2-1: Use the YOLOv8 object detection network to obtain the bounding box information of the target object in the visual image, realize the localization of the target object in the visual image, and preprocess the visual image:

[0189] The YOLOv8 object detection network includes an image preprocessing module, a feature extraction backbone module, a feature fusion neck module, a detection and prediction head module, and a post-processing optimization module. The image preprocessing module receives and standardizes the input image; specifically, it processes the visual image... Process into standardized images :in, The height of the visual image. The width of the visual image;

[0190] Size standardization scales images of any size to a fixed size: ;

[0191] Pixel value normalization: ;in, This represents the mean of the RGB channels in the ImageNet dataset. This represents the standard deviation of the RGB channels in the ImageNet dataset. This represents the bilinear interpolation scaling function;

[0192] The feature extraction backbone module is used to extract multi-level, multi-scale feature representations from images. The specific operations are as follows:

[0193] Initial convolutional layer: performs preliminary feature extraction and 2x downsampling, represented as:

[0194] ;

[0195] in, This represents a 3×3 convolution operation. This indicates a batch normalization operation, which normalizes each channel. This represents the Sigmoid linear unit activation function; Indicates the step size;

[0196] Deep features are extracted step by step using four CSP modules:

[0197] Feature extraction and downsampling: ;

[0198] Extracting mid-level features to balance semantic information and spatial details:

[0199] ;

[0200] Extracting high-level semantic features: ;

[0201] Extracting the deepest features: ;

[0202] in, This indicates modules that span multiple stages. This indicates a downsampling operation; the original multi-scale feature map extracted by the feature extraction backbone network. ;

[0203] The feature fusion neck module includes a top-down path and a bottom-up path. The top-down path is used for feature fusion, ensuring that features at each scale contain semantic information from higher layers, thus addressing the problem of insufficient semantic information in lower-level features. The specific operation is as follows:

[0204] Top-level feature projection: Channel compression of the largest-scale features output by the backbone network.

[0205] ;

[0206] Mid-level feature fusion: The upsampled high-level semantic features are added to the mid-level features to achieve the sinking of semantic information.

[0207] ;

[0208] Low-level feature fusion: The features fused from the middle layer are further upsampled and combined with the low-level features to complete the transmission of semantic information throughout the process. ;

[0209] in, This represents a 1×1 convolution operation; Indicates an upsampling operation;

[0210] Feature enhancement is performed using a bottom-up approach to obtain enhanced multi-scale feature maps. The specific steps are as follows:

[0211] Low-level feature enhancement involves refining low-level features through convolution to extract rich spatial detail information. ;

[0212] Mid-level feature enhancement adds the downsampled low-level detail features to the mid-level features, improving the localization accuracy of the mid-level features:

[0213] ;

[0214] Top-level feature enhancement combines downsampled mid-level enhanced features with the top-level features:

[0215] ;

[0216] in, This indicates a downsampling operation, where the neck network generates a fused and enhanced multi-scale feature map;

[0217] The detection and prediction head module predicts the bounding box coordinates, confidence score, and class probability of the target, specifically:

[0218] Small-scale target prediction: Fine-grained scanning of images using a dense 80×80 grid, specifically designed for detecting small-sized targets. ;

[0219] Mesoscale target prediction: Balancing detection accuracy and computational efficiency with a moderately dense 40×40 grid, suitable for detecting medium-sized targets. ;

[0220] Large-scale target prediction: Detecting large targets using a sparse 20×20 grid with a large receptive field.

[0221] ;in, This represents a 1×1 convolution operation;

[0222] Convert the raw parameters output by the network into actual bounding box coordinates: for each grid location ,in, , indicates row index, , indicates the column index;

[0223] Raw output extraction: ;

[0224] Center point coordinate decoding: , ;

[0225] Width and height decoding: , ;

[0226] Confidence calculation: ;

[0227] in, This represents the Sigmoid activation function. Indicates the center point Offset, Indicates the center point Offset; These represent the width and height ratio parameters, respectively. , ; The object presence confidence score represents the probability that a target exists at that location. Maximum class probability; original detection results ;

[0228] The post-processing optimization module performs quality filtering on the original detection results, selects the optimal detection box, and outputs the normalized result. Specifically, it merges the outputs of the three detection heads into a unified set of candidate detection boxes. The calculation process is as follows:

[0229] ,in, This represents the decoding and set merging functions; This represents the merged set of detection boxes;

[0230] Confidence filtering filters high-quality detection results based on confidence thresholds, significantly reducing the number of candidate boxes.

[0231] ;in, Indicates the confidence threshold; This represents the confidence score of the i-th detection box; This represents the set of high-quality candidate boxes filtered by the threshold.

[0232] Select the best result from the high-quality candidate boxes and normalize the output:

[0233] ;in, This represents a sorting function that operates on descending confidence levels.

[0234] Optimal result selection: ;in, This represents a function with a maximum value parameter.

[0235] Bounding box coordinate correction: ;in, Bounding box clipping function; final bounding box , , , , , , , , Ensure the bounding box does not extend beyond the image boundaries.

[0236] Step S2-2: Use a deep feature extraction network based on the Transformer architecture to extract image features of the target object and obtain deep feature descriptors of key points in the image.

[0237] Preprocessed target region image The image is segmented into fixed-size patches, and each patch is mapped to a high-dimensional embedding space through a trainable linear projection layer, resulting in an image patch embedding sequence. Embed the obtained image patches into the sequence. Perform position encoding and summation:

[0238] The spatial location information of each image patch in the original two-dimensional image is encoded to generate a location encoding matrix. Each row of the position coding matrix corresponds to a unique two-dimensional position code for an image patch;

[0239] The location encoding matrix is ​​added element-wise to the image patch embedding sequence to inject spatial location information into each feature vector in the sequence: ;in, A feature sequence of spatial information;

[0240] The feature sequence of spatial information is input into the encoder of the deep feature extraction network for deep feature transformation, at each layer. , , The following two sub-operations are executed in sequence:

[0241] Calculate multi-head self-attention, and then normalize the result through residual connections to obtain the intermediate feature sequence. The formula is: ;in, This represents a multi-head self-attention function. Indicates the first residual connection. Indicates the layer normalization function;

[0242] intermediate feature sequence By using a feedforward neural network, residual connections and layer normalization are performed to output the feature sequence. The formula is: ;in, This represents a feedforward neural network function that enables nonlinear transformation and enhancement of features; Indicates the second residual connection; after The final high-level feature sequence after layer encoder processing ;

[0243] Generate deep feature descriptors for image keypoints: Employ a feature selection strategy to filter out keypoints from 196 feature vectors. One of the most representative features, serving as a deep feature descriptor for image keypoints. ,in, The number of feature points, For feature dimensions.

[0244] Step S2-3: 2D-3D coordinate mapping correspondence:

[0245] The depth features extracted from the current image are matched with the preset 3D object model features to find and determine the unique 3D spatial point corresponding to each key 2D pixel in the image in the 3D model of the object, and establish the correspondence between the 2D pixel coordinates and the 3D spatial point of the object, providing a unique and necessary input for subsequent calculation of the object's precise 6D pose relative to the camera.

[0246] A 3D object model is obtained using a 3D scanner, and multiple feature points are defined using the 3D object model. Each 3D feature point generates a depth feature descriptor with viewpoint invariance, which are then aggregated into a template feature library.

[0247] Construct a similarity matrix Calculate the similarity between the current image features and all features in the template feature library, and use the similarity matrix. Matrix elements Indicates the first The image features and the first The similarity of template features is calculated using the following expression:

[0248] ;in, This represents the natural exponential function. Indicates temperature parameter, This represents the summation index, traversing the template feature library from 1 to... All feature points, The first deep feature descriptor Feature vectors of key points in an image The first in the template feature library Feature vectors of template feature points;

[0249] Finding the optimal correspondence: Preliminary determination of the matching relationship: a matching pair The condition for acceptance is that the following conditions must be met simultaneously for image points. Template points It is its most similar match; for template points Image points It is also its most similar match; all matching pairs that meet the above conditions are included in the initial matching set. ;

[0250] For the initial matching set Each matching pair in Calculate its ratio: ;

[0251] like If the match is , then the matching pair is retained, where The preset ratio threshold; the final set of matched pairs after filtering. ; Indicates intermediate variables;

[0252] 2D-3D coordinate mapping generation: based on descriptors in the template feature library index Find and obtain the corresponding three-dimensional spatial coordinates from the preset three-dimensional object model. Clarify the 2D-3D coordinate mapping pair ,in, The number of final matching pairs;

[0253] In step S3, the 2D-3D coordinate mapping correspondence is fused with the running state predicted by IMU-based inertial measurement and system dynamics model to obtain the 6D pose information of the target object in the robot's end effector, specifically:

[0254] Step S3-1: Based on the 2D-3D coordinate mapping correspondence, the optimal pose transformation matrix is ​​obtained by solving the optimization problem, and then converted into a Lie algebra format suitable for subsequent filtering and fusion.

[0255] The optimal pose transformation matrix is: ;

[0256] Among them, optimization problems This indicates minimizing the reprojection error. Let represent a 4×4 homogeneous transformation matrix. Let denote the special Euclidean group, which is the set of all 4×4 rigid body transformation matrices; This represents the Huber loss function, used to reduce the impact of mismatches; This represents the perspective projection function, which projects 3D points onto a 2D image plane. This indicates that 3D points in the world coordinate system Through pose transformation and camera internal reference Projected onto the image plane; Indicates the first The 2D pixel coordinates of a feature point in the image. ; Indicates the first The 3D coordinates of each feature point in the world coordinate system ; This represents the camera intrinsic parameter matrix. ; Indicates the number of final matching feature pairs; Let represent the pose transformation matrix of the camera relative to the target object; Denotes the Euclidean norm and calculates the length of a vector;

[0257] The optimal pose transformation matrix is ​​converted into a Lie algebra form suitable for subsequent filtering and fusion: ;

[0258] ;in, Indicates will Transformation matrices in a group mapped to Lie algebras In space, a 6-dimensional vector representation is obtained; Represents the visual observation vector; Represents the Jacobian matrix of visual observation; This represents the noise covariance matrix of feature point detection, reflecting the uncertainty of feature extraction; Represents the visual observation noise covariance matrix;

[0259] Step S3-2: Based on the IMU's inertial measurement and system dynamics model, predict the robot's current motion state and uncertainties, specifically:

[0260] Define the state vector of the dynamic model IMU inertial measurement: ;

[0261] in, This is a position vector, representing the position of the machine in the world coordinate system; For attitude quaternions, representing the rotation of the machine relative to the world coordinate system; Let be the velocity vector, representing the linear velocity of the machine in the world coordinate system; Zero bias of the gyroscope indicates the systematic error in angular velocity measurement; Zero bias of the accelerometer indicates the systematic error in acceleration measurement; Indicates the transpose operation;

[0262] Based on the IMU-based dynamic model, the robot's current motion state is predicted: ;

[0263] in, Indicates time Measurement of angular velocity and acceleration; For IMU dynamics model; This represents the state vector from the previous time step. Indicates time Angular velocity and acceleration values ​​measured by the IMU;

[0264] IMU dynamic model Discretization is performed using the median integral method, and attitude, velocity, and position are updated sequentially through step-by-step integration, as follows:

[0265] Zero bias correction: Eliminating systematic errors in IMU sensors. , ;

[0266] in, This represents the corrected angular velocity. This indicates the corrected acceleration. This indicates the zero-bias estimate of the gyroscope at the previous moment. This indicates the zero bias estimate of the accelerometer at the previous moment; express The vector of raw acceleration values ​​obtained from IMU measurements at time 1;

[0267] Median calculation: , ;

[0268] in, This represents the median angular velocity. This represents the median acceleration. This indicates the angular velocity after correction at the previous moment. This represents the acceleration after correction at the previous moment;

[0269] Attitude update: Update the body rotation attitude based on angular velocity integral. ;

[0270] ; ;

[0271] in, Represents the rotation vector. This represents the quaternion corresponding to the rotation vector. This represents the quaternion multiplication operator. Represents the transformation function from a rotated vector to a quaternion;

[0272] Coordinate system transformation: Transform the acceleration in the body coordinate system to the world coordinate system:

[0273] ; ;

[0274] This represents the rotation matrix corresponding to the median pose; Represents the attitude quaternion at the median time. Represents acceleration in the world coordinate system; Represented as the gravitational acceleration vector in the world coordinate system;

[0275] Speed ​​update: Update the linear velocity of the machine body based on the integral of acceleration. ;

[0276] in, Represents the velocity vector at the current moment; Indicates the IMU integration time step;

[0277] Position Update: Update the unit's position based on velocity integral. ;

[0278] in, Represents the current position vector;

[0279] Zero bias prediction: estimating slow changes in sensor zero bias. , ;

[0280] in, This represents the gyroscope's zero-bias estimate at the current moment. This represents the current accelerometer bias estimate; [the result is missing from the original text] ;

[0281] Covariance prediction for the robot at the current moment: ;in, Represents the state transition Jacobian matrix. Represents the noise Jacobian matrix. Represents the IMU noise covariance matrix; Indicates based on Time information Prediction of the time-varying covariance matrix;

[0282] The IMU's predicted state is converted into a predicted pose that is directly compared with the visual observation pose. The predicted pose is:

[0283] ;

[0284] in, The observation model function directly extracts the position component from the state vector. quaternion pose Convert to angular axis representation The position and angular axis orientation are concatenated into a 6-dimensional vector to extract pose information from the state vector. This represents a function that converts a quaternion to an angular axis representation;

[0285] Step S3-3: Use the Kalman filter algorithm to perform optimal weighted fusion of the 2D-3D coordinate mapping on the system dynamics model to obtain the optimal state estimate;

[0286] Differences between computational vision pose observations and IMU dynamics predictions: ;

[0287] in, for The residual vector is observed at all times; for Time-based observation matrix;

[0288] Calculate the Kalman gain matrix to determine the weights of the observation residuals on the state correction:

[0289] ;in, ,express The Kalman gain matrix at each time step; express Time covariance; express Time-based visual observation noise covariance matrix; express Time observation matrix Transpose of; Indicates based on Time information Prediction of the time-varying covariance matrix;

[0290] By correcting the predicted state using Kalman gain and observation residuals, the optimal state estimate is obtained: ;

[0291] The uncertainty in updating the state estimate reflects the confidence level of the fused state estimate:

[0292] ;

[0293] in, For based on Time information Prediction of the time-varying covariance matrix;

[0294] Step S3-4: Convert the fused state estimate into the end-effector pose directly used by the robot control system, and perform necessary coordinate transformations, specifically:

[0295] Extract position and orientation information from the state vector to construct a homogeneous transformation matrix. : ;

[0296] in, For quaternions The resulting rotation matrix; for The optimal position vector at time t; , representing the pose transformation matrix of the camera relative to the target;

[0297] The pose in the camera coordinate system is transformed to the robot end effector coordinate system through matrix multiplication, resulting in the standard coordinate system representation required for robot control. ;

[0298] in, The hand-eye calibration matrix is ​​obtained through the calibration process; This is the end-effector pose transformation matrix;

[0299] In step S4, the acquired pose information and the target pose information are compared to calculate the error vector, specifically:

[0300] The relative transformation matrix is ​​calculated using matrix inversion and multiplication operations. ;

[0301] in, Here is the error transformation matrix. Through the The inverse operation is used to obtain it;

[0302] By applying the logarithmic mapping operation in Lie group theory, the elements of the SE(3) group are mapped to the Lie algebra space, transforming the complex matrix differential geometry operations into a linear representation in a 6-dimensional vector space, as expressed by: ;

[0303] in, Indicates translation error; This indicates rotational error.

[0304] In step S5, the nonlinear dynamic equations of the robot are constructed, transforming the control objective into an optimization problem with multiple constraints, to obtain the optimal control command at the current moment, specifically as follows:

[0305] Step S5-1: Construct the nonlinear dynamic equations of the robot to predict future states and discretize them;

[0306] Define the state vector of the MPC controller model , ;

[0307] in, express Pose error vector at any given time. express The joint angle vector at any moment, express Joint velocity vector at any given moment; Indicates the transpose operation;

[0308] The continuous-time nonlinear dynamic equations are established as follows: ;

[0309] in, This represents the derivative of the state vector with respect to time. It is the control input vector, representing the joint acceleration command; It is a nonlinear function describing the dynamic characteristics of the system. Expanded according to the three components of the state vector:

[0310] ;in, These are the dynamic component functions of pose error, joint position, and joint velocity, respectively. It is the geometric Jacobian matrix of the robot; This represents the joint velocity vector. Represents the joint angle vector. Represents the control input vector;

[0311] Discrete state-space equations are obtained using a discretization method:

[0312] Discretization using the first-order forward Euler method ; ;

[0313] The linearized discrete state-space equations are as follows: ;

[0314] in, It is a discrete-time index. This represents the state vector at the k-th sampling time. It is a control cycle. express Discrete state transition matrix at any given time; express Discrete control input matrix at different times; express Time-time discretization error and unmodeled dynamic terms; express The joint velocity vector at any moment, express Timing control vector;

[0315] Step S5-2: Based on optimal control theory, construct an optimization problem for the Model Predictive Controller (MPC) that includes an objective function and constraints, transforming the control objective into a solvable mathematical optimization problem:

[0316] The optimization problem is: ;

[0317] ;

[0318] ; ;

[0319] ; ;

[0320] in, This represents the objective function that needs to be minimized. This represents the state error penalty term, which penalizes the cumulative deviation of the system state from the expected trajectory in the prediction time domain; This represents the control input penalty term, which minimizes the energy consumed during the control process. This indicates a control increment penalty term, ensuring the timing smoothness of control instructions and suppressing abrupt changes; This represents the terminal state penalty term, which ensures the stability and convergence of the predicted time-domain terminal system; This indicates that the state error weight matrix is ​​positive definite and symmetric. This indicates that the control energy weight matrix is ​​positive definite and symmetric. This indicates that the control increment weight matrix is ​​positive definite and symmetric. This indicates that the terminal state weight matrix is ​​positive definite and symmetric. Indicates control increment; express Time-state vector; express Timing control vector; Indicates the length of the prediction time domain; Indicates dynamic constraints. Represents state constraints. Indicates control constraints. This indicates control increment constraints; This is the lower limit vector of the control quantity. This is the upper limit vector of the control quantity; This represents the lower bound of the state vector. Represents the upper bound of the state vector; This indicates the lower limit of the control increment. This indicates the upper limit of the control increment;

[0321] In step S5-3, the optimization problem is transformed into a standard quadratic programming form, and the effective set method is used for the solution, specifically:

[0322] The optimization problem is transformed into a standard quadratic programming form: ;in, Optimize variables, , dimension ; It is a Hessian matrix; The gradient vector; This is the constraint matrix; For constraint vectors;

[0323] The optimization problem is solved using the effective set method: an initial feasible control sequence is selected. Initial active constraint index set Set gradient convergence tolerance Constraints to violate tolerance ;

[0324] Solve the equality constraint subproblem: ;

[0325] The optimal solution is obtained by solving using the Lagrange method. and Lagrange multiplier vectors ;

[0326] in, This represents the optimal solution in the current iteration; Represents the Lagrange multiplier vector; Represents the row vector of the i-th constraint; This represents the boundary value of the i-th constraint; This represents the working set at the k-th iteration;

[0327] Optimality test: If For all If true, check the gradient of the objective function; otherwise, remove the constraint corresponding to the least negative multiplier. Indicates the relationship with the first The Lagrange multipliers associated with each activity constraint; the gradient of the objective function is determined as follows:

[0328] And all inactive constraints are satisfied. Then return the optimal solution. ;in, Represents the gradient of the objective function. ; The Jacobian matrix represents the constraints of the working set.

[0329] Step S5-4, the rolling optimization process based on model prediction, generates the optimal control quantity at the current moment, specifically as follows:

[0330] The optimal control sequence obtained by optimization is transformed into control instructions that can be actually executed at the current moment, and it is ensured that they meet the physical constraints of the system.

[0331] The first-term control strategy is adopted, which extracts only the first control variable from the sequence and applies it to the current time step: ;

[0332] in, This is the control vector to be applied; This represents the first control variable extracted from the optimal control sequence;

[0333] Limiting ensures that control commands remain within the actuator's tolerance range: ;

[0334] in, This is the lower limit vector of the control quantity. This is the upper limit vector of the control quantity; This represents the operation of finding the maximum value element by element in a vector. This represents the operation of finding the minimum value of each element of a vector;

[0335] Based on the system model and the control variables used in practical applications, predict the state at the next moment: ;

[0336] in, To predict the state vector, Here is the state transition matrix. Let this be the current state vector. To control the input matrix, This is the model error estimation vector;

[0337] Obtain actual state measurements and calculate prediction error: ;

[0338] in, For vector norm operations, For the prediction error scalar, This is the actual measured state vector;

[0339] The current solution is used to accelerate the optimization calculation in the next time step, and the controller parameters are dynamically adjusted according to the prediction error to balance performance and robustness. Specifically, a warm-start initialization is performed, using the current solution as the initial value for the next time step optimization to improve computational efficiency.

[0340] ;in, This is the initial vector for a hot start;

[0341] Parameters are adaptively adjusted, and the weights of the optimization problem are dynamically adjusted based on the prediction error.

[0342] ;in, This is the adjusted state error weight matrix. This is the adjusted control weight matrix. This is the original state error weight matrix. This is the original control weight matrix. For state weight adaptive gain coefficients, To control the adaptive gain coefficient of the weights.

[0343] In step S6, the optimized control commands are converted into torque signals that actually drive the robot's motion, specifically as follows:

[0344] Step S6-1: Using a numerical integration method, the acceleration command output by the MPC controller is converted into a joint position and velocity reference trajectory that can be directly tracked by the underlying driver. The numerical integration method includes velocity integration and position integration. Velocity integration obtains the velocity change by integrating the acceleration over time, and the expression is: ;

[0345] in, for Joint velocity vector at any given moment; For the present Joint velocity vector at any given moment; Indicates the current Optimal joint acceleration command at all times To control the cycle;

[0346] The position integral is obtained by integrating the velocity twice to obtain the position change, and the expression is: ;

[0347] in, for The joint position vector at any time, Indicates the current Joint position vector at any given time; This represents the displacement increment caused by the current velocity. This represents the displacement increment caused by acceleration;

[0348] Step S6-2: Compare the desired trajectory with the actual trajectory in real time, calculate the joint torque using a PID control algorithm, and send it to the actuator; the joint torque vector obtained by the PID control algorithm is: ;

[0349] in, express The joint torque vector calculated at each moment. Represents the proportional gain matrix. express Position tracking error vector at any given time. ; Represents the differential gain matrix. express The velocity tracking error vector at any given time, ; Represents the integral gain matrix. express The vector of cumulative error values ​​at each time step. ; This represents the cumulative error at time k. This represents the position tracking error at time k; express The actual joint position vector at any given moment; Indicates the discrete time step index; express The actual joint velocity vector at any given time will Send to the joint driver.

[0350] To verify the feasibility and technical effectiveness of the present invention, the following experiments were conducted:

[0351] Experimental conditions: Sensor parameter settings: Monocular camera, global shutter, 30Hz, 1280×720 resolution, focal length 4mm, pixel Gaussian noise added;

[0352] Inertial Measurement Unit (IMU): Model BMI088, 100Hz, accelerometer noise density 0.02 m / s² / √Hz, gyroscope noise density 0.1 ° / s / √Hz; motion range: translation ±0.5m, rotation ±60°, simulating a typical indoor scene;

[0353] PVO pure vision: pose estimation is based solely on visual features, serving as a performance benchmark; VIO-Loose vision-inertial loose coupling: complementary filtering is used to fuse independent visual and IMU pose estimation results at the decision layer; the present invention's scheme: a tightly coupled scheme based on error state Kalman filtering, jointly optimizing visual reprojection error and IMU pre-integration error, and estimating IMU zero bias online; comparing the translation and rotation error performance of these three schemes in pose estimation under uniform motion and acceleration / deceleration motion scenarios; Figure 4 As shown, the translation error of the present invention is only 6.8mm and 10.2mm in uniform motion scenarios and acceleration / deceleration motion scenarios, respectively, which is much lower than the 12.3mm and 19.7mm of the VIO-Loose solution and the 23.7mm and 38.4mm of the PVO solution.

[0354] The rotational error of the present invention is only 0.41° and 0.58° in uniform motion scenarios and acceleration / deceleration motion scenarios, respectively, which is far lower than the 0.73° and 1.12° of the VIO-Loose scheme and the 1.28° and 2.15° of the PVO scheme. The core advantage of the present invention comes from its tightly coupled design based on error state Kalman filtering, which deeply integrates visual and IMU data at the most primitive state level.

[0355] The fundamental problem with the PVO scheme is its complete reliance on visual observation and lack of time-series dynamic model constraints. Specific issues include: 1. Sensitivity to high-frequency noise: visual observation is directly affected by image noise and fluctuations in feature point extraction. 2. Poor dynamic performance: during acceleration and deceleration, images are prone to motion blur, leading to decreased accuracy in feature extraction and matching.

[0356] The fundamental problem with the VIO-Loose approach is that although it fuses vision and IMU, it typically does so at the pose level rather than through deep fusion at the level of raw observations and states. Specific issues include: 1. Suboptimal fusion: The loosely coupled fusion rules are heuristic, resulting in inferior fusion performance compared to tightly coupled approaches. 2. Weak bias handling: Most loosely coupled approaches cannot accurately estimate IMU bias online. During acceleration and deceleration, uncompensated bias can cause significant drift in IMU predictions, which loose coupling cannot effectively correct. 3. Error propagation between modules: Pose estimation errors from the vision module are directly transmitted to the fusion center, which cannot correct for vision module defects through internal states as in tightly coupled approaches.

[0357] Experimental conditions: Sensor parameter settings: monocular camera, global shutter, 30Hz, 1280×720 resolution, focal length 4mm, pixel Gaussian noise added. IMU (Inertial Measurement Unit): BMI088 model, 100Hz, accelerometer noise density 0.02m / s² / √Hz, gyroscope noise density 0.1° / s / √Hz. Robot model: 6-DOF UR5 industrial robot. Target object: standard cubic workpiece.

[0358] Experimental scenario design: Static target alignment task: The robot aligns with a fixed target from different initial poses; Dynamic target tracking task: The target moves at a constant speed of 0.1 m / s, and the robot tracks it; Anti-interference test: An instantaneous external force disturbance is applied during the movement; Traditional PID control: Scheme design: Perception module: Only visual pose estimation is used, YOLOv8 network plus Transformer; Control module: 6 independent PID controllers; Vision + IMU fusion PID control: Perception module: Complete vision-IMU Kalman filter fusion; Control module: 6 independent PID controllers;

[0359] The present invention employs a sensing module that fuses visual perception with IMU Kalman filtering, and a control module that utilizes constrained MPC optimization control.

[0360] Comparing the performance of the three schemes above in terms of settling time (the time required for the 6D pose error vector to enter and maintain the allowable error band centered on the target value within ±2%) and maximum over-adjustment (the percentage of the maximum instantaneous deviation of the output from the target value to the target change), as follows: Figure 5 As shown, the adjustment time of the present invention is 1.5s, which is lower than the 2.4s of vision + IMU fusion PID control and the 3.2s of traditional PID control. The maximum adjustment amount of the present invention is 5.2%, which is far lower than the 13.1% of vision + IMU fusion PID control and the 17.6% of traditional PID control.

[0361] Experimental Scenario Design: Traditional PID Control: Scheme Design: Perception Module: Visual pose estimation only, YOLOv8 network plus Transformer. Control Module: 6 independent PID controllers. Vision + IMU Fusion PID Control: Perception Module: Complete vision-IMU Kalman filter fusion; Control Module: 6 independent PID controllers. In this Invention: Perception Module: Vision-IMU Kalman filter fusion; Control Module: Constrained MPC optimization control.

[0362] Comparing the performance of the three solutions above in terms of cycle jitter and real-time performance guarantee:

[0363] Cycle jitter is defined as the statistical mean square error of the actual execution time of the control cycle, and real-time performance guarantee rate is defined as the proportion of the system control cycle time that meets the real-time requirements within the statistical period.

[0364] Cycle jitter performance analysis: This invention achieves an ultra-low cycle jitter of 0.55ms, which is significantly superior to the 4.2ms of traditional PID control and the 1.8ms of vision + IMU fusion PID control. This is mainly due to the following technological innovations: 1. Effectiveness of the warm-start MPC mechanism: By shifting the solution sequence of the previous optimization cycle as the initial guess for the current cycle, the number of MPC iterations is significantly reduced, improving the time determinism of the control cycle. 2. Timing optimization through multi-sensor fusion: Vision-IMU Kalman filtering fusion effectively smooths out the time fluctuations in vision processing, and the high-frequency data from the IMU compensates for the delay in vision processing, ensuring high temporal continuity of the pose estimation output.

[0365] Real-time performance guarantee rate analysis: This invention achieves a real-time performance guarantee rate of 99.1%, a significant improvement over the 71.3% of traditional PID control and the 90.8% of vision + IMU fusion PID control, meeting the hard real-time requirements of industrial-grade control systems: 1. Numerical reliability of the effective set method: The effective set method is used to solve the quadratic programming problem, ensuring a high success rate of convergence to the KKT point within a finite number of steps. Even under numerical ill-conditioned conditions, it can still return a feasible solution, ensuring continuous output of control commands. 2. Intelligent degradation with adaptive weight adjustment: Based on the prediction error, the MPC weight parameters are dynamically adjusted, automatically simplifying the complexity of the optimization problem when computational resources are limited, prioritizing the fulfillment of the control cycle time limit.

[0366] In pose estimation, the tightly coupled design based on error-state Kalman filtering demonstrates excellent accuracy and dynamic disturbance resistance in both constant speed and acceleration / deceleration scenarios. Regarding control performance, the proposed solution, combining tightly coupled pose estimation with constrained model predictive control, achieves the shortest settling time and minimum overshoot, proving its fast, stable, and accurate closed-loop control capabilities. In terms of system real-time performance, the proposed solution achieves extremely low period jitter and high real-time performance through innovations such as hot-start MPC and an effective set solver, meeting the hard real-time requirements of industrial applications.

[0367] The above description is merely an embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principle of the present invention should be included within the scope of the claims of the present invention.

Claims

1. A coordinated control method for a GIS-based in-situ robot with attitude correction, characterized in that: The method comprises the following steps: Step S1: acquiring real-time visual images by using a vision sensor carried by a robot; Step S2: processing the visual images to establish a corresponding relationship between two-dimensional pixel coordinates in the visual images and three-dimensional space points of an object; the corresponding relationship is a 2D-3D coordinate mapping corresponding relationship; Step S3: fusing the 2D-3D coordinate mapping corresponding relationship with inertial measurement based on an IMU and a predicted running state of a system dynamics model to obtain 6D pose information of a target object in an end effector of the robot; Step S4: performing error calculation on the obtained 6D pose information and target pose information to obtain an error vector; Step S5: constructing a nonlinear dynamics equation of the robot to convert a control target into an optimization problem with multiple constraints to obtain optimal control instructions at a current time; Step S6: converting the optimized control instructions into torque signals for actually driving the robot to move to ensure that the robot moves as expected.

2. The method according to claim 1, characterized in that: In the step S2, the visual images are processed to establish a corresponding relationship between two-dimensional pixel coordinates in the visual images and three-dimensional space points of an object, specifically as follows: Step S2-1: acquiring boundary box information of the target object in the visual images by using a YOLOv8 target detection network to realize positioning of the target object in the visual images and to pre-process the visual images: Step S2-1-1: obtaining a visual image processing the visual image into a normalized image ; wherein, is a height of the visual image, is a width of the visual image; Step S2-1-2: extracting multi-level and multi-scale feature representations by using a feature extraction backbone network of the YOLOv8 target detection network: An initial convolutional layer is used for preliminary feature extraction and 2 times down-sampling, which is represented as: ； wherein, denotes a 3x3 convolution operation, denotes a batch normalization operation, normalizing each channel; denotes a Sigmoid linear unit activation function; denotes a step size; Deep layer features are gradually extracted through 4 CSP modules: extract features and downsample: ; Middle-level features are extracted to balance semantic information and spatial details: ； Extracting high-level semantic features: ; Extracting the deepest features: ; wherein, denotes a cross-stage partial module, denotes a down-sampling operation; original multi-scale feature maps extracted by the feature extraction backbone network ; Step S2-1-3: feature fusion is performed by using a neck network: Feature fusion is performed by using a top-down path so that features at each scale contain semantic information from higher layers, solving the problem of insufficient semantic information of low-level features: Top layer feature projection: the maximum scale feature output by the backbone network is compressed in channels: ； Middle layer feature fusion: the high layer semantic features after up-sampling are added to the middle layer features to realize sinking of semantic information: ； Bottom layer feature fusion: the middle layer fused features are further up-sampled and combined with the bottom layer features to complete the whole process of semantic information transmission: ; wherein denotes a 1 x 1 convolution operation; denotes an up-sampling operation; Feature enhancement is performed by using a bottom-up path to obtain enhanced multi-scale feature maps: Bottom layer feature enhancement: the bottom layer features are refined by convolution to extract rich spatial detail information: ； Middle layer feature enhancement: the bottom layer detail features after down-sampling are added to the middle layer features to improve the positioning accuracy of the middle layer features: ； Top layer feature enhancement: the middle layer enhanced features after down-sampling are combined with the top layer features: ; wherein, denotes a down-sampling operation, the neck network generates fused enhanced multi-scale feature maps ; Step S2-1-4: based on the multi-scale feature maps fused by the neck network, a detection and prediction head module is used to predict boundary box coordinates, confidence and class probability of the target; Step S2-1-5: quality filtering is performed on the multi-scale prediction results output by the detection prediction head module by using a post-processing optimization module, an optimal detection frame is selected, and a final bounding box is obtained ; wherein, are respectively coordinates and coordinates; is a bounding box width; is a height of the bounding box; Step S2-2: a deep feature extraction network based on a Transformer architecture is used to extract image features of the target object to obtain a depth feature descriptor of a key point in the current image; Step S2-3: a 2D-3D coordinate mapping corresponding relationship is established.

3. The method according to claim 2, characterized in that: In the step S2-2, the deep feature extraction network based on the Transformer architecture is used to extract features of the target region image, specifically as follows: Step S2-2-1: segment the pre-processed target region image into fixed-size image patches, and map each image patch through a linear projection layer to a high-dimensional embedding space to obtain an image patch embedding sequence ; Step S2-2-2: Embedding the obtained image block into the sequence Position encoding and addition fusion: spatial location information of each image block in the original two-dimensional image is position encoded to generate a position encoding matrix each row of the position encoding matrix corresponds to a unique two-dimensional position encoding of one image block; The position encoding matrix is added element-wise to the image patch embedding sequence to inject spatial position information for each feature vector in the sequence: ; wherein a feature sequence for spatial information; Step S2-2-3: the feature sequence of the spatial information is input into an encoder of the deep feature extraction network for deep feature transformation, and each layer of the deep feature extraction network is configured to perform the following two sub-operations in sequence: , , , The multi-head self-attention is calculated, and an intermediate feature sequence is obtained through a residual connection and layer normalization ; The intermediate feature sequence is obtained by The output feature sequence is obtained by performing residual connection and layer normalization on the intermediate feature sequence through a feedforward neural network ; after final high-level feature sequence processed by the layer encoder ; Step S2-2-4: generate a deep feature descriptor of the image key point: Select from 196 feature vectors One of the most representative features, serving as a deep feature descriptor for image keypoints. ,in, The number of feature points, For feature dimensions.

4. The method of claim 2, wherein the method further comprises: determining a position of the robot; and determining a position of the GIS; and determining a position of the robot relative to the GIS. The step S2-3 establishes a 2D-3D coordinate mapping correspondence, specifically: S2-3-1: calculate the similarity between the current image features and all features in the template feature library: A three-dimensional object model is acquired by a three-dimensional scanner, and the three-dimensional object model is used to define a plurality of feature points Each 3D feature point generates a depth feature descriptor with viewpoint invariance, and finally converges into a template feature library Construct a similarity matrix Calculate the similarity between the current image features and all features in the template feature library, and use the similarity matrix. Matrix elements Indicates the first The image features and the first The similarity of template features is calculated using the following expression: ； wherein, denotes the natural exponential function, denotes the temperature parameter, denotes the summation index, iterating over all feature points in the template feature library from 1 to denotes the feature vector of the th image keypoint of the depth feature descriptor, denotes the feature vector of the th template feature point in the template feature library; is the feature dimension;​ S2-3-2: find the optimal correspondence: Preliminary determination of matching relationship: one matching pair The conditions for acceptance are that the following conditions are simultaneously met: For image point , the template point is its most similar match; for template point , the image point is also its most similar match; all matching pairs satisfying the above conditions are included in the preliminary matching set ; For the initial matching set Each matching pair in Calculate its ratio: ; If , then the matching pair is reserved, wherein is a preset ratio threshold; the final matching pair set after screening ; represents an intermediate variable; S2-3-3: 2D-3D coordinate mapping generation: Based on the descriptors in the template feature library index Find and obtain the corresponding three-dimensional spatial coordinates from the preset three-dimensional object model. Clarify the 2D-3D coordinate mapping pair ,in, This represents the final number of matching pairs.

5. The coordinated control method of the GIS installation robot with posture correction according to claim 1, characterized in that: The step S3 fuses the 2D-3D coordinate mapping correspondence with the inertial measurement based on IMU and the predicted running state of the system dynamics model to obtain the 6D pose information of the target object in the robot end effector, specifically: Step S3-1: based on the 2D-3D coordinate mapping correspondence, the optimal pose transformation matrix is obtained by solving the optimization problem, and it is converted into Lie algebra format suitable for subsequent filtering fusion; The optimal pose transformation matrix is: ; where the optimization problem minimizes the reprojection error, is a 4x4 homogeneous transformation matrix, denotes the special Euclidean group, which is the set of all 4x4 rigid body transformation matrices; denotes the Huber loss function, which reduces the influence of mis-matched points; denotes the perspective projection function, which projects a 3D point to a 2D image plane; denotes the 3D point in the world coordinate system, is transformed by the pose and the camera intrinsic to the image plane; denotes the 2D pixel coordinate of the th feature point in the image, ; denotes the 3D coordinate of the th feature point in the world coordinate system, ; denotes the camera intrinsic matrix, ; denotes the number of final matched feature pairs; denotes the pose transformation matrix of the camera with respect to the target object to be solved; denotes the Euclidean norm, which calculates the length of a vector; Convert the optimal pose transformation matrix into Lie algebra format suitable for subsequent filtering fusion: ; ； wherein, represents mapping the transformation matrix in the group to the Lie algebra space, resulting in a 6-dimensional vector representation; represents the visual observation vector; represents the visual observation Jacobian matrix; represents the feature point detection noise covariance matrix, reflecting the uncertainty of feature extraction; represents the visual observation noise covariance matrix;​ Step S3-2: based on the inertial measurement of IMU and the system dynamics model, the motion state and uncertainty of the robot at the current time are predicted, specifically: Define the state vector of the dynamic model IMU inertial measurement: ; wherein, is a position vector representing the position of the body in the world coordinate frame; is a quaternion representing the rotation of the body relative to the world coordinate frame; is a velocity vector representing the linear velocity of the body in the world coordinate frame; is a gyroscope bias representing systematic errors in angular velocity measurements; is an accelerometer bias representing systematic errors in acceleration measurements; denotes the transpose operation; Based on the IMU-based dynamics model, the motion state of the robot at the current time is predicted: ; wherein, represents the angular velocity and acceleration measurements at time is the IMU dynamics model; represents the state vector at the previous time instant, represents the angular velocity and acceleration values measured by the IMU at time t​ IMU dynamics model Discretization is performed using the trapezoidal rule, and the attitude, velocity, and position are updated sequentially by step-by-step integration as follows: Zero bias correction: Eliminate systematic errors of IMU sensors: , ; wherein, denotes the corrected angular velocity, denotes the corrected acceleration, denotes the gyroscope bias estimate of the previous time instant, denotes the accelerometer bias estimate of the previous time instant; denotes the raw acceleration value vector measured by the IMU at the time instant Median calculation: , ; wherein represents an angular velocity median value, represents an acceleration median value, represents a corrected angular velocity at the previous time, represents a corrected acceleration at the previous time; pose update: update the body rotation pose based on angular velocity integration: ; ； ； wherein, represents a rotation vector, represents a quaternion corresponding to the rotation vector, represents a quaternion multiplication operator, represents a conversion function from a rotation vector to a quaternion; Coordinate system conversion: convert the acceleration in the body coordinate system to the world coordinate system: ； ； represents a rotation matrix corresponding to the median pose; represents a quaternion of the median time instant pose, represents an acceleration in the world coordinate system; represents a gravity acceleration vector in the world coordinate system; Velocity update: Update body linear velocity based on acceleration integration: ; wherein, represents the current time velocity vector; represents the IMU integration time step; Position update: Update body position based on velocity integration: ; wherein represents the current time position vector; Zero offset prediction: estimates slow changes in sensor zero offset: , ; wherein, represents the current time gyro bias estimate, represents the current time accelerometer bias estimate; obtained ; Covariance prediction for the current time instant of the robot: ; wherein, denotes the state transition Jacobian matrix, denotes the noise Jacobian matrix, denotes the IMU noise covariance matrix; denotes the prediction of the state based on the time information to the prediction of the time covariance matrix; Convert the predicted state of the IMU to the predicted pose which is directly compared with the visual observation pose, and the predicted pose is: ； wherein, is an observation model function, the observation model function directly extracts the position component from the state vector , a quaternion pose is converted to an angular-axis representation , a position and an angular-axis pose are concatenated into a 6-dimensional vector, implementing the extraction of the pose information from the state vector; denotes a function converting a quaternion to an angular-axis representation; Step S3-3: use Kalman filtering algorithm to optimally weight the system dynamics model based on 2D-3D coordinate mapping to obtain the optimal state estimation; Calculate the difference between the visual pose observation and the IMU dynamics prediction: ； wherein is the observation residual vector at time instant is the observation matrix at time instant denotes the visual observation vector Calculate the Kalman gain matrix to determine the weight of the observation residual to the state correction: ； wherein represents the Kalman gain matrix at time represents the covariance at time represents the visual observation noise covariance matrix at time represents the transpose of the observation matrix at time represents a prediction of the covariance matrix at time based on the information at time t-1​ The predicted state is corrected by using Kalman gain and observation residual to obtain optimal state estimation: ; Update the uncertainty of the state estimation to reflect the confidence of the fused state estimation: ； wherein is based on the time information pair a prediction of the time covariance matrix; is the identity matrix; Step S3-4: convert the fused state estimation into the end pose directly used by the robot control system and perform coordinate transformation; The position and posture information is extracted from the state vector to construct a homogeneous transformation matrix : ; wherein, is a rotation matrix resulting from converting the quaternion ; is optimal position vector at the time instant represents a pose transformation matrix of the camera with respect to the target; The pose in the camera coordinate system is converted to the robot end coordinate system by matrix multiplication to obtain a standard coordinate system representation required for robot control: ; wherein, is a hand-eye calibration matrix, obtained through a calibration process; is an end pose transformation matrix.

6. The coordinated control method of the GIS robot with attitude correction according to claim 5, characterized in that: In the step S4, the obtained pose information is compared with the target pose information to calculate the error, and the error vector is obtained, specifically: Through matrix inverse operation and multiplication operation, the error transformation matrix is calculated; ； wherein is the error transform matrix, by solving the inverse operation Apply the logarithmic mapping operation in Lie group theory to map the SE(3) group element to the Lie algebra space, and convert the complex matrix differential geometry operation to the linear representation in the 6-dimensional vector space, and the expression is: ； wherein, represents a translation error; represents a rotation error.

7. The coordinated control method of the GIS installation robot with posture correction according to claim 1, characterized in that: In the step S5, the nonlinear dynamics equation of the robot is constructed, the control target is converted into an optimization problem with multiple constraints, and the optimal control command at the current time is obtained, specifically: Step S5-1: construct the nonlinear dynamics equation of the robot to predict the future state and perform discretization; Defining a state vector of the MPC controller model , ; wherein denotes the time instant pose error vector, denotes the time instant joint angle vector, denotes the time instant joint velocity vector; denotes the transpose operation; The continuous-time nonlinear dynamic equation is established as: ; where denotes the derivative of the state vector with respect to time; is the control input vector, representing joint acceleration commands; is a nonlinear function describing the dynamic behavior of the system, relating is expanded in terms of the three components of the state vector: ； wherein, are respectively the dynamic component functions of the pose error, joint position, joint velocity, is the geometric Jacobian matrix of the robot; denotes the joint velocity vector, denotes the joint angle vector, denotes the control input vector; Use the discretization method to obtain the discrete state space equation: discretized using a first order forward Euler method ; ； , the linearized discrete state space equation: ; wherein, is a discrete-time index, denotes a state vector at the kth sampling instant, is a control period, denotes a discrete state transition matrix at the kth instant; denotes a discrete control input matrix at the kth instant; denotes a discretized error and unmodeled dynamics term at the kth instant; denotes a joint velocity vector at the kth instant, denotes a control vector at the kth instant; Step S5-2: based on the optimal control theory, construct an optimization problem including the objective function and the constraint condition for the model predictive controller MPC, and convert the control target into a solvable mathematical optimization problem: The optimization problem is: ; ： ； ； ； ； wherein, represents a target function to be minimized, represents a state error penalty term penalizing the accumulation of deviation of the system state from the desired trajectory over the prediction horizon; represents a control input penalty term minimizing the energy consumed in the control process; represents a control increment penalty term ensuring the temporal smoothness of the control command and suppressing abrupt changes; represents a terminal state penalty term ensuring the stability and convergence of the system at the end of the prediction horizon; represents that the state error weight matrix is positive definite and symmetric; represents that the control energy weight matrix is positive definite and symmetric; represents that the control increment weight matrix is positive definite and symmetric; represents that the terminal state weight matrix is positive definite and symmetric; represents a control increment; represents a state vector at time instant ; represents a control vector at time instant ; represents the length of the prediction horizon; represents a dynamic constraint, represents a state constraint, represents a control constraint, represents a control increment constraint; is a lower bound vector for the control quantity, is an upper bound vector for the control quantity; represents a lower bound for the state vector, represents an upper bound for the state vector; represents a lower bound for the control increment, represents an upper bound for the control increment; Step S5-3: convert the optimization problem into a standard quadratic programming form, and solve the optimal control sequence by using the active set method; Step S5-4: based on the rolling optimization process of model prediction, the optimal control amount at the current time is generated.

8. The coordinated control method of the GIS robot with attitude correction according to claim 7, characterized in that: In the step S5-3, the optimization problem is converted into a standard quadratic programming form, and the active set method is used for solving, specifically: Step S5-3-1: convert the optimization problem into a standard quadratic programming form: ； wherein, optimization variables, , having a dimension of ; is a Hessian matrix; is a gradient vector; is a constraint matrix; is a constraint vector; Step S5-3-2: the optimization problem is solved by using the effective set method: selecting an initial feasible control sequence , an initial active constraint index set , setting a gradient convergence tolerance , a constraint violation tolerance ; solving the equality constrained subproblem: ； The optimal solution is obtained by Lagrange method and a Lagrange multiplier vector ; wherein, represents the optimal solution of the current iteration; represents the Lagrange multiplier vector; represents the row vector of the i-th constraint; represents the boundary value of the i-th constraint; represents the working set at the k-th iteration; Optimality check: if holds for all , judge the gradient of the objective function; otherwise, remove the constraint corresponding to the smallest negative multiplier; denotes the Lagrange multiplier associated with the th activity constraint; The gradient of the objective function is determined: and all non-activity constraints are satisfied then return the optimal solution ; wherein, denotes the gradient of the objective function, ; denotes the Jacobian matrix of the working set constraints, denotes the boundary value of the jth constraint not in the current working set .

9. The coordinated control method of the GIS robot with attitude correction according to claim 7, characterized in that: The step S5-4 produces the optimal control quantity at the current time based on the model prediction, specifically: The optimal control sequence obtained by optimization is converted into actual executable control instructions at the current time, and it is ensured that it meets the physical constraints of the system; With the first item control strategy, only the first control quantity in the sequence is extracted and applied to the current time: ; wherein, is the control vector to be applied; denotes the first control quantity extracted from the optimal control sequence; The control command is ensured to be within the actuator tolerance range by clipping: ; wherein, is a lower limit vector for the control quantity, is an upper limit vector for the control quantity; denotes a vector-wise maximum operation, denotes a vector-wise minimum operation; Based on the system model and the control quantity of the actual application, the state of the next moment is predicted: ; wherein, is a predicted state vector, is a state transition matrix, is a current state vector, is a control input matrix, is a model error estimation vector; Obtaining actual state measurements, calculating prediction errors: ; wherein, is a vector norm operation, is a prediction error scalar, is an actual measured state vector; The current solution result is used to accelerate the optimization calculation at the next time, and the controller parameters are dynamically adjusted according to the prediction error to balance performance and robustness, specifically: Hot start initialization, current de-translation as next time optimization initial value: ; wherein is a hot start initial vector; Parameter adaptive adjustment: dynamically adjust the optimization problem weight according to the prediction error: ; wherein, is the adjusted state error weight matrix, is the adjusted control weight matrix, is the original state error weight matrix, is the original control weight matrix, is the state weight adaptive gain coefficient, is the control weight adaptive gain coefficient.

10. The coordinated control method of the GIS installation robot with attitude correction according to claim 1, characterized in that: In the step S6, the control instruction obtained by optimization is converted into torque signal actually driving the robot motion, specifically: Step S6-1: using numerical integration method, the acceleration instruction output by MPC controller is converted into joint position and speed reference trajectory which can be directly tracked by bottom driver; The numerical integration method includes a velocity integration and a position integration, the velocity integration obtains a velocity change amount by integrating the acceleration over time, and the expression is: ; wherein, is the joint velocity vector at the current time instant; is the current joint velocity vector; denotes the current optimal joint acceleration command at the current time instant, is the control period; Position integration gives the change in position by a second integration of velocity, expressed as: ; wherein, is the current joint position vector, represents the current joint position vector; represents the displacement increment caused by the current velocity, represents the displacement increment caused by the acceleration; Step S6-2: real-time comparison of expected trajectory and actual trajectory state, using PID control algorithm to calculate joint torque and send to driver; The joint torque vector obtained by the PID control algorithm is: ; wherein, represents joint torque vector calculated at time k, represents a proportional gain matrix, represents position tracking error vector at time k, ; represents a derivative gain matrix, represents velocity tracking error vector at time k, ; represents an integral gain matrix, represents error accumulation vector at time k, , represents error accumulation at time k, represents position tracking error at time k; represents actual joint position vector at time k; represents a discrete time step index; represents actual joint velocity vector at time k, which is sent to joint actuators.

Images