Agricultural picking robot mechanical arm system fault diagnosis method based on robust speed interval observer

Abstract

The invention discloses an agricultural picking robot mechanical arm system fault diagnosis method based on a robust speed interval observer. Firstly, a preliminary mechanical arm system mathematical model is established, then a mechanical arm state space model is formed through state selection, mixed performance indexes are set, and a robust speed interval observer suitable for mechanical arm system fault diagnosis is developed. The method mainly aims at a mechanical arm system with disturbance and nonlinear characteristics, the problem that a mechanical arm model does not conform to reality is solved, and the interval observer established through the model can achieve interval estimation on the system state under disturbance of external climate, terrains and the like, so that convergence of an error system is ensured. When the picking robot system has state deviation due to faults, the actual state of the system deviates from the observation interval of the interval observer, and therefore whether the picking robot system breaks down or not is judged. According to the method, accurate fault diagnosis can be carried out on the picking robot under the condition of external disturbance.

Description

Technical Field

[0001] The invention relates to the technical field of interval observers, and in particular to a fault diagnosis method for an agricultural harvesting robot mechanical arm system based on a robust speed interval observer. Background Art

[0002] With the growth of the national economy and the aging population in recent years, the cost of labor has increased dramatically. Currently, most fruits are picked manually. Due to the low efficiency and increasing cost of this method, agricultural picking robots have received widespread attention from the business and academic circles. They are usually composed of components such as vision systems, robotic arms, control systems, and mobile platforms, and are mainly used to perform the three core tasks of fruit recognition, positioning, and separation. Agricultural picking robots are expected to have good picking accuracy under a variety of complex weather, terrain, and other conditions to meet the picking needs of various orchards.

[0003] In the robotic arm system of agricultural harvesting robots, fault diagnosis often uses observer design methods to monitor the robotic arm model, especially for those models containing unknown but bounded terms. The fault detection strategy of interval observers is not restricted by model uncertainty and matching conditions, which greatly improves its application flexibility in agricultural harvesting robot fault detection and has significant theoretical and practical value. However, interval observers have high requirements on gain, and it is often difficult to achieve ideal observer gain settings. Therefore, most of the research on interval observers is still in the theoretical exploration stage. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the technical problem to be solved by the present invention is to provide a fault diagnosis method for an agricultural harvesting robot arm system based on an interval observer. The interval observer can detect faults in the arm system without obtaining the precise model parameters of the harvesting robot arm in advance, thereby overcoming the defects of traditional fault diagnosis methods and improving the adaptability of fault diagnosis methods for agricultural harvesting robot arm systems.

[0005] To achieve the above object, the present invention proposes a method for diagnosing a mechanical arm system fault of an agricultural harvesting robot based on a robust speed interval observer, comprising:

[0006] S1: According to the angles and angular velocities of each joint of the robotic arm, the system dynamics model of the picking robot robotic arm is obtained, and then the state space model of the picking robot robotic arm is obtained through state selection.

[0007] S2: Set specific performance indicators based on the state space model of the picking robot arm.

[0008] S3: Based on the optimization of robust performance indicators, a robust velocity interval observer suitable for fault diagnosis of robotic arm systems is developed using positive system theory.

[0009] S4: Based on the interval observer designed in step three, define the upper and lower bounds of the system state error, construct a dynamic error dynamics model of the interval observer to obtain the estimated values ​​of the upper and lower bounds of the state error, and determine whether the picking robot arm system has a fault. If the estimated values ​​of the upper and lower bounds of the state error of the interval observer are both greater than 0, there is no fault; otherwise, the picking robot arm has a fault.

[0010] The mathematical model of the first robotic arm system established in S1 is:

[0011]

[0012] In this formula, q and are the position and angular velocity of the robot joint, M(q) is the inertia matrix, is the centrifugal force matrix, G(q) is the gravity matrix, u is the control input, τ d is the system disturbance.

[0013] The state space model of the picking robot arm established in S1 is:

[0014] Select state variables The derivative is:

[0015]

[0016] Where d = M(x1) -1 τ d ,H(x1)=M(x1) -1 ,f(x1,x2)=M(x1) -1 (C(x1,x2)x2+G(x1)),

[0017] when

[0018] The state space model expression of the picking robot manipulator system can be obtained:

[0019]

[0020] where x(t)=[x1(t),x2(t)] T ,,y(t) is the output, u(t) is the control input, F(x(t)) is the nonlinear term, matrices A, B, C, are constant matrices, and w(t) is the interference term.

[0021] In S2, according to the state space model of the picking robot manipulator, according to the mixed H2 / H ∞ Performance indicators, optimizing the interval observer of the picking robot arm subject to external disturbances:

[0022] H ∞ Performance indicators: for the system: where A∈R n×n ,B∈R n×q ,C∈R p×n ,D∈R p×q . There exists a scalar γ>0, the system (5) is stable and its transfer function G(s)=C(sI-A) -1 B+D satisfies ||G(s)|| ∞ <γ, if and only if there exists a diagonal matrix P>0 such that the following conditions hold:

[0023] H2 performance index: There exists a scalar γ>0, the system (1) is stable and its transfer function G(s)=C(sI-A) -1 B+D satisfies: ||G(s)||2<γ, if and only if there exists a diagonal matrix P>0 and a matrix W such that the following LMIs (linear matrices inequalities) hold: trace(W)<γ 2 .

[0024] Consider combining H2 performance indicators and H ∞ The performance index is mixed as the optimization index, with scalar α1, α2 as the H2 performance index and H ∞ The proportion of performance indicators in the dynamic error system realizes the comprehensive optimization of the error system.

[0025] The method for developing a robust speed interval observer suitable for fault diagnosis of a robotic arm system according to the robust optimization model in S3 is:

[0026] Determine whether the state matrix of the error system is a Metzler matrix;

[0027] If it is a Metzler matrix, a robust speed interval observer and error system are constructed. If it is not a Metzler matrix, the state matrix of the error system is transformed into a Metzler matrix by introducing two appropriate and interrelated matrices T and N. New vectors are defined, and the state space model of the picking robot manipulator system with intermediate variables is constructed. Based on the reconstructed state space model of the picking robot manipulator system, an interval observer and error system under external interference are constructed.

[0028] When the state matrix of the error system is a Metzler matrix, the method to construct the robust velocity interval observer and the error system is:

[0029] Construct an upper bound observer and a lower bound observer. The upper bound observer always observes values ​​greater than the actual state, and the lower bound observer always observes values ​​less than the actual state. The initial conditions satisfy have Construct the following interval observer:

[0030]

[0031] in, is the upper bound state representation of the system state, x (t) is the lower bound state representation of the system state, For the nonlinear term F(x) to satisfy the condition The upper and lower bounds of .

[0032]

[0033] Construct the error system:

[0034]

[0035] in:

[0036]

[0037] A-LC is the state matrix of the error system

[0038] When the state matrix of the error system is not a Metzler matrix, the method of constructing the interval observer and the error system subject to external disturbance is:

[0039] When the state matrix of the error system is not a Metzler matrix, an appropriate matrix T, N with dimensions satisfying the relationship T+NC=I is introduced so that the state matrix of the error system is a Metzler matrix. The solution to the above relationship can be obtained by the following formula:

[0040]

[0041] Among them, L is the interval observer gain to be calculated, W1, W2 are matrices of appropriate dimensions to be calculated, so that for the initial conditions have The interval observer under the state space model of the picking robot manipulator system with intermediate variables is constructed as follows:

[0042]

[0043] in ξ(t) is the intermediate variable, δ (t) is in the following form:

[0044]

[0045]

[0046] Construct the estimated error system:

[0047]

[0048] where TA-LC is the state matrix of the estimated error system.

[0049] The interval observer of the manipulator system based on the robust optimization model includes:

[0050] Consider a system:

[0051]

[0052] Where R is a known constant matrix and the nonlinear function Then when R is a Metzler matrix, the system is a positive system.

[0053] The advantages of the present invention compared to the prior art are:

[0054] The present invention discloses a method for diagnosing a mechanical arm system fault of an agricultural harvesting robot based on a robust speed interval observer. First, a preliminary mathematical model of the mechanical arm system is established according to the angles and angular velocities of each joint of the mechanical arm of the agricultural harvesting robot, and then a further state space model of the mechanical arm of the agricultural harvesting robot is formed through state selection. Based on the mechanical arm dynamics model, performance indicators are set, and a robust speed interval observer suitable for fault diagnosis of the mechanical arm system is developed by using the positive system theory. The present invention is mainly aimed at a mechanical arm system with disturbances and nonlinear characteristics. This modeling method solves the problem that the mechanical arm model is inconsistent with the actual situation. The interval observer established by the model can maintain an accurate interval estimation of the system state under disturbances such as external lighting and terrain, and ensure that the dynamic error system converges to a predetermined limit. When the state deviation of the harvesting robot system occurs due to a fault, the actual state of the system deviates from the observation interval of the designed interval observer, thereby judging whether the harvesting robot system has a fault. This method can realize accurate fault diagnosis of the harvesting robot under external disturbances. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 Flow chart of the method of the present invention.

[0056] Figure 2This is a structural diagram of the mechanical arm of the agricultural harvesting robot of the present invention. DETAILED DESCRIPTION

[0057] The technical solution of the present invention will be further described below in conjunction with the accompanying drawings and specific examples so that those skilled in the art can have a deep understanding of the present invention and implement it.

[0058] The parameters of the robot arm are described in Table 1:

[0059] Table 1

[0060] parameter Connecting rod 1 Connecting rod 2 Connecting rod quality 2kg 1.2kg Connecting rod length 0.4m 0.3m

[0061] S1: According to the angles and angular velocities of each joint of the robotic arm, the system dynamics model of the picking robot robotic arm is obtained, and then the state space model of the picking robot robotic arm is obtained through state selection.

[0062] S2: Set specific performance indicators based on the state space model of the picking robot arm.

[0063] S3: Based on the optimization of robust performance indicators, a robust velocity interval observer suitable for fault diagnosis of robotic arm systems is developed using positive system theory.

[0064] S4: Based on the interval observer designed in step three, define the upper and lower bounds of the system state error, construct a dynamic error dynamics model of the interval observer to obtain the estimated values ​​of the upper and lower bounds of the state error, and determine whether the picking robot arm system has a fault. If the estimated values ​​of the upper and lower bounds of the state error of the interval observer are both greater than 0, there is no fault; otherwise, the picking robot arm has a fault.

[0065] The mathematical model of the first robotic arm system established in S1 is:

[0066]

[0067] In this formula, q and are the position and angular velocity of the robot joint, M(q) is the inertia matrix, is the centrifugal force matrix, G(q) is the gravity matrix, u is the control input, τ d is the system disturbance.

[0068] The state space model of the picking robot arm established in S1 is:

[0069] Select state variables The derivative is:

[0070]

[0071] Where d = M(x1) -1 τd ,H(x1)=M(x1) -1 ,f(x1,x2)=M(x1) -1 (C(x1,x2)x2+G(x1)),

[0072] when

[0073] The state space model expression of the picking robot manipulator system can be obtained:

[0074]

[0075] where x(t)=[x1(t),x2(t)] T ,,y(t) is the output, u(t) is the control input, F(x(t)) is the nonlinear term, matrices A, B, C are constant matrices, and w(t) is the interference term.

[0076] In S2, according to the state space model of the picking robot manipulator, according to the mixed H2 / H ∞ Performance indicators, optimizing the interval observer of the picking robot arm subject to external disturbances:

[0077] H ∞ Performance indicators: for the system: where A∈R n×n ,B∈R n×q ,C∈R p×n ,D∈R p×q . There exists a scalar γ>0, the system (5) is stable and its transfer function G(s)=C(sI-A) -1 B+D satisfies ||G(s)|| ∞ <γ, if and only if there exists a diagonal matrix P>0 such that the following conditions hold:

[0078] H2 performance index: There exists a scalar γ>0, the system (1) is stable and its transfer function G(s)=C(sI-A) -1 B+D satisfies: ||G(s)||2<γ, if and only if there exists a diagonal matrix P>0 and a matrix W such that the following LMIs (linear matrices inequalities) hold: trace(W)<γ 2 .

[0079] Consider combining H2 performance indicators and H ∞The performance index is mixed as the optimization index, with scalar α1, α2 as the H2 performance index and H ∞ The proportion of performance indicators in the dynamic error system realizes the comprehensive optimization of the error system.

[0080] The method for developing a robust speed interval observer suitable for fault diagnosis of a robotic arm system according to the robust optimization model in S3 is:

[0081] Determine whether the state matrix of the error system is a Metzler matrix;

[0082] If it is a Metzler matrix, a robust speed interval observer and error system are constructed. If it is not a Metzler matrix, the state matrix of the error system is transformed into a Metzler matrix by introducing two appropriate and interrelated matrices T and N. New vectors are defined, and the state space model of the picking robot manipulator system with intermediate variables is constructed. Based on the reconstructed state space model of the picking robot manipulator system, an interval observer and error system under external interference are constructed.

[0083] When the state matrix of the error system is a Metzler matrix, the method to construct the robust velocity interval observer and the error system is:

[0084] Construct an upper bound observer and a lower bound observer. The upper bound observer always observes values ​​greater than the actual state, and the lower bound observer always observes values ​​less than the actual state. The initial conditions satisfy have Construct the following interval observer:

[0085]

[0086] in, is the upper bound state representation of the system state, x(t) is the lower bound state representation of the system state, For the nonlinear term F(x) to satisfy the condition The upper and lower bounds of .

[0087]

[0088] Construct the error system:

[0089]

[0090] in:

[0091]

[0092] A-LC is the state matrix of the error system

[0093] When the state matrix of the error system is not a Metzler matrix, the method of constructing the interval observer and the error system subject to external disturbance is:

[0094] When the state matrix of the error system is not a Metzler matrix, an appropriate matrix T, N with dimensions satisfying the relationship T+NC=I is introduced so that the state matrix of the error system is a Metzler matrix. The solution to the above relationship can be obtained by the following formula:

[0095]

[0096] Among them, L is the interval observer gain to be calculated, W1, W2 are matrices of appropriate dimensions to be calculated, so that for the initial conditions have The interval observer under the state space model of the picking robot manipulator system with intermediate variables is constructed as follows:

[0097]

[0098] in ξ (t) is the intermediate variable, δ (t) is in the following form:

[0099]

[0100]

[0101] Construct the estimated error system:

[0102]

[0103] where TA-LC is the state matrix of the estimated error system.

[0104] The interval observer of the manipulator system based on the robust optimization model includes:

[0105] Consider a system:

[0106]

[0107] Where R is a known constant matrix and the nonlinear function Then when R is a Metzler matrix, the system is a positive system.

[0108] It should be noted that the above-mentioned specific implementation methods are only used to help understand the overall scheme and core ideas of the present invention, rather than limiting them; for other ordinary technicians in this field, they can still make modifications or substitutions based on the specific implementation methods described above; these modifications or substitutions are still within the scope of protection created by the present invention.

Claims

1. A method for fault diagnosis of an agricultural harvesting robot arm system based on a robust velocity interval observer, characterized by the following steps: S1: According to the angles and angular velocities of each joint of the robotic arm, the system dynamics model of the picking robot robotic arm is obtained, and then the state space model of the picking robot robotic arm is obtained through state selection; S2: Setting specific performance indicators based on the state space model of the picking robot arm; S3: Based on the optimization of robust performance indicators, a robust velocity interval observer suitable for fault diagnosis of the manipulator system is developed using positive system theory; S4: Based on the interval observer designed in step three, define the upper and lower bounds of the system state error, construct a dynamic error dynamics model of the interval observer to obtain the estimated values ​​of the upper and lower bounds of the state error, and determine whether the picking robot arm system has a fault. If the estimated values ​​of the upper and lower bounds of the state error of the interval observer are both greater than 0, there is no fault; otherwise, the picking robot arm has a fault.

2. The method for fault diagnosis of an agricultural harvesting robot arm system based on a robust velocity interval observer according to claim 1 is characterized in that: The mathematical model of the first robotic arm system established in S1 is: In this formula, q and are the position and angular velocity of the robot joint, M(q) is the inertia matrix, is the centrifugal force matrix, G(q) is the gravity matrix, u is the control input, τ d is the system disturbance.

3. The method for fault diagnosis of an agricultural harvesting robot arm system based on a robust velocity interval observer according to claim 1, characterized in that: The state space model of the picking robot arm established in S1 is: Select the state variable x1=q, The derivative is: where d = M(x1) -1 τ d , H(x1) = M(x1) -1 , f(x1, x2) = M(x1) -1 (C(x1, x2)x2 + G(x1)), C=[I n×n O n×n ]∈R n×2n , E=I 2n×2n The state space model expression of the picking robot arm system can be obtained: where x(t)=[x1(t),x2(t)] T ,,y(t) is the output, u(t) is the control input, F(x(t)) is the nonlinear term, matrices A, B, C are constant matrices, and w(t) is the interference term.

4. The method for fault diagnosis of an agricultural harvesting robot arm system based on a robust velocity interval observer according to claim 1, characterized in that: In S2, according to the state space model of the picking robot manipulator, according to the mixed H2 / H ∞ Performance indicators, optimizing the interval observer of the picking robot arm subject to external disturbances: H ∞ Performance Indicators For the system: where A∈R n×n ,B∈R n×q ,C∈R p×n ,D∈R p×q . There exists a scalar γ>0, the system (5) is stable and its transfer function G(s)=C(sI-A) -1 B+D Satisfaction If and only if there exists a diagonal matrix P>0 such that the following conditions hold: H2 Performance Indicators There exists a scalar γ>0, the system (1) is stable and its transfer function G(s)=C(sI-A) -1 B+D Satisfaction If and only if there exists a diagonal matrix P>0 and a matrix W such that the following LMIs (linear matrices inequalities) hold: Consider combining H2 performance index and H ∞ The performance index is mixed as the optimization index, with scalar α1, α2 as H2 performance index and H ∞ The proportion of performance indicators in the dynamic error system realizes the comprehensive optimization of the error system.

5. The method for fault diagnosis of an agricultural harvesting robot arm system based on a robust velocity interval observer according to claim 1, characterized in that: The method of developing a robust speed interval observer suitable for fault diagnosis of a robot arm system according to a robust optimization model in S3 is: determining whether a state matrix of the error system is a Metzler matrix; If it is a Metzler matrix, a robust speed interval observer and error system are constructed. If it is not a Metzler matrix, the state matrix of the error system is transformed into a Metzler matrix by introducing two appropriate and interrelated matrices T and N. New vectors are defined, and the state space model of the picking robot manipulator system with intermediate variables is constructed. Based on the reconstructed state space model of the picking robot manipulator system, an interval observer and error system under external interference are constructed.

6. The method for fault diagnosis of an agricultural harvesting robot arm system based on a robust velocity interval observer according to claim 5, characterized in that: When the state matrix of the error system is a Metzler matrix, the method to construct the robust velocity interval observer and the error system is: Construct an upper bound observer and a lower bound observer. The upper bound observer always observes values ​​greater than the actual state, and the lower bound observer always observes values ​​less than the actual state. The initial conditions satisfy have Construct the following interval observer: in, is the upper bound state representation of the system state, x(t) is the lower bound state representation of the system state, For the nonlinear term F(x) to satisfy the condition The upper and lower bounds of ; Construct the error system: in: A-LC is the state matrix of the error system.

7. The method for fault diagnosis of an agricultural harvesting robot arm system based on a robust velocity interval observer according to claim 5 is characterized in that In one embodiment of the present invention, when the state matrix of the error system is not a Metzler matrix, the method for constructing an interval observer and an error system subjected to external disturbance is: When the state matrix of the error system is not a Metzler matrix, an appropriate matrix T, N with dimensions satisfying the relationship T+NC=I is introduced so that the state matrix of the error system is a Metzler matrix. The solution to the above relationship can be obtained by the following formula: L=P -1 W1. Among them, L is the interval observer gain to be calculated, W1, W2 are matrices of appropriate dimensions to be calculated, so that for the initial conditions have The interval observer under the state space model of the picking robot manipulator system with intermediate variables is constructed as follows: in ξ (t) is the intermediate variable, δ (t) is in the following form: Construct the estimated error system: where TA-LC is the state matrix of the estimated error system.

8. The method for fault diagnosis of an agricultural harvesting robot arm system based on a robust speed interval observer according to claim 5 is characterized in that , The interval observer of the manipulator system based on the robust optimization model includes: Consider a system: Where R is a known constant matrix and the nonlinear function Then when R is a Metzler matrix, the system is a positive system.

Images