MPGA-EKF-based state parameter estimation method for electro-hydraulic proportional system of guniting manipulator

Abstract

The invention provides a state parameter estimation method for an electro-hydraulic proportional system of a guniting manipulator based on MPGA-EKF, and the method comprises the steps: building a nonlinear mathematical model of the electro-hydraulic proportional control system through deducing a data model of each element of the manipulator according to a theoretical block diagram of the electro-hydraulic proportional control system of the manipulator; performing linearization processing on the nonlinear system model of the electro-hydraulic proportional system; optimizing a noise matrix of the extended Kalman filtering algorithm by using a multi-population genetic algorithm; sensing data on the manipulator are collected, speed estimation is carried out, and a covariance matrix corresponding to speed estimation is obtained; and fusing the speed estimation and the covariance matrix by adopting an optimized extended Kalman filtering algorithm to obtain pose data and manipulator state parameter estimation. Based on the state estimation capability of the extended Kalman filter and the global search capability of the multi-population genetic algorithm, the precision, adaptability, stability and reliability of state estimation of the guniting manipulator system are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent follow-up vehicles, and specifically to a method for estimating state parameters of the electro-hydraulic proportional system of a shotcreting machine based on MPGA-EKF. Background Art

[0002] With the continuous increase in the depth of coal mine mining, the conditions of roadways are becoming increasingly complex, and the requirements for roadway support are also constantly improving. As a result, a new support method has emerged, which is also the main support method at present, namely shotcreting support. During the shotcreting support process, due to manual operation, problems such as harm to workers' health, low construction efficiency, and uneven construction quality have emerged. The research and application of shotcreting manipulators have received increasing attention.

[0003] The joints of the shotcreting manipulator are driven by a hydraulic system, and the movement of the hydraulic working device is controlled by an electro-hydraulic proportional valve to achieve automatic spraying on the working surface, which can greatly reduce manual participation. The general mathematical model of the electro-hydraulic proportional system can be derived from each link, but it is difficult to obtain the precise parameters in the model. Only by determining the specific parameters of the model can the electro-hydraulic proportional control system be optimized and controlled to improve its control accuracy and dynamic performance. With the development of parameter identification technology, the Kalman filter (KF) algorithm has good effects when dealing with linear systems. However, in the actual electro-hydraulic proportional control system, there are large nonlinear problems during the operation of the system. The Extended Kalman Filter (EKF) algorithm is widely used to solve the filtering problems of nonlinear systems, but the performance of EKF depends to a large extent on the selection of the noise covariance matrices Q and R.

[0004] Secondly, for the shotcreting manipulator to achieve automatic continuous shotcreting work, its own positioning problem is a very important issue. Due to reasons such as the environment, sensors, actuators, models, and calculations, there are great uncertainties in the motion control of the shotcreting robot. For the shotcreting robot, the most important thing is to solve problems such as how to identify the surrounding environmental conditions, how to move to the target position, and how to avoid obstacles. The key to these problems lies in how to achieve precise positioning of the robot in the environment. The existing related positioning technologies mainly include the robot autonomous positioning technology based on the inertial navigation system, the robot autonomous positioning technology based on machine vision, the robot positioning technology based on lidar, and the robot autonomous positioning technology based on ultra-wideband. However, the above methods have the following deficiencies:

[0005] (1) For the robot autonomous positioning technology based on the inertial navigation system, this method uses an inertial measurement unit to measure the acceleration of the robot, and then obtains the position information through integration. However, errors such as gyroscope deviation in the inertial measurement unit will accumulate over time, and the positioning accuracy will decrease over time, making it impossible to perform long-term precise positioning of the robot.

[0006] (2) Robot autonomous positioning technology based on machine vision. This method uses a camera to obtain the image information of the robot, thereby obtaining the position information. However, in underground coal mines, factors such as high temperature, high humidity, high dust, and low environmental illumination seriously affect the operation of the camera, resulting in incorrect positioning information and low practicality.

[0007] (3) Robot positioning technology based on lidar. This method uses the trilateration principle to determine the position of the robot. However, it is greatly affected by the environment, affecting the positioning accuracy, and the system complexity is relatively high, the cost is expensive, and it is not suitable for robot positioning in complex mine environments.

[0008] (4) Robot autonomous positioning based on ultra-wideband. This method uses the distance information between ultra-wideband mobile nodes and anchors. However, this method assumes that the robot is in a constant-speed motion state, resulting in delayed positioning and low accuracy.

[0009] Finally, the method for determining the posture of the robot is an important research content. The accuracy of posture determination will affect the working performance and safety of the robot. Therefore, there is an urgent need to propose a more accurate method for robot positioning and state estimation. Summary of the Invention

[0010] The technical problem to be solved by the present invention is to provide a method for estimating the state parameters of the electro-hydraulic proportional system of a shotcreting machine based on MPGA-EKF to solve the problems that the optimization of the linear control processing algorithm of the existing operation robot in the background technology is not perfect enough, and the robot positioning and state estimation are not accurate enough.

[0011] To solve the above technical problems, the embodiments of the present invention provide the following technical solutions: A method for estimating the state parameters of the electro-hydraulic proportional system of a shotcreting machine based on MPGA-EKF, including the following steps:

[0012] S1. According to the theoretical block diagram of the electro-hydraulic proportional control system of the manipulator, by deriving the data models of each component of the manipulator, establish the nonlinear mathematical model of the electro-hydraulic proportional control system;

[0013] S2. Linearize the nonlinear system model of the electro-hydraulic proportional system to obtain the Jacobian matrix of the system, and design an extended Kalman filter algorithm, including state prediction, measurement update, prediction and update of the covariance matrix;

[0014] S3. Use the multi-population genetic algorithm to optimize the noise matrix of the extended Kalman filter algorithm, design a suitable objective function, optimize the noise covariance matrices Q and R in the extended Kalman filter algorithm, and terminate the iteration when the fitness function reaches the minimum value or the algorithm reaches the maximum number of iterations;

[0015] S4. Collect the perception data of the sensors set on the robotic arm, which are not limited to wheel encoders, inertial measurement units, GNSS positioning units, and laser scanning units, and perform speed estimation to obtain the covariance matrix corresponding to the speed estimation.

[0016] S5. Use the optimized extended Kalman filter algorithm in step S3 to fuse the speed estimations and covariance matrices of the sensors, which are not limited to wheel encoders, inertial measurement units, and laser scanning units, to obtain pose data; the specific pose data are as follows:

[0017] Time increment: Δt = current_t - last_t

[0018] Displacement increment in the X direction: Δx = (vx.cosθ - vy.sinθ).Δt

[0019] Displacement increment in the Y direction: Δy = (vx.sinθ - vy.cosθ).Δt

[0020] Attitude angle increment: Δθ = vθ.Δt

[0021] X coordinate: x+ = Δt

[0022] Y coordinate: y+ = Δy

[0023] Attitude angle: θ+ = Δθ

[0024] Where, vx represents the linear velocity in the X direction, vy represents the linear velocity in the Y direction, vθ represents the angular velocity, and θ represents the attitude angle of the robotic arm.

[0025] S6. According to the pose data, use the extended Kalman filter algorithm to fuse and process the pose data to complete the estimation of the robotic arm state parameters.

[0026] Furthermore, the step S1 of establishing the non - linear mathematical model of the electro - hydraulic proportional control system is specifically as follows:

[0027] The original input signal is an analog voltage signal U r After the action of the electronic amplifier, the input signal is converted into a current signal I, which drives the electromagnetic proportional iron in the electro - hydraulic proportional valve. The electro - hydraulic proportional valve is the core control element of the electro - hydraulic proportional control system. Its main part consists of a proportional electromagnet and a valve body. Its input is the current signal I, and its output is the flow rate Q of the hydraulic oil. The valve - controlled hydraulic cylinder is controlled by the electro - hydraulic proportional valve. The electro - hydraulic proportional valve adjusts its opening size according to the input electrical signal I, and then controls the oil flow rate Q into the hydraulic cylinder to achieve precise control of position, speed, and force. The magnetostrictive sensor is a measurement feedback element. The input of this link is the displacement Y of the piston rod, and the output is the feedback voltage U f;According to the derivation of the mathematical models of the above-mentioned links, the open-loop transfer function of the electro-hydraulic proportional control system of the roadway shotcreting machine is obtained as follows:

[0028]

[0029] Among them, K q is the flow gain of the valve; K i is the proportional amplification coefficient; K sv is the gain of the electro-hydraulic proportional valve; K f is the sensor feedback coefficient; ω h is the natural frequency of the hydraulic cylinder; ζ h is the hydraulic damping ratio of the hydraulic cylinder;

[0030] Since K sv , K q , K i , A, K f , ζ h , ω h are all unknown parameters, formula (1) is converted to:

[0031]

[0032] Formula (2) is used as the simplified open-loop transfer function.

[0033] Furthermore, the nonlinear system model of the electro-hydraulic proportional system is linearized to obtain the Jacobian matrix of the system, specifically:

[0034] Define the state variables for the simplified open-loop transfer function as follows:

[0035] x1 = y

[0036]

[0037] Among them, y is the displacement of the piston rod; x2 is the first derivative of x1; x3 is the second derivative of x1;

[0038] Converting formula (2) into a differential equation can obtain:

[0039]

[0040] Among them, u(t) is the input voltage signal, and y is the displacement output of the piston rod;

[0041] Rewrite the differential equation into the Jacobian matrix form of the state equation:

[0042]

[0043] Furthermore, the designed extended Kalman filter algorithm includes prediction and update steps, specifically as follows:

[0044] Among them, the prediction step is divided into state prediction and covariance prediction:

[0045]

[0046] In the formula, is the state vector at time k, which at least includes the position (x, y, z), attitude (roll, pitch, yaw), velocity (vx, vy, vz) of the manipulator, and the IMU bias; u k is the control input, w k is the process noise, A k is the Jacobian matrix of the state prediction function f(·), P k is the covariance matrix of the state vector, Q k represents the covariance matrix of the Gaussian noise of the predicted state;

[0047] The update step is divided into Kalman gain calculation, state update and covariance update:

[0048]

[0049] P′ k = P k - K g H k P k (9)

[0050] In the formula, K g is the Kalman gain, H k is the Jacobian matrix of the measurement function h(·), z k is the state vector of the sensor measurement value, R k is the covariance matrix of the Gaussian noise of the measurement value.

[0051] Furthermore, step S3 uses a multi-population genetic algorithm to optimize the noise matrix of the extended Kalman filter algorithm, including selection operation, crossover operation, and mutation operation, specifically as follows:

[0052] First, the dimensions of the process noise covariance matrix Q and the measurement noise covariance matrix R of the extended Kalman filter algorithm are respectively set to 3×3 and 1×1, that is:

[0053]

[0054] Then, perform the selection operation to select individuals with high fitness from the current population to generate the next generation, which is achieved by calculating the fitness function value of each individual:

[0055] Fitness function: J = trace(P) or J = log(det(P))

[0056] Selection probability:

[0057] where P is the error covariance matrix and N is the population size;

[0058] Crossover operation, which generates new offspring by combining the characteristics of two parent individuals. The crossover operation formula is as follows:

[0059] Q c = αQ1 + (1 - α)Q2 (11)

[0060] R c = βR1 + (1 - β)R2 (12)

[0061] where Q c and R c are the new generation matrices after crossover, and α and β are crossover coefficients, whose value ranges are [0, 1].

[0062] Mutation operation is achieved by adding a small random matrix. The mutation operation formula is as follows:

[0063] Q m = Q + γΔQ (13)

[0064] R m = R + δΔR (14)

[0065] where ΔQ and ΔR are random matrices with the same dimension as Q and R, and γ and δ are small parameters controlling the degree of mutation.

[0066] Furthermore, the roulette wheel method is adopted during selection. The sum of the fitness probabilities of all individuals is set to 1. Each individual corresponds to a fitness probability. After roulette wheel selection, individuals with large probabilities are retained, and individuals with small probabilities are screened out, ultimately enabling the population to evolve towards the optimal individual.

[0067] Furthermore, the probability calculation formulas for crossover and mutation of each population are as follows:

[0068] P c = P co + c·f rand (15)

[0069] P m = P mo + m·f rand (16)

[0070] where P co is the initial value of the crossover probability, Pmo is the initial value of the mutation probability, c is the value range of the crossover probability, m is the value range of the mutation probability, and f rand function is used to generate random numbers, and P c is the finally determined crossover probability for each population, and P m is the finally determined mutation probability for each population.

[0071] Furthermore, the design of a suitable objective function is specifically as follows:

[0072]

[0073] Among them, y i is the actual observed value, is the observed value estimated by the EKF, and N is the number of observed values;

[0074] After determining the objective function, select the identification evaluation index. The evaluation index selects the goodness of fit R 2 , and its value range is [0, 1]. The expression of the goodness of fit is:

[0075]

[0076] In the formula, y t is the actual value, is the identification output value, is the average value;

[0077] Finally, evaluate the fitness of the new generation of population. If the stop condition is met, stop the algorithm; otherwise, re - perform optimization processes such as selection, crossover, and mutation on the population members, and select the individual with the highest fitness in the final population as the optimal Q and R matrices to act on the EKF. Among them, the stop condition is to reach the maximum number of iterations or the improvement of the goodness of fit is less than a certain threshold.

[0078] Furthermore, the use of the extended Kalman filter algorithm to fuse the pose data is specifically as follows:

[0079] Obtain the system state quantity, and calculate the error state prediction value and the system state noise covariance matrix;

[0080] From the calculated observation matrix and noise covariance matrix, calculate the system gain matrix;

[0081] From the gain matrix, update the system state quantity and the system covariance matrix;

[0082] Calculate the quaternion expression form from the system state quantity, generate a quaternion vector, and perform normalization processing on the quaternion vector;

[0083] Convert the quaternion vector into Euler angles convenient for pose display to represent the pose state of the manipulator.

[0084] Furthermore, it also includes calculating the integer ambiguity using the quaternion vector, specifically:

[0085] x N = argmin(C(x N )) (19)

[0086]

[0087] In Formulas (1) and (2), x N represents the optimal solution vector of the integer ambiguity, represents the estimation of the integer ambiguity, C(x N ) represents the set of all possible values of the integer ambiguity, P NN represents the covariance matrix of the error between the optimal solution and the estimation of the integer ambiguity, x q (x N ) represents the quaternion vector corresponding to xN, represents the corresponding quaternion vector, p q(N)q(N) represents the quaternion covariance matrix..

[0088] The beneficial effects of the above technical solutions of the present invention are as follows:

[0089] 1. Aiming at the problem of parameter identification of the shotcreting manipulator, through the global search ability of the multi-population genetic algorithm, the Q and R matrix parameters that can optimize the EKF performance can be found more accurately. The optimized Q and R matrices can better describe the process noise and measurement noise of the system, thereby reducing the influence of external interference and sensor noise on the system parameter identification. Based on the state estimation ability of the extended Kalman filter and the global search ability of the multi-population genetic algorithm, it brings improved accuracy, enhanced adaptability, and improved stability and reliability to the parameter identification of the shotcreting manipulator system, which is of great significance for improving the overall performance and working efficiency of the shotcreting manipulator system.

[0090] 2. The present invention collects the perception data of the wheel encoder, inertial measurement unit, GNSS positioning unit, and laser scanning unit, calculates the respective speed estimations and covariance matrices (i.e., uncertainties) for the manipulator based on each independent perception source, and uses the optimized extended Kalman filter algorithm to fuse the speed estimations and covariance matrices to obtain the pose data and state parameter estimations of the manipulator. At the same time, by solving the integer ambiguity, the influence brought by the uncertain factors in the GNSS raw data is reduced, and the accuracy of the GNSS output result is further improved, that is, the positioning accuracy of the manipulator motion control is improved. Description of the Drawings

[0091] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0092] Figure 1 This is the structural schematic diagram of the present invention based on MPGA-EKF parameter identification;

[0093] Figure 2 This is the theoretical block diagram of the electro-hydraulic proportional control system of the present invention. Specific embodiments

[0094] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0095] The present invention proposes a method for estimating the state parameters of the electro-hydraulic proportional system of a shotcreting machine based on MPGA-EKF, including the following steps:

[0096] S1. According to the theoretical block diagram of the electro-hydraulic proportional control system of the manipulator, by deriving the data models of each component of the manipulator, a non-linear mathematical model of the electro-hydraulic proportional control system is established;

[0097] S2. Linearize the non-linear system model of the electro-hydraulic proportional system to obtain the Jacobian matrix of the system, and design an extended Kalman filter algorithm, including state prediction, measurement update, prediction and update of the covariance matrix;

[0098] S3. Use the multi-population genetic algorithm to optimize the noise matrix of the extended Kalman filter algorithm, design a suitable objective function, optimize the noise covariance matrices Q and R in the extended Kalman filter algorithm, and terminate the iteration when the fitness function reaches the minimum value or the algorithm reaches the maximum number of iterations;

[0099] S4. Collect the perception data set on the manipulator, which is not limited to wheel encoders, inertial measurement units, GNSS positioning units, and laser scanning units, and perform speed estimation to obtain the covariance matrix corresponding to the speed estimation;

[0100] S5. Use the extended Kalman filter algorithm optimized in step S3 to fuse the speed estimations and covariance matrices of the wheel encoder, inertial measurement unit, and laser scanning unit, which are not limited to, to obtain pose data; The pose data is specifically:

[0101] Time increment: Δt = current_t - last_t

[0102] Displacement increment in the X direction: Δx = (vx.cosθ - vy.sinθ).Δt

[0103] Displacement increment in the Y direction: Δy = (vx.sinθ - vy.cosθ).Δt

[0104] Attitude angle increment: Δθ = vθ.Δt

[0105] X coordinate: x+ = Δt

[0106] Y coordinate: y+ = Δy

[0107] Attitude angle: θ+ = Δθ

[0108] Where, vx represents the linear velocity in the X direction, vy represents the linear velocity in the Y direction, vθ represents the angular velocity, and θ represents the attitude angle of the manipulator;

[0109] S6. According to the pose data, use the extended Kalman filter algorithm to fuse and process the pose data to complete the estimation of the manipulator state parameters.

[0110] Among them, in step S1, a non-linear mathematical model of the electro-hydraulic proportional control system is established, specifically:

[0111] The original input signal is the analog voltage signal U r After the action of the electronic amplifier, the input signal is converted into the current signal I, and this signal drives the electromagnetic proportional iron in the electro-hydraulic proportional valve. The electro-hydraulic proportional valve is the core control element of the electro-hydraulic proportional control system. Its main part consists of a proportional electromagnet and a valve body. Its input is the current signal I, and its output is the flow rate Q of the hydraulic oil. The valve-controlled hydraulic cylinder is controlled by the electro-hydraulic proportional valve. The electro-hydraulic proportional valve adjusts its opening size according to the input electrical signal I, and then controls the oil flow rate Q entering the hydraulic cylinder to achieve precise control of position, speed and force. The magnetostrictive sensor is a measurement feedback element. The input of this link is the displacement Y of the piston rod, and the output is the feedback voltage U f ; According to the derivation of the mathematical models of the above-mentioned links, the open-loop transfer function of the electro-hydraulic proportional control system of the roadway shotcreting machine is obtained as:

[0112]

[0113] Where, K q is the flow gain of the valve; K i is the proportional amplification coefficient; K sv is the gain of the electro-hydraulic proportional valve; K f is the sensor feedback coefficient; ω his the natural frequency of the hydraulic cylinder; ζ h is the hydraulic damping ratio of the hydraulic cylinder;

[0114] Since K sv 、K q 、K i 、A、K f 、ζ h 、ω h are all unknown parameters, convert formula (1) to:

[0115]

[0116] Formula (2) is used as the simplified open-loop transfer function.

[0117] In step S2, linearize the nonlinear system model of the electro-hydraulic proportional system to obtain the Jacobian matrix of the system, specifically:

[0118] Define the state variables for the simplified open-loop transfer function as follows:

[0119] x1 = y

[0120]

[0121] where y is the displacement of the piston rod; x2 is the first derivative of x1; x3 is the second derivative of x1;

[0122] Converting formula (2) to a differential equation gives:

[0123]

[0124] where u(t) is the input voltage signal, and y is the displacement output of the piston rod;

[0125] Rewrite the differential equation in the form of the Jacobian matrix of the state equation:

[0126]

[0127] In step S2, design an extended Kalman filter algorithm, including prediction and update steps, specifically:

[0128] where the prediction step is divided into state prediction and covariance prediction:

[0129]

[0130] In the formula, is the state vector at time k, which includes at least the position (x, y, z), attitude (roll, pitch, yaw), velocity (vx, vy, vz) of the manipulator, and the IMU bias; u k is the control input, wk is the process noise, A k is the Jacobian matrix of the state prediction function, f(·), P k is the covariance matrix of the state vector, Q k represents the covariance matrix of the Gaussian noise of the predicted state;

[0131] The update steps are divided into Kalman gain calculation, state update, and covariance update:

[0132]

[0133] P′ k = P k - K g H k P k (9)

[0134] In the formula, K g is the Kalman gain, H k is the measurement function, the Jacobian matrix of h(·), z k is the state vector of the sensor measurement value, R k is the covariance matrix of the Gaussian noise of the measurement value.

[0135] In the process of Kalman filtering, the process noise covariance matrix Q describes the uncertainty of the system dynamic model, and the measurement noise covariance matrix R characterizes the noise level in the measurement process. Too low or too high Q and R will both lead to deviations in state estimation. Therefore, the present invention introduces a multi-population genetic algorithm to optimize the matrices Q and R in the extended Kalman filtering (EKF) process, improving the accuracy of state estimation and the stability of the filter. Optimizing the noise matrices of the extended Kalman filtering algorithm using the multi-population genetic algorithm includes selection operation, crossover operation, and mutation operation, specifically:

[0136] First, set the dimensions of the process noise covariance matrix Q and the measurement noise covariance matrix R of the extended Kalman filtering algorithm to 3×3 and 1×1 respectively, that is:

[0137]

[0138] Then, perform the selection operation to select individuals with high fitness from the current population to generate the next generation, which is achieved by calculating the fitness function values of each individual:

[0139] In a genetic algorithm, fitness is the main indicator to describe the performance of an individual. According to the fitness value, individuals are selected for survival of the fittest. Fitness is the driving force for the genetic algorithm. Biologically speaking, fitness is equivalent to the survival ability of organisms in "survival of the fittest in the struggle for existence", which is of great significance in the genetic process. By establishing a mapping relationship between the objective function of the optimization problem and the fitness of an individual, the optimization of the objective function of the optimization problem can be achieved during the population evolution process. The fitness function, also known as the evaluation function, is a criterion for distinguishing good and bad individuals in the population determined according to the objective function. It is always non - negative, and in any case, it is hoped that its value is as large as possible. In the selection operation, there are two problems that can lead to deception in the genetic algorithm:

[0140] 1) In the initial stage of the genetic algorithm, some super - normal individuals usually appear. According to the proportional selection method, these super - normal individuals will control the selection process due to their outstanding competitiveness, affecting the global optimization performance of the algorithm;

[0141] 2) In the later stage of the genetic algorithm, when the algorithm tends to converge, due to the small difference in individual fitness in the population, the potential for further optimization decreases, and a local optimal solution may be obtained. Therefore, if the fitness function is not properly selected, the above - mentioned deception problems will occur. It can be seen that the selection of the fitness function is of great significance for the genetic algorithm.

[0142] The selection of the fitness function directly affects the convergence speed of the genetic algorithm and whether the optimal solution can be found. Because the genetic algorithm basically does not use external information in the evolutionary search and only relies on the fitness function, and uses the fitness of each individual in the population for search. Since the complexity of the fitness function is the main component of the complexity of the genetic algorithm, the design of the fitness function should be as simple as possible to minimize the computational time complexity. The fitness function selected in this invention is: J = trace(P) or J = log(det(P)).

[0143] The selection probability in the genetic algorithm refers to the probability of selecting an individual from the current population to enter the next generation. The role of the selection probability is to determine which individuals have a greater chance of being selected for crossover and mutation operations according to the fitness of the individuals, so as to optimize the fitness of the population. Individuals with high fitness have a greater selection probability, while individuals with low fitness have a greater probability of being eliminated. The calculation method of the selection probability

[0144] Objective function: That is, the function to be solved, the fitness function.

[0145] Domain upper and lower limits: Used to determine the interval length for encoding.

[0146] Population size (popSize): That is, the size of the population.

[0147] Selection probability (selPrb): Determine how many individuals to retain each time according to the selection probability.

[0148] Crossover probability (croPrb): Determine whether to perform crossover between two individuals according to the crossover probability.

[0149] Mutation probability (varPrb): Determine whether an individual needs to mutate according to the mutation probability.

[0150] Number of generations (generNum): Determine the number of loops according to the number of generations.

[0151] Accuracy: Encode according to the accuracy and the interval length.

[0152] The selection probability formula of the present invention is: where P is the error covariance matrix and N is the population size.

[0153] When selecting, the roulette method is adopted. The sum of the fitness probabilities of all individuals is set to 1. Each individual corresponds to a fitness probability. After roulette selection, individuals with high probabilities are retained, and individuals with low probabilities are screened out, ultimately enabling the population to evolve towards the optimal individual.

[0154] For the crossover operation, new offspring are generated by combining the characteristics of two parent individuals. The crossover operation formula is as follows:

[0155] Q c = αQ1 + (1 - α)Q2 (11)

[0156] R c = βR1 + (1 - β)R2 (12)

[0157] where Q c and R c are the new generation matrices after crossover, α and β are crossover coefficients, and their value ranges are [0, 1].

[0158] For the mutation operation, it is achieved by adding a small random matrix. The mutation operation formula is as follows:

[0159] Q m = Q + γΔQ (13)

[0160] R m = R + δΔR (14)

[0161] where ΔQ and ΔR are random matrices with the same dimensions as Q and R, and γ and δ are small parameters controlling the degree of mutation.

[0162] The advantage of the multi-population genetic algorithm compared with the classical genetic algorithm is the introduction of the immigration operator. The immigration strategy is to replace the worst individuals of one population with the best individuals of another population. During the process of each population's self-iterative optimization, information exchange between populations is also achieved.

[0163] The calculation formulas for the crossover and mutation probabilities of each of the above populations are as follows:

[0164] P c = P co + c·f rand (15)

[0165] P m = P mo + m·f rand (16)

[0166] Where, P co is the initial value of the crossover probability, P mo is the initial value of the mutation probability, c is the value range of the crossover probability, m is the value range of the mutation probability, f rand function is used to generate random numbers, P c is the finally determined crossover probability for each population, P m is the finally determined mutation probability for each population.

[0167] Based on the established multi-population genetic algorithm model above, and based on the output acquisition situation of the electro-hydraulic proportional control system, design an appropriate objective function, specifically:

[0168]

[0169] Where, y i is the actual observed value, is the observed value estimated by EKF, and N is the number of observed values;

[0170] After determining the objective function, select the identification evaluation index. The evaluation index selects the goodness of fit R 2 , and the value range is [0, 1]. The expression of the goodness of fit is:

[0171]

[0172] In the formula, y t is the actual value, is the identification output value, is the average value;

[0173] Finally, evaluate the fitness of the new generation group. If the stopping condition is met, stop the algorithm; otherwise, reselect, crossover, mutate, and perform other optimization processes on the population members, and select the individual with the highest fitness in the final group as the optimal Q and R matrices to be applied to the EKF. Here, the stopping condition is reaching the maximum number of iterations or the improvement in goodness of fit being less than a certain threshold, thereby improving the state estimation ability of the EKF.

[0174] After the above steps are completed, the optimized extended Kalman filter algorithm can be used to estimate the state parameters of the manipulator. First, collect the sensing data set on the manipulator, which is not limited to wheel encoders, inertial measurement units, GNSS positioning units, and laser scanning units, and perform speed estimation to obtain the covariance matrix corresponding to the speed estimation; then, fuse the speed estimations and covariance matrices of the wheel encoders, inertial measurement units, and laser scanning units (not limited to) to obtain pose data. The pose data specifically includes:

[0175] Time increment: Δt = current_t - last_t

[0176] Displacement increment in the X direction: Δx = (vx.cosθ - vy.sinθ).Δt

[0177] Displacement increment in the Y direction: Δy = (vx.sinθ - vy.cosθ).Δt

[0178] Attitude angle increment: Δθ = vθ.Δt

[0179] X coordinate: x+ = Δt

[0180] Y coordinate: y+ = Δy

[0181] Attitude angle: θ+ = Δθ

[0182] Among them, vx represents the linear velocity in the X direction, vy represents the linear velocity in the Y direction, vθ represents the angular velocity, and θ represents the attitude angle of the manipulator;

[0183] Finally, based on the pose data, use the extended Kalman filter algorithm to fuse the pose data and complete the estimation of the manipulator state parameters.

[0184] Using the extended Kalman filter algorithm to fuse the pose data specifically includes:

[0185] Obtain the system state variables, calculate the predicted value of the error state and the system state noise covariance matrix;

[0186] From the calculated observation matrix and noise covariance matrix, calculate the gain matrix of the system;

[0187] From the gain matrix, update the system state variables and the system covariance matrix;

[0188] Calculate the expression form of the quaternion from the system state variables, generate a quaternion vector, and normalize the quaternion vector;

[0189] Convert the quaternion vector into Euler angles convenient for pose display to represent the pose state of the manipulator.

[0190] The present invention also calculates the integer ambiguity using the quaternion vector, specifically:

[0191] x N = argmin(C(x N )) (19)

[0192]

[0193] In Formulas (1) and (2), x N represents the optimal solution vector of the integer ambiguity, represents the estimation of the integer ambiguity, C(x N ) represents the set of all possible values of the integer ambiguity, P NN represents the covariance matrix of the error between the optimal solution and the estimation of the integer ambiguity, x q (x N ) represents the quaternion vector corresponding to xN, represents corresponding quaternion vector, p q(N)q(N) represents the quaternion covariance matrix.

[0194] To reduce the computational amount, the lower and upper bounds of C(x N ) can be determined in advance and given by empirical values. The integer ambiguity search first uses the LAMBDA method to find the optimal integer vector in the original search space, selects those vectors that meet the upper and lower bound requirements, evaluates the corresponding cost function values, and selects the vector that minimizes the cost function as the optimal solution of the integer ambiguity.

[0195] In summary, for the problem of parameter identification of the shotcreting manipulator, through the global search ability of the multi-population genetic algorithm, the Q and R matrix parameters that can optimize the EKF performance can be found more accurately. The optimized Q and R matrices can better describe the process noise and measurement noise of the system, thereby reducing the influence of external interference and sensor noise on the system parameter identification. The state estimation ability based on the extended Kalman filter and the global search ability of the multi-population genetic algorithm bring improved accuracy, enhanced adaptability, improved stability and reliability to the parameter identification of the shotcreting manipulator system, which is of great significance for improving the overall performance and working efficiency of the shotcreting manipulator system.

[0196] Meanwhile, the present invention collects the perception data of the wheel encoder, the inertial measurement unit, the GNSS positioning unit, and the laser scanning unit, calculates the respective speed estimations and covariance matrices (i.e., uncertainties) for the manipulator based on each independent perception source, and fuses the speed estimations and covariance matrices by using the optimized extended Kalman filtering algorithm to obtain the pose data and state parameter estimations of the manipulator. At the same time, by solving the integer ambiguity, the influence brought by the uncertain factors in the GNSS raw data is reduced, and the accuracy of the GNSS output result is further improved, that is, the positioning accuracy of the manipulator motion control is improved.

[0197] The above are the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle described in the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A method for estimating state parameters of the electro-hydraulic proportional system of a shotcreting machine based on MPGA-EKF, characterized in that It includes the following steps: S1. According to the theoretical block diagram of the mechanical hand electro-hydraulic proportional control system, by deriving the data models of each component of the manipulator, establish the non-linear mathematical model of the electro-hydraulic proportional control system; S2. Linearize the non-linear system model of the electro-hydraulic proportional system to obtain the Jacobian matrix of the system, and design the extended Kalman filter algorithm, including state prediction, measurement update, prediction and update of the covariance matrix; S3. Use the multi-population genetic algorithm to optimize the noise matrix of the extended Kalman filter algorithm, design a suitable objective function, optimize the noise covariance matrices Q and R in the extended Kalman filter algorithm, and terminate the iteration when the fitness function reaches the minimum value or the algorithm reaches the maximum number of iterations; S4. Collect the perception data set on the manipulator, which is not limited to wheel encoders, inertial measurement units, GNSS positioning units and laser scanning units, and perform speed estimation to obtain the covariance matrix corresponding to the speed estimation; S5. Use the optimized extended Kalman filter algorithm in step S3 to fuse the speed estimations and covariance matrices of components not limited to wheel encoders, inertial measurement units and laser scanning units to obtain pose data; The pose data is specifically: Time increment: Δt = current_t - last_t Displacement increment in the X direction: Δx = (vx.cosθ - vy.sinθ).Δt Displacement increment in the Y direction: Δy = (vx.sinθ - vy.cosθ).Δt Attitude angle increment: Δθ = vθ.Δt X coordinate: x+ = Δt Y coordinate: y+ = Δy Attitude angle: θ+ = Δθ Where, vx represents the linear velocity in the X direction, vy represents the linear velocity in the Y direction, vθ represents the angular velocity, and θ represents the attitude angle of the manipulator; S6. According to the pose data, use the extended Kalman filter algorithm to fuse and process the pose data to complete the estimation of the manipulator state parameters.

2. The state parameter estimation method for the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 1, characterized in that The step S1 of establishing the non-linear mathematical model of the electro-hydraulic proportional control system is specifically: The original input signal is an analog voltage signal U r After the action of the electronic amplifier, the input signal is converted into a current signal I, which drives the electromagnetic proportional iron in the electro-hydraulic proportional valve. The electro-hydraulic proportional valve is the core control element of the electro-hydraulic proportional control system. Its main part consists of a proportional electromagnet and a valve body. Its input is the current signal I, and its output is the flow rate Q of the hydraulic oil. The valve-controlled hydraulic cylinder is controlled by the electro-hydraulic proportional valve. The electro-hydraulic proportional valve adjusts its opening size according to the input electrical signal I, thereby controlling the oil flow rate Q entering the hydraulic cylinder to achieve precise control of position, speed, and force. The magnetostrictive sensor is a measurement feedback element. The input of this link is the displacement Y of the piston rod, and the output is the feedback voltage U f ; According to the derivation of the mathematical models of the above-mentioned links, the open-loop transfer function of the control system of the electro-hydraulic proportional control system of the roadway shotcreting machine is obtained as follows: Among them, K q is the flow gain of the valve; K i is the proportional amplification factor; K sv is the gain of the electro-hydraulic proportional valve; K f is the sensor feedback coefficient; ω h is the natural frequency of the hydraulic cylinder; ζ h is the hydraulic damping ratio of the hydraulic cylinder; Since K sv , K q , K i , A, K f , ζ h , ω h are all unknown parameters, convert formula (1) to: Formula (2) is used as the simplified open-loop transfer function.

3. The state parameter estimation method of the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 2, wherein, The linearization process of the non-linear system model of the electro-hydraulic proportional system to obtain the Jacobian matrix of the system is specifically: Define the state variables for the simplified open-loop transfer function as follows: x1 = y Where, y is the displacement of the piston rod; x2 is the first derivative of x1; x3 is the second derivative of x1; The differential equation can be obtained by converting formula (2): Where, u(t) is the input voltage signal, and y is the displacement output of the piston rod; Rewrite the differential equation into the Jacobian matrix form of the state equation:

4. The state parameter estimation method of the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 1, wherein The design of the extended Kalman filter algorithm includes prediction and update steps, specifically: Among them, the prediction step is divided into state prediction and covariance prediction: wherein, is the state vector at time k, including at least the position (x, y, z), attitude (roll, pitch, yaw), velocity (vx, vy, vz) of the manipulator, and the IMU bias; u k is the control input, w k is the process noise, A k is the state prediction function, the Jacobian matrix of f(·), P k is the covariance matrix of the state vector, Q k represents the covariance matrix of the Gaussian noise of the predicted state; The update step is divided into Kalman gain calculation, state update and covariance update: P′ k = P k - K g H k P k (9) where K g is the Kalman gain, H k is the Jacobian matrix of the measurement function h(·), z k is the state vector of the sensor measurement, and R k is the covariance matrix of the Gaussian noise of the measurement value.

5. The state parameter estimation method for the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 1, characterized in that, The step S3 uses the multi-population genetic algorithm to optimize the noise matrix of the extended Kalman filter algorithm, including selection operation, crossover operation and mutation operation, specifically: First, set the dimensions of the process noise covariance matrix Q and the measurement noise covariance matrix R of the extended Kalman filter algorithm to 3×3 and 1×1 respectively, that is: Then, a selection operation is performed to select individuals with high fitness from the current population to generate the next generation, which is achieved by calculating the fitness function value of each individual: Fitness function: J = trace(P) or J = log(det(P)) Selection probability: where P is the error covariance matrix and N is the population size; Crossover operation, which generates new offspring by combining the characteristics of two parent individuals. The crossover operation formula is as follows: Q c = αQ1 + (1 - α)Q2 (11) R c = βR1 + (1 - β)R2 (12) Among them, Q c and R c are the new generation matrices after crossover, and α and β are crossover coefficients, whose value ranges are [0, 1]. Mutation operation, which is achieved by adding a small random matrix. The mutation operation formula is as follows: Q m = Q + γΔQ (13) R m = R + δΔR (14) where ΔQ and ΔR are random matrices with the same dimensions as Q and R, and γ and δ are small parameters that control the degree of mutation.

6. The state parameter estimation method of the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 5, characterized in that, The roulette wheel method is used for the selection. The sum of the fitness probabilities of all individuals is set to 1. Each individual corresponds to a fitness probability. After roulette wheel selection, individuals with large probabilities are retained, and individuals with small probabilities are screened out, ultimately enabling the population to evolve towards the optimal individual.

7. The state parameter estimation method of the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 5, characterized in that, The probability calculation formulas for crossover and mutation of each population are as follows: P c = P co + c·f rand (15) P m = P mo + m·f rand (16) Among them, P co is the initial value of the crossover probability, P mo is the initial value of the mutation probability, c is the value range of the crossover probability, m is the value range of the mutation probability, and f rand function is used to generate random numbers, P c is the finally determined crossover probability for each population, and P m is the finally determined mutation probability for each population.

8. The state parameter estimation method for the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 1, characterized in that, Design a suitable objective function, specifically: where y i is the actual observed value, is the observed value estimated by EKF, and N is the number of observed values; After determining the objective function, select the identification evaluation index. The evaluation index is the goodness of fit R 2 , with a value range of [0, 1]. The expression for the goodness of fit is: where y t is the actual value, is the identified output value, is the average value; Finally, evaluate the fitness of the new generation population. If the stopping condition is met, stop the algorithm; otherwise, re-perform optimization processes such as selection, crossover, and mutation on the population members. Select the individual with the highest fitness in the final population as the optimal Q and R matrices to be applied to the EKF. The stopping condition is reaching the maximum number of iterations or the improvement in goodness of fit being less than a certain threshold.

9. The state parameter estimation method of the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 1, characterized in that, The extended Kalman filter algorithm is used to perform fusion processing on the pose data, specifically: Obtain the system state quantity, and calculate the error state prediction value and the system state noise covariance matrix; Calculate the observation matrix and the noise covariance matrix from the calculated results, and calculate the system gain matrix; Update the system state quantity and the system covariance matrix from the gain matrix; Calculate the quaternion representation form from the system state quantity, generate a quaternion vector, and perform normalization processing on the quaternion vector; Convert the quaternion vector into Euler angles convenient for pose display to represent the pose state of the manipulator.

10. The state parameter estimation method of the electro-hydraulic proportional system of the shotcreting machine based on MPGA-EKF according to claim 9, characterized in that, It also includes calculating the integer ambiguity using the quaternion vector, specifically: x N = argmin(C(x N )) (19) In Formulas (1) and (2), x N represents the optimal solution vector of the integer ambiguity, represents the estimation of the integer ambiguity, C(x N ) represents the set of all possible values of the integer ambiguity, P NN represents the covariance matrix of the error between the optimal solution and the estimation of the integer ambiguity, x q (x N ) represents the quaternion vector corresponding to xN, represents corresponding quaternion vector, p q(N)q(N) represents the quaternion covariance matrix.

Images