A motion control method for tunnel trench robots under complex environmental interference

Abstract

This invention discloses a motion control method for a tunnel trench robot under complex environmental interference, comprising the following steps: S1, constructing an ideal mathematical model of the tunnel trench robot based on its differential drive method; S2, decoupling the external environmental interference along X1 and Y1 into a longitudinal disturbance v. er v el And lateral perturbation v h S3. Establish an outer-loop sliding mode variable structure controller and derive ideal velocity and angular velocity control laws; S4. Construct an extended state observer to observe the lateral and longitudinal disturbances of the tunnel robot; S5. Establish an inner-loop sliding mode variable structure controller and correct the velocity and angular velocity control laws of the outer loop. The motion control method proposed in this invention improves the anti-interference capability of the tunnel robot under complex environmental disturbances, suppresses observed disturbances, and improves the robustness and stability of the entire closed-loop system.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot control technology, and specifically relates to a motion control method for a tunnel trench robot under complex environmental interference. Background Technology

[0002] Tunneling robots are a special type of wheeled mobile robot (WMR). They are differentially driven mobile robots, a classic multi-input multi-output system, characterized by strong coupling and time-varying system parameters that are easily affected by external environmental disturbances. Precise control is fundamental for robots to complete complex tasks. Due to the existence of Brockett constraints, robots cannot achieve continuously differentiable, linear, and time-invariant control systems. Therefore, robot control has been a hot topic in recent years.

[0003] In recent years, significant progress has been made in addressing the motion control problem of mobile robots. Currently, some mature control methods include: backstepping control; fuzzy control; PID control; adaptive control; and neural network control. Adaptive control offers good response to changes in the environment and performs well in cases of structural uncertainty, but it is ineffective for unstructured problems. Fuzzy control has traditionally relied on empirical formulas to modify parameters, failing to provide effective guidance for uncertain systems. While these methods have improved the control performance of mobile robots to some extent, real-world environmental interferences exist, such as silt, water accumulation, and sediment. Single controllers in these situations often suffer from performance limitations under complex and disturbed conditions.

[0004] To address the aforementioned issues, some researchers have proposed the idea of ​​an observer, which uses an observer to detect disturbance values ​​and suppress them in advance, thus designing a controller based on the disturbance observer. This observer-based controller can suppress environmental interference to a certain extent, but it lacks strong robustness, which can negatively impact motion control performance in complex system conditions. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and provide a motion control method for tunneling robots under complex environmental interference that can improve the anti-interference ability of tunneling robots under complex environmental interference, suppress observed disturbances, and improve the robustness and stability of the entire closed-loop system.

[0006] The objective of this invention is achieved through the following technical solution: a motion control method for a tunnel trench robot under complex environmental interference, comprising the following steps:

[0007] S1. Based on the differential drive method of the tunnel trench robot, an ideal mathematical model of the tunnel trench robot is constructed. The specific method is as follows: there is a motor on each side of the tunnel trench robot, and the motor is connected to the two tires through a gear and rack. The two wheels have the same speed.

[0008] With the geometric center of the tunnel boring machine (TBM) robot as the origin O1 of the translation coordinate system [X1, Y1, O1], the forward direction of the TBM robot is taken as its horizontal axis X1, and the direction perpendicular to the forward direction is taken as its vertical axis Y1; θ is the angle between the translation coordinate system of the TBM robot and the X-axis of the world coordinate system [X, Y, O]; the radius of the left and right wheels of the TBM robot is R, and the distance between the geometric centers of the left and right wheels is 2D; the overall velocity of the TBM robot is V, and its angular velocity is denoted as W; the velocity of the left wheel of the TBM robot is denoted as v. l The speed of the right wheel is denoted as v. r ;

[0009] The relationship between the overall speed and angular velocity of the tunneling robot and the speeds of its left and right wheels is as follows:

[0010]

[0011] The theoretical kinematic equations are established as follows:

[0012]

[0013] In the formula This represents the actual pose of the tunneling robot.

[0014] S2. When external environmental interference is introduced, the interference is decoupled along X1 and Y1 into a longitudinal disturbance v. er v el And lateral perturbation v h ;

[0015] The overall velocity and angular velocity of the tunneling robot are expressed by the following equations:

[0016]

[0017] The original kinematic model of the tunneling robot is transformed into the following equations:

[0018]

[0019] S3. Establish an outer-loop sliding mode variable structure controller and derive ideal speed and angular velocity control laws;

[0020] S4. Construct an extended state observer to observe the lateral and longitudinal disturbances of the trench robot;

[0021] S5. Establish an inner loop sliding mode variable structure controller and correct the speed and angle control law of the outer loop.

[0022] The beneficial effects of this invention are as follows: Considering the impact of complex environmental disturbances on the motion control performance of tunnel trench robots in actual working environments, this invention proposes an inner and outer loop sliding mode variable structure control method using an extended state observer under complex environmental disturbances. The tunnel trench robot is a differentially driven wheeled mobile robot. Considering complex environmental disturbances, these disturbances are decoupled into X-direction and Y-direction disturbances, establishing a kinematic model of the tunnel trench robot under complex environmental disturbances. Next, an outer loop sliding mode variable structure controller is established, deriving ideal velocity and angular velocity control laws. Based on the kinematic model of the tunnel trench robot, an inner loop extended state observer (ESO) is designed. Using the observed disturbances, an inner loop sliding mode variable structure controller is established, which corrects the velocity and angular velocity control laws of the outer loop. The motion control method proposed in this invention improves the anti-interference capability of the tunnel trench robot under complex environmental disturbances, suppresses observed disturbances, and improves the robustness and stability of the entire closed-loop system. Attached Figure Description

[0023] Figure 1 This is a flowchart of the motion control method for the tunnel trench robot of the present invention;

[0024] Figure 2 This is a mathematical model diagram of the tunnel trench robot of the present invention;

[0025] Figure 3 A block diagram of a control system constructed using the control method of the present invention;

[0026] Figure 4 The state of the tunnel trench robot under constant disturbance conditions;

[0027] Figure 5 Position of the tunnel trench robot under constant interference;

[0028] Figure 6 The state of the tunnel trench robot under non-constant disturbances;

[0029] Figure 7 The position of the tunnel trench robot under non-value interference. Detailed Implementation

[0030] The technical solution of the present invention will be further described below with reference to the accompanying drawings.

[0031] like Figure 1 As shown, the present invention provides a motion control method for a tunnel trench robot under complex environmental interference, comprising the following steps:

[0032] S1. Based on the differential drive method of the tunnel trench robot, an ideal mathematical model of the tunnel trench robot is constructed. The specific method is as follows: there is a motor on each side of the tunnel trench robot, and the motor is connected to the two tires through a gear and rack. The two wheels have the same speed.

[0033] like Figure 2 As shown, the origin O1 of the translation coordinate system [X1,Y1,O1] is the geometric center of the tunneling robot. The direction of travel of the tunneling robot is taken as the horizontal axis X1, and the direction perpendicular to the direction of travel is taken as the vertical axis Y1. θ is the angle between the translation coordinate system of the tunneling robot and the X-axis of the world coordinate system [X,Y,O]. The radius of the left and right wheels of the tunneling robot is R, and the distance between the geometric centers of the left and right wheels is 2D. The overall velocity of the tunneling robot is V, and the angular velocity is denoted as W. The velocity of the left wheel of the tunneling robot is denoted as v. l The speed of the right wheel is denoted as v. r The purpose of modeling the tunneling robot is to derive the positional relationship of the tunneling robot in the global world coordinate system, as well as the relationship of the corresponding wheel speed in the world coordinate system.

[0034] The relationship between the overall speed and angular velocity of the tunneling robot and the speeds of its left and right wheels is as follows:

[0035]

[0036] The theoretical kinematic equations are established as follows:

[0037]

[0038] In the formula

[0039] This represents the actual pose of the tunneling robot.

[0040] S2. When external environmental interference is introduced, the interference is decoupled along X1 and Y1 into a longitudinal disturbance v. er v el And lateral perturbation v h (The terms "horizontal" and "vertical" are fixed terms for mobile robots; "vertical" refers to the direction indicated by the left and right tires.)

[0041] The overall velocity and angular velocity of the tunneling robot are expressed by the following equations:

[0042]

[0043] The original kinematic model of the tunneling robot is transformed into the following equations:

[0044]

[0045] S3. Establish an outer-loop sliding mode variable structure controller and derive the ideal velocity and angular velocity control laws; the specific implementation method is as follows: define the ideal position as [x d ,y d The goal is to achieve the tracking of the actual position [x, y] of the tunnel boring machine to the ideal position; the error equation is defined as: x e =xx d ,y e =yy d ;

[0046] Select the sliding surface s1 = x e ,s2=y e The following controller equations are obtained:

[0047]

[0048] k1 and k2 are the control surface parameters of the sliding mode involved. Both k1 and k2 need to be greater than 0 to ensure convergence. The larger the value of k1 and k2, the greater the adjustment range. x d ,y d The differential (the dot above the letters in the following text indicates the differential);

[0049] Based on the above equation, the ideal angle is obtained as follows:

[0050] The theoretical actual speed of the tunneling robot is derived as follows:

[0051]

[0052] A nonlinear differential controller is introduced to derive the angular velocity control law for the tunnel boring machine. The nonlinear differential controller is as follows:

[0053]

[0054] Where σ1=θ d σ2 is θ d The differential signal, k3 and k4 are parameters that need to be designed; the sat() function has the following description:

[0055]

[0056] In the formula, sign(a) is a sign switching function. When a≥0, the value of the function is 1, and otherwise the value of the function is -1.

[0057] Design of the outer ring angular velocity controller: The control law for the angular velocity of the tunnel boring machine is θ from the above formula. d , This is generated. To ensure the stability of the entire tunnel trench robot control system, the angular velocity needs to be tracked to the ideal angular velocity more quickly. The method selected in this invention is to increase the convergence speed of the angular velocity significantly more than the convergence speed of the velocity controller. The sliding surface is defined as s3 = θ - θ d Then we have the following equation:

[0058]

[0059] In the formula, k5 and ε1 are parameters that need to be designed to ensure that the convergence speed of angular velocity is faster than that of velocity.

[0060] S4. Construct an Extended State Observer (ESO) to observe the lateral and longitudinal disturbances of the trench robot; the specific implementation method is as follows: redefine the error equation as e x =xx d e y =yy d The error equation above is rewritten as follows:

[0061]

[0062] u in the formula x u y d represents the total input of the system. x d y The total disturbance received;

[0063] Based on the above formula, let x1 = e x x2 = d x y1=e y y2=d y Define the extended state observer as follows:

[0064]

[0065] in Let x1, x2, y1, y2 be the observed values. Let α1, α2, α3, α4, β1, β2 be the observation gain parameters of the observer. We design α1, β1, α3, β4 > 0 and α2, α4 < 0 so that the root locus of the observer system is located in the left half of the complex plane, which makes the observer converge (the determinant of the characteristic matrix of the system's state space is not 0).

[0066] S5. Establish the sliding mode variable structure controller for the inner loop, modify the speed and angle control law of the outer loop, and select the sliding surface as: s4 = x1, s5 = y1. The established sliding mode variable structure controller for the inner loop is as follows:

[0067]

[0068] u in the formula x u y These are the corrected input parameters. k7 is a parameter of the inner ring slip membrane variable structure. k6, k7 > 0. The larger the value, the greater the adjustment range, but the basic requirement is > 0. The specific value can be set by the user.

[0069] Using the control method designed in the above steps, a system such as... Figure 3 The control system block diagram shown illustrates that the outer-loop sliding mode variable structure controller provides ideal velocity and angular velocity control laws. An extended state observer observes disturbances in the complex environment. The inner-loop sliding mode variable structure controller utilizes the observed disturbance values, combined with the preceding kinematic controller, to construct the inner-loop sliding mode variable structure controller, thus correcting the outer-loop velocity and angular velocity control laws. Simulation results demonstrate that, under complex environmental disturbances, the inner and outer-loop sliding mode variable structure controllers based on extended state observation designed in this invention exhibit strong anti-interference capabilities, ensuring good motion control performance of the vehicle.

[0070] To verify the effectiveness of the motion control method for the tunnel trench robot of the present invention, a system was built in Matlab as follows: Figure 4 The Simulink simulation environment shown uses the following simulation parameters: The ideal trajectory of the tunneling robot is: x d =t,y d =sin(0.5x) d )+0.5x d +1; The sliding mode variable structure parameters of the outer ring are: k1=0.4, k2=0.4; The parameters of the expansion state observer are as follows: [α1α2β1]=[0.8 -0.6 0.1], [α3α4β2]=[0.8 -0.6 0.1], and the sliding mode variable structure parameters of the inner ring are: k6=12, k7=12.

[0071] The simulation environment compares two motion control schemes: (1) the internal and external loop sliding mode variable structure control method without adding an extended state observer; (2) the motion control method with an extended state observer.

[0072] Two types of environmental disturbances were selected for simulation: (1) constant disturbance environment, where the disturbance value is a constant. (2) non-constant disturbance environment, where the disturbance value is a sine function.

[0073] (1) Simulation experiment of constant interference environment: The state of the tunnel trench robot under constant interference is as follows Figure 4 As shown, the position of the tunnel trench robot under constant interference is as follows: Figure 5As shown in the figure, the controller with ESO designed using this invention exhibits significantly better angle control performance when controlling the robot, and the position control performance is also slightly better using the method of this invention.

[0074] (2) Simulation experiment of unconventional environmental interference: The state of the tunnel trench robot under unconventional interference is as follows: Figure 6 As shown, the position of the tunnel trench robot under non-value interference is as follows: Figure 7 As shown in the figure, the controller with ESO designed using this invention exhibits significantly better angle control performance when controlling the robot, and the position control performance is also slightly better using the method of this invention.

[0075] Those skilled in the art will recognize that the embodiments described herein are intended to help the reader understand the principles of the invention, and should be understood that the scope of protection of the invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific modifications and combinations based on the technical teachings disclosed in this invention without departing from the spirit of the invention, and these modifications and combinations are still within the scope of protection of this invention.

Claims

1. A motion control method for a tunnel trench robot under complex environmental interference, characterized in that, Includes the following steps: S1. Based on the differential drive method of the tunnel trench robot, an ideal mathematical model of the tunnel trench robot is constructed. The specific method is as follows: there is a motor on each side of the tunnel trench robot, and the motor is connected to the two tires through a gear and rack. The two wheels have the same speed. With the geometric center of the tunnel boring machine (TBM) robot as the origin O1 of the translation coordinate system [X1, Y1, O1], the forward direction of the TBM robot is taken as its horizontal axis X1, and the direction perpendicular to the forward direction is taken as its vertical axis Y1; θ is the angle between the translation coordinate system of the TBM robot and the X-axis of the world coordinate system [X, Y, O]; the radius of the left and right wheels of the TBM robot is R, and the distance between the geometric centers of the left and right wheels is 2D; the overall velocity of the TBM robot is V, and its angular velocity is denoted as W; the velocity of the left wheel of the TBM robot is denoted as v. l The speed of the right wheel is denoted as v. r ; The relationship between the overall speed and angular velocity of the tunneling robot and the speeds of its left and right wheels is as follows: The theoretical kinematic equations are established as follows: In the formula This represents the actual pose of the tunneling robot. S2. When external environmental interference is introduced, the interference is decoupled along X1 and Y1 into a longitudinal disturbance v. er v el And lateral perturbation v h ; The overall velocity and angular velocity of the tunneling robot are expressed by the following equations: The original kinematic model of the tunneling robot is transformed into the following equations: S3. Establish an outer-loop sliding mode variable structure controller and derive the ideal velocity and angular velocity control laws; the specific implementation method is as follows: define the ideal position as [x d ,y d Define the error equation: x e =xx d ,y e =yy d ; Select the sliding surface s1 = x e ,s2=y e The following controller equations are obtained: k1 and k2 are the control surface parameters of the sliding mode involved; x d ,y d The differential; Based on the above equation, the ideal angle is obtained as follows: The theoretical actual speed of the tunneling robot is derived as follows: A nonlinear differential controller is introduced to derive the angular velocity control law for the tunnel boring machine. The nonlinear differential controller is as follows: Where σ1=θ d σ2 is θ d The differential signal, k3 and k4 are parameters that need to be designed; the sat() function has the following description: In the formula, sign(a) is a sign switching function. When a≥0, the value of the function is 1, and otherwise the value of the function is -1. Design the angular velocity controller for the outer ring: Define the sliding surface as s3 = θ - θ d Then we have the following equation: In the formula, k5 and ε1 are parameters that need to be designed to ensure that the convergence speed of angular velocity is faster than that of velocity. S4. Construct an extended state observer to observe the lateral and longitudinal disturbances of the trench robot; S5. Establish an inner loop sliding mode variable structure controller and correct the speed and angle control law of the outer loop.

2. The method for motion control of a tunnel trench robot under complex environmental interference as described in claim 1, characterized in that, The specific implementation method of step S4 is as follows: redefine the error equation as e x =xx d e y =yy d The error equation above is rewritten as follows: u in the formula x u y d represents the total input of the system. x d y The total disturbance received; Based on the above formula, let x1 = e x x2 = d x y1=e y y2=d y Define the extended state observer as follows: in Let x1, x2, y1, y2 be the observed values. Let α1, α2, α3, α4, β1, β2 in the above equation be the observation gain parameters of the observer. We design α1, β1, α3, β4 > 0 and α2, α4 < 0 so that the root locus of the observer system is located in the left half of the complex plane, which makes the observer converge.

3. The method for motion control of a tunnel trench robot under complex environmental interference as described in claim 1, characterized in that, The selected sliding surfaces are s4 = x1 and s5 = y1. The sliding variable structure controller for the inner ring established in step S5 is as follows: u in the formula x u y These are the corrected input parameters. k6 and k7 are the parameters of the inner ring sliding mode variable structure.

Images