阅读说明：本技术 一种杆状作物收割机器人控制方法 (Control method of rod-shaped crop harvesting robot ) 是由 陈刚 李超 李涛 王卓麟 张筱婕 于 2020-08-17 设计创作，主要内容包括：本发明涉及一种杆状作物收割机器人控制方法,属于农业自动化领域。包括杆状作物的自主识别、机器人控制方法、设计夹持机构、设计捆缚机构、设计运送机构和多电机检测故障诊断。设计了一种机器人的整体硬件结构,在保持成本低廉的前提前下,解决了杆状作物的收割问题,通过对算法的优化和多传感器的融合使系统更稳定且精准。设计了一种卷积神经网络对目标进行识别,并使用简单的识别算法进行识别以提高实时性,并对两种算法得到的数据进行数据融合,得到一致性解释。本发明使用一种对超声波数据进行优化的算法,在低成本的超声波上通过算法优化得到更为准确的数据。并使用一种自适应加权融合估计算法,优化数据得到对环境的一致性解释。(The invention relates to a control method of a rod-shaped crop harvesting robot, belonging to the field of agricultural automation. The method comprises the steps of autonomous identification of the rod-shaped crops, a robot control method, design of a clamping mechanism, design of a binding mechanism, design of a conveying mechanism and multi-motor detection fault diagnosis. The whole hardware structure of the robot is designed, the harvesting problem of the rod-shaped crops is solved in advance before the cost is kept low, and the system is more stable and accurate through the optimization of an algorithm and the fusion of multiple sensors. A convolutional neural network is designed to identify a target, a simple identification algorithm is used for identifying so as to improve the real-time performance, and data obtained by the two algorithms are subjected to data fusion to obtain a consistency explanation. The invention uses an algorithm for optimizing ultrasonic data, and obtains more accurate data through algorithm optimization on the ultrasonic with low cost. And a self-adaptive weighting fusion estimation algorithm is used, and the data is optimized to obtain the consistency explanation of the environment.)

1. A control method of a rod-shaped crop harvesting robot is characterized in that: the method comprises the steps of autonomous identification of the rod-shaped crops, a robot control method, design of a clamping mechanism, design of a binding mechanism, design of a conveying mechanism and multi-motor detection fault diagnosis.

2. The rod crop harvesting robot control method of claim 1, wherein: the autonomous identification of the crop stalks is in three parts:

a first part:

(1) acquiring an image of a target rod-shaped crop, constructing a data set of the rod-shaped crop, and carrying out primary processing and labeling on the data set according to the following steps of 3: 1: 1, preparing a training set, a verification set and a test set;

(2) selecting a neural network comprising 10 convolutional layers and 5 full-connection layers, and performing feature extraction with gradually-increased levels on the target through convolutional operation; the method of transfer learning is adopted, the last layer of the convolutional neural network is removed, the last layer of the model is retrained on different data sets, in popular terms, the last layer of the model is retrained to identify different high-grade characteristics, and the training time for different rod-shaped crops is reduced;

(3) using a simulated annealing algorithm to adjust the learning rate, so that the learning rate is higher at the beginning of training, and the learning rate is reduced along with the time so as to ensure that the model can find the optimal value;

a second part:

designing a simple recognition algorithm to make up the defect that the deep convolutional neural network recognition algorithm cannot realize real-time detection under the condition of limited cost; the influence of sunlight is reduced by adopting a color block segmentation method of self-adaptive illumination;

and a third part:

(1) a filtering processing algorithm for ultrasonic ranging:

a. continuously collecting 8 points;

b. recording the number m of points which can be successfully collected;

c. if m is greater than 2, sequencing m points by using a bubbling method, otherwise, continuously executing circulation if the data has misadjustment a;

d. removing the difference between every two adjacent pairs of values, and recording a group of positions with the maximum difference value;

e. if the maximum group is positioned in the first half part, the next step is carried out, otherwise, the data has misadjustment a and continues to execute circulation;

f. taking the value of the latter half of the position as an effective value;

g. taking the average value of the effective values as a final value;

h. ending or returning to a for continuing circulation;

(2) an algorithm for multi-sensor fusion:

adopting a distributed multi-sensor fusion system structure, bringing the sensor data into the system one by one for gradual fusion to obtain a final result; fusing the two data by adopting a multi-sensor fusion algorithm; then, regarding the fused data as one kind of data, and continuing to add other data for fusion until a final result is output;

the self-adaptive weighting fusion estimation algorithm: on the premise of minimum total mean square error, dynamically distributing corresponding optimal weighting factors W to each sensor according to data measured by each sensor in real time_i(i ═ 1, 2.. n), where the larger the variance of the sensor, the smaller the weight assigned to it, to find the final estimate

the total mean square error sigma is derived from the above equation²Comprises the following steps:

according to the theory of extreme value of multivariate function, the total mean square error sigma is obtained²The minimum condition is that the weight corresponding to each sensor is W_i(i ═ 1,2,. n), the smaller the variance, the greater its corresponding weight; minimum total mean square error sigma²Comprises the following steps:

weighting factor W of each sensor corresponding thereto_iComprises the following steps:

corresponding weight W of a plurality of sensors by a formula (1.5)_iThe calculation of (2) is substituted into the formula (1.7), and the final fusion result is obtained

setting a confidence upper limit eta, and taking the value of a confidence coefficient as:

when in use_it1, fusion results

3. The rod crop harvesting robot control method of claim 1, wherein: the robot control method comprises the following steps:

a state feedback optimization controller design, the controller designed is a general controller, not only aiming at static targets, but also suitable for dynamic targets;

the dynamic model of the robot is:

observing the state and the position of a target through a perception system of the robot, and designing a state observer as follows:

the local error is

e_i＝x_i-ω_i1(2.3)

Using the equations (2.1) and (2.3), the local tracking error e is obtained_iIs a dynamic model of

The performance function is of

The control targets are as follows: for the robot, design controller u_iThe following steps are performed:

1) local tracking error e_iThe dynamic model (2.4) is asymptotically stable;

2) performance function V_i(x_i,ω_i1,u_i) (2.5) reaching a minimum value;

defined by local tracking error e_iAnd a state to be tracked omega_i1Constituent augmented system states

The dynamic model of the augmentation system obtained by the formulas (2.2) and (2.4) is

Wherein

Based on the augmented system dynamic model (2.7), the performance function (2.5) is written as

Wherein

The equation (2.9) is derived over time t to give the following Bellman equation

Wherein, let V_i ^*Is the optimal solution of Bellman equation (2.11), the optimality requirement is satisfied

Substituting equation (2.12) into Bellman equation (2.11) yields the Hamilton Jacobi Bellman, i.e., HJB equation, for the following trace

If there is a continuous semi-positive solution V of formula (2.13)_i ^*Then the optimum controller obtained from equation (2.12)Converging the performance function (2.9) to a minimum value V_i ^*(X_i(0) ); if there is a continuous semi-positive solution V of formula (2.13)_i ^*Then the optimum controller obtained from equation (2.12)The local tracking error system (2.4) is asymptotically stabilized;

the HJB equation is:

initialization: given a feasible controller

Step 1: policy evaluation, for a givenBy the following formula to obtain V_i ^j：

Step 2: strategy improvement, updating the controller using the following formula:

and step 3: order toReturning to the step 1 until V_i ^jThe convergence to the minimum value is made to be,

controller obtained by equation (2.15)

4. The rod crop harvesting robot control method of claim 1, wherein: the design fixture is as follows:

a) the front half part of the clamping mechanism is made of soft materials, and the tail end of the clamping mechanism is of a spherical structure;

b) the rear half part of the clamping mechanism is made of hard materials, and a rubber pad is pasted inside the clamping mechanism to prevent the mechanism from damaging the rod-shaped crops;

c) the joint of the front half part and the rear half part and the bending part of the rear half part are both rotatable shafts; both of them are stressed shafts, i.e. they rotate only under a certain force, the latter force being greater than the former;

d) the entire clamping mechanism is designed to be adjustable up and down to accommodate harvesting of rod-like crops of different heights.

5. The rod crop harvesting robot control method of claim 1, wherein: the design binding mechanism is as follows:

a) two auxiliary rotating wheels are respectively arranged on two sides, the right side is a No. 1 rotating wheel which is a driving wheel and is provided with a driving rotating mechanism for tightening the binding rope, the left side is a driven wheel which can not rotate actively and is provided with an emergency stop mechanism which can stop the rotation of the driven wheel;

b) an auxiliary binding structure is arranged on the clamping mechanism, and the binding mechanism is tightened outwards under the assistance of the front half part of the clamping mechanism;

c) the heater is arranged at the tightening position of the clamping mechanism, namely the position where the binding rope is wound on the clamping mechanism, and the binding rope is heated and fused after being tightened.

6. The rod crop harvesting robot control method of claim 1, wherein: the multi-motor detection fault diagnosis is as follows:

1) failure analysis of an encoder

The method avoids faults caused by serious faults or complete failures of the encoder, and further improves the robustness and safety of the system;

2) multi-motor detection fault diagnosis system strategy

Based on a principal component analysis method, utilizing the correlation between an encoder signal and a speed signal in a numerical control system, projecting a data covariance matrix reflecting the correlation to the direction with the maximum change, and monitoring the working condition of an encoder through the length of the projection;

3) multi-motor detection fault diagnosis algorithm based on principal component analysis method and reliability judgment

There are m motors in the system that need to be diagnosed, first collecting information, including encoder signals and speedThe signals are processed in real time in order to represent the correlation, and the position signals are processed in a forward differential mode and are expressed as q epsilon R^mAnd w ∈ R^mAssuming that the diagnostic process is started after n times of data are collected, and taking n > 2m according to actual experience; obtaining a data matrix F e R^2mn：

Wherein q is_iAnd w_iN is the ith sampled value; by using covariance matrix in probability theory (M is equal to R)^2m) Representing the correlation of two columns of signals, which is a symmetrical positive definite matrix;

the matrix M with positive definite symmetry of 2M × 2M has the characteristic value of λ₁≥λ₂≥...λ_2m，a_iTo correspond to lambda_iThe feature vector of (2); the maximum change direction of the F matrix is the characteristic vector a_iThe direction of (a); let L be the unit vector a in the maximum projection direction_iA straight line is formed by stretching, and each rotating shaft is provided with a vector v which is obtained by measuring or difference value on the rotating shaft of the auxiliary rotating wheel 1 in the binding mechanism, the rotating shaft driving the robot crawler to rotate and the rotating shaft driving the cutting mechanism to rotate<F>Operation of<F>Expectation for each column of the data matrix F; the projection of the covariance matrix onto this subspace is defined as

wherein the content of the first and second substances,representing data v to u_iProjection on' stretched subspace, operator<·>Represents the expectation of data, Σ_vv＝<vv^T>F represents a covariance matrix of the data matrix, namely the maximum value of the dispersion degree is obtained;

projecting the expected v of the measured data matrix along the eigenvector direction corresponding to the maximum eigenvalue of M, and solving a norm through the projection result to obtain the maximum variation; the variable quantity corresponds to a principal element in a principal element analysis method and is used for representing the correlation among signals in each row; so that a first criterion for a fault determination is obtained;

secondly, introducing a relation between the detection value and a theoretical optimal value, and evaluating whether the detection value is a fault by using the credibility as a second standard; the principal component analysis method is to take the coded signal and the speed signal as information basis and use the speed signal as information basis;

V_iand V_tRespectively, the speed data detected by the ith encoder and the theoretical optimum value, V, of the speed_iSubject to a gaussian distribution,is a probability distribution curve thereof and is a characteristic function of each sensor, v_iIs a V_iAn observed value of_iIs its variance, quoted Δ a_itTo reflect v_iDeviation from the theoretical optimum;

assuming that the optimal data set has M data, the data is represented by Δ a_ijFormed confidence matrix T_mComprises the following steps:

setting a confidence upper limit η_itTaking the confidence coefficient_itThe values of (A) are:

when in use_it1 means that the ith sensor is closer to the theoretical optimum value, the sensor is more reliable, and conversely,_ijif the value is 0, the principle of the ith sensor is the theoretical optimal value, and the sensor is less credible; after the N sensors are evaluated, the parameter precision is obviously higher than that of a single sensor; by a relation matrix R_mThe sensor with the highest reliability can be found, the measured data is defined as the optimal measured value, the optimal measured value can be used for replacing the rejected abnormal data under the condition of a plurality of same sensors, and finally dynamic weighting fusion estimation is carried out; relation matrix R_mAs follows

I.e. the relation matrix R_mThe sum of the ith column of (a) is the confidence level of the ith sensor; the calculation formula of the sensor reliability is as follows:

thus obtaining a second standard of fault judgment; setting confidence thresholds xi based on system noise statistics including expectations and variances_iAnd a diagnosis threshold value ML, which judges whether the following conditions are satisfied according to the comparison between the data analysis result and the value:

if the conditions are met, namely the output exceeds a set threshold value, indicating that a fault occurs, recording the moment in real time, and adopting a corresponding fault coping test or carrying out early warning; otherwise, the encoder is normal.

Technical Field

The invention belongs to the field of agricultural automation, and relates to a control method of a rod-shaped crop harvesting robot.

Background

The agricultural robot belongs to the category of the robot fundamentally, not only is the simple fusion of a mechanical structure and a control algorithm, but also relates to a plurality of technologies such as a multi-sensor fusion technology, an information processing technology, an automatic control technology, a computer technology and a multi-system control technology. Is a comprehensive interdisciplinary. The general structure of the robot system comprises a mechanical structure, a sensing system and a control system; the system comprises six subsystems of a driving system, a mechanical structure system, a feeling system, a robot-environment interaction system, a man-machine interaction system and a control system.

The application of agricultural robot and industrial robot is that there is great difference and agricultural robot's characteristics:

(1) the objects operated by the industrial robot are generally more definite, regular and more recognizable, the application environment is also definite, the possibility of mechanical continuous repeated operation is provided, and the target object of the operation is generally not easy to damage. The object operated by the agricultural robot is generally more irregular, the application environment is more complicated and changeable, the condition of mechanical continuous repeated operation is not provided, the operated target object is easily damaged by a mechanical mechanism, and the damage is irreversible. So agricultural robots are to a certain extent more complex than industrial robots.

(2) Industrial robots can be operated mechanically repeatedly, to the extent that they do not require a sensor for support. However, due to the complexity of the environment and the uncertainty of the target position, the agricultural robot must have a certain sensing capability. It is more difficult for an agricultural robot to be as widespread and widely available as an industrial robot, from only these two points of view.

(3) The operating personnel of the industrial robot is generally technical personnel who are out of the home of the industrial department or personnel trained through system technology, and is personnel with a certain electromechanical operation foundation, and the field of application of the agricultural robot is that the operating personnel are determined to be farmers, and the operating personnel have rich experience in agricultural planting but lack corresponding technical operation experience, so that more human-computer interaction problems need to be considered when the agricultural robot is researched and developed, and the human-computer interaction is simpler and easier to understand.

On the one hand, the barriers caused by the complexity of the technology; another aspect is the economic barrier caused by the incoordination between its inputs and outputs; finally, the demand for human-computer interaction is high. Therefore, the present invention needs to make a coordinated balance in the first two aspects and intensively study in the last aspect. On the basis of ensuring the functions, the hardware cost needs to be reduced as much as possible, and the human-computer interaction needs to be perfected on the basis of the functions and the cost.

Disclosure of Invention

In view of the above, the present invention provides a method for controlling a harvesting robot for rod-shaped crops.

In order to achieve the purpose, the invention provides the following technical scheme:

a control method of a rod-shaped crop harvesting robot comprises the steps of rod-shaped crop autonomous identification, robot control, designed clamping mechanism, designed binding mechanism, designed conveying mechanism and multi-motor detection fault diagnosis.

Optionally, the autonomous identification of the crop stalks is in three parts:

a first part:

(1) acquiring an image of a target rod-shaped crop, constructing a data set of the rod-shaped crop, and carrying out primary processing and labeling on the data set according to the following steps of 3: 1: 1, preparing a training set, a verification set and a test set;

(2) selecting a neural network comprising 10 convolutional layers and 5 full-connection layers, and performing feature extraction with gradually-increased levels on the target through convolutional operation; the method of transfer learning is adopted, the last layer of the convolutional neural network is removed, the last layer of the model is retrained on different data sets, in popular terms, the last layer of the model is retrained to identify different high-grade characteristics, and the training time for different rod-shaped crops is reduced;

(3) using a simulated annealing algorithm to adjust the learning rate, so that the learning rate is higher at the beginning of training, and the learning rate is reduced along with the time so as to ensure that the model can find the optimal value;

a second part:

designing a simple recognition algorithm to make up the defect that the deep convolutional neural network recognition algorithm cannot realize real-time detection under the condition of limited cost; the influence of sunlight is reduced by adopting a color block segmentation method of self-adaptive illumination;

and a third part:

(1) a filtering processing algorithm for ultrasonic ranging:

a. continuously collecting 8 points;

b. recording the number m of points which can be successfully collected;

c. if m is greater than 2, sequencing m points by using a bubbling method, otherwise, continuously executing circulation if the data has misadjustment a;

d. removing the difference between two adjacent pairs of values, and recording a group of positions with the maximum difference value

e. If the maximum group is positioned in the first half part, the next step is carried out, otherwise, the data has misadjustment a and continues to execute circulation;

f. taking the value of the latter half of the position as an effective value;

g. taking the average value of the effective values as a final value;

h. ending or returning to a for continuing circulation;

(2) an algorithm for multi-sensor fusion:

adopting a distributed multi-sensor fusion system structure, bringing the sensor data into the system one by one for gradual fusion to obtain a final result; fusing the two data by adopting a multi-sensor fusion algorithm; then, regarding the fused data as one kind of data, and continuing to add other data for fusion until a final result is output;

the self-adaptive weighting fusion estimation algorithm: on the premise of minimum total mean square error, dynamically distributing corresponding optimal weighting factors W to each sensor according to data measured by each sensor in real time_i(i＝1,2,...n) Wherein, the larger the variance of the sensor is, the smaller the weight value distributed correspondingly is, so as to obtain the final estimated valueClose to the true value Z; suppose that: using n sensors to measure the same characteristic parameter, sigma, of an object₁,σ₂,σ₃,...,σ_nIs the variance of n sensors, the measured theoretical value is Z, and the detection data of each sensor is Z_iN, wherein the detected data are independent of each other and are unbiased estimation of Z, and the estimated parameters are finally obtained fusion results; whereinAnd W_iIt must satisfy:

the total mean square error sigma is derived from the above equation²Comprises the following steps:

according to the theory of extreme value of multivariate function, the total mean square error sigma is obtained²The minimum condition is that the weight corresponding to each sensor is W_i(i ═ 1,2,. n), the smaller the variance, the greater its corresponding weight; minimum total mean square error sigma²Comprises the following steps:

weighting factor W of each sensor corresponding thereto_iComprises the following steps:

corresponding weight W of a plurality of sensors by a formula (1.5)_iThe calculation of (2) is substituted into the formula (1.7), and the final fusion result is obtainedEvaluating the fusion result to judge whether the fusion result is credible;

and Z_tRespectively, the detection data and the theoretical optimum value of the fused sensor, assuming

Obey a Gaussian distribution, then

Is a probability distribution curve thereof and can be taken as a characteristic function of each sensor, sigma_iIs its variance, quote e to reflect

Deviation from the theoretical optimum;

setting a confidence upper limit eta, and taking the value of a confidence coefficient as:

when in use_it1, fusion results

Obtaining a theoretical optimal value Z_tThe fusion result is credible, otherwise,_ijwhen it is 0, the result is fusedTheoretical optimum value Z_tIf the result is negative, the fusion result is not credible; and if the single sensor data is not credible, the data of the single sensor is considered, the data of the single sensor is evaluated according to a certain sequence, the sensor with the minimum deviation is found, and the sensor is used as the optimal detection data.

Optionally, the robot control method includes:

a state feedback optimization controller design, the controller designed is a general controller, not only aiming at static targets, but also suitable for dynamic targets;

the dynamic model of the robot is:

observing the state and the position of a target through a perception system of the robot, and designing a state observer as follows:

the local error is

e_i＝x_i-ω_i1(2.3)

Using the equations (2.1) and (2.3), the local tracking error e is obtained_iIs a dynamic model of

The performance function is of

The control targets are as follows: for the robot, design controller u_iThe following steps are performed:

1) local tracking error e_iThe dynamic model (2.4) is asymptotically stable;

2) performance function V_i(x_i,ω_i1,u_i) (2.5) reaching a minimum value;

defined by local tracking error e_iAnd a state to be tracked omega_i1Constituent augmented system states

The dynamic model of the augmentation system obtained by the formulas (2.2) and (2.4) is

Wherein

Based on the augmented system dynamic model (2.7), the performance function (2.5) is written as

Wherein

The equation (2.9) is derived over time t to give the following Bellman equation

Wherein, let V_i ^*Is the optimal solution of Bellman equation (2.11), the optimality requirement is satisfiedOptimal controllerIs composed of

Substituting equation (2.12) into Bellman equation (2.11) to obtain Hamilton Jacobi Bellman, HJB equation

If there is a continuous semi-positive solution V of formula (2.13)_i ^*Then the optimum controller obtained from equation (2.12)Converging the performance function (2.9) to a minimum value V_i ^*(X_i(0) ); if there is a continuous semi-positive solution V of formula (2.13)_i ^*Then the optimum controller obtained from equation (2.12)

The local tracking error system (2.4) is asymptotically stabilized;

the HJB equation is:

initialization: given a feasible controller

Step 1: policy evaluation, for a givenIs solved by the following formula

Step 2: strategy improvement, updating the controller using the following formula:

and step 3: order to

Return to step 1 until

The convergence to the minimum value is made to be,

controller obtained by equation (2.15)

Make V_i ^j+1≤V_i ^jIn which V is_i ^jSatisfies formula (2.14); the performance function eventually converges to its minimum value, i.e.

Controller obtained by equation (2.15)

The augmentation system (2.7) is gradually stabilized, even if the local tracking error system (2.4) is gradually stabilized.

Optionally, the design clamping mechanism is:

a) the front half part of the clamping mechanism is made of soft materials, and the tail end of the clamping mechanism is of a spherical structure;

b) the rear half part of the clamping mechanism is made of hard materials, and a rubber pad is pasted inside the clamping mechanism to prevent the mechanism from damaging the rod-shaped crops;

c) the joint of the front half part and the rear half part and the bending part of the rear half part are both rotatable shafts; both of them are stressed shafts, i.e. they rotate only under a certain force, the latter force being greater than the former;

d) the entire clamping mechanism is designed to be adjustable up and down to accommodate harvesting of rod-like crops of different heights.

Optionally, the design binding mechanism is:

a) two auxiliary rotating wheels are respectively arranged on two sides, the right side is a No. 1 rotating wheel which is a driving wheel and is provided with a driving rotating mechanism for tightening the binding rope, the left side is a driven wheel which can not rotate actively and is provided with an emergency stop mechanism which can stop the rotation of the driven wheel;

b) an auxiliary binding structure is arranged on the clamping mechanism, and the binding mechanism is tightened outwards under the assistance of the front half part of the clamping mechanism;

c) the heater is arranged at the tightening position of the clamping mechanism, namely the position where the binding rope is wound on the clamping mechanism, and the binding rope is heated and fused after being tightened.

Optionally, the multi-motor detection fault diagnosis is designed as follows:

1) failure analysis of an encoder

The method avoids faults caused by serious faults or complete failures of the encoder, and further improves the robustness and safety of the system;

2) multi-motor detection fault diagnosis system strategy

Based on a principal component analysis method, utilizing the correlation between an encoder signal and a speed signal in a numerical control system, projecting a data covariance matrix reflecting the correlation to the direction with the maximum change, and monitoring the working condition of an encoder through the length of the projection;

3) multi-motor detection fault diagnosis algorithm based on principal component analysis method and reliability judgment

The method comprises the steps that m motors in a system need to be diagnosed, firstly, information is collected, wherein the information comprises encoder signals and speed signals, in order to represent the correlation of the signals, data are processed in real time, forward differential processing is carried out on position signals, and the forward differential processing is expressed as q belongs to R^mAnd w ∈ R^mAssuming that the diagnostic process is started after n times of data are collected, and taking n > 2m according to actual experience; obtaining a data matrix F e R^2mn：

Wherein q is_iAnd w_iN is the ith sampled value; by using covariance matrix in probability theory (M is equal to R)^2m) Representing the correlation of two columns of signals, which is a symmetrical positive definite matrix;

the matrix M with positive definite symmetry of 2M × 2M has the characteristic value of λ₁≥λ₂≥...λ_2m，a_iTo correspond to lambda_iThe feature vector of (2); the maximum change direction of the F matrix is the characteristic vector a_iThe direction of (a); let L be the unit vector a in the maximum projection direction_iA straight line is formed by stretching, and each rotating shaft is provided with a vector v which is obtained by measuring or difference value on the rotating shaft of the auxiliary rotating wheel 1 in the binding mechanism, the rotating shaft driving the robot crawler to rotate and the rotating shaft driving the cutting mechanism to rotate<F>Operation of<F>Expectation for each column of the data matrix F; the projection of the covariance matrix onto this subspace is defined as

The degree of dispersion of the data in this direction is defined as:

wherein the content of the first and second substances,representing data v to u_iProjection on' stretched subspace, operator<·>Represents the expectation of data, Σ_vv＝<vv^T>F represents a covariance matrix of the data matrix, namely the maximum value of the dispersion degree is obtained;

projecting the expected v of the measured data matrix along the eigenvector direction corresponding to the maximum eigenvalue of M, and solving a norm through the projection result to obtain the maximum variation; the variable quantity corresponds to a principal element in a principal element analysis method and is used for representing the correlation among signals in each row; so that a first criterion for a fault determination is obtained;

secondly, introducing a relation between the detection value and a theoretical optimal value, and evaluating whether the detection value is a fault by using the credibility as a second standard; the principal component analysis method is to take the coded signal and the speed signal as information basis and use the speed signal as information basis;

V_iand V_tRespectively, the speed data detected by the ith encoder and the theoretical optimum value, V, of the speed_iSubject to a gaussian distribution,is a probability distribution curve thereof and is a characteristic function of each sensor, v_iIs a V_iAn observed value of_iIs its variance, quoted Δ a_itTo reflect v_iDeviation from the theoretical optimum;

assuming that the optimal data set has M data, the data is represented by Δ a_ijFormed confidence matrix T_mComprises the following steps:

setting a confidence upper limit η_itTaking the confidence coefficient_itThe values of (A) are:

when in use_it1 means that the ith sensor is closer to the theoretical optimum value, the sensor is more reliable, and conversely,_ijif the value is 0, the principle of the ith sensor is the theoretical optimal value, and the sensor is less credible; after the N sensors are evaluated, the parameter precision is obviously higher than that of a single sensor; by a relation matrix R_mThe sensor with the highest reliability can be found, the measured data is defined as the optimal measured value, the optimal measured value can be used for replacing the rejected abnormal data under the condition of a plurality of same sensors, and finally dynamic weighting fusion estimation is carried out; relation matrix R_mAs follows

I.e. the relation matrix R_mThe sum of the ith column of (a) is the confidence level of the ith sensor; the calculation formula of the sensor reliability is as follows:

thus obtaining a second standard of fault judgment; setting confidence thresholds xi based on system noise statistics including expectations and variances_iAnd a diagnosis threshold value ML, which judges whether the following conditions are satisfied according to the comparison between the data analysis result and the value:

if the conditions are met, namely the output exceeds a set threshold value, indicating that a fault occurs, recording the moment in real time, and adopting a corresponding fault coping test or carrying out early warning; otherwise, the encoder is normal.

The invention has the beneficial effects that:

1. the whole hardware structure of the robot is designed, the harvesting problem of the rod-shaped crops is solved in advance before the cost is kept low, and the system is more stable and accurate through the optimization of an algorithm and the fusion of multiple sensors.

2. A convolutional neural network is designed to identify the target, a simple identification algorithm is used for identification to improve the real-time performance, and data obtained by the two algorithms are subjected to data fusion to obtain a consistency explanation

3. The invention uses an algorithm for optimizing ultrasonic data, and obtains more accurate data through algorithm optimization on the ultrasonic with low cost. And a self-adaptive weighting fusion estimation algorithm is used, and the data is optimized to obtain the consistency explanation of the environment.

4. A universal state feedback optimization controller is designed. The controller observes static or dynamic targets and designs a good and stable controller through the feedback of observation results.

5. The automatic clamping mechanism for clamping the rod-shaped crops without energy consumption is designed, the binding mechanism for binding the rod-shaped crops is designed, and the conveying mechanism for binding the rod-shaped crops is designed.

6. Because the hardware system designed in the invention comprises a plurality of motors and a cutting mechanism, a multi-motor detection fault diagnosis algorithm is applied to increase the safety coefficient of the whole system in consideration of the safety problem.

7. The invention has certain reference value, and can improve and complete the design on the basis of the invention not only for crops but also for higher rod-shaped weeds and the like.

Additional advantages, objects, and features of the invention will be set forth in part in the description which follows and in part will become apparent to those having ordinary skill in the art upon examination of the following or may be learned from practice of the invention. The objectives and other advantages of the invention may be realized and attained by the means of the instrumentalities and combinations particularly pointed out hereinafter.

Drawings

For the purposes of promoting a better understanding of the objects, aspects and advantages of the invention, reference will now be made to the following detailed description taken in conjunction with the accompanying drawings in which:

FIG. 1 is a flow chart of convolutional neural network design;

FIG. 2 is a flow chart of a simple recognition algorithm design;

FIG. 3 is a flow chart of ultrasonic algorithm design;

FIG. 4 is a robot hardware system and a thumbnail of each part design drawing;

FIG. 5 is a diagram of the internal hardware design of the robot;

FIG. 6 is an overall side view of the exterior of the robot;

FIG. 7 is an overall top view of the exterior of the robot;

FIG. 8 is a view of the binding mechanism;

FIG. 9 is a view of the cutting mechanism;

FIG. 10 is a diagram of a distributed multi-sensor fusion process of the present invention;

FIG. 11 is a flow chart of a multi-sensor fusion algorithm of the present invention;

FIG. 12 is a flow chart of a multi-motor detection fault diagnosis system;

fig. 13 is a flowchart of the entire system.

Detailed Description

The embodiments of the present invention are described below with reference to specific embodiments, and other advantages and effects of the present invention will be easily understood by those skilled in the art from the disclosure of the present specification. The invention is capable of other and different embodiments and of being practiced or of being carried out in various ways, and its several details are capable of modification in various respects, all without departing from the spirit and scope of the present invention. It should be noted that the drawings provided in the following embodiments are only for illustrating the basic idea of the present invention in a schematic way, and the features in the following embodiments and examples may be combined with each other without conflict.

Wherein the showings are for the purpose of illustrating the invention only and not for the purpose of limiting the same, and in which there is shown by way of illustration only and not in the drawings in which there is no intention to limit the invention thereto; to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of an actual product; it will be understood by those skilled in the art that certain well-known structures in the drawings and descriptions thereof may be omitted.

The same or similar reference numerals in the drawings of the embodiments of the present invention correspond to the same or similar components; in the description of the present invention, it should be understood that if there is an orientation or positional relationship indicated by terms such as "upper", "lower", "left", "right", "front", "rear", etc., based on the orientation or positional relationship shown in the drawings, it is only for convenience of description and simplification of description, but it is not an indication or suggestion that the referred device or element must have a specific orientation, be constructed in a specific orientation, and be operated, and therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes, and are not to be construed as limiting the present invention, and the specific meaning of the terms may be understood by those skilled in the art according to specific situations.

Aiming at the harvesting problem of the rod-shaped crops, the invention builds a rod-shaped crop harvesting robot platform which integrates subsystems such as sensing, motion control, clamping, harvesting, conveying and the like. The robot can autonomously complete crop identification, robot movement and crop harvesting tasks according to given target rod-shaped crops, and people are liberated from a complex and severe harvesting environment. Meanwhile, in order to ensure the safe and stable operation of the system, a motor detection fault diagnosis method is designed.

FIG. 1 is a flow chart of convolutional neural network design; FIG. 2 is a flow chart of a simple recognition algorithm design; FIG. 3 is a flow chart of ultrasonic algorithm design; FIG. 4 is a robot hardware system and a thumbnail of each part design drawing; FIG. 5 is a diagram of the internal hardware design of the robot; FIG. 6 is an overall side view of the exterior of the robot; FIG. 7 is an overall top view of the exterior of the robot; FIG. 8 is a view of the binding mechanism; FIG. 9 is a view of the cutting mechanism; FIG. 10 is a diagram of a distributed multi-sensor fusion process of the present invention; FIG. 11 is a flow chart of a multi-sensor fusion algorithm of the present invention; FIG. 12 is a flow chart of a multi-motor detection fault diagnosis system; fig. 13 is a flowchart of the entire system.

In order to achieve the above object. The invention comprises the following technical links:

autonomous identification of first, rod crop

The autonomous recognition is divided into three parts:

the first part

Designing a recognition algorithm aiming at certain definite rod-shaped crops based on a deep convolutional neural network, and obtaining an optimal recognition model by adjusting and optimizing hyper-parameters.

(1) Acquiring an image of a target rod-shaped crop, constructing a data set of the rod-shaped crop, and carrying out primary processing and labeling on the data set according to the following steps of 3: 1: a scale of 1 prepares the training set, validation set, and test set.

(2) Because the object is identified simply, and the concentrated crops are identified in a concentrated way instead of identifying single crops, a plurality of layers of neural networks are not needed, so that the neural networks comprising 10 convolutional layers and 5 full-connection layers are selected, and the feature extraction with gradually-increased levels is carried out on the target through convolutional operation. Considering that the characteristic coincident points of the rod crop are very many, and the last layers in the convolutional neural network are usually specific to the input data; on the other hand, the previous layers are more general, and simple models are mainly found in a large class. Therefore, the method of transfer learning is adopted by the people, the last layer of the convolutional neural network is removed, the last layer of the model is retrained on different data sets, and the last layer of the model is retrained to identify different advanced features. The training time for different rod-shaped crops can be greatly reduced, and the requirement for the data volume is relatively smaller.

(3) The learning rate is very important for the neural network, the learning rate controls the speed of adjusting the weight of the neural network based on the loss gradient, the gradient is decreased slowly when the learning rate is smaller, the convergence is slower, and intuitively, the training speed consumes longer time and calculation power; but if the gradient is too large, the gradient is easy to cross the optimal value or fluctuate around the optimal value, so that the optimal value cannot be converged; therefore, the simulated annealing algorithm is used in the invention, the learning rate is adjusted from the main, so that the learning rate is higher at the beginning of the training and can be reduced rapidly, but the learning rate is reduced along with the time so as to ensure that the model can find the optimal value, namely, the training speed is ensured, and the problem of local optimization is also avoided.

The second part

A simple recognition algorithm is designed to make up for the defect that the deep convolutional neural network recognition algorithm cannot realize real-time detection under the condition of limited cost. By adopting the color block segmentation algorithm, the expected effect can be achieved by adopting the color block segmentation because the color of the rod-shaped crops is single and the environment where the rod-shaped crops are located is single. Meanwhile, according to the change of light, in order to avoid interference and noise caused by illumination, a color block segmentation method of self-adaptive illumination is adopted, and the influence of sunlight is reduced to the minimum as much as possible.

Third part

The invention relates to an ultrasonic wave filtering method and a multi-sensor fusion algorithm, which are used in the invention, so as to obtain three-dimensional space position information of rod-shaped target crops. The ultrasonic ranging method adopts the ultrasonic sensor with relatively low cost in order to reduce the cost, and the ultrasonic ranging has the characteristic of generating error data irregularly, which is the error of the sensor.

(1) A filtering processing algorithm for ultrasonic ranging is provided:

a. continuously collecting 8 points;

b. recording the number m of points which can be successfully collected;

c. if m is greater than 2, sequencing m points by using a bubbling method, otherwise, continuously executing circulation if the data has misadjustment a;

d. removing the difference between two adjacent pairs of values, and recording a group of positions with the maximum difference value

e. If the maximum group is positioned in the first half part, the next step is carried out, otherwise, the data has misadjustment a and continues to execute circulation;

f. taking the value of the latter half of the position as an effective value;

g. taking the average value of the effective values as a final value;

h. ending or returning to a for continuing circulation;

(2) an algorithm for multi-sensor fusion:

the invention adopts a distributed multi-sensor fusion system structure, and gradually incorporates the sensor data into the system one by one for gradual fusion to obtain a final result. Therefore, the invention adopts the following multi-sensor fusion algorithm to fuse the two data. And then, regarding the fused data as one kind of data, and continuing to add other data for fusion until a final result is output.

Firstly, capturing images, respectively processing the captured images, on one hand, directly processing the images by using a color block segmentation algorithm to obtain the position of a target in a two-dimensional plane, on the other hand, sending the images into a trained convolutional neural network model for recognition, and also outputting the position result of the target in the two-dimensional plane, and then performing preliminary fusion on data by applying the fusion algorithm provided by the invention. And secondly, ranging by using ultrasonic waves, processing data obtained by the ultrasonic waves by using an ultrasonic wave filtering algorithm designed in the invention to obtain processed data, and further fusing the processed data with the fused more accurate two-dimensional plane position data to obtain the position of the target in the three-dimensional space.

The self-adaptive weighting fusion estimation algorithm: on the premise of minimum total mean square error, dynamically distributing corresponding optimal weighting factors W to each sensor according to data measured by each sensor in real time_i(i ═ 1, 2.. n), where the larger the variance of the sensor, the smaller the weight assigned to it, to find the final estimate

Close to the true value Z. Suppose that: using n sensors to measure the same characteristic parameter, sigma, of an object₁,σ₂,σ₃,...,σ_nIs the variance of n sensors, the measured theoretical value is Z, and the detection data of each sensor is Z_iAnd (i ═ 1,2,. n), wherein each detection data is independent of each other and is an unbiased estimation of Z, and the estimated parameters are finally obtained fusion results. Wherein

And W_iIt must satisfy:

the total mean square error sigma is derived from the above equation²Comprises the following steps:

according to the theory of extreme value of multivariate function, the total mean square error sigma is obtained²Minimum condition, each sensor is rightThe weight is W_i(i ═ 1,2,. n), the smaller the variance, the greater its corresponding weight. Minimum total mean square error sigma²Comprises the following steps:

weighting factor W of each sensor corresponding thereto_iComprises the following steps:

corresponding weight W of a plurality of sensors by a formula (1.5)_iThe calculation of (2) is substituted into the formula (1.7), and the final fusion result can be obtained

And evaluating the fusion result to judge whether the fusion result is credible.

And Z_tRespectively, the detection data and the theoretical optimum value of the fused sensor, assumingObey a Gaussian distribution, then

Is a probability distribution curve thereof and can be taken as a characteristic function of each sensor, sigma_iIs its variance, quote e to reflect

And the deviation from the theoretical optimum.

Setting a confidence upper limit eta, and taking the value of a confidence coefficient as:

when in use_it1, fusion results

Obtaining a theoretical optimal value Z_tThe fusion result is credible, otherwise,_ijwhen it is 0, the result is fused

Theoretical optimum value Z_tAnd if not, the fusion result is not credible. And if the single sensor data is not credible, the data of the single sensor is considered, the data of the single sensor is evaluated according to a certain sequence, the sensor with the minimum deviation is found, and the sensor is used as the optimal detection data.

Robot control algorithm

The invention relates to a state feedback optimization controller design, wherein the controller designed by the invention is a universal controller, not only aims at a static target, but also is suitable for a dynamic target.

The dynamic model of the robot is:

the state and the position of a target can be observed through a perception system of the robot, and a state observer is designed as follows:

then the local error is

e_i＝x_i-ω_i1(2.3)

Using equations (2.1) and (2.3), the local tracking error e can be obtained_iIs a dynamic model of

Is then a function of the performance

Thus, the control targets are: for the robot, design controller u_iThe following steps are performed: 1) local tracking error e_iThe dynamic model (2.4) is asymptotically stable; 2) performance function V_i(x_i,ω_i1,u_i) (2.5) reaching a minimum value.

Defined by local tracking error e_iAnd a state to be tracked omega_i1Constituent augmented system states

The dynamic model of the augmented system obtained from equations (2.2) and (2.4) is

Wherein

Based on the augmented system dynamic model (2.7), the performance function (2.5) can be written as

Wherein

The equation (2.9) is derived over time t to give the following Bellman equation

Wherein. Let V_i ^*Is the optimal solution of Bellman equation (2.11), the optimality requirement is satisfied

Available optimal controller

Is composed of

Substituting equation (2.12) into Bellmann equation (2.11) yields the following tracking HJB (Hamilton JacobiBellman) equation

If there is a continuous semi-positive solution V of formula (2.13)_i ^*Then the optimum controller obtained from equation (2.12)The performance function (2.9) can be converged to a minimum value V_i ^*(X_i(0)). If there is a continuous semi-positive solution V of formula (2.13)_i ^*Then the optimum controller obtained from equation (2.12)The local tracking error system (2.4) can be asymptotically stabilized.

Controller obtained by equation (2.15)Can make V_i ^j+1≤V_i ^jIn which V is_i ^jThe formula (2.14) is satisfied. Thus, the performance function may eventually converge to its minimum, i.e.Controller obtained by equation (2.15)The augmentation system (2.7) can be gradually stabilized even if the local tracking error system (2.4) is gradually stabilized.

Third, the design of the clamping mechanism

A clamping mechanism is designed, and can clamp the rod-shaped vegetables on the basis of not damaging the rod-shaped crops.

The design points are as follows:

a) the front half part of the clamping mechanism is made of soft material, and the tail end of the clamping mechanism is of a spherical structure. The soft material and the spherical structure can prevent the damage to the rod-shaped crops on one hand, and on the other hand, the soft material can more effectively tighten the rod-shaped crops after being clamped, so that the size of the bundle can be adjusted according to the position of the robot;

b) the rear half part of the clamping mechanism is made of hard materials, but a rubber pad is pasted inside the clamping mechanism, so that the mechanism can be prevented from damaging the rod-shaped crops;

c) the joint of the front half part and the rear half part and the bending part of the rear half part are both rotatable shafts. Both of which are stressed shafts (i.e., they rotate only when subjected to a certain force), the latter force being greater than the former force;

d) the whole clamping mechanism is designed to be adjustable up and down so as to adapt to the harvesting of rod-shaped crops with different heights

e) The clamping process of the whole clamping mechanism does not need any control operation, and is realized by completely using a rotating shaft and depending on a lever principle and a force action principle, so that the energy is saved, and the expected work can be completed.

Design of binding mechanism

The binding mechanism can bind the rod-shaped crops through a preset binding rope after the rod-shaped crops enter a clamping range, and two ends of the binding mechanism are respectively provided with a rotating wheel.

The design points are as follows:

a) two sides of the main body are respectively provided with an auxiliary rotating wheel, the right side of the main body is a No. 1 rotating wheel which is a driving wheel and is provided with a driving rotating mechanism, the binding rope can be tightened, the left side of the main body is a driven wheel which can not rotate actively, but the main body is provided with an emergency stop mechanism which can stop the rotation of the auxiliary rotating wheel.

b) The clamping mechanism is provided with an auxiliary binding structure, and the binding mechanism is tightened outwards under the assistance of the front half part of the clamping mechanism.

c) The heater is arranged at the tightening position of the clamping mechanism (namely the position where the binding rope is wound on the clamping mechanism), and the binding rope can be heated and fused after being tightened, so that the binding action can be finished, and the continuity of the binding rope can be kept.

Design of conveying mechanism

The conveying mechanism can send the bound rod-shaped crops out of the clamping mechanism after the rod-shaped crops are bound, so that the rod-shaped crops enter the next cycle for harvesting.

The design points are as follows:

a) the push rod is designed to be a rotary structure, but the inner side of the push rod is made of soft materials so as to prevent the damage to the rod-shaped crops.

b) The push rod is designed into a structure with the function of being adjustable up and down, and the purpose is to adapt to rod-shaped crops with different heights.

Six-motor and multi-motor detection fault diagnosis system

In the hardware design of the invention, a plurality of motors are used to complete some necessary functions. The robot moves by means of driving a crawler belt to move forward by a motor arranged at the tail part of the robot, and changing the direction by differential speed; the No. 1 auxiliary rotating wheel of the clamping mechanism is a driving wheel and a cutting mechanism, and the two small motors are also used for completing work respectively. The system belongs to a discrete event system, each subsystem does not have continuity, but has strict sequence, the error of any link can cause the error of the subsequent link, and the measurement error of any motor can cause the runaway of the whole system, so the invention uses a multi-motor detection fault diagnosis system.

Description of the problem

The actual controller typically uses only the angle of rotation feedback signal measured from the encoder, while the speed feedback signal required by the controller algorithm is typically derived by a difference in the position signal. Because of this strong dependence of the controller on the encoded signal, there are high requirements on the accuracy and real-time of the feedback signal. When the encoder fails, the system will fail inevitably, and the job task cannot be completed.

Failure analysis of an encoder

The effects of aging, external impacts, and severe disturbances can all cause failure of the encoder. Missing codes and complete failures are major faults. In general, for the latter, the servo driver will close the output and enter an alarm state; the phenomenon of missing codes is difficult to diagnose and protect accurately, which restricts the high precision and the high reliability of the control system, but has great significance to the robustness of the control system. Through a great deal of practical experience, the phenomenon of missing codes is generally found before the complete failure of the encoder. Therefore, as long as the phenomenon can be timely and accurately captured and diagnosed, and corresponding measures are taken, the fault caused by serious fault or complete failure of the encoder can be avoided, and the robustness and the safety of the system are further improved.

Multi-motor detection fault diagnosis system strategy

In motor servo control systems, in addition to the encoder signal as feedback, the motor drive typically provides a further signal, i.e. a speed signal which is output in the form of an analog quantity, and the actual torque monitoring signal for the drive is derived from a filter applied to the encoder, which speed monitoring signal can also be switched to a control input (desired speed signal). In practical systems, these signals are affected by strong interference and measurement inaccuracies are not usually applied directly to the control algorithm, but these redundant signals are used to monitor the speed or torque or are left unused. The signals have high correlation with the encoder feedback position signals, for example, the forward difference of the encoder rotation angle measuring signals is a rotating speed signal, and therefore, the analytical redundancy is provided for the fault diagnosis of the sensor.

The invention is based on principal component analysis, utilizes the correlation between the encoder signal and the speed signal in the numerical control system, projects the data covariance matrix reflecting the correlation to the direction with the maximum change, and monitors the working condition of the encoder according to the length of the projection.

Multi-motor detection fault diagnosis algorithm based on principal component analysis method and reliability judgment

The method comprises the steps that m motors in a system need to be diagnosed, firstly, information is collected, wherein the information comprises encoder signals and speed signals, in order to represent the correlation of the signals, data are processed in real time, forward differential processing is carried out on position signals, and the forward differential processing is expressed as q belongs to R^mAnd w ∈ R^mAnd assuming that the diagnostic process is started after n times of data are collected, and taking n > 2m according to actual experience. Obtaining a data matrix F e R^2mn：

Wherein q is_iAnd w_iN is the ith sample value. By using covariance matrix in probability theory (M is equal to R)^2m) The correlation of signals of two columns is characterized, and the correlation is a symmetrical positive definite matrix.

The matrix M with positive definite symmetry of 2M × 2M has the characteristic value of λ₁≥λ₂≥...λ_2m，a_iTo correspond to lambda_iThe feature vector of (2). The maximum change direction of the F matrix is the characteristic vector a_iIn the direction of (a). Let L be the unit vector a in the maximum projection direction_iA vector v obtained by measuring or differentiating values on each rotating shaft (i.e. the rotating shaft of the auxiliary rotating wheel 1 in the binding mechanism, the rotating shaft for driving the robot crawler to rotate and the rotating shaft for driving the cutting mechanism to rotate) is set as a straight line<F>Operation of<F>As desired for each column of the data matrix F. Then, the projection of the covariance matrix onto this subspace is defined asTherefore, the degree of dispersion of the data in this direction is defined as:

wherein the content of the first and second substances,

representing data v to u_iProjection on' stretched subspace, operator<·>Represents the expectation of data, Σ_vv＝<vv^T>F denotes the covariance matrix of the data matrix. The maximum value of the dispersion degree is obtained.

According to the deduction, the expected v of the measured data matrix is projected along the direction of the eigenvector corresponding to the maximum eigenvalue of M, and the norm is solved through the projection result, so that the maximum variation is obtained. The variance corresponds to the principal component in the principal component analysis method, and is used to characterize the correlation between the signals in each row. So that the first criterion for failure diagnosis is obtained.

And secondly, introducing the relation between the detection value and the theoretical optimal value, and evaluating whether the detection value is a fault by using the credibility as a second standard. The principal component analysis method is to use the coded signal and the speed signal as information bases, and only the speed signal is used as the information bases, so that on one hand, the speed signal is more intuitive, and more importantly, the theoretical optimal value of the speed signal is more easily obtained. V_iAnd V_tRespectively, the speed data detected by the ith encoder and the theoretical optimum value, V, of the speed_iSubject to a gaussian distribution,is a probability distribution curve thereof and is a characteristic function of each sensor, v_iIs a V_iAn observed value of_iIs its variance, quoted Δ a_itTo reflect v_iAnd the deviation from the theoretical optimum.

Assuming that the optimal data set has M data, the data is represented by Δ a_ijFormed confidence matrix T_mComprises the following steps:

setting a confidence upper limit η_itTaking the confidence coefficient_itThe values of (A) are:

when in use_it1 means that the ith sensor is closer to the theoretical optimum value, the sensor is more reliable, and conversely,_ijand 0, the ith sensor principle is the theoretical optimal value, and the sensor is less credible. After the evaluation of the N sensors, the parameter precision is obviously higher than that of a single sensor. By a relation matrix R_mThe sensor with the highest reliability can be found, the measured data is defined as the optimal measured value, the optimal measured value can be used for replacing the rejected abnormal data under the condition of a plurality of same sensors, and finally dynamic weighting fusion estimation is carried out. Relation matrix R_mAs follows

I.e. the relation matrix R_mThe sum of the ith column of (a) is the confidence level of the ith sensor. The calculation formula of the sensor reliability is as follows:

this results in a second criterion for failure determination. Similar to the application of the conventional fault diagnosis method, the reliability threshold ξ is set according to the statistical characteristics (including expectation and variance) of the system noise by applying the diagnosis process_iAnd a diagnosis threshold value ML, which judges whether the following conditions are satisfied according to the comparison between the data analysis result and the value:

if the conditions are met, namely the output exceeds a set threshold value, the fault is shown to occur, the time is recorded in real time, and corresponding fault coping test or early warning is adopted. Otherwise, the encoder is normal.

Finally, the above embodiments are only intended to illustrate the technical solutions of the present invention and not to limit the present invention, and although the present invention has been described in detail with reference to the preferred embodiments, it will be understood by those skilled in the art that modifications or equivalent substitutions may be made on the technical solutions of the present invention without departing from the spirit and scope of the technical solutions, and all of them should be covered by the claims of the present invention.