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

Abstract

The application discloses a kind of agricultural picking robot mechanical arm system fault diagnosis method based on robust speed interval observer.First, the preliminary mathematical model of mechanical arm system is established, then the state space model of mechanical arm is formed by state selection, mixed performance index is set, and the robust speed interval observer suitable for mechanical arm system fault diagnosis is developed.The present application is mainly aimed at the mechanical arm system with disturbance and nonlinear characteristics.This method solves the problem that the mechanical arm model does not match the actual situation.The interval observer established by the model can realize interval estimation of the system state under external climate, terrain and other disturbances, and ensure the convergence of error system.When the picking robot system deviates from the interval observer observation interval due to fault state deviation, it is judged whether the picking robot system has failed.This method can accurately diagnose the fault of picking robot under external disturbance.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of interval observer, in particular to a fault diagnosis method for agricultural picking robot mechanical arm system based on robust speed interval observer. BACKGROUND

[0002] With the growth of the national economy and the intensification of aging in recent years, the cost of labor force has increased sharply. At present, most of the fruit picking is in the form of manual picking. Because this method is inefficient and the cost is gradually high, agricultural picking robots have attracted widespread attention from the business and academic communities. The composition of agricultural picking robots usually includes a vision system, a mechanical arm, a control system and a mobile platform, etc. components, mainly used to perform three core tasks of fruit identification, positioning and separation. Agricultural picking robots hope to have good picking accuracy under various complex weather, terrain and other conditions to meet the picking needs of various orchards.

[0003] In the mechanical arm system of the agricultural picking robot, the observer design method is often used for fault diagnosis of the mechanical arm model, especially for those models containing unknown but bounded items. The fault detection strategy of the interval observer is not limited by model uncertainty and matching conditions, which greatly improves the application flexibility of the interval observer in fault detection of agricultural picking robots, and has significant theoretical and practical value. However, the interval observer has high requirements for gain, and it is often difficult to achieve the ideal observer gain setting, so most of the research on interval observers is still in the theoretical exploration stage. SUMMARY

[0004] In view of the above problems of the prior art, the technical problem to be solved by the present application is to provide a fault diagnosis method for an agricultural picking robot mechanical arm system based on an interval observer. The interval observer is used to detect faults of the mechanical arm system without obtaining the accurate model parameters of the picking robot mechanical arm in advance, overcome the defects of traditional fault diagnosis methods, and improve the adaptability of the fault diagnosis method for the agricultural picking robot mechanical arm system.

[0005] To achieve the above purpose, the present application provides a fault diagnosis method for an agricultural picking robot mechanical arm system based on a robust speed interval observer, comprising:

[0006] S1: obtaining a dynamics model of the picking robot mechanical arm system according to the angles and angular velocities of each joint of the mechanical arm, and then obtaining a state space model of the picking robot mechanical arm through state selection.

[0007] S2: setting a specific performance index based on the state space model of the picking robot mechanical arm.

[0008] S3: According to the robust performance index optimization, a robust velocity interval observer suitable for fault diagnosis of the mechanical arm system is developed by using the positive system theory.

[0009] S4: Based on the interval observer designed in step three, the upper and lower bounds of the system state error are defined, the dynamic error dynamics model of the interval observer is constructed to obtain the estimated values of the upper and lower bounds of the state error, and it is judged whether the picking robot mechanical arm system fails. 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 mechanical arm fails.

[0010] The first mechanical arm system mathematical model established in S1 is:

[0011]

[0012] In the formula, q and are the positions and angular velocities of the joints of the mechanical arm, 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 mechanical arm established in S1 is:

[0014] The state variable is selected

[0015]

[0016] wherein 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]

[0019] The state space model expression of the picking robot mechanical arm system can be obtained:

[0020]

[0021] wherein 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 disturbance term.

[0022] In step S2, based on the state space model of the harvesting robot's robotic arm, according to the hybrid H2 / H... ∞ Performance metrics were optimized for the range observer of the harvesting robot arm subjected to external disturbances:

[0023] H ∞ Performance metrics: For the system: Where A∈R n×n ,B∈R n×q ,C∈R p×n ,D∈R p×q .

[0024] There exists a scalar γ > 0, the system (5) is stable and its transfer function is 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 condition holds:

[0025] 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 satisfies: ||G(s)||2<γ, if and only if there exist a diagonal matrix P>0 and a matrix W such that the following LMIs (linear matricesinequalities) hold:

[0026]

[0027] trace(W) < γ 2 .

[0028] Consider combining H2 performance metrics and H ∞ The performance metrics are combined as optimization metrics, with scalars α1 and α2 used as the performance metrics for H2 and H. ∞ The proportion of performance indicators in a dynamic error system enables comprehensive optimization of the error system.

[0029] The method for developing a robust velocity range observer suitable for fault diagnosis of robotic arm systems based on a robust optimization model in S3 is as follows:

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

[0031] If the Metzler matrix, the robust velocity interval observer and error system are constructed, if it is not Metzler matrix, by introducing two dimension appropriate and interrelated matrix T, N, the state matrix of error system is Metzler matrix, the new vector is defined, the picking robot arm system state space model with intermediate variable is constructed, according to the restructured picking robot arm system state space model, the interval observer and error system under external disturbance are constructed.

[0032] When the state matrix of error system is Metzler matrix, the method for constructing robust velocity interval observer and error system is:

[0033] The upper bound observer and lower bound observer are constructed, the upper bound observer observes the value greater than the actual state at the moment, the lower bound observer observes the value less than the actual state at the moment, the initial condition satisfies There are The interval observer is constructed as follows:

[0034]

[0035] Wherein, The upper bound state of system state is represented as x The lower bound state of system state is represented as The upper and lower bound representations of nonlinear term F(x) satisfy the condition

[0036] The error system is constructed as follows:

[0037]

[0038]

[0039] Wherein:

[0040]

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

[0042] When the state matrix of error system is not Metzler matrix, the method for constructing interval observer and error system under external disturbance is:

[0043] When the state matrix of error system is not Metzler matrix, the dimension appropriate matrix T, N satisfying the relationship T+NC=I is introduced, so that the state matrix of error system is Metzler matrix, and the solution of the above relationship can be obtained by the following formula,

[0044]

[0045] ​Wherein, L is the interval observer gain to be solved, W1, W2 are dimension appropriate to be solved matrix, so that for initial condition There are The interval observer of picking robot mechanical arm system state space model with intermediate variable is constructed as:

[0046]

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

[0048]

[0049]

[0050] The estimation error system is constructed as:

[0051]

[0052] Wherein, TA-LC is the state matrix of the estimation error system.

[0053] The interval observer of mechanical arm system based on robust optimization model includes:

[0054] Consider a system:

[0055]

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

[0057] The advantages of the present application compared with the prior art are:

[0058] The agricultural picking robot mechanical arm system fault diagnosis method based on the robust speed interval observer provided by the application firstly establishes a preliminary mechanical arm system mathematical model according to the angles and angular velocities of each joint of the agricultural picking robot mechanical arm, and then forms a further agricultural picking robot mechanical arm state space model through state selection. Based on the mechanical arm dynamics model, performance indicators are set, and a robust speed interval observer suitable for mechanical arm system fault diagnosis is developed by using the positive system theory. The application mainly aims at the mechanical arm system with disturbance and nonlinear characteristics. This modeling method solves the problem that the mechanical arm model does not conform to the actual situation. The interval observer established through the model can maintain accurate interval estimation of the system state under external disturbance such as illumination terrain, and ensure that the dynamic error system converges to a predetermined limit. When the picking robot system deviates from the state due to fault, the actual state of the system deviates from the observation interval of the designed interval observer, so as to judge whether the picking robot system has a fault. This method can realize accurate fault diagnosis of the picking robot under external disturbance. BRIEF DESCRIPTION OF DRAWINGS

[0059] Figure 1 The flowchart of the method of the application.

[0060] Figure 2 The structure diagram of the agricultural picking robot mechanical arm of the application DETAILED DESCRIPTION

[0061] The technical solutions of the application will be further described below with reference to the drawings and specific examples, so that those skilled in the art can have a deep understanding of the application and implement it.

[0062] The mechanical arm parameters are as shown in Table 1:

[0063] Table 1

[0064] Parameter Connecting rod 1 Connecting rod 2 Connecting rod mass 2 kg 1.2 kg Connecting rod length 0.4m 0.3m

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

[0066] S2: Based on the picking robot mechanical arm state space model, specific performance indicators are set.

[0067] S3: According to the robust performance index optimization, a robust speed interval observer suitable for mechanical arm system fault diagnosis is developed by using the positive system theory.

[0068] S4: Based on the interval observer designed in step three, the upper and lower bounds of the system state error are defined, the dynamic error dynamics model of the interval observer is constructed to obtain the estimated values of the upper and lower bounds of the state error, and it is judged whether the picking robot manipulator system fails. 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 failure; otherwise, the picking robot manipulator fails.

[0069] The first mechanical arm system mathematical model established in S1 is:

[0070]

[0071] In the formula, q and are the positions and angular velocities of the joints of the mechanical arm, 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.

[0072] The state space model of the picking robot manipulator established in S1 is:

[0073] The state variable is selected

[0074]

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

[0076] When

[0077]

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

[0079]

[0080] Wherein 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, matrix A, B, C is a constant matrix, w(t) is the disturbance term.

[0081] In S2, according to the state space model of the picking robot manipulator, the interval observer of the picking robot manipulator disturbed by external disturbance is optimized according to the mixed H2 / H ∞ performance index:

[0082] H ∞ Performance index: for the system: where A∈R n×n ,B∈R n×q ,C∈R p×n ,D∈R p×q .

[0083] 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)||2 ∞ < γ, if and only if there exists a diagonal matrix P>0 such that the following conditions are met:

[0084] 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 matrix inequalities) are met:

[0085]

[0086] trace(W)< γ 2 .

[0087] Consider the H2 performance index and H ∞ The performance index is mixed as an optimization index, and α1, α2 are used as the proportion of H2 performance index and H ∞ The performance index in the dynamic error system, realize the comprehensive optimization of error system.

[0088] The method for developing a robust velocity interval observer suitable for fault diagnosis of a mechanical arm system in S3 according to a robust optimization model is:

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

[0090] If it is a Metzler matrix, construct a robust velocity interval observer and an error system, if it is not a Metzler matrix, by introducing two dimensionally appropriate and interrelated matrices T and N, the state matrix of the error system is changed into a Metzler matrix, a new vector is defined, a picking robot mechanical arm system state space model with intermediate variables is constructed, and an interval observer and an error system under external disturbance are constructed according to the reconstructed picking robot mechanical arm system state space model.

[0091] When the state matrix of the error system is Metzler matrix, the method for constructing the robust velocity interval observer and the error system is:

[0092] The upper bound observer and the lower bound observer are constructed, the upper bound observer observes the value greater than the actual state at each moment, the lower bound observer observes the value less than the actual state at each moment, and the initial condition satisfies There are The interval observer is constructed as follows:

[0093]

[0094] Wherein, is the upper bound state representation of the system state, x(t) is the lower bound state representation of the system state, is the upper and lower bound representation of the nonlinear term F(x) satisfying the condition

[0095]

[0096] The error system is constructed as follows:

[0097]

[0098] Wherein:

[0099]

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

[0101] When the state matrix of the error system is not Metzler matrix, the method for constructing the interval observer and the error system under external disturbance is:

[0102] When the state matrix of the error system is not Metzler matrix, a dimension appropriate matrix T, N satisfying the relationship T+NC=I is introduced, so that the state matrix of the error system is Metzler matrix, and the solution of the above relationship can be obtained by

[0103]

[0104] Wherein, L is the interval observer gain to be solved, W1, W2 are dimension appropriate matrices to be solved, so that for the initial condition There are The interval observer under the state space model of the picking robot mechanical arm system with intermediate variable is constructed as:

[0105]

[0106] Wherein ξ (t) is the intermediate variable, ​δ (t) is in the form of:

[0107]

[0108]

[0109] Construct an estimation error system:

[0110]

[0111] Where TA-LC is the state matrix of the estimation error system.

[0112] Preceding the interval observer of the manipulator system based on the robust optimization model includes:

[0113] Consider a system:

[0114]

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

[0116] It should be noted that the above specific embodiments are only used to help understand the overall scheme and core idea of the present application, and are not its limitation; for other ordinary skilled in the art, according to the specific implementation scheme described above, can still be modified or replaced; these modifications or replacements are still within the protection scope of the present application.

Claims

1. A method for fault diagnosis of agricultural picking robot manipulator system based on robust velocity interval observer, characterized by the following steps: S1: obtaining a picking robot manipulator system dynamics model according to the angles and angular velocities of each joint of the manipulator, and then obtaining a picking robot manipulator state space model through state selection; S2: setting a mixed H2 / H∞ performance index based on the picking robot manipulator state space model, and optimizing the interval observer based on the performance index; H ∞ Performance indicators For the system: where A e R n×n , B e R n×q , C e R p×n , D e R p×q , There exists a scalar γ > 0 such that the system (1) is stable and its transfer function G(s) = C(sI-A) -1 B + D satisfies ||G(s)| ∞ <γ, There exists a diagonal matrix P > 0 if and only if the following condition is met: H2 performance index There exists a scalar γ > 0 such that the system (1) is stable and its transfer function G(s) = C(sI-A) -1 B + D satisfies ||G(s)||2< γ, There exists a diagonal matrix P > 0 and a matrix W if and only if the following LMIs are met: trace(W) < γ 2 ; Consider the H2 performance index and H ∞ performance index mixing, with scalar a1, a2 as H2 performance index and H ∞ The weight of the performance index in the dynamic error system realizes the comprehensive optimization of the error system. S3: developing a robust velocity interval observer suitable for manipulator system fault diagnosis using positive system theory based on robust performance index optimization; S4: defining the upper and lower bounds of the system state error based on the interval observer designed in step three, constructing 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 judging whether the picking robot manipulator system has failed, if the estimated values of the upper and lower bounds of the interval observer state error are both greater than 0, there is no fault; otherwise, the picking robot manipulator has failed.

2. The robust velocity interval observer based fault diagnosis method for agricultural picking robot manipulator system according to claim 1, characterized in that, The manipulator system dynamics model established in S1 is: where q and are the position and angular velocity of the robot joints, respectively, 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 robust velocity interval observer based fault diagnosis method for agricultural picking robot manipulator system according to claim 1, characterized in that, The picking robot manipulator state space model established in S1 is: The state variable x1 = q is selected, 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), When C = [I n×n O n×n ] ∈ R n×2n , E = I 2n×2n , the state space model expression of the picking robot manipulator 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 disturbance term.

4. The robust velocity interval observer based fault diagnosis method for agricultural picking robot manipulator system according to claim 1, characterized in that, The method for developing a robust velocity interval observer suitable for manipulator system fault diagnosis based on robust optimization model in S3 is: judging whether the state matrix of the error system is a Metzler matrix; If it is a Metzler matrix, construct a robust velocity interval observer and an error system, if it is not a Metzler matrix, introduce two dimensionally appropriate and interrelated matrices T, N to make the state matrix of the error system a Metzler matrix, define a new vector, construct a picking robot manipulator system state space model with intermediate variables, and construct an interval observer and an error system under external disturbance based on the reconstructed picking robot manipulator system state space model.

5. The robust velocity interval observer based fault diagnosis method for agricultural picking robot manipulator system according to claim 4, characterized in that, When the state matrix of the error system is a Metzler matrix, the method for constructing a robust velocity interval observer and an error system is: The upper bound observer and the lower bound observer are constructed. The upper bound observer observes the value greater than the actual state at any time, and the lower bound observer observes the value less than the actual state at any time. The initial condition satisfies There are The interval observer is constructed as follows: wherein, is an upper bound state representation of the system state, x(t) is a lower bound state representation of the system state, is a nonlinear term F(x) satisfying the condition is an upper and lower bound representation, L is an observer gain matrix; W = E + w - -E - w + , Construct the error system: Where: e(t) = x(t) - x(t), e w = E + w - - E - w + - Ew, A-LC is the state matrix of the error system.

6. The robust velocity interval observer based fault diagnosis method for agricultural picking robot manipulator system according to claim 4, characterized in that When the state matrix of the error system is not a Metzler matrix, the method for constructing an interval observer and an error system under external disturbance is: When the state matrix of the error system is not a Metzler matrix, introduce a dimensionally appropriate matrix T, N that satisfies the relationship T+NC=I, so that the state matrix of the error system is a Metzler matrix, and the solution of the above relationship can be obtained by the following formula, L = P -1 W1; Wherein, L is the interval observer gain to be solved, W1, W2 are dimension appropriate to be solved matrices, so that for the initial condition There are The interval observer construction under the state space model of the picking robot mechanical arm system with intermediate variables is as follows: wherein ξ (t) is an intermediate variable, δ (t) is of the form: Construct an estimated error system: Where TA-LC is the state matrix of the estimated error system.

7. The robust velocity interval observer based fault diagnosis method for agricultural picking robot manipulator system according to claim 4, characterized in that , Before the interval observer of the manipulator system based on the robust optimization model includes: Consider a system: where R is a known constant matrix, x0is the initial state of the system, and the nonlinear function Then the system is a positive system when R is a Metzler matrix.

Images