Predictive control method and device for displacement oil cylinder of drill arm of drilling and anchoring robot

Abstract

The invention discloses a displacement prediction control method and equipment for a drill arm of a drilling and anchoring robot, and belongs to the field of valve-controlled hydraulic control. Establishing a drill arm friction force model of the drilling and anchoring robot and a nonlinear state space equation of a drill arm displacement system; designing a prediction controller of the displacement of the drill arm, obtaining a state variable estimated by the expansion reduced order type linear expansion state observer through the prediction controller by combining the nonlinear state-space equation and the friction force model of the displacement system of the drill arm, and solving the optimal input; and constructing an extended state observer, feeding back the state estimated by the extended state observer and lumped disturbance to a prediction model part, updating a state variable on line, and solving an optimal control law through a prediction controller. The method is simple in step and good in prediction effect, and can effectively ensure the accuracy of displacement control of the drill arm of the drilling and anchoring robot.

Description

Technical Field

[0001] The present invention belongs to the field of valve-controlled hydraulic control, and in particular relates to a method and device for predicting and controlling the displacement cylinder of a drilling arm of an anchor drilling robot. Background Art

[0002] Currently, drilling and anchoring robots are used for anchor bolting in tunnel support. These support operations must be performed according to pre-set steel strip hole positions. The accuracy of drill boom displacement control determines hole alignment, and currently, drilling operations are primarily performed manually. The drill boom displacement system utilizes an asynchronous motor to drive a metering pump, which pumps oil to a three-position, four-way electromagnetic proportional directional valve. Adjusting the proportional directional valve allows oil to enter the drill boom displacement cylinder of the drilling and anchoring robot, driving the piston in the cylinder. By varying the position of the proportional directional valve, and thereby the flow rate into the cylinder, the piston's movement direction and speed can be altered, achieving control of the drill boom's displacement. The core components of the drill boom displacement system are the cylinder and the electromagnetic proportional directional valve. Therefore, a model of the drill boom displacement system of the drilling and anchoring robot was developed by analyzing the dynamic characteristics of the electromagnetic proportional directional valve and valve-controlled drill boom displacement cylinder. Furthermore, electro-hydraulic systems exhibit nonlinearities. By designing a control algorithm, the effects of nonlinearities on displacement control are eliminated, effectively improving control accuracy. In the motion control of the drill boom of the drilling and anchoring robot, a suitable controller is required to ensure precise operation of the boom. The drilling arm of the anchor drilling robot is hydraulically driven, which is subject to nonlinear friction, uncertain disturbances and time-varying parameters. It is necessary to design a controller to ensure that the displacement of the drilling arm is accurately tracked to the given value.

[0003] Prior art publication number CN116079713A discloses a method, system, device, and medium for collaborative control of multiple drill arms of a drilling and anchoring robot, relating to the field of drilling and anchoring robot control. The method comprises: acquiring an image of a drill hole to be identified; inputting the image into a drill hole recognition model to obtain a drill hole recognition image; the drill hole recognition model is constructed based on a K-Means clustering algorithm and a Mask R-CNN network; locating the drill holes in the drill hole recognition image to obtain drill hole position information; planning the motion trajectory and workspace of each drill arm of a target drilling and anchoring robot based on the drill hole position information to obtain drill arm planning information; and collaboratively controlling each drill arm of the target drilling and anchoring robot based on the drill arm planning information. This method aims at drilling and uses image recognition to help the robot find the drilling location. It requires high computing power and image recognition. Due to the complex underground environment and poor lighting, a large number of problems that interfere with the sensor's recognition progress will inevitably occur. Therefore, this method has many recognition errors. At the same time, due to the large amount of calculation, the recognition time is also relatively long, affecting the overall construction speed. Summary of the Invention

[0004] In view of the shortcomings of the existing technology, a predictive control method and equipment for the displacement cylinder of the drill arm of a drilling and anchoring robot are provided. The position controller of the drill arm displacement cylinder of the drilling and anchoring robot is designed by combining model predictive control and extended state observer. The extended state observer is used to estimate the state of the drill arm displacement system and the lumped disturbance. The incremental model predictive control is used to solve the problem of sudden change rate of the input signal, make the input smooth, and realize precise control of the drill arm position of the drilling and anchoring robot.

[0005] To achieve the above objectives, the present invention discloses a method for predicting and controlling the displacement of a drill arm of a drilling and anchoring robot. The method is directed to a drill arm displacement system comprising an interconnected drill arm displacement cylinder, a hydraulic proportional valve, a hydraulic pump, and connecting pipelines of the drilling and anchoring robot. The steps are as follows:

[0006] Step 1: Establish a friction model of the drilling arm of the drilling and anchoring robot based on the force and motion of the drilling arm displacement system of the drilling and anchoring robot, and establish the nonlinear state space equation of the drilling arm displacement system;

[0007] Step 2: Based on the control accuracy requirements of the drill arm displacement system, the drill arm displacement is precisely controlled under a given displacement setting signal. The drill arm friction model of the drilling and anchoring robot is introduced into the drill arm displacement system. Combined with the nonlinear state space equation of the drill arm displacement system, a predictive controller for the drill arm displacement is designed.

[0008] A predictive controller is used to obtain the state variables in the drill boom displacement system estimated by an expanded reduced-order linear expanded state observer, thereby solving for the optimal input for controlling the drill boom displacement system. The predictive controller predicts the control input at the previous moment and adjusts the current control input. An incremental prediction model is used to control the input increment, thereby constraining the input change rate and achieving precise control.

[0009] The predictive controller includes a prediction model part and an objective function part. The prediction model part uses the nonlinear state space equation of the drill arm displacement system to predict the state of the drill arm displacement system in the prediction time domain. By optimizing the objective function to minimize the control output error, the control input signal sequence in the prediction time domain is solved.

[0010] Step 3: Based on the nonlinear state-space equation of the drill boom displacement system, an extended state observer is constructed. The state of the drill boom displacement system and the lumped disturbance estimated by the extended state observer are fed back to the prediction model part. The prediction model part of the predictive controller updates the state variables online, and then the optimal control law is solved through the predictive controller.

[0011] Furthermore, the friction model of the drill arm of the drilling and anchoring robot is formed by combining the Stribeck and Karnopp friction models. The nonlinear state space equation of the drill arm displacement system is established using the force balance equation, the improved model, and the flow continuity equation of the drill arm displacement system:

[0012] Assuming the displacement x1, moving speed x2, pressure x3 of the hydraulic oil in the rodless chamber, and pressure x4 of the hydraulic oil in the rod chamber of the drilling and anchoring robot arm displacement system as state variables, the nonlinear state space equation of the arm displacement system is expressed as follows:

[0013]

[0014] Where A1 is the effective area of ​​the rodless cavity of the drill arm displacement cylinder, A2 is the effective area of ​​the rod cavity of the drill arm displacement cylinder, m is the equivalent load mass of the drill arm displacement cylinder, B is the equivalent load damping of the drill arm displacement cylinder, K is the equivalent load elastic modulus of the drill arm displacement cylinder, and F is the equivalent load modulus of the drill arm displacement cylinder. f is the load friction, F is the drill arm load, C i is the leakage coefficient of the boom displacement cylinder, V1 and V2 represent the equivalent volumes of the rodless and rod-end chambers of the boom displacement cylinder, respectively, and β e is the bulk elastic modulus of the oil in the drill arm displacement cylinder, L represents the stroke of the drill arm displacement cylinder, ρ is the density of the hydraulic oil, P s is the oil supply pressure of the proportional reversing valve of the drill arm displacement cylinder, ω is the gradient coefficient of the valve core opening throttling area and valve core displacement, C d is the flow coefficient of the proportional reversing valve core opening channel, x v It is the displacement of the proportional directional valve core.

[0015] Furthermore, the drill arm displacement system predictive controller construction process is as follows:

[0016] Establish the discrete time state space equation of the drilling and anchoring robot's drill arm displacement system;

[0017] The discrete-time state equation of the drill arm displacement system is approximately linearized to obtain the linearized model of the drill arm displacement system.

[0018] A rolling optimization strategy is adopted to re-obtain future control inputs based on the current state of the drill arm displacement system at each sampling moment;

[0019] The following methods are used to reduce the error caused by linearization on the displacement system of the drilling arm of the anchor drilling robot:

[0020] 1) The sampling period of the controller is 0.001 seconds, and the state feedback of the drill arm displacement system is 0.01 seconds;

[0021] 2) In each cycle, the Jacobian matrix of the current state is calculated using the feedback data and linearized;

[0022] 3) In the optimization stage, a relaxation factor is introduced to soften the constraints and ensure that the drill arm displacement system remains stable even when linearization errors exist.

[0023] Furthermore, the nonlinear state space equation of the drill arm displacement system is expressed as follows:

[0024]

[0025] Where f(x(k)) is the state matrix of the drill arm displacement system, B(k) is the control matrix of the drill arm displacement system, G(k) is the nonlinear friction force and lumped disturbance of the drill arm displacement system, h(x(k)) is the observation matrix of the drill arm displacement system, and u(k) is the control input matrix.

[0026] Where,

[0027]

[0028] h(x(k))=[x1,0,0,0] T ;

[0029] Where k i is the proportional amplifier coefficient.

[0030] Furthermore, the linearized model of the drill arm displacement system is expressed as follows:

[0031]

[0032] Where T is the sampling period, and A(k) is the state coefficient matrix of the drill arm displacement system:

[0033]

[0034] Elements in the gradient matrix of the drill arm displacement system: A (3,1) 、A (4,1) 、A (3,3) 、A (4,4) are all elements of the matrix:

[0035]

[0036]

[0037]

[0038]

[0039] B(k) is the control matrix of the drill arm displacement system, which is expressed as follows:

[0040]

[0041] H(k) is the observation matrix of the drill arm displacement system:

[0042] H(k)=

[1000] T.

[0043] Furthermore, the prediction model of the drill arm displacement prediction controller is solved using the augmented state equation of the drill arm displacement system:

[0044]

[0045] Where x(k|k) is the state vector at the current time k, and u(k-1|k) is the input control at the previous time k-1;

[0046] The control increment and state quantity of the drill arm displacement system are combined and brought into the linearized model of the discrete-time state space equation of the drill arm displacement system to obtain the augmented state space equation:

[0047]

[0048]

[0049] Where A k,t =I+TA(k),B k,t =TB(k), T is the sampling time, is the augmented state coefficient matrix, is the augmented control coefficient matrix, is the output coefficient matrix;

[0050] Assume that the prediction time domain of the drill arm displacement prediction controller is N p , the control time domain is N c , the prediction output matrix of the prediction model for the drill arm displacement system at the future moment is expressed as:

[0051] Y(k)=M ss ξ(k|k)+S u ΔU(k)+S d Δd t

[0052] Where,

[0053]

[0054]

[0055]

[0056] Among them, Y(k) is the predicted output vector, M ss is the predicted output coefficient vector, S d is the control input coefficient matrix, Δd t is the lumped disturbance vector, S u is the lumped perturbation coefficient matrix;

[0057] In the prediction domain N P and control time domain N c The current state ξ(k|k) of the drill arm displacement system and the control increment sequence ΔU(k) ​​are used to predict the state and output quantities.

[0058] Furthermore, the objective function J of the model predictive controller is expressed as:

[0059]

[0060] Where y ref (k+i|k) represents the reference displacement setting value predicted at the k+i moment, y(k+i|k) represents the actual displacement value at the k+i moment, Q represents the control error weight, R represents the control increment weight, S represents the control amount weight, ε represents the relaxation factor, and ρ represents the relaxation factor weight coefficient;

[0061] The objective function J is composed of four terms. The first term weight coefficient Q is used to represent the N P The deviation between the predicted output and the expected output reflects the tracking ability of the model predictive controller to the displacement set value; the second weight coefficient R represents the deviation between the predicted output and the expected output in the control time domain N c The size of the internal control increment; the third item is the total control weight coefficient S, which is used to limit the size of the control input current signal; the last coefficient is the relaxation factor weight, which is used to soften the constraints of the objective function.

[0062] Furthermore, in the control of the drill arm displacement, the input signal that controls the drill arm displacement distance will be subject to corresponding upper and lower limit constraints. The upper and lower limit constraints of the drill arm displacement actuator are set as:

[0063]

[0064] Where: Δu min is the lower limit of the control input increment, Δu is the control input increment, Δu max is the upper limit of the control input increment, u min is the lower limit of the control quantity, u is the control input quantity, u max To control the upper limit of input;

[0065] By solving the objective function through the state x(k) of the drill arm displacement system at time k and the control input u(k-1) at the previous time, we can get the c The optimal control increment sequence ΔU(k) ​​in ΔU(k) ​​is selected as the actual control increment, and combined with the control input u(k-1) at the previous moment, the final input signal of the control cycle is:

[0066] u(k)=[1 0 … 0]·ΔU(k)+u(k-1)

[0067] At time k, the drill boom displacement system executes the final input signal u(k) of the control cycle, which is used as the input signal to control the drill boom displacement system. At the next moment, the drill boom displacement system re-solves a new optimal control increment sequence based on the new state quantity x(k+1) and the input signal u(k) at the previous moment.

[0068] Furthermore, according to the oil inlet pressure x3 and oil outlet pressure x4 measured by the sensor, the reduced-order linear extended state observer is expressed as:

[0069]

[0070] The reduced-order linear extended state observer is expressed in discrete form:

[0071]

[0072] The friction force is expressed as follows:

[0073]

[0074] Where, F ext is the drill arm thrust, v d is the dead zone velocity of the Karnopp model, B is the viscous resistance, F s F is the maximum static friction force of the drill arm displacement cylinder, c is the Coulomb friction force, v s is the Stribeck velocity, and α is the exponential factor.

[0075] A computer device comprises a processor and a memory, wherein the processor is electrically connected to the memory, the memory is used to store instructions and data, and the processor is used to execute the drilling and anchoring robot drill arm displacement prediction and control method according to any one of claims 1 to 9.

[0076] Beneficial effects: The present invention relates to a control method for the displacement system of the drill arm of a drilling and anchoring robot. The friction force is modeled by combining the Stribeck model and the Karnopp model, and the controller of the drill arm displacement system is designed by using model predictive control and an extended state observer. The method has the following technical advantages:

[0077] 1. Effectively suppress nonlinear friction: The Stribeck and Karnopp models are used to accurately model the friction of the drill boom displacement system. Combined with an expanded state observer and a predictive controller, the nonlinear friction effect in the drill boom displacement system is eliminated in real time.

[0078] 2. Smooth control of input signals and improved system stability: The present invention adopts an incremental model predictive control method to avoid sudden changes in the control input signal, effectively reducing the instability of the drill arm displacement system caused by drastic changes in the control input signal, ensuring the smooth movement of the drill arm and improving the control accuracy of the drill arm displacement.

[0079] 3. Dynamic state estimation and disturbance compensation: The state and uncertain disturbances of the drill boom displacement system are estimated in real time through the extended state observer, and the estimated state and disturbances are fed back to the predictive controller in real time, significantly eliminating the influence of uncertain disturbances and time-varying parameters in the drill boom displacement system.

[0080] 4. Improve operational efficiency and safety: Precise drill arm displacement control enables the drilling and anchoring robot to efficiently complete tunnel support tasks in various complex environments, reduce human intervention, improve overall operational efficiency, and reduce safety risks during operation.

[0081] 5. Ability to adapt to complex working conditions: This predictive control method can adapt to load changes, ensuring that the drilling and anchoring robot's drill arm can still maintain precise displacement control performance under various changing working environments.

[0082] In summary, the control method for the displacement system of the drilling and anchoring robot's drill arm provided by the present invention significantly improves the working accuracy of the drilling and anchoring robot's drill arm in tunnel support by modeling friction force, designing an incremental model predictive control method, and performing real-time state estimation and disturbance compensation. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] Figure 1 It is the equivalent model of the drilling and anchoring robot's drill arm displacement system used in the present invention;

[0084] Figure 2 This is a schematic diagram of the structure of the drilling and anchoring robot's drill arm displacement system used in the present invention;

[0085] Figure 3 This is a block diagram of the predictive controller structure of the drill arm displacement system according to an embodiment of the present invention;

[0086] Figure 4 is a load-speed observation curve for modeling friction force by combining the Stribeck model and the Karnopp model in an embodiment of the present invention;

[0087] Figure 5 2. It is a schematic diagram of the effect of friction force model estimation in an embodiment of the present invention;

[0088] Figure 6 This is a comparison of experimental data with and without friction compensation;

[0089] Figure 7 2 is a schematic diagram of a drill arm displacement tracking curve in an embodiment of the present invention;

[0090] Figure 8 is a control input curve of the controller controlling the hydraulic proportional valve in an embodiment of the present invention;

[0091] Figure 9 This is a performance comparison experimental data diagram using different controllers in an embodiment of the present invention;

[0092] Figure 10 This is a partially enlarged comparison data diagram of the controller performance comparison in an embodiment of the present invention.

[0093] Attachment Figure 9 and attached Figure 10 The full name of friction compensation MPC is friction compensation model predictive controller (MPC), the full name of frictionless compensation MPC is frictionless compensation model predictive controller (MPC), the full name of MPC is model predictive controller (MPC), and the full name of PID is proportional-integral-derivative (PID) controller. DETAILED DESCRIPTION

[0094] The embodiments of the present invention are further described below with reference to the accompanying drawings:

[0095] like Figure 1 and Figure 2 As shown, the control system used in the displacement prediction control method of the drilling and anchoring robot drill arm of the present invention includes: an asynchronous motor drives a quantitative pump to send hydraulic oil from the oil tank to the hydraulic proportional valve, thereby driving the drill arm displacement cylinder to achieve the drill arm displacement movement. The displacement prediction control method is as follows:

[0096] Step 1: Establish a mathematical model of the drill arm displacement system based on its working principle and characteristics;

[0097] Step 1-1: Perform force analysis on the drill arm displacement cylinder. The force balance equation is as follows:

[0098]

[0099] Where, P A is the oil pressure of the rodless chamber hydraulic oil, P B is the hydraulic oil pressure of the rod cavity, A1 is the effective area of ​​the rodless cavity, A2 is the effective area of ​​the rod cavity, x d is the load displacement of the drill arm displacement cylinder piston rod, m d represents the equivalent load mass, B represents the equivalent load damping, K represents the equivalent load elastic modulus, F fis the load friction force, and F is the drill arm load.

[0100] Steps 1-2 combine the Stribeck and Karnopp models, using the Karnopp model to set a velocity dead zone near zero velocity. When the boom displacement cylinder's velocity exceeds the dead zone, the Stribeck model is used to improve the accuracy of friction estimation. At high speeds, the drill arm friction estimate is equivalent to that of the Coulomb-viscous drag model. The established friction model is shown below:

[0101]

[0102] Where, F ext Indicates the thrust provided by the drill arm actuator, F v represents the viscous resistance, v d represents the dead zone velocity of the Karnopp model, F s Indicates the maximum static friction, F c represents the Coulomb friction force, v s represents the Stribeck velocity, and α represents the exponential factor. Under the premise of lubrication, the Coulomb friction coefficient is 0.05-0.1, and the static friction coefficient is 0.1-0.12. Both the Coulomb friction and static friction coefficients take the theoretical maximum values ​​of 0.1 and 0.12, respectively.

[0103] Steps 1-3: The boom displacement cylinder is divided into two chambers: a rod chamber and a rodless chamber. The flow continuity equation for each chamber represents the relationship between the hydraulic oil flow and pressure. When the piston rod of the boom displacement cylinder is in the extended state, the flow continuity equation is as follows:

[0104]

[0105]

[0106] Where Q A is the rodless cavity load flow, Q B is the rod cavity load flow, C i is the leakage coefficient of the drill arm displacement cylinder, V1 and V2 represent the equivalent volumes of the rodless cavity and the rod cavity respectively, β e is the bulk elastic modulus of the oil in the drill arm displacement cylinder, and L represents the stroke of the drill arm displacement cylinder.

[0107] The hydraulic proportional valve in steps 1-4 is a three-position four-way proportional reversing valve. Based on fluid mechanics, the flow coupling equation of the drill arm displacement cylinder is obtained. The flow of the rodless chamber of the drill arm displacement cylinder is:

[0108]

[0109] Where C dis the flow coefficient of the proportional reversing valve core opening channel, ω is the gradient coefficient of the valve core opening throttling area and the valve core displacement, x v is the displacement of the spool of the reversing valve, ρ is the density of the hydraulic oil, P s It is the oil supply pressure of the hydraulic station to the reversing valve.

[0110] Steps 1-5: Flow rate in the rod chamber of the drill arm displacement cylinder:

[0111]

[0112] Where, P0 is the oil return chamber pressure of the reversing valve, and the oil return chamber pressure is set to 0.

[0113] Step 1-6 Select the drilling robot's drill arm displacement and cylinder displacement x d , the first-order derivative of the drill arm displacement cylinder displacement Rodless chamber hydraulic oil pressure P a and the rod chamber hydraulic oil pressure P b As a state variable Based on equations (1) to (6), the nonlinear state space equation of the drill arm displacement system model is obtained:

[0114]

[0115] Step 2: Combined with the drill arm displacement system mathematical model established in step 1, according to the displacement control requirements, design the drill arm displacement prediction controller, the structure of which is as follows: Figure 3 shown.

[0116] Due to the compressibility of the fluid, hydraulic oil requires a certain amount of time to transmit force. Under practical conditions, sudden changes in the control signal cannot promptly drive the drill boom displacement cylinder to the specified position due to the hysteresis of the proportional valve's electromagnet, friction between the valve core and the valve body, and inertia. Therefore, it is necessary to limit the rate of change of the input signal to prevent sudden changes. This invention adopts an augmented form of model predictive control, using the input current at the previous moment as an expansion dimension to implement input increment control, thereby constraining the input rate of change.

[0117] Step 2-1 Based on the state equation of the drill arm displacement system established by equation (7), the general state equation can be obtained:

[0118]

[0119] Where f(x(k)) is the state matrix of the drill arm displacement system, B(k) is the control matrix of the drill arm displacement system, G(k) is the nonlinear friction and disturbance of the drill arm displacement system, and h(x(k)) is the observation matrix of the drill arm displacement system. f(x(k)) is the state matrix of the drill arm displacement system:

[0120]

[0121] B(k) is the control matrix of the drill arm displacement system:

[0122]

[0123] G(k) is the nonlinear friction force on the drill arm displacement system:

[0124]

[0125] h(x(k)) is the observation matrix of the drill arm displacement system:

[0126] h(x(k))=[x1,0,0,0] T (12)

[0127] Step 2-1 Based on the state equation of the drill arm displacement system established by equation (7), the general state equation can be obtained:

[0128] Step 2-2: Since the model predictive control adopts a rolling horizon strategy, the future control input is re-optimized based on the current state at each sampling moment, and the drill arm displacement control system is implemented based on a computer or microprocessor. The controller samples the signal in a discrete form and controls the input and output of the signal. Therefore, it is necessary to discretize the drill arm displacement system model to obtain the discrete time state space equation of the drill arm displacement system.

[0129]

[0130] Step 2-3 performs approximate linearization on the discrete-time state equation of the drill boom displacement cylinder. The linearized model of the discrete-time state space equation of the drill boom displacement system is expressed as follows:

[0131]

[0132] Where T is the sampling period, and A(k) is the gradient matrix of the drill arm displacement system:

[0133]

[0134] make The elements in the gradient matrix of the drill arm displacement system are as follows:

[0135]

[0136]

[0137]

[0138]

[0139] B(k) is the control matrix of the drill arm displacement system, which is expressed as follows:

[0140]

[0141] H(k) is the observation matrix of the drill arm displacement system:

[0142] H(k)=

[1000] T (twenty one)

[0143] In step 2-4, due to the nonlinear characteristics of the boom displacement system itself, such as the nonlinearity of the boom displacement cylinder volume transformation, the reversal of the proportional valve, friction, etc., the use of linearization will affect the stability of the boom displacement system. Therefore, the following method is used to reduce the linearization error:

[0144] (1) The sampling period of the controller is 0.001 seconds, and the state feedback of the drill arm displacement system is 0.01 seconds.

[0145] (2) In each cycle, the Jacobian matrix near the current state is calculated using the feedback data and linearized.

[0146] (3) In the optimization stage, a relaxation factor is introduced to soften the constraints and ensure that the drill arm displacement system remains stable when linearization errors exist.

[0147] Step 2-5: Assume the augmented state equation:

[0148]

[0149] Let A k,t =I+TA(k),B k,t =TB(k), The control increment and state quantity of the drill arm displacement system are combined and brought into the linearized model of the drill arm displacement system to obtain the following augmented state space equation:

[0150]

[0151] Where, is the augmented state coefficient matrix, is the augmented control coefficient matrix, is the output coefficient matrix.

[0152] Step 2-6 Simplify the above coefficient matrix symbols to A t =A k,t , B t =B k,t , C t =C k,t .

[0153] Assume that the prediction time domain of the model predictive controller is N p , the control time domain is N c , the output matrix of the drill arm displacement system at the future moment is:

[0154] Y(k)=M ss ξ(k|k)+S u ΔU(k)+S d Δd t (twenty four)

[0155] Where,

[0156]

[0157]

[0158]

[0159] The above is the prediction model part of the drill arm displacement controller. P and control time domain N c The current state ξ(k|t) and control increment ΔU(t) of the drill arm displacement system are used to predict the state quantity and output quantity.

[0160] Steps 2-7 require stable and accurate control of the boom displacement cylinder position. By designing and solving a suitable objective function, a control sequence with optimal performance is obtained. The objective function of the model predictive controller designed in this invention is as follows:

[0161]

[0162] Where y ref (k+i|k) represents the reference displacement setting value at the k+i moment, y(k+i|k) represents the actual displacement value at the k+i moment, Q represents the control error weight, R represents the control increment weight, S represents the control amount weight, ε represents the relaxation factor, and ρ represents the relaxation factor weight coefficient;

[0163] The objective function is divided into four items. The first item, weight coefficient Q, is used to represent N in the prediction time domain. P The deviation between the predicted output and the expected output reflects the tracking ability of the model predictive controller to the displacement set value; the second weight coefficient R represents the deviation between the predicted output and the expected output in the control time domain N c The size of the internal control increment; the third item is the total control weight coefficient S, which is used to limit the size of the control input current signal; the last coefficient is the relaxation factor weight, which is used to soften the constraints of the objective function.

[0164] The objective function of a conventional model predictive controller mainly consists of two parts, namely limiting the state tracking deviation and limiting the size of the control input, which correspond to the first and third terms of the above formula (26). The objective function adopted by the present invention adds restrictions on the control increment and the introduction of a relaxation factor. The purpose of adding restrictions on the control increment is to limit the input change rate, ensure that the input electromagnetic signal corresponds to the position and speed change of the proportional valve spool, and prevent sudden changes in the input signal. The proportional valve spool cannot adjust its position in time, resulting in a decrease in control performance. Adding a relaxation factor is used to soften the objective function solution. Due to the existence of unmodeled disturbances and parameter uncertainty in the drill arm displacement system, in the process of solving the optimization algorithm, if the constraints of the objective function are strict, it will not be able to meet all the constraint requirements, resulting in no feasible solution for the optimization calculation process. Adding a relaxation factor can convert the hard constraints of the objective function into soft constraints, ensuring that the calculation of the optimization algorithm always has a solution.

[0165] The optimal solution of the control variable in steps 2-8 can be transformed into a quadratic programming (QP) problem. The control input signal with optimal control performance is obtained by solving the minimum value of the MPC objective function. Solving the QP problem requires converting the above objective function into a standard quadratic form. The expression of the standard quadratic form is as follows:

[0166]

[0167] Where x is the vector to be solved, H is the positive semidefinite matrix describing the coefficients of the quadratic term, f is the coefficient vector of the linear term, and the x vector to be solved must simultaneously satisfy the inequality constraints, the solution size constraints, and the equality constraints.

[0168] In step 2-9, in the control of the drill arm displacement, the magnitude and rate of change of the input signal will be subject to corresponding upper and lower limits. The constraints of the drill arm displacement actuator are set as follows:

[0169]

[0170] Perform constraint transformation on the control increment:

[0171]

[0172] Convert the size of the control input to an inequality constraint:

[0173] Δ(k)=u(k)-u(k-1) (29)

[0174]

[0175] Step 2-10 solves the objective function by the state x(k) of the drill arm displacement system at time k and the control input u(k-1) at the previous time, and obtains the N in the control time domain. c The optimal control increment sequence ΔU(k) ​​in ΔU(k) ​​is selected as the actual control increment, and combined with the control input u(k-1) at the previous moment, the final input signal of the control cycle is:

[0176] u(k)=[1 0 … 0]·ΔU(k)+u(k-1) (31)

[0177] At moment k, the drill arm displacement system will execute this control quantity, which will act on the drill arm displacement system as an input signal. At the next moment, the drill arm displacement system will re-solve a new optimal control increment sequence based on the new state quantity x(k+1) and the input signal u(k) at the previous moment.

[0178] By establishing the MPC objective function, setting the corresponding control increment constraints and control quantity constraints, and converting the above equations into standard quadratic form, a QP problem solver is used to calculate the corresponding optimal control increment sequence. This design combines incremental MPC to limit the rate of change of the input signal, while also incorporating relaxation factors to soften the hard constraints of the objective function, thus addressing the problem of QP optimization problems being unsolvable due to imprecise model establishment and uncertain disturbances.

[0179] Step 3: Based on the mathematical model of the hydraulic drill arm displacement cylinder established in step 1, design the expansion state observer.

[0180] The design of a drill boom model predictive controller requires full state feedback of the drill boom displacement system, specifically the displacement and velocity of the drill boom displacement cylinder, as well as the pressure signals of the two chambers of the drill boom displacement cylinder. However, in actual engineering, velocity signals are difficult to obtain. Typically, velocity signals are obtained by differentiating the position signal, a method that places high demands on the accuracy of the position sensor. The sensor also experiences sampling noise during sampling, and using differentiation to obtain velocity signals amplifies this noise, resulting in an inaccurate velocity signal and inaccurate friction estimation, impacting the controller's effectiveness. This approach employs an extended state observer to obtain the drill boom velocity signal, which is then used to calculate and compensate for load friction.

[0181] Compared to the nonlinear extended state observer, the linear extended state observer is simpler to design, requires fewer parameters, and avoids chattering caused by state switching in nonlinear functions. Although friction is modeled, some errors that cannot be modeled still exist. Therefore, the linear extended state observer is combined with the friction model to compensate for both observed disturbances and friction.

[0182] Step 3-1: Since conventional speed detection methods have errors, an observer is used to estimate the movement speed of the drill arm and calculate the friction force based on the observed speed. The linear expansion state observer is designed as follows:

[0183]

[0184] Where, β i (i=1,2,3,4,5) represents the gain coefficient, Represents an estimate of friction.

[0185] Step 3-2 is measurable for the position and the states of the two chambers of the boom displacement cylinder, namely x1, x3, and x4. If it is close enough to x1, the error e1 is small and cannot effectively estimate other state quantities. Usually, the gain coefficient β is increased. i To improve the accuracy of the estimation. Since the position of the drill arm displacement system and the two-chamber states x1, x3, and x4 can be measured, in order to reduce the gain coefficient of the extended state observer, the friction force estimation and the lumped disturbance are used as extended state terms. The designed linear extended state observer is as follows:

[0186]

[0187] Step 3-3 Since the model predictive control uses discrete form to output the control signal to control the proportional valve, the above linear extended state observer is in discrete form:

[0188]

[0189] An estimate of friction is calculated as follows:

[0190]

[0191] Where, F ext is the drill arm thrust, v d is the dead zone velocity of the Karnopp model, B is the viscous resistance, F s F is the maximum static friction force of the drill arm displacement cylinder, c is the Coulomb friction force, v s is the Stribeck velocity, and α is the exponential factor.

[0192] Step 4: Use Matlab2022a and AMESim2022 to realize the joint simulation of the drill arm displacement system of the drilling and anchoring robot to verify the effect and effectiveness of the designed controller on the drill arm displacement cylinder control and friction force estimation under different working conditions.

[0193] Example

[0194] A controller was built in Simulink in Matlab2022a, and a model predictive controller was built using S-function. The extended state observer and friction estimation function were also built through S-function. A drill arm displacement system model was built in AMESim, and the data transmission of the drill arm displacement cylinder position, speed, and two-chamber pressure was realized through the Simulink_cosim interface, and the control input signal of the model predictive control in Matlab2022a was returned.

[0195] The boom displacement system uses a metering pump to provide flow. The load simulates the force applied to the boom and introduces nonlinear friction. When the pressure provided by the metering pump exceeds the set supply pressure, the excess hydraulic oil is discharged to the tank through a relief valve, reducing the flow and pressure. The boom displacement cylinder is controlled by a proportional reversing valve. In AMESim, the proportional solenoid adjusts the valve core displacement by current. The boom displacement cylinder uses a symmetrical cylinder, meaning that the two cylinders have the same effective area. A pressure sensor is installed in the pipeline connecting the boom displacement cylinder to obtain the pressure values ​​of the two chambers. The displacement and velocity of the load are measured, and the velocity signal is used for comparison with the expansion state observer signal. The relevant parameters and values ​​of the boom displacement system are shown in Table 1.

[0196] Table 1 Parameters and values ​​of the drill arm displacement cylinder

[0197]

[0198]

[0199] Considering the impact of friction on controller performance, a friction estimation model combining the Stribeck model and the state transition model is established. This model requires the acquisition of a speed signal, which is subject to noise interference. Therefore, an extended state observer is used to observe the speed and unmodeled disturbances.

[0200] Through the step signal analysis results, the observation results of the speed signal are as follows Figure 4 As shown, the friction force estimation results are as follows Figure 5 As shown, the friction force estimation is based on the result from 5 seconds ago. Figure 3 The results of friction estimation using the Stribeck model under the same parameters are shown. Because the friction estimation is coupled with the velocity signal, the accuracy of the friction estimation also affects the extended state observer's estimation of the velocity signal. It can be seen that the observer's observation of the velocity signal is accurate at higher speeds, so the extended state observer still has the ability to observe accurately. However, the Stribeck model cannot solve the zero-crossing problem of the velocity signal at low speeds. Because the estimated friction force direction constantly changes, the estimated velocity value also oscillates around zero, forming a sawtooth pattern. Figure 4 It can be concluded that the extended state observer has a good observation effect on the speed signal, solves the problem of the discrete system speed signal crossing zero, and also has a good observation effect under the low-speed movement of the drill arm displacement cylinder. On this basis, the designed model is used to estimate the friction force. Figure 5 The curve shown can reflect that the designed friction model can effectively estimate the magnitude of friction, and it also describes the Stribeck effect well, which is consistent with the change of friction under load.

[0201] In order to verify the effectiveness of friction feedforward compensation on performance improvement, the effects of the model predictive controller and the proposed controller are compared. The parameters used are as follows: prediction step size Np = 10, control step size N c =5, the cost function parameters Q = 3e10, R = 4.5e5, S = 1000, ρ = 1000, the extended state observer parameters ω = 0.001, β1 = 50ω, β2 = 13000ω 2 , β3=1e10ω 3 The results are as above Figure 6 As shown in the figure, in the low-speed range, when the distance between the boom displacement cylinder and the target position is small, the controller with friction compensation can provide a larger input signal to the boom displacement cylinder, thereby increasing the output force of the boom displacement cylinder and achieving a shorter adjustment time. In contrast, the controller without friction compensation suffers from a slower adjustment speed due to the increased friction, which causes the boom displacement cylinder output force to fail to keep up with the change in friction. In summary, the MPC controller with friction compensation exhibits better control performance.

[0202] like Figure 7 As shown in the figure, when the drill arm displacement system gives the expected displacement, the drill arm can quickly reach the specified position, and the drill arm displacement cylinder can be extended and retracted. In addition, the steady-state error is small after the drill arm displacement system reaches the desired position, meeting the requirements of the drill arm motion control. The current output by the controller is as follows Figure 8 As shown in the figure, the model predictive control adopts an incremental design. Since the sampling step of the model predictive controller is short, a smaller input signal change is set to prevent the input signal from jumping. The input current changes smoothly, and there is no sudden change in the input control signal. In addition, the control accuracy is high, which can achieve precise position control of the drill arm displacement cylinder.

[0203] In order to verify the control effect of the designed controller compared with other controls, under the condition of friction, the PID control parameters are selected: Kp = 20, K i =0.05, K d =0.1. Model predictive controller parameters: Np=10, N c =5, Q=1.7·10 6, R = 0.1, ρ = 1000. The parameters of the incremental model predictive controller are as follows: Np = 10, N c =5, Q=3·10 10 , R=4.5·10 5 , S = 1000, ρ = 1000. Designed controller parameters: ω = 0.001, β1 = 50ω, β2 = 130000ω 2 , β3=1·10 10 ω 3 , and the rest of the parameters are consistent with the incremental model predictive controller. The step signal is used as the reference signal, and the results are compared through AMESim2022 and Matlab2022a / simulink joint simulation. Figure 9 The following figure shows a comparison of the control effects of the designed controller and other controllers. The controller designed in the figure below is a friction compensation MPC. Under the condition of friction, the MPC controller based on the extended state observer designed by the present invention has better performance. The results of MPC and incremental MPC are similar. Except for the friction compensation MPC, the other controllers show a decrease in control performance when the position signal is close to the reference signal. Figure 10 A local comparison found that the other controllers had longer adjustment times and sudden position changes. The reasons were analyzed as follows: when the speed of the controller without friction compensation decreases, the friction force on the load gradually increases due to the Stribeck effect, but the input of the controller decreases as the error between the position signal and the reference signal decreases. Therefore, the load displacement gradually decreases until it stops. At this time, since the controller still has a position error, the input current signal gradually increases. When the force provided by the drill arm displacement cylinder is greater than the static friction force of the load, the drill arm displacement cylinder will continue to move to the target position. The designed controller can effectively overcome the influence of friction disturbances, thereby obtaining better control performance and smaller steady-state error, and realizing precise control of the position of the drill arm displacement cylinder.

[0204] The controller designed in this invention produces a smooth, non-mutational output current signal. Using an extended state observer, it accurately estimates the friction and Stribeck phenomenon acting on the load, obtaining an accurate velocity signal. Friction is then incorporated into model predictive control to achieve friction feedforward compensation. The designed controller was compared with three other controllers to analyze the impact of nonlinear friction on boom displacement control. Comparative experiments demonstrated that the proposed controller effectively compensates for the friction acting on the drilling and anchoring robot's boom, eliminating the impact of friction disturbances on boom displacement control and enabling the robot to accurately and quickly reach the designated position.

Claims

1. A method for predicting and controlling the displacement of a drilling arm of an anchor drilling robot, wherein the drilling arm displacement system includes an interconnected drilling arm displacement cylinder, a hydraulic proportional valve, a hydraulic pump, and connecting pipelines; characterized in that: Here are the steps: Step 1: Establish a friction model of the drill arm of the drilling and anchoring robot based on the force and motion of the drill arm displacement system of the drilling and anchoring robot, and establish the nonlinear state space equation of the drill arm displacement system; Step 2: Based on the control accuracy requirements of the drill arm displacement system, the drill arm displacement is precisely controlled under a given displacement setting signal. The drill arm friction model of the drilling and anchoring robot is introduced into the drill arm displacement system. Combined with the nonlinear state space equation of the drill arm displacement system, a predictive controller for the drill arm displacement is designed. A predictive controller is used to obtain the state variables in the drill boom displacement system estimated by an expanded reduced-order linear expanded state observer, thereby solving for the optimal input for controlling the drill boom displacement system. The predictive controller predicts the control input at the previous moment and adjusts the current control input. An incremental prediction model is used to control the input increment, thereby constraining the input change rate and achieving precise control. The predictive controller includes a prediction model part and an objective function part. The prediction model part uses the nonlinear state space equation of the drill arm displacement system to predict the state of the drill arm displacement system in the prediction time domain. By optimizing the objective function to minimize the control output error, the control input signal sequence in the prediction time domain is solved. Step 3: Based on the nonlinear state-space equation of the drill boom displacement system, an extended state observer is constructed. The state of the drill boom displacement system and the lumped disturbance estimated by the extended state observer are fed back to the prediction model part. The prediction model part of the predictive controller updates the state variables online, and then the optimal control law is solved through the predictive controller.

2. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 1, characterized in that: The friction model of the drill arm of the drilling and anchoring robot is formed by combining the Stribeck and Karnopp friction models. The nonlinear state space equation of the drill arm displacement system is established using the force balance equation, the improved model, and the flow continuity equation of the drill arm displacement system: Assuming the displacement x1, moving speed x2, pressure x3 of the hydraulic oil in the rodless chamber, and pressure x4 of the hydraulic oil in the rod chamber of the drilling and anchoring robot arm displacement system as state variables, the nonlinear state space equation of the arm displacement system is expressed as follows: Where A1 is the effective area of ​​the rodless cavity of the drill arm displacement cylinder, A2 is the effective area of ​​the rod cavity of the drill arm displacement cylinder, m is the equivalent load mass of the drill arm displacement cylinder, B is the equivalent load damping of the drill arm displacement cylinder, K is the equivalent load elastic modulus of the drill arm displacement cylinder, and F is the equivalent load modulus of the drill arm displacement cylinder. f is the load friction, F is the drill arm load, C i is the leakage coefficient of the boom displacement cylinder, V1 and V2 represent the equivalent volumes of the rodless and rod-end chambers of the boom displacement cylinder, respectively, and β e is the bulk elastic modulus of the oil in the drill arm displacement cylinder, L represents the stroke of the drill arm displacement cylinder, ρ is the density of the hydraulic oil, P s is the oil supply pressure of the proportional reversing valve of the drill arm displacement cylinder, ω is the gradient coefficient of the valve core opening throttling area and valve core displacement, C d is the flow coefficient of the proportional reversing valve core opening channel, x v It is the displacement of the proportional directional valve core.

3. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 2, characterized in that: The construction process of the predictive controller for the drill arm displacement system is as follows: Establish the discrete time state space equation of the drilling and anchoring robot's drill arm displacement system; The discrete-time state equation of the drill arm displacement system is approximately linearized to obtain the linearized model of the drill arm displacement system. A rolling optimization strategy is adopted to re-obtain future control inputs based on the current state of the drill arm displacement system at each sampling moment; The following methods are used to reduce the error caused by linearization on the displacement system of the drilling arm of the anchor drilling robot: 1) The sampling period of the controller is 0.001 seconds, and the state feedback of the drill arm displacement system is 0.01 seconds; 2) In each cycle, the Jacobian matrix of the current state is calculated using the feedback data and linearized; 3) In the optimization stage, a relaxation factor is introduced to soften the constraints and ensure that the drill arm displacement system remains stable even when linearization errors exist.

4. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 3, characterized in that: The nonlinear state space equation of the drill arm displacement system is expressed as follows: Where f(x(k)) is the state matrix of the drill arm displacement system, B(k) is the control matrix of the drill arm displacement system, G(k) is the nonlinear friction force and lumped disturbance of the drill arm displacement system, h(x(k)) is the observation matrix of the drill arm displacement system, and u(k) is the control input matrix. Where, h(x(k))=[x1,0,0,0] T ; Where k i is the proportional amplifier coefficient.

5. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 4, characterized in that: The linearized model of the drill arm displacement system is expressed as follows: Where T is the sampling period, and A(k) is the state coefficient matrix of the drill arm displacement system: Elements in the gradient matrix of the drill arm displacement system: A (3,1) 、A (4,1) 、A (3,3) 、A (4,4) are all elements of the matrix: B(k) is the control matrix of the drill arm displacement system, which is expressed as follows: H(k) is the observation matrix of the drill arm displacement system: H(k)=[1000] T 。 6. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 5, characterized in that: The prediction model of the drill arm displacement prediction controller is solved using the augmented state equation of the drill arm displacement system: Where x(k|k) is the state vector at the current time k, and u(k-1|k) is the input control at the previous time k-1; The control increment and state quantity of the drill arm displacement system are combined and brought into the linearized model of the discrete-time state space equation of the drill arm displacement system to obtain the augmented state space equation: Where A k,t =I+TA(k),B k,t =TB(k), T is the sampling time, is the augmented state coefficient matrix, is the augmented control coefficient matrix, is the output coefficient matrix; Assume that the prediction time domain of the drill arm displacement prediction controller is N p , the control time domain is N c , the prediction output matrix of the prediction model for the drill arm displacement system at the future moment is expressed as: Y(k)=M ss ξ(k|k)+S u ΔU(k)+S d Δd t Where, Among them, Y(k) is the predicted output vector, M ss is the predicted output coefficient vector, S d is the control input coefficient matrix, Δd t is the lumped disturbance vector, S u is the lumped perturbation coefficient matrix; In the prediction domain N P and control time domain N c The current state ξ(k|k) of the drill arm displacement system and the control increment sequence ΔU(k) ​​are used to predict the state and output quantities.

7. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 6, characterized in that: The objective function J of the model predictive controller is expressed as: Where y ref (k+i|k) represents the reference displacement setting value predicted at the k+i moment, y(k+i|k) represents the actual displacement value at the k+i moment, Q represents the control error weight, R represents the control increment weight, S represents the control amount weight, ε represents the relaxation factor, and ρ represents the relaxation factor weight coefficient; The objective function J is composed of four terms. The first term weight coefficient Q is used to represent the N P The deviation between the predicted output and the expected output reflects the tracking ability of the model predictive controller to the displacement set value; the second weight coefficient R represents the deviation between the predicted output and the expected output in the control time domain N c The size of the internal control increment; the third item is the total control weight coefficient S, which is used to limit the size of the control input current signal; the last coefficient is the relaxation factor weight, which is used to soften the constraints of the objective function.

8. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 7, characterized in that: In the control of the drill arm displacement, the input signal that controls the drill arm displacement distance will be subject to corresponding upper and lower limit constraints. The upper and lower limit constraints of the drill arm displacement actuator are set as: Where: Δu min is the lower limit of the control input increment, Δu is the control input increment, Δu max is the upper limit of the control input increment, u min is the lower limit of the control quantity, u is the control input quantity, u max To control the upper limit of input; By solving the objective function through the state x(k) of the drill arm displacement system at time k and the control input u(k-1) at the previous time, we can get the c The optimal control increment sequence ΔU(k) ​​in ΔU(k) ​​is selected as the actual control increment, and combined with the control input u(k-1) at the previous moment, the final input signal of the control cycle is: u(k)=[1 0 … 0]·ΔU(k)+u(k-1) At time k, the drill boom displacement system executes the final input signal u(k) of the control cycle, which is used as the input signal to control the drill boom displacement system. At the next moment, the drill boom displacement system re-solves a new optimal control increment sequence based on the new state quantity x(k+1) and the input signal u(k) at the previous moment.

9. The method for predicting and controlling the displacement of the drilling arm of the anchor drilling robot according to claim 8, characterized in that: According to the oil inlet pressure x3 and oil outlet pressure x4 measured by the sensor, the reduced-order linear extended state observer is expressed as: The reduced-order linear extended state observer is expressed in discrete form: The friction force is expressed as follows: Where, F ext is the drill arm thrust, v d is the dead zone velocity of the Karnopp model, B is the viscous resistance, F s F is the maximum static friction force of the drill arm displacement cylinder, c is the Coulomb friction force, v s is the Stribeck velocity, and α is the exponential factor.

10. A computer device, characterized in that: The invention comprises a processor and a memory, wherein the processor is electrically connected to the memory, the memory is used to store instructions and data, and the processor is used to execute the drilling and anchoring robot drill arm displacement prediction control method according to any one of claims 1 to 9.

Images