A delay compensation control method for remote control loader

Abstract

The present invention discloses a delay compensation control method for a remote control loader, and the steps include: building a network environment; delay calculation; modeling the loader; designing the parameters of the predictor; system linearization; Smith delay compensation control; and obtaining an improved Smith predictive control system with an adaptive module. The present invention calculates the system delay time by exchanging time frames at the transmission layer, thereby achieving real-time and accurate measurement of the delay time; the inertia time constant and proportional magnification in the Smith predictor are obtained according to the linearization result, thereby avoiding complicated manual adjustment of the predictor; a PID controller is selected for feedback control, so that the end of the equivalent two-link track the given expected trajectory, and a Smith compensator is used for time compensation, thereby reducing the influence of the modeling error on the Smith predictive control system, eliminating the pure lag term in the closed-loop characteristic equation, and significantly improving the system performance.

Description

Technical Field

[0001] The invention relates to a time delay compensation control method for a remote control loader, belonging to the technical field of engineering machinery. Background Art

[0002] As one of the most common construction machinery, loaders are widely used in engineering construction, ore mining and other fields. Traditional loaders must be manually driven by the driver. When working in a harsh working environment, it will not only increase the difficulty of the driver's operation, but also may have a significant impact on the driver's health and even endanger personal safety. With the development of technology, the intelligence and informatization of construction machinery have become a new development direction. Remote control of loaders through computer networks can greatly improve work efficiency, effectively avoid risks and ensure the safety of workers.

[0003] However, in remote control systems based on computer networks, transmission delay is an unavoidable problem that greatly reduces the stability of the system. How to eliminate the impact of delay as much as possible is still a problem that needs to be studied and solved. Summary of the invention

[0004] In view of the problems existing in the above-mentioned prior art, the present invention provides a delay compensation control method for a remote-controlled loader, which can realize real-time and accurate measurement of the delay time, and at the same time, can realize delay compensation for the control system, thereby minimizing the impact of the delay on the system.

[0005] In order to achieve the above object, the present invention provides a delay compensation control method for a remote control loader, comprising the following steps:

[0006] A. Build the network environment:

[0007] The camera installed on the loader collects the image information of the loader's working environment in real time, and transmits the collected image data signal to the control end in real time; after receiving the image data signal, the control end sends a command to realize real-time control of the loader;

[0008] B. Delay calculation:

[0009] (1) The message sending and receiving process under the network protocol synchronization mechanism is as follows:

[0010] ① Send message A from the master clock and record the precise sending time t1 when message A leaves the master clock;

[0011] ②Message B encapsulates the precise sending time t1 and sends it to the slave clock;

[0012] ③ The slave clock records the precise arrival time of message A at the slave clock as t2;

[0013] ④ When sending C message from the clock, record the precise sending time t3;

[0014] ⑤ The master clock records the precise arrival time t4 of the C message at the master clock;

[0015] ⑥ The master clock sends a D message carrying precise timestamp information t4 to the slave clock;

[0016] (2) A master clock is specified in the system. Offset is used to represent the deviation between the master and slave clocks. Delay is used to represent the network delay, that is, the delay of the message during transmission. The calculation formulas for clock deviation and network delay are as follows:

[0017]

[0018] C. Loader modeling:

[0019] (1) The DH parameter method is used for kinematic modeling, that is, a spatial coordinate system is established on each joint of the equivalent two-link, and the transformation relationship between two different spatial coordinate systems is established through homogeneous coordinate transformation;

[0020] Assume that the fixed base is link 0, the connection between it and link 1 is joint 1, the connection between link 1 and link 2 is joint 2, the end of link 2 is joint 3, establish the base coordinate system and coordinate system 1 at joint 1, establish coordinate system 2 at joint 2, and establish coordinate system 3 at joint 3;

[0021] (2) Through forward kinematics solution, the end coordinates of the two-link are obtained as follows:

[0022]

[0023] In formula (2), θ1 is the rotation angle of joint 1, θ2 is the rotation angle of joint 2, L1 is the length of rod 1, and L2 is the length of rod 2;

[0024] (3) Inverse kinematics analysis:

[0025] In contrast to forward kinematics, inverse kinematics is to find the joint angle based on the known end position. A perpendicular line is drawn from the end point of the two links to the y-axis and connected to the origin to form a right triangle with side lengths of X, Y, and L0. Suppose the upper left angle of the right triangle is θ0, then:

[0026]

[0027] In formula (3),

[0028] According to formula (2), we can get:

[0029]

[0030] Taking the squares and adding them together gives:

[0031]

[0032] Combining equation (3), we get:

[0033]

[0034] Find:

[0035]

[0036] D. Estimator parameter design:

[0037] (1) The two-link dynamics model is as follows:

[0038]

[0039]

[0040] In equations (8) and (9), θ3 and θ4 represent the rotation angles of the two connecting rods. represents the angular velocity of the two-link, represents the angular acceleration of the two-link, M(θ) is the inertia matrix of the two-link; represents the centrifugal force and Coriolis force matrix; G(θ) represents the gravity matrix of the two-link; τ represents the generalized force;

[0041] E. System Linearization:

[0042] Since the reference model in the predictor is linear, and the robot dynamics model is nonlinear, it needs to be linearized near the working point. For the system, the state variables are selected as follows:

[0043]

[0044] The state space form of the system is written as:

[0045]

[0046] After Jacobi linearization, the system matrix is:

[0047]

[0048] This matrix is ​​represented as:

[0049] F.Smith delay compensation control:

[0050] R(s) and Y(s) represent the input and output of the control system, respectively.P (s) are the transfer functions between the controller and the controlled object, e -τs It is called the time lag factor, which represents the pure lag link of the controlled object.

[0051] The transfer function of the control system is:

[0052]

[0053] The characteristic equation of the system is obtained from formula (10):

[0054] 1+D(s)G p (s)e -τs =0 (11);

[0055] By connecting the Smith predictor in parallel to D(s), the transfer function of the controller is obtained as:

[0056]

[0057] The closed-loop transfer function of the system after Smith predictor compensation is:

[0058]

[0059] G. The improved Smith predictive control system with adaptive module is obtained:

[0060] The process channel transfer function is:

[0061]

[0062] but:

[0063] In formula (15), ε(s) is the error signal; T1 and T2 are the inertia time constants, and K is the gain of the inertia link;

[0064] Transforming equation (15) into a differential equation, we get:

[0065]

[0066] Similarly, there are: In the formula, is the parameter to be designed to compensate for the modeling error, given by:

[0067] ε(s)=Y(s)-X(s)

[0068] The error equation is:

[0069] In order to make the equation stable, we need to find a Lyapunov function V(v,t) such that make but:

[0070]

[0071] The Lyapunov function is constructed as follows:

[0072]

[0073] Select the adaptive law as: The Lyapunov stability condition is satisfied;

[0074] Since the system parameters change slowly, have to The expression of the adaptive module is:

[0075]

[0076] In formula (21), K0 is the integration constant.

[0077] Furthermore, in step A, setting up the network environment includes the following steps:

[0078] (1) Network topology design: camera vision nodes are set up in the network domain, these nodes are connected to the server through the wireless LAN, and the server then transmits to the client through the limited LAN;

[0079] (2) Communication protocol settings: the video captured by the camera is encoded using the MPEG-4 standard, the application layer uses the FTP protocol, the transport layer uses the UDP protocol, the network layer uses the IP protocol, and the wireless LAN uses the IEEE802.11 protocol; at the same time, the network protocol is applied to ensure that all devices on this layer of the network are synchronized with the clock.

[0080] The present invention calculates the delay time of the system by building a network environment and exchanging time frames at the transmission layer, thereby realizing real-time and accurate measurement of the delay time; the bucket and boom of the loader are equivalent to a two-link mechanical arm, and the equivalent mechanical arm model is kinematically modeled and dynamically modeled, and a simulation model of the loader is obtained through dynamic modeling. The simulation model is a nonlinear system model, and the dynamic model is linearized near the working point through the Jacobian matrix. The inertia time constant and the proportional magnification in the Smith predictor are obtained according to the linearization result, thereby avoiding complicated manual adjustment of the predictor; due to the modeling error after linearization, the present invention selects a PID controller for feedback control, so that the end of the equivalent two-link tracks the given expected trajectory, and a Smith compensator is used for time compensation, which greatly reduces the influence of the modeling error on the Smith predictive control system, eliminates the pure lag term in the closed-loop characteristic equation, and significantly improves the system performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 It is a work flow chart of the present invention;

[0082] Figure 2 It is a synchronization principle diagram of the network protocol of the present invention;

[0083] Figure 3 DH model schematic diagram of the two-link mechanical arm of the present invention;

[0084] Figure 4 It is a control principle diagram of a control system with a pure lag link of the present invention;

[0085] Figure 5 It is the control principle diagram of the Smith predictive compensation system of the present invention;

[0086] Figure 6 It is a schematic diagram of the Smith predictive control system with an adaptive module of the present invention;

[0087] Figure 7 is a graph of experimental results without adding the Smith predictor in an embodiment of the present invention;

[0088] Figure 8 It is a graph of experimental results of adding a Smith predictor in an embodiment of the present invention. DETAILED DESCRIPTION

[0089] The present invention will be further described below in conjunction with the accompanying drawings.

[0090] like Figure 1 As shown, a delay compensation control method for a remote control loader comprises the following steps:

[0091] A. Build the network environment:

[0092] The camera installed on the loader collects the image information of the loader's working environment in real time, and transmits the collected image data signal to the control end in real time; after receiving the image data signal, the control end sends a command to realize real-time control of the loader;

[0093] B. Delay calculation:

[0094] like Figure 2 The synchronous principle diagram of the network protocol is shown as follows:

[0095] (1) The message sending and receiving process under the network protocol synchronization mechanism is as follows:

[0096] ① Send out message A from the master clock, and record the precise sending time t1 when message A leaves the master clock; message A may or may not carry the sending time information;

[0097] ② Message B encapsulates the precise sending time t1 and sends it to the slave clock; compared with the sending time carried by message A, the sending time carried by message B is accurate;

[0098] ③ The slave clock records the precise arrival time of message A at the slave clock as t2;

[0099] ④ When sending C message from the clock, record the precise sending time t3;

[0100] ⑤ The master clock records the precise arrival time t4 of the C message at the master clock;

[0101] ⑥ The master clock sends a D message carrying precise timestamp information t4 to the slave clock;

[0102] (2) A master clock is specified in the system. Offset is used to represent the deviation between the master and slave clocks. Delay is used to represent the network delay, that is, the delay of the message during transmission. The calculation formulas for clock deviation and network delay are as follows:

[0103]

[0104] C. Loader modeling:

[0105] like Figure 3 The figure shows the DH model of a two-link mechanical arm. Taking the HT-25J multi-functional loader as an example, the bucket and arm of the loader are equivalent to a two-link mechanical arm, and the kinematic modeling and dynamic modeling of the equivalent mechanical arm model are performed:

[0106] (1) The DH parameter method is used for kinematic modeling, that is, a spatial coordinate system is established on each joint of the equivalent two-link, and the transformation relationship between two different spatial coordinate systems is established through homogeneous coordinate transformation;

[0107] Assume that the fixed base is link 0, the connection between it and link 1 is joint 1, the connection between link 1 and link 2 is joint 2, the end of link 2 is joint 3, establish the base coordinate system and coordinate system 1 at joint 1, establish coordinate system 2 at joint 2, and establish coordinate system 3 at joint 3;

[0108] (2) Through forward kinematics solution, the end coordinates of the two-link are obtained as follows:

[0109]

[0110] In formula (2), θ1 is the rotation angle of joint 1, θ2 is the rotation angle of joint 2, L1 is the length of rod 1, and L2 is the length of rod 2;

[0111] (3) Inverse kinematics analysis:

[0112] In contrast to forward kinematics, inverse kinematics is to find the joint angle based on the known end position. A perpendicular line is drawn from the end point of the two links to the y-axis and connected to the origin to form a right triangle with side lengths of X, Y, and L0. Suppose the upper left angle of the right triangle is θ0, then:

[0113]

[0114] In formula (3),

[0115] According to formula (2), we can get:

[0116]

[0117] Taking the squares and adding them together gives:

[0118]

[0119] Combining equation (3), we get:

[0120]

[0121] Find:

[0122]

[0123]

[0124] D. Estimator parameter design:

[0125] like Figure 4 The control principle diagram of the control system with pure lag link is shown as follows:

[0126] (1) The two-link dynamics model is as follows:

[0127]

[0128] In equations (8) and (9), θ3 and θ4 represent the rotation angles of the two connecting rods. represents the angular velocity of the two-link, represents the angular acceleration of the two-link, M(θ) is the inertia matrix of the two-link; represents the centrifugal force and Coriolis force matrix; G(θ) represents the gravity matrix of the two-link; τ represents the generalized force;

[0129] E. System Linearization:

[0130] like Figure 5 The control principle diagram of Smith predictive compensation system is shown as follows:

[0131] Since the reference model in the predictor is linear, and the robot dynamics model is nonlinear, it needs to be linearized near the working point. For the system, the state variables are selected as follows:

[0132]

[0133] The state space form of the system can be written as:

[0134]

[0135] After Jacobi linearization, the system matrix is:

[0136]

[0137] This matrix is ​​represented as: The order of each sub-matrix is ​​2×2. To simplify the model, we can transform the non-diagonal matrix A into 12 , A 21 Approximately 0, taking 45° as the working point, substituting the specific values ​​into the result is:

[0138]

[0139] Corresponding to two second-order systems, the transfer function is:

[0140]

[0141] The transfer function of the approximated system is used as the reference model. 12 , A 22 is considered as 0, so there must be a modeling error.

[0142] F.Smith delay compensation control:

[0143] R(s) and Y(s) represent the input and output of the control system, respectively. P (s) are the transfer functions between the controller and the controlled object, e -τs It is called the time lag factor, which represents the pure lag link of the controlled object.

[0144] The transfer function of the control system is:

[0145]

[0146] The characteristic equation of the system is obtained from formula (10):

[0147] 1+D(s)G p (s)e -τs =0 (11);

[0148] By connecting the Smith predictor in parallel to D(s), the transfer function of the controller is obtained as:

[0149]

[0150] The closed-loop transfer function of the system after Smith predictor compensation is:

[0151]

[0152] G. The improved Smith predictive control system with adaptive module is obtained:

[0153] like Figure 6 As shown, this is the schematic diagram of the Smith predictive control system with an adaptive module:

[0154] The process channel transfer function is:

[0155]

[0156] but:

[0157] In formula (15), ε(s) is the error signal; T1 and T2 are the inertia time constants, and K is the gain of the inertia link;

[0158] Transforming equation (15) into a differential equation, we get:

[0159]

[0160] Similarly, there are: In the formula, is the parameter to be designed to compensate for the modeling error, given by:

[0161] ε(s)=Y(s)-X(s)

[0162] The error equation is:

[0163] Where X(s) is the output of the Smith estimation model;

[0164] In order to make the equation stable, we need to find a Lyapunov function V(v,t) such that make but:

[0165]

[0166] The Lyapunov function is constructed as follows:

[0167]

[0168] Select the adaptive law as: The Lyapunov stability condition is satisfied;

[0169] Since the system parameters change slowly, have to The expression of the adaptive module is:

[0170]

[0171] In formula (21), K0 is the integration constant.

[0172] As a preferred implementation, in step A, building a network environment includes the following steps:

[0173] (1) Network topology design: camera vision nodes are set up in the network domain, these nodes are connected to the server through the wireless LAN, and the server then transmits to the client through the limited LAN;

[0174] (2) Communication protocol settings: the video captured by the camera is encoded using the MPEG-4 standard, the application layer uses the FTP protocol, the transport layer uses the UDP protocol, the network layer uses the IP protocol, and the wireless LAN uses the IEEE802.11 protocol; at the same time, the network protocol is applied to ensure that all devices on this layer of the network are synchronized with the clock.

[0175] The following experiments were conducted using a simulated loader:

[0176] like Figure 7 and Figure 8 The following are the experimental result diagrams without and with the Smith Predictor. It can be seen from the diagram that the network without the Smith Predictor has a delay that makes the system unstable. However, after adding the Smith Predictor, after about 100 seconds of adjustment time, the system remains in a relatively stable state. At this time, the expected value and the actual value basically coincide, and the Smith Predictor has achieved good results.

Claims

1. A delay compensation control method for a remote control loader, characterized in that: The following steps are involved: A. Build the network environment: The camera installed on the loader collects the image information of the loader's working environment in real time, and transmits the collected image data signal to the control end in real time; after receiving the image data signal, the control end sends a command to realize real-time control of the loader; B. Delay calculation: (1) The message sending and receiving process under the network protocol synchronization mechanism is as follows: ① Send message A from the master clock and record the precise sending time t1 when message A leaves the master clock; ②Message B encapsulates the precise sending time t1 and sends it to the slave clock; ③ The slave clock records the precise arrival time of message A at the slave clock as t2; ④ When sending C message from the clock, record the precise sending time t3; ⑤ The master clock records the precise arrival time t4 of the C message at the master clock; ⑥ The master clock sends a D message carrying precise timestamp information t4 to the slave clock; (2) A master clock is specified in the system. Offset is used to represent the deviation between the master and slave clocks. Delay is used to represent the network delay, that is, the delay of the message during transmission. The calculation formulas for clock deviation and network delay are as follows: C. Modeling of loader: (1) The DH parameter method is used for kinematic modeling, that is, a spatial coordinate system is established on each joint of the equivalent two-link, and the transformation relationship between two different spatial coordinate systems is established through homogeneous coordinate transformation; Assume that the fixed base is link 0, the connection between it and link 1 is joint 1, the connection between link 1 and link 2 is joint 2, the end of link 2 is joint 3, establish the base coordinate system and coordinate system 1 at joint 1, establish coordinate system 2 at joint 2, and establish coordinate system 3 at joint 3; (2) Through forward kinematics solution, the end coordinates of the two-link are obtained as follows: In formula (2), θ1 is the rotation angle of joint 1, θ2 is the rotation angle of joint 2, L1 is the length of rod 1, and L2 is the length of rod 2; (3) Inverse kinematics analysis: In contrast to forward kinematics, inverse kinematics is to find the joint angle based on the known end position. A perpendicular line is drawn from the end point of the two links to the y-axis and connected to the origin to form a right triangle with side lengths of X, Y, and L0. Assume that the upper left angle of the right triangle is θ0, then: In formula (3), According to formula (2), we can get: Taking the squares and adding them together gives: Combining equation (3), we get: Find: D. Estimator parameter design: (1) The two-link dynamics model is as follows: In equations (8) and (9), θ3 and θ4 represent the rotation angles of the two connecting rods. represents the angular velocity of the two-link, represents the angular acceleration of the two-link, M(θ) is the inertia matrix of the two-link; represents the centrifugal force and Coriolis force matrix; G(θ) represents the gravity matrix of the two-link; τ represents the generalized force; E. System Linearization: Since the reference model in the predictor is linear, and the robot dynamics model is nonlinear, it needs to be linearized near the working point. For the system, the state variables are selected as follows: x1=θ3, x3=θ4, The state space form of the system is written as: After Jacobi linearization, the system matrix is: This matrix is ​​represented as: F.Smith delay compensation control: R(s) and Y(s) represent the input and output of the control system, respectively. P (s) are the transfer functions between the controller and the controlled object, e -τs It is called the time lag factor, which represents the pure lag link of the controlled object. The transfer function of the control system is: The characteristic equation of the system is obtained from formula (10): 1+D(s)G p (s)e -τs =0 (11); By connecting the Smith predictor in parallel to D(s), the transfer function of the controller is obtained as: The closed-loop transfer function of the system after Smith predictor compensation is: G. The improved Smith predictive control system with adaptive module is obtained: The process channel transfer function is: but: In formula (15), ε(s) is the error signal; T1 and T2 are the inertia time constants, and K is the gain of the inertia link; Transforming equation (15) into a differential equation, we get: Similarly, there are: In the formula, is the parameter to be designed to compensate for the modeling error, given by: ε(s)=Y(s)-X(s) The error equation is: In order to make the equation stable, we need to find a Lyapunov function V(v,t) such that make but: The Lyapunov function is constructed as follows: Select the adaptive law as: The Lyapunov stability condition is satisfied; Since the system parameters change slowly, have to The expression of the adaptive module is: In formula (21), K0 is the integration constant.

2. A time delay compensation control method for a remote control loader according to claim 1, characterized in that: In step A, setting up the network environment includes the following steps: (1) Network topology design: camera vision nodes are set up in the network domain, these nodes are connected to the server through the wireless LAN, and the server then transmits to the client through the limited LAN; (2) Communication protocol settings: the video captured by the camera is encoded using the MPEG-4 standard, the application layer uses the FTP protocol, the transport layer uses the UDP protocol, the network layer uses the IP protocol, and the wireless LAN uses the IEEE802.11 protocol; at the same time, the network protocol is applied to ensure that all devices on this layer of the network are synchronized with the clock.

Images