A data-driven magnetically controlled capsule pointing control method

Abstract

A data-driven magnetically controlled capsule pointing control method comprises the following steps: Step 1, establishing a dynamic model of a capsule robot system, initializing the system state and control parameters; Step 2, based on a capsule robot system with unknown dynamics, defining its tracking error and disturbance, and designing a controller. The present invention is aimed at a capsule robot system with uncertainties such as multiple variables, strong coupling, and disturbances, and utilizes a two-phase power reaching law to improve the transient convergence speed of the controlled variable and reduce the control jitter when the controlled variable enters a steady state. At the same time, a first-order filter is designed based on the two-phase power reaching law to ensure the continuity of the control signal and weaken the control jitter problem caused by the sign function.

Description

Technical Field

[0001] The present invention relates to a data-driven magnetically controlled capsule pointing control method to meet the performance requirements of a capsule robot system for achieving high-precision pointing to a target area and weakening control jitter. Background Art

[0002] As the demand for gastrointestinal testing in the medical field continues to increase, endoscopic technology is also constantly innovating. Traditional medical endoscopes enter the body through the mouth to complete the observation of gastrointestinal lesions. However, when using traditional endoscopes for examination, the endoscope has a complex structure and cumbersome operation. It may also cause physical damage to the patient, and the patient needs to endure the pain it brings. Therefore, in order to solve the problems of traditional endoscopes, capsule robots have gradually developed to replace them. In recent years, domestic and foreign scholars have conducted a lot of research on actively controlled capsule robots and have achieved certain results in various aspects. However, capsule robots are still in the early stages of development, and there are differences and deficiencies in the research of various scholars on capsule robots in terms of structure and control strategy. Therefore, studying the active control of capsule robots and realizing a series of movements such as displacement and posture adjustment of capsule robots is of great value to the development of capsule robots.

[0003] However, since the capsule robot control system is a typical multivariable, strongly coupled, nonlinear system, controlling the displacement and pointing angle of the capsule robot to achieve various movements will encounter many complex problems. For example, the accuracy of measurement and modeling does not meet the requirements, coupled with the influence of uncertain factors such as external load disturbances and friction changes in non-structural environments, it is impossible to obtain an accurate and complete dynamic model that satisfies the capsule robot. Therefore, conventional capsule robot control strategies and algorithms such as PD control and calculated torque method control can no longer meet the control requirements of the capsule robot application field.

[0004] First-order filtering is often used to further improve control quality when designing MPC or other controllers. For example, filtering the measured value of the controlled variable can prevent measurement noise or measurement fluctuations from causing the controller to act too frequently; softening the constant value can prevent the set value from causing the control action obtained by the solution to be too aggressive, resulting in a large overshoot of the system. Therefore, applying first-order filtering to the capsule robot system has important theoretical and practical significance. Summary of the invention

[0005] In order to overcome the shortcomings of the existing technology, for capsule robot systems with uncertainties such as multi-variables, strong coupling and disturbance, the two-phase power reaching law can improve the transient convergence speed of the controlled variables and reduce the control jitter when the controlled variables enter the steady state. At the same time, a first-order filter is designed based on the two-phase power reaching law to ensure the continuity of the control signal and weaken the control jitter problem caused by the sign function.

[0006] The technical solutions proposed to solve the above technical problems are as follows:

[0007] A data-driven magnetically controlled capsule pointing control method comprises the following steps:

[0008] Step 1: Establish a dynamic model of the capsule robot system and initialize the system state and control parameters. The process is as follows:

[0009] 1.1 Let the center position of the capsule be [x, y, z]. According to the coordinate transformation, the coordinate C of any point in the capsule is expressed as:

[0010]

[0011] Where l represents the actual length of the capsule; ψ represents the rotation angle of the capsule around the z-axis; θ represents the rotation angle of the capsule around the x-axis; Indicates the rotation angle of the capsule around the y-axis;

[0012] 1.2 The expression of the sum of kinetic energy and potential energy L of the capsule robot system is:

[0013]

[0014] Where 2R represents the ideal length of the capsule; M represents the total mass of the capsule;

[0015] Considering that the capsule robot rotates slowly around the z-axis, let its pointing angle q = [θ φ] T , according to the Lagrangian dynamics equation, the capsule model expression is obtained as follows:

[0016]

[0017] G=

[00] T (6)

[0018] Among them, B, C, and G are the inertia matrix, Coriolis matrix, and gravity matrix respectively; They are respectively expressed as the second and first order differentials of the pointing angle q; τ represents the total torque acting on the system;

[0019] Step 2: Based on the capsule robot system with unknown dynamics, define its tracking error and disturbance and design the controller. The process is as follows:

[0020] 2.1 Define the tracking error of the system:

[0021] e=qq d (7)

[0022] Among them, q d represents the desired pointing angle of the capsule;

[0023] Define the disturbances present in the system:

[0024]

[0025] in, a and b represent two arbitrary given parameters; Indicates the maximum value of a and b;

[0026] Based on formula (3), the model expression of the system is expressed as:

[0027]

[0028] Among them, q represents q s A controlled variable in , B1, C1, G1 represent the relevant parameters of q;

[0029] 2.2 Design of first-order filter:

[0030]

[0031] Derivation of formula (10) yields:

[0032]

[0033] Where λ>0, They represent the first and second order differentials of the pointing angle error e respectively;

[0034] 2.3 Design of two-phase power reaching law:

[0035] S=-ρs-η|s| α(s) sgn(s) (12)

[0036] Among them, ρ>0, η>0, p1, p2, q1, q2 are all positive odd numbers, and p1>q1>0, p2>q2>0;

[0037] Design τ′ as:

[0038]

[0039] Combining equations (7), (8), (9) and (13), we get:

[0040]

[0041] Combining equations (11) and (14), we get:

[0042]

[0043] The technical concept of the present invention is as follows: first, the two-phase power approach rate is used to adjust the convergence speed of the controlled variable, so that it has a faster convergence speed before reaching the deceleration point, and after reaching the deceleration point, it can effectively weaken the vibration; on this basis, a first-order filter is designed to ensure that the system pointing angle output can quickly and stably track the desired pointing angle.

[0044] The beneficial effects of the present invention are as follows: the two-phase power approaching law form ensures that the system output has good transient and steady-state performance. The first-order filter compensates for the influence of the uncertainty term on the system tracking accuracy, thereby enhancing the robustness of the system. At the same time, since the design does not include discontinuous functions, the continuity of the control signal can be guaranteed and the jitter problem can be reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 Schematic diagram of the pointing angle tracking effect of the capsule robot system (taking pointing around the x-axis as an example), where the red dotted line represents the desired pointing angle, and the blue solid line represents the actual pointing angle of the system based on the data-driven magnetically controlled capsule pointing control method.

[0046] Figure 2 Schematic diagram of the total torque change of the capsule robot system, where the blue solid line represents the system torque change based on the data-driven magnetically controlled capsule pointing control method.

[0047] Figure 3 Figure 3 is the convergence process of the controlled variables of the capsule robot system, where the blue solid line represents the convergence process of the controller variables based on the data-driven magnetically controlled capsule pointing control method.

[0048] Figure 4 It is a control flow diagram of the present invention. DETAILED DESCRIPTION

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

[0050] Reference Figure 1-Figure 4 , a data-driven magnetically controlled capsule pointing control method, comprising the following steps:

[0051] Step 1: Establish a dynamic model of the capsule robot system and initialize the system state and control parameters. The process is as follows:

[0052] 1.1 Let the center position of the capsule be [x, y, z]. According to the coordinate transformation, the coordinate C of any point in the capsule is expressed as:

[0053]

[0054] Where l represents the actual length of the capsule; ψ represents the rotation angle of the capsule around the z-axis; θ represents the rotation angle of the capsule around the x-axis; Indicates the rotation angle of the capsule around the y-axis;

[0055] 1.2 The expression of the sum of kinetic energy and potential energy L of the capsule robot system is:

[0056]

[0057] Where 2R represents the ideal length of the capsule; M represents the total mass of the capsule;

[0058] Considering that the capsule robot rotates slowly around the z-axis, let its pointing angle q = [θ φ] T , according to the Lagrangian dynamics equation, the capsule model expression is obtained as follows:

[0059]

[0060] G=

[00] T (6)

[0061] Among them, B, C, and G are the inertia matrix, Coriolis matrix, and gravity matrix respectively; They are respectively expressed as the second and first order differentials of the pointing angle q; τ represents the total torque acting on the system;

[0062] Step 2: Based on the capsule robot system with unknown dynamics, define its tracking error and disturbance and design the controller. The process is as follows:

[0063] 2.1 Define the tracking error of the system:

[0064] e=qq d (7)

[0065] Among them, q d represents the desired pointing angle of the capsule;

[0066] Define the disturbances present in the system:

[0067]

[0068] in, a and b represent two arbitrary given parameters; Indicates the maximum value of a and b;

[0069] Based on formula (3), the model expression of the system is expressed as:

[0070]

[0071] Among them, q represents q s A controlled variable in , B1, C1, G1 represent the relevant parameters of q;

[0072] 2.2 Design of first-order filter:

[0073]

[0074] Derivation of formula (10) yields:

[0075]

[0076] Where λ>0, They represent the first and second order differentials of the pointing angle error e respectively;

[0077] 2.3 Design of two-phase power reaching law:

[0078] S=-ρs-η|s| α(s) sgn(s) (13)

[0079] Among them, ρ>0, η>0, p1, p2, q1, q2 are all positive odd numbers, and p1>q1>0, p2>q2>0;

[0080] Design τ′ as:

[0081]

[0082] Combining equations (7), (8), (9) and (13), we get:

[0083]

[0084] Combining equations (11) and (14), we get:

[0085]

[0086] In order to simulate more effectively, the capsule robot system model parameters are set to R = 1 cm, M = 0.01 kg. The system initial value is set to θ = 0 cm, τ = 0. The expected pointing angle is set to θ d =5, the first-order filter gain is set to λ=3. The interference coefficients a=1, b=1.

[0087] Figure 1 This is a schematic diagram of the pointing angle tracking effect of the capsule robot system (taking pointing around the x-axis as an example). It can be seen that the system's pointing angle tracking speed is fast and its transient performance is good. Figure 2 This is a schematic diagram of the total torque change of the capsule robot system. It can be seen from the figure that the total torque of the system has a faster approaching speed. Figure 3The figure is the convergence process diagram of the sliding mode variable of the capsule robot system. In addition, the symbol function is weakened in the controller design to ensure the continuity of the controller, so the chattering is weakened.

[0088] The above describes the excellent optimization effect shown by an embodiment of the present invention. Obviously, the present invention is not limited to the above embodiment, and various modifications can be made to it without departing from the basic spirit of the present invention and without exceeding the scope of the essential content of the present invention.

Claims

1. A data-driven magnetically controlled capsule pointing control method, characterized in that the control method comprises the following steps: Step 1: Establish a dynamic model of the capsule robot system and initialize the system state and control parameters. The process is as follows: 1.1 Let the center position of the capsule be [x, y, z]. According to the coordinate transformation, the coordinate C of any point in the capsule is expressed as: in, l represents the actual length of the capsule; ψ represents the rotation angle of the capsule around the z-axis; θ represents the rotation angle of the capsule around the x-axis; Indicates the rotation angle of the capsule around the y-axis; 1.2 The expression of the sum of kinetic energy and potential energy L of the capsule robot system is: Where 2R represents the ideal length of the capsule; M represents the total mass of the capsule; Considering that the capsule robot rotates slowly around the z-axis, let its pointing angle q = [θφ] T , according to the Lagrangian dynamics equation, the capsule model expression is obtained as follows: G＝[00] T (6) Among them, B, C, and G are the inertia matrix, Coriolis matrix, and gravity matrix respectively; They are respectively expressed as the second and first order differentials of the pointing angle q; τ represents the total torque acting on the system; Step 2: Based on the capsule robot system with unknown dynamics, define its tracking error and disturbance and design the controller. The process is as follows: 2.1 Define the tracking error of the system: e=q-q d (7) in, q d represents the desired pointing angle of the capsule; Define the disturbances present in the system: in, a and b represent two arbitrary given parameters; Indicates the maximum value of a and b; Based on formula (3), the model expression of the system is expressed as: Among them, q represents q s A controlled variable in , B1, C1, G1 represent the relevant parameters of q; 2.2 Design of first-order filter: Derivation of formula (10) yields: Where λ>0, They represent the first and second order differentials of the pointing angle error e respectively; 2.3 Design of two-phase power reaching law: S=-ρs-η|s| α(s) sgn(s) (12) Among them, ρ>0, η>0, p1, p2, q1, q2 are all positive odd numbers, and p1>q1>0, p2>q2>0; Design τ′ as: Combining equations (7), (8), (9) and (13), we get: Combining equations (11) and (14), we get:

Images